#include "Messages.h"

namespace RoboidControl {

/// @brief Message to communicate the pose of the thing
/// The pose is in local space relative to the parent. If there is not parent (the thing is a root thing), the pose will
/// be in world space.
class PoseMsg : public IMessage {
 public:
  /// @brief The message ID
  static const unsigned char id = 0x10;
  /// @brief The length of the message
  unsigned char length = 4 + 4 + 4;

  /// @brief The network ID of the thing
  unsigned char networkId;
  /// @brief The ID of the thing
  unsigned char thingId;

  /// @brief Bit pattern stating which pose components are available
  unsigned char poseType;
  /// @brief Bit pattern for a pose with position
  static const unsigned char Pose_Position = 0x01;
  /// @brief Bit pattern for a pose with orientation
  static const unsigned char Pose_Orientation = 0x02;
  /// @brief Bit pattern for a pose with linear velocity
  static const unsigned char Pose_LinearVelocity = 0x04;
  /// @brief Bit pattern for a pose with angular velocity
  static const unsigned char Pose_AngularVelocity = 0x08;

  /// @brief The position of the thing in local space in meters
  Spherical16 position;
  /// @brief The orientation of the thing in local space
  SwingTwist16 orientation;
  /// @brief The linear velocity of the thing in local space in meters per second
  Spherical16 linearVelocity;
  /// @brief The angular velocity of the thing in local space
  Spherical16 angularVelocity;

  /// @brief Create a new message for sending
  /// @param networkId The network ID of the thing
  /// @param thingId The ID of the thing
  /// @param poseType Bit pattern stating which pose components are available
  /// @param position The position of the thing in local space in meters
  /// @param orientation The orientation of the thing in local space
  /// @param linearVelocity The linear velocity of the thing in local space in meters per second
  /// @param angularVelocity The angular velocity of the thing in local space
//   PoseMsg(unsigned char networkId,
//           unsigned char thingId,
//           unsigned char poseType,
//           Spherical16 position,
//           SwingTwist16 orientation,
//           Spherical16 linearVelocity = Spherical16(),
//           Spherical16 angularVelocity = Spherical16());

  /// @brief Create a new message for sending
  /// @param networkId he network ID of the thing
  /// @param thing The thing for which the pose shouldbe sent
  PoseMsg(unsigned char networkId, Thing* thing, bool force = false);

  /// @copydoc RoboidControl::IMessage::IMessage(char*)
  PoseMsg(const char* buffer);
  /// @brief Destructor for the message
  virtual ~PoseMsg();

  /// @copydoc RoboidControl::IMessage::Serialize
  virtual unsigned char Serialize(char* buffer) override;
};

}  // namespace RoboidControl