#include "Supervised_PV_Controller.hpp"

Supervised_PV_Controller::Supervised_PV_Controller(std::string id, uint32_t motorMaxCountsPerSec)
	:
	Platform_Controller(id), velController_("AH_PID_VelCtrl"), lastControlMode_(ControlMode::NONE),
	lastCtdMode_(SBE_CTD_MODE::NONE), lastStateEst_({}), currentBand_(controlBand_t::OUTTER_BAND),
	approachSpeed_dBar_per_sec_(0.1f), deadBandPressure_dBar_(1.0f),
	approachBandPressure_dBar_(2.0f), motorMaxCps_(motorMaxCountsPerSec)
{
}

void Supervised_PV_Controller::start(const stateEstimate_t initial)
{
	velController_.resetState();
	velController_.setGains(defaultPIDGains_);

	lastStateEst_ = initial;

	initialized_ = true;
}

void Supervised_PV_Controller::stop()
{
}

void Supervised_PV_Controller::sleep()
{
}

void Supervised_PV_Controller::setPressureBands(float deadBandPressure_dBar,
												float approachBandPressure_dBar)
{
	// Guard against accidental negative values
	deadBandPressure_dBar = std::abs(deadBandPressure_dBar);
	approachBandPressure_dBar = std::abs(approachBandPressure_dBar);

	// Ensure the approach band is > than the dead band
	if(approachBandPressure_dBar > deadBandPressure_dBar)
	{
		deadBandPressure_dBar_ = deadBandPressure_dBar;
		approachBandPressure_dBar_ = approachBandPressure_dBar;
	}
	else
	{
		// TODO: Log warning!
		deadBandPressure_dBar_ = DEFAULT_DEADBAND_PRESSURE_DBAR;
		approachBandPressure_dBar_ = DEFAULT_APPROACHBAND_PRESSURE_DBAR;
	}
}

float Supervised_PV_Controller::update(const stateEstimate_t currentEst,
	const float targetPressure_dBar,
	const bool parkAtTargetPressure,
	const float velocityDesired_dBar_s,
	const ControlMode ctrlMode)
{
	// Controller is initialized after the first call
	if(!initialized_)
	{
		lastStateEst_ = {};
		lastStateEst_.timestamp = systime::getDateTimeNow();
		initialized_ = true;

		// return zero motor speed on this call
		return 0.0f;
	}

	AH_PID::logVals_t pidResults = {};
	switch(ctrlMode)
	{
		case(ControlMode::VELOCITY):
		{
			// Pure control on velocity
			pidResults = runVelocityLaw(currentEst, velocityDesired_dBar_s, currentEst.mode);
			break;
		}
		case(ControlMode::PRESSURE_VELOCITY):
		{
			pidResults = runPressVelLaw(currentEst, targetPressure_dBar, parkAtTargetPressure,
				velocityDesired_dBar_s, currentEst.mode);
			break;
		}
		default:
		{
			// TODO: log error!
			break;
		}
	}

	// Note: This value is output limited, and respects output constraints that are set in the
	// controller. You can see find the constraints at velController_.getGains().limOutput_***
	float motorCmd = 1.0f * pidResults.state.u_rev;

	// Save state estimate for computing dt on next iteration.
	lastStateEst_ = currentEst;

	return motorCmd;
}

AH_PID::state_t Supervised_PV_Controller::getPVControllerState() const
{
	return velController_.getState();
}

Supervised_PV_Controller::controlBand_t
	Supervised_PV_Controller::getCurrentPressureBand(const stateEstimate_t est,
													 const float setpoint_dBar)
{
	controlBand_t currentBand = controlBand_t::DEAD_BAND;

	float absDistance = std::abs(setpoint_dBar - est.press_dBar);

	if(absDistance > approachBandPressure_dBar_)
	{
		currentBand = controlBand_t::OUTTER_BAND;
	}
	else if((absDistance > deadBandPressure_dBar_) && (absDistance < approachBandPressure_dBar_))
	{
		currentBand = controlBand_t::APPROACH_BAND;
	}

	return currentBand;
}

