// 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 "CTREPDP.h"

#include <string>

#include <fmt/format.h>
#include <wpi/mutex.h>

#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/CAN.h"
#include "hal/CANAPI.h"
#include "hal/Errors.h"
#include "hal/handles/IndexedHandleResource.h"

using namespace hal;

static constexpr HAL_CANManufacturer manufacturer =
    HAL_CANManufacturer::HAL_CAN_Man_kCTRE;

static constexpr HAL_CANDeviceType deviceType =
    HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;

static constexpr int32_t Status1 = 0x50;
static constexpr int32_t Status2 = 0x51;
static constexpr int32_t Status3 = 0x52;
static constexpr int32_t StatusEnergy = 0x5D;

static constexpr int32_t Control1 = 0x70;

static constexpr int32_t TimeoutMs = 100;

/* encoder/decoders */
union PdpStatus1 {
  uint8_t data[8];
  struct Bits {
    unsigned chan1_h8 : 8;
    unsigned chan2_h6 : 6;
    unsigned chan1_l2 : 2;
    unsigned chan3_h4 : 4;
    unsigned chan2_l4 : 4;
    unsigned chan4_h2 : 2;
    unsigned chan3_l6 : 6;
    unsigned chan4_l8 : 8;
    unsigned chan5_h8 : 8;
    unsigned chan6_h6 : 6;
    unsigned chan5_l2 : 2;
    unsigned reserved4 : 4;
    unsigned chan6_l4 : 4;
  } bits;
};

union PdpStatus2 {
  uint8_t data[8];
  struct Bits {
    unsigned chan7_h8 : 8;
    unsigned chan8_h6 : 6;
    unsigned chan7_l2 : 2;
    unsigned chan9_h4 : 4;
    unsigned chan8_l4 : 4;
    unsigned chan10_h2 : 2;
    unsigned chan9_l6 : 6;
    unsigned chan10_l8 : 8;
    unsigned chan11_h8 : 8;
    unsigned chan12_h6 : 6;
    unsigned chan11_l2 : 2;
    unsigned reserved4 : 4;
    unsigned chan12_l4 : 4;
  } bits;
};

union PdpStatus3 {
  uint8_t data[8];
  struct Bits {
    unsigned chan13_h8 : 8;
    unsigned chan14_h6 : 6;
    unsigned chan13_l2 : 2;
    unsigned chan15_h4 : 4;
    unsigned chan14_l4 : 4;
    unsigned chan16_h2 : 2;
    unsigned chan15_l6 : 6;
    unsigned chan16_l8 : 8;
    unsigned internalResBattery_mOhms : 8;
    unsigned busVoltage : 8;
    unsigned temp : 8;
  } bits;
};

union PdpStatusEnergy {
  uint8_t data[8];
  struct Bits {
    unsigned TmeasMs_likelywillbe20ms_ : 8;
    unsigned TotalCurrent_125mAperunit_h8 : 8;
    unsigned Power_125mWperunit_h4 : 4;
    unsigned TotalCurrent_125mAperunit_l4 : 4;
    unsigned Power_125mWperunit_m8 : 8;
    unsigned Energy_125mWPerUnitXTmeas_h4 : 4;
    unsigned Power_125mWperunit_l4 : 4;
    unsigned Energy_125mWPerUnitXTmeas_mh8 : 8;
    unsigned Energy_125mWPerUnitXTmeas_ml8 : 8;
    unsigned Energy_125mWPerUnitXTmeas_l8 : 8;
  } bits;
};

namespace {
struct PDP {
  HAL_CANHandle canHandle;
  std::string previousAllocation;
  bool streamHandleAllocated{false};
  uint32_t streamSessionHandles[3];
};
}  // namespace

static IndexedHandleResource<HAL_PDPHandle, PDP, kNumCTREPDPModules,
                             HAL_HandleEnum::CTREPDP>* pdpHandles;

namespace hal::init {
void InitializeCTREPDP() {
  static IndexedHandleResource<HAL_PDPHandle, PDP, kNumCTREPDPModules,
                               HAL_HandleEnum::CTREPDP>
      pH;
  pdpHandles = &pH;
}
}  // namespace hal::init

