// clang-format off
#include <AH_PID.hpp>
#include "../../Tests/mocks/CPF_Simulation_Euler.hpp"
#include <CppUTest/CommandLineTestRunner.h>
#include <iostream>
// clang-format on

AH_PID* pid;

// Unity gain for everybody
AH_PID::gains_t g_1 = {
	.K = 1, .Td = 1, .Ti = 1, .Tf = 1, .Tt = 1, .limOutput_max = 1, .limOutput_min = -1};

// From orig. C# code
AH_PID::gains_t g_actual = {.K = 80,
							.Td = 10,
							.Ti = 50e3,
							.Tf = 10,
							.Tt = 100,
							.limOutput_max = 3000.0 / 60.0,
							.limOutput_min = -3000.0 / 60.0};

// Hand fiddled values
AH_PID::gains_t g_test = {.K = 120,
						  .Td = 12,
						  .Ti = 500,
						  .Tf = 10,
						  .Tt = 10,
						  .limOutput_max = 3000.0 / 60.0,
						  .limOutput_min = -3000.0 / 60.0};

// Simple non-linear dynamical model of CPF which assumes quadratic drag.
// Developed by Laughlin Barker. Shows good agreement with  with ode45
// solution for small dt (=<0.1).
//class CPF_SIM_Euler
//{
//  public:
//	typedef struct state
//	{
//		float x_pos; // position (m)
//		float x_vel; // velocity (m/s)
//		float x_dVolume; // bellows dVolume (m^3)
//		float dt_sec;
//		float now_sec;
//	} state;
//
//	typedef struct cpf_params
//	{
//		float densitySW = 1025.0f;
//		float gravity = 9.81f;
//		float surfaceArea = 0.032429f; // m^2 from r=0.2032, See BHa MBARI Intern report 2018
//		float coeffDrag = 0.9000f; // From See BHa MBARI Intern report 2018, pg 24, line 66
//		float mass = 38.0f;
//		float pump_disp = 1.56e-7f; // From BHa MBARI Intern report 2018
//		float pump_eff = 0.6f; // From BHa MBARI Intern report 2018
//	} cpf_params;
//
//	static state propagate(const float dt, const state in, const cpf_params p,
//						   float input_revs_per_sec)
//	{
//		state out;
//		out.dt_sec = dt;
//		out.now_sec = in.now_sec + dt;
//
//		out.x_pos = in.x_pos + in.x_vel * dt;
//		out.x_vel = in.x_vel - (dt * p.densitySW / p.mass) *
//								   (p.gravity * in.x_dVolume +
//									0.5f * p.coeffDrag * p.surfaceArea * in.x_vel * abs(in.x_vel));
//		out.x_dVolume = in.x_dVolume + dt * p.pump_eff * p.pump_disp * input_revs_per_sec;
//
//		return out;
//	}
//};

TEST_GROUP(ahpid){void setup(){pid = new AH_PID("myPID");
}

void teardown()
{
	delete pid;
}
}
;

TEST(ahpid, ctorInitZero)
{
	AH_PID::state_t s = pid->getState();

	DOUBLES_EQUAL(0.0, s.y_sp, 1e-7);
	DOUBLES_EQUAL(0.0, s.y_meas, 1e-7);
	DOUBLES_EQUAL(0.0, s.y1, 1e-7);
	DOUBLES_EQUAL(0.0, s.y2, 1e-7);
	DOUBLES_EQUAL(0.0, s.v_rev, 1e-7);
	DOUBLES_EQUAL(0.0, s.u_rev, 1e-7);
	DOUBLES_EQUAL(0.0, s.I, 1e-7);

	AH_PID::gains_t g_z = pid->getGains();

	DOUBLES_EQUAL(80.0, g_z.K, 1e-7);
	DOUBLES_EQUAL(10.0, g_z.Td, 1e-7);
	DOUBLES_EQUAL(50e3, g_z.Ti, 1e-7);
	DOUBLES_EQUAL(10.0, g_z.Tf, 1e-7);
	DOUBLES_EQUAL(100.0, g_z.Tt, 1e-7);
	DOUBLES_EQUAL(50.0, g_z.limOutput_max, 1e-7);
	DOUBLES_EQUAL(-50.0, g_z.limOutput_min, 1e-7);

	AH_PID::parameters_t p = pid->getParams();

	DOUBLES_EQUAL(0.0, p.h, 1e-7);
	DOUBLES_EQUAL(0.0, p.p1, 1e-7);
	DOUBLES_EQUAL(0.0, p.p2, 1e-7);
	DOUBLES_EQUAL(0.0, p.p3, 1e-7);
	DOUBLES_EQUAL(0.0, p.p4, 1e-7);
	DOUBLES_EQUAL(0.0, p.p5, 1e-7);
}

