/*
 * 	startup.c
 *
 */
#define SIMULATOR
//#define USBDM

#include "box.xbm"
#include "box_hit.xbm"
#include "player.xbm"
#include "wall.xbm"


typedef unsigned int uint32_t; 
typedef unsigned short uint16_t;
typedef unsigned char uint8_t; 

typedef struct {
	uint32_t moder;
	uint16_t otyper; // +0x4
	uint16_t otReserved;
	uint32_t ospeedr; // +0x8
	uint32_t pupdr; // +0xc
	uint8_t idrLow; // +0x10
	uint8_t idrHigh; // +0x11
	uint16_t idrReserved;
	uint8_t odrLow; // +0x14
	uint8_t odrHigh; // +0x15
	uint16_t odrReserved;
} GPIO;

typedef volatile GPIO* gpioPtr;

#define GPIO_D (*((gpioPtr) 0x40020C00))	/* GPIO HIGH AND LOW Keypad */
#define GPIO_E (*((gpioPtr) 0x40021000))	/* Graphic */
 
#define SYS_TICK 0xE000E010
#define STK_CTRL ((volatile unsigned int *) (SYS_TICK))
#define STK_LOAD ((volatile unsigned int *) (SYS_TICK + 0x4))
#define STK_VAL ((volatile unsigned int *) (SYS_TICK + 0x8))
#define STK_CALIB ((volatile unsigned int *) (SYS_TICK + 0xC))

/* MASKS FOR ASCII DISPLAY*/
#define B_E 0x40 /* Enable-signal */
#define B_SELECT 4 /* Select ASCII-display */
#define B_RW 2 /* 0=Write, 1=Read */
#define B_RS 1 /* 0=Control, 1=Data */

/* MASKS FOR GRAPHIC DISPLAY */
#define B_RST 0x20
#define B_CS2 0x10
#define B_CS1 0x08

#define LCD_ON 0x3F
#define	LCD_OFF 0x3E
#define LCD_DISP_START 0xC0
#define LCD_SET_ADD 0x40
#define LCD_SET_PAGE 0xB8
#define LCD_BUSY 0x80

typedef unsigned char uint8_t;

/* TYPEDEFS FOR SPRITES */
typedef struct tSpriteData{
	unsigned char width;
	unsigned char height;
	unsigned char* data;
}SPRITEDATA, *PSPRITEDATA;

typedef struct tSprite{
	char posx, posy;
	char dirx, diry;
	char speedx, speedy;
} SPRITE, *PSPRITE;

static void load_sprite(PSPRITEDATA s, unsigned char* data, int width, int height){
	s->width = width;
	s->height = height;
	s->data = data;
}


unsigned char ascii_read_status (void);



void startup(void) __attribute__((naked)) __attribute__((section (".start_section")) );

void startup ( void )
{
__asm volatile(
	" LDR R0,=0x2001C000\n"		/* set stack */
	" MOV SP,R0\n"
	" BL main\n"				/* call main */
	"_exit: B .\n"				/* never return */
	) ;
}

void delay_250ns(void){
	*STK_CTRL = 0;
	*STK_LOAD = ((168/4)-1);	// One micro sec = 168 clock cycles -> 168/4 = 250 nano sec
	*STK_VAL = 0;
	*STK_CTRL = 0x5;
	while((*STK_CTRL & 0x10000) == 0){}
	*STK_CTRL = 0;
}

void delay_500ns(void){
	delay_250ns();
	delay_250ns();
}

void delay_micro(unsigned int us){
	#ifdef	SIMULATOR
		us = us/1000;
		us++;
	#endif
	while(us > 0){
		delay_250ns();
		delay_250ns();
		delay_250ns();
		delay_250ns();
		us--;
	}
}

void delay_milli(unsigned int ms){
	#ifdef	SIMULATOR
		ms = ms/1000;
		ms++;
	#endif
	while(ms > 0){
		delay_micro(1000);
		ms--;
	}
}

void init_app (void){	
	GPIO_E.moder = 0x55555555;
	GPIO_D.moder = 0x55000000;
}

