#include <OBSWConfig.h>
#include <devices/powerSwitcherList.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/housekeeping/HousekeepingSnapshot.h>
#include <fsfw/ipc/MutexFactory.h>
#include <fsfw/ipc/QueueFactory.h>
#include <mission/devices/PcduHandler.h>
#include <mission/devices/devicedefinitions/GomSpacePackets.h>

PCDUHandler::PCDUHandler(object_id_t setObjectId, size_t cmdQueueSize)
    : SystemObject(setObjectId),
      poolManager(this, nullptr),
      p60CoreHk(objects::P60DOCK_HANDLER),
      pdu1CoreHk(this),
      pdu2CoreHk(this),
      switcherSet(this),
      cmdQueueSize(cmdQueueSize) {
  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
  commandQueue = QueueFactory::instance()->createMessageQueue(
      cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
  pwrLock = MutexFactory::instance()->createMutex();
}

PCDUHandler::~PCDUHandler() {}

ReturnValue_t PCDUHandler::performOperation(uint8_t counter) {
  if (counter == DeviceHandlerIF::PERFORM_OPERATION) {
    readCommandQueue();
  }
  uint8_t switchState = 0;
  {
    PoolReadGuard pg(&p60CoreHk.outputEnables);
    if (pg.getReadResult() == returnvalue::OK) {
      switchState = p60CoreHk.outputEnables.value[10];
    } else {
      return returnvalue::OK;
    }
  }
  {
    PoolReadGuard pg(&switcherSet.p60Dock5VStack);
    if (pg.getReadResult() == returnvalue::OK) {
      if (switcherSet.p60Dock5VStack.value != switchState) {
        triggerEvent(power::SWITCH_HAS_CHANGED, switchState, pcdu::Switches::P60_DOCK_5V_STACK);
        MutexGuard mg(pwrLock);
        switchStates[pcdu::P60_DOCK_5V_STACK] = switchState;
      }
      switcherSet.p60Dock5VStack.setValid(true);
      switcherSet.p60Dock5VStack.value = switchState;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t PCDUHandler::initialize() {
  ReturnValue_t result;

  IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
  if (IPCStore == nullptr) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  result = poolManager.initialize(commandQueue);
  if (result != returnvalue::OK) {
    return result;
  }

  /* Subscribing for housekeeping table update messages of the PDU2 */
  HasLocalDataPoolIF* pdu2Handler =
      ObjectManager::instance()->get<HasLocalDataPoolIF>(objects::PDU2_HANDLER);
  if (pdu2Handler == nullptr) {
    sif::error << "PCDUHandler::initialize: Invalid pdu2Handler" << std::endl;
    return returnvalue::FAILED;
  }
  result = pdu2Handler->getSubscriptionInterface()->subscribeForSetUpdateMessage(
      static_cast<uint32_t>(P60System::SetIds::CORE), this->getObjectId(), commandQueue->getId(),
      true);
  if (result != returnvalue::OK) {
    sif::error << "PCDUHandler::initialize: Failed to subscribe for set update messages from "
               << "PDU2Handler" << std::endl;
    return result;
  }

  /* Subscribing for housekeeping table update messages of the PDU1 */
  HasLocalDataPoolIF* pdu1Handler =
      ObjectManager::instance()->get<HasLocalDataPoolIF>(objects::PDU1_HANDLER);
  if (pdu1Handler == nullptr) {
    sif::error << "PCDUHandler::initialize: Invalid pdu1Handler" << std::endl;
    return returnvalue::FAILED;
  }
  result = pdu1Handler->getSubscriptionInterface()->subscribeForSetUpdateMessage(
      static_cast<uint32_t>(P60System::SetIds::CORE), this->getObjectId(), commandQueue->getId(),
      true);
  if (result != returnvalue::OK) {
    sif::error << "PCDUHandler::initialize: Failed to subscribe for set update messages from "
               << "PDU1Handler" << std::endl;
    return result;
  }

  return returnvalue::OK;
}

void PCDUHandler::initializeSwitchStates() {
  using namespace pcdu;
  try {
    for (uint8_t idx = 0; idx < NUMBER_OF_SWITCHES; idx++) {
      if (idx < PDU::CHANNELS_LEN) {
        switchStates[idx] = INIT_SWITCHES_PDU1.at(idx);
      } else if (idx < PDU::CHANNELS_LEN * 2) {
        switchStates[idx] = INIT_SWITCHES_PDU2.at(idx - PDU::CHANNELS_LEN);
      } else {
        switchStates[idx] = OFF;
      }
    }
  } catch (const std::out_of_range& err) {
    sif::error << "PCDUHandler::initializeSwitchStates: " << err.what() << std::endl;
  }
}

void PCDUHandler::readCommandQueue() {
  ReturnValue_t result = returnvalue::OK;
  CommandMessage command;

  for (result = commandQueue->receiveMessage(&command); result == returnvalue::OK;
       result = commandQueue->receiveMessage(&command)) {
    result = poolManager.handleHousekeepingMessage(&command);
    if (result == returnvalue::OK) {
      continue;
    }
  }
}

MessageQueueId_t PCDUHandler::getCommandQueue() const { return commandQueue->getId(); }

void PCDUHandler::handleChangedDataset(sid_t sid, store_address_t storeId, bool* clearMessage) {
  if (sid == sid_t(objects::PDU2_HANDLER, static_cast<uint32_t>(P60System::SetIds::CORE))) {
    updateHkTableDataset(storeId, &pdu2CoreHk, &timeStampPdu2HkDataset);
    updatePdu2SwitchStates();
  } else if (sid == sid_t(objects::PDU1_HANDLER, static_cast<uint32_t>(P60System::SetIds::CORE))) {
    updateHkTableDataset(storeId, &pdu1CoreHk, &timeStampPdu1HkDataset);
    updatePdu1SwitchStates();
  } else {
    sif::error << "PCDUHandler::handleChangedDataset: Invalid sid" << std::endl;
  }
}

void PCDUHandler::updateHkTableDataset(store_address_t storeId, LocalPoolDataSetBase* dataset,
                                       CCSDSTime::CDS_short* datasetTimeStamp) {
  ReturnValue_t result;

  HousekeepingSnapshot packetUpdate(reinterpret_cast<uint8_t*>(datasetTimeStamp),
                                    sizeof(CCSDSTime::CDS_short), dataset);
  const uint8_t* packet_ptr = nullptr;
  size_t size = 0;
  result = IPCStore->getData(storeId, &packet_ptr, &size);
  if (result != returnvalue::OK) {
    sif::error << "PCDUHandler::updateHkTableDataset: Failed to get data from IPCStore."
               << std::endl;
  }
  result = packetUpdate.deSerialize(&packet_ptr, &size, SerializeIF::Endianness::MACHINE);
  if (result != returnvalue::OK) {
    sif::error << "PCDUHandler::updateHkTableDataset: Failed to deserialize received packet "
                  "in hk table dataset"
               << std::endl;
  }
  result = IPCStore->deleteData(storeId);
  if (result != returnvalue::OK) {
    sif::error << "PCDUHandler::updateHkTableDataset: Failed to delete data in IPCStore"
               << std::endl;
  }
}

void PCDUHandler::updatePdu2SwitchStates() {
  using namespace pcdu;
  using namespace PDU2;
  GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU2;
  PoolReadGuard rg0(&switcherSet);
  if (rg0.getReadResult() == returnvalue::OK) {
    for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) {
      switcherSet.pdu2Switches[idx] = pdu2CoreHk.outputEnables[idx];
    }
    switcherSet.pdu2Switches.setValid(true);
    MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH0_Q7S, pdu2CoreHk.outputEnables[Channels::Q7S]);

    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8,
                            pdu2CoreHk.outputEnables[Channels::PAYLOAD_PCDU_CH1]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH2_RW_5V, pdu2CoreHk.outputEnables[Channels::RW]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V,
                            pdu2CoreHk.outputEnables[Channels::TCS_HEATER_IN]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH4_SUS_REDUNDANT_3V3,
                            pdu2CoreHk.outputEnables[Channels::SUS_REDUNDANT]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH5_DEPLOYMENT_MECHANISM_8V,
                            pdu2CoreHk.outputEnables[Channels::DEPY_MECHANISM]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH6_PL_PCDU_BATT_1_14V8,
                            pdu2CoreHk.outputEnables[Channels::PAYLOAD_PCDU_CH6]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3,
                            pdu2CoreHk.outputEnables[Channels::ACS_B_SIDE]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH8_PAYLOAD_CAMERA,
                            pdu2CoreHk.outputEnables[Channels::PAYLOAD_CAMERA]);
    if (firstSwitchInfoPdu2) {
      firstSwitchInfoPdu2 = false;
    }
  } else {
    sif::debug << "PCDUHandler::updatePdu2SwitchStates: Failed to read PDU2 Hk Dataset"
               << std::endl;
  }
}

void PCDUHandler::updatePdu1SwitchStates() {
  using namespace pcdu;
  using namespace PDU1;
  PoolReadGuard rg0(&switcherSet);
  GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU1;
  if (rg0.getReadResult() == returnvalue::OK) {
    for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) {
      switcherSet.pdu1Switches[idx] = pdu1CoreHk.outputEnables[idx];
    }
    switcherSet.pdu1Switches.setValid(true);
    MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH0_TCS_BOARD_3V3,
                            pdu1CoreHk.outputEnables[Channels::TCS_BOARD_3V3]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH1_SYRLINKS_12V,
                            pdu1CoreHk.outputEnables[Channels::SYRLINKS]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH2_STAR_TRACKER_5V,
                            pdu1CoreHk.outputEnables[Channels::STR]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH3_MGT_5V,
                            pdu1CoreHk.outputEnables[Channels::MGT]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH4_SUS_NOMINAL_3V3,
                            pdu1CoreHk.outputEnables[Channels::SUS_NOMINAL]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH5_SOLAR_CELL_EXP_5V,
                            pdu1CoreHk.outputEnables[Channels::SOL_CELL_EXPERIMENT]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH6_PLOC_12V,
                            pdu1CoreHk.outputEnables[Channels::PLOC]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH7_ACS_A_SIDE_3V3,
                            pdu1CoreHk.outputEnables[Channels::ACS_A_SIDE]);
    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH8_UNOCCUPIED,
                            pdu1CoreHk.outputEnables[Channels::UNUSED]);
    if (firstSwitchInfoPdu1) {
      firstSwitchInfoPdu1 = false;
    }
  } else {
    sif::debug << "PCDUHandler::updatePdu1SwitchStates: Failed to read dataset" << std::endl;
  }
}