extern "C" {

HAL_PDPHandle HAL_InitializePDP(int32_t busId, int32_t module,
                                const char* allocationLocation,
                                int32_t* status) {
  hal::init::CheckInit();
  if (!HAL_CheckPDPModule(module)) {
    *status = RESOURCE_OUT_OF_RANGE;
    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PDP", 0,
                                     kNumCTREPDPModules - 1, module);
    return HAL_kInvalidHandle;
  }

  HAL_PDPHandle handle;
  auto pdp = pdpHandles->Allocate(module, &handle, status);

  if (*status != 0) {
    if (pdp) {
      hal::SetLastErrorPreviouslyAllocated(status, "CTRE PDP", module,
                                           pdp->previousAllocation);
    } else {
      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PDP", 0,
                                       kNumCTREPDPModules - 1, module);
    }
    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
  }

  pdp->canHandle =
      HAL_InitializeCAN(busId, manufacturer, module, deviceType, status);
  if (*status != 0) {
    pdpHandles->Free(handle);
    return HAL_kInvalidHandle;
  }

  pdp->previousAllocation = allocationLocation ? allocationLocation : "";

  return handle;
}

void HAL_CleanPDP(HAL_PDPHandle handle) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp) {
    HAL_CleanCAN(pdp->canHandle);
  }
  pdpHandles->Free(handle);
}

int32_t HAL_GetPDPModuleNumber(HAL_PDPHandle handle, int32_t* status) {
  return hal::getHandleIndex(handle);
}

HAL_Bool HAL_CheckPDPModule(int32_t module) {
  return module < kNumCTREPDPModules && module >= 0;
}

HAL_Bool HAL_CheckPDPChannel(int32_t channel) {
  return channel < kNumCTREPDPChannels && channel >= 0;
}

double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return 0;
  }

  HAL_CANReceiveMessage message;
  PdpStatus3 pdpStatus;

  HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, &message, TimeoutMs,
                           status);

  if (*status != 0) {
    return 0;
  } else {
    std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
    return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
  }
}

double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return 0;
  }

  HAL_CANReceiveMessage message;
  PdpStatus3 pdpStatus;

  HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, &message, TimeoutMs,
                           status);

  if (*status != 0) {
    return 0;
  } else {
    std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
    return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
  }
}

double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
                                int32_t* status) {
  if (!HAL_CheckPDPChannel(channel)) {
    *status = PARAMETER_OUT_OF_RANGE;
    hal::SetLastError(status, fmt::format("Invalid pdp channel {}", channel));
    return 0;
  }

  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return 0;
  }

  HAL_CANReceiveMessage message;

  double raw = 0;

  if (channel <= 5) {
    PdpStatus1 pdpStatus;
    HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, &message, TimeoutMs,
                             status);
    if (*status != 0) {
      return 0;
    }
    std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
    switch (channel) {
      case 0:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
              pdpStatus.bits.chan1_l2;
        break;
      case 1:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
              pdpStatus.bits.chan2_l4;
        break;
      case 2:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
              pdpStatus.bits.chan3_l6;
        break;
      case 3:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
              pdpStatus.bits.chan4_l8;
        break;
      case 4:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
              pdpStatus.bits.chan5_l2;
        break;
      case 5:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
              pdpStatus.bits.chan6_l4;
        break;
    }
  } else if (channel <= 11) {
    PdpStatus2 pdpStatus;
    HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, &message, TimeoutMs,
                             status);
    if (*status != 0) {
      return 0;
    }
    std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
    switch (channel) {
      case 6:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
              pdpStatus.bits.chan7_l2;
        break;
      case 7:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan8_h6) << 4) |
              pdpStatus.bits.chan8_l4;
        break;
      case 8:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan9_h4) << 6) |
              pdpStatus.bits.chan9_l6;
        break;
      case 9:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan10_h2) << 8) |
              pdpStatus.bits.chan10_l8;
        break;
      case 10:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan11_h8) << 2) |
              pdpStatus.bits.chan11_l2;
        break;
      case 11:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan12_h6) << 4) |
              pdpStatus.bits.chan12_l4;
        break;
    }
  } else {
    PdpStatus3 pdpStatus;
    HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, &message, TimeoutMs,
                             status);
    if (*status != 0) {
      return 0;
    }
    std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
    switch (channel) {
      case 12:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
              pdpStatus.bits.chan13_l2;
        break;
      case 13:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan14_h6) << 4) |
              pdpStatus.bits.chan14_l4;
        break;
      case 14:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan15_h4) << 6) |
              pdpStatus.bits.chan15_l6;
        break;
      case 15:
        raw = (static_cast<uint32_t>(pdpStatus.bits.chan16_h2) << 8) |
              pdpStatus.bits.chan16_l8;
        break;
    }
  }

  /* convert to amps */
  return raw * 0.125; /* 7.3 fixed pt value in Amps */
}

