#include <math.h>

#include "assemblage_includes.h"
#ifdef USE_FLOAT
#define MATH_COS cosf
#define MATH_SIN sinf
#else
#define MATH_COS cos
#define MATH_SIN sin
#endif
/*
 * The following include file is generated by the Prelude compiler
 * Theoretically we should include main node specific include
 * i.e. assemblage_vX.h but we know that every main node
 * assemblage, assemblage_v2, assemblage_v3, etc... share the
 * very same data type.
 */
//#include "assemblage.h"

/* Period/Frequency of the nodes */
// const REAL_TYPE Ts_h    = 1.0/50.0;
// const REAL_TYPE Ts_K1   = 1.0/50.0;
// const REAL_TYPE Ts_K2   = 1.0/50.0;
// const REAL_TYPE Ts_f_Va = 1.0/100.0;
// const REAL_TYPE Ts_f_Vz = 1.0/100.0;
// const REAL_TYPE Ts_f_q  = 1.0/100.0;
// const REAL_TYPE Ts_f_az = 1.0/100.0;
// const REAL_TYPE Ts_f_h  = 1.0/100.0;
const REAL_TYPE dt       = 1.0f/200.0;
const REAL_TYPE dt_de    = 1.0/200.0;
const REAL_TYPE dt_dx    = 1.0/200.0;

/* Controller parameters */
/* Altitude hold */
const REAL_TYPE Kp_h       = 0.1014048;
const REAL_TYPE Ki_h       = 0.0048288;
const REAL_TYPE h_switch   = 50.0;


// Setpoint commands
REAL_TYPE Vz_c     = -2.5;
REAL_TYPE Va_c     =  0.0;
REAL_TYPE h_c      =  10000;

/* Va Speed controller */

// const REAL_TYPE K1_intVa =  0.06018;
// const REAL_TYPE K1_Va    = -0.53115;
// const REAL_TYPE K1_Vz    = -0.08956;
// const REAL_TYPE K1_q     =  24.44890;

const REAL_TYPE K1_intVa =   0.049802610664357;
const REAL_TYPE K1_Va     = -0.486813084356079;
const REAL_TYPE K1_Vz     = -0.077603095495388;
const REAL_TYPE K1_q      = 21.692383376322041;

/* Vz Speed controller */

// const REAL_TYPE K2_intVz =  0.0006545;
// const REAL_TYPE K2_Vz    = -0.0031107;
// const REAL_TYPE K2_q     =  0.4490749;
// const REAL_TYPE K2_az    = -0.0002038;

const REAL_TYPE K2_intVz   = 0.000627342822264;
const REAL_TYPE K2_Vz     = -0.003252836726554;
const REAL_TYPE K2_q       = 0.376071446897134;
const REAL_TYPE K2_az     = -0.001566907423747;


/* Trimming parameters */
const REAL_TYPE h_eq        = 10000.0;
const REAL_TYPE Va_eq       = 230.0;
const REAL_TYPE Vz_eq       = 0.0;
const REAL_TYPE alpha_eq    = 0.026485847681737;
const REAL_TYPE theta_eq  = 0.026485847681737;

/* Atmosphere parameters */
const REAL_TYPE rho0   = 1.225;
const REAL_TYPE g0     = 9.80665;
const REAL_TYPE T0_0   = 288.15;
const REAL_TYPE T0_h  = -0.0065;
const REAL_TYPE Rs     = 287.05;

/* Aircraft parameters */
const REAL_TYPE masse       = 57837.5;
const REAL_TYPE I_y         = 3781272.0;
const REAL_TYPE S          =  122.6;
const REAL_TYPE cbar        = 4.29;
const REAL_TYPE CD_0        = 0.016;
const REAL_TYPE CD_alpha    = 2.5;
const REAL_TYPE CD_deltae   = 0.05;
const REAL_TYPE CL_alpha    = 5.5;
const REAL_TYPE CL_deltae   = 0.193;
const REAL_TYPE alpha_0    = -0.05;
const REAL_TYPE Cm_0        = 0.04;
const REAL_TYPE Cm_alpha   = -0.83;
const REAL_TYPE Cm_deltae  = -1.5;
const REAL_TYPE Cm_q       = -30;

