#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/power/GomSpacePackets.h>
#include <mission/power/Pdu2Handler.h>

#include "devices/powerSwitcherList.h"

Pdu2Handler::Pdu2Handler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie,
                         FailureIsolationBase *customFdir)
    : GomspaceDeviceHandler(objectId, comIF, comCookie, cfg, customFdir),
      coreHk(this),
      auxHk(this) {
  initPduConfigTable();
}

Pdu2Handler::~Pdu2Handler() {}

ReturnValue_t Pdu2Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  *id = GOMSPACE::REQUEST_HK_TABLE;
  return buildCommandFromCommand(*id, NULL, 0);
}

void Pdu2Handler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) {
  parseHkTableReply(packet);
}

void Pdu2Handler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) {
  handleDeviceTm(packet, PDU::CONFIG_TABLE_SIZE, id);
}

void Pdu2Handler::assignChannelHookFunction(GOMSPACE::ChannelSwitchHook hook, void *args) {
  this->channelSwitchHook = hook;
  this->hookArgs = args;
}

LocalPoolDataSetBase *Pdu2Handler::getDataSetHandle(sid_t sid) {
  if (sid == coreHk.getSid()) {
    return &coreHk;
  } else if (sid == auxHk.getSid()) {
    return &auxHk;
  }
  return nullptr;
}

void Pdu2Handler::parseHkTableReply(const uint8_t *packet) {
  GomspaceDeviceHandler::parsePduHkTable(coreHk, auxHk, packet);
}

ReturnValue_t Pdu2Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                   LocalDataPoolManager &poolManager) {
  initializePduPool(localDataPoolMap, poolManager, pcdu::INIT_SWITCHES_PDU2);
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(auxHk.getSid(), false, 30.0));
  return returnvalue::OK;
}

ReturnValue_t Pdu2Handler::printStatus(DeviceCommandId_t cmd) {
  ReturnValue_t result = returnvalue::OK;
  switch (cmd) {
    case (GOMSPACE::PRINT_SWITCH_V_I): {
      PoolReadGuard pg(&coreHk);
      result = pg.getReadResult();
      if (result != returnvalue::OK) {
        break;
      }
      printHkTableSwitchVI();
      break;
    }
    case (GOMSPACE::PRINT_LATCHUPS): {
      PoolReadGuard pg(&auxHk);
      result = pg.getReadResult();
      if (result != returnvalue::OK) {
        break;
      }
      printHkTableLatchups();
      break;
    }
    default: {
      return DeviceHandlerIF::COMMAND_NOT_SUPPORTED;
    }
  }
  if (result != returnvalue::OK) {
    sif::warning << "Reading PDU1 HK table failed!" << std::endl;
  }
  return result;
}

void Pdu2Handler::printHkTableSwitchVI() {
  using namespace PDU2;
  sif::info << "PDU2 Info:" << std::endl;
  sif::info << "Boot Cause: " << auxHk.bootcause << " | Boot Count: " << std::setw(4) << std::right
            << coreHk.bootcount << std::endl;
  sif::info << "Reset Cause: " << auxHk.resetcause
            << " | Battery Mode: " << static_cast<int>(coreHk.battMode.value) << std::endl;
  sif::info << "SwitchState, Currents [mA], Voltages [mV]: " << std::endl;
  auto printerHelper = [&](std::string channelStr, Channels idx) {
    sif::info << std::setw(30) << std::left << channelStr << std::dec << "| "
              << unsigned(coreHk.outputEnables[idx]) << ", " << std::setw(4) << std::right
              << coreHk.currents[idx] << ", " << std::setw(4) << coreHk.voltages[idx] << std::endl;
  };
  printerHelper("Q7S", Channels::Q7S);
  printerHelper("PL PCDU CH1", Channels::PAYLOAD_PCDU_CH1);
  printerHelper("Reaction Wheels", Channels::RW);
  printerHelper("TCS Board Heater Input", Channels::TCS_HEATER_IN);
  printerHelper("SUS Redundant", Channels::SUS_REDUNDANT);
  printerHelper("Deployment Mechanism", Channels::DEPY_MECHANISM);
  printerHelper("PL PCDU CH6", Channels::PAYLOAD_PCDU_CH6);
  printerHelper("ACS Board B Side", Channels::ACS_B_SIDE);
  printerHelper("Payload Camera", Channels::PAYLOAD_CAMERA);
}

void Pdu2Handler::printHkTableLatchups() {
  using namespace PDU2;
  sif::info << "PDU2 Latchup Information" << std::endl;
  auto printerHelper = [&](std::string channelStr, Channels idx) {
    sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "TCS Board" << std::dec << "| "
              << std::setw(4) << std::right << auxHk.latchups[idx] << std::endl;
  };
  printerHelper("Q7S", Channels::Q7S);
  printerHelper("PL PCDU CH1", Channels::PAYLOAD_PCDU_CH1);
  printerHelper("Reaction Wheels", Channels::RW);
  printerHelper("TCS Board Heater Input", Channels::TCS_HEATER_IN);
  printerHelper("SUS Redundant", Channels::SUS_REDUNDANT);
  printerHelper("Deployment Mechanism", Channels::DEPY_MECHANISM);
  printerHelper("PL PCDU CH6", Channels::PAYLOAD_PCDU_CH6);
  printerHelper("ACS Board B Side", Channels::ACS_B_SIDE);
  printerHelper("Payload Camera", Channels::PAYLOAD_CAMERA);
}

ReturnValue_t Pdu2Handler::setParamCallback(SetParamMessageUnpacker &unpacker,
                                            bool afterExecution) {
  using namespace PDU2;
  GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU2;
  if (not afterExecution) {
    return returnvalue::OK;
  }
  if (channelSwitchHook != nullptr and unpacker.getParameterSize() == 1) {
    switch (unpacker.getAddress()) {
      case (CONFIG_ADDRESS_OUT_EN_Q7S): {
        channelSwitchHook(pdu, 0, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_PAYLOAD_PCDU_CH1): {
        channelSwitchHook(pdu, 1, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_RW): {
        channelSwitchHook(pdu, 2, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_TCS_BOARD_HEATER_IN): {
        channelSwitchHook(pdu, 3, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_SUS_REDUNDANT): {
        channelSwitchHook(pdu, 4, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_DEPLOYMENT_MECHANISM): {
        channelSwitchHook(pdu, 5, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_PAYLOAD_PCDU_CH6): {
        channelSwitchHook(pdu, 6, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_ACS_BOARD_SIDE_B): {
        channelSwitchHook(pdu, 7, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_PAYLOAD_CAMERA): {
        channelSwitchHook(pdu, 8, unpacker.getParameter()[0], hookArgs);
        break;
      }
    }
  }
  return returnvalue::OK;
}