/**
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

    linux arduino library:
    /home/sem/.arduino15/packages/arduino/hardware/sam/1.6.12/system/CMSIS/Device/ATMEL/sam3xa/include
*/

#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"
#include "bars.h"
#include "defines.h"

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;
char update_slow = 0;

obd2_elm327_t elm327;

uint16_t last_coolant_temp = 0;
uint16_t last_intake_air_temp = 0;
uint16_t last_ambient_air_temp = 0;
uint16_t last_engine_oil_temp = 0;

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_CAR_INFO,
        .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);

void update_percent(uint8_t amount)
{
    init_percent += amount;
    init_flag |= (1 << FLAG_INIT_UPDATE_PERCENT_POS);
}

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

void on_init_enter()
{
#if (DEBUG == 1)
    Serial.println("Entering init state!");
#endif
    display.clrScr();

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

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

    Timer3.attachInterrupt(update_ram_kb);

    logo_flag |= (1 << FLAG_LOGO_UPDATE);
    init_flag |= (1 << FLAG_INIT_UPDATE_PERCENT_POS);
}

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);
        if (init_percent > 90)
        {
            update_percent(8);
        }
    }
    /* 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_LIGHT_GRAY);
    display.fillRect(0, initialization_y + display.getFontYsize() + 3, ((float)init_percent / 100.0) * display.getDisplayXSize(), initialization_y + display.getFontYsize() + 13);

    obd2_elm327_check_connection(&elm327);

    /* 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] = "poep";
    obd2_elm327_get_state(&elm327, 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));
                update_percent(1);
                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));
                update_percent(1);
                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);
            update_percent(10);
        }
        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);
        }
    }

    if (init_percent == 100)
    {
        statemachine_next();
    }
}

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
    Timer0.attachInterrupt(query_slow_obd2_values);
    Timer0.start(MS(OBD2_SLOW_VALUES_QUERY_INTERVAL_MS));

    // draw section for temps
    display.setColor(TEMP_BOX_COLOR);
    display.drawRect(TEMP_BOX_X_START, TEMP_BOX_Y_START, TEMP_BOX_X_START + TEMP_BOX_WIDTH, TEMP_BOX_Y_START + TEMP_BOX_HEIGHT);

    // draw T for title
    display.setBackColor(VGA_TEAL);
    display.setColor(VGA_BLACK);
    display.print("T", TEMP_BOX_X_START + TEMP_BOX_WIDTH - display.getFontXsize(), TEMP_BOX_Y_START);

    // reset colors
    display.setColor(VGA_AQUA);
    display.setBackColor(VGA_BLACK);

    // update slow values so we don't have to wait for the first time the timer fires
    // TODO do this while initializing
    query_slow_obd2_values();
}

/**
 * @brief      Main loop of the main state. Queries the OBD2 scanner for values and updates the display.
 */
