#include <stdio.h>
#include <math.h>
#include <string.h>

#include "bsp/board_api.h"
#include "tusb.h"


#ifdef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
#undef PICO_STDIO_USB_RESET_INTERFACE_SUPPORT_MS_OS_20_DESCRIPTOR
#endif

#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "pico/time.h"
#include "quadrature.h"
#include "quadrature.pio.h"

#include "config.h"
#include "stepper.h"

static substep_state_t state_l;
static substep_state_t state_r;

static double base_x = 0;
static double base_y = 0;
static double base_theta = 0;
static double wheel_separation = WHEEL_SEPARATION;
static double wheel_ratio_l = 1;
static double wheel_ratio_r = 1;

static uint prev_time;
static int prev_position_l = 0;
static int prev_position_r = 0;


static unsigned char stepper_instr[2+sizeof(double)*2] = {};

typedef struct instr_buf_t{
    char pos;
    char data[2+sizeof(double) * 2];
} instr_buf_t;

instr_buf_t stepper_buf = {};

typedef struct calib_diff_t {
    long long left_pulse;
    long long right_pulse;
    double left_gain;
    double right_gain;
} calib_diff_t;

static calib_diff_t calib_enc = {
    .left_gain = 0.997648,
    .right_gain = 1.002333
};

bool update_pos_cb() {

    substep_update(&state_l);
    substep_update(&state_r);

    int position_l= state_l.position;
    int position_r= state_r.position;

    const int diff_l = position_l - prev_position_l;
    const int diff_r = position_r - prev_position_r;

    calib_enc.left_pulse += diff_l;
    calib_enc.right_pulse -= diff_r;

    double vel_l = diff_l;
    double vel_r = diff_r;

    prev_position_l = state_l.position;
    prev_position_r = state_r.position;

    vel_l /=64 * ENCODER_CPR;
    vel_r /=64 * ENCODER_CPR;
    vel_l *=  WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
    vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;

    const double linear = (vel_l + vel_r) / 2;
    const double angular = (vel_r - vel_l) / wheel_separation;
    const double r = linear / angular;

    if(fabs(r) > 100) {
        const double dir = base_theta + angular * 0.5;

        base_x += linear * cos(dir);
        base_y += linear * sin(dir);
        base_theta += angular;
    }
    else {
        const double base_theta_old = base_theta;
        base_theta += angular;
        base_x += r * (sin(base_theta) - sin(base_theta_old));
        base_y += -r * (cos(base_theta) - cos(base_theta_old));
    }
}

void zero() {
    base_x = 0;
    base_y = 0; 
    base_theta = 0;  
    calib_enc.left_pulse = 0;
    calib_enc.right_pulse = 0;
}

void start_calib() {
    calib_enc.left_pulse = 0;
    calib_enc.right_pulse = 0;
}

void end_calib() {
    const double l_adjust = (double)calib_enc.left_pulse * calib_enc.left_gain;
    const double r_adjust = (double)calib_enc.right_pulse * calib_enc.right_gain;
    const double delta_angle = l_adjust - r_adjust;
    const double distance = 0.5 * (l_adjust + r_adjust);
    const double factor = delta_angle / distance * 0.7;
    calib_enc.left_gain = (1.0 - factor) * calib_enc.left_gain;
    calib_enc.right_gain = (1.0 + factor) * calib_enc.right_gain;
}


