/*
 Code *with* scalar source.

  q = 2!!
*/



#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>


#define PI 3.1415926535897932384626433832795028841971693993751


#define Nmax  100 

#define power(x,n) pow(x,n)

double totv = 10. ;

int N ;

double amp = 0. ;

double amp2 ;



double dv, scalvev, Tvev ;

double x[Nmax], Dmat[Nmax][Nmax], D2mat[Nmax][Nmax] ;

double aval ;

double Var[5*Nmax], NewVar[5*Nmax], OldVar[5*Nmax], Eqn[5*Nmax], Lich[5*Nmax][5*Nmax], InitVar[5*Nmax], EqnConst[8*Nmax] ;

double Lmat[5*Nmax*5*Nmax] ;

void calcm(double, double *, double *, double *, double *) ;
void setupCheb(void), readin(void), writeout(double, FILE *), calcstep(double), calcLich(double), calcEqn(double,double *), calcEqnConst(double) ;
void linsolve(double *, double *) ;


int main(void)
{
    FILE *outfile, *paramfile ;
    
    int ii, tt, iter, numsteps, skip, numwrites ;
    double v, buf ;
    
    outfile = fopen("output.dat","w") ; paramfile = fopen("param.dat","w") ;
    
    if(outfile==NULL || paramfile==NULL) {
        printf("\n\nProblem opening output files\n\n") ; 
        exit(1) ;
    }

    readin() ;

    setupCheb() ;
    
    buf = (double) N ;     fwrite(&buf,sizeof(double),1,paramfile) ;
    buf = (double) aval ;  fwrite(&buf,sizeof(double),1,paramfile) ;
    for(ii=0;ii<N;ii++)    fwrite(&x[ii],sizeof(double),1,paramfile) ;
            
    dv = 0.2/((double) N) ;
    
    numsteps = ceil(totv/dv) ;
    
    skip = ceil(((double) numsteps)/200.) ;
    
    
    amp2 = -0. ;
    
    for(iter=0;iter<1;iter++) {

        amp2 = amp2 - 0.7 ;    
        
    printf("amp = %7.3e,  amp2 = %7.3e\n",amp,amp2) ;

    buf = (double) amp ;   fwrite(&buf,sizeof(double),1,paramfile) ;
    buf = (double) amp2 ;  fwrite(&buf,sizeof(double),1,paramfile) ;
    fflush(paramfile) ;
    
    for(ii=0;ii<5*N;ii++) { Var[ii] = InitVar[ii] ;  OldVar[ii] = InitVar[ii] ; }
    
    v = 0. ;
    numwrites=0;
    
    printf(" Initial :   v = 0.   < O > = %7.3f   < rho > = %7.3e\n",sqrt(pow(Var[1],2)+pow(Var[N+1],2))/x[1],Var[3*N+1]/x[1]) ;
    
    for(tt=1;tt<numsteps;tt++) {
        
        calcstep(v) ;
        
        calcEqnConst(v) ;
        
        v+=dv ;
                        
        if(tt%skip==0) {
            
            printf(" ii = %d :   v = %7.3f   < O > = %7.3f   < rho > = %7.3e\n",tt,v,sqrt(pow(Var[1],2)+pow(Var[N+1],2))/x[1],Var[3*N+1]/x[1]) ;
            
            writeout(v,outfile) ; numwrites+=1 ;
            
        }
        
        for(ii=0;ii<5*N;ii++) {
            
            OldVar[ii] = Var[ii] ;
            
            Var[ii] = NewVar[ii] ;
            
        }    
        
    }
        
    buf = (double) numwrites ;   fwrite(&buf,sizeof(double),1,paramfile) ;    
    
    }
    
    
    fclose(outfile) ; fclose(paramfile) ;

    
}



void readin(void)
{
    int ii, pos, chk ;
    FILE *infile ;
        
    infile = fopen("init.dat","r") ;
    
    if(infile==NULL) {
        printf("\n\nProblem opening file\n\n") ; 
        exit(1) ;
    }
    
    chk = fread(&N,sizeof(int),1,infile) ;
    chk += fread(&aval,sizeof(double),1,infile) ;
    
    printf("N = %d  ;  aval = %e\n",N,aval) ;
        
    for(ii=0;ii<N;ii++) chk += fread(&InitVar[ii],sizeof(double),1,infile) ;
    for(ii=0;ii<N;ii++) chk += fread(&InitVar[N+ii],sizeof(double),1,infile) ;
    for(ii=0;ii<N;ii++) chk += fread(&InitVar[2*N+ii],sizeof(double),1,infile) ;
    for(ii=0;ii<N;ii++) chk += fread(&InitVar[3*N+ii],sizeof(double),1,infile) ;
    for(ii=0;ii<N;ii++) chk += fread(&InitVar[4*N+ii],sizeof(double),1,infile) ;
    
    fclose(infile) ;
    
    if(chk!=2+5*N) {
        printf("\n\nProblem reading file - EOF encountered  chk = %d\n\n",chk) ; 
        exit(1) ;
    }
        
}