void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
                                  int32_t* status) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }

  HAL_CANReceiveMessage message;
  PdpStatus1 pdpStatus;
  HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, &message, TimeoutMs,
                           status);
  if (*status != 0) {
    return;
  }
  std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
  PdpStatus2 pdpStatus2;
  HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, &message, TimeoutMs,
                           status);
  if (*status != 0) {
    return;
  }
  std::memcpy(pdpStatus2.data, message.message.data, sizeof(pdpStatus2.data));
  PdpStatus3 pdpStatus3;
  HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, &message, TimeoutMs,
                           status);
  if (*status != 0) {
    return;
  }
  std::memcpy(pdpStatus3.data, message.message.data, sizeof(pdpStatus3.data));

  currents[0] = ((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
                 pdpStatus.bits.chan1_l2) *
                0.125;
  currents[1] = ((static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
                 pdpStatus.bits.chan2_l4) *
                0.125;
  currents[2] = ((static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
                 pdpStatus.bits.chan3_l6) *
                0.125;
  currents[3] = ((static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
                 pdpStatus.bits.chan4_l8) *
                0.125;
  currents[4] = ((static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
                 pdpStatus.bits.chan5_l2) *
                0.125;
  currents[5] = ((static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
                 pdpStatus.bits.chan6_l4) *
                0.125;

  currents[6] = ((static_cast<uint32_t>(pdpStatus2.bits.chan7_h8) << 2) |
                 pdpStatus2.bits.chan7_l2) *
                0.125;
  currents[7] = ((static_cast<uint32_t>(pdpStatus2.bits.chan8_h6) << 4) |
                 pdpStatus2.bits.chan8_l4) *
                0.125;
  currents[8] = ((static_cast<uint32_t>(pdpStatus2.bits.chan9_h4) << 6) |
                 pdpStatus2.bits.chan9_l6) *
                0.125;
  currents[9] = ((static_cast<uint32_t>(pdpStatus2.bits.chan10_h2) << 8) |
                 pdpStatus2.bits.chan10_l8) *
                0.125;
  currents[10] = ((static_cast<uint32_t>(pdpStatus2.bits.chan11_h8) << 2) |
                  pdpStatus2.bits.chan11_l2) *
                 0.125;
  currents[11] = ((static_cast<uint32_t>(pdpStatus2.bits.chan12_h6) << 4) |
                  pdpStatus2.bits.chan12_l4) *
                 0.125;

  currents[12] = ((static_cast<uint32_t>(pdpStatus3.bits.chan13_h8) << 2) |
                  pdpStatus3.bits.chan13_l2) *
                 0.125;
  currents[13] = ((static_cast<uint32_t>(pdpStatus3.bits.chan14_h6) << 4) |
                  pdpStatus3.bits.chan14_l4) *
                 0.125;
  currents[14] = ((static_cast<uint32_t>(pdpStatus3.bits.chan15_h4) << 6) |
                  pdpStatus3.bits.chan15_l6) *
                 0.125;
  currents[15] = ((static_cast<uint32_t>(pdpStatus3.bits.chan16_h2) << 8) |
                  pdpStatus3.bits.chan16_l8) *
                 0.125;
}

double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return 0;
  }

  HAL_CANReceiveMessage message;
  PdpStatusEnergy pdpStatus;

  HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, &message, TimeoutMs,
                           status);
  if (*status != 0) {
    return 0;
  }
  std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));

  uint32_t raw;
  raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8;
  raw <<= 4;
  raw |= pdpStatus.bits.TotalCurrent_125mAperunit_l4;
  return 0.125 * raw; /* 7.3 fixed pt value in Amps */
}

