// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "REVPDH.h"

#include <hal/CANAPI.h>
#include <hal/CANAPITypes.h>
#include <hal/Errors.h>
#include <hal/handles/HandlesInternal.h>
#include <hal/handles/IndexedHandleResource.h>

#include <cstring>

#include <fmt/format.h>

#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "rev/PDHFrames.h"

using namespace hal;

static constexpr HAL_CANManufacturer manufacturer =
    HAL_CANManufacturer::HAL_CAN_Man_kREV;

static constexpr HAL_CANDeviceType deviceType =
    HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;

static constexpr int32_t kDefaultControlPeriod = 50;

namespace {

struct REV_PDHObj {
  int32_t controlPeriod;
  HAL_CANHandle hcan;
  std::string previousAllocation;
};

}  // namespace

static constexpr uint32_t APIFromExtId(uint32_t extId) {
  return (extId >> 6) & 0x3FF;
}

static constexpr uint32_t PDH_SET_SWITCH_CHANNEL_FRAME_API =
    APIFromExtId(PDH_SET_SWITCH_CHANNEL_FRAME_ID);

static constexpr uint32_t PDH_STATUS_0_FRAME_API =
    APIFromExtId(PDH_STATUS_0_FRAME_ID);
static constexpr uint32_t PDH_STATUS_1_FRAME_API =
    APIFromExtId(PDH_STATUS_1_FRAME_ID);
static constexpr uint32_t PDH_STATUS_2_FRAME_API =
    APIFromExtId(PDH_STATUS_2_FRAME_ID);
static constexpr uint32_t PDH_STATUS_3_FRAME_API =
    APIFromExtId(PDH_STATUS_3_FRAME_ID);
static constexpr uint32_t PDH_STATUS_4_FRAME_API =
    APIFromExtId(PDH_STATUS_4_FRAME_ID);

static constexpr uint32_t PDH_CLEAR_FAULTS_FRAME_API =
    APIFromExtId(PDH_CLEAR_FAULTS_FRAME_ID);

static constexpr uint32_t PDH_VERSION_FRAME_API =
    APIFromExtId(PDH_VERSION_FRAME_ID);

static constexpr int32_t kPDHFrameStatus0Timeout = 20;
static constexpr int32_t kPDHFrameStatus1Timeout = 20;
static constexpr int32_t kPDHFrameStatus2Timeout = 20;
static constexpr int32_t kPDHFrameStatus3Timeout = 20;
static constexpr int32_t kPDHFrameStatus4Timeout = 20;

static IndexedHandleResource<HAL_REVPDHHandle, REV_PDHObj, kNumREVPDHModules,
                             HAL_HandleEnum::REVPDH>* REVPDHHandles;

namespace hal::init {
void InitializeREVPDH() {
  static IndexedHandleResource<HAL_REVPDHHandle, REV_PDHObj, kNumREVPDHModules,
                               HAL_HandleEnum::REVPDH>
      rH;
  REVPDHHandles = &rH;
}
}  // namespace hal::init

extern "C" {

static PDH_status_0_t HAL_ReadREVPDHStatus0(HAL_CANHandle hcan,
                                            int32_t* status) {
  uint8_t packedData[8] = {0};
  int32_t length = 0;
  uint64_t timestamp = 0;
  PDH_status_0_t result = {};

  HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_0_FRAME_API, packedData, &length,
                           &timestamp, kPDHFrameStatus0Timeout * 2, status);

  if (*status != 0) {
    return result;
  }

  PDH_status_0_unpack(&result, packedData, PDH_STATUS_0_LENGTH);

  return result;
}

static PDH_status_1_t HAL_ReadREVPDHStatus1(HAL_CANHandle hcan,
                                            int32_t* status) {
  uint8_t packedData[8] = {0};
  int32_t length = 0;
  uint64_t timestamp = 0;
  PDH_status_1_t result = {};

  HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_1_FRAME_API, packedData, &length,
                           &timestamp, kPDHFrameStatus1Timeout * 2, status);

  if (*status != 0) {
    return result;
  }

  PDH_status_1_unpack(&result, packedData, PDH_STATUS_1_LENGTH);

  return result;
}

