/**
Program to create a car monitor display using:
    - Arduino Due
    - a TFT display (https://www.tinytronics.nl/shop/en/displays/tft/3.5-inch-tft-display-320*480-pixels-mega-compatible-ili9486)
    - ELM327 Bluetooth OBD2 scanner
    - FSC-DB004 (BT 836B) bluetooth module
*/

#include "Arduino.h"
/* include UTFT library */
#include <UTFT.h>
#include <DueTimer.h>
// #include "obd2_timer.h"
#include "obd2_display.h"
#include "statemachine.h"
#include "obd2_util.h"
#include "obd2_elm327.h"

#define DEBUG 1
// #define RTT_MR 0x400E1A30U

#define MS(x) x * 1000
#define S(x) MS(x) * 1000

#define INIT_TEXT_WIDTH 15
#define INIT_PERCENTAGE_WIDTH 4
#define FLAG_INIT_UPDATE_TEXT_POS 0x01
#define FLAG_INIT_CLEAR_POS 0x02
#define FLAG_INIT_UPDATE_PERCENT_POS 0x03
#define FLAG_DEVICE_LABEL_UPDATE_POS 0x04
#define FLAG_DEVICE_LABEL_SHOULD_UPDATE_POS 0x05 /* 0 if label should be updated, 1 if valyue should be updated */
#define FLAG_DEVICE_LABEL_UPDATE_KB_POS 0x06     /* count KB up */

#define LOGO_MIN_STEP 20
#define LOGO_MAX_POS_Y 124
#define FLAG_LOGO_UPDATE 0x00

#define LOGO_TEXT_WIDTH 16

#define RAM_AMOUNT_KB 96
#define RAM_AMOUNT_B 98304
#define RAM_TEXT_WIDTH 9
#define DEV_LABEL_LENGTH 10

#define COLOR_ORANGE 255, 96, 33

enum DeviceLabel
{
    DEV_CPU = 0,
    DEV_RAM = 1,
    DEV_DISPLAY = 2,
    DEV_DONE = 3
};

/* extern fonts */
extern uint8_t BigFont[];
extern uint8_t SmallFont[];
extern uint8_t OCR_A_Extended_M[];

/* display strings */
char init_text[] PROGMEM = "Initialising...";
char device_labels[3][11] PROGMEM = {"CPU     : ", "RAM     : ", "DISPLAY : "};
char cpu_text[] PROGMEM = "ATSAM3X8E";
char display_size_text[] PROGMEM = "480x320";

/* initialising... variables */
char should_clear = 1;
char led_state = LOW;
char init_flag = 0;
char init_text_i = -1;
int init_text_x = 100;
char text_temp[2] = {'a', '\0'};
int init_percent = 0;

/* logo variables */
char logo_text[] PROGMEM = "DS3 OBD2 display";
int logo_pos_y = 0;
int last_logo_pos_y = 0;
int logo_pos_i = 0;
char logo_flag = 0;

/* device info variables */
char init_device_info = 0; /* wether we are drawing CPU (0), RAM (1) or DISPLAY (2) */
uint32_t ram_b_amount = 0; /* counts up to 96 to show the KB of ram */
char device_label_i = -1;  /* counter in the current device label */

char bt_state = BT_INITIALISING;

obd2_elm327_t elm327;

void on_init_enter();
void on_init_run();
void on_init_exit();
void on_main_enter();
void on_main_run();
void __state_none()
{ /* do nothing */
}

state_t init_state =
    {
        .id = STATE_INIT,
        .next = STATE_INIT,
        .on_enter = &on_init_enter,
        .on_run = &on_init_run,
        .on_exit = &on_init_exit};

state_t main_state =
    {
        .id = STATE_CAR_INFO,
        .next = STATE_CAR_INFO,
        .on_enter = &on_main_enter,
        .on_run = &on_main_run,
        .on_exit = &__state_none};

/* Set the pins for the display and dev board                                */
/* Standard Arduino Mega/Due shield            : <display model>,38,39,40,41 */
UTFT display(ILI9486, 38, 39, 40, 41);

