#pragma once

#include <StateEstimator_Interface.hpp>
#include "FIR_Filter.hpp"
#include <stdint.h>

class LPF_Estimator : public PlatformStateEstimator
{
  public:
	LPF_Estimator(const std::string id);

	// Initialize the estimator with the initial state estimate
	void init(const stateEstimate_t x0) final;

	stateEstimate_t update(const stateObservation_t obs, const SBE_CTD_MODE mode) final;

	const stateEstimate_t getCurrentEstimate() final;

  private:
	FIR_Filter presFilt_;
	FIR_Filter velFilt_;
	stateEstimate_t currentEstimate_, lastEstimate_;
	stateObservation_t lastObservation_;

	// See .m files in directory for non-CP Mode PFilter design parameters
	// for CP Mode using filter design and automation tool from SPTool with
	// Response type = lowpass, Design Method = FIR Window, Order = 10, Window = Blackman-Harris,
	// Units = Hz, Fs = 1, Fc = .02
	static constexpr size_t NUM_PRES_COEFF = 11;
	static constexpr double NonCPMode_Pressure_Coef[NUM_PRES_COEFF] = {
		8.81131298055604e-06, 0.00211708177469335, 0.0240149216743935,	0.102119533086314,
		0.225973798612221,	  0.291531707078795,   0.225973798612221,	0.102119533086314,
		0.0240149216743935,	  0.00211708177469335, 8.81131298055604e-06};

	static constexpr double CPMode_Pressure_Coef[NUM_PRES_COEFF] = {
		1.57246287686607e-05, 0.00294876425891433, 0.0281799141639923,	0.106973234459911,
		0.221807456935806,	  0.280149811105215,   0.221807456935806,	0.106973234459911,
		0.0281799141639923,	  0.00294876425891433, 1.57246287686607e-05};

	// Designed with for nonCP mode
	// d = fdesign.differentiator('N,Fp,Fst', 8, .1, .4);
	// Hd = design(d, 'equiripple');
	// and for CP mode
	// d = fdesign.differentiator('N,Fp,Fst', 8, .1/3, .4/3);
	// Hd = design(d, 'equiripple')
	static constexpr size_t NUM_VEL_COEFF = 9;
	static constexpr double NonCPMode_Velocity_Coef[NUM_VEL_COEFF] = {
		-0.035538709937166, 0.121018262495538,	0.110204653307495,	0.075228720456011, 0.0,
		-0.075228720456011, -0.110204653307495, -0.121018262495538, 0.035538709937166};

	static constexpr double CPMode_Velocity_Coef[NUM_VEL_COEFF] = {
		0.123601865482124,	0.000071541838701,	0.000051713468660,	0.000027758225109, 0.0,
		-0.000027758225109, -0.000051713468660, -0.000071541838701, -0.123601865482124};
};