#include <atmel_start.h>
#include <stdlib.h>

#include <COAST.h>
#include <i2c_slave.h>
#include <flash_utils.h>
#include <float_utils.h>
#include <usb_debug.h>

#define STATUS_BYTE_CML_SHIFT 1
#define FAN_N 3

const char MFR_ID[] = "CERN (BE/CEM)";
const char MFR_MDL[] = "DI/OT MoniMod";
#if defined MMRTSB
const char MFR_REV[] = "0.3p-RTSB";
#elif defined MMFANT
const char MFR_REV[] = "0.3p-FANT";
#elif defined MMRATO
const char MFR_REV[] = "0.3p-RATO";
#elif defined MMPROT
const char MFR_REV[] = "0.3p-PROT";
#else
const char MFR_REV[] = "0.3p-ERRO";
#error Exactly one of MMRTSB, MMFANT, MMRATO or MMPROT has to be defined
#endif
const char MFR_LOC[] = "Geneva";
const char MFR_DAT[] = "211020";
const char MFR_SER[] = "123456789";
const char IC_DEVICE_REV[] = __GIT_VER__;

#ifdef USE_LINEAR16
const uint8_t vout_mode = 0x16; // linear mode, fixed 2^10 exponent
#else
const uint8_t vout_mode = 0xe0; // not compliant to the standard, set mode bits to (invalid) 3'b111
#endif

void page_chk();
uint8_t page_tmp;
uint16_t page __xMR;

extern uint16_t use_pec __xMR;
uint8_t use_pec_tmp;
void __xMR get_pec(void);
void __xMR set_pec(void);

void read_status_b(void);
void get_status_cml(void);
void write_status_cml(void);
static uint8_t status_b;
extern uint8_t status_cml;
static uint8_t status_cml_tmp;

void accvolt();
void acccurr();
void accpowr();
uint16_t curpage_volt;
uint16_t curpage_curr;
uint16_t curpage_powr;

void query_prp();
void query_cln();
uint8_t query_r;

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

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

extern uint32_t seconds_up;

uint32_t TMR_ERROR_CNT;

static uint8_t reset_byte;
static uint8_t fw_type;
static uint8_t capablity;

static uint8_t tc_curve_curr_fan;
static uint8_t tc_matrix_curr_fan;
struct __attribute__((__packed__)) temp_curve_point {
    uint16_t t;
    uint16_t s;
};

struct __attribute__((__packed__)) {
    uint8_t fan_n;
    struct temp_curve_point points[3];
} temp_curve_points_data;

struct __attribute__((__packed__)) {
    uint8_t fan_n;
    uint16_t m[3];
} temp_matrix_row;


void get_capablity(void);
void get_frpms(void);
void set_frpms();
uint16_t setfrpms_lin[3];
extern uint16_t setfrpms[3] __xMR;

void get_tc_curve();
void set_tc_curve();

extern float temp_curve_points_x[3][3] __xMR;
extern float temp_curve_points_y[3][3] __xMR;

void get_tc_matrix();
void set_tc_matrix();
void set_tc_onoff();

uint8_t tc_on;
extern float temp_matrix[3][3] __xMR;
extern uint16_t temp_control_on __xMR;

uint8_t fan_config_1_2;
uint8_t fan_config_3_4;
void fan_config();
void get_fan_configs();
extern uint16_t fan_installed[3] __xMR;
extern uint16_t fan_cmdrpm[3] __xMR;
extern uint16_t fan_ppr[3] __xMR;

void uc_reset();
void get_fw_type(void);
void boot_new_fw();

