#include <stm32/rcc.h>
#include <stm32/gpio.h>
#include <stm32/afio.h>
#include <stm32/spi.h>
#include "fixed.h"

void delay(unsigned timeout) {
    while(timeout--) asm volatile("nop");
}

#define delay_ms(ms) delay(ms*(8000*5/4)/2/3)

/* PINOUT
 *  PC0 = D/!C
 *  PC1 = !EN
 *  PC2 = !RST
 *  PB3 = CLK
 *  PB5 = MOSI
 */

inline static void lcd_cmd()     { GPIOC->ODR &= ~BIT(0); }
inline static void lcd_data()    { GPIOC->ODR |=  BIT(0); }
inline static void lcd_enable()  { GPIOC->ODR &= ~BIT(1); }
inline static void lcd_disable() { GPIOC->ODR |=  BIT(1); }

inline static void lcd_reset() {
    GPIOC->ODR &= ~BIT(2);
    delay_ms(10);
    GPIOC->ODR |= BIT(2);
}

static void lcd_write(u8 byte) {
    lcd_enable();
    while(!(SPI1->SR & SPI_TXE));
    SPI1->DR = byte;
    while(!(SPI1->SR & SPI_RXNE));
    SPI1->DR;
    lcd_disable();
}

static void lcd_init() {
    RCC->APB2ENR = IOPCEN | IOPBEN | SPI1EN | AFIOEN;
    AFIO->MAPR = SWJ_CFG(SWJ_CFG_SW) | SPI1_REMAP;
    GPIOC->CRL = 0x44444111; // PC[0-2] as outputs
    GPIOB->CRL = 0x44949444; // PB3, PB5 as AF outputs

    SPI1->CR1 = SPI_MSTR | SPI_BR(0) | SPI_SSM | SPI_SSI | SPI_SPE;

    lcd_reset();

    lcd_cmd();
    lcd_write(0x21); // !PD=ChipActive H=ExtendedCmd
    lcd_write(0xe0); // set value of Vop (controls contrast) = 0x80 | 0x60 (arbitrary)
    lcd_write(0x04); // set temperature coefficient
    lcd_write(0x14); // set bias mode to 1:48.
    lcd_write(0x22); // !PD=ChipActive !H=NormalCmd
    lcd_write(0x0c); // D=NormalMode
}

static void lcd_refresh(u8* buffer) {
    lcd_cmd();
    lcd_write(0x40);
    lcd_write(0x80);

    lcd_data();
    for(int i = 0; i < 6 * 84; i++)
        lcd_write(buffer[i]);
}

static u8 framebuffer[6*84];
static u32* const frameband = (u32*)(0x2000000 + ((u8*)framebuffer));
#define W 48

#define abs(f) ((f)>=0?(f):-(f))
#define sq(f) ((f)*(f))

static void draw_ball(u8 x, u8 y) {
    for(s8 dx = -8; dx <= 8; dx++)
        for(s8 dy = -8; dy <= 8; dy++)
            if(sq(dy)+sq(dx)<=sq(4)) {
                if(y+dy >= 0 && y+dy < 84 && x+dx >= 0 && x+dx < 48)
                    frameband[((int)(y+dy))*W+((int)(x+dx))] = 1;
            }
}

s16 rand;

void main() {
    RCC->CR = HSEON;
    while((RCC->CR & HSERDY) == 0);

    W_SW(RCC->CFGR, SW_HSE);
    while(R_SW(RCC->CFGR) != SW_HSE);

    lcd_init();

    GPIOC->CRH = 0x44444411;

    RCC->APB2ENR |= IOPAEN;
    GPIOA->CRL = 0x44444448;

    fixed bx = f(50, 0), by = f(5, 0);
    fixed vx = f(1, 0), vy = f(2, 0);
    fixed g  = f(0, 0.1);

    while(1) {
        for(int i = 0; i < 6*84; i++)
            framebuffer[i] = 0;

        //vy = f_add(vy, g);

        bx = f_add(bx, vx);
        //by = f_add(by, vy);

        int coll = 0;

/*
#define BOUND(p, d, v, m) \
        if(f_i(p) - d <= 0 && f_i(v) < 0) { \
            v = f_neg(v); \
            v = f_mul(v, f(0, 0.7)); \
            p = f(d, 0); \
            coll = 1; \
        } \
        if(f_i(p) + d >= m && f_i(v) > 0) { \
            v = f_neg(v); \
            v = f_mul(v, f(0, 0.7)); \
            p = f(m - d, 0); \
            while(1); \
        }
*/

        volatile int k = f_i(bx);

        if(f_i(bx) - 4 <= 0 && f_i(vx) < 0) {
            vx = f_neg(vx);
            vx = f_mul(vx, f(0, 0.7));
        }

        //BOUND(by, 4, vy, 48);
//      BOUND(bx, 4, vx, 84);

        draw_ball(f_i(by), f_i(bx));

        lcd_refresh(framebuffer);
        if(coll) GPIOC->ODR |= BIT(9);
        delay_ms(100);
        GPIOC->ODR &= ~BIT(9);
    }
}