#include "preamble.h"
#ifdef  Global015prior
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "filzbach.h"
#include <iostream>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
using namespace std;
using namespace boost::numeric::odeint;
void pause(){PAUSE}
void fake_data();
void read_data();
void setup_parameters();
void fit_model();
void final_output();
double aTSRot;
 double aTLRot;
 double aTOCop;
 double aTDapR;
 double aTAsp;
 double aTMes;
 double aTDapP;
 double aTHol;
 double aTBos;
 double aJSRot;
 double aJLRot;
 double aJOCop;
 double aJDapR;
 double aJAsp;
 double aJMes;
 double aJDapP;
 double aJHol;
 double aJBos;
 double aRBac;
 double aRDia;
 double aRCr;
 double aRSGr;
 double aRLGr;
 double aRSGo;
 double aRLGo;
 double aRInC;

 double	start1Bac	=	0.0050	;
double	start1Dia	=	0.0181	;
double	start1Cr	=	0.0070	;
double	start1SGr	=	0.0010	;
double	start1LGr	=	0.0205	;
double	start1SGo	=	0.0156	;
double	start1LGo	=	0.0620	;
double	start1InC	=	0.0173	;
double	start1SRot	=	0.9854	;
double	start1LRot	=	0.1002	;
double	start1OCop	=	22.6953	;
double	start1DapR	=	0.1705	;
double	start1Asp	=	8.5935	;
double	start1Mes	=	0.7572	;
double	start1DapP	=	1.8839	;
double	start1Hol	=	9.6428	;
double	start1Bos	=	0.1002	;
double	start2Bac	=	0.0218	;
double	start2Dia	=	0.0010	;
double	start2Cr	=	0.0702	;
double	start2SGr	=	0.0010	;
double	start2LGr	=	0.0069	;
double	start2SGo	=	0.0249	;
double	start2LGo	=	0.0450	;
double	start2InC	=	0.0035	;
double	start2SRot	=	3.2703	;
double	start2LRot	=	0.1026	;
double	start2OCop	=	54.6490	;
double	start2DapR	=	1.0775	;
double	start2Asp	=	0.6661	;
double	start2Mes	=	0.1005	;
double	start2DapP	=	1.9573	;
double	start2Hol	=	0.3979	;
double	start2Bos	=	0.0996	;
double	start3Bac	=	0.0291	;
double	start3Dia	=	0.0119	;
double	start3Cr	=	0.0079	;
double	start3SGr	=	0.0010	;
double	start3LGr	=	0.0010	;
double	start3SGo	=	0.0171	;
double	start3LGo	=	0.0120	;
double	start3InC	=	0.01090	;
double	start3SRot	=	2.5080	;
double	start3LRot	=	1.4960	;
double	start3OCop	=	49.7455	;
double	start3DapR	=	0.1023	;
double	start3Asp	=	0.1015	;
double	start3Mes	=	0.8759	;
double	start3DapP	=	4.5128	;
double	start3Hol	=	1.7653	;
double	start3Bos	=	0.10	;
double	start4Bac	=	0.0330	;
double	start4Dia	=	0.0147	;
double	start4Cr	=	0.0428	;
double	start4SGr	=	0.0039	;
double	start4LGr	=	0.0010	;
double	start4SGo	=	0.0010	;
double	start4LGo	=	0.0180	;
double	start4InC	=	0.0010	;
double	start4SRot	=	1.3421	;
double	start4LRot	=	0.0431	;
double	start4OCop	=	0.1119	;
double	start4DapR	=	0.3765	;
double	start4Asp	=	14.7573	;
double	start4Mes	=	71.8387	;
double	start4DapP	=	0.1542	;
double	start4Hol	=	92.4456	;
double	start4Bos	=	8.3233	;
double	start5Bac	=	0.0090	;
double	start5Dia	=	0.0030	;
double	start5Cr	=	0.0282	;
double	start5SGr	=	0.0010	;
double	start5LGr	=	0.0116	;
double	start5SGo	=	0.0107	;
double	start5LGo	=	0.0803	;
double	start5InC	=	0.0010	;
double	start5SRot	=	4.8803	;
double	start5LRot	=	0.0986	;
double	start5OCop	=	70.6117	;
double	start5DapR	=	3.1821	;
double	start5Asp	=	0.0999	;
double	start5Mes	=	1.3518	;
double	start5DapP	=	4.4872	;
double	start5Hol	=	18.7869	;
double	start5Bos	=	0.3588	;

double fT =  0.9 ;
double fJ =  0.9 ;
      double fR =  0.9 ;
      double d =  0.005 ;
      double K =  0.91 ;
      double sdZ =  2.5 ;
      double sdP =  1.4 ;
      double W =  1 ;
      double q =  1.2 ;
      double PhytoTotal;
 double Q0; double W0;
double Q1; double W1;
double Q2; double W2;
double Q3; double W3;
double Q4; double W4;
double Q5; double W5;
double Q6; double W6;
double Q7; double W7;
double Q8; double W8;
double Q9; double W9;
double Q10; double W10;
double Q11; double W11;
double Q12; double W12;
double Q13; double W13;
double Q14; double W14;
double Q15; double W15;
double Q16; double W16;