enum {
        cmd_0x00 = 0,
        cmd_0x19,
        cmd_0x1A,
        cmd_0x20,
        cmd_0x3A,
        cmd_0x3B,
        cmd_0x3C,
        cmd_0x3D,
        cmd_0x3E,
        cmd_0x78,
        cmd_0x7E,
        cmd_0x8B,
        cmd_0x8C,
        cmd_0x8D,
        cmd_0x8E,
        cmd_0x8F,
        cmd_0x90,
        cmd_0x91,
        cmd_0x92,
        cmd_0x96,
        cmd_0x99,
        cmd_0x9A,
        cmd_0x9B,
        cmd_0x9C,
        cmd_0x9D,
        cmd_0x9E,
        cmd_0xAE,
        /* MFR specific commands */
        cmd_0xD5,
        cmd_0xD6,
        cmd_0xD7,
        cmd_0xD8,
        cmd_0xD9,
        cmd_0xE0,
        cmd_0xE1,
        cmd_0xE2,
};

/* "- 1" to avoid adding null char at the end of a string */
static const int8_t cmd_data_lengths[] = {
        [cmd_0x00] = 1,
        [cmd_0x19] = 1,
        [cmd_0x1A] = -1,
        [cmd_0x20] = 1,
        [cmd_0x3A] = 1,
        [cmd_0x3B] = 2,
        [cmd_0x3C] = 2,
        [cmd_0x3D] = 1,
        [cmd_0x3E] = 2,
        [cmd_0x78] = 1,
        [cmd_0x7E] = 1,
        [cmd_0x8B] = 2,
        [cmd_0x8C] = 2,
        [cmd_0x8D] = 2,
        [cmd_0x8E] = 2,
        [cmd_0x8F] = 2,
        [cmd_0x90] = 2,
        [cmd_0x91] = 2,
        [cmd_0x92] = 2,
        [cmd_0x96] = 2,
        [cmd_0x99] = -((int8_t)sizeof(MFR_ID) - 1),
        [cmd_0x9A] = -((int8_t)sizeof(MFR_MDL) - 1),
        [cmd_0x9B] = -((int8_t)sizeof(MFR_REV) - 1),
        [cmd_0x9C] = -((int8_t)sizeof(MFR_LOC) - 1),
        [cmd_0x9D] = -((int8_t)sizeof(MFR_DAT) - 1),
        [cmd_0x9E] = -((int8_t)sizeof(MFR_SER) - 1),
        [cmd_0xAE] = -((int8_t)sizeof(IC_DEVICE_REV) - 1),
        /* MFR specific commands */
        [cmd_0xD5] = 1,
        [cmd_0xD6] = 1,
        [cmd_0xD7] = -((int8_t)sizeof(seconds_up)), /* no null char, no -1 */
        [cmd_0xD8] = -((int8_t)sizeof(TMR_ERROR_CNT)), /* no null char, no -1 */
        [cmd_0xD9] = 1,
        [cmd_0xE0] = -((int8_t)sizeof(temp_curve_points_data)), /* no null char, no -1 */
        [cmd_0xE1] = -((int8_t)sizeof(temp_matrix_row)), /* no null char, no -1 */
        [cmd_0xE2] = 1
};

// only block write-block read process calls modify the length
static int8_t cmd_data_length_query = -1;

