// ode

/*
 * 	startup.c
 *
 */
 
typedef unsigned char uint8_t;
typedef unsigned int uint32_t;
 
#define GPIO_E 0x40021000
#define GPIO_MODER ((volatile uint32_t *) (GPIO_E))
#define GPIO_OTYPER ((volatile unsigned short *) (GPIO_E+0x4))
#define GPIO_OSPEEDR ((volatile uint32_t *) (GPIO_E+0x8))
#define GPIO_PUPDR ((volatile uint32_t *) (GPIO_E+0xC))
#define GPIO_IDR_LOW ((volatile uint8_t *) (GPIO_E+0x10))
#define GPIO_IDR_HIGH ((volatile uint8_t *) (GPIO_E+0x11))
#define GPIO_ODR_LOW ((volatile uint8_t *) (GPIO_E+0x14))
#define GPIO_ODR_HIGH ((volatile uint8_t *) (GPIO_E+0x15))

#define STK_CTRL ((volatile int *) 0xE000E010)
#define STK_LOAD ((volatile int *) 0xE000E014)
#define STK_VAL ((volatile int *) 0xE000E018)

#define B_E 0x40 //Enable
#define B_RST 0x20 //Reset
#define B_CS2 0x10 //CS2 - Skärmdel till höger
#define B_CS1 0x8 // CS1 - Skärmdel till vänster
#define B_SELECT 4 // 0 Graphics, 1 Ascii
#define B_RW 2 // 0 Write, 1 Read
#define B_RS 1 // 0 Command, 1 Data

#define LCD_ON 0x3F // Display on
#define LCD_OFF 0x3E // Display off
#define LCD_SET_ADD 0x40 // Set horizontal coordinate
#define LCD_SET_PAGE 0xB8 // Set vertical coordinate
#define LCD_DISP_START 0xC0 // Start address
#define LCD_BUSY 0x80 // Read busy status

#define SIMULATOR



typedef struct tpoint{
    uint8_t x;
    uint8_t y;
}POINT;

#define MAX_POINTS 20

typedef struct tGeometry{
    uint32_t numpoints;
    uint32_t sizex;
    uint32_t sixey;
    POINT px[MAX_POINTS];
}GEOMETRY, *PGEOMETRY;

typedef struct tObj{
    PGEOMETRY geo;
    signed int dirx;
    signed int diry;
    signed int posx;
    signed int posy;
    void (* draw)(struct tObj*);
    void (* clear)(struct tObj*);
    void (* move)(struct tObj*);
    void (* set_speed)(struct tObj*, signed int, signed int);
}OBJECT, *POBJECT;
 
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 */
	) ;
}

//------------ObjectFunktioner----//

void set_object_speed(POBJECT o, uint32_t speedx, uint32_t speedy){
    o->dirx = speedx;
    o->diry = speedy;
}

void draw_object (POBJECT object){
    for(uint8_t p = 0; p < object->geo->numpoints; p++){
        POINT centerPoint = object->geo->px[p];
        pixel((object->posx + centerPoint.x),(object->posy + centerPoint.y), 1);
    }
}

void clear_object (POBJECT object){
    for(uint8_t p = 0; p < object->geo->numpoints; p++){
        POINT centerPoint = object->geo->px[p];
        pixel((object->posx + centerPoint.x),(object->posy + centerPoint.y), 0);
    }
}

void move_object (POBJECT object){
    clear_object(object);
    object->posx += object->dirx;
    object->posy += object->diry;
    if(object->posx < 1){
        object->posx = 1;
        object->dirx *= -1; 
    }
    if(object->posx > 128){
        object->posx = 127;
        object->dirx *= -1; 
    }
    if(object->posy < 1){
        object->posy = 1;
        object->diry *= -1; 
    }
    if(object->posy > 64){
        object->posy = 63;
        object->diry *= -1; 
    }
    draw_object(object);
}

//------------Drivrutiner---------//

void graphic_ctrl_bit_set(uint8_t x){
    uint8_t c;
    c = *GPIO_ODR_LOW;
    c|=(~B_SELECT & x);
    *GPIO_ODR_LOW = c;
}

void graphic_ctrl_bit_clear(uint8_t x){
    uint8_t c;
    c = *GPIO_ODR_LOW;
    c &= (~B_SELECT & ~x);
    *GPIO_ODR_LOW = 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){
    uint32_t c;
    graphic_ctrl_bit_clear(B_E);
    c = 0x00005555;
    *GPIO_MODER = c;
    graphic_ctrl_bit_clear(B_RS);
    graphic_ctrl_bit_set(B_RW);
    delay_500();
    do{
        graphic_ctrl_bit_set(B_E);
        delay_500();
        c = *GPIO_IDR_HIGH & LCD_BUSY;
        graphic_ctrl_bit_clear(B_E);
        delay_500();
    }while (c);
    graphic_ctrl_bit_set(B_E);
    c = 0x55555555;
    *GPIO_MODER = c;    
}

