#include "main.h"

// random skip factor, must be larger than the number of random draws per particle
#define SKIP  100000

float rad;
float Rc, zc;

main(argc,argv)
int argc;
char **argv;
{
        int i, j, k, nobj=10000;
        int seed= -123;
        long long lseed=0;
        int icofm=1;
        float q=0.9, rtrunc=5.0;
        float rhoran, massapp;
        float x, y, z, vx, vy, v, v2, R, vmax, vmax2;
        float phi, cph, sph, cth, sth, vR, vp, vz;
        float E, Lz, rad, rad2;
        float f0, frand, fmax, fmax1, vphimax1, psi, fnorm;
        float Fdisk();
        float ran1();
        void ran_seed();
        float invu();
        float dr, rhomax1;
        float t, mass;
        float u1, v1, u1max, v1max;
        float xcm, ycm, zcm, vxcm, vycm, vzcm;
        float zip = 0.0, psi0;
        float rhomax=0.15, rhomin, rhotst;
        float rhoguess, zcon;
        float stream=0.5;
        float con, routdisk, drtrunc;
        float omega, kappa;
        float dv, v0;
        float broadcon=1.0;
        float diskdensf_(), sigr2_(), sigz2_();
        float FindMax(), FindMax1(), Fmax();
        float simpson(), massring();
        FILE *rhoout;
        FILE *errfile;
        FILE *outfile;
        char harmfile[40];

        errfile = fopen("errmsg.dat","w");
        strcpy(harmfile,"dbh.dat");
        iquery("Enter the number of particles",&nobj);
/*        fquery("Enter distribution broadening constant",&broadcon); */
      	llquery("Enter negative integer seed",&lseed);
        iquery("Center the simulation (0=no,1=yes)",&icofm);
        cquery("Enter harmonics file",harmfile);

        r = (phase *) calloc(nobj,sizeof(phase));

/*    readmassrad(); */
        readdiskdf_(harmfile,&gparam);
        mdisk = gparam.mdisk; rdisk = gparam.rdisk; zdisk = gparam.zdisk;
    routdisk = gparam.routdisk; drtrunc = gparam.drtrunc;
        rd = 1.2*rdisk;
        zd = 1.2*zdisk;
    rtrunc = (routdisk + 2.0*drtrunc);

        diskmass = 4.0*M_PI*simpson(massring,0.0,rtrunc,128);
        mass   = diskmass/nobj;
        dr = rtrunc/100.;
        rhomax = 0.0;
        rhoout = fopen("rhotst.dat","w");
        for(i=0; i<100; i++) {
                R = i*dr;
                z = 0.0;
                rhoguess = exp(-R/rd);
                rhotst = diskdensf_(&R,&z);
                rhotst /= rhoguess;
                if( rhotst > rhomax ) rhomax = rhotst;
                fprintf(rhoout,"%g %g\n",R, rhotst);
        }
        rhomin = 0.0;
        rhomax *=1.2;

        fprintf(stderr,"Calculating disk positions and velocities\n");
        if(lseed<0) lseed=-lseed;
        ran_seed(SKIP*lseed);
        for(i=0, j=0, k=0; i<nobj;) {

                u1 = -ran1(&seed);
                do{
                v1 = 2.0*(ran1(&seed) - 0.5);
                }while(v1<=-1. || v1>=1.);
                R = rd*invu(u1);
                
                z = zd*atanh(v1);

                zcon = cosh(z/zd);
                rhoguess = exp(-R/rd)/(zcon*zcon);
/* Guess at the approximate functional form of the density */

                rhotst = diskdensf_(&R,&z);

                rhotst /= rhoguess;

                k++;
                if( rhotst < rhomin )
                        continue;

                rhoran = (rhomax - rhomin)*ran1(&seed);
                if( rhoran > rhotst )
                        continue;
                phi = 2.0*M_PI*ran1(&seed);
                x = R*cos(phi);
                y = R*sin(phi);

                omekap_(&R, &omega, &kappa);
        vphimax = omega*R;

        vsigR = sqrt(sigr2_(&R));
        vsigp = kappa/(2.0*omega)*vsigR;
        vsigz = sqrt(sigz2_(&R));
/*
                fmax = Fdisk(0.0,vphimax,0.0,R,z);
                fprintf(stderr,"%g %g %g\n",vphimax, fmax, vsigp);
                fmax = 1.1*FindMax(R,z,&vphimax);
                fprintf(stderr,"%g %g\n",vphimax, fmax);
                fmax = Fmax(R,z,0.0,0.0,4.0*vsigp,&vphimax);
                fprintf(stderr,"%g %g\n",vphimax, fmax);
*/
/*
        vphimax1 = vphimax;
                fmax1 = 1.1*FindMax1(R,z,&vphimax1);
                fprintf(stderr,"R %g z %g %g %g %g %g\n",R,z,vphimax1, fmax1, vphimax, fmax);
*/
                fmax = 1.1*FindMax(R,z,&vphimax);
                f0 = 0.0; frand = 1.0; /* dummy starters */
                while( frand > f0 ) {
                        float f1;
                        float gr, gp, gz, g2;

                        g2 = 999.;
                        while( g2 > 1.0) {
                                gr = 8.0*(ran1(&seed) - 0.5);
                                gp = 16.0*(ran1(&seed) - 0.5);
                                gz =  8.0*(ran1(&seed) - 0.5);
                                g2 = gr*gr/16. + gp*gp/64. + gz*gz/16.;
                        }

                        vR = broadcon*vsigR*gr;
                        vp = vphimax + broadcon*vsigp*gp;
                        vz = broadcon*vsigz*gz;

                        f0 = Fdisk(vR, vp, vz, R, z);
                        frand = fmax*ran1(&seed);
                        if( f0 > fmax ) {
                                float vpmax;
                                fprintf(errfile,"f0 > fmax at R=%g z=%g\nvr=%g vp=%g, vz=%g vphimax=%g f0=%g, fmax=%g\n", 
                                        R,z, 
                                        vR*broadcon/vsigR, 
                                        (vp - vphimax)*broadcon/vsigp, 
                                        vz*broadcon/vsigz, vphimax, f0, fmax);
/*
                                vpmax = vphimax;
                                fprintf(errfile,"fmax is %g at true vmax %g guess %g\n",Fmax(R,z,0.0,0.0,4.0*vsigp,&vpmax), vpmax, vphimax);
                                fprintf(errfile,"guess fmax %g f0 %g vp %g\n",fmax,f0,vp);
*/
                                fflush(errfile);
                        }
                        j++;
                }

                cph = x/R; sph = y/R;
                vx = vR*cph - vp*sph;
                vy = vR*sph + vp*cph;
        /*  vz stays the same */

                r[i].x = (float) x;
                r[i].y = (float) y;
                r[i].z = (float) z;
                r[i].vx = (float)vx;
                r[i].vy = (float)vy;
                r[i].vz = (float)vz;
                i++; ran_seed(SKIP*(lseed+i));
                if( i % 1000 == 0 ) {
                        fprintf(stderr,".");
                        fflush(stderr);
                }
        }
        fprintf(stderr,"\n");
        fprintf(stderr,"number of density trials %d\n",k);
        fprintf(stderr,"number of velocity trials %d\n",j);

        if( icofm ) {
                xcm = ycm =zcm = vxcm =vycm =vzcm = 0;
                for(i=0; i<nobj; i++) {
                        xcm += r[i].x;
                        ycm += r[i].y;
                        zcm += r[i].z;
                        vxcm += r[i].vx;
                        vycm += r[i].vy;
                        vzcm += r[i].vz;
                }
                xcm /= nobj; ycm /=nobj; zcm /= nobj;
                vxcm /= nobj; vycm /=nobj; vzcm /= nobj;
        
                for(i=0; i<nobj; i++) {
                        r[i].x -= xcm;
                        r[i].y -= ycm;
                        r[i].z -= zcm;
                        r[i].vx -= vxcm;
                        r[i].vy -= vycm;
                        r[i].vz -= vzcm;
                }
        }

        t = 0.0;
#ifdef ASCII
	fprintf(stdout,"%d %15.7e\n",nobj,t);
	for(i=0; i<nobj; i++) {
	  fprintf(stdout,"%14.7e %14.7e %14.7e %14.7e %14.7e %14.7e %14.7e\n",
		  mass, r[i].x, r[i].y, r[i].z, r[i].vx, r[i].vy, r[i].vz);
		  }
#else
	for(i=0; i<nobj; i++) {
	  fwrite(&mass,sizeof(float),1,stdout);
      fwrite(r+i,sizeof(phase),1,stdout);
	}
#endif
    exit(0);
}