// Note that these are mass ^ -0.15
const double imBac= 8.814	;
const double imDia= 3.127	;
const double imCr= 	5.068	;
const double imSGr= 4.568	;
const double imLGr= 3.127	;
const double imSGo=	5.068	;
const double imLGo= 3.588	;
const double imInC= 3.588	;
const double imSRot= 2.115	;
const double imLRot= 0.784	;
const double imOCop=  0.864	;
const double imDapR=  0.682	;
const double imAsp=  0.673	;
const double imMes=  0.515	;
const double imDapP= 0.550	;
const double imHol= 0.538	;
const double imBos= 1.031	;

                      typedef boost::array< double , 17 > state_type; // Create internal array the size of what needs to be tracked
                      typedef runge_kutta_cash_karp54< state_type > error_stepper_type; // Type of stepper used see http://headmyshoulder.github.io/odeint-v2/index.html docs for details
                      typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;
                      double abs_err = 1e-12 , rel_err = 1e-06 , a_x = 1.0 , a_dxdt = 1.0;
                      controlled_stepper_type controlled_stepper(
                      default_error_checker< double , range_algebra , default_operations >( abs_err , rel_err , a_x , a_dxdt ) );
                      int timerow = 0;

 void ODE( const state_type &x , state_type &dxdt , double t )
{W0=x[0]/W;Q0=pow(W0,q);W1=x[1]/W;Q1=pow(W1,q);W2=x[2]/W;Q2=pow(W2,q);W3=x[3]/W;Q3=pow(W3,q);W4=x[4]/W;Q4=pow(W4,q);W5=x[5]/W;Q5=pow(W5,q);W6=x[6]/W;Q6=pow(W6,q);W7=x[7]/W;Q7=pow(W7,q);W8=x[8]/W;Q8=pow(W8,q);W9=x[9]/W;Q9=pow(W9,q);W10=x[10]/W;Q10=pow(W10,q);W11=x[11]/W;Q11=pow(W11,q);W12=x[12]/W;Q12=pow(W12,q);W13=x[13]/W;Q13=pow(W13,q);W14=x[14]/W;Q14=pow(W14,q);W15=x[15]/W;Q15=pow(W15,q);W16=x[16]/W;Q16=pow(W16,q);PhytoTotal = 0+x[0]+x[1]+x[2]+x[3]+x[4]+x[5]+x[6]+x[7];
 /*  Bac     */ dxdt[0] = fR*aRBac*imBac*x[0]*(1 -(PhytoTotal/K))
                              -(fJ*aJSRot*imSRot*2*x[8] * Q0/(1+d*x[8]+(Q0+Q2+Q3+Q5+0))) // Impact of SRot
                           -(fJ*aJLRot*imLRot*2*x[9] * Q0/(1+d*x[9]+(Q0+Q2+Q3+Q5+0))) // Impact of LRot
                           -(fJ*aJDapR*imDapR*2*x[11] * Q0/(1+d*x[11]+(Q0+Q1+Q2+Q4+Q8+0))) // Impact of DapR
                           -(fJ*aJDapP*imDapP*2*x[14] * Q0/(1+d*x[14]+(Q0+Q1+Q2+Q3+Q4+Q5+Q6+Q7+0))) // Impact of DapP
                           -(fJ*aJHol*imHol*2*x[15] * Q0/(1+d*x[15]+(Q0+Q1+Q2+Q3+Q4+Q5+0))) // Impact of Hol
                           -(fJ*aJBos*imBos*2*x[16] * Q0/(1+d*x[16]+(Q0+Q2+Q3+0))) // Impact of Bos
                           ;
 /*  Dia     */ dxdt[1] = fR*aRDia*imDia*x[1]*(1 -(PhytoTotal/K))
                              -(fJ*aJOCop*imOCop*2*x[10] * Q1/(1+d*x[10]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+Q10+0))) // Impact of OCop
                           -(fJ*aJDapR*imDapR*2*x[11] * Q1/(1+d*x[11]+(Q0+Q1+Q2+Q4+Q8+0))) // Impact of DapR
                           -(fJ*aJAsp*imAsp*2*x[12] * Q1/(1+d*x[12]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+0))) // Impact of Asp
                           -(fJ*aJDapP*imDapP*2*x[14] * Q1/(1+d*x[14]+(Q0+Q1+Q2+Q3+Q4+Q5+Q6+Q7+0))) // Impact of DapP
                           -(fJ*aJHol*imHol*2*x[15] * Q1/(1+d*x[15]+(Q0+Q1+Q2+Q3+Q4+Q5+0))) // Impact of Hol
                           ;
 /*  Cr      */ dxdt[2] = fR*aRCr*imCr*x[2]*(1 -(PhytoTotal/K))
                              -(fJ*aJSRot*imSRot*2*x[8] * Q2/(1+d*x[8]+(Q0+Q2+Q3+Q5+0))) // Impact of SRot
                           -(fJ*aJLRot*imLRot*2*x[9] * Q2/(1+d*x[9]+(Q0+Q2+Q3+Q5+0))) // Impact of LRot
                           -(fJ*aJOCop*imOCop*2*x[10] * Q2/(1+d*x[10]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+Q10+0))) // Impact of OCop
                           -(fJ*aJDapR*imDapR*2*x[11] * Q2/(1+d*x[11]+(Q0+Q1+Q2+Q4+Q8+0))) // Impact of DapR
                           -(fJ*aJAsp*imAsp*2*x[12] * Q2/(1+d*x[12]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+0))) // Impact of Asp
                           -(fJ*aJDapP*imDapP*2*x[14] * Q2/(1+d*x[14]+(Q0+Q1+Q2+Q3+Q4+Q5+Q6+Q7+0))) // Impact of DapP
                           -(fJ*aJHol*imHol*2*x[15] * Q2/(1+d*x[15]+(Q0+Q1+Q2+Q3+Q4+Q5+0))) // Impact of Hol
                           -(fJ*aJBos*imBos*2*x[16] * Q2/(1+d*x[16]+(Q0+Q2+Q3+0))) // Impact of Bos
                           ;
 /*  SGr     */ dxdt[3] = fR*aRSGr*imSGr*x[3]*(1 -(PhytoTotal/K))
                              -(fJ*aJSRot*imSRot*2*x[8] * Q3/(1+d*x[8]+(Q0+Q2+Q3+Q5+0))) // Impact of SRot
                           -(fJ*aJLRot*imLRot*2*x[9] * Q3/(1+d*x[9]+(Q0+Q2+Q3+Q5+0))) // Impact of LRot
                           -(fJ*aJOCop*imOCop*2*x[10] * Q3/(1+d*x[10]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+Q10+0))) // Impact of OCop
                           -(fJ*aJAsp*imAsp*2*x[12] * Q3/(1+d*x[12]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+0))) // Impact of Asp
                           -(fJ*aJDapP*imDapP*2*x[14] * Q3/(1+d*x[14]+(Q0+Q1+Q2+Q3+Q4+Q5+Q6+Q7+0))) // Impact of DapP
                           -(fJ*aJHol*imHol*2*x[15] * Q3/(1+d*x[15]+(Q0+Q1+Q2+Q3+Q4+Q5+0))) // Impact of Hol
                           -(fJ*aJBos*imBos*2*x[16] * Q3/(1+d*x[16]+(Q0+Q2+Q3+0))) // Impact of Bos
                           ;
 /*  LGr     */ dxdt[4] = fR*aRLGr*imLGr*x[4]*(1 -(PhytoTotal/K))
                              -(fJ*aJOCop*imOCop*2*x[10] * Q4/(1+d*x[10]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+Q10+0))) // Impact of OCop
                           -(fJ*aJDapR*imDapR*2*x[11] * Q4/(1+d*x[11]+(Q0+Q1+Q2+Q4+Q8+0))) // Impact of DapR
                           -(fJ*aJAsp*imAsp*2*x[12] * Q4/(1+d*x[12]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+0))) // Impact of Asp
                           -(fJ*aJDapP*imDapP*2*x[14] * Q4/(1+d*x[14]+(Q0+Q1+Q2+Q3+Q4+Q5+Q6+Q7+0))) // Impact of DapP
                           -(fJ*aJHol*imHol*2*x[15] * Q4/(1+d*x[15]+(Q0+Q1+Q2+Q3+Q4+Q5+0))) // Impact of Hol
                           ;
 /*  SGo     */ dxdt[5] = fR*aRSGo*imSGo*x[5]*(1 -(PhytoTotal/K))
                              -(fJ*aJSRot*imSRot*2*x[8] * Q5/(1+d*x[8]+(Q0+Q2+Q3+Q5+0))) // Impact of SRot
                           -(fJ*aJLRot*imLRot*2*x[9] * Q5/(1+d*x[9]+(Q0+Q2+Q3+Q5+0))) // Impact of LRot
                           -(fJ*aJOCop*imOCop*2*x[10] * Q5/(1+d*x[10]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+Q10+0))) // Impact of OCop
                           -(fJ*aJAsp*imAsp*2*x[12] * Q5/(1+d*x[12]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+0))) // Impact of Asp
                           -(fJ*aJDapP*imDapP*2*x[14] * Q5/(1+d*x[14]+(Q0+Q1+Q2+Q3+Q4+Q5+Q6+Q7+0))) // Impact of DapP
                           -(fJ*aJHol*imHol*2*x[15] * Q5/(1+d*x[15]+(Q0+Q1+Q2+Q3+Q4+Q5+0))) // Impact of Hol
                           ;
 /*  LGo     */ dxdt[6] = fR*aRLGo*imLGo*x[6]*(1 -(PhytoTotal/K))
                              -(fJ*aJDapP*imDapP*2*x[14] * Q6/(1+d*x[14]+(Q0+Q1+Q2+Q3+Q4+Q5+Q6+Q7+0))) // Impact of DapP
                           ;
 /*  InC     */ dxdt[7] = fR*aRInC*imInC*x[7]*(1 -(PhytoTotal/K))
                              -(fJ*aJDapP*imDapP*2*x[14] * Q7/(1+d*x[14]+(Q0+Q1+Q2+Q3+Q4+Q5+Q6+Q7+0))) // Impact of DapP
                           ;
 /*  SRot    */ dxdt[8] =  fJ*aJSRot*imSRot*x[8] *(0+Q0+Q2+Q3+Q5)/(1 + d*x[8] +(0+Q0+Q2+Q3+Q5))//  Gain from predation
                           -(aTSRot*fT*imSRot*x[8]) // Loss from Respiration
                            -(fJ*aJOCop*imOCop*2*x[10] * Q8/(1+d*x[10]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+Q10+0))) // Impact of OCop
                           -(fJ*aJDapR*imDapR*2*x[11] * Q8/(1+d*x[11]+(Q0+Q1+Q2+Q4+Q8+0))) // Impact of DapR
                           -(fJ*aJAsp*imAsp*2*x[12] * Q8/(1+d*x[12]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+0))) // Impact of Asp
                           -(fJ*aJMes*imMes*2*x[13] * Q8/(1+d*x[13]+(Q8+Q9+Q10+Q11+Q12+Q16+0))) // Impact of Mes
                           ;
 /*  LRot    */ dxdt[9] =  fJ*aJLRot*imLRot*x[9] *(0+Q0+Q2+Q3+Q5)/(1 + d*x[9] +(0+Q0+Q2+Q3+Q5))//  Gain from predation
                           -(aTLRot*fT*imLRot*x[9]) // Loss from Respiration
                            -(fJ*aJOCop*imOCop*2*x[10] * Q9/(1+d*x[10]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+Q10+0))) // Impact of OCop
                           -(fJ*aJAsp*imAsp*2*x[12] * Q9/(1+d*x[12]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+0))) // Impact of Asp
                           -(fJ*aJMes*imMes*2*x[13] * Q9/(1+d*x[13]+(Q8+Q9+Q10+Q11+Q12+Q16+0))) // Impact of Mes
                           ;
 /*  OCop    */ dxdt[10] =  fJ*aJOCop*imOCop*x[10] *(0+Q1+Q2+Q3+Q4+Q5+Q8+Q9+Q10)/(1 + d*x[10] +(0+Q1+Q2+Q3+Q4+Q5+Q8+Q9+Q10))//  Gain from predation
                           -(aTOCop*fT*imOCop*x[10]) // Loss from Respiration
                            -(fJ*aJOCop*imOCop*2*x[10] * Q10/(1+d*x[10]+(Q1+Q2+Q3+Q4+Q5+Q8+Q9+Q10+0))) // Impact of OCop
                           -(fJ*aJMes*imMes*2*x[13] * Q10/(1+d*x[13]+(Q8+Q9+Q10+Q11+Q12+Q16+0))) // Impact of Mes
                           ;
 /*  DapR    */ dxdt[11] =  fJ*aJDapR*imDapR*x[11] *(0+Q0+Q1+Q2+Q4+Q8)/(1 + d*x[11] +(0+Q0+Q1+Q2+Q4+Q8))//  Gain from predation
                           -(aTDapR*fT*imDapR*x[11]) // Loss from Respiration
                            -(fJ*aJMes*imMes*2*x[13] * Q11/(1+d*x[13]+(Q8+Q9+Q10+Q11+Q12+Q16+0))) // Impact of Mes
                           ;
 /*  Asp     */ dxdt[12] =  fJ*aJAsp*imAsp*x[12] *(0+Q1+Q2+Q3+Q4+Q5+Q8+Q9)/(1 + d*x[12] +(0+Q1+Q2+Q3+Q4+Q5+Q8+Q9))//  Gain from predation
                           -(aTAsp*fT*imAsp*x[12]) // Loss from Respiration
                            -(fJ*aJMes*imMes*2*x[13] * Q12/(1+d*x[13]+(Q8+Q9+Q10+Q11+Q12+Q16+0))) // Impact of Mes
                           ;
 /*  Mes     */ dxdt[13] =  fJ*aJMes*imMes*x[13] *(0+Q8+Q9+Q10+Q11+Q12+Q16)/(1 + d*x[13] +(0+Q8+Q9+Q10+Q11+Q12+Q16))//  Gain from predation
                           -(aTMes*fT*imMes*x[13]) // Loss from Respiration
                            ;
 /*  DapP    */ dxdt[14] =  fJ*aJDapP*imDapP*x[14] *(0+Q0+Q1+Q2+Q3+Q4+Q5+Q6+Q7)/(1 + d*x[14] +(0+Q0+Q1+Q2+Q3+Q4+Q5+Q6+Q7))//  Gain from predation
                           -(aTDapP*fT*imDapP*x[14]) // Loss from Respiration
                            ;
 /*  Hol     */ dxdt[15] =  fJ*aJHol*imHol*x[15] *(0+Q0+Q1+Q2+Q3+Q4+Q5)/(1 + d*x[15] +(0+Q0+Q1+Q2+Q3+Q4+Q5))//  Gain from predation
                           -(aTHol*fT*imHol*x[15]) // Loss from Respiration
                            ;
 /*  Bos     */ dxdt[16] =  fJ*aJBos*imBos*x[16] *(0+Q0+Q2+Q3)/(1 + d*x[16] +(0+Q0+Q2+Q3))//  Gain from predation
                           -(aTBos*fT*imBos*x[16]) // Loss from Respiration
                            -(fJ*aJMes*imMes*2*x[13] * Q16/(1+d*x[13]+(Q8+Q9+Q10+Q11+Q12+Q16+0))) // Impact of Mes
                           ;

 }
 void read_data()
{table_read("mydata1","./workspace/Pauldata91.txt",17);
table_addcolumn("mydata1", "foundBac");
table_addcolumn("mydata1", "foundDia");
table_addcolumn("mydata1", "foundCr");
table_addcolumn("mydata1", "foundSGr");
table_addcolumn("mydata1", "foundLGr");
table_addcolumn("mydata1", "foundSGo");
table_addcolumn("mydata1", "foundLGo");
table_addcolumn("mydata1", "foundInC");
table_addcolumn("mydata1", "foundSRot");
table_addcolumn("mydata1", "foundLRot");
table_addcolumn("mydata1", "foundOCop");
table_addcolumn("mydata1", "foundDapR");
table_addcolumn("mydata1", "foundAsp");
table_addcolumn("mydata1", "foundMes");
table_addcolumn("mydata1", "foundDapP");
table_addcolumn("mydata1", "foundHol");
table_addcolumn("mydata1", "foundBos");
table_read("mydata2","./workspace/Pauldata92.txt",17);
table_addcolumn("mydata2", "foundBac");
table_addcolumn("mydata2", "foundDia");
table_addcolumn("mydata2", "foundCr");
table_addcolumn("mydata2", "foundSGr");
table_addcolumn("mydata2", "foundLGr");
table_addcolumn("mydata2", "foundSGo");
table_addcolumn("mydata2", "foundLGo");
table_addcolumn("mydata2", "foundInC");
table_addcolumn("mydata2", "foundSRot");
table_addcolumn("mydata2", "foundLRot");
table_addcolumn("mydata2", "foundOCop");
table_addcolumn("mydata2", "foundDapR");
table_addcolumn("mydata2", "foundAsp");
table_addcolumn("mydata2", "foundMes");
table_addcolumn("mydata2", "foundDapP");
table_addcolumn("mydata2", "foundHol");
table_addcolumn("mydata2", "foundBos");
table_read("mydata3","./workspace/Pauldata93alt.txt",17);
table_addcolumn("mydata3", "foundBac");
table_addcolumn("mydata3", "foundDia");
table_addcolumn("mydata3", "foundCr");
table_addcolumn("mydata3", "foundSGr");
table_addcolumn("mydata3", "foundLGr");
table_addcolumn("mydata3", "foundSGo");
table_addcolumn("mydata3", "foundLGo");
table_addcolumn("mydata3", "foundInC");
table_addcolumn("mydata3", "foundSRot");
table_addcolumn("mydata3", "foundLRot");
table_addcolumn("mydata3", "foundOCop");
table_addcolumn("mydata3", "foundDapR");
table_addcolumn("mydata3", "foundAsp");
table_addcolumn("mydata3", "foundMes");
table_addcolumn("mydata3", "foundDapP");
table_addcolumn("mydata3", "foundHol");
table_addcolumn("mydata3", "foundBos");
table_read("mydata4","./workspace/Pauldata94.txt",17);
table_addcolumn("mydata4", "foundBac");
table_addcolumn("mydata4", "foundDia");
table_addcolumn("mydata4", "foundCr");
table_addcolumn("mydata4", "foundSGr");
table_addcolumn("mydata4", "foundLGr");
table_addcolumn("mydata4", "foundSGo");
table_addcolumn("mydata4", "foundLGo");
table_addcolumn("mydata4", "foundInC");
table_addcolumn("mydata4", "foundSRot");
table_addcolumn("mydata4", "foundLRot");
table_addcolumn("mydata4", "foundOCop");
table_addcolumn("mydata4", "foundDapR");
table_addcolumn("mydata4", "foundAsp");
table_addcolumn("mydata4", "foundMes");
table_addcolumn("mydata4", "foundDapP");
table_addcolumn("mydata4", "foundHol");
table_addcolumn("mydata4", "foundBos");
table_read("mydata5","./workspace/Pauldata95.txt",17);
table_addcolumn("mydata5", "foundBac");
table_addcolumn("mydata5", "foundDia");
table_addcolumn("mydata5", "foundCr");
table_addcolumn("mydata5", "foundSGr");
table_addcolumn("mydata5", "foundLGr");
table_addcolumn("mydata5", "foundSGo");
table_addcolumn("mydata5", "foundLGo");
table_addcolumn("mydata5", "foundInC");
table_addcolumn("mydata5", "foundSRot");
table_addcolumn("mydata5", "foundLRot");
table_addcolumn("mydata5", "foundOCop");
table_addcolumn("mydata5", "foundDapR");
table_addcolumn("mydata5", "foundAsp");
table_addcolumn("mydata5", "foundMes");
table_addcolumn("mydata5", "foundDapP");
table_addcolumn("mydata5", "foundHol");
table_addcolumn("mydata5", "foundBos");
return;}

