#include <atmel_start.h>

#include "usb_debug.h"
#include "checksums.h"
#include "flash_utils.h"
#include "float_utils.h"
#include "i2c_slave.h"
#include "pid.h"
#include "temp_curve.h"
#include "COAST.h"

__DEFAULT_NO_xMR

#define PWM_CC 1.041
/* #define PWM_CC 1.0 */

//! PMBus commmands structure
extern cmd_space_t cmds;

extern uint32_t TMR_ERROR_CNT;

#ifdef MMRTSB
#define MAX_PAGE 3
#else
#define MAX_PAGE 2
#endif

uint16_t volts_lin[MAX_PAGE+1];
uint16_t currs_lin[MAX_PAGE+1];
uint16_t powrs_lin[MAX_PAGE+1];
uint16_t temps_lin[3];
uint16_t frpms_lin[3];

volatile uint32_t seconds_up __xMR;

#define TOTAL_PWM_CLOCKS 255

int16_t curr_pwm_duty1000_1 = 10;
int16_t curr_pwm_duty1000_2 = 10;
int16_t curr_pwm_duty1000_3 = 10;

#define FAN_RAMPUP 950

int16_t goal_pwm_duty1000_1 = FAN_RAMPUP;
int16_t goal_pwm_duty1000_2 = FAN_RAMPUP;
int16_t goal_pwm_duty1000_3 = FAN_RAMPUP;

static uint16_t iter __xMR;

#if defined(MMFANT) || defined(MMPROT)
static uint16_t fan_ramp_up_count __xMR = 0;
#endif

uint16_t tacho_cnt[3];
uint16_t tacho_rpm[3];

// when fan_cmdrpm is 0, setfrpms and temp_curve_points_y represent
// duty cycle (0 to 1000)
#if defined(MMFANT) || defined(MMPROT)
uint16_t fan_installed[3] __xMR = {1, 1, 1};
uint16_t setfrpms[3] __xMR = {500, 500, 500};
#else
uint16_t fan_installed[3] __xMR = {0, 0, 0};
uint16_t setfrpms[3] __xMR = {0, 0, 0};
#endif
uint16_t fan_cmdrpm[3] __xMR = {0, 0, 0};
uint16_t fan_ppr[3] __xMR = {1, 1, 1}; // a 1 here means 2 pulses per revolution

// static user_flash_t myflash __attribute__((aligned(256))) __attribute__((used)) = {.copy_fw = 0, .setfrpms = {600, 600, 600}, .fan_installed = {1, 1, 1}, .fan_cmdrpm = {0, 0, 0}, .fan_ppr = {1, 1, 1}}; // helper struct to grab user_flash.bin initialization data

pid_cntrl_t PID1 __xMR;
pid_cntrl_t PID2 __xMR;
pid_cntrl_t PID3 __xMR;

float temp_curve_points_x[3][3] __xMR = {{20, 25, 30}, {20, 25, 30}, {20, 25, 30}};
float temp_curve_points_y[3][3] __xMR = {{1000, 2000, 5000}, {1000, 2000, 4000}, {100, 200, 1000}};
float temp_matrix[3][3] __xMR = {{1, 0, 0}, {0, 1, 0}, {1, 0, 0}};
uint16_t temp_control_on __xMR = 0;

// V_ref and the readout it corresponds to
#define VREF 2.5
#define ADC_MAX 65535.0

