#include "Propulsion.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++;
  }
  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) {
  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) {}