#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <unistd.h>
#include <dirent.h>
#include "hdf5.h"
#include <math.h>
#include <time.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_sf_bessel.h>
#include "mclib_3d.h"
#include <omp.h>

#define R_DIM 1260
#define THETA_DIM 280
#define PHI_DIM 280

void read_hydro(char hydro_prefix[200], int frame, double r_inj, double **x, double **y,  double **z, double **szx, double **szy, double **r, double **theta, double **phi,\
  double **velx, double **vely, double **velz, double **dens, double **pres, double **gamma, double **dens_lab, double **temp, int *number,  int ph_inj_switch, double min_r, double max_r, double fps, FILE *fPtr)
{
    FILE *hydroPtr=NULL;
    char hydrofile[200]="", file_num[200]="", full_file[200]="",file_end[200]=""  ;
    char buf[10]="";
    int i=0, j=0, k=0, elem=0, elem_factor=0;
    int phi_min_index=0, phi_max_index=0, r_min_index=0, r_max_index=0, theta_min_index=0, theta_max_index=0; //all_index_buffer contains phi_min, phi_max, theta_min, theta_max, r_min, r_max indexes to get from grid files
    int r_index=0, theta_index=0, phi_index=0, hydro_index=0, all_index_buffer=0, adjusted_remapping_index=0, dr_index=0;
    int *remapping_indexes=NULL;
    float buffer=0;
    float *dens_unprc=NULL;
    float *vel_r_unprc=NULL;
    float *vel_theta_unprc=NULL;
    float *vel_phi_unprc=NULL;
    float *pres_unprc=NULL;
    double ph_rmin=0, ph_rmax=0;
    double r_in=1e10, r_ref=2e13;
    double *r_edge=NULL;
    double *dr=NULL;
    double *r_unprc=malloc(sizeof(double)*R_DIM);
    double *theta_unprc=malloc(sizeof(double)*THETA_DIM);
    double *phi_unprc=malloc(sizeof(double)*PHI_DIM);

    if (ph_inj_switch==0)
    {
        ph_rmin=min_r;
        ph_rmax=max_r;
    }

    snprintf(file_end,sizeof(file_end),"%s","small.data" );

    //density
    snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"u0", 1,"-" );
    modifyFlashName(file_num, hydrofile, frame,1);

    fprintf(fPtr,">> Opening file %s\n", file_num);
    fflush(fPtr);

    snprintf(full_file, sizeof(full_file), "%s%s", file_num, file_end);
    /*
    fprintf(fPtr,"Reading Density: %s\n", full_file);
    fflush(fPtr);
    */
    hydroPtr=fopen(full_file, "rb");
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    fread(&phi_min_index, sizeof(int)*1, 1,hydroPtr); //min and max indexes for the grid
    fread(&phi_max_index, sizeof(int)*1, 1,hydroPtr);
    fread(&theta_min_index, sizeof(int)*1, 1,hydroPtr);
    fread(&theta_max_index, sizeof(int)*1, 1,hydroPtr);
    fread(&r_min_index, sizeof(int)*1, 1,hydroPtr);
    fread(&r_max_index, sizeof(int)*1, 1,hydroPtr);
    fclose(hydroPtr);

    //fortran indexing starts @ 1, but C starts @ 0
    r_min_index--;
    r_max_index--;
    theta_min_index--;
    theta_max_index--;
    phi_min_index--;
    phi_max_index--;

    //number of elements defined by this now
    elem=(r_max_index+1-r_min_index)*(theta_max_index+1-theta_min_index)*(phi_max_index+1-phi_min_index); //add 1 b/c max_index is 1 less than max number of elements in file
    /*
    fprintf(fPtr,"Elem %d\n", elem);
    fprintf(fPtr,"Limits %d, %d, %d, %d, %d, %d\n", phi_min_index, phi_max_index, theta_min_index, theta_max_index, r_min_index, r_max_index);
    fflush(fPtr);
    */
    //now with number of elements allocate data, remember last element is some garbage that only fortran uses
    dens_unprc=malloc(elem*sizeof(float));
    vel_r_unprc=malloc(elem*sizeof(float));
    vel_theta_unprc=malloc(elem*sizeof(float));
    pres_unprc=malloc(elem*sizeof(float));
    vel_phi_unprc=malloc(elem*sizeof(float));

    hydroPtr=fopen(full_file, "rb");
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr); //min and max indexes for the grid, dont need anymore so just save to dummy variable
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran

    fread(dens_unprc, sizeof(float),elem, hydroPtr); //data
    fclose(hydroPtr);

    /*
    for (i=0;i<R_DIM*THETA_DIM*PHI_DIM;i++)
    {
        if ((i>98784000-5) || (i<5))
        {
            fprintf(fPtr,"Density %d: %0.7e\n", i, *(dens_unprc+i));
            fflush(fPtr);
        }
    }
    */

    //velocities divided by c
    //v_r
    snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"u0", 2,"-" );
    modifyFlashName(file_num, hydrofile, frame,1);
    snprintf(full_file, sizeof(full_file), "%s%s", file_num, file_end);
    /*
    fprintf(fPtr,"Reading v_r: %s\n", full_file);
    fflush(fPtr);
    */
    hydroPtr=fopen(full_file, "rb");
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr); //min and max indexes for the grid, dont need anymore so just save to dummy variable
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran

    fread(vel_r_unprc, sizeof(float),elem, hydroPtr);
    fclose(hydroPtr);

    /*
    for (i=0;i<5;i++)
    {
        fprintf(fPtr,"V_r %d: %e\n", i, *(vel_r_unprc+i));
        fflush(fPtr);
    }
    */

    //v_theta
    snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"u0", 3,"-" );
    modifyFlashName(file_num, hydrofile, frame,1);
    snprintf(full_file, sizeof(full_file), "%s%s", file_num, file_end);
    /*
    fprintf(fPtr,"Reading v_theta: %s\n", full_file);
    fflush(fPtr);
    */
    hydroPtr=fopen(full_file, "rb");
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr); //min and max indexes for the grid, dont need anymore so just save to dummy variable
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran

    fread(vel_theta_unprc, sizeof(float),elem, hydroPtr);
    fclose(hydroPtr);

    /*
    for (i=0;i<5;i++)
    {
        fprintf(fPtr,"V_theta %d: %e\n", i, *(vel_theta_unprc+i));
        fflush(fPtr);
    }
    */

    //v_phi
    snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"u0", 4,"-" );
    modifyFlashName(file_num, hydrofile, frame,1);
    snprintf(full_file, sizeof(full_file), "%s%s", file_num, file_end);
    /*
    fprintf(fPtr,"Reading v_phi: %s\n", full_file);
    fflush(fPtr);
    */
    hydroPtr=fopen(full_file, "rb");
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr); //min and max indexes for the grid, dont need anymore so just save to dummy variable
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran

    fread(vel_phi_unprc, sizeof(float),elem, hydroPtr);
    fclose(hydroPtr);

    /*
    for (i=0;i<5;i++)
    {
        fprintf(fPtr,"V_phi %d: %e\n", i, *(vel_phi_unprc+i));
        fflush(fPtr);
    }
    */

    //pressure (divided by c^2)
    snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"u0", 8,"-" );
    modifyFlashName(file_num, hydrofile, frame,1);
    snprintf(full_file, sizeof(full_file), "%s%s", file_num, file_end);
    /*
    fprintf(fPtr,"Reading pres: %s\n", full_file);
    fflush(fPtr);
    */
    hydroPtr=fopen(full_file, "rb");
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr); //min and max indexes for the grid, dont need anymore so just save to dummy variable
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&all_index_buffer, sizeof(int)*1, 1,hydroPtr);
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran

    fread(pres_unprc, sizeof(float),elem, hydroPtr);
    fclose(hydroPtr);
    /*
    for (i=PHI_DIM-1;i<PHI_DIM;i++)
    {
        for (j=THETA_DIM-1;j<THETA_DIM;j++)
        {
            for (k=R_DIM-5;k<R_DIM;k++)
            {

            fprintf(fPtr,"Pres %d: %e\n", (i*R_DIM*THETA_DIM + j*R_DIM + k  ), *(pres_unprc+(i*R_DIM*THETA_DIM + j*R_DIM + k  )));
            fflush(fPtr);

            }
        }
    }
    */

    // see how many elements there are to test if reading correctly
    /*
    hydroPtr=fopen(full_file, "rb");
    fread(&buffer, sizeof(float), 1,hydroPtr); //random stuff about the file from fortran
    while (1 == fread(&buffer,sizeof(float),1,hydroPtr))
    {
        elem++;
    }
    //fread(pres_unprc, sizeof(double)*HYDRO_DIM,HYDRO_DIM, hydroPtr);
    fclose(hydroPtr);

    fprintf(fPtr,"Elem %d\n", elem);
    */

    //R
    //remapping_indexes=getIndexesForRadialRemapping(hydro_prefix); //can run this once in debug mode to find out delta index for each remapping and number of total r elements
    //for given set of remappings on July 12th 2017, grid00-x1.data[420]=grid01-x1.data[0], grid01-x1.data[420]=grid02-x1.data[0], etc. total number of r is 3780

    if (frame<=1300)
    {
        snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"grid0", 0,"-x1.data" );
        //adjusted_remapping_index=(*(remapping_indexes+0))+r_min_index ; //this if I am not hardcoding the dr index values
        adjusted_remapping_index=(0*420)+r_min_index;
    }
    else if (frame<=2000)
    {
        snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"grid0", 1,"-x1.data" );
        //adjusted_remapping_index=(*(remapping_indexes+1))+r_min_index;
        adjusted_remapping_index=(1*420)+r_min_index;
    }
    else if (frame<=10000)
    {
        snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"grid0", 2,"-x1.data" );
        //adjusted_remapping_index=(*(remapping_indexes+2))+r_min_index;
        adjusted_remapping_index=(2*420)+r_min_index;
    }
    else if (frame<=20000)
    {
        snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"grid0", 3,"-x1.data" );
        //adjusted_remapping_index=(*(remapping_indexes+3))+r_min_index;
        adjusted_remapping_index=(3*420)+r_min_index;
    }
    else if (frame<=35000)
    {
        snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"grid0", 4,"-x1.data" );
        //adjusted_remapping_index=(*(remapping_indexes+4))+r_min_index;
        adjusted_remapping_index=(4*420)+r_min_index;
    }
    else if (frame<=50000)
    {
        snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"grid0", 5,"-x1.data" );
        //adjusted_remapping_index=(*(remapping_indexes+5))+r_min_index;
        adjusted_remapping_index=(5*420)+r_min_index;
    }
    else if (frame<=60000)
    {
        snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"grid0", 6,"-x1.data" );
        //adjusted_remapping_index=(*(remapping_indexes+6))+r_min_index;
        adjusted_remapping_index=(6*420)+r_min_index;
    }

    fprintf(fPtr,"Reading Radius: %s\n", hydrofile);
    fflush(fPtr);

    hydroPtr=fopen(hydrofile, "r");

    i=0;
    while (i<R_DIM)
    {
        fscanf(hydroPtr, "%lf", (r_unprc+i));  //read value
        fgets(buf, 3,hydroPtr); //read comma
        /*
        if (i<5)
        {
            fprintf(fPtr,"R %d: %e\n", i, *(r_unprc+i));
            fflush(fPtr);
        }
        */
        i++;
    }

    fclose(hydroPtr);

    r_edge=malloc(sizeof(double)*(3780+1));
    dr=malloc(sizeof(double)*(3780));

    //calculate radial grid edges
    *(r_edge+0)=r_in;
    i=0;
    for (i=1;i<3780;i++)
    {
        *(r_edge+i)=(*(r_edge+i-1))+((*(r_edge+i-1))*(M_PI/560)/(1+((*(r_edge+i-1))/r_ref))); //r_i = r_(i-1) + Dq r_(i-1) [1 + r_(i-1)/r0]-1
        *(dr+i-1)=(*(r_edge+i))-(*(r_edge+i-1));
        /*
        if (i<5)
        {
            fprintf(fPtr,"R Edge: %d: %e Dr: %e\n", i, *(r_edge+i), *(dr+i-1));
            fflush(fPtr);
        }
        */
    }
    free(r_edge);

    //Theta
    snprintf(hydrofile,sizeof(hydrofile),"%s%s",hydro_prefix,"grid-x2.data" );

    fprintf(fPtr,"Reading Theta: %s\n", hydrofile);
    fflush(fPtr);

    hydroPtr=fopen(hydrofile, "r");

    i=0;
    while (i<THETA_DIM)
    {
        fscanf(hydroPtr, "%lf", (theta_unprc+i));  //read value
        fgets(buf, 3,hydroPtr); //read comma
        /*
        if (i<5)
        {
            fprintf(fPtr,"R %d: %e\n", i, *(theta_unprc+i));
            fflush(fPtr);
        }
        */
        i++;
    }

    fclose(hydroPtr);

    //Phi
    snprintf(hydrofile,sizeof(hydrofile),"%s%s",hydro_prefix,"grid-x3.data" );
    /*
    fprintf(fPtr,"Reading Phi: %s\n", hydrofile);
    fflush(fPtr);
    */
    hydroPtr=fopen(hydrofile, "r");

    i=0;
    while (i<PHI_DIM)
    {
        fscanf(hydroPtr, "%lf", (phi_unprc+i));  //read value
        fgets(buf, 3,hydroPtr); //read comma
        /*
        if (i<5)
        {
            fprintf(fPtr,"R %d: %e\n", i, *(phi_unprc+i));
            fflush(fPtr);
        }
        */
        i++;
    }

    fclose(hydroPtr);

    //limit number of array elements PUT WHILE LOOP TO MAKE SURE NUMBER OF ELEMENTS >0
    elem_factor=0;
    elem=0;
    while (elem==0)
    {
        elem=0;
        elem_factor++;
        for (i=0;i<(phi_max_index+1-phi_min_index);i++)
        {
            for (j=0;j<(theta_max_index+1-theta_min_index);j++)
            {
                for (k=0;k<(r_max_index+1-r_min_index);k++)
                {
                    r_index=r_min_index+k;
                    //if I have photons do selection differently than if injecting photons
                    if (ph_inj_switch==0)
                    {
                        //printf("R's:%d, %e\n", k, *(r_unprc+r_index));
                        //if calling this function when propagating photons, choose blocks based on where the photons are
                        if (((ph_rmin - elem_factor*C_LIGHT/fps)<(*(r_unprc+r_index))) && (*(r_unprc+r_index)  < (ph_rmax + elem_factor*C_LIGHT/fps) ))
                        {
                            // *(pres_unprc+(i*R_DIM*THETA_DIM + j*R_DIM + k  )
                            elem++;
                        }
                    }
                    else
                    {
                        //if calling this function to inject photons choose blocks based on injection parameters, r_inj, which is sufficient
                        if (((r_inj - elem_factor*C_LIGHT/fps)<(*(r_unprc+r_index))) && (*(r_unprc+r_index)  < (r_inj + elem_factor*C_LIGHT/fps) ))
                        {
                            // *(pres_unprc+(i*R_DIM*THETA_DIM + j*R_DIM + k  )
                            elem++;
                        }

                    }

                }
            }
        }
    }

    fprintf(fPtr,"Number of post restricted Elems: %d %e\n", elem, r_inj);
    //fprintf(fPtr,"Ph_min, Ph_max: %e, %e\n With c: min: %e max: %e \n", ph_rmin, ph_rmax, (ph_rmin - 2*C_LIGHT/fps), (ph_rmax + 2*C_LIGHT/fps));
    fflush(fPtr);

    //allocate space for new set of data
    (*pres)=malloc (elem * sizeof (double ));
    (*velx)=malloc (elem * sizeof (double ));
    (*vely)=malloc (elem * sizeof (double ));
    (*velz)=malloc (elem * sizeof (double ));
    (*dens)=malloc (elem * sizeof (double ));
    (*x)=malloc (elem * sizeof (double ));
    (*y)=malloc (elem * sizeof (double ));
    (*z)=malloc (elem * sizeof (double ));
    (*r)=malloc (elem * sizeof (double ));
    (*theta)=malloc (elem * sizeof (double ));
    (*phi)=malloc (elem * sizeof (double ));
    (*gamma)=malloc (elem * sizeof (double ));
    (*dens_lab)=malloc (elem * sizeof (double ));
    (*szx)=malloc (elem * sizeof (double )); //theta and phi resolution
    (*szy)=malloc (elem * sizeof (double )); //r resolution
    (*temp)=malloc (elem * sizeof (double ));

    //limit number of array elements
    elem=0;
    for (i=0;i<(phi_max_index+1-phi_min_index);i++)
    {
        for (j=0;j<(theta_max_index+1-theta_min_index);j++)
        {
            for (k=0;k<(r_max_index+1-r_min_index);k++)
            {
                r_index=r_min_index+k; //look at indexes of r that are included in small hydro file
                theta_index=theta_min_index+j;
                phi_index=phi_min_index+i;
                dr_index=adjusted_remapping_index+k;
                hydro_index=(i*(r_max_index+1-r_min_index)*(theta_max_index+1-theta_min_index) + j*(r_max_index+1-r_min_index) + k  );

                //if I have photons do selection differently than if injecting photons
                if (ph_inj_switch==0)
                {
                    //if calling this function when propagating photons, choose blocks based on where the photons are
                    if (((ph_rmin - elem_factor*C_LIGHT/fps)<(*(r_unprc+r_index))) && (*(r_unprc+r_index)  < (ph_rmax + elem_factor*C_LIGHT/fps) ))
                    {
                        (*pres)[elem] = *(pres_unprc+hydro_index);
                        (*dens)[elem] = *(dens_unprc+hydro_index);
                        (*temp)[elem] =  pow(3*(*(pres_unprc+hydro_index))*pow(C_LIGHT,2.0)/(A_RAD) ,1.0/4.0);

                        (*gamma)[elem] = pow(pow(1.0-(pow(*(vel_r_unprc+hydro_index),2)+ pow(*(vel_theta_unprc+hydro_index),2)+pow(*(vel_phi_unprc+hydro_index),2)),0.5),-1);
                        (*dens_lab)[elem] = (*(dens_unprc+hydro_index))*pow(pow(1.0-(pow(*(vel_r_unprc+hydro_index),2)+ pow(*(vel_theta_unprc+hydro_index),2)+pow(*(vel_phi_unprc+hydro_index),2)),0.5),-1);
                        (*r)[elem] = *(r_unprc+r_index);
                        (*theta)[elem] = *(theta_unprc+theta_index);
                        (*phi)[elem] = *(phi_unprc+phi_index);
                        (*x)[elem] = (*(r_unprc+r_index))*sin(*(theta_unprc+theta_index))*cos(*(phi_unprc+phi_index));
                        (*y)[elem] = (*(r_unprc+r_index))*sin(*(theta_unprc+theta_index))*sin(*(phi_unprc+phi_index));
                        (*z)[elem] = (*(r_unprc+r_index))*cos(*(theta_unprc+theta_index));
                        (*szx)[elem] =  *(dr+dr_index);
                        (*szy)[elem] =  M_PI/560;
                        (*velx)[elem]=((*(vel_r_unprc+hydro_index))*sin(*(theta_unprc+theta_index))*cos(*(phi_unprc+phi_index))) + ((*(vel_theta_unprc+hydro_index))*cos(*(theta_unprc+theta_index))*cos(*(phi_unprc+phi_index))) - ((*(vel_phi_unprc+hydro_index))*sin(*(phi_unprc+phi_index)));
                        (*vely)[elem]=((*(vel_r_unprc+hydro_index))*sin(*(theta_unprc+theta_index))*sin(*(phi_unprc+phi_index))) + ((*(vel_theta_unprc+hydro_index))*cos(*(theta_unprc+theta_index))*sin(*(phi_unprc+phi_index))) + ((*(vel_phi_unprc+hydro_index))*cos(*(phi_unprc+phi_index)));
                        (*velz)[elem]=((*(vel_r_unprc+hydro_index))*cos(*(theta_unprc+theta_index))) - ((*(vel_theta_unprc+hydro_index))*sin(*(theta_unprc+theta_index)));

                        elem++;

                    }
                }
                else
                {
                    //if calling this function to inject photons choose blocks based on injection parameters, r_inj, which is sufficient
                    if (((r_inj - elem_factor*C_LIGHT/fps)<(*(r_unprc+r_index))) && (*(r_unprc+r_index)  < (r_inj + elem_factor*C_LIGHT/fps) ))
                    {
                        (*pres)[elem] = *(pres_unprc+hydro_index);
                        (*dens)[elem] = *(dens_unprc+hydro_index);
                        (*temp)[elem] =  pow(3*(*(pres_unprc+hydro_index))*pow(C_LIGHT,2.0)/(A_RAD) ,1.0/4.0);
                        (*gamma)[elem] = pow(pow(1.0-(pow(*(vel_r_unprc+hydro_index),2)+ pow(*(vel_theta_unprc+hydro_index),2)+pow(*(vel_phi_unprc+hydro_index),2)),0.5),-1);
                        (*dens_lab)[elem] = (*(dens_unprc+hydro_index))*pow(pow(1.0-(pow(*(vel_r_unprc+hydro_index),2)+ pow(*(vel_theta_unprc+hydro_index),2)+pow(*(vel_phi_unprc+hydro_index),2)),0.5),-1);
                        (*r)[elem] = *(r_unprc+r_index);
                        (*theta)[elem] = *(theta_unprc+theta_index);
                        (*phi)[elem] = *(phi_unprc+phi_index);
                        (*x)[elem] = (*(r_unprc+r_index))*sin(*(theta_unprc+theta_index))*cos(*(phi_unprc+phi_index));
                        (*y)[elem] = (*(r_unprc+r_index))*sin(*(theta_unprc+theta_index))*sin(*(phi_unprc+phi_index));
                        (*z)[elem] = (*(r_unprc+r_index))*cos(*(theta_unprc+theta_index));
                        (*szx)[elem] =  *(dr+dr_index);
                        (*szy)[elem] =  M_PI/560;
                        (*velx)[elem]=((*(vel_r_unprc+hydro_index))*sin(*(theta_unprc+theta_index))*cos(*(phi_unprc+phi_index))) + ((*(vel_theta_unprc+hydro_index))*cos(*(theta_unprc+theta_index))*cos(*(phi_unprc+phi_index))) - ((*(vel_phi_unprc+hydro_index))*sin(*(phi_unprc+phi_index)));
                        (*vely)[elem]=((*(vel_r_unprc+hydro_index))*sin(*(theta_unprc+theta_index))*sin(*(phi_unprc+phi_index))) + ((*(vel_theta_unprc+hydro_index))*cos(*(theta_unprc+theta_index))*sin(*(phi_unprc+phi_index))) + ((*(vel_phi_unprc+hydro_index))*cos(*(phi_unprc+phi_index)));
                        (*velz)[elem]=((*(vel_r_unprc+hydro_index))*cos(*(theta_unprc+theta_index))) - ((*(vel_theta_unprc+hydro_index))*sin(*(theta_unprc+theta_index)));

                        elem++;
                    }

                }

            }
        }
    }

    *number=elem;

    free(pres_unprc); free(dens_unprc); free(r_unprc); free(theta_unprc); free(phi_unprc);free(dr);free(vel_r_unprc); free(vel_theta_unprc); free(vel_phi_unprc);

}

