#include "DifferentialDrive.h"
#include "VectorAlgebra/FloatSingle.h"

#include <math.h>

DifferentialDrive::DifferentialDrive(){};

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

  this->wheelSeparation = separation;
  float distance = wheelSeparation / 2;
  leftMotor->direction = Motor::Direction::CounterClockwise;
  leftMotor->position.angle = -90;
  leftMotor->position.distance = distance;
  rightMotor->direction = Motor::Direction::Clockwise;
  rightMotor->position.angle = 90;
  rightMotor->position.distance = distance;
}

#include <Arduino.h>

void DifferentialDrive::SetTargetSpeeds(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]->position.angle;
    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);
  float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
  SetTargetSpeeds(leftSpeed, rightSpeed);
}

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

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

#include <Arduino.h>

Polar 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 * wheelDiameter * M_PI;    // in meters per second
  rightSpeed = rightSpeed * wheelDiameter * M_PI;  // in meters per second
  float speed = (leftSpeed + rightSpeed) / 2;

  float direction = speed >= 0 ? 0.0F : 180.0F;
  float magnitude = fabsf(speed);
  Polar velocity = 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 * wheelDiameter * M_PI;    // in meters per second
  rightSpeed = rightSpeed * wheelDiameter * M_PI;  // in meters per second

  float angularSpeed = (rightSpeed - leftSpeed) / 2;
  float angularDistance = wheelSeparation / 2 * M_PI;
  float rotationsPerSecond = angularSpeed / angularDistance;
  float degreesPerSecond = 360 * rotationsPerSecond;
  float angularVelocity = degreesPerSecond;

  // Serial.print(leftSpeed);
  // Serial.print("  -  ");
  // Serial.println(rightSpeed);

  // Serial.print(angularSpeed);
  // Serial.print("  ");
  // Serial.print(angularDistance);
  // Serial.print("  ");
  // Serial.println(rotationsPerSecond);

  return angularVelocity;
}