#include <fstream>
#include <iostream>

#include "utils.hpp"
#include "FlyByWire.hpp"

using std::cout;
using std::endl;

using FlyByWire::Real;
using FlyByWire::Complex;

int main()
{
	cout.precision(16);

	if(0)
	{// step response of simple 1st order filter
		double	tend = 10.,
				dt = 0.1,
				t, y = 0.;
		FlyByWire::Filter1 filt1(1, 2, 3, 4, dt, y);

		for(int i = 0 ; i <= (tend/dt) ; i++)
		{
			t = i*dt;
			cout << t << " " << y << "\n";
			y = filt1.Filter(1.);
		}
	}

	// if(0)
	{// Generate some uniform noise and filter it to see if the filter class works as expected
		uint32_t state[5] = {123456, 654897, 812656, 87913951, 0};

		double	tend = 10.,
				dt = 0.025,
				omega = 2*M_PI*10,
				Q = 20.,
				t, y = 0., y2;
		FlyByWire::Filter2 filt1 = FlyByWire::Filter2::Bandpass(omega, Q, dt);

		std::ofstream out("data_out.m", std::ios_base::out | std::ios_base::trunc);

		if(!out.is_open())
		{
			std::cerr << "Could not open the file in writing mode.\n";
			return 1;
		}

		out << "a = [";
		for(int i = 0 ; i <= (tend/dt) ; i++)
		{
			t = i*dt;
			y = RAND_XORWOW_A_B(state, -1., 1.);
			y2 = filt1.Filter(y);
			out << t << " " << y << " " << y2 << "\n";
		}
		out << "];\nplot(a(:,1), a(:,2), a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('original', 'filtered')";
	}

	return 0;
}