#ifndef _VN200_
#define _VN200_

#include <map>
#include <stdint.h>
#include <vector>

#define VN200_SPI_READ 0x01
#define VN200_SPI_WRITE 0x02

namespace VN200 {
namespace SPI {
namespace BinaryGroups {
typedef uint8_t bit_offset_t;
namespace Group1 {
// /* The bit offset of group 1 */
// const bit_offset_t BitOffset = 0;
// /* The output fields of group 1 */
// enum OutputFields {
//   TimeStartup = 0,
//   TimeGps,
//   TimeSyncIn,
//   YawPitchRoll,
//   Quaternion,
//   AngularRate,
//   Position,
//   Velocity,
//   Accel,
//   Imu,
//   MagPres,
//   DeltaTheta,
//   InsStatus,
//   SyncInCnt,
//   TimeGpsPps,
//   EMPTY15
// };

// /* TODO */
// const uint8_t payload_sizes[16] = {8, 8, 8, 12, 16, 12, 24, 12, 12, 24, 20, 28, 2, 4, 8, 0};
} // namespace Group1
} // namespace BinaryGroups

// /* Abstract base class for all packages. */
// struct binary_package_t {
//   /* TODO */
//   binary_package_t(){};
//   /* TODO */
//   virtual uint16_t size() = 0;
//   /* TODO */
//   virtual uint8_t *to_array() = 0;

//   std::vector<uint8_t> v;
// };

/* TODO */
union header_t {
  struct request_t {
    uint8_t Cmd = 0;    /* Defined in VN200_SPI_READ - set in standard constructor */
    uint8_t ID = 0;     /* Register ID */
    uint16_t Empty = 0; /* Spacer */
    request_t(uint8_t ID, uint8_t Cmd) {
      this->ID = ID;
      this->Cmd = Cmd;
    };
  };
  struct response_t {
    uint8_t Empty1 = 0; /* Spacer */
    uint8_t Cmd = 0;    /* Defined in VN200_SPI_READ - set in standard constructor */
    uint8_t ID = 0;     /* Register ID */
    uint8_t Empty2 = 0; /* Spacer */
    response_t(){};
    response_t(uint8_t ID, uint8_t Cmd) {
      this->ID = ID;
      this->Cmd = Cmd;
    };
  };
};

/* TODO */
// struct configuration_t : binary_package_t {
//   uint16_t AsyncMode;
//   uint16_t RateDivisor;
//   uint16_t OutputGroup;
//   uint8_t NumOutputFields;
//   uint16_t *OutputFields; /* Array with unknown size. Set in constructor. */
//   /* TODO */
//   configuration_t(uint8_t NumOutputFields = 1 /* TODO */) {
//     OutputFields = new uint16_t[NumOutputFields];
//     this->NumOutputFields = NumOutputFields;
//   };
//   /* TODO */
//   ~configuration_t() {
//     delete OutputFields;
//     OutputFields = nullptr;
//   }
//   /* TODO */
//   uint16_t size() override;
//   /* TODO */
//   uint8_t *to_array() override;
//   /* TODO */
//   void setBinaryGroups(std::vector<SPI::BinaryGroups::bit_offset_t> desired_groups);
//   /* TODO */
//   void setOutputFields(SPI::BinaryGroups::bit_offset_t group_bit_offset, std::vector<int> desired_output_fields);
// };

/* Writes the desired payload into the specified register */
template <typename payload_t> struct pkg_request_write_t {
  struct header_t::request_t header;
  payload_t payload;
  /* TODO */
  pkg_request_write_t(uint8_t ID) : header(ID, VN200_SPI_WRITE){};
};

/* Writes the desired payload into the specified register */
struct pkg_request_read_t {
  struct header_t::request_t header;
  /* TODO */
  pkg_request_read_t(uint8_t ID) : header(ID, VN200_SPI_READ){};
};

/* Writes the desired payload into the specified register */
template <typename payload_t> struct pkg_response_t {
  struct header_t::response_t header;
  payload_t payload;
  /* TODO */
  pkg_response_t() : header(){};
};

namespace IMU {
const uint8_t ID = 54;
struct imu_t {
  float MagX = 0.0;
  float MagY = 0.0;
  float MagZ = 0.0;
  float AccelX = 0.0;
  float AccelY = 0.0;
  float AccelZ = 0.0;
  float GyroX = 0.0;
  float GyroY = 0.0;
  float GyroZ = 0.0;
  float Pressure = 0.0;
};
} // namespace IMU
namespace ATT {
const uint8_t ID = 239;
struct attitude_t {
  float Yaw = 0.0;
  float Pitch = 0.0;
  float Roll = 0.0;
  float BodyAccelX = 0.0;
  float BodyAccelY = 0.0;
  float BodyAccelZ = 0.0;
  float GyroX = 0.0;
  float GyroY = 0.0;
  float GyroZ = 0.0;
};
} // namespace ATT
namespace INS {
namespace LLA {
const uint8_t ID = 63;
struct lla_t {
  float Yaw = 0.0;
  float Pitch = 0.0;
  float Roll = 0.0;
  double Lattitude = 0.0;
  double Longitude = 0.0;
  double Altitude = 0.0;
  float VelocityX = 0.0;
  float VelocityY = 0.0;
  float VelocityZ = 0.0;
  float AccelX = 0.0;
  float AccelY = 0.0;
  float AccelZ = 0.0;
  float AngularRateX = 0.0;
  float AngularRateY = 0.0;
  float AngularRateZ = 0.0;
};
} // namespace LLA
} // namespace INS
namespace GPS {
namespace LLA {
const uint8_t ID = 58;
struct lla_t {
  double Time = 0.0;
  uint16_t Week = 0;
  uint8_t GpsFix = 0;
  uint8_t NumSats = 0;
  uint32_t PADDING = 0;
  double Latitude = 0.0;
  double Longitude = 0.0;
  double Altitude = 0.0;
  float NedVelX = 0.0;
  float NedVelY = 0.0;
  float NedVelZ = 0.0;
  float NorthAcc = 0.0;
  float EastAcc = 0.0;
  float VertAcc = 0.0;
  float SpeedAcc = 0.0;
  float TimeAcc = 0.0;
};
} // namespace LLA
} // namespace GPS
} // namespace SPI
} // namespace VN200

#endif