float Fdisk(vr, vp, vz, R, z)
float vr, vp, vz, R, z;
{
        float vr0, vp0, vz0, R0, z0;
        float diskdf5ez_();

        vr0 = vr; vp0 = vp; vz0 = vz; R0 = R; z0 = z;
        return diskdf5ez_(&vr0, &vp0, &vz0, &R0, &z0);
}

float invu(u)
float u;
{
        /* invert the u function to find R */
        int i;
        double dr, rg, rnew, eps=1.0e-8;

        rg = 1.0; 
        dr = 1.0;
        eps = 1.0e-8;
        for(i=0; (i<20 && dr > eps); i++) {
                rnew = rg - (-(1.0 + rg)*exp(-rg) - u)
                                /(rg*exp(-rg));
                dr = fabs(rnew - rg);
                rg = rnew;
        }
        if( i == 20 ) fprintf(stderr,"Warning R did not converge\n");
        return (float) rg;
}

/* A approximate estimate of the local maximum of the distribution function */
float Fmax(R,z,vR,vz,vlimit, vpmax)
float R, z, vR, vz;
float *vpmax, vlimit;
{
        int i;
        float v, dv, fmax, vmax, v0;
        float Fdisk();

        dv = 2.0*vlimit/100;
    v0 = *vpmax - 50*dv;
        fmax = Fdisk(vR, v0, vz, R, z);
        for(i=0; i<100; i++) {
                float ftry, vtry;

                vtry = v0 + i*dv;
                ftry = Fdisk(vR, vtry, vz, R, z);
                if( ftry > fmax ) {
                        fmax = ftry;
                        vmax = vtry;
                }
        }
        *vpmax = vmax;

        return fmax;
}

