#include "P60DockHandler.h"

#include <fsfw/datapool/PoolReadGuard.h>

#include "OBSWConfig.h"

P60DockHandler::P60DockHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie)
    : GomspaceDeviceHandler(objectId, comIF, comCookie, P60Dock::MAX_CONFIGTABLE_ADDRESS,
                            P60Dock::MAX_HKTABLE_ADDRESS, P60Dock::HK_TABLE_REPLY_SIZE,
                            &p60dockHkTableDataset),
      p60dockHkTableDataset(this) {}

P60DockHandler::~P60DockHandler() {}

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

void P60DockHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) {
  parseHkTableReply(packet);
  /**
   * Hk table will be sent to the commander if hk table request was not triggered by the
   * P60DockHandler itself.
   */
  handleDeviceTM(&p60dockHkTableDataset, id, true);

#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_P60DOCK == 1
  p60dockHkTableDataset.read();
  sif::info << "P60 Dock: ACU VCC switch: "
            << static_cast<unsigned int>(p60dockHkTableDataset.outputEnableStateAcuVcc.value)
            << std::endl;
  sif::info << "P60 Dock: PDU1 VCC switch: "
            << static_cast<unsigned int>(p60dockHkTableDataset.outputEnableStatePdu1Vcc.value)
            << std::endl;
  sif::info << "P60 Dock: PDU2 VCC switch: "
            << static_cast<unsigned int>(p60dockHkTableDataset.outputEnableStatePdu2Vcc.value)
            << std::endl;
  sif::info << "P60 Dock: ACU VBAT switch: "
            << static_cast<unsigned int>(p60dockHkTableDataset.outputEnableStateAcuVbat.value)
            << std::endl;
  sif::info << "P60 Dock: PDU1 VBAT switch: "
            << static_cast<unsigned int>(p60dockHkTableDataset.outputEnableStatePdu1Vbat.value)
            << std::endl;
  sif::info << "P60 Dock: PDU2 VBAT switch: "
            << static_cast<unsigned int>(p60dockHkTableDataset.outputEnableStatePdu2Vbat.value)
            << std::endl;
  sif::info << "P60 Dock: Stack VBAT switch: "
            << static_cast<unsigned int>(p60dockHkTableDataset.outputEnableStateStackVbat.value)
            << std::endl;
  sif::info << "P60 Dock: Stack 3V3 switch: "
            << static_cast<unsigned int>(p60dockHkTableDataset.outputEnableStateStack3V3.value)
            << std::endl;
  sif::info << "P60 Dock: Stack 5V switch: "
            << static_cast<unsigned int>(p60dockHkTableDataset.outputEnableStateStack5V.value)
            << std::endl;

  float temperatureC = p60dockHkTableDataset.temperature1.value * 0.1;
  sif::info << "P60 Dock: Temperature 1: " << temperatureC << " °C" << std::endl;
  temperatureC = p60dockHkTableDataset.temperature2.value * 0.1;
  sif::info << "P60 Dock: Temperature 2: " << temperatureC << " °C" << std::endl;
  sif::info << "P60 Dock: Watchdog Timer seconds left before reboot: "
            << p60dockHkTableDataset.wdtGndLeft << " seconds" << std::endl;
  sif::info << "P60 Dock: CSP 1, pings left before reboot: "
            << (int)p60dockHkTableDataset.wdtCspLeft1.value << std::endl;
  sif::info << "P60 Dock: CSP 2, pings left before reboot: "
            << (int)p60dockHkTableDataset.wdtCspLeft1.value << std::endl;

  p60dockHkTableDataset.commit();
#endif
}