static PDH_status_2_t HAL_ReadREVPDHStatus2(HAL_CANHandle hcan,
                                            int32_t* status) {
  uint8_t packedData[8] = {0};
  int32_t length = 0;
  uint64_t timestamp = 0;
  PDH_status_2_t result = {};

  HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_2_FRAME_API, packedData, &length,
                           &timestamp, kPDHFrameStatus2Timeout * 2, status);

  if (*status != 0) {
    return result;
  }

  PDH_status_2_unpack(&result, packedData, PDH_STATUS_2_LENGTH);

  return result;
}

static PDH_status_3_t HAL_ReadREVPDHStatus3(HAL_CANHandle hcan,
                                            int32_t* status) {
  uint8_t packedData[8] = {0};
  int32_t length = 0;
  uint64_t timestamp = 0;
  PDH_status_3_t result = {};

  HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_3_FRAME_API, packedData, &length,
                           &timestamp, kPDHFrameStatus3Timeout * 2, status);

  if (*status != 0) {
    return result;
  }

  PDH_status_3_unpack(&result, packedData, PDH_STATUS_3_LENGTH);

  return result;
}

static PDH_status_4_t HAL_ReadREVPDHStatus4(HAL_CANHandle hcan,
                                            int32_t* status) {
  uint8_t packedData[8] = {0};
  int32_t length = 0;
  uint64_t timestamp = 0;
  PDH_status_4_t result = {};

  HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_4_FRAME_API, packedData, &length,
                           &timestamp, kPDHFrameStatus4Timeout * 2, status);

  if (*status != 0) {
    return result;
  }

  PDH_status_4_unpack(&result, packedData, PDH_STATUS_4_LENGTH);

  return result;
}

/**
 * Helper function for the individual getter functions for status 4
 */
PDH_status_4_t HAL_GetREVPDHStatus4(HAL_REVPDHHandle handle, int32_t* status) {
  PDH_status_4_t statusFrame = {};
  auto hpdh = REVPDHHandles->Get(handle);
  if (hpdh == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return statusFrame;
  }

  statusFrame = HAL_ReadREVPDHStatus4(hpdh->hcan, status);
  return statusFrame;
}

HAL_REVPDHHandle HAL_InitializeREVPDH(int32_t module,
                                      const char* allocationLocation,
                                      int32_t* status) {
  hal::init::CheckInit();
  if (!HAL_CheckREVPDHModuleNumber(module)) {
    *status = RESOURCE_OUT_OF_RANGE;
    return HAL_kInvalidHandle;
  }

  HAL_REVPDHHandle handle;
  auto hpdh = REVPDHHandles->Allocate(module, &handle, status);
  if (*status != 0) {
    if (hpdh) {
      hal::SetLastErrorPreviouslyAllocated(status, "REV PDH", module,
                                           hpdh->previousAllocation);
    } else {
      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PDH", 0,
                                       kNumREVPDHModules, module);
    }
    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
  }

  HAL_CANHandle hcan =
      HAL_InitializeCAN(manufacturer, module, deviceType, status);

  if (*status != 0) {
    REVPDHHandles->Free(handle);
    return HAL_kInvalidHandle;
  }

  hpdh->previousAllocation = allocationLocation ? allocationLocation : "";
  hpdh->hcan = hcan;
  hpdh->controlPeriod = kDefaultControlPeriod;

  return handle;
}

void HAL_FreeREVPDH(HAL_REVPDHHandle handle) {
  auto hpdh = REVPDHHandles->Get(handle);
  if (hpdh == nullptr) {
    return;
  }

  HAL_CleanCAN(hpdh->hcan);

  REVPDHHandles->Free(handle);
}

int32_t HAL_GetREVPDHModuleNumber(HAL_REVPDHHandle handle, int32_t* status) {
  return hal::getHandleIndex(handle);
}

HAL_Bool HAL_CheckREVPDHModuleNumber(int32_t module) {
  return ((module >= 1) && (module < kNumREVPDHModules)) ? 1 : 0;
}

