#include "GomspaceDeviceHandler.h"

#include <common/config/commonObjects.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/arrayprinter.h>

#include "devicedefinitions/GomSpacePackets.h"
#include "devicedefinitions/powerDefinitions.h"
#include <cassert>

using namespace GOMSPACE;

GomspaceDeviceHandler::GomspaceDeviceHandler(object_id_t objectId, object_id_t comIF,
                                             CookieIF* comCookie, FailureIsolationBase* customFdir,
                                             uint16_t maxConfigTableAddress,
                                             uint16_t maxHkTableAddress, uint16_t hkTableReplySize)
    : DeviceHandlerBase(objectId, comIF, comCookie, customFdir),
      maxConfigTableAddress(maxConfigTableAddress),
      maxHkTableAddress(maxHkTableAddress),
      hkTableReplySize(hkTableReplySize) {
  if (comCookie == nullptr) {
    sif::error << "GomspaceDeviceHandler::GomspaceDeviceHandler: Invalid com cookie" << std::endl;
  }
}

GomspaceDeviceHandler::~GomspaceDeviceHandler() {}

void GomspaceDeviceHandler::doStartUp() {}

void GomspaceDeviceHandler::doShutDown() {}

ReturnValue_t GomspaceDeviceHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                             const uint8_t* commandData,
                                                             size_t commandDataLen) {
  ReturnValue_t result = childCommandHook(deviceCommand, commandData, commandDataLen);
  switch (deviceCommand) {
    case (GOMSPACE::PING): {
      result = generatePingCommand(commandData, commandDataLen);
      if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
      }
      break;
    }
    case (GOMSPACE::REBOOT): {
      generateRebootCommand();
      break;
    }
    case (GOMSPACE::PARAM_SET): {
      result = generateSetParamCommand(commandData, commandDataLen);
      if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
      }
      break;
    }
    case (GOMSPACE::PARAM_GET): {
      result = generateGetParamCommand(commandData, commandDataLen);
      if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
      }
      break;
    }
    case (GOMSPACE::GNDWDT_RESET): {
      result = generateResetWatchdogCmd();
      if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
      }
      break;
    }
    case (GOMSPACE::PRINT_SWITCH_V_I):
    case (GOMSPACE::PRINT_LATCHUPS): {
      result = printStatus(deviceCommand);
      break;
    }
    case (GOMSPACE::REQUEST_HK_TABLE): {
      auto reqType = SpecialRequestTypes::DEFAULT_COM_IF;
      if(getObjectId() == objects::PDU1_HANDLER or getObjectId() == objects::PDU2_HANDLER) {
        reqType = SpecialRequestTypes::GET_PDU_HK;
      } else if(getObjectId() == objects::ACU_HANDLER) {
        reqType = SpecialRequestTypes::GET_ACU_HK;
      } else if(getObjectId() == objects::P60DOCK_HANDLER) {
        reqType = SpecialRequestTypes::GET_P60DOCK_HK;
      }
      result = generateRequestFullTableCmd(reqType, GOMSPACE::TableIds::HK, hkTableReplySize);
      if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
      }
      break;
    }
    case (GOMSPACE::REQUEST_CONFIG_TABLE): {

      result = generateRequestFullTableCmd(SpecialRequestTypes::DEFAULT_COM_IF,
          GOMSPACE::TableIds::CONFIG, configTableReplySize);
      if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
      }
      break;
    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return HasReturnvaluesIF::RETURN_OK;
}

void GomspaceDeviceHandler::fillCommandAndReplyMap() {
  this->insertInCommandAndReplyMap(GOMSPACE::PING, 3);
  this->insertInCommandMap(GOMSPACE::REBOOT);
  this->insertInCommandAndReplyMap(GOMSPACE::PARAM_SET, 3);
  this->insertInCommandAndReplyMap(GOMSPACE::PARAM_GET, 3);
  this->insertInCommandAndReplyMap(GOMSPACE::REQUEST_HK_TABLE, 3);
  this->insertInCommandMap(GOMSPACE::GNDWDT_RESET);
  this->insertInCommandMap(GOMSPACE::PRINT_SWITCH_V_I);
  this->insertInCommandMap(GOMSPACE::PRINT_LATCHUPS);
}