void setup_parameters()
{   /* min - max - initial - norm/lnorm -  fixed? - display?  */
parameter_create("aRBac",0.01,20,8.146,1,0,1);
parameter_create("aRDia",0.01,20,15.614,1,0,1);
parameter_create("aRCr",0.01,20,19.259,1,0,1);
parameter_create("aRSGr",0.01,20,18.927,1,0,1);
parameter_create("aRLGr",0.01,20,2.225,1,0,1);
parameter_create("aRSGo",0.01,20,15.875,1,0,1);
parameter_create("aRLGo",0.01,20,6.98,1,0,1);
parameter_create("aRInC",0.01,20,6.181,1,0,1);
parameter_create("aJSRot",0.01,40,16.79,1,0,1);
parameter_create("aJLRot",0.01,40,14.107,1,0,1);
parameter_create("aJOCop",0.01,40,2.844,1,0,1);
parameter_create("aJDapR",0.01,40,4.103,1,0,1);
parameter_create("aJAsp",0.01,40,5.631,1,0,1);
parameter_create("aJMes",0.01,40,2.647,1,0,1);
parameter_create("aJDapP",0.01,40,32.828,1,0,1);
parameter_create("aJHol",0.01,40,36.797,1,0,1);
parameter_create("aJBos",0.01,40,6.746,1,0,1);
parameter_create("aTSRot",0.00001,2,0.22,1,0,1);
parameter_create("aTLRot",0.00001,2,0.351,1,0,1);
parameter_create("aTOCop",0.00001,2,0.065,1,0,1);
parameter_create("aTDapR",0.00001,2,1.331,1,0,1);
parameter_create("aTAsp",0.00001,2,1.192,1,0,1);
parameter_create("aTMes",0.00001,2,0.308,1,0,1);
parameter_create("aTDapP",0.00001,2,0.842,1,0,1);
parameter_create("aTHol",0.000001,2,1.35,1,0,1);
parameter_create("aTBos",0.000001,2,1.575,1,0,1);

parameter_create("fT",0.05,0.07,0.06,0,1,1);
parameter_create("fR",0.12,0.14,0.13,0,1,1);
parameter_create("K",0.1,10,0.3,0,1,1);
parameter_create("sdZ",1.5,8,1.537505,0,-1,1);
parameter_create("sdP",1,6,1.467582,0,-1,1);
parameter_create("fJ",0.14,0.16,0.14,0,1,1);
parameter_create("d",1e-05,100,10,0,1,1);
parameter_create("W",0.1,5,0.15,0,1,1);
parameter_create("q",1,2.5,1.4,0,1,1);

parameter_delay("sdZ", 8000);
parameter_delay("sdP", 8000);

parameter_addprior("aRBac",9,0.145);
parameter_addprior("aRDia",9,0.145);
parameter_addprior("aRCr",9,0.145);
parameter_addprior("aRSGr",9,0.145);
parameter_addprior("aRLGr",9,0.145);
parameter_addprior("aRSGo",9,0.145);
parameter_addprior("aRLGo",9,0.145);
parameter_addprior("aRInC",9,0.145);

parameter_addprior("aJSRot",10,0.125);
parameter_addprior("aJLRot",10,0.125);
parameter_addprior("aJOCop",10,0.125);
parameter_addprior("aJDapR",10,0.125);
parameter_addprior("aJAsp",10,0.125);
parameter_addprior("aJMes",10,0.125);
parameter_addprior("aJDapP",10,0.125);
parameter_addprior("aJHol",10,0.125);
parameter_addprior("aJBos",10,0.125);

parameter_addprior("aTSRot",0.110,1.6);
parameter_addprior("aTLRot",0.110,1.6);
parameter_addprior("aTOCop",0.110,1.6);
parameter_addprior("aTDapR",0.110,1.6);
parameter_addprior("aTAsp",0.110,1.6);
parameter_addprior("aTMes",0.110,1.6);
parameter_addprior("aTDapP",0.110,1.6);
parameter_addprior("aTHol",0.110,1.6);
parameter_addprior("aTBos",0.110,1.6);

parameter_showall();
                PAUSE
                return;
}
 void write_Hy1( const state_type &x , const double t )
{
table_writevalue_multichain("mydata1", "foundBac", timerow,x[0]);
table_writevalue_multichain("mydata1", "foundDia", timerow,x[1]);
table_writevalue_multichain("mydata1", "foundCr", timerow,x[2]);
table_writevalue_multichain("mydata1", "foundSGr", timerow,x[3]);
table_writevalue_multichain("mydata1", "foundLGr", timerow,x[4]);
table_writevalue_multichain("mydata1", "foundSGo", timerow,x[5]);
table_writevalue_multichain("mydata1", "foundLGo", timerow,x[6]);
table_writevalue_multichain("mydata1", "foundInC", timerow,x[7]);
table_writevalue_multichain("mydata1", "foundSRot", timerow,x[8]);
table_writevalue_multichain("mydata1", "foundLRot", timerow,x[9]);
table_writevalue_multichain("mydata1", "foundOCop", timerow,x[10]);
table_writevalue_multichain("mydata1", "foundDapR", timerow,x[11]);
table_writevalue_multichain("mydata1", "foundAsp", timerow,x[12]);
table_writevalue_multichain("mydata1", "foundMes", timerow,x[13]);
table_writevalue_multichain("mydata1", "foundDapP", timerow,x[14]);
table_writevalue_multichain("mydata1", "foundHol", timerow,x[15]);
table_writevalue_multichain("mydata1", "foundBos", timerow,x[16]);
timerow++;
}
void write_Hy2( const state_type &x , const double t )
{
table_writevalue_multichain("mydata2", "foundBac", timerow,x[0]);
table_writevalue_multichain("mydata2", "foundDia", timerow,x[1]);
table_writevalue_multichain("mydata2", "foundCr", timerow,x[2]);
table_writevalue_multichain("mydata2", "foundSGr", timerow,x[3]);
table_writevalue_multichain("mydata2", "foundLGr", timerow,x[4]);
table_writevalue_multichain("mydata2", "foundSGo", timerow,x[5]);
table_writevalue_multichain("mydata2", "foundLGo", timerow,x[6]);
table_writevalue_multichain("mydata2", "foundInC", timerow,x[7]);
table_writevalue_multichain("mydata2", "foundSRot", timerow,x[8]);
table_writevalue_multichain("mydata2", "foundLRot", timerow,x[9]);
table_writevalue_multichain("mydata2", "foundOCop", timerow,x[10]);
table_writevalue_multichain("mydata2", "foundDapR", timerow,x[11]);
table_writevalue_multichain("mydata2", "foundAsp", timerow,x[12]);
table_writevalue_multichain("mydata2", "foundMes", timerow,x[13]);
table_writevalue_multichain("mydata2", "foundDapP", timerow,x[14]);
table_writevalue_multichain("mydata2", "foundHol", timerow,x[15]);
table_writevalue_multichain("mydata2", "foundBos", timerow,x[16]);
timerow++;
}
void write_Hy3( const state_type &x , const double t )
{
table_writevalue_multichain("mydata3", "foundBac", timerow,x[0]);
table_writevalue_multichain("mydata3", "foundDia", timerow,x[1]);
table_writevalue_multichain("mydata3", "foundCr", timerow,x[2]);
table_writevalue_multichain("mydata3", "foundSGr", timerow,x[3]);
table_writevalue_multichain("mydata3", "foundLGr", timerow,x[4]);
table_writevalue_multichain("mydata3", "foundSGo", timerow,x[5]);
table_writevalue_multichain("mydata3", "foundLGo", timerow,x[6]);
table_writevalue_multichain("mydata3", "foundInC", timerow,x[7]);
table_writevalue_multichain("mydata3", "foundSRot", timerow,x[8]);
table_writevalue_multichain("mydata3", "foundLRot", timerow,x[9]);
table_writevalue_multichain("mydata3", "foundOCop", timerow,x[10]);
table_writevalue_multichain("mydata3", "foundDapR", timerow,x[11]);
table_writevalue_multichain("mydata3", "foundAsp", timerow,x[12]);
table_writevalue_multichain("mydata3", "foundMes", timerow,x[13]);
table_writevalue_multichain("mydata3", "foundDapP", timerow,x[14]);
table_writevalue_multichain("mydata3", "foundHol", timerow,x[15]);
table_writevalue_multichain("mydata3", "foundBos", timerow,x[16]);
timerow++;
}
void write_Hy4( const state_type &x , const double t )
{
table_writevalue_multichain("mydata4", "foundBac", timerow,x[0]);
table_writevalue_multichain("mydata4", "foundDia", timerow,x[1]);
table_writevalue_multichain("mydata4", "foundCr", timerow,x[2]);
table_writevalue_multichain("mydata4", "foundSGr", timerow,x[3]);
table_writevalue_multichain("mydata4", "foundLGr", timerow,x[4]);
table_writevalue_multichain("mydata4", "foundSGo", timerow,x[5]);
table_writevalue_multichain("mydata4", "foundLGo", timerow,x[6]);
table_writevalue_multichain("mydata4", "foundInC", timerow,x[7]);
table_writevalue_multichain("mydata4", "foundSRot", timerow,x[8]);
table_writevalue_multichain("mydata4", "foundLRot", timerow,x[9]);
table_writevalue_multichain("mydata4", "foundOCop", timerow,x[10]);
table_writevalue_multichain("mydata4", "foundDapR", timerow,x[11]);
table_writevalue_multichain("mydata4", "foundAsp", timerow,x[12]);
table_writevalue_multichain("mydata4", "foundMes", timerow,x[13]);
table_writevalue_multichain("mydata4", "foundDapP", timerow,x[14]);
table_writevalue_multichain("mydata4", "foundHol", timerow,x[15]);
table_writevalue_multichain("mydata4", "foundBos", timerow,x[16]);
timerow++;
}
void write_Hy5( const state_type &x , const double t )
{
table_writevalue_multichain("mydata5", "foundBac", timerow,x[0]);
table_writevalue_multichain("mydata5", "foundDia", timerow,x[1]);
table_writevalue_multichain("mydata5", "foundCr", timerow,x[2]);
table_writevalue_multichain("mydata5", "foundSGr", timerow,x[3]);
table_writevalue_multichain("mydata5", "foundLGr", timerow,x[4]);
table_writevalue_multichain("mydata5", "foundSGo", timerow,x[5]);
table_writevalue_multichain("mydata5", "foundLGo", timerow,x[6]);
table_writevalue_multichain("mydata5", "foundInC", timerow,x[7]);
table_writevalue_multichain("mydata5", "foundSRot", timerow,x[8]);
table_writevalue_multichain("mydata5", "foundLRot", timerow,x[9]);
table_writevalue_multichain("mydata5", "foundOCop", timerow,x[10]);
table_writevalue_multichain("mydata5", "foundDapR", timerow,x[11]);
table_writevalue_multichain("mydata5", "foundAsp", timerow,x[12]);
table_writevalue_multichain("mydata5", "foundMes", timerow,x[13]);
table_writevalue_multichain("mydata5", "foundDapP", timerow,x[14]);
table_writevalue_multichain("mydata5", "foundHol", timerow,x[15]);
table_writevalue_multichain("mydata5", "foundBos", timerow,x[16]);
timerow++;
}
double probBac; double realBac; double foundBac;
double probDia; double realDia; double foundDia;
double probCr; double realCr; double foundCr;
double probSGr; double realSGr; double foundSGr;
double probLGr; double realLGr; double foundLGr;
double probSGo; double realSGo; double foundSGo;
double probLGo; double realLGo; double foundLGo;
double probInC; double realInC; double foundInC;
double probSRot; double realSRot; double foundSRot;
double probLRot; double realLRot; double foundLRot;
double probOCop; double realOCop; double foundOCop;
double probDapR; double realDapR; double foundDapR;
double probAsp; double realAsp; double foundAsp;
double probMes; double realMes; double foundMes;
double probDapP; double realDapP; double foundDapP;
double probHol; double realHol; double foundHol;
double probBos; double realBos; double foundBos;
void likelihood()
{
set_metr_ltotnew(0.0); /* set sum over log-Likelihood to zero */
set_metr_number_ok(0);
int numdata;
timerow = 0;
aRBac=cv("aRBac");
aRDia=cv("aRDia");
aRCr=cv("aRCr");
aRSGr=cv("aRSGr");
aRLGr=cv("aRLGr");
aRSGo=cv("aRSGo");
aRLGo=cv("aRLGo");
aRInC=cv("aRInC");
aTSRot=cv("aTSRot");
aTLRot=cv("aTLRot");
aTOCop=cv("aTOCop");
aTDapR=cv("aTDapR");
aTAsp=cv("aTAsp");
aTMes=cv("aTMes");
aTDapP=cv("aTDapP");
aTHol=cv("aTHol");
aTBos=cv("aTBos");
aJSRot=cv("aJSRot");
aJLRot=cv("aJLRot");
aJOCop=cv("aJOCop");
aJDapR=cv("aJDapR");
aJAsp=cv("aJAsp");
aJMes=cv("aJMes");
aJDapP=cv("aJDapP");
aJHol=cv("aJHol");
aJBos=cv("aJBos");

fR=cv("fR");
fT=cv("fT");
K=cv("K");
sdZ=cv("sdZ");
sdP=cv("sdP");
fJ=cv("fJ");
d=cv("d");
W=cv("W");
q=cv("q");

  timerow = 0;

                state_type x1 = {start1Bac,start1Dia,start1Cr,start1SGr,start1LGr,start1SGo,start1LGo,start1InC,start1SRot,start1LRot,start1OCop,start1DapR,start1Asp,start1Mes,start1DapP,start1Hol,start1Bos};
integrate_const(controlled_stepper,ODE,x1,0.0,1.4,0.1,write_Hy1 );
numdata = 14;
                 for(int ii = 0; ii < numdata; ii++)
{ realBac = table_getvalue("mydata1", "Bac",ii);
realDia = table_getvalue("mydata1", "Dia",ii);
realCr = table_getvalue("mydata1", "Cr",ii);
realSGr = table_getvalue("mydata1", "SGr",ii);
realLGr = table_getvalue("mydata1", "LGr",ii);
realSGo = table_getvalue("mydata1", "SGo",ii);
realLGo = table_getvalue("mydata1", "LGo",ii);
realInC = table_getvalue("mydata1", "InC",ii);
realSRot = table_getvalue("mydata1", "SRot",ii);
realLRot = table_getvalue("mydata1", "LRot",ii);
realOCop = table_getvalue("mydata1", "OCop",ii);
realDapR = table_getvalue("mydata1", "DapR",ii);
realAsp = table_getvalue("mydata1", "Asp",ii);
realMes = table_getvalue("mydata1", "Mes",ii);
realDapP = table_getvalue("mydata1", "DapP",ii);
realHol = table_getvalue("mydata1", "Hol",ii);
realBos = table_getvalue("mydata1", "Bos",ii);
foundBac = table_getvalue("mydata1", "foundBac",ii);
foundDia = table_getvalue("mydata1", "foundDia",ii);
foundCr = table_getvalue("mydata1", "foundCr",ii);
foundSGr = table_getvalue("mydata1", "foundSGr",ii);
foundLGr = table_getvalue("mydata1", "foundLGr",ii);
foundSGo = table_getvalue("mydata1", "foundSGo",ii);
foundLGo = table_getvalue("mydata1", "foundLGo",ii);
foundInC = table_getvalue("mydata1", "foundInC",ii);
foundSRot = table_getvalue("mydata1", "foundSRot",ii);
foundLRot = table_getvalue("mydata1", "foundLRot",ii);
foundOCop = table_getvalue("mydata1", "foundOCop",ii);
foundDapR = table_getvalue("mydata1", "foundDapR",ii);
foundAsp = table_getvalue("mydata1", "foundAsp",ii);
foundMes = table_getvalue("mydata1", "foundMes",ii);
foundDapP = table_getvalue("mydata1", "foundDapP",ii);
foundHol = table_getvalue("mydata1", "foundHol",ii);
foundBos = table_getvalue("mydata1", "foundBos",ii);
inc_metr_ltotnew(log(probBac=lognormal_density(realBac, foundBac,sdP)));
inc_metr_ltotnew(log(probDia=lognormal_density(realDia, foundDia,sdP)));
inc_metr_ltotnew(log(probCr=lognormal_density(realCr, foundCr,sdP)));
inc_metr_ltotnew(log(probSGr=lognormal_density(realSGr, foundSGr,sdP)));
inc_metr_ltotnew(log(probLGr=lognormal_density(realLGr, foundLGr,sdP)));
inc_metr_ltotnew(log(probSGo=lognormal_density(realSGo, foundSGo,sdP)));
inc_metr_ltotnew(log(probLGo=lognormal_density(realLGo, foundLGo,sdP)));
inc_metr_ltotnew(log(probInC=lognormal_density(realInC, foundInC,sdP)));
inc_metr_ltotnew(log(probSRot=lognormal_density(realSRot, foundSRot,sdZ)));
inc_metr_ltotnew(log(probLRot=lognormal_density(realLRot, foundLRot,sdZ)));
inc_metr_ltotnew(log(probOCop=lognormal_density(realOCop, foundOCop,sdZ)));
inc_metr_ltotnew(log(probDapR=lognormal_density(realDapR, foundDapR,sdZ)));
inc_metr_ltotnew(log(probAsp=lognormal_density(realAsp, foundAsp,sdZ)));
inc_metr_ltotnew(log(probMes=lognormal_density(realMes, foundMes,sdZ)));
inc_metr_ltotnew(log(probDapP=lognormal_density(realDapP, foundDapP,sdZ)));
inc_metr_ltotnew(log(probHol=lognormal_density(realHol, foundHol,sdZ)));
inc_metr_ltotnew(log(probBos=lognormal_density(realBos, foundBos,sdZ)));
inc_metr_number_ok(1); // adds one to the sample size, used in BIC later on
 }
  timerow = 0;

                state_type x2 = {start2Bac,start2Dia,start2Cr,start2SGr,start2LGr,start2SGo,start2LGo,start2InC,start2SRot,start2LRot,start2OCop,start2DapR,start2Asp,start2Mes,start2DapP,start2Hol,start2Bos};
integrate_const(controlled_stepper,ODE,x2,0.0,1.4,0.1,write_Hy2 );
numdata = 14;
                 for(int ii = 0; ii < numdata; ii++)
{ realBac = table_getvalue("mydata2", "Bac",ii);
realDia = table_getvalue("mydata2", "Dia",ii);
realCr = table_getvalue("mydata2", "Cr",ii);
realSGr = table_getvalue("mydata2", "SGr",ii);
realLGr = table_getvalue("mydata2", "LGr",ii);
realSGo = table_getvalue("mydata2", "SGo",ii);
realLGo = table_getvalue("mydata2", "LGo",ii);
realInC = table_getvalue("mydata2", "InC",ii);
realSRot = table_getvalue("mydata2", "SRot",ii);
realLRot = table_getvalue("mydata2", "LRot",ii);
realOCop = table_getvalue("mydata2", "OCop",ii);
realDapR = table_getvalue("mydata2", "DapR",ii);
realAsp = table_getvalue("mydata2", "Asp",ii);
realMes = table_getvalue("mydata2", "Mes",ii);
realDapP = table_getvalue("mydata2", "DapP",ii);
realHol = table_getvalue("mydata2", "Hol",ii);
realBos = table_getvalue("mydata2", "Bos",ii);
foundBac = table_getvalue("mydata2", "foundBac",ii);
foundDia = table_getvalue("mydata2", "foundDia",ii);
foundCr = table_getvalue("mydata2", "foundCr",ii);
foundSGr = table_getvalue("mydata2", "foundSGr",ii);
foundLGr = table_getvalue("mydata2", "foundLGr",ii);
foundSGo = table_getvalue("mydata2", "foundSGo",ii);
foundLGo = table_getvalue("mydata2", "foundLGo",ii);
foundInC = table_getvalue("mydata2", "foundInC",ii);
foundSRot = table_getvalue("mydata2", "foundSRot",ii);
foundLRot = table_getvalue("mydata2", "foundLRot",ii);
foundOCop = table_getvalue("mydata2", "foundOCop",ii);
foundDapR = table_getvalue("mydata2", "foundDapR",ii);
foundAsp = table_getvalue("mydata2", "foundAsp",ii);
foundMes = table_getvalue("mydata2", "foundMes",ii);
foundDapP = table_getvalue("mydata2", "foundDapP",ii);
foundHol = table_getvalue("mydata2", "foundHol",ii);
foundBos = table_getvalue("mydata2", "foundBos",ii);
inc_metr_ltotnew(log(probBac=lognormal_density(realBac, foundBac,sdP)));
inc_metr_ltotnew(log(probDia=lognormal_density(realDia, foundDia,sdP)));
inc_metr_ltotnew(log(probCr=lognormal_density(realCr, foundCr,sdP)));
inc_metr_ltotnew(log(probSGr=lognormal_density(realSGr, foundSGr,sdP)));
inc_metr_ltotnew(log(probLGr=lognormal_density(realLGr, foundLGr,sdP)));
inc_metr_ltotnew(log(probSGo=lognormal_density(realSGo, foundSGo,sdP)));
inc_metr_ltotnew(log(probLGo=lognormal_density(realLGo, foundLGo,sdP)));
inc_metr_ltotnew(log(probInC=lognormal_density(realInC, foundInC,sdP)));
inc_metr_ltotnew(log(probSRot=lognormal_density(realSRot, foundSRot,sdZ)));
inc_metr_ltotnew(log(probLRot=lognormal_density(realLRot, foundLRot,sdZ)));
inc_metr_ltotnew(log(probOCop=lognormal_density(realOCop, foundOCop,sdZ)));
inc_metr_ltotnew(log(probDapR=lognormal_density(realDapR, foundDapR,sdZ)));
inc_metr_ltotnew(log(probAsp=lognormal_density(realAsp, foundAsp,sdZ)));
inc_metr_ltotnew(log(probMes=lognormal_density(realMes, foundMes,sdZ)));
inc_metr_ltotnew(log(probDapP=lognormal_density(realDapP, foundDapP,sdZ)));
inc_metr_ltotnew(log(probHol=lognormal_density(realHol, foundHol,sdZ)));
inc_metr_ltotnew(log(probBos=lognormal_density(realBos, foundBos,sdZ)));
inc_metr_number_ok(1); // adds one to the sample size, used in BIC later on
 }
  timerow = 0;

                state_type x3 = {start3Bac,start3Dia,start3Cr,start3SGr,start3LGr,start3SGo,start3LGo,start3InC,start3SRot,start3LRot,start3OCop,start3DapR,start3Asp,start3Mes,start3DapP,start3Hol,start3Bos};
integrate_const(controlled_stepper,ODE,x3,0.0,1.4,0.1,write_Hy3 );
numdata = 14;
                 for(int ii = 0; ii < numdata; ii++)
{ realBac = table_getvalue("mydata3", "Bac",ii);
realDia = table_getvalue("mydata3", "Dia",ii);
realCr = table_getvalue("mydata3", "Cr",ii);
realSGr = table_getvalue("mydata3", "SGr",ii);
realLGr = table_getvalue("mydata3", "LGr",ii);
realSGo = table_getvalue("mydata3", "SGo",ii);
realLGo = table_getvalue("mydata3", "LGo",ii);
realInC = table_getvalue("mydata3", "InC",ii);
realSRot = table_getvalue("mydata3", "SRot",ii);
realLRot = table_getvalue("mydata3", "LRot",ii);
realOCop = table_getvalue("mydata3", "OCop",ii);
realDapR = table_getvalue("mydata3", "DapR",ii);
realAsp = table_getvalue("mydata3", "Asp",ii);
realMes = table_getvalue("mydata3", "Mes",ii);
realDapP = table_getvalue("mydata3", "DapP",ii);
realHol = table_getvalue("mydata3", "Hol",ii);
realBos = table_getvalue("mydata3", "Bos",ii);
foundBac = table_getvalue("mydata3", "foundBac",ii);
foundDia = table_getvalue("mydata3", "foundDia",ii);
foundCr = table_getvalue("mydata3", "foundCr",ii);
foundSGr = table_getvalue("mydata3", "foundSGr",ii);
foundLGr = table_getvalue("mydata3", "foundLGr",ii);
foundSGo = table_getvalue("mydata3", "foundSGo",ii);
foundLGo = table_getvalue("mydata3", "foundLGo",ii);
foundInC = table_getvalue("mydata3", "foundInC",ii);
foundSRot = table_getvalue("mydata3", "foundSRot",ii);
foundLRot = table_getvalue("mydata3", "foundLRot",ii);
foundOCop = table_getvalue("mydata3", "foundOCop",ii);
foundDapR = table_getvalue("mydata3", "foundDapR",ii);
foundAsp = table_getvalue("mydata3", "foundAsp",ii);
foundMes = table_getvalue("mydata3", "foundMes",ii);
foundDapP = table_getvalue("mydata3", "foundDapP",ii);
foundHol = table_getvalue("mydata3", "foundHol",ii);
foundBos = table_getvalue("mydata3", "foundBos",ii);
inc_metr_ltotnew(log(probBac=lognormal_density(realBac, foundBac,sdP)));
inc_metr_ltotnew(log(probDia=lognormal_density(realDia, foundDia,sdP)));
inc_metr_ltotnew(log(probCr=lognormal_density(realCr, foundCr,sdP)));
inc_metr_ltotnew(log(probSGr=lognormal_density(realSGr, foundSGr,sdP)));
inc_metr_ltotnew(log(probLGr=lognormal_density(realLGr, foundLGr,sdP)));
inc_metr_ltotnew(log(probSGo=lognormal_density(realSGo, foundSGo,sdP)));
inc_metr_ltotnew(log(probLGo=lognormal_density(realLGo, foundLGo,sdP)));
inc_metr_ltotnew(log(probInC=lognormal_density(realInC, foundInC,sdP)));
inc_metr_ltotnew(log(probSRot=lognormal_density(realSRot, foundSRot,sdZ)));
inc_metr_ltotnew(log(probLRot=lognormal_density(realLRot, foundLRot,sdZ)));
inc_metr_ltotnew(log(probOCop=lognormal_density(realOCop, foundOCop,sdZ)));
inc_metr_ltotnew(log(probDapR=lognormal_density(realDapR, foundDapR,sdZ)));
inc_metr_ltotnew(log(probAsp=lognormal_density(realAsp, foundAsp,sdZ)));
inc_metr_ltotnew(log(probMes=lognormal_density(realMes, foundMes,sdZ)));
inc_metr_ltotnew(log(probDapP=lognormal_density(realDapP, foundDapP,sdZ)));
inc_metr_ltotnew(log(probHol=lognormal_density(realHol, foundHol,sdZ)));
inc_metr_ltotnew(log(probBos=lognormal_density(realBos, foundBos,sdZ)));
inc_metr_number_ok(1); // adds one to the sample size, used in BIC later on
 }
  timerow = 0;

                state_type x4 = {start4Bac,start4Dia,start4Cr,start4SGr,start4LGr,start4SGo,start4LGo,start4InC,start4SRot,start4LRot,start4OCop,start4DapR,start4Asp,start4Mes,start4DapP,start4Hol,start4Bos};
integrate_const(controlled_stepper,ODE,x4,0.0,1.4,0.1,write_Hy4 );
numdata = 14;
                 for(int ii = 0; ii < numdata; ii++)
{ realBac = table_getvalue("mydata4", "Bac",ii);
realDia = table_getvalue("mydata4", "Dia",ii);
realCr = table_getvalue("mydata4", "Cr",ii);
realSGr = table_getvalue("mydata4", "SGr",ii);
realLGr = table_getvalue("mydata4", "LGr",ii);
realSGo = table_getvalue("mydata4", "SGo",ii);
realLGo = table_getvalue("mydata4", "LGo",ii);
realInC = table_getvalue("mydata4", "InC",ii);
realSRot = table_getvalue("mydata4", "SRot",ii);
realLRot = table_getvalue("mydata4", "LRot",ii);
realOCop = table_getvalue("mydata4", "OCop",ii);
realDapR = table_getvalue("mydata4", "DapR",ii);
realAsp = table_getvalue("mydata4", "Asp",ii);
realMes = table_getvalue("mydata4", "Mes",ii);
realDapP = table_getvalue("mydata4", "DapP",ii);
realHol = table_getvalue("mydata4", "Hol",ii);
realBos = table_getvalue("mydata4", "Bos",ii);
foundBac = table_getvalue("mydata4", "foundBac",ii);
foundDia = table_getvalue("mydata4", "foundDia",ii);
foundCr = table_getvalue("mydata4", "foundCr",ii);
foundSGr = table_getvalue("mydata4", "foundSGr",ii);
foundLGr = table_getvalue("mydata4", "foundLGr",ii);
foundSGo = table_getvalue("mydata4", "foundSGo",ii);
foundLGo = table_getvalue("mydata4", "foundLGo",ii);
foundInC = table_getvalue("mydata4", "foundInC",ii);
foundSRot = table_getvalue("mydata4", "foundSRot",ii);
foundLRot = table_getvalue("mydata4", "foundLRot",ii);
foundOCop = table_getvalue("mydata4", "foundOCop",ii);
foundDapR = table_getvalue("mydata4", "foundDapR",ii);
foundAsp = table_getvalue("mydata4", "foundAsp",ii);
foundMes = table_getvalue("mydata4", "foundMes",ii);
foundDapP = table_getvalue("mydata4", "foundDapP",ii);
foundHol = table_getvalue("mydata4", "foundHol",ii);
foundBos = table_getvalue("mydata4", "foundBos",ii);
inc_metr_ltotnew(log(probBac=lognormal_density(realBac, foundBac,sdP)));
inc_metr_ltotnew(log(probDia=lognormal_density(realDia, foundDia,sdP)));
inc_metr_ltotnew(log(probCr=lognormal_density(realCr, foundCr,sdP)));
inc_metr_ltotnew(log(probSGr=lognormal_density(realSGr, foundSGr,sdP)));
inc_metr_ltotnew(log(probLGr=lognormal_density(realLGr, foundLGr,sdP)));
inc_metr_ltotnew(log(probSGo=lognormal_density(realSGo, foundSGo,sdP)));
inc_metr_ltotnew(log(probLGo=lognormal_density(realLGo, foundLGo,sdP)));
inc_metr_ltotnew(log(probInC=lognormal_density(realInC, foundInC,sdP)));
inc_metr_ltotnew(log(probSRot=lognormal_density(realSRot, foundSRot,sdZ)));
inc_metr_ltotnew(log(probLRot=lognormal_density(realLRot, foundLRot,sdZ)));
inc_metr_ltotnew(log(probOCop=lognormal_density(realOCop, foundOCop,sdZ)));
inc_metr_ltotnew(log(probDapR=lognormal_density(realDapR, foundDapR,sdZ)));
inc_metr_ltotnew(log(probAsp=lognormal_density(realAsp, foundAsp,sdZ)));
inc_metr_ltotnew(log(probMes=lognormal_density(realMes, foundMes,sdZ)));
inc_metr_ltotnew(log(probDapP=lognormal_density(realDapP, foundDapP,sdZ)));
inc_metr_ltotnew(log(probHol=lognormal_density(realHol, foundHol,sdZ)));
inc_metr_ltotnew(log(probBos=lognormal_density(realBos, foundBos,sdZ)));
inc_metr_number_ok(1); // adds one to the sample size, used in BIC later on
 }
  timerow = 0;

                state_type x5 = {start5Bac,start5Dia,start5Cr,start5SGr,start5LGr,start5SGo,start5LGo,start5InC,start5SRot,start5LRot,start5OCop,start5DapR,start5Asp,start5Mes,start5DapP,start5Hol,start5Bos};
integrate_const(controlled_stepper,ODE,x5,0.0,1.4,0.1,write_Hy5 );
numdata = 14;
                 for(int ii = 0; ii < numdata; ii++)
{ realBac = table_getvalue("mydata5", "Bac",ii);
realDia = table_getvalue("mydata5", "Dia",ii);
realCr = table_getvalue("mydata5", "Cr",ii);
realSGr = table_getvalue("mydata5", "SGr",ii);
realLGr = table_getvalue("mydata5", "LGr",ii);
realSGo = table_getvalue("mydata5", "SGo",ii);
realLGo = table_getvalue("mydata5", "LGo",ii);
realInC = table_getvalue("mydata5", "InC",ii);
realSRot = table_getvalue("mydata5", "SRot",ii);
realLRot = table_getvalue("mydata5", "LRot",ii);
realOCop = table_getvalue("mydata5", "OCop",ii);
realDapR = table_getvalue("mydata5", "DapR",ii);
realAsp = table_getvalue("mydata5", "Asp",ii);
realMes = table_getvalue("mydata5", "Mes",ii);
realDapP = table_getvalue("mydata5", "DapP",ii);
realHol = table_getvalue("mydata5", "Hol",ii);
realBos = table_getvalue("mydata5", "Bos",ii);
foundBac = table_getvalue("mydata5", "foundBac",ii);
foundDia = table_getvalue("mydata5", "foundDia",ii);
foundCr = table_getvalue("mydata5", "foundCr",ii);
foundSGr = table_getvalue("mydata5", "foundSGr",ii);
foundLGr = table_getvalue("mydata5", "foundLGr",ii);
foundSGo = table_getvalue("mydata5", "foundSGo",ii);
foundLGo = table_getvalue("mydata5", "foundLGo",ii);
foundInC = table_getvalue("mydata5", "foundInC",ii);
foundSRot = table_getvalue("mydata5", "foundSRot",ii);
foundLRot = table_getvalue("mydata5", "foundLRot",ii);
foundOCop = table_getvalue("mydata5", "foundOCop",ii);
foundDapR = table_getvalue("mydata5", "foundDapR",ii);
foundAsp = table_getvalue("mydata5", "foundAsp",ii);
foundMes = table_getvalue("mydata5", "foundMes",ii);
foundDapP = table_getvalue("mydata5", "foundDapP",ii);
foundHol = table_getvalue("mydata5", "foundHol",ii);
foundBos = table_getvalue("mydata5", "foundBos",ii);
inc_metr_ltotnew(log(probBac=lognormal_density(realBac, foundBac,sdP)));
inc_metr_ltotnew(log(probDia=lognormal_density(realDia, foundDia,sdP)));
inc_metr_ltotnew(log(probCr=lognormal_density(realCr, foundCr,sdP)));
inc_metr_ltotnew(log(probSGr=lognormal_density(realSGr, foundSGr,sdP)));
inc_metr_ltotnew(log(probLGr=lognormal_density(realLGr, foundLGr,sdP)));
inc_metr_ltotnew(log(probSGo=lognormal_density(realSGo, foundSGo,sdP)));
inc_metr_ltotnew(log(probLGo=lognormal_density(realLGo, foundLGo,sdP)));
inc_metr_ltotnew(log(probInC=lognormal_density(realInC, foundInC,sdP)));
inc_metr_ltotnew(log(probSRot=lognormal_density(realSRot, foundSRot,sdZ)));
inc_metr_ltotnew(log(probLRot=lognormal_density(realLRot, foundLRot,sdZ)));
inc_metr_ltotnew(log(probOCop=lognormal_density(realOCop, foundOCop,sdZ)));
inc_metr_ltotnew(log(probDapR=lognormal_density(realDapR, foundDapR,sdZ)));
inc_metr_ltotnew(log(probAsp=lognormal_density(realAsp, foundAsp,sdZ)));
inc_metr_ltotnew(log(probMes=lognormal_density(realMes, foundMes,sdZ)));
inc_metr_ltotnew(log(probDapP=lognormal_density(realDapP, foundDapP,sdZ)));
inc_metr_ltotnew(log(probHol=lognormal_density(realHol, foundHol,sdZ)));
inc_metr_ltotnew(log(probBos=lognormal_density(realBos, foundBos,sdZ)));
inc_metr_number_ok(1); // adds one to the sample size, used in BIC later on
 }
                   return;
}
void final_output()
{ /* run the most likely set of parameters */
               params_set_to_posterior_mean();
               likelihood();char fname1[100];
                                 get_filzbach_path(fname1, 100);
                                 strcat_s(fname1,"_my_outputYEAR1.txt");
                                 table_output("mydata1",fname1);char fname2[100];
                                 get_filzbach_path(fname2, 100);
                                 strcat_s(fname2,"_my_outputYEAR2.txt");
                                 table_output("mydata2",fname2);char fname3[100];
                                 get_filzbach_path(fname3, 100);
                                 strcat_s(fname3,"_my_outputYEAR3.txt");
                                 table_output("mydata3",fname3);char fname4[100];
                                 get_filzbach_path(fname4, 100);
                                 strcat_s(fname4,"_my_outputYEAR4.txt");
                                 table_output("mydata4",fname4);char fname5[100];
                                 get_filzbach_path(fname5, 100);
                                 strcat_s(fname5,"_my_outputYEAR5.txt");
                                 table_output("mydata5",fname5);
return;
}

               /* ************************************************* */
               /* The control function that calls everything else.  */
               /* ************************************************* */

               int main()
{
               atexit(pause);
               // set the likelihood function pointer
               pfn_likelihood = &likelihood;
               initialize_filzbach();
               name_analysis("Priors015");
            read_data();
             setup_parameters();
             set_chains(2);
             runmcmc(10000, 20000, 0, 0); // Burn-in, Bayes, MLE adaption, MLE search
             final_output();
}
             #endif