HAL_Bool HAL_CheckREVPDHChannelNumber(int32_t channel) {
  return ((channel >= 0) && (channel < kNumREVPDHChannels)) ? 1 : 0;
}

double HAL_GetREVPDHChannelCurrent(HAL_REVPDHHandle handle, int32_t channel,
                                   int32_t* status) {
  auto hpdh = REVPDHHandles->Get(handle);
  if (hpdh == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return 0;
  }

  if (!HAL_CheckREVPDHChannelNumber(channel)) {
    *status = RESOURCE_OUT_OF_RANGE;
    return 0;
  }

  // Determine what periodic status the channel is in
  if (channel < 6) {
    // Periodic status 0
    PDH_status_0_t statusFrame = HAL_ReadREVPDHStatus0(hpdh->hcan, status);
    switch (channel) {
      case 0:
        return PDH_status_0_channel_0_current_decode(
            statusFrame.channel_0_current);
      case 1:
        return PDH_status_0_channel_1_current_decode(
            statusFrame.channel_1_current);
      case 2:
        return PDH_status_0_channel_2_current_decode(
            statusFrame.channel_2_current);
      case 3:
        return PDH_status_0_channel_3_current_decode(
            statusFrame.channel_3_current);
      case 4:
        return PDH_status_0_channel_4_current_decode(
            statusFrame.channel_4_current);
      case 5:
        return PDH_status_0_channel_5_current_decode(
            statusFrame.channel_5_current);
    }
  } else if (channel < 12) {
    // Periodic status 1
    PDH_status_1_t statusFrame = HAL_ReadREVPDHStatus1(hpdh->hcan, status);
    switch (channel) {
      case 6:
        return PDH_status_1_channel_6_current_decode(
            statusFrame.channel_6_current);
      case 7:
        return PDH_status_1_channel_7_current_decode(
            statusFrame.channel_7_current);
      case 8:
        return PDH_status_1_channel_8_current_decode(
            statusFrame.channel_8_current);
      case 9:
        return PDH_status_1_channel_9_current_decode(
            statusFrame.channel_9_current);
      case 10:
        return PDH_status_1_channel_10_current_decode(
            statusFrame.channel_10_current);
      case 11:
        return PDH_status_1_channel_11_current_decode(
            statusFrame.channel_11_current);
    }
  } else if (channel < 18) {
    // Periodic status 2
    PDH_status_2_t statusFrame = HAL_ReadREVPDHStatus2(hpdh->hcan, status);
    switch (channel) {
      case 12:
        return PDH_status_2_channel_12_current_decode(
            statusFrame.channel_12_current);
      case 13:
        return PDH_status_2_channel_13_current_decode(
            statusFrame.channel_13_current);
      case 14:
        return PDH_status_2_channel_14_current_decode(
            statusFrame.channel_14_current);
      case 15:
        return PDH_status_2_channel_15_current_decode(
            statusFrame.channel_15_current);
      case 16:
        return PDH_status_2_channel_16_current_decode(
            statusFrame.channel_16_current);
      case 17:
        return PDH_status_2_channel_17_current_decode(
            statusFrame.channel_17_current);
    }
  } else if (channel < 24) {
    // Periodic status 3
    PDH_status_3_t statusFrame = HAL_ReadREVPDHStatus3(hpdh->hcan, status);
    switch (channel) {
      case 18:
        return PDH_status_3_channel_18_current_decode(
            statusFrame.channel_18_current);
      case 19:
        return PDH_status_3_channel_19_current_decode(
            statusFrame.channel_19_current);
      case 20:
        return PDH_status_3_channel_20_current_decode(
            statusFrame.channel_20_current);
      case 21:
        return PDH_status_3_channel_21_current_decode(
            statusFrame.channel_21_current);
      case 22:
        return PDH_status_3_channel_22_current_decode(
            statusFrame.channel_22_current);
      case 23:
        return PDH_status_3_channel_23_current_decode(
            statusFrame.channel_23_current);
    }
  }
  return 0;
}