TEST(ahpid, gainSetting)
{
	pid->setGains(g_1);

	AH_PID::gains_t g_ret = pid->getGains();

	DOUBLES_EQUAL(1.0, g_ret.K, 1e-7);
	DOUBLES_EQUAL(1.0, g_ret.Td, 1e-7);
	DOUBLES_EQUAL(1.0, g_ret.Ti, 1e-7);
	DOUBLES_EQUAL(1.0, g_ret.Tf, 1e-7);
	DOUBLES_EQUAL(1.0, g_ret.Tt, 1e-7);
	DOUBLES_EQUAL(1.0, g_ret.limOutput_max, 1e-7);
	DOUBLES_EQUAL(-1.0, g_ret.limOutput_min, 1e-7);
}

TEST(ahpid, satFunction)
{
	// Has [-1, 1] output limits
	pid->setGains(g_1);

	const float dt_sec = 3;
	const float output_des = 10000000;
	const float output_meas = 0.0;

	// Saturate the controller
	AH_PID::logVals_t log = pid->update(dt_sec, output_des, output_meas);

	DOUBLES_EQUAL(1.0, log.state.u_rev, 1e-7);

	// Now in the other direction
	log = pid->update(dt_sec, -output_des, output_meas);

	DOUBLES_EQUAL(-1.0, log.state.u_rev, 1e-7);
}

TEST(ahpid, udpate)
{
	const float dt_sec = 3;
	const float output_des = 0;
	const float output_meas = output_des;

	pid->setGains(g_actual);

	AH_PID::logVals_t log = pid->update(dt_sec, output_des, output_meas);

	for(size_t i = 0; i < 100; i++)
	{
		log = pid->update(dt_sec, output_des, output_meas);
	}

	// Since desired=measured, we expect zero output
	// DOUBLES_EQUAL(0.0, log.state.I, 1e-7);
	DOUBLES_EQUAL(output_des, log.state.y_sp, 1e-7);
	DOUBLES_EQUAL(output_meas, log.state.y_meas, 1e-7);
	DOUBLES_EQUAL(0.0, log.state.v_rev, 1e-7);
}

TEST(ahpid, dynamicSim)
{
	const float dt_sec = 0.1f;
	const float t_final = 100.0f;
	size_t numSteps = static_cast<size_t>(t_final / dt_sec);
	const float vel_sp = 0.2f;

	CPF_SIM_Euler::state x0 = {.x_press_dBar = 0.0, // depth
							   .x_press_vel_dBar_s = 0.0, // velocity
							   .x_dVolume = 0.0, // bladder volume
							   .dt_sec = dt_sec,
							   .now_sec = 0.0};

	CPF_SIM_Euler::state x = {};
	CPF_SIM_Euler::cpf_params p;
	p.simulated_pump_dir = CPF_SIM_Euler::PumpDirection::POS_MOTOR_COUNTS_DEFLATE_INNER_BELLOWS;
	
	x = x0;
	float u_revs = 0.0;

	pid->setGains(g_test);

	AH_PID::logVals_t log = pid->update(dt_sec, vel_sp, x.x_press_vel_dBar_s);

	for(size_t i = 1; i < numSteps; i++)
	{
		// NEGATIVE Feedback...
		u_revs = -log.state.u_rev;

		x = CPF_SIM_Euler::propagate(dt_sec, x, p, u_revs);
		log = pid->update(dt_sec, vel_sp, x.x_press_vel_dBar_s);
		// std::cout << "t,x_pos,x_vel,x_dVolume,u: "  << x.now_sec << "," << x.x_pos << "," <<
		// x.x_vel << "," << x.x_dVolume <<  "," << u_revs << std::endl; std::cout << "Vel Error @
		// t=" << x.now_sec << ":" << (vel_sp - x.x_vel) << std::endl;
	}

	float error = vel_sp - x.x_press_vel_dBar_s;
	// Does it converge, within 3mm/s?
	DOUBLES_EQUAL(0.0, error, 0.003);
}

TEST(ahpid, reset_after_run)
{
	const float dt_sec = 3;
	const float output_des = 0.5;
	const float output_meas = 0;

	pid->setGains(g_actual);

	AH_PID::logVals_t log = pid->update(dt_sec, output_des, output_meas);

	for(size_t i = 0; i < 10; i++)
	{
		log = pid->update(dt_sec, output_des, output_meas);
	}

	pid->resetState();
	AH_PID::state_t x = pid->getState();

	DOUBLES_EQUAL(0.0, x.y_sp, 1e-7);
	DOUBLES_EQUAL(0.0, x.y_meas, 1e-7);
	DOUBLES_EQUAL(0.0, x.y1, 1e-7);
	DOUBLES_EQUAL(0.0, x.y2, 1e-7);
	DOUBLES_EQUAL(0.0, x.v_rev, 1e-7);
	DOUBLES_EQUAL(0.0, x.u_rev, 1e-7);
	DOUBLES_EQUAL(0.0, x.I, 1e-7);
}