void photonInjection3D( struct photon **ph, int *ph_num, double r_inj, double ph_weight, int min_photons, int max_photons, char spect, int array_length, double fps, double theta_min, double theta_max,\
 double *x, double *y, double *z, double *szx, double *szy, double *r, double *theta, double *phi, double *temps, double *vx, double *vy, double *vz, gsl_rng * rand, FILE *fPtr)
{
    int i=0, block_cnt=0, *ph_dens=NULL, ph_tot=0, j=0,k=0;
    double ph_dens_calc=0.0, fr_dum=0.0, y_dum=0.0, yfr_dum=0.0, fr_max=0, bb_norm=0, position_phi, ph_weight_adjusted, theta_prime=0;
    double com_v_phi, com_v_theta, *p_comv=NULL, *boost=NULL; //comoving phi, theta, comoving 4 momentum for a photon, and boost for photon(to go to lab frame)
    double *l_boost=NULL; //pointer to hold array of lorentz boost, to lab frame, values
    float num_dens_coeff;

    if (spect=='w') //from MCRAT paper, w for wien spectrum
    {
        num_dens_coeff=8.44;
        //printf("in wien spectrum\n");
    }
    else
    {
        num_dens_coeff=20.29; //this is for black body spectrum
        //printf("in BB spectrum");
    }

    //find how many blocks are near the injection radius within the angles defined in mc.par, get temperatures and calculate number of photons to allocate memory for
    //and then rcord which blocks have to have "x" amount of photons injected there
    printf("%e, %e\n",*(phi+i), theta_max);
    for(i=0;i<array_length;i++)
    {
        //look at all boxes in width delta r=c/fps and within angles we are interested in NEED TO modify for RIKEN data- dont need r anymore, just theta and phi? (didnt work), just look at pojection on x-z plane
        theta_prime=acos(*(y+i)/(*(r+i))); //jet axis here is the y axis
            if ( (theta_prime< theta_max) && (theta_prime >= theta_min) ) //(*(r+i) > (r_inj - C_LIGHT/fps))  &&   (*(r+i)  < (r_inj + C_LIGHT/fps)  ) &&
            {
                //printf("%e\n", theta_prime );
                block_cnt++;
            }
    }
    printf("Blocks: %d\n", block_cnt);

    ph_dens=malloc(block_cnt * sizeof(int));

    //calculate the photon density for each block and save it to the array
    j=0;
    ph_tot=0;
    ph_weight_adjusted=ph_weight;
    //printf("%d %d\n", max_photons, min_photons);
    while ((ph_tot>max_photons) || (ph_tot<min_photons) )
    {
        j=0;
        ph_tot=0;
        //allocate memory to record density of photons for each block
        //ph_dens=malloc(block_cnt * sizeof(int));

        for (i=0;i<array_length;i++)
        {
            //printf("%d\n",i);
            //printf("%e, %e, %e, %e, %e, %e\n", *(r+i),(r_inj - C_LIGHT/fps), (r_inj + C_LIGHT/fps), *(theta+i) , theta_max, theta_min);
                //NEED TO modify for RIKEN data - modified
                theta_prime=acos(*(y+i)/(*(r+i)));
                if ( (theta_prime< theta_max) && (theta_prime >= theta_min) )
                {
                    //NEED TO modify for RIKEN data - modified
                    ph_dens_calc=(num_dens_coeff*pow(*(temps+i),3.0)*pow(*(r+i),2)*sin(*(theta+i))* pow(*(szy+i),2.0)*(*(szx+i)) /(ph_weight_adjusted))*pow(pow(1.0-(pow(*(vx+i),2)+pow(*(vy+i),2)+pow(*(vz+i),2)),0.5),-1) ; //a*T^3/(weight) dV, dV=2*PI*x*dx^2,

                     (*(ph_dens+j))=gsl_ran_poisson(rand,ph_dens_calc) ; //choose from poission distribution with mean of ph_dens_calc

                    //printf("%d, %lf \n",*(ph_dens+j), ph_dens_calc);

                     //sum up all the densities to get total number of photons
                     ph_tot+=(*(ph_dens+j));

                     j++;
                }
        }

        if (ph_tot>max_photons)
        {
            //if the number of photons is too big make ph_weight larger
            ph_weight_adjusted*=10;
            //free(ph_dens);
        }
        else if (ph_tot<min_photons)
        {
            ph_weight_adjusted*=0.5;
            //free(ph_dens);
        }
        //printf("dens: %d, photons: %d\n", *(ph_dens+(j-1)), ph_tot);

    }

    printf("%d\n", ph_tot);

    //allocate memory for that many photons and also allocate memory to hold comoving 4 momentum of each photon and the velocity of the fluid
    (*ph)=malloc (ph_tot * sizeof (struct photon ));

    p_comv=malloc(4*sizeof(double));
    boost=malloc(3*sizeof(double));
    l_boost=malloc(4*sizeof(double));

    //go through blocks and assign random energies/locations to proper number of photons
    ph_tot=0;
    k=0;
    for (i=0;i<array_length;i++)
    {
        theta_prime=acos(*(y+i)/(*(r+i)));
        if ( (theta_prime< theta_max) && (theta_prime >= theta_min) )  //NEED TO modify for RIKEN data - modified
        {

            //*(temps+i)=0.76*(*(temps+i));
            for(j=0;j<( *(ph_dens+k) ); j++ )
            {
                    //have to get random frequency for the photon comoving frequency
                    y_dum=1; //initalize loop
                    yfr_dum=0;
                    while (y_dum>yfr_dum)
                    {
                        fr_dum=gsl_rng_uniform_pos(rand)*6.3e11*(*(temps+i)); //in Hz
                        //printf("%lf, %lf ",gsl_rng_uniform_pos(rand), (*(temps+i)));
                        y_dum=gsl_rng_uniform_pos(rand);
                        //printf("%lf ",fr_dum);

                        if (spect=='w')
                        {
                            yfr_dum=(1.0/(1.29e31))*pow((fr_dum/(*(temps+i))),3.0)/(exp((PL_CONST*fr_dum)/(K_B*(*(temps+i)) ))-1); //curve is normalized to maximum
                        }
                        else
                        {
                            fr_max=(5.88e10)*(*(temps+i));//(C_LIGHT*(*(temps+i)))/(0.29); //max frequency of bb
                            bb_norm=(PL_CONST*fr_max * pow((fr_max/C_LIGHT),2.0))/(exp(PL_CONST*fr_max/(K_B*(*(temps+i))))-1); //find value of bb at fr_max
                            yfr_dum=((1.0/bb_norm)*PL_CONST*fr_dum * pow((fr_dum/C_LIGHT),2.0))/(exp(PL_CONST*fr_dum/(K_B*(*(temps+i))))-1); //curve is normalized to vaue of bb @ max frequency
                        }
                        //printf("%lf, %lf,%lf,%e \n",(*(temps+i)),fr_dum, y_dum, yfr_dum);

                    }
                   //printf("%lf\n ",fr_dum);
                   //position_phi= gsl_rng_uniform(rand)*2*M_PI; //NEED TO modify for RIKEN data-modified, dont need anymore
                   com_v_phi=gsl_rng_uniform(rand)*2*M_PI;
                   com_v_theta=acos((gsl_rng_uniform(rand)*2)-1);
                   //printf("%lf, %lf, %lf\n", position_phi, com_v_phi, com_v_theta);

                   //populate 4 momentum comoving array
                   *(p_comv+0)=PL_CONST*fr_dum/C_LIGHT;
                   *(p_comv+1)=(PL_CONST*fr_dum/C_LIGHT)*sin(com_v_theta)*cos(com_v_phi);
                   *(p_comv+2)=(PL_CONST*fr_dum/C_LIGHT)*sin(com_v_theta)*sin(com_v_phi);
                   *(p_comv+3)=(PL_CONST*fr_dum/C_LIGHT)*cos(com_v_theta);

                    //populate boost matrix, not sure why multiplying by -1, seems to give correct answer in old python code...
                    //NEED TO modify for RIKEN data - modified
                    *(boost+0)=-1*(*(vx+i));
                    *(boost+1)=-1*(*(vy+i));
                    *(boost+2)=-1*(*(vz+i));
                    //printf("%lf, %lf, %lf\n", *(boost+0), *(boost+1), *(boost+2));

                    //boost to lab frame
                    lorentzBoost(boost, p_comv, l_boost, 'p', fPtr);
                    //printf("Assignemnt: %e, %e, %e, %e\n", *(l_boost+0), *(l_boost+1), *(l_boost+2),*(l_boost+3));

                (*ph)[ph_tot].p0=(*(l_boost+0));
                (*ph)[ph_tot].p1=(*(l_boost+1));
                (*ph)[ph_tot].p2=(*(l_boost+2));
                (*ph)[ph_tot].p3=(*(l_boost+3));
                //NEED TO modify for RIKEN data-modified
                (*ph)[ph_tot].r0= (*(x+i)); //put photons @ center of box that they are supposed to be in with random phi
                (*ph)[ph_tot].r1=(*(y+i)) ;
                (*ph)[ph_tot].r2=(*(z+i)); //y coordinate in flash becomes z coordinate in MCRaT
                (*ph)[ph_tot].num_scatt=0;
                (*ph)[ph_tot].weight=ph_weight_adjusted;
                //printf("%d\n",ph_tot);
                ph_tot++;
            }
            k++;
        }
    }

    *ph_num=ph_tot; //save number of photons
    //printf(" %d: %d\n", *(ph_dens+(k-1)), *ph_num);
    free(ph_dens); free(p_comv);free(boost); free(l_boost);

}