void HAL_GetREVPDHAllChannelCurrents(HAL_REVPDHHandle handle, double* currents,
                                     int32_t* status) {
  auto hpdh = REVPDHHandles->Get(handle);
  if (hpdh == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }

  PDH_status_0_t statusFrame0 = HAL_ReadREVPDHStatus0(hpdh->hcan, status);
  PDH_status_1_t statusFrame1 = HAL_ReadREVPDHStatus1(hpdh->hcan, status);
  PDH_status_2_t statusFrame2 = HAL_ReadREVPDHStatus2(hpdh->hcan, status);
  PDH_status_3_t statusFrame3 = HAL_ReadREVPDHStatus3(hpdh->hcan, status);

  currents[0] =
      PDH_status_0_channel_0_current_decode(statusFrame0.channel_0_current);
  currents[1] =
      PDH_status_0_channel_1_current_decode(statusFrame0.channel_1_current);
  currents[2] =
      PDH_status_0_channel_2_current_decode(statusFrame0.channel_2_current);
  currents[3] =
      PDH_status_0_channel_3_current_decode(statusFrame0.channel_3_current);
  currents[4] =
      PDH_status_0_channel_4_current_decode(statusFrame0.channel_4_current);
  currents[5] =
      PDH_status_0_channel_5_current_decode(statusFrame0.channel_5_current);
  currents[6] =
      PDH_status_1_channel_6_current_decode(statusFrame1.channel_6_current);
  currents[7] =
      PDH_status_1_channel_7_current_decode(statusFrame1.channel_7_current);
  currents[8] =
      PDH_status_1_channel_8_current_decode(statusFrame1.channel_8_current);
  currents[9] =
      PDH_status_1_channel_9_current_decode(statusFrame1.channel_9_current);
  currents[10] =
      PDH_status_1_channel_10_current_decode(statusFrame1.channel_10_current);
  currents[11] =
      PDH_status_1_channel_11_current_decode(statusFrame1.channel_11_current);
  currents[12] =
      PDH_status_2_channel_12_current_decode(statusFrame2.channel_12_current);
  currents[13] =
      PDH_status_2_channel_13_current_decode(statusFrame2.channel_13_current);
  currents[14] =
      PDH_status_2_channel_14_current_decode(statusFrame2.channel_14_current);
  currents[15] =
      PDH_status_2_channel_15_current_decode(statusFrame2.channel_15_current);
  currents[16] =
      PDH_status_2_channel_16_current_decode(statusFrame2.channel_16_current);
  currents[17] =
      PDH_status_2_channel_17_current_decode(statusFrame2.channel_17_current);
  currents[18] =
      PDH_status_3_channel_18_current_decode(statusFrame3.channel_18_current);
  currents[19] =
      PDH_status_3_channel_19_current_decode(statusFrame3.channel_19_current);
  currents[20] =
      PDH_status_3_channel_20_current_decode(statusFrame3.channel_20_current);
  currents[21] =
      PDH_status_3_channel_21_current_decode(statusFrame3.channel_21_current);
  currents[22] =
      PDH_status_3_channel_22_current_decode(statusFrame3.channel_22_current);
  currents[23] =
      PDH_status_3_channel_23_current_decode(statusFrame3.channel_23_current);
}

uint16_t HAL_GetREVPDHTotalCurrent(HAL_REVPDHHandle handle, int32_t* status) {
  PDH_status_4_t statusFrame = HAL_GetREVPDHStatus4(handle, status);

  if (*status != 0) {
    return 0;
  }

  return PDH_status_4_total_current_decode(statusFrame.total_current);
}

void HAL_SetREVPDHSwitchableChannel(HAL_REVPDHHandle handle, HAL_Bool enabled,
                                    int32_t* status) {
  auto hpdh = REVPDHHandles->Get(handle);
  if (hpdh == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }

  uint8_t packedData[8] = {0};
  PDH_set_switch_channel_t frame;
  frame.output_set_value = enabled;
  PDH_set_switch_channel_pack(packedData, &frame,
                              PDH_SET_SWITCH_CHANNEL_LENGTH);

  HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_SET_SWITCH_CHANNEL_LENGTH,
                     PDH_SET_SWITCH_CHANNEL_FRAME_API, status);
}

