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

#include <cstddef>

#include <hal/CAN.h>
#include <hal/HALBase.h>
#include <hal/Power.h>

#include "frc/Errors.h"

using namespace frc;

int RobotController::GetFPGAVersion() {
  int32_t status = 0;
  int version = HAL_GetFPGAVersion(&status);
  FRC_CheckErrorStatus(status, "GetFPGAVersion");
  return version;
}

int64_t RobotController::GetFPGARevision() {
  int32_t status = 0;
  int64_t revision = HAL_GetFPGARevision(&status);
  FRC_CheckErrorStatus(status, "GetFPGARevision");
  return revision;
}

std::string RobotController::GetSerialNumber() {
  // Serial number is 8 characters
  char serialNum[9];
  size_t len = HAL_GetSerialNumber(serialNum, sizeof(serialNum));
  return std::string(serialNum, len);
}

std::string RobotController::GetComments() {
  char comments[65];
  size_t len = HAL_GetComments(comments, sizeof(comments));
  return std::string(comments, len);
}

uint64_t RobotController::GetFPGATime() {
  int32_t status = 0;
  uint64_t time = HAL_GetFPGATime(&status);
  FRC_CheckErrorStatus(status, "GetFPGATime");
  return time;
}

bool RobotController::GetUserButton() {
  int32_t status = 0;
  bool value = HAL_GetFPGAButton(&status);
  FRC_CheckErrorStatus(status, "GetUserButton");
  return value;
}

units::volt_t RobotController::GetBatteryVoltage() {
  int32_t status = 0;
  double retVal = HAL_GetVinVoltage(&status);
  FRC_CheckErrorStatus(status, "GetBatteryVoltage");
  return units::volt_t{retVal};
}

bool RobotController::IsSysActive() {
  int32_t status = 0;
  bool retVal = HAL_GetSystemActive(&status);
  FRC_CheckErrorStatus(status, "IsSysActive");
  return retVal;
}

bool RobotController::IsBrownedOut() {
  int32_t status = 0;
  bool retVal = HAL_GetBrownedOut(&status);
  FRC_CheckErrorStatus(status, "IsBrownedOut");
  return retVal;
}

bool RobotController::GetRSLState() {
  int32_t status = 0;
  bool retVal = HAL_GetRSLState(&status);
  FRC_CheckErrorStatus(status, "GetRSLState");
  return retVal;
}

double RobotController::GetInputVoltage() {
  int32_t status = 0;
  double retVal = HAL_GetVinVoltage(&status);
  FRC_CheckErrorStatus(status, "GetInputVoltage");
  return retVal;
}

double RobotController::GetInputCurrent() {
  int32_t status = 0;
  double retVal = HAL_GetVinCurrent(&status);
  FRC_CheckErrorStatus(status, "GetInputCurrent");
  return retVal;
}

double RobotController::GetVoltage3V3() {
  int32_t status = 0;
  double retVal = HAL_GetUserVoltage3V3(&status);
  FRC_CheckErrorStatus(status, "GetVoltage3V3");
  return retVal;
}

double RobotController::GetCurrent3V3() {
  int32_t status = 0;
  double retVal = HAL_GetUserCurrent3V3(&status);
  FRC_CheckErrorStatus(status, "GetCurrent3V3");
  return retVal;
}

void RobotController::SetEnabled3V3(bool enabled) {
  int32_t status = 0;
  HAL_SetUserRailEnabled3V3(enabled, &status);
  FRC_CheckErrorStatus(status, "SetEnabled3V3");
}

bool RobotController::GetEnabled3V3() {
  int32_t status = 0;
  bool retVal = HAL_GetUserActive3V3(&status);
  FRC_CheckErrorStatus(status, "GetEnabled3V3");
  return retVal;
}

int RobotController::GetFaultCount3V3() {
  int32_t status = 0;
  int retVal = HAL_GetUserCurrentFaults3V3(&status);
  FRC_CheckErrorStatus(status, "GetFaultCount3V3");
  return retVal;
}

