#include <ControlledMotor.h>
#include <Propulsion.h>

#include <FloatSingle.h>

Propulsion::Propulsion() {
  this->motors = nullptr;
  this->motorCount = 0;
}

void Propulsion::AddMotors(MotorPlacement* motors, unsigned int motorCount) {
  this->motors = motors;
  this->motorCount = motorCount;
}

void Propulsion::AddMotors(Placement* motors, unsigned int motorCount) {
  this->placement = motors;
  this->motorCount = motorCount;
}

void Propulsion::Update() {
  // Hmmm. Arduino dependent code
  // unsigned long curMillis = millis();
  // float timeStep = (float)(curMillis - lastMillis) / 1000;
  // lastMillis = curMillis;

  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    MotorPlacement placement = motors[motorIx];
    // placement.controlledMotor->Update(timeStep);
  }
}

void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    Motor* motor = motors[motorIx].motor;
    if (motor == nullptr)
      continue;

    float xPosition = motors[motorIx].position.x;
    if (xPosition < 0)
      motor->SetSpeed(leftSpeed);
    else if (xPosition > 0)
      motor->SetSpeed(rightSpeed);
  };
}

void Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) {
  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    MotorPlacement placement = motors[motorIx];
    if (placement.position.x < 0)
      placement.controlledMotor->SetTargetVelocity(leftVelocity);
    else if (placement.position.x > 0)
      placement.controlledMotor->SetTargetVelocity(rightVelocity);
  };
}

void Propulsion::SetTwistSpeed(float forward, float yaw) {
  // This is configuration dependent, a drone will do something completely different...
  float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
  float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
  SetDiffDriveSpeed(leftSpeed, rightSpeed);
}

void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) {
  float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
  float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
  SetDiffDriveSpeed(leftSpeed, rightSpeed);
}

void Propulsion::SetTwistVelocity(float forwardVelocity, float turningVelocity) {
  float leftVelocity = Float::Clamp(forwardVelocity - turningVelocity, -1, 1);
  float rightVelocity = Float::Clamp(forwardVelocity + turningVelocity, -1, 1);
  SetDiffDriveVelocities(leftVelocity, rightVelocity);
}