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

namespace Passer {
namespace Control {

#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 basic building block
class Thing {
public:
  // Participant *client;
  unsigned char networkId = 0;
  /// @char The id of the thing
  unsigned char id = 0;

  Thing *FindThing(const char *name);
  //  Thing *FindChild(unsigned char id);

  /// @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);
  Thing *RemoveChild(Thing *child);

  unsigned char childCount = 0;
  Thing *GetChild(unsigned char id, bool recursive = false);
  Thing *GetChildByIndex(unsigned char ix);

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

public:
  /// @brief The type of Thing
  unsigned char type = 0;
  const char *name = nullptr;
  const char *modelUrl = nullptr;
  float modelScale = 1;
  // protected Sensor sensor;

  /// @brief Basic Thing types
  enum class Type {
    Undetermined,
    // Sensor,
    Switch,
    DistanceSensor,
    DirectionalSensor,
    TemperatureSensor,
    // Motor,
    ControlledMotor,
    UncontrolledMotor,
    Servo,
    // Other
    Roboid,
    Humanoid,
    ExternalSensor,
  };

  void SetPosition(Spherical16 position);
  Spherical16 GetPosition();
  void SetOrientation(SwingTwist16 orientation);
  SwingTwist16 GetOrientation();
  float scale = 1; // assuming uniform scale

  bool positionUpdated = false;
  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;
  virtual Spherical16 GetLinearVelocity();
  virtual Spherical16 GetAngularVelocity();

public:
  Thing(unsigned char networkId = 0,
        unsigned char thingType = (unsigned char)Type::Undetermined);
  /// @brief Terminated thins 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) {};

  virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
  virtual void ProcessBytes(unsigned char *bytes) {};

protected:
  virtual void Init();

  //------------ All things
public:
  static Thing *Get(unsigned char networkId, unsigned char thingId);
  static int Add(Thing *thing);
  static void Remove(Thing *thing);
  static void UpdateAll(unsigned long currentTimeMs);

private:
  static Thing *allThings[];
};

} // namespace Control
} // namespace Passer
using namespace Passer::Control;