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



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

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

LinearHallSensor linearSensor = LinearHallSensor(A0, A1);

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

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

GenericSensor sensor = GenericSensor(callback, initSensor);


float targetX = 0.0;
float targetY = 0.0;

float target = 6.3;

void setup()
{
  // 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.voltage_limit = 0.6;
  // motorX.PID_velocity.P = 0.001f;
  // motorX.PID_velocity.I = 20;
  // motorX.PID_velocity.D = 0;
  // motorX.LPF_velocity.Tf = 0.01f;
  // motorX.P_angle.P = 20;
  motorX.velocity_limit = 10;

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



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

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

  motorX.disable();

  Serial.println(F("Motor ready."));
  _delay(100);
  Serial.println("calibrating...");
  sensor.init();
  Serial.println("Done\r\n");
  motorY.disable();   
}

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();
  // 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();
}