ReturnValue_t GomspaceDeviceHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                                  DeviceCommandId_t* foundId, size_t* foundLen) {
  switch (rememberCommandId) {
    case (GOMSPACE::PING):
      *foundId = GOMSPACE::PING;
      *foundLen = PING_REPLY_SIZE;
      rememberCommandId = GOMSPACE::NONE;
      break;
    case (GOMSPACE::PARAM_GET): {
      *foundId = GOMSPACE::PARAM_GET;
      *foundLen = rememberRequestedSize + GOMSPACE::GS_HDR_LENGTH;
      rememberCommandId = GOMSPACE::NONE;
      break;
    }
    case (GOMSPACE::PARAM_SET): {
      *foundId = GOMSPACE::PARAM_SET;
      *foundLen = rememberRequestedSize;
      rememberCommandId = GOMSPACE::NONE;
      break;
    }
    case (GOMSPACE::REQUEST_HK_TABLE): {
      *foundId = GOMSPACE::REQUEST_HK_TABLE;
      *foundLen = rememberRequestedSize + GOMSPACE::GS_HDR_LENGTH;
      rememberCommandId = GOMSPACE::NONE;
      break;
    }
    default:
      return IGNORE_REPLY_DATA;
  }
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                          const uint8_t* packet) {
  switch (id) {
    case (GOMSPACE::PING): {
      SerializeElement<uint32_t> replyTime = *packet;
      handleDeviceTM(&replyTime, id, true);
      break;
    }
    case (GOMSPACE::PARAM_GET): {
      // -2 to subtract address size from gomspace parameter reply packet
      uint16_t payloadLength = (*(packet + 2) << 8 | *(packet + 3)) - 2;
      if (payloadLength > sizeof(uint32_t)) {
        sif::error << "GomspaceDeviceHandler: PARAM_GET: Invalid payload "
                   << "size in reply" << std::endl;
        return INVALID_PAYLOAD_SIZE;
      }
      uint8_t tempPayloadBuffer[payloadLength];
      /* Extract information from received data */
      CspGetParamReply cspGetParamReply(tempPayloadBuffer, payloadLength);
      size_t size = GOMSPACE::GS_HDR_LENGTH + payloadLength;
      ReturnValue_t result =
          cspGetParamReply.deSerialize(&packet, &size, SerializeIF::Endianness::BIG);
      if (result != HasReturnvaluesIF::RETURN_OK) {
        sif::error << "GomspaceDeviceHandler: Failed to deserialize get parameter"
                   << "reply" << std::endl;
        return result;
      }
      uint8_t action = cspGetParamReply.getAction();
      uint8_t tableId = cspGetParamReply.getTableId();
      uint16_t address = cspGetParamReply.getAddress();
      /* Pack relevant information into a tm packet */
      ParamReply paramReply(action, tableId, address, payloadLength, tempPayloadBuffer);
      handleDeviceTM(&paramReply, id, true);
      break;
    }
    case (GOMSPACE::PARAM_SET): {
      /* When setting a parameter, the p60dock sends back the state of the
       * operation */
      if (*packet != PARAM_SET_OK) {
        return HasReturnvaluesIF::RETURN_FAILED;
      }
      setParamCallback(setParamCacher, true);
      break;
    }
    case (GOMSPACE::REQUEST_HK_TABLE): {
      letChildHandleHkReply(id, packet);
      break;
    }
    default:
      break;
  }
  return HasReturnvaluesIF::RETURN_OK;
}

