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

#include <usb_debug.h>
#include <float_utils.h>
#include <checksums.h>
#include <i2c_master_mod_fns.h>

#define INTERACTIVE_MODE 1

#define MOD_FW_START 0x22000
#define MOD_FW_LENGTH 0x1e000

static uint16_t checksum;
static uint8_t oneline;
static uint8_t mm_selected;

void check_res_print_error(uint32_t res)
{
        if (!res)
                return;

        delay_ms(2);
        switch (res) {
        case ERR_BAD_DATA:
                debug("I2C error: ERR_BAD_DATA (res = %lx)", res);
                break;
        case I2C_ERR_BUSY:
                debug("I2C error: I2C_ERR_BUSY (res = %lx)", res);
                break;
        case I2C_ERR_BUS:
                debug("I2C error: I2C_ERR_BUS (res = %lx)", res);
                break;
        case I2C_ERR_BAD_ADDRESS:
                debug("I2C error: I2C_ERR_BAD_ADDRESS (res = %lx)", res);
                break;
        case I2C_NACK:
                debug("I2C error: I2C_NACK (res = %lx)", res);
                break;
        default:
                debug("I2C error: OTHER (res = %lx)", res);
        }
        delay_ms(2);
}

void select_monimod(uint8_t n)
{
        if (!n) {
                i2c_m_sync_set_slaveaddr(&I2C_0, 0x12, I2C_M_SEVEN);
                mm_selected = 0;
        } else {
                i2c_m_sync_set_slaveaddr(&I2C_0, 0x16, I2C_M_SEVEN);
                mm_selected = 1;
        }
}

uint8_t get_digit(uint8_t echo)
{
        uint8_t temp_num __attribute__((__aligned__(4)));
        while (cdcdf_acm_read(&temp_num, 1))
                ;
        if (echo) {
                cdcdf_acm_write((uint8_t *)&temp_num, 1);
                delay_ms(2);
        }
        temp_num = (temp_num > '`')?(10+temp_num-'a'):(temp_num-'0');
        return temp_num;
}

char get_char(uint8_t echo)
{
        char temp_char __attribute__((__aligned__(4)));
        while (cdcdf_acm_read((uint8_t *)&temp_char, 1))
                ;
        if (echo) {
                cdcdf_acm_write((uint8_t *)&temp_char, 1);
                delay_ms(2);
        }
        return temp_char;
}

void print_log();

float adc_temp;

