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

BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie)
    : DeviceHandlerBase(objectId, comIF, comCookie), hkSet(this), cfgSet(this) {}

BpxBatteryHandler::~BpxBatteryHandler() {}

void BpxBatteryHandler::doStartUp() {
  if (state == States::CHECK_COM) {
    if (commandExecuted) {
      state = States::IDLE;
      commandExecuted = false;
      if (goToNormalModeImmediately) {
        setMode(MODE_NORMAL);
      } else {
        setMode(_MODE_TO_ON);
      }
    }
  }
}

void BpxBatteryHandler::doShutDown() {
  // Perform a COM check on reboot
  state = States::CHECK_COM;
}

ReturnValue_t BpxBatteryHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  *id = BpxBattery::GET_HK;
  return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t BpxBatteryHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  if (state == States::CHECK_COM) {
    *id = BpxBattery::PING;
    return buildCommandFromCommand(*id, nullptr, 0);
  }
  return returnvalue::OK;
}

void BpxBatteryHandler::fillCommandAndReplyMap() {
  using namespace BpxBattery;
  insertInCommandAndReplyMap(GET_HK, 1, &hkSet, GET_HK_REPLY_LEN);
  insertInCommandAndReplyMap(BpxBattery::PING, 1, nullptr, PING_REPLY_LEN);
  insertInCommandAndReplyMap(BpxBattery::REBOOT, 1, nullptr, 0);
  insertInCommandAndReplyMap(BpxBattery::RESET_COUNTERS, 1, nullptr, EMPTY_REPLY_LEN);
  insertInCommandAndReplyMap(BpxBattery::CONFIG_CMD, 1, nullptr, EMPTY_REPLY_LEN);
  insertInCommandAndReplyMap(BpxBattery::CONFIG_GET, 1, &cfgSet, CONFIG_GET_REPLY_LEN);
}

ReturnValue_t BpxBatteryHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                         const uint8_t* commandData,
                                                         size_t commandDataLen) {
  switch (deviceCommand) {
    case (BpxBattery::GET_HK): {
      cmdBuf[0] = BpxBattery::PORT_GET_HK;
      this->rawPacketLen = 1;
      break;
    }
    case (BpxBattery::PING): {
      if (commandDataLen == 1 and commandData != nullptr) {
        sentPingByte = commandData[0];
      } else {
        sentPingByte = BpxBattery::DEFAULT_PING_SENT_BYTE;
      }

      cmdBuf[0] = BpxBattery::PORT_PING;
      cmdBuf[1] = sentPingByte;
      this->rawPacketLen = 2;
      break;
    }
    case (BpxBattery::REBOOT): {
      cmdBuf[0] = BpxBattery::PORT_REBOOT;
      cmdBuf[1] = 0x80;
      cmdBuf[2] = 0x07;
      cmdBuf[3] = 0x80;
      cmdBuf[4] = 0x07;
      this->rawPacketLen = 5;
      // This instructs the FDIR to set the device mode off and on again
      // to ensure the I2C communication is also verified
      triggerEvent(DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT);
      break;
    }
    case (BpxBattery::RESET_COUNTERS): {
      cmdBuf[0] = BpxBattery::PORT_RESET_COUNTERS;
      cmdBuf[1] = BpxBattery::RESET_COUNTERS_MAGIC_VALUE;
      this->rawPacketLen = 2;
      break;
    }
    case (BpxBattery::CONFIG_CMD): {
      cmdBuf[0] = BpxBattery::PORT_CONFIG_CMD;
      // Needs to be set to 0x01 according to datasheet
      cmdBuf[1] = 0x01;
      this->rawPacketLen = 2;
      break;
    }
    case (BpxBattery::CONFIG_GET): {
      cmdBuf[0] = BpxBattery::PORT_CONFIG_GET;
      this->rawPacketLen = 1;
      break;
    }
    case (BpxBattery::CONFIG_SET): {
      cmdBuf[0] = BpxBattery::PORT_CONFIG_SET;
      if (commandDataLen != 3) {
        return DeviceHandlerIF::INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
      }
      for (uint8_t idx = 0; idx < 3; idx++) {
        cmdBuf[idx + 1] = commandData[idx];
      }
      this->rawPacketLen = 4;
      break;
    }
    case (BpxBattery::MAN_HEAT_ON): {
      cmdBuf[0] = BpxBattery::PORT_MAN_HEAT_ON;
      if (commandDataLen != 2) {
        return DeviceHandlerIF::INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
      }
      for (uint8_t idx = 0; idx < 2; idx++) {
        cmdBuf[idx + 1] = commandData[idx];
      }
      this->rawPacketLen = 3;
      break;
    }
    case (BpxBattery::MAN_HEAT_OFF): {
      cmdBuf[0] = BpxBattery::PORT_MAN_HEAT_OFF;
      this->rawPacketLen = 1;
      break;
    }
    default: {
      return DeviceHandlerIF::COMMAND_NOT_SUPPORTED;
    }
  }
  this->rawPacket = cmdBuf.data();

  lastCmd = deviceCommand;
  return returnvalue::OK;
}