/****************************/
/*INIT STATE*/
/****************************/

void on_init_enter()
{
    display.clrScr();

    Timer0.attachInterrupt(update_init_text);
    Timer0.start(MS(200));

    Timer2.attachInterrupt(update_device_info);
    Timer2.start(MS(100));

    // TODO change to update when initializing bluetooth..
    Timer1.attachInterrupt(update_percent_test);
    Timer1.start(MS(60));

    Timer3.attachInterrupt(update_ram_kb);

    logo_flag |= (1 << FLAG_LOGO_UPDATE);
#if (DEBUG == 1)
    Serial.println("Entering init state!");
#endif
}

void on_init_run()
{
    uint16_t initialization_y = display.getDisplayYSize() / 2 + 50;

    /* clear initialization text if necessary*/
    if (init_flag & (1 << FLAG_INIT_CLEAR_POS))
    {
        init_flag &= ~(1 << FLAG_INIT_CLEAR_POS);
        display.setColor(VGA_BLACK);
        /*clear only initializing text*/
        display.print("               ", (display.getDisplayXSize() / 2) - (INIT_TEXT_WIDTH * display.getFontXsize() / 2) - (INIT_PERCENTAGE_WIDTH / 2), display.getDisplayYSize() / 2 + 50);
    }
    /* update initialization text */
    if (init_flag & (1 << FLAG_INIT_UPDATE_TEXT_POS))
    {
        init_flag &= ~(1 << FLAG_INIT_UPDATE_TEXT_POS);
        display.setColor(VGA_AQUA);
        int x_position = (display.getDisplayXSize() / 2 - (INIT_TEXT_WIDTH * display.getFontXsize() / 2)) + (init_text_i * display.getFontXsize()) - (INIT_PERCENTAGE_WIDTH / 2);
        // Serial.println(x_position);
        text_temp[0] = init_text[init_text_i];
        display.print(text_temp, x_position, initialization_y); // print as string with one character
    }

    /* update initialization percentage */
    if (init_flag & (1 << FLAG_INIT_UPDATE_PERCENT_POS))
    {
        init_flag &= ~(1 << FLAG_INIT_UPDATE_PERCENT_POS);
        int percent_x_pos = (display.getDisplayXSize() / 2 - (INIT_TEXT_WIDTH * display.getFontXsize() / 2)) + (INIT_TEXT_WIDTH * display.getFontXsize()) - (INIT_PERCENTAGE_WIDTH / 2);
        display.setBackColor(VGA_BLACK);

        display.setColor(VGA_FUCHSIA);
        char *percent_text = (char *)malloc((INIT_PERCENTAGE_WIDTH - 1) * sizeof(char));
        sprintf(percent_text, "%d", init_percent);
        display.print(percent_text, percent_x_pos, initialization_y);
        display.print("%", percent_x_pos + (INIT_PERCENTAGE_WIDTH - 1) * display.getFontXsize(), initialization_y);
        free(percent_text);
    }

    /* update initialization progress bar */
    display.setColor(COLOR_ORANGE);
    display.fillRect(0, initialization_y + display.getFontYsize() + 3, ((float)init_percent / 100.0) * display.getDisplayXSize(), initialization_y + display.getFontYsize() + 13);

    /* update bluetooth state */
    display.setBackColor(VGA_FUCHSIA);
    display.setColor(VGA_BLACK);
    display.print("Bluetooth ", (display.getDisplayXSize() / 2) - (11 * display.getFontXsize()), initialization_y + 50);

    char current_bt_state[BT_STATE_LENGTH];
    obd2_elm327_get_state(&elm327, (char *)current_bt_state);
    display.print(current_bt_state, (display.getDisplayXSize() / 2) + display.getFontXsize(), initialization_y + 50);
    display.setBackColor(VGA_BLACK);

    /* update device labels and values (CPU, RAM, DISPLAY) */
    if (init_flag & (1 << FLAG_DEVICE_LABEL_UPDATE_POS))
    {
        if (init_flag & (1 << FLAG_DEVICE_LABEL_SHOULD_UPDATE_POS))
        {
            /* draw device label values*/
            init_flag &= ~(1 << FLAG_DEVICE_LABEL_UPDATE_POS);
            display.setColor(VGA_FUCHSIA);
            display.setBackColor(VGA_BLACK);

            int x_offset = 10 + 10 * display.getFontXsize();
            switch (init_device_info)
            {
            case DEV_CPU:
            {
                text_temp[0] = cpu_text[device_label_i];
                int x_position = x_offset + (device_label_i * display.getFontXsize());
                display.print(text_temp, x_position, 10 + (init_device_info * display.getFontYsize() + 3));
                break;
            }
            case DEV_RAM:
            {
                char *ram_text = (char *)malloc((RAM_TEXT_WIDTH) * sizeof(char));
                sprintf(ram_text, "%d B", ram_b_amount);
                display.print(ram_text, x_offset, 10 + (init_device_info * display.getFontYsize() + 3));
                free(ram_text);
                break;
            }
            case DEV_DISPLAY:
            {
                text_temp[0] = display_size_text[device_label_i];
                int x_position = x_offset + (device_label_i * display.getFontXsize());
                display.print(text_temp, x_position, 10 + (init_device_info * display.getFontYsize() + 3));
                break;
            }
            default:
                break;
            }
        }
        else
        {
            /* draw device labels */
            init_flag &= ~(1 << FLAG_DEVICE_LABEL_UPDATE_POS);
            text_temp[0] = device_labels[init_device_info][device_label_i];
            display.setColor(device_label_i == 8 ? VGA_GRAY : VGA_AQUA); /* make the ":" gray */
            int x_position = 10 + (device_label_i * display.getFontXsize());
            display.print(text_temp, x_position, 10 + (init_device_info * display.getFontYsize() + 3));
        }
    }

    logo_pos_i += 3;
    last_logo_pos_y = logo_pos_y;
    logo_pos_y = logo_step(logo_pos_i);

    if (logo_flag & (1 << FLAG_LOGO_UPDATE))
    {
        if (logo_pos_y >= LOGO_MAX_POS_Y)
        {
            logo_flag &= ~(1 << FLAG_LOGO_UPDATE);
        }
        if (logo_pos_i > LOGO_MIN_STEP)
        {
            display.setColor(VGA_BLACK);
            display.print(logo_text, CENTER, last_logo_pos_y);
            display.setColor(VGA_AQUA);
            display.print(logo_text, CENTER, logo_pos_y);
        }
    }
}

