#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 = new Propulsion();
  this->networkSync = nullptr;
  this->position = Spherical16::zero;
  this->orientation = SwingTwist16::identity;
}

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

#include <Arduino.h>
void Roboid::Update(unsigned long currentTimeMs) {
  if (perception != nullptr)
    perception->Update(currentTimeMs);

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

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

    this->linearVelocity = this->propulsion->GetVelocity();
    this->angularVelocity = this->propulsion->GetAngularVelocity();

    Spherical16 translation =
        this->orientation * this->linearVelocity * deltaTime;
    SetPosition(this->position + translation);

    SwingTwist16 rotation =
        SwingTwist16::AngleAxis(this->angularVelocity.distance * deltaTime,
                                this->angularVelocity.direction);
    if (perception != nullptr)
      perception->UpdatePose(rotation);

    SetOrientation(this->orientation * rotation);
  }

  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;
}

/*
void Roboid::SetPosition(Spherical16 newWorldPosition) {
  SwingTwist16 roboidOrientation = this->GetOrientation();
  Spherical16 translation = newWorldPosition - this->roboidPosition;
  float distance = translation.distance;
  Angle16 angle = Spherical16::SignedAngleBetween(
      roboidOrientation * Spherical16::forward, translation, Spherical16::up);
  Polar16 polarTranslation = Polar16(
      distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance));
  if (perception != nullptr) {
    printf("roboid translation %f, %f\n", polarTranslation.distance,
           polarTranslation.angle.InDegrees());
    perception->UpdatePose(polarTranslation);
  }

  this->position = newWorldPosition; // roboid is the root?
  // World position should be set in the update recursion
  // this->worldPosition = newWorldPosition;

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

#include <math.h>
void Roboid::SetOrientation(SwingTwist16 newOrientation) {
  SwingTwist16 delta =
      SwingTwist16::Inverse(this->orientation) * newOrientation;
  if (perception != nullptr)
    perception->UpdatePose(delta);

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

void Passer::RoboidControl::Roboid::Release(Thing *child) {
  if (RemoveChild(child) != nullptr)
    this->perception->AddTrackedObject(nullptr, Spherical16::zero,
                                       SwingTwist16::identity, child->type);
}

void Roboid::LoadModel(const char *url) {
  this->modelUrl = url;
  if (this->networkSync == nullptr)
    return;

  this->networkSync->Download(url);
}