void phMinMax(struct photon *ph, int ph_num, double *min, double *max, double *min_theta, double *max_theta, FILE *fPtr)
{
    double temp_r_max=0, temp_r_min=DBL_MAX, temp_theta_max=0, temp_theta_min=DBL_MAX;
    int i=0, num_thread=omp_get_num_threads();
    double ph_r=0, ph_theta=0;

#pragma omp parallel for num_threads(num_thread) firstprivate(ph_r, ph_theta) reduction(min:temp_r_min) reduction(max:temp_r_max) reduction(min:temp_theta_min) reduction(max:temp_theta_max)
    for (i=0;i<ph_num;i++)
    {
        if ((ph+i)->weight != 0)
        {
            ph_r=pow(pow( ((ph+i)->r0), 2.0) + pow(((ph+i)->r1),2.0 ) + pow(((ph+i)->r2) , 2.0),0.5);
            ph_theta=acos(((ph+i)->r2) /ph_r); //this is the photons theta psition in the FLASH grid, gives in radians
            if (ph_r > temp_r_max )
            {
                temp_r_max=ph_r;
                //fprintf(fPtr, "The new max is: %e from photon %d with x: %e y: %e z: %e\n", temp_r_max, i, ((ph+i)->r0), (ph+i)->r1, (ph+i)->r2);
            }

            //if ((i==0) || (ph_r<temp_r_min))
            if (ph_r<temp_r_min)
            {
                temp_r_min=ph_r;
                //fprintf(fPtr, "The new min is: %e from photon %d with x: %e y: %e z: %e\n", temp_r_min, i, ((ph+i)->r0), (ph+i)->r1, (ph+i)->r2);
            }

            if (ph_theta > temp_theta_max )
            {
                temp_theta_max=ph_theta;
                //fprintf(fPtr, "The new max is: %e from photon %d with x: %e y: %e z: %e\n", temp_r_max, i, ((ph+i)->r0), (ph+i)->r1, (ph+i)->r2);
            }

            //if ((i==0) || (ph_r<temp_r_min))
            if (ph_theta<temp_theta_min)
            {
                temp_theta_min=ph_theta;
                //fprintf(fPtr, "The new min is: %e from photon %d with x: %e y: %e z: %e\n", temp_r_min, i, ((ph+i)->r0), (ph+i)->r1, (ph+i)->r2);
            }
        }
    }

    *max=temp_r_max;
    *min=temp_r_min;
    *max_theta=temp_theta_max;
    *min_theta=temp_theta_min;
}

int *getIndexesForRadialRemapping(char hydro_prefix[200])
{
    FILE *hydroPtr=NULL;
    char hydrofile[200]="";
    char buf[10]="";
    int i=0, j=0;
    int *remapping_start_index=malloc(sizeof(int)*7); //what index out of total range of r does each remapping begin at
    double r_in=1e10, r_ref=2e13;
    double *r_unprc_0=malloc(sizeof(double)*R_DIM), *r_unprc_1=malloc(sizeof(double)*R_DIM), *r_unprc_2=malloc(sizeof(double)*R_DIM), *r_unprc_3=malloc(sizeof(double)*R_DIM);
    double *r_unprc_4=malloc(sizeof(double)*R_DIM), *r_unprc_5=malloc(sizeof(double)*R_DIM), *r_unprc_6=malloc(sizeof(double)*R_DIM);
    double *r_edge=NULL, *dr=NULL, *rPtr=NULL;

    for (i=0;i<7;i++)
    {
        snprintf(hydrofile,sizeof(hydrofile),"%s%s%d%s",hydro_prefix,"grid0", i,"-x1.data" );

        hydroPtr=fopen(hydrofile, "r");

        j=0;
        while (j<R_DIM)
        {
            switch (i)
            {
                case 0: fscanf(hydroPtr, "%lf", (r_unprc_0+j));  //read value
                case 1: fscanf(hydroPtr, "%lf", (r_unprc_1+j));
                case 2: fscanf(hydroPtr, "%lf", (r_unprc_2+j));
                case 3: fscanf(hydroPtr, "%lf", (r_unprc_3+j));
                case 4: fscanf(hydroPtr, "%lf", (r_unprc_4+j));
                case 5: fscanf(hydroPtr, "%lf", (r_unprc_5+j));
                case 6: fscanf(hydroPtr, "%lf", (r_unprc_6+j));

            }
            fgets(buf, 3,hydroPtr); //read comma
            /*
            if (i<5)
            {
                fprintf(fPtr,"R %d: %e\n", i, *(r_unprc+i));
                fflush(fPtr);
            }
            */
            j++;
        }

        fclose(hydroPtr);
    }

    //calculate the indexes in which each remapping takes over
    j=0; //keeps track of indexes of all of the possible r values
    i=0; //keeps track of index within a certain remapping, when get to R_DIM know that were @ end of last remapping b/c remappings overlap with one another
    rPtr=r_unprc_0; //start off looking at 0th remapping
    *(remapping_start_index+1)=j; //0th remapping starts at index 0
    while (i<R_DIM)
    {
        if (*(rPtr+i)== *(r_unprc_1+0))
        {
            //if the element of the 0th remapping is equal to the 1st element of the 1st remapping, start to look at the 1st remapping
            rPtr=r_unprc_1;
            i=0;
            *(remapping_start_index+1)=j;
        }
        else if (*(rPtr+i)== *(r_unprc_2+0))
        {
            rPtr=r_unprc_2;
            i=0;
            *(remapping_start_index+2)=j;
        }
        else if (*(rPtr+i)== *(r_unprc_3+0))
        {
            rPtr=r_unprc_3;
            i=0;
            *(remapping_start_index+3)=j;
        }
        else if (*(rPtr+i)== *(r_unprc_4+0))
        {
            rPtr=r_unprc_4;
            i=0;
            *(remapping_start_index+4)=j;
        }
        else if (*(rPtr+i)== *(r_unprc_5+0))
        {
            rPtr=r_unprc_5;
            i=0;
            *(remapping_start_index+5)=j;
        }
        else if (*(rPtr+i)== *(r_unprc_6+0))
        {
            rPtr=r_unprc_6;
            i=0;
            *(remapping_start_index+6)=j;
        }

        j++;
        i++;
    }

    printf("Indexes %d, %d, %d, %d, %d, %d, %d\n Elems: %d\n", *(remapping_start_index+0), *(remapping_start_index+1), *(remapping_start_index+2), *(remapping_start_index+3), *(remapping_start_index+4), *(remapping_start_index+5), *(remapping_start_index+6), j);
    //exit(0);
    r_edge=malloc(sizeof(double)*(j+1));
    dr=malloc(sizeof(double)*j);

    //calculate radial grid edges
    *(r_edge+0)=r_in;
    i=0;
    for (i=1;i<j;i++)
    {
        *(r_edge+i)=(*(r_edge+i-1))+((*(r_edge+i-1))*(M_PI/560)/(1+((*(r_edge+i-1))/r_ref))); //r_i = r_(i-1) + Dq r_(i-1) [1 + r_(i-1)/r0]-1
        *(dr+i-1)=(*(r_edge+i))-(*(r_edge+i-1));
        /*
        if (i<5)
        {
            fprintf(fPtr,"R Edge: %d: %e Dr: %e\n", i, *(r_edge+i), *(dr+i-1));
            fflush(fPtr);
        }
        */
    }
    free(r_edge);
    free(r_unprc_0);
    free(r_unprc_1);
    free(r_unprc_2);
    free(r_unprc_3);
    free(r_unprc_4);
    free(r_unprc_5);
    free(r_unprc_6);

    return remapping_start_index;

}


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C code headers for the implementation of the Fourier domain response for LISA-like detectors
 *
 */