void graphic_ctrl_bit_set(uint8_t x){
	unsigned char c;
	c = GPIO_E.odrLow;
	c |= x;
	GPIO_E.odrLow = c;
}

void graphic_ctrl_bit_clear(uint8_t x){
	unsigned char c;
	c = GPIO_E.odrLow;
	c &= ~x;
	GPIO_E.odrLow = c;
}

void select_controller(uint8_t controller)
{
        switch(controller)
        {
                case 0:
                        graphic_ctrl_bit_clear(B_CS1|B_CS2);
                        break;
                case B_CS1 :
                        graphic_ctrl_bit_set(B_CS1);
                        graphic_ctrl_bit_clear(B_CS2);
                        break;
                case B_CS2 :
                        graphic_ctrl_bit_set(B_CS2);
                        graphic_ctrl_bit_clear(B_CS1);
                        break;
                case B_CS1|B_CS2 :
                        graphic_ctrl_bit_set(B_CS1|B_CS2);
                        break;
        }
}

void graphic_wait_ready(void){
	graphic_ctrl_bit_clear(B_E);
	GPIO_E.moder = 0x00005555;
	graphic_ctrl_bit_clear(B_RS);
	graphic_ctrl_bit_set(B_RW);
	delay_500ns();
	
	while(GPIO_E.odrLow & 0x1){
		graphic_ctrl_bit_set(B_E);
		delay_500ns();
		graphic_ctrl_bit_clear(B_E);
		delay_500ns();
	}
	graphic_ctrl_bit_set(B_E);
	GPIO_E.moder = 0x55555555;
}

unsigned char graphic_read(uint8_t controller){
	uint8_t c;
	graphic_ctrl_bit_clear(B_E);
	GPIO_E.moder = 0x00005555;
	graphic_ctrl_bit_set(B_RS | B_RW);
	select_controller(controller);
	delay_500ns();
	graphic_ctrl_bit_set(B_E);
	delay_500ns();
	c = GPIO_E.idrHigh; //unsure if this is correct, but it should be (data in the higher bits?)
	graphic_ctrl_bit_clear(B_E);
	GPIO_E.moder = 0x55555555;
	
	if(controller == B_CS1){
		select_controller(B_CS1);
		graphic_wait_ready();
	}
	if(controller == B_CS2){
		select_controller(B_CS2);
		graphic_wait_ready();
	}
	
	return c; // Hmm?
}

static void graphic_write(uint8_t value, uint8_t controller)
{
        GPIO_E.odrHigh = value;
        select_controller(controller);
        delay_500ns();
        graphic_ctrl_bit_set(B_E);
        delay_500ns();
        graphic_ctrl_bit_clear(B_E);
        
        if (controller == B_CS1)
        {
			select_controller(B_CS1);
			graphic_wait_ready();
        }
        else if (controller == B_CS2)
        {
			select_controller(B_CS2);
			graphic_wait_ready();
        }
        GPIO_E.odrHigh = 0;
        graphic_ctrl_bit_set(B_E);
        select_controller(0);
}

void graphic_write_command(uint8_t cmd, uint8_t controller){
	graphic_ctrl_bit_clear(B_E);
	select_controller(controller);

	graphic_ctrl_bit_clear(B_RS);
	graphic_ctrl_bit_clear(B_RW);
	
	graphic_write(cmd, controller);
}

void graphic_write_data(uint8_t data, uint8_t controller){
	graphic_ctrl_bit_clear(B_E);
	select_controller(controller);

	graphic_ctrl_bit_set(B_RS);
	graphic_ctrl_bit_clear(B_RW);
	
	graphic_write(data, controller);
}

uint8_t graphic_read_data(unsigned char controller){
	(void) graphic_read(controller); /* returns bullshit */
	return graphic_read(controller); /* returns correct data */
}

void graphic_initialize(void){
	graphic_ctrl_bit_set(B_E);
	delay_micro(10);
	
	graphic_ctrl_bit_clear(B_CS1 | B_CS2 | B_RST | B_E);
	delay_milli(30);
	
	graphic_ctrl_bit_set(B_RST);
	graphic_write_command(LCD_OFF, B_CS1 | B_CS2);
	graphic_write_command(LCD_ON, B_CS1 | B_CS2);
	graphic_write_command(LCD_DISP_START, B_CS1 | B_CS2);
	graphic_write_command(LCD_SET_ADD, B_CS1 | B_CS2);
	graphic_write_command(LCD_SET_PAGE, B_CS1 | B_CS2);
	select_controller(0);
}