// Copyright PinaPL
//
// test.cpp
// PinaPL
//

#include <Eigen/Dense>
#include <vector>
#include <fstream>
#include <string>
#include "weights.hpp"
#include "cell.hpp"
#include "test.hpp"
#include "iostream"

void single_cell_test() {
    int input_size = 4;
    int output_size = 4;

    std::cout << "sequence_size is 4, word is warp" << std::endl;
    std::cout
    << "a b c d e f g h i j k l m n o p q r s t u v w x y z"
    << std::endl
    << "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0"
    << std::endl
    << "1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0"
    << std::endl
    << "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0"
    << std::endl
    << "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0"
    << std::endl;

    Weights* cell_weight = new Weights(input_size, output_size);

    Cell cell = Cell(cell_weight);

    std::vector<Eigen::MatrixXd> inputs;

    Eigen::MatrixXd inputW(input_size, 1);
    inputW << 1, 0, 0, 0;

    Eigen::MatrixXd inputA(input_size, 1);
    inputA << 0, 1, 0, 0;

    Eigen::MatrixXd inputR(input_size, 1);
    inputR << 0, 0, 1, 0;

    Eigen::MatrixXd inputP(input_size, 1);
    inputP << 0, 0, 0, 1;

    inputs.push_back(inputW);
    inputs.push_back(inputA);
    inputs.push_back(inputR);
    inputs.push_back(inputP);

    std::vector<Eigen::MatrixXd> deltas;

    for (int j=0; j < 1000; j++) {
        std::cout << "run :" << j << std::endl;

        for (int i=0; i < 3; ++i) {
            Eigen::MatrixXd input = inputs.at(i);

            cell.compute(&input);

            Eigen::MatrixXd output = cell.cell_out.back();

            Eigen::MatrixXd delta(output_size, 1);
            delta = (output-inputs.at(i+1)).cwiseProduct(output-inputs.at(i+1));

            deltas.push_back(delta);
        }

        for (int i=3-1; i >= 0; --i) {
            cell.compute_gate_gradient(&deltas.at(i), i);
            // std::cout << "gate gradient input gate" << std::endl;
            // std::cout << cell.delta_input_gate_out.at(3-i) << std::endl;
        }

        double lambda = -0.1;
        cell.compute_weight_gradient();
        // std::cout << "====== delta poids input_gate ======" << std::endl;
        // std::cout << cell_weight->delta_weight_in_input_gate << std::endl;
        // std::cout << cell.weights->delta_weight_st_input_gate << std::endl;

        cell.update_weights(lambda);
        deltas.clear();
        cell.reset();
    }

    cell.compute(&inputW);

    std::cout << "Result of learning :" << std::endl;
    std::cout << cell.cell_out.back() << std::endl;

    cell.compute(&inputA);
    std::cout << "------------------" << std::endl;
    std::cout << cell.cell_out.back() << std::endl;

    cell.compute(&inputR);
    std::cout << "------------------" << std::endl;
    std::cout << cell.cell_out.back() << std::endl;
}

