#pragma once
#include <Platform_Controller.hpp>
#include "ControlLoop/AH_PID.hpp"
#include <cmath>

/**
 * @brief This class implements a supervised velocity controller for the CPF.
 *
 * The supervised PV controller adjust the velocity setpoint according to a table
 * based on distance to the desired pressure setpoint. The controller commands
 * a zero velocity when the platform is within the Dead Band, and a near-zero velocity
 * when in the approach band.
 *
 * For example, assume the platform starts at the surface with the following:
 *    Pressure setpoint: 100 dBar
 *    Dead band pressure: 1 dBar
 *    Approach band pressure: 5 dBar
 *    Approach band speed: 0.1 dBar/sec
 *    Nominal descent speed: 0.5 dBar/sec
 *
 * Given this set of parameters, the controller should command platform speeds as follows:
 *  _____________________________________________________________
 *  | Pressure Range |   Velocity  (direction) | Velocity band   |
 *  ==============================================================
 *  |   0-95 dBar    | 0.5 dBar/sec (descend)  | [OUTTER_BAND]   |
 *  |   95-99 dBar   | 0.1 dBar/sec (descend)  | [APPROACH BAND] |
 *  |   99-101 dBar  | 0.0 dBar/sec (descend)  | [DEAD BAND]     |
 *  |  105-101 dBar  | 0.1 dBar/sec (ascend)   | [APPROACH BAND] |
 *  |  > 105 dBar    | 0.5 dBar/sec (ascend)   | [OUTTER_BAND]   |
 *  ==============================================================
 *
 * Outputs:
 *  -- Motor command: Normalized motor velocity to be passed to motor controller.
 */
class Supervised_PV_Controller final : public Platform_Controller
{
  public:
	enum class controlBand_t : uint8_t
	{
		DEAD_BAND = 0,
		APPROACH_BAND,
		OUTTER_BAND
	};

  public:
	Supervised_PV_Controller(std::string id, uint32_t motorMaxCountsPerSecond);

	void start(const stateEstimate_t initial) final;
	void stop() final;
	void sleep() final;
	
	/**
	 * @brief Update the controller with the current state estimate, and desired targets.
	 * 
	 * @param currentEst Current state estimate.
	 * @param targetPressure_dBar Desired pressure setpoint.
	 * @param parkAtTargetPressure if platform should stop at targetPressure_dBar
	 * @param velocityDesired_dBar_s Desired velocity
	 * @param ctrlMode control mode
	 * 
	 * @return float Motor command in rad/s.
	 **/
	float update(const stateEstimate_t currentEst,
		const float targetPressure_dBar,
		const bool parkAtTargetPressure,
		const float velocityDesired_dBar_s,
		const ControlMode ctrlMode) final;

	/**
	 * @brief Set the Approach Speed. Approach speed is the desired platform speed as the platform
	 * nears the dead band.
	 *
	 * @param approachSpeed_dBar_per_sec Approach speed.
	 */
	void setApproachSpeed(const float approachSpeed_dBar_per_sec)
	{
		approachSpeed_dBar_per_sec_ = std::abs(approachSpeed_dBar_per_sec);
	}

	/**
	 * @brief Set both pressure bands. Approach Band MUST be > than Dead Band
	 *
	 * @param deadBandPressure_dBar Dead band pressure.
	 * @param approachBandPressure_dBar Approach band pressure.
	 */
	void setPressureBands(float deadBandPressure_dBar, float approachBandPressure_dBar);

	AH_PID::state_t getPVControllerState() const;

	static constexpr float DEFAULT_DEADBAND_PRESSURE_DBAR = 2.0;
	static constexpr float DEFAULT_APPROACHBAND_PRESSURE_DBAR = 5.0;

  private:
	/**
	 * @brief Get the Current Pressure Band
	 *
	 * @param est Current platform state estimate
	 * @param setpoint_dBar Current platform pressure setpoint
	 * @return controlBand_t Current band the platform is in
	 */
	controlBand_t getCurrentPressureBand(const stateEstimate_t est, const float setpoint_dBar);

	/**
	 * @brief Run the Velocity control Law
	 *
	 * @param est Current State Estimate
	 * @param velocityDesired_dBar_s Desired velocity in dBar/sec
	 * @param ctdMode Current CTD mode
	 * @return AH_PID::logVals_t result from the PID controller
	 */
	AH_PID::logVals_t runVelocityLaw(const stateEstimate_t est, const float velocityDesired_dBar_s,
									 const SBE_CTD_MODE ctdMode);

	/**
	 * @brief Run the Pressure Velocity control law. This law looks up the platform velocity based
	 * on the platforms current pressure, and the pressure bands and velocities.
	 *
	 * @param est Current State Estimate
	 * @param targetPressure_dBar desired pressure to transit to
	 * @param parkAtTargetPressure If the platform should stop at the desired pressure
	 * @param transitVelocity_dBar_s Nominal transit velocity (when not in approach or dead bands)
	 * @param ctdMode Current CTD mode
	 * @return AH_PID::logVals_t result from the underlying PID controller
	 */
	AH_PID::logVals_t runPressVelLaw(const stateEstimate_t est, const float targetPressure_dBar,
									 const bool parkAtTargetPressure,
									 const float transitVelocity_dBar_s,
									 const SBE_CTD_MODE ctdMode);

	float getDurationWithinLimits(const systime::DateTimeStruct tf,
								  const systime::DateTimeStruct ti, const float min,
								  const float max);
	/**
	 * @brief Return the a platform velocity setpoint based on current state and target pressure.
	 *
	 * @param est Current platform state estimate
	 * @param targetPressure_dBar Target pressure
	 * @param nominalVelocity_dBar_per_sec Target nominal velocity when operating in the outter
	 * pressure band
	 * @return float Required Platform velocity. If > 0, platform must descend to reach target, if <
	 * 0, platform must ascend to reach target.
	 */
	float lookupPVSetpoint(const stateEstimate_t est, const float targetPressure_dBar,
						   const float nominalVelocity_dBar_per_sec);

	bool initialized_;
	AH_PID velController_;
	ControlMode lastControlMode_;
	SBE_CTD_MODE lastCtdMode_;
	stateEstimate_t lastStateEst_;
	controlBand_t currentBand_;

	float approachSpeed_dBar_per_sec_;
	float deadBandPressure_dBar_;
	float approachBandPressure_dBar_;
	
	uint32_t motorMaxCps_;

	// PID constants
	// Constants based on Matlab analysis 2018 Aug 31 for 0.156 cm^3/rev pump
	const AH_PID::gains_t defaultPIDGains_ = {.K = 80,
											  .Td = 10,
											  .Ti = 50e3,
											  .Tf = 10,
											  .Tt = 100,
											  .limOutput_max = 3000.0 / 60.0,
											  .limOutput_min = -3000.0 / 60.0};
};