HAL_Bool HAL_GetREVPDHSwitchableChannelState(HAL_REVPDHHandle handle,
                                             int32_t* status) {
  PDH_status_4_t statusFrame = HAL_GetREVPDHStatus4(handle, status);

  if (*status != 0) {
    return 0.0;
  }

  return PDH_status_4_switch_channel_state_decode(
      statusFrame.switch_channel_state);
}

double HAL_GetREVPDHVoltage(HAL_REVPDHHandle handle, int32_t* status) {
  PDH_status_4_t statusFrame = HAL_GetREVPDHStatus4(handle, status);

  if (*status != 0) {
    return 0.0;
  }

  return PDH_status_4_v_bus_decode(statusFrame.v_bus);
}

HAL_REVPDHVersion HAL_GetREVPDHVersion(HAL_REVPDHHandle handle,
                                       int32_t* status) {
  HAL_REVPDHVersion version;
  std::memset(&version, 0, sizeof(version));
  uint8_t packedData[8] = {0};
  int32_t length = 0;
  uint64_t timestamp = 0;
  PDH_version_t result = {};
  auto hpdh = REVPDHHandles->Get(handle);
  if (hpdh == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return version;
  }

  HAL_WriteCANRTRFrame(hpdh->hcan, PDH_VERSION_LENGTH, PDH_VERSION_FRAME_API,
                       status);

  if (*status != 0) {
    return version;
  }

  HAL_ReadCANPacketTimeout(hpdh->hcan, PDH_VERSION_FRAME_API, packedData,
                           &length, &timestamp, kDefaultControlPeriod * 2,
                           status);

  if (*status != 0) {
    return version;
  }

  PDH_version_unpack(&result, packedData, PDH_VERSION_LENGTH);

  version.firmwareMajor = result.firmware_year;
  version.firmwareMinor = result.firmware_minor;
  version.firmwareFix = result.firmware_fix;
  version.hardwareMinor = result.hardware_minor;
  version.hardwareMajor = result.hardware_major;
  version.uniqueId = result.unique_id;

  return version;
}

HAL_REVPDHFaults HAL_GetREVPDHFaults(HAL_REVPDHHandle handle, int32_t* status) {
  HAL_REVPDHFaults faults = {};
  auto hpdh = REVPDHHandles->Get(handle);
  if (hpdh == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return faults;
  }

  PDH_status_0_t status0 = HAL_ReadREVPDHStatus0(hpdh->hcan, status);
  PDH_status_1_t status1 = HAL_ReadREVPDHStatus1(hpdh->hcan, status);
  PDH_status_2_t status2 = HAL_ReadREVPDHStatus2(hpdh->hcan, status);
  PDH_status_3_t status3 = HAL_ReadREVPDHStatus3(hpdh->hcan, status);
  PDH_status_4_t status4 = HAL_ReadREVPDHStatus4(hpdh->hcan, status);

  faults.channel0BreakerFault = status0.channel_0_breaker_fault;
  faults.channel1BreakerFault = status0.channel_1_breaker_fault;
  faults.channel2BreakerFault = status0.channel_2_breaker_fault;
  faults.channel3BreakerFault = status0.channel_3_breaker_fault;
  faults.channel4BreakerFault = status1.channel_4_breaker_fault;
  faults.channel5BreakerFault = status1.channel_5_breaker_fault;
  faults.channel6BreakerFault = status1.channel_6_breaker_fault;
  faults.channel7BreakerFault = status1.channel_7_breaker_fault;
  faults.channel8BreakerFault = status2.channel_8_breaker_fault;
  faults.channel9BreakerFault = status2.channel_9_breaker_fault;
  faults.channel10BreakerFault = status2.channel_10_breaker_fault;
  faults.channel11BreakerFault = status2.channel_11_breaker_fault;
  faults.channel12BreakerFault = status3.channel_12_breaker_fault;
  faults.channel13BreakerFault = status3.channel_13_breaker_fault;
  faults.channel14BreakerFault = status3.channel_14_breaker_fault;
  faults.channel15BreakerFault = status3.channel_15_breaker_fault;
  faults.channel16BreakerFault = status3.channel_16_breaker_fault;
  faults.channel17BreakerFault = status3.channel_17_breaker_fault;
  faults.channel18BreakerFault = status3.channel_18_breaker_fault;
  faults.channel19BreakerFault = status3.channel_19_breaker_fault;
  faults.channel20BreakerFault = status3.channel_20_breaker_fault;
  faults.channel21BreakerFault = status3.channel_21_breaker_fault;
  faults.channel22BreakerFault = status3.channel_22_breaker_fault;
  faults.channel23BreakerFault = status3.channel_23_breaker_fault;
  faults.brownout = status4.brownout_fault;
  faults.canWarning = status4.can_warning_fault;
  faults.hardwareFault = status4.hardware_fault;

  return faults;
}

