#include "Thing.h"

// #include "Roboid.h"

using namespace Passer::RoboidControl;

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

const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch;
const unsigned int Thing::DistanceSensorType =
    SensorType | (unsigned int)Type::DistanceSensor;
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;

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::GetChild(unsigned char childIx) {
  if (childIx >= 0 && childIx < this->childCount) {
    return this->children[childIx];
  } else
    return nullptr;
}