#include "stm32f0xx_hal.h"

#include "config.h"
#include "syslib.h"
#include "pid.h"
#include "states.h"
#include "ssd1306.h"
#ifdef MAX31855_TC_SENSOR
#include "max31855.h"
#endif
#ifdef MAX31865_RTD_SENSOR
#include "max31865.h"
#endif
#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();
    
    #ifdef MAX31865_RTD_SENSOR
    max31865_config(spi_get());
    #endif

    // Init OLED over SPI
    ssd1306_init();
    ssd1306_clearscreen();

    // 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(spi_get(), &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);
                HAL_GPIO_WritePin(LED_POWER, 0);
            }
            
        }
        
        // Transmit temperature over USB-CDC on a regulat basis
        if(HAL_GetTick() - last_vcp_tx > VCP_TX_FREQ)
        {
            // Print temp to cdc
            char tempstr[16];
            itoa_fp(status.temp, status.temp_frac, tempstr);
            uint8_t numlen = strlen(tempstr);
            tempstr[numlen] = '\r';
            tempstr[numlen+1] = '\n';

    //        if(set.val.usb_plugged)
    //            CDC_Transmit_FS(tempstr, numlen+2);
           // while(CDC_Transmit_FS("\r\n", 2) == USBD_BUSY);

            last_vcp_tx = HAL_GetTick();
        }

        // Run state machine
        display_process(&set, &status); 
    }
}
// vim:softtabstop=4 shiftwidth=4 expandtab