// V[V] = V_adc * scaling_coeff
// I[mA] = V_adc * scaling_coeff
// T[degC] = (V_adc + a) * b
#if defined MMRTSB
#define ADC_CH_MAX 18
// Voltages: P12V, P1V2, P3V3, PPERIPH
const float volt_scaling_coeff[4] = {(10.0+2.0)/2.0, 1.0/(1.0+(1.0/1.0)), (4.7+10.0)/10.0, (4.7+2.0)/2.0};
// Current: P12V
const float curr_scaling_coeff[3] = {(1.0/10.0)/0.05, 0, 0};
// Temperatures: ambient, 2.5V LDO, FPGA
const float temp_coeff_a[3] = {0, 0, 0};
const float temp_coeff_b[3] = {100.0/(1.0+10.0/10.0), 100.0/(1.0+10.0/10.0), 100.0/(1.0+10.0/10.0)};
#elif defined MMFANT
#define ADC_CH_MAX 11
// Voltages: P12V, P5V
const float volt_scaling_coeff[3] = {(10.0+2.0)/2.0, (16.0+10.0)/10.0, 0};
// Currents: P12V, P5V
const float curr_scaling_coeff[3] = {(200.0+2000.0)/200.0, (200.0+2000.0)/200.0, (200.0+2000.0)/200.0};
const float temp_coeff_a[3] = {0, 0, 0};
const float temp_coeff_b[3] = {100.0/(1.0+10.0/10.0), 0, 0};
#elif defined MMRATO
#define ADC_CH_MAX 11
// Voltages: P12V, P5V
const float volt_scaling_coeff[3] = {(10.0+2.0)/2.0, (16.0+10.0)/10.0, 0.0};
// Currents: P12V, P5V
const float curr_scaling_coeff[3] = {(16.0+10.0)/10.0, (16.0+10.0)/10.0, 0.0};
// Temperatures: ambient
const float temp_coeff_a[3] = {0, 0, 0};
const float temp_coeff_b[3] = {100.0/(1.0+10.0/10.0), 0, 0};
#elif defined MMPROT
#define ADC_CH_MAX 11
// Voltages: P12V, P5V, P5V
const float volt_scaling_coeff[3] = {(10.0+2.0)/2.0, (18.0+10.0)/10.0, (10.0+10.0)/10.0};
// Currents: P12V, P5V, P5V
const float curr_scaling_coeff[3] = {(1.0/10.0)/0.1, (1.0/10.0)/10.0, (1.0/10.0)/10.0};
const float temp_coeff_a[3] = {0, -1e-3*100.0*19.0, -1e-3*100.0*19.0};
const float temp_coeff_b[3] = {100.0, 100.0/(1e-3*19.0*(138.5-100.0)), 100.0/(1e-3*19.0*(138.5-100.0))};
#else
#error Exactly one of MMRTSB, MMFANT or MMRATO has to be defined
#define ADC_CH_MAX 0
const float volt_scaling_coeff[3] = {0, 0, 0};
const float curr_scaling_coeff[3] = {0, 0, 0};
const float temp_coeff_a[3] = {0, 0, 0};
const float temp_coeff_b[3] = {0, 0, 0};
#endif

uint16_t adc_ch __xMR;
uint16_t adc_vals[ADC_CH_MAX+1];
float temps[3];
float volts[4];
float currs[3];

uint8_t status_b;

uint16_t trig_adc_next_second __xMR = 0;

#if defined(MMFANT) || defined(MMPROT)

int16_t sign(int16_t x)
{
        if (x < 0)
                return -1;
        if (x > 0)
                return 1;
        return 0;
}

void __xMR update_rpm()
{
        // 2 sensor periods = 1 revolution
        if (fan_installed[0]) {
                tacho_rpm[0] = 60*tacho_cnt[0]/(1+fan_ppr[0]);
                tacho_cnt[0] = 0;
                frpms_lin[0] = float_to_linear(tacho_rpm[0]);
        } else {
                tacho_rpm[0] = 0;
                tacho_cnt[0] = 0;
                frpms_lin[0] = float_to_linear(0);
        }

        if (fan_installed[1]) {
                tacho_rpm[1] = 60*tacho_cnt[1]/(1+fan_ppr[1]);
                tacho_cnt[1] = 0;
                frpms_lin[1] = float_to_linear(tacho_rpm[1]);
        } else {
                tacho_rpm[1] = 0;
                tacho_cnt[1] = 0;
                frpms_lin[1] = float_to_linear(0);
        }

        if (fan_installed[2]) {
                tacho_rpm[2] = 60*tacho_cnt[2]/(1+fan_ppr[2]);
                tacho_cnt[2] = 0;
                frpms_lin[2] = float_to_linear(tacho_rpm[2]);
        } else {
                tacho_rpm[2] = 0;
                tacho_cnt[2] = 0;
                frpms_lin[2] = float_to_linear(0);
        }
}

