#include "Roboid.h"

#include "NetworkSync.h"

#ifdef RC_DEBUG
#include <Arduino.h>
#endif

Roboid::Roboid() {
#ifdef RC_DEBUG
  Serial.begin(115200);
#endif
}

Roboid::Roboid(Perception *perception, Propulsion *propulsion) : Roboid() {
  this->perception = perception;
  perception->roboid = this;
  this->propulsion = propulsion;
  propulsion->roboid = this;
}

Roboid::Roboid(ServoMotor **actuation) : actuation(actuation) {}

void Roboid::Update(float currentTimeMs) {
  if (perception != nullptr)
    perception->Update(currentTimeMs);
  if (propulsion != nullptr)
    propulsion->Update(currentTimeMs);
  if (networkSync != nullptr)
    networkSync->NetworkUpdate(this);
}

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

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

void Roboid::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);
  perception->UpdatePose(polarTranslation);
  this->worldPosition = newWorldPosition;
}

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