#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);

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

GenericSensor sensor1 = GenericSensor(callback1, initSensor1);

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

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

    driver1.voltage_power_supply = 7.1;
    driver1.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
    motor1.init();
    Serial.println("calibrating sensor in open loop...");
    sensor1.init();
    Serial.println("Done");
    delay(1000);

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

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