double RobotController::GetVoltage5V() {
  int32_t status = 0;
  double retVal = HAL_GetUserVoltage5V(&status);
  FRC_CheckErrorStatus(status, "GetVoltage5V");
  return retVal;
}

double RobotController::GetCurrent5V() {
  int32_t status = 0;
  double retVal = HAL_GetUserCurrent5V(&status);
  FRC_CheckErrorStatus(status, "GetCurrent5V");
  return retVal;
}

void RobotController::SetEnabled5V(bool enabled) {
  int32_t status = 0;
  HAL_SetUserRailEnabled5V(enabled, &status);
  FRC_CheckErrorStatus(status, "SetEnabled5V");
}

bool RobotController::GetEnabled5V() {
  int32_t status = 0;
  bool retVal = HAL_GetUserActive5V(&status);
  FRC_CheckErrorStatus(status, "GetEnabled5V");
  return retVal;
}

int RobotController::GetFaultCount5V() {
  int32_t status = 0;
  int retVal = HAL_GetUserCurrentFaults5V(&status);
  FRC_CheckErrorStatus(status, "GetFaultCount5V");
  return retVal;
}

double RobotController::GetVoltage6V() {
  int32_t status = 0;
  double retVal = HAL_GetUserVoltage6V(&status);
  FRC_CheckErrorStatus(status, "GetVoltage6V");
  return retVal;
}

double RobotController::GetCurrent6V() {
  int32_t status = 0;
  double retVal = HAL_GetUserCurrent6V(&status);
  FRC_CheckErrorStatus(status, "GetCurrent6V");
  return retVal;
}

void RobotController::SetEnabled6V(bool enabled) {
  int32_t status = 0;
  HAL_SetUserRailEnabled6V(enabled, &status);
  FRC_CheckErrorStatus(status, "SetEnabled6V");
}

bool RobotController::GetEnabled6V() {
  int32_t status = 0;
  bool retVal = HAL_GetUserActive6V(&status);
  FRC_CheckErrorStatus(status, "GetEnabled6V");
  return retVal;
}

int RobotController::GetFaultCount6V() {
  int32_t status = 0;
  int retVal = HAL_GetUserCurrentFaults6V(&status);
  FRC_CheckErrorStatus(status, "GetFaultCount6V");
  return retVal;
}

units::volt_t RobotController::GetBrownoutVoltage() {
  int32_t status = 0;
  double retVal = HAL_GetBrownoutVoltage(&status);
  FRC_CheckErrorStatus(status, "GetBrownoutVoltage");
  return units::volt_t{retVal};
}

void RobotController::SetBrownoutVoltage(units::volt_t brownoutVoltage) {
  int32_t status = 0;
  HAL_SetBrownoutVoltage(brownoutVoltage.value(), &status);
  FRC_CheckErrorStatus(status, "SetBrownoutVoltage");
}

units::celsius_t RobotController::GetCPUTemp() {
  int32_t status = 0;
  double retVal = HAL_GetCPUTemp(&status);
  FRC_CheckErrorStatus(status, "GetCPUTemp");
  return units::celsius_t{retVal};
}

CANStatus RobotController::GetCANStatus() {
  int32_t status = 0;
  float percentBusUtilization = 0;
  uint32_t busOffCount = 0;
  uint32_t txFullCount = 0;
  uint32_t receiveErrorCount = 0;
  uint32_t transmitErrorCount = 0;
  HAL_CAN_GetCANStatus(&percentBusUtilization, &busOffCount, &txFullCount,
                       &receiveErrorCount, &transmitErrorCount, &status);
  FRC_CheckErrorStatus(status, "GetCANStatus");
  return {percentBusUtilization, static_cast<int>(busOffCount),
          static_cast<int>(txFullCount), static_cast<int>(receiveErrorCount),
          static_cast<int>(transmitErrorCount)};
}