//
// Therm Firmware
// Copyright 2018 Ethan Zonca
// Author(s): Ethan Zonca and Matthew Reed
//

#include "stm32f3xx_hal.h"
#include "config.h"
#include "watchdog.h"
#include "system.h"
#include "display.h"
#include "thermostat.h"
#include "gpio.h"
#include "tempsense.h"
#include "pid.h"
#include "error.h"
#include "flash.h"
#include "ssd1306/ssd1306.h"
#include "pwmout.h"


int main(void)
{
    sysclock_init();
    hal_init();
    gpio_init();
    flash_init();

    ssd1306_init();

    // Startup screen
    display_startup_screen();
    HAL_Delay(2000);
    ssd1306_clearscreen();
    //ssd1306_drawlogo();

    // Default status
    runtime_status()->temp = 0.0;
    runtime_status()->state_resume = 0;
    runtime_status()->setpoint = 70.0;
    runtime_status()->pid_enabled = 0;
    runtime_status()->setpoint_index = 0;
    if (flash_getsettings()->val.boottobrew)
    {
        runtime_status()->state = STATE_PREHEAT;
    }
    else
    {
        runtime_status()->state = STATE_IDLE;
    }

    pid_init();
    pwmout_init();
    watchdog_init();
    tempsense_init();

    // Soft timers
    uint32_t last_pid = 0;
    uint32_t last_thermostat = 0;
    uint32_t last_1hz = 0;
    uint32_t last_5hz = 0;

    int16_t duty = 0;

    while (1)
    {

        if(HAL_GetTick() - last_1hz > 750)
        {
            display_1hz();
            last_1hz = HAL_GetTick();
        }

        if(HAL_GetTick() - last_5hz > 200)
        {
            tempsense_readtemp();
            runtime_status()->temp = tempsense_gettemp();
            last_5hz = HAL_GetTick();
        }

        if(flash_getsettings()->val.control_mode == MODE_PID && (HAL_GetTick() - last_pid > PID_PERIOD))
        {
            //duty = pid_process();
            last_pid = HAL_GetTick();
        }

        // Thermostatic control
        if(flash_getsettings()->val.control_mode == MODE_THERMOSTAT && HAL_GetTick() - last_thermostat > SSR_PERIOD)
        {
            duty = thermostat_process();
            last_thermostat = HAL_GetTick();
        }

        //pwmout_process((int16_t)duty);
        display_process();
        watchdog_feed();


        //        // 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();
        //        }

    }
}