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

BLDCMotor motor1 = BLDCMotor(4);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(M3_PWM1, M3_PWM2, M3_PWM3);
LinearHallSensor linearSensor1 = LinearHallSensor(M3_Hall1, M3_Hall2);

BLDCMotor motor2 = BLDCMotor(4);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(M1_PWM2, M1_PWM1, M1_PWM3);
LinearHallSensor linearSensor2 = LinearHallSensor(M1_Hall1, M1_Hall2);

void initSensor1()
{
    linearSensor1.init(motor1);
}
float callback1()
{
    return linearSensor1.readSensorCallback();
}

void initSensor2()
{
    linearSensor2.init(motor2);
}
float callback2()
{
    return linearSensor2.readSensorCallback();
}

GenericSensor sensor1 = GenericSensor(callback1, initSensor1);
GenericSensor sensor2 = GenericSensor(callback2, initSensor2);

float targetX = 0.0;
float targetY = 0.0;

float target = 0.0;

void serialLoop()
{
    static String received_chars;
    while (Serial.available())
    {
        char inChar = (char)Serial.read();
        received_chars += inChar;
        if (inChar == '\n')
        {
            target = received_chars.toFloat();
            Serial.print("Target = ");
            Serial.print(target);
            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 setup()
{

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

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

    // ### MOTOR 1 ###
    Serial.println("\n\t\t### MOTOR 1 ###\n");
    driver1.voltage_power_supply = 7.0;
    Serial.println("Driver1 init: " + String(driver1.init()));
    Serial.println("Driver2 init: " + String(driver2.init()));
    motor1.linkDriver(&driver1);
    motor1.useMonitoring(Serial);
    motor1.controller = MotionControlType::angle;
    motor1.foc_modulation = FOCModulationType::SinePWM;
    motor1.voltage_limit = 1.0;
    motor1.voltage_sensor_align = 1.0;
    motor1.PID_velocity.P = 0.05f;
    motor1.PID_velocity.I = 0.01;
    motor1.PID_velocity.D = 0.0;
    motor1.LPF_velocity.Tf = 0.01f;
    motor1.P_angle.P = 150.0;
    motor1.P_angle.I = 10.0;
    motor1.velocity_limit = 25;

    // Init sensor 1
    motor1.init();
    Serial.println("calibrating sensor in open loop...");
    sensor1.init();
    Serial.println("Sensor 1 done");
    delay(1000);

    motor1.linkSensor(&sensor1);
    motor1.init();
    motor1.initFOC(2.98, CW);
    // motor1.initFOC();
    Serial.println("Motor 1 Done");
    motor1.disable();

    delay(1000);

    // ### MOTOR 2 ###
    Serial.println("\n\t\t### MOTOR 2 ###\n");
    motor2.useMonitoring(Serial);
    driver2.voltage_power_supply = 7.0;
    // Serial.println("Driver init: " + String(driver2.init()));
    motor2.linkDriver(&driver2);

    motor2.controller = MotionControlType::angle;
    motor2.foc_modulation = FOCModulationType::SinePWM;
    motor2.voltage_limit = 1.0;
    motor2.voltage_sensor_align = 1.0;
    motor2.PID_velocity.P = 0.05f;
    motor2.PID_velocity.I = 0.01;
    motor2.PID_velocity.D = 0.0;
    motor2.LPF_velocity.Tf = 0.01f;
    motor2.P_angle.P = 150.0;
    motor2.P_angle.I = 10.0;
    motor2.velocity_limit = 25;

    // Init sensor 2
    motor2.init();
    Serial.println("calibrating sensor 2 in open loop...");
    sensor2.init();
    Serial.println("Sensor 2 done");
    delay(1000);

    motor2.linkSensor(&sensor2);
    motor2.init();
    // motor.initFOC(5.48, CCW);
    motor2.initFOC();
    Serial.println("Motor 2 Done");
}

void loop()
{
    serialLoop();
    motor1.move(target);
    motor1.loopFOC();
    motor1.monitor();

    motor2.move(target);
    motor2.loopFOC();
    motor2.monitor();
}