RoboidControl-cpp/Thing.cpp

#include "Thing.h"

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

namespace RoboidControl {

Thing::Thing(Participant* owner, Type thingType) : Thing(owner, (unsigned char)thingType) {}

Thing::Thing(Participant* owner, unsigned char thingType) {
  this->participant = owner;
  this->id = 0;
  this->type = thingType;
  this->networkId = 0;

  this->position = Spherical16::zero;
  this->orientation = SwingTwist16::identity;

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

  // std::cout << "add thing to participant\n";
  owner->Add(this);
}

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

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

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

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

#if defined(ARDUINO)
void Thing::Update() {
  Update(millis());
}
#endif

void Thing::Update(unsigned long currentTimeMs) {
  (void)currentTimeMs;

  // PoseMsg* poseMsg = new PoseMsg(this->networkId, this);
  // participant->Send(remoteParticipant, poseMsg);
  // delete poseMsg;

}

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

void Thing::SetLinearVelocity(Spherical16 linearVelocity) {
  this->linearVelocity = linearVelocity;
  this->linearVelocityUpdated = true;
}

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

void Thing::SetAngularVelocity(Spherical16 angularVelocity) {
  this->angularVelocity = angularVelocity;
  this->angularVelocityUpdated = true;
}

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

}  // namespace RoboidControl