/**
* \file f.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$ 
* and stuff related to the ODE solver (initialisation, solving, closing)
*/



/*
Wrap function for solver in dx=f(t,x,parameters)
*/



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

#include "string.h"

#include "hydro_source/hydro_includes.h"

#include "CVODEincludes.h"  // ODE solver includes


#include "f.h"


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




#define Ith(v,i)    NV_Ith_S(v,i-1)       /* Ith numbers components 1..NEQ */
#define IJth(A,i,j) DENSE_ELEM(A,i-1,j-1) /* IJth numbers rows,cols 1..NEQ */

/* Problem Constants */

#define NEQ   12//18                /* number of equations  */
#define T0    RCONST(0.0)      /* initial time           */






//  dy/dt = f(t,y,parameters)

///Wrap function for solver f for computing of \f$ f \f$ in \f$ \frac{\partial\vec{q}}{\partial t}=f(\vec{q}, \vec{Q}, \vec{p}) \f$
/**
* This function makes the bridge between the Sundials ODE-solver and the evaluation function compute_state_dot()
* \param t : time
* \param y : state (\f$ \vec{q} \f$) in the datatype needed by the solver
* \param ydot : evaluation output (\f$ \frac{\partial\vec{q}}{\partial t} \f$)
* \param *f_data: pointer of type void, pointing on the ODE_data type struct containing all the model's data
* \return : 0 (no meaning yet <=> no integration error handling yet!!!)
*/
static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data)
{
	datas *d;
	parameters *P;
	ODE_data *f_dat;
	double *state;

	f_dat = (ODE_data*)f_data;

	d = f_dat->d;
	P = f_dat->P;
	state = f_dat->state;



	state[0] = Ith(y,1);
	state[1] = Ith(y,2);
	state[2] = Ith(y,3);
	state[3] = Ith(y,4);
	state[4] = Ith(y,5);
	state[5] = Ith(y,6);
	state[6] = Ith(y,7);
	state[7] = Ith(y,8);
	state[8] = Ith(y,9);
	state[9] = Ith(y,10);
	state[10] = Ith(y,11);
	state[11] = Ith(y,12);
	/*state[12] = Ith(y,13); //unused camera stuff
	state[13] = Ith(y,14);
	state[14] = Ith(y,15);
	state[15] = Ith(y,16);
	state[16] = Ith(y,17);
	state[17] = Ith(y,18);*/

	data_update_state(d, state);
	
	compute_state_dot(d,P,state);

	Ith(ydot,1) = d->dx;
	Ith(ydot,2) = d->dy;
	Ith(ydot,3) = d->dz;
	Ith(ydot,4) = d->dphi;
	Ith(ydot,5) = d->dtheta;
	Ith(ydot,6) = d->dpsi;
	Ith(ydot,7) = d->ddq[0];
	Ith(ydot,8) = d->ddq[1];
	Ith(ydot,9) = d->ddq[2];
	Ith(ydot,10) = d->ddq[3];
	Ith(ydot,11) = d->ddq[4];
	Ith(ydot,12) = d->ddq[5];
	/*Ith(ydot,13) = d->dx_cam;  //unused camera stuff
	Ith(ydot,14) = d->dy_cam;
	Ith(ydot,15) = d->dz_cam;
	Ith(ydot,16) = d->dd_cam[0];
	Ith(ydot,17) = d->dd_cam[1];
	Ith(ydot,18) = d->dd_cam[2];*/

/*
	Ith(ydot,1) = f_dat->d->t;  //integrator test:  dy/dt = t
	Ith(ydot,2) = f_dat->d->t;
	Ith(ydot,3) = f_dat->d->t;
	Ith(ydot,4) = f_dat->d->t;
	Ith(ydot,5) = f_dat->d->t;
	Ith(ydot,6) = f_dat->d->t;
	Ith(ydot,7) = f_dat->d->t;
	Ith(ydot,8) = f_dat->d->t;
	Ith(ydot,9) = f_dat->d->t;
	Ith(ydot,10) = f_dat->d->t;
	Ith(ydot,11) = f_dat->d->t;
	Ith(ydot,12) = f_dat->d->t;
	Ith(ydot,13) = f_dat->d->t;
	Ith(ydot,14) = f_dat->d->t;
	Ith(ydot,15) = f_dat->d->t;
	Ith(ydot,16) = f_dat->d->t;
	Ith(ydot,17) = f_dat->d->t;
	Ith(ydot,18) = f_dat->d->t;
*/	

  return(0);
}