HAL_REVPDHStickyFaults HAL_GetREVPDHStickyFaults(HAL_REVPDHHandle handle,
                                                 int32_t* status) {
  HAL_REVPDHStickyFaults stickyFaults = {};
  auto hpdh = REVPDHHandles->Get(handle);
  if (hpdh == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return stickyFaults;
  }

  PDH_status_4_t status4 = HAL_ReadREVPDHStatus4(hpdh->hcan, status);

  stickyFaults.channel0BreakerFault = status4.sticky_ch0_breaker_fault;
  stickyFaults.channel1BreakerFault = status4.sticky_ch1_breaker_fault;
  stickyFaults.channel2BreakerFault = status4.sticky_ch2_breaker_fault;
  stickyFaults.channel3BreakerFault = status4.sticky_ch3_breaker_fault;
  stickyFaults.channel4BreakerFault = status4.sticky_ch4_breaker_fault;
  stickyFaults.channel5BreakerFault = status4.sticky_ch5_breaker_fault;
  stickyFaults.channel6BreakerFault = status4.sticky_ch6_breaker_fault;
  stickyFaults.channel7BreakerFault = status4.sticky_ch7_breaker_fault;
  stickyFaults.channel8BreakerFault = status4.sticky_ch8_breaker_fault;
  stickyFaults.channel9BreakerFault = status4.sticky_ch9_breaker_fault;
  stickyFaults.channel10BreakerFault = status4.sticky_ch10_breaker_fault;
  stickyFaults.channel11BreakerFault = status4.sticky_ch11_breaker_fault;
  stickyFaults.channel12BreakerFault = status4.sticky_ch12_breaker_fault;
  stickyFaults.channel13BreakerFault = status4.sticky_ch13_breaker_fault;
  stickyFaults.channel14BreakerFault = status4.sticky_ch14_breaker_fault;
  stickyFaults.channel15BreakerFault = status4.sticky_ch15_breaker_fault;
  stickyFaults.channel16BreakerFault = status4.sticky_ch16_breaker_fault;
  stickyFaults.channel17BreakerFault = status4.sticky_ch17_breaker_fault;
  stickyFaults.channel18BreakerFault = status4.sticky_ch18_breaker_fault;
  stickyFaults.channel19BreakerFault = status4.sticky_ch19_breaker_fault;
  stickyFaults.channel20BreakerFault = status4.sticky_ch20_breaker_fault;
  stickyFaults.channel21BreakerFault = status4.sticky_ch21_breaker_fault;
  stickyFaults.channel22BreakerFault = status4.sticky_ch22_breaker_fault;
  stickyFaults.channel23BreakerFault = status4.sticky_ch23_breaker_fault;
  stickyFaults.brownout = status4.sticky_brownout_fault;
  stickyFaults.canWarning = status4.sticky_can_warning_fault;
  stickyFaults.canBusOff = status4.sticky_can_bus_off_fault;
  stickyFaults.hasReset = status4.sticky_has_reset_fault;

  return stickyFaults;
}

void HAL_ClearREVPDHStickyFaults(HAL_REVPDHHandle handle, int32_t* status) {
  auto hpdh = REVPDHHandles->Get(handle);
  if (hpdh == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }

  uint8_t packedData[8] = {0};
  HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_CLEAR_FAULTS_LENGTH,
                     PDH_CLEAR_FAULTS_FRAME_API, status);
}

}  // extern "C"