#include <fstream>
#include <iostream>

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

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

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

void test_Filter1_step_response();
void test_Filter2_random_noise();
void test_PID_simple_system();

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

	// test_Filter1_step_response();
	// test_Filter2_random_noise();
	test_PID_simple_system();

	return 0;
}

void test_Filter1_step_response()
{
	// 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.);
	}
}

void test_Filter2_random_noise()
{
	// 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;
	}

	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')";
}

void test_PID_simple_system()
{
	// Simple mass system with gravity g and perturbation w
	// xdotdot = -g + u + w
	const Real tf = 10., dt = 1./50.;
	const Real g = 9.81, wMag = 1.;
	const Real Kp = 1., Ki = 0.1, Kd = 0.1;
	Real x = 0., xdot = 0., xdotdot = 0., u, w, t, xd, e;

	uint32_t state[5] = {123456, 654897, 812656, 87913951, 0};// noise generator state

	FlyByWire::PID pid(
		Kp,
		Ki,
		Kd,
		dt,
		-HUGE_VALUE_REAL,//output_lower_bound_
		HUGE_VALUE_REAL,//output_upper_bound_
		Real(0.01),//tau_filtered_derivative
		-HUGE_VALUE_REAL,//integrator_lower_bound_
		HUGE_VALUE_REAL,//integrator_upper_bound_
		true,// auto_anti_windup_
		1e-3);// anti_windup margin

	// Output to file
	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;
	}

	out << "a = [";
	for(t = 0. ; t <= tf ; t += dt)
	{
		// compute system target
		xd = sin(2*M_PI*t/2);
		e = xd-x;

		// compute system Input
		// u = pid.Filter(e);

		// simulate system
		w = RAND_XORWOW_A_B(state, -wMag, wMag);
		xdotdot  = -g + u + w;
		xdot 	+= xdotdot*dt;	// dirty Forward Euler
		x 		+= xdot*dt;		// dirty Forward Euler

		// write to file
		out << t << " " << xd << " " << x << " " << xdot << " " << xdotdot << " " << e << " " << u << " " << w << "\n";
	}

	out << "];\nfigure(1), plot(a(:,1), a(:,2), a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('xd', 'x')\n";
	out << "figure(2), plot(a(:,1), a(:,6), a(:,1), a(:,7), 'linewidth', 2.), grid on, legend('e', 'u')\n";
}