#ifndef _LISAFDRESPONSE_H
#define _LISAFDRESPONSE_H

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "waveform.h"
#include "EOBNRv2HMROMstruct.h"
#include "LISAgeometry.h"

#if defined(__cplusplus)
extern "C" {
#elif 0
} /* so that editors will match preceding brace */
#endif

/**************************************************/
/**************** Prototypes **********************/

/* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LISA response, for given values of the inclination, position in the sky and polarization angle - here simplified version for just the y_21 observable */
int LISASimFDResponse21(
  LISAconstellation *variant,                                 /* Provides specifics on the variant of LISA */
  struct tagListmodesCAmpPhaseFrequencySeries **list,  /* Input/Output: list of modes in Frequency-domain amplitude and phase form as produced by the ROM, and output after FD response processing */
  const double inclination,                                   /* Inclination of the source */
  const double lambda,                                        /* First angle for the position in the sky */
  const double beta,                                          /* Second angle for the position in the sky */
  const double psi);                                          /* Polarization angle */

//WARNING: tRef is ignored for now in the response - i.e. set to 0
/* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LISA response, for given values of the inclination, position in the sky and polarization angle */
int LISASimFDResponsey12(
  LISAconstellation *variant,                                 /* Provides specifics on the variant of LISA */
  struct tagListmodesCAmpPhaseFrequencySeries **list,      /* Input: list of modes in Frequency-domain amplitude and phase form as produced by the ROM */
  struct tagListmodesCAmpPhaseFrequencySeries **listy12,   /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the y12 observable */
  const double torb,                                       /* Reference orbital time - tf as read from the hlm gives t-tinj, this arg allows to pass tinj to the response */
  const double lambda,                                        /* First angle for the position in the sky */
  const double beta,                                          /* Second angle for the position in the sky */
  const double inclination,                                   /* Inclination of the source */
  const double psi,                                           /* Polarization angle */
  const int tagfrozenLISA,                                    /* Tag to treat LISA as frozen at its torb configuration  */
  const ResponseApproxtag responseapprox);                    /* Tag to select possible low-f approximation level in FD response */

//WARNING: tRef is ignored for now in the response - i.e. set to 0
/* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LISA response, for given values of the inclination, position in the sky and polarization angle */
int LISASimFDResponseTDI3Chan(
  int tagtRefatLISA,                                          /* 0 to measure Tref from SSB arrival, 1 at LISA guiding center */
  LISAconstellation *variant,                                 /* Provides specifics on the variant of LISA */
  struct tagListmodesCAmpPhaseFrequencySeries **list,      /* Input: list of modes in Frequency-domain amplitude and phase form as produced by the ROM */
  struct tagListmodesCAmpPhaseFrequencySeries **listTDI1,  /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the TDI channel 1 */
  struct tagListmodesCAmpPhaseFrequencySeries **listTDI2,  /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the TDI channel 2 */
  struct tagListmodesCAmpPhaseFrequencySeries **listTDI3,  /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the TDI channel 3 */
  const double torb,                                       /* Reference orbital time - tf as read from the hlm gives t-tinj, this arg allows to pass tinj to the response */
  const double lambda,                                        /* First angle for the position in the sky */
  const double beta,                                          /* Second angle for the position in the sky */
  const double inclination,                                   /* Inclination of the source */
  const double psi,                                           /* Polarization angle */
  const double m1,                                            /* m1 in solar masses - used for resampling */
  const double m2,                                            /* m2 in solar masses - used for resampling */
  const double maxf,                                          /* Maximal frequency to consider - used to ignore hard-to-resolve response at f>1Hz - NOTE: for now, no recomputation of the boundary, so when not resampling can lose a bit of support between the last frequency point covered and maxf */
  const TDItag tditag,                                        /* Selector for the set of TDI observables */
  const int tagfrozenLISA,                                    /* Tag to treat LISA as frozen at its torb configuration  */
  const ResponseApproxtag responseapprox);                    /* Tag to select possible low-f approximation level in FD response */

// int LISASimFDResponseTDI1Chan(
//   struct tagListmodesCAmpPhaseFrequencySeries **list,      /* Input: list of modes in Frequency-domain amplitude and phase form as produced by the ROM */
//   struct tagListmodesCAmpPhaseFrequencySeries **listTDI,   /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the TDI channel 1 */
//   const double tRef,                                          /* Time at coalescence */
//   const double lambda,                                        /* First angle for the position in the sky */
//   const double beta,                                          /* Second angle for the position in the sky */
//   const double inclination,                                   /* Inclination of the source */
//   const double psi,                                           /* Polarization angle */
//   const double maxf,                                          /* Maximal frequency to consider - used to ignore hard-to-resolve response at f>1Hz - NOTE: for now, no recomputation of the boundary, so when not resampling can lose a bit of support between the last frequency point covered and maxf */
//   const TDItag tditag);                                       /* Selector for the set of TDI observables */

#if 0
{ /* so that editors will match succeeding brace */
#elif defined(__cplusplus)
}
#endif

#endif /* _LISAFDRESPONSE_H */


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

#include "allvars.h"
#include "proto.h"
#include "domain.h"

#include <gsl/gsl_heapsort.h>

static struct peano_hilbert_data
{
  peanokey key;
  int index;
}
 *mp;

static int *Id;

void peano_hilbert_order(void)
{
  int i;

  if(ThisTask == 0)
    printf("begin Peano-Hilbert order...\n");

  if(N_gas)
    {
      mp = (struct peano_hilbert_data *) mymalloc(sizeof(struct peano_hilbert_data) * N_gas);
      Id = (int *) mymalloc(sizeof(int) * N_gas);

      for(i = 0; i < N_gas; i++)
	{
	  mp[i].index = i;
	  mp[i].key = Key[i];
	}

#ifdef MYSORT
      mysort_peano(mp, N_gas, sizeof(struct peano_hilbert_data), peano_compare_key);
#else
      qsort(mp, N_gas, sizeof(struct peano_hilbert_data), peano_compare_key);
#endif

      for(i = 0; i < N_gas; i++)
	Id[mp[i].index] = i;

      reorder_gas();

      myfree(Id);
      myfree(mp);
    }

  if(NumPart - N_gas > 0)
    {
      mp = (struct peano_hilbert_data *) mymalloc(sizeof(struct peano_hilbert_data) * (NumPart - N_gas));
      mp -= (N_gas);

      Id = (int *) mymalloc(sizeof(int) * (NumPart - N_gas));
      Id -= (N_gas);

      for(i = N_gas; i < NumPart; i++)
	{
	  mp[i].index = i;
	  mp[i].key = Key[i];
	}

#ifdef MYSORT
      mysort_peano(mp + N_gas, NumPart - N_gas, sizeof(struct peano_hilbert_data), peano_compare_key);
#else
      qsort(mp + N_gas, NumPart - N_gas, sizeof(struct peano_hilbert_data), peano_compare_key);
#endif

      for(i = N_gas; i < NumPart; i++)
	Id[mp[i].index] = i;

      reorder_particles();

      Id += N_gas;
      myfree(Id);
      mp += N_gas;
      myfree(mp);
    }

  if(ThisTask == 0)
    printf("Peano-Hilbert done.\n");
}

int peano_compare_key(const void *a, const void *b)
{
  if(((struct peano_hilbert_data *) a)->key < (((struct peano_hilbert_data *) b)->key))
    return -1;

  if(((struct peano_hilbert_data *) a)->key > (((struct peano_hilbert_data *) b)->key))
    return +1;

  return 0;
}

void reorder_gas(void)
{
  int i;
  struct particle_data Psave, Psource;
  struct sph_particle_data SphPsave, SphPsource;
  int idsource, idsave, dest;

  for(i = 0; i < N_gas; i++)
    {
      if(Id[i] != i)
	{
	  Psource = P[i];
	  SphPsource = SphP[i];

	  idsource = Id[i];
	  dest = Id[i];

	  do
	    {
	      Psave = P[dest];
	      SphPsave = SphP[dest];
	      idsave = Id[dest];

	      P[dest] = Psource;
	      SphP[dest] = SphPsource;
	      Id[dest] = idsource;

	      if(dest == i)
		break;

	      Psource = Psave;
	      SphPsource = SphPsave;
	      idsource = idsave;

	      dest = idsource;
	    }
	  while(1);
	}
    }
}

void reorder_particles(void)
{
  int i;
  struct particle_data Psave, Psource;
  int idsource, idsave, dest;

  for(i = N_gas; i < NumPart; i++)
    {
      if(Id[i] != i)
	{
	  Psource = P[i];
	  idsource = Id[i];

	  dest = Id[i];

	  do
	    {
	      Psave = P[dest];
	      idsave = Id[dest];

	      P[dest] = Psource;
	      Id[dest] = idsource;
#ifdef LT_STELLAREVOLUTION
	      if(P[dest].Type == 4)
		MetP[P[dest].MetID].PID = dest;
#endif
	      if(dest == i)
		break;

	      Psource = Psave;
	      idsource = idsave;

	      dest = idsource;
	    }
	  while(1);
	}
    }
}

/*  The following rewrite of the original function
 *  peano_hilbert_key_old() has been written by MARTIN REINECKE.
 *  It is about a factor 2.3 - 2.5 faster than Volker's old routine!
 */
const unsigned char rottable3[48][8] = {
  {36, 28, 25, 27, 10, 10, 25, 27},
  {29, 11, 24, 24, 37, 11, 26, 26},
  {8, 8, 25, 27, 30, 38, 25, 27},
  {9, 39, 24, 24, 9, 31, 26, 26},
  {40, 24, 44, 32, 40, 6, 44, 6},
  {25, 7, 33, 7, 41, 41, 45, 45},
  {4, 42, 4, 46, 26, 42, 34, 46},
  {43, 43, 47, 47, 5, 27, 5, 35},
  {33, 35, 36, 28, 33, 35, 2, 2},
  {32, 32, 29, 3, 34, 34, 37, 3},
  {33, 35, 0, 0, 33, 35, 30, 38},
  {32, 32, 1, 39, 34, 34, 1, 31},
  {24, 42, 32, 46, 14, 42, 14, 46},
  {43, 43, 47, 47, 25, 15, 33, 15},
  {40, 12, 44, 12, 40, 26, 44, 34},
  {13, 27, 13, 35, 41, 41, 45, 45},
  {28, 41, 28, 22, 38, 43, 38, 22},
  {42, 40, 23, 23, 29, 39, 29, 39},
  {41, 36, 20, 36, 43, 30, 20, 30},
  {37, 31, 37, 31, 42, 40, 21, 21},
  {28, 18, 28, 45, 38, 18, 38, 47},
  {19, 19, 46, 44, 29, 39, 29, 39},
  {16, 36, 45, 36, 16, 30, 47, 30},
  {37, 31, 37, 31, 17, 17, 46, 44},
  {12, 4, 1, 3, 34, 34, 1, 3},
  {5, 35, 0, 0, 13, 35, 2, 2},
  {32, 32, 1, 3, 6, 14, 1, 3},
  {33, 15, 0, 0, 33, 7, 2, 2},
  {16, 0, 20, 8, 16, 30, 20, 30},
  {1, 31, 9, 31, 17, 17, 21, 21},
  {28, 18, 28, 22, 2, 18, 10, 22},
  {19, 19, 23, 23, 29, 3, 29, 11},
  {9, 11, 12, 4, 9, 11, 26, 26},
  {8, 8, 5, 27, 10, 10, 13, 27},
  {9, 11, 24, 24, 9, 11, 6, 14},
  {8, 8, 25, 15, 10, 10, 25, 7},
  {0, 18, 8, 22, 38, 18, 38, 22},
  {19, 19, 23, 23, 1, 39, 9, 39},
  {16, 36, 20, 36, 16, 2, 20, 10},
  {37, 3, 37, 11, 17, 17, 21, 21},
  {4, 17, 4, 46, 14, 19, 14, 46},
  {18, 16, 47, 47, 5, 15, 5, 15},
  {17, 12, 44, 12, 19, 6, 44, 6},
  {13, 7, 13, 7, 18, 16, 45, 45},
  {4, 42, 4, 21, 14, 42, 14, 23},
  {43, 43, 22, 20, 5, 15, 5, 15},
  {40, 12, 21, 12, 40, 6, 23, 6},
  {13, 7, 13, 7, 41, 41, 22, 20}
};

