#ifndef MISSION_DEVICES_IMTQHANDLER_H_
#define MISSION_DEVICES_IMTQHANDLER_H_

#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <string.h>

#include "events/subsystemIdRanges.h"
#include "returnvalues/classIds.h"

/**
 * @brief	This is the device handler for the ISIS Magnetorquer iMTQ.
 *
 * @author	J. Meier
 */
class ImtqHandler : public DeviceHandlerBase {
 public:
  enum NormalPollingMode { UNCALIBRATED = 0, CALIBRATED = 1, BOTH = 2 };

  ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
              power::Switch_t pwrSwitcher);
  virtual ~ImtqHandler();

  void setPollingMode(NormalPollingMode pollMode);

  /**
   * @brief   Sets mode to MODE_NORMAL. Can be used for debugging.
   */
  void setToGoToNormal(bool enable);

  void setDebugMode(bool enable);

 protected:
  ReturnValue_t performOperation(uint8_t opCode);
  void doStartUp() override;
  void doShutDown() override;
  ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override;
  ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override;
  void fillCommandAndReplyMap() override;
  ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData,
                                        size_t commandDataLen) override;
  ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId,
                             size_t* foundLen) override;
  ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) override;
  virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
  uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
  ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                        LocalDataPoolManager& poolManager) override;
  ReturnValue_t getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) override;

 private:
  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::IMTQ_HANDLER;

  //! [EXPORT] : [COMMENT] Get self test result returns I2C failure
  //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 ->
  //! -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA
  static const Event SELF_TEST_I2C_FAILURE = MAKE_EVENT(1, severity::LOW);
  //! [EXPORT] : [COMMENT] Get self test result returns SPI failure. This concerns the MTM
  //! connectivity. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3
  //! -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA
  static const Event SELF_TEST_SPI_FAILURE = MAKE_EVENT(2, severity::LOW);
  //! [EXPORT] : [COMMENT] Get self test result returns failure in measurement of current and
  //! temperature. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3
  //! -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA
  static const Event SELF_TEST_ADC_FAILURE = MAKE_EVENT(3, severity::LOW);
  //! [EXPORT] : [COMMENT] Get self test result returns PWM failure which concerns the coil
  //! actuation. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 ->
  //! +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA
  static const Event SELF_TEST_PWM_FAILURE = MAKE_EVENT(4, severity::LOW);
  //! [EXPORT] : [COMMENT] Get self test result returns TC failure (system failure)
  //! P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 ->
  //! -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA
  static const Event SELF_TEST_TC_FAILURE = MAKE_EVENT(5, severity::LOW);
  //! [EXPORT] : [COMMENT] Get self test result returns failure that MTM values were outside of the
  //! expected range. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X,
  //! 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA
  static const Event SELF_TEST_MTM_RANGE_FAILURE = MAKE_EVENT(6, severity::LOW);
  //! [EXPORT] : [COMMENT] Get self test result returns failure indicating that the coil current was
  //! outside of the expected range P1: Indicates on which axis the failure occurred. 0 -> INIT, 1
  //! -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA
  static const Event SELF_TEST_COIL_CURRENT_FAILURE = MAKE_EVENT(7, severity::LOW);
  //! [EXPORT] : [COMMENT] Received invalid error byte. This indicates an error of the communication
  //! link between IMTQ and OBC.
  static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW);

  imtq::StatusDataset statusSet;
  imtq::DipoleActuationSet dipoleSet;
  imtq::RawMtmMeasurementNoTorque rawMtmNoTorque;
  imtq::HkDatasetNoTorque hkDatasetNoTorque;

  imtq::RawMtmMeasurementWithTorque rawMtmWithTorque;
  imtq::HkDatasetWithTorque hkDatasetWithTorque;

  imtq::CalibratedMtmMeasurementSet calMtmMeasurementSet;
  imtq::PosXSelfTestSet posXselfTestDataset;
  imtq::NegXSelfTestSet negXselfTestDataset;
  imtq::PosYSelfTestSet posYselfTestDataset;
  imtq::NegYSelfTestSet negYselfTestDataset;
  imtq::PosZSelfTestSet posZselfTestDataset;
  imtq::NegZSelfTestSet negZselfTestDataset;
  bool manualTorqueCmdActive = false;
  bool ignoreActForRestOfComSteps = false;
  Countdown manuallyCommandedTorqueDuration = Countdown();

  NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;

  PoolEntry<uint8_t> statusMode = PoolEntry<uint8_t>({0});
  PoolEntry<uint8_t> statusError = PoolEntry<uint8_t>({0});
  PoolEntry<uint8_t> statusConfig = PoolEntry<uint8_t>({0});
  PoolEntry<uint32_t> statusUptime = PoolEntry<uint32_t>({0});

  PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
  PoolEntry<int16_t> dipolesPoolEntry = PoolEntry<int16_t>({0, 0, 0}, false);
  PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>({0}, false);
  PoolEntry<float> coilCurrentsMilliampsNoTorque = PoolEntry<float>(3);
  PoolEntry<float> coilCurrentsMilliampsWithTorque = PoolEntry<float>(3);
  PoolEntry<int16_t> coilTempsNoTorque = PoolEntry<int16_t>(3);
  PoolEntry<int16_t> coilTempsWithTorque = PoolEntry<int16_t>(3);
  PoolEntry<float> mtmRawNoTorque = PoolEntry<float>(3);
  PoolEntry<uint8_t> actStatusNoTorque = PoolEntry<uint8_t>(1);
  PoolEntry<float> mtmRawWithTorque = PoolEntry<float>(3);
  PoolEntry<uint8_t> actStatusWithTorque = PoolEntry<uint8_t>(1);

  power::Switch_t switcher = power::NO_SWITCH;

  ImtqRequest request{};
  DeviceCommandId_t expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
  bool goToNormalMode = false;
  bool debugMode = false;
  bool specialRequestActive = false;
  bool firstReplyCycle = true;

  imtq::RequestType requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;

  /**
   * @brief   In case of a status reply to a single axis self test command, this function
   *          searches for the actual pending command.
   */
  ReturnValue_t getSelfTestCommandId(DeviceCommandId_t* id);

  /**
   * @brief   Each reply contains a status byte giving information about a request. This function
   *          parses this byte and returns the associated failure message.
   *
   * @param packet    Pointer to the received message containing the status byte.
   *
   * @return  The return code derived from the received status byte.
   */
  ReturnValue_t parseStatusByte(imtq::CC::CC command, const uint8_t* packet);

  /**
   * @brief   This function fills the engineering housekeeping dataset with the received data.

   * @param packet    Pointer to the received data.
   *
   */
  void fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet);

  void fillSystemStateIntoDataset(const uint8_t* packet);

  /**
   * @brief   This function parses the reply containing the calibrated MTM measurement and writes
   *          the values to the appropriate dataset.
   * @param packet    Pointer to the reply data.
   */
  void fillCalibratedMtmDataset(const uint8_t* packet);

  /**
   * @brief   This function copies the raw MTM measurements to the MTM raw dataset.
   * @param packet    Pointer to the reply data requested with the GET_RAW_MTM_MEASUREMENTS
   *                  command.
   */
  void fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet);

  /**
   * @brief   This function handles all self test results. This comprises parsing the error byte
   *          and step byte and calling the function to fill the respective dataset.
   */
  void handleSelfTestReply(const uint8_t* packet);

  /**
   * @brief   The following functions fill the respective dataset of the single axis self tests.
   * @param packet    Pointer to the reply data holding the self test result.
   */
  void handlePositiveXSelfTestReply(const uint8_t* packet);
  void handleNegativeXSelfTestReply(const uint8_t* packet);
  void handlePositiveYSelfTestReply(const uint8_t* packet);
  void handleNegativeYSelfTestReply(const uint8_t* packet);
  void handlePositiveZSelfTestReply(const uint8_t* packet);
  void handleNegativeZSelfTestReply(const uint8_t* packet);

  // ReturnValue_t buildDipoleActuationCommand();
  /**
   * @brief   This function checks the error byte of a self test measurement.
   *
   * @param errorByte The received error byte to check
   * @param step  The step of the error byte.
   */
  void checkErrorByte(const uint8_t errorByte, const uint8_t step);

  std::string makeStepString(const uint8_t step);
};

#endif /* MISSION_DEVICES_IMTQHANDLER_H_ */