LocalDataPoolManager* PCDUHandler::getHkManagerHandle() { return &poolManager; }

ReturnValue_t PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) {
  using namespace pcdu;
  ReturnValue_t result;
  uint16_t memoryAddress = 0;
  size_t parameterValueSize = sizeof(uint8_t);
  uint8_t parameterValue = 0;
  GomspaceDeviceHandler* module = nullptr;

  switch (switchNr) {
    case pcdu::PDU1_CH0_TCS_BOARD_3V3: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_TCS_BOARD_3V3;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH1_SYRLINKS_12V: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_SYRLINKS;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH2_STAR_TRACKER_5V: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_STAR_TRACKER;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH3_MGT_5V: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_MGT;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH4_SUS_NOMINAL_3V3: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_SUS_NOMINAL;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH5_SOLAR_CELL_EXP_5V: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_SOLAR_CELL_EXP;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH6_PLOC_12V: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_PLOC;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH7_ACS_A_SIDE_3V3: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_ACS_BOARD_SIDE_A;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH8_UNOCCUPIED: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_CHANNEL8;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    // This is a dangerous command. Reject/Igore it for now
    case pcdu::PDU2_CH0_Q7S: {
      return returnvalue::FAILED;
      // memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_Q7S;
      // pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      // break;
    }
    case pcdu::PDU2_CH1_PL_PCDU_BATT_0_14V8: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_PAYLOAD_PCDU_CH1;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH2_RW_5V: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_RW;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH3_TCS_BOARD_HEATER_IN_8V: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_TCS_BOARD_HEATER_IN;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH4_SUS_REDUNDANT_3V3: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_SUS_REDUNDANT;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH5_DEPLOYMENT_MECHANISM_8V: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_DEPLOYMENT_MECHANISM;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH6_PL_PCDU_BATT_1_14V8: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_PAYLOAD_PCDU_CH6;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH7_ACS_BOARD_SIDE_B_3V3: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_ACS_BOARD_SIDE_B;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH8_PAYLOAD_CAMERA: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_PAYLOAD_CAMERA;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::P60_DOCK_5V_STACK: {
      memoryAddress = P60Dock::CONFIG_ADDRESS_OUT_EN_5V_STACK;
      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::P60DOCK_HANDLER);
      break;
    }

    default: {
      sif::error << "PCDUHandler::sendSwitchCommand: Invalid switch number " << std::endl;
      return returnvalue::FAILED;
    }
  }

  switch (onOff) {
    case PowerSwitchIF::SWITCH_ON:
      parameterValue = 1;
      break;
    case PowerSwitchIF::SWITCH_OFF:
      parameterValue = 0;
      break;
    default:
      sif::error << "PCDUHandler::sendSwitchCommand: Invalid state commanded" << std::endl;
      return returnvalue::FAILED;
  }

  GomspaceSetParamMessage setParamMessage(memoryAddress, &parameterValue, parameterValueSize);

  size_t serializedLength = 0;
  uint8_t command[4];
  uint8_t* commandPtr = command;
  size_t maxSize = sizeof(command);
  setParamMessage.serialize(&commandPtr, &serializedLength, maxSize, SerializeIF::Endianness::BIG);

  store_address_t storeAddress;
  result = IPCStore->addData(&storeAddress, command, sizeof(command));

  CommandMessage message;
  ActionMessage::setCommand(&message, GOMSPACE::PARAM_SET, storeAddress);

  result = commandQueue->sendMessage(module->getCommandQueue(), &message, 0);
  if (result != returnvalue::OK) {
    sif::debug << "PCDUHandler::sendSwitchCommand: Failed to send message to PDU Handler"
               << std::endl;
  } else {
    // Can't use trigger event because of const function constraint, but this hack seems to work
    this->forwardEvent(power::SWITCH_CMD_SENT, parameterValue, switchNr);
  }
  return result;
}

