#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 = 100., dt = 1./50.;
	const Real g = 1*9.81, wMag = 1*1.;
	// const Real Kp = 0., Ki = .1, Kd = 0.;
	const Real Kp = 10, Ki = Kp*.2, Kd = Kp*0.5;
	Real x = 5., 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

	// Check initialization of PID
	PRINT_VAR(pid.integrator.Ts);
	PRINT_VAR(pid.integrator.x_n_1);
	PRINT_VAR(pid.integrator.y_n_1);
	PRINT_VAR(pid.integrator.lower_bound);
	PRINT_VAR(pid.integrator.upper_bound);

	// 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 = 1.*FlyByWire::Sat1(2. * sin(2*M_PI*t/10), -1., 1.);
		e = xd-x;

		// change pid settings on the fly
		// force freeze integration
		if(t >= 10. && t <= 20.)
			pid.SetFreezeIntegration(true);
		else
			pid.SetFreezeIntegration(false);

		// change output limits
		if(t >= 40. && t <= 50.)
		{
			pid.output_lower_bound = 7.;
			pid.output_upper_bound = 10.;
		}
		else
		{
			pid.output_lower_bound = -15.;//HUGE_VALUE_REAL
			pid.output_upper_bound =  15.;
		}

		// 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 << " "// 1
			<< xd << " "// 2
			<< x << " "// 3
			<< xdot << " "// 4
			<< xdotdot << " "// 5
			<< e << " "// 6
			<< u << " "// 7
			<< w << " "// 8
			<< pid.integrator.y_n_1 << " "// 9
			<< pid.ComputeFreezeIntegrator() << " "// 10
			<< "\n";
	}

	out << "];\nfigure(1), subplot(2,2,1), plot(a(:,1), a(:,2), 'linewidth', 2., a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('xd', 'x')\n";
	out << "subplot(2,2,2), plot(a(:,1), a(:,6), 'linewidth', 2., a(:,1), a(:,7), 'linewidth', 2.), grid on, legend('e', 'u')\n";
	out << "subplot(2,2,3), plot(a(:,1), a(:,9), 'linewidth', 2.), grid on, legend('int(e)')\n";
	out << "subplot(2,2,4), plot(a(:,1), a(:,10), 'linewidth', 2.), grid on, legend('freeze integration')\n";
}