#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 = Spherical16::zero;
  this->worldOrientation = SwingTwist16::identity;
  // this->worldOrientation = Quaternion::identity;
  // this->worldAngleAxis = AngleAxisOf<float>();
}

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

void Passer::RoboidControl::Roboid::SetName(char* name) {
  this->name = name;
}

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

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

    float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;

    SetPosition(this->worldPosition +
                this->worldOrientation * Spherical16::forward *
                    this->propulsion->GetVelocity().distance * deltaTime);
    SetOrientation(this->worldOrientation *
                   SwingTwist16::AngleAxis(
                       this->propulsion->GetAngularVelocity() * deltaTime,
                       Spherical16::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;
}

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

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

// 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.vertical.InDegrees() > maxAngle)
//     return this->worldAngleAxis.angle;
//   if (this->worldAngleAxis.axis.vertical.InDegrees() < -maxAngle)
//     return -this->worldAngleAxis.angle;

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

void Roboid::SetPosition(Spherical16 newWorldPosition) {
  SwingTwist16 roboidOrientation = this->GetOrientation();
  Spherical16 translation = newWorldPosition - this->worldPosition;
  float distance = translation.distance;
  Angle16 angle = Spherical16::SignedAngleBetween(
      roboidOrientation * Spherical16::forward, translation, Spherical16::up);
  Polar polarTranslation = Polar(angle.ToFloat(), 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(SwingTwist16 worldOrientation) {
  // float angle;
  // Vector3 axis;
  // worldOrientation.ToAngleAxis(&angle, &axis);

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

  // AngleAxisOf<float> angleAxis =
  //     AngleAxisOf<float>(angle, DirectionOf<float>(axis));
  // this->worldAngleAxis = angleAxis;
}

// void Roboid::SetOrientation2D(float angle) {
//   this->worldAngleAxis = AngleAxisOf<float>(angle, DirectionOf<float>::up);
// }

Vector3 Passer::RoboidControl::Roboid::GetVelocity() {
  return Vector3();
}

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