#include "LPF_Estimator.hpp"
#include <system_time.hpp>
#include <cmath>

LPF_Estimator::LPF_Estimator(const std::string id) :
	PlatformStateEstimator(id), presFilt_(), velFilt_()
{
}

void LPF_Estimator::init(const stateEstimate_t x0)
{
	presFilt_.init(x0.x_press_dBar);
	velFilt_.init(x0.x_vel_dBar_s);

	lastEstimate_ = x0;
	currentEstimate_ = x0;
	lastObservation_ = {.z_press_dBar = 0,
						.z_motorRate_rads_s = 0,
						.z_innerBellowsPos_pctFull = 0,
						.timestamp = systime::getDateTimeNow()};
}

stateEstimate_t LPF_Estimator::update(const stateObservation_t obs, const SBE_CTD_MODE mode)
{
	lastEstimate_ = currentEstimate_;
	float dt_sec = static_cast<float>(systime::getDuration_ms(obs.timestamp, lastObservation_.timestamp)) / 1000.0f;
	if(!std::isnan(obs.z_press_dBar))
	{
		switch(mode)
		{
			case(SBE_CTD_MODE::CP):
			{
				currentEstimate_.x_press_dBar = static_cast<float>(
					presFilt_.update(obs.z_press_dBar, CPMode_Pressure_Coef, NUM_PRES_COEFF));
				// currentEstimate_.x_vel_dBar_s = velFilt_.update(obs.z_press_dBar,
				// CPMode_Velocity_Coef, NUM_VEL_COEFF) / dt_sec;
				break;
			}
			case(SBE_CTD_MODE::NON_CP):
			{
				currentEstimate_.x_press_dBar = static_cast<float>(
					presFilt_.update(obs.z_press_dBar, NonCPMode_Pressure_Coef, NUM_PRES_COEFF));
				// currentEstimate_.x_vel_dBar_s = velFilt_.update(obs.z_press_dBar,
				// NonCPMode_Velocity_Coef, NUM_PRES_COEFF) / dt_sec;
				break;
			}
			default:
			{
				currentEstimate_.x_press_dBar = 0.0f;
				currentEstimate_.x_vel_dBar_s = 0.0f;
				break;
			}
		}
	}

	// guard against dangerous division
	if(dt_sec > 0 && !std::isnan(dt_sec))
	{
		currentEstimate_.x_vel_dBar_s =
			(currentEstimate_.x_press_dBar - lastEstimate_.x_press_dBar) / dt_sec;
	}

	currentEstimate_.x_bellowsDeltaVol_m3 = NAN;
	currentEstimate_.timestamp = obs.timestamp;

	lastObservation_ = obs;

	return currentEstimate_;
}

const stateEstimate_t LPF_Estimator::getCurrentEstimate()
{
	return currentEstimate_;
}