void graphic_clear_screen(void){
	for(int page = 0; page <= 7; page++){
		graphic_write_command(LCD_SET_PAGE | page, B_CS1 | B_CS2);
		graphic_write_command(LCD_SET_ADD | 0, B_CS1 | B_CS2);
		for(int add = 0; add <= 63; add++){
			graphic_write_data(0, B_CS1 | B_CS2);
		}
	}
}

void graphic_clear_lower(void){
	for(int page = 0; page <= 7; page++){
		graphic_write_command(LCD_SET_PAGE | page, B_CS1 | B_CS2);
		graphic_write_command(LCD_SET_ADD | 0, B_CS1 | B_CS2);
		for(int add = 0; add <= 63; add++){
			graphic_write_data(0, B_CS1 | B_CS2);
		}
	}
}

void pixel(int x, int y, int set) {
	uint8_t mask, c, controller;
	int index;
	
	if((x < 0) || (y < 0) || (x > 128) || (y > 64)) return;
	
	index = (y-1)/8;
	
	switch( (y-1)%8 ) {
		case 0: mask = 1; break;
		case 1: mask = 2; break;
		case 2: mask = 4; break;
		case 3: mask = 8; break;
		case 4: mask = 0x10; break;
		case 5: mask = 0x20; break;
		case 6: mask = 0x40; break;
		case 7: mask = 0x80; break;
	}
	if(set == 0)
		mask = ~mask;
	if(x > 64){
		controller = B_CS2;
		x = x - 65;
	} else {
		controller = B_CS1;
		x = x-1;
	}
	
	graphic_write_command(LCD_SET_ADD | x, controller );
	graphic_write_command(LCD_SET_PAGE | index, controller );
	c = graphic_read_data(controller);
	graphic_write_command(LCD_SET_ADD | x, controller);
	
	if(set)
		mask = mask | c;
	else
		mask = mask & c;
		
	graphic_write_data(mask, controller);
}

//void pixel_buffer(int x, int y) {
//	uint8_t mask;
//	int index = 0;
//	if( (x > 128 ) || (x < 0) || (y > 64) || (y < 0) ) return;
//	
//	mask = 1 << ((y-1)%8);
//	
//	if(x > 64) {
//		x -= 65;
//		index = 512;
//	}
//	
//	index += x + ((y-1)/8)*64;
//	backBuffer[index] |= mask;
//}
//
//
//void graphic_draw_screen(void) {
//	uint8_t i, j, controller, c;
//	unsigned int k = 0;
//	for(c = 0; c <= 1; c++){
//		controller = (c == 0) ? B_CS1 : B_CS2;
//		for(j = 0; j < 8; j++){
//			graphic_write_command(LCD_SET_PAGE | j, controller);
//			graphic_write_command(LCD_SET_ADD | 0, controller);
//			for(i = 0; i < 64; i++, k++){
//				graphic_write_data(backBuffer[k], controller);
//			}
//		}
//	}
//}

/*----------------------- ASCII -----------------------*/

/* Addressera ASCII-display och ettställ de bitar som är 1 i x */
void ascii_ctrl_bit_set(unsigned char x){
	unsigned char c;
	c = GPIO_E.odrLow;
	c |= (B_SELECT | x);
	GPIO_E.odrLow = c;
}

void ascii_ctrl_bit_clear(unsigned char x){
	unsigned char c;
	c = GPIO_E.odrLow;
	c &= (B_SELECT | ~x);
	GPIO_E.odrLow = c;
}

void ascii_write_controller(unsigned char c){
	ascii_ctrl_bit_set(B_E);
	GPIO_E.odrHigh = c;
	delay_250ns();
	ascii_ctrl_bit_clear(B_E);
}