void on_init_exit()
{
#if (DEBUG == 1)
    Serial.println("Exiting init state!");
#endif
    display.clrScr();
    Timer0.detachInterrupt();
    Timer0.stop();
    Timer2.detachInterrupt();
    Timer2.stop();
}

/****************************/
/*MAIN STATE*/
/****************************/

void on_main_enter()
{
#if (DEBUG == 1)
    Serial.println("Entering main loop");
#endif
}

void on_main_run()
{
    delay(500);
#if (DEBUG == 1)
    Serial.println("main!");
#endif
}

void update_init_text()
{
    init_text_i++;
    if (init_text_i == 15)
    {
        init_text_i = 0;
        init_flag |= (1 << FLAG_INIT_CLEAR_POS);
    }
    init_flag |= (1 << FLAG_INIT_UPDATE_TEXT_POS);
}

void update_percent_test()
{
    init_percent++;
    if (init_percent > 100)
    {
        init_percent = 0;
    }
    init_flag |= (1 << FLAG_INIT_UPDATE_PERCENT_POS);

    /* count KB up */
    if (init_device_info == DEV_RAM && (0 == (init_flag & (1 << FLAG_DEVICE_LABEL_SHOULD_UPDATE_POS))))
    {
    }
}

void update_device_info()
{
    if (init_flag & (1 << FLAG_DEVICE_LABEL_SHOULD_UPDATE_POS))
    {
        /* update device label values */
        switch (init_device_info)
        {
        case DEV_CPU:
            device_label_i++;
            if (device_label_i >= SIZE_OF(cpu_text))
            {
                init_device_info = DEV_RAM;
                device_label_i = 0;
                Timer3.start(50);
#if (DEBUG == 1)
                Serial.println("DEV RAM");
#endif
            }
            break;
        case DEV_RAM:
            if (ram_b_amount >= RAM_AMOUNT_B)
            {
                Timer3.detachInterrupt();
                Timer3.stop();
                init_device_info = DEV_DISPLAY;
                device_label_i = 0;
            }
            break;
        case DEV_DISPLAY:
            device_label_i++;
            if (device_label_i >= SIZE_OF(display_size_text))
            {
                init_device_info = DEV_DONE;
                // init_flag &= ~(1 << FLAG_DEVICE_LABEL_UPDATE_POS);
            }
            break;
        }
        init_flag |= (1 << FLAG_DEVICE_LABEL_UPDATE_POS);
    }
    else
    {
        /* update device labels */
        switch (init_device_info)
        {
        case DEV_CPU:
            device_label_i++;
            if (device_label_i > DEV_LABEL_LENGTH)
            {
                init_device_info = DEV_RAM;
                device_label_i = 0;
                // init_flag |= (1 << FLAG_DEVICE_LABEL_SHOULD_UPDATE_POS);
            }
            break;
        case DEV_RAM:
            device_label_i++;
            if (device_label_i > DEV_LABEL_LENGTH)
            {
                init_device_info = DEV_DISPLAY;
                device_label_i = 0;
                // init_flag |= (1 << FLAG_DEVICE_LABEL_SHOULD_UPDATE_POS);
            }
            break;
        case DEV_DISPLAY:
            device_label_i++;
            if (device_label_i > DEV_LABEL_LENGTH)
            {
                device_label_i = 0;
                /* switch to drawing device values */
                init_flag |= (1 << FLAG_DEVICE_LABEL_SHOULD_UPDATE_POS);
                init_device_info = 0;
            }
            break;
        default:
            break;
        }
        init_flag |= (1 << FLAG_DEVICE_LABEL_UPDATE_POS);
    }
}

