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()
  {
    Participant *p = Participant::LocalParticipant;
    Thing *localRoot = p->root;
    return localRoot;
  }

  // Only use this for root things
  Thing::Thing(Participant *owner)
  {
    this->type = Type::Root; // should become root

    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 = owner;
    // this->owner->Add(this, true);
    //std::cout << this->owner->name << ": New root thing " << std::endl;
  }

  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 = parent->owner;
    this->owner->Add(this, true);
    this->SetParent(parent);

    std::cout << this->owner->name << ": New thing for " << parent->name
              << std::endl;
  }

  Thing::~Thing()
  {
    std::cout << "Destroy thing " << this->name << std::endl;
  }

  Thing Thing::Reconstruct(Participant *owner, unsigned char thingType, unsigned char thingId)
  {
    Thing thing = Thing(thingType, owner->root);
    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() || this->parent == nullptr; //&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::PrepareForUpdate() {}

  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