/* in-memory buffer for traces */
//REAL_TYPE sample[SPL_SIZE][NBMAX_SAMPLE];
//static unsigned long instant = 0;
//static unsigned long sample_instant=0;

#define FMTFLOAT "%5.15f"

/* Va filter 100 Hz */
REAL_TYPE
Va_filter_100(REAL_TYPE Va) {
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 100 Hz coefficients */
	static REAL_TYPE a0 = 0.956543675476034;
	static REAL_TYPE a1 = -1.955578398054313;
	static REAL_TYPE b0 = 0.000479064865372430;
	static REAL_TYPE b1 =  0.000486212556348925;

	if (debut) {
		debut  = 0;
		x1     = Va_eq * (1.0 + a1 - b1);
		x2     = Va_eq;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * Va;
	x2_tmp =  x1 - a1 * x2 + b1 * Va;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of Va filter 100 Hz */

/* Va filter 50 Hz */
REAL_TYPE
Va_filter_50(REAL_TYPE Va) {
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 50 Hz coefficients */
	static REAL_TYPE a0 = 0.914975803093201;
	static REAL_TYPE a1 = -1.911199519984605;
	static REAL_TYPE b0 = 0.001860178914816;
	static REAL_TYPE b1 =  0.001916104193780;

	if (debut) {
		debut  = 0;
		x1     = Va_eq * (1.0 + a1 - b1);
		x2     = Va_eq;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * Va;
	x2_tmp =  x1 - a1 * x2 + b1 * Va;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of Va filter 50 Hz */

/* Va filter 33 Hz */
REAL_TYPE
Va_filter_33(REAL_TYPE Va) {
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 33 Hz coefficients */
	static REAL_TYPE a0 = 0.874036784828483;
	static REAL_TYPE a1 = -1.865563793814790;
	static REAL_TYPE b0 = 0.004141433623051;
	static REAL_TYPE b1 =  0.004331557390642;

	if (debut) {
		debut  = 0;
		x1     = Va_eq * (1.0 + a1 - b1);
		x2     = Va_eq;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * Va;
	x2_tmp =  x1 - a1 * x2 + b1 * Va;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of Va filter 33 Hz */

/* Va filter 25 Hz */
REAL_TYPE
Va_filter_25(REAL_TYPE Va) {
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE	x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 25 Hz coefficients */
	static REAL_TYPE a0 = 0.837180720246048;
	static REAL_TYPE a1 = -1.822731999002980;
	static REAL_TYPE b0 = 0.007010380719078;
	static REAL_TYPE b1 =  0.007438340523990;

	if (debut) {
		debut  = 0;
		x1     = Va_eq * (1.0 + a1 - b1);
		x2     = Va_eq;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * Va;
	x2_tmp =  x1 - a1 * x2 + b1 * Va;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of Va filter 25 Hz */


/* Vz filter 100 Hz */
REAL_TYPE
Vz_filter_100(REAL_TYPE Vz) {
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 100 Hz coefficients */
	static REAL_TYPE a0 = 0.956543675476034;
	static REAL_TYPE 	a1 = -1.955578398054313;
	static REAL_TYPE b0 = 0.000479064865372430;
	static REAL_TYPE 	b1 =  0.000486212556348925;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * Vz;
	x2_tmp =  x1 - a1 * x2 + b1 * Vz;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of Vz filter 100 Hz */

/* Vz filter 50 Hz */
REAL_TYPE
Vz_filter_50(REAL_TYPE Vz) {
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0, x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 50 Hz coefficients */
	static REAL_TYPE a0 = 0.914975803093201;
	static REAL_TYPE a1 = -1.911199519984605;
	static REAL_TYPE b0 = 0.001860178914816;
	static REAL_TYPE b1 =  0.001916104193780;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * Vz;
	x2_tmp =  x1 - a1 * x2 + b1 * Vz;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of Vz filter 50 Hz */

/* Vz filter 33 Hz */
REAL_TYPE
Vz_filter_33(REAL_TYPE Vz) {
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0, x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 33 Hz coefficients */
	static REAL_TYPE a0 = 0.874036784828483;
	static REAL_TYPE a1 = -1.865563793814790;
	static REAL_TYPE b0 = 0.004141433623051;
	static REAL_TYPE b1 =  0.004331557390642;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * Vz;
	x2_tmp =  x1 - a1 * x2 + b1 * Vz;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of Vz filter 33 Hz */

/* Vz filter 25 Hz */
REAL_TYPE
Vz_filter_25(REAL_TYPE Vz) {
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 25 Hz coefficients */
	static REAL_TYPE a0 = 0.837180720246048;
	static REAL_TYPE a1 = -1.822731999002980;
	static REAL_TYPE b0 = 0.007010380719078;
	static REAL_TYPE b1 =  0.007438340523990;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * Vz;
	x2_tmp =  x1 - a1 * x2 + b1 * Vz;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of Vz filter 25 Hz */

/* q filter 100 Hz */
REAL_TYPE
q_filter_100(REAL_TYPE q){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 100 Hz coefficients */
	static REAL_TYPE a0 = 0.766000101841272;
	static REAL_TYPE a1 = -1.734903205885821;
	static REAL_TYPE b0 = 0.014857648981438;
	static REAL_TYPE b1 =  0.016239246974013;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * q;
	x2_tmp =  x1 - a1 * x2 + b1 * q;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of q filter 100 Hz */

/* q filter 50 Hz */
REAL_TYPE
q_filter_50(REAL_TYPE q){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 50 Hz coefficients */
	static REAL_TYPE a0 = 0.586756156020839;
	static REAL_TYPE a1 = -1.477888930110354;
	static REAL_TYPE b0 = 0.049596808318647; 
	static REAL_TYPE b1 =  0.059270417591839;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * q;
	x2_tmp =  x1 - a1 * x2 + b1 * q;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of q filter 50 Hz */

/* q filter 33 Hz */
REAL_TYPE
q_filter_33(REAL_TYPE q){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 33 Hz coefficients */
	static REAL_TYPE a0 = 0.445839214374383;
	static REAL_TYPE a1 = -1.227970132817902;
	static REAL_TYPE b0 = 0.094268996251840;
	static REAL_TYPE b1 =  0.123600085304640;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * q;
	x2_tmp =  x1 - a1 * x2 + b1 * q;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of q filter 33 Hz */

/* q filter 25 Hz */
REAL_TYPE
q_filter_25(REAL_TYPE q){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 25 Hz coefficients */
	static REAL_TYPE a0 = 0.344282786628352;
	static REAL_TYPE a1 = -1.010643377701049;
	static REAL_TYPE b0 = 0.137177088974822;
	static REAL_TYPE b1 =  0.196462319952482;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * q;
	x2_tmp =  x1 - a1 * x2 + b1 * q;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of q filter 25 Hz */

/* az filter 100 Hz */
REAL_TYPE
az_filter_100(REAL_TYPE az){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 100 Hz coefficient */
	static REAL_TYPE a0 = 0.411240701442774;
	static REAL_TYPE a1 = -1.158045899830964;
	static REAL_TYPE b0 = 0.107849979167580;
	static REAL_TYPE b1 =  0.145344822444230;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * az;
	x2_tmp =  x1 - a1 * x2 + b1 * az;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of az filter 100 Hz */

/* az filter 50 Hz */
REAL_TYPE
az_filter_50(REAL_TYPE az){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 50 Hz coefficients */
	static REAL_TYPE a0 = 0.169118914523145;
	static REAL_TYPE a1 = -0.518588903229759;
	static REAL_TYPE b0 = 0.229019233988375;
	static REAL_TYPE b1 =  0.421510777305010;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * az;
	x2_tmp =  x1 - a1 * x2 + b1 * az;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of az filter 50 Hz */

/* az filter 33 Hz */
REAL_TYPE
az_filter_33(REAL_TYPE az){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 33 Hz coefficients */
	static REAL_TYPE a0 = 0.067700864731348;
	static REAL_TYPE a1 = -0.115832026705568;
	static REAL_TYPE b0 = 0.263451167882487;
	static REAL_TYPE b1 =  0.688417670143293;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * az;
	x2_tmp =  x1 - a1 * x2 + b1 * az;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of az filter 33 Hz */

/* az filter 25 Hz */
REAL_TYPE
az_filter_25(REAL_TYPE az){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 25 Hz coefficients */
	static REAL_TYPE a0 = 0.028601207249487;
	static REAL_TYPE a1 =  0.069303378493245;
	static REAL_TYPE b0 = 0.228783762747218;
	static REAL_TYPE b1 =  0.869120822995514;

	if (debut) {
		debut  = 0;
		x1     = 0.0;
		x2     = 0.0;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * az;
	x2_tmp =  x1 - a1 * x2 + b1 * az;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of az filter 25 Hz */

/* h filter 100 Hz*/
REAL_TYPE
h_filter_100(REAL_TYPE h){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 100 Hz coefficients */
	static REAL_TYPE a0 = 0.766000101841272;
	static REAL_TYPE a1 = -1.734903205885821;
	static REAL_TYPE b0 = 0.014857648981438;
	static REAL_TYPE b1 =  0.016239246974013;

	if (debut) {
		debut  = 0;
		x1     = h_eq * (1.0 + a1 - b1);
		x2     = h_eq;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * h;
	x2_tmp =  x1 - a1 * x2 + b1 * h;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of h filter 100 Hz */

/* h filter 50 Hz*/
REAL_TYPE
h_filter_50(REAL_TYPE h){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 50 Hz coefficients */
	static REAL_TYPE a0 = 0.586756156020839;
	static REAL_TYPE a1 = -1.477888930110354;
	static REAL_TYPE b0 = 0.049596808318647;
	static REAL_TYPE b1 =  0.059270417591839;

	if (debut) {
		debut  = 0;
		x1     = h_eq * (1.0 + a1 - b1);
		x2     = h_eq;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * h;
	x2_tmp =  x1 - a1 * x2 + b1 * h;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of h filter 50 Hz */

/* h filter 33 Hz*/
REAL_TYPE
h_filter_33(REAL_TYPE h){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 33 Hz coefficients */
	static REAL_TYPE a0 = 0.445839214374383;
	static REAL_TYPE a1 = -1.227970132817902;
	static REAL_TYPE b0 = 0.094268996251840;
	static REAL_TYPE b1 =  0.123600085304640;

	if (debut) {
		debut  = 0;
		x1     = h_eq * (1.0 + a1 - b1);
		x2     = h_eq;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * h;
	x2_tmp =  x1 - a1 * x2 + b1 * h;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of h filter 33 Hz */

/* h filter 25 Hz*/
REAL_TYPE
h_filter_25(REAL_TYPE h){
	static REAL_TYPE y  = 0.0;
	static REAL_TYPE x1 = 0.0;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_tmp = 0.0;
	static REAL_TYPE x2_tmp = 0.0;
	static unsigned short debut = 1;
	/* 25 Hz coefficients */
	static REAL_TYPE a0 = 0.344282786628352;
	static REAL_TYPE a1 = -1.010643377701049;
	static REAL_TYPE b0 = 0.137177088974822;
	static REAL_TYPE b1 =  0.196462319952482; /**/

	if (debut) {
		debut  = 0;
		x1     = h_eq * (1.0 + a1 - b1);
		x2     = h_eq;
	}
	// Output
	y = x2;
	// State
	x1_tmp =     - a0 * x2 + b0 * h;
	x2_tmp =  x1 - a1 * x2 + b1 * h;
	// Update
	x1 = x1_tmp;
	x2 = x2_tmp;

	return y;
} /* end of h filter 25 Hz */


/* Altitude hold controller 50 Hz */

REAL_TYPE
altitude_hold_50(REAL_TYPE h_f, REAL_TYPE h_c){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_h = 1.0/50.0;
	static REAL_TYPE integrator = 532.2730285;

	if ((h_f - h_c) < -50) {
		// Output
		y = Vz_c;
	}
	else if ((h_f - h_c) > 50) {
		// Output
		y = -Vz_c;
	}
	else {
		// Output
		y = Kp_h * (h_f - h_c) + Ki_h * integrator;
		// State
		integrator += Ts_h * (h_f - h_c);
	}

	return y;
}

/* Altitude hold controller 33 Hz */

REAL_TYPE
altitude_hold_33(REAL_TYPE h_f, REAL_TYPE h_c){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_h = 1.0/33.0;
	static REAL_TYPE integrator = 532.2730285;

	if ((h_f - h_c) < -50) {
		// Output
		y = Vz_c;
	}
	else if ((h_f - h_c) > 50) {
		// Output
		y = -Vz_c;
	}
	else {
		// Output
		y = Kp_h * (h_f - h_c) + Ki_h * integrator;
		// State
		integrator += Ts_h * (h_f - h_c);
	}

	return y;
}

/* Altitude hold controller 25 Hz */

REAL_TYPE
altitude_hold_25(REAL_TYPE h_f, REAL_TYPE h_c){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_h = 1.0/25.0;
	static REAL_TYPE integrator = 532.2730285;

	if ((h_f - h_c) < -50) {
		// Output
		y = Vz_c;
	}
	else if ((h_f - h_c) > 50) {
		// Output
		y = -Vz_c;
	}
	else {
		// Output
		y = Kp_h * (h_f - h_c) + Ki_h * integrator;
		// State
		integrator += Ts_h * (h_f - h_c);
	}

	return y;
}

/* Altitude hold controller 10 Hz */

REAL_TYPE
altitude_hold_10(REAL_TYPE h_f, REAL_TYPE h_c){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_h = 1.0/10.0;
	static REAL_TYPE integrator = 532.2730285;

	if ((h_f - h_c) < -50) {
		// Output
		y = Vz_c;
	}
	else if ((h_f - h_c) > 50) {
		// Output
		y = -Vz_c;
	}
	else {
		// Output
		y = Kp_h * (h_f - h_c) + Ki_h * integrator;
		// State
		integrator += Ts_h * (h_f - h_c);
	}

	return y;
}

/* Va Speed controller 50 Hz */
REAL_TYPE
Va_control_50(REAL_TYPE Va_f, REAL_TYPE Vz_f, REAL_TYPE q_f, REAL_TYPE Va_c){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_K1 = 1.0/50.0;
	static REAL_TYPE integrator = 0.0;

	// Output
	y = K1_intVa * integrator + K1_Va * (Va_f - Va_eq) + K1_Vz * Vz_f + K1_q * q_f + delta_th_eq;
	// State
	integrator += Ts_K1 * (Va_c - Va_f + Va_eq);

	return y;
}

/* Va Speed controller 33 Hz */
REAL_TYPE
Va_control_33(REAL_TYPE Va_f, REAL_TYPE Vz_f, REAL_TYPE q_f, REAL_TYPE Va_c){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_K1 = 1.0/33.0;
	static REAL_TYPE integrator = 0.0;

	// Output
	y = K1_intVa * integrator + K1_Va * (Va_f - Va_eq) + K1_Vz * Vz_f + K1_q * q_f + delta_th_eq;
	// State
	integrator += Ts_K1 * (Va_c - Va_f + Va_eq);

	return y;
}

/* Va Speed controller 25 Hz */
REAL_TYPE
Va_control_25(REAL_TYPE Va_f, REAL_TYPE Vz_f, REAL_TYPE q_f, REAL_TYPE Va_c){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_K1 = 1.0/25.0;
	static REAL_TYPE integrator = 0.0;

	// Output
	y = K1_intVa * integrator + K1_Va * (Va_f - Va_eq) + K1_Vz * Vz_f + K1_q * q_f + delta_th_eq;
	// State
	integrator += Ts_K1 * (Va_c - Va_f + Va_eq);

	return y;
}

/* Va Speed controller 10 Hz */
REAL_TYPE
Va_control_10(REAL_TYPE Va_f, REAL_TYPE Vz_f, REAL_TYPE q_f, REAL_TYPE Va_c){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_K1 = 1.0/10.0;
	static REAL_TYPE integrator = 0.0;

	// Output
	y = K1_intVa * integrator + K1_Va * (Va_f - Va_eq) + K1_Vz * Vz_f + K1_q * q_f + delta_th_eq;
	// State
	integrator += Ts_K1 * (Va_c - Va_f + Va_eq);

	return y;
}

/* Vz Speed controller 50 Hz */
REAL_TYPE
Vz_control_50(REAL_TYPE Vz_f, REAL_TYPE Vz_c, REAL_TYPE q_f, REAL_TYPE az_f){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_K2 = 1.0/50.0;
	static REAL_TYPE integrator = 0.0;

	// Output
	y = K2_intVz * integrator + K2_Vz * Vz_f + K2_q * q_f + K2_az * az_f + delta_e_eq;
	// State
	integrator += Ts_K2 * (Vz_c - Vz_f);

	return y;
}

/* Vz Speed controller 33 Hz */
REAL_TYPE
Vz_control_33(REAL_TYPE Vz_f, REAL_TYPE Vz_c, REAL_TYPE q_f, REAL_TYPE az_f){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_K2 = 1.0/33.0;
	static REAL_TYPE integrator = 0.0;

	// Output
	y = K2_intVz * integrator + K2_Vz * Vz_f + K2_q * q_f + K2_az * az_f + delta_e_eq;
	// State
	integrator += Ts_K2 * (Vz_c - Vz_f);

	return y;
}

/* Vz Speed controller 25 Hz */
REAL_TYPE
Vz_control_25(REAL_TYPE Vz_f, REAL_TYPE Vz_c, REAL_TYPE q_f, REAL_TYPE az_f){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_K2 = 1.0/25.0;
	static REAL_TYPE integrator = 0.0;

	// Output
	y = K2_intVz * integrator + K2_Vz * Vz_f + K2_q * q_f + K2_az * az_f + delta_e_eq;
	// State
	integrator += Ts_K2 * (Vz_c - Vz_f);

	return y;
}

/* Vz Speed controller 10 Hz */
REAL_TYPE
Vz_control_10(REAL_TYPE Vz_f, REAL_TYPE Vz_c, REAL_TYPE q_f, REAL_TYPE az_f){
	static REAL_TYPE y = 0.0;
	static REAL_TYPE Ts_K2 = 1.0/10.0;
	static REAL_TYPE integrator = 0.0;

	// Output
	y = K2_intVz * integrator + K2_Vz * Vz_f + K2_q * q_f + K2_az * az_f + delta_e_eq;
	// State
	integrator += Ts_K2 * (Vz_c - Vz_f);

	return y;
}

/* Engine */
REAL_TYPE
engine(REAL_TYPE delta_th_c) {

	static REAL_TYPE y      = delta_th_eq;
	static REAL_TYPE x1     = delta_th_eq;
	static REAL_TYPE x1_dot = 0.0;
	static REAL_TYPE tau    = 0.75;

	// Output
	y = 26350.0 * x1;
	// State Equation
	x1_dot = -tau * x1 + tau * delta_th_c;
	// Update State
	x1 += dt_dx * x1_dot;

	return y;
}

/* Elevator */
REAL_TYPE
elevator(REAL_TYPE delta_e_c) {

	static REAL_TYPE y  = delta_e_eq;
	static REAL_TYPE x1 = delta_e_eq;
	static REAL_TYPE x2 = 0.0;
	static REAL_TYPE x1_dot = 0.0;
	static REAL_TYPE x2_dot = 0.0;
	static REAL_TYPE omega = 25.0;
	static REAL_TYPE xi = 0.85;

	// Output
	y = x1;
	// State Equation
	x1_dot = x2;
	x2_dot = -omega * omega * x1 - 2.0 * xi * omega * x2 + omega * omega * delta_e_c;
	// Update State
	x1 += dt_de * x1_dot;
	x2 += dt_de * x2_dot;

	return y;
}


/* Flight dynamics */
void aircraft_dynamics (REAL_TYPE delta_e, REAL_TYPE T,  struct aircraft_dynamics_outs_t *outputs){

	static int debut = 1;

	static REAL_TYPE u 	= 0.0;
	static REAL_TYPE w 	= 0.0;
	static REAL_TYPE q 	= 0.0;
	static REAL_TYPE theta 	= 0.0;
	static REAL_TYPE h 	= 0.0;
	static REAL_TYPE u_dot 	= 0.0;
	static REAL_TYPE w_dot 	= 0.0;
	static REAL_TYPE q_dot 	= 0.0;
	static REAL_TYPE theta_dot = 0.0;
	static REAL_TYPE h_dot 	= 0.0;

	static REAL_TYPE CD 	= 0.0;
	static REAL_TYPE CL 	= 0.0;
	static REAL_TYPE Cm 	= 0.0;

	static REAL_TYPE Xa 	= 0.0;
	static REAL_TYPE Za 	= 0.0;
	static REAL_TYPE Ma 	= 0.0;

	static REAL_TYPE alpha 	= 0.0;
	static REAL_TYPE qbar 	= 0.0;
	static REAL_TYPE V 	= 0.0;
	static REAL_TYPE rho 	= 0.0;

	if (debut) {
		debut = 0;
		u     = Va_eq * MATH_COS(theta_eq);
		w     = Va_eq * MATH_SIN(theta_eq);
		q     = 0.0;
		theta = theta_eq;
		h     = h_eq;
	}

	rho   = rho0 * pow(1.0 + T0_h / T0_0 * h,- g0 / (Rs * T0_h) - 1.0);
	alpha = atan(w/u);
	V     = sqrt(u * u + w * w);
	qbar  = 0.5 * rho * V * V;
	CL    = CL_deltae * delta_e + CL_alpha * (alpha - alpha_0);
	CD    = CD_0 + CD_deltae * delta_e + CD_alpha * (alpha - alpha_0) * (alpha - alpha_0);
	Cm    = Cm_0 + Cm_deltae * delta_e + Cm_alpha * alpha + 0.5 * Cm_q * q * cbar / V;
	Xa    = - qbar * S * (CD * MATH_COS(alpha) - CL * MATH_SIN(alpha));
	Za    = - qbar * S * (CD * MATH_SIN(alpha) + CL * MATH_COS(alpha));
	Ma    = qbar * cbar * S * Cm;

	// Output
	outputs -> Va = V;
	outputs -> Vz = w * MATH_COS(theta) - u * MATH_SIN(theta);
	outputs -> q  = q;
	outputs -> az = g0 * MATH_COS(theta) + Za / masse;
	outputs -> h  = h;
	// State Equation
	u_dot     = - g0 * sin (theta) - q * w + (Xa + T) / masse;
	w_dot     = g0 * MATH_COS (theta) + q * u + Za / masse;
	q_dot     = Ma / I_y;
	theta_dot = q;
	h_dot     = u * MATH_SIN(theta) - w * MATH_COS(theta);
	// Update State
	u     += dt * u_dot;
	w     += dt * w_dot;
	q     += dt * q_dot;
	theta += dt * theta_dot;
	h     += dt * h_dot;


	static REAL_TYPE Time = 0.0;


	// instant++;
	Time = Time + dt;
}

/*
 * The commanded altitude
 */
REAL_TYPE input_h_c() {
	return h_c;
}

/*
 * The commanded airspeed
 */
REAL_TYPE input_Va_c() {
	return Va_c;
}

void output_delta_th_c(REAL_TYPE delta_th_c){

}

void output_delta_e_c(REAL_TYPE delta_e_c){

}