unsigned char ascii_read_controller(void){
	ascii_ctrl_bit_set(B_E);
	delay_250ns();
	delay_250ns();
	unsigned char c = GPIO_E.idrHigh;
	ascii_ctrl_bit_clear(B_E);
	return c;
}

void ascii_write_cmd(unsigned char command){
	ascii_ctrl_bit_clear(B_RS | B_RW);
	ascii_write_controller(command);
}

void ascii_write_data(unsigned char data){
	ascii_ctrl_bit_set(B_RS);
	ascii_ctrl_bit_clear(B_RW);
	ascii_write_controller(data);
}

void ascii_write_char(unsigned char c){
	while((ascii_read_status() & 0x80) == 0x80){}
	delay_micro(8);
	ascii_write_data(c);
	delay_micro(45);
}

unsigned char ascii_read_status(void){
	unsigned char c;
	GPIO_E.moder = 0x00005555;
	ascii_ctrl_bit_clear(B_RS);
	ascii_ctrl_bit_set(B_RW);
	c = ascii_read_controller();
	GPIO_E.moder = 0x55555555;
	return c;
}

unsigned char ascii_read_data(void){
	GPIO_E.moder = 0x00005555;
	ascii_ctrl_bit_set(B_RS);
	ascii_ctrl_bit_set(B_RW);
	unsigned char c = ascii_read_controller();
	GPIO_E.moder = 0x55555555;
	return c;
}

void ascii_init(void){
	while((ascii_read_status() & 0x80) == 0x80){}
	delay_micro(8);
	ascii_write_cmd(0x38); // Rows: 2, Size: 5x8
	delay_micro(40);
	ascii_write_cmd(0x0E); // Display: on, Cursor: constant on
	delay_micro(40);
	ascii_write_cmd(0x06); // Increment, Shift: off
	delay_micro(40);
}

void ascii_command (unsigned char command)
{
	while(ascii_read_status() & 0x80 == 0x80){}
	delay_micro(8);								/* Latenstid för kommando */
	ascii_write_cmd(command);					/* Kommandot som ska exekveras; t.ex. 1 - Clear Display */
	delay_micro(40);								/* Avrundat uppåt; istället för 1,53ms */
	
}


  
void ascii_gotoxy(int x, int y){
	unsigned char adress;
	
	if(y != 1)
	{
		adress = 0x40|(x - 1);
	}
	else
	{
		adress = x - 1;
	}
	ascii_write_cmd(0x80|adress);
	delay_micro(45);
}


/*--------------------------- ASCII END ------------------------------*/
/*--------------------------- KEYPAD -----------------------------*/
//void activateRow(unsigned int row){
//	switch(row){
//		case 1: GPIO_D.odrHigh = 0x10; break;
//		case 2: GPIO_D.odrHigh = 0x20; break;
//		case 3: GPIO_D.odrHigh = 0x40; break;
//		case 4: GPIO_D.odrHigh = 0x80; break;
//		case 0: GPIO_D.odrHigh = 0x00; break;
//	}
//}

//unsigned char keyb(void){
//	int row, column;
//	unsigned char key[]={1,2,3,0xA,4,5,6,0xB,7,8,9,0xC,0xE,0,0xF,0xD};
//	
//	for(row = 1; row <= 4; row++){
//		activateRow(row);
//		column = readColumn();
//		
//		if(column){
//			activateRow(0);
//			return key[4*(row - 1) + column - 1];
//		}
//	}
//	activateRow(0);
//	return 0xFF;
//}

/*--------------------------- KEYPAD END-----------------------------*/


void draw_sprite(PSPRITEDATA sd, PSPRITE s, char set){
	
	int i,j,k, width_in_bytes;
	
	if (sd->width % 8 == 0)
		width_in_bytes = (sd->width / 8); 
	else 
		width_in_bytes = (sd->width / 8 + 1);
	
	for(i = 0; i < sd->height; i++){
		
		for (j = 0; j < width_in_bytes; j++){
			unsigned char byte = sd->data[i * width_in_bytes + j];
			
			for (k = 0; k < 8; k++){
				if (byte & (1 << k))
					pixel(8 * j + k + s->posx + 1, i + s->posy + 1, set);
			}
		}
	}
}

