#pragma once

#include "LinearAlgebra/AngleAxis.h"
#include "LinearAlgebra/Quaternion.h"
#include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h"

namespace Passer {
namespace RoboidControl {

/// @brief A thing is a functional component on a robot
class Thing {
public:
  /// @brief Default constructor for a Thing
  Thing(unsigned char id);

  /// @char The id of the thing
  unsigned char id;
  /// @brief The type of Thing
  unsigned int type;

  // I hate this, better is to have an additional field stating the thing
  // classificaton Motor, Sensor etc.
  /// @brief The type of a switch sensor
  static const unsigned int SwitchType;
  /// @brief The type of a distance sensor
  static const unsigned int DistanceSensorType;
  static const unsigned int TemperatureSensorType;
  /// @brief The type of a controlled motor
  static const unsigned int ControlledMotorType;
  /// @brief The type of an uncontrolled motor
  static const unsigned int UncontrolledMotorType;
  /// @brief The type of an object received from the network
  static const unsigned int ServoType;

  /// @brief Check if the Thing is a Motor
  /// @return True when the Thing is a Motor and False otherwise
  bool IsMotor();
  /// @brief Check if the Thing is a Sensor
  /// @return True when the Thing is a Sensor and False otherwise
  bool IsSensor();
  /// @brief Check if the Thing is a Roboid
  /// @return True when the Thing is a Roboid and False otherwise
  bool IsRoboid();

  /// @brief The position of this Thing
  /// @remark When this Thing has a parent, the position is relative to the
  /// parent's position and orientation
  Spherical16 position;
  Spherical16 worldPosition;
  /// @brief The orientation of this Thing
  /// @remark When this Thing has a parent, the orientation is relative to the
  /// parent's orientation
  SwingTwist16 orientation;
  SwingTwist16 worldOrientation;

  virtual Spherical16 GetLinearVelocity();
  virtual Spherical16 GetAngularVelocity();

  /// @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);
  /// @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 Get the child at the given index
  /// @param childIx The index of the child
  /// @return The child at the given index or nullptr when the index is invalid
  /// or the child could not be found
  Thing *GetChild(unsigned char childIx);
  Thing *RemoveChild(Thing *child);

  /// @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) {}

  unsigned char childCount = 0;

  const char *modelUrl = nullptr;

protected:
  /// @brief Bitmask for Motor type
  static const unsigned int MotorType = 0x8000;
  /// @brief Bitmap for Sensor type
  static const unsigned int SensorType = 0x4000;
  /// @brief Bitmap for Roboid type
  static const unsigned int RoboidType = 0x2000;

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

  Thing *parent = nullptr;
  Thing **children = nullptr;

public:
  Spherical16 angularVelocity = Spherical16::zero;
  Spherical16 linearVelocity = Spherical16::zero;
};

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