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

BLDCMotor motor = BLDCMotor(4);
BLDCDriver3PWM driver = BLDCDriver3PWM(M2_PWM1, M2_PWM2, M2_PWM3);

LinearHallSensor linearSensor = LinearHallSensor(M2_Hall1, M2_Hall2, -0.75, 2.35);

void initSensor()
{
    linearSensor.init(motor);
}

float callback()
{
    return linearSensor.readSensorCallback();
}

GenericSensor sensor = GenericSensor(callback, initSensor);

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(2000);
    Serial.println("INIT");

    // pinMode(M1_PWM1, OUTPUT);
    // pinMode(M1_PWM2, OUTPUT);
    // pinMode(M1_PWM3, OUTPUT);
    // digitalWrite(M1_PWM1, LOW);
    // digitalWrite(M1_PWM2, LOW);
    // digitalWrite(M1_PWM3, LOW);

    // pinMode(M2_PWM1, OUTPUT);
    // pinMode(M2_PWM2, OUTPUT);
    // pinMode(M2_PWM3, OUTPUT);
    // digitalWrite(M2_PWM1, LOW);
    // digitalWrite(M2_PWM2, LOW);
    // digitalWrite(M2_PWM3, LOW);

    // pinMode(M3_PWM1, OUTPUT);
    // pinMode(M3_PWM2, OUTPUT);
    // pinMode(M3_PWM3, OUTPUT);
    // digitalWrite(M3_PWM1, LOW);
    // digitalWrite(M3_PWM2, LOW);
    // digitalWrite(M3_PWM3, LOW);

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

    driver.voltage_power_supply = 5.0;
    driver.init();
    motor.linkDriver(&driver);
    motor.useMonitoring(Serial);
    motor.controller = MotionControlType::angle;
    motor.foc_modulation = FOCModulationType::SinePWM;
    motor.voltage_limit = 1.5;
    motor.voltage_sensor_align = 0.8;
    motor.PID_velocity.P = 0.075f;
    motor.PID_velocity.I = 0.01;
    motor.PID_velocity.D = 0.0;
    motor.LPF_velocity.Tf = 0.01f;
    motor.P_angle.P = 120.0;
    motor.P_angle.I = 10.0;
    motor.velocity_limit = 50;

    motor.useMonitoring(Serial);

    // Init sensor
    motor.init();
    Serial.println("calibrating sensor in open loop...");
    sensor.init();
    Serial.println("Done");
    delay(1000);

    motor.linkSensor(&sensor);
    motor.init();
    // motor.initFOC(5.48, CCW);
    motor.initFOC();
}

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