void writeout(double time, FILE *outfile)
{
    int ii, chk ;
    
    chk = 0 ;
    
    chk += fwrite(&time,sizeof(double),1,outfile) ;
        
    for(ii=0;ii<N;ii++) chk += fwrite(&Var[ii],sizeof(double),1,outfile) ;
    for(ii=0;ii<N;ii++) chk += fwrite(&Var[N+ii],sizeof(double),1,outfile) ;
    for(ii=0;ii<N;ii++) chk += fwrite(&Var[2*N+ii],sizeof(double),1,outfile) ;
    for(ii=0;ii<N;ii++) chk += fwrite(&Var[3*N+ii],sizeof(double),1,outfile) ;
    for(ii=0;ii<N;ii++) chk += fwrite(&Var[4*N+ii],sizeof(double),1,outfile) ;

    for(ii=0;ii<N;ii++) chk += fwrite(&EqnConst[ii],sizeof(double),1,outfile) ;
    for(ii=0;ii<N;ii++) chk += fwrite(&EqnConst[N+ii],sizeof(double),1,outfile) ;
    for(ii=0;ii<N;ii++) chk += fwrite(&EqnConst[2*N+ii],sizeof(double),1,outfile) ;
    for(ii=0;ii<N;ii++) chk += fwrite(&EqnConst[3*N+ii],sizeof(double),1,outfile) ;
    for(ii=0;ii<N;ii++) chk += fwrite(&EqnConst[4*N+ii],sizeof(double),1,outfile) ;
    for(ii=0;ii<N;ii++) chk += fwrite(&EqnConst[5*N+ii],sizeof(double),1,outfile) ;
    for(ii=0;ii<N;ii++) chk += fwrite(&EqnConst[6*N+ii],sizeof(double),1,outfile) ;
    for(ii=0;ii<N;ii++) chk += fwrite(&EqnConst[7*N+ii],sizeof(double),1,outfile) ;
    
    
    fflush(outfile) ;
    
    if(chk!=1+13*N) {
        printf("\n\nProblem writing data to outfile\n\n") ; 
        exit(1) ;
    }
    
}




void calcm(double v, double *m, double *p, double *pv, double *pvv) 
{
    double w, v0 ;
    
    w  = 100. ;
    
    v0 = 1. ;
    
    *m = aval ;
       
    
    *p = amp2*exp(-(w*power(v - v0,2))) ;
    *pv = -2*amp2*(v - v0)*w*exp(-(w*power(v - v0,2))) ;
    *pvv = 2*amp2*w*exp(-(w*power(v - v0,2)))*(-1 - 4*v*v0*w + 2*w*power(v,2) + 2*w*power(v0,2)) ;
        
}



void calcstep(double time)
{
    int ii, iter ;
    double err ;
    
    double step[5*Nmax] ;
    
    
    for(ii=0;ii<5*N;ii++) {
    
        NewVar[ii] = Var[ii] ;
    
    }
    
    for(iter=0;iter<20;iter++) {
        
        calcEqn(time,Eqn) ;
        
        calcLich(time) ;
        
        linsolve(step,Eqn) ;
        
        for(ii=0;ii<5*N;ii++) NewVar[ii] -= step[ii] ;
        
        for(err=0.,ii=0;ii<5*N;ii++) { if(fabs(Eqn[ii])>err) err = fabs(Eqn[ii]) ; }
        
        if(err<1e-5) break ;
        
    } 
    
    if(iter>15) { printf("   *** WARNING: Precision issue;   iter = %d,  err = %7.3e\n",iter,err) ; }
    

}


void calcLich(double time)
{
    int ii, jj, kk ;
    double EqnM[5*Nmax], EqnP[5*Nmax] ;
    
    double store[5*Nmax] ;
    
    double delta = 1.e-6 ;
    
    
    
    for(ii=0;ii<5*N;ii++) store[ii] = NewVar[ii] ;
    
    calcEqn(time,Eqn) ;
    
    
    for(ii=0;ii<5*N;ii++) {
        
        NewVar[ii] = store[ii] + delta ;
        
        calcEqn(time,EqnP) ;
        
        NewVar[ii] = store[ii] - delta ;
        
        calcEqn(time,EqnM) ;
        
        NewVar[ii] = store[ii] ;

        for(jj=0;jj<5*N;jj++) {
            
            Lich[jj][ii] = ( EqnP[jj] - EqnM[jj] )/( 2*delta ) ;
            
        }
        
    }
    
    
}