double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return 0;
  }

  HAL_CANReceiveMessage message;
  PdpStatusEnergy pdpStatus;

  HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, &message, TimeoutMs,
                           status);
  if (*status != 0) {
    return 0;
  }
  std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));

  uint32_t raw;
  raw = pdpStatus.bits.Power_125mWperunit_h4;
  raw <<= 8;
  raw |= pdpStatus.bits.Power_125mWperunit_m8;
  raw <<= 4;
  raw |= pdpStatus.bits.Power_125mWperunit_l4;
  return 0.125 * raw; /* 7.3 fixed pt value in Watts */
}

double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return 0;
  }

  HAL_CANReceiveMessage message;
  PdpStatusEnergy pdpStatus;

  HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, &message, TimeoutMs,
                           status);
  if (*status != 0) {
    return 0;
  }
  std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));

  uint32_t raw;
  raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4;
  raw <<= 8;
  raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_mh8;
  raw <<= 8;
  raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_ml8;
  raw <<= 8;
  raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_l8;

  double energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
  energyJoules *= 0.001;             /* convert from mW to W */
  energyJoules *=
      pdpStatus.bits
          .TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
  return energyJoules;
}

void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }

  HAL_CANMessage message;
  std::memset(&message, 0, sizeof(message));
  message.dataSize = 1;
  message.data[0] = 0x40; /* only bit set is ResetEnergy */

  HAL_WriteCANPacket(pdp->canHandle, Control1, &message, status);
}

void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }

  HAL_CANMessage message;
  std::memset(&message, 0, sizeof(message));
  message.dataSize = 1;
  message.data[0] = 0x80; /* only bit set is ClearStickyFaults */

  HAL_WriteCANPacket(pdp->canHandle, Control1, &message, status);
}

uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth,
                            int32_t* status);

void HAL_StartPDPStream(HAL_PDPHandle handle, int32_t* status) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }

  if (pdp->streamHandleAllocated) {
    *status = RESOURCE_IS_ALLOCATED;
    return;
  }

  pdp->streamSessionHandles[0] =
      HAL_StartCANStream(pdp->canHandle, Status1, 50, status);
  if (*status != 0) {
    return;
  }
  pdp->streamSessionHandles[1] =
      HAL_StartCANStream(pdp->canHandle, Status2, 50, status);
  if (*status != 0) {
    HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]);
    return;
  }
  pdp->streamSessionHandles[2] =
      HAL_StartCANStream(pdp->canHandle, Status3, 50, status);
  if (*status != 0) {
    HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]);
    HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[1]);
    return;
  }
  pdp->streamHandleAllocated = true;
}

