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

BLDCMotor motor = BLDCMotor(4);
BLDCDriver3PWM driver = BLDCDriver3PWM(7, 8, 9);

LinearHallSensor linearSensor = LinearHallSensor(A0, A1, -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(100);
  Serial.println("INIT");
  pinMode(3, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  digitalWrite(3, LOW);
  digitalWrite(5, LOW);
  digitalWrite(6, LOW);

  driver.voltage_power_supply = 6.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();
}