#include <pico/stdio.h>
#include <pico/types.h>
#include <stdint.h>
#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 "../tusb/tusb_stdio_driver.h"

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

#include "msgs.h"
#include "crc.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 base_vl = 0;
static double base_vr = 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 uint last_speed_change = 100;


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 = 1.000,
    .right_gain = 1.0000
};

void 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 vl_enc = state_l.speed;
    const double vr_enc = state_r.speed;

    const double vl = (vl_enc/(64 * ENCODER_CPR)) * WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
    const double vr = (vr_enc/(64 * ENCODER_CPR)) * -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;

    base_vl = (vl + vr)/2;
    base_vr = (vr - vl)/wheel_separation;

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

    uint16_t cmd = 0;
    uint readNum = 0;
    uint8_t cmd_id = 0;
    tMsgBuf data;
    uint16_t cmd_timeout = 100;

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

            if(cmd_id > 0) {
                if(push_tMsgBuf(&data, readNum++, ch,cmd_id)) {
                    if(cmd_id == TMSG_SET_WIDTH && crcFooter(data.msg_width) == data.msg_width.crc){
                        wheel_separation = data.msg_width.width;
                    } else if (cmd_id == TMSG_SET_RATIO && crcFooter(data.msg_ratio) == data.msg_width.crc) {
                        wheel_ratio_l = 1 + data.msg_ratio.ratio;
                    }
                    cmd_id = 0;
                }
            } else {
                if(cmd == TCMD_ID(TMSG_REQ_STATE)) {
                    tRespState resp = {
                        base_x,
                        base_y,
                        base_theta,
                        base_vl,
                        base_vr,
                    };
                    send_tRespState(&resp);
                } else if(cmd == TCMD_ID(TMSG_SET_RATIO)) {
                    readNum = 0;
                    cmd_id = TMSG_SET_RATIO;
                } else if(cmd == TCMD_ID(TMSG_SET_WIDTH)){
                    readNum = 0;
                    cmd_id = TMSG_SET_WIDTH;
                } else if(cmd == TCMD_ID(TMSG_ZERO)) {
                    zero();
                } else if(cmd == TCMD_ID(TMSG_END_CALIB)) {
                    end_calib();
                    tRespEndCalib resp = {
                        calib_enc.left_gain,
                        calib_enc.right_gain
                    };
                    send_tRespEndCalib(&resp);
                }
            }
            
        }

        update_pos_cb();
        if(!cmd_timeout--) {
            cmd_id = 0;
        }
        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) {
    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 run_init() 
{
    board_init();
    tusb_init();
    tud_init(BOARD_TUD_RHPORT);



    if (board_init_after_tusb) {
        board_init_after_tusb();
    }

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

}


int main()
{
    run_init();

    multicore_launch_core1(core2_entry);

    int c = 0;
    while (true) {
        if((c = stdio_getchar_timeout_us(0) ) != PICO_ERROR_TIMEOUT) {
            stepper_fifo(c);
        }
        sem_acquire_blocking(&stdio_dual_usb_mutex);
        tud_task();
        sem_release(&stdio_dual_usb_mutex);
    }
}

