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

MagneticSensorAnalog sensor = MagneticSensorAnalog(A0, 183, 512);

BLDCMotor motorX = BLDCMotor(4);
BLDCDriver3PWM driverX = BLDCDriver3PWM(3, 5, 6);

BLDCMotor motorY = BLDCMotor(4);
BLDCDriver3PWM driverY = BLDCDriver3PWM(7, 8, 9);

float targetX = 0.0;
float targetY = 0.0;

float target = 6.3;

void setup()
{

  sensor.init();
  // motorY.linkSensor(&sensor);

  // X MOTR STUFF
  driverX.voltage_power_supply = 5.0;
  driverX.init();
  motorX.linkDriver(&driverX);
  motorX.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motorX.controller = MotionControlType::angle_openloop;
  motorX.PID_velocity.P = 0.02f;
  motorX.PID_velocity.I = 20;
  motorX.PID_velocity.D = 0;
  motorX.voltage_limit = 0.8;
  motorX.LPF_velocity.Tf = 0.01f;
  motorX.P_angle.P = 20;
  motorX.velocity_limit = 20;

  // *************** Y MOTOR STUFF ******************
  driverY.voltage_power_supply = 5.0;
  driverY.init();
  motorY.linkDriver(&driverY);
  motorY.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motorY.controller = MotionControlType::angle_openloop;
  motorY.PID_velocity.P = 0.001f;
  motorY.PID_velocity.I = 0;
  motorY.PID_velocity.D = 0;
  motorY.voltage_limit = 0.8;
  motorY.LPF_velocity.Tf = 0.01f;
  motorY.P_angle.P = 0.1;
  motorY.velocity_limit = 3;

  Serial.begin(115200);
  _delay(100);
  Serial.println("INIT");

  // initialize motor
  motorX.init();
  motorX.initFOC();
  motorY.init();
  motorY.initFOC();

  Serial.println(F("Motor ready."));
  _delay(1000);
  // Serial.println(sensor.getPreciseAngle());

}

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 loop()
{
  sensor.update();

  // Serial.println(sensor.getPreciseAngle());
  // delay(5);

  serialLoop();

  int senseX = analogRead(A2);
  int senseY = analogRead(A3);

  targetX = mapfloat(senseX, 0.0, 1024.0, -0.6, 1.2);
  targetY = mapfloat(senseY, 0.0, 1024.0, -0.5, 2.2);

  Serial.print("senseX=");
  Serial.print(senseX);
  Serial.print("\t mappedX=");
  Serial.print(targetX);
  Serial.print("\t senseY=");
  Serial.print(senseY);
  Serial.print("\t mappedY=");
  Serial.println(targetY);

  motorX.move(targetX);
  motorX.loopFOC();

  motorY.move(targetY);
  motorY.loopFOC();
}