/****************************************************************************
                      main.c  -  description
                         -------------------
begin                : Tue Oct 21 15:04:46 CEST 2003
copyright            : (C) 2003 by fab

****************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/

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

// Hubble constant in m.s^-1.Mpc^-1
#define H0 70000
// Speed of light in m.s^-1
#define cv 3*powl(10,8)

// Absolute value for long double
long double ldabs (long double x) {
   return ((long double) x > 0 ? (long double) x : (long double) -x);
}

// Time (ct) coordinate of geodesic
inline long double F(long double a,long double ap,long double b,long double bp,long double c,long double cp,long double d,long double dp)
{
   long double r;
   r=-((2./3)*powl(3*H0/(2*cv),4./3)*powl(a,1./3))*(powl(bp,2)+powl(b,2)*powl(cp,2)+powl(b,2)*powl(sinl(c),2)*powl(dp,2));
   return r;
}

// Radial coordinate of geodesic : actually, radial coordinate solved is r' = R0*r
inline long double G(long double a,long double ap,long double b,long double bp,long double c,long double cp,long double d,long double dp)
{
   long double r;
   r=-((4./(3*(a)))*ap*bp)+ldabs(b)*(powl(cp,2)+powl(sinl(c),2)*powl(dp,2));
   return r;
}

// Theta coordinate of geodesic
inline long double H(long double a,long double ap,long double b,long double bp,long double c,long double cp,long double d,long double dp)
{
   long double r;
   r=-(2./ldabs(b))*bp*cp-(4./(3*a))*ap*cp+(sinl(2*c)*powl(dp,2))/2;
   return r;
}

// Phi coordinate of geodesic
inline long double I(long double a,long double ap,long double b,long double bp,long double c,long double cp,long double d,long double dp)
{
   long double r;
   r=-2*dp*(2./(3*a)*ap+(1./tanl(c))*cp+(1./ldabs(b))*bp);
   return r;
}

int main(int argc, char *argv[]) {

   // Output files
   FILE *file1;
   FILE *file2;

   // Today time (ct)
   long double ctk=(2*cv)/(3*H0);
   // Initial distance of light ray
   long double distanceInit;
   // Step for Runge-Kutta
   register long double h=0.0001;

   // All variables for numerical integration
   // w = ct, x = r' = R0*r, y = theta, z = phi
   // wp = d(ct)/ds, xp = dr/ds, yp = d(theta)/ds, zp = d(phi)/ds
   register long double w;
   register long double x;
   register long double y;
   register long double z;
   register long double wp;
   register long double xp;
   register long double yp;
   register long double zp;

   // All variables for integration with adaptative step
   register long double w2;
   register long double x2;
   register long double y2;
   register long double z2;
   register long double wp2;
   register long double xp2;
   register long double yp2;
   register long double zp2;

   // Initial values for all variables
   register long double w1;
   register long double x1;
   register long double y1;
   register long double z1;
   register long double wp1;
   register long double xp1;
   register long double yp1;
   register long double zp1;

   // Error for adaptative step method
   register long double Er=powl(10,-15);

   // Runge-Kutta coefficients
   long double k[4][8];

   // Indices
   int i,j,l;

   // Radial coordinate of light ray
   register long double distanceLight;
   // Comoving distance of galaxy source
   register long double distanceGalaxy;

   // coefficients array k[4][8] initialization
   for (i=0;i<=3;i++)
      for (j=0;j<=7;j++)
         k[i][j]=0.0;

   // Bad execution command
   if (argc!=2)
     printf("Argument Error : Parameter is Distance of Galaxy source in Mpc\n");
   else {
     distanceInit= atof(argv[1]);
     printf("Initial Distance of Galaxy source = %f Mpc\n",(float)distanceInit);
   }
   
   // initial conditions 
   w=ctk;
   x=distanceInit;
   y=M_PI_2;
   z=M_PI_2;
   
   // Initial derivatives
   xp=-1;
   yp=0.0;
   zp=0.0;
   
   // Initial relation between I.C - still considering r' = R0*r for "x" variable
   wp=powl((3*H0/(2*cv)*w),2./3)*sqrtl(powl(xp,2)+powl(x,2)*(powl(yp,2)+powl(sinl(y),2)*powl(zp,2))); 
   
   // Open output files
   file1=fopen("verif.dat","w");
   file2=fopen("graph.dat","w");
   
   // Main loop
   do {

     k[0][0]=h*wp;
     k[0][1]=h*F(w,wp,x,xp,y,yp,z,zp);
     k[0][2]=h*xp;
     k[0][3]=h*G(w,wp,x,xp,y,yp,z,zp);
     k[0][4]=h*yp;
     k[0][5]=h*H(w,wp,x,xp,y,yp,z,zp);
     k[0][6]=h*zp;
     k[0][7]=h*I(w,wp,x,xp,y,yp,z,zp);

     k[1][0]=h*(wp+k[0][1]/2);
     k[1][1]=h*F(w+k[0][0]/2,wp+k[0][1]/2,x+k[0][2]/2,xp+k[0][3]/2,y+k[0][4]/2,yp+k[0][5]/2,z+k[0][6]/2,zp+k[0][7]/2);
     k[1][2]=h*(xp+k[0][3]/2);
     k[1][3]=h*G(w+k[0][0]/2,wp+k[0][1]/2,x+k[0][2]/2,xp+k[0][3]/2,y+k[0][4]/2,yp+k[0][5]/2,z+k[0][6]/2,zp+k[0][7]/2);
     k[1][4]=h*(yp+k[0][5]/2);
     k[1][5]=h*H(w+k[0][0]/2,wp+k[0][1]/2,x+k[0][2]/2,xp+k[0][3]/2,y+k[0][4]/2,yp+k[0][5]/2,z+k[0][6]/2,zp+k[0][7]/2);
     k[1][6]=h*(zp+k[0][7]/2);
     k[1][7]=h*I(w+k[0][0]/2,wp+k[0][1]/2,x+k[0][2]/2,xp+k[0][3]/2,y+k[0][4]/2,yp+k[0][5]/2,z+k[0][6]/2,zp+k[0][7]/2);

     k[2][0]=h*(wp+k[1][1]/2);
     k[2][1]=h*F(w+k[1][0]/2,wp+k[1][1]/2,x+k[1][2]/2,xp+k[1][3]/2,y+k[1][4]/2,yp+k[1][5]/2,z+k[1][6]/2,zp+k[1][7]/2);
     k[2][2]=h*(xp+k[1][3]/2);
     k[2][3]=h*G(w+k[1][0]/2,wp+k[1][1]/2,x+k[1][2]/2,xp+k[1][3]/2,y+k[1][4]/2,yp+k[1][5]/2,z+k[1][6]/2,zp+k[1][7]/2);
     k[2][4]=h*(yp+k[1][5]/2);
     k[2][5]=h*H(w+k[1][0]/2,wp+k[1][1]/2,x+k[1][2]/2,xp+k[1][3]/2,y+k[1][4]/2,yp+k[1][5]/2,z+k[1][6]/2,zp+k[1][7]/2);
     k[2][6]=h*(zp+k[1][7]/2);
     k[2][7]=h*I(w+k[1][0]/2,wp+k[1][1]/2,x+k[1][2]/2,xp+k[1][3]/2,y+k[1][4]/2,yp+k[1][5]/2,z+k[1][6]/2,zp+k[1][7]/2);

     k[3][0]=h*(wp+k[2][1]);
     k[3][1]=h*F(w+k[2][0],wp+k[2][1],x+k[2][2],xp+k[2][3],y+k[2][4],yp+k[2][5],z+k[2][6],zp+k[2][7]);
     k[3][2]=h*(xp+k[2][3]);
     k[3][3]=h*G(w+k[2][0],wp+k[2][1],x+k[2][2],xp+k[2][3],y+k[2][4],yp+k[2][5],z+k[2][6],zp+k[2][7]);
     k[3][4]=h*(yp+k[2][5]);
     k[3][5]=h*H(w+k[2][0],wp+k[2][1],x+k[2][2],xp+k[2][3],y+k[2][4],yp+k[2][5],z+k[2][6],zp+k[2][7]);
     k[3][6]=h*(zp+k[2][7]);
     k[3][7]=h*I(w+k[2][0],wp+k[2][1],x+k[2][2],xp+k[2][3],y+k[2][4],yp+k[2][5],z+k[2][6],zp+k[2][7]);

     // Save values before updating
     w1=w;
     wp1=wp;
     x1=x;
     xp1=xp;
     y1=y;
     yp1=yp;
     z1=z;
     zp1=zp;

     // Save values before updating
     w2=w;
     wp2=wp;
     x2=x;
     xp2=xp;
     y2=y;
     yp2=yp;
     z2=z;
     zp2=zp;

     // Runge-Kutta recurrence formula
     w=w+(k[0][0]+2*k[1][0]+2*k[2][0]+k[3][0])/6;
     wp=wp+(k[0][1]+2*k[1][1]+2*k[2][1]+k[3][1])/6;
     x=x+(k[0][2]+2*k[1][2]+2*k[2][2]+k[3][2])/6;
     xp=xp+(k[0][3]+2*k[1][3]+2*k[2][3]+k[3][3])/6;
     y=y+(k[0][4]+2*k[1][4]+2*k[2][4]+k[3][4])/6;
     yp=yp+(k[0][5]+2*k[1][5]+2*k[2][5]+k[3][5])/6;
     z=z+(k[0][6]+2*k[1][6]+2*k[2][6]+k[3][6])/6;
     zp=zp+(k[0][7]+2*k[1][7]+2*k[2][7]+k[3][7])/6;

     // Two loops with step h/2
     for (l=0;l<2;l++) {

        k[0][0]=(h/2)*wp1;
        k[0][1]=(h/2)*F(w1,wp1,x1,xp1,y1,yp1,z1,zp1);
        k[0][2]=(h/2)*xp1;
        k[0][3]=(h/2)*G(w1,wp1,x1,xp1,y1,yp1,z1,zp1);
        k[0][4]=(h/2)*yp1;
        k[0][5]=(h/2)*H(w1,wp1,x1,xp1,y1,yp1,z1,zp1);
        k[0][6]=(h/2)*zp1;
        k[0][7]=(h/2)*I(w1,wp1,x1,xp1,y1,yp1,z1,zp1);
        
        k[1][0]=(h/2)*(wp1+k[0][1]/2);
        k[1][1]=(h/2)*F(w1+k[0][0]/2,wp1+k[0][1]/2,x1+k[0][2]/2,xp1+k[0][3]/2,y1+k[0][4]/2,yp1+k[0][5]/2,z1+k[0][6]/2,zp1+k[0][7]/2);
        k[1][2]=(h/2)*(xp1+k[0][3]/2);
        k[1][3]=(h/2)*G(w1+k[0][0]/2,wp1+k[0][1]/2,x1+k[0][2]/2,xp1+k[0][3]/2,y1+k[0][4]/2,yp1+k[0][5]/2,z1+k[0][6]/2,zp1+k[0][7]/2);
        k[1][4]=(h/2)*(yp1+k[0][5]/2);
        k[1][5]=(h/2)*H(w1+k[0][0]/2,wp1+k[0][1]/2,x1+k[0][2]/2,xp1+k[0][3]/2,y1+k[0][4]/2,yp1+k[0][5]/2,z1+k[0][6]/2,zp1+k[0][7]/2);
        k[1][6]=(h/2)*(zp1+k[0][7]/2);
        k[1][7]=(h/2)*I(w1+k[0][0]/2,wp1+k[0][1]/2,x1+k[0][2]/2,xp1+k[0][3]/2,y1+k[0][4]/2,yp1+k[0][5]/2,z1+k[0][6]/2,zp1+k[0][7]/2);
        
        k[2][0]=(h/2)*(wp1+k[1][1]/2);
        k[2][1]=(h/2)*F(w1+k[1][0]/2,wp1+k[1][1]/2,x1+k[1][2]/2,xp1+k[1][3]/2,y1+k[1][4]/2,yp1+k[1][5]/2,z1+k[1][6]/2,zp1+k[1][7]/2);
        k[2][2]=(h/2)*(xp1+k[1][3]/2);
        k[2][3]=(h/2)*G(w1+k[1][0]/2,wp1+k[1][1]/2,x1+k[1][2]/2,xp1+k[1][3]/2,y1+k[1][4]/2,yp1+k[1][5]/2,z1+k[1][6]/2,zp1+k[1][7]/2);
        k[2][4]=(h/2)*(yp1+k[1][5]/2);
        k[2][5]=(h/2)*H(w1+k[1][0]/2,wp1+k[1][1]/2,x1+k[1][2]/2,xp1+k[1][3]/2,y1+k[1][4]/2,yp1+k[1][5]/2,z1+k[1][6]/2,zp1+k[1][7]/2);
        k[2][6]=(h/2)*(zp1+k[1][7]/2);
        k[2][7]=(h/2)*I(w1+k[1][0]/2,wp1+k[1][1]/2,x1+k[1][2]/2,xp1+k[1][3]/2,y1+k[1][4]/2,yp1+k[1][5]/2,z1+k[1][6]/2,zp1+k[1][7]/2);
        
        k[3][0]=(h/2)*(wp1+k[2][1]);
        k[3][1]=(h/2)*F(w1+k[2][0],wp1+k[2][1],x1+k[2][2],xp1+k[2][3],y1+k[2][4],yp1+k[2][5],z1+k[2][6],zp1+k[2][7]);
        k[3][2]=(h/2)*(xp1+k[2][3]);
        k[3][3]=(h/2)*G(w1+k[2][0],wp1+k[2][1],x1+k[2][2],xp1+k[2][3],y1+k[2][4],yp1+k[2][5],z1+k[2][6],zp1+k[2][7]);
        k[3][4]=(h/2)*(yp1+k[2][5]);
        k[3][5]=(h/2)*H(w1+k[2][0],wp1+k[2][1],x1+k[2][2],xp1+k[2][3],y1+k[2][4],yp1+k[2][5],z1+k[2][6],zp1+k[2][7]);
        k[3][6]=(h/2)*(zp1+k[2][7]);
        k[3][7]=(h/2)*I(w1+k[2][0],wp1+k[2][1],x1+k[2][2],xp1+k[2][3],y1+k[2][4],yp1+k[2][5],z1+k[2][6],zp1+k[2][7]);
        
        // Runge-Kutta recurrence formula 
        w1=w1+(k[0][0]+2*k[1][0]+2*k[2][0]+k[3][0])/6;
        wp1=wp1+(k[0][1]+2*k[1][1]+2*k[2][1]+k[3][1])/6;
        x1=x1+(k[0][2]+2*k[1][2]+2*k[2][2]+k[3][2])/6;
        xp1=xp1+(k[0][3]+2*k[1][3]+2*k[2][3]+k[3][3])/6;
        y1=y1+(k[0][4]+2*k[1][4]+2*k[2][4]+k[3][4])/6;
        yp1=yp1+(k[0][5]+2*k[1][5]+2*k[2][5]+k[3][5])/6;
        z1=z1+(k[0][6]+2*k[1][6]+2*k[2][6]+k[3][6])/6;
        zp1=zp1+(k[0][7]+2*k[1][7]+2*k[2][7]+k[3][7])/6;
     }
      
     // Proper distance of light ray (with r = r'/R0) 
     // distanceLight = R(t)*r = R(t)*r'/R0 = (((3*H0)/(2*cv))*(ct))^(2./3)*r'
     distanceLight=powl(((3*H0)/(2*cv))*(w1),2./3)*abs(x1);
     // Proper distance of galaxy source : idem as above but for distanceGalaxy
     distanceGalaxy=powl(((3*H0)/(2*cv))*(w1),2./3)*distanceInit;
      
     // Comparison with h and h/2 results
     if (ldabs(x-x1)>=Er) {
        // Decrease step
        h=h*powl(Er/(ldabs(x-x1)),0.2);
        // Updating values with step/2 results 
        w=w2;
        wp=wp2;
        x=x2;
        xp=xp2;
        y=y2;
        yp=yp2;
        z=z2;
        zp=zp2;
     }
     else if ((ldabs((x-x1))==0.0)) {
        // Increase step
        h=h*1.1;
        // Save values with current step
        fprintf(file1,"%.30Le %.30Le %.30Le %.30Le %Le %Le\n",w1,x,x1,ldabs(x-x1),xp1,h);
        fprintf(file2,"%Le %Le %Le %Le %Le %Le %Le\n",w1,distanceLight,distanceGalaxy,y1,xp1,z1,h);
     }
     else {
        // Increase step
        h=h*powl(Er/(ldabs(x-x1)),0.2);
        // Save values with current step
        fprintf(file1,"%.30Le %.30Le %.30Le %.30Le %Le %Le\n",w1,x,x1,ldabs(x-x1),xp1,h);
        fprintf(file2,"%Le %Le %Le %Le %Le %Le %Le\n",w1,distanceLight,distanceGalaxy,y1,xp1,z1,h);
     }
   } while (w1<3*ctk);
   
   // Close output files
   fclose(file1);
   fclose(file2);
   
   return 0;
}