void user_interaction()
{
        static uint8_t i;
        uint8_t *send_p;
        uint16_t myblock = 0;
        uint16_t adc_val;
        uint8_t user_input_char;
        uint8_t tmp_byte;
        uint32_t tmp_word;
        uint8_t query_cmd;
        uint8_t read_buf_long[32];
        uint8_t read_buf_len;
        uint16_t volt;
        uint16_t curr;
        uint16_t powr;
        uint16_t fan_n;
        uint16_t speed_target;
        uint16_t speed;
        int32_t res = 0;
        user_input_char = get_char(0);
        switch (user_input_char) {
        case 'F':
                if (!oneline)
                        print("s = %ld, fw_size = %ld (%ld blocks), fw_remainder = %ld", (uint32_t)MOD_FW_START, (uint32_t)MOD_FW_LENGTH, (uint32_t)MOD_FW_LENGTH / 8, (uint32_t)MOD_FW_LENGTH % 8);
                uint16_t n_blocks = MOD_FW_LENGTH / 8;

                // first write FW size in 8-byte blocks
                i2c_m_sync_blk_cmd_write(&I2C_0, ADDR_WRITTEN_FW_SIZE, (uint8_t *)&n_blocks, 2);

                // then write the FW in 8-byte write commands, any extra bytes will be ignored
                send_p = (uint8_t *)MOD_FW_START;
                myblock = 0;
                while ((send_p + 7) < (uint8_t *)(MOD_FW_START + 8*n_blocks)) {
                        i2c_m_sync_blk_cmd_write(&I2C_0, ADDR_WRITTEN_FW_BLOCK, send_p, 8);
                        send_p += 8;
                        if (!oneline)
                                print("written block %d / %d, a = %d", ++myblock, n_blocks, (int)send_p);
                }
                res = i2c_m_sync_blk_cmd_write(&I2C_0, ADDR_WRITTEN_FW_CHKSUM, (uint8_t *)&checksum, 2);
                if (!oneline)
                        check_res_print_error(res);
                break;
        case 'r':
                // write magic for the reset
                res = i2c_m_sync_blk_cmd_write(&I2C_0, ADDR_UC_RESET, &tmp_byte, 0x5A);
                if (!oneline)
                        check_res_print_error(res);
                if (!oneline)
                        print("resetting slave...");
                break;
        case 'R':
                // write value doesn't matter, use whatever
                res = i2c_m_sync_blk_cmd_write(&I2C_0, ADDR_BOOT_NEW_FW, &tmp_byte, 0xAD);
                if (!oneline)
                        check_res_print_error(res);
                if (!oneline)
                        print("resetting slave to write new FW...");
                break;
        case 'E':
                if (!oneline)
                        print("enabling PEC...");
                res = enable_pec(&I2C_0);
                if (!oneline)
                        check_res_print_error(res);
                break;
        case 'e':
                if (!oneline)
                        print("disabling PEC...");
                res = disable_pec(&I2C_0);
                if (!oneline)
                        check_res_print_error(res);
                break;
        case 'S':
                NVIC_SystemReset();
                break;
        case 's':
                if (!oneline)
                        print("enter fan number (0-2) and a 4-digit RPM to set it to:");
                delay_ms(2);
                fan_n = get_digit(!oneline);
                if (!oneline)
                        print("");
                speed_target = 1000*get_digit(!oneline);
                speed_target += 100*get_digit(!oneline);
                speed_target += 10*get_digit(!oneline);
                speed_target += get_digit(!oneline);
                if (!oneline)
                        print("");
                speed = float_to_linear(speed_target);
                if (!oneline)
                        print("setting fan %d speed to %d (%x)", fan_n, speed_target, speed);
                res = i2c_m_sync_my_cmd_write(&I2C_0, 0x3B+((fan_n>=2)?3:fan_n), (uint8_t *)&speed, 2);
                if (!oneline)
                        check_res_print_error(res);
                break;
        case 'p':
                if (!oneline)
                        print("enter a single-digit channel:");
                user_input_char = get_char(1);
                if (!oneline)
                        print("");
                if (user_input_char == 'p') {
                        res = i2c_m_sync_my_cmd_read(&I2C_0, 0x96, (uint8_t *)&powr, 2);
                        if (!oneline) {
                                check_res_print_error(res);
                                print("READ_POUT: %.3f (%x)", linear_to_float(powr), powr);
                        }
                } else {
                        if (!oneline)
                                print("setting page to %c", user_input_char);
                        tmp_byte = (user_input_char > '`')?(10+user_input_char-'a'):(user_input_char-'0');
                        res = i2c_m_sync_my_cmd_write(&I2C_0, 0x0, &tmp_byte, 1);
                        if (!oneline)
                                check_res_print_error(res);
                }
                break;
        case 'P':
                if (!oneline)
                        print("Power cycling the MoniMods...");
                gpio_set_pin_level(PCYC, 1);
                delay_ms(1000);
                gpio_set_pin_level(PCYC, 0);
                break;
        case 'v':
                res = i2c_m_sync_my_cmd_read(&I2C_0, 0x8B, (uint8_t *)&volt, 2);
                if (!oneline)
                        check_res_print_error(res);
                print("READ_VOUT: %.3f (%x)", linear_to_float(volt), volt);
                break;
        case 'i':
                res = i2c_m_sync_my_cmd_read(&I2C_0, 0x8C, (uint8_t *)&curr, 2);
                if (!oneline) {
                        check_res_print_error(res);
                        print("READ_IOUT: %.3f (%x)", linear_to_float(curr), curr);
                }
                break;
        case 'f':
                print("enter a number from 0 to 2:");
                tmp_byte = get_digit(1);
                if (!oneline)
                        print("");
                res = i2c_m_sync_my_cmd_read(&I2C_0, 0x90+tmp_byte, (uint8_t *)&speed, 2);
                if (!oneline)
                        check_res_print_error(res);
                print("READ_FAN_SPEED_%u: %.3f (%x)", tmp_byte, linear_to_float(speed), speed);
                break;
        case 'q':
                if (!oneline)
                        print("enter a two-digit CMD:");
                query_cmd = 16*get_digit(1);
                query_cmd += get_digit(1);
                if (!oneline)
                        print("");

                if (!query_cmd) {
                        print("querying manufacturer strings...");
                        delay_ms(2);

                        res |= i2c_m_sync_blk_cmd_read(&I2C_0, 0x99, read_buf_long, &read_buf_len);
                        print("MFR_ID string: %s - %d bytes", read_buf_long, read_buf_len);
                        res |= i2c_m_sync_blk_cmd_read(&I2C_0, 0x9A, read_buf_long, &read_buf_len);
                        print("MFR_MODEL string: %s - %d bytes", read_buf_long, read_buf_len);
                        res |= i2c_m_sync_blk_cmd_read(&I2C_0, 0x9B, read_buf_long, &read_buf_len);
                        print("MFR_REVISION string: %s - %d bytes", read_buf_long, read_buf_len);
                        res |= i2c_m_sync_blk_cmd_read(&I2C_0, 0x9C, read_buf_long, &read_buf_len);
                        print("MFR_LOCATION string: %s - %d bytes", read_buf_long, read_buf_len);
                        res |= i2c_m_sync_blk_cmd_read(&I2C_0, 0x9D, read_buf_long, &read_buf_len);
                        print("MFR_DATE string: %s - %d bytes", read_buf_long, read_buf_len);
                        res |= i2c_m_sync_blk_cmd_read(&I2C_0, 0x9E, read_buf_long, &read_buf_len);
                        print("MFR_SERIAL string: %s - %d bytes", read_buf_long, read_buf_len);
                } else {
                        print("querying cmd %x", query_cmd);

                        res = i2c_m_sync_blk_cmd_write_and_read(&I2C_0, 0x1A, &query_cmd, 1, read_buf_long, &read_buf_len);
                        print("cmd %x query returned %x - %d bytes", query_cmd, read_buf_long[0], read_buf_len);
                }
                check_res_print_error(res);
                break;
        case 't':
                if (!oneline)
                        print("Power cycling the MoniMods...");
                gpio_set_pin_level(PCYC, 1);
                delay_ms(1000);
                gpio_set_pin_level(PCYC, 0);
                delay_ms(500);
                if (!oneline)
                        print("enabling PEC...");
                select_monimod(0);
                res |= enable_pec(&I2C_0);
                if (!oneline)
                        check_res_print_error(res);
                select_monimod(1);
                res |= enable_pec(&I2C_0);
                if (!oneline)
                        check_res_print_error(res);
                delay_ms(500);
                if (!oneline)
                        print("taking readouts...");
                select_monimod(0);
                delay_ms(10);
                print_log();
                select_monimod(1);
                delay_ms(10);
                print_log();
                break;
        case 'T':
                // write value doesn't matter, use whatever
                res = i2c_m_sync_my_cmd_read(&I2C_0, ADDR_TMR_ERROR_CNT, (uint8_t *)&tmp_word, 4);
                if (!oneline) {
                        check_res_print_error(res);
                        print("TMR_ERROR_CNT: %lu...", tmp_word);
                }
                break;
        case 'l':
                oneline = 0;
                break;
        case 'L':
                oneline = 1;
                break;
        case 'a':
                print_log();
                break;
        case 'd':
                if (!oneline)
                        print("Selecting first MoniMod...");
                select_monimod(0);
                break;
        case 'D':
                if (!oneline)
                        print("Selecting second MoniMod...");
                select_monimod(1);
                break;
        default:
                break;
        }
}