/// Solver initialisation
/**
* Solver initialisation \n
* Don't forgett to call solver_free()!
* \param *f_dat: pointer to structure of type ODE_data
*/
void solver_init(ODE_data* f_dat)
{
	int k;
	
	f_dat->y = NULL;
	f_dat->abstol = NULL;
	f_dat->cvode_mem = NULL;

  /* Create serial vector of length NEQ for I.C. and abstol */
	f_dat->y = N_VNew_Serial(NEQ);
	f_dat->abstol = N_VNew_Serial(NEQ); 

  /* Initialize y */
	for (k=1; k<=NEQ; k++)
	{
		Ith(f_dat->y,k) = f_dat->state[k-1];
		//printf("Ith(y,%d) = %f \n",k,Ith(y,k));
	}


  /* Set the scalar relative tolerance */
	f_dat->reltol = f_dat->P->reltol;
  /* Set the vector absolute tolerance */
	for (k=1; k<=NEQ; k++)
	{
		Ith(f_dat->abstol,k) = f_dat->P->abstol[k-1];
	}


	  /* 
     Call CVodeCreate to create the solver memory:
     
     CV_BDF     specifies the Backward Differentiation Formula
     CV_NEWTON  specifies a Newton iteration

     A pointer to the integrator problem memory is returned and stored in cvode_mem.
  */

	f_dat->cvode_mem = CVodeCreate(CV_BDF, CV_NEWTON);


  /* 
     Call CVodeMalloc to initialize the integrator memory: 
     
     cvode_mem is the pointer to the integrator memory returned by CVodeCreate
     f         is the user's right hand side function in y'=f(t,y)
     T0        is the initial time
     y         is the initial dependent variable vector
     CV_SV     specifies scalar relative and vector absolute tolerances
     &reltol   is a pointer to the scalar relative tolerance
     abstol    is the absolute tolerance vector
  */
	
	f_dat->flag = CVodeMalloc(f_dat->cvode_mem, f, T0, f_dat->y, CV_SV, f_dat->reltol, f_dat->abstol);

	f_dat->flag = CVodeSetFdata(f_dat->cvode_mem, (void*)f_dat);

	/* Call CVodeRootInit to specify the root function g with 2 components */
	//flag = CVodeRootInit(cvode_mem, 2, g, NULL);

	/* Call CVDense to specify the CVDENSE dense linear solver */
	f_dat->flag = CVDense(f_dat->cvode_mem, NEQ);  
	//flag = CVDiag(cvode_mem);

	/* Set the Jacobian routine to Jac (user-supplied) */
	//flag = CVDenseSetJacFn(cvode_mem, Jac, NULL);
}






/// ODE solving
/**
* Solve the ODE from current time up to \f$ t_{out} \f$
* \param *f_dat: pointer to structure of type ODE_data
* \param tout: output time
*/
void solve_ODE(ODE_data* f_dat, realtype tout)
{
	int k, tempk;
	long ti, nst;

	//integrate to next time value:
	f_dat->flag = CVode(f_dat->cvode_mem, tout, f_dat->y, &f_dat->d->t, CV_NORMAL);

	//store results and time if necessary:	
	if (f_dat->time_param != 0.0) store_state(f_dat->d->t, f_dat->y);


	f_dat->flag = CVodeGetNumSteps(f_dat->cvode_mem, &nst);
	f_dat->CumulNumOfSteps = (double)nst;
}


/// Free solver memory
/**
* Free solver memory
* \param *f_dat: pointer to structure of type ODE_data
*/
void solver_free(ODE_data* f_dat)
{
	/* Free y vector */
	N_VDestroy_Serial(f_dat->y);

	/* Free integrator memory */
	CVodeFree(&f_dat->cvode_mem);
}