void single_cell_grammar_test() {
    int input_size = 7;
    int output_size = 7;
    int words_to_learn = 5;

    Weights* cell_weight = new Weights(input_size, output_size);
    Cell cell = Cell(cell_weight);

    std::ifstream file("reber_test_1M.txt");
    std::string str;
    std::vector<Eigen::MatrixXd> deltas;

    while ((std::getline(file, str)) && (0 < words_to_learn)) {
        int lenght_word = str.length();
        for (int i = 0; i < lenght_word-1; ++i) {
            Eigen::MatrixXd in = get_input(str.at(i));
            Eigen::MatrixXd expected = get_input(str.at(i+1));
            cell.compute(&in);
            deltas.push_back((expected - cell.cell_out.back())
                .cwiseProduct((expected - cell.cell_out.back())));
        }

        for (int i = lenght_word - 2 ; i >= 0; --i) {
            Eigen::MatrixXd delta = deltas.at(i);
            cell.compute_gate_gradient(&delta, i);
        }
        cell.compute_weight_gradient();
        cell.update_weights(0.3);
        cell.reset();
        words_to_learn -= 1;
    }

    Eigen::MatrixXd B = get_input('B');
    Eigen::MatrixXd P = get_input('P');
    Eigen::MatrixXd V = get_input('V');
    Eigen::MatrixXd E = get_input('E');
    std::cout << "==================" << std::endl;
    cell.compute(&B);
    std::cout << cell.cell_out.back() << std::endl;
    std::cout << "------------------" << std::endl;
    cell.compute(&P);
    std::cout << cell.cell_out.back() << std::endl;
    std::cout << "------------------" << std::endl;
    cell.compute(&V);
    std::cout << cell.cell_out.back() << std::endl;
    std::cout << "------------------" << std::endl;
    cell.compute(&V);
    std::cout << cell.cell_out.back() << std::endl;
}

