/**
 * @author Rémi Heredero
 * @version. 0.0.0
 * @date August 2023
 * @file kartculator.c
 */

#include "kartculator.h"

void deadJoystick(void* p){
    eKart.torque = 0;
    eKart.position = eKart.center;
}

void initRamp() {
    exp[0] = 0;
    exp[1] = 0;
    exp[2] = 0;
    exp[3] = 0;
    exp[4] = 0;
    exp[5] = 0;
    exp[6] = 0;
    exp[7] = 0;
    exp[8] = 0;
    exp[9] = 0;
    exp[10] = 1;
    exp[11] = 1;
    exp[12] = 1;
    exp[13] = 1;
    exp[14] = 1;
    exp[15] = 1;
    exp[16] = 1;
    exp[17] = 1;
    exp[18] = 1;
    exp[19] = 1;
    exp[20] = 1;
    exp[21] = 2;
    exp[22] = 2;
    exp[23] = 2;
    exp[24] = 2;
    exp[25] = 2;
    exp[26] = 2;
    exp[27] = 2;
    exp[28] = 2;
    exp[29] = 3;
    exp[30] = 3;
    exp[31] = 3;
    exp[32] = 3;
    exp[33] = 3;
    exp[34] = 4;
    exp[35] = 4;
    exp[36] = 4;
    exp[37] = 4;
    exp[38] = 5;
    exp[39] = 5;
    exp[40] = 5;
    exp[41] = 6;
    exp[42] = 6;
    exp[43] = 6;
    exp[44] = 7;
    exp[45] = 7;
    exp[46] = 7;
    exp[47] = 8;
    exp[48] = 8;
    exp[49] = 9;
    exp[50] = 9;
    exp[51] = 10;
    exp[52] = 10;
    exp[53] = 11;
    exp[54] = 11;
    exp[55] = 12;
    exp[56] = 12;
    exp[57] = 13;
    exp[58] = 14;
    exp[59] = 14;
    exp[60] = 15;
    exp[61] = 16;
    exp[62] = 16;
    exp[63] = 17;
    exp[64] = 18;
    exp[65] = 19;
    exp[66] = 20;
    exp[67] = 21;
    exp[68] = 22;
    exp[69] = 23;
    exp[70] = 24;
    exp[71] = 25;
    exp[72] = 27;
    exp[73] = 28;
    exp[74] = 29;
    exp[75] = 31;
    exp[76] = 32;
    exp[77] = 34;
    exp[78] = 36;
    exp[79] = 37;
    exp[80] = 39;
    exp[81] = 41;
    exp[82] = 43;
    exp[83] = 45;
    exp[84] = 47;
    exp[85] = 50;
    exp[86] = 52;
    exp[87] = 54;
    exp[88] = 57;
    exp[89] = 60;
    exp[90] = 63;
    exp[91] = 66;
    exp[92] = 69;
    exp[93] = 72;
    exp[94] = 76;
    exp[95] = 79;
    exp[96] = 83;
    exp[97] = 87;
    exp[98] = 91;
    exp[99] = 95;
    exp[100] = 100;

    log[0] = 0;
}

void calcTorque(uint8_t joy_pos) {
    int32_t calcTorque;
    static int8_t lastPos = 0;
    int8_t diff = ((int8_t)joy_pos) - lastPos;
    
    if (diff > KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE) {
        calcTorque = lastPos + KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE;
    } else if (diff < -KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE) {
        calcTorque = lastPos - KART_CST.CONTROL_PARAM_MAX_CHANGE_DRIVE;
    } else {
        calcTorque = (int8_t) joy_pos;                  // joystick position
    }
    lastPos = calcTorque;
    
    calcTorque *= KART_CST.CONTROL_POWER_FACTOR;    // convert by power factor
    calcTorque /= 1000;                             // torque define by joystick
    eKart.torque = (int16_t) calcTorque;
}

void calcPosition(uint8_t joy_pos){
    int32_t calcPosition;
    static int8_t lastPos = 0;
    int8_t diff = ((int8_t)joy_pos) - lastPos;
    
    if (diff > KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING) {
        calcPosition = lastPos + KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING;
    } else if (diff < -KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING) {
        calcPosition = lastPos - KART_CST.CONTROL_PARAM_MAX_CHANGE_STEERING;
    } else {
        calcPosition = (int8_t) joy_pos;
    }
    lastPos = calcPosition;
    
    calcPosition *= -1;     // change left and right
    calcPosition *= (int32_t) KART_CST.CONTROL_STEERING_FACTOR;
    calcPosition /= 1000;
    calcPosition += eKart.center;
    eKart.position = (uint32_t) calcPosition;
}

void calcSpeed(int32_t rpm) {
    int32_t calcSpeed;
    if(rpm>=0){
        calcSpeed = rpm;
    } else {
        calcSpeed = -rpm;
    }
    calcSpeed *= 1000;
    calcSpeed /= KART_CST.CONTROL_SPEED_FACTOR;
    eKart.speed = (uint8_t) calcSpeed;
}

int16_t getTorque() {
    
}

uint32_t getPosition() {
    
}

uint8_t getSpeed() {
    
}