//
// 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 "gpio.h"
#include "pid.h"
#include "error.h"
#include "flash.h"
#include "ssd1306.h"
#include "dma.h"
#include "interrupts.h"
#include "buttons.h"


therm_settings_t set;
therm_status_t status;
pid_state_t pid_state;


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


	ssd1306_init();

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

	ssd1306_drawlogo();

	watchdog_init();

	//just some example code for getting flash values
//	flash_getsettings()->values.can_id = 67;
//
//	if(flash_getsettings()->values.can_id == 12);

//	ssd1306_drawstring(const char *dataPtr, unsigned char row, unsigned char xPos)
//	ssd1306_drawstring("[ ProtoFuse ]", 0, 0);

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

    flash_init();


	float temp_counter = 0;

	// Software timers
	uint32_t last_screen_update_time = HAL_GetTick();


    // 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;


	while (1)
	{

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

		watchdog_feed();
	}
}