#include "DifferentialDrive.h"

namespace RoboidControl {

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

void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation) {
  this->wheelDiameter = abs(wheelDiameter);
  this->wheelSeparation = abs(wheelSeparation);
  this->rpsToMs = wheelDiameter * Passer::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::Update(unsigned long currentMs) {}

}  // namespace RoboidControl