// clang-format off
#include <Supervised_PV_Controller.hpp>
#include "LPF_Estimator.hpp"
#include <CppUTest/CommandLineTestRunner.h>
#include <iostream>
#include <CPF_Simulation_Euler.hpp>
// clang-format on

TEST_GROUP(PV)
{
	CPF_SIM_Euler::state sim_state = {};
	CPF_SIM_Euler::cpf_params sim_p;

	Supervised_PV_Controller* PV;
	LPF_Estimator* lpf_est;

	float approach_speed = 0.01f;
	float dead_band = 1.0f;
	float approach_band = 5.0f;
	static constexpr uint32_t motorMaxCountsPerSecond = 200000;

	stateEstimate_t x0_zero = {
		.press_dBar = 0, .vel_dBar_s = 0, .bellowsDeltaVol_m3 = 0, .timestamp = {}};

	void setup()
	{
		PV = new Supervised_PV_Controller("platformPVController", motorMaxCountsPerSecond);
		lpf_est = new LPF_Estimator("lpf_estimator");
	}

	void teardown()
	{
		delete PV;
		delete lpf_est;
	}

	bool time_to_sample(const float t_now, const float t_nextSample)
	{
		return (t_now > t_nextSample);
	}

	void advanceDateTimeStruct(DateTimeStruct & dateTimeStruct, const float dt_sec)
	{
		dateTimeStruct.second += static_cast<uint8_t>(dt_sec);
		dateTimeStruct.milliSecond +=
			static_cast<uint16_t>((dt_sec - static_cast<uint8_t>(dt_sec)) * 1000);
		if(dateTimeStruct.milliSecond >= 1000)
		{
			dateTimeStruct.milliSecond -= 1000;
			dateTimeStruct.second += 1;
		}
		if(dateTimeStruct.second >= 60)
		{
			dateTimeStruct.second -= 60;
			dateTimeStruct.minute += 1;
		}
		if(dateTimeStruct.minute >= 60)
		{
			dateTimeStruct.minute -= 60;
			dateTimeStruct.hour += 1;
		}
		if(dateTimeStruct.hour >= 24)
		{
			dateTimeStruct.hour -= 24;
			dateTimeStruct.date += 1;
		}
		// not going to deal with days of month...
	}
};

TEST(PV, pass)
{
	CHECK(true);
}

TEST(PV, zero_error_zero_output)
{
	// Initializes the velocity controller with appropriate gains
	PV->start(x0_zero);

	float target_pressure = 0.0;
	float target_velocity = 0.0;

	// Request zero pressure and speed setpoints
	float rawMotorCmd = PV->update(x0_zero, target_pressure, true, target_velocity,
								   ControlMode::PRESSURE_VELOCITY);

	DOUBLES_EQUAL(0.0, rawMotorCmd, 1e-7);
}

TEST(PV, output_limits_respected)
{
	PV->start(x0_zero);

	float target_pressure = 10000.0f;
	float target_velocity = 1.0f;

	stateEstimate_t est = x0_zero;

	float rawMotorCmd = PV->update(est, target_pressure, true, target_velocity,
								   ControlMode::PRESSURE_VELOCITY);

	// This expected value comes from const AH_PID::gains_t defaultPIDGains_, which is private, so
	// can't set expectation programatically. Most importantly, it is not random! Negative motor
	// velocity should result in oil leaving the external bladder!
	DOUBLES_EQUAL(-50.0, rawMotorCmd, 1e-7);

	// Now the platform is really deep, and we want to send it to the surface
	est.press_dBar = 100000.0f;
	target_pressure = 0.0f;

	rawMotorCmd = PV->update(est, target_pressure, true, target_velocity,
							 ControlMode::PRESSURE_VELOCITY);

	// Should have negative maximum, value, which comes from same location described above.
	DOUBLES_EQUAL(50.0, rawMotorCmd, 1e-7);
}