const unsigned char subpix3[48][8] = {
  {0, 7, 1, 6, 3, 4, 2, 5},
  {7, 4, 6, 5, 0, 3, 1, 2},
  {4, 3, 5, 2, 7, 0, 6, 1},
  {3, 0, 2, 1, 4, 7, 5, 6},
  {1, 0, 6, 7, 2, 3, 5, 4},
  {0, 3, 7, 4, 1, 2, 6, 5},
  {3, 2, 4, 5, 0, 1, 7, 6},
  {2, 1, 5, 6, 3, 0, 4, 7},
  {6, 1, 7, 0, 5, 2, 4, 3},
  {1, 2, 0, 3, 6, 5, 7, 4},
  {2, 5, 3, 4, 1, 6, 0, 7},
  {5, 6, 4, 7, 2, 1, 3, 0},
  {7, 6, 0, 1, 4, 5, 3, 2},
  {6, 5, 1, 2, 7, 4, 0, 3},
  {5, 4, 2, 3, 6, 7, 1, 0},
  {4, 7, 3, 0, 5, 6, 2, 1},
  {6, 7, 5, 4, 1, 0, 2, 3},
  {7, 0, 4, 3, 6, 1, 5, 2},
  {0, 1, 3, 2, 7, 6, 4, 5},
  {1, 6, 2, 5, 0, 7, 3, 4},
  {2, 3, 1, 0, 5, 4, 6, 7},
  {3, 4, 0, 7, 2, 5, 1, 6},
  {4, 5, 7, 6, 3, 2, 0, 1},
  {5, 2, 6, 1, 4, 3, 7, 0},
  {7, 0, 6, 1, 4, 3, 5, 2},
  {0, 3, 1, 2, 7, 4, 6, 5},
  {3, 4, 2, 5, 0, 7, 1, 6},
  {4, 7, 5, 6, 3, 0, 2, 1},
  {6, 7, 1, 0, 5, 4, 2, 3},
  {7, 4, 0, 3, 6, 5, 1, 2},
  {4, 5, 3, 2, 7, 6, 0, 1},
  {5, 6, 2, 1, 4, 7, 3, 0},
  {1, 6, 0, 7, 2, 5, 3, 4},
  {6, 5, 7, 4, 1, 2, 0, 3},
  {5, 2, 4, 3, 6, 1, 7, 0},
  {2, 1, 3, 0, 5, 6, 4, 7},
  {0, 1, 7, 6, 3, 2, 4, 5},
  {1, 2, 6, 5, 0, 3, 7, 4},
  {2, 3, 5, 4, 1, 0, 6, 7},
  {3, 0, 4, 7, 2, 1, 5, 6},
  {1, 0, 2, 3, 6, 7, 5, 4},
  {0, 7, 3, 4, 1, 6, 2, 5},
  {7, 6, 4, 5, 0, 1, 3, 2},
  {6, 1, 5, 2, 7, 0, 4, 3},
  {5, 4, 6, 7, 2, 3, 1, 0},
  {4, 3, 7, 0, 5, 2, 6, 1},
  {3, 2, 0, 1, 4, 5, 7, 6},
  {2, 5, 1, 6, 3, 4, 0, 7}
};

/*! This function computes a Peano-Hilbert key for an integer triplet (x,y,z),
  *  with x,y,z in the range between 0 and 2^bits-1.
  */
peanokey peano_hilbert_key(int x, int y, int z, int bits)
{
  int mask;
  unsigned char rotation = 0;
  peanokey key = 0;

  for(mask = 1 << (bits - 1); mask > 0; mask >>= 1)
    {
      unsigned char pix = ((x & mask) ? 4 : 0) | ((y & mask) ? 2 : 0) | ((z & mask) ? 1 : 0);

      key <<= 3;
      key |= subpix3[rotation][pix];
      rotation = rottable3[rotation][pix];
    }

  return key;
}

peanokey morton_key(int x, int y, int z, int bits)
{
  int mask;
  peanokey morton = 0;

  for(mask = 1 << (bits - 1); mask > 0; mask >>= 1)
    {
      morton <<= 3;
      morton += ((z & mask) ? 4 : 0) + ((y & mask) ? 2 : 0) + ((x & mask) ? 1 : 0);
    }

  return morton;
}

peanokey peano_and_morton_key(int x, int y, int z, int bits, peanokey * morton_key)
{
  int mask;
  unsigned char rotation = 0;
  peanokey key = 0;
  peanokey morton = 0;

  for(mask = 1 << (bits - 1); mask > 0; mask >>= 1)
    {
      unsigned char pix = ((x & mask) ? 4 : 0) | ((y & mask) ? 2 : 0) | ((z & mask) ? 1 : 0);

      key <<= 3;
      key |= subpix3[rotation][pix];
      rotation = rottable3[rotation][pix];

      morton <<= 3;
      morton += ((z & mask) ? 4 : 0) + ((y & mask) ? 2 : 0) + ((x & mask) ? 1 : 0);
    }

  *morton_key = morton;

  return key;
}

static int quadrants[24][2][2][2] = {
  /* rotx=0, roty=0-3 */
  {{{0, 7}, {1, 6}}, {{3, 4}, {2, 5}}},
  {{{7, 4}, {6, 5}}, {{0, 3}, {1, 2}}},
  {{{4, 3}, {5, 2}}, {{7, 0}, {6, 1}}},
  {{{3, 0}, {2, 1}}, {{4, 7}, {5, 6}}},
  /* rotx=1, roty=0-3 */
  {{{1, 0}, {6, 7}}, {{2, 3}, {5, 4}}},
  {{{0, 3}, {7, 4}}, {{1, 2}, {6, 5}}},
  {{{3, 2}, {4, 5}}, {{0, 1}, {7, 6}}},
  {{{2, 1}, {5, 6}}, {{3, 0}, {4, 7}}},
  /* rotx=2, roty=0-3 */
  {{{6, 1}, {7, 0}}, {{5, 2}, {4, 3}}},
  {{{1, 2}, {0, 3}}, {{6, 5}, {7, 4}}},
  {{{2, 5}, {3, 4}}, {{1, 6}, {0, 7}}},
  {{{5, 6}, {4, 7}}, {{2, 1}, {3, 0}}},
  /* rotx=3, roty=0-3 */
  {{{7, 6}, {0, 1}}, {{4, 5}, {3, 2}}},
  {{{6, 5}, {1, 2}}, {{7, 4}, {0, 3}}},
  {{{5, 4}, {2, 3}}, {{6, 7}, {1, 0}}},
  {{{4, 7}, {3, 0}}, {{5, 6}, {2, 1}}},
  /* rotx=4, roty=0-3 */
  {{{6, 7}, {5, 4}}, {{1, 0}, {2, 3}}},
  {{{7, 0}, {4, 3}}, {{6, 1}, {5, 2}}},
  {{{0, 1}, {3, 2}}, {{7, 6}, {4, 5}}},
  {{{1, 6}, {2, 5}}, {{0, 7}, {3, 4}}},
  /* rotx=5, roty=0-3 */
  {{{2, 3}, {1, 0}}, {{5, 4}, {6, 7}}},
  {{{3, 4}, {0, 7}}, {{2, 5}, {1, 6}}},
  {{{4, 5}, {7, 6}}, {{3, 2}, {0, 1}}},
  {{{5, 2}, {6, 1}}, {{4, 3}, {7, 0}}}
};

static int rotxmap_table[24] = { 4, 5, 6, 7, 8, 9, 10, 11,
  12, 13, 14, 15, 0, 1, 2, 3, 17, 18, 19, 16, 23, 20, 21, 22
};

static int rotymap_table[24] = { 1, 2, 3, 0, 16, 17, 18, 19,
  11, 8, 9, 10, 22, 23, 20, 21, 14, 15, 12, 13, 4, 5, 6, 7
};

static int rotx_table[8] = { 3, 0, 0, 2, 2, 0, 0, 1 };
static int roty_table[8] = { 0, 1, 1, 2, 2, 3, 3, 0 };

static int sense_table[8] = { -1, -1, -1, +1, +1, -1, -1, -1 };

peanokey peano_hilbert_key_old(int x, int y, int z, int bits)
{
  int i, quad, bitx, bity, bitz;
  int mask, rotation, rotx, roty, sense;
  peanokey key;

  mask = 1 << (bits - 1);
  key = 0;
  rotation = 0;
  sense = 1;

  for(i = 0; i < bits; i++, mask >>= 1)
    {
      bitx = (x & mask) ? 1 : 0;
      bity = (y & mask) ? 1 : 0;
      bitz = (z & mask) ? 1 : 0;

      quad = quadrants[rotation][bitx][bity][bitz];

      key <<= 3;
      key += (sense == 1) ? (quad) : (7 - quad);

      rotx = rotx_table[quad];
      roty = roty_table[quad];
      sense *= sense_table[quad];

      while(rotx > 0)
	{
	  rotation = rotxmap_table[rotation];
	  rotx--;
	}

      while(roty > 0)
	{
	  rotation = rotymap_table[rotation];
	  roty--;
	}
    }

  return key;
}

peanokey peano_and_morton_key_old(int x, int y, int z, int bits, peanokey * morton_key)
{
  int i, quad, bitx, bity, bitz;
  int mask, rotation, rotx, roty, sense;
  peanokey key, morton;

  mask = 1 << (bits - 1);
  key = 0;
  rotation = 0;
  sense = 1;
  morton = 0;

  for(i = 0; i < bits; i++, mask >>= 1)
    {
      bitx = (x & mask) ? 1 : 0;
      bity = (y & mask) ? 1 : 0;
      bitz = (z & mask) ? 1 : 0;

      quad = quadrants[rotation][bitx][bity][bitz];

      key <<= 3;
      key += (sense == 1) ? (quad) : (7 - quad);

      rotx = rotx_table[quad];
      roty = roty_table[quad];
      sense *= sense_table[quad];

      while(rotx > 0)
	{
	  rotation = rotxmap_table[rotation];
	  rotx--;
	}

      while(roty > 0)
	{
	  rotation = rotymap_table[rotation];
	  roty--;
	}

      morton <<= 3;
      morton += (bitz << 2) + (bity << 1) + bitx;
    }

  *morton_key = morton;

  return key;
}

peanokey morton_key_old(int x, int y, int z, int bits)
{
  int i, bitx, bity, bitz;
  int mask;
  peanokey morton;

  mask = 1 << (bits - 1);
  morton = 0;

  for(i = 0; i < bits; i++, mask >>= 1)
    {
      bitx = (x & mask) ? 1 : 0;
      bity = (y & mask) ? 1 : 0;
      bitz = (z & mask) ? 1 : 0;

      morton <<= 3;
      morton += (bitz << 2) + (bity << 1) + bitx;
    }

  return morton;
}

static void msort_peano_with_tmp(struct peano_hilbert_data *b, size_t n, struct peano_hilbert_data *t)
{
  struct peano_hilbert_data *tmp;
  struct peano_hilbert_data *b1, *b2;
  size_t n1, n2;

  if(n <= 1)
    return;

  n1 = n / 2;
  n2 = n - n1;
  b1 = b;
  b2 = b + n1;

  msort_peano_with_tmp(b1, n1, t);
  msort_peano_with_tmp(b2, n2, t);

  tmp = t;

  while(n1 > 0 && n2 > 0)
    {
      if(b1->key <= b2->key)
	{
	  --n1;
	  *tmp++ = *b1++;
	}
      else
	{
	  --n2;
	  *tmp++ = *b2++;
	}
    }

  if(n1 > 0)
    memcpy(tmp, b1, n1 * sizeof(struct peano_hilbert_data));
  memcpy(b, t, (n - n2) * sizeof(struct peano_hilbert_data));
}

void mysort_peano(void *b, size_t n, size_t s, int (*cmp) (const void *, const void *))
{
  const size_t size = n * s;

  struct peano_hilbert_data *tmp = (struct peano_hilbert_data *) mymalloc(size);

  msort_peano_with_tmp((struct peano_hilbert_data *) b, n, tmp);

  myfree(tmp);
}


/*
 *  vbHmm_template.c
 *  Template for original model to be analyzed by VB-HMM.
 *
 *  Created by OKAMOTO Kenji and SAKO Yasushi
 *  Copyright 2011
 *  Cellular Informatics Laboratory, Advance Science Institute, RIKEN, Japan.
 *  All rights reserved.
 *
 *  Ver. 1.0.0
 *  Last modified on 2011.04.19
 */

#include <math.h>
#include <string.h>

// Special functions is probably necessary.
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_psi.h>

#include "vbHmm_template.h"

// For random number generation.
#include "rand.h"

#ifdef _OPENMP
#include "omp.h"
#endif

#define  MAX(a,b)  ((a)>(b)?(a):(b))
#define  MIN(a,b)  ((a)<(b)?(a):(b))

static int isGlobalAnalysis = 0;

void setFunctions__(){
    commonFunctions funcs;
    funcs.newModelParameters    = newModelParameters__;
    funcs.freeModelParameters   = freeModelParameters__;
    funcs.newModelStats         = newModelStats__;
    funcs.freeModelStats        = freeModelStats__;
    funcs.initializeVbHmm       = initializeVbHmm__;
    funcs.pTilde_z1             = pTilde_z1__;
    funcs.pTilde_zn_zn1         = pTilde_zn_zn1__;
    funcs.pTilde_xn_zn          = pTilde_xn_zn__;
    funcs.calcStatsVars         = calcStatsVars__;
    funcs.maximization          = maximization__;
    funcs.varLowerBound         = varLowerBound__;
    funcs.reorderParameters     = reorderParameters__;
    funcs.outputResults         = outputResults__;
    setFunctions( funcs );
}

