#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_
#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_

#include <fsfw/datapoollocal/StaticLocalDataSet.h>
#include <fsfw/datapoollocal/localPoolDefinitions.h>

#include <cstdint>

namespace acsctrl {

enum SetIds : uint32_t {
  MGM_SENSOR_DATA,
  MGM_PROCESSED_DATA,
  SUS_SENSOR_DATA,
  SUS_PROCESSED_DATA,
  GYR_SENSOR_DATA,
  GYR_PROCESSED_DATA,
  GPS_PROCESSED_DATA,
  MEKF_DATA,
  CTRL_VAL_DATA,
  ACTUATOR_CMD_DATA
};

enum PoolIds : lp_id_t {
  // MGM Raw
  MGM_0_LIS3_UT,
  MGM_1_RM3100_UT,
  MGM_2_LIS3_UT,
  MGM_3_RM3100_UT,
  MGM_IMTQ_CAL_NT,
  MGM_IMTQ_CAL_ACT_STATUS,
  // MGM Processed
  MGM_0_VEC,
  MGM_1_VEC,
  MGM_2_VEC,
  MGM_3_VEC,
  MGM_4_VEC,
  MGM_VEC_TOT,
  MGM_VEC_TOT_DERIVATIVE,
  MAG_IGRF_MODEL,
  // SUS Raw
  SUS_0_N_LOC_XFYFZM_PT_XF,
  SUS_6_R_LOC_XFYBZM_PT_XF,

  SUS_1_N_LOC_XBYFZM_PT_XB,
  SUS_7_R_LOC_XBYBZM_PT_XB,

  SUS_2_N_LOC_XFYBZB_PT_YB,
  SUS_8_R_LOC_XBYBZB_PT_YB,

  SUS_3_N_LOC_XFYBZF_PT_YF,
  SUS_9_R_LOC_XBYBZB_PT_YF,

  SUS_4_N_LOC_XMYFZF_PT_ZF,
  SUS_10_N_LOC_XMYBZF_PT_ZF,

  SUS_5_N_LOC_XFYMZB_PT_ZB,
  SUS_11_R_LOC_XBYMZB_PT_ZB,
  // SUS Processed
  SUS_0_VEC,
  SUS_1_VEC,
  SUS_2_VEC,
  SUS_3_VEC,
  SUS_4_VEC,
  SUS_5_VEC,
  SUS_6_VEC,
  SUS_7_VEC,
  SUS_8_VEC,
  SUS_9_VEC,
  SUS_10_VEC,
  SUS_11_VEC,
  SUS_VEC_TOT,
  SUS_VEC_TOT_DERIVATIVE,
  SUN_IJK_MODEL,
  // GYR Raw
  GYR_0_ADIS,
  GYR_1_L3,
  GYR_2_ADIS,
  GYR_3_L3,
  // GYR Processed
  GYR_0_VEC,
  GYR_1_VEC,
  GYR_2_VEC,
  GYR_3_VEC,
  GYR_VEC_TOT,
  // GPS Processed
  GC_LATITUDE,
  GD_LONGITUDE,
  GPS_VELOCITY,
  // MEKF
  SAT_ROT_RATE_MEKF,
  QUAT_MEKF,
  // Ctrl Values
  TGT_QUAT,
  ERROR_QUAT,
  ERROR_ANG,
  // Actuator Cmd
  RW_TARGET_TORQUE,
  RW_TARGET_SPEED,
  MTQ_TARGET_DIPOLE,
};

static constexpr uint8_t MGM_SET_RAW_ENTRIES = 10;
static constexpr uint8_t MGM_SET_PROCESSED_ENTRIES = 8;
static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12;
static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 3;
static constexpr uint8_t MEKF_SET_ENTRIES = 2;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;

/**
 * @brief   Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
 */
class MgmDataRaw : public StaticLocalDataSet<MGM_SET_RAW_ENTRIES> {
 public:
  MgmDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MGM_SENSOR_DATA) {}