void P60DockHandler::parseHkTableReply(const uint8_t *packet) {
  using namespace P60Dock;
  uint16_t dataOffset = 0;
  PoolReadGuard pg(&p60dockHkTableDataset);
  if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
    return;
  }
  /**
   * Fist 10 bytes contain the gomspace header. Each variable is preceded by the 16-bit table
   * address.
   */
  dataOffset += 12;
  for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
    p60dockHkTableDataset.currents[idx] = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
  }
  for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
    p60dockHkTableDataset.voltages[idx] = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
  }
  for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
    p60dockHkTableDataset.outputEnables[idx] = *(packet + dataOffset);
    dataOffset += 3;
  }

  p60dockHkTableDataset.temperature1 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  p60dockHkTableDataset.temperature2 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;

  p60dockHkTableDataset.bootcause = *(packet + dataOffset) << 24 |

                                    *(packet + dataOffset + 1) << 16 |
                                    *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  p60dockHkTableDataset.bootCount = *(packet + dataOffset) << 24 |
                                    *(packet + dataOffset + 1) << 16 |
                                    *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  p60dockHkTableDataset.uptime = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
                                 *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  p60dockHkTableDataset.resetcause = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  p60dockHkTableDataset.battMode = *(packet + dataOffset);
  dataOffset += 3;
  p60dockHkTableDataset.heaterOn = *(packet + dataOffset);
  /* + 13 because here begins a new gomspace csp data field */
  dataOffset += 13;
  p60dockHkTableDataset.converter5VStatus = *(packet + dataOffset);
  dataOffset += 3;

  for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
    p60dockHkTableDataset.latchups[idx] = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
  }

  p60dockHkTableDataset.dockVbatVoltageValue =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  p60dockHkTableDataset.dockVccCurrent = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  p60dockHkTableDataset.batteryCurrent = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  p60dockHkTableDataset.batteryVoltage = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;

  p60dockHkTableDataset.batteryTemperature1 =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  p60dockHkTableDataset.batteryTemperature2 =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;

  for (uint8_t idx = 0; idx < NUM_DEVS; idx++) {
    p60dockHkTableDataset.devicesType[idx] = *(packet + dataOffset);
    dataOffset += 3;
  }
  for (uint8_t idx = 0; idx < NUM_DEVS; idx++) {
    p60dockHkTableDataset.devicesStatus[idx] = *(packet + dataOffset);
    dataOffset += 3;
  }

  p60dockHkTableDataset.dearmStatus = *(packet + dataOffset);
  dataOffset += 3;

  p60dockHkTableDataset.wdtCntGnd = *(packet + dataOffset) << 24 |
                                    *(packet + dataOffset + 1) << 16 |
                                    *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  p60dockHkTableDataset.wdtCntI2c = *(packet + dataOffset) << 24 |
                                    *(packet + dataOffset + 1) << 16 |
                                    *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  p60dockHkTableDataset.wdtCntCan = *(packet + dataOffset) << 24 |
                                    *(packet + dataOffset + 1) << 16 |
                                    *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  p60dockHkTableDataset.wdtCntCsp1 = *(packet + dataOffset) << 24 |
                                     *(packet + dataOffset + 1) << 16 |
                                     *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  p60dockHkTableDataset.wdtCntCsp2 = *(packet + dataOffset) << 24 |
                                     *(packet + dataOffset + 1) << 16 |
                                     *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;

  p60dockHkTableDataset.wdtGndLeft = *(packet + dataOffset) << 24 |
                                     *(packet + dataOffset + 1) << 16 |
                                     *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  p60dockHkTableDataset.wdtI2cLeft = *(packet + dataOffset) << 24 |
                                     *(packet + dataOffset + 1) << 16 |
                                     *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  p60dockHkTableDataset.wdtCanLeft = *(packet + dataOffset) << 24 |
                                     *(packet + dataOffset + 1) << 16 |
                                     *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  /* +16 because here begins a new gomspace csp packet */
  dataOffset += 16;

  p60dockHkTableDataset.wdtCspLeft1 = *(packet + dataOffset);
  dataOffset += 3;
  p60dockHkTableDataset.wdtCspLeft2 = *(packet + dataOffset);
  dataOffset += 3;

  p60dockHkTableDataset.batteryChargeCurrent =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  p60dockHkTableDataset.batteryDischargeCurrent =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  p60dockHkTableDataset.ant6Depl = *(packet + dataOffset);
  dataOffset += 3;
  p60dockHkTableDataset.ar6Depl = *(packet + dataOffset);
  p60dockHkTableDataset.setValidity(true, true);
}