void setGFunctions__(){
    gCommonFunctions funcs;
    funcs.newModelParameters    = newModelParameters__;
    funcs.freeModelParameters   = freeModelParameters__;
    funcs.newModelStats         = newModelStats__;
    funcs.freeModelStats        = freeModelStats__;
    funcs.newModelStatsG        = newModelStatsG__;
    funcs.freeModelStatsG       = freeModelStatsG__;
    funcs.initializeVbHmmG      = initializeVbHmmG__;
    funcs.pTilde_z1             = pTilde_z1__;
    funcs.pTilde_zn_zn1         = pTilde_zn_zn1__;
    funcs.pTilde_xn_zn          = pTilde_xn_zn__;
    funcs.calcStatsVarsG        = calcStatsVarsG__;
    funcs.maximizationG         = maximizationG__;
    funcs.varLowerBoundG        = varLowerBoundG__;
    funcs.reorderParametersG    = reorderParametersG__;
    funcs.outputResultsG        = outputResultsG__;
    setGFunctions( funcs );
    isGlobalAnalysis = 1;
}

// Redirect output function to that for model-specific data.
void outputResults__( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    output__Results( xn, gv, iv, logFP );
}

void outputResultsG__( xns, gv, ivs, logFP )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
FILE *logFP;
{
    output__ResultsG( xns, gv, ivs, logFP );
}

void *newModelParameters__( xn, sNo )
xnDataSet *xn;
int sNo;
{
    int i;
    tempParameters *p = (void*)malloc( sizeof(tempParameters) );

    p->uPiArr = (double*)malloc( sNo * sizeof(double) );
    p->sumUPi = 0.0;
    p->uAMat = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->uAMat[i] = (double*)malloc( sNo * sizeof(double) );
    }
    p->sumUAArr = (double*)malloc( sNo * sizeof(double) );

    p->avgPi = (double *)malloc( sNo * sizeof(double) );
    p->avgLnPi = (double *)malloc( sNo * sizeof(double) );
    p->avgA = (double **)malloc( sNo * sizeof(double*) );
    p->avgLnA = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->avgA[i] = (double *)malloc( sNo * sizeof(double) );
        p->avgLnA[i] = (double *)malloc( sNo * sizeof(double) );
    }

    return p;
}

void freeModelParameters__( p, xn, sNo )
void **p;
xnDataSet *xn;
int sNo;
{
    tempParameters *gp = *p;
    int i;

    free( gp->uPiArr );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->uAMat[i] );
    }
    free( gp->uAMat );
    free( gp->sumUAArr );

    free( gp->avgPi );
    free( gp->avgLnPi );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->avgA[i] );
        free( gp->avgLnA[i] );
    }
    free( gp->avgA );
    free( gp->avgLnA );

    free( *p );
    *p = NULL;
}

void *newModelStats__( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    if( isGlobalAnalysis == 0 ){
        int sNo = gv->sNo;
        tempStats *s = (tempStats*)malloc( sizeof(tempStats) );

        int i;
        s->Ni = (double *)malloc( sNo * sizeof(double) );
        s->Nij = (double **)malloc( sNo * sizeof(double*) );
        for( i = 0 ; i < sNo ; i++ )
        {   s->Nij[i] = (double *)malloc( sNo * sizeof(double) );   }
        s->Nii = (double *)malloc( sNo * sizeof(double) );

        return s;

    } else {

        return NULL;

    }
}

void freeModelStats__( s, xn, gv, iv )
void **s;
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    if( isGlobalAnalysis == 0 ){
        int sNo = gv->sNo;
        tempStats *gs = *s;
        int i;
        free( gs->Ni );
        for( i = 0 ; i < sNo ; i++ )
        {   free( gs->Nij[i] );   }
        free( gs->Nij );
        free( gs->Nii );

        free( gs );
        *s = NULL;
    }
}

void *newModelStatsG__( xns, gv, ivs)
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    int sNo = gv->sNo;
    tempGlobalStats *gs = (tempGlobalStats*)malloc( sizeof(tempGlobalStats) );

    int i;
    gs->NiR = (double *)malloc( sNo * sizeof(double) );
    gs->NijR = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   gs->NijR[i] = (double *)malloc( sNo * sizeof(double) );   }
    gs->NiiR = (double *)malloc( sNo * sizeof(double) );
    gs->z1iR = (double *)malloc( sNo * sizeof(double) );

    return gs;
}

void freeModelStatsG__( gs, xns, gv, ivs )
void **gs;
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    int sNo = gv->sNo;
    tempGlobalStats *ggs = *gs;
    int i;
    free( ggs->NiR );
    for( i = 0 ; i < sNo ; i++ )
    {   free( ggs->NijR[i] );   }
    free( ggs->NijR );
    free( ggs->NiiR );
    free( ggs->z1iR );

    free( *gs );
    *gs = NULL;
}

void initializeVbHmm__( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    // an example of hyperparameter initialization

    int sNo = gv->sNo;
    tempParameters *p = gv->params;

    int i, j;
    // hyper parameter for p( pi(i) )
    p->sumUPi = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        p->uPiArr[i] = 1.0;
        p->sumUPi += p->uPiArr[i];
    }

    // hyper parameter for p( A(i,j) )
    for( i = 0 ; i < sNo ; i++ ){
        p->sumUAArr[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            if( j == i ){
                p->uAMat[i][j] = 5.0;
            } else {
                p->uAMat[i][j] = 1.0;
            }
            p->sumUAArr[i] += p->uAMat[i][j];
        }
    }

    initialize_indVars__( xn, gv, iv );

    calcStatsVars__( xn, gv, iv );
    maximization__( xn, gv, iv );
}

void initializeVbHmmG__( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    // an example of hyperparameter initialization

    int sNo = gv->sNo, rNo = xns->R;
    tempParameters *p = gv->params;

    int i, j, r;

    p->sumUPi = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        p->uPiArr[i] = 1.0;
        p->sumUPi += p->uPiArr[i];
    }

    for( i = 0 ; i < sNo ; i++ ){
        p->sumUAArr[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            if( j == i ){
                p->uAMat[i][j] = 5.0;
            } else {
                p->uAMat[i][j] = 1.0;
            }
            p->sumUAArr[i] += p->uAMat[i][j];
        }
    }

    for( r = 0 ; r < rNo ; r++ ){
        initialize_indVars__( xns->xn[r], gv, ivs->indVars[r] );
    }

    calcStatsVarsG__( xns, gv, ivs );
    maximizationG__( xns, gv, ivs );
}

void initialize_indVars__( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    // an example of latent variable distribution

    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;

    int i;
    size_t n;
    double sumPar;
    for( n = 0 ; n < dLen ; n++ ){
        sumPar = 0.0;
        for( i = 0 ; i < sNo ; i++ ){
            gmMat[n][i] = enoise(1.0) + 1.0;
            sumPar += gmMat[n][i];
        }
        for( i = 0 ; i < sNo ; i++ ){
            gmMat[n][i] /= sumPar;
        }
    }
}

xnDataSet *newXnDataSet__( filename )
const char *filename;
{
    xnDataSet *xn = (xnDataSet*)malloc( sizeof(xnDataSet) );
    xn->name = (char*)malloc( strlen(filename) + 2 );
    strncpy( xn->name, filename, strlen(filename)+1 );
    xn->data = (tempData*)malloc( sizeof(tempData) );
    tempData *d = (tempData*)xn->data;
    return xn;
}

void freeXnDataSet__( xn )
xnDataSet **xn;
{
    tempData *d = (tempData*)(*xn)->data;
    free( (*xn)->data );
    free( (*xn)->name );
    free( *xn );
    *xn = NULL;
}

double pTilde_z1__( i, params )
int i;
void *params;
{
    // Return the value for p(z_{1i}|pi_i) for given i.

    // for common HMM
    tempParameters *p = (tempParameters*)params;
    return exp( p->avgLnPi[i] );
}

double pTilde_zn_zn1__( i, j, params )
int i, j;
void *params;
{
    // Return the value for p(z_{nj}|z_{n-1,i},theta) for given i and j.

    // for common HMM
    tempParameters *p = (tempParameters*)params;
    return exp( p->avgLnA[i][j] );
}

double pTilde_xn_zn__( xnWv, n, i, params )
xnDataSet *xnWv;
size_t n;
int i;
void *params;
{
    // Return the value for p(x_{mi}|z_{ni},phi) for given i.

    // return emission probability for the model.
    tempParameters *p = (tempParameters*)params;
    return 0.0;
}

void calcStatsVars__( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    // Execute calculation of statistics, which may be necessary for M-step.

    // calculation of stats for common HMM
    tempStats *s = (tempStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *Nii = s->Nii, **Nij = s->Nij, *Ni = s->Ni;
    size_t n;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        Ni[i]   = 1e-10;
        Nii[i]  = 1e-10;
        for( j = 0 ; j < sNo ; j++ ){
            Nij[i][j] = 1e-10;
        }

        for( n = 0 ; n < dLen ; n++ ){
            Ni[i]   += gmMat[n][i];
            for( j = 0 ; j < sNo ; j++ ){
                Nii[i]    += xiMat[n][i][j];
                Nij[i][j] += xiMat[n][i][j];
            }
        }
    }
}

void calcStatsVarsG__( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    // Execute calculation of statistics for global analysis, which may be necessary for M-step.

    // calculation of stats for global analysis of common HMM
    tempGlobalStats *gs = (tempGlobalStats*)ivs->stats;
    int sNo = gv->sNo, rNo = xns->R;
    double **gmMat, ***xiMat;
    double *NiiR = gs->NiiR, **NijR = gs->NijR, *NiR = gs->NiR, *z1iR = gs->z1iR;
    size_t dLen, n;
    int i, j, r;

    for( i = 0 ; i < sNo ; i++ ){
        NiR[i]   = 1e-10;
        NiiR[i]  = 1e-10;
        for( j = 0 ; j < sNo ; j++ ){
            NijR[i][j] = 1e-10;
        }
        z1iR[i] = 1e-10;
    }
    for( r = 0 ; r < rNo ; r++ ){
        dLen = xns->xn[r]->N;
        gmMat = ivs->indVars[r]->gmMat;
        xiMat = ivs->indVars[r]->xiMat;
        for( i = 0 ; i < sNo ; i++ ){
            z1iR[i] += gmMat[0][i];
            for( n = 0 ; n < dLen ; n++ ){
                NiR[i]   += gmMat[n][i];
                for( j = 0 ; j < sNo ; j++ ){
                    NiiR[i]  += xiMat[n][i][j];
                    NijR[i][j] += xiMat[n][i][j];
                }
            }
        }
    }
}

void maximization__( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    // Calculation of M-step, depending on the model.

    // calculation of M-step for common HMM
    tempParameters *p = (tempParameters*)gv->params;
    tempStats *s = (tempStats*)iv->stats;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *Nii = s->Nii, **Nij = s->Nij;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        avgPi[i] = ( uPiArr[i] + gmMat[0][i] ) / ( sumUPi + 1.0 );
        avgLnPi[i] = gsl_sf_psi( uPiArr[i] + gmMat[0][i] ) - gsl_sf_psi( sumUPi + 1.0 );

        for( j = 0 ; j < sNo ; j++ ){
            avgA[i][j] = ( uAMat[i][j] + Nij[i][j] ) / ( sumUAArr[i] + Nii[i] );
            avgLnA[i][j] = gsl_sf_psi( uAMat[i][j] + Nij[i][j] ) - gsl_sf_psi( sumUAArr[i] + Nii[i] );
        }
    }
}

void maximizationG__( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    // Calculation of M-step for global analysis, depending on the model.

    // calculation of stats for global analysis of common HMM
    tempParameters *p = (tempParameters*)gv->params;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    tempGlobalStats *gs = (tempGlobalStats*)ivs->stats;
    double *NiiR = gs->NiiR, **NijR = gs->NijR, *z1iR = gs->z1iR, dR = (double)(xns->R);
    int sNo = gv->sNo;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        avgPi[i] = ( uPiArr[i] + z1iR[i] ) / ( sumUPi + dR );
        avgLnPi[i] = gsl_sf_psi( uPiArr[i] + z1iR[i] ) - gsl_sf_psi( sumUPi + dR );

        for( j = 0 ; j < sNo ; j++ ){
            avgA[i][j] = ( uAMat[i][j] + NijR[i][j] ) / ( sumUAArr[i] + NiiR[i] );
            avgLnA[i][j] = gsl_sf_psi( uAMat[i][j] + NijR[i][j] ) - gsl_sf_psi( sumUAArr[i] + NiiR[i] );
        }
    }
}