void GomspaceDeviceHandler::setNormalDatapoolEntriesInvalid() {}

ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* commandData,
                                                             size_t commandDataLen) {
  ReturnValue_t result =
      setParamCacher.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::BIG);
  // This breaks layering but I really don't want to accept this command..
  if (setParamCacher.getAddress() == PDU2::CONFIG_ADDRESS_OUT_EN_Q7S and
      this->getObjectId() == objects::PDU2_HANDLER) {
    triggerEvent(power::SWITCHING_Q7S_DENIED, 0, 0);
    return HasReturnvaluesIF::RETURN_FAILED;
  }
  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::error << "GomspaceDeviceHandler: Failed to deserialize set parameter "
                  "message"
               << std::endl;
    return result;
  }
  result = setParamCallback(setParamCacher, false);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  /* Get and check address */
  uint16_t address = setParamCacher.getAddress();
  if (address > maxConfigTableAddress) {
    sif::error << "GomspaceDeviceHandler: Invalid address for set parameter "
               << "action" << std::endl;
    return INVALID_ADDRESS;
  }
  /* CSP reply only contains the transaction state */
  uint16_t querySize = 1;
  const uint8_t* parameterPtr = setParamCacher.getParameter();
  uint8_t parameterSize = setParamCacher.getParameterSize();
  uint16_t payloadlength = sizeof(address) + parameterSize;

  /* Generate command for CspComIF */
  CspSetParamCommand setParamCmd(querySize, payloadlength, address, parameterPtr, parameterSize);
  size_t cspPacketLen = 0;
  uint8_t* buffer = cspPacket;
  result = setParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
                                 SerializeIF::Endianness::BIG);

  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::error << "GomspaceDeviceHandler: Failed to serialize command for "
               << "CspComIF" << std::endl;
    return result;
  }
  if (cspPacketLen > MAX_PACKET_LEN) {
    sif::error << "GomspaceDeviceHandler: Invalid length of set parameter "
                  "command"
               << std::endl;
    return PACKET_TOO_LONG;
  }
  rawPacket = cspPacket;
  rawPacketLen = cspPacketLen;
  rememberRequestedSize = querySize;
  rememberCommandId = GOMSPACE::PARAM_SET;
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(const uint8_t* commandData,
                                                             size_t commandDataLen) {
  ReturnValue_t result;
  /* Unpack the received action message */
  GetParamMessageUnpacker getParamMessage;
  result = getParamMessage.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::BIG);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::error << "Failed to deserialize message to extract information "
                  "from get parameter message"
               << std::endl;
    return result;
  }
  /* Get an check table id to read from */
  uint8_t tableId = getParamMessage.getTableId();
  if (not validTableId(tableId)) {
    sif::error << "GomspaceDeviceHandler: Invalid table id in get parameter"
                  " message"
               << std::endl;
    return INVALID_TABLE_ID;
  }
  /* Get and check address */
  uint16_t address = getParamMessage.getAddress();
  if (address > maxHkTableAddress and tableId == TableIds::HK) {
    sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from "
               << "housekeeping table" << std::endl;
    return INVALID_ADDRESS;
  }
  if (address > maxConfigTableAddress and tableId == TableIds::CONFIG) {
    sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from "
               << "configuration table" << std::endl;
    return INVALID_ADDRESS;
  }
  uint16_t length = sizeof(address);
  uint8_t parameterSize = getParamMessage.getParameterSize();
  if (parameterSize > sizeof(uint32_t)) {
    sif::error << "GomspaceDeviceHandler: GET_PARAM: Invalid parameter "
               << "size" << std::endl;
    return INVALID_PARAM_SIZE;
  }
  uint16_t querySize = parameterSize + GOMSPACE::GS_HDR_LENGTH;

  /* Generate the CSP command to send to the P60 Dock */
  CspGetParamCommand getParamCmd(querySize, tableId, length, address);
  size_t cspPacketLen = 0;
  uint8_t* buffer = cspPacket;
  result = getParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
                                 SerializeIF::Endianness::BIG);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::error << "GomspaceDeviceHandler: Failed to serialize command to "
               << "get parameter" << std::endl;
  }
  if (cspPacketLen > MAX_PACKET_LEN) {
    sif::error << "GomspaceDeviceHandler: Received invalid get parameter "
                  "command"
               << std::endl;
    return PACKET_TOO_LONG;
  }
  rawPacket = cspPacket;
  rawPacketLen = cspPacketLen;
  rememberRequestedSize = querySize;
  rememberCommandId = GOMSPACE::PARAM_GET;
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::generatePingCommand(const uint8_t* commandData,
                                                         size_t commandDataLen) {
  CspPingCommand cspPingCommand(commandData, commandDataLen);
  size_t cspPacketLen = 0;
  uint8_t* buffer = cspPacket;
  ReturnValue_t result = cspPingCommand.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
                                                  SerializeIF::Endianness::BIG);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::error << "GomspaceDeviceHandler: Failed to serialize ping command" << std::endl;
    return result;
  }
  if (cspPacketLen > MAX_PACKET_LEN) {
    sif::error << "GomspaceDeviceHandler: Received invalid ping message" << std::endl;
    return PACKET_TOO_LONG;
  }
  rawPacket = cspPacket;
  rawPacketLen = cspPacketLen;
  rememberCommandId = GOMSPACE::PING;
  return HasReturnvaluesIF::RETURN_OK;
}

