#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <getopt.h>
#include <iniparser.h>
#include <limits.h>
#include "prepmt/prepmt.h"
#include "ispl/process.h"
#include "iscl/array/array.h"
#include "iscl/memory/memory.h"
#include "iscl/os/os.h"
#ifdef PARMT_USE_INTEL
#include <mkl_cblas.h>
#else
#include <cblas.h>
#endif

#define PROGRAM_NAME "xgrnsTeleB"

static void printUsage(void);
static int parseArguments(int argc, char *argv[],
                          char iniFile[PATH_MAX], char section[256]);
struct sacData_struct *
    prepmt_grnsTeleB_readData(const char *archiveFile, int *nobs, int *ierr);

int prepmt_grnsTeleB_readParameters(const char *iniFile,
                                    const char *section,
                                    char archiveFile[PATH_MAX],
                                    char parmtDataFile[PATH_MAX],
                                    bool *luseCrust1,
                                    char crustDir[PATH_MAX],
                                    bool *luseSourceModel,
                                    char sourceModel[PATH_MAX],
                                    bool *luseTstarTable,
                                    double *defaultTstar,
                                    char tstarTable[PATH_MAX],
                                    bool *lrepickGrns,
                                    double *staWin, double *ltaWin,
                                    double *staltaThreshPct,
                                    bool *lalignXC,
                                    bool *luseEnvelope,
                                    bool *lnormalizeXC,
                                    double *maxXCtimeLag,
                                    int *ndepth, double **depths);
int prepmt_grnsTeleB_loadTstarTable(const char *tstarTable,
                                    const double defaultTstar,
                                    const int nobs,
                                    struct sacData_struct *data,
                                    double **tstars);

