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

LPF_Estimator* lpf_est;

stateEstimate_t x0_rand = {
	.press_dBar = 123, .vel_dBar_s = 456, .bellowsDeltaVol_m3 = 7, .mode = SBE_CTD_MODE::NONE, .timestamp = { }};

stateEstimate_t x0_zero = {
	.press_dBar = 0, .vel_dBar_s = 0, .bellowsDeltaVol_m3 = 0, .mode = SBE_CTD_MODE::NONE, .timestamp = { }};

TEST_GROUP(LPF_EST){void setup(){lpf_est = new LPF_Estimator("lpf_estimator");
}

void teardown()
{
	delete lpf_est;
}
}
;

TEST(LPF_EST, InitZero)
{
	lpf_est->init(x0_rand);
	stateEstimate_t e = lpf_est->getCurrentEstimate();

	DOUBLES_EQUAL(x0_rand.press_dBar, e.press_dBar, 1e-7);
	DOUBLES_EQUAL(x0_rand.vel_dBar_s, e.vel_dBar_s, 1e-7);
	DOUBLES_EQUAL(x0_rand.bellowsDeltaVol_m3, e.bellowsDeltaVol_m3, 1e-7);
}

TEST(LPF_EST, Convergance)
{
	lpf_est->init(x0_zero);

	stateObservation_t o = {};
	o.timestamp = systime::getDateTimeNow();
	o.timestamp.second = 0;
	stateEstimate_t e = {};

	for(size_t i = 0; i <= 10; i++)
	{
		o.timestamp.second++;
		// Increment dbar by 1 each time (akin to a 1m change in depth)
		o.press_dBar = 1.0f * float(i);
		e = lpf_est->update(o, SBE_CTD_MODE::NON_CP);
	}

	// We expect velocity to be 1 dBar/s
	DOUBLES_EQUAL(1.0, e.vel_dBar_s, 1e-4);

	// Platform no longer descending, but feeding it same pressure value
	for(size_t i = 0; i < 15; i++)
	{
		o.timestamp.second++;
		e = lpf_est->update(o, SBE_CTD_MODE::NON_CP);
	}

	// Should be 10 dBar, and 0 dBar/s
	DOUBLES_EQUAL(10, e.press_dBar, 1e-5);
	DOUBLES_EQUAL(0, e.vel_dBar_s, 1e-5);
}