Eigen::MatrixXd get_input(char letter) {
    Eigen::MatrixXd in(7, 1);
    switch (letter) {
        case 'B':
            in << 1, 0, 0, 0, 0, 0, 0;
            break;
        case 'T':
            in << 0, 1, 0, 0, 0, 0, 0;
            break;
        case 'P':
            in << 0, 0, 1, 0, 0, 0, 0;
            break;
        case 'S':
            in << 0, 0, 0, 1, 0, 0, 0;
            break;
        case 'X':
            in << 0, 0, 0, 0, 1, 0, 0;
            break;
        case 'V':
            in << 1, 0, 0, 0, 0, 1, 0;
            break;
        case 'E':
            in << 1, 0, 0, 0, 0, 0, 1;
            break;
    }
    return in;
}


#include "FokkerPlanck.hpp"
#include "Constants.hpp"
#include <cmath>
#include <iostream>

#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

using namespace std;

template<class T>
FokkerPlanck<T>::FokkerPlanck(
        const std::vector<T>& excitation_energies,
        const std::vector< dynarray<T> >& excitation_rates,
        const T ionization_energy,
        const dynarray<T> & ionization_rate,
        unsigned int num_velocities,
        T max_velocity,
        T Te,
        T ne,
        T rate_multiplier)
    :
        mExcitationEnergies(excitation_energies),
        mIonizationEnergy(ionization_energy),

        mF(num_velocities, 0, max_velocity),
        mTe(Te),
        mNe(ne),
        mRateMultiplier(rate_multiplier),

        m_tmp1_v_c(num_velocities, 0, max_velocity),
        m_tmp2_v_c(num_velocities, 0, max_velocity),
        m_tmp3_v_c(num_velocities, 0, max_velocity),
        m_tmp4_v_c(num_velocities, 0, max_velocity),
        m_tmp1_v_d(num_velocities, 0, max_velocity),
        m_tmp2_v_d(num_velocities, 0, max_velocity),
        m_tmp3_v_d(num_velocities, 0, max_velocity),
        m_tmp4_v_d(num_velocities, 0, max_velocity)

    {
        T energy_factor = 0.5*ELECTRON_MASS/ELEMENTAL_CHARGE;

        mExcitationRates.resize(excitation_rates.size());

        // put excitation rates into velocity space
        for (unsigned int r = 0; r < mExcitationRates.size(); ++r)
        {
            dynarray<T> & rates_v = mExcitationRates[r];
            const dynarray<T> & rates_e = excitation_rates[r];

            rates_v.resize(mF.length());
            rates_v.set_xminmax(mF.xmin(), mF.xmax());

            for(unsigned int i = 0; i < rates_v.length(); ++i)
            {
                T energy = energy_factor*rates_v.x(i)*rates_v.x(i);
                rates_v(i) = rates_e.interp(energy);
            }

        }

        mIonizationRate.resize(mF.length());
        mIonizationRate.set_xminmax(mF.xmin(), mF.xmax());

        for(unsigned int i = 0; i < mIonizationRate.length(); ++i)
        {
            T energy = energy_factor*mIonizationRate.x(i)*mIonizationRate.x(i);
            mIonizationRate(i) = ionization_rate.interp(energy);
        }

        // inital F is maxwellian
        for (unsigned int i = 0; i < mF.length(); ++i)
        {
            mF(i) = exp(-0.5*ELECTRON_MASS*pow(mF.x(i),2.0)/(ELEMENTAL_CHARGE*mTe));
        }

        dynarray<T> F_tmp(mF.length(), mF.xmin(), mF.xmax());

        // normalize

        for(unsigned int i = 0; i < mF.length(); ++i)
        {
            F_tmp(i) = 4.0*PI_CONST*mF(i)*pow(mF.x(i),2.0);

        }

        T A = F_tmp.integral_total();

        for(unsigned int i = 0; i < mF.length(); ++i)
        {
            mF(i) /= A;
        }
    }

template<class T>
dynarray<T>& FokkerPlanck<T>::f()
    {
        return mF;
    }

template<class T>
T FokkerPlanck<T>::step(
        T dt,
        T dt_rate_recalc,
        T min_equib_rate,
        unsigned int max_iterations,
        T& Te_eff)
    {

        T D0 = 0; // heating diffusion term
        T dfdt = 0;
        T equib_rate = 0;
        unsigned int iterations = 0;

        unsigned int num_steps_between_recalc = ceil(dt_rate_recalc/dt);

        dynarray<T> F_derivative(mF.length(), mF.xmin(), mF.xmax());

        dynarray<T> F_2nd_derivative(mF.length(), mF.xmin(), mF.xmax());
        dynarray<T> D(mF.length(), mF.xmin(), mF.xmax());
        dynarray<T> R(mF.length(), mF.xmin(), mF.xmax());
        dynarray<T> K_exc(mF.length(), mF.xmin(), mF.xmax());
        dynarray<T> K_ion(mF.length(), mF.xmin(), mF.xmax());
        dynarray<T> D_derivative(mF.length(), mF.xmin(), mF.xmax());
        dynarray<T> R_derivative(mF.length(), mF.xmin(), mF.xmax());

        dynarray<T> F_tmp(mF.length(), mF.xmin(), mF.xmax());

        dynarray<T> tmp1_v_a(mF.length(), mF.xmin(), mF.xmax());

        dynarray<T> tmp1_v_b(mF.length(), mF.xmin(), mF.xmax());

        dynarray<T> M1_tmp(mF.length(), mF.xmin(), mF.xmax());
        dynarray<T> M2_tmp(mF.length(), mF.xmin(), mF.xmax());

        dynarray<T> K_power_tmp(mF.length(), mF.xmin(), mF.xmax());
        dynarray<T> D0_power_tmp(mF.length(), mF.xmin(), mF.xmax());

        // adjustable to help match target Te
        T heating_factor = 1;

        do{

            mF.derivative(F_derivative);
            mF.derivative_2(F_2nd_derivative);

            if (iterations%num_steps_between_recalc == 0 || iterations == 0)
            {

                calc_D(mF, D, tmp1_v_a);
                calc_R(mF, F_derivative, R, tmp1_v_b);
                calc_K_exc(K_exc);
                calc_K_ion(K_ion);

/*
                boost::thread_group rate_threads;

                rate_threads.create_thread(boost::bind(&FokkerPlanck<T>::calc_D, this, boost::ref(mF), boost::ref(D), boost::ref(tmp1_v_a)));
                rate_threads.create_thread(boost::bind(&FokkerPlanck<T>::calc_R, this, boost::ref(mF), boost::ref(F_derivative), boost::ref(R), boost::ref(tmp1_v_b)));
                rate_threads.create_thread(boost::bind(&FokkerPlanck<T>::calc_K_exc, this, boost::ref(K_exc)));
                rate_threads.create_thread(boost::bind(&FokkerPlanck<T>::calc_K_ion, this, boost::ref(K_ion)));

                rate_threads.join_all();
*/
                D.derivative(D_derivative);
                R.derivative(R_derivative);

            }

            for(unsigned int i = 1; i < mF.length(); ++i)
            {

                M1_tmp(i) = 2.0*F_derivative(i)/mF.x(i);
                M2_tmp(i) = 2.0*R(i)/mF.x(i);

            }

            M1_tmp(0) = M1_tmp(1); // NaN b/c of 1/v, but does converge so use nearest point
            M2_tmp(0) = M2_tmp(1);

            // determin D0 to balance power loss from K
            for (unsigned int i = 0; i < mF.length(); ++i)
            {

                K_power_tmp(i) = (K_exc(i) + K_ion(i))*pow(mF.x(i),4.0);

                D0_power_tmp(i) = (M1_tmp(i) + F_2nd_derivative(i))*pow(mF.x(i),4.0);
            }

            D0 = -K_power_tmp.integral_total()/D0_power_tmp.integral_total();

            //cerr << K_power_tmp.integral_total() << endl;

            // calculate effective temperature
            for(unsigned int i = 0; i < mF.length(); ++i)
            {
                tmp1_v_a(i) = 4.0*PI_CONST*mF(i)*pow(mF.x(i),4.0);

            }

            Te_eff = (1.0/3.0)*ELECTRON_MASS*tmp1_v_a.integral_total()/ELEMENTAL_CHARGE;

            //cerr << heating_factor << endl;

            heating_factor = 1 + (mTe-Te_eff)/mTe;

            //cerr << D0 << endl;

            // redo the M1

            for(unsigned int i = 1; i < mF.length(); ++i)
            {

                M1_tmp(i) = 2.0*F_derivative(i)*(D(i) + heating_factor*D0)/mF.x(i);

            }

            M1_tmp(0) = M1_tmp(1);

            // calc next time step
            for(unsigned int i = 0; i < mF.length(); ++i)
            {

                dfdt = M1_tmp(i) + (D_derivative(i) - R(i))*F_derivative(i) + F_2nd_derivative(i)*(D(i) + heating_factor*D0) - (M2_tmp(i) + R_derivative(i))*mF(i) + K_exc(i) + K_ion(i);

                //std::cerr << dfdt << std::endl;

                F_tmp(i) = max(0.0, mF(i) + dt*dfdt);
            }

            // re-normalize

            for(unsigned int i = 0; i < mF.length(); ++i)
            {
                tmp1_v_a(i) = 4.0*PI_CONST*F_tmp(i)*pow(F_tmp.x(i),2.0);

            }

            T A = tmp1_v_a.integral_total();

            for(unsigned int i = 0; i < mF.length(); ++i)
            {
                F_tmp(i) = F_tmp(i)/A;
            }

            // calculate equib rate
            for(unsigned int i = 0; i < mF.length(); ++i)
            {
                tmp1_v_a(i) = 4.0*PI_CONST*abs(F_tmp(i)-mF(i))*pow(mF.x(i),2.0);

                // can now copy over new value of F.
                mF(i) = F_tmp(i);

            }

            T delta = tmp1_v_a.integral_total();

            equib_rate = delta/dt;

            ++iterations;

            if (iterations%10000 == 0)
            {

                //std::cerr << "Iteration " << iterations << ", Te_eff = " << Te_eff << ", equib_rate = " << equib_rate << " (" << delta << ")" << std::endl;
            }

        }while(equib_rate > min_equib_rate && iterations < max_iterations);

        return equib_rate;

    }