double varLowerBound__( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    // Calculation of lower bound, depending on the model.

    // calculation of lower bound of common HMM
    tempParameters *p = (tempParameters*)gv->params;
    tempStats *s = (tempStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, *cn = iv->cn;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *avgLnPi = p->avgLnPi, **avgLnA = p->avgLnA;
    double *Nii = s->Nii, **Nij = s->Nij;
    size_t n;
    int i, j;

    double lnpPi = gsl_sf_lngamma(sumUPi);
    double lnpA = 0.0;
    double lnpPhi = 0.0;            // ln p(phi) for model-specific parameters
    double lnqPi = gsl_sf_lngamma(sumUPi + 1.0);
    double lnqA = 0.0;
    double lnqPhi = 0.0;            // ln q(phi) for model-specific parameters
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1.0) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnqPi += (uPiArr[i]+gmMat[0][i]-1.0) * (gsl_sf_psi(uPiArr[i]+gmMat[0][i]) - gsl_sf_psi(sumUPi+1.0));
        lnqPi -= gsl_sf_lngamma(uPiArr[i] + gmMat[0][i]);

        lnpA += gsl_sf_lngamma(sumUAArr[i]);
        lnqA += gsl_sf_lngamma(sumUAArr[i] + Nii[i]);
        for( j = 0 ; j < sNo ; j++ ){
            lnpA += (uAMat[i][j]-1.0)*avgLnA[i][j] - gsl_sf_lngamma(uAMat[i][j]);

            lnqA += (uAMat[i][j] + Nij[i][j] - 1.0) * (gsl_sf_psi(uAMat[i][j]+Nij[i][j]) - gsl_sf_psi(sumUAArr[i]+Nii[i]));
            lnqA -= gsl_sf_lngamma( uAMat[i][j] + Nij[i][j] );
        }
    }

    double lnpX = 0.0;
    for( n = 0 ; n < dLen ; n++ ){
        lnpX += log( cn[n] );
    }

    double val;
    val  = lnpPi + lnpA + lnpPhi;
    val -= lnqPi + lnqA + lnqPhi;
    val += lnpX;
    val += log(gsl_sf_fact(sNo));

    return val;
}

double varLowerBoundG__( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    // Calculation of lower bound for global analysis, depending on the model.

    // calculation of lower bound for global analysis of common HMM
    int sNo = gv->sNo, rNo = xns->R;
    tempParameters *p = (tempParameters*)gv->params;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *avgLnPi = p->avgLnPi, **avgLnA = p->avgLnA;
    tempGlobalStats *gs = (tempGlobalStats*)ivs->stats;
    double *NiiR = gs->NiiR, **NijR = gs->NijR, *z1iR = gs->z1iR, dR = (double)xns->R;
    size_t n;
    int i, j, r;

    double lnpPi = gsl_sf_lngamma(sumUPi);
    double lnpA = 0.0;
    double lnpPhi = 0.0;            // ln p(phi) for model-specific parameters
    double lnqPi = gsl_sf_lngamma(sumUPi + dR);
    double lnqA = 0.0;
    double lnqPhi = 0.0;            // ln q(phi) for model-specific parameters
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1.0) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnqPi += (uPiArr[i]+z1iR[i]-1.0) * (gsl_sf_psi(uPiArr[i]+z1iR[i]) - gsl_sf_psi(sumUPi+dR));
        lnqPi -= gsl_sf_lngamma(uPiArr[i] + z1iR[i]);

        lnpA += gsl_sf_lngamma(sumUAArr[i]);
        lnqA += gsl_sf_lngamma(sumUAArr[i] + NiiR[i]);
        for( j = 0 ; j < sNo ; j++ ){
            lnpA += (uAMat[i][j]-1.0)*avgLnA[i][j] - gsl_sf_lngamma(uAMat[i][j]);

            lnqA += (uAMat[i][j] + NijR[i][j] - 1.0) * (gsl_sf_psi(uAMat[i][j]+NijR[i][j]) - gsl_sf_psi(sumUAArr[i]+NiiR[i]));
            lnqA -= gsl_sf_lngamma( uAMat[i][j] + NijR[i][j] );
        }
    }

    double lnpX = 0.0;
    for( r = 0 ; r < rNo ; r++ ){
        size_t dLen = xns->xn[r]->N;
        for( n = 0 ; n < dLen ; n++ ){
            lnpX += log( ivs->indVars[r]->cn[n] );
        }
    }

    double val;
    val  = lnpPi + lnpA + lnpPhi;
    val -= lnqPi + lnqA + lnqPhi;
    val += lnpX;
    val += log(gsl_sf_fact(sNo));

    return val;
}

void reorderParameters__( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    // Reorder states in the final result, for example, in the order of intensity.

    // an example for common HMM.
    tempParameters *p = (tempParameters*)gv->params;
    tempStats *s = (tempStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *Ni = s->Ni;
    size_t n;
    int i, j;

    int *index = (int*)malloc( sNo * sizeof(int) );
    double *store = (double*)malloc( sNo * sizeof(double) );
    double **s2D = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   s2D[i] = (double*)malloc( MAX(sNo,2) * sizeof(double) );   }

    // index indicates order of avgPi values (0=biggest avgMu -- sNo=smallest avgMu).
    for( i = 0 ; i < sNo ; i++ ){
        index[i] = sNo - 1;
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                if( avgPi[i] < avgPi[j] ){
                    index[i]--;
                } else if( avgPi[i] == avgPi[j] ){
                    if( j > i )
                    {   index[i]--;   }
                }
            }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgPi[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnPi[i] = store[i];   }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgA[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgA[i][j] = s2D[i][j];   }
    }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgLnA[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgLnA[i][j] = s2D[i][j];   }
    }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ni[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ni[i] = store[i];   }

    for( n = 0 ; n < dLen ; n++ ){
        for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = gmMat[n][i];   }
        for( i = 0 ; i < sNo ; i++ ){   gmMat[n][i] = store[i];   }
    }

    for( n = 0 ; n < dLen ; n++ ){
        for( j = 0 ; j < sNo ; j++ ){
            for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = xiMat[n][i][j];   }
        }
        for( j = 0 ; j < sNo ; j++ ){
            for( i = 0 ; i < sNo ; i++ ){   xiMat[n][i][j] = s2D[i][j];   }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   free( s2D[i] );   }
    free( s2D );
    free( store );
    free( index );
}

void reorderParametersG__( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    // Reorder states in the final result, for example, in the order of intensity.

    // an example for global analysis of common HMM.
    size_t dLen;
    int sNo = gv->sNo, rNo = xns->R;
    tempParameters *p = (tempParameters*)gv->params;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;

    size_t n;
    int i, j, r;

    int *index = (int*)malloc( sNo * sizeof(int) );
    double *store = (double*)malloc( sNo * sizeof(double) );
    double **s2D = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   s2D[i] = (double*)malloc( MAX(sNo,2) * sizeof(double) );   }

    // index indicates order of avgPi values (0=biggest avgMu -- sNo=smallest avgMu).
    for( i = 0 ; i < sNo ; i++ ){
        index[i] = sNo - 1;
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                if( avgPi[i] < avgPi[j] ){
                    index[i]--;
                } else if( avgPi[i] == avgPi[j] ){
                    if( j > i )
                    {   index[i]--;   }
                }
            }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgPi[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnPi[i] = store[i];   }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgA[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgA[i][j] = s2D[i][j];   }
    }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgLnA[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgLnA[i][j] = s2D[i][j];   }
    }

    double *NiR = ((tempGlobalStats*)ivs->stats)->NiR;
    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = NiR[i];   }
    for( i = 0 ; i < sNo ; i++ ){   NiR[i] = store[i];   }

    for( r = 0 ; r < rNo ; r++ ){
        double **gmMat = ivs->indVars[r]->gmMat;
        dLen = xns->xn[r]->N;
        for( n = 0 ; n < dLen ; n++ ){
            for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = gmMat[n][i];   }
            for( i = 0 ; i < sNo ; i++ ){   gmMat[n][i] = store[i];   }
        }
    }

    for( r = 0 ; r < rNo ; r++ ){
        double ***xiMat = ivs->indVars[r]->xiMat;
        dLen = xns->xn[r]->N;
        for( n = 0 ; n < dLen ; n++ ){
            for( j = 0 ; j < sNo ; j++ ){
                for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = xiMat[n][i][j];   }
            }
            for( j = 0 ; j < sNo ; j++ ){
                for( i = 0 ; i < sNo ; i++ ){   xiMat[n][i][j] = s2D[i][j];   }
            }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   free( s2D[i] );   }
    free( s2D );
    free( store );
    free( index );
}

void output__Results( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    // Output the final result (after reordering) in any format, e.g. stdout, stderr or files.

    // an example for common HMM parameters.
    tempParameters *p = (tempParameters*)gv->params;
    int sNo = gv->sNo;

    int i, j;
    fprintf(logFP, "  results: K = %d \n", sNo);

    fprintf(logFP, "   pi: ( %g", p->avgPi[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgPi[i]);
    }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   A_matrix: [");
    for( i = 0 ; i < sNo ; i++ ){
        fprintf(logFP, " ( %g", p->avgA[i][0]);
        for( j = 1 ; j < sNo ; j++ )
        {   fprintf(logFP, ", %g", p->avgA[i][j]);   }
        fprintf(logFP, ")");
    }
    fprintf(logFP, " ] \n\n");

    char fn[256];
    FILE *fp;
    size_t n;

    sprintf( fn, "%s.param%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        fprintf(fp, "pi");
        for( i = 0 ; i < sNo ; i++ )
        {   fprintf(fp, ", A%dx", i);   }
        fprintf(fp, "\n");

        for( i = 0 ; i < sNo ; i++ ){
            fprintf(fp, "%g", p->avgPi[i]);
            for( j = 0 ; j < sNo ; j++ )
            {   fprintf(fp, ", %g", p->avgA[j][i]);   }
            fprintf(fp, "\n");
        }
        fclose(fp);
    }

    sprintf( fn, "%s.Lq%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        for( n = 0 ; n < gv->iteration ; n++ ){
            fprintf( fp, "%24.20e\n", gv->LqArr[n] );
        }
        fclose(fp);
    }

    sprintf( fn, "%s.maxS%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        for( n = 0 ; n < xn->N ; n++ ){
            fprintf( fp, "%d\n", iv->stateTraj[n] );
        }
        fclose(fp);
    }

}