ReturnValue_t BpxBatteryHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                              DeviceCommandId_t* foundId, size_t* foundLen) {
  using namespace BpxBattery;
  switch (lastCmd) {
    case (BpxBattery::GET_HK): {
      if (remainingSize != GET_HK_REPLY_LEN) {
        return DeviceHandlerIF::LENGTH_MISSMATCH;
      }
      break;
    }
    case (BpxBattery::PING):
    case (BpxBattery::MAN_HEAT_ON):
    case (BpxBattery::MAN_HEAT_OFF): {
      if (remainingSize != PING_REPLY_LEN) {
        return DeviceHandlerIF::LENGTH_MISSMATCH;
      }
      break;
    }
    case (BpxBattery::REBOOT): {
      // Ignore
      break;
    }
    case (BpxBattery::RESET_COUNTERS):
    case (BpxBattery::CONFIG_CMD):
    case (BpxBattery::CONFIG_SET): {
      if (remainingSize != EMPTY_REPLY_LEN) {
        return DeviceHandlerIF::LENGTH_MISSMATCH;
      }
      break;
    }
    case (BpxBattery::CONFIG_GET): {
      if (remainingSize != CONFIG_GET_REPLY_LEN) {
        return DeviceHandlerIF::LENGTH_MISSMATCH;
      }

      break;
    }
    default: {
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }
  *foundLen = remainingSize;
  *foundId = lastCmd;
  return returnvalue::OK;
}

ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
  if (id != BpxBattery::REBOOT and packet[1] != 0) {
    return DeviceHandlerIF::DEVICE_REPORTED_ERROR;
  }
  switch (id) {
    case (BpxBattery::GET_HK): {
      PoolReadGuard rg(&hkSet);
      ReturnValue_t result = hkSet.parseRawHk(packet + 2, 21);
      hkSet.setValidity(true, true);
      if (result != returnvalue::OK) {
        return result;
      }
      if (debugMode) {
        sif::info << "BPX Battery HK output:" << std::endl;
        sif::info << "Charge current [mA]: " << hkSet.chargeCurrent << std::endl;
        sif::info << "Discharge current [mA]: " << hkSet.dischargeCurrent << std::endl;
        sif::info << "Heater current [mA]: " << hkSet.heaterCurrent << std::endl;
        sif::info << "Battery voltage [mV]: " << hkSet.battVoltage << std::endl;
        sif::info << "Battery Temperature 1 [C]: " << hkSet.battTemp1 << std::endl;
        sif::info << "Battery Temperature 2 [C]: " << hkSet.battTemp2 << std::endl;
        sif::info << "Battery Temperature 3 [C]: " << hkSet.battTemp3 << std::endl;
        sif::info << "Battery Temperature 4 [C]: " << hkSet.battTemp4 << std::endl;
        sif::info << "Battery Reboot Counter: " << hkSet.rebootCounter << std::endl;
        sif::info << "Battery Boot Cause: " << static_cast<int>(hkSet.bootcause.value) << std::endl;
      }
      break;
    }
    case (BpxBattery::PING): {
      if (packet[2] != sentPingByte) {
        return DeviceHandlerIF::INVALID_DATA;
      }
      if (getMode() == _MODE_START_UP) {
        commandExecuted = true;
      }
      break;
    }
    case (BpxBattery::RESET_COUNTERS):
    case (BpxBattery::CONFIG_CMD):
    case (BpxBattery::CONFIG_SET): {
      break;
    }
    case (BpxBattery::MAN_HEAT_ON):
    case (BpxBattery::MAN_HEAT_OFF): {
      if (packet[2] != 0x01) {
        return DeviceHandlerIF::DEVICE_DID_NOT_EXECUTE;
      }
      break;
    }
    case (BpxBattery::CONFIG_GET): {
      PoolReadGuard rg(&cfgSet);
      ReturnValue_t result = cfgSet.parseRawHk(packet + 2, 3);
      if (result != returnvalue::OK) {
        return result;
      }
      cfgSet.setValidity(true, true);
      break;
    }
    case (BpxBattery::REBOOT): {
      break;
    }
    default: {
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }
  return returnvalue::OK;
}

uint32_t BpxBatteryHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 10000; }

ReturnValue_t BpxBatteryHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                         LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(BpxBattery::BATT_TEMP_1, &battTemp1);
  localDataPoolMap.emplace(BpxBattery::BATT_TEMP_2, &battTemp2);
  localDataPoolMap.emplace(BpxBattery::BATT_TEMP_3, &battTemp3);
  localDataPoolMap.emplace(BpxBattery::BATT_TEMP_4, &battTemp4);
  localDataPoolMap.emplace(BpxBattery::CHARGE_CURRENT, &chargeCurrent);
  localDataPoolMap.emplace(BpxBattery::DISCHARGE_CURRENT, &dischargeCurrent);
  localDataPoolMap.emplace(BpxBattery::HEATER_CURRENT, &heaterCurrent);
  localDataPoolMap.emplace(BpxBattery::BATT_VOLTAGE, &battVolt);
  localDataPoolMap.emplace(BpxBattery::REBOOT_COUNTER, &rebootCounter);
  localDataPoolMap.emplace(BpxBattery::BOOTCAUSE, &bootCause);

  localDataPoolMap.emplace(BpxBattery::BATTERY_HEATER_MODE, &battheatMode);
  localDataPoolMap.emplace(BpxBattery::BATTHEAT_LOW_LIMIT, &battheatLow);
  localDataPoolMap.emplace(BpxBattery::BATTHEAT_HIGH_LIMIT, &battheatHigh);
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(hkSet.getSid(), false, 30.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(cfgSet.getSid(), false, 30.0));
  return returnvalue::OK;
}

void BpxBatteryHandler::setToGoToNormalMode(bool enable) {
  this->goToNormalModeImmediately = enable;
}

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

void BpxBatteryHandler::performOperationHook() {
#if OBSW_THREAD_TRACING == 1
  trace::threadTrace(opCounter, "BPX BATT");
#endif
}