void calcEqn(double time, double *eqnVec)
{
    int ii, jj, pos ;
    
    double v, z, m, p, pv, pvv ;
    
    double f0[Nmax], f0z[Nmax], f0zz[Nmax], h0[Nmax], h0z[Nmax], h0zz[Nmax], a0[Nmax], a0z[Nmax], a0zz[Nmax], t0[Nmax], t0z[Nmax], t0zz[Nmax], s0[Nmax], s0z[Nmax], s0zz[Nmax] ;
    
    double f1[Nmax], f1z[Nmax], f1zz[Nmax], h1[Nmax], h1z[Nmax], h1zz[Nmax], a1[Nmax], a1z[Nmax], a1zz[Nmax], t1[Nmax], t1z[Nmax], t1zz[Nmax], s1[Nmax], s1z[Nmax], s1zz[Nmax] ;
    
    double f, h, a, t, s, fz, hz, az, tz, sz, fzz, hzz, azz, tzz, szz, fv, hv, av, tv, sv, fvz, hvz, avz, tvz, svz ;    
    
    
    v = time + 0.5*dv ;
    
    calcm(v, &m, &p, &pv, &pvv) ; 
    
    
    for(ii=0;ii<N;ii++) {
        
        f0[ii] = Var[ii] ; h0[ii] = Var[N+ii] ; a0[ii] = Var[2*N+ii] ; t0[ii] = Var[3*N+ii] ; s0[ii] = Var[4*N+ii] ; 
        
        f1[ii] = NewVar[ii] ; h1[ii] = NewVar[N+ii] ; a1[ii] = NewVar[2*N+ii] ; t1[ii] = NewVar[3*N+ii] ; s1[ii] = NewVar[4*N+ii] ; 
        
    }
    
    
    for(ii=0;ii<N;ii++) {
        
        f0z[ii] = 0. ; h0z[ii] = 0. ; a0z[ii] = 0. ; t0z[ii] = 0. ; s0z[ii] = 0. ;
        f1z[ii] = 0. ; h1z[ii] = 0. ; a1z[ii] = 0. ; t1z[ii] = 0. ; s1z[ii] = 0. ;
        
        f0zz[ii] = 0. ; h0zz[ii] = 0. ; a0zz[ii] = 0. ; t0zz[ii] = 0. ; s0zz[ii] = 0. ;
        f1zz[ii] = 0. ; h1zz[ii] = 0. ; a1zz[ii] = 0. ; t1zz[ii] = 0. ; s1zz[ii] = 0. ;
        
        
        for(jj=0;jj<N;jj++) {
            
            f0z[ii] += Dmat[ii][jj]*f0[jj] ;
            h0z[ii] += Dmat[ii][jj]*h0[jj] ;
            a0z[ii] += Dmat[ii][jj]*a0[jj] ;
            t0z[ii] += Dmat[ii][jj]*t0[jj] ;
            s0z[ii] += Dmat[ii][jj]*s0[jj] ;
            
            f1z[ii] += Dmat[ii][jj]*f1[jj] ;
            h1z[ii] += Dmat[ii][jj]*h1[jj] ;
            a1z[ii] += Dmat[ii][jj]*a1[jj] ;
            t1z[ii] += Dmat[ii][jj]*t1[jj] ;
            s1z[ii] += Dmat[ii][jj]*s1[jj] ;
            
            f0zz[ii] += D2mat[ii][jj]*f0[jj] ;
            h0zz[ii] += D2mat[ii][jj]*h0[jj] ;
            a0zz[ii] += D2mat[ii][jj]*a0[jj] ;
            t0zz[ii] += D2mat[ii][jj]*t0[jj] ;
            s0zz[ii] += D2mat[ii][jj]*s0[jj] ;
            
            f1zz[ii] += D2mat[ii][jj]*f1[jj] ;
            h1zz[ii] += D2mat[ii][jj]*h1[jj] ;
            a1zz[ii] += D2mat[ii][jj]*a1[jj] ;
            t1zz[ii] += D2mat[ii][jj]*t1[jj] ;
            s1zz[ii] += D2mat[ii][jj]*s1[jj] ;
            
        }
        
    }
    
    
    
    for(pos=0,ii=0;ii<N;ii++) {
        
        z = x[ii] ;
        
        f = 0.5*( f1[ii] + f0[ii] ) ;
        h = 0.5*( h1[ii] + h0[ii] ) ;
        a = 0.5*( a1[ii] + a0[ii] ) ;
        t = 0.5*( t1[ii] + t0[ii] ) ;
        s = 0.5*( s1[ii] + s0[ii] ) ;
        
        fz = 0.5*( f1z[ii] + f0z[ii] ) ;
        hz = 0.5*( h1z[ii] + h0z[ii] ) ;
        az = 0.5*( a1z[ii] + a0z[ii] ) ;
        tz = 0.5*( t1z[ii] + t0z[ii] ) ;
        sz = 0.5*( s1z[ii] + s0z[ii] ) ;
        
        fzz = 0.5*( f1zz[ii] + f0zz[ii] ) ;
        hzz = 0.5*( h1zz[ii] + h0zz[ii] ) ;
        azz = 0.5*( a1zz[ii] + a0zz[ii] ) ;
        tzz = 0.5*( t1zz[ii] + t0zz[ii] ) ;
        szz = 0.5*( s1zz[ii] + s0zz[ii] ) ;
        
        fv = ( f1[ii] - f0[ii] )/dv ;
        hv = ( h1[ii] - h0[ii] )/dv ;
        av = ( a1[ii] - a0[ii] )/dv ;
        tv = ( t1[ii] - t0[ii] )/dv ;
        sv = ( s1[ii] - s0[ii] )/dv ;
        
        fvz = ( f1z[ii] - f0z[ii] )/dv ;
        hvz = ( h1z[ii] - h0z[ii] )/dv ;
        avz = ( a1z[ii] - a0z[ii] )/dv ;
        tvz = ( t1z[ii] - t0z[ii] )/dv ;
        svz = ( s1z[ii] - s0z[ii] )/dv ;
        
        if(ii==0) {
            
            eqnVec[pos++] = f - p ;
            
            eqnVec[pos++] = h ;
            
            eqnVec[pos++] = av + 4*p*(hz + 2*m*p) ;
            
            eqnVec[pos++] = s + 0.25*p*p ;
            
            eqnVec[pos++] = t + 0.5*p*p ;
            
        } else {
            
            eqnVec[pos++] = fvz - (fzz - 4*hz*m + 4*f*s - 2*h*z*(2*m*(2*s + sz*z) + az*(1 + s*pow(z,2))) - 2*a*(2*hz*z*(1 + s*pow(z,2)) + h*(1 + 5*s*pow(z,2) + 2*sz*pow(z,3))) + z*(2*fz*t - 2*fz*sv*z - 2*fv*sz*z + 2*fz*sz*z + fzz*t*z + fz*tz*z + f*(-2*sv + tz + s*z*(4*t + tz*z) + 2*sz*(1 + t*pow(z,2))) + 2*fz*sz*t*pow(z,3) + s*(-4*fv - 4*hz*m*z + fzz*z*(1 + t*pow(z,2)) + fz*(4 + 6*t*pow(z,2) + tz*pow(z,3)))))*pow(2 + 2*s*pow(z,2),-1) ; 
            
            eqnVec[pos++] = hvz - (hzz + 4*fz*m + 2*az*f*z - 4*hv*s*z + 4*hz*s*z + 8*f*m*s*z + 2*hz*t*z + h*tz*z - 2*sv*z*(h + hz*z) + hzz*s*pow(z,2) + 4*fz*m*s*pow(z,2) - 2*hv*sz*pow(z,2) + 2*hz*sz*pow(z,2) + 4*f*m*sz*pow(z,2) + hzz*t*pow(z,2) + hz*tz*pow(z,2) + 2*h*sz*z*(1 + t*pow(z,2)) + 2*az*f*s*pow(z,3) + 6*hz*s*t*pow(z,3) + h*s*(4 + 4*t*pow(z,2) + tz*pow(z,3)) + 2*a*(2*fz*z*(1 + s*pow(z,2)) + f*(1 + 5*s*pow(z,2) + 2*sz*pow(z,3))) + hzz*s*t*pow(z,4) + 2*hz*sz*t*pow(z,4) + hz*s*tz*pow(z,4))*pow(2 + 2*s*pow(z,2),-1) ; 
            
            eqnVec[pos++] = avz - z*(-2*sv*(a + az*z) - av*pow(z,-2)*(1 + s*pow(z,2)) - 4*pow(z,-2)*(1 + s*pow(z,2))*(2*m*pow(f,2) + 2*a*z*(pow(f,2) + pow(h,2)) + h*(fv + 2*h*m - fz*(1 + t*pow(z,2))) + f*(-hv + hz*(1 + t*pow(z,2)))))*pow(1 + s*pow(z,2),-1) ; 
            
            if(ii==1) {
                
                eqnVec[pos++] = tv + 2*m*p*(h + 2*m*z*p) + p*fv - z*p*pvv ;   
                
            } else {
                
                eqnVec[pos++] = tzz - (-2*(6*s - t + pow(h,2)) + pow(a,2)*pow(z,2)*pow(1 + s*pow(z,2),2) - 2*pow(f,2)*(1 + t*pow(z,2))*pow(1 + s*pow(z,2),2) - 2*f*z*(-fv + 2*hz*m*z + 2*fz*(1 + t*pow(z,2)))*pow(1 + s*pow(z,2),2) + 2*a*(az + 2*fz*h - 2*f*hz)*pow(z,3)*pow(1 + s*pow(z,2),2) - z*(2*(-2*sv + 6*sz + tz) + 2*z*pow(h,2)*(t + s*(2 + s*pow(z,2))*(1 + t*pow(z,2))) + z*(-(pow(az,2)*pow(z,2)) + 2*(-(fv*fz) - hv*hz + szz + pow(hz,2) - sz*(-2*sv + sz)*pow(z,2) + pow(fz,2)*(1 + t*pow(z,2)) + t*z*(6*sz + z*(szz + pow(hz,2) - pow(sz,2)*pow(z,2)))) + 2*s*(z*(2*(sv + sz + tz) + (2*fz*(-fv + fz) + 2*hz*(-hv + hz) + szz)*z - pow(az,2)*pow(z,3)) + t*(4 + (szz*z + 2*(sz + z*(pow(fz,2) + pow(hz,2))))*pow(z,3))) + pow(s,2)*(2*(2 + (tz + z*(-(fv*fz) - hv*hz + pow(fz,2) + pow(hz,2)))*pow(z,3)) - pow(az,2)*pow(z,6) + 2*t*(pow(z,2) + (pow(fz,2) + pow(hz,2))*pow(z,6)))) + 2*h*(-hv - 2*fz*m*z + 2*hz*(1 + t*pow(z,2)))*pow(1 + s*pow(z,2),2)))*pow(z + s*pow(z,3),-2) ; 
                
            }
                
            eqnVec[pos++] = svz + (pow(z,-4)*(1 + s*pow(z,2))*(12 + 4*(2*t + tz*z)*pow(z,2) + 4*pow(f,2)*pow(z,2) + 4*pow(h,2)*pow(z,2) - pow(z,4)*pow(a + az*z,2) + 2*(1 + t*pow(z,2))*(-6 + pow(z,2)*pow(f + fz*z,2) + pow(z,2)*pow(h + hz*z,2)) - 4*(2*s + sz*z)*(-2*sv*z + (2*s + sz*z)*(1 + t*pow(z,2)))*pow(z,4)*pow(1 + s*pow(z,2),-2) + 16*(2*s + sz*z)*pow(z,2)*(1 + t*pow(z,2))*pow(1 + s*pow(z,2),-1) - 4*(2*s + sz*z)*(2*t + tz*z)*pow(z,4)*pow(1 + s*pow(z,2),-1)))/8. ; 
                 
            
        }
        
    }
    
    
}




