// clang-format off
#include "CPFBuoyancyEngine.hpp"
#include "MotorizedValveSpy.hpp"
#include "MotorControllerSpy.hpp"
#include "BellowsPositionSpy.hpp"
#include "LPF_Estimator.hpp"
#include "UARTSpy.hpp"
#include "Supervised_PV_Controller.hpp"
#include <CppUTest/CommandLineTestRunner.h>
#include <iostream>
// clang-format on

TEST_GROUP(BE)
{
	MV_Spy valve_spy;

	BellowsPositionSpy* bellows_spy;

	const uint32_t maxVelocity_cps = 4 * 1024 * 50;

	MC_Spy* motor_spy;

	Supervised_PV_Controller* PV;
	LPF_Estimator* lpf_est;

	BellowsPosition::bellowsPosSensorCfg_t bellCfg;
	BellowsPosition::Measurement fakeInitialMeasurement;

	CPFBuoyancyEngine* be;

	void setup()
	{
		// setup fake valve
		valve_spy.SPY_setOpen(false);
		valve_spy.SPY_setReady(true);

		// setup fake motor
		motor_spy = new MC_Spy("motor_controller", maxVelocity_cps);

		// configure bellows sensor parameters
		bellCfg.sensor_slope_mm_per_volt = 10.0f;
		bellCfg.sensor_offset_mm = 50.0f;
		bellCfg.positionLowLim_mm = 0.0f;
		bellCfg.positionHighLim_mm = 100.0f;

		//setup bellows spy
		bellows_spy = new BellowsPositionSpy("bellows", bellCfg);
		
		fakeInitialMeasurement.position_pctFull = 50.0f;

		bellows_spy->SPY_setFakeValues(fakeInitialMeasurement);

		// Assume a nominal pump direction
		BuoyancyEngine::PumpDirection nominalPumpDir =
			BuoyancyEngine::PumpDirection::POS_MOTOR_COUNTS_INFLATE_INNER_BELLOWS;

		// setup controller and estimator
		PV = new Supervised_PV_Controller("platformPVController", maxVelocity_cps);
		lpf_est = new LPF_Estimator("lpf_estimator");

		be = new CPFBuoyancyEngine("testBE", *motor_spy, *bellows_spy, valve_spy, *PV, *lpf_est, nominalPumpDir);
	}

	void teardown()
	{
		delete motor_spy;
		delete PV;
		delete lpf_est;
		delete bellows_spy;
		delete be;
	}

	void sleepAllPeripherals(bool sleep)
	{
		if(sleep)
		{
			motor_spy->SPY_setMotorToSleepState(true);
			bellows_spy->SPY_setBellowsToSleepState(true);
			valve_spy.SPY_setReady(false);
			valve_spy.SPY_setOpen(false);
		}
		else
		{
			motor_spy->SPY_setMotorToSleepState(false);
			bellows_spy->SPY_setBellowsToSleepState(false);
		}
	}
};

TEST(BE, startRespectsValveStatusAndOpensOnStart)
{
	valve_spy.SPY_setReady(false);
	CHECK_FALSE(valve_spy.isMVReady());

	BuoyancyEngine::Status err = be->start(false, true);

	CHECK_FALSE(err == BuoyancyEngine::Status::READY);
	CHECK_FALSE_TEXT(valve_spy.isMVOpen(), "Main valve was opened before it was ready");

	// Now allow the valve to be ready and see if Buoyancy Engine acts accordingly
	valve_spy.SPY_setReady(true);
	CHECK(valve_spy.isMVReady());

	err = be->start(true, true);

	CHECK(err == BuoyancyEngine::Status::READY);
	CHECK_TEXT(valve_spy.isMVOpen(),
			   "Main valve was not opened after call to Buoyancy Engine start");
}

TEST(BE, valveGetsClosedOnStopAndSleep)
{
	// Valve is closed by the test setup(), so we know it's initial state
	be->start(false, false);
	CHECK_FALSE_TEXT(valve_spy.isMVOpen(), "Main valve was opened after call to Buoyancy Engine "
										   "start despite request to remain closed");

	be->stop();
	CHECK_FALSE_TEXT(valve_spy.isMVOpen(), "Main valve should be closed after calling stop");

	be->start(false, true);
	CHECK(valve_spy.isMVOpen());
	be->sleep();
	CHECK_FALSE_TEXT(valve_spy.isMVOpen(), "Main valve should be closed after calling sleep");
}

TEST(BE, motorControllerStartStopSleepCalled)
{
	// pretend motor is asleep and we're waking it up
	motor_spy->SPY_setMotorToSleepState(true);

	be->start();

	CHECK_FALSE(motor_spy->SPY_getMotorSleepState());
	CHECK_FALSE(motor_spy->SPY_getMotorStopState());

	motorVel_normalized vel = 0.0f;
	motor_spy->getSpeed_normalized(vel);

	// motor should be moving on wake
	DOUBLES_EQUAL(0.0, vel, 1e-6);

	be->stop();
	CHECK_TRUE(motor_spy->SPY_getMotorStopState());
	// should be asleep yet, that's a low power state
	CHECK_FALSE(motor_spy->SPY_getMotorSleepState());

	be->sleep();
	CHECK_TRUE(motor_spy->SPY_getMotorStopState());
	CHECK_TRUE(motor_spy->SPY_getMotorSleepState());
}

TEST(BE, returnCorrectBellowsPosition)
{
	float currentBellowsPos = 0.0f;
	be->getInnerBellowsPosition_pctFull(currentBellowsPos);

	DOUBLES_EQUAL(fakeInitialMeasurement.position_pctFull, currentBellowsPos, 1e-6);

	BellowsPosition::Measurement meas = {};
	meas.position_pctFull = 3.14159f;
	bellows_spy->SPY_setFakeValues(meas);

	be->getInnerBellowsPosition_pctFull(currentBellowsPos);

	DOUBLES_EQUAL(3.14159, currentBellowsPos, 1e-6);
}