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

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


}

void calcTorque(uint8_t joy_pos) {
    int32_t calcTorque;
    calcTorque = (int8_t) joy_pos;                  // joystick position
    
    // if mode race, use special curve for torque
    if(eKart.powerMode == 2) {
        if(calcTorque >= 0) {
            calcTorque = rampTorque[calcTorque];
        } else {
            calcTorque *= -1;
            calcTorque = rampTorque[calcTorque];
            calcTorque *= -1;
        }
    }
    
    calcTorque *= KART_CST.CONTROL_POWER_FACTOR;    // convert by power factor
    calcTorque /= 1000;                             // torque define by joystick
    
    if(eKart.button) {
        calcTorque *= 1150;
        calcTorque /= 1000;
    }
    
    if(eKart.batteryVoltage <= 18750) {
        calcTorque = 0;
    }
    
    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 mode race, use special curve for steering
    if (eKart.powerMode == 2) {
        if(calcPosition >= 0) {
            calcPosition = rampPosition[calcPosition];
        } else {
            calcPosition *= -1;
            calcPosition = rampPosition[calcPosition];
            calcPosition *= -1;
        }
    }
    
    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 *= 2000;
    calcSpeed /= KART_CST.CONTROL_SPEED_FACTOR;
    eKart.speed = (uint8_t) calcSpeed;
}

int16_t getTorque() {
    
}

uint32_t getPosition() {
    
}

uint8_t getSpeed() {
    
}