/**
* \file dataFillings.cpp
* \author basile.graf@epfl.ch
* 
* This file contains stuff for updating the datas datastructure pointed by *d
*/



#include "math.h"
#include "mex.h"  
#include "string.h"

#include "Datas.h"
#include "dataFillings.h"


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



//init datas computable without P.para nor state:
void data_init(datas* d)
{
	d->g = 9.81;

	d->Long = 18.0;

	d->tanCal_left = 0.042; //Max LD
	d->tanCal_right = 0.042;
	d->tanCal_pitch = 0.0;

	d->L_mast = 27.0;
	d->A_sail = 165.0;
	d->Baume = 10.0;
	d->A_structure = 50.0;
	d->CD_structure = 0.0;

	d->Ix = 4.0e5; //2e5
	d->Iy = 2.0e5;
	d->Iz = 3.5e5;
	d->M = 7.0e3;

	d->E_foils = 6.5;
	d->Chord = 0.5;

		//Min & max chord for the hydrofoils
	d->Chord_min = 0.2;
	d->Chord_max = 2.5;
	d->epsilon = 1.0e-2;

	d->rho = 1.0e3;
	d->rho_air = 1.293;

	d->E_yaw = 3.6; //2;

	d->Env_pitch = 2.0;

	d->Chord_yaw = 0.3;
	d->Chord_pitch = 0.3;

	d->Wind = 10;

	d->Wave_angle0 = pi/4.0;
	d->Wave_amp0 = 0.6;
	d->V_wave0 = 2.0;//0.0;

	d->Fac_update = 1.0;



	d->A_pitch = d->Env_pitch * d->Chord_pitch; 

	d->L_nose = 3.0;

	d->target_x = 0.0f;
	d->target_y = 0.0f;
	d->Vslider_x = 0.0f;
	d->Hslider_x = 0.0f;
}

//Update datas computable from state
void data_update_state(datas* d, double *state)
{
	d->x = state[0];
	d->y = state[1]; 
	d->z = state[2]; 

	d->phi = state[3]; 
	d->theta = state[4]; 
	d->psi = state[5]; 

	d->dx = state[6];
	d->dy = state[7]; 
	d->dz = state[8]; 

	d->dphi = state[9]; 
	d->dtheta = state[10];
	d->dpsi = state[11];

	/*d->x_cam = state[12]; //unused camera dynamic stuff
	d->y_cam = state[13];
	d->z_cam = state[14];

	d->dx_cam = state[15];
	d->dy_cam = state[16];
	d->dz_cam = state[17];*/
}


//Update datas computable from parameters
void data_update_param(datas* d, parameters *P)
{
	d->x_foils = P->param[0];
	d->tanCal_yaw0 = P->param[1];
	d->tanAngle_sail = P->param[4];
	d->G_shift = 0.0;//P->param[5];
	d->alpha_foils = P->param[6];
	d->Wind_angle = P->param[8];
	d->z_foils = P->param[9];

	d->alpha_left = d->alpha_foils*pi/180.0;
	d->alpha_right = d->alpha_foils*pi/180.0;

	d->y_foil_left = 12.0- d->G_shift;
	d->y_foil_right = -12.0- d->G_shift;

	d->x_sail = d->x_foils;
	d->y_sail = 0.0 - d->G_shift;
	d->z_sail = d->z_foils + d->L_mast/3.0;


	d->x_yaw = -0.5*d->Long;
	d->y_yaw = 0 - d->G_shift;
	d->z_yaw = d->z_foils-1.5; //E_yaw/2 - 5;

	d->x_pitch = -0.5*d->Long;
	d->y_pitch = 0 - d->G_shift;
	d->z_pitch = d->z_yaw - d->E_yaw;
	
	d->Wind_x = -d->Wind*cos(d->Wind_angle);
	d->Wind_y = -d->Wind*sin(d->Wind_angle);

	d->Env = d->y_foil_left - d->y_foil_right;
	d->Lambda0 = 1.7*d->Env;

	d->N = 2; //length(Lambda);

	d->tanCal_yaw = d->tanCal_yaw0;

	d->Fac_update = 1.0;


	d->Lambda[0] = d->Lambda0/3.0;
	d->Lambda[1] = d->Lambda0/5.0;
	d->Wave_angle[0] = d->Wave_angle0 + 0.5*pi/4.0;
	d->Wave_angle[1] = d->Wave_angle0 + pi/4.0;
	d->Wave_amp[0] = d->Wave_amp0/2.0;
	d->Wave_amp[1] = d->Wave_amp0/4.0;
	d->V_wave[0] = d->V_wave0/2.0;
	d->V_wave[1] = d->V_wave0/4.0;

	d->Lambda1 = d->Lambda[0];
	d->Lambda2 = d->Lambda[1];
	d->Wave_amp1 = d->Wave_amp[0];
	d->Wave_amp2 = d->Wave_amp[1]; 
	d->V_wave1 = d->V_wave[0];
	d->V_wave2 = d->V_wave[1];
	d->Wave_angle1 = d->Wave_angle[0];
	d->Wave_angle2 = d->Wave_angle[1];

	//store neutral cal angles:
	d->Cal_right_neutral = 0.5*(atan(d->tanCal_left)+atan(d->tanCal_right));
	d->Cal_left_neutral  = 0.5*(atan(d->tanCal_left)+atan(d->tanCal_right));
	d->Cal_pitch_neutral = 0.0;//atan(d->tanCal_pitch);
	d->Cal_yaw_neutral   = 0.0;//atan(d->tanCal_yaw);

	//init baume and girouette:
	d->max_abs_angle_baume = atan(d->tanAngle_sail);
	d->angle_girouette = atan(d->tanAngle_sail);
}


void angles_update(datas *d)
{
	float debat = 0.1745; // max cal angle variation (0.1745==10deg)

	d->tanCal_right = tan(d->Cal_right_neutral - debat*d->target_x + debat*d->Vslider_x);
	d->tanCal_left  = tan(d->Cal_left_neutral  + debat*d->target_x + debat*d->Vslider_x);

	d->tanCal_pitch = tan(d->Cal_pitch_neutral + debat*d->target_y);
	d->tanCal_yaw   = tan(d->Cal_yaw_neutral   + debat*d->Hslider_x);
	d->tanCal_yaw0 = d->tanCal_yaw;
}