#include "Propulsion.h"
#include "ControlledMotor.h"

#include "FloatSingle.h"

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

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

void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
  // this->placement = motors;
  // this->motorCount = motorCount;
  this->motorCount = 0;
  for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
    Thing* thing = things[thingIx].thing;
    if (thing->isMotor)
      motorCount++;
  }
  this->placement = new Placement[motorCount];

  unsigned int motorIx = 0;
  for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
    Thing* thing = things[thingIx].thing;
    if (thing->isMotor)
      this->placement[motorIx++] = things[thingIx];
  }
}

void Propulsion::AddQuadcopter(Quadcopter* quadcopter) {
  this->quadcopter = quadcopter;
}

unsigned int Propulsion::GetMotorCount() {
  return this->motorCount;
}

Motor* Propulsion::GetMotor(unsigned int motorId) {
  if (motorId >= this->motorCount)
    return nullptr;

  Thing* thing = this->placement[motorId].thing;
  if (thing->isMotor)
    return (Motor*)thing;

  return nullptr;
}

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++) {
    // Placement placement = placement[motorIx];
    //  placement.controlledMotor->Update(timeStep);
  }
}

void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    Motor* motor = (Motor*)placement[motorIx].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 Propulsion::SetDiffDriveVelocities(float leftVelocity, float rightVelocity) {
  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    // Placement placement = placement[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);

  if (quadcopter != nullptr) {
    quadcopter->SetTwistSpeed(forward, yaw, pitch);
  }
}

void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
  if (quadcopter != nullptr)
    quadcopter->SetTwistSpeed(linear, yaw);
}

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);
}

void Propulsion::SetLinearSpeed(Vector3 velocity, float yawSpeed, float rollSpeed) {
  if (quadcopter != nullptr)
    quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
}

Quadcopter* Propulsion::GetQuadcopter() {
  return quadcopter;
}