/**
* \file get_file_arguments.cpp
* \author basile.graf@epfl.ch
* 
* Functions to read the input parameters from the input parameters file. This file is in the program folder or is generated by MATLAB
*/

#include "get_file_arguments.h"



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


struct readStruct
{
	double SimTime;
	double X0[STATENUM];
	double reltol;
	double abstol[STATENUM];
	double param[PARAMNUM];
	int dirDo[8];	
};


///Initialise parameters struct
/**
* Set parameters values to default/0
* \param *P : pointer to the parameterss struct to be initialized
*/
void initParameters(struct parameters *P) //default values
{
	int i;
		for (i=0; i<PARAMNUM; i++) P->param[i] = 0;
		for (i=0; i<STATENUM; i++) P->abstol[i] = (1e-6);
		P->reltol = (1.0e-4);
	return;
}


///Initialise joystick struct
/**
* Set joystick values to default
* \param *jP : pointer to the joy_parameters struct to be initialized
*/
void initJoyParameters(struct joy_parameters *jP) //default values
{
	jP->dirX = 1.0;
	jP->dirY = 1.0;
	jP->dirZ = 1.0;
	jP->dirR = 1.0;

	jP->doX = TRUE;
	jP->doY = TRUE;
	jP->doZ = TRUE;
	jP->doR = TRUE;
}


///Print parameters and initial conditions
/**
* Print parameters and initial conditions to the standard error
* \param *P     : pointer to the parameters struct 
* \param *jP    : pointer to the joy_parameters struct
* \param *state : pointer to the state vector
* \param *time  : pointer to the time parameter
*/
void print_arguments_values(struct parameters *P, struct joy_parameters *jP, double *state, double *time)
{
	int i;
	fprintf( stderr,"======================= Arguments from file ========================\n");
	fprintf( stderr,"Simulation time: %f \n Initial state vector:  ", *time);
	for (i=0; i<STATENUM; i++) fprintf( stderr,"%f ", state[i]);
	
	fprintf( stderr,"\nP.param: ");
	for (i=0; i<PARAMNUM; i++) fprintf( stderr," %f", P->param[i]);
	
	fprintf( stderr,"\nP.abstol: ");
	for (i=0; i<STATENUM; i++) fprintf( stderr," %f", P->abstol[i]);
	
	fprintf( stderr,"\nP.reltol: %f", P->reltol);
	
	fprintf( stderr,"\njP.dirX: %f",jP->dirX);
	fprintf( stderr,"\njP.dirY: %f",jP->dirY);
	fprintf( stderr,"\njP.dirZ: %f",jP->dirZ);
	fprintf( stderr,"\njP.dirR: %f",jP->dirR);

	fprintf( stderr,"\njP.doX: %d", (int) jP->doX);
	fprintf( stderr,"\njP.doY: %d", (int) jP->doY);
	fprintf( stderr,"\njP.doZ: %d", (int) jP->doZ);
	fprintf( stderr,"\njP.doR: %d", (int) jP->doR);
	
	fprintf( stderr,"\n====================================================================\n");
}



///Copy readed values to their "destination"
/**
* Copy readed values (from file) to their "destination"
* \param *rd    : pointer to the stuct containing readed values
* \param *P     : pointer to the parameters struct 
* \param *jP    : pointer to the joy_parameters struct
* \param *state : pointer to the state vector
* \param *time  : pointer to the time parameter
*/
void copy_to_arg(readStruct *rd, struct parameters *P, struct joy_parameters *jP, double *state, double *time)
{
	int k;
	
	*time = rd->SimTime;
	for (k=0;k<STATENUM;k++) state[k] = rd->X0[k];
	P->reltol = rd->reltol;
	for (k=0;k<STATENUM;k++) P->abstol[k] = rd->abstol[k];
	for (k=0;k<PARAMNUM;k++) P->param[k] = rd->param[k];
	jP->dirX = rd->dirDo[0];
	jP->dirY = rd->dirDo[1];
	jP->dirZ = rd->dirDo[2];
	jP->dirR = rd->dirDo[3];
	jP->doX = rd->dirDo[4];
	jP->doY = rd->dirDo[5];
	jP->doZ = rd->dirDo[6];
	jP->doR = rd->dirDo[7];
}


///Read arguments from file
/**
* Global function to get the arguments values from the file (the one to use from outside this scope)
* \param *P     : pointer to the parameters struct 
* \param *jP    : pointer to the joy_parameters struct
* \param *state : pointer to the state vector
* \param *time  : pointer to the time parameter
*/
int read_arguments_from_file(struct parameters *P, struct joy_parameters *jP, double *state, double *time)
{
	int i;
	readStruct rd;
	
	//Default values in case of error:
	*time = 0.0;
	for (i=0; i<STATENUM; i++) state[i] = 0.0;
	initParameters(P);
	initJoyParameters(jP);
	
	FILE *fp = NULL;
	
	fp=fopen("InitialConditions.icd", "rb");
	
	if (fp!=NULL)
	{
		fread(&rd.SimTime, sizeof(double), 1, fp);
		fread(&rd.X0, sizeof(double), STATENUM, fp); 
		fread(&rd.reltol, sizeof(double), 1, fp);
		fread(&rd.abstol, sizeof(double), STATENUM, fp); 
		fread(&rd.param, sizeof(double), PARAMNUM, fp); 
		fread(&rd.dirDo, sizeof(int), 8, fp); 
	
		copy_to_arg(&rd, P, jP, state, time);
	
	if (fclose(fp)) fprintf( stderr, "Could not close file... \n");
		
		print_arguments_values(P, jP, state, time);
	}
	else
	{
		fprintf( stderr, "Could not open 'InitialConditions.icd' file... \n");
	}

	return 1;
}