AH_PID::logVals_t Supervised_PV_Controller::runVelocityLaw(const stateEstimate_t est,
														   const float velocityDesired_dBar_s,
														   const SBE_CTD_MODE ctdMode)
{
	(void)ctdMode;
	// Guard against impractical dt's
	float dt_sec = getDurationWithinLimits(est.timestamp, lastStateEst_.timestamp, 0.01f, 5.0f);

	// Guard against excessive desired velocity
	float velSetpoint_dbar_s = this->constrainValueSymetric(
		velocityDesired_dBar_s, DEFAULT_PRESSURE_SPEED_DBAR_PER_SEC);

	// Update the PID controller, and get the output
	AH_PID::logVals_t pidResults =
		velController_.update(dt_sec, velSetpoint_dbar_s, est.vel_dBar_s);

	return pidResults;
}

AH_PID::logVals_t Supervised_PV_Controller::runPressVelLaw(const stateEstimate_t est,
														   const float targetPressure_dBar,
														   const bool parkAtTargetPressure,
														   const float transitVelocity_dBar_s,
														   const SBE_CTD_MODE ctdMode)
{
	float velSetpoint_dBar_per_sec = 0.0f;
	float presSetpoint_dBar = 0.0f;

	// Prevent request to go excessively deep
	presSetpoint_dBar =
		this->constrainValueSymetric(targetPressure_dBar, DEFAULT_MAX_TARGET_PRESSURE_DBAR);

	// Possible to have waypoints that are not park locations, so don't (necessarily)
	// want to slow down.
	if(parkAtTargetPressure)
	{
		velSetpoint_dBar_per_sec = lookupPVSetpoint(est, presSetpoint_dBar, transitVelocity_dBar_s);
	}
	else
	{
		velSetpoint_dBar_per_sec = transitVelocity_dBar_s;
	}

	return runVelocityLaw(est, velSetpoint_dBar_per_sec, ctdMode);
}

float Supervised_PV_Controller::getDurationWithinLimits(const systime::DateTimeStruct tf,
														const systime::DateTimeStruct ti,
														const float min, const float max)
{
	// Compute dt in seconds since the last call
	float dt_sec = static_cast<float>(systime::getDuration_ms(tf, ti)) / 1000.0f;

	// Guard crazy dt
	if(dt_sec > max)
	{
		dt_sec = max;
	}
	else if(dt_sec <= min)
	{
		dt_sec = min;
	}

	return dt_sec;
}

float Supervised_PV_Controller::lookupPVSetpoint(const stateEstimate_t est,
												 const float targetPressure_dBar,
												 const float nominalVelocity_dBar_per_sec)
{
	float velocitySetpoint_dBar_per_sec = 0.0f;

	controlBand_t curBand = getCurrentPressureBand(est, targetPressure_dBar);

	switch(curBand)
	{
		case controlBand_t::DEAD_BAND:
		{
			velocitySetpoint_dBar_per_sec = 0.0f;
			break;
		}
		case controlBand_t::APPROACH_BAND:
		{
			if(est.press_dBar > targetPressure_dBar)
			{
				// Platform is deeper than desired. Negative veloicty to bring back up
				velocitySetpoint_dBar_per_sec = -1 * approachSpeed_dBar_per_sec_;
			}
			else
			{
				velocitySetpoint_dBar_per_sec = approachSpeed_dBar_per_sec_;
			}
			break;
		}
		case controlBand_t::OUTTER_BAND:
		{
			if(est.press_dBar > targetPressure_dBar)
			{
				// Platform is deeper than desired. Negative veloicty to bring back up
				velocitySetpoint_dBar_per_sec = -1 * nominalVelocity_dBar_per_sec;
			}
			else
			{
				velocitySetpoint_dBar_per_sec = nominalVelocity_dBar_per_sec;
			}
			break;
		}
		default:
		{
			// TODO: log error - this shouldn't be possible!
			break;
		}
	}

	return velocitySetpoint_dBar_per_sec;
}