#include "DifferentialDrive.h"

#include "FloatSingle.h"

DifferentialDrive::DifferentialDrive(){};

DifferentialDrive::DifferentialDrive(Placement leftMotorPlacement,
                                     Placement rightMotorPlacement) {
  this->motorCount = 2;
  this->placement = new Placement[2];
  this->placement[0] = leftMotorPlacement;
  this->placement[1] = rightMotorPlacement;
  Motor* leftMotor = (Motor*)leftMotorPlacement.thing;
  leftMotor->direction = Motor::Direction::CounterClockwise;
  Motor* rightMotor = (Motor*)rightMotorPlacement.thing;
  rightMotor->direction = Motor::Direction::Clockwise;
}

void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    Thing* thing = placement[motorIx].thing;
    if (thing->type == Thing::UncontrolledMotorType) {
      Motor* motor = (Motor*)thing;
      if (motor == nullptr)
        continue;

      float xPosition = placement[motorIx].position.x;
      if (xPosition < 0)
        motor->SetSpeed(leftSpeed);
      else if (xPosition > 0)
        motor->SetSpeed(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);
}