void GomspaceDeviceHandler::generateRebootCommand() {
  uint8_t cspPort = GOMSPACE::REBOOT_PORT;
  uint16_t querySize = 0;
  *cspPacket = GOMSPACE::REBOOT_PORT;
  *(cspPacket + 1) = querySize;
  size_t cspPacketLen = sizeof(cspPort) + sizeof(cspPacketLen);
  rawPacket = cspPacket;
  rawPacketLen = cspPacketLen;
}

ReturnValue_t GomspaceDeviceHandler::childCommandHook(DeviceCommandId_t cmd,
                                                      const uint8_t* commandData,
                                                      size_t commandDataLen) {
  return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}

ReturnValue_t GomspaceDeviceHandler::setParamCallback(SetParamMessageUnpacker& unpacker,
                                                      bool afterExecution) {
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::initializePduPool(
    localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager,
    std::array<uint8_t, PDU::CHANNELS_LEN> initOutEnb) {
  localDataPoolMap.emplace(P60System::pool::PDU_CURRENTS, new PoolEntry<int16_t>(9));
  localDataPoolMap.emplace(P60System::pool::PDU_VOLTAGES, new PoolEntry<int16_t>(9));

  localDataPoolMap.emplace(P60System::pool::PDU_VCC, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_VBAT, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_TEMPERATURE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_CONV_EN, new PoolEntry<uint8_t>(3));

  localDataPoolMap.emplace(P60System::pool::PDU_OUT_ENABLE,
                           new PoolEntry<uint8_t>(initOutEnb.data(), initOutEnb.size()));

  localDataPoolMap.emplace(P60System::pool::PDU_BOOTCAUSE, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_BOOTCNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_UPTIME, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_RESETCAUSE, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_BATT_MODE, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(P60System::pool::PDU_LATCHUPS, new PoolEntry<uint16_t>(9));

  localDataPoolMap.emplace(P60System::pool::PDU_DEVICES, new PoolEntry<uint8_t>(8));
  localDataPoolMap.emplace(P60System::pool::PDU_STATUSES, new PoolEntry<uint8_t>(8));

  localDataPoolMap.emplace(P60System::pool::PDU_WDT_CNT_GND, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_WDT_CNT_I2C, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_WDT_CNT_CAN, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_WDT_CNT_CSP1, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_WDT_CNT_CSP2, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_WDT_GND_LEFT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_WDT_I2C_LEFT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_WDT_CAN_LEFT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_WDT_CSP_LEFT1, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU_WDT_CSP_LEFT2, new PoolEntry<uint8_t>({0}));
  return RETURN_OK;
}

bool GomspaceDeviceHandler::validTableId(uint8_t id) {
  if (id == TableIds::CONFIG or id == TableIds::BOARD_PARAMS or id == TableIds::CALIBRATION or
      id == TableIds::HK) {
    return true;
  }
  return false;
}

ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd() {
  WatchdogResetCommand watchdogResetCommand;
  size_t cspPacketLen = 0;
  uint8_t* buffer = cspPacket;
  ReturnValue_t result = watchdogResetCommand.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
                                                        SerializeIF::Endianness::BIG);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::error << "GomspaceDeviceHandler: Failed to serialize watchdog reset "
               << "command" << std::endl;
    return result;
  }
  rawPacket = cspPacket;
  rawPacketLen = cspPacketLen;
  rememberRequestedSize = 0;  // No bytes will be queried with the ground
  // watchdog command.
  rememberCommandId = GOMSPACE::GNDWDT_RESET;
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::generateRequestFullTableCmd(SpecialRequestTypes reqType,
    uint8_t tableId,
                                                                 uint16_t tableReplySize) {
  uint16_t querySize = tableReplySize;
  if(reqType != SpecialRequestTypes::DEFAULT_COM_IF) {
    auto* cspCookie = dynamic_cast<CspCookie*>(comCookie);
    cspCookie->setRequest(reqType, tableReplySize);
    cspCookie->setCspPort(CspPorts::P60_PORT_RPARAM_ENUM);
  } else {
    RequestFullTableCommand requestFullTableCommand(querySize, tableId);

    size_t cspPacketLen = 0;
    uint8_t* buffer = cspPacket;
    ReturnValue_t result = requestFullTableCommand.serialize(
        &buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG);
    if (result != HasReturnvaluesIF::RETURN_OK) {
      sif::error << "GomspaceDeviceHandler::generateRequestFullTableCmd Failed to serialize "
                    "full table request command "
                 << std::endl;
      return result;
    }
    rawPacket = cspPacket;
    rawPacketLen = cspPacketLen;
  }

  rememberRequestedSize = querySize;
  rememberCommandId = GOMSPACE::REQUEST_HK_TABLE;
  return RETURN_OK;
}

uint32_t GomspaceDeviceHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 0; }

