RoboidControl-cpp/Thing.h

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

namespace Passer {
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:
  // RemoteParticipant *client;

  /// @brief The network ID of this thing
  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,
    // 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(Type thingType = Type::Undetermined);
  /// @brief Create a new thing of the give type
  /// @param thingType The custom type of the thing
  Thing(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;

protected:
  /// @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;

public:
  Spherical16 linearVelocity;
  Spherical16 angularVelocity;

  /// @brief Get the linear velocity of the thing
  /// @return The linear velocity in local space, in meters per second
  virtual Spherical16 GetLinearVelocity();
  /// @brief Get the angular velocity of the thing
  /// @return The angular velocity in local space
  virtual Spherical16 GetAngularVelocity();

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

  /// @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);

protected:
  virtual void Init();
};


} // namespace RoboidConttrol
} // namespace Passer
using namespace Passer::RoboidControl;