uint8_t temp_changed = status->temp != last_temp || status->temp_frac != last_temp_frac;

    last_temp = status->temp;

    last_temp_frac = status->temp_frac;

    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 || state_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);

            }

            if (state_changed) {

            	ssd1306_drawlogo();

            }

            switch(goto_mode) {

                case MODE_HEAT:

                {

                    if(set->val.plant_type == PLANT_HEATER)

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

                    else

                        ssd1306_drawstring("-> cool     ", 1, 40);

                } break;

                case MODE_SETUP:

                {

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

                } break;

                case MODE_RESET:

                {

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

                } break;

				#ifdef BOOTLOADER_SHORTCUT

                case MODE_BOOTLOADER:

                {

                    ssd1306_drawstring("-> dfu      ", 1, 40);

                }

				#endif

            }

            // Button handler

            if(SW_BTN_PRESSED) {

                switch(goto_mode) {

                    case MODE_HEAT:

                        status->state = STATE_PREHEAT;

                        break;

                    case MODE_SETUP:

                        status->state = STATE_SETMODE;

                        break;

                    case MODE_RESET:

                        status->state = STATE_RESET;

                        reset_mode = RESET_REBOOT;

                        break;

					#ifdef BOOTLOADER_SHORTCUT

                    case MODE_BOOTLOADER:

                        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_RESET; // Just in case

                        break;

					#endif

                    default:

                        status->state = STATE_PREHEAT;

                }

            }

            else if(SW_DOWN_PRESSED && goto_mode < (MODE_SIZE - 1)) {

                goto_mode++;

            }

            else if(SW_UP_PRESSED && goto_mode > 0) {

                goto_mode--;

            }

            // Event Handler

            // N/A

        } break;

        case STATE_SETMODE:

        {

            // Write text to OLED

            // [ therm :: set mode ]

            // [ m =          ]

            ssd1306_drawstring("Control Mode", 0, 40);

            ssd1306_drawlogo();

            if(set->val.control_mode == MODE_PID)

                ssd1306_drawstring("PID       ", 1, 60);

            else

                ssd1306_drawstring("Thermostat", 1, 60);

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

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_SETPLANTTYPE;

            }

            else if (!HAL_GPIO_ReadPin(SW_UP)) {

                set->val.control_mode = MODE_PID;

            }

            else if(!HAL_GPIO_ReadPin(SW_DOWN)) {

                set->val.control_mode = MODE_THERMOSTAT;

            }

            // Event Handler

            // N/A

        } break;

        case STATE_SETPLANTTYPE:

        {

            // Write text to OLED

            // [ therm :: set mode ]

            // [ m =          ]

            ssd1306_drawstring("Plant Type", 0, 40);

            ssd1306_drawlogo();

            if(set->val.plant_type == PLANT_HEATER)

                ssd1306_drawstring("Heater", 1, 60);

            else

                ssd1306_drawstring("Cooler", 1, 60);

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

            // Button handler

            if(SW_BTN_PRESSED) {

                if(set->val.control_mode == MODE_PID)

                    status->state = STATE_SETP;

                else

                    status->state = STATE_SETBOOTTOBREW;

            }

            else if (!HAL_GPIO_ReadPin(SW_UP)) {

                set->val.plant_type = PLANT_COOLER;

            }

            else if(!HAL_GPIO_ReadPin(SW_DOWN)) {

                set->val.plant_type = PLANT_HEATER;

            }

            // 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->val.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((uint16_t*)&set->val.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->val.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((uint16_t*)&set->val.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->val.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((uint16_t*)&set->val.k_d);

            }

            // 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->val.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((uint16_t*)&set->val.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->val.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->val.boottobrew = 1;

            }

            else if(!HAL_GPIO_ReadPin(SW_DOWN)) {

                set->val.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->val.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->val.temp_units = TEMP_UNITS_FAHRENHEIT;

            }

            else if(!HAL_GPIO_ReadPin(SW_DOWN)) {

                set->val.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->val.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->val.temp_offset);

            }

            // Event Handler

            // N/A

        } break;

        case STATE_PREHEAT:

        {

            // Write text to OLED

            // [ therm : preheating brew ]

            // [ 30 => 120 C             ]

            //ssd1306_drawlogo();

            draw_setpoint(status);

            status->pid_enabled = 1;

            status->setpoint = set->val.setpoint_brew;

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_IDLE;

            }

            else {

                user_input((uint16_t*)&set->val.setpoint_brew);

            }

            // Event Handler

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

                status->state = STATE_MAINTAIN;

            }

        } break;

        case STATE_MAINTAIN:

        {

            // Write text to OLED

            // [ therm : ready to brew ]

            // [ 30 => 120 C           ]

            if(set->val.plant_type == PLANT_HEATER)

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

            else

                ssd1306_drawstring("Precooled!", 0, 0);

            draw_setpoint(status);

            status->pid_enabled = 1;

            status->setpoint = set->val.setpoint_brew;

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_IDLE;

            }

            else {

                user_input((uint16_t*)&set->val.setpoint_brew);

            }

            // Event Handler

            // N/A

        } break;

        // Thermocouple error

        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->error_code, tempstr, 10);

            ssd1306_drawstring(tempstr, 0, 57);

            //TODO: add RTD error codes

            if(status->error_code == 1)

                ssd1306_drawstring("    TC Open Circuit", 1, 0);

            else if(status->error_code == 4)

                ssd1306_drawstring("    TC Short to GND", 1, 0);

            else if(status->error_code == 8)

                ssd1306_drawstring("    TC Short to VCC", 1, 0);

            else

                ssd1306_drawstring("#?, Unknown Error", 1, 0);

            ssd1306_drawstring("                    ", 2, 0);

            ssd1306_drawstring("-> to ignore all or", 2, 0);

            ssd1306_drawstring("press to continue", 3, 0);

            // Button handler

            if(SW_BTN_PRESSED) {

                status->state = STATE_IDLE;

				#ifdef MAX31865_RTD_SENSOR

				max31865_clear_errors(spi_get());

				#endif

            }

            else if(SW_RIGHT_PRESSED) {

                set->val.ignore_error = 1;

                status->state = STATE_IDLE;

            }

            // Event Handler

            // Maybe handle if TC is plugged in

            // N/A

        } break;

        // Reset state

        case STATE_RESET:

        {

            // Write text to OLED

            // [ therm :: reset ]

            ssd1306_drawstring("therm :: reset ", 0, 40);

            status->pid_enabled = 0;

            ssd1306_drawlogo();

            switch(reset_mode) {

				case RESET_DEFAULTS:

				{

					ssd1306_drawstring("-> defaults   ", 1, 40);

				} break;

                case RESET_BOOTLOADER:

                {

                    ssd1306_drawstring("-> bootloader ", 1, 40);

                } break;

                case RESET_REBOOT:

                {

                    ssd1306_drawstring("-> reboot     ", 1, 40);

                } break;

                case RESET_EXIT:

                {

                    ssd1306_drawstring("-> exit       ", 1, 40);

                } break;

            }

            // Button handler

            if(SW_BTN_PRESSED) {

                switch(reset_mode) {

                    case RESET_BOOTLOADER:

                    {

                        ssd1306_clearscreen();

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

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

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

                        HAL_Delay(1000);

                        bootloader_enter(); // Resets into bootloader

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

                    } break;

                    case RESET_DEFAULTS:

                    {

                        status->state = STATE_RESET;

                        flash_load_defaults(set);

                        flash_save(set);

                        NVIC_SystemReset();

                    } break;

                    case RESET_REBOOT:

                    {

                        status->state = STATE_RESET;

                        NVIC_SystemReset();

                    } break;

                    case RESET_EXIT:

                    {

                        status->state = STATE_IDLE;

                    } break;

                }

            }

            else if(SW_DOWN_PRESSED && reset_mode < (RESET_SIZE-1)) {

                reset_mode++;

            }

            else if(SW_UP_PRESSED && reset_mode > 0) {

                reset_mode--;

            }

            // Event Handler

            // 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 = MODE_HEAT;

        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;

}

static int32_t temp_last = 43002;

static int32_t temp_frac_last = 43002;

static int32_t setpoint_last = 10023;

// Draw current setpoint on display

static void draw_setpoint(therm_status_t* status) {

    // FIXME: need to do this when switching modes too

    if(status->temp != temp_last || status->temp_frac != temp_frac_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;

    temp_frac_last = status->temp_frac;

}

void display_startup_screen() {

    ssd1306_clearscreen();

    ssd1306_drawstring("therm v0.2", 1, 40);

    ssd1306_drawstring("protofusion.org/therm", 3, 0);

}

// vim:softtabstop=4 shiftwidth=4 expandtab