/**
* \file compute_state_dot.cpp
* \author basile.graf@epfl.ch
* 
* This file contains stuff related to evaluation
* of \f$ f \f$ in \f$ \frac{\partial\vec{q}}{\partial t}=f(\vec{q}, \vec{Q}, \vec{p}) \f$
*/




#include "math.h"
#include "mex.h"   //--This one is required
#include "string.h"

#include "hydro_includes.h"



#ifndef PI_DEF 
#define pi 3.141592653589793 
#define PI 3.141592653589793 
#endif 



/// Evaluates right hand side of partial differential equation
/**
* This function evaluates \f$ f \f$ in \f$ \frac{\partial\vec{q}}{\partial t}=f(\vec{q}, \vec{Q}, \vec{p}) \f$ \n
* \f$ \vec{q} \f$ : state, i.e. state \n
* \f$ \vec{Q} \f$ : generalized applied forces, i.e. computed from *d and *P \n
* \f$ \vec{p} \f$ : lots of parameters , i.e. from *d and *P \n
* \param *d : pointer to the datas data-structure
* \param *P : parameters struct comming from MATLAB
* \param state: state vector (double[12])
*/
void compute_state_dot(struct datas *d, struct parameters *P, double *state)
{
	int k;

//	data_init(d);
//	data_update_param(d, P);
	data_update_state(d, state);

	//d->tanCal_yaw = d->tanCal_yaw0;

	EE_flat_compute(d);
	


	d->EE_left = d->EE_left_flat;
	d->EE_right = d->EE_right_flat;
	d->EE_yaw = d->EE_yaw_flat;

	
	d->Fac_update = 1.0;

	for (k=0; k<2; k++)
	{

		equ_compute(d);
	   
		d->EE_left = d->EE_left - d->Fac_update*d->Equ_left/d->dEqu_left;
		d->EE_right = d->EE_right - d->Fac_update*d->Equ_right/d->dEqu_right;
		d->EE_yaw = d->EE_yaw - d->Fac_update*d->Equ_yaw/d->dEqu_yaw;
	}


	if (d->EE_left+d->epsilon < 0) d->EE_left = d->epsilon;
	if (d->EE_right+d->epsilon < 0) d->EE_right = d->epsilon;
	if (d->EE_left > d->E_foils-d->epsilon) d->EE_left = d->E_foils-d->epsilon;
	if (d->EE_right > d->E_foils-d->epsilon) d->EE_right = d->E_foils-d->epsilon;
	if (d->EE_yaw > d->E_yaw-d->epsilon) d->EE_yaw = d->E_yaw-d->epsilon;
	if (d->EE_yaw+d->epsilon < 0) d->EE_yaw = d->epsilon;


	angles_compute(d);

	
//	if (d->Tan_AoA_sail < 0)  d->Tan_AoA_sail = 0;


	aero_coeff_control(d);

	forces_compute(d);

	acceleration_compute(d);


/*	//% System definition //unused camera stuff

	if (d->x+P->x_rel < P->x0)
	{
		d->dd_cam[0] = 0.0;
		d->dd_cam[1] = 0.0; 
		d->dd_cam[2] = 0.0;
		
		d->d_cam[0] = 0.0;
		d->d_cam[1] = 0.0; 
		d->d_cam[2] = 0.0;
	}
	else
	{
		if (d->t < P->time_stop)
		{
		d->dd_cam[0] = P->Gain * (P->alpha_cam*(d->x_cam - (d->x + P->x_rel)) + P->beta_cam*(d->dx_cam - d->dx));
		d->dd_cam[1] = P->Gain * (P->alpha_cam*(d->y_cam - (d->y + P->y_rel)) + P->beta_cam*(d->dy_cam - d->dy));
		d->dd_cam[2] = P->Gain * (P->alpha_cam*(d->z_cam - (d->z + P->z_rel)) + P->beta_cam*(d->dz_cam - d->dz));
		}
		else
		{
    
		d->dd_cam[0] = P->Gain * (P->beta_cam*d->dx_cam/3.0);
        d->dd_cam[1] = P->Gain * (P->beta_cam*d->dy_cam/3.0);
        d->dd_cam[2] = P->Gain * (P->beta_cam*d->dz_cam/3.0 + 1.0);
		}
	}

*/

	

	return;
}