static const cmd_t cmds_cmds[] = (cmd_t[]){
/*       addr, int8_t *const data_len,                uint8_t *const data_pnt,            access callback,  write callback,    read callback,    query byte                          PEC */
        {0x00, (int8_t *)&cmd_data_lengths[cmd_0x00], (uint8_t *)&page_tmp,               (fp_t)NULL,       &page_chk,         (fp_t)NULL,       QUERY_WR | QUERY_RD | QUERY_FMT_8B, 0},
        {0x19, (int8_t *)&cmd_data_lengths[cmd_0x19], (uint8_t *)&capablity,              &get_capablity,   (fp_t)NULL,        (fp_t)NULL,       QUERY_RD,                           0},
        {0x1A, (int8_t *)&cmd_data_length_query,      (uint8_t *)&query_r,                (fp_t)NULL,       &query_prp,        (fp_t)NULL,       QUERY_WR,                           1},
        {0x20, (int8_t *)&cmd_data_lengths[cmd_0x20], (uint8_t *)&vout_mode,              (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD,                           1},
        {0x3A, (int8_t *)&cmd_data_lengths[cmd_0x3A], (uint8_t *)&fan_config_1_2,         &get_fan_configs, &fan_config,       (fp_t)NULL,       QUERY_WR | QUERY_RD | QUERY_FMT_NAN,0},
        {0x3B, (int8_t *)&cmd_data_lengths[cmd_0x3B], (uint8_t *)&setfrpms_lin[0],        &get_frpms,       &set_frpms,        (fp_t)NULL,       QUERY_WR | QUERY_RD | QUERY_FMT_LIN,0},
        {0x3C, (int8_t *)&cmd_data_lengths[cmd_0x3C], (uint8_t *)&setfrpms_lin[1],        &get_frpms,       &set_frpms,        (fp_t)NULL,       QUERY_WR | QUERY_RD | QUERY_FMT_LIN,0},
        {0x3D, (int8_t *)&cmd_data_lengths[cmd_0x3D], (uint8_t *)&fan_config_3_4,         &get_fan_configs, &fan_config,       (fp_t)NULL,       QUERY_WR | QUERY_RD | QUERY_FMT_NAN,0},
        {0x3E, (int8_t *)&cmd_data_lengths[cmd_0x3E], (uint8_t *)&setfrpms_lin[2],        &get_frpms,       &set_frpms,        (fp_t)NULL,       QUERY_WR | QUERY_RD | QUERY_FMT_LIN,0},
        {0x78, (int8_t *)&cmd_data_lengths[cmd_0x78], (uint8_t *)&status_b,               &read_status_b,   (fp_t)NULL,        (fp_t)NULL,       QUERY_RD,                           0},
        {0x7E, (int8_t *)&cmd_data_lengths[cmd_0x7E], (uint8_t *)&status_cml_tmp,         &get_status_cml,  &write_status_cml, (fp_t)NULL,       QUERY_WR | QUERY_RD,                0},
        {0x8B, (int8_t *)&cmd_data_lengths[cmd_0x8B], (uint8_t *)&curpage_volt,           &accvolt,         (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_LIN,           0},
        {0x8C, (int8_t *)&cmd_data_lengths[cmd_0x8C], (uint8_t *)&curpage_curr,           &acccurr,         (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_LIN,           0},
        {0x8D, (int8_t *)&cmd_data_lengths[cmd_0x8D], (uint8_t *)&temps_lin[0],           (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_LIN,           0},
        {0x8E, (int8_t *)&cmd_data_lengths[cmd_0x8E], (uint8_t *)&temps_lin[1],           (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_LIN,           0},
        {0x8F, (int8_t *)&cmd_data_lengths[cmd_0x8F], (uint8_t *)&temps_lin[2],           (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_LIN,           0},
        {0x90, (int8_t *)&cmd_data_lengths[cmd_0x90], (uint8_t *)&frpms_lin[0],           (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_LIN,           0},
        {0x91, (int8_t *)&cmd_data_lengths[cmd_0x91], (uint8_t *)&frpms_lin[1],           (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_LIN,           0},
        {0x92, (int8_t *)&cmd_data_lengths[cmd_0x92], (uint8_t *)&frpms_lin[2],           (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_LIN,           0},
        {0x96, (int8_t *)&cmd_data_lengths[cmd_0x96], (uint8_t *)&curpage_powr,           &accpowr,         (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_LIN,           0},
        {0x99, (int8_t *)&cmd_data_lengths[cmd_0x99], (uint8_t *)&MFR_ID,                 (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_NAN,           0},
        {0x9A, (int8_t *)&cmd_data_lengths[cmd_0x9A], (uint8_t *)&MFR_MDL,                (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_NAN,           0},
        {0x9B, (int8_t *)&cmd_data_lengths[cmd_0x9B], (uint8_t *)&MFR_REV,                (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_NAN,           0},
        {0x9C, (int8_t *)&cmd_data_lengths[cmd_0x9C], (uint8_t *)&MFR_LOC,                (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_NAN,           0},
        {0x9D, (int8_t *)&cmd_data_lengths[cmd_0x9D], (uint8_t *)&MFR_DAT,                (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_NAN,           0},
        {0x9E, (int8_t *)&cmd_data_lengths[cmd_0x9E], (uint8_t *)&MFR_SER,                (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_NAN,           0},
        {0xAE, (int8_t *)&cmd_data_lengths[cmd_0xAE], (uint8_t *)&IC_DEVICE_REV,          (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_NAN,           0},
        {0xD5, (int8_t *)&cmd_data_lengths[cmd_0xD5], (uint8_t *)&fw_type,                &get_fw_type,     &boot_new_fw,      (fp_t)NULL,       QUERY_WR | QUERY_RD,                0},
        {0xD6, (int8_t *)&cmd_data_lengths[cmd_0xD6], (uint8_t *)&reset_byte,             (fp_t)NULL,       &uc_reset,         (fp_t)NULL,       QUERY_WR,                           0},
        {0xD7, (int8_t *)&cmd_data_lengths[cmd_0xD7], (uint8_t *)&seconds_up,             (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_RD | QUERY_FMT_NAN,           0},
        {0xD8, (int8_t *)&cmd_data_lengths[cmd_0xD8], (uint8_t *)&TMR_ERROR_CNT,          (fp_t)NULL,       (fp_t)NULL,        (fp_t)NULL,       QUERY_WR | QUERY_RD | QUERY_FMT_NAN,0},
        {0xD9, (int8_t *)&cmd_data_lengths[cmd_0xD9], (uint8_t *)&use_pec_tmp,            &get_pec,         &set_pec,          (fp_t)NULL,       QUERY_WR | QUERY_RD,                1},
        {0xE0, (int8_t *)&cmd_data_lengths[cmd_0xE0], (uint8_t *)&temp_curve_points_data, &get_tc_curve,    &set_tc_curve,     (fp_t)NULL,       QUERY_WR | QUERY_RD | QUERY_FMT_NAN,0},
        {0xE1, (int8_t *)&cmd_data_lengths[cmd_0xE1], (uint8_t *)&temp_matrix_row,        &get_tc_matrix,   &set_tc_matrix,    (fp_t)NULL,       QUERY_WR | QUERY_RD | QUERY_FMT_NAN,0},
        {0xE2, (int8_t *)&cmd_data_lengths[cmd_0xE2], (uint8_t *)&tc_on,                  (fp_t)NULL,       &set_tc_onoff,     (fp_t)NULL,       QUERY_WR | QUERY_RD,                0}
};

cmd_space_t cmds = {
        sizeof(cmds_cmds)/sizeof(cmd_t),
        cmds_cmds
};

static uint32_t fw_write_buf[FLASH_ROW_SIZE/4];

void uc_reset()
{
        /* Check magic */
        if (reset_byte != UC_RESET_MAGIC) {
                /* Wrong magic, abort */
                return;
        }

#ifdef USB_ENABLE
        usbdc_detach();
        usbdc_stop();
        usbdc_deinit();
#endif
        NVIC_SystemReset();
}

void get_fw_type(void)
{
        fw_type = FW_MAIN;
}

void boot_new_fw()
{
        uint32_t *char_user_flash = (uint32_t *)&user_flash;
        user_flash_t *tmp_user_flash = (user_flash_t *)&fw_write_buf;

        /* Check magic */
        if (fw_type != BOOT_NEW_FW_MAGIC) {
                /* Wrong magic, abort */
                return;
        }

        /* backup existing user data */
        for (uint8_t i = 0; i < FLASH_ROW_SIZE/4; ++i)
                fw_write_buf[i] = char_user_flash[i];

        /* set 0xBEC0ABCD copy FW code */
        tmp_user_flash->copy_fw = 0xbec0abcd;

        /* copy modified block back to the flash */
        flash_write_row((uint32_t *)&user_flash, (uint32_t *)fw_write_buf);

        /* Set a proper Magic for uC reset */
        reset_byte = UC_RESET_MAGIC;
        /* Reset the uC */
        uc_reset();
}


void __xMR page_chk()
{
        if (page_tmp > MAX_PAGE)
                page = MAX_PAGE;
        else
                page = page_tmp;
}

void __xMR get_capablity()
{
        capablity = use_pec ? CAPABILITY_PEC : 0;
}

void __xMR accvolt()
{
        curpage_volt = volts_lin[page];
}


void __xMR acccurr()
{
        curpage_curr = currs_lin[page];
}


void __xMR accpowr()
{
        curpage_powr = powrs_lin[page];
}


void query_prp()
{
        int16_t cmd_idx = in_addr_space(&cmds, query_r);
        if (cmd_idx != -1)
                query_r = QUERY_SUP | cmds.cmds[cmd_idx].query_byte;
        else
                query_r = 0;
}

void __xMR get_pec(void)
{
        use_pec_tmp = use_pec;
}

void __xMR set_pec(void)
{
        use_pec = !!use_pec_tmp;
}

void read_status_b(void)
{
        status_b = status_cml ? 1 << STATUS_BYTE_CML_SHIFT : 0;
}


void get_status_cml(void)
{
        /* Set CML_MEM_FAULT bit if TMR error detected */
        status_cml |= TMR_ERROR_CNT ? CML_MEM_FAULT : 0;

        status_cml_tmp = status_cml;
}

void write_status_cml(void)
{
        /* Clear only bits set to 1 */
        status_cml &= ~status_cml_tmp;

        /* Set TMR_ERROR_CNT to zero if CML_MEM_FAULT bit is to be cleared */
        if (status_cml_tmp & CML_MEM_FAULT)
                TMR_ERROR_CNT = 0;
}

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

void __xMR fan_config()
{
        fan_installed[0] = (fan_config_1_2 >> 7) & 0x1;
        fan_cmdrpm[0] = (fan_config_1_2 >> 6) & 0x1;
        fan_ppr[0] = (fan_config_1_2 >> 4) & 0x3;

        fan_installed[1] = (fan_config_1_2 >> 3) & 0x1;
        fan_cmdrpm[1] = (fan_config_1_2 >> 2) & 0x1;
        fan_ppr[1] = fan_config_1_2 & 0x3;

        fan_installed[2] = (fan_config_3_4 >> 7) & 0x1;
        fan_cmdrpm[2] = (fan_config_3_4 >> 6) & 0x1;
        fan_ppr[2] = (fan_config_3_4 >> 4) & 0x3;

        uint32_t *char_user_flash = (uint32_t *)&user_flash;
        user_flash_t *tmp_user_flash = (user_flash_t *)&fw_write_buf;

        /* backup existing user data */
        for (uint8_t i = 0; i < FLASH_ROW_SIZE/4; ++i)
                fw_write_buf[i] = char_user_flash[i];

        /* set 0xBEC0ABCD copy FW code */
        for (uint8_t i = 0; i < 3; ++i) {
                tmp_user_flash->fan_installed[i] = fan_installed[i];
                tmp_user_flash->fan_cmdrpm[i] = fan_cmdrpm[i];
                tmp_user_flash->fan_ppr[i] = fan_ppr[i];
        }

        /* copy modified block back to the flash */
        flash_write_row((uint32_t *)&user_flash, (uint32_t *)fw_write_buf);
}

void __xMR get_fan_configs()
{
        fan_config_1_2 = (fan_installed[0] << 7) | (fan_cmdrpm[0] << 6) | (fan_ppr[0] << 4) |
                (fan_installed[1] << 3) | (fan_cmdrpm[1] << 2) | (fan_ppr[1]);
        fan_config_3_4 = (fan_installed[2] << 7) | (fan_cmdrpm[2] << 6) | (fan_ppr[2] << 4);
}

void __xMR get_frpms(void)
{
        /* get frpms from flash */
        for (uint8_t i = 0; i < 3; ++i)
                setfrpms_lin[i] = float_to_linear(user_flash.setfrpms[i]);
}

void __xMR set_frpms()
{
        setfrpms[0] = linear_to_float(setfrpms_lin[0]);
        setfrpms[1] = linear_to_float(setfrpms_lin[1]);
        setfrpms[2] = linear_to_float(setfrpms_lin[2]);

        uint32_t *char_user_flash = (uint32_t *)&user_flash;
        user_flash_t *tmp_user_flash = (user_flash_t *)&fw_write_buf;

        /* backup existing user data */
        for (uint8_t i = 0; i < FLASH_ROW_SIZE/4; ++i)
                fw_write_buf[i] = char_user_flash[i];

        /* set 0xBEC0ABCD copy FW code */
        for (uint8_t i = 0; i < 3; ++i)
                tmp_user_flash->setfrpms[i] = setfrpms[i];

        /* copy modified block back to the flash */
        flash_write_row((uint32_t *)&user_flash, (uint32_t *)fw_write_buf);
}

void __xMR get_tc_curve()
{
        uint8_t fan_n = tc_curve_curr_fan;

        for (uint8_t i = 0; i < FAN_N; ++i) {
                temp_curve_points_data.points[i].t = float_to_linear(temp_curve_points_x[fan_n][i]);
                temp_curve_points_data.points[i].s = float_to_linear(temp_curve_points_y[fan_n][i]);
        }

        temp_curve_points_data.fan_n = fan_n;
        /* Increase fan counter, at the next read return data for the next fan */
        tc_curve_curr_fan = (tc_curve_curr_fan + 1) % FAN_N;
}

void __xMR set_tc_curve()
{
        uint8_t fan_n = temp_curve_points_data.fan_n;

        if (fan_n >= FAN_N) {
            /* Fan number out of range */
            return;
        }

        for (uint8_t i = 0; i < FAN_N; ++i) {
                temp_curve_points_x[fan_n][i] = linear_to_float(temp_curve_points_data.points[i].t);
                temp_curve_points_y[fan_n][i] = linear_to_float(temp_curve_points_data.points[i].s);
        }

        tc_curve_curr_fan = fan_n;
}

void __xMR get_tc_matrix()
{
        uint8_t fan_n = tc_matrix_curr_fan;

        for (uint8_t i = 0; i < FAN_N; ++i) {
                temp_matrix_row.m[i] = float_to_linear(temp_matrix[fan_n][i]);
        }

        temp_matrix_row.fan_n = fan_n;
        /* Increase fan counter, at the next read return data for the next fan */
        tc_matrix_curr_fan = (tc_matrix_curr_fan + 1) % FAN_N;
}

void __xMR set_tc_matrix()
{
        uint8_t fan_n = temp_matrix_row.fan_n;

        if (fan_n >= FAN_N) {
            /* Fan number out of range */
            return;
        }

        for (uint8_t i = 0; i < FAN_N; ++i) {
                temp_matrix[fan_n][i] = linear_to_float(temp_matrix_row.m[i]);
        }

        tc_matrix_curr_fan = fan_n;
}


void __xMR set_tc_onoff()
{
        temp_control_on = tc_on;
}

#else

void fan_config()
{
}

void get_fan_configs()
{
}

void get_frpms(void)
{
}

void set_frpms()
{
}

void get_tc_curve()
{
}

void set_tc_curve()
{
}

void get_tc_matrix()
{
}

void set_tc_matrix()
{
}

void set_tc_onoff()
{
}

#endif
