#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.press_dBar);
	velFilt_.init(x0.vel_dBar_s);

	lastEstimate_ = x0;
	currentEstimate_ = x0;
	lastObservation_ = {.press_dBar = 0,
						.motorRate_rads_s = 0,
						.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;
	
	// guard against dangerous division
	if (dt_sec > 0 && !std::isnan(dt_sec))
	{
		// don't let bad pressure values into the filter
		if (!std::isnan(obs.press_dBar))
		{
			switch (mode)
			{
				case(SBE_CTD_MODE::CP):
				{
					currentEstimate_.press_dBar = static_cast<float>(
						presFilt_.update(obs.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_.press_dBar = static_cast<float>(
						presFilt_.update(obs.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_.press_dBar = 0.0f;
					currentEstimate_.vel_dBar_s = 0.0f;
					break;
				}
			}
			
			currentEstimate_.press_dBar = obs.press_dBar;
			// Given a good dt, and good pressure values, we estimate velocity.
			currentEstimate_.vel_dBar_s =
				(currentEstimate_.press_dBar - lastEstimate_.press_dBar) / dt_sec;
		}
	}

	// TODO: use bellows position to aid in the estimate (Kalman filter perhaps?)
	currentEstimate_.bellowsDeltaVol_m3 = NAN;
	currentEstimate_.timestamp = obs.timestamp;

	lastObservation_ = obs;

	return currentEstimate_;
}

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