#pragma once

#if !defined(NO_STD)
#include <iostream>
#include <list>
#endif
#include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h"

namespace RoboidControl {

class Participant;
class ParticipantUDP;

#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:
  /// @brief Predefined thing types
  enum Type : unsigned char {
    Undetermined,
    // Sensor,
    Switch,
    DistanceSensor,
    DirectionalSensor,
    TemperatureSensor,
    TouchSensor,
    // Motor,
    ControlledMotor,
    UncontrolledMotor,
    Servo,
    IncrementalEncoder,
    // Other
    Roboid,
    Humanoid,
    ExternalSensor,
    DifferentialDrive
  };

#pragma region Init
  static Thing Root;  // = Thing(Type::Undetermined, Root);

  Thing(unsigned char thingType = Type::Undetermined, Thing& parent = Root);

  /// @brief Create a new thing for a participant
  /// @param owner The owning participant
  /// @param thingType The type of thing (can use Thing::Type)
  /// @param thingId The ID of the thing, leave out or set to zero to generate
  /// an ID
  Thing(Participant* owner = nullptr,
        unsigned char thingType = Type::Undetermined);
  // unsigned char thingId = 0);

  /// @brief Create a new child thing
  /// @param parent The parent thing
  /// @param thingType The type of thing (can use Thing::Type)
  /// @param thingId The ID of the thing, leave out or set to zero to generate
  /// an ID
  /// @note The owner will be the same as the owner of the parent thing
  Thing(Thing* parent,
        unsigned char thingType = 0);  //, unsigned char thingId = 0);

  static Thing Reconstruct(Participant* owner,
                           unsigned char thingType,
                           unsigned char thingId);

#pragma endregion Init

 public:
  /// @brief Terminated things are no longer updated
  bool terminate = false;

#pragma region Properties

  /// @brief The participant managing this thing
  Participant* owner = nullptr;
  /// @brief The ID of the thing
  unsigned char id = 0;

  /// @brief The type of Thing
  /// This can be either a Thing::Type of a byte value for custom types
  unsigned char type = Type::Undetermined;

  /// @brief The name of the thing
  const char* name = nullptr;

 public:
  void SetName(const char* name);
  const char* GetName() const;
  bool nameChanged = false;

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

#pragma endregion Properties

#pragma region Hierarchy

  /// @brief Sets the parent of this Thing
  /// @param parent The Thing which should become the parent
  virtual void SetParent(Thing* parent);
  /// @brief Gets the parent of this Thing
  /// @return The parent Thing
  Thing* GetParent();

  /// @brief The number of children
  unsigned char childCount = 0;
  /// @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);

  /// @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 Get a child by thing Id
  /// @param id The thing ID to find
  /// @param recurse Look recursively through all descendants
  /// @return The found thing of nullptr when nothing is found
  Thing* GetChild(unsigned char id, bool recurse = false);

  /// @brief Find a thing by name
  /// @param name The name of the thing
  /// @param recurse Look recursively through all descendants
  /// @return The found thing or nullptr when nothing is found
  Thing* FindChild(const char* name, bool recurse = true);

  /// @brief Indicator that the hierarchy of the thing has changed
  bool hierarchyChanged = true;

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

#pragma endregion Hierarchy

#pragma region Pose

 public:
  /// @brief Set the position of the thing
  /// @param position The new position in local space, in meters
  void SetPosition(Spherical position);
  /// @brief Get the position of the thing
  /// @return The position in local space, in meters
  Spherical GetPosition();
  /// @brief Boolean indicating that the thing has an updated position
  bool positionUpdated = false;

  /// @brief Set the orientation of the thing
  /// @param orientation The new orientation in local space
  void SetOrientation(SwingTwist orientation);
  /// @brief Get the orientation of the thing
  /// @return The orienation in local space
  SwingTwist GetOrientation();
  /// @brief Boolean indicating the thing has an updated orientation
  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(Spherical linearVelocity);
  /// @brief Get the linear velocity of the thing
  /// @return The linear velocity in local space, in meters per second
  virtual Spherical GetLinearVelocity();
  /// @brief Boolean indicating the thing has an updated linear velocity
  bool linearVelocityUpdated = false;

  /// @brief Set the angular velocity of the thing
  /// @param angularVelocity the new angular velocity in local space
  virtual void SetAngularVelocity(Spherical angularVelocity);
  /// @brief Get the angular velocity of the thing
  /// @return The angular velocity in local space
  virtual Spherical GetAngularVelocity();
  /// @brief Boolean indicating the thing has an updated angular velocity
  bool angularVelocityUpdated = false;

 private:
  /// @brief The position of the thing in local space, in meters
  /// @remark When this Thing has a parent, the position is relative to the
  /// parent's position and orientation
  Spherical position;
  /// @brief The orientation of the thing in local space
  /// @remark When this Thing has a parent, the orientation is relative to the
  /// parent's orientation
  SwingTwist orientation;

  /// @brief The linear velocity of the thing in local space, in meters per
  /// second
  Spherical linearVelocity;
  /// @brief The angular velocity of the thing in local space, in degrees per
  /// second
  Spherical angularVelocity;

#pragma endregion Pose

#pragma region Update

 public:
  /// @brief Get the current time in milliseconds
  /// @return The current time in milliseconds
  static unsigned long GetTimeMs();

  /// @brief Updates the state of the thing
  // void Update(bool recursive = false);

  /// @brief Updates the state of the thing
  /// @param currentTimeMs The current clock time in milliseconds; if this is
  /// zero, the current time is retrieved automatically
  /// @param recurse When true, this will Update the descendants recursively
  virtual void Update(bool recurse = false);

  static void UpdateThings();

#pragma endregion Update

 public:
  /// @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
  /// @return The size of the binary data
  virtual int 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