// 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 "wpi/hardware/discrete/PWM.hpp"

#include "wpi/hal/PWM.h"
#include "wpi/hal/UsageReporting.hpp"
#include "wpi/system/Errors.hpp"
#include "wpi/util/SensorUtil.hpp"
#include "wpi/util/StackTrace.hpp"
#include "wpi/util/sendable/SendableBuilder.hpp"
#include "wpi/util/sendable/SendableRegistry.hpp"

using namespace wpi;

PWM::PWM(int channel, bool registerSendable) {
  auto stack = wpi::util::GetStackTrace(1);
  int32_t status = 0;
  m_handle = HAL_InitializePWMPort(channel, stack.c_str(), &status);
  WPILIB_CheckErrorStatus(status, "Channel {}", channel);

  m_channel = channel;

  SetDisabled();

  HAL_ReportUsage("IO", channel, "PWM");
  if (registerSendable) {
    wpi::util::SendableRegistry::Add(this, "PWM", channel);
  }
}

PWM::~PWM() {
  if (m_handle != HAL_INVALID_HANDLE) {
    SetDisabled();
  }
}

void PWM::SetPulseTime(wpi::units::microsecond_t time) {
  int32_t status = 0;
  HAL_SetPWMPulseTimeMicroseconds(m_handle, time.value(), &status);
  WPILIB_CheckErrorStatus(status, "Channel {}", m_channel);
}

wpi::units::microsecond_t PWM::GetPulseTime() const {
  int32_t status = 0;
  double value = HAL_GetPWMPulseTimeMicroseconds(m_handle, &status);
  WPILIB_CheckErrorStatus(status, "Channel {}", m_channel);

  return wpi::units::microsecond_t{value};
}

void PWM::SetDisabled() {
  int32_t status = 0;
  HAL_SetPWMPulseTimeMicroseconds(m_handle, 0, &status);
  WPILIB_CheckErrorStatus(status, "Channel {}", m_channel);
}

void PWM::SetOutputPeriod(wpi::units::millisecond_t period) {
  int32_t status = 0;

  switch (static_cast<int>(period.value())) {
    case 5:
      // Don't squelch any outputs
      HAL_SetPWMOutputPeriod(m_handle, 0, &status);
      break;
    case 10:
      // Squelch 1 out of 2 outputs
      HAL_SetPWMOutputPeriod(m_handle, 1, &status);
      break;
    case 20:
    default:  // default to 20ms if invalid value is given
      // Squelch 3 out of 4 outputs
      HAL_SetPWMOutputPeriod(m_handle, 3, &status);
      break;
  }

  WPILIB_CheckErrorStatus(status, "Channel {}", m_channel);
}

int PWM::GetChannel() const {
  return m_channel;
}

void PWM::SetSimDevice(HAL_SimDeviceHandle device) {
  HAL_SetPWMSimDevice(m_handle, device);
}

void PWM::InitSendable(wpi::util::SendableBuilder& builder) {
  builder.SetSmartDashboardType("PWM");
  builder.SetActuator(true);
  builder.AddDoubleProperty(
      "Value", [=, this] { return GetPulseTime().value(); },
      [=, this](double value) {
        SetPulseTime(wpi::units::microsecond_t{value});
      });
}