void output__ResultsG( xns, gv, ivs, logFP )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
FILE *logFP;
{
    // Output the final result (after reordering) in any format, e.g. stdout, stderr or files.

    // an example for parameters of global analysis of common HMM.
    int sNo = gv->sNo, rNo = xns->R;
    tempParameters *p = (tempParameters*)gv->params;
    int i, j, r;

    fprintf(logFP, "  results: K = %d \n", sNo);

    fprintf(logFP, "   pi: ( %g", p->avgPi[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgPi[i]);
    }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   A_matrix: [");
    for( i = 0 ; i < sNo ; i++ ){
        fprintf(logFP, " ( %g", p->avgA[i][0]);
        for( j = 1 ; j < sNo ; j++ )
        {   fprintf(logFP, ", %g", p->avgA[i][j]);   }
        fprintf(logFP, ")");
    }
    fprintf(logFP, " ] \n\n");

    char fn[256];
    FILE *fp;
    size_t n;

    sprintf( fn, "%s.param%03d", xns->xn[0]->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        fprintf(fp, "pi");
        for( i = 0 ; i < sNo ; i++ )
        {   fprintf(fp, ", A%dx", i);   }
        fprintf(fp, "\n");

        for( i = 0 ; i < sNo ; i++ ){
            fprintf(fp, "%g", p->avgPi[i]);
            for( j = 0 ; j < sNo ; j++ )
            {   fprintf(fp, ", %g", p->avgA[j][i]);   }
            fprintf(fp, "\n");
        }
        fclose(fp);
    }

    sprintf( fn, "%s.Lq%03d", xns->xn[0]->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        for( n = 0 ; n < gv->iteration ; n++ ){
            fprintf( fp, "%24.20e\n", gv->LqArr[n] );
        }
        fclose(fp);
    }

    for( r = 0 ; r < rNo ; r++ ){
        xnDataSet *xn = xns->xn[r];
        indVars *iv = ivs->indVars[r];

        sprintf( fn, "%s.maxS%03d", xn->name, sNo );
        if( (fp = fopen( fn, "w")) != NULL ){
            for( n = 0 ; n < xn->N ; n++ ){
                fprintf( fp, "%d\n", iv->stateTraj[n] );
            }
            fclose(fp);
        }
    }
}

//


/* specfunc/bessel_I0.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_bessel.h"

#include "error.h"

#include "chebyshev.h"
#include "cheb_eval.c"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* based on SLATEC besi0 */

/* chebyshev expansions

 series for bi0        on the interval  0.	    to  9.00000d+00
					with weighted error   2.46e-18
					 log weighted error  17.61
			       significant figures required  17.90
				    decimal places required  18.15

 series for ai0        on the interval  1.25000d-01 to  3.33333d-01
					with weighted error   7.87e-17
					 log weighted error  16.10
			       significant figures required  14.69
				    decimal places required  16.76

 series for ai02       on the interval  0.	    to  1.25000d-01
					with weighted error   3.79e-17
					 log weighted error  16.42
			       significant figures required  14.86
				    decimal places required  17.09
*/

static double bi0_data[12] = {
  -.07660547252839144951,
  1.92733795399380827000,
   .22826445869203013390,
   .01304891466707290428,
   .00043442709008164874,
   .00000942265768600193,
   .00000014340062895106,
   .00000000161384906966,
   .00000000001396650044,
   .00000000000009579451,
   .00000000000000053339,
   .00000000000000000245
};
static cheb_series bi0_cs = {
  bi0_data,
  11,
  -1, 1,
  11
};

static double ai0_data[21] = {
   .07575994494023796,
   .00759138081082334,
   .00041531313389237,
   .00001070076463439,
  -.00000790117997921,
  -.00000078261435014,
   .00000027838499429,
   .00000000825247260,
  -.00000001204463945,
   .00000000155964859,
   .00000000022925563,
  -.00000000011916228,
   .00000000001757854,
   .00000000000112822,
  -.00000000000114684,
   .00000000000027155,
  -.00000000000002415,
  -.00000000000000608,
   .00000000000000314,
  -.00000000000000071,
   .00000000000000007
};
static cheb_series ai0_cs = {
  ai0_data,
  20,
  -1, 1,
  13
};

static double ai02_data[22] = {
   .05449041101410882,
   .00336911647825569,
   .00006889758346918,
   .00000289137052082,
   .00000020489185893,
   .00000002266668991,
   .00000000339623203,
   .00000000049406022,
   .00000000001188914,
  -.00000000003149915,
  -.00000000001321580,
  -.00000000000179419,
   .00000000000071801,
   .00000000000038529,
   .00000000000001539,
  -.00000000000004151,
  -.00000000000000954,
   .00000000000000382,
   .00000000000000176,
  -.00000000000000034,
  -.00000000000000027,
   .00000000000000003
};
static cheb_series ai02_cs = {
  ai02_data,
  21,
  -1, 1,
  11
};

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int gsl_sf_bessel_I0_scaled_e(const double x, gsl_sf_result * result)
{
  double y = fabs(x);

  /* CHECK_POINTER(result) */

  if(y < 2.0 * GSL_SQRT_DBL_EPSILON) {
    result->val = 1.0 - y;
    result->err = 0.5*y*y;
    return GSL_SUCCESS;
  }
  else if(y <= 3.0) {
    const double ey = exp(-y);
    gsl_sf_result c;
    cheb_eval_e(&bi0_cs, y*y/4.5-1.0, &c);
    result->val = ey * (2.75 + c.val);
    result->err = GSL_DBL_EPSILON * fabs(result->val) + ey * c.err;
    return GSL_SUCCESS;
  }
  else if(y <= 8.0) {
    const double sy = sqrt(y);
    gsl_sf_result c;
    cheb_eval_e(&ai0_cs, (48.0/y-11.0)/5.0, &c);
    result->val  = (0.375 + c.val) / sy;
    result->err  = 2.0 * GSL_DBL_EPSILON * (0.375 + fabs(c.val)) / sy;
    result->err += c.err / sy;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    const double sy = sqrt(y);
    gsl_sf_result c;
    cheb_eval_e(&ai02_cs, 16.0/y-1.0, &c);
    result->val = (0.375 + c.val) / sy;
    result->err  = 2.0 * GSL_DBL_EPSILON * (0.375 + fabs(c.val)) / sy;
    result->err += c.err / sy;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int gsl_sf_bessel_I0_e(const double x, gsl_sf_result * result)
{
  double y = fabs(x);

  /* CHECK_POINTER(result) */

  if(y < 2.0 * GSL_SQRT_DBL_EPSILON) {
    result->val = 1.0;
    result->err = 0.5*y*y;
    return GSL_SUCCESS;
  }
  else if(y <= 3.0) {
    gsl_sf_result c;
    cheb_eval_e(&bi0_cs, y*y/4.5-1.0, &c);
    result->val  = 2.75 + c.val;
    result->err  = GSL_DBL_EPSILON * (2.75 + fabs(c.val));
    result->err += c.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(y < GSL_LOG_DBL_MAX - 1.0) {
    const double ey = exp(y);
    gsl_sf_result b_scaled;
    gsl_sf_bessel_I0_scaled_e(x, &b_scaled);
    result->val  = ey * b_scaled.val;
    result->err  = ey * b_scaled.err + y*GSL_DBL_EPSILON*fabs(result->val);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    OVERFLOW_ERROR(result);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_bessel_I0_scaled(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_I0_scaled_e(x, &result); )
}

double gsl_sf_bessel_I0(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_I0_e(x, &result); )
}


#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpRNG.h>
#include <gbpMCMC.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_interp.h>

void init_MCMC(MCMC_info * MCMC,
               const char *problem_name,
               void *      params,
               int (*f)(double *, MCMC_info *, double **),
               int     n_P,
               double *P_init,
               char ** P_names,
               double *P_limit_min,
               double *P_limit_max,
               int     n_arrays,
               ...) {
    int     i_P;
    int     i_array;
    int     i;
    FILE *  ft, *ft_restart;
    char    test_dir[256], test_restart[256];
    va_list vargs;
    va_start(vargs, n_arrays);

    SID_log("Initializing MCMC structure...", SID_LOG_OPEN);

    // Set defaults to bare minimums
    MCMC->n_avg                 = 100;
    MCMC->n_iterations_burn     = 4;
    MCMC->n_iterations          = 8;
    MCMC->n_thin                = 1;
    MCMC->coverage_size         = 100;
    MCMC->flag_autocor_on       = GBP_FALSE;
    MCMC->flag_integrate_on     = GBP_FALSE;
    MCMC->flag_analysis_on      = GBP_TRUE;
    MCMC->first_map_call        = GBP_TRUE;
    MCMC->first_link_call       = GBP_TRUE;
    MCMC->flag_init_chain       = GBP_TRUE;
    MCMC->first_chain_call      = GBP_TRUE;
    MCMC->first_parameter_call  = GBP_TRUE;
    MCMC->first_likelihood_call = GBP_TRUE;
    MCMC->ln_likelihood_last    = 0.;
    MCMC->ln_likelihood_new     = 0.;
    MCMC->ln_likelihood_chain   = 0.;
    MCMC->P_init                = NULL;
    MCMC->P_new                 = NULL;
    MCMC->P_last                = NULL;
    MCMC->P_chain               = NULL;
    MCMC->P_limit_min           = NULL;
    MCMC->P_limit_max           = NULL;
    MCMC->P_min                 = NULL;
    MCMC->P_max                 = NULL;
    MCMC->P_avg                 = NULL;
    MCMC->dP_avg                = NULL;
    MCMC->P_best                = NULL;
    MCMC->P_peak                = NULL;
    MCMC->P_lo_68               = NULL;
    MCMC->P_hi_68               = NULL;
    MCMC->P_lo_95               = NULL;
    MCMC->P_hi_95               = NULL;
    MCMC->n_M                   = NULL;
    MCMC->M_new                 = NULL;
    MCMC->M_last                = NULL;
    MCMC->DS                    = NULL;
    MCMC->last                  = NULL;
    MCMC->V                     = NULL;
    MCMC->m                     = NULL;
    MCMC->b                     = NULL;
    MCMC->RNG                   = NULL;
    MCMC->params                = NULL;
    MCMC->temperature           = 1.0;
    MCMC->n_DS                  = 0;
    MCMC->n_M_total             = 0;
    MCMC->n_fail                = 0;
    MCMC->n_success             = 0;
    MCMC->n_propositions        = 0;
    MCMC->n_map_calls           = 0;

    // Process the passed arguments
    MCMC->map_P_to_M                 = f;
    MCMC->compute_MCMC_ln_likelihood = compute_MCMC_ln_likelihood_default;
    MCMC->params                     = params;
    MCMC->n_P                        = n_P;
    sprintf(MCMC->problem_name, "%s", problem_name);
    MCMC->P_names       = (char **)SID_malloc(sizeof(char *) * MCMC->n_P);
    MCMC->P_name_length = 0;
    for(i_P = 0; i_P < n_P; i_P++) {
        MCMC->P_names[i_P] = (char *)SID_malloc(sizeof(char) * MCMC_NAME_SIZE);
        sprintf(MCMC->P_names[i_P], "%s", P_names[i_P]);
        MCMC->P_name_length = GBP_MAX((size_t)(MCMC->P_name_length), strlen(MCMC->P_names[i_P]));
    }
    sprintf(MCMC->P_name_format, "%%-%ds", MCMC->P_name_length);

    // Initialize the MCMC mode and set things associated with it
    set_MCMC_mode(MCMC, MCMC_MODE_DEFAULT); // MCMC->my_chain is set here

    // Set parameter arrays and limits
    MCMC->P_init      = (double *)SID_malloc(sizeof(double) * MCMC->n_P);
    MCMC->P_new       = (double *)SID_malloc(sizeof(double) * MCMC->n_P);
    MCMC->P_last      = (double *)SID_malloc(sizeof(double) * MCMC->n_P);
    MCMC->P_chain     = (double *)SID_malloc(sizeof(double) * MCMC->n_P);
    MCMC->P_limit_min = (double *)SID_malloc(sizeof(double) * MCMC->n_P);
    MCMC->P_limit_max = (double *)SID_malloc(sizeof(double) * MCMC->n_P);
    if(P_limit_min == NULL) {
        for(i_P = 0; i_P < n_P; i_P++)
            MCMC->P_limit_min[i_P] = -DBL_MAX * 1e-3;
    } else {
        for(i_P = 0; i_P < n_P; i_P++)
            MCMC->P_limit_min[i_P] = P_limit_min[i_P];
    }
    if(P_limit_max == NULL) {
        for(i_P = 0; i_P < n_P; i_P++)
            MCMC->P_limit_max[i_P] = DBL_MAX * 1e-3;
    } else {
        for(i_P = 0; i_P < n_P; i_P++)
            MCMC->P_limit_max[i_P] = P_limit_max[i_P];
    }

    // Set parameter initial values
    memcpy(MCMC->P_init, P_init, (size_t)MCMC->n_P * sizeof(double));
    memcpy(MCMC->P_new, P_init, (size_t)MCMC->n_P * sizeof(double));
    memcpy(MCMC->P_last, P_init, (size_t)MCMC->n_P * sizeof(double));
    memcpy(MCMC->P_chain, P_init, (size_t)MCMC->n_P * sizeof(double));

    // Set arrays
    MCMC->n_arrays = n_arrays;
    if(n_arrays > 0) {
        MCMC->array      = (double **)SID_malloc(sizeof(double *) * MCMC->n_arrays);
        MCMC->array_name = (char **)SID_malloc(sizeof(char *) * MCMC->n_arrays);
        for(i_array = 0; i_array < n_arrays; i_array++) {
            MCMC->array[i_array]      = (double *)SID_malloc(sizeof(double) * MCMC->n_P);
            MCMC->array_name[i_array] = (char *)SID_malloc(sizeof(char) * MCMC_NAME_SIZE);
            memcpy(MCMC->array[i_array], (double *)va_arg(vargs, double *), (size_t)(MCMC->n_P) * sizeof(double));
            sprintf(MCMC->array_name[i_array], "%s", (char *)va_arg(vargs, char *));
        }
    } else
        MCMC->array = NULL;

    // Set autotune defaults (if needed)
    if(SID_CHECK_BITFIELD_SWITCH(MCMC->mode, MCMC_MODE_AUTOTUNE))
        set_MCMC_autotune(MCMC, -1., -1., -1., -1, -1, -1, -1); // Negatives mean use defaults (set in gbpMCMC.h)

    // Initialize Communicator
    SID_Comm_init(&(MCMC->comm));
    SID_Comm_split(SID_COMM_WORLD, MCMC->my_chain, SID.My_rank, MCMC->comm);

    // Set the base output directory
    if(MCMC->my_chain == SID.My_rank) {
        // Generate directory and stop filenames
        sprintf(test_dir, "./%s_MCMC/", SID.My_binary);
        sprintf(test_restart, "./%s_MCMC/stop", SID.My_binary);

        // analyze_MCMC() needs to know what the filename root is.  We also need to strip trailing '/'s.
        strcpy(MCMC->filename_output_root, test_dir);
        int i_char;
        int flag_continue;
        for(i_char = strlen(MCMC->filename_output_root) - 1, flag_continue = GBP_TRUE; i_char >= 0 && flag_continue; i_char--) {
            if(MCMC->filename_output_root[i_char] != '/')
                flag_continue = GBP_FALSE;
            else
                MCMC->filename_output_root[i_char] = '\0';
        }

        i = 0;
        // Try to open them ...
        ft         = fopen(test_dir, "r");
        ft_restart = fopen(test_restart, "r");
        // If the directory exists & there is no stop file in it then...
        if((ft != NULL) && (ft_restart == NULL)) {
            // Increment the directory suffix until we don't find a directory,
            // whilst always ensuring we don't come across a stop file (which indicates
            // the previous run was stopped prematurely and we want restart the run).
            do {
                fclose(ft);
                i++;
                sprintf(test_dir, "./%s_MCMC.%d/", SID.My_binary, i);
                ft = fopen(test_dir, "r");
                sprintf(test_restart, "./%s_MCMC.%d/stop", SID.My_binary, i);
                ft_restart = fopen(test_restart, "r");
            } while((ft != NULL) && (ft_restart == NULL));
        }
        // Finally, if we did find a stop file then close it and remove it.
        if(ft_restart != NULL) {
            fclose(ft_restart);
            remove(test_restart);
        }
        // Copy the directory name we settled on to the MCMC structure
        strcpy(MCMC->filename_output_dir, test_dir);
        SID_log("ouput_dir set to: %s", SID_LOG_COMMENT, MCMC->filename_output_dir);
    }
    // Broadcast the output directory to all the other cores.
    SID_Bcast(MCMC->filename_output_dir, SID_MAX_FILENAME_LENGTH, SID_CHAR, MCMC->my_chain, MCMC->comm);

    // Initilize the random number generator
    MCMC->RNG = (RNG_info *)SID_malloc(sizeof(RNG_info));
    init_seed_from_clock(&(MCMC->seed));
    SID_Bcast(&(MCMC->seed), 1, SID_INT, SID_MASTER_RANK, MCMC->comm);
    init_RNG(&(MCMC->seed), MCMC->RNG, RNG_DEFAULT);

    SID_log("Done.", SID_LOG_CLOSE);

    va_end(vargs);
}