void __xMR update_pwm()
{
        if (fan_installed[0]) {
                int16_t add_factor_1 = (goal_pwm_duty1000_1 - curr_pwm_duty1000_1)/50;
                if (add_factor_1 == 0)
                        add_factor_1 = sign(goal_pwm_duty1000_1 - curr_pwm_duty1000_1);
                curr_pwm_duty1000_1 = curr_pwm_duty1000_1 + add_factor_1;
        } else {
                curr_pwm_duty1000_1 = 0;
        }

        if (fan_installed[1]) {
                int16_t add_factor_2 = (goal_pwm_duty1000_2 - curr_pwm_duty1000_2)/50;
                if (add_factor_2 == 0)
                        add_factor_2 = sign(goal_pwm_duty1000_2 - curr_pwm_duty1000_2);
                curr_pwm_duty1000_2 = curr_pwm_duty1000_2 + add_factor_2;
        } else {
                curr_pwm_duty1000_2 = 0;
        }

        if (fan_installed[2]) {
                int16_t add_factor_3 = (goal_pwm_duty1000_3 - curr_pwm_duty1000_3)/50;
                if (add_factor_3 == 0)
                        add_factor_3 = sign(goal_pwm_duty1000_3 - curr_pwm_duty1000_3);
                curr_pwm_duty1000_3 = curr_pwm_duty1000_3 + add_factor_3;
        } else {
                curr_pwm_duty1000_3 = 0;
        }

        uint16_t target_duty_1 = (TOTAL_PWM_CLOCKS*curr_pwm_duty1000_1)/1000;
        uint16_t target_duty_2 = (TOTAL_PWM_CLOCKS*curr_pwm_duty1000_2)/1000;
        uint16_t target_duty_3 = (TOTAL_PWM_CLOCKS*curr_pwm_duty1000_3)/1000;

	pwm_set_parameters(&PWM_0, TOTAL_PWM_CLOCKS, (target_duty_1 < 255)?(target_duty_1):(254));
        pwm_set_parameters(&PWM_1, TOTAL_PWM_CLOCKS, (target_duty_2 < 255)?(target_duty_2):(254));
        pwm_set_parameters(&PWM_2, TOTAL_PWM_CLOCKS, (target_duty_3 < 255)?(target_duty_3):(254));
}

#endif