  // The ACS board measurement are in floating point uT
  lp_vec_t<float, 3> mgm0Lis3 = lp_vec_t<float, 3>(sid.objectId, MGM_0_LIS3_UT, this);
  lp_vec_t<float, 3> mgm1Rm3100 = lp_vec_t<float, 3>(sid.objectId, MGM_1_RM3100_UT, this);
  lp_vec_t<float, 3> mgm2Lis3 = lp_vec_t<float, 3>(sid.objectId, MGM_2_LIS3_UT, this);
  lp_vec_t<float, 3> mgm3Rm3100 = lp_vec_t<float, 3>(sid.objectId, MGM_3_RM3100_UT, this);
  // The IMTQ measurements are in integer nT
  lp_vec_t<float, 3> imtqRaw = lp_vec_t<float, 3>(sid.objectId, MGM_IMTQ_CAL_NT, this);
  lp_var_t<uint8_t> actuationCalStatus =
      lp_var_t<uint8_t>(sid.objectId, MGM_IMTQ_CAL_ACT_STATUS, this);

 private:
};

class MgmDataProcessed : public StaticLocalDataSet<MGM_SET_PROCESSED_ENTRIES> {
 public:
  MgmDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MGM_PROCESSED_DATA) {}

  lp_vec_t<float, 3> mgm0vec = lp_vec_t<float, 3>(sid.objectId, MGM_0_VEC, this);
  lp_vec_t<float, 3> mgm1vec = lp_vec_t<float, 3>(sid.objectId, MGM_1_VEC, this);
  lp_vec_t<float, 3> mgm2vec = lp_vec_t<float, 3>(sid.objectId, MGM_2_VEC, this);
  lp_vec_t<float, 3> mgm3vec = lp_vec_t<float, 3>(sid.objectId, MGM_3_VEC, this);
  lp_vec_t<float, 3> mgm4vec = lp_vec_t<float, 3>(sid.objectId, MGM_4_VEC, this);
  lp_vec_t<double, 3> mgmVecTot = lp_vec_t<double, 3>(sid.objectId, MGM_VEC_TOT, this);
  lp_vec_t<double, 3> mgmVecTotDerivative =
      lp_vec_t<double, 3>(sid.objectId, MGM_VEC_TOT_DERIVATIVE, this);
  lp_vec_t<double, 3> magIgrfModel = lp_vec_t<double, 3>(sid.objectId, MAG_IGRF_MODEL, this);

 private:
};

class SusDataRaw : public StaticLocalDataSet<SUS_SET_RAW_ENTRIES> {
 public:
  SusDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_SENSOR_DATA) {}

  lp_vec_t<uint16_t, 6> sus0 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_0_N_LOC_XFYFZM_PT_XF, this);
  lp_vec_t<uint16_t, 6> sus1 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_1_N_LOC_XBYFZM_PT_XB, this);
  lp_vec_t<uint16_t, 6> sus2 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_2_N_LOC_XFYBZB_PT_YB, this);
  lp_vec_t<uint16_t, 6> sus3 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_3_N_LOC_XFYBZF_PT_YF, this);
  lp_vec_t<uint16_t, 6> sus4 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_4_N_LOC_XMYFZF_PT_ZF, this);
  lp_vec_t<uint16_t, 6> sus5 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_5_N_LOC_XFYMZB_PT_ZB, this);
  lp_vec_t<uint16_t, 6> sus6 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_6_R_LOC_XFYBZM_PT_XF, this);
  lp_vec_t<uint16_t, 6> sus7 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_7_R_LOC_XBYBZM_PT_XB, this);
  lp_vec_t<uint16_t, 6> sus8 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_8_R_LOC_XBYBZB_PT_YB, this);
  lp_vec_t<uint16_t, 6> sus9 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_9_R_LOC_XBYBZB_PT_YF, this);
  lp_vec_t<uint16_t, 6> sus10 =
      lp_vec_t<uint16_t, 6>(sid.objectId, SUS_10_N_LOC_XMYBZF_PT_ZF, this);
  lp_vec_t<uint16_t, 6> sus11 =
      lp_vec_t<uint16_t, 6>(sid.objectId, SUS_11_R_LOC_XBYMZB_PT_ZB, this);

 private:
};

