/*
 * 	startup.c
 *
 */

 #import "delay.h"

 #define GPIO_E	0x40021000
 #define GPIO_E_MODER ((volatile unsigned int *) GPIO_E)
 #define GPIO_E_OTYPER ((volatile unsigned short *) GPIO_E + 0x4)
 #define GPIO_E_OSPEEDR ((volatile unsigned int *) GPIO_E + 0x8)
 #define GPIO_E_PUPDR ((volatile unsigned int *) GPIO_E + 0xC)
 #define GPIO_E_IDR_LOW ((volatile unsigned char*) GPIO_E + 0x10)
 #define GPIO_E_IDR_HIGH ((volatile unsigned char*) GPIO_E + 0x11)
 #define GPIO_E_ODR_LOW ((volatile unsigned char*) GPIO_E + 0x14)
 #define GPIO_E_ODR_HIGH ((volatile unsigned char*) GPIO_E + 0x15)
 #define B_E      0b01000000  //Flagga för att indikera arbetscykel (t.ex. läsa / skriva), 1 om den inleds, 0 om den är klar
 #define B_SELECT 0b00000100
 #define B_RW     0b00000010
 #define B_RS	  0b00000001
 #define B_CS1    0b00001000
 #define B_CS2    0b00010000
 #define B_RESET  0b00100000
 #define LCD_ON   0b00111111
 #define LCD_OFF  0b00111110
 #define LCD_SET_ADD    0b01000000
 #define LCD_SET_PAGE   0b10111000
 #define LCD_DISP_START 0b11000000
 #define LCD_BUSY       0b10000000

 typedef unsigned char uint_8t;

/* Välj grafik-display och ettställ de bitar som är 1 i x */
void graphic_ctrl_bit_set( uint_8t x )
{
	*GPIO_E_ODR_LOW |= ( (~B_SELECT) & x);
}

/* Välj grafik-display och nollställ de bitar som är 1 i x */
void graphic_ctrl_bit_clear( uint_8t x )
{
	*GPIO_E_ODR_LOW &= ( (~B_SELECT) & ~x);
}

void select_controller( uint_8t controller )
{
	switch( controller )
	{
		case 0:
			graphic_ctrl_bit_clear( B_CS1|B_CS2 ); //Clear both CS1 and 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 ); //Set both CS1 and CS2 flag-bits
			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(1) //while LCD is busy
	{
		graphic_ctrl_bit_set( B_E );
		delay_500ns();
		graphic_ctrl_bit_clear( B_E );
		delay_500ns();

		if( (*GPIO_E_IDR_HIGH & LCD_BUSY) == 0 ) //Break out of loop (to continue with program) when LCD_BUSY busy-flag is set to false (not busy)
		{
			break;
		}
	}
	graphic_ctrl_bit_set( B_E );
	*GPIO_E_MODER = 0x55555555;
}

uint_8t graphic_read( uint_8t controller)
{
	graphic_ctrl_bit_clear( B_E );
	*GPIO_E_MODER = 0x00005555;
	graphic_ctrl_bit_set( B_RS|B_RW ); //Set both RS and RW flags
	select_controller( controller );
	delay_500ns();
	graphic_ctrl_bit_set( B_E );
	delay_500ns();
	uint_8t rv = *GPIO_E_IDR_HIGH;
	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 rv;
}

void graphic_write(uint_8t value, uint_8t controller)
{
	*GPIO_E_ODR_HIGH = 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();
	}
	if( controller & B_CS2 )
	{
		select_controller( B_CS2 );
		graphic_wait_ready();
	}

	*GPIO_E_ODR_HIGH = 0;
	graphic_ctrl_bit_set( B_E );
	select_controller( 0 );
}

void graphic_write_command(uint_8t command, uint_8t controller)
{
	graphic_ctrl_bit_clear( B_E );
	select_controller( controller ); //Väljer CS_1, CS_2, båda eller ingen
	graphic_ctrl_bit_clear( B_RS|B_RW );
	graphic_write( command, controller );
}

void graphic_write_data(uint_8t data, uint_8t controller)
{
	graphic_ctrl_bit_clear( B_E );
	select_controller( controller ); //Väljer CS_1, CS_2, båda eller igen
	graphic_ctrl_bit_set( B_RS );
	graphic_ctrl_bit_clear( B_RW );
	graphic_write( data, controller );
}

