#include "ImtqDummy.h"

#include <mission/acs/imtqHelpers.h>

ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie,
                     bool enableHkSets)
    : DeviceHandlerBase(objectId, comif, comCookie),
      setNoTorque(this),
      setWithTorque(this),
      enableHkSets(enableHkSets) {}

ImtqDummy::~ImtqDummy() = default;

void ImtqDummy::doStartUp() { setMode(MODE_NORMAL); }

void ImtqDummy::doShutDown() { setMode(_MODE_POWER_DOWN); }

ReturnValue_t ImtqDummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; }

ReturnValue_t ImtqDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  return NOTHING_TO_SEND;
}

ReturnValue_t ImtqDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                 const uint8_t *commandData,
                                                 size_t commandDataLen) {
  return returnvalue::OK;
}

ReturnValue_t ImtqDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
                                      size_t *foundLen) {
  return returnvalue::OK;
}

ReturnValue_t ImtqDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
  return returnvalue::OK;
}

void ImtqDummy::fillCommandAndReplyMap() {}

uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; }

ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                 LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::MGM_CAL_NT, new PoolEntry<float>({0.0, 0.0, 0.0}));
  localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry<float>({0.12, 0.76, -0.45}, true));
  localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(imtq::DIPOLES_ID, new PoolEntry<int16_t>({0, 0, 0}));
  localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, new PoolEntry<uint16_t>({0}));

  // ENG HK No Torque
  localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(imtq::DIGITAL_CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(imtq::ANALOG_CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(imtq::COIL_CURRENTS, &coilCurrentsMilliampsNoTorque);
  localDataPoolMap.emplace(imtq::COIL_TEMPERATURES, &coilTempsNoTorque);
  localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));

  // ENG HK With Torque
  localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV_WT, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV_WT, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(imtq::DIGITAL_CURRENT_WT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(imtq::ANALOG_CURRENT_WT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(imtq::COIL_CURRENTS_WT, &coilCurrentsMilliampsWithTorque);
  localDataPoolMap.emplace(imtq::COIL_TEMPERATURES_WT, &coilTempsWithTorque);
  localDataPoolMap.emplace(imtq::MCU_TEMPERATURE_WT, new PoolEntry<int16_t>({0}));

  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(setNoTorque.getSid(), enableHkSets, 30.0));
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(setWithTorque.getSid(), enableHkSets, 30.0));
  return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
}

LocalPoolDataSetBase *ImtqDummy::getDataSetHandle(sid_t sid) {
  if (sid == setNoTorque.getSid()) {
    return &setNoTorque;
  } else if (sid == setWithTorque.getSid()) {
    return &setWithTorque;
  }
  return nullptr;
}