ReturnValue_t P60DockHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {
  using namespace P60System;
  localDataPoolMap.emplace(pool::CURRENTS, &hkCurrents);

  localDataPoolMap.emplace(pool::VOLTAGES, &hkVoltages);

  localDataPoolMap.emplace(pool::OUTPUT_ENABLE, &outputEnables);

  localDataPoolMap.emplace(pool::P60DOCK_TEMPERATURE_1, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_TEMPERATURE_2, new PoolEntry<int16_t>({0}));

  localDataPoolMap.emplace(pool::P60DOCK_BOOT_CAUSE, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_BOOT_CNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_UPTIME, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_RESETCAUSE, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_BATT_MODE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_HEATER_ON, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_CONV_5V_ENABLE_STATUS, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(pool::LATCHUPS, &latchups);

  localDataPoolMap.emplace(pool::P60DOCK_DOCK_VBAT, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_DOCK_VCC_CURRENT, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_BATTERY_CURRENT, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_BATTERY_VOLTAGE, new PoolEntry<uint16_t>({0}));

  localDataPoolMap.emplace(pool::P60DOCK_BATTERY_TEMPERATURE_1, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_BATTERY_TEMPERATURE_2, new PoolEntry<int16_t>({0}));

  localDataPoolMap.emplace(pool::DEVICES_TYPE, &devicesType);
  localDataPoolMap.emplace(pool::DEVICES_STATUS, &devicesStatus);

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

  localDataPoolMap.emplace(pool::P60DOCK_WDT_CNT_GND, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_WDT_CNT_I2C, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_WDT_CNT_CAN, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_WDT_CNT_CSP_1, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_WDT_CNT_CSP_2, new PoolEntry<uint32_t>({0}));

  localDataPoolMap.emplace(pool::P60DOCK_WDT_GND_LEFT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_WDT_I2C_LEFT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_WDT_CAN_LEFT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_WDT_CSP_LEFT_1, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_WDT_CSP_LEFT_2, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(pool::P60DOCK_BATT_CHARGE_CURRENT, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_BATT_DISCHARGE_CURRENT, new PoolEntry<int16_t>({0}));

  localDataPoolMap.emplace(pool::P60DOCK_ANT6_DEPL, new PoolEntry<int8_t>({0}));
  localDataPoolMap.emplace(pool::P60DOCK_AR6_DEPL, new PoolEntry<int8_t>({0}));
  poolManager.subscribeForPeriodicPacket(p60dockHkTableDataset.getSid(), false, 0.4, true);
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t P60DockHandler::printStatus(DeviceCommandId_t cmd) {
  ReturnValue_t result = RETURN_OK;
  switch (cmd) {
    case (GOMSPACE::PRINT_SWITCH_V_I): {
      PoolReadGuard pg(&p60dockHkTableDataset);
      result = pg.getReadResult();
      if (result != HasReturnvaluesIF::RETURN_OK) {
        break;
      }
      printHkTableSwitchIV();
      return HasReturnvaluesIF::RETURN_OK;
    }
    case (GOMSPACE::PRINT_LATCHUPS): {
      PoolReadGuard pg(&p60dockHkTableDataset);
      result = pg.getReadResult();
      printHkTableLatchups();
      if (result != HasReturnvaluesIF::RETURN_OK) {
        break;
      }
      return HasReturnvaluesIF::RETURN_OK;
    }
    default: {
      return HasReturnvaluesIF::RETURN_FAILED;
    }
  }
  sif::warning << "Reading P60 Dock HK table failed" << std::endl;
  return HasReturnvaluesIF::RETURN_FAILED;
}

void P60DockHandler::printHkTableSwitchIV() {
  using namespace P60Dock;
  sif::info << "P60 Dock Info:" << std::endl;
  sif::info << "Boot Cause: " << p60dockHkTableDataset.bootcause
            << " | Boot Count: " << std::setw(4) << std::right << p60dockHkTableDataset.bootCount
            << std::endl;
  sif::info << "Reset Cause: " << p60dockHkTableDataset.resetcause
            << " | Battery Mode: " << static_cast<int>(p60dockHkTableDataset.battMode.value)
            << std::endl;
  sif::info << "SwitchState, Currents [mA], Voltages [mV]:" << std::endl;
  sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "Dock VBAT VCC" << std::dec
            << "| -, " << std::setw(4) << std::right << p60dockHkTableDataset.dockVccCurrent << ", "
            << std::setw(5) << p60dockHkTableDataset.dockVbatVoltageValue << std::endl;
  sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "BATT" << std::dec << "| -, "
            << std::setw(4) << std::right << p60dockHkTableDataset.batteryCurrent.value << ", "
            << std::setw(5) << p60dockHkTableDataset.batteryVoltage.value << std::endl;

  auto genericPrintoutHandler = [&](std::string name, uint8_t idx) {
    sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << name << std::dec << "| "
              << unsigned(p60dockHkTableDataset.outputEnables[idx]) << ", " << std::setw(4)
              << std::right << p60dockHkTableDataset.currents[idx] << ", " << std::setw(5)
              << p60dockHkTableDataset.voltages[idx] << std::endl;
  };

  genericPrintoutHandler("ACU VCC", hk::ACU_VCC);
  genericPrintoutHandler("ACU VBAT", hk::ACU_VBAT);
  genericPrintoutHandler("PDU1 VCC", hk::PDU1_VCC);
  genericPrintoutHandler("PDU1 VBAT", hk::PDU1_VBAT);
  genericPrintoutHandler("PDU2 VCC", hk::PDU2_VCC);
  genericPrintoutHandler("PDU2 VBAT", hk::PDU2_VBAT);
  genericPrintoutHandler("Stack VBAT", hk::STACK_VBAT);
  genericPrintoutHandler("Stack 3V3", hk::STACK_3V3);
  genericPrintoutHandler("Stack 5V", hk::STACK_5V);
}

void P60DockHandler::printHkTableLatchups() {
  using namespace P60Dock;
  sif::info << "P60 Latchup Information" << std::endl;
  auto genericPrintoutHandler = [&](std::string name, uint8_t idx) {
    sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << name << std::dec << "| "
              << std::setw(4) << std::right << p60dockHkTableDataset.latchups[idx] << std::endl;
  };
  genericPrintoutHandler("ACU VCC", hk::ACU_VCC);
  genericPrintoutHandler("ACU VBAT", hk::ACU_VBAT);
  genericPrintoutHandler("PDU1 VCC", hk::PDU1_VCC);
  genericPrintoutHandler("PDU1 VBAT", hk::PDU1_VBAT);
  genericPrintoutHandler("PDU2 VCC", hk::PDU2_VCC);
  genericPrintoutHandler("PDU2 VBAT", hk::PDU2_VBAT);
  genericPrintoutHandler("STACK VBAT", hk::STACK_VBAT);
  genericPrintoutHandler("STACK 3V3", hk::STACK_3V3);
  genericPrintoutHandler("STACK 5V", hk::STACK_5V);
  genericPrintoutHandler("GS 3V3", hk::GS3V3);
  genericPrintoutHandler("GS 5V", hk::GS5V);
  genericPrintoutHandler("X3 VBAT", hk::X3_IDLE_VBAT);
  genericPrintoutHandler("X3 VCC", hk::X3_IDLE_VCC);
}