#include "Roboid.h"

#include "LinearAlgebra/FloatSingle.h"
#include "NetworkSync.h"

#include <string.h>

// #define RC_DEBUG

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

Roboid::Roboid() : Thing(0) {
#ifdef RC_DEBUG
  Serial.begin(115200);
#endif
  this->type = RoboidType;
  this->perception = new Perception();
  this->perception->roboid = this;
  this->propulsion = nullptr;
  this->networkSync = nullptr;
  // this->actuation = nullptr;
  this->worldPosition = Vector3::zero;
  // this->worldOrientation = Quaternion::identity;
  this->worldAngleAxis = AngleAxis();
}

Roboid::Roboid(Propulsion *propulsion) : Roboid() {
  this->propulsion = propulsion;
  if (propulsion != nullptr)
    propulsion->roboid = this;
}

void Roboid::Update(unsigned long currentTimeMs) {
  if (perception != nullptr)
    perception->Update(currentTimeMs);

  if (propulsion != nullptr) {
    propulsion->Update(currentTimeMs);

    float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
    Quaternion roboidOrientation = this->GetOrientation();
    SetPosition(this->worldPosition +
                roboidOrientation * Vector3::forward *
                    this->propulsion->GetVelocity().distance * deltaTime);
    SetOrientation(roboidOrientation *
                   Quaternion::AngleAxis(this->propulsion->GetAngularVelocity(),
                                         Vector3::up));
  }

  if (childCount > 0 && children != nullptr) {
    for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
      children[childIx]->Update(currentTimeMs);
    }
  }

  if (networkSync != nullptr)
    networkSync->NetworkUpdate(this);

  lastUpdateTimeMs = currentTimeMs;
}

Vector3 Roboid::GetPosition() { return this->worldPosition; }
Vector2 Roboid::GetPosition2D() {
  return Vector2(this->worldPosition.Right(), this->worldPosition.Forward());
}

Quaternion Roboid::GetOrientation() {
  Vector3 axis = this->worldAngleAxis.axis.ToVector3();
  Quaternion q = Quaternion::AngleAxis(this->worldAngleAxis.angle, axis);
  return q;
}

float Roboid::GetOrientation2D() {
  float maxAngle = 90 - Float::epsilon; // note: range vertical angle = -90..90

  // rotation axis is vertical, so we have a simple 2D orientation
  if (this->worldAngleAxis.axis.verticalAngle > maxAngle)
    return this->worldAngleAxis.angle;
  if (this->worldAngleAxis.axis.verticalAngle < -maxAngle)
    return -this->worldAngleAxis.angle;

  Quaternion q = GetOrientation();
  return Quaternion::GetAngleAround(Vector3::up, q);
}

void Roboid::SetPosition(Vector3 newWorldPosition) {
  Quaternion roboidOrientation = this->GetOrientation();
  Vector3 translation = newWorldPosition - this->worldPosition;
  float distance = translation.magnitude();
  float angle = Vector3::SignedAngle(roboidOrientation * Vector3::forward,
                                     translation, Vector3::up);
  Polar polarTranslation = Polar(angle, distance);
  if (perception != nullptr)
    perception->UpdatePose(polarTranslation);
  this->worldPosition = newWorldPosition;

  if (networkSync != nullptr)
    // networkSync->SendPosition(this->worldPosition);
    networkSync->SendPose(this->worldPosition, roboidOrientation);
}

#include <math.h>
void Roboid::SetOrientation(Quaternion worldOrientation) {
  float angle;
  Vector3 axis;
  worldOrientation.ToAngleAxis(&angle, &axis);

  Quaternion delta = Quaternion::Inverse(GetOrientation()) * worldOrientation;
  if (perception != nullptr)
    perception->UpdatePose(delta);

  AngleAxis angleAxis = AngleAxis(angle, Axis(axis));
  this->worldAngleAxis = angleAxis;
}

void Roboid::SetOrientation2D(float angle) {
  this->worldAngleAxis = AngleAxis(angle, Axis::up);
}

void Roboid::AddChild(Thing *child) {
  Thing::AddChild(child);
  if (child->IsSensor()) {
    Sensor *childSensor = (Sensor *)child;
    this->perception->AddSensor(childSensor);
  }
}