#include <assert.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <stdio.h>
#include <math.h>

#include "board.h"
#include "driver.h"
#include "clock_face.h"

uint16_t image[240 * 240];

static void bmp_draw_pixel(uint16_t *image, uint16_t x, uint16_t y, uint16_t color)
{
    assert(image != NULL);

    image[x + 240 * y] = color;
}

#ifndef _swap_int16_t
#define _swap_int16_t(a, b) { int16_t t = a; a = b; b = t; }
#endif

static void img_draw_line(uint16_t *image, int16_t x0, int16_t y0, int16_t x1, int16_t y1, uint16_t color)
{
    assert(image != NULL);

    int16_t steep = abs(y1 - y0) > abs(x1 - x0);
    if(steep) {
        _swap_int16_t(x0, y0);
        _swap_int16_t(x1, y1);
    }
    if(x0 > x1) {
        _swap_int16_t(x0, x1);
        _swap_int16_t(y0, y1);
    }

    int16_t dx, dy;
    dx = x1 - x0;
    dy = abs(y1 - y0);

    int16_t err = dx / 2;
    int16_t ystep;

    if(y0 < y1) {
        ystep = 1;
    } else {
        ystep = -1;
    }

    for(; x0 <= x1; x0++) {
        if(steep) {
            bmp_draw_pixel(image, y0, x0, color);
        } else {
            bmp_draw_pixel(image, x0, y0, color);
        }
        err -= dy;
        if (err < 0) {
            y0 += ystep;
            err += dx;
        }
    }
}

static void img_load(uint16_t *dst, uint16_t *src, uint32_t length)
{
    for(uint32_t i = 0; i < length; i++) {
        dst[i] = src[i];
    }
}

static void img_set(uint16_t *dst, uint16_t value, uint32_t length)
{
    for (uint32_t i = 0; i < length; i++) {
        image[i] = value;
    }
}

int main(void)
{
    drv_open(&lcd_drv);

    double min_len = 80;
    double std_len = 60;
    double root_x = 120;
    double root_y = 120;
    double min_angle = 90;
    double std_angle = 90;
    double dest_x;
    double dest_y;

    while(1) {
        img_load(image, clock_face, 240 * 240);

        dest_x = root_x + min_len * cos(min_angle * M_PI / 180);
        dest_y = root_y + min_len * sin(min_angle * M_PI / 180);
        img_draw_line(image, (uint16_t)root_x, (uint16_t)root_y, (uint16_t)dest_x, (uint16_t)dest_y, 0xffff);
        min_angle += 360 / 60;

        dest_x = root_x + std_len * cos(std_angle * M_PI / 180);
        dest_y = root_y + std_len * sin(std_angle * M_PI / 180);
        img_draw_line(image, (uint16_t)root_x, (uint16_t)root_y, (uint16_t)dest_x, (uint16_t)dest_y, 0xffff);
        std_angle += 360.0 / (60 * 12);

        drv_write(&lcd_drv, (const char *)image, 240 * 240);
        sleep(1);

        if(min_angle >= 360.0) {
            min_angle -= 360.0;
        }
        if(std_angle >= 360.0) {
            std_angle -= 360.0;
        }
    }

    drv_close(&lcd_drv);

    return 0;
}