void core2_entry() 
{

    pio_add_program(pio1, &quadrature_encoder_substep_program);
    substep_init_state(pio1, 0, ENCODER_LEFT_PIN_A , &state_l);
    substep_init_state(pio1, 1, ENCODER_RIGHT_PIN_A, &state_r);

    // The sets the positions to initial values
    substep_update(&state_l);
    substep_update(&state_r);
    prev_position_l = state_l.position;
    prev_position_r = state_r.position;

    prev_time = time_us_32();

    // alarm_pool_t *ap = alarm_pool_create_with_unused_hardware_alarm(2);
    // repeating_timer_t rt;
    // alarm_pool_add_repeating_timer_us(ap, TIMER_CYCLE_US, update_pos_cb, NULL, &rt);

    uint16_t cmd = 0;
    uint64_t data = 0;
    uint readNum = 0;
    char type = 'w';

    while (true) {
        uint ch;
        if(( ch = stdio_getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
            cmd = (cmd << 8) | ch;

            if(readNum > 0) {
                data = (data >> 8ll) | ((uint64_t)ch<<56ll);
                readNum--;
                if(!readNum) {
                    if(type == 'w')
                        wheel_separation = *((double*)&data);
                    else
                        wheel_ratio_l = 1 + (*((double *)&data));
                        wheel_ratio_r = 1 - (*((double *)&data));
                }
            }
            
            if(cmd == (((uint16_t)'g' << 8) | ';')) {
                printf("%lf %lf %lf\n", base_x, base_y, base_theta);
                cmd = 0;
            } else if(cmd == (((uint16_t)'w' << 8) | ';')) {
                readNum = 8;
                type = (cmd >> 8) & 0xFF;
            } else if(cmd == (((uint16_t)'r' << 8) | ';')){
                readNum = 8;
                type = (cmd >> 8) & 0xFF;
            } else if(cmd == (((uint16_t)'z' << 8) | ';')) {
                zero();
            } else if(cmd == (((uint16_t)'c' << 8) | ';')) {
                end_calib();
                printf("%lf %lf\n", calib_enc.left_gain, calib_enc.right_gain);
            }
        }

        update_pos_cb();
        sleep_ms(1);
    }
}

double btod(unsigned char * input) {
    uint64_t data = 0;
    for (int i = 0; i<8; i++){
        uint a = *(input + i);
        data >>= 8;
        data  |= (uint64_t)a<<56;
    }
    const double ret = *((double*)&data);
    return ret;
}

void stepper_fifo(char c, uint8_t itf) {
    char instrh = stepper_buf.data[(stepper_buf.pos + 1) % sizeof(stepper_buf.data)];
    char instrl = stepper_buf.data[(stepper_buf.pos + 2) % sizeof(stepper_buf.data)];
    stepper_buf.data[stepper_buf.pos] = c;
    stepper_buf.pos = (stepper_buf.pos + 1) % sizeof(stepper_buf.data);

    if(instrh == 's' && instrl == ';') {
        memcpy(stepper_instr,stepper_buf.data + stepper_buf.pos, sizeof(stepper_buf.data) - stepper_buf.pos);
        memcpy(stepper_instr - stepper_buf.pos + sizeof(stepper_buf.data),stepper_buf.data, stepper_buf.pos);

        memset(&stepper_buf, 0, sizeof(stepper_buf));
        double vl = btod(stepper_instr + 2);
        double vr = btod(stepper_instr + 10);

        set_speeds(vl, vr);

        // tud_cdc_n_write(itf, (uint8_t const *) stepper_instr, 18);
        // tud_cdc_n_write_flush(itf);
    }

}

void tud_cdc_rx_cb(uint8_t itf) {
    // read the available data 
    // | IMPORTANT: also do this for CDC0 because otherwise
    // | you won't be able to print anymore to CDC0
    // | next time this function is called
    if (itf == 1) {
        int c;
        while((c = tud_cdc_n_read_char(itf)) > -1) {
            stepper_fifo(c, itf);
        }
        // tud_cdc_n_write(itf, (uint8_t const *) "OK\r\n", 4);
        // tud_cdc_n_write_flush(itf);
    }
}

void run_init() 
{
    board_init();
    tusb_init();
    tud_init(BOARD_TUD_RHPORT);



    if (board_init_after_tusb) {
        board_init_after_tusb();
    }

    if(!stdio_usb_init()) {
        board_led_write(1);
    }
    stepper_init();

}


int main()
{
    run_init();

    multicore_launch_core1(core2_entry);

    
    
    while (true) {
        tud_task();
        sleep_ms(1);
    }
}