void update_ram_kb()
{
    if (ram_b_amount < RAM_AMOUNT_B)
    {
        ram_b_amount++;
    }
    init_flag |= (1 << FLAG_DEVICE_LABEL_UPDATE_POS);
}

void setup()

{

    /* TODO change for TRNG (section 42 of datasheet)*/
    randomSeed(analogRead(0));

#if (DEBUG == 1)
    Serial.begin(9600);
#endif

    /* Init display */
    display.InitLCD();
    display.setFont(OCR_A_Extended_M);

#if (DEBUG == 1)
    Serial.println("Starting");
#endif

    pinMode(LED_BUILTIN, OUTPUT);

    statemachine_register_state(&init_state, STATE_INIT);
    statemachine_register_state(&main_state, STATE_CAR_INFO);
    statemachine_init();

    /* Serial1 is bluetooth module, pin 18 and 19. */
    Serial1.begin(BT_BAUD);
    obd2_elm327_init(&elm327);
}

void loop()
{

    statemachine_loop();
    // Serial.println("Running");
    // delay(10);
    //   while (Serial1.available())
    //   {
    //     char rec = Serial1.read();
    // #if (DEBUG == 1)
    //     Serial.println(rec);
    // #endif
    //   }

    //   if (should_clear)
    //   {
    //     display.clrScr();
    //     display.setColor(255, 0, 166);
    //     display.fillRect(LCD_W / 2 - 200, LCD_H / 2 - 100, LCD_W / 2 + 200, LCD_H / 2 + 100);
    //     display.setColor(0, 247, 255);
    //     display.print("OBD2 display yeet", CENTER, LCD_H / 2);
    //     delay(10);
    //     should_clear = 0;
    //   }
}