void GomspaceDeviceHandler::setModeNormal() { mode = MODE_NORMAL; }

ReturnValue_t GomspaceDeviceHandler::printStatus(DeviceCommandId_t cmd) {
  sif::info << "No printHkTable implementation given.." << std::endl;
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU::PduAuxHk& auxHk,
                                                     const uint8_t* packet) {
  PoolReadGuard pg0(&coreHk);
  PoolReadGuard pg1(&auxHk);
  if (pg0.getReadResult() != HasReturnvaluesIF::RETURN_OK or
      pg1.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
    sif::warning << "Reading PDU1 datasets failed!" << std::endl;
    return HasReturnvaluesIF::RETURN_FAILED;
  }
  /* 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 < PDU::CHANNELS_LEN; idx++) {
    coreHk.currents[idx] = as<int16_t>(packet + (idx * 2));
  }
  for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) {
    coreHk.voltages[idx] = as<uint16_t>(packet + 0x12 + (idx*2));
  }
  auxHk.vcc.value = as<int16_t>(packet + 0x24);
  auxHk.vbat.value = as<int16_t>(packet + 0x26);
  coreHk.temperature = as<int16_t>(packet + 0x28) * 0.1;

  for (uint8_t idx = 0; idx < 3; idx++) {
    auxHk.converterEnable[idx] = packet[0x2a + idx];
  }

  for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) {
    coreHk.outputEnables[idx] = packet[0x2e + idx];
  }
  auxHk.bootcause = as<uint32_t>(packet + 0x38);
  coreHk.bootcount = as<uint32_t>(packet + 0x3c);
  auxHk.uptime = as<uint32_t>(packet + 0x40);
  auxHk.resetcause = as<uint16_t>(packet + 0x44);
  coreHk.battMode = *(packet + 0x46);

  for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) {
    auxHk.latchups[idx] = as<uint16_t>(packet + 0x48 + (idx*2));
  }

  for (uint8_t idx = 0; idx < PDU::DEVICES_NUM; idx++) {
    auxHk.deviceTypes[idx] = *(packet + 0x5a + idx);
  }
  for (uint8_t idx = 0; idx < PDU::DEVICES_NUM; idx++) {
    auxHk.devicesStatus[idx] = *(packet + 0x62 + idx);
  }

  auxHk.gndWdtReboots = as<uint32_t>(packet + 0x6c);
  auxHk.i2cWdtReboots = as<uint32_t>(packet + 0x70);
  auxHk.canWdtReboots = as<uint32_t>(packet + 0x74);
  auxHk.csp1WdtReboots = as<uint32_t>(packet + 0x78);
  auxHk.csp2WdtReboots = as<uint32_t>(packet + 0x78 + 4);
  auxHk.groundWatchdogSecondsLeft = as<uint32_t>(packet + 0x80);
  auxHk.i2cWatchdogSecondsLeft = as<uint32_t>(packet + 0x84);
  auxHk.canWatchdogSecondsLeft = as<uint32_t>(packet + 0x88);
  auxHk.csp1WatchdogPingsLeft = *(packet + 0x8c);
  auxHk.csp2WatchdogPingsLeft = *(packet + 0x8c + 1);
  coreHk.setChanged(true);
  if (not coreHk.isValid()) {
    coreHk.setValidity(true, true);
  }
  if (not auxHk.isValid()) {
    auxHk.setValidity(true, true);
  }
  return RETURN_OK;
}