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

#include "FloatSingle.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) {}

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

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

Vector3 Propulsion::GetPosition() { return this->worldPosition; }

Quaternion Propulsion::GetOrientation() { return this->worldOrientation; }

void Propulsion::SetPosition(Vector3 newWorldPosition) {
  Vector3 translation = newWorldPosition - this->worldPosition;
  float distance = translation.magnitude();
  float angle = Vector3::SignedAngle(this->worldOrientation * Vector3::forward,
                                     translation, Vector3::up);
  Polar polarTranslation = Polar(angle, distance);
  roboid->perception->UpdatePose(polarTranslation);
  this->worldPosition = newWorldPosition;
}

void Propulsion::SetOrientation(Quaternion worldOrientation) {
  Quaternion delta =
      Quaternion::Inverse(this->worldOrientation) * worldOrientation;
  roboid->perception->UpdatePose(delta);
  this->worldOrientation = worldOrientation;
}