RoboidControl-cpp/Thing.h

#pragma once
#include <iostream>
#include <list>
#include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h"

namespace RoboidControl {

class Participant;

#define THING_STORE_SIZE 256
// IMPORTANT: values higher than 256 will need to change the Thing::id type
// to 16-bit or higher, breaking the networking protocol!

/// @brief A thing is the primitive building block
class Thing {
 public:
  Participant* participant; // -> owner
  unsigned char networkId = 0;
  /// @brief The ID of the thing
  unsigned char id = 0;

  /// @brief Predefined thing types
  enum class Type {
    Undetermined,
    // Sensor,
    Switch,
    DistanceSensor,
    DirectionalSensor,
    TemperatureSensor,
    TouchSensor,
    // Motor,
    ControlledMotor,
    UncontrolledMotor,
    Servo,
    // Other
    Roboid,
    Humanoid,
    ExternalSensor,
  };
  /// @brief The type of Thing
  unsigned char type = 0;

  /// @brief Create a new thing of the given type
  /// @param thingType The predefined type of thing
  Thing(Participant* participant, Type thingType = Type::Undetermined);
  /// @brief Create a new thing of the give type
  /// @param thingType The custom type of the thing
  Thing(Participant* participant, unsigned char thingType);
  /// @brief Create a new thing for the given participant
  /// @param participant The participant for which this thing is created
  /// @param networkId The network ID of the thing
  /// @param thingId The ID of the thing
  /// @param thingType The type of thing
  Thing(Participant* participant,
        unsigned char networkId,
        unsigned char thingId,
        Type thingType = Type::Undetermined);

  /// @brief Find a thing by name
  /// @param name Rhe name of the thing
  /// @return The found thing or nullptr when nothing is found
  Thing* FindThing(const char* name);

  /// @brief Sets the parent Thing
  /// @param parent The Thing which should become the parnet
  /// @remark This is equivalent to calling parent->AddChild(this);
  virtual void SetParent(Thing* parent);
  void SetParent(Thing* root, const char* name);
  /// @brief Gets the parent Thing
  /// @return The parent Thing
  Thing* GetParent();

  /// @brief Add a child Thing to this Thing
  /// @param child The Thing which should become a child
  /// @remark When the Thing is already a child, it will not be added again
  virtual void AddChild(Thing* child);
  /// @brief Remove the given thing as a child of this thing
  /// @param child The child to remove
  /// @return The removed child or nullptr if the child could not be found
  Thing* RemoveChild(Thing* child);

  /// @brief The number of children
  unsigned char childCount = 0;
  /// @brief Get a child by thing Id
  /// @param id The thing ID to find
  /// @param recursive Look recursively through all descendants
  /// @return The found thing of nullptr when nothing is found
  Thing* GetChild(unsigned char id, bool recursive = false);
  /// @brief Get a child by index
  /// @param ix The child index
  /// @return The found thing of nullptr when nothing is found
  Thing* GetChildByIndex(unsigned char ix);

 protected:
  Thing* parent = nullptr;
  Thing** children = nullptr;

 public:
  /// @brief The name of the thing
  const char* name = nullptr;
  /// @brief An URL pointing to the location where a model of the thing can be found
  const char* modelUrl = nullptr;
  /// @brief The scale of the model (deprecated I think)
  float modelScale = 1;

  /// @brief Set the position of the thing
  /// @param position The new position in local space, in meters
  void SetPosition(Spherical16 position);
  /// @brief Get the position of the thing
  /// @return The position in local space, in meters
  Spherical16 GetPosition();
  /// @brief Set the orientation of the thing
  /// @param orientation The new orientation in local space
  void SetOrientation(SwingTwist16 orientation);
  /// @brief Get the orientation of the thing
  /// @return The orienation in local space
  SwingTwist16 GetOrientation();
  /// @brief The scale of the thing (deprecated I think)
  float scale = 1;  // assuming uniform scale

  /// @brief boolean indicating if the position was updated
  bool positionUpdated = false;
  /// @brief boolean indicating if the orientation was updated
  bool orientationUpdated = false;

  /// @brief Set the linear velocity of the thing
  /// @param linearVelocity The new linear velocity in local space, in meters per second
  void SetLinearVelocity(Spherical16 linearVelocity);
  /// @brief Get the linear velocity of the thing
  /// @return The linear velocity in local space, in meters per second
  virtual Spherical16 GetLinearVelocity();
  /// @brief Set the angular velocity of the thing
  /// @param angularVelocity the new angular velocity in local space
  void SetAngularVelocity(Spherical16 angularVelocity);
  /// @brief Get the angular velocity of the thing
  /// @return The angular velocity in local space
  virtual Spherical16 GetAngularVelocity();
  bool linearVelocityUpdated = false;
  bool angularVelocityUpdated = false;

 private:
  /// @brief The position in local space
  /// @remark When this Thing has a parent, the position is relative to the
  /// parent's position and orientation
  Spherical16 position;
  /// @brief The orientation in local space
  /// @remark When this Thing has a parent, the orientation is relative to the
  /// parent's orientation
  SwingTwist16 orientation;

  /// @brief The linear velocity in local space
  Spherical16 linearVelocity;
  /// @brief The angular velocity in local spze
  Spherical16 angularVelocity;

 public:
  /// @brief Terminated things are no longer updated
  void Terminate();

  /// @brief Sets the location from where the 3D model of this Thing can be
  /// loaded from
  /// @param url The url of the model
  /// @remark Although the roboid implementation is not dependent on the model,
  /// the only official supported model format is .obj
  void SetModel(const char* url);

#if defined(ARDUINO)
  void Update();
#endif

  /// @brief Updates the state of the thing
  /// @param currentTimeMs The current clock time in milliseconds
  virtual void Update(unsigned long currentTimeMs); // { (void)currentTimeMs; };

  /// @brief Function used to generate binary data for this thing
  /// @param buffer The byte array for thw binary data
  /// @param ix The starting position for writing the binary data
  virtual void GenerateBinary(char* buffer, unsigned char* ix);
  // /// @brief FUnction used to process binary data received for this thing
  /// @param bytes The binary data
  virtual void ProcessBinary(char* bytes);
};

}  // namespace RoboidControl