#include "KalmanFilter.hpp"

// Include however you write out files and generate random gaussian variables here.
// You will also need to change them in the code below.
//#include <Filesystem/StorageSubsystem.hpp>
//#include <Math/GaussianRandomFloat.h>
//#include <Streams/OutputStreamWriter.h>

int main
{
// Open a file and get ready to write to it.
	const Soma::Filesystem::File outputFile("KalmanFilter.txt");
	OutputStream* pOut = Engine::getEngine()->getStorageSubsystem()->openOutput( outputFile, true );
	PrintWriter pw(new OutputStreamWriter(pOut, true), true);

	Soma::Math::KalmanFilter kalmanTest;

	const float accelUncer = 0.05f;// m/s/s. Uncertainty in output of accelerometer.
// Measurement uncert already set up in here.
	const float measUncer = kalmanTest.getMeasurementPropUncertainty(0);

// Updates every hundredth of a second. In genereal it does not have to be constant.
	const float deltaTime = 0.01f;

// Random variables for testing the filter.
	Soma::GaussianRandomFloat randomAccel(0.0f, accelUncer );
	Soma::GaussianRandomFloat randomPos(0.0f, measUncer );

// Used to output state estimates.
	float accelBiasEstimate = 0.0f;
	float accelBiasUncertainty = 0.0f;

// These are considered the "real" bias and positions
	const float avePosEst = 1.01f;
	const float accelBias = 0.25f;// m/s/s

// These are the fake 'real' input.
	float measuredAccel;
	float testMeasurement;

// Set the positon equal to a first estimate.
	testMeasurement = avePosEst + randomPos.next();
	kalmanTest.setCurrentStateBestEstimate(0,testMeasurement);

// Run for 2 seconds.
	for( int i = 0; i < 200; ++i )
	{
// Get the current estimate.
		accelBiasEstimate = kalmanTest.getCurrentStateBestEstimate(2);
		accelBiasUncertainty = kalmanTest.getCurrentStateUncertainty(2);

// Output the current estimate.
		pw.print( (float) (i) / 100.0f );
		pw.print(", ");
		pw.print( accelBiasEstimate );
		pw.print(", ");
		pw.println(accelBiasUncertainty);

// Make up the next 'real' input.
		measuredAccel = accelBias + randomAccel.next();
		testMeasurement = avePosEst + randomPos.next();

// First propagate from the previous time to the current time
		kalmanTest.updateTime( deltaTime, measuredAccel );

// Then update the filter with the current position measurement 
		kalmanTest.updateMeasurement( &testMeasurement );
	}
}