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

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

}

void calcTorque(uint8_t joy_pos) {
    int32_t calcTorque;
    calcTorque = (int8_t) joy_pos;                  // joystick position
    if(joy_pos >= 0) {
        calcTorque = rampTorque[joy_pos];
    } else {
        calcTorque = -rampTorque[-joy_pos];
    }
    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;
    calcPosition = (int8_t) joy_pos;
    calcPosition *= -1;     // change left and right
    if(joy_pos >= 0) {
        calcPosition = rampPosition[joy_pos];
    } else {
        calcPosition = -rampPosition[-joy_pos];
    }
    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() {
    
}