#include "stm32f0xx_hal.h"

#include "ssd1306.h"

#include "stringhelpers.h"

#include "display.h"

#include "config.h"

#include "states.h"

#include "bootlib.h"

#include "flash.h"

#include "gpio.h"

uint8_t goto_mode = 2;

// State machine

uint8_t sw_btn_last = 0;

uint8_t sw_up_last = 0;

uint8_t sw_down_last = 0;

uint8_t sw_left_last = 0;

uint8_t sw_right_last = 0;

#define SW_BTN_PRESSED (sw_btn_last == 0 && sw_btn == 1) // rising edge on buttonpress

#define SW_UP_PRESSED (sw_up_last == 0 && sw_up == 1)

#define SW_DOWN_PRESSED (sw_down_last == 0 && sw_down == 1)

#define SW_LEFT_PRESSED (sw_left_last == 0 && sw_left == 1)

#define SW_RIGHT_PRESSED (sw_right_last == 0 && sw_right == 1)

///////////////////////////////////////////////////////////////////////////////////////

/// freaking multiple setpoint support ///

uint8_t step_duration[10] = {0,0,0,0,0,0,0,0,0,0};

int16_t step_setpoint[10] = {0,0,0,0,0,0,0,0,0,0};

uint8_t final_setpoint = 0;

// Multiple screens to set setpoint and duration on each screen

// press center to go to the next one, and press left or right or something to confirm

// When executing, complete on time AND(?) temperature. Maybe allow switching to OR via settings

////////////////////////////////////////////////////////////////////////////////////////////////

uint8_t trigger_drawsetpoint = 1;

int16_t last_temp = 21245;

void display_process(therm_settings_t* set, therm_status_t* status)

