#include "DifferentialDrive.h"
#include "LinearAlgebra/Angle.h"
#include "LinearAlgebra/FloatSingle.h"

#include <math.h>

DifferentialDrive::DifferentialDrive() {};

DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor) {
  this->motorCount = 2;
  this->motors = new Motor *[2];
  this->motors[0] = leftMotor;
  this->motors[1] = rightMotor;

  float distance = this->wheelSeparation / 2;
  leftMotor->direction = Motor::Direction::CounterClockwise;
  leftMotor->SetPosition(Spherical16(distance, Direction16::left));
  // leftMotor->position.direction.horizontal = Angle16::Degrees(-90);
  // leftMotor->position.distance = distance;
  rightMotor->direction = Motor::Direction::Clockwise;
  rightMotor->SetPosition(Spherical16(distance, Direction16::right));
  // rightMotor->position.direction.horizontal = Angle16::Degrees(90);
  // rightMotor->position.distance = distance;
}

void DifferentialDrive::SetDimensions(float wheelDiameter,
                                      float wheelSeparation) {
  this->wheelDiameter = wheelDiameter;
  this->wheelSeparation = wheelSeparation;
  this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;

  float distance = this->wheelSeparation / 2;
  Spherical16 motor0Position = this->motors[0]->GetPosition();
  motor0Position.distance = distance;
  this->motors[0]->SetPosition(motor0Position);
  Spherical16 motor1Position = this->motors[0]->GetPosition();
  motor1Position.distance = distance;
  this->motors[1]->SetPosition(motor1Position);
}

void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
                                             float rightSpeed) {
  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    Motor *motor = motors[motorIx];
    if (motor == nullptr)
      continue;

    float xPosition =
        motors[motorIx]->GetPosition().direction.horizontal.InDegrees();
    if (xPosition < 0)
      motor->SetTargetSpeed(leftSpeed);
    else if (xPosition > 0)
      motor->SetTargetSpeed(rightSpeed);
  };
}

void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
  float leftSpeed =
      Float::Clamp(forward + yaw, -1, 1); // revolutions per second
  float rightSpeed =
      Float::Clamp(forward - yaw, -1, 1); // revolutions per second

  float leftMotorSpeed = leftSpeed / rpsToMs;   // meters per second
  float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second

  SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
}

void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
  SetTwistSpeed(linear.y, yaw);
}

void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
                                      float roll) {
  SetTwistSpeed(linear.Forward(), yaw);
}

// void DifferentialDrive::SetVelocity(Polar velocity) {
//   SetTwistSpeed(velocity.distance, velocity.angle.InDegrees());
// }

// Spherical16 DifferentialDrive::GetVelocity() {
//   Motor *leftMotor = motors[0];
//   Motor *rightMotor = motors[1];
//   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per
//   second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions
//   per second

//   leftSpeed = leftSpeed * rpsToMs;   // in meters per second
//   rightSpeed = rightSpeed * rpsToMs; // in meters per second
//   float speed = (leftSpeed + rightSpeed) / 2;

//   float direction = speed >= 0 ? 0.0F : 180.0F;
//   float magnitude = fabsf(speed);
//   Spherical16 velocity =
//       Spherical16(magnitude, Angle16::Degrees(direction),
//                   Angle16::Degrees(0)); // Polar(direction, magnitude);
//   return velocity;
// }

// float DifferentialDrive::GetAngularVelocity() {
//   Motor *leftMotor = motors[0];
//   Motor *rightMotor = motors[1];
//   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per
//   second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions
//   per second

//   leftSpeed = leftSpeed * rpsToMs;   // in meters per second
//   rightSpeed = rightSpeed * rpsToMs; // in meters per second

//   float angularSpeed = (leftSpeed - rightSpeed) / 2;
//   float angularDistance = wheelSeparation / 2 * Passer::LinearAlgebra::pi;
//   float rotationsPerSecond = angularSpeed / angularDistance;
//   float degreesPerSecond =
//       Angle::Normalize(Angle::Degrees(360 * rotationsPerSecond)).InDegrees();
//   float angularVelocity = degreesPerSecond;

//   return angularVelocity;
// }