uint8_t graphic_read (uint8_t controller){
    graphic_ctrl_bit_clear(B_E);
    uint32_t c = 0x00005555;
    *GPIO_MODER = c;
    graphic_ctrl_bit_set(B_RS);
    graphic_ctrl_bit_set(B_RW);
    select_controller(controller);
    delay_500();
    graphic_ctrl_bit_set(B_E);
    delay_500();
    uint32_t rv = *GPIO_IDR_HIGH;
    graphic_ctrl_bit_clear(B_E);
    c = 0x55555555;
    *GPIO_MODER = c;
    help_select_controller(controller);
    return rv;
}

void graphic_write(uint8_t value, uint8_t controller){
    //uint32_t c = *GPIO_MODER;
    *GPIO_ODR_HIGH = value;
    select_controller(controller);
    delay_500();
    graphic_ctrl_bit_set(B_E);
    delay_500();
    graphic_ctrl_bit_clear(B_E);
    
    help_select_controller(controller);
    
    *GPIO_ODR_HIGH = value;
    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(uint8_t controller){
    graphic_read(controller);
    return graphic_read(controller);
}

void help_select_controller(uint8_t controller){
    if(controller == B_CS1){
        select_controller(B_CS1);
        graphic_wait_ready();
    }
    if (controller == B_CS2){
        select_controller(B_CS2);
        graphic_wait_ready();
    }
}

// ------------------INIT-------------//

void app_init(void){
//Display Data ut E15-8
*GPIO_MODER = (*GPIO_MODER & 0) | 0x55555555;
//Styrregister ut E7-0
}

void graphic_initalize(void){
    graphic_ctrl_bit_set(B_E);
    delay_micro(10);
    graphic_ctrl_bit_clear(B_CS1);
    graphic_ctrl_bit_clear(B_CS2);
    graphic_ctrl_bit_clear(B_RST);
    graphic_ctrl_bit_clear(B_E);
    delay_milli(30);
    graphic_ctrl_bit_set(B_RST);
    //Utelämna vid simulator?
    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 (uint8_t page = 0; page < 8; page++){
        graphic_write_command(LCD_SET_PAGE | page, B_CS1|B_CS2);
        graphic_write_command(LCD_SET_ADD| 0, B_CS1|B_CS2);
        for (uint8_t add = 0; add < 64; add++){
            graphic_write_data(0, B_CS1|B_CS2);
        }
    }
}

void pixel (uint8_t x, uint8_t y, uint8_t set){
    if(x >= 1 && x <= 128 && y >= 1 && y <= 64){}else{return;}
    uint8_t index = (y-1)/8;
    uint8_t mask = 1<<((y-1)%8);
    if (set == 0){
        mask = ~mask;
    }
    uint8_t controller = B_CS1;
    uint8_t x_fysisk = x-1;
    if(x>64){
        controller = B_CS2;
        x_fysisk = x-65;
    }
    graphic_write_command(LCD_SET_ADD | x_fysisk, controller);
    graphic_write_command(LCD_SET_PAGE | index, controller);
    uint8_t temp = graphic_read_data(controller);
    graphic_write_command(LCD_SET_ADD | x_fysisk, controller);
    if(set){
        mask |= temp;
    }else{
        mask &= temp;
    }
    graphic_write_data(mask, controller);
}

// ------------------DELAY------------//
 void delay_500(void){     
    #ifndef SIMULATOR
        delay_250();
        delay_250();
    #endif      
 }
void delay_250(void){
    *STK_CTRL = 0; //Återställer räknaren
    *STK_LOAD = (168/4)-1;
    *STK_VAL = 0;
    *STK_CTRL = 5; //Starta räknaren
    while ((*STK_CTRL & 0x10000) == 0){
        }
    *STK_CTRL = 0;
}

void delay_micro(uint32_t us){
    us *=4;
    while (us>0){
        delay_250();
        us--;
    }
}

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

void main(void)
{
    uint8_t i;
    app_init();
    graphic_initalize();
    #ifndef SIMULATOR
        graphic_clear_screen();
    #endif
    
    GEOMETRY ball_geometry = {
        12,
        4,4,
        {
        {0,1}, {0,2}, {1,0}, {1,1}, {1,2}, {1,3}, {2,0}, {2,1}, {2,2}, {2,3}, {3,2}
        }
    };
    PGEOMETRY pball_geometry = &ball_geometry;
    
    OBJECT ball =
    {
        pball_geometry,
        0,0,
        1,1,
        draw_object,
        clear_object,
        move_object,
        set_object_speed
    };
    POBJECT p = &ball;
    p->set_speed(p, 0, 4);
    while(1){
        p->move(p);
        delay_milli(10);
    }
    
    /*
    graphic_write_command(LCD_SET_ADD | 10, B_CS1|B_CS2);
    graphic_write_command(LCD_SET_PAGE | 1, B_CS1|B_CS2);
    graphic_write_data(0xFF, B_CS1|B_CS2);
     */
     
     /*
     for (i = 1; i <= 128; i++){
         pixel(i, 10, 1);
     }
     for(i = 1; i <= 64; i++){
         pixel(10, i, 1);
     }
     //delay_milli(500);
     for (i = 1; i <= 128; i++){
         pixel(i, 10, 0);
     }
     for(i = 1; i <= 64; i++){
         pixel(10, i, 0);
     }
     */
}