#include "Propulsion.h"
#include "Roboid.h"

#include "VectorAlgebra/FloatSingle.h"

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

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

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

  Motor *motor = this->motors[motorId];
  return motor;
}

void Propulsion::Update(float currentTimeMs) {
  for (unsigned char motorIx = 0; motorIx < this->motorCount; motorIx++) {
    Motor *motor = this->motors[motorIx];
    if (motor == nullptr)
      continue;

    motor->Update(currentTimeMs);
  }
}

void Propulsion::SetTargetVelocity(float forwardVelocity) {
  this->targetVelocity = Vector3::forward * forwardVelocity;
}
void Propulsion::SetTargetVelocity(Vector2 horizontalVelocity) {
  this->targetVelocity = Vector3::FromHorizontal(horizontalVelocity);
}
void Propulsion::SetTargetVelocity(Vector3 velocity) {
  this->targetVelocity = velocity;
}
// void Propulsion::SetTargetVelocity(Polar velocity) {}
// void Propulsion::SetTargetVelocity(Spherical velocity) {}

void Propulsion::SetTargetYawVelocity(float yawVelocity) {
  this->targetYawVelocity = yawVelocity;
}
// void Propulsion::SetAngularVelocity(float yaw, float pitch, float roll) {}

float Propulsion::GetTargetForwardVelocity() {
  return this->targetVelocity.Forward();
}

float Propulsion::GetActualForwardVelocity() { return 0; }

Polar Propulsion::GetActualVelocity() { return Polar(0, 0); }

float Propulsion::GetActualYawVelocity() { return 0; }