void print_log()
{
        static uint32_t sample_no;
        char sprintf_buf[256] __attribute__((__aligned__(4)));
        uint16_t temp[3];
        uint16_t volt[3];
        uint16_t curr[3];
        uint16_t powr[3];
        uint16_t speed[3];
        uint32_t tmr_err;
        int32_t res = 0;
        for (uint8_t i = 0; i < 3; ++i) {
                res |= i2c_m_sync_my_cmd_write(&I2C_0, 0x0, &i, 1); // set page
                if (!oneline)
                        check_res_print_error(res);
                res |= i2c_m_sync_my_cmd_read(&I2C_0, 0x8B, (uint8_t *)&(volt[i]), 2);
                if (!oneline)
                        check_res_print_error(res);
                res |= i2c_m_sync_my_cmd_read(&I2C_0, 0x8C, (uint8_t *)&(curr[i]), 2);
                if (!oneline)
                        check_res_print_error(res);
                res |= i2c_m_sync_my_cmd_read(&I2C_0, 0x96, (uint8_t *)&(powr[i]), 2);
                if (!oneline)
                        check_res_print_error(res);
                res |= i2c_m_sync_my_cmd_read(&I2C_0, 0x90+i, (uint8_t *)&(speed[i]), 2);
                if (!oneline)
                        check_res_print_error(res);
                res |= i2c_m_sync_my_cmd_read(&I2C_0, 0x8D+i, (uint8_t *)&(temp[i]), 2);
                if (!oneline)
                        check_res_print_error(res);
        }
        i2c_m_sync_my_cmd_read(&I2C_0, ADDR_TMR_ERROR_CNT, (uint8_t *)&tmr_err, 4);
        if (!oneline)
                        check_res_print_error(res);
        
        if (!oneline)
        {
                sprintf(sprintf_buf, "sample %lu\r\n", sample_no++);
                cdcdf_acm_write((uint8_t*)sprintf_buf, strlen(sprintf_buf));
                delay_ms(10);

                sprintf(sprintf_buf, "V1: %.3f\tV2: %.3f\tV3: %.3f\r\n",
                        linear_to_float(volt[0]), linear_to_float(volt[1]), linear_to_float(volt[2]));
                cdcdf_acm_write((uint8_t*)sprintf_buf, strlen(sprintf_buf));
                delay_ms(10);

                sprintf(sprintf_buf, "I1: %.3f\tI2: %.3f\tI3: %.3f\r\n",
                        linear_to_float(curr[0]), linear_to_float(curr[1]), linear_to_float(curr[2]));
                cdcdf_acm_write((uint8_t*)sprintf_buf, strlen(sprintf_buf));
                delay_ms(10);

                sprintf(sprintf_buf, "P1: %.3f\tP2: %.3f\tP3: %.3f\r\n",
                        linear_to_float(powr[0]), linear_to_float(powr[1]), linear_to_float(powr[2]));
                cdcdf_acm_write((uint8_t*)sprintf_buf, strlen(sprintf_buf));
                delay_ms(10);

                sprintf(sprintf_buf, "F1: %4d\tF2: %4d\tF3: %4d\r\n",
                        (uint16_t)linear_to_float(speed[0]),
                        (uint16_t)linear_to_float(speed[1]),
                        (uint16_t)linear_to_float(speed[2]));
                cdcdf_acm_write((uint8_t*)sprintf_buf, strlen(sprintf_buf));
                delay_ms(10);

                sprintf(sprintf_buf, "T1: %.3f\tT2: %.3f\tT3: %.3f\r\n",
                        linear_to_float(temp[0]), linear_to_float(temp[1]), linear_to_float(temp[2]));
                cdcdf_acm_write((uint8_t*)sprintf_buf, strlen(sprintf_buf));
                delay_ms(10);

                sprintf(sprintf_buf, "TMR_ERROR_CNT: %lu...\r\n", tmr_err);
                cdcdf_acm_write((uint8_t*)sprintf_buf, strlen(sprintf_buf));
                delay_ms(10);
        } else {
                delay_ms(10);
                sprintf(sprintf_buf, "%lu\t%d\t%lu\t%ld\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\r\n", sample_no++, mm_selected, tmr_err, res,
                        linear_to_float(volt[0]), linear_to_float(volt[1]), linear_to_float(volt[2]),
                        linear_to_float(curr[0]), linear_to_float(curr[1]), linear_to_float(curr[2]),
                        linear_to_float(temp[0]), linear_to_float(temp[1]), linear_to_float(temp[2]),
                        linear_to_float(speed[0]), linear_to_float(speed[1]), linear_to_float(speed[2])
                        );
                cdcdf_acm_write((uint8_t*)sprintf_buf, strlen(sprintf_buf));
                delay_ms(10);
        }
}


int main(void)
{
	/* Initializes MCU, drivers and middleware */
	atmel_start_init();

	adc_sync_enable_channel(&ADC_0, 0);

        struct io_descriptor *I2C_0_io;

        checksum = sysv_checksum((uint8_t *)MOD_FW_START, (uint8_t *)(MOD_FW_START + MOD_FW_LENGTH));

        i2c_m_sync_get_io_descriptor(&I2C_0, &I2C_0_io);
        i2c_m_sync_enable(&I2C_0);
        i2c_m_sync_set_slaveaddr(&I2C_0, 0x12, I2C_M_SEVEN);

        uint8_t zero8[8];
        memset(zero8, 0, 8);

	while (1) {
                gpio_toggle_pin_level(LED);
#if INTERACTIVE_MODE > 0
                user_interaction();
                delay_ms(100);
#else
                print_log();
                delay_ms(1000);
#endif
        }
}
