#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
  Spherical position;
  /// @brief The orientation of the thing in local space
  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
  Spherical angularVelocity;

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