void on_main_run()
{
    if (update_slow)
    {
#if (DEBUG == 1)
        Serial.println("Updating slow values");
#endif
        update_slow = 0;
        obd2_elm327_process_slow(&elm327);
    }
    else
    {
        obd2_elm327_process_fast(&elm327);
    }

    if (elm327.value_updates & (1 << UPDATE_COOLANT_TEMP_POS))
    {
        elm327.value_updates &= ~(1 << UPDATE_COOLANT_TEMP_POS);
        display.setColor(VGA_FUCHSIA);
        display.print("coo ", TEMP_BOX_CONTENT_X_START, TEMP_BOX_COOLANT_TEXT_Y_START);
        display.setColor(VGA_AQUA);
        display.printNumI(elm327.engine_coolant_temp, TEMP_BOX_CONTENT_X_START + (4 * display.getFontXsize()), TEMP_BOX_COOLANT_TEXT_Y_START, 4, '0');

        // if the current temp is lower than the last temp (bar dropped) remove part of the drawn bar instead of drawing a new one
        if (last_coolant_temp > elm327.engine_coolant_temp)
        {
            bar_clear_part_horizontal(TEMP_BOX_CONTENT_X_START, TEMP_BOX_COOLANT_BAR_Y_START, TEMP_BOX_BAR_WIDTH, TEMP_BOX_BAR_HEIGHT, elm327.engine_coolant_temp, COOLANT_TEMP_MAX, VGA_BLACK, 1, &display);
        }
        else
        {
            bar_draw_horizontal(TEMP_BOX_CONTENT_X_START, TEMP_BOX_COOLANT_BAR_Y_START, TEMP_BOX_BAR_WIDTH, TEMP_BOX_BAR_HEIGHT, elm327.engine_coolant_temp, COOLANT_TEMP_MAX, COLOR_LIGHT_GRAY, 1, &display);
        }
        last_coolant_temp = elm327.engine_coolant_temp;

#if (DEBUG == 1)
        Serial.print("coolant temp: ");
        Serial.println(elm327.engine_coolant_temp);
#endif // DEBUG
    }

    if (elm327.value_updates & (1 << UPDATE_FUEL_PRESSURE_POS))
    {
        elm327.value_updates &= ~(1 << UPDATE_FUEL_PRESSURE_POS);
        display.print("fuel pressure", 0, 120);
        display.printNumF(elm327.fuel_pressure, 200, 120, 4, '0');
    }

    if (elm327.value_updates & (1 << UPDATE_FUEL_LEVEL_POS))
    {
        elm327.value_updates &= ~(1 << UPDATE_FUEL_LEVEL_POS);
        display.print("fuel level", 0, 140);
        display.printNumF(elm327.fuel_level, 200, 140, 4, '0');
    }

    if (elm327.value_updates & (1 << UPDATE_INTAKE_AIR_TEMP_POS))
    {
        elm327.value_updates &= ~(1 << UPDATE_INTAKE_AIR_TEMP_POS);
        display.setColor(VGA_FUCHSIA);
        display.print("int ", TEMP_BOX_CONTENT_X_START, TEMP_BOX_INTAKE_AIR_TEXT_Y_START);
        display.setColor(VGA_AQUA);
        display.printNumI(elm327.intake_air_temp, TEMP_BOX_CONTENT_X_START + (4 * display.getFontXsize()), TEMP_BOX_INTAKE_AIR_TEXT_Y_START, 4, '0');

        // if the current temp is lower than the last temp (bar dropped) remove part of the drawn bar instead of drawing a new one
        if (last_intake_air_temp > elm327.intake_air_temp)
        {
            bar_clear_part_horizontal(TEMP_BOX_CONTENT_X_START, TEMP_BOX_INTAKE_AIR_BAR_Y_START, TEMP_BOX_BAR_WIDTH, TEMP_BOX_BAR_HEIGHT, elm327.intake_air_temp, INTAKE_AIR_TEMP_MAX, VGA_BLACK, 1, &display);
        }
        else
        {
            bar_draw_horizontal(TEMP_BOX_CONTENT_X_START, TEMP_BOX_INTAKE_AIR_BAR_Y_START, TEMP_BOX_BAR_WIDTH, TEMP_BOX_BAR_HEIGHT, elm327.intake_air_temp, INTAKE_AIR_TEMP_MAX, COLOR_LIGHT_GRAY, 1, &display);
        }
        last_intake_air_temp = elm327.intake_air_temp;

#if (DEBUG == 1)
        Serial.print("intake air temp: ");
        Serial.println(elm327.intake_air_temp);
#endif
    }

    if (elm327.value_updates & (1 << UPDATE_AMBIENT_AIR_TEMP_POS))
    {
        elm327.value_updates &= ~(1 << UPDATE_AMBIENT_AIR_TEMP_POS);
        display.setColor(VGA_FUCHSIA);
        display.print("amb ", TEMP_BOX_CONTENT_X_START, TEMP_BOX_AMBIENT_AIR_TEXT_Y_START);
        display.setColor(VGA_AQUA);
        display.printNumI(elm327.ambient_air_temp, TEMP_BOX_CONTENT_X_START + (4 * display.getFontXsize()), TEMP_BOX_AMBIENT_AIR_TEXT_Y_START, 4, '0');

        // if the current temp is lower than the last temp (bar dropped) remove part of the drawn bar instead of drawing a new one
        if (last_ambient_air_temp > elm327.ambient_air_temp)
        {
            bar_clear_part_horizontal(TEMP_BOX_CONTENT_X_START, TEMP_BOX_AMBIENT_AIR_BAR_Y_START, TEMP_BOX_BAR_WIDTH, TEMP_BOX_BAR_HEIGHT, elm327.ambient_air_temp, AMBIENT_AIR_TEMP_MAX, VGA_BLACK, 1, &display);
        }
        else
        {
            bar_draw_horizontal(TEMP_BOX_CONTENT_X_START, TEMP_BOX_AMBIENT_AIR_BAR_Y_START, TEMP_BOX_BAR_WIDTH, TEMP_BOX_BAR_HEIGHT, elm327.ambient_air_temp, AMBIENT_AIR_TEMP_MAX, COLOR_LIGHT_GRAY, 1, &display);
        }
        last_ambient_air_temp = elm327.ambient_air_temp;

#if (DEBUG == 1)
        Serial.print("ambient air temp: ");
        Serial.println(elm327.ambient_air_temp);
#endif
    }

    if (elm327.value_updates & (1 << UPDATE_OIL_TEMP_POS))
    {
        elm327.value_updates &= ~(1 << UPDATE_OIL_TEMP_POS);
        display.setColor(VGA_FUCHSIA);
        display.print("oil ", TEMP_BOX_CONTENT_X_START, TEMP_BOX_OIL_TEXT_Y_START);
        display.setColor(VGA_AQUA);
        display.printNumI(elm327.engine_oil_temp, TEMP_BOX_CONTENT_X_START + (4 * display.getFontXsize()), TEMP_BOX_OIL_TEXT_Y_START, 4, '0');

        // if the current temp is lower than the last temp (bar dropped) remove part of the drawn bar instead of drawing a new one
        if (last_engine_oil_temp > elm327.engine_oil_temp)
        {
            bar_clear_part_horizontal(TEMP_BOX_CONTENT_X_START, TEMP_BOX_OIL_BAR_Y_START, TEMP_BOX_BAR_WIDTH, TEMP_BOX_BAR_HEIGHT, elm327.engine_oil_temp, OIL_TEMP_MAX, VGA_BLACK, 1, &display);
        }
        else
        {
            bar_draw_horizontal(TEMP_BOX_CONTENT_X_START, TEMP_BOX_OIL_BAR_Y_START, TEMP_BOX_BAR_WIDTH, TEMP_BOX_BAR_HEIGHT, elm327.engine_oil_temp, OIL_TEMP_MAX, COLOR_LIGHT_GRAY, 1, &display);
        }
        
        last_engine_oil_temp = elm327.engine_oil_temp;

#if (DEBUG == 1)
        Serial.print("oil temp: ");
        Serial.println(elm327.engine_oil_temp);
#endif
    }

    if (elm327.value_updates & (1 << UPDATE_ENGINE_LOAD_POS))
    {
        elm327.value_updates &= ~(1 << UPDATE_ENGINE_LOAD_POS);
        int width = (int)(display.getDisplayXSize() * ((float)elm327.engine_load / 100.0));
        display.fillRect(0, 50, width, 50);
        display.setColor(VGA_FUCHSIA);
        display.print("el", 0, 50);
        display.printNumI(elm327.engine_load, 20, 50, 4, '0');
        display.setColor(VGA_AQUA);
    }

    // if (elm327.value_updates & (1 << UPDATE_MANIFOLD_PRESSURE_POS))
    // {
    //     elm327.value_updates &= ~(1 << UPDATE_MANIFOLD_PRESSURE_POS);
    //     display.print("manifold pressure", 0, 240);
    //     display.printNumI(elm327.manifold_pressure, 200, 240, 4, '0');
    // }
}

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_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);
            }
            break;
        case DEV_RAM:
            if (ram_b_amount >= RAM_AMOUNT_B)
            {
                Timer3.detachInterrupt();
                Timer3.stop();
                init_device_info = DEV_DISPLAY;
                device_label_i = 0;
                update_percent(10);
            }
            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);
                update_percent(10);
            }
            break;
        }
        init_flag |= (1 << FLAG_DEVICE_LABEL_UPDATE_POS);
    }
    else
    {
        /* update device labels */
        switch (init_device_info)
        {
        case DEV_CPU:
            device_label_i++;
            update_percent(1);
            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++;
            update_percent(1);
            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++;
            update_percent(1);
            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 bt_state_changed()
{
    update_percent(10);
}

void query_slow_obd2_values()
{
    update_slow = 1;
}

void setup()
{

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

#if (DEBUG == 1)
    Serial.begin(115200);
#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();

    display.clrScr();

    display.setFont(BigFont);
    display.setColor(VGA_AQUA);
    display.print("Welcome Sem", (display.getDisplayXSize() / 2) - ((11 * display.getFontXsize()) / 2), 120);
    display.setColor(VGA_FUCHSIA);
    display.print("Just a moment...", (display.getDisplayXSize() / 2) - ((16 * display.getFontXsize()) / 2), 140);
    display.setColor(COLOR_LIGHT_GRAY);
    display.print("Waiting for ELM327", (display.getDisplayXSize() / 2) - ((18 * display.getFontXsize()) / 2), 160);

#if (DEBUG == 1)
    Serial.println("checking for bt");
#endif
    if (!obd2_elm327_init(&elm327))
    {
#if (DEBUG == 1)
        Serial.println("Shit man its fucked");
#endif
    }
    elm327.on_state_change = &bt_state_changed;
#if (DEBUG == 1)
    Serial.println("done with elm327 init");
    Serial.println(elm327.elm327->connected);
#endif

#if (SKIP_BT_CHECK == 1)
    update_percent(10);
#endif

    display.clrScr();

    display.setFont(OCR_A_Extended_M);
}

void loop()
{
    statemachine_loop();
}