#include "AH_PID.hpp"

AH_PID::AH_PID(std::string id) : moduleID_(id)
{
	gains_ = default_gains_;
}

const AH_PID::state_t AH_PID::getState() const
{
	return state_;
}

const AH_PID::gains_t AH_PID::getGains() const
{
	return gains_;
}

const AH_PID::parameters_t AH_PID::getParams() const
{
	return param_;
}

void AH_PID::setGains(const gains_t desiredGains)
{
	gains_ = desiredGains;
}

AH_PID::logVals_t AH_PID::update(float dt_sec, const float setpoint, const float measurement)
{
	// Set the state variables.
	//  Guard against excessively large dt
	if(dt_sec > 5.0f)
	{
		dt_sec = 5.0f;
	}
	else if(dt_sec <= 0)
	{
		// Shouldn't happen, but could, if error in RTC or systime code.
		// make small to avoid any potential divide-by-zero error downstream.
		dt_sec = 0.01f;
	}
	computeParams(dt_sec);

	state_.y_sp = setpoint;
	state_.y_meas = measurement;

	// Compute filter state
	state_.y2 = (param_.p1 * state_.y2) + (param_.p2 * (state_.y_meas - state_.y1));
	state_.y1 = state_.y1 + state_.y2;

	// Compute controller state
	state_.v_rev =
		gains_.K * ((gains_.b * state_.y_sp) - state_.y1) - (param_.p4 * state_.y2) + state_.I;

	// Apply saturation limits (which reside in gains_)
	limitOutput(state_.v_rev, state_.u_rev);

	// Update integral term
	state_.I = state_.I + (param_.p3 * (state_.y_sp - state_.y1)) +
			   (param_.p5 * (state_.u_rev - state_.v_rev));

	logVals_t pidlog = {.state = state_, .params = param_, .timeStamp = {}};
	pidlog.timeStamp = systime::getDateTimeNow();

	return pidlog;
}

void AH_PID::computeParams(const float samplePeriod_s)
{
	param_.h = samplePeriod_s;
	// helper variable, as in A&H
	float den = (gains_.Tf * gains_.Tf) + (2 * param_.h * gains_.Tf) + (2 * param_.h * param_.h);

	// compute remainder of parameters
	param_.p1 = gains_.Tf * gains_.Tf / den;
	param_.p2 = 2 * param_.h * param_.h / den;
	param_.p3 = gains_.K * param_.h / gains_.Ti;
	param_.p4 = gains_.K * gains_.Td / param_.h;
	param_.p5 = param_.h / gains_.Tt;
}

bool AH_PID::limitOutput(const float output_desired, float& sat_output)
{
	bool saturated = false;

	if(output_desired >= gains_.limOutput_max)
	{
		saturated = true;
		sat_output = gains_.limOutput_max;
	}
	else if(output_desired <= gains_.limOutput_min)
	{
		saturated = true;
		sat_output = gains_.limOutput_min;
	}
	else
	{
		sat_output = output_desired;
	}

	return saturated;
}