/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include "frc/controller/ProfiledPIDController.h"

#include <algorithm>
#include <cmath>

#include "frc/smartdashboard/SendableBuilder.h"

using namespace frc;

ProfiledPIDController::ProfiledPIDController(
    double Kp, double Ki, double Kd,
    frc::TrapezoidProfile::Constraints constraints, units::second_t period)
    : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}

void ProfiledPIDController::SetP(double Kp) { m_controller.SetP(Kp); }

void ProfiledPIDController::SetI(double Ki) { m_controller.SetI(Ki); }

void ProfiledPIDController::SetD(double Kd) { m_controller.SetD(Kd); }

double ProfiledPIDController::GetP() const { return m_controller.GetP(); }

double ProfiledPIDController::GetI() const { return m_controller.GetI(); }

double ProfiledPIDController::GetD() const { return m_controller.GetD(); }

units::second_t ProfiledPIDController::GetPeriod() const {
  return m_controller.GetPeriod();
}

void ProfiledPIDController::SetGoal(units::meter_t goal) {
  m_goal = {goal, 0_mps};
}

units::meter_t ProfiledPIDController::GetGoal() const {
  return m_goal.position;
}

bool ProfiledPIDController::AtGoal(
    double positionTolerance, double velocityTolerance,
    frc2::PIDController::Tolerance toleranceType) const {
  return AtSetpoint(positionTolerance, velocityTolerance, toleranceType) &&
         m_goal == m_setpoint;
}

bool ProfiledPIDController::AtGoal() const {
  return AtSetpoint() && m_goal == m_setpoint;
}

void ProfiledPIDController::SetConstraints(
    frc::TrapezoidProfile::Constraints constraints) {
  m_constraints = constraints;
}

double ProfiledPIDController::GetSetpoint() const {
  return m_controller.GetSetpoint();
}

bool ProfiledPIDController::AtSetpoint(
    double positionTolerance, double velocityTolerance,
    frc2::PIDController::Tolerance toleranceType) const {
  return m_controller.AtSetpoint(positionTolerance, velocityTolerance,
                                 toleranceType);
}

bool ProfiledPIDController::AtSetpoint() const {
  return m_controller.AtSetpoint();
}

void ProfiledPIDController::SetInputRange(double minimumInput,
                                          double maximumInput) {
  m_controller.SetInputRange(minimumInput, maximumInput);
}

void ProfiledPIDController::EnableContinuousInput(double minimumInput,
                                                  double maximumInput) {
  m_controller.EnableContinuousInput(minimumInput, maximumInput);
}

void ProfiledPIDController::DisableContinuousInput() {
  m_controller.DisableContinuousInput();
}

void ProfiledPIDController::SetOutputRange(double minimumOutput,
                                           double maximumOutput) {
  m_controller.SetOutputRange(minimumOutput, maximumOutput);
}

void ProfiledPIDController::SetAbsoluteTolerance(double positionTolerance,
                                                 double velocityTolerance) {
  m_controller.SetAbsoluteTolerance(positionTolerance, velocityTolerance);
}

void ProfiledPIDController::SetPercentTolerance(double positionTolerance,
                                                double velocityTolerance) {
  m_controller.SetPercentTolerance(positionTolerance, velocityTolerance);
}

double ProfiledPIDController::GetPositionError() const {
  return m_controller.GetPositionError();
}

double ProfiledPIDController::GetVelocityError() const {
  return m_controller.GetVelocityError();
}

double ProfiledPIDController::Calculate(double measurement) {
  frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
  m_setpoint = profile.Calculate(GetPeriod());
  return m_controller.Calculate(measurement, m_setpoint.position.to<double>());
}

double ProfiledPIDController::Calculate(double measurement,
                                        units::meter_t goal) {
  SetGoal(goal);
  return Calculate(measurement);
}

double ProfiledPIDController::Calculate(
    double measurement, units::meter_t goal,
    frc::TrapezoidProfile::Constraints constraints) {
  SetConstraints(constraints);
  return Calculate(measurement, goal);
}

void ProfiledPIDController::Reset() { m_controller.Reset(); }

void ProfiledPIDController::InitSendable(frc::SendableBuilder& builder) {
  builder.SetSmartDashboardType("ProfiledPIDController");
  builder.AddDoubleProperty("p", [this] { return GetP(); },
                            [this](double value) { SetP(value); });
  builder.AddDoubleProperty("i", [this] { return GetI(); },
                            [this](double value) { SetI(value); });
  builder.AddDoubleProperty("d", [this] { return GetD(); },
                            [this](double value) { SetD(value); });
  builder.AddDoubleProperty(
      "goal", [this] { return GetGoal().to<double>(); },
      [this](double value) { SetGoal(units::meter_t{value}); });
}