RoboidControl-cpp/Thing.cpp

#include "Thing.h"

#include "Messages/PoseMsg.h"
#include "Participant.h"
#include "Participants/IsolatedParticipant.h"

#include <string.h>
// #include <iostream>

#if defined(ARDUINO)
#include "Arduino.h"
#else
#include <algorithm>
#include <chrono>
#include <iostream>
#include <list>
#endif

namespace RoboidControl {

#pragma region Init

Thing& Thing::LocalRoot() {
  static Thing localRoot(Thing::Type::Undetermined, localRoot);
  return localRoot;
}

Thing::Thing(unsigned char thingType, Thing& parent) {
  this->type = thingType;

  this->position = Spherical::zero;
  this->positionUpdated = true;
  this->orientation = SwingTwist::identity;
  this->orientationUpdated = true;
  this->hierarchyChanged = true;

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

  this->owner = &Participant::LocalParticipant();
  this->owner->Add(this, true);
  this->SetParent(parent);
}

// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType,
// unsigned char thingId) {
//   Thing thing = Thing(owner, thingType);
//   thing.id = thingId;
//   return thing;
// }

#pragma endregion Init

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

const char* Thing::GetName() const {
  return this->name;
}

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

#pragma region Hierarchy

// 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);
//   this->hierarchyChanged = true;
// }

void Thing::SetParent(Thing& parent) {
  parent.AddChild(this);
  this->hierarchyChanged = true;
}

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

bool Thing::IsRoot() const {
  return this == &LocalRoot();  //&Thing::Root;
}

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

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

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

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 = &Thing::LocalRoot();

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

  return child;
}

Thing* Thing::GetChild(unsigned char id, bool recurse) {
  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 (recurse) {
      Thing* foundChild = child->GetChild(id, recurse);
      if (foundChild != nullptr)
        return foundChild;
    }
  }
  return nullptr;
}

Thing* Thing::FindChild(const char* name, bool recurse) {
  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->FindChild(name);
    if (foundChild != nullptr)
      return foundChild;
  }
  return nullptr;
}

#pragma endregion Hierarchy

#pragma region Pose

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

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

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

void Thing::SetLinearVelocity(Spherical linearVelocity) {
  if (this->linearVelocity.distance != linearVelocity.distance) {
    this->linearVelocity = linearVelocity;
    this->linearVelocityUpdated = true;
  }
}

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

void Thing::SetAngularVelocity(Spherical angularVelocity) {
  if (this->angularVelocity.distance != angularVelocity.distance) {
    this->angularVelocity = angularVelocity;
    this->angularVelocityUpdated = true;
  }
}

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

#pragma endregion Pose

#pragma region Update

unsigned long Thing::GetTimeMs() {
#if defined(ARDUINO)
  unsigned long ms = millis();
  return ms;
#else
  auto now = std::chrono::steady_clock::now();
  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
      now.time_since_epoch());
  return static_cast<unsigned long>(ms.count());
#endif
}

// void Thing::Update(bool recursive) {
//   Update(GetTimeMs(), recursive);
// }

void Thing::Update(bool recursive) {
  // if (this->positionUpdated || this->orientationUpdated)
  //   OnPoseChanged callback
  this->positionUpdated = false;
  this->orientationUpdated = false;
  // this->linearVelocityUpdated = false;
  // this->angularVelocityUpdated = false;
  this->hierarchyChanged = false;
  this->nameChanged = false;

  if (recursive) {
    // std::cout << "# children: " << (int)this->childCount << std::endl;
    for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
      Thing* child = this->children[childIx];
      if (child == nullptr)
        continue;
      child->Update(recursive);
    }
  }
}

void Thing::UpdateThings() {
  IsolatedParticipant::Isolated()->Update();
}

#pragma endregion Update

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

}  // namespace RoboidControl