#include <stdio.h>
#include <string.h>
#include "control.h"
#include "driver/uart.h"

/*
 * Helper functions
 */

static void callback_enc_func(int i_callback, int8_t dir, uint32_t counter) {
    motors[i_callback].vel_counter+=dir;
    motors[i_callback].odom_counter+=dir;
}

/*This function transforms speed from the robot frame to each wheel frame */
static void getW(sF3dVector_t* v, float phi){

    float dosPi_3 = 2*PI/3;

    sF3dMatrix_t m;
    m.x1 = -sin(phi);
    m.y1 = cos(phi);
    m.z1 = ROBOT_RADIUS;
	m.x2 = -sin(phi + dosPi_3);
    m.y2 = cos(phi + dosPi_3);
    m.z2 = ROBOT_RADIUS;
	m.x3 = -sin(phi - dosPi_3);
    m.y3 = cos(phi - dosPi_3);
    m.z3 = ROBOT_RADIUS;

	vector_mul(m, v);
}

static void getInverseW(sF3dVector_t* v, float phi){

    float dosPi_3 = 2*PI/3;
    float dostercios = 2.0/3;
    float robot_r_3 = 1.0/(ROBOT_RADIUS*3);

	sF3dMatrix_t m;
    m.x1 = -sin(phi)*dostercios;
    m.y1 = -sin(phi + dosPi_3)*dostercios;
    m.z1 = -sin(phi - dosPi_3)*dostercios;
    m.x2 = cos(phi)*dostercios;
    m.y2 = cos(phi + dosPi_3)*dostercios;
    m.z2 = cos(phi + dosPi_3)*dostercios;
    m.x3 = robot_r_3;
    m.y3 = robot_r_3;
    m.z3 = robot_r_3;

	vector_mul(m, v);

}

static void callback_motors_pid(){
    for (int i=0; i<NMOTORS; i++) {
        servo_t *m = &(motors[i]);

        float current_v  = m->vel_counter / OMNI_PID_CTRL_TIMER;  // compute curren motor vel in counts/s
        m->vel_counter = 0; // reset counter

        float error = m->target_v - current_v; // compute error

        float accum_error = m->accum_error + (error * OMNI_PID_CTRL_TIMER); // accumulate error as an integral
        m->output = KP * error;
        m->output += KI * accum_error;

        if (m->output>MAX_OUTPUT) {
            m->output=MAX_OUTPUT;
        } else if (m->output<-MAX_OUTPUT) {
            m->output=-MAX_OUTPUT;
        } else {
            m->accum_error = accum_error; // only here, for windup protection
        }

        m->prev_error = error; // save prev error
    }


    // set motors speeds
    for (int i=0; i<NMOTORS; i++) {
        motor_set_speed(motors[i].driver, motors[i].output);
    }
}

static void callback_odometry(){
    odom_t *o = &(odometry); // get odometry actual info
    
    int32_t tics_motores[NMOTORS];
    for (int i=0; i<NMOTORS; i++) {
        servo_t *m = &(motors[i]);
        tics_motores[i] = m->odom_counter; // get motors odomo counter
        m->odom_counter = 0; // reset counter
    }

    sF3dVector_t *v = calloc(1, sizeof(sF3dVector_t));
    v->x = tics_motores[0] * COUNTS_TO_RAD_PER_SEC * WHEEL_RADIUS;
    v->y = tics_motores[1] * COUNTS_TO_RAD_PER_SEC * WHEEL_RADIUS;
    v->z = tics_motores[2] * COUNTS_TO_RAD_PER_SEC * WHEEL_RADIUS;

    getInverseW(v, 0);

    o->x        += v->x * OMNI_ODOM_CTRL_TIMER;
    o->x_dot     = v->x;
    o->y        += v->y * OMNI_ODOM_CTRL_TIMER;
    o->y_dot     = v->y;
    o->phi      += v->z * OMNI_ODOM_CTRL_TIMER;
    o->phi_dot   = v->z;
    

    float_buffer[0] = o->x;
    float_buffer[1] = o->x_dot;
    float_buffer[2] = o->y;
    float_buffer[3] = o->y_dot;
    float_buffer[4] = o->phi;
    float_buffer[5] = o->phi_dot;
    uart_write_bytes(UART_NUM_2, float_buffer, sizeof(float)*6 );
    free(v);
}

