/* library needed to include the int16_t and int32_t types.
it is needed also in the floating point controllers for the
definition of outputs and inputs. */
#include <inttypes.h>
#include <stdlib.h>
#include <stdio.h>

#define FRAC_BITS 6 //16-bits, 6 fractional bits. 2^6 = 64.
#define int2fix(x) ((x)<<FRAC_BITS) // shifts x to the left by a nr of bits
#define fix2int(x) ((x)>>FRAC_BITS) // shifts x to the right by a nr of bits
/*********************************/
// define controller states here //
/*********************************/
/*
Here use only int16_t variables for the fixed-point
implementation.
*/

int16_t pos_fixed(int16_t r, int16_t y){
	// To prevent that the variables are re-allocated every
	// time (and therefore erased) you can use the keyword static

	static int16_t x1 = 0; // The estimated angular velocity state (not shifted up)
	static int16_t x2 = 0; // The estimated angular position state (not shifted up)
	static int16_t v = 0;  // Integral term of the control signal (not shifted up)

	int16_t x1_new = 0; // The updated x1-state (not shifted up)
	int16_t x2_new = 0; // The updated x2-state (not shifted up)
	int16_t v_new = 0; // The updated v-state (not shifted up)

	// The phi matrix is the A matrix for the discretised system.
	int16_t phi11_fp = 64; //Fixed point value of the first member of the phi matrix, original value: 0.9940;
	int16_t phi12_fp = 0; //Fixed point value of the second member of the phi matrix, original value: 0
	int16_t phi21_fp = 16; //Fixed point value of the third member of the phi matrix, original value: 0.2493;
	int16_t phi22_fp = 64; //Fixed point value of the fourth member of the phi matrix, original value: 1;

	// The gamma vector is the B matrix for the discretised system.
	int16_t gamma1_fp = 7; //Fixed point value of the first member of the gamma vector, original value: 0.1122;
	int16_t gamma2_fp = 1; //Fixed point value of the second member of the gamma vector, original value: 0.0140;

	// The L vector is the feedback term
	int16_t L1_fp = 211; //Fixed point value of the first member of the L vector, original value: 3.2898;
	int16_t L2_fp = 114; //Fixed point value of the second member of the L vector, original value: 1.7831;

	// Lr is the scaling factor to achieve the right amplitude of the step response
	int16_t lr_fp = 114; //Fixed point value of the scaling factor, original value: 1.7831;

	// Ke is the augmented observer vector
	int16_t Ke1_fp = 128; //Fixed point value of the first member of the Ke vector, original value: 2.0361;
	int16_t Ke2_fp = 80; //Fixed point value of the second member of the gamma vector, original value: 1.2440;
	int16_t Kev_fp = 205; //Fixed point value of the third member of the gamma vector, original value: 3.2096;

	int16_t u = 0; // The control signal (not shifted up)
	int16_t eps = 0; // Epsilon is (measured angular pos) - (estimated angular position), so it is the estimated angular pos error. (not shifted up)

    /**********************************/
    // Implement your controller here //
    /**********************************/
    /*
    Use only int32_t and int16_t variables for the fixed-point
    implementation.
    */

	//The factors of the u-value are calculated separately, cast to int32_t and shifted down to avoid overflow, before being summed.
	int32_t lr_r = fix2int(lr_fp*r);
	int32_t L1_x1 = fix2int(L1_fp*x1);
	int32_t L2_x2 = fix2int(L2_fp*x2);

	u = lr_r-L1_x1-L2_x2-v; //Function for u: u = lr_fp*r-L1_fp*x1-L2_fp*x2-v

	//Calculate the new epsilon value (for the next sample)
	eps = y-x2;

	//update the accumulated control signal value
	int32_t u_v = u+v;

	//The factors of the next x1-value are calculated separately, cast to int32_t and shifted down to avoid overflow, before being summed.
	int32_t phi11_x1 =  fix2int(phi11_fp*x1);
	int32_t phi12_x2 = fix2int(phi12_fp*x2);
	int32_t gamma1_uv = fix2int(gamma1_fp*u_v);
	int32_t K1_eps = fix2int(Ke1_fp*eps);

	x1_new = phi11_x1 + phi12_x2 + gamma1_uv + K1_eps; // The estimated angular velocity of the next sample

	//The factors of the next x2-value are calculated separately, cast to int32_t and shifted down to avoid overflow, before being summed.
	int32_t phi21_x1 =  fix2int(phi21_fp*x1);
	int32_t phi22_x2 = fix2int(phi22_fp*x2);
	int32_t gamma2_uv = fix2int(gamma2_fp*u_v);
	int32_t K2_eps = fix2int(Ke2_fp*eps);

	x2_new = phi21_x1 + phi22_x2 + gamma2_uv + K2_eps; // The estimated angular position of the next sample

	//The factors of the next v-value are calculated separately, cast to int32_t and shifted down to avoid overflow, before being summed.
	int32_t Kev_eps = fix2int(Kev_fp*eps);
	v_new = Kev_eps + v; //The updated integral value of the control signal

	//Update variables for next loop
	x1 = x1_new;
	x2 = x2_new;
	v = v_new;

	if(u > 511){ //make sure the control signal is not too big
		u = 511;
	}else if(u < -512){ //and not too small
		u = -512;
	}

    return u; // write output to D/A and end function
}