void calcEqnConst(double time)
{
    int ii, jj, pos ;
    
    double v, z, m, p, pv, pvv ;
    
    double f0[Nmax], f0z[Nmax], f0zz[Nmax], h0[Nmax], h0z[Nmax], h0zz[Nmax], a0[Nmax], a0z[Nmax], a0zz[Nmax], t0[Nmax], t0z[Nmax], t0zz[Nmax], s0[Nmax], s0z[Nmax], s0zz[Nmax] ;

    double f1[Nmax], f1z[Nmax], f1zz[Nmax], h1[Nmax], h1z[Nmax], h1zz[Nmax], a1[Nmax], a1z[Nmax], a1zz[Nmax], t1[Nmax], t1z[Nmax], t1zz[Nmax], s1[Nmax], s1z[Nmax], s1zz[Nmax] ;

    double f2[Nmax], f2z[Nmax], f2zz[Nmax], h2[Nmax], h2z[Nmax], h2zz[Nmax], a2[Nmax], a2z[Nmax], a2zz[Nmax], t2[Nmax], t2z[Nmax], t2zz[Nmax], s2[Nmax], s2z[Nmax], s2zz[Nmax] ;
    
    double f, h, a, t, s, fz, hz, az, tz, sz, fzz, hzz, azz, tzz, szz, fv, hv, av, tv, sv, fvz, hvz, avz, tvz, svz, fvv, hvv, avv, tvv, svv ;    
    
    
    v = time ;
    
    calcm(v, &m, &p, &pv, &pvv) ; 
    
    
    for(ii=0;ii<N;ii++) {
        
        f0[ii] = OldVar[ii] ; h0[ii] = OldVar[N+ii] ; a0[ii] = OldVar[2*N+ii] ; t0[ii] = OldVar[3*N+ii] ; s0[ii] = OldVar[4*N+ii] ; 
        
        f1[ii] = Var[ii] ; h1[ii] = Var[N+ii] ; a1[ii] = Var[2*N+ii] ; t1[ii] = Var[3*N+ii] ; s1[ii] = Var[4*N+ii] ; 
        
        f2[ii] = NewVar[ii] ; h2[ii] = NewVar[N+ii] ; a2[ii] = NewVar[2*N+ii] ; t2[ii] = NewVar[3*N+ii] ; s2[ii] = NewVar[4*N+ii] ; 
        
    }
    
    
    for(ii=0;ii<N;ii++) {
        
        f0z[ii] = 0. ; h0z[ii] = 0. ; a0z[ii] = 0. ; t0z[ii] = 0. ; s0z[ii] = 0. ;
        f1z[ii] = 0. ; h1z[ii] = 0. ; a1z[ii] = 0. ; t1z[ii] = 0. ; s1z[ii] = 0. ;
        f2z[ii] = 0. ; h2z[ii] = 0. ; a2z[ii] = 0. ; t2z[ii] = 0. ; s2z[ii] = 0. ;
        
        f0zz[ii] = 0. ; h0zz[ii] = 0. ; a0zz[ii] = 0. ; t0zz[ii] = 0. ; s0zz[ii] = 0. ;
        f1zz[ii] = 0. ; h1zz[ii] = 0. ; a1zz[ii] = 0. ; t1zz[ii] = 0. ; s1zz[ii] = 0. ;
        f2zz[ii] = 0. ; h2zz[ii] = 0. ; a2zz[ii] = 0. ; t2zz[ii] = 0. ; s2zz[ii] = 0. ;

        
        for(jj=0;jj<N;jj++) {
            
            f0z[ii] += Dmat[ii][jj]*f0[jj] ;
            h0z[ii] += Dmat[ii][jj]*h0[jj] ;
            a0z[ii] += Dmat[ii][jj]*a0[jj] ;
            t0z[ii] += Dmat[ii][jj]*t0[jj] ;
            s0z[ii] += Dmat[ii][jj]*s0[jj] ;
            
            f1z[ii] += Dmat[ii][jj]*f1[jj] ;
            h1z[ii] += Dmat[ii][jj]*h1[jj] ;
            a1z[ii] += Dmat[ii][jj]*a1[jj] ;
            t1z[ii] += Dmat[ii][jj]*t1[jj] ;
            s1z[ii] += Dmat[ii][jj]*s1[jj] ;
            
            f2z[ii] += Dmat[ii][jj]*f2[jj] ;
            h2z[ii] += Dmat[ii][jj]*h2[jj] ;
            a2z[ii] += Dmat[ii][jj]*a2[jj] ;
            t2z[ii] += Dmat[ii][jj]*t2[jj] ;
            s2z[ii] += Dmat[ii][jj]*s2[jj] ;
            
            f0zz[ii] += D2mat[ii][jj]*f0[jj] ;
            h0zz[ii] += D2mat[ii][jj]*h0[jj] ;
            a0zz[ii] += D2mat[ii][jj]*a0[jj] ;
            t0zz[ii] += D2mat[ii][jj]*t0[jj] ;
            s0zz[ii] += D2mat[ii][jj]*s0[jj] ;
            
            f1zz[ii] += D2mat[ii][jj]*f1[jj] ;
            h1zz[ii] += D2mat[ii][jj]*h1[jj] ;
            a1zz[ii] += D2mat[ii][jj]*a1[jj] ;
            t1zz[ii] += D2mat[ii][jj]*t1[jj] ;
            s1zz[ii] += D2mat[ii][jj]*s1[jj] ;
            
            f2zz[ii] += D2mat[ii][jj]*f2[jj] ;
            h2zz[ii] += D2mat[ii][jj]*h2[jj] ;
            a2zz[ii] += D2mat[ii][jj]*a2[jj] ;
            t2zz[ii] += D2mat[ii][jj]*t2[jj] ;
            s2zz[ii] += D2mat[ii][jj]*s2[jj] ;

        }

    }

    
    for(pos=0,ii=0;ii<N;ii++) {
    
        z = x[ii] ;
        
        f = f1[ii] ;
        h = h1[ii] ;
        a = a1[ii] ;
        t = t1[ii] ;
        s = s1[ii] ;
        
        fz = f1z[ii] ;
        hz = h1z[ii] ;
        az = a1z[ii] ;
        tz = t1z[ii] ;
        sz = s1z[ii] ;

        fzz = f1zz[ii] ;
        hzz = h1zz[ii] ;
        azz = a1zz[ii] ;
        tzz = t1zz[ii] ;
        szz = s1zz[ii] ;

        fv = ( f2[ii] - f0[ii] )/(2*dv) ;
        hv = ( h2[ii] - h0[ii] )/(2*dv) ;
        av = ( a2[ii] - a0[ii] )/(2*dv) ;
        tv = ( t2[ii] - t0[ii] )/(2*dv) ;
        sv = ( s2[ii] - s0[ii] )/(2*dv) ;
        
        fvz = ( f2z[ii] - f0z[ii] )/(2*dv) ;
        hvz = ( h2z[ii] - h0z[ii] )/(2*dv) ;
        avz = ( a2z[ii] - a0z[ii] )/(2*dv) ;
        tvz = ( t2z[ii] - t0z[ii] )/(2*dv) ;
        svz = ( s2z[ii] - s0z[ii] )/(2*dv) ;
        
        fvv = ( f2[ii] + f0[ii] - 2*f1[ii] )/(dv*dv) ;
        hvv = ( h2[ii] + h0[ii] - 2*h1[ii] )/(dv*dv) ;
        avv = ( a2[ii] + a0[ii] - 2*a1[ii] )/(dv*dv) ;
        tvv = ( t2[ii] + t0[ii] - 2*t1[ii] )/(dv*dv) ;
        svv = ( s2[ii] + s0[ii] - 2*s1[ii] )/(dv*dv) ;

        
        if(ii==0) {
            
            EqnConst[pos++] = 0. ; 
            
            EqnConst[pos++] = 0. ; 
            
            EqnConst[pos++] = 0. ; 
            
            EqnConst[pos++] = 0. ; 
            
            EqnConst[pos++] = 0. ; 
            
            EqnConst[pos++] = 0. ; 
            
            EqnConst[pos++] = 0. ; 
            
            EqnConst[pos++] = 0. ; 

            
        } else {
            
            EqnConst[pos++] = fvz - (fzz - 4*hz*m + 4*f*s - 2*h*z*(2*m*(2*s + sz*z) + az*(1 + s*pow(z,2))) - 2*a*(2*hz*z*(1 + s*pow(z,2)) + h*(1 + 5*s*pow(z,2) + 2*sz*pow(z,3))) + z*(2*fz*t - 2*fz*sv*z - 2*fv*sz*z + 2*fz*sz*z + fzz*t*z + fz*tz*z + f*(-2*sv + tz + s*z*(4*t + tz*z) + 2*sz*(1 + t*pow(z,2))) + 2*fz*sz*t*pow(z,3) + s*(-4*fv - 4*hz*m*z + fzz*z*(1 + t*pow(z,2)) + fz*(4 + 6*t*pow(z,2) + tz*pow(z,3)))))*pow(2 + 2*s*pow(z,2),-1) ; 
            
            EqnConst[pos++] = hvz - (hzz + 4*fz*m + 2*az*f*z - 4*hv*s*z + 4*hz*s*z + 8*f*m*s*z + 2*hz*t*z + h*tz*z - 2*sv*z*(h + hz*z) + hzz*s*pow(z,2) + 4*fz*m*s*pow(z,2) - 2*hv*sz*pow(z,2) + 2*hz*sz*pow(z,2) + 4*f*m*sz*pow(z,2) + hzz*t*pow(z,2) + hz*tz*pow(z,2) + 2*h*sz*z*(1 + t*pow(z,2)) + 2*az*f*s*pow(z,3) + 6*hz*s*t*pow(z,3) + h*s*(4 + 4*t*pow(z,2) + tz*pow(z,3)) + 2*a*(2*fz*z*(1 + s*pow(z,2)) + f*(1 + 5*s*pow(z,2) + 2*sz*pow(z,3))) + hzz*s*t*pow(z,4) + 2*hz*sz*t*pow(z,4) + hz*s*tz*pow(z,4))*pow(2 + 2*s*pow(z,2),-1) ; 
            
            EqnConst[pos++] = avz - z*(-2*sv*(a + az*z) - av*pow(z,-2)*(1 + s*pow(z,2)) - 4*pow(z,-2)*(1 + s*pow(z,2))*(2*m*pow(f,2) + 2*a*z*(pow(f,2) + pow(h,2)) + h*(fv + 2*h*m - fz*(1 + t*pow(z,2))) + f*(-hv + hz*(1 + t*pow(z,2)))))*pow(1 + s*pow(z,2),-1) ; 
            
            
            EqnConst[pos++] = tzz - (-2*(6*s - t + pow(h,2)) + pow(a,2)*pow(z,2)*pow(1 + s*pow(z,2),2) - 2*pow(f,2)*(1 + t*pow(z,2))*pow(1 + s*pow(z,2),2) - 2*f*z*(-fv + 2*hz*m*z + 2*fz*(1 + t*pow(z,2)))*pow(1 + s*pow(z,2),2) + 2*a*(az + 2*fz*h - 2*f*hz)*pow(z,3)*pow(1 + s*pow(z,2),2) - z*(2*(-2*sv + 6*sz + tz) + 2*z*pow(h,2)*(t + s*(2 + s*pow(z,2))*(1 + t*pow(z,2))) + z*(-(pow(az,2)*pow(z,2)) + 2*(-(fv*fz) - hv*hz + szz + pow(hz,2) - sz*(-2*sv + sz)*pow(z,2) + pow(fz,2)*(1 + t*pow(z,2)) + t*z*(6*sz + z*(szz + pow(hz,2) - pow(sz,2)*pow(z,2)))) + 2*s*(z*(2*(sv + sz + tz) + (2*fz*(-fv + fz) + 2*hz*(-hv + hz) + szz)*z - pow(az,2)*pow(z,3)) + t*(4 + (szz*z + 2*(sz + z*(pow(fz,2) + pow(hz,2))))*pow(z,3))) + pow(s,2)*(2*(2 + (tz + z*(-(fv*fz) - hv*hz + pow(fz,2) + pow(hz,2)))*pow(z,3)) - pow(az,2)*pow(z,6) + 2*t*(pow(z,2) + (pow(fz,2) + pow(hz,2))*pow(z,6)))) + 2*h*(-hv - 2*fz*m*z + 2*hz*(1 + t*pow(z,2)))*pow(1 + s*pow(z,2),2)))*pow(z + s*pow(z,3),-2) ; 
                
            EqnConst[pos++] = svz + (pow(z,-4)*(1 + s*pow(z,2))*(12 + 4*(2*t + tz*z)*pow(z,2) + 4*pow(f,2)*pow(z,2) + 4*pow(h,2)*pow(z,2) - pow(z,4)*pow(a + az*z,2) + 2*(1 + t*pow(z,2))*(-6 + pow(z,2)*pow(f + fz*z,2) + pow(z,2)*pow(h + hz*z,2)) - 4*(2*s + sz*z)*(-2*sv*z + (2*s + sz*z)*(1 + t*pow(z,2)))*pow(z,4)*pow(1 + s*pow(z,2),-2) + 16*(2*s + sz*z)*pow(z,2)*(1 + t*pow(z,2))*pow(1 + s*pow(z,2),-1) - 4*(2*s + sz*z)*(2*t + tz*z)*pow(z,4)*pow(1 + s*pow(z,2),-1)))/8. ; 
            
            EqnConst[pos++] = szz + (power(z,-2)*(4*s + 8*sz*z + 2*f*fz*z*(1 + s*power(z,2)) + power(f,2)*(1 + s*power(z,2)) + (1 + s*power(z,2))*(power(fz,2)*power(z,2) + power(h + hz*z,2))))/2. ; 
            
            EqnConst[pos++] = azz + 2*(a*z*(2*s + sz*z) - 2*(fz*h - f*hz)*(1 + s*power(z,2)) + az*(1 + 3*s*power(z,2) + sz*power(z,3)))*power(z + s*power(z,3),-1) ; 
            
            EqnConst[pos++] = svv + ((-4*(-8*s + t) + 6*power(h,2) + power(a,2)*power(z,2)*(-1 + (-t + 16*(power(f,2) + power(h,2)))*power(z,2))*power(1 + s*power(z,2),2) - 4*f*z*(4*hv*m*z - (1 + t*power(z,2))*(-fv + 2*hz*m*z + fz*(1 + t*power(z,2))))*power(1 + s*power(z,2),2) + 2*power(f,2)*(3 + power(z,2)*(8*power(m,2) + t*(4 + t*power(z,2))))*power(1 + s*power(z,2),2) + 2*a*(16*m*power(f,2) - az*(1 + t*power(z,2)) + 4*h*(2*fv + 4*h*m - fz*(1 + t*power(z,2))) + 4*f*(-2*hv + hz*(1 + t*power(z,2))))*power(z,3)*power(1 + s*power(z,2),2) + z*(4*(-4*sv + 4*sz + tv + tz) + 4*h*(2*m*z*(2*fv - fz*(1 + t*power(z,2))) + (1 + t*power(z,2))*(-hv + hz*(1 + t*power(z,2))))*power(1 + s*power(z,2),2) + 2*z*power(h,2)*(3*s*(2 + s*power(z,2)) + 4*t*power(1 + s*power(z,2),2) + 8*power(m,2)*power(1 + s*power(z,2),2) + power(t,2)*power(z,2)*power(1 + s*power(z,2),2)) + z*(2*(-2*fv*fz - 2*hv*hz + power(fz,2) + 2*(power(fv,2) + power(hv,2)) + power(hz,2)) - (power(az,2) + 4*(-(sv*tz) + sz*(-2*sv + tv + tz) + power(sz,2)))*power(z,2) - t*z*(power(az,2)*power(z,3) + 4*(2*sv - tz + fv*fz*z + hv*hz*z - z*power(fz,2) - z*power(hz,2) + sz*(-6 + (-2*sv + 2*sz + tz)*power(z,3)))) + power(s,2)*(16 - power(z,3)*(4*(tv + tz) - 2*z*(-2*fv*fz - 2*hv*hz + power(fz,2) + 2*(power(fv,2) + power(hv,2)) + power(hz,2)) + power(az,2)*power(z,3)) + 2*power(t,2)*power(z,4)*(-2 + (power(fz,2) + power(hz,2))*power(z,4)) - t*power(z,2)*(4*(-3 + (tz + fv*fz*z + (hv - hz)*hz*z - z*power(fz,2))*power(z,3)) + power(az,2)*power(z,6))) + s*(-2*power(z,2)*(-2*(-2*fv*fz - 2*hv*hz + power(fz,2) + 2*(power(fv,2) + power(hv,2)) + power(hz,2)) + (-2*sv*tz + 2*sz*(tv + tz) + power(az,2))*power(z,2)) + 4*power(t,2)*power(z,2)*(2 + (-2*sz + z*(power(fz,2) + power(hz,2)))*power(z,3)) - 2*t*(-20 + 2*power(z,3)*(-2*sv + 2*fv*fz*z + 2*hv*hz*z - 2*z*power(fz,2) - 2*z*power(hz,2) + sz*(2 + tz*power(z,3))) + power(az,2)*power(z,6))) + 2*power(t,2)*(power(fz,2)*power(z,4) + power(hz,2)*power(z,4) - 2*power(-1 + sz*power(z,3),2)))))*power(power(z,2) + s*power(z,4),-1))/8. ;
            
        }
        
    }
    
    
}