{

    uint8_t last_state = status->state;

    uint8_t temp_changed = status->temp != last_temp;

    last_temp = status->temp;

    uint8_t sw_btn = !HAL_GPIO_ReadPin(SW_BTN);

    uint8_t sw_up = !HAL_GPIO_ReadPin(SW_UP);

    uint8_t sw_down = !HAL_GPIO_ReadPin(SW_DOWN);

    uint8_t sw_left = !HAL_GPIO_ReadPin(SW_LEFT);

    uint8_t sw_right = !HAL_GPIO_ReadPin(SW_RIGHT);

    switch(status->state)

    {

        // Idle state

        case STATE_IDLE:

        {

            // Write text to OLED

            // [ therm :: idle ]

            ssd1306_DrawString("therm :: idle ", 0, 40);

            status->pid_enabled = 0;

            if(temp_changed) {

                char tempstr[6];

                itoa_fp(status->temp, status->temp_frac, tempstr);

                ssd1306_DrawString("Temp: ", 3, 40);

                ssd1306_DrawString("    ", 3, 72);

                ssd1306_DrawString(tempstr, 3, 72);

            }

            ssd1306_drawlogo();

            switch(goto_mode) {

                case 2:

                {

                    ssd1306_DrawString("-> heat     ", 1, 40);

                } break;

                case 1:

                {

                    ssd1306_DrawString("-> setup    ", 1, 40);

                } break;

                case 0:

                {

                    ssd1306_DrawString("-> reset    ", 1, 40);

                }

            }

            // Button handler

            if(SW_BTN_PRESSED) {

                switch(goto_mode) {

                    case 3:

                    {

                        ssd1306_clearscreen();

                        ssd1306_DrawString("Bootloader Entered", 0, 0);

                        ssd1306_DrawString("Device won't boot", 2, 0);

                        ssd1306_DrawString("until reflashed!", 3, 0);

                        bootloader_enter(); // Resets into bootloader

                        status->state = STATE_IDLE; // Just in case

                    } break;

                    default:

                        status->state = STATE_PREHEAT_BREW;

                }

            }

            else if(SW_UP_PRESSED && goto_mode < 3) {

                goto_mode++;

            }

            else if(SW_DOWN_PRESSED && goto_mode > 0) {

                goto_mode--;

            }

            // Event Handler

            // N/A

        } break;

        case STATE_SETP:

        {

            // Write text to OLED

            // [ therm :: set p ]

            // [ p = 12         ]

            ssd1306_DrawString("Proportional", 0, 40);

            ssd1306_drawlogo();

            char tempstr[6];

            itoa(set->k_p, tempstr, 10);

            ssd1306_DrawString("P=", 1, 45);

            ssd1306_DrawString("    ", 1, 57);

            ssd1306_DrawString(tempstr, 1, 57);

            ssd1306_DrawString("Press to accept", 3, 40);

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_SETI;

            }

            else {

                user_input(&set->k_p);

            }

            // Event Handler

            // N/A

        } break;

        case STATE_SETI:

        {

            // Write text to OLED

            // [ therm :: set i ]

            // [ i = 12         ]

            ssd1306_DrawString("Integral", 0, 40);

            ssd1306_drawlogo();

            char tempstr[6];

            itoa(set->k_i, tempstr, 10);

            ssd1306_DrawString("I=", 1, 45);

            ssd1306_DrawString("    ", 1, 57);

            ssd1306_DrawString(tempstr, 1, 57);

            ssd1306_DrawString("Press to accept", 3, 40);

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_SETD;

            }

            else {

                user_input(&set->k_i);

            }

            // Event Handler

            // N/A

        } break;

        case STATE_SETD:

        {

            // Write text to OLED

            // [ therm :: set d ]

            // [ d = 12         ]

            ssd1306_DrawString("Derivative", 0, 40);

            ssd1306_drawlogo();

            char tempstr[6];

            itoa(set->k_d, tempstr, 10);

            ssd1306_DrawString("D=", 1, 45);

            ssd1306_DrawString("    ", 1, 57);

            ssd1306_DrawString(tempstr, 1, 57);

            ssd1306_DrawString("Press to accept", 3, 40);

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_SETWINDUP;

            }

            else {

                user_input(&set->k_d);

            }

            // Event Handler

            // N/A

        } break;

        case STATE_SETSTEPS:

        {

            // Write text to OLED

            // [ step #1:: Duration: ### ]

            // [           Setpoint: ### ]

            char tempstr[6];

            itoa(final_setpoint, tempstr, 10);

            ssd1306_DrawString("Step #", 0, 0);

            ssd1306_DrawString(tempstr, 0, 40);

            ssd1306_DrawString("Duration: ", 0, 5);

            itoa(step_duration[final_setpoint], tempstr, 10);

            ssd1306_DrawString(tempstr, 0, 70);

            ssd1306_DrawString("Setpoint: ", 0, 0);

            itoa(step_setpoint[final_setpoint], tempstr, 10);

            ssd1306_DrawString(tempstr, 0, 70);

            ssd1306_DrawString("Press to accept", 3, 40);

            // Button handler - TODO: increment max_step if pressed

            // return and go to next state otherwise

            if(SW_BTN_PRESSED) {

                status->state = STATE_SETSTEPS;

                final_setpoint++;

            }

        //    else if(SW_LEFT_PRESSED) {

        //        state++; // go to next state or something

        //    }

            else {

                user_input(&set->k_p);

            }

            // Event Handler

            // N/A

        } break;

        case STATE_SETWINDUP:

        {

            // Write text to OLED

            // [ therm :: set windup ]

            // [ g = 12         ]

            ssd1306_DrawString("Windup Guard", 0, 40);

            ssd1306_drawlogo();

            char tempstr[6];

            itoa(set->windup_guard, tempstr, 10);

            ssd1306_DrawString("G=", 1, 45);

            ssd1306_DrawString("    ", 1, 57);

            ssd1306_DrawString(tempstr, 1, 57);

            ssd1306_DrawString("Press to accept", 3, 40);

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_SETBOOTTOBREW;

            }

            else {

                user_input(&set->windup_guard);

            }

            // Event Handler

            // N/A

        } break;

        case STATE_SETBOOTTOBREW:

        {

            // Write text to OLED

            // [ therm :: set windup ]

            // [ g = 12         ]

            ssd1306_DrawString("Start on Boot", 0, 40);

            ssd1306_drawlogo();

            ssd1306_DrawString("sob=", 1, 45);

            if(set->boottobrew)

                ssd1306_DrawString("Enabled ", 1, 70);

            else

                ssd1306_DrawString("Disabled", 1, 70);

            ssd1306_DrawString("Press to accept", 3, 40);

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_SETUNITS;

            }

            else if(!HAL_GPIO_ReadPin(SW_UP)) {

                set->boottobrew = 1;

            }

            else if(!HAL_GPIO_ReadPin(SW_DOWN)) {

                set->boottobrew = 0;

            }

            // Event Handler

            // N/A

        } break;

        case STATE_SETUNITS:

        {

            // Write text to OLED

            // [ therm :: set windup ]

            // [ g = 12         ]

            ssd1306_DrawString("Units: ", 0, 40);

            ssd1306_drawlogo();

            if(set->temp_units == TEMP_UNITS_FAHRENHEIT)

                ssd1306_DrawString("Fahrenheit", 1, 60);

            else

                ssd1306_DrawString("Celsius   ", 1, 60);

            ssd1306_DrawString("Press to accept", 3, 40);

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_SETTEMPOFFSET;

            }

            else if(!HAL_GPIO_ReadPin(SW_UP)) {

                set->temp_units = TEMP_UNITS_FAHRENHEIT;

            }

            else if(!HAL_GPIO_ReadPin(SW_DOWN)) {

                set->temp_units = TEMP_UNITS_CELSIUS;

            }

            // Event Handler

            // N/A

        } break;

        case STATE_SETTEMPOFFSET:

        {

            // Write text to OLED

            // [ therm :: set temp offset ]

            // [ g = 12         ]

            ssd1306_DrawString("Temp Cal Offset", 0, 40);

            ssd1306_drawlogo();

            char tempstr[6];

            itoa(set->temp_offset, tempstr, 10);

            ssd1306_DrawString("O=", 1, 45);

            ssd1306_DrawString("    ", 1, 57);

            ssd1306_DrawString(tempstr, 1, 57);

            ssd1306_DrawString("Press to accept", 3, 40);

            // Button handler

            if(SW_BTN_PRESSED) {

                flash_save(&set);

                status->state = STATE_IDLE;

            }

            else {

                user_input_signed(&set->temp_offset);

            }

            // Event Handler

            // N/A

        } break;

        case STATE_PREHEAT_BREW:

        {

            // Write text to OLED

            // [ therm : preheating brew ]

            // [ 30 => 120 C             ]

            ssd1306_DrawString("Preheating...", 0, 0);

            //ssd1306_drawlogo();

            draw_setpoint(status);

            status->pid_enabled = 1;

	    status->setpoint = set->setpoint_brew;

            // Button handler

            if(SW_BTN_PRESSED) {

		save_setpoints(&set); // TODO: Check for mod

                status->state = STATE_IDLE;

            }

            else {

                user_input(&set->setpoint_brew);

            }

            // Event Handler

            if(status->temp >= status->setpoint) {

                status->state = STATE_MAINTAIN_BREW;

            }

        } break;

        case STATE_MAINTAIN_BREW:

        {

            // Write text to OLED

            // [ therm : ready to brew ]

            // [ 30 => 120 C           ]

            ssd1306_DrawString("Preheated!", 0, 0);

            //ssd1306_drawlogo();

            draw_setpoint(status);

            status->pid_enabled = 1;

	    status->setpoint = set->setpoint_brew;

            // Button handler

            if(SW_BTN_PRESSED) {

		save_setpoints(&set); // TODO: Check for mod

                status->state = STATE_IDLE;

            }

            else {

                user_input(&set->setpoint_brew);

            }

            // Event Handler

            // N/A

        } break;

        case STATE_PREHEAT_STEAM:

        {

            // Write text to OLED

            // [ therm : preheating steam ]

            // [ 30 => 120 C           ]

            ssd1306_DrawString("Preheating...", 0, 0);

            //ssd1306_drawlogo();

            draw_setpoint(status);

            status->pid_enabled = 1;

	    status->setpoint = set->setpoint_steam;

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_IDLE;

		save_setpoints(&set); // TODO: Check for mod

            }

            else {

                user_input(&set->setpoint_steam);

            }

            // Event Handler

            if(status->temp >= status->setpoint) {

                status->state = STATE_MAINTAIN_STEAM;

            }

        } break;

        case STATE_MAINTAIN_STEAM:

        {

            // Write text to OLED

            // [ therm : ready to steam ]

            // [ 30 => 120 C            ]

            ssd1306_DrawString("Ready to Steam!", 0, 0);

            //ssd1306_drawlogo();

            draw_setpoint(status);

            status->pid_enabled = 1;

	    status->setpoint = set->setpoint_steam;

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_IDLE;

		save_setpoints(&set); // TODO: Check for mod

            }

            else {

                user_input(&set->setpoint_steam);

            }

            // Event Handler

            // N/A

        } break;

        case STATE_TC_ERROR:

        {

            // Write text to OLED

            // [ therm : ready to steam ]

            // [ 30 => 120 C            ]

            ssd1306_DrawString("Error:              ", 0, 0);

            char tempstr[6];

            itoa(status->tc_errno, tempstr, 10);

            ssd1306_DrawString(tempstr, 0, 57);