#include "ACUHandler.h"

#include "OBSWConfig.h"

ACUHandler::ACUHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie,
                       FailureIsolationBase *customFdir)
    : GomspaceDeviceHandler(objectId, comIF, comCookie, customFdir, ACU::MAX_CONFIGTABLE_ADDRESS,
                            ACU::MAX_HKTABLE_ADDRESS, ACU::HK_TABLE_REPLY_SIZE),
      coreHk(this),
      auxHk(this) {}

ACUHandler::~ACUHandler() {}

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

void ACUHandler::fillCommandAndReplyMap() { GomspaceDeviceHandler::fillCommandAndReplyMap(); }

void ACUHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) {
  parseHkTableReply(packet);
  if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
    PoolReadGuard pg0(&auxHk);
    PoolReadGuard pg1(&coreHk);
    if (pg0.getReadResult() != RETURN_OK or pg1.getReadResult() != RETURN_OK) {
      return;
    }
    for (size_t idx = 0; idx < 3; idx++) {
      float tempC = coreHk.temperatures[idx] * 0.1;
      sif::info << "ACU: Temperature " << idx << ": " << tempC << " °C" << std::endl;
    }
    sif::info << "ACU: Ground Watchdog Timer Count: " << auxHk.wdtCntGnd.value << std::endl;
    sif::info << "ACU: Ground watchdog timer, seconds left before reboot: "
              << auxHk.wdtGndLeft.value << std::endl;
#endif
  }
}

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

ReturnValue_t ACUHandler::parseHkTableReply(const uint8_t *packet) {
  uint16_t dataOffset = 0;
  PoolReadGuard pg0(&coreHk);
  PoolReadGuard pg1(&auxHk);
  auto res0 = pg0.getReadResult();
  auto res1 = pg1.getReadResult();
  if (res0 != RETURN_OK) {
    return res0;
  }
  if (res1 != RETURN_OK) {
    return res1;
  }
  dataOffset += 12;
  for (size_t idx = 0; idx < 6; idx++) {
    coreHk.currentInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1];
    dataOffset += 4;
  }
  for (size_t idx = 0; idx < 6; idx++) {
    coreHk.voltageInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1];
    dataOffset += 4;
  }

  coreHk.vcc = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  coreHk.vbat = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;

  for (size_t idx = 0; idx < 3; idx++) {
    coreHk.temperatures[idx] =
        static_cast<int16_t>((packet[dataOffset] << 8) | packet[dataOffset + 1]) * 0.1;
    dataOffset += 4;
  }

  coreHk.mpptMode = packet[dataOffset];
  dataOffset += 3;

  for (size_t idx = 0; idx < 6; idx++) {
    coreHk.vboostInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1];
    dataOffset += 4;
  }
  for (size_t idx = 0; idx < 6; idx++) {
    coreHk.powerInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1];
    dataOffset += 4;
  }
  for (size_t idx = 0; idx < 3; idx++) {
    auxHk.dacEnables[idx] = packet[dataOffset];
    dataOffset += 3;
  }
  for (size_t idx = 0; idx < 6; idx++) {
    auxHk.dacRawChannelVals[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1];
    dataOffset += 4;
  }

  auxHk.bootCause = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
                    *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  coreHk.bootcnt = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
                   *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  coreHk.uptime = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
                  *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  auxHk.resetCause = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  coreHk.mpptTime = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  /* +12 because here starts the second csp packet */
  dataOffset += 2 + 12;

  coreHk.mpptPeriod = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;

  for (size_t idx = 0; idx < 8; idx++) {
    auxHk.deviceTypes[idx] = packet[dataOffset];
    dataOffset += 3;
  }
  for (size_t idx = 0; idx < 8; idx++) {
    auxHk.devicesStatus[idx] = packet[dataOffset];
    dataOffset += 3;
  }

  auxHk.wdtCntGnd = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
                    *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  auxHk.wdtGndLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
                     *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  coreHk.setValidity(true, true);
  auxHk.setValidity(true, true);
  return RETURN_OK;
}

ReturnValue_t ACUHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                  LocalDataPoolManager &poolManager) {
  using namespace P60System;
  localDataPoolMap.emplace(pool::ACU_CURRENT_IN_CHANNELS, new PoolEntry<int16_t>(6));
  localDataPoolMap.emplace(pool::ACU_VOLTAGE_IN_CHANNELS, new PoolEntry<uint16_t>(6));

  localDataPoolMap.emplace(pool::ACU_VCC, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(pool::ACU_VBAT, new PoolEntry<uint16_t>({0}));

  localDataPoolMap.emplace(pool::ACU_TEMPERATURES, new PoolEntry<float>(3));

  localDataPoolMap.emplace(pool::ACU_MPPT_MODE, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(pool::ACU_VBOOST_IN_CHANNELS, new PoolEntry<uint16_t>(6));
  localDataPoolMap.emplace(pool::ACU_POWER_IN_CHANNELS, new PoolEntry<uint16_t>(6));

  localDataPoolMap.emplace(pool::ACU_DAC_ENABLES, new PoolEntry<uint8_t>(3));
  localDataPoolMap.emplace(pool::ACU_DAC_RAW_CHANNELS, new PoolEntry<uint16_t>(6));

  localDataPoolMap.emplace(pool::ACU_BOOTCAUSE, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::ACU_BOOTCNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::ACU_UPTIME, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::ACU_RESET_CAUSE, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(pool::ACU_MPPT_TIME, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(pool::ACU_MPPT_PERIOD, new PoolEntry<uint16_t>({0}));

  localDataPoolMap.emplace(pool::ACU_DEVICES, new PoolEntry<uint8_t>(8));
  localDataPoolMap.emplace(pool::ACU_DEVICES_STATUS, new PoolEntry<uint8_t>(8));

  localDataPoolMap.emplace(pool::ACU_WDT_CNT_GND, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::ACU_WDT_GND_LEFT, new PoolEntry<uint32_t>({0}));

  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(auxHk.getSid(), false, 30.0));
  return HasReturnvaluesIF::RETURN_OK;
}

void ACUHandler::printChannelStats() {
  PoolReadGuard pg(&coreHk);
  sif::info << "ACU Info: Current [mA], Voltage [mV]" << std::endl;
  for (size_t idx = 0; idx < 6; idx++) {
    sif::info << std::setw(8) << std::left << "Channel " << idx << std::dec << "| "
              << static_cast<unsigned int>(coreHk.currentInChannels[idx]) << std::setw(15)
              << std::right << coreHk.voltageInChannels[idx] << std::endl;
  }
}

void ACUHandler::setDebugMode(bool enable) { this->debugMode = enable; }

ReturnValue_t ACUHandler::printStatus(DeviceCommandId_t cmd) {
  ReturnValue_t result = RETURN_OK;
  switch (cmd) {
    case (GOMSPACE::PRINT_SWITCH_V_I): {
      PoolReadGuard pg(&coreHk);
      result = pg.getReadResult();
      if (result != HasReturnvaluesIF::RETURN_OK) {
        break;
      }
      printChannelStats();
      break;
    }
    default: {
      return DeviceHandlerIF::COMMAND_NOT_SUPPORTED;
    }
  }
  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::warning << "Reading PDU1 HK table failed!" << std::endl;
  }
  return result;
}