float FindMax1(R,z,vpmax)
float R, z, *vpmax;
{
        int upflag, flag;
        float dv, vpm, vpmold, v0, v1;
        float f0, f1, ftmp, fmid;
        float Fdisk();

        dv = 0.1*vsigp;
        vpm = *vpmax;
        v0 = vpm - dv;
        v1 = vpm + dv;

        f0 = Fdisk(0.0,v0,0.0,R,z);
        fmid = Fdisk(0.0,vpm,0.0,R,z);
        f1 = Fdisk(0.0,v1,0.0,R,z);
        if( fmid >= f0 && fmid >= f1 )
                return fmid;

        if( f0 > f1 ) {
                ftmp = f0;
                f0 = f1;
                f1 = ftmp;
                v1 = v0;
                dv = -dv;
        }
        vpm = v1;

        flag = 1;
        while( flag ) {

                dv *= 2.0;
                vpmold = vpm;
                vpm += dv;
                f0 = f1;
                f1 = Fdisk(0.0,vpm,0.0,R,z);
                flag = (f1 > f0);
        }
        *vpmax = vpmold;
        return f0;
}

float FindMax(R,z,vpmax)
float R, z, *vpmax;
{
        float vpm;
        float v0, v1;
        float eps=0.01;
        float fmax, golden(), fgold();
        
        Rc = R; zc = z;

        vpm = *vpmax;
    v0 = *vpmax - 3.0*vsigp;
    v1 = *vpmax + 3.0*vsigp;
/*
        f0 = Fdisk(0.0,v0,0.0,R,z);
        f1 = Fdisk(0.0,v1,0.0,R,z);
        fm = Fdisk(0.0,vpm,0.0,R,z);
*/

        fmax = -golden(v0,vpm,v1, fgold, eps, vpmax);

        return fmax;

}

float fgold(vp)
float vp;
{
        float R, z;

        R = Rc; z = zc;
        return( -Fdisk(0.0,vp,0.0,R,z) );
}

float massring(r)
float r;
{
    float denz();
    float simpson();

    rad = r;
    return( rad*simpson(denz,0.0,5.0*zdisk,128) );
}

float denz(z)
float z;
{
    float z0;
    float diskdensf_();

    z0 = z;
    return diskdensf_(&rad, &z0);
}