class SusDataProcessed : public StaticLocalDataSet<SUS_SET_PROCESSED_ENTRIES> {
 public:
  SusDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_PROCESSED_DATA) {}

  lp_vec_t<float, 3> sus0vec = lp_vec_t<float, 3>(sid.objectId, SUS_0_VEC, this);
  lp_vec_t<float, 3> sus1vec = lp_vec_t<float, 3>(sid.objectId, SUS_1_VEC, this);
  lp_vec_t<float, 3> sus2vec = lp_vec_t<float, 3>(sid.objectId, SUS_2_VEC, this);
  lp_vec_t<float, 3> sus3vec = lp_vec_t<float, 3>(sid.objectId, SUS_3_VEC, this);
  lp_vec_t<float, 3> sus4vec = lp_vec_t<float, 3>(sid.objectId, SUS_4_VEC, this);
  lp_vec_t<float, 3> sus5vec = lp_vec_t<float, 3>(sid.objectId, SUS_5_VEC, this);
  lp_vec_t<float, 3> sus6vec = lp_vec_t<float, 3>(sid.objectId, SUS_6_VEC, this);
  lp_vec_t<float, 3> sus7vec = lp_vec_t<float, 3>(sid.objectId, SUS_7_VEC, this);
  lp_vec_t<float, 3> sus8vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
  lp_vec_t<float, 3> sus9vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
  lp_vec_t<float, 3> sus10vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
  lp_vec_t<float, 3> sus11vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
  lp_vec_t<double, 3> susVecTot = lp_vec_t<double, 3>(sid.objectId, SUS_VEC_TOT, this);
  lp_vec_t<double, 3> susVecTotDerivative =
      lp_vec_t<double, 3>(sid.objectId, SUS_VEC_TOT_DERIVATIVE, this);
  lp_vec_t<double, 3> sunIjkModel = lp_vec_t<double, 3>(sid.objectId, SUN_IJK_MODEL, this);

 private:
};

class GyrDataRaw : public StaticLocalDataSet<GYR_SET_RAW_ENTRIES> {
 public:
  GyrDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GYR_SENSOR_DATA) {}

  lp_vec_t<double, 3> gyr0Adis = lp_vec_t<double, 3>(sid.objectId, GYR_0_ADIS, this);
  lp_vec_t<float, 3> gyr1L3 = lp_vec_t<float, 3>(sid.objectId, GYR_1_L3, this);
  lp_vec_t<double, 3> gyr2Adis = lp_vec_t<double, 3>(sid.objectId, GYR_2_ADIS, this);
  lp_vec_t<float, 3> gyr3L3 = lp_vec_t<float, 3>(sid.objectId, GYR_3_L3, this);

 private:
};

class GyrDataProcessed : public StaticLocalDataSet<GYR_SET_PROCESSED_ENTRIES> {
 public:
  GyrDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GYR_PROCESSED_DATA) {}

  lp_vec_t<double, 3> gyr0vec = lp_vec_t<double, 3>(sid.objectId, GYR_0_VEC, this);
  lp_vec_t<double, 3> gyr1vec = lp_vec_t<double, 3>(sid.objectId, GYR_1_VEC, this);
  lp_vec_t<double, 3> gyr2vec = lp_vec_t<double, 3>(sid.objectId, GYR_2_VEC, this);
  lp_vec_t<double, 3> gyr3vec = lp_vec_t<double, 3>(sid.objectId, GYR_3_VEC, this);
  lp_vec_t<double, 3> gyrVecTot = lp_vec_t<double, 3>(sid.objectId, GYR_VEC_TOT, this);

 private:
};

class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
 public:
  GpsDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GPS_PROCESSED_DATA) {}

  lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this);
  lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this);
  lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);

 private:
};

class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
 public:
  MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}

  lp_vec_t<double, 4> quatMekf = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
  lp_vec_t<double, 3> satRotRateMekf = lp_vec_t<double, 3>(sid.objectId, SAT_ROT_RATE_MEKF, this);

 private:
};

class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
 public:
  CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {}

  lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
  lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
  lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);

 private:
};

class ActuatorCmdData : public StaticLocalDataSet<ACT_CMD_SET_ENTRIES> {
 public:
  ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {}

  lp_vec_t<double, 4> rwTargetTorque = lp_vec_t<double, 4>(sid.objectId, RW_TARGET_TORQUE, this);
  lp_vec_t<int32_t, 4> rwTargetSpeed = lp_vec_t<int32_t, 4>(sid.objectId, RW_TARGET_SPEED, this);
  lp_vec_t<int16_t, 3> mtqTargetDipole =
      lp_vec_t<int16_t, 3>(sid.objectId, MTQ_TARGET_DIPOLE, this);

 private:
};

}  // namespace acsctrl

#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */