//
// Therm Firmware
// Copyright 2017 Ethan Zonca
// Author(s): Ethan Zonca
//

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

	ssd1306_init();

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

	ssd1306_drawlogo();

    // Default status
	runtime_status()->temp = 0.0;
	runtime_status()->state_resume = 0;
	runtime_status()->state = STATE_IDLE;
	runtime_status()->setpoint = 70;
	runtime_status()->pid_enabled = 0;

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

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

	while (1)
	{
		float duty = 0.0;

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

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

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

	}
}