#include "stm32f0xx_hal.h"

#include "config.h"
#include "syslib.h"
#include "pid.h"
#include "states.h"
#include "ssd1306.h"
#ifdef MAX31865_RTD_SENSOR
#include "max31865.h"
#else
#include "max31855.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;
pid_state_t pid_state;

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 RTD chip
    #ifdef MAX31865_RTD_SENSOR
    max31865_config(spi_get());
    #endif

    // Init OLED over SPI
    ssd1306_init();

    // Startup screen
    display_startup_screen();

    // Default status
    status.temp = 0;
    status.temp_frac = 0;
    status.state_resume = 0;
    status.state = STATE_IDLE;
    status.setpoint = 70;
    status.pid_enabled = 0;

    pid_init(&pid_state);

    // Go to brew instead of idle if configured thusly
    if(set.val.boottobrew)
      status.state = STATE_PREHEAT; 

    // Restore settings from flash memory
    flash_restore(&set);

    HAL_Delay(2000);
    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;
    uint32_t last_thermostat = 0;
    int16_t ssr_output = 0; // Duty cycle of ssr, 0 to SSR_PERIOD 

    uint8_t thermostat_plant_on = 0;

    // Main loop
    while(1)
    {
        // Process sensor inputs

        if(HAL_GetTick() - last_led > 400) 
        {
            last_led = HAL_GetTick();
        }

        if(set.val.control_mode == MODE_PID && (HAL_GetTick() - last_pid > PID_PERIOD))
        {

            #ifdef MAX31865_RTD_SENSOR
            max31865_readtemp(spi_get(), &set, &status);
			#else
			max31855_readtemp(spi_get(), &set, &status); // Read MAX31855
			#endif


            if(status.pid_enabled) 
            {
                // Get ssr output for next time
                int16_t power_percent = pid_update(&set, &status, &pid_state);

                if(set.val.plant_type == PLANT_HEATER)
                    power_percent *= -1;

                //power-percent is 0-1000?
                ssr_output = power_percent; //(((uint32_t)SSR_PERIOD * (uint32_t)10 * (uint32_t)100) * power_percent) / (uint32_t)1000000;


            	// put ssr output on display
                ssd1306_drawstring("      ", 0, 90); //fixme: this is bad, but I can't get the old digits to clear otherwise
                char tempstr[6];
                itoa(ssr_output, tempstr, 10);
                ssd1306_drawstring(tempstr, 0, 90);
            }
            else 
            {
                ssr_output = 0;
            }

            last_pid = HAL_GetTick();
        }

        // Kill SSR once the desired on-time has elapsed
        if(set.val.control_mode == MODE_PID && (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(set.val.control_mode == MODE_PID && HAL_GetTick() - last_ssr_on > SSR_PERIOD)
        {
            // Heat or cool, if we need to 
            if(ssr_output > 0) 
            {
                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);
            }
            
        }
        

        // Thermostatic control
        if(set.val.control_mode == MODE_THERMOSTAT && HAL_GetTick() - last_thermostat > SSR_PERIOD)
        {

            #ifdef MAX31865_RTD_SENSOR
            max31865_readtemp(spi_get(), &set, &status);
			#else
			max31855_readtemp(spi_get(), &set, &status); // Read MAX31855
			#endif

            // TODO: Migrate this FxP conversion to the readtemp code or similar
            int8_t temp_frac = status.temp_frac > 9 ? status.temp_frac / 10 : status.temp_frac;
            temp_frac = status.temp > 0 ? temp_frac : temp_frac * -1;
            int32_t temp = (status.temp * 10) + temp_frac;


            // EMZ FIXME: This could be way simpler
            if(set.val.plant_type == PLANT_HEATER && status.setpoint * 10 < temp - set.val.hysteresis * 10)
                thermostat_plant_on = 1;
            else if(set.val.plant_type == PLANT_HEATER && status.setpoint * 10 > temp + set.val.hysteresis * 10)
                thermostat_plant_on = 0;

            if(set.val.plant_type == PLANT_COOLER && status.setpoint * 10 > temp + set.val.hysteresis * 10)
                thermostat_plant_on = 1;
            else if(set.val.plant_type == PLANT_COOLER && status.setpoint * 10 < temp - set.val.hysteresis * 10)
                thermostat_plant_on = 0;

            // EMZ: TODO: Refactor to output_enabled or something
            if(status.pid_enabled && thermostat_plant_on)
            {
                // EMZ TODO: functionalize this
            	// put ssr output on display
                ssd1306_drawstring("      ", 0, 90); //fixme: this is bad, but I can't get the old digits to clear otherwise
                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);
            }
            else
            {
                HAL_GPIO_WritePin(SSR_PIN, 0);
                HAL_GPIO_WritePin(LED_POWER, 0);
            }
            
            last_thermostat = HAL_GetTick();
        }


        // Transmit temperature over USB-CDC on a regular 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