#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->IsMotor())
      motorCount++;
    if (thing->type == Thing::ControlledMotorType)
      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->IsMotor())
      this->placement[motorIx++] = things[thingIx];
  }
}

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

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

  Placement* placement = &this->placement[motorId];
  if (placement->thing->IsMotor())
    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::SetTwistSpeed(float forward, float yaw) {}

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

void Propulsion::SetTwistSpeed(Vector3 linear,
                               float yaw,
                               float pitch,
                               float roll) {}

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