void __xMR adc_cb(const struct adc_async_descriptor *const descr, const uint8_t channel)
{
        adc_async_read_channel(&ADC_0, 0, (uint8_t *)&(adc_vals[adc_ch]), 2);
        // debug("ADC has just read ch %u", adc_ch);
        // channels are ordered from AIN0 to AIN11
        // breakout board has the temp sensor connected to AIN10 and two pins at AIN4 and AIN5
        // prototype mapping:
        // AIN0: t1
        // AIN1: REFA
        // AIN2: i2
        // AIN3: v2
        // AIN4: t2
        // AIN5: t3
        // AIN6: v1
        // AIN7: i1
        // AIN8: N/A
        // AIN9: N/A
        // AIN10: i3
        // AIN11: v3
        // any scaling is applied here
        // prototype board only has 1 pin of [t, v, i] connected, write some value to the other two

#ifdef USE_COAST
#ifdef TEST_TMR
        // Test TMR by trying to corrupt the adc_ch variable. By using
        // assembly we trick the TMR logic: it just sees one of the
        // three copies be corrupted, fixes it when it's accessed in
        // the switch statement below (ADC readout remains unaffected)
        // and increments the TMR_ERROR_CNT variable (implemented and
        // exposed in i2c_impl.c).
        asm volatile("movs	r4, 0x12;"
                     "lsls	r4, r4, 8;"
                     "adds	r4, 0x34;"
                     "strh	r4, [%0]"
                     ::"r"(&adc_ch):"r4");
#endif
#endif

        switch (adc_ch) {
        case 0:
                temps[0] = (VREF*(adc_vals[adc_ch] / ADC_MAX) + temp_coeff_a[0])*temp_coeff_b[0];
#ifdef DEBUG_PID
                temps_lin[0] = float_to_linear(PID1.p_factor);
#else
                temps_lin[0] = float_to_linear(temps[0]);
#endif
                // set MUXPOS and MUXNEG, 0x18 is internal GND
                adc_ch = 2;
                adc_async_set_inputs(&ADC_0, adc_ch, 0x18, 0);
                break;
        case 2:
                currs[1] = VREF*(adc_vals[adc_ch] / ADC_MAX)*curr_scaling_coeff[1];
#ifdef DEBUG_PID
                currs_lin[1] = float_to_linear(PID1.output_sum);
#else
                currs_lin[1] = float_to_linear(currs[1]);
#endif
                adc_ch = 3;
                adc_async_set_inputs(&ADC_0, adc_ch, 0x18, 0);
                break;
        case 3:
                volts[1] = VREF*(adc_vals[adc_ch] / ADC_MAX)*volt_scaling_coeff[1];
#ifdef USE_LINEAR16
                volts_lin[1] = float_to_linear16(volts[1]);
#else
                volts_lin[1] = float_to_linear(volts[1]);
#endif
                powrs_lin[1] = float_to_linear(volts[1] * currs[1]);
                adc_ch = 4;
                adc_async_set_inputs(&ADC_0, adc_ch, 0x18, 0);
                break;
        case 4:
                temps[1] = (VREF*(adc_vals[adc_ch] / ADC_MAX) + temp_coeff_a[1])*temp_coeff_b[1];
#ifdef DEBUG_PID
                temps_lin[1] = float_to_linear(PID1.i_factor);
#else
                temps_lin[1] = float_to_linear(temps[1]);
#endif
                adc_ch = 5;
                adc_async_set_inputs(&ADC_0, adc_ch, 0x18, 0);
                break;
        case 5:
                temps[2] = (VREF*(adc_vals[adc_ch] / ADC_MAX) + temp_coeff_a[2])*temp_coeff_b[2];
#ifdef DEBUG_PID
                temps_lin[2] = float_to_linear(PID1.d_factor);
#else
                temps_lin[2] = float_to_linear(temps[2]);
#endif
                adc_ch = 6;
                adc_async_set_inputs(&ADC_0, adc_ch, 0x18, 0);
                break;
        case 6:
                volts[0] = VREF*(adc_vals[adc_ch] / ADC_MAX)*volt_scaling_coeff[0];
#ifdef USE_LINEAR16
                volts_lin[0] = float_to_linear16(volts[0]);
#else
                volts_lin[0] = float_to_linear(volts[0]);
#endif
                adc_ch = 7;
                adc_async_set_inputs(&ADC_0, adc_ch, 0x18, 0);
                break;
        case 7:
                currs[0] = VREF*(adc_vals[adc_ch] / ADC_MAX)*curr_scaling_coeff[0];
#ifdef DEBUG_PID
                currs_lin[0] = float_to_linear(PID1.last_input);
#else
                currs_lin[0] = float_to_linear(currs[0]);
#endif
                powrs_lin[0] = float_to_linear(volts[0] * currs[0]);
                adc_ch = 10;
                adc_async_set_inputs(&ADC_0, adc_ch, 0x18, 0);
                break;
        case 10:
                currs[2] = VREF*(adc_vals[adc_ch] / ADC_MAX)*curr_scaling_coeff[2];
#ifdef DEBUG_PID
                currs_lin[2] = float_to_linear(PID1.setpoint);
#else
                currs_lin[2] = float_to_linear(currs[2]);
#endif
                adc_ch = 11;
                adc_async_set_inputs(&ADC_0, adc_ch, 0x18, 0);
                break;
        case 11:
                volts[2] = VREF*(adc_vals[adc_ch] / ADC_MAX)*volt_scaling_coeff[2];
#ifdef DEBUG_PID
                volts_lin[2] = float_to_linear(goal_pwm_duty1000_1);
#else
# ifdef USE_LINEAR16
                volts_lin[2] = float_to_linear16(volts[2]);
# else
                volts_lin[2] = float_to_linear(volts[2]);
# endif
#endif
                powrs_lin[2] = float_to_linear(volts[2] * currs[2]);
#ifdef MMRTSB
                adc_ch = 18;
#else
                adc_ch = 0;
#endif
                adc_async_set_inputs(&ADC_0, adc_ch, 0x18, 0);
                break;
#ifdef MMRTSB
        case 18:
                volts[3] = VREF*(adc_vals[adc_ch] / ADC_MAX)*volt_scaling_coeff[3];
                volts_lin[3] = float_to_linear(volts[3]);
                powrs_lin[3] = float_to_linear(0);
                adc_ch = 0;
                adc_async_set_inputs(&ADC_0, adc_ch, 0x18, 0);
                break;
#endif
        }
        if (adc_ch == 0)
                trig_adc_next_second = 1;
        else
                adc_async_start_conversion(&ADC_0);
}

