#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/CRC.h>
#include <mission/devices/SyrlinksHandler.h>

#include "OBSWConfig.h"

SyrlinksHandler::SyrlinksHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
                                 power::Switch_t powerSwitch, FailureIsolationBase* customFdir)
    : DeviceHandlerBase(objectId, comIF, comCookie, customFdir),
      rxDataset(this),
      txDataset(this),
      temperatureSet(this),
      powerSwitch(powerSwitch) {
  if (comCookie == nullptr) {
    sif::warning << "SyrlinksHkHandler: Invalid com cookie" << std::endl;
  }
}

SyrlinksHandler::~SyrlinksHandler() = default;

void SyrlinksHandler::doStartUp() {
  switch (internalState) {
    case InternalState::OFF: {
      internalState = InternalState::ENABLE_TEMPERATURE_PROTECTION;
      break;
    }
    case InternalState::IDLE: {
      setMode(_MODE_TO_ON);
      commandExecuted = false;
      break;
    }
    default:
      break;
  }
}

void SyrlinksHandler::doShutDown() {
  setMode(_MODE_POWER_DOWN);
  commandExecuted = false;
  internalState = InternalState::OFF;
  temperatureSet.setValidity(false, true);
}

ReturnValue_t SyrlinksHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  switch (nextCommand) {
    case (syrlinks::READ_RX_STATUS_REGISTERS):
      *id = syrlinks::READ_RX_STATUS_REGISTERS;
      nextCommand = syrlinks::READ_TX_STATUS;
      break;
    case (syrlinks::READ_TX_STATUS):
      *id = syrlinks::READ_TX_STATUS;
      nextCommand = syrlinks::READ_TX_WAVEFORM;
      break;
    case (syrlinks::READ_TX_WAVEFORM):
      *id = syrlinks::READ_TX_WAVEFORM;
      nextCommand = syrlinks::READ_TX_AGC_VALUE_HIGH_BYTE;
      break;
    case (syrlinks::READ_TX_AGC_VALUE_HIGH_BYTE):
      *id = syrlinks::READ_TX_AGC_VALUE_HIGH_BYTE;
      nextCommand = syrlinks::READ_TX_AGC_VALUE_LOW_BYTE;
      break;
    case (syrlinks::READ_TX_AGC_VALUE_LOW_BYTE):
      *id = syrlinks::READ_TX_AGC_VALUE_LOW_BYTE;
      nextCommand = syrlinks::TEMP_POWER_AMPLIFIER_HIGH_BYTE;
      break;
    case (syrlinks::TEMP_POWER_AMPLIFIER_HIGH_BYTE):
      *id = syrlinks::TEMP_POWER_AMPLIFIER_HIGH_BYTE;
      nextCommand = syrlinks::TEMP_POWER_AMPLIFIER_LOW_BYTE;
      break;
    case (syrlinks::TEMP_POWER_AMPLIFIER_LOW_BYTE):
      *id = syrlinks::TEMP_POWER_AMPLIFIER_LOW_BYTE;
      nextCommand = syrlinks::TEMP_BASEBAND_BOARD_HIGH_BYTE;
      break;
    case (syrlinks::TEMP_BASEBAND_BOARD_HIGH_BYTE):
      *id = syrlinks::TEMP_BASEBAND_BOARD_HIGH_BYTE;
      nextCommand = syrlinks::TEMP_BASEBAND_BOARD_LOW_BYTE;
      break;
    case (syrlinks::TEMP_BASEBAND_BOARD_LOW_BYTE):
      *id = syrlinks::TEMP_BASEBAND_BOARD_LOW_BYTE;
      nextCommand = syrlinks::READ_RX_STATUS_REGISTERS;
      break;
    default:
      sif::debug << "SyrlinksHkHandler::buildNormalDeviceCommand: rememberCommandId has invalid"
                 << "command id" << std::endl;
      break;
  }
  return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t SyrlinksHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  switch (internalState) {
    case InternalState::ENABLE_TEMPERATURE_PROTECTION: {
      *id = syrlinks::WRITE_LCL_CONFIG;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case InternalState::SET_TX_MODULATION: {
      *id = syrlinks::SET_TX_MODE_MODULATION;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case InternalState::SET_TX_CW: {
      *id = syrlinks::SET_TX_MODE_CW;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case InternalState::SET_TX_STANDBY: {
      *id = syrlinks::SET_TX_MODE_STANDBY;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    default:
      break;
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t SyrlinksHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                       const uint8_t* commandData,
                                                       size_t commandDataLen) {
  switch (deviceCommand) {
    case (syrlinks::RESET_UNIT): {
      prepareCommand(resetCommand, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::SET_TX_MODE_STANDBY): {
      prepareCommand(setTxModeStandby, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::SET_TX_MODE_MODULATION): {
      prepareCommand(setTxModeModulation, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::SET_TX_MODE_CW): {
      prepareCommand(setTxModeCw, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::WRITE_LCL_CONFIG): {
      prepareCommand(writeLclConfig, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::READ_RX_STATUS_REGISTERS): {
      prepareCommand(readRxStatusRegCommand, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::READ_LCL_CONFIG): {
      prepareCommand(readLclConfig, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::READ_TX_STATUS): {
      prepareCommand(readTxStatus, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::READ_TX_WAVEFORM): {
      prepareCommand(readTxWaveform, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::READ_TX_AGC_VALUE_HIGH_BYTE): {
      prepareCommand(readTxAgcValueHighByte, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::READ_TX_AGC_VALUE_LOW_BYTE): {
      prepareCommand(readTxAgcValueLowByte, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::TEMP_POWER_AMPLIFIER_HIGH_BYTE): {
      prepareCommand(tempPowerAmpBoardHighByte, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::TEMP_POWER_AMPLIFIER_LOW_BYTE): {
      prepareCommand(tempPowerAmpBoardLowByte, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::TEMP_BASEBAND_BOARD_HIGH_BYTE): {
      prepareCommand(tempBasebandBoardHighByte, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::TEMP_BASEBAND_BOARD_LOW_BYTE): {
      prepareCommand(tempBasebandBoardLowByte, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::CONFIG_BPSK): {
      prepareCommand(configBPSK, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::CONFIG_OQPSK): {
      prepareCommand(configOQPSK, deviceCommand);
      return returnvalue::OK;
    }
    case (syrlinks::ENABLE_DEBUG): {
      debugMode = true;
      rawPacketLen = 0;
      return returnvalue::OK;
    }
    case (syrlinks::DISABLE_DEBUG): {
      debugMode = false;
      rawPacketLen = 0;
      return returnvalue::OK;
    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return returnvalue::FAILED;
}

void SyrlinksHandler::fillCommandAndReplyMap() {
  this->insertInCommandAndReplyMap(syrlinks::RESET_UNIT, 1, nullptr, syrlinks::ACK_SIZE, false,
                                   true, syrlinks::ACK_REPLY);
  this->insertInCommandAndReplyMap(syrlinks::SET_TX_MODE_STANDBY, 1, nullptr, syrlinks::ACK_SIZE,
                                   false, true, syrlinks::ACK_REPLY);
  this->insertInCommandAndReplyMap(syrlinks::SET_TX_MODE_MODULATION, 1, nullptr, syrlinks::ACK_SIZE,
                                   false, true, syrlinks::ACK_REPLY);
  this->insertInCommandAndReplyMap(syrlinks::SET_TX_MODE_CW, 1, nullptr, syrlinks::ACK_SIZE, false,
                                   true, syrlinks::ACK_REPLY);
  this->insertInCommandAndReplyMap(syrlinks::WRITE_LCL_CONFIG, 1, nullptr, syrlinks::ACK_SIZE,
                                   false, true, syrlinks::ACK_REPLY);
  this->insertInCommandAndReplyMap(syrlinks::CONFIG_BPSK, 1, nullptr, syrlinks::ACK_SIZE, false,
                                   true, syrlinks::ACK_REPLY);
  this->insertInCommandAndReplyMap(syrlinks::CONFIG_OQPSK, 1, nullptr, syrlinks::ACK_SIZE, false,
                                   true, syrlinks::ACK_REPLY);
  this->insertInCommandMap(syrlinks::ENABLE_DEBUG);
  this->insertInCommandMap(syrlinks::DISABLE_DEBUG);
  this->insertInCommandAndReplyMap(syrlinks::READ_LCL_CONFIG, 1, nullptr,
                                   syrlinks::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(syrlinks::READ_TX_STATUS, 1, &txDataset,
                                   syrlinks::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(syrlinks::READ_TX_WAVEFORM, 1, &txDataset,
                                   syrlinks::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(syrlinks::READ_TX_AGC_VALUE_HIGH_BYTE, 1, &txDataset,
                                   syrlinks::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(syrlinks::READ_TX_AGC_VALUE_LOW_BYTE, 1, &txDataset,
                                   syrlinks::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(syrlinks::TEMP_POWER_AMPLIFIER_HIGH_BYTE, 1, nullptr,
                                   syrlinks::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(syrlinks::TEMP_POWER_AMPLIFIER_LOW_BYTE, 1, nullptr,
                                   syrlinks::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(syrlinks::TEMP_BASEBAND_BOARD_HIGH_BYTE, 1, nullptr,
                                   syrlinks::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(syrlinks::TEMP_BASEBAND_BOARD_LOW_BYTE, 1, nullptr,
                                   syrlinks::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(syrlinks::READ_RX_STATUS_REGISTERS, 1, &rxDataset,
                                   syrlinks::RX_STATUS_REGISTERS_REPLY_SIZE);
}

ReturnValue_t SyrlinksHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                            DeviceCommandId_t* foundId, size_t* foundLen) {
  ReturnValue_t result = returnvalue::OK;

  if (*start != '<') {
    sif::warning << "SyrlinksHkHandler::scanForReply: Missing start frame character" << std::endl;
    return MISSING_START_FRAME_CHARACTER;
  }

  switch (*(start + 1)) {
    case ('A'):
      *foundLen = syrlinks::ACK_SIZE;
      *foundId = syrlinks::ACK_REPLY;
      break;
    case ('E'):
      *foundLen = syrlinks::RX_STATUS_REGISTERS_REPLY_SIZE;
      *foundId = syrlinks::READ_RX_STATUS_REGISTERS;
      break;
    case ('R'):
      *foundId = rememberCommandId;
      *foundLen = syrlinks::READ_ONE_REGISTER_REPLY_SIE;
      break;
    default:
      sif::warning << "SyrlinksHkHandler::scanForReply: Unknown reply identifier" << std::endl;
      result = IGNORE_REPLY_DATA;
      break;
  }

  return result;
}

ReturnValue_t SyrlinksHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) {
  if (powerSwitch == power::NO_SWITCH) {
    return DeviceHandlerBase::NO_SWITCH;
  }
  *numberOfSwitches = 1;
  *switches = &powerSwitch;
  return returnvalue::OK;
}

ReturnValue_t SyrlinksHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
  ReturnValue_t result;

  switch (id) {
    case (syrlinks::ACK_REPLY): {
      result = verifyReply(packet, syrlinks::ACK_SIZE);
      if (result != returnvalue::OK) {
        sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Acknowledgment reply has "
                        "invalid crc"
                     << std::endl;
        return CRC_FAILURE;
      }
      result = handleAckReply(packet);
      if (result != returnvalue::OK) {
        return result;
      }
      break;
    }
    case (syrlinks::READ_RX_STATUS_REGISTERS): {
      result = verifyReply(packet, syrlinks::RX_STATUS_REGISTERS_REPLY_SIZE);
      if (result != returnvalue::OK) {
        sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read rx status registers reply "
                     << "has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      parseRxStatusRegistersReply(packet);
      break;
    }
    case (syrlinks::READ_LCL_CONFIG): {
      result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE);
      if (result != returnvalue::OK) {
        sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read config lcl reply "
                     << "has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      parseLclConfigReply(packet);
      break;
    }
    case (syrlinks::READ_TX_STATUS): {
      result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE);
      if (result != returnvalue::OK) {
        sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read tx status reply "
                     << "has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      parseTxStatusReply(packet);
      break;
    }
    case (syrlinks::READ_TX_WAVEFORM): {
      result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE);
      if (result != returnvalue::OK) {
        sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read tx waveform reply "
                     << "has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      parseTxWaveformReply(packet);
      break;
    }
    case (syrlinks::READ_TX_AGC_VALUE_HIGH_BYTE): {
      result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE);
      if (result != returnvalue::OK) {
        sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read tx AGC high byte reply "
                     << "has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      parseAgcHighByte(packet);
      break;
    }
    case (syrlinks::READ_TX_AGC_VALUE_LOW_BYTE): {
      result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE);
      if (result != returnvalue::OK) {
        sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read tx AGC low byte reply "
                     << "has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      parseAgcLowByte(packet);
      break;
    }
    case (syrlinks::TEMP_BASEBAND_BOARD_HIGH_BYTE): {
      result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE);
      if (result != returnvalue::OK) {
        sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read temperature baseband board "
                     << "high byte reply has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      rawTempBasebandBoard = convertHexStringToUint8(reinterpret_cast<const char*>(
                                 packet + syrlinks::MESSAGE_HEADER_SIZE))
                             << 8;
      break;
    }
    case (syrlinks::TEMP_BASEBAND_BOARD_LOW_BYTE): {
      result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE);
      if (result != returnvalue::OK) {
        sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read temperature baseband board"
                        " low byte reply has invalid crc"
                     << std::endl;
        return CRC_FAILURE;
      }
      rawTempBasebandBoard |= convertHexStringToUint8(
          reinterpret_cast<const char*>(packet + syrlinks::MESSAGE_HEADER_SIZE));
      tempBasebandBoard = calcTempVal(rawTempBasebandBoard);
      PoolReadGuard rg(&temperatureSet);
      temperatureSet.temperatureBasebandBoard = tempBasebandBoard;
      temperatureSet.temperatureBasebandBoard.setValid(true);
      if (debugMode) {
        sif::info << "Syrlinks temperature baseband board: " << tempBasebandBoard << "  °C"
                  << std::endl;
      }
      break;
    }
    case (syrlinks::TEMP_POWER_AMPLIFIER_HIGH_BYTE): {
      result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE);
      if (result != returnvalue::OK) {
        sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read temperature power amplifier "
                     << "board high byte reply has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      rawTempPowerAmplifier = 0;
      rawTempPowerAmplifier = convertHexStringToUint8(reinterpret_cast<const char*>(
                                  packet + syrlinks::MESSAGE_HEADER_SIZE))
                              << 8;
      break;
    }
    case (syrlinks::TEMP_POWER_AMPLIFIER_LOW_BYTE): {
      result = verifyReply(packet, syrlinks::READ_ONE_REGISTER_REPLY_SIE);
      if (result != returnvalue::OK) {
        sif::warning << "SyrlinksHkHandler::interpretDeviceReply: Read temperature power amplifier"
                     << " board low byte reply has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      rawTempPowerAmplifier |= convertHexStringToUint8(
          reinterpret_cast<const char*>(packet + syrlinks::MESSAGE_HEADER_SIZE));
      tempPowerAmplifier = calcTempVal(rawTempPowerAmplifier);
      PoolReadGuard rg(&temperatureSet);
      temperatureSet.temperaturePowerAmplifier = tempPowerAmplifier;
      temperatureSet.temperaturePowerAmplifier.setValid(true);
      if (debugMode) {
        sif::info << "Syrlinks temperature power amplifier board: " << tempPowerAmplifier << "  °C"
                  << std::endl;
      }
      break;
    }
    default: {
      sif::debug << "SyrlinksHkHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }

  return returnvalue::OK;
}

LocalPoolDataSetBase* SyrlinksHandler::getDataSetHandle(sid_t sid) {
  if (sid == rxDataset.getSid()) {
    return &rxDataset;
  } else if (sid == txDataset.getSid()) {
    return &txDataset;
  } else if (sid == temperatureSet.getSid()) {
    return &temperatureSet;
  } else {
    sif::warning << "SyrlinksHkHandler::getDataSetHandle: Invalid sid" << std::endl;
    return nullptr;
  }
}

std::string SyrlinksHandler::convertUint16ToHexString(uint16_t intValue) {
  std::stringstream stream;
  stream << std::setfill('0') << std::setw(4) << std::hex << std::uppercase << intValue;
  return stream.str();
}

uint8_t SyrlinksHandler::convertHexStringToUint8(const char* twoChars) {
  uint32_t value;
  std::string hexString(twoChars, 2);
  std::stringstream stream;
  stream << std::hex << hexString;
  stream >> value;
  return static_cast<uint8_t>(value);
}

uint16_t SyrlinksHandler::convertHexStringToUint16(const char* fourChars) {
  uint16_t value = 0;
  value = convertHexStringToUint8(fourChars) << 8 | convertHexStringToUint8(fourChars + 2);
  return value;
}

ReturnValue_t SyrlinksHandler::parseReplyStatus(const char* status) {
  switch (*status) {
    case '0':
      return returnvalue::OK;
    case '1':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Uart framing or parity error"
                 << std::endl;
      return UART_FRAMIN_OR_PARITY_ERROR_ACK;
    case '2':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad character detected" << std::endl;
      return BAD_CHARACTER_ACK;
    case '3':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad parameter value (unexpected value "
                 << "detected" << std::endl;
      return BAD_PARAMETER_VALUE_ACK;
    case '4':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad end of frame" << std::endl;
      return BAD_END_OF_FRAME_ACK;
    case '5':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Unknown command id or attempt to access"
                 << " a protected register" << std::endl;
      return UNKNOWN_COMMAND_ID_ACK;
    case '6':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad CRC" << std::endl;
      return BAD_CRC_ACK;
    default:
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Status reply contains an invalid "
                 << "status id" << std::endl;
      return returnvalue::FAILED;
  }
}

ReturnValue_t SyrlinksHandler::verifyReply(const uint8_t* packet, uint8_t size) {
  int result = 0;
  /* Calculate crc from received packet */
  uint16_t crc =
      CRC::crc16ccitt(packet, size - syrlinks::SIZE_CRC_AND_TERMINATION, CRC_INITIAL_VALUE);
  std::string recalculatedCrc = convertUint16ToHexString(crc);

  const char* startOfCrc =
      reinterpret_cast<const char*>(packet + size - syrlinks::SIZE_CRC_AND_TERMINATION);
  const char* endOfCrc = reinterpret_cast<const char*>(packet + size - 1);

  std::string replyCrc(startOfCrc, endOfCrc);

  result = recalculatedCrc.compare(replyCrc);
  if (result != 0) {
    return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

void SyrlinksHandler::parseRxStatusRegistersReply(const uint8_t* packet) {
  PoolReadGuard readHelper(&rxDataset);
  uint16_t offset = syrlinks::MESSAGE_HEADER_SIZE;
  rxDataset.rxStatus = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
  offset += 2;
  rxDataset.rxSensitivity =
      convertHexStringTo32bit<uint32_t>(reinterpret_cast<const char*>(packet + offset), 6);
  offset += 6;
  rxDataset.rxFrequencyShift =
      convertHexStringTo32bit<int32_t>(reinterpret_cast<const char*>(packet + offset), 6);
  offset += 6;
  rxDataset.rxIqPower = convertHexStringToUint16(reinterpret_cast<const char*>(packet + offset));
  offset += 4;
  rxDataset.rxAgcValue = convertHexStringToUint16(reinterpret_cast<const char*>(packet + offset));
  offset += 4;
  offset += 2;  // reserved register
  rxDataset.rxDemodEb =
      convertHexStringTo32bit<uint32_t>(reinterpret_cast<const char*>(packet + offset), 6);
  offset += 6;
  rxDataset.rxDemodN0 =
      convertHexStringTo32bit<uint32_t>(reinterpret_cast<const char*>(packet + offset), 6);
  offset += 6;
  rxDataset.rxDataRate = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));

  rxDataset.setValidity(true, true);
  if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "Syrlinks RX Status: 0x" << std::hex << (unsigned int)rxDataset.rxStatus.value
              << std::endl;
    sif::info << "Syrlinks RX Sensitivity: " << std::dec << rxDataset.rxSensitivity << std::endl;
    sif::info << "Syrlinks RX Frequency Shift: " << rxDataset.rxFrequencyShift << std::endl;
    sif::info << "Syrlinks RX IQ Power: " << rxDataset.rxIqPower << std::endl;
    sif::info << "Syrlinks RX AGC Value: " << rxDataset.rxAgcValue << std::endl;
    sif::info << "Syrlinks RX Demod Eb: " << rxDataset.rxDemodEb << std::endl;
    sif::info << "Syrlinks RX Demod N0: " << rxDataset.rxDemodN0 << std::endl;
    sif::info << "Syrlinks RX Datarate: " << (unsigned int)rxDataset.rxDataRate.value << std::endl;
#endif
  }
}

void SyrlinksHandler::parseLclConfigReply(const uint8_t* packet) {
  uint16_t offset = syrlinks::MESSAGE_HEADER_SIZE;
  uint8_t lclConfig = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
  if (debugMode) {
    sif::info << "SyrlinksHkHandler::parseRxStatusRegistersReply: Lcl config: "
              << static_cast<unsigned int>(lclConfig) << std::endl;
  }
}

void SyrlinksHandler::parseTxStatusReply(const uint8_t* packet) {
  PoolReadGuard readHelper(&txDataset);
  uint16_t offset = syrlinks::MESSAGE_HEADER_SIZE;
  txDataset.txStatus = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
  txDataset.txStatus.setValid(true);
  if (debugMode) {
    sif::info << "Syrlinks TX Status: 0x" << std::hex << (unsigned int)txDataset.txStatus.value
              << std::endl;
  }
}

void SyrlinksHandler::parseTxWaveformReply(const uint8_t* packet) {
  PoolReadGuard readHelper(&txDataset);
  uint16_t offset = syrlinks::MESSAGE_HEADER_SIZE;
  txDataset.txWaveform = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
  txDataset.txWaveform.setValid(true);
  if (debugMode) {
    sif::info << "Syrlinks TX Waveform: 0x" << std::hex << (unsigned int)txDataset.txWaveform.value
              << std::endl;
  }
}

void SyrlinksHandler::parseAgcLowByte(const uint8_t* packet) {
  PoolReadGuard readHelper(&txDataset);
  uint16_t offset = syrlinks::MESSAGE_HEADER_SIZE;
  txDataset.txAgcValue = agcValueHighByte << 8 |
                         convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
  txDataset.txAgcValue.setValid(true);
  if (debugMode) {
    sif::info << "Syrlinks TX AGC Value: " << txDataset.txAgcValue << std::endl;
  }
}

void SyrlinksHandler::parseAgcHighByte(const uint8_t* packet) {
  PoolReadGuard readHelper(&txDataset);
  uint16_t offset = syrlinks::MESSAGE_HEADER_SIZE;
  agcValueHighByte = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
}

void SyrlinksHandler::setNormalDatapoolEntriesInvalid() {}

uint32_t SyrlinksHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; }

ReturnValue_t SyrlinksHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                       LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(syrlinks::RX_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(syrlinks::RX_SENSITIVITY, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(syrlinks::RX_FREQUENCY_SHIFT, new PoolEntry<int32_t>({0}));
  localDataPoolMap.emplace(syrlinks::RX_IQ_POWER, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(syrlinks::RX_AGC_VALUE, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(syrlinks::RX_DEMOD_EB, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(syrlinks::RX_DEMOD_N0, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(syrlinks::RX_DATA_RATE, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(syrlinks::TX_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(syrlinks::TX_WAVEFORM, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(syrlinks::TX_AGC_VALUE, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(syrlinks::TEMP_BASEBAND_BOARD, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(syrlinks::TEMP_POWER_AMPLIFIER, new PoolEntry<float>({0}));

  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(txDataset.getSid(), false, 5.0));
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(rxDataset.getSid(), false, 5.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
  return returnvalue::OK;
}

void SyrlinksHandler::setModeNormal() { setMode(MODE_NORMAL); }

float SyrlinksHandler::calcTempVal(uint16_t raw) { return 0.126984 * raw - 67.87; }

ReturnValue_t SyrlinksHandler::handleAckReply(const uint8_t* packet) {
  ReturnValue_t result =
      parseReplyStatus(reinterpret_cast<const char*>(packet + syrlinks::MESSAGE_HEADER_SIZE));
  switch (rememberCommandId) {
    case (syrlinks::WRITE_LCL_CONFIG): {
      if (result != returnvalue::OK) {
        internalState = InternalState::OFF;
      } else if (result == returnvalue::OK) {
        internalState = InternalState::IDLE;
      }
      break;
    }
    case (syrlinks::SET_TX_MODE_STANDBY): {
      if (result == returnvalue::OK) {
        internalState = InternalState::IDLE;
        commandExecuted = true;
      }
      break;
    }
    case (syrlinks::SET_TX_MODE_MODULATION): {
      if (result == returnvalue::OK) {
        internalState = InternalState::IDLE;
        commandExecuted = true;
      }
      break;
    }
    case (syrlinks::SET_TX_MODE_CW): {
      if (result == returnvalue::OK) {
        internalState = InternalState::IDLE;
        commandExecuted = true;
      }
      break;
    }
  }
  return result;
}

void SyrlinksHandler::prepareCommand(std::string command, DeviceCommandId_t commandId) {
  command.copy(reinterpret_cast<char*>(commandBuffer), command.size(), 0);
  rawPacketLen = command.size();
  rememberCommandId = commandId;
  rawPacket = commandBuffer;
}

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

void SyrlinksHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
  Mode_t tgtMode = getBaseMode(getMode());
  if (tgtMode == HasModesIF::MODE_ON or tgtMode == DeviceHandlerIF::MODE_NORMAL) {
    switch (getSubmode()) {
      case (syrlinks::Submode::TX_MODULATION): {
        if (commandExecuted) {
          setMode(tgtMode);
          return;
        }
        internalState = InternalState::SET_TX_MODULATION;
        break;
      }
      case (syrlinks::Submode::TX_STANDBY): {
        if (commandExecuted) {
          setMode(tgtMode);
          return;
        }
        internalState = InternalState::SET_TX_STANDBY;
        break;
      }
      case (syrlinks::Submode::TX_CW): {
        if (commandExecuted) {
          setMode(tgtMode);
          return;
        }
        internalState = InternalState::SET_TX_CW;
        break;
      }
      default: {
        setMode(tgtMode);
      }
    }
  } else {
    setMode(tgtMode);
  }
}