#include "Propulsion.h"

#include "FloatSingle.h"

#include <Arduino.h>

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

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

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