/*
 * Operation functions
 */

esp_err_t omni_init(){

    int8_t default_pins[] = MOTOR_PINS;
    int8_t default_enc[] = MOTOR_ENC;
    mcpwm_io_signals_t default_pwm_subunit_io[] = MOTOR_PWM_SUBUNIT_IO;
    mcpwm_timer_t default_pwm_subunit_timer[] = MOTOR_PWM_SUBUNIT_TIMER;
    

    for (int i=0; i<NMOTORS; i++) {

        int8_t pin1 = default_pins[2*i];
        int8_t pin2 = default_pins[2*i+1];
        int8_t encA = default_enc[2*i];
        int8_t encB = default_enc[2*i+1];
        mcpwm_io_signals_t pwm_a = default_pwm_subunit_io[2*i];
        mcpwm_io_signals_t pwm_b = default_pwm_subunit_io[2*i+1];
        mcpwm_timer_t timer_unit = default_pwm_subunit_timer[i];

        motor_h_t *m;
        motor_install(pin1,pin2, MOTOR_PWM_UNIT, pwm_a, pwm_b, timer_unit, &m);

        //saving servo structure
        motors[i].driver= m;

        //encoder
        encoder_h_t *e;
        encoder_setup(encA,encB,&e);
        encoder_register_callback(e, callback_enc_func, i, 1);

        //saving encoder structure
        motors[i].encoder=e;

        //printf(" done\r\n");
        motors[i].target_v=0;
    }

    odometry.x = 0;
    odometry.x_dot = 0;
    odometry.y = 0;
    odometry.y_dot = 0;
    odometry.phi = 0;
    odometry.phi_dot = 0;    

    //  SET TASK TO RUN ISR for motors pid and odometry. 
    
    if (tmr_setup(OMNI_PID_NRO_TIMER, 1000*1000*OMNI_PID_CTRL_TIMER, callback_motors_pid) != ESP_OK) {
        return ESP_FAIL;
    }

    if (tmr_setup(OMNI_ODOM_NRO_TIMER, 1000*1000*OMNI_ODOM_CTRL_TIMER, callback_odometry) != ESP_OK) {
        return ESP_FAIL;
    }

    return ESP_OK;
};

esp_err_t omni_set_enable(bool start){
    if (start){
        if ((tmr_start(OMNI_PID_NRO_TIMER) != ESP_OK)) {
            return ESP_FAIL;
        }
        if ((tmr_start(OMNI_ODOM_NRO_TIMER) != ESP_OK)) {
            return ESP_FAIL;
        }
        for (int i=0; i<NMOTORS; i++) {
            motor_start(motors[i].driver);
            motors[i].vel_counter = 0;
            motors[i].odom_counter = 0;
        }
    }else{
        if ((tmr_stop(OMNI_PID_NRO_TIMER) != ESP_OK)) {
            return ESP_FAIL;
        }
        if ((tmr_stop(OMNI_ODOM_NRO_TIMER) != ESP_OK)) {
            return ESP_FAIL;
        }
        for (int i=0; i<NMOTORS; i++) {
            motor_stop(motors[i].driver);
        }
    }
    return ESP_OK;
}

esp_err_t omni_drive(float x_dot ,float y_dot ,float w_dot ,float phi){
    
    sF3dVector_t *v = calloc(1, sizeof(sF3dVector_t));
    v->x = x_dot;
    v->y = y_dot;
    v->z = w_dot;
    
    getW(v, phi);
    
    /* Here we transform each motor speed from m/s to motor speeds in count/s */ 
    motors[0].target_v = v->x * M_PER_SEC_TO_COUNTS_PER_SEC;
    motors[1].target_v = v->y * M_PER_SEC_TO_COUNTS_PER_SEC;
    motors[2].target_v = v->z * M_PER_SEC_TO_COUNTS_PER_SEC;
    
    free(v);
    return ESP_OK;
}