#include "Thing.h"

#include "Participant.h"
#include <algorithm>
#include <iostream>
#include <list>
#include <string.h>

Thing::Thing(Type thingType) : Thing((unsigned char)thingType) {}

Thing::Thing(unsigned char thingType) {
  // this->position = Spherical16::zero;
  // this->orientation = SwingTwist16::identity;

  this->id = 0;
  this->type = thingType;
  this->networkId = 0;
  this->Init();

  // int thingId = Thing::Add(this);

  // if (thingId < 0) {
  //   std::cout << "ERROR: Thing store is full\n";
  //   this->id = 0; // what to do when we cannot store any more things?
  // } else
  //   this->id = thingId;

  this->linearVelocity = Spherical16::zero;
  this->angularVelocity = Spherical16::zero;
}

Passer::RoboidControl::Thing::Thing(Participant *participant,
                                    unsigned char networkId,
                                    unsigned char thingId, Type thingType) {
  // no participant reference yet..
  this->networkId = networkId;
  this->id = thingId;
  this->type = (unsigned char)thingType;
  this->Init();

  // thingId = Thing::Add(this);

  // if (thingId < 0) {
  //   std::cout << "ERROR: Thing store is full\n";
  //   this->id = 0; // what to do when we cannot store any more things?
  // } else
  // this->id = thingId;

  this->linearVelocity = Spherical16::zero;
  this->angularVelocity = Spherical16::zero;
  std::cout << "Created thing " << (int)this->networkId << "/" << (int)this->id
            << "\n";
}

void Thing::Terminate() {
  // Thing::Remove(this);
}

void Thing::Init() {}

Thing *Thing::FindThing(const char *name) {
  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
    Thing *child = this->children[childIx];
    if (child == nullptr || child->name == nullptr)
      continue;

    if (strcmp(child->name, name) == 0)
      return child;

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

void Thing::SetParent(Thing *parent) {
  if (parent == nullptr) {
    Thing *parentThing = this->parent;
    if (parentThing != nullptr)
      parentThing->RemoveChild(this);
    this->parent = nullptr;
  } else
    parent->AddChild(this);
}

void Thing::SetParent(Thing *root, const char *name) {
  Thing *thing = root->FindThing(name);
  if (thing != nullptr)
    this->SetParent(thing);
}

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

void Thing::AddChild(Thing *child) {

  unsigned char newChildCount = this->childCount + 1;
  Thing **newChildren = new Thing *[newChildCount];

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

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 id, bool recursive) {
  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
    Thing *child = this->children[childIx];
    if (child == nullptr)
      continue;
    if (child->id == id)
      return child;

    if (recursive) {
      Thing *foundChild = child->GetChild(id, recursive);
      if (foundChild != nullptr)
        return foundChild;
    }
  }
  return nullptr;
}

Thing *Thing::GetChildByIndex(unsigned char ix) { return this->children[ix]; }

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

void Thing::GenerateBinary(char *buffer, unsigned char *ix) {
  (void)buffer;
  (void)ix;
}
void Thing::ProcessBinary(char *bytes) { (void)bytes; };

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

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

SwingTwist16 Thing::GetOrientation() { return this->orientation; }

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

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

// All things
// std::list<Thing *> Thing::allThings;

// Thing *Thing::Get(unsigned char networkId, unsigned char thingId) {
//   std::cout << "Get " << (int)networkId << "/" << (int)thingId << " from "
//             << allThings.size() << " things\n";
//   for (auto &thing : allThings) {
//     std::cout << "  ? " << (int)thing->networkId << "/" << (int)thing->id
//               << "\n";
//     if (thing->networkId == networkId && thing->id == thingId) {
//       return thing;
//     }
//   }
//   return nullptr;
// }

// int Thing::Add(Thing *newThing) {
//   for (Thing *thing : allThings) {
//     if (thing == newThing)
//       return thing->id;
//   }
//   std::cout << "Adding " << (int)newThing->networkId << "/" <<
//   (int)newThing->id
//             << "\n";
//   allThings.push_back(newThing);
//   return allThings.size();
// }

// void Thing::Remove(Thing *thing) {
//   Thing::allThings.remove_if([thing](Thing *obj) { return obj == thing; });
//   std::cout << "Removing " << thing->networkId << "/" << thing->id
//             << " list size = " << allThings.size() << "\n";
// }

// void Thing::UpdateAll(unsigned long currentTimeMs) {
//   // Not very efficient, but it works for now.

//   for (Thing *thing : Thing::allThings) {
//     if (thing != nullptr &&
//         thing->parent == nullptr) { // update all root things
//       // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
//       //           << "\n";
//       thing->Update(currentTimeMs);
//     }
//   }
//}