template<class T>
void FokkerPlanck<T>::calc_D(
    const dynarray<T> & F,
    dynarray<T> & out,
    dynarray<T> & tmp)
    {
        if (out.length() != F.length())
        {
            out.resize(F.length());
        }

        out.set_xminmax(F.xmin(), F.xmax());

        T factor = collision_factor();

        for (unsigned int i = 0; i < F.length(); ++i)
        {

            for (unsigned int j = 0; j < i; ++j)
            {
                // velocities below current velocity

                tmp(j) = F(j)*F.x(j)*(4.0/3.0)*pow(F.x(j)/F.x(i), 3.0);

            }

            for (unsigned int j = i; j < F.length(); ++j)
            {
                // velocities above current velocity

                tmp(j) = F(j)*F.x(j)*(4.0/3.0);

            }

            out(i) = factor*2.0*PI_CONST*tmp.integral_total();

        }
    }

template<class T>
void FokkerPlanck<T>::calc_R(
    const dynarray<T> & F,
    const dynarray<T> & F_derivative,
    dynarray<T> & out,
    dynarray<T> & tmp)
    {
        if (out.length() != F.length())
        {
            out.resize(F.length());
        }

        out.set_xminmax(F.xmin(), F.xmax());

        T factor = collision_factor();

        //cerr << factor << endl;

        for (unsigned int i = 0; i < F.length(); ++i)
        {

            for (unsigned int j = 0; j < i; ++j)
            {
                // velocities below current velocity

                tmp(j) = F_derivative(j)*(4.0/3.0)*pow(F.x(j)/F.x(i), 3.0);

            }

            for (unsigned int j = i; j < F.length(); ++j)
            {
                // velocities above current velocity

                tmp(j) = F_derivative(j)*(4.0/3.0);
            }

            out(i) = factor*2.0*PI_CONST*F.x(i)*tmp.integral_total();

        }
    }

template<class T>
void FokkerPlanck<T>::calc_K_exc(dynarray<T> & out)
    {

        dynarray<T>& loss_v = m_tmp1_v_c;
        dynarray<T>& gain_v = m_tmp2_v_c;

        T energy_factor = 0.5*ELECTRON_MASS/ELEMENTAL_CHARGE;

        for (unsigned int i = 0; i < mF.length(); ++i)
        {
            loss_v(i) = 0;
            gain_v(i) = 0;

            T v2 = pow(mF.x(i), 2.0);

            for (unsigned int r = 0; r < mExcitationRates.size(); ++r)
            {
                const dynarray<T> & rate_function_r = mExcitationRates[r];

                if (energy_factor*v2 > mExcitationEnergies[r])
                {

                    loss_v(i) += mF(i)*rate_function_r(i);

                }

                T s_v2 = v2 + mExcitationEnergies[r]/energy_factor;
                T source_velocity = sqrt(s_v2);

                if (source_velocity <= mF.xmax())
                {
                    // ????
                    gain_v(i) += (source_velocity/mF.x(i))*mF.interp(source_velocity)*rate_function_r.interp(source_velocity);
                }

            }

        }

        gain_v(0) = gain_v(1);

        // make sure total rate will be exactly zero

        dynarray<T>& gain_integral = m_tmp3_v_c;
        dynarray<T>& loss_integral = m_tmp4_v_c;

        for(unsigned int i = 0; i < mF.length(); ++i)
        {
            gain_integral(i) = gain_v(i)*4*PI_CONST*mF.x(i)*mF.x(i);
            loss_integral(i) = loss_v(i)*4*PI_CONST*mF.x(i)*mF.x(i);
        }

        T G_O_v = gain_integral.integral_total();
        T L_O_v = loss_integral.integral_total();

        T gain_norm_factor = L_O_v/G_O_v;

        //cerr << gain_norm_factor << endl;

        for(unsigned int i = 0; i < mF.length(); ++i)
        {
            out(i) = mRateMultiplier*(gain_norm_factor*gain_v(i) - loss_v(i));

        }

    }

template<class T>
void FokkerPlanck<T>::calc_K_ion(dynarray<T> & out)
    {

        dynarray<T>& loss_v = m_tmp1_v_d;
        dynarray<T>& gain_v = m_tmp2_v_d;
        dynarray<T>& gain_density = m_tmp3_v_d;
        dynarray<T>& gain_integral = m_tmp4_v_d;

        T energy_factor = 0.5*ELECTRON_MASS/ELEMENTAL_CHARGE;

        // construct gain density function
        for(unsigned int i = 0; i < mIonizationRate.length(); ++i)
        {
            T v2 = pow(mF.x(i),2.0);
            T energy = energy_factor*v2;

            if (energy > mIonizationEnergy)
            {

                gain_density(i) = v2*mF(i)*mIonizationRate(i)/(energy - mIonizationEnergy);
            }else{
                gain_density(i) = 0;
            }

            //cerr << gain_density(i);
        }

        gain_density.integral(gain_integral);

        // use limits of integral to calculate gain
        for(unsigned int i = 1; i < mF.length(); ++i)
        {
            T v_min = sqrt(pow(mF.x(i),2.0) + mIonizationEnergy/energy_factor);

            unsigned int i_min = min(mF.length()-1, (unsigned int)ceil((v_min - mF.xmin())/mF.dx()));

            // factor of 2 comes from the assumption that secondary electrons are distributed the same as final energy of incident electrons
            gain_v(i) = 2*(gain_integral(mF.length()-1) - gain_integral(i_min))*ELECTRON_MASS/(ELEMENTAL_CHARGE*mF.x(i));
        }

        gain_v(0) = gain_v(1);

        // calculate total loss rate
        for(unsigned int i = 0; i < mF.length(); ++i)
        {

            loss_v(i) = mF(i)*mIonizationRate(i)*4*PI_CONST*mF.x(i)*mF.x(i);
        }

        // calulate total ionization rate
        T L_0 = loss_v.integral_total();

        // total loss rate needs to be twice the ionization rate, to get equilibrium solution

        for(unsigned int i = 0; i < mF.length(); ++i)
        {

            loss_v(i) = mF(i)*(L_0 + mIonizationRate(i));
        }

        // make sure total rate will be exactly zero

        dynarray<T>& loss_integral = m_tmp3_v_d;

        for(unsigned int i = 0; i < mF.length(); ++i)
        {
            gain_integral(i) = gain_v(i)*4*PI_CONST*mF.x(i)*mF.x(i);
            loss_integral(i) = loss_v(i)*4*PI_CONST*mF.x(i)*mF.x(i);
        }

        T G_O_v = gain_integral.integral_total();
        T L_O_v = loss_integral.integral_total();

        if (L_O_v > 0)
        {
            T gain_norm_factor = L_O_v/G_O_v;

            //cerr << gain_norm_factor << endl;

            for(unsigned int i = 0; i < mF.length(); ++i)
            {
                out(i) = mRateMultiplier*(gain_norm_factor*gain_v(i) - loss_v(i));

            }
        }else{

            for(unsigned int i = 0; i < mF.length(); ++i)
            {
                out(i) = 0;

            }
        }

    }

template<class T>
T FokkerPlanck<T>::collision_factor()
    {
        // from NRL plasma formulary for e-e collisions
        T coulomb_logarithm = 23.5 - 0.5*log(mNe/1E6) + (5.0/4.0)*log(mTe) - sqrt(1E-5 + pow(log(mTe)-2.0, 2)/16.0);

        //cerr << coulomb_logarithm << endl;

        return mNe*pow(ELEMENTAL_CHARGE,4.0)*coulomb_logarithm/(8.0*PI_CONST*pow(ELECTRON_MASS*ELECTRIC_CONST, 2.0));
    }


#include <iostream>
#include <NTL/ZZX.h>
#include <random>
#include <cmath>
using namespace std;
using namespace NTL;
ZZX final;
//在范围n之间生存生成随机数
inline int RandomNum()
{
    std::random_device rd;

    std::mt19937 rng(rd());

    //在这里取到一个范围在0～2q的随机 数字
    //在取一个矩阵的时候mod q 取四舍五入
    std::uniform_int_distribution<int> uni(INT_MIN,INT_MAX);

    auto random_integer = uni(rng);

    return random_integer;

}

inline int Mymod(double num,int n)
{
    int dev=(int)std::round(num/n);
    return num-n*dev;
}
//分圆不等式
// x`n+1
ZZX cyclotomic(int N)
{
//    const int  N =5;

    ZZX Inequality;

    //设置常数项为1
    SetCoeff(Inequality, 0, 1);

    SetCoeff(Inequality, N, N);

//    cout<<Inequality<<endl;

    return  Inequality;
}
//多项式环
//设置一个n项的不等式环 使得常数落在范围(-q/2,q/2)
ZZX PolyRing(int n,int q)
{

    ZZX tmp;

    for(unsigned int i=0;i<n;++i)
    {

        int pow=Mymod(RandomNum(), q);
        if(!pow)pow+=1;
        SetCoeff(tmp, i, pow);

    }
//    cout<<tmp<<endl;

    return tmp;
}
//得到密钥
ZZX  PrivateKey(int n,int q)
{
    ZZX f;
    //得到分圆不等式
    ZZX aN=cyclotomic(n);

    ZZX ring=PolyRing(n,q);

    ZZX tmpres;
    while(1)
    {
        int random=std::abs(RandomNum());

        unsigned int idx=random%(n);

//        if(ring[idx]!=NULL)continue;
        ZZ num= ring[idx];

        SetCoeff(f, idx, num*2);
        SetCoeff(f, 0, 1);

        GCD(tmpres, ring, aN);

        //找到   f `
        if(tmpres[0]==1)break;
    }
    final=f;
    return f;
}
//h=2g *(f`-1) -1∈Rq
ZZX  PublicKey(int n,int q)
{
    ZZX ring=PolyRing(n,q);
//    cout<<ring<<endl;
    ZZX f=PrivateKey(n, q);
    ZZ r(1);//比较结果
    ZZX cys=cyclotomic(n); //分圆式 1+x`n
    ZZX eq1;//不等式1，从ring中得到
    ZZX eq2;//不等式2 ,  从ring中得到
    ZZX g;
    ZZX h;//最终结果

    int random2=std::abs(RandomNum());
    unsigned int idx2=random2%(n);
    ZZ num2= ring[idx2];
    SetCoeff(g, idx2, num2);

    int random=std::abs(RandomNum());
    unsigned int idx=random%(n);
    ZZ num= ring[idx];
    SetCoeff(eq1, idx, num);

    int random1=std::abs(RandomNum());
    unsigned int idx1=random1%(n);
    ZZ num1= ring[idx1];
    SetCoeff(eq2, idx1, num1);
    XGCD(r, eq2, f, eq1, cys);
//    ZZ &get=eq2[eq2.rep.length()-1];
    return 2*g*eq2;

}
//加密过程
ZZX Enc(int n,int q)
{
    //需要加密的消息
    ZZX s;
    ZZX e;
    ZZX ring=PolyRing(n,q);
    ZZX m(1);

    int random=std::abs(RandomNum());
    unsigned int idx2=random%(n);
    ZZ num2= ring[idx2];
    SetCoeff(s, idx2, num2);

    int random1=std::abs(RandomNum());
    unsigned int idx1=random1%(n);
    ZZ num1= ring[idx1];
    SetCoeff(e, idx1, num1);

    //private Key
    ZZX h=PublicKey(n, q);
    //密文
    ZZX c=m+h*2+2*e;
    return c;
}
ZZ Dec(int n,int q)
{
    //密文
    ZZX c=   Enc(n,q);
    ZZX dec=c*final;
    return dec[0];
}

