#pragma once

#include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h"
#include "Thing.h"
#include "float16.h"

namespace Passer {
namespace Control {

class Participant;

class IMessage {
public:
  IMessage();
  virtual unsigned char Serialize(char *buffer);

  static unsigned char *ReceiveMsg(unsigned char packetSize);

  // bool Publish(Participant *participant);
  // bool SendTo(Participant *participant);
};

class InvestigateMsg : public IMessage {
public:
  static const unsigned char id = 0x81;
  static const unsigned char length = 3;
  unsigned char networkId;
  unsigned char thingId;

  InvestigateMsg(char *buffer);
  InvestigateMsg(unsigned char networkId, unsigned char thingId);

  virtual unsigned char Serialize(char *buffer) override;
};

class PoseMsg : public IMessage {
public:
  static const unsigned char id = 0x10;
  unsigned char length = 4 + 4 + 4;

  unsigned char networkId;
  unsigned char thingId;

  unsigned char poseType;
  static const unsigned char Pose_Position = 0x01;
  static const unsigned char Pose_Orientation = 0x02;
  static const unsigned char Pose_LinearVelocity = 0x04;  // For future use
  static const unsigned char Pose_AngularVelocity = 0x08; // For future use

  Spherical16 position;
  SwingTwist16 orientation;
  Spherical16 linearVelocity;
  Spherical16 angularVelocity;

  PoseMsg(unsigned char networkId, unsigned char thingId,
          unsigned char poseType, Spherical16 position,
          SwingTwist16 orientation, Spherical16 linearVelocity = Spherical16(),
          Spherical16 angularVelocity = Spherical16());
  PoseMsg(const char *buffer);

  virtual unsigned char Serialize(char *buffer) override;
};

class CustomMsg : public IMessage {
public:
  static const unsigned char id = 0xB1;
  static const unsigned length = 3;

  unsigned char networkId;
  unsigned char thingId;
  Thing *thing;

  unsigned char bytesSize;
  char *bytes;

  CustomMsg(char *buffer);
  CustomMsg(unsigned char networkId, Thing *thing);

  virtual unsigned char Serialize(char *buffer) override;

  static CustomMsg Receive(char *buffer, unsigned char bufferSize);
};

class DestroyMsg : public IMessage {
public:
  static const unsigned char id = 0x20;
  static const unsigned length = 3;
  unsigned char networkId;
  unsigned char thingId;

  DestroyMsg(unsigned char networkId, Thing *thing);

  virtual unsigned char Serialize(char *buffer) override;
};

} // namespace Control
} // namespace Passer

using namespace Passer::Control;