ReturnValue_t PCDUHandler::sendFuseOnCommand(uint8_t fuseNr) { return returnvalue::OK; }

ReturnValue_t PCDUHandler::getSwitchState(uint8_t switchNr) const {
  if (switchNr >= pcdu::NUMBER_OF_SWITCHES) {
    sif::debug << "PCDUHandler::getSwitchState: Invalid switch number" << std::endl;
    return returnvalue::FAILED;
  }
  uint8_t currentState = 0;
  {
    MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
      currentState = switchStates[switchNr];
  }
  if (currentState == 1) {
    return PowerSwitchIF::SWITCH_ON;
  } else {
    return PowerSwitchIF::SWITCH_OFF;
  }
}

ReturnValue_t PCDUHandler::getFuseState(uint8_t fuseNr) const { return returnvalue::OK; }

uint32_t PCDUHandler::getSwitchDelayMs(void) const { return 20000; }

object_id_t PCDUHandler::getObjectId() const { return SystemObject::getObjectId(); }

ReturnValue_t PCDUHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                   LocalDataPoolManager& poolManager) {
  using namespace pcdu;
  localDataPoolMap.emplace(PoolIds::PDU1_SWITCHES, &pdu1Switches);
  localDataPoolMap.emplace(PoolIds::PDU2_SWITCHES, &pdu2Switches);
  localDataPoolMap.emplace(PoolIds::P60DOCK_SWITCHES, &p60Dock5VSwitch);
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(switcherSet.getSid(), false, 5.0));
  return returnvalue::OK;
}

ReturnValue_t PCDUHandler::initializeAfterTaskCreation() {
  if (executingTask != nullptr) {
    pstIntervalMs = executingTask->getPeriodMs();
  }
  this->poolManager.initializeAfterTaskCreation();

  initializeSwitchStates();

  return returnvalue::OK;
}

uint32_t PCDUHandler::getPeriodicOperationFrequency() const { return pstIntervalMs; }

void PCDUHandler::setTaskIF(PeriodicTaskIF* task) { executingTask = task; }

LocalPoolDataSetBase* PCDUHandler::getDataSetHandle(sid_t sid) {
  if (sid == switcherSet.getSid()) {
    return &switcherSet;
  } else {
    sif::error << "PCDUHandler::getDataSetHandle: Invalid sid" << std::endl;
    return nullptr;
  }
}

void PCDUHandler::checkAndUpdatePduSwitch(GOMSPACE::Pdu pdu, pcdu::Switches switchIdx,
                                          uint8_t setValue) {
  using namespace pcdu;
  if (switchStates[switchIdx] != setValue) {
    triggerEvent(power::SWITCH_HAS_CHANGED, setValue, switchIdx);
  }
  switchStates[switchIdx] = setValue;
}