TEST(PV, PV_Controller_zerod_after_wake)
{
	PV->start(x0_zero);

	stateEstimate_t est = x0_zero;
	est.press_dBar = 10;
	est.vel_dBar_s = 1;
	DateTimeStruct simTime = systime::getDateTimeNow();
	simTime.second = 0;
	est.timestamp = simTime;

	// wind up the controller
	for(size_t i = 0; i < 10; i++)
	{
		est.timestamp.second += i * 3;
		est.vel_dBar_s += float(i) * 0.1f;
		est.press_dBar += float(i);
		PV->update(est, 0.0, false, 0.5, ControlMode::PRESSURE_VELOCITY);
	}

	AH_PID::state_t x = PV->getPVControllerState();

	// Integral term should be big with those errors
	bool controller_off_the_rails = x.I > 50 || x.I < -50;

	CHECK(controller_off_the_rails);

	// Simulate platform going to sleep and waking
	PV->sleep();
	PV->start(x0_zero);

	// PV Controller internal state should be all zero
	x = PV->getPVControllerState();

	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);
}

TEST(PV, dynamics_tests)
{
	// The singleton CPF_SIM_Euler calls new, but doesn't get formally deleted after the test,
	// so we disable memeory leak tests for this test. This isn't a problem because when the program fully
	// exists, the singleton will be deleted.
	MemoryLeakWarningPlugin::turnOffNewDeleteOverloads();

	// Estimator setup
	lpf_est->init(x0_zero);
	stateObservation_t obs = {};
	stateEstimate_t est = {};
	est = lpf_est->getCurrentEstimate();
	obs.timestamp = systime::getDateTimeNow();

	// Simulation setup parameters
	const float sim_dt_sec = 0.01f;
	float sim_time = 0.0f;
	const float sim_end = 1500.0f;
	const float sample_period = 1.0f; // Corresponds to Non CP MODE
	float time_next_sample = 0.0f;
	float time_last_sample = 0.0f;

	// Target
	const float wypt_dBar = 100.0f;
	const bool park_at_wypt = true;
	const float vel_desired = 0.1f;

	float motor_cmd = 0.0f;

	// Start the platform controller with the current state estimate
	PV->start(est);
	PV->setApproachSpeed(0.01f);
	PV->setPressureBands(1.0f, 6.0f);

	sim_p.simulated_pump_dir = CPF_SIM_Euler::PumpDirection::POS_MOTOR_COUNTS_DEFLATE_INNER_BELLOWS;

	CPF_SIM_Euler::get_instance()->init(sim_state, sim_p);

	while(sim_time < sim_end)
	{
		sim_state = CPF_SIM_Euler::get_instance()->propagate(sim_dt_sec, sim_state, sim_p, motor_cmd);

		// check if time take a simulated pressure measurement
		if(time_to_sample(sim_time, time_next_sample))
		{
			advanceDateTimeStruct(obs.timestamp, (sim_time - time_last_sample));

			// neglect noise
			obs.press_dBar = sim_state.x_press_dBar;

			// provide the raw pressure measurement to LPF estimator
			est = lpf_est->update(obs, SBE_CTD_MODE::NON_CP);

			// provide the current state estimate, along with our destination to the controller
			motor_cmd = PV->update(est, wypt_dBar, park_at_wypt, vel_desired,
								   ControlMode::PRESSURE_VELOCITY);

			// establish time we take the next pressure sample
			time_next_sample = sim_time + sample_period;

			// save the time at which this sample was taken
			time_last_sample = sim_time;
		}

		// update times
		sim_time += sim_dt_sec;
	}

	// Did the CPF arrive to the destination, within 2dBar (~2 meters)?
	DOUBLES_EQUAL(wypt_dBar, sim_state.x_press_dBar, 2.0);

	// Did the CPF come to a stop (within ~5cm/sec)?
	DOUBLES_EQUAL(0.0, sim_state.x_press_vel_dBar_s, 0.05);

	// Re-enable memory leak tests
	MemoryLeakWarningPlugin::restoreNewDeleteOverloads();
}