/**
 *
 * @author horacio a. aliaga
 */
#ifndef SDEINTEGRATOR_CPP
#define SDEINTEGRATOR_CPP
/** Integrates the stochastic equation:
 * dX = a(X) dt + b(X) dW
 * up to order 4, using the Ito-Taylor scheme and strong convergence.
 * It needs a(X) and its first and second derivatives with respect of X, 
 * and b(X) and its first, second and third derivatives with respect of X. 
 * The initial value X0 of X is needed as well as dt.
 * TODO add the capability of jumps
 */
#include "SdeIntegrator.h"
#include <vector>
#include <iostream>
#include <fstream>
#include <string>
#include <math.h>
using namespace std;

/** Creates a new instance of SDEIntegrator */
SDEIntegrator::SDEIntegrator(){
		X0=0.0;
		X=0.0;
		dt=0.0;
		a=0.0;
		d_a=0.0;
		dd_a=0.0;
		b=0.0;
		d_b=0.0;
		dd_b=0.0;
		ddd_b=0.0;           
		set_dW(dt);
		set_dZ(dt,dwt,dwt2);        
}
SDEIntegrator::SDEIntegrator(double _a,double _d_a,double _dd_a,
                         double _b,double _d_b,double _dd_b,
                         double _ddd_b,double _dt,double _X0) {
		X0=_X0;
		X=_X0;
		dt=_dt;
		a=_a;
		d_a=_d_a;
		dd_a=_dd_a;
		b=_b;
		d_b=_d_b;
		dd_b=_dd_b;
		ddd_b=_ddd_b;           
		set_dW(dt);
		set_dZ(dt,dwt,dwt2);        
	}
void SDEIntegrator::set_SDEIntegrator(double _a,double _d_a,double _dd_a,
                                  double _b,double _d_b,double _dd_b,
                                  double _ddd_b,double _dt,double _X) {
		X=_X;
		dt=_dt;
		a=_a;
		d_a=_d_a;
		dd_a=_dd_a;
		b=_b;
		d_b=_d_b;
		dd_b=_dd_b;
		ddd_b=_ddd_b;           
		set_dW(dt);
		set_dZ(dt,dwt,dwt2);        
	}    
    /** sets the value of a(X) */
void SDEIntegrator::set_a(double _a){
		a=_a;
	}
    /** sets the value of da(X)/dX */
void SDEIntegrator::set_d_a(double _a){
        d_a=_a;
    }
    /** sets the value of d^2a(X)/dX^2 */
void SDEIntegrator::set_dd_a(double _a){
        dd_a=_a;
    }
    /** sets the value of b(X) */
void SDEIntegrator::set_b(double _a){
        b=_a;
    }
    /** sets the value of db(X)/dX */
void SDEIntegrator::set_d_b(double _a){
        d_b=_a;
    }
    /** sets the value of d^2a(X)/dX^2 */
void SDEIntegrator::set_dd_b(double _a){
        dd_b=_a;
    }    
    /** sets the value of d^3a(X)/dX^3 */
void SDEIntegrator::set_ddd_b(double _a){
        ddd_b=_a;
    }
    /** sets the value of dZ, a gaussian random variable
     generated using combining the two independent gaussian 
     random variables dwt and dwt2 */
void SDEIntegrator::set_dZ(double _dt,double _dwt,double _dwt2){
        dZ=generate_X_Y(_dt,_dwt,_dwt2);
    }
    /** sets the random gaussian variables dwt and dwt2*/
void SDEIntegrator::set_dW(double _dt){
        double* dw_aux = new double[2];
        dw_aux = pol_mas(_dt);        
        dwt = dw_aux[0];        
        dwt2= dw_aux[1];
    }
    /** sets the time step dt */
void SDEIntegrator::set_dt(double _dt){
        dt = _dt;
    }
    /** sets the initials value of X(0)=X0*/
void SDEIntegrator::set_X0(double _X0){
        X0=_X0;
    }
void SDEIntegrator::set_X(double _X){
        X=_X;
    }
    /** gets the value of a(X)*/
double SDEIntegrator::get_a(){
        return a;
    }
    /** gets the value of da(X)/dX*/
double SDEIntegrator::get_d_a(){
        return d_a;
    }
    /** gets the value of d^2a(X)/dX^2*/
double SDEIntegrator::get_dd_a(){
        return dd_a;
    }
    /** gets the value of b(X)*/
double SDEIntegrator::get_b(){
        return b;
    }
    /** gets the value of db(X)/dX*/
double SDEIntegrator::get_d_b(){
        return d_b;
    }
    /** gets the value of d^2b(X)/dX^2*/
double SDEIntegrator::get_dd_b(){
        return dd_b;
    }
    /** gets the value of d^3b(X)/dX^3*/
