#include "Thing.h"

#include <string.h>

using namespace Passer::RoboidControl;

int Thing::lastThingId = 1;

Thing::Thing()
    : CoreThing(0, lastThingId++, (unsigned char)Type::Undetermined) {
  this->id = lastThingId++;
  this->type = (unsigned int)Type::Undetermined;
  this->childCount = 0;
  this->parent = nullptr;
  this->children = nullptr;
}

Thing::Thing(unsigned char id)
    : CoreThing(0, id, (unsigned char)Type::Undetermined) {
  this->id = id;
  this->type = (unsigned int)Type::Undetermined;
  this->childCount = 0;
  this->parent = nullptr;
  this->children = nullptr;
  //  this->position = SphericalOf<signed short>::zero;
}

void Thing::SetName(const char *name) { this->name = name; }

const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch;
const unsigned int Thing::DistanceSensorType =
    SensorType | (unsigned int)Type::DistanceSensor;
const unsigned int Thing::TemperatureSensorType =
    SensorType | (unsigned int)Type::TemperatureSensor;

const unsigned int Thing::ControlledMotorType =
    MotorType | (unsigned int)Type::ControlledMotor;
const unsigned int Thing::UncontrolledMotorType =
    MotorType | (unsigned int)Type::UncontrolledMotor;
const unsigned int Thing::ServoType = (unsigned int)Type::Servo;
const unsigned int Thing::HumanoidType = (unsigned int)Type::Humanoid;

bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; }

bool Thing::IsSensor() { return (type & Thing::SensorType) != 0; }

bool Thing::IsRoboid() { return (type & Thing::RoboidType) != 0; }

void Thing::SetModel(const char *url) { this->modelUrl = url; }

void Thing::SetParent(Thing *parent) {
  if (parent == nullptr)
    return;

  parent->AddChild(this);
}

Thing *Thing::GetParent() { return this->parent; }

void Thing::AddChild(Thing *child) {
  Thing **newChildren = new Thing *[this->childCount + 1];
  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
    newChildren[childIx] = this->children[childIx];
    if (this->children[childIx] == child) {
      // child is already present, stop copying do not update the children
      delete[] newChildren;
      return;
    }
  }

  newChildren[this->childCount] = child;
  child->parent = this;

  if (this->children != nullptr)
    delete[] this->children;

  this->children = newChildren;
  this->childCount++;
}

Thing *Thing::RemoveChild(Thing *child) {
  unsigned char newChildCount = this->childCount - 1;
  Thing **newChildren = new Thing *[newChildCount];

  unsigned char newChildIx = 0;
  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
    if (this->children[childIx] != child) {
      if (newChildIx == newChildCount) { // We did not find the child
        // stop copying and return nothing
        delete[] newChildren;
        return nullptr;
      } else
        newChildren[newChildIx++] = this->children[childIx];
    }
  }

  child->parent = nullptr;

  delete[] this->children;
  this->children = newChildren;
  this->childCount = newChildCount;

  return child;
}

Thing *Thing::GetChild(unsigned char childIx) {
  if (childIx >= 0 && childIx < this->childCount) {
    return this->children[childIx];
  } else
    return nullptr;
}

Thing **Passer::RoboidControl::Thing::GetChildren() { return this->children; }

Thing *Thing::FindChild(const char *name, bool recursive) {
  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
    Thing *child = this->children[childIx];
    if (child == nullptr)
      continue;
    if (strcmp(child->name, name) == 0)
      return child;

    if (recursive) {
      Thing *foundChild = child->FindChild(name);
      if (foundChild != nullptr)
        return foundChild;
    }
  }
  return nullptr;
}

Thing *Passer::RoboidControl::Thing::FindChild(unsigned char thingId,
                                               bool recursive) {
  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
    Thing *child = this->children[childIx];
    if (child == nullptr)
      continue;
    if (child->id == thingId)
      return child;

    if (recursive) {
      Thing *foundChild = child->FindChild(thingId);
      if (foundChild != nullptr)
        return foundChild;
    }
  }
  return nullptr;
}

void Passer::RoboidControl::Thing::SetPosition(Spherical16 position) {
  this->position = position;
  this->positionUpdated = true;
}

void Passer::RoboidControl::Thing::SetOrientation(SwingTwist16 orientation) {
  this->orientation = orientation;
  this->orientationUpdated = true;
}

Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; }

Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; }