#include <Arduino.h>
#include <SimpleFOC.h>
#include <linearHallSensor.h>
#include <pinout.h>

BLDCMotor motor[3] =
    {
        BLDCMotor(4),
        BLDCMotor(4),
        BLDCMotor(4)};

BLDCDriver3PWM driver[3] =
    {
        BLDCDriver3PWM(M1_PWM1, M1_PWM2, M1_PWM3),
        BLDCDriver3PWM(M2_PWM1, M2_PWM2, M2_PWM3),
        BLDCDriver3PWM(M3_PWM1, M3_PWM2, M3_PWM3)};

LinearHallSensor linearSensor[3] =
    {
        LinearHallSensor(M1_Hall1, M1_Hall2),
        LinearHallSensor(M2_Hall1, M2_Hall2),
        LinearHallSensor(M3_Hall1, M3_Hall2)};

void initSensor0()
{
    linearSensor[0].init(motor[0]);
}
float callback0()
{
    return linearSensor[0].readSensorCallback();
}

void initSensor1()
{
    linearSensor[1].init(motor[1]);
}
float callback1()
{
    return linearSensor[1].readSensorCallback();
}

void initSensor2()
{
    linearSensor[2].init(motor[2]);
}
float callback2()
{
    return linearSensor[2].readSensorCallback();
}

GenericSensor sensor[3] =
    {
        GenericSensor(callback0, initSensor0),
        GenericSensor(callback1, initSensor1),
        GenericSensor(callback2, initSensor2)};

float target[3] = {0.5, 0.5, 0.5};

String str;

void serialLoop()
{
    static String received_chars;
    while (Serial.available())
    {
        char inChar = (char)Serial.read();
        received_chars += inChar;
        if (inChar == '\n')
        {
            target[0] = received_chars.toFloat();
            Serial.print("Target = ");
            Serial.print(target[0]);
            received_chars = "";
        }
    }
}

float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
    return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}

void initMotor(u_int8_t m)
{
    Serial.printf("\n\t\t### MOTOR %d ###\n\n", m + 1);
    driver[m].voltage_power_supply = 7.0;
    driver[m].pwm_frequency = 50000;
    Serial.printf("Driver%d init: %d\n", m + 1, driver[m].init());
    motor[m].linkDriver(&driver[m]);
    motor[m].useMonitoring(Serial);
    motor[m].controller = MotionControlType::angle;
    motor[m].foc_modulation = FOCModulationType::SinePWM;
    motor[m].voltage_limit = 2.0;
    motor[m].voltage_sensor_align = 2.0;
    motor[m].PID_velocity.P = 0.05f;
    motor[m].PID_velocity.I = 0.008;
    motor[m].PID_velocity.D = 0.0;
    motor[m].LPF_velocity.Tf = 0.02f;
    motor[m].P_angle.P = 150.0;
    motor[m].P_angle.I = 5.0;
    motor[m].velocity_limit = 20;

    // Init sensor
    motor[m].init();
    Serial.println("calibrating sensor in open loop...");
    sensor[m].init();
    Serial.printf("Sensor %d done\n", m + 1);

    motor[m].linkSensor(&sensor[m]);
    motor[m].init();
    if (m == 2)
        motor[m].initFOC(3.0, CW);
    else if (m == 1)
        motor[m].initFOC(3.79, CW);
        // motor[m].initFOC();
    else
        motor[m].initFOC(0.18, CW);
    Serial.printf("Motor %d Done\n", m + 1);
}

void setup()
{
    Serial.begin(115200);
    delay(3000);
    Serial.println("INIT");

    pinMode(LED_BUILTIN, OUTPUT); // Lightup LED
    digitalWrite(LED_BUILTIN, LOW);

    motor[1].useMonitoring(Serial);
    initMotor(2);
    initMotor(1);
    initMotor(0);
}

void loop()
{
    int len = Serial.available();
    if (len > 0)
    {
        str = Serial.readStringUntil('\n'); // get new targets from serial (target are from 0 to 1000)
        char axis = str[0];
        str.remove(0, 1);
        str.remove(len,1);
        switch (axis)
        {
        case 'X':
            target[1] = str.toFloat();
            // Serial.println("Target X: " + String(target[1]));
            target[1] = mapfloat(target[1], 0.0, 1000.0, 0.0, linearSensor[1].getMaxAngle());   // remap targets to motor angles
            break;
        case 'Y':
            target[0] = str.toFloat();
            // Serial.println("Target Y: " + String(target[0]));
            target[0] = mapfloat(target[0], 0.0, 1000.0, 0.0, linearSensor[0].getMaxAngle());   // remap targets to motor angles
            break;
        case 'Z':
            target[2] = str.toFloat();
            // Serial.println("Target Z: " + String(target[2]));
            target[2] = mapfloat(target[2], 0.0, 1000.0, 0.0, linearSensor[2].getMaxAngle());   // remap targets to motor angles
            break;
        default:
            break;
        }
    }

    for (u_int8_t i = 0; i < 3; i++) // update motor target and Run FOC
    {
        
        // serialLoop();
        // Serial.println("Target" + String(i) + ": " + String(target[i]));
        motor[i].move(target[i]);
        motor[i].loopFOC();
        // motor[i].monitor();
    }
}