static void __xMR mytimercallback(const struct timer_task *const timer_task)
{
#if defined(MMFANT) || defined(MMPROT)
        update_pwm();
#endif
        if (iter == 99) {
                ++seconds_up;
                iter = 0;
#if defined(MMFANT) || defined(MMPROT)
                update_rpm();
                if (temp_control_on) {
                        PID1.setpoint = compute_rpm(0);
                        PID2.setpoint = compute_rpm(1);
                        PID3.setpoint = compute_rpm(2);
                } else {
                        PID1.setpoint = setfrpms[0];
                        PID2.setpoint = setfrpms[1];
                        PID3.setpoint = setfrpms[2];
                }

                if (fan_ramp_up_count < 3) {
                        ++fan_ramp_up_count;

                        goal_pwm_duty1000_1 = FAN_RAMPUP;
                        goal_pwm_duty1000_2 = FAN_RAMPUP;
                        goal_pwm_duty1000_3 = FAN_RAMPUP;

                        PID1.last_input = tacho_rpm[0];
                        PID2.last_input = tacho_rpm[1];
                        PID3.last_input = tacho_rpm[2];

                        PID1.setpoint = FAN_RAMPUP;
                        PID2.setpoint = FAN_RAMPUP;
                        PID3.setpoint = FAN_RAMPUP;
                } else {
			/* Pull-up tacho input, but only after fans started */
			gpio_set_pin_pull_mode(PIN_PA17, GPIO_PULL_UP);
			gpio_set_pin_pull_mode(PIN_PA16, GPIO_PULL_UP);
			gpio_set_pin_pull_mode(PIN_PA13, GPIO_PULL_UP);
                        if (fan_cmdrpm[0])
                                goal_pwm_duty1000_1 = pid_compute(&PID1, tacho_rpm[0]);
                        else
                                goal_pwm_duty1000_1 = PID1.setpoint;
                        if (fan_cmdrpm[1])
                                goal_pwm_duty1000_2 = pid_compute(&PID2, tacho_rpm[1]);
                        else
                                goal_pwm_duty1000_2 = PID2.setpoint;
                        if (fan_cmdrpm[2])
                                goal_pwm_duty1000_3 = pid_compute(&PID3, tacho_rpm[2]);
                        else
                                goal_pwm_duty1000_3 = PID3.setpoint;
                }
#endif
                if (trig_adc_next_second) {
                        trig_adc_next_second = 0;
                        adc_async_start_conversion(&ADC_0);
                }
        } else
                ++iter;

        wdt_feed(&WDT_0);
}

#if defined(MMFANT) || defined(MMPROT)

static void inc_tacho1(void)
{
        ++tacho_cnt[0];
}

static void inc_tacho2(void)
{
        ++tacho_cnt[1];
}

static void inc_tacho3(void)
{
        ++tacho_cnt[2];
}

void __xMR load_fan_config()
{
        for (uint8_t i = 0; i < 3; ++i) {
                fan_installed[i] = user_flash.fan_installed[i];
                fan_cmdrpm[i] = user_flash.fan_cmdrpm[i];
                fan_ppr[i] = user_flash.fan_ppr[i];
                setfrpms[i] = user_flash.setfrpms[i];
        }
}

#endif

struct timer_task mytask;

static void __attribute__((used)) __attribute__((noinline))
__attribute__((noreturn)) __attribute__((__section__(".endless_loop")))
endless_loop(void)
{
        asm volatile("" :::);
        while (1)
                ;
}

int main(void)
{
	/* Initializes MCU, drivers and middleware */
	atmel_start_init();
        gpio_set_pin_pull_mode(ADDR0, GPIO_PULL_UP);
        gpio_set_pin_pull_mode(ADDR1, GPIO_PULL_UP);
        gpio_set_pin_pull_mode(ADDR2, GPIO_PULL_UP);

#if defined(MMFANT) || defined(MMPROT)
        ext_irq_register(PIN_PA17, inc_tacho1);
        ext_irq_register(PIN_PA16, inc_tacho2);
        ext_irq_register(PIN_PA13, inc_tacho3);
        ext_irq_enable(PIN_PA17);
        ext_irq_enable(PIN_PA16);
        ext_irq_enable(PIN_PA13);

        load_fan_config();

        update_pwm();
	pwm_enable(&PWM_0);
	pwm_enable(&PWM_1);
	pwm_enable(&PWM_2);
#else
        frpms_lin[0] = float_to_linear(0);
        frpms_lin[1] = float_to_linear(0);
        frpms_lin[2] = float_to_linear(0);
#endif
        adc_async_register_callback(&ADC_0, 0, ADC_ASYNC_CONVERT_CB, &adc_cb);
	adc_async_enable_channel(&ADC_0, 0);
        adc_async_start_conversion(&ADC_0);

        setup_I2C_slave(&cmds);

        // set the timer interrupt to a lower priority otherwise i2c won't work
        NVIC_SetPriority(RTC_IRQn, 1);

        mytask.interval = 1;
        mytask.cb = mytimercallback;
        mytask.mode = TIMER_TASK_REPEAT;
        timer_add_task(&TIMER_0, &mytask);
        timer_start(&TIMER_0);

        wdt_set_timeout_period(&WDT_0, 1000, 8192);
        wdt_enable(&WDT_0);

        SCB->SCR |= SCB_SCR_SLEEPONEXIT_Msk;

        TMR_ERROR_CNT = 0;      // initialize TMR error counter

        endless_loop();
}

#ifdef USE_STACK_PROTECTOR
void emergency_recover(void);

void __stack_chk_fail(void)
{
        emergency_recover();
}
#endif