void sprite_move(PSPRITE s, uint8_t player){
	char newPos = s->dirx*s->speedx + s->posx;
	
	if(player == 1){
		if(newPos > 2 && newPos < 45)
			s->posx = newPos;
		
	}else if(player == 2){
		if(newPos < 69 && newPos > 128)
			s->posx = newPos;
	}
		
	//s->posy += s->speedy*s->diry;
}


SPRITE box[40];

SPRITE player1 = {
	5,60,
	0,0,
	2,2
};

SPRITE player2 = {
	90,60,
	0,0,
	2,2
};

PSPRITE p1 = &player1;
PSPRITE p2 = &player2;

int readColumn(){
	unsigned char c;
	unsigned int r;
	r = 0;
	c = GPIO_D.idrHigh;
	if(c & 0x1)
		r += 1;
	if(c & 0x2)
		r += 10;
	if(c & 0x4)
		r += 100;
	if(c & 0x8)
		r += 1000;
	return r;
}

unsigned char keyb(void){
	
	int r = 0;
	
	GPIO_D.odrHigh = 0x10; // AKTIVERA RAD 1
	
	r = readColumn();
	
	switch(r){
		case 1: p1->dirx = -1; break;
		case 10: p1->dirx = 1; break;
		case 100: p2->dirx = -1; break;
		case 1000: p2->dirx = 1; break;
		case 101: p1->dirx = -1; p2->dirx = -1; break;
		case 1001: p1->dirx = -1; p2->dirx = 1; break;
		case 110: p1->dirx = 1; p2->dirx = -1; break;
		case 1010: p1->dirx = 1; p2->dirx = 1; break;
		default: p2->dirx = 0; p1->dirx = 0;
	}
	
	GPIO_D.odrHigh = 0x00;
	return 0;
}

void main(void){
	char c;
	int i, j, k;
	
	k = 0;
	for(i = 0; i < 5; i++){
		for(j = 0; j < 4; j++){
			box[k].posx = i * 11 + 2;
			box[k].posy = j * 4;
			k++;
		}
	}
	for(i = 0; i < 5; i++){
		for(j = 0; j < 4; j++){
			box[k].posx = i * 11 + 70;
			box[k].posy = j * 4;
			k++;
		}
	}
	
	init_app();
	graphic_initialize();
	ascii_init();
	
	#ifndef SIMULATOR
		graphic_clear_screen();
	#endif
	
	ascii_ctrl_bit_set(2);
	ascii_gotoxy(1, 1);
	
	char *s;
	char test1[] = "Break Out!";
	char test2[] = "0-Player 1 | Player 2-0";
	
	s = test1;
	while(*s)
		ascii_write_char(*s++);
	ascii_gotoxy(1, 2);
	s = test2;
	while(*s)
		ascii_write_char(*s++);
	
	pixel(10, 10, 1);
	
	load_sprite("box.xbm", box_bits, box_width, box_height);
	load_sprite("box_hit.xbm", box_hit_bits, box_hit_width, box_hit_height);
	load_sprite("player.xbm", player_bits, player_width, player_height);
	load_sprite("wall.xbm", wall_bits, wall_width, wall_height);
	
	#ifndef SIMULATOR
	/* WRITE BORDERS */
	for(i = 0; i < 64; i++){
			pixel(1, i, 1);
			pixel(60, i, 1);
			pixel(69, i, 1);
			pixel(128, i, 1);
	}
	#endif
	
	int count;
	while(1){
		/* CLEAR THEN WRITE */
		#ifndef SIMULATOR
		count++;
		if(count % 4){
			for(i = 0; i < 40; i++){
				draw_sprite("box.xbm", &box[i], 0);
				draw_sprite("box.xbm", &box[i], 1);
			}
		}
		#endif
		
		draw_sprite("player.xbm", p1, 0);
		draw_sprite("player.xbm", p2, 0);
		
		keyb(); /* Read keypad */
		
		sprite_move(p1, 1);
		sprite_move(p2, 2);
		
		draw_sprite("player.xbm", p1, 1);
		draw_sprite("player.xbm", p2, 1);
		
		//delay_milli(20);
	}
	
	return;
}