#include "DifferentialDrive.h"

namespace RoboidControl {
DifferentialDrive::DifferentialDrive() : Thing() {}

RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant)
    : Thing(participant) {}

void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
                                           float wheelSeparation) {
  this->wheelRadius =
      wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
  this->wheelSeparation =
      wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
  this->rpsToMs = wheelDiameter * LinearAlgebra::pi;

  float distance = this->wheelSeparation / 2;
  if (leftWheel != nullptr)
    this->leftWheel->SetPosition(Spherical(distance, Direction::left));
  if (rightWheel != nullptr)
    this->rightWheel->SetPosition(Spherical(distance, Direction::right));
}

void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
  float distance = this->wheelSeparation / 2;
  this->leftWheel = leftWheel;
  ;
  if (leftWheel != nullptr)
    this->leftWheel->SetPosition(Spherical(distance, Direction::left));

  this->rightWheel = rightWheel;
  if (rightWheel != nullptr)
    this->rightWheel->SetPosition(Spherical(distance, Direction::right));
}

void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
  if (this->leftWheel != nullptr)
    this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
  if (this->rightWheel != nullptr)
    this->rightWheel->SetAngularVelocity(
        Spherical(speedRight, Direction::right));
}

void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
  if (this->linearVelocityUpdated == false)
    return;
  // this assumes forward velocity only....
  float linearVelocity = this->GetLinearVelocity().distance;

  Spherical angularVelocity = this->GetAngularVelocity();
  float angularSpeed = angularVelocity.distance * Deg2Rad;  // in degrees/sec
  // Determine the rotation direction
  if (angularVelocity.direction.horizontal.InDegrees() < 0)
    angularSpeed = -angularSpeed;

  // wheel separation can be replaced by this->leftwheel->position->distance
  float speedLeft =
      (linearVelocity + angularSpeed * this->wheelSeparation / 2) /
      this->wheelRadius * Rad2Deg;
  float speedRight =
      (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
      this->wheelRadius * Rad2Deg;

  this->SetWheelVelocity(speedLeft, speedRight);
  Thing::Update(currentMs, recursive);
  // std::cout << "lin. speed " << linearVelocity << "   ang. speed " <<
  // angularVelocity.distance << "   left wheel  "
  //         << speedLeft << "   right wheel " << speedRight << "\n";
}

}  // namespace RoboidControl