#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() {
#ifdef RC_DEBUG
  Serial.begin(115200);
#endif
  this->type = (unsigned char)RoboidType;
  this->perception = new Perception();
  this->perception->roboid = this;
  this->propulsion = new Propulsion();
  this->networkSync = nullptr;
  this->position = Spherical16::zero;
  this->orientation = SwingTwist16::identity;
  this->lastUpdateTimeMs = 0;
}

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

void Roboid::Update(unsigned long currentTimeMs) {
  if (this->lastUpdateTimeMs == 0)
    this->lastUpdateTimeMs = currentTimeMs;

  // if (perception != nullptr)
  //   perception->Update(currentTimeMs);

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

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

    this->angularVelocity = this->propulsion->GetAngularVelocity();
    SwingTwist16 rotation =
        SwingTwist16::AngleAxis(this->angularVelocity.distance * deltaTime,
                                this->angularVelocity.direction);
    SetOrientation(this->orientation * rotation);

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

  if (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::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) {
    child->position = this->position;
    child->orientation = this->orientation;
    // this creates an new thing, I wish I could avoid this.
    this->perception->AddTrackedObject(nullptr, child);
  }
}

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

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