uint_8t graphic_read_data(uint_8t controller)
{
	(void) graphic_read( controller ); //Returnerar nonsens
	return graphic_read( controller ); //Returnerar korrekt data
}


void graphic_clear_screen( void )
{
	for( char 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( char addr = 0; addr <= 63; addr++ )
		{
			graphic_write_data( 0, B_CS1|B_CS2 );
		}
	}
}

void pixel( unsigned x, unsigned y, unsigned set )
{
	unsigned x_fysisk = 0;
	unsigned index = (y - 1) / 8; //Calculate modulo 8 for the pixel
	uint_8t controller = 0;
	uint_8t bitMask = 0;

	if(x > 128 || y > 64)
	{
		return; //Invalid pixel coords
	}

	//Skapa bitmask för pixeln (man kan tända / släcka 8 st pixlar på följd, måste ställa in så endast en tänds/släcks på rätt koordinater.
	switch( (y - 1) % 8)
	{
		case 0:
			bitMask = 1;
			break;
		case 1:
			bitMask = 2;
			break;
		case 2:
			bitMask = 4;
			break;
		case 3:
			bitMask = 8;
			break;
		case 4:
			bitMask = 0x10;
			break;
		case 5:
			bitMask = 0x20;
			break;
		case 6:
			bitMask = 0x40;
			break;
		case 7:
			bitMask = 0x80;
			break;
	}

	if( set == 0)
	{
		bitMask = ~bitMask;
	}

	//Bestäm fysiska koordinater och välj styrkrets
	if( x > 64)
	{
		controller = B_CS2;
		x_fysisk = x - 65;
	}
	else
	{
		controller = B_CS1;
		x_fysisk = x - 1;
	}

	graphic_write_command( LCD_SET_ADD  | x_fysisk, controller );
	graphic_write_command( LCD_SET_PAGE | index, controller );
	uint_8t temp = graphic_read_data( controller );
	graphic_write_command( LCD_SET_ADD | x_fysisk, controller );

	if( set == 1 )
	{
		bitMask |= temp;
	}
	else
	{
		bitMask &= temp;
	}

	graphic_write_data( bitMask, controller );
}

void init_app( void )
{
	*GPIO_E_MODER = 0x55555555; //Utgång
}

void graphic_initialize( void )
{
	graphic_ctrl_bit_set( B_E );
	delay_mikro(10);
	graphic_ctrl_bit_clear( B_CS1|B_CS2|B_RS|B_E );
	delay_milli(30);
	graphic_ctrl_bit_set( B_RS );
	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 ); //Toggle display
	graphic_write_command( LCD_SET_ADD, B_CS1|B_CS2 );  //Address = 0 (Y)
	graphic_write_command( LCD_SET_PAGE, B_CS1|B_CS2 ); //Page = 0 (X)
	select_controller(0); //deaktivera båda CS-signalerna
}

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 main( void )
{
	unsigned i;
	init_app();
	graphic_initialize();
#ifndef SIMULATOR
	graphic_clear_screen();
#endif
	for( i = 0; i < 128; i++ ) //Rita horisontell linje
	{
		pixel( i, 10, 1 );
	}
	for( i = 0; i < 64; i++ ) //Rita vertikal linje
	{
		pixel( 10, i, 1 );
	}
	delay_milli( 500 );        //Vänta 0,5 sekunder
	for( i = 0; i < 128; i++ ) //Sudda horisontella linjen
	{
		pixel( i, 10, 0 );
	}
	for( i = 0; i < 64; i++ )  //Sudda vertikala linjen
	{
		pixel ( 10, i , 0 );
	}
	/*
	graphic_write_command( LCD_SET_ADD  | 40, B_CS1|B_CS2 ); //Address = x coord
	graphic_write_command( LCD_SET_PAGE | 40, B_CS1|B_CS2 ); //Page = y coord
	graphic_write_data( 0b11111111, B_CS1|B_CS2 ); //Varje bit i mönstret indikerar om motsvarande bit på skärmen ska tändas eller inte.
	                                               //Bit 0 kommer tända pixel (40,40), bit 1 kommer släcka pixel (40,41) osv (inställd på att öka i y-led)
	*/
}