#include "BpxBatteryHandler.h"
#include "OBSWConfig.h"

#include <fsfw/datapool/PoolReadGuard.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 HasReturnvaluesIF::RETURN_OK;
}

void BpxBatteryHandler::fillCommandAndReplyMap() {
  insertInCommandAndReplyMap(BpxBattery::GET_HK, 1, &hkSet);
  insertInCommandAndReplyMap(BpxBattery::PING, 1);
  insertInCommandAndReplyMap(BpxBattery::REBOOT, 1);
  insertInCommandAndReplyMap(BpxBattery::RESET_COUNTERS, 1);
  insertInCommandAndReplyMap(BpxBattery::CONFIG_CMD, 1);
  insertInCommandAndReplyMap(BpxBattery::CONFIG_GET, 1, &cfgSet);
}

ReturnValue_t BpxBatteryHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                         const uint8_t* commandData,
                                                         size_t commandDataLen) {
  switch (deviceCommand) {
    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 HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t BpxBatteryHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                              DeviceCommandId_t* foundId, size_t* foundLen) {
  switch (lastCmd) {
    case (BpxBattery::GET_HK): {
      if (remainingSize != 21) {
        return DeviceHandlerIF::LENGTH_MISSMATCH;
      }
      *foundLen = 21;
      break;
    }
    case (BpxBattery::PING):
    case (BpxBattery::MAN_HEAT_ON):
    case (BpxBattery::MAN_HEAT_OFF): {
      if (remainingSize != 1) {
        return DeviceHandlerIF::LENGTH_MISSMATCH;
      }
      *foundLen = 1;
      break;
    }
    case (BpxBattery::REBOOT):
    case (BpxBattery::RESET_COUNTERS):
    case (BpxBattery::CONFIG_CMD):
    case (BpxBattery::CONFIG_SET): {
      if (remainingSize != 0) {
        return DeviceHandlerIF::LENGTH_MISSMATCH;
      }
      *foundLen = 0;
      break;
    }
    case (BpxBattery::CONFIG_GET): {
      if (remainingSize != 3) {
        return DeviceHandlerIF::LENGTH_MISSMATCH;
      }
      *foundLen = 3;
      break;
    }
    default: {
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }
  *foundId = lastCmd;
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
  switch (id) {
    case (BpxBattery::GET_HK): {
      PoolReadGuard rg(&hkSet);
      ReturnValue_t result = hkSet.parseRawHk(packet, 21);
      if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
      }
#if OBSW_DEBUG_BPX_BATT == 1
     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: " << hkSet.bootcause << std::endl;
#endif
      break;
    }
    case (BpxBattery::PING): {
      if (packet[0] != sentPingByte) {
        return DeviceHandlerIF::INVALID_DATA;
      }
      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[0] != 0x01) {
        return DeviceHandlerIF::DEVICE_DID_NOT_EXECUTE;
      }
      break;
    }
    case (BpxBattery::CONFIG_GET): {
      PoolReadGuard rg(&cfgSet);
      ReturnValue_t result = cfgSet.parseRawHk(packet, 3);
      if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
      }
      break;
    }
    default: {
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }
  return HasReturnvaluesIF::RETURN_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::HkPoolIds::BATT_TEMP_1, &battTemp1);
  localDataPoolMap.emplace(BpxBattery::HkPoolIds::BATT_TEMP_2, &battTemp2);
  localDataPoolMap.emplace(BpxBattery::HkPoolIds::BATT_TEMP_3, &battTemp3);
  localDataPoolMap.emplace(BpxBattery::HkPoolIds::BATT_TEMP_4, &battTemp4);
  localDataPoolMap.emplace(BpxBattery::HkPoolIds::CHARGE_CURRENT, &chargeCurrent);
  localDataPoolMap.emplace(BpxBattery::HkPoolIds::DISCHARGE_CURRENT, &dischargeCurrent);
  localDataPoolMap.emplace(BpxBattery::HkPoolIds::HEATER_CURRENT, &heaterCurrent);
  localDataPoolMap.emplace(BpxBattery::HkPoolIds::BATT_VOLTAGE, &battVolt);
  localDataPoolMap.emplace(BpxBattery::HkPoolIds::REBOOT_COUNTER, &rebootCounter);
  localDataPoolMap.emplace(BpxBattery::HkPoolIds::BOOTCAUSE, &bootCause);
  return HasReturnvaluesIF::RETURN_OK;
}

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