int main(int argc, char **argv)
{
    char iniFile[PATH_MAX], archiveFile[PATH_MAX], tstarTable[PATH_MAX],
         parmtDataFile[PATH_MAX], crustDir[PATH_MAX],
         sourceModel[PATH_MAX], section[256];
    struct prepmtCommands_struct cmds;
    struct vmodel_struct *recmod, telmod, srcmod;
    struct prepmtEventParms_struct event;
    struct hudson96_parms_struct hudson96Parms;
    struct hpulse96_parms_struct hpulse96Parms;
    struct prepmtModifyCommands_struct options;
    struct sacData_struct *sacData, *grns, *ffGrns, *locFF;
    const char *hudsonSection = "hudson96\0";
    const char *hpulseSection = "hpulse96\0";
    const int ntstar = 1;
    double *depths, *tstars, defaultTstar, ltaWin, maxXCtimeLag,
           staltaThreshPct, staWin;
    int i, ierr, iobs, k, kndx, ndepth, nobs;
    bool lalignXC, lnormalizeXC, lrepickGrns, luseCrust1,
         luseSourceModel, luseEnvelope, luseTstarTable;

    iscl_init();
    depths = NULL;
    memset(&options, 0, sizeof(struct prepmtModifyCommands_struct));
    ierr = parseArguments(argc, argv, iniFile, section);
    if (ierr != 0)
    {
        if (ierr ==-2){return EXIT_FAILURE;}
        return EXIT_SUCCESS;
    }
    // Load the event
    ierr = prepmt_event_initializeFromIniFile(iniFile, &event);
    if (ierr != 0)
    {
        printf("%s: Error reading event info\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    // Read the forward modeling parameters
    ierr = prepmt_hudson96_readHudson96Parameters(iniFile, hudsonSection,
                                                  &hudson96Parms);
    if (ierr != 0)
    {
        printf("%s: Failed to read hudson parameters\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    ierr = prepmt_hpulse96_readHpulse96Parameters(iniFile, hpulseSection,
                                                  &hpulse96Parms);
    if (ierr != 0)
    {
        printf("%s: Failed to read hpulse parameters\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    ierr = prepmt_grnsTeleB_readParameters(iniFile, section,
                                           archiveFile,
                                           parmtDataFile,
                                           &luseCrust1, crustDir,
                                           &luseSourceModel, sourceModel,
                                           &luseTstarTable,
                                           &defaultTstar,
                                           tstarTable,
                                           &lrepickGrns,
                                           &staWin, &ltaWin, &staltaThreshPct,
                                           &lalignXC, &luseEnvelope,
                                           &lnormalizeXC, &maxXCtimeLag,
                                           &ndepth, &depths);
    if (ierr != 0)
    {
        printf("%s: Failed to read modeling parameters\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    // Read how the default commands will be modified
    options.iodva = hpulse96Parms.iodva;
    options.ldeconvolution = false; // We are working with the greens fns
    ierr = prepmt_prepData_getDefaultDTAndWindowFromIniFile(iniFile, section,
                                                            &options.targetDt,
                                                            &options.cut0,
                                                            &options.cut1);
    if (ierr != 0)
    {
        printf("%s: Failed to read default command info\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    // Load the data
//memset(archiveFile, 0, PATH_MAX*sizeof(char));
//strcpy(archiveFile, "windowedPData/observedWaveforms.h5\0");
    printf("%s: Loading data...\n", PROGRAM_NAME);
    sacData = prepmt_prepData_readArchivedWaveforms(archiveFile, &nobs, &ierr);
    if (ierr != 0)
    {
        printf("%s: Error loading data\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    if (luseTstarTable)
    {
        printf("%s: Loading tstar table...\n", PROGRAM_NAME);
        ierr = prepmt_grnsTeleB_loadTstarTable(tstarTable, defaultTstar,
                                               nobs, sacData, &tstars);
        if (ierr != 0)
        {
            printf("%s: Failed to load t* table\n", PROGRAM_NAME);
            return EXIT_FAILURE;
        }
    }
    else
    {
        printf("%s: Using default tstar: %f\n", PROGRAM_NAME, defaultTstar);
        tstars = array_set64f(nobs, defaultTstar, &ierr);
    }
    printf("%s: Reading pre-processing commands...\n", PROGRAM_NAME);
    cmds = prepmt_commands_readFromIniFile(iniFile, section, nobs,
                                           sacData, &ierr);
    if (ierr != 0)
    {
        printf("%s: Error loading grns prep commands\n", PROGRAM_NAME);
        return -1;
    }
    printf("%s: Loading velocity models...\n", PROGRAM_NAME);
    recmod = (struct vmodel_struct *)
             calloc((size_t) nobs, sizeof(struct vmodel_struct));
    ierr = hudson96_getModels(nobs, sacData,
                              luseCrust1, crustDir,
                              luseSourceModel, sourceModel,
                              &telmod, &srcmod, recmod);
    if (ierr != 0)
    {
        printf("%s: Failed to load models\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    printf("%s: Computing fundamental fault solutions...\n", PROGRAM_NAME);
    ffGrns = (struct sacData_struct *)
             calloc((size_t) (ndepth*nobs*ntstar*10),
                    sizeof(struct sacData_struct));
    for (k=0; k<nobs; k++)
    {
        locFF = prepmt_hudson96_computeGreensFF(hudson96Parms,
                                                hpulse96Parms,
                                                telmod, srcmod, recmod,
                                                ntstar, &tstars[1],
                                                ndepth, depths,
                                                1, &sacData[k], &ierr);
        for (i=0; i<10*ndepth; i++)
        {
            kndx = k*10*ndepth + i;
            sacio_copy(locFF[i], &ffGrns[kndx]);
            sacio_freeData(&locFF[i]);
        }
        free(locFF);
    }
    printf("%s: Contextualing Green's functions...\n", PROGRAM_NAME);
    grns = (struct sacData_struct *)
           calloc((size_t) (6*ndepth*nobs*ntstar),
                  sizeof(struct sacData_struct));
    ierr = prepmt_greens_ffGreensToGreens(nobs, sacData,
                                          ndepth, ntstar, ffGrns, grns);
    if (ierr != 0)
    {
        printf("%s: Error contextualizing Greens functions\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    for (k=0; k<10*nobs*ndepth*ntstar; k++){sacio_freeData(&ffGrns[k]);}
    free(ffGrns);
    // Release memory
    for (k=0; k<nobs; k++)
    {
        cps_utils_freeVmodelStruct(&recmod[k]);
    }
    free(recmod);
    cps_utils_freeVmodelStruct(&srcmod);
    cps_utils_freeVmodelStruct(&telmod);
    // Process the Green's functions
    printf("%s: Modifying Green's functions processing commands...\n",
           PROGRAM_NAME);
    for (k=0; k<cmds.nobs; k++)
    {
        ierr = prepmt_commands_modifyCommandsCharsStruct(options,
                                                         sacData[k],
                                                         &cmds.cmds[k]);
        /*
        for (int l=0; l<cmds.cmds[k].ncmds; l++)
        {
             printf("%s\n", cmds.cmds[k].cmds[l]);
        }
        printf("\n");
        */
        if (ierr != 0)
        {
            printf("%s: Failed to modify processing commands\n", PROGRAM_NAME);
            return EXIT_FAILURE;
        }
    }
    printf("%s: Processing Green's functions...\n", PROGRAM_NAME);
    ierr = prepmt_greens_processHudson96Greens(nobs, ntstar, ndepth,
                                               cmds, grns);
    if (ierr != 0)
    {
        printf("%s: Error processing Green's functions\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    // Repick the Green's functions with an STA/LTA
    if (lrepickGrns)
    {
        printf("%s: Repicking Green's functions...\n", PROGRAM_NAME);
        for (k=0; k<ndepth*ntstar*nobs*6; k=k+6)
        {
            ierr = prepmt_greens_repickGreensWithSTALTA(staWin, ltaWin,
                                                        staltaThreshPct,
                                                        &grns[k]);
            if (ierr != 0)
            {
                printf("%s: Error repicking Green's functions\n", PROGRAM_NAME);
                return EXIT_FAILURE;
            }
        }
    }
    // Shift
    if (lalignXC)
    {
        printf("%s: Refining waveform alignment with cross-correlation\n",
               PROGRAM_NAME);
        for (iobs=0; iobs<nobs; iobs++)
        {
            for (k=0; k<ndepth*ntstar; k++)
            {
                kndx = iobs*ndepth*ntstar*6 + k*6;
                ierr = prepmt_greens_xcAlignGreensToData(sacData[iobs],
                                                         luseEnvelope,
                                                         lnormalizeXC,
                                                         maxXCtimeLag,
                                                         &grns[kndx]);
                if (ierr != 0)
                {
                    printf("%s: Error aligning with XC\n", PROGRAM_NAME);
                    return EXIT_FAILURE;
                }
            }
        }
    }
    // Trim
    printf("%s: Windowing Green's functions to data\n", PROGRAM_NAME);
    ierr = prepmt_greens_cutHudson96FromData(nobs, sacData,
                                             ndepth, ntstar, grns);
    if (ierr != 0)
    {
        printf("%s: Error trimming Green's functions\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    // Dump archive
/*
sacio_writeTimeSeriesFile("test_gxx.sac", grns[11*ndepth*6+6]);
sacio_writeTimeSeriesFile("test_gyy.sac", grns[11*ndepth*6+7]);
sacio_writeTimeSeriesFile("test_gzz.sac", grns[11*ndepth*6+8]);
sacio_writeTimeSeriesFile("test_gxy.sac", grns[11*ndepth*6+9]);
sacio_writeTimeSeriesFile("test_gxz.sac", grns[11*ndepth*6+10]);
sacio_writeTimeSeriesFile("test_gyz.sac", grns[11*ndepth*6+11]);
*/
    printf("%s: Writing archive: %s\n", PROGRAM_NAME, parmtDataFile);
    ierr = prepmt_greens_writeArchive(parmtDataFile, //"./", "pwave",
                                      nobs, ndepth,
                                      event.latitude, event.longitude,
                                      depths, sacData, grns);
    if (ierr != 0)
    {
        printf("%s: Failed to write archive file\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    // release rest of memory
    for (k=0; k<nobs; k++){sacio_freeData(&sacData[k]);}
    for (k=0; k<ndepth*ntstar*nobs*6; k++){sacio_freeData(&grns[k]);}
    memory_free64f(&tstars);
    free(grns);
    free(sacData);
    iscl_finalize();

    return EXIT_SUCCESS;
}
//============================================================================//
/*!
 * @brief Obtains the forward modeling t* values for each teleseismic
 *        waveform observation from a text file.
 *
 * @param[in] tstarTable    Name of tstar table.
 * @param[in] defaultTstar  Default t* if a piece of data cannot be found in
 *                          the table.
 * @param[in] nobs          Number of observations.
 * @param[in] data          List of teleseismic waveforms to map t* values to.
 *                          This is an array of dimension [nobs].
 *
 * @param[out] tstars       tstar values corresponding to each station.
 *                          If a SNCL in the data list cannot be found in the
 *                          tstarTable file then this tstar will be set to the
 *                          default value.
 *                          This is an array of length [nobs].
 *
 * @result 0 indicates success.
 *
 * @author Ben Baker
 *
 */
int prepmt_grnsTeleB_loadTstarTable(const char *tstarTable,
                                    const double defaultTstar,
                                    const int nobs,
                                    struct sacData_struct *data,
                                    double **tstars)
{
    FILE *tsf;
    char cline[64], search[64], item[64];
    double *tstar, tin;
    int i, ierr, k, nlines;
    bool lfound;
    *tstars = NULL;
    tstar = NULL;
    if (!os_path_isfile(tstarTable))
    {
        fprintf(stderr, "%s: Error tstar table %s does not exist\n",
                 __func__, tstarTable);
        return -1;
    }
    if (nobs < 1 || data == NULL)
    {
        fprintf(stderr, "%s: No data\n", __func__);
        return -1;
    }
    // Initialize to default
    tstar = array_set64f(nobs, defaultTstar, &ierr);
    // Get number of lines in text file
    tsf = fopen(tstarTable, "r");
    nlines = 0;
    while (fgets(cline, 64, tsf) != NULL){nlines = nlines + 1;}
    rewind(tsf);
    // Loop through and match observations to tstar's in table
    for (k=0; k<nobs; k++)
    {
        memset(search, 0, 64*sizeof(char));
        sprintf(search, "%s.%s.%s.%s",
                data[k].header.knetwk, data[k].header.kstnm,
                data[k].header.kcmpnm, data[k].header.khole);
        lfound = false;
        for (i=0; i<nlines; i++)
        {
            memset(cline, 0, 64*sizeof(char));
            memset(item,  0, 64*sizeof(char));
            fgets(cline, 64, tsf);
            sscanf(cline, "%s %lf\n", item, &tin);
            if (strcasecmp(search, item) == 0)
            {
                tstar[k] = defaultTstar;
                lfound = true;
                break;
            }
        }
        if (!lfound)
        {
            fprintf(stderr, "%s: Setting %s tstar to: %f\n",
                    __func__, search, defaultTstar);
        }
        rewind(tsf);
    }
    fclose(tsf);
    *tstars = tstar;
    return 0;
}
//============================================================================//
/*!
 * @brief TODO: delete this function.
 */
int prepmt_grnsTeleB_windowHudson96(
    const int nobs, const int ntstar, const int ndepth,
    const struct sacData_struct *data,
    struct sacData_struct *grns)
{
    double epoch, epochGrns;
    int indices[6], idep, ierr, iobs, it;
    for (iobs=0; iobs<nobs; iobs++)
    {
        // Figure out the origin time
        ierr = sacio_getEpochalStartTime(data[iobs].header, &epoch);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Failed to get start time\n", __func__);
            break;
        }
        for (idep=0; idep<ndepth; idep++)
        {
            for (it=0; it<ntstar; it++)
            {
                ierr = prepmt_greens_getHudson96GreensFunctionsIndices(
                                        nobs, ntstar, ndepth,
                                        iobs, it, idep, indices);
                // Figure out the origin time
                ierr = sacio_getEpochalStartTime(grns[indices[0]].header,
                                                 &epochGrns);
                if (ierr != 0)
                {
                    fprintf(stderr, "%s: Failed to get start time\n", __func__);
                    break;
                }
                // Fig
            }
        }
    }
    return 0;
}
//============================================================================//
/*!
 * @brief Processes the Green's functions.
 *
 * @param[in,out] grns    On input contains the Green's functions for all
 *                        observations, depths, and t*'s as well as the
 *                        processing chains. \n
 *                        On output contains the filtered Green's functions
 *                        for all observations, depths, and t*'s.
 *
 * @result 0 indicates success.
 *
 * @author Ben Baker, ISTI
 *
 */
int prepmt_grnsTeleB_processHudson96Greens(
    const int nobs, const int ntstar, const int ndepth,
    const struct prepmtCommands_struct cmds,
    struct sacData_struct *grns)
{
    struct serialCommands_struct commands;
    struct parallelCommands_struct parallelCommands;
    double *G, dt, dt0, epoch, epoch0, time;
    int *dataPtr, indices[6],
        i, i0, i1, i2, ierr, iobs, idep, it, kndx, l, npts, npts0, nq,
        nwork, ny, ns, nsuse;
    bool lnewDt, lnewStartTime;
    const int nTimeVars = 12;
    const enum sacHeader_enum timeVars[12]
       = {SAC_FLOAT_A, SAC_FLOAT_O,
          SAC_FLOAT_T0, SAC_FLOAT_T1, SAC_FLOAT_T2, SAC_FLOAT_T3,
          SAC_FLOAT_T4, SAC_FLOAT_T5, SAC_FLOAT_T6, SAC_FLOAT_T7,
          SAC_FLOAT_T8, SAC_FLOAT_T9};
    const int spaceInquiry =-1;
    // Loop on the observations
    ns = ndepth*ntstar*6;
    dataPtr = memory_calloc32i(ns);
    for (iobs=0; iobs<nobs; iobs++)
    {
        // Set the processing structure
        memset(&parallelCommands, 0,
               sizeof(struct parallelCommands_struct));
               memset(&commands, 0, sizeof(struct serialCommands_struct));
        kndx = prepmt_greens_getHudson96GreensFunctionIndex(G11_GRNS,
                                                      nobs, ntstar, ndepth,
                                                      iobs, 0, 0);
        // Parse the commands
        ierr = process_stringsToSerialCommandsOptions(
                                      cmds.cmds[iobs].ncmds,
                                      (const char **) cmds.cmds[iobs].cmds,
                                      &commands);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error setting serial command string\n",
                    __func__);
            goto ERROR;
        }
        // Determine some characteristics of the processing
        sacio_getEpochalStartTime(grns[kndx].header, &epoch0);
        sacio_getFloatHeader(SAC_FLOAT_DELTA, grns[kndx].header, &dt0);
        lnewDt = false;
        lnewStartTime = false;
        epoch = epoch0;
        dt = dt0;
        for (i=0; i<commands.ncmds; i++)
        {
            if (commands.commands[i].type == CUT_COMMAND)
            {
                i0 = commands.commands[i].cut.i0;
                epoch = epoch + (double) i0*dt;
                lnewStartTime = true;
            }
            if (commands.commands[i].type == DOWNSAMPLE_COMMAND)
            {
                nq = commands.commands[i].downsample.nq;
                dt = dt*(double) nq;
                lnewDt = true;
            }
            if (commands.commands[i].type == DECIMATE_COMMAND)
            {
                nq = commands.commands[i].decimate.nqAll;
                dt = dt*(double) nq;
                lnewDt = true;
            }
        }
        // Set the commands on the parallel processing structure
        ierr = process_setCommandOnAllParallelCommands(ns, commands,
                                                       &parallelCommands);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error setting the parallel commands\n",
                    __func__);
            goto ERROR;
        }
        // Get the data
        dataPtr[0] = 0;
        for (idep=0; idep<ndepth; idep++)
        {
            for (it=0; it<ntstar; it++)
            {
                kndx = prepmt_greens_getHudson96GreensFunctionIndex(G11_GRNS,
                                                          nobs, ntstar, ndepth,
                                                          iobs, 0, 0);
                ierr = sacio_getIntegerHeader(SAC_INT_NPTS,
                                              grns[kndx].header, &npts);
                if (ierr != 0)
                {
                    fprintf(stderr, "%s: Error getting npts\n", __func__);
                    goto ERROR;
                }
                i1 = idep*6*ntstar + it*6 + 0;
                i2 = i1 + 6;
                for (i=i1; i<i2; i++)
                {
                    dataPtr[i+1] = dataPtr[i] + npts;
                }
            }
        }
        nwork = dataPtr[6*ndepth*ntstar];
        if (nwork < 1)
        {
            fprintf(stderr, "%s: Invalid workspace size: %d\n",
                    __func__, nwork);
            ierr = 1;
            goto ERROR;
        }
        G = memory_calloc64f(nwork);
        for (idep=0; idep<ndepth; idep++)
        {
            for (it=0; it<ntstar; it++)
            {
                ierr = prepmt_greens_getHudson96GreensFunctionsIndices(
                                        nobs, ntstar, ndepth,
                                        iobs, it, idep, indices);
                if (ierr != 0)
                {
                    fprintf(stderr, "%s: Error getting index\n", __func__);
                    goto ERROR;
                }
                for (i=0; i<6; i++)
                {
                    i1 = idep*6*ntstar + it*6 + i;
                    cblas_dcopy(grns[indices[i]].npts,
                                grns[indices[i]].data, 1, &G[dataPtr[i1]], 1);
                }
            }
        }
        // Set the data
        ierr =  process_setParallelCommandsData64f(ns, dataPtr,
                                                   G, &parallelCommands);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error setting data\n", __func__);
            goto ERROR;
        }
        // Apply the commands
        ierr = process_applyParallelCommands(&parallelCommands);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error processing data\n", __func__);
            goto ERROR;
        }
        // Get the data
        ierr = process_getParallelCommandsData64f(parallelCommands,
                                                  spaceInquiry, spaceInquiry,
                                                  &ny, &nsuse,
                                                  dataPtr, G);
        if (ny < nwork)
        {
            memory_free64f(&G);
            G = memory_calloc64f(ny);
        }
        ny = nwork;
        ierr = process_getParallelCommandsData64f(parallelCommands,
                                                  nwork, ns,
                                                  &ny, &nsuse, dataPtr, G);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error getting data\n", __func__);
            goto ERROR;
        }
        // Unpack the data
        for (idep=0; idep<ndepth; idep++)
        {
            for (it=0; it<ntstar; it++)
            {
                ierr = prepmt_greens_getHudson96GreensFunctionsIndices(
                                        nobs, ntstar, ndepth,
                                        iobs, it, idep, indices);
                if (ierr != 0)
                {
                    fprintf(stderr, "%s: Error getting index\n", __func__);
                    goto ERROR;
                }
                for (i=0; i<6; i++)
                {
                    i1 = idep*6*ntstar + it*6 + i;
                    sacio_getIntegerHeader(SAC_INT_NPTS,
                                           grns[indices[i]].header, &npts0);
                    npts = dataPtr[i1+1] - dataPtr[i1];
                    // Resize event
                    if (npts != npts0)
                    {
                        sacio_freeData(&grns[indices[i]]);
                        grns[indices[i]].data = sacio_malloc64f(npts);
                        grns[indices[i]].npts = npts;
                        sacio_setIntegerHeader(SAC_INT_NPTS, npts,
                                               &grns[indices[i]].header);
                        ierr = array_copy64f_work(npts,
                                                  &G[dataPtr[i]],
                                                  grns[indices[i]].data);
                    }
                    else
                    {
                        ierr = array_copy64f_work(npts,
                                                  &G[dataPtr[i]],
                                                  grns[indices[i]].data);
                    }
                    // Update the times
                    if (lnewStartTime)
                    {
                        for (i=0; i<6; i++)
                        {
                            // Update the picks
                            for (l=0; l<nTimeVars; l++)
                            {
                                ierr = sacio_getFloatHeader(timeVars[l],
                                              grns[indices[i]].header, &time);
                                if (ierr == 0)
                                {
                                    time = time + epoch0; // Turn to real time
                                    time = time - epoch;  // Relative to new time
                                    sacio_setFloatHeader(timeVars[l], time,
                                                    &grns[indices[i]].header);
                                }
                            } // Loop on picks
                            sacio_setEpochalStartTime(epoch,
                                                      &grns[indices[i]].header);
                        } // Loop on signals
                    }
                    // Update the sampling period
                    if (lnewDt)
                    {
                        for (i=0; i<6; i++)
                        {
                            sacio_setFloatHeader(SAC_FLOAT_DELTA, dt,
                                                 &grns[indices[i]].header);
                        }
                    }
                } // Loop on Green's functions gxx, gyy, ...
            } // Loop on t*
        } // Loop on depths
        process_freeSerialCommands(&commands);
        process_freeParallelCommands(&parallelCommands);
        memory_free64f(&G);
    }
ERROR:;
    memory_free32i(&dataPtr);
    return 0;
}
//============================================================================//
/*!
 * @brief Reads the teleseismic body wave Green's functions modeling parameters.
 *
 * @param[in] iniFile           Name of initialization file.
 * @param[in] section           Section of initalization file to read.  For
 *                              example, this may be prepmt:telePGrns.
 * @param[out] archiveFile      This is the HDF5 file with the observed waveforms.
 * @param[out] parmtDataFile    This is the name of the data archive file to
 *                              be used by parmt.
 * @param[out] luseCrust1       If true then the modeling may use crust1
 *                              at the source and receiver (unless superseded
 *                              by a specified local model). \n
 *                              Otherwise, use a local model or the global
 *                              ak135 earth model.
 * @param[out] crustDir         The crust1.0 directory.  This is only relevant
 *                              if luseCrust1 is true.
 * @param[out] luseSourceModel  If true then use a local source model.
 * @param[out] sourceModel      If luseSourceModel is true then this is the
 *                              filename with the local source model.
 * @param[out] luseTstarTable   If true then read the t* attenuation factors
 *                              from a table.
 * @param[out] defaultTstar     This is the default t* (e.g., 0.4).
 * @param[out] tstarTable       If luseTstarTable is true then this is the file
 *                              with the station/t* table pairings.
 * @param[out] lreprickGrns     If true then repick the Green's functions with
 *                              an STA/LTA function.  This is in the context of
 *                              severe attenutation which noticeably effect
 *                              theoretical arrival times.
 * @param[out] staWin           If repicking Green's functions then this
 *                              is the short term window (seconds).
 * @param[out] ltaWin           If repicking Green's functions then this
 *                              is the long term window (seconds).
 * @param[out] staltaThreshPct  This is the fraction (0,1) after which an
 *                              arrival is declared in the STA/LTA repicking.
 * @param[out] lalignXC         If true the the Green's functions will be
 *                              re-aligned with cross-correlation.
 * @param[out] luseEnvelope     If true and lalignXC is true then the
 *                              Green's functions will be aligned based on
 *                              the cross-correlation of their envelopes.
 * @param[out] lnormalizeXC     If true then each Green's function is normalized
 *                              in the Green's function cross-correlation thus
 *                              Green's functions with larger amplitudes may
 *                              bias the result.
 * @param[out] maxXCtimeLag     This is the max time lag (seconds) available to
 *                              the waveform cross-correlation alignment.
 * @param[out] ndepth           Number of depths in modeling.
 * @param[in,out] depths        On input this is a NULL pointer.
 *                              On output this is a pointer to an array
 *                              of dimension [ndepth] with the modeling depths
 *                              (km).
 *
 * @result 0 indicates success.
 *
 * @author Ben Baker, ISTI
 *
 */
int prepmt_grnsTeleB_readParameters(const char *iniFile,
                                    const char *section,
                                    char archiveFile[PATH_MAX],
                                    char parmtDataFile[PATH_MAX],
                                    bool *luseCrust1,
                                    char crustDir[PATH_MAX],
                                    bool *luseSourceModel,
                                    char sourceModel[PATH_MAX],
                                    bool *luseTstarTable,
                                    double *defaultTstar,
                                    char tstarTable[PATH_MAX],
                                    bool *lrepickGrns,
                                    double *staWin, double *ltaWin,
                                    double *staltaThreshPct,
                                    bool *lalignXC,
                                    bool *luseEnvelope,
                                    bool *lnormalizeXC,
                                    double *maxXCtimeLag,
                                    int *ndepth, double **depths)
{
    const char *s;
    char vname[256];
    dictionary *ini;
    char *dirName;
    double *deps, dmin, dmax;
    int ierr;

    ierr = 0;
    deps = NULL;
    memset(archiveFile, 0, PATH_MAX*sizeof(char));
    memset(crustDir, 0, PATH_MAX*sizeof(char));
    memset(sourceModel, 0, PATH_MAX*sizeof(char));
    memset(tstarTable, 0, PATH_MAX*sizeof(char));
    memset(parmtDataFile, 0, PATH_MAX*sizeof(char));
    if (!os_path_isfile(iniFile))
    {
        fprintf(stderr, "%s: Error ini file does not exist\n", __func__);
        return -1;
    }
    ini = iniparser_load(iniFile);

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:dataH5File", section);
    s = iniparser_getstring(ini, vname, NULL);
    if (!os_path_isfile(s))
    {
        fprintf(stderr, "%s: Data file %s does not exist\n", __func__, s);
        ierr = 1;
        goto ERROR;
    }
    strcpy(archiveFile, s);

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:parmtDataFile", section);
    s = iniparser_getstring(ini, vname, "bodyWave.h5");
    strcpy(parmtDataFile, s);

    dirName = os_dirname(parmtDataFile, &ierr);
    if (!os_path_isdir(dirName))
    {
        ierr = os_makedirs(dirName);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Failed to make directory: %s\n",
                    __func__, dirName);
            goto ERROR;
        }
    }
    memory_free8c(&dirName);

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:lrepickGrns", section);
    *lrepickGrns = iniparser_getboolean(ini, vname, false);

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:staWin", section);
    *staWin = iniparser_getdouble(ini, vname, 0.2);
    if (*staWin <= 0.0)
    {
        fprintf(stderr, "%s: Invalid STA length %f\n", __func__, *staWin);
        ierr = 1;
        goto ERROR;
    }

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:ltaWin", section);
    *ltaWin = iniparser_getdouble(ini, vname, 1.2);
    if (*ltaWin <= *staWin)
    {
        fprintf(stderr, "%s: Invalid LTA/STA lengths %f %f\n",
                __func__, *staWin, *ltaWin);
        ierr = 1;
        goto ERROR;
    }

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:staltaThreshPct", section);
    *staltaThreshPct = iniparser_getdouble(ini, vname, 0.8);
    if (*staltaThreshPct <= 0.0 || *staltaThreshPct > 1.0)
    {
        fprintf(stderr, "%s: Invalid STA/LTA thresh pct %f\n",
                 __func__, *staltaThreshPct);
        ierr = 1;
        goto ERROR;
    }

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:luseTstarTable", section);
    *luseTstarTable = iniparser_getboolean(ini, vname, false);
    if (*luseTstarTable)
    {
        memset(vname, 0, 256*sizeof(char));
        sprintf(vname, "%s:tstarTable", section);
        s = iniparser_getstring(ini, vname, NULL);
        if (!os_path_isfile(s))
        {
            fprintf(stderr, "%s: tstar table %s doesn't exist\n",
                    __func__, s);
            ierr = 1;
            goto ERROR;
        }
        strcpy(tstarTable, s);
    }

    memset(vname, 0, 256*sizeof(char));
    strcpy(vname, "precompute:ndepths\0");
    *ndepth = iniparser_getint(ini, vname, 0);
    if (*ndepth < 1)
    {
        fprintf(stderr, "%s: Inadequate number of depths %d\n",
                __func__, *ndepth);
        ierr = 1;
        goto ERROR;
    }

    memset(vname, 0, 256*sizeof(char));
    strcpy(vname, "precompute:depthMin\0");
    dmin = iniparser_getdouble(ini, vname, -1.0);

    memset(vname, 0, 256*sizeof(char));
    strcpy(vname, "precompute:depthMax\0");
    dmax = iniparser_getdouble(ini, vname, -1.0);
    if (dmin < 0.0 || dmin > dmax)
    {
        fprintf(stderr, "%s: Invalid dmin/dmax %f %f\n", __func__, dmin, dmax);
        ierr = 1;
        goto ERROR;
    }
    deps = array_linspace64f(dmin, dmax, *ndepth, &ierr);

    memset(vname, 0, 256*sizeof(char));
    strcpy(vname, "precompute:luseCrust1\0");
    *luseCrust1 = iniparser_getboolean(ini, vname, true);
    if (*luseCrust1)
    {
        memset(vname, 0, 256*sizeof(char));
        strcpy(vname, "precompute:crustDir\0");
        s = iniparser_getstring(ini, vname, CPS_DEFAULT_CRUST1_DIRECTORY);
        if (!os_path_isdir(s))
        {
            fprintf(stderr, "%s: crust1.0 directory %s doesn't exist\n",
                    __func__, s);
            ierr = 1;
            goto ERROR;
        }
        strcpy(crustDir, s);
    }

    memset(vname, 0, 256*sizeof(char));
    strcpy(vname, "precompute:luseSourceModel\0");
    *luseSourceModel = iniparser_getboolean(ini, vname, false);
    if (*luseSourceModel)
    {
        memset(vname, 0, 256*sizeof(char));
        strcpy(vname, "precompute:sourceModel\0");
        s = iniparser_getstring(ini, vname, NULL);
        if (!os_path_isfile(s))
        {
            fprintf(stderr, "%s: Source model %s does not exist\n",
                    __func__, s);
            ierr = 1;
            goto ERROR;
        }
        strcpy(sourceModel, s);
    }

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:defaultTstar", section);
    *defaultTstar = iniparser_getdouble(ini, vname, 0.4);

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:lalignXC", section);
    *lalignXC = iniparser_getboolean(ini, vname, false);

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:luseEnvelope", section);
    *luseEnvelope = iniparser_getboolean(ini, vname, false);

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:lnormalizeXC", section);
    *lnormalizeXC = iniparser_getboolean(ini, vname, false);

    memset(vname, 0, 256*sizeof(char));
    sprintf(vname, "%s:maxXCtimeLag", section);
    *maxXCtimeLag = iniparser_getdouble(ini, vname, -1.0);

ERROR:;
    *depths = deps;
    iniparser_freedict(ini);
    return ierr;
}
//============================================================================//
/*!
 * @brief Utility function for reading the data for which the Green's functions
 *        will be generated.
 *
 * @param[in] archiveFile  Name of HDF5 archive file containing the
 *                         observations.
 *
 * @param[out] nobs        Number of observations.
 * @param[out] ierr        0 indicates success.
 *
 * @result The SAC data from the H5 archive for which the Green's functions
 *         will be created.  This is an array of dimension [nobs].
 *
 * @author Ben Baker, ISTI
 *
 */
struct sacData_struct *
    prepmt_grnsTeleB_readData(const char *archiveFile, int *nobs, int *ierr)
{
    char **sacFiles;
    hid_t groupID, fileID;
    int i, nfiles;
    struct sacData_struct *sacData;
    *nobs = 0;
    sacData = NULL;
    if (!os_path_isfile(archiveFile))
    {
        fprintf(stderr, "%s: Error archive file %s doesn't exist\n",
                __func__, archiveFile);
        *ierr = 1;
        return sacData;
    }
    fileID = H5Fopen(archiveFile, H5F_ACC_RDONLY, H5P_DEFAULT);
    groupID = H5Gopen2(fileID, "/ObservedWaveforms", H5P_DEFAULT);
    sacFiles = sacioh5_getFilesInGroup(groupID, &nfiles, ierr);
    if (*ierr != 0 || sacFiles == NULL)
    {
        fprintf(stderr, "%s: Error getting names of SAC flies\n", __func__);
        *ierr = 1;
        return sacData;
    }
    sacData = sacioh5_readTimeSeriesList(nfiles, (const char **) sacFiles,
                                         groupID, nobs, ierr);
    if (*ierr != 0)
    {
        fprintf(stderr, "%s: Errors while reading SAC files\n", __func__);
    }
    if (*nobs != nfiles)
    {
        fprintf(stderr, "%s: Warning - subset of data was read\n", __func__);
    }
    // Clean up and close archive file
    for (i=0; i<nfiles; i++)
    {
        if (sacFiles[i] != NULL){free(sacFiles[i]);}
    }
    free(sacFiles);
    H5Gclose(groupID);
    H5Fclose(fileID);
    return sacData;
}
//============================================================================//
/*!
 * @brief Parses the command line arguments for the ini file.
 *
 * @param[in] argc      Number of arguments input to the program.
 * @param[in] argv      Command line arguments.
 *
 * @param[out] iniFile  If the result is 0 then this is the ini file.
 * @param[out] section  Section of ini file to read.  The default is
 *                      prepmt:telePGrns.
 *
 * @result 0 indicates success. \n
 *        -1 indicates the user inquired about the usage. \n
 *        -2 indicates the command line argument is invalid.
 *
 * @author Ben Baker, ISTI
 *
 */
static int parseArguments(int argc, char *argv[],
                          char iniFile[PATH_MAX], char section[256])
{
    bool linFile, lsection;
    linFile = false;
    lsection = false;
    memset(iniFile, 0, PATH_MAX*sizeof(char));
    memset(section, 0, 256*sizeof(char));
    while (true)
    {
        static struct option longOptions[] =
        {
            {"help", no_argument, 0, '?'},
            {"help", no_argument, 0, 'h'},
            {"ini_file", required_argument, 0, 'i'},
            {"section", required_argument, 0, 's'},
            {0, 0, 0, 0}
        };
        int c, optionIndex;
        c = getopt_long(argc, argv, "?hi:s:",
                        longOptions, &optionIndex);
        if (c ==-1){break;}
        if (c == 'i')
        {
            strcpy(iniFile, (const char *) optarg);
            linFile = true;
        }
        else if (c == 's')
        {
            strcpy(section, (const char *) optarg);
            lsection = true;
        }
        else if (c == 'h' || c == '?')
        {
            printUsage();
            return -2;
        }
        else
        {
            printf("%s: Unknown options: %s\n",
                   PROGRAM_NAME, argv[optionIndex]);
        }
    }
    if (!linFile)
    {
        printf("%s: Error must specify ini file\n\n", PROGRAM_NAME);
        printUsage();
        return -1;
    }
    else
    {
        if (!os_path_isfile(iniFile))
        {
            printf("%s: Error - ini file: %s does not exist\n",
                   PROGRAM_NAME, iniFile);
            return EXIT_FAILURE;
        }
    }
    if (!lsection){strcpy(section, "prepmt:telePGrns\0");}
    return 0;
}
//============================================================================//
static void printUsage(void)
{
    printf("Usage:\n   %s -i ini_file\n\n", PROGRAM_NAME);
    printf("Required arguments:\n");
    printf("    -i ini_file specifies the initialization file\n");
    printf("\n");
    printf("Optional arguments:\n");
    printf("    -h displays this message\n");
    printf("    -s section of ini file to read\n");
    return;
}


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

#include <kdcount/kdtree.h>

#include <fastpm/libfastpm.h>
#include <gsl/gsl_rng.h>

#include <fastpm/prof.h>
#include <fastpm/logging.h>
#include <fastpm/store.h>

#include <fastpm/fof.h>

#include "pmpfft.h"
#include "pm2lpt.h"
#include "pmghosts.h"
#include "vpm.h"

#include <fastpm/io.h>
#include <fastpm/string.h>
#include <bigfile-mpi.h>

//#define FASTPM_FOF_DEBUG

struct FastPMFOFFinderPrivate {
    int ThisTask;
    int NTask;
    double * boxsize;
    MPI_Comm comm;
};

/* creating a kdtree struct
 * for store with np particles starting from start.
 * */
struct KDTreeNodeBuffer {
    void * mem;
    void * base;
    char * ptr;
    char * end;
    struct KDTreeNodeBuffer * prev;
};

static void *
_kdtree_buffered_malloc(void * userdata, size_t size)
{
    struct KDTreeNodeBuffer ** pbuffer = (struct KDTreeNodeBuffer**) userdata;

    struct KDTreeNodeBuffer * buffer = *pbuffer;

    if(buffer->base == NULL || buffer->ptr + size >= buffer->end) {
        struct KDTreeNodeBuffer * newbuffer = malloc(sizeof(newbuffer[0]));
        newbuffer->mem = buffer->mem;
        size_t newsize = 1024 * 1024 * 4; /* 4 MB for each block */
        if(newsize < size) {
            newsize = size;
        }
        newbuffer->base = fastpm_memory_alloc(buffer->mem, "KDTreeBase", newsize, FASTPM_MEMORY_STACK);
        newbuffer->ptr = newbuffer->base;
        newbuffer->end = newbuffer->base + newsize;
        newbuffer->prev = buffer;

        *pbuffer = newbuffer;
        buffer = newbuffer;
    }

    void * r = buffer->ptr;
    buffer->ptr += size;
    return r;
}

static void
_kdtree_buffered_free(void * userdata, size_t size, void * ptr)
{
    /* do nothing; */
}

static
KDNode *
_create_kdtree (KDTree * tree, int thresh,
    FastPMStore ** stores, int nstore,
    double boxsize[])
{
    /* if boxsize is NULL the tree will be non-periodic. */
    /* the allocator; started empty. */
    struct KDTreeNodeBuffer ** pbuffer = malloc(sizeof(void*));
    struct KDTreeNodeBuffer * headbuffer = malloc(sizeof(headbuffer[0]));
    *pbuffer = headbuffer;

    headbuffer->mem = stores[0]->mem;
    headbuffer->base = NULL;
    headbuffer->prev = NULL;

    int s;
    ptrdiff_t i;

    tree->userdata = pbuffer;

    tree->input.dims[0] = 0;

    for(s = 0; s < nstore; s ++) {
        tree->input.dims[0] += stores[s]->np;
    }

    tree->input.dims[1] = 3;

    if(tree->input.dims[0] < stores[0]->np_upper) {
        /* if the first store is big enough, use it for the tree */
        tree->input.buffer = (void*) &stores[0]->x[0][0];
    } else {
        /* otherwise, allocate a big buffer and make a copy */
        tree->input.buffer = _kdtree_buffered_malloc(pbuffer,
                    tree->input.dims[0] * sizeof(stores[0]->x[0]));
        memcpy(tree->input.buffer, &stores[0]->x[0][0], stores[0]->np * sizeof(stores[0]->x[0]));
    }

    i = stores[0]->np;

    /* copy the other positions to the base pointer. */
    for(s = 1; s < nstore; s ++) {
        memcpy(((char*) tree->input.buffer) + i * sizeof(stores[0]->x[0]),
                &stores[s]->x[0][0],
                stores[s]->np * sizeof(stores[0]->x[0]));

        i = i + stores[s]->np;
    }

    tree->input.strides[0] = sizeof(stores[0]->x[0]);
    tree->input.strides[1] = sizeof(stores[0]->x[0][0]);
    tree->input.elsize = sizeof(stores[0]->x[0][0]);
    tree->input.cast = NULL;

    tree->ind = _kdtree_buffered_malloc(pbuffer, tree->input.dims[0] * sizeof(tree->ind[0]));
    for(i = 0; i < tree->input.dims[0]; i ++) {
        tree->ind[i] = i;
    }
    tree->ind_size = tree->input.dims[0];

    tree->malloc = _kdtree_buffered_malloc;
    tree->free = _kdtree_buffered_free;

    tree->thresh = thresh;

    tree->boxsize = boxsize;

    KDNode * root = kd_build(tree);
    fastpm_info("Creating KDTree with %td nodes for %td particles\n", tree->size, tree->ind_size);
    return root;
}

void
_free_kdtree (KDTree * tree, KDNode * root)
{
    kd_free(root);
    struct KDTreeNodeBuffer * buffer, *q, **pbuffer = tree->userdata;

    for(buffer = *pbuffer; buffer; buffer = q) {
        if(buffer->base)
            fastpm_memory_free(buffer->mem, buffer->base);
        q = buffer->prev;
        free(buffer);
    }
    free(pbuffer);
}

void
fastpm_fof_init(FastPMFOFFinder * finder, FastPMStore * store, PM * pm)
{
    finder->priv = malloc(sizeof(FastPMFOFFinderPrivate));
    finder->p = store;
    finder->pm = pm;

    finder->event_handlers = NULL;

    if (finder->periodic)
        finder->priv->boxsize = pm_boxsize(pm);
    else
        finder->priv->boxsize = NULL;

    finder->priv->comm = pm_comm(pm);
    MPI_Comm comm = finder->priv->comm;
    MPI_Comm_rank(comm, &finder->priv->ThisTask);
    MPI_Comm_size(comm, &finder->priv->NTask);
}

static void
_fof_local_find(FastPMFOFFinder * finder,
            FastPMStore * p,
            PMGhostData * pgd,
            ptrdiff_t * head, double linkinglength)
{
    /* local find of p and the the ghosts */
    KDTree tree;

    FastPMStore * stores[2] = {p, pgd->p};

    KDNode * root = _create_kdtree(&tree, finder->kdtree_thresh, stores, 2, finder->priv->boxsize);

    kd_fof(root, linkinglength, head);

    _free_kdtree(&tree, root);
}

static int
_merge(uint64_t * src, ptrdiff_t isrc, uint64_t * dest, ptrdiff_t idest, ptrdiff_t * head)
{
    int merge = 0;
    ptrdiff_t j = head[idest];
    if(src[isrc] < dest[j]) {
        merge = 1;
    }

    if(merge) {
        dest[j] = src[isrc];
    }
    return merge;
}

struct reduce_fof_data {
    ptrdiff_t * head;
    size_t nmerged;
};

static void
FastPMReduceFOF(FastPMStore * src, ptrdiff_t isrc, FastPMStore * dest, ptrdiff_t idest, int ci, void * userdata)
{

    struct reduce_fof_data * data = userdata;

    data->nmerged += _merge(src->minid, isrc, dest->minid, idest, data->head);
}

static void
_fof_global_merge(
    FastPMFOFFinder * finder,
    FastPMStore * p,
    PMGhostData * pgd,
    uint64_t * minid,
    ptrdiff_t * head
)
{
    ptrdiff_t i;

    MPI_Comm comm = finder->priv->comm;

    size_t npmax = p->np;

    MPI_Allreduce(MPI_IN_PLACE, &npmax, 1, MPI_LONG, MPI_MAX, comm);

    /* initialize minid, used as a global tag of groups as we merge */
    for(i = 0; i < p->np; i ++) {
        /* assign unique ID to each particle; could use a better scheme with true offsets */
        minid[i] = i + finder->priv->ThisTask * npmax;
        #ifdef FASTPM_FOF_DEBUG
        /* for debugging, overwrite the previous unique ID with the true ID of particles */
        minid[i] = p->id[i];
        #endif
    }

    /* send minid */
    p->minid = minid; /* only send up to p->np */
    pm_ghosts_send(pgd, COLUMN_MINID);
    p->minid = NULL;

    /* copy over to minid for storage; FIXME: allow overriding  */
    for(i = 0; i < pgd->p->np; i ++) {
        minid[i + p->np] = pgd->p->minid[i];
    }

    /* reduce the minid of the head items according to the local connection. */

    while(1) {
        size_t nmerge = 0;
        for(i = 0; i < p->np + pgd->p->np; i ++) {
            nmerge += _merge(minid, i, minid, i, head);
        }
        if(nmerge == 0) break;
    }

#ifdef FASTPM_FOF_DEBUG
    {
        FILE * fp = fopen(fastpm_strdup_printf("dump-pos-%d.f8", finder->priv->ThisTask), "w");
        fwrite(p->x, p->np, sizeof(double) * 3, fp);
        fwrite(pgd->p->x, pgd->p->np, sizeof(double) * 3, fp);
        fclose(fp);
    }
    {
        FILE * fp = fopen(fastpm_strdup_printf("dump-id-%d.f8", finder->priv->ThisTask), "w");
        fwrite(p->id, p->np, sizeof(int64_t), fp);
        fwrite(pgd->p->id, pgd->p->np, sizeof(int64_t) * 3, fp);
        fclose(fp);
    }

#endif
    int iter = 0;

    while(1) {

        /* prepare the communication buffer, every ghost has
         * the minid and task of the current head. such that
         * they will connect to the other ranks correctly */

        for(i = 0; i < pgd->p->np; i ++) {
            pgd->p->minid[i] = minid[head[i + p->np]];
        }

        /* at this point all items on ghosts have local minid and task, ready to reduce */

        struct reduce_fof_data data = {
            .head = head,
            .nmerged = 0,
        };

        /* merge ghosts into the p, reducing the MINID on p */

        p->minid = minid; /* only update up to p->np */
        pm_ghosts_reduce(pgd, COLUMN_MINID, FastPMReduceFOF, &data);
        p->minid = NULL;

        size_t nmerged = data.nmerged;

        /* at this point all items on p->fof with head[i] = i have present minid and task */

        MPI_Allreduce(MPI_IN_PLACE, &nmerged, 1, MPI_LONG, MPI_SUM, comm);

        MPI_Barrier(comm);

        fastpm_info("FOF reduction iteration %d : merged %td crosslinks\n", iter, nmerged);

        if(nmerged == 0) break;

        for(i = 0; i < p->np + pgd->p->np; i ++) {
            if(minid[i] < minid[head[i]]) {
                fastpm_raise(-1, "p->fof invariance is broken i = %td np = %td\n", i, p->np);
            }
        }

        iter++;
    }

    /* previous loop only updated head[i]; now make sure every particles has the correct minid. */
    for(i = 0; i < p->np + pgd->p->np; i ++) {
        minid[i] = minid[head[i]];
    }

    #ifdef FASTPM_FOF_DEBUG
    {
    for(i = 0; i < p->np + pgd->p->np ; i ++) {
        uint64_t id = i <p->np?p->id[i]:pgd->p->id[i - p->np];
        if(minid[i] == 88) {
            fastpm_ilog(INFO, "%d MINID == %ld ID = %ld headminid == %ld i = %td / %td head=%td", finder->priv->ThisTask,
                minid[i], id, minid[head[i]], i, p->np, head[i]);
        }
        if(id == 88 || id == 96 || id == 152 || id == 160) {
            fastpm_ilog(INFO, "%d MINID == %ld ID = %ld headminid == %ld i = %td / %td head=%td", finder->priv->ThisTask,
                minid[i], id, minid[head[i]], i, p->np, head[i]);
        }
    }
    }
    #endif
}

/* set head[i] to hid*/
static size_t
_assign_halo_attr(FastPMFOFFinder * finder, PMGhostData * pgd, ptrdiff_t * head, size_t np, size_t np_ghosts, int nmin)
{
    ptrdiff_t * offset = fastpm_memory_alloc(finder->p->mem, "FOFOffset", sizeof(offset[0]) * (np + np_ghosts), FASTPM_MEMORY_STACK);
    uint8_t * has_remote = fastpm_memory_alloc(finder->p->mem, "FOFHasRemote", sizeof(has_remote[0]) * (np + np_ghosts), FASTPM_MEMORY_STACK);
    uint8_t * has_local = fastpm_memory_alloc(finder->p->mem, "FOFHasLocal", sizeof(has_local[0]) * (np + np_ghosts), FASTPM_MEMORY_STACK);

    ptrdiff_t i;
    for(i = 0; i < np + np_ghosts; i ++) {
        offset[i] = 0;
        has_remote[i] = 0;
        has_local[i] = 0;
    }

    /* set offset to number of particles in the halo */
    for(i = 0; i < np + np_ghosts; i ++) {
        offset[head[i]] ++;
    }

    for(i = 0; i < np; i ++) {
        has_local[head[i]] = 1;
    }

    /* if the group is connected to a remote component */
    /* if the particle has a ghost, then */
    pm_ghosts_has_ghosts(pgd, has_remote);

    /* if connected to a particle that has ghost */
    for(i = 0; i < np; i ++) {
        if(has_remote[i]) has_remote[head[i]] = 1;
    }

    /* if connected to a ghost */
    for(i = np; i < np + np_ghosts; i ++) {
        has_remote[head[i]] = 1;
    }

    size_t it = 0;

    /* assign attr index for groups at least contain 1 local particle */
    for(i = 0; i < np + np_ghosts; i ++) {
        if(has_local[i] && (has_remote[i] || offset[i] >= nmin)) {
            offset[i] = it;
            it ++;
        } else {
            offset[i] = -1;
        }
    }

    size_t nhalos = it;

    /* update head [i] to offset[head[i]], which stores the index in the halo store for this particle. */
    for(i = 0; i < np + np_ghosts; i ++) {
        head[i] = offset[head[i]];
        /* this will not happen if nmin == 1 */
        if(head[i] != -1 && head[i] > nhalos) {
            fastpm_raise(-1, "head[i] (%td) > nhalos (%td) This shall not happen.\n", head[i], nhalos);
        }
    }

    fastpm_memory_free(finder->p->mem, has_local);
    fastpm_memory_free(finder->p->mem, has_remote);
    fastpm_memory_free(finder->p->mem, offset);

    return it;
}
static double
periodic_add(double x, double wx, double y, double wy, double L)
{
    if(wx > 0) {
        while(y - x > L / 2) y -= L;
        while(y - x < -L / 2) y += L;

        return wx * x + wy * y;
    } else {
        return wy * y;
    }
}

/*
 * apply mask to a halo storage.
 * relable head, and halo->id
 * */
static void
fastpm_fof_subsample(FastPMFOFFinder * finder, FastPMStore * halos, FastPMParticleMaskType * mask, ptrdiff_t * head)
{
    /* mapping goes from the old head[i] value to the new head[i] value */
    ptrdiff_t * mapping = fastpm_memory_alloc(finder->p->mem, "FOFMapping", sizeof(mapping[0]) * halos->np, FASTPM_MEMORY_STACK);

    ptrdiff_t i;
    for(i = 0; i < halos->np; i ++) {
        mapping[i] = -1;
        halos->id[i] = i;
    }

    /* remove non-contributing halo segments */
    fastpm_store_subsample(halos, mask, halos);

    for(i = 0; i < halos->np; i ++) {
        mapping[halos->id[i]] = i;
        halos->id[i] = i;
    }

    /* adjust head[i] */

    for(i = 0; i < finder->p->np; i ++) {
        if(head[i] >= 0) {
            head[i] = mapping[head[i]];
        }
    }

    fastpm_memory_free(finder->p->mem, mapping);
}

static void
fastpm_fof_apply_length_cut(FastPMFOFFinder * finder, FastPMStore * halos, ptrdiff_t * head)
{
    MPI_Comm comm = finder->priv->comm;

    FastPMParticleMaskType * mask = fastpm_memory_alloc(finder->p->mem, "LengthMask", sizeof(mask[0]) * halos->np, FASTPM_MEMORY_STACK);
    ptrdiff_t i;
    for(i = 0; i < halos->np; i ++) {
        /* remove halos that are shorter than nmin */
        if(halos->length[i] < finder->nmin) {
            mask[i] = 0;
        } else {
            mask[i] = 1;
        }
    }
    fastpm_fof_subsample(finder, halos, mask, head);
    fastpm_memory_free(finder->p->mem, mask);

    fastpm_info("After length cut we have %td halos (%td including ghost halos).\n",
            fastpm_store_get_mask_sum(halos, comm),
            fastpm_store_get_np_total(halos, comm));
}

/* first every undecided halo; then the real halo with particles */
static int
FastPMLocalSortByMinID(const int i1,
                    const int i2,
                    FastPMStore * p)
{
    int v1 = (p->minid[i1] < p->minid[i2]);
    int v2 = (p->minid[i1] > p->minid[i2]);

    return v2 - v1;
}

static int
FastPMTargetMinID(FastPMStore * store, ptrdiff_t i, void * userdata)
{
    FastPMFOFFinder * finder = userdata;

    const uint32_t GOLDEN32 = 2654435761ul;
    /* const uint64_t GOLDEN64 = 11400714819323198549; */
    /* may over flow, but should be okay here as the periodicity is ggt NTask */
    int key = (store->minid[i] * GOLDEN32) % (unsigned) finder->priv->NTask;
    return key;
}

static int
FastPMTargetTask(FastPMStore * store, ptrdiff_t i, void * userdata)
{
    return store->task[i];
}

/* for debugging, move particles to a spatially unrelated rank */
static int
FastPMTargetFOF(FastPMStore * store, ptrdiff_t i, void * userdata)
{
#ifdef FASTPM_FOF_DEBUG
    PM * pm = userdata;
    int NTask;
    MPI_Comm_size(pm_comm(pm), &NTask);
    const uint32_t GOLDEN32 = 2654435761ul;
    /* const uint64_t GOLDEN64 = 11400714819323198549; */
    /* may over flow, but should be okay here as the periodicity is ggt NTask */
    int key = (store->id[i] * GOLDEN32) % (unsigned) NTask;
    return key;
#else
    return FastPMTargetPM(store, i, userdata);
#endif
}
/*
 * This function group halos by halos->minid[i].
 *
 * All halos with the same minid will have the same attribute values afterwards, except
 * a few book keeping items named in this function (see comments inside)
 *
 * */
static void
fastpm_fof_reduce_halo_attrs(FastPMFOFFinder * finder, FastPMStore * halos,
        void (* add_func) (FastPMFOFFinder * finder, FastPMStore * halo1, ptrdiff_t i1, FastPMStore * halo2, ptrdiff_t i2 ),
        void (* reduce_func)(FastPMFOFFinder * finder, FastPMStore * halo1, ptrdiff_t i1)
)
{

    ptrdiff_t i;

    ptrdiff_t first = -1;
    uint64_t lastminid = 0;

    /* ind is the array to use to replicate items */
    int * ind = fastpm_memory_alloc(finder->p->mem, "HaloPermutation", sizeof(ind[0]) * halos->np, FASTPM_MEMORY_STACK);

    /* the following items will have mask[i] == 1, but we will mark some to 0 if
     * they are not the principle (first) halos segment with this minid */
    for(i = 0; i < halos->np + 1; i++) {
        /* we use i == halos->np to terminate the last segment */
        if(first == -1 || i == halos->np || lastminid != halos->minid[i]) {
            if (first >= 0) {
                /* a segment ended */
                ptrdiff_t j;
                for(j = first; j < i; j ++) {
                    ind[j] = first;
                }
            }
            if (i < halos->np) {
                /* a segment started */
                halos->mask[i] = 1;
                lastminid = halos->minid[i];
            }
            /* starting a segment of the same halos */
            first = i;
        } else {
            /* inside segment, simply add the ith halo to the first of the segment */
            halos->mask[i] = 0;
            add_func(finder, halos, first, halos, i);
        }
    }

    /* use permute to replicate the first halo attr to the rest:
     *
     * we do not want to replicate
     * - fof, as fof.task is the original mpi rank of the halo
     * - id, as it is the original location of the halo on the original mpi rank.
     * - mask, whether it is primary or not
     *
     * we need to replicate because otherwise when we return the head array on the
     * original ranks will be violated.
     *   */

    FastPMStore save[1];

    /* FIXME: add a method for this! */
    memcpy(save->columns, halos->columns, sizeof(save->columns));

    halos->minid = NULL;
    halos->task = NULL;
    halos->id = NULL;
    halos->mask = NULL;

    fastpm_store_permute(halos, ind);

    memcpy(halos->columns, save->columns, sizeof(save->columns));

    for(i = 0; i < halos->np; i++) {
        reduce_func(finder, halos, i);
    }

    fastpm_memory_free(finder->p->mem, ind);
}

static void
fastpm_fof_compute_halo_attrs(FastPMFOFFinder * finder, FastPMStore * halos,
            ptrdiff_t * head,
            void (*convert_func)(FastPMFOFFinder * finder, FastPMStore * p, ptrdiff_t i, FastPMStore * halos),
            void (*add_func)(FastPMFOFFinder * finder, FastPMStore * halos, ptrdiff_t i1, FastPMStore * halos2, ptrdiff_t i2),
            void (*reduce_func)(FastPMFOFFinder * finder, FastPMStore * halos, ptrdiff_t i)
)
{
    MPI_Comm comm = finder->priv->comm;

    FastPMStore h1[1];
    fastpm_store_init(h1, "FOF", 1, halos->attributes, FASTPM_MEMORY_HEAP);
    ptrdiff_t i;

    for(i = 0; i < finder->p->np; i++) {
        ptrdiff_t hid = head[i];
        if(hid < 0) continue;

        if(hid >= halos->np) {
            fastpm_raise(-1, "halo of a particle out of bounds (%td > %td)\n", hid, halos->np);
        }

        /* initialize h1 with existing halo attributes for this particle */
        fastpm_store_take(halos, hid, h1, 0);

        convert_func(finder, finder->p, i, h1);

        add_func(finder, halos, hid, h1, 0);
    }

    fastpm_store_destroy(h1);
    /* decompose halos by minid (gather); if all halo segments of the same minid are on the same rank, we can combine
     * these into a single entry, then replicate for each particle to look up;
     * halo segments that have no local particles are never exchanged to another rank. */
    if(0 != fastpm_store_decompose(halos,
            (fastpm_store_target_func) FastPMTargetMinID, finder, comm)) {

        fastpm_raise(-1, "out of space sending halos by MinID.\n");
    }

    /* now head[i] is no longer the halo attribute of particle i. */

    /* to combine, first local sort by minid; those without local particles are moved to the beginning
     * so we can easily skip them. */
    fastpm_store_sort(halos, FastPMLocalSortByMinID);

    /* reduce and update properties */
    fastpm_fof_reduce_halo_attrs(finder, halos, add_func, reduce_func);

    /* decompose halos by task (return) */
    if (0 != fastpm_store_decompose(halos,
            (fastpm_store_target_func) FastPMTargetTask, finder, comm)) {
        fastpm_raise(-1, "out of space for gathering halos this shall never happen.\n");
    }
    /* local sort by id (restore the order) */
    fastpm_store_sort(halos, FastPMLocalSortByID);

    /* now head[i] is again the halo attribute of particle i. */
}

/*
 * compute the attrs of the local halo segments based on local particles.
 * head : the hid of each particle;
 * fofsave : the minid of each particle (unique label of each halo)
 *
 * if a halo has no local particles,  halos->mask[i] is set to 0.
 * if a halo has any local particles, and halos->mask[i] is set to 1.
 * */
static void
fastpm_fof_remove_empty_halos(FastPMFOFFinder * finder, FastPMStore * halos, uint64_t * minid, ptrdiff_t * head)
{

    /* */
    ptrdiff_t i;

    /* set minid and task of the halo; all of the halo particles of the same minid needs to be reduced */
    for(i = 0; i < finder->p->np; i++) {
        /* set the minid of the halo; need to take care of the ghosts too.
         * even though we do not add them to the attributes */
        ptrdiff_t hid = head[i];

        if(hid < 0) continue;

        if(halos->mask[hid] == 0) {
            /* halo will be reduced by minid */
            halos->minid[hid] = minid[i];
            halos->mask[hid] = 1;
        } else {
            if(halos->minid[hid] != minid[i]) {
                fastpm_raise(-1, "Consistency check failed after FOF global merge.\n");
            }
        }
    }

    /* halo will be returned to this task;
     * we save ThisTask here.*/
    for(i = 0; i < halos->np; i ++) {
        halos->task[i] = finder->priv->ThisTask;
    }

    fastpm_fof_subsample(finder, halos, halos->mask, head);
}

static void
_add_basic_halo_attrs(FastPMFOFFinder * finder, FastPMStore * halos, ptrdiff_t hid, FastPMStore * h1, ptrdiff_t i)
{
    double * boxsize = finder->priv->boxsize;

    if(halos->aemit)
        halos->aemit[hid] += h1->aemit[i];

    int d;

    for(d = 0; d < 3; d++) {
        if(halos->v)
            halos->v[hid][d] += h1->v[i][d];
        if(halos->dx1)
            halos->dx1[hid][d] += h1->dx1[i][d];
        if(halos->dx2)
            halos->dx2[hid][d] += h1->dx2[i][d];
    }

    if(halos->x) {
        for(d = 0; d < 3; d++) {
            if(boxsize) {
                halos->x[hid][d] = periodic_add(
                    halos->x[hid][d] / halos->length[hid], halos->length[hid],
                    h1->x[i][d] / h1->length[i], h1->length[i], boxsize[d]);
            } else {
                halos->x[hid][d] += h1->x[i][d];
            }
        }
    }

    if(halos->q) {
        for(d = 0; d < 3; d ++) {
            if (boxsize) {
                halos->q[hid][d] = periodic_add(
                    halos->q[hid][d] / halos->length[hid], halos->length[hid],
                    h1->q[i][d] / h1->length[i], h1->length[i], boxsize[d]);
            } else {
                halos->q[hid][d] += h1->q[i][d];
            }
        }
    }
    /* do this after the loop because x depends on the old length. */
    halos->length[hid] += h1->length[i];
}

/* convert a particle to the first halo in the halo store */
static void
_convert_basic_halo_attrs(FastPMFOFFinder * finder, FastPMStore * p, ptrdiff_t i, FastPMStore * halos)
{
    int hid = 0;

    int d;

    double q[3];

    if(halos->q && fastpm_store_has_q(p)) {
        fastpm_store_get_q_from_id(p, p->id[i], q);
    }
    halos->length[hid] = 1;

    for(d = 0; d < 3; d++) {
        if(halos->x)
            halos->x[hid][d] = p->x[i][d];
        if(halos->v)
            halos->v[hid][d] = p->v[i][d];
        if(halos->dx1)
            halos->dx1[hid][d] = p->dx1[i][d];
        if(halos->dx2)
            halos->dx2[hid][d] = p->dx2[i][d];
        if(halos->q && fastpm_store_has_q(p)) {
            halos->q[hid][d] = q[d];
        }
    }

    if(halos->aemit)
        halos->aemit[hid] = p->aemit[i];
}
static void
_reduce_basic_halo_attrs(FastPMFOFFinder * finder, FastPMStore * halos, ptrdiff_t i)
{
    int d;
    double n = halos->length[i];

    for(d = 0; d < 3; d++) {
        if(halos->x)
            halos->x[i][d] /= n;
        if(halos->v)
            halos->v[i][d] /= n;
        if(halos->dx1)
            halos->dx1[i][d] /= n;
        if(halos->dx2)
            halos->dx2[i][d] /= n;
        if(halos->q)
            halos->q[i][d] /= n;
    }
    if(halos->aemit)
        halos->aemit[i] /= n;
}

static void
_add_extended_halo_attrs(FastPMFOFFinder * finder, FastPMStore * h1, ptrdiff_t i1, FastPMStore * h2, ptrdiff_t i2)
{
    int d;
    if(h1->rvdisp) {
        for(d = 0; d < 9; d ++) {
            h1->rvdisp[i1][d] += h2->rvdisp[i2][d];
        }
    }
    if(h1->vdisp) {
        for(d = 0; d < 6; d ++) {
            h1->vdisp[i1][d] += h2->vdisp[i2][d];
        }
    }
    if(h1->rdisp) {
        for(d = 0; d < 6; d ++) {
            h1->rdisp[i1][d] += h2->rdisp[i2][d];
        }
    }
}

static void
_convert_extended_halo_attrs(FastPMFOFFinder * finder, FastPMStore * p, ptrdiff_t i, FastPMStore * halos)
{
    int hid = 0;

    int d;
    double rrel[3];

    for(d = 0; d < 3; d ++) {
        rrel[d] = p->x[i][d] - halos->x[hid][d];

        if(finder->priv->boxsize) {
            double L = finder->priv->boxsize[d];
            while(rrel[d] > L / 2) rrel[d] -= L;
            while(rrel[d] < -L / 2) rrel[d] += L;
        }
    }

    if(halos->vdisp) {
        /* FIXME: add hubble expansion term based on aemit and hubble function? needs to modify Finder object */

        double vrel[3];
        for(d = 0; d < 3; d ++) {
            vrel[d] = p->v[i][d] - halos->v[hid][d];
        }
        for(d = 0; d < 3; d ++) {
            halos->vdisp[hid][d] = vrel[d] * vrel[d];
            halos->vdisp[hid][d + 3] = vrel[d] * vrel[(d + 1) % 3];
        }
    }
    if(halos->rvdisp) {
        /* FIXME: add hubble expansion term based on aemit and hubble function? needs to modify Finder object */

        double vrel[3];
        for(d = 0; d < 3; d ++) {
            vrel[d] = p->v[i][d] - halos->v[hid][d];
        }
        for(d = 0; d < 3; d ++) {
            halos->rvdisp[hid][d] = rrel[d] * vrel[d];
            halos->rvdisp[hid][d + 3] = rrel[d] * vrel[(d + 1) % 3];
            halos->rvdisp[hid][d + 6] = rrel[d] * vrel[(d + 2) % 3];
        }
    }
    if(halos->rdisp) {
        for(d = 0; d < 3; d ++) {
            halos->rdisp[hid][d] = rrel[d] * rrel[d];
            halos->rdisp[hid][d + 3] = rrel[d] * rrel[(d + 1) % 3];
        }
    }
}
static void
_reduce_extended_halo_attrs(FastPMFOFFinder * finder, FastPMStore * halos, ptrdiff_t hid)
{
    double n = halos->length[hid];
    int d;
    if(halos->rvdisp) {
        for(d = 0; d < 9; d ++) {
            halos->rvdisp[hid][d] /= n;
        }
    }
    if(halos->vdisp) {
        for(d = 0; d < 6; d ++) {
            halos->vdisp[hid][d] /= n;
        }
    }
    if(halos->rdisp) {
        for(d = 0; d < 3; d ++) {
            halos->rdisp[hid][d] /= n;
        }
    }
}

/* This function creates the storage object for halo segments that are local on this
 * rank. We store mnay attributes. We only allow a flucutation of 2 around avg_halos.
 * this should be OK, since we will only redistribute by the MinID, which are supposed
 * to be very uniform.
 * */
static void
fastpm_fof_create_local_halos(FastPMFOFFinder * finder, FastPMStore * halos, size_t nhalos)
{

    MPI_Comm comm = finder->priv->comm;

    FastPMColumnTags attributes = finder->p->attributes;
    attributes |= COLUMN_MASK;
    attributes |= COLUMN_LENGTH | COLUMN_MINID | COLUMN_TASK;
    attributes |= COLUMN_RDISP | COLUMN_VDISP | COLUMN_RVDISP;
    attributes |= COLUMN_ACC; /* ACC used as the first particle position offset */
    attributes &= ~COLUMN_POTENTIAL;
    attributes &= ~COLUMN_DENSITY;
    attributes &= ~COLUMN_TIDAL;

    /* store initial position only for periodic case. non-periodic suggests light cone and
     * we cannot infer q from ID sensibly. (crashes there) */
    if(finder->priv->boxsize) {
        attributes |= COLUMN_Q;
    } else {
        attributes &= ~COLUMN_Q;
    }

    double avg_halos;
    double max_halos;
    /* + 1 to ensure avg_halos > 0 */
    MPIU_stats(comm, nhalos + 1, "->", &avg_halos, &max_halos);

    fastpm_info("Allocating %d halos per rank for final catalog.\n", (size_t) max_halos * 2);

    /* give it enough space for rebalancing. */
    fastpm_store_init(halos, NULL, (size_t) (max_halos * 2),
            attributes,
            FASTPM_MEMORY_HEAP);

    halos->np = nhalos;
    halos->meta = finder->p->meta;

    ptrdiff_t i;
    for(i = 0; i < halos->np; i++) {
        halos->id[i] = i;

        halos->mask[i] = 0; /* unselect the halos ; will turn this only if any particle is used */
        /* everthing should have been set to zero already by fastpm_store_init */
    }
}

void
fastpm_fof_execute(FastPMFOFFinder * finder, FastPMStore * halos)
{
    /* initial decompose -- reduce number of ghosts */
    FastPMStore * p = finder->p;
    PM * pm = finder->pm;
    MPI_Comm comm = finder->priv->comm;

    /* only do wrapping for periodic data */
    if(finder->priv->boxsize)
        fastpm_store_wrap(p, finder->priv->boxsize);

    double npmax, npmin, npstd, npmean;

    MPIU_stats(comm, p->np, "<->s", &npmin, &npmean, &npmax, &npstd);

    fastpm_info("load balance before fof decompose : min = %g max = %g mean = %g std = %g\n",
        npmin, npmax, npmean, npstd
        );

#if 1
    /* still route particles to the pm pencils as if they are periodic. */
    /* should still work (albeit use crazy memory) if we skip this. */
    if(0 != fastpm_store_decompose(p,
                (fastpm_store_target_func) FastPMTargetFOF, pm, comm)
    ) {
        fastpm_raise(-1, "out of storage space decomposing for FOF\n");
    }
#endif
    MPIU_stats(comm, p->np, "<->s", &npmin, &npmean, &npmax, &npstd);

    fastpm_info("load balance after fof decompose : min = %g max = %g mean = %g std = %g\n",
        npmin, npmax, npmean, npstd
        );

    /* create ghosts mesh size is usually > ll so we are OK here. */
    double below[3], above[3];

    int d;
    for(d = 0; d < 3; d ++) {
        /* bigger padding reduces number of iterations */
        below[d] = -finder->linkinglength * 1;
        above[d] = finder->linkinglength * 1;
    }

    PMGhostData * pgd = pm_ghosts_create_full(pm, p,
            COLUMN_POS | COLUMN_ID | COLUMN_MINID,
            below, above
        );

    pm_ghosts_send(pgd, COLUMN_POS);
    pm_ghosts_send(pgd, COLUMN_ID);

    size_t np_and_ghosts = p->np + pgd->p->np;

    #ifdef FASTPM_FOF_DEBUG
    fastpm_ilog(INFO, "Rank %d has %td particles including ghost\n", finder->priv->ThisTask, np_and_ghosts);
    #endif

    ptrdiff_t * head = fastpm_memory_alloc(p->mem, "FOFHead",
                    sizeof(head[0]) * np_and_ghosts, FASTPM_MEMORY_STACK);

    FastPMStore savebuff[1];
    fastpm_store_init(savebuff, p->name, np_and_ghosts, COLUMN_MINID, FASTPM_MEMORY_STACK);

    _fof_local_find(finder, p, pgd, head, finder->linkinglength);

    _fof_global_merge (finder, p, pgd, savebuff->minid, head);

    /* assign halo attr entries. This will keep only candidates that can possibly reach to nmin */
    size_t nsegments = _assign_halo_attr(finder, pgd, head, p->np, pgd->p->np, finder->nmin);

    fastpm_info("Found %td halos segments >= %d particles; or cross linked. \n", nsegments, finder->nmin);
    pm_ghosts_free(pgd);

    /* create local halos and modify head to index the local halos */
    fastpm_fof_create_local_halos(finder, halos, nsegments);
    /* remove halos without any local particles */
    fastpm_fof_remove_empty_halos(finder, halos, savebuff->minid, head);

    fastpm_store_destroy(savebuff);

    /* reduce the primary halo attrs */
    fastpm_fof_compute_halo_attrs(finder, halos, head, _convert_basic_halo_attrs, _add_basic_halo_attrs, _reduce_basic_halo_attrs);

    #ifdef FASTPM_FOF_DEBUG
    {
        int i;
        for(i  = 0; i < halos->np; i ++) {
            fastpm_ilog(INFO, "Task = %d, Halo[%d] = %d mask=%d MINID=%ld\n", finder->priv->ThisTask, i, halos->length[i], halos->mask[i], halos->minid[i]);
        }
    }
    #endif

    /* apply length cut */
    fastpm_fof_apply_length_cut(finder, halos, head);

    /* reduce the primary halo attrs */
    fastpm_fof_compute_halo_attrs(finder, halos, head, _convert_extended_halo_attrs, _add_extended_halo_attrs, _reduce_extended_halo_attrs);

    /* the event is called with full halos, only those where mask==1 are primary
     * the others are ghosts with the correct properties but shall not show up in the
     * catalog.
     * */
    FastPMHaloEvent event[1];
    event->halos = halos;
    event->p = finder->p;
    event->ihalo = head;

    fastpm_emit_event(finder->event_handlers, FASTPM_EVENT_HALO,
                    FASTPM_EVENT_STAGE_AFTER, (FastPMEvent*) event, finder);

    fastpm_memory_free(finder->p->mem, head);

    fastpm_store_subsample(halos, halos->mask, halos);

    fastpm_info("After event: %td halos.\n", fastpm_store_get_np_total(halos, comm));
}

void
fastpm_fof_destroy(FastPMFOFFinder * finder)
{
    fastpm_destroy_event_handlers(&finder->event_handlers);
    free(finder->priv);
}