#include "DifferentialDrive.h"

namespace RoboidControl {

// DifferentialDrive::DifferentialDrive(Participant* participant,
//                                      unsigned char thingId)
//     : Thing(participant, Thing::Type::DifferentialDrive, thingId) {
//   this->leftWheel = new Motor();
//   this->rightWheel = new Motor();
// }

// DifferentialDrive::DifferentialDrive(Thing* parent, unsigned char thingId)
//     : Thing(parent, Thing::Type::DifferentialDrive, thingId) {}

DifferentialDrive::DifferentialDrive(Participant* owner) : Thing(owner, Type::DifferentialDrive) {
  this->leftWheel = new Motor();
  this->rightWheel = new Motor();
}
DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent, Type::DifferentialDrive) {
  this->leftWheel = new Motor();
  this->rightWheel = new Motor();
}

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(bool recursive) {
  if (this->linearVelocityUpdated) {
    // 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(recursive);
}

}  // namespace RoboidControl