HAL_PowerDistributionChannelData* HAL_GetPDPStreamData(HAL_PDPHandle handle,
                                                       int32_t* count,
                                                       int32_t* status) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return nullptr;
  }

  if (!pdp->streamHandleAllocated) {
    *status = RESOURCE_OUT_OF_RANGE;
    return nullptr;
  }

  *count = 0;
  // 3 streams, 6 channels per stream, 50 depth per stream
  HAL_PowerDistributionChannelData* retData =
      new HAL_PowerDistributionChannelData[3 * 6 * 50];

  HAL_CANStreamMessage messages[50];
  uint32_t messagesRead = 0;
  HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[0], messages, 50,
                            &messagesRead, status);
  if (*status < 0) {
    goto Exit;
  }

  for (uint32_t i = 0; i < messagesRead; i++) {
    PdpStatus1 pdpStatus;
    std::memcpy(pdpStatus.data, messages[i].message.message.data,
                sizeof(pdpStatus));
    uint64_t timestamp = messages[i].message.timeStamp;

    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
         pdpStatus.bits.chan1_l2) *
        0.125;
    retData[*count].channel = 1;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
         pdpStatus.bits.chan2_l4) *
        0.125;
    retData[*count].channel = 2;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
         pdpStatus.bits.chan3_l6) *
        0.125;
    retData[*count].channel = 3;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
         pdpStatus.bits.chan4_l8) *
        0.125;
    retData[*count].channel = 4;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
         pdpStatus.bits.chan5_l2) *
        0.125;
    retData[*count].channel = 5;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
         pdpStatus.bits.chan6_l4) *
        0.125;
    retData[*count].channel = 6;
    retData[*count].timestamp = timestamp;
    (*count)++;
  }

  messagesRead = 0;
  HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[1], messages, 50,
                            &messagesRead, status);
  if (*status < 0) {
    goto Exit;
  }

  for (uint32_t i = 0; i < messagesRead; i++) {
    PdpStatus2 pdpStatus;
    std::memcpy(pdpStatus.data, messages[i].message.message.data,
                sizeof(pdpStatus));
    uint64_t timestamp = messages[i].message.timeStamp;

    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
         pdpStatus.bits.chan7_l2) *
        0.125;
    retData[*count].channel = 7;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan8_h6) << 4) |
         pdpStatus.bits.chan8_l4) *
        0.125;
    retData[*count].channel = 8;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan9_h4) << 6) |
         pdpStatus.bits.chan9_l6) *
        0.125;
    retData[*count].channel = 9;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan10_h2) << 8) |
         pdpStatus.bits.chan10_l8) *
        0.125;
    retData[*count].channel = 10;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan11_h8) << 2) |
         pdpStatus.bits.chan11_l2) *
        0.125;
    retData[*count].channel = 11;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan12_h6) << 4) |
         pdpStatus.bits.chan12_l4) *
        0.125;
    retData[*count].channel = 12;
    retData[*count].timestamp = timestamp;
    (*count)++;
  }

  messagesRead = 0;
  HAL_CAN_ReadStreamSession(pdp->streamSessionHandles[2], messages, 50,
                            &messagesRead, status);
  if (*status < 0) {
    goto Exit;
  }

  for (uint32_t i = 0; i < messagesRead; i++) {
    PdpStatus3 pdpStatus;
    std::memcpy(pdpStatus.data, messages[i].message.message.data,
                sizeof(pdpStatus));
    uint64_t timestamp = messages[i].message.timeStamp;

    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
         pdpStatus.bits.chan13_l2) *
        0.125;
    retData[*count].channel = 13;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan14_h6) << 4) |
         pdpStatus.bits.chan14_l4) *
        0.125;
    retData[*count].channel = 14;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan15_h4) << 6) |
         pdpStatus.bits.chan15_l6) *
        0.125;
    retData[*count].channel = 15;
    retData[*count].timestamp = timestamp;
    (*count)++;
    retData[*count].current =
        ((static_cast<uint32_t>(pdpStatus.bits.chan16_h2) << 8) |
         pdpStatus.bits.chan16_l8) *
        0.125;
    retData[*count].channel = 16;
    retData[*count].timestamp = timestamp;
    (*count)++;
  }

Exit:
  if (*status < 0) {
    delete[] retData;
    retData = nullptr;
  }
  return retData;
}

void HAL_StopPDPStream(HAL_PDPHandle handle, int32_t* status) {
  auto pdp = pdpHandles->Get(handle);
  if (pdp == nullptr) {
    *status = HAL_HANDLE_ERROR;
    return;
  }

  if (!pdp->streamHandleAllocated) {
    *status = RESOURCE_OUT_OF_RANGE;
    return;
  }

  HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[0]);
  HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[1]);
  HAL_CAN_CloseStreamSession(pdp->streamSessionHandles[2]);

  pdp->streamHandleAllocated = false;
}

}  // extern "C"