int main()
{

}


#pragma once

#include <Eigen/Dense>
#include <Eigen/QR>

using Eigen::Matrix;
using Eigen::Dynamic;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::VectorXi;

VectorXd filter(VectorXd & B, VectorXd & A, VectorXd const & X, VectorXd & SI);

VectorXd filter(VectorXd & B, VectorXd & A, VectorXd const & X);

VectorXd unifloess(VectorXd const & y, int span, bool useLoess);


// Copyright (C) 2014  Victor Fragoso <vfragoso@cs.ucsb.edu>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of the University of California, Santa Barbara nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL VICTOR FRAGOSO BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//

#include "optimo/core/objects.h"
#include "optimo/core/objects_ls.h"
#include "optimo/solvers/gradient_descent.h"
#include "optimo/core/numerical_gradient.h"
#include "optimo/core/numerical_hessian.h"
#include <glog/logging.h>
#include "gtest/gtest.h"
#include <Eigen/LU>

namespace optimo {
namespace solvers {
namespace {
using Eigen::Matrix;
using Eigen::Dynamic;
// Quadratic Objective
template <typename Scalar, uint n>
class QuadraticObjective : public ObjectiveFunctor<Scalar, n> {
 public:
  QuadraticObjective(const Matrix<Scalar, n, n>& _Q,
                     const Matrix<Scalar, n, 1>& _P,
                     const Scalar& _c) {
    Q = _Q;
    P = _P;
    c = _c;
  }

  ~QuadraticObjective(void) { }

  virtual Scalar operator()(const Matrix<Scalar, n, 1>& x) const {
    Matrix<Scalar, 1, 1> sum = x.transpose()*Q*x + P.transpose()*x;
    return sum(0,0) + c;
  }

 protected:
  Matrix<Scalar, n, n> Q;
  Matrix<Scalar, n, 1> P;
  Scalar c;
};

// Quadratic Objective LS
template <typename Scalar>
class QuadraticObjectiveLS : public ObjectiveFunctorLS<Scalar> {
 public:
  QuadraticObjectiveLS(const Matrix<Scalar, Dynamic, Dynamic>& _Q,
                       const Matrix<Scalar, Dynamic, 1>& _P,
                       const Scalar& _c) {
    Q = _Q;
    P = _P;
    c = _c;
  }

  ~QuadraticObjectiveLS(void) { }

  virtual Scalar operator()(const Matrix<Scalar, Dynamic, 1>& x) const {
    Matrix<Scalar, 1, 1> sum = x.transpose()*Q*x + P.transpose()*x;
    return sum(0,0) + c;
  }

 protected:
  Matrix<Scalar, Dynamic, Dynamic> Q;
  Matrix<Scalar, Dynamic, 1> P;
  Scalar c;
};

// Quadratic Program
template <typename Scalar, uint m, uint n>
class QuadraticProgram : public Problem<Scalar, m, n> {
 public:
  QuadraticProgram(const QuadraticObjective<Scalar, n>& qobj,
                   const GradientFunctor<Scalar, n>& g,
                   const HessianFunctor<Scalar, n>& h) :
      Problem<Scalar, m, n>(qobj, g, h) {
  }

  virtual ~QuadraticProgram(void) { }
};

// Quadratic Program LS
template <typename Scalar>
class QuadraticProgramLS : public ProblemLS<Scalar> {
 public:
  QuadraticProgramLS(const QuadraticObjectiveLS<Scalar>& qobj,
                   const GradientFunctorLS<Scalar>& g,
                   const HessianFunctorLS<Scalar>& h) :
      ProblemLS<Scalar>(qobj, g, h) {
  }

  virtual ~QuadraticProgramLS(void) { }
};

// Gradient Functor
template <typename Scalar, uint n>
class QGradient : public GradientFunctor<Scalar, n> {
 public:
  QGradient(const Matrix<Scalar, n, n>& _Q,
            const Matrix<Scalar, n, 1>& _P) {
    Q = _Q;
    P = _P;
  }

  virtual void operator()(const Matrix<Scalar, n, 1>& x,
                          Matrix<Scalar, n, 1>* g) const {
    *g = 2*Q*x + P;
  }

 protected:
  Matrix<Scalar, n, n> Q;
  Matrix<Scalar, n, 1> P;
};

// Gradient Functor LS
template <typename Scalar>
class QGradientLS : public GradientFunctorLS<Scalar> {
 public:
  QGradientLS(const Matrix<Scalar, Dynamic, Dynamic>& _Q,
              const Matrix<Scalar, Dynamic, 1>& _P) {
    Q = _Q;
    P = _P;
  }

  virtual void operator()(const Matrix<Scalar, Dynamic, 1>& x,
                          Matrix<Scalar, Dynamic, 1>* g) const {
    *g = 2*Q*x + P;
  }

 protected:
  Matrix<Scalar, Dynamic, Dynamic> Q;
  Matrix<Scalar, Dynamic, 1> P;
};

// Hessian Functor
template <typename Scalar, uint n>
class QHessian : public HessianFunctor<Scalar, n> {
 public:
  QHessian(const Matrix<Scalar, n, n>& _Q) {
    Q = _Q;
  }

  virtual void operator()(const Matrix<Scalar, n, 1>& x,
                          Matrix<Scalar, n, n>* h) const {
    *h = 2*Q;
  }

 protected:
  Matrix<Scalar, n, n> Q;
};

// Hessian Functor
template <typename Scalar>
class QHessianLS : public HessianFunctorLS<Scalar> {
 public:
  QHessianLS(const Matrix<Scalar, Dynamic, Dynamic>& _Q) {
    Q = _Q;
  }

  virtual void operator()(const Matrix<Scalar, Dynamic, 1>& x,
                          Matrix<Scalar, Dynamic, Dynamic>* h) const {
    *h = 2*Q;
  }

 protected:
  Matrix<Scalar, Dynamic, Dynamic> Q;
};
}  // namespace

TEST(GradientDescent, Quadratic_Programs) {
  const uint n = 3;
  const uint m = 0;
  Matrix<double, n, n> Q;
  Matrix<double, n, 1> P;
  double c = 15.0;
  Matrix<double, n, 1> x;

  Q << 5, 0, 0,
       0, 4, 0,
       0, 0, 1.5;
  P << 0.01, 0.5, -0.1;
  x << 101, 101, 101;

  QuadraticObjective<double, n> qobj(Q, P, c);
  QGradient<double, n> qgradient(Q, P);
  QHessian<double, n> qhessian(Q);

  QuadraticProgram<double, m, n>
      qprogram(qobj, qgradient, qhessian);

  GradientDescent<double, m, n> gdescent;
  double min_val;
  auto res = gdescent(qprogram, &x, &min_val);

  LOG(INFO) << "Min_val: " << min_val
            << " Minimizer: [" << x.transpose() << "]"
            << " Result: " << res
            << std::endl;
  ASSERT_LT(min_val - c, 0.1);
}

TEST(GradientDescent, Quatratic_ProgramLS) {
  const uint n = 3;
  const uint m = 0;
  Matrix<double, Dynamic, Dynamic> Q(n, n);
  Matrix<double, Dynamic, 1> P(n);
  double c = 15.0;
  Matrix<double, Dynamic, 1> x(n);

  Q << 5, 0, 0,
       0, 4, 0,
       0, 0, 1.5;
  P << 0.01, 0.5, -0.1;
  x << 101, 101, 101;

  QuadraticObjectiveLS<double> qobj(Q, P, c);
  QGradientLS<double> qgradient(Q, P);
  QHessianLS<double> qhessian(Q);

  QuadraticProgramLS<double>
      qprogram(qobj, qgradient, qhessian);

  GradientDescentLS<double> gdescent(n);
  double min_val;
  auto res = gdescent(qprogram, &x, &min_val);

  LOG(INFO) << "Min_val: " << min_val
            << " Minimizer: [" << x.transpose() << "]"
            << " Result: " << res
            << std::endl;
  ASSERT_LT(min_val - c, 0.1);
}
}  // solvers
}  // optimo


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 boost.simd.ieee toolbox - modf/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.ieee components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 04/12/2010
///
#include <boost/simd/toolbox/ieee/include/functions/modf.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/simd/include/functions/trunc.hpp>
#include <boost/simd/include/functions/frac.hpp>

#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/toolbox/constant/constant.hpp>
#include <boost/simd/sdk/memory/is_aligned.hpp>
#include <boost/simd/sdk/memory/aligned_type.hpp>
#include <boost/simd/include/functions/load.hpp>
#include <boost/simd/include/functions/splat.hpp>
#include <boost/simd/toolbox/constant/constant.hpp>

NT2_TEST_CASE_TPL ( modf_real__1_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::modf;
  using boost::simd::tag::modf_;
  using boost::simd::load;
  using boost::simd::native;
  using boost::simd::meta::cardinal_of;
  typedef typename boost::dispatch::meta::as_floating<T>::type ftype;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef typename boost::dispatch::meta::upgrade<T>::type   u_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       ivT;
  typedef typename boost::dispatch::meta::call<modf_(vT)>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;
  vT vn, vf;
  T n, f;
  T a[6] = {T(0), T(1), T(1.5), boost::simd::Inf<T>(), boost::simd::Minf<T>(), boost::simd::Nan<T>()};
  for(int i=0; i < 6; i++)
    {
      modf(a[i], n, f);
      modf(boost::simd::splat<vT>(a[i]), vn, vf);
      NT2_TEST_EQUAL(vn[0], n);
      NT2_TEST_EQUAL(vf[0], f);
    }
} // end of test for floating_


#include "gtest/gtest.h"
#include <Eigen/Core>
#include <vector>
#include "MeshIntersection.h"
#include "IntersectionPoint.h"
#include "MeshFactory.h"

namespace {
    using namespace Geotree;

    class MeshIntersectionTest : public ::testing::Test {
    };

    int countPointsAdjascent(IntersectionPoints ip)
    {
        std::pair <int, int> points;
        FaceSet pointsset;

        int previouspoint = -1;
        int point = 0;
        int count = 0;
        do
        {
            pointsset = ip.getPoint(point).connectedPoints;

            if (pointsset.size() == 3)
            {
                for (int i: pointsset)
                {
                    if (ip.getPoint(i).connectedPoints.size() == 3)
                    {
                        pointsset.erase(i);
                    }
                }
            }

            if (pointsset.size() != 2)
            {
                std::cout << "Size is not 2: " << pointsset.size() <<std::endl;
                return -1;
            }

            points.first = *pointsset.begin();
            points.second = *pointsset.rbegin();
            //std::cout << point << " points: " << points.first << ", " <<points.second << std::endl;
            if (points.first != previouspoint)
            {
                previouspoint = point;
                point = points.first;
            }
            else if (points.second != previouspoint)
            {
                previouspoint = point;
                point = points.second;
            }
            else
            {
                return -1;
            }
            count++;
        } while(point != 0);

        return count;
    }

    TEST_F(MeshIntersectionTest, DISABLED_TetrahedronHole) {

        Verticies Vm(4, 3);
        Faces Fm(4, 3);
        Vm << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1;
        Fm << 0, 1, 2, 0, 1, 3, 0, 2, 3, 1, 2, 3;

        Verticies Vn(4, 3);
        Faces Fn(4, 3);
        Vn << 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1;
        Fn << 0, 1, 2, 0, 1, 3, 0, 2, 3, 1, 2, 3;

        Mesh m(Vm, Fm);
        Mesh n(Vn, Fn);

        n.translate(Vector(0.1, 0.1, 0.1));

        MeshIntersection mi(m, n);

        mi.merge();

        EXPECT_EQ(mi.mesh.size(), 8);

        EXPECT_EQ(mi.points.size(), 3);

        for (int i = 0; i < mi.points.size(); i++) {
            IntersectionPoint p = mi.points.getPoint(i);
            EXPECT_EQ(p.first.type, FACE);
            EXPECT_EQ(p.second.type, SEGMENT);
        }

        EXPECT_EQ(countPointsAdjascent(mi.points), 3);

        std::vector<FaceSet> subpaths;
        int face;
        std::vector<EdgePoint> edgePoints;
        FaceSet faces = mi.points.getIntersectedFaces();

        EXPECT_EQ(faces.size(), 3 + 1);

        face = *faces.begin();
        subpaths = mi.paths.getSubPaths(face);
        ASSERT_EQ(subpaths.size(), 1);
        EXPECT_EQ(subpaths[0].size(), 3);
    }

}