#include "stm32f0xx_hal.h"
#include "ssd1306.h"
#include "stringhelpers.h"
#include "display.h"
#include "config.h"
#include "states.h"
#include "bootlib.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);
                } break;
            }

            // Button handler
            if(SW_BTN_PRESSED) {
                switch(goto_mode) {
                    case 2:
                        status->state = STATE_PREHEAT_BREW;
                        break;
                    case 1:
                        status->state = STATE_SETP;
                        break;
                    case 0:
                    {
                        ssd1306_clearscreen();
                        ssd1306_DrawString("Entering Bootloader", 1, 0);
                        ssd1306_DrawString("(hopefully)", 2, 0);
                        HAL_Delay(1000);
                        bootloader_enter();

/*
                        HAL_RCC_DeInit();
                        SysTick->CTRL = 0;
                        SysTick->LOAD = 0;
                        SysTick->VAL = 0;
                        __set_PRIMASK(1);
                        __set_MSP(0x200010000);
                        *((unsigned long *)0x200017F0) = 0xDEADBEEF; // 6KB STM32F042
                        NVIC_SystemReset();
*/
                        status->state = STATE_IDLE;
                    } break;

                    default:
                        status->state = STATE_PREHEAT_BREW;
                }
            }
            else if(SW_UP_PRESSED && goto_mode < 2) {
                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) {
                save_settings(&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);

            if(status->tc_errno == 1)
                ssd1306_DrawString("#1, Check Sensor", 1, 0);
            else if(status->tc_errno == 4)
                ssd1306_DrawString("#4, Check Sensor", 1, 0);
            else
                ssd1306_DrawString("#?, Unknown Error", 1, 0);

            ssd1306_DrawString("Press -> to ignore", 3, 0);

            // Button handler
            if(SW_BTN_PRESSED) {
                status->state = STATE_IDLE;
            }
            else if(SW_RIGHT_PRESSED) {
                set->ignore_tc_error = 1;
                status->state = STATE_IDLE;
            }
            // Event Handler
            // Maybe handle if TC is plugged in
            // N/A
 
        } break;

        // Something is terribly wrong
        default:
        {
            status->state = STATE_IDLE;
            status->pid_enabled = 0;

        } break;
            
    }

    if(last_state != status->state) {
        // Clear screen on state change
        goto_mode = 2;
        trigger_drawsetpoint = 1;
        ssd1306_clearscreen();
    }

    // Last buttonpress
    sw_btn_last = sw_btn;
    sw_up_last = sw_up;
    sw_down_last = sw_down;
    sw_left_last = sw_left;
    sw_right_last = sw_right;
}


int32_t temp_last = 43002;
int32_t setpoint_last = 10023;
void draw_setpoint(therm_status_t* status) {
    // FIXME: need to do this when switching modes too
    if(status->temp != temp_last || trigger_drawsetpoint) { 
        char tempstr[3];
        itoa_fp(status->temp, status->temp_frac, tempstr);
        ssd1306_DrawStringBig("      ", 3, 0);
        ssd1306_DrawStringBig(tempstr, 3, 0);
    }

    if(trigger_drawsetpoint) 
        ssd1306_DrawStringBig(">", 3, 74);

    if(status->setpoint != setpoint_last || trigger_drawsetpoint) {
        char tempstr[3];
        itoa(status->setpoint, tempstr, 10);
        ssd1306_DrawStringBig("   ", 3, 90);
        ssd1306_DrawStringBig(tempstr, 3, 90);
    }

    trigger_drawsetpoint = 0;
    setpoint_last = status->setpoint;
    temp_last = status->temp;
}

// vim:softtabstop=4 shiftwidth=4 expandtab