#include "stm32f0xx_hal.h"

#include "config.h"

#include "syslib.h"

#include "pid.h"

#include "states.h"

#include "ssd1306.h"

#include "max31855.h"

#include "gpio.h"

#include "spi.h"

#include "flash.h"

#include "stringhelpers.h"

#include "display.h"

#include "usb_device.h"

#include "usbd_cdc_if.h"

therm_settings_t set;

therm_status_t status;

int main(void)

{

    // Initialize HAL

    hal_init();

    // Configure the system clock

    systemclock_init();

    // Unset bootloader option bytes (if set)

    // FIXME this was never getting called. Try again sometime.

    //bootloader_unset();

    // Init GPIO

    gpio_init();

    // Init USB (TODO: Handle plugged/unplugged with external power)

    MX_USB_DEVICE_Init();

//    set.val.usb_plugged = 

    // USB startup delay

    HAL_Delay(500);

    HAL_GPIO_WritePin(LED_POWER, 1);

    // Enter into bootloader if up button pressed on boot

    if(!HAL_GPIO_ReadPin(SW_UP))

        bootloader_enter(); 

    // Init SPI busses

    spi_init();

    // Init OLED over SPI

    ssd1306_init();

    ssd1306_clearscreen();

    // Default settings 

    set.val.boottobrew = 0;

    set.val.temp_units = TEMP_UNITS_FAHRENHEIT;

    set.val.k_p = 10;

    set.val.k_d = 1;

    set.val.ignore_tc_error = 0;

    set.val.setpoint_brew = 70;

    set.val.setpoint_steam = 70;

    // Default status

    status.temp = 0;

    status.temp_frac = 0;

    status.state_resume = 0;

    status.state = STATE_IDLE;

    status.setpoint = 70;

    status.pid_enabled = 0;

    // Go to brew instead of idle if configured thusly

    if(set.val.boottobrew)

      status.state = STATE_PREHEAT; 

    // Startup screen 

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

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

    HAL_Delay(1000);

    // Restore settings from flash memory

    flash_restore(&set);

    HAL_Delay(1000);

    ssd1306_clearscreen();

    // Soft timers

    uint32_t last_ssr_on = 0;

    uint32_t last_vcp_tx = 0;

    uint32_t last_led = 0;

    uint32_t last_pid = 0;

    int16_t ssr_output = 0; // Duty cycle of ssr, 0 to SSR_PERIOD 

    // Main loop

    while(1)

    {

        // Process sensor inputs

        if(HAL_GetTick() - last_led > 400) 

        {

            last_led = HAL_GetTick();

        }

        if((HAL_GetTick() - last_pid > PID_PERIOD))

        {

            #ifdef MAX31855_TC_SENSOR

            max31855_readtemp(spi_get(), &set, &status); // Read MAX31855

            #endif

            #ifdef MAX31865_RTD_SENSOR

            max31865_readtemp(&set, &status);

            #endif

            if(status.pid_enabled) 

            {

                // Get ssr output for next time

                int16_t power_percent = pid_update(set.val.k_p, set.val.k_i, set.val.k_d, status.temp, status.temp_frac, status.setpoint, &set, &status);

                //power-percent is 0-1000?

                ssr_output = power_percent; //(((uint32_t)SSR_PERIOD * (uint32_t)10 * (uint32_t)100) * power_percent) / (uint32_t)1000000;

            }

            else 

            {

                ssr_output = 0;

            }

            last_pid = HAL_GetTick();

        }

        // Kill SSR once the desired on-time has elapsed

        if(HAL_GetTick() - last_ssr_on > ssr_output || ssr_output <= 0)

        {

            HAL_GPIO_WritePin(SSR_PIN, 0);

            HAL_GPIO_WritePin(LED_POWER, 0);

        }

        // Every 200ms, set the SSR on unless output is 0

        if(HAL_GetTick() - last_ssr_on > SSR_PERIOD)

        {

            // Only support heating (ssr_output > 0) right now

            if(ssr_output > 0) 

            {

                char tempstr[6];

                itoa(ssr_output, tempstr, 10);

                ssd1306_drawstring(tempstr, 0, 90);

                HAL_GPIO_WritePin(SSR_PIN, 1);

                HAL_GPIO_WritePin(LED_POWER, 1);

                last_ssr_on = HAL_GetTick();

            }

            else {

                // Make sure everything is off

                HAL_GPIO_WritePin(SSR_PIN, 0);