ssd1306_DrawString("therm v0.1", 1, 40);

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

    delay(1500);

    ssd1306_clearscreen();

    restore_settings();

    if(boottobrew)

      state = STATE_PREHEAT_BREW; // Go to brew instead of idle if configured thusly

    GPIO_ResetBits(LED_STAT);

    // Main loop

    while(1)

    {

        // Process sensor inputs

        process();

        // Run state machine

        machine(); 

    }

}

// Read temperature and update global temp vars

int32_t temp = 0;

uint8_t temp_frac = 0;

void update_temp() {

    // Assert CS

    GPIO_ResetBits(MAX_CS);

    delay(1);

    // This may not clock at all... might need to send 16 bits first

    SPI_I2S_SendData(SPI2, 0xAAAA); // send dummy data

    //SPI_I2S_SendData(SPI2, 0xAA); // send dummy data

    uint16_t temp_pre = SPI_I2S_ReceiveData(SPI2);

    if(temp_pre & 0b0000000000000010) {

        ssd1306_DrawString("Fatal Error", 3, 35);

        state = STATE_TC_ERROR;

    }

    else if(temp_pre & 0b0000000000000001 && !ignore_tc_error) {

        state = STATE_TC_ERROR;

        temp = 0;

        temp_frac = 0;

    }

    else 

    {

        uint8_t sign = temp >> 15;// top bit is sign

        temp_pre = temp_pre >> 2; // Drop 2 lowest bits

        temp_frac = temp_pre & 0b11; // get fractional part

        temp_frac *= 25; // each bit is .25 a degree, up to fixed point

        temp_pre = temp_pre >> 2; // Drop 2 fractional bits 

        if(sign) {

            temp = -temp_pre;

        }

        else {

            temp = temp_pre;

        }

        if(temp_units == TEMP_UNITS_FAHRENHEIT) {

            temp *= 9; // fixed point mul by 1.8

            temp /= 5;

            temp += 32;

            temp_frac *= 9;

            temp_frac /= 5;

            temp_frac += 32;

            temp += temp_frac/100; // add overflow to above

            temp_frac %= 100;

        }

    }

    // Deassert CS

    delay(1);

    GPIO_SetBits(MAX_CS);

}

// PID implementation

// TODO: Make struct that has the last_temp and i_state in it, pass by ref. Make struct that has other input values maybe.

int16_t last_pid_temp = 0;

uint8_t last_pid_temp_frac = 0;

int16_t i_state = 0;

int16_t update_pid(uint16_t k_p, uint16_t k_i, uint16_t k_d, int16_t temp, uint8_t temp_frac, int16_t setpoint) 

{

  // Calculate instantaneous error

  int16_t error = (int16_t)setpoint - (int16_t)temp; // TODO: Use fixed point fraction

  // Proportional component

  int16_t p_term = k_p * error;

  // Error accumulator (integrator)

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

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

void machine()

{

    uint8_t last_state = state;

    uint8_t temp_changed = temp != last_temp;

    last_temp = temp;

    uint8_t sw_btn = !GPIO_ReadInputDataBit(SW_BTN);

    uint8_t sw_up = !GPIO_ReadInputDataBit(SW_UP);

    uint8_t sw_down = !GPIO_ReadInputDataBit(SW_DOWN);

    uint8_t sw_left = !GPIO_ReadInputDataBit(SW_LEFT);

    uint8_t sw_right = !GPIO_ReadInputDataBit(SW_RIGHT);

    switch(state)

    {

        // Idle state

        case STATE_IDLE:

        {

            // Write text to OLED

            // [ therm :: idle ]

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

            pid_enabled = 0;

            if(temp_changed) {

                char tempstr[6];

                itoa_fp(temp, temp_frac, tempstr);

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

                ssd1306_DrawString("    ", 3, 72);

                ssd1306_DrawString(tempstr, 3, 72);

            }

            ssd1306_drawlogo();

            switch(goto_mode) {

                case 2:

                {

                } 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:

                        state = STATE_PREHEAT_BREW;

                        break;

                    case 1:

                        state = STATE_SETP;

                        break;

                    case 0:

                        state = STATE_IDLE;

                        break;

                    default:

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

                final_setpoint++;

            }

        //    else if(SW_LEFT_PRESSED) {

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

        //    }

            else {

                user_input(&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(windup_guard, tempstr);

            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) {

                state = STATE_SETBOOTTOBREW;

            }

            else {

                user_input(&windup_guard);

            }

            // Event Handler

            // N/A

        } break;

        case STATE_SETBOOTTOBREW:

        {

            // Write text to OLED

            // [ therm :: set windup ]

            // [ g = 12         ]

            ssd1306_drawlogo();

            if(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) {

                state = STATE_SETUNITS;

            }

            else if(!GPIO_ReadInputDataBit(SW_UP)) {

                boottobrew = 1;

            }

            else if(!GPIO_ReadInputDataBit(SW_DOWN)) {

                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(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) {

                save_settings();

                state = STATE_IDLE;

            }

            else if(!GPIO_ReadInputDataBit(SW_UP)) {

                temp_units = TEMP_UNITS_FAHRENHEIT;

            }

            else if(!GPIO_ReadInputDataBit(SW_DOWN)) {