void setupCheb(void)
{
    int ii, jj, kk ;
    
    
    /* Std - for r  */
    
    for(ii=0;ii<N;ii++) {
        
        x[ii] = (1 + cos(PI*(((double) (N-ii))-1)/(((double) N)-1)) )/2 ;
        
    }
    
    for(ii=1;ii<N-1;ii++) {
		for(jj=1;jj<N-1;jj++) {
            
            if(ii==jj) 
                Dmat[ii][jj] = - (x[jj] - 0.5)/(2*(1-x[jj])*x[jj]) ;
            else
                Dmat[ii][jj] = ((double) pow(-1,ii+jj))/( x[ii] - x[jj] ) ;
            
        }
    }
    
    Dmat[0][0] = - ( 2*((double) pow(N-1,2)) + 1 )/3 ;
    
    for(jj=1;jj<N-1;jj++) {
        
        Dmat[0][jj] = -2*((double) pow(-1.,jj))/x[jj] ;
        
    }
    
    Dmat[0][N-1] = - pow(-1,N-1) ;
    
    Dmat[N-1][N-1] = + ( 2*((double) pow(N-1,2)) + 1 )/3 ;
    
    for(jj=1;jj<N-1;jj++) {
        
        Dmat[N-1][jj] = 2*((double) pow(-1,N+jj-1))/(1 - x[jj]) ;
        
    }
    
    Dmat[N-1][0] = + pow(-1,N-1) ;
    
    for(ii=1;ii<N-1;ii++) {
        
        Dmat[ii][0] = +((double) pow(-1,ii))/x[ii]/2 ;
        
    }
    
    for(ii=1;ii<N-1;ii++) {
        
        Dmat[ii][N-1] = - ((double) pow(-1,N+ii-1))/(1 - x[ii])/2 ;
        
    }
    
    for(ii=0;ii<N;ii++) {
        for(jj=0;jj<N;jj++) {
            
            D2mat[ii][jj] = 0. ;
            
            for(kk=0;kk<N;kk++) {
                D2mat[ii][jj] += Dmat[ii][kk]*Dmat[kk][jj] ;
            }
            
        }
    }
    
    
}





void unpackLich(void)
{
    int ii, jj ;
    
	for(ii=0;ii<5*N;ii++) {
        for(jj=0;jj<5*N;jj++) {
            
            Lmat[ jj*5*N + ii ] = Lich[ii][jj] ;
            
        }
    }
    
}



/* LAPACK solver */

void linsolve(double *x, double *b)
{
    int ii, Nvec, nrhs, ldLmat, ldx, info ;
    int ipiv[5*Nmax];
    
    
    unpackLich() ;
    
    Nvec    = 5*N ;
    nrhs = 1 ;
    ldLmat  = Nvec ;
    ldx  = Nvec ;
    
    for(ii=0;ii<Nvec;ii++) x[ii] = b[ii] ;
    
    dgesv_(&Nvec, &nrhs, Lmat, &ldLmat, ipiv, x, &ldx, &info);
    
    if(info != 0) {
        
        printf("\n\nFailure to solve linear system - %d\n\n", info);
        
        exit(1) ;
        
    }
    
    
}






