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

#include "FloatSingle.h"

#include <Arduino.h>

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

void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
  this->motorCount = 0;
  for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
    Thing* thing = things[thingIx].thing;
    if ((thing->type & Thing::MotorType) != 0)
      motorCount++;
    if (thing->type == (int)Thing::Type::ControlledMotor)
      hasOdometer = true;
  }
  this->placement = new Placement[motorCount];

  unsigned int motorIx = 0;
  for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
    Thing* thing = things[thingIx].thing;
    if ((thing->type & Thing::MotorType) != 0)
      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->type & Thing::MotorType) != 0)
    return (Motor*)thing;

  return nullptr;
}

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

  Placement* placement = &this->placement[motorId];
  if ((placement->thing->type & Thing::MotorType) != 0)
    return placement;

  return nullptr;
}

void Propulsion::Update(float currentTimeMs) {
  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    Thing* thing = placement[motorIx].thing;
    if (thing->type == Thing::ControlledMotorType) {
      ControlledMotor* motor = (ControlledMotor*)thing;
      motor->Update(currentTimeMs);
    }
  }
  this->lastUpdateTime = currentTimeMs;
}

void Propulsion::SetMaxSpeed(float maxSpeed) {
  this->maxSpeed = abs(maxSpeed);
}

// void Propulsion::SetDiffDriveSpeed(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);

//     } else if (thing->type == Thing::ControlledMotorType) {
//       ControlledMotor *motor = (ControlledMotor *)placement[motorIx].thing;
//       if (motor == nullptr)
//         continue;

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

void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}

void Propulsion::SetTwistSpeed(Vector3 linear,
                               float yaw,
                               float pitch,
                               float roll) {
  // if (quadcopter != nullptr)
  //   quadcopter->SetTwistSpeed(linear, yaw);
  // else
  //   SetTwistSpeed(linear.z, yaw);
}

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

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

/// @brief Odometer returns the total distance traveled since start
/// @return The total distance
/// This returns the average distance of all wheels. The odometer cannot be
/// reset. When using a non-directional encoder, the distance is always
/// increasing. When using a directional encoder, the distance may go down when
/// the robot is driving backward.
/// When no wheel encoder is present, this function always returns zero.
float Propulsion::GetOdometer() {
  float odometer = 0;
  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    Thing* thing = placement[motorIx].thing;
    if ((thing->type & Thing::ControlledMotorType) != 0) {
      ControlledMotor* motor = (ControlledMotor*)thing;
      odometer += motor->encoder->GetDistance() / this->motorCount;
    }
  }
  return odometer;
}

bool Propulsion::Drive(Vector3 point, float rotation, float currentTimeMs) {
  if (!this->driving) {
    this->startTime = time(NULL);
    this->targetDistance = point.magnitude();
    if (hasOdometer)
      this->startOdometer = GetOdometer();
    this->driving = true;
  }
  if (hasOdometer) {
    float distance = GetOdometer() - this->startOdometer;
    if (distance >= this->targetDistance) {
      this->driving = false;
      point = Vector3::zero;
      rotation = 0;
    }
  } else {
    double duration = difftime(time(NULL), this->startTime);
    if (duration >= this->targetDistance) {
      this->driving = false;
      point = Vector3::zero;
      rotation = 0;
    }
  }

  if (rotation > 0)
    rotation = 1;
  if (rotation < 0)
    rotation = -1;
  SetTwistSpeed(point.normalized() * this->maxSpeed, rotation);

  Update(currentTimeMs);

  return (!this->driving);
}