double SDEIntegrator::get_ddd_b(){
        return ddd_b;
    }
    /** gets the value dZ(dt)*/
double SDEIntegrator::get_dZ(){
        return dZ;
    }
    /** gets the value of dt*/
double SDEIntegrator::get_dt(){
        return dt;
    }
    /** gets the value of dwt*/
double SDEIntegrator::get_dwt(){
        return dwt;
    }
    /** gets the value of dwt2*/
double SDEIntegrator::get_dwt2(){
        return dwt2;
    }
    /** gets the value of X0*/
double SDEIntegrator::get_X0(){
        return X0;
    }
double SDEIntegrator::get_X(){
        return X;
    }
    /** Order zero in series of terms comprising the integration*/
double SDEIntegrator::Strong_Integrator_Zero(){
        double aux = log(X);
        return aux;
    }
    /** Order one in series of terms comprising the integration*/
double SDEIntegrator::Strong_Integrator_First(){
        double aux = a*dt + b*dwt;
        return aux;
    }
    /** Order second in series of terms comprising the integration*/
double SDEIntegrator::Strong_Integrator_Second(){
        double aux = 0.5*b*d_b*(dwt*dwt-dt);
        return aux;
    }
    /** Order three in series of terms comprising the integration*/
double SDEIntegrator::Strong_Integrator_Third(){
        double aux=0;
        aux = d_a*b*dZ; 
        aux = aux+ 0.5*(a*d_a+0.5*b*b*dd_a)*dt*dt;
        aux = aux+(a*d_b + 0.5*b*b*dd_b)*(dwt*dt - dZ);
        aux = aux+0.5*b*(b*dd_b + d_b*d_b)*(dwt*dwt/3.0 - dt)*dwt;
        return aux;
    }
    /** Order four in series of terms comprising the integration, it 
     requires the calculation of the Stratonovich Integrals J011, 
     J101 and J011*/
double SDEIntegrator::Strong_Integrator_Fourth(){
        double J_0_1_1,J_1_0_1,J_1_1_0;
        double* J = new double[6];
		for(int i=0;i<6;i++) J[i]=0.0;
        J = J_XXX(dwt, dt);
        J_0_1_1 = J[4];
        J_1_0_1 = J[5];
        J_1_1_0 = J[6];
        double aux=0;                
        double aa = a-0.5*b*d_b;
        double daa=d_a-0.5*(d_b*d_b+b*dd_b);
		double ddaa=dd_a-0.5*(3*dd_b*d_b+b*ddd_b);        
        
        aux = b/24*(d_b*(d_b*d_b+b*dd_b)
                    +b*(3*dd_b*d_b + b*ddd_b))*pow(dwt,4);
        aux =+aa*(d_b*d_b + b*dd_b)*J_0_1_1;
        aux =+b*(daa*d_b + aa*dd_b)*J_1_0_1;
        aux =+b*(d_b*daa + b*ddaa)*J_1_1_0;
        return aux;
    }
    /** generates the dZ correlated random variable */
double SDEIntegrator::generate_X_Y(double V1,double dWt1,double dWt2)
    {
	double g1,g2,sqV,Yc;
	sqV=sqrt(V1);
	g1=dWt1/sqV;
	g2=dWt2/sqV;
	Yc=0.5*sqV*V1*(g1+(1.0/sqrt(3))*g2);
        return(Yc);
    }
    /** calculates the multiple stratonovich integrals:
     * J1, J01, J10, J11, J110, J101 and J011 */ 
double* SDEIntegrator::J_XXX(double dWt,double DELTA_int)
    {
		const int p=20;
        int j=1;
        double ceta_j_r[p];
        double eta_j_r[p];
        double a_j_r[p];
        double b_j_r[p];
        double Xsi_j,mu_j_p,phi_j_p,rho_p,alpha_p,sum_ceta,sum_eta,sum1,sum2;
        double tk=0.0,a_j_0,b_j,B_j_j,C_j_j,sqrtD,sqrt2D,sqrtD2,DELTA2;
        int k=0,i=0,l=0,r;
        double pi=3.1415926535;
        double J_1=0.0,J_1_1=0.0,J_0_1=0.0,J_1_0=0.0,J_0_1_1=0.0;
        double J_1_0_1=0.0,J_1_1_0=0.0;
        double* J = new double[6];
        DELTA2=DELTA_int*DELTA_int;
        sqrtD=sqrt(DELTA_int);
        sqrtD2=sqrt(DELTA_int/2);
        sqrt2D=sqrt(DELTA_int*2);
        Xsi_j=dWt/sqrtD;
        double* dwtz = new double[2];
		dwtz[0]=0.0;
		dwtz[1]=0.0;
        dwtz = pol_mas(1.0);
        mu_j_p = dwtz[0];
        phi_j_p = dwtz[1];
        sum1=0.0;sum2=0.0;sum_ceta=0.0;sum_eta=0.0;B_j_j=0.0;	
        r=1;
        do   /* Generating a sequence of standard gaussian */
        {
            dwtz=pol_mas(1.0);
            ceta_j_r[r-1]=dwtz[0];
            sum_ceta=sum_ceta+(1.0/r)*ceta_j_r[r-1];
            eta_j_r[r-1]=dwtz[1];
            sum_eta=sum_eta+(1.0/(r*r))*eta_j_r[r-1];
            a_j_r[r-1]=ceta_j_r[r-1]*sqrtD2/(pi*r);
            b_j_r[r-1]=eta_j_r[r-1]*sqrtD2/(pi*r);
            sum1=sum1+1.0/(r*r);
            sum2=sum2+1.0/(r*r*r*r);
            B_j_j=B_j_j+(1.0/(r*r))*(ceta_j_r[r-1]*ceta_j_r[r-1]
                       +eta_j_r[r-1]*eta_j_r[r-1]);
            r=r+1;
        }while(r<=p);
        C_j_j=0.0;
        r=1;
        do{
            l=1;
            do{
                if(l!=r){
                    C_j_j=C_j_j + (r*1.0)/((r*r)-(l*l))*((1.0/l)*ceta_j_r[r-1]
                             *ceta_j_r[l-1]-((l*1.0)/r)*eta_j_r[r-1]*eta_j_r[l-1]);
                };
                l=l+1;
            }while(l<=p);
            r=r+1;
        }while(r<=p);
        rho_p=(1.0/12.0)-(1.0/(2*pi*pi))*sum1;
        alpha_p=(pi*pi/180)-(1.0/(2*pi*pi))*sum2;
        a_j_0=-(1.0/pi)*sqrt2D*sum_ceta-2*sqrt(DELTA_int*rho_p)*mu_j_p;
        b_j=sqrtD2*sum_eta+sqrt(DELTA_int*alpha_p)*phi_j_p;
        B_j_j=B_j_j/(4*pi*pi);
        C_j_j=C_j_j/(-2*pi*pi);
    
        J_1=sqrtD*Xsi_j;
        J_1_0=(1.0/2.0)*DELTA_int*(sqrtD*Xsi_j+a_j_0);
        J_0_1=(1.0/2.0)*DELTA_int*(sqrtD*Xsi_j-a_j_0);
        J_1_1=(1.0/2.0)*DELTA_int*Xsi_j*Xsi_j;
        J_0_1_1=(1.0/6.0)*DELTA2*Xsi_j*Xsi_j-(1.0/pi)*DELTA_int*sqrtD*Xsi_j*b_j+DELTA2*B_j_j-(1.0/4)*DELTA_int*sqrtD*a_j_0*Xsi_j+(1.0/(2*pi))*DELTA_int*sqrtD*Xsi_j*b_j+DELTA2*C_j_j;
        J_1_0_1=(1.0/6.0)*DELTA2*Xsi_j*Xsi_j+(1.0/2.0)*a_j_0*J_0_1+(1.0/(2*pi))*DELTA_int*sqrtD*Xsi_j*b_j-DELTA2*B_j_j-(1.0/4.0)*DELTA_int*sqrtD*a_j_0*Xsi_j+(1.0/(2*pi))*DELTA_int*sqrtD*Xsi_j*b_j;
        J_1_1_0=(1.0/2.0)*DELTA2*Xsi_j*Xsi_j-J_1_0_1-J_0_1_1;
        J[0]=J_1;
        J[1]=J_1_0;
        J[2]=J_0_1;
        J[3]=J_1_1;
        J[4]=J_0_1_1;
        J[5]=J_1_0_1;
        J[6]=J_1_1_0;
        return J;
    }   
   /** Polar Marsaglia generator of random gaussian variables 
    dwt and dwt2, with variance dt */
double* SDEIntegrator::pol_mas(double t)
    {
		double u1,u2,v1,v2,w,wn;
        double* dwtz = new double[2];
        for(int i=0;i<10000;i++)
        {
            u1= (double) rand()/RAND_MAX;
            v1=2*u1-1;
            u2= (double) rand()/RAND_MAX;
            v2=2*u2-1;
            w=v1*v1+v2*v2;
            if ((w <= 1.0)&&(w >0.0))
            {
                wn=sqrt(-2*log(w)/w);	  
                dwtz[0] =sqrt(t)*v1*wn;
                dwtz[1]=sqrt(t)*v2*wn;
                break;
            }
        }
        return dwtz;
    }    
#endif SDEINTEGRATOR_CPP