text:
/*
 *  vbHmmPcFret.c
 *  Model-specific core functions for VB-HMM-PC-FRET.
 *
 *  Created by OKAMOTO Kenji, SAKO Yasushi and RIKEN
 *  Copyright 2011-2015
 *  Cellular Informatics Laboratory, Advance Science Institute, RIKEN, Japan.
 *  All rights reserved.
 *
 *  Ver. 1.1.0
 *  Last modified on 2015.09.17
 */

#include "vbHmmPcFret.h"
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_psi.h>
#include <string.h>
#include "rand.h"

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

//#define  DEBUG

//// Uncomment one/both of the following defenitions to activate constraint on I and K.
//#define  INTENSITY_CAP
#ifdef INTENSITY_CAP
#define  maxIntensityRatio  10.0
#endif

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

//static int isGlobalAnalysis = 0;

void setFunctions_pcFret(){
    commonFunctions funcs;
    funcs.newModelParameters    = newModelParameters_pcFret;
    funcs.freeModelParameters   = freeModelParameters_pcFret;
    funcs.newModelStats         = newModelStats_pcFret;
    funcs.freeModelStats        = freeModelStats_pcFret;
    funcs.initializeVbHmm       = initializeVbHmm_pcFret;
    funcs.pTilde_z1             = pTilde_z1_pcFret;
    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_pcFret;
    funcs.pTilde_xn_zn          = pTilde_xn_zn_pcFret;
    funcs.calcStatsVars         = calcStatsVars_pcFret;
    funcs.maximization          = maximization_pcFret;
    funcs.varLowerBound         = varLowerBound_pcFret;
    funcs.reorderParameters     = reorderParameters_pcFret;
    funcs.outputResults         = outputResults_pcFret;
    setFunctions( funcs );
}

//void setGFunctions_pcFret(){
//    gCommonFunctions funcs;
//    funcs.newModelParameters    = newModelParameters_pcFret;
//    funcs.freeModelParameters   = freeModelParameters_pcFret;
//    funcs.newModelStats         = newModelStats_pcFret;
//    funcs.freeModelStats        = freeModelStats_pcFret;
//    funcs.newModelStatsG        = newModelStatsG_pcFret;
//    funcs.freeModelStatsG       = freeModelStatsG_pcFret;
//    funcs.initializeVbHmmG      = initializeVbHmmG_pcFret;
//    funcs.pTilde_z1             = pTilde_z1_pcFret;
//    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_pcFret;
//    funcs.pTilde_xn_zn          = pTilde_xn_zn_pcFret;
//    funcs.calcStatsVarsG        = calcStatsVarsG_pcFret;
//    funcs.maximizationG         = maximizationG_pcFret;
//    funcs.varLowerBoundG        = varLowerBoundG_pcFret;
//    funcs.reorderParametersG    = reorderParametersG_pcFret;
//    funcs.outputResultsG        = outputResultsG_pcFret;
//    setGFunctions( funcs );
//    isGlobalAnalysis = 1;
//}

void outputResults_pcFret( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    outputPcFretResults( xn, gv, iv, logFP );
}

//void outputResultsG_pcFret( xns, gv, ivs, logFP )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//FILE *logFP;
//{
//    outputPcFretResultsG( xns, gv, ivs, logFP );
//}

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

    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->aIArr = (double*)malloc( sNo * sizeof(double) );
    p->bIArr = (double*)malloc( sNo * sizeof(double) );
    p->uEArr = (double*)malloc( sNo * sizeof(double) );
    p->vEArr = (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) );
    }

    p->avgI = (double *)malloc( sNo * sizeof(double) );
    p->avgLnI = (double *)malloc( sNo * sizeof(double) );
    p->avgE = (double *)malloc( sNo * sizeof(double) );
    p->avgLnE = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   p->avgLnE[i] = (double *)malloc( 2 * sizeof(double) );   }

    return p;
}

void freeModelParameters_pcFret( p, xn, sNo )
void **p;
xnDataSet *xn;
int sNo;
{
    pcFretParameters *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->aIArr );
    free( gp->bIArr );
    free( gp->uEArr );
    free( gp->vEArr );

    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( gp->avgI );
    free( gp->avgLnI );
    free( gp->avgE );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->avgLnE[i] );
    }
    free( gp->avgLnE );

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

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

    int i;
    s->Ni = (double *)malloc( sNo * sizeof(double) );
    s->Ci = (double *)malloc( sNo * sizeof(double) );
    s->Di = (double *)malloc( sNo * sizeof(double) );
    s->Ai = (double *)malloc( sNo * sizeof(double) );
    s->Mi = (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) );   }

    return s;

//    } else {
//
//        return NULL;
//
//    }
}

void freeModelStats_pcFret( s, xn, gv, iv )
void **s;
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
//    if( isGlobalAnalysis == 0 ){
    int sNo = gv->sNo;
    pcFretStats *gs = *s;
    int i;
    free( gs->Ni );
    free( gs->Ci );
    free( gs->Di );
    free( gs->Ai );
    free( gs->Mi );
    for( i = 0 ; i < sNo ; i++ )
    {   free( gs->Nij[i] );   }
    free( gs->Nij );

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

//void *newModelStatsG_pcFret( xns, gv, ivs)
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//    int sNo = gv->sNo;
//    pcFretGlobalStats *gs = (pcFretGlobalStats*)malloc( sizeof(pcFretGlobalStats) );
//
//    return gs;
//}

//void freeModelStatsG_pcFret( gs, xns, gv, ivs )
//void **gs;
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//    int sNo = gv->sNo;
//    pcFretGlobalStats *ggs = *gs;
//
//    free( *gs );
//    *gs = NULL;
//}

void initializeVbHmm_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcFretData *d = xn->data;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    pcFretParameters *p = gv->params;

    int i, j;
    size_t totalC = 0;
    for( i = 0 ; i < dLen ; i++ ){
        totalC += d->dCounts[i] + d->aCounts[i];
    }
    double meanI = (double)totalC / (double)dLen;

    // 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] = 100.0;
            } else {
                p->uAMat[i][j] = 1.0;
            }
            p->sumUAArr[i] += p->uAMat[i][j];
        }
    }

    // hyper parameter for p( I(k) )
    for( i = 0 ; i < sNo ; i++ ){
        p->aIArr[i] = 1.0;
        p->bIArr[i] = 1.0 / meanI;
    }

    // hyper parameter for p( E(i) )
    for( i = 0 ; i < sNo ; i++ ){
        p->uEArr[i] = 1.0;
        p->vEArr[i] = 1.0;
    }

    initialize_indVars_pcFret( xn, gv, iv );

    calcStatsVars_pcFret( xn, gv, iv );
    maximization_pcFret( xn, gv, iv );
}

//void initializeVbHmmG_pcFret( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void initialize_indVars_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    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_pcFret( 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 = (pcFretData*)malloc( sizeof(pcFretData) );
    pcFretData *d = (pcFretData*)xn->data;
    d->binSize = 0.0;
    d->dCounts = NULL;
    d->aCounts = NULL;
    return xn;
}

void freeXnDataSet_pcFret( xn )
xnDataSet **xn;
{
    pcFretData *d = (pcFretData*)(*xn)->data;
    free( d->dCounts );
    free( d->aCounts );
    free( (*xn)->data );
    free( (*xn)->name );
    free( *xn );
    *xn = NULL;
}

double pTilde_z1_pcFret( i, params )
int i;
void *params;
{
    pcFretParameters *p = (pcFretParameters*)params;
    return exp( p->avgLnPi[i] );
}

double pTilde_zn_zn1_pcFret( i, j, params )
int i, j;
void *params;
{
    pcFretParameters *p = (pcFretParameters*)params;
    return exp( p->avgLnA[i][j] );
}

double pTilde_xn_zn_pcFret( xn, n, i, params )
xnDataSet *xn;
size_t n;
int i;
void *params;
{
    pcFretParameters *p = (pcFretParameters*)params;
    pcFretData *d = (pcFretData*)xn->data;
    return exp( d->dCounts[n]*p->avgLnE[i][0] + d->aCounts[n]*p->avgLnE[i][1] + (d->dCounts[n]+d->aCounts[n])*p->avgLnI[i] - p->avgI[i] ) / gsl_sf_fact( d->dCounts[n] ) / gsl_sf_fact( d->aCounts[n] );
}

void calcStatsVars_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcFretData *d = (pcFretData*)xn->data;
    pcFretStats *s = (pcFretStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *Ni = s->Ni, *Ci = s->Ci, *Di = s->Di, *Ai = s->Ai;
    double *Mi = s->Mi, **Nij = s->Nij;
    size_t n;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        Ni[i] = 1e-10;
        Ci[i] = 1e-10;
        Di[i] = 1e-10;
        Ai[i] = 1e-10;
        Mi[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];
            Di[i]  += gmMat[n][i] * (double)d->dCounts[n];
            Ai[i]  += gmMat[n][i] * (double)d->aCounts[n];
            for( j = 0 ; j < sNo ; j++ ){
                Mi[i]     += xiMat[n][i][j];
                Nij[i][j] += xiMat[n][i][j];
            }
        }
        Ci[i] = Di[i] + Ai[i];
    }

//#ifdef DEBUG
//#pragma omp critical
//{
//    for( n = 0 ; n < 20 ; n++ ){
//        for( i = 0 ; i < sNo ; i++ ){
//            fprintf(logFP, "%g,", gmMat[n][i]);
//        }
//        fprintf(logFP, "; ");
//    }
//    fprintf(logFP, "\n");
//    for( i = 0 ; i < sNo ; i++ ){
//        fprintf(logFP, "Ni(%d)=%g,  ", i, Ni[i]);
//        fprintf(logFP, "Ti(%d)=%g,  ", i, Ti[i]);
//        fprintf(logFP, "Mi(%d)=%g,  ", i, Mi[i]);
//        for( j = 0 ; j < sNo ; j++ ){
//            if( j != i )
//                fprintf(logFP, "Nij(%d,%d)=%g, ", i, j, Nij[i][j]);
//        }
//        fprintf(logFP, "\n");
//    }
//}
//#endif
}

//void calcStatsVarsG_pcFret( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void maximization_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcFretParameters *p = (pcFretParameters*)gv->params;
    pcFretStats *s = (pcFretStats*)iv->stats;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uEArr = p->uEArr, *vEArr = p->vEArr;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgE = p->avgE, **avgLnE = p->avgLnE;
    double *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *Ni = s->Ni, *Ci = s->Ci, *Di = s->Di, *Ai = s->Ai;
    double *Mi = s->Mi, **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] + Mi[i] );
            avgLnA[i][j] = gsl_sf_psi( uAMat[i][j] + Nij[i][j] ) - gsl_sf_psi( sumUAArr[i] + Mi[i] );
        }

        avgI[i] = (Ci[i] + aIArr[i]) / (Ni[i] + bIArr[i]);
        avgLnI[i] = gsl_sf_psi( Ci[i] + aIArr[i] ) - log( Ni[i] + bIArr[i] );
#ifdef  INTENSITY_CAP
        size_t n, totalC = 0;
        pcFretData *pc = xnWv->data;
        for( n = 0 ; n < xnWv->N ; n++ ){
            totalC += pc->dCounts[n] + pc->aCounts[n];
        }
        double meanI = (double)totalC / (double)xnWv->N;
        avgI[i] = MIN( avgI[i], maxIntensityRatio * meanI );
        avgLnI[i] = MIN( avgLnI[i], log(maxIntensityRatio * meanI) );
#endif

        avgE[i] = ( uEArr[i] + Ai[i] ) / ( uEArr[i] + vEArr[i] + Ci[i] );
        // ln(1-E) for donor
        avgLnE[i][0]  = gsl_sf_psi( vEArr[i] + Di[i] ) - gsl_sf_psi( uEArr[i] + vEArr[i] + Ci[i] );
        // ln(E) for acceptor
        avgLnE[i][1]  = gsl_sf_psi( uEArr[i] + Ai[i] ) - gsl_sf_psi( uEArr[i] + vEArr[i] + Ci[i] );
    }
}

//void maximizationG_pc( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

double varLowerBound_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcFretParameters *p = (pcFretParameters*)gv->params;
    pcFretStats *s = (pcFretStats*)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, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uEArr = p->uEArr, *vEArr = p->vEArr;
    double *avgLnPi = p->avgLnPi, **avgLnA = p->avgLnA;
    double *avgI = p->avgI, *avgLnI = p->avgLnI, **avgLnE = p->avgLnE;
    double *Ni = s->Ni, *Ci = s->Ci, *Di = s->Di, *Ai = s->Ai;
    double *Mi = s->Mi, **Nij = s->Nij;
    size_t n;
    int i, j;

    double lnpPi = gsl_sf_lngamma(sumUPi);
    double lnpA = 0.0;
    double lnpI = 0.0;
    double lnpE = 0.0;
    double lnqPi = gsl_sf_lngamma(sumUPi + 1.0);
    double lnqA = 0.0;
    double lnqI = 0.0;
    double lnqE = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1.0) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnpI += aIArr[i] * log(bIArr[i]) - gsl_sf_lngamma(aIArr[i]);
        lnpI += (aIArr[i] - 1.0) * avgLnI[i] - bIArr[i] * avgI[i];

        lnpE += gsl_sf_lngamma(uEArr[i]+vEArr[i]) - gsl_sf_lngamma(uEArr[i]);
        lnpE += -gsl_sf_lngamma(vEArr[i]);
        lnpE += (uEArr[i]-1.0)*avgLnE[i][1] + (vEArr[i]-1.0)*avgLnE[i][0];

        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]);

        lnqI += (Ci[i] + aIArr[i]) * log(Ni[i] + bIArr[i]) - gsl_sf_lngamma(Ci[i] + aIArr[i]);
        lnqI += (Ci[i] + aIArr[i] - 1.0) * avgLnI[i] - (Ni[i] + bIArr[i]) * avgI[i];

        lnqE += gsl_sf_lngamma(uEArr[i] + vEArr[i] + Ci[i]);
        lnqE -= gsl_sf_lngamma(uEArr[i] + Ai[i]);
        lnqE -= gsl_sf_lngamma(vEArr[i] + Di[i]);
        lnqE += (uEArr[i] + Ai[i] - 1.0) * avgLnE[i][1];
        lnqE += (vEArr[i] + Di[i] - 1.0) * avgLnE[i][0];

        lnpA += gsl_sf_lngamma(sumUAArr[i]);
        lnqA += gsl_sf_lngamma(sumUAArr[i] + Mi[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]+Mi[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 + lnpI + lnpE;
    val -= lnqPi + lnqA + lnqI + lnqE;
    val += lnpX;
    val += log(gsl_sf_fact(sNo));

//#ifdef DEBUG
//#pragma omp critical
//{
//    FILE *logFP = stderr;
//    if( val > 100000 ){
//        fprintf(logFP, "  > %g; %g; %g; %g;", lnpPi, lnpA, lnpI, lnpE);
//        fprintf(logFP, " %g; %g; %g; %g; %g\n", lnqPi, lnqA, lnqI, lnqE, lnpX);
//    }
//}
//#endif

    return val;
}

//double varLowerBoundG_pcFret( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void reorderParameters_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcFretParameters *p = (pcFretParameters*)gv->params;
    pcFretStats *s = (pcFretStats*)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;
    double **avgLnA = p->avgLnA;
    double *avgI = p->avgI, *avgLnI = p->avgLnI, *avgE = p->avgE, **avgLnE = p->avgLnE;
    double *Ni = s->Ni, *Ci = s->Ci, *Di = s->Di, *Ai = s->Ai;
    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 avgE values (0=biggest avgE -- sNo=smallest avgE).
    for( i = 0 ; i < sNo ; i++ ){
        index[i] = sNo - 1;
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                if( avgE[i] < avgE[j] ){
                    index[i]--;
                } else if( avgE[i] == avgE[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( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgI[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnI[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]] = avgE[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgE[i] = store[i];   }

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

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

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

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

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ai[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ai[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_pc( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void outputPcFretResults( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    pcFretParameters *p = (pcFretParameters*)gv->params;
    int sNo = gv->sNo;
    int i, j;
    fprintf(logFP, "  results: K = %d \n", sNo);

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

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

    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, "I, E, pi");
        for( i = 0 ; i < sNo ; i++ )
        {   fprintf(fp, ", A%dx", i);   }
        fprintf(fp, "\n");

        for( i = 0 ; i < sNo ; i++ ){
            fprintf(fp, "%g, %g", p->avgI[i], p->avgE[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 outputPcFretResultsG( xns, gv, ivs, logFP )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//FILE *logFP;
//{
//}

//

max_stars_repo_name: okamoto-kenji/varBayes-HMM

max_issues_repo_name: okamoto-kenji/varBayes-HMM

max_forks_repo_name: okamoto-kenji/varBayes-HMM


text:
// The GSL extra library provides linear algebra functions using a
// similar interface to the rest of the GSL library.

#ifndef COMMON_C_MATH_GSL_LINALG_EXTRA_H_
#define COMMON_C_MATH_GSL_LINALG_EXTRA_H_

#include <stdint.h>

#include <gsl/gsl_blas_types.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

#ifdef __cplusplus
extern "C" {
#endif

int32_t GslTriangularSolve(CBLAS_SIDE_t side, CBLAS_UPLO_t uplo,
                           CBLAS_TRANSPOSE_t transpose, const gsl_matrix *T,
                           const gsl_matrix *B, gsl_matrix *X);
void GslTrapezoidalToTriangular(const gsl_matrix *R, gsl_matrix *T,
                                gsl_vector *tau);
void GslTrapezoidalToTriangularZTMat(const gsl_matrix *T, const gsl_vector *tau,
                                     const gsl_matrix *X, gsl_matrix *Zt_X);
int32_t GslMatrixDivide(CBLAS_SIDE_t side, const gsl_matrix *A,
                        const gsl_matrix *B, gsl_matrix *X);

#ifdef __cplusplus
}  // extern "C"
#endif

#endif  // COMMON_C_MATH_GSL_LINALG_EXTRA_H_

ext: h

max_stars_repo_name: OpenFAST/KiteFAST

max_issues_repo_name: OpenFAST/KiteFAST

max_forks_repo_name: OpenFAST/KiteFAST


text:
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_statistics.h>
#include "tz_error.h"
#include "tz_constant.h"
#include "tz_image_lib.h"
#include "tz_stack_lib.h"
#include "tz_objdetect.h"
#include "tz_imatrix.h"
#include "tz_stack_math.h"
#include "tz_stack_bwdist.h"
#include "tz_stack_bwmorph.h"
#include "tz_voxel_linked_list.h"
#include "tz_stack_sampling.h"
#include "tz_stack_attribute.h"
#include "tz_dimage_lib.h"
#include "tz_arrayview.h"
#include "tz_dmatrix.h"

INIT_EXCEPTION_MAIN(e)

int main(int argc, char* argv[]) {
  char neuron_name[100];
  if (argc == 1) {
    strcpy(neuron_name, "fly_neuron");
  } else {
    strcpy(neuron_name, argv[1]);
  }

  char file_path[100];

#if 0 /* local maxima */
  sprintf(file_path, "../data/%s/mask.tif", neuron_name);
  Stack *stack1 = Read_Stack(file_path);

  sprintf(file_path, "../data/%s/highpass_locmin.tif", neuron_name);
  Stack *stack2 = Read_Stack(file_path);

  Stack_Add(stack1, stack2, stack1);
  sprintf(file_path, "../data/%s/cell_mark.tif", neuron_name);
  Write_Stack(file_path, stack1);
#endif

#if 0
  sprintf(file_path, "../data/%s.tif", neuron_name);
  Stack *stack1 = Read_Stack(file_path);

  DMatrix *filter = Mexihat_2D_D(2.0, NULL);
  filter->ndim = 3;
  filter->dim[2] = 1;

  DMatrix *out = Filter_Stack_Fast_D(stack1, filter, NULL, 0);

# if 1
  int nvoxel = Stack_Voxel_Number(stack1);
  int i;
  for (i = 0; i < nvoxel; i++) {
    out->array[i] = fabs(out->array[i]);
  }
# endif

  Stack *stack2 = Scale_Double_Stack(out->array, out->dim[0], out->dim[1],
				   out->dim[2], GREY);

  sprintf(file_path, "../data/%s/edge.tif", neuron_name);
  Write_Stack(file_path, stack2);

  Kill_Stack(stack1);
  Kill_Stack(stack2);
#endif

#if 1
  sprintf(file_path, "../data/%s/mask.tif", neuron_name);
  Stack *stack2 = Read_Stack(file_path);
  sprintf(file_path, "../data/%s/seeds.pa", neuron_name);
  Pixel_Array *pa = Pixel_Array_Read(file_path);
  double *pa_array = (double *) pa->array;
  double max_dist = gsl_stats_max(pa_array, 1, pa->size);
  double mean_dist = gsl_stats_mean(pa_array, 1, pa->size);
  double std_dist = sqrt(gsl_stats_variance(pa_array, 1, pa->size));

  int erode_size = (int) (mean_dist + std_dist * 2 + 0.5);
  Struct_Element *se = Make_Ball_Se(erode_size);
  Stack *stack3 = Stack_Erode_Fast(stack2, NULL, se);
  int dilate_size = (int) (max_dist + 0.5);
  se = Make_Ball_Se(dilate_size);
  Stack *stack4 = Stack_Dilate(stack3, NULL, se);

  Stack_Sub(stack2, stack4, stack2);
  Write_Stack("../data/test.tif", stack2);

  Kill_Stack(stack2);
  Kill_Stack(stack3);
  Kill_Stack(stack4);
  Kill_Pixel_Array(pa);

#endif

  return 0;
}

ext: c


text:
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <float.h>
#define COMPEARTH_PRIVATE_CROSS3 1
#define COMPEARTH_PRIVATE_NORM3 1
#define COMPEARTH_PRIVATE_DOT3 1
#define COMPEARTH_PRIVATE_WRAP360 1
#include "compearth.h"
#ifdef COMPEARTH_USE_MKL
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreserved-id-macro"
#pragma clang diagnostic ignored "-Wstrict-prototypes"
#endif
#include <mkl_cblas.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#else
#include <cblas.h>
#endif

//#define MAXMT 64

static void setZero(const int nmt, const double tol, double *__restrict__ X);
static void faultVec2Ang(const double *__restrict__ S,
                         const double *__restrict__ N,
                         double *theta, double *sigma,
                         double *kappa, double *__restrict__ K, int *ierr);
static int pickP1(const double thetaA, const double sigmaA, const double kappaA,
                  const double thetaB, const double sigmaB, const double kappaB,
                  const double tol, const int p1, const int p2);

/*!
 * @brief Converts a moment tensor to six parameters of Tape and Tape 2012.
 *        The reverse program is TT2CMT.c
 *
 * @param[in] nmt           Number of moment tensors.
 * @param[in] Min           [6 x nmt] array of moment tensors in CMT
 *                          convention (i.e. UP-SOUTH-EAST).  M is packed:
 *                          \f$
 *                           M = \{ M_{rr}, M_{\theta \theta}, M_{\phi \phi}
 *                                  M_{r \theta}, M_{r \phi}, M_{\theta \phi} \}
 *                          \f$.
 *                          The leading dimension is 6.
 * @param[in] ldisplay      If true then display the results. \n
 *                          Otherwise, this routine will be quiet unless
 *                          an error is encountered.
 *
 * @param[out] gamma        Angle from DC meridian to MT point.
 *                          Note that \f$ \gamma \in [-30, 30] \f$.
 *                          This is an array of dimension [nmt].
 * @param[out] delta        Angle from deviatoric plane to MT point.
 *                          Note that \f$ \delta \in [-90, 90] \f$.
 *                          This is an array of dimension [nmt].
 * @param[out] M0           Seismic moment in N-m.  This is an array of
 *                          dimension [nmt].
 * @param[out] kappa        Strike angle \f$ \kappa \in [0,360] \f$.
 *                          This is an array of dimension [nmt].
 * @param[out] theta        Dip angle \f$ \theta \in [0,90] \f$.
 *                          This is an array of dimension [nmt].
 * @param[out] sigma        Slip (or rake) angle \f$ \sigma \in [-90,90] \f$.
 *                          This is an array of dimension [nmt].
 * @param[out] K            If K is not NULL then it is the strike vector
 *                          (SOUTH-EAST-UP).  In this case K is an array
 *                          of dimension [3 x nmt] with leading dimension 3.
 * @param[out] N            If N is not NULL then it is the normal vector
 *                          (SOUTH-EAST-UP).  In this case N is an array
 *                          of dimension [3 x nmt] with leading dimension 3.
 * @param[out] S            If S Is not NULL then it is the slip vector
 *                          (SOUTH-EAST-UP).  In this case S is an array
 *                          of dimension [3 x nmt] with leading dimension 3.
 * @param[out] thetadc      If thetadc is not NULL then it is angle between
 *                          the moment tensor and the double couple where
 *                          \f$ \theta_{DC} \in [0,90] \f$.
 *                          In this case thetadc is an array of dimension [nmt].
 * @param[out] lam          If lam is not NULL then these are the eigenvalues
 *                          corresponding to the moment tensor.  In this
 *                          case lam is an array of dimension [3 x nmt]
 *                          with leading dimension 3.
 * @param[out] U            If U is not NULL then this is the basis in
 *                          SOUTH-EAST-UP for the moment tensor.  In this
 *                          case U an array of dimension [3 x 3 x nmt] with
 *                          leading dimension 9 and the understanding that
 *                          each 3 x 3 matrix is in column major format.
 *
 * @result 0 indicates success.
 *
 * @author Carl Tape and translated to C by Ben Baker
 *
 * @date July 2017
 *
 * @copyright MIT
 *
 */
int compearth_CMT2TT(const int nmt, const double *__restrict__ Min,
                     const bool ldisplay,
                     double *__restrict__ gamma,
                     double *__restrict__ delta,
                     double *__restrict__ M0,
                     double *__restrict__ kappa,
                     double *__restrict__ theta,
                     double *__restrict__ sigma,
                     double *__restrict__ K, double *__restrict__ N,
                     double *__restrict__ S, double *__restrict__ thetadc,
                     double *__restrict__ lam, double *__restrict__ U)
{
    double *lamdev, *lamiso, Vwork[9], Yrot[9], M[6],
           Sloc[12], Kloc[12], Nloc[12], kappaL[4], sigmaL[4], thetaL[4];
    double *thetadcWork;
    double Uwork[9*CE_CHUNKSIZE] __attribute__((aligned(64)));
    double lamWork[3*CE_CHUNKSIZE] __attribute__((aligned(64)));
    double Nwork[3*CE_CHUNKSIZE] __attribute__((aligned(64)));
    double Swork[3*CE_CHUNKSIZE] __attribute__((aligned(64)));
    //double lamSpace[3*MAXMT];
    //double uSpace[9*MAXMT];
    bool lwantLam, lwantK, lwantN, lwantS, lwantU;
    int itemp[4], ierr, ierrAll, match, imt, j, jmt, nmtLoc, nMatch;
    const int isort = 1;
    const double rotAngle = 45.0;
    const double tol = 1.e-6;
    ierr = 0;
    if (nmt < 1 || Min == NULL || gamma == NULL || delta == NULL ||
        M0 == NULL || kappa == NULL || theta == NULL || sigma == NULL)
    {
        if (nmt < 1){fprintf(stderr, "%s: No moment tensors\n", __func__);}
        if (Min == NULL){fprintf(stderr, "%s: Min is NULL\n", __func__);}
        if (gamma == NULL){fprintf(stderr, "%s: gamma is NULL\n", __func__);}
        if (delta == NULL){fprintf(stderr, "%s: delta is NULL\n", __func__);}
        if (M0 == NULL){fprintf(stderr, "%s: M0 is NULL\n", __func__);}
        if (kappa == NULL){fprintf(stderr, "%s: kappa is NULL\n", __func__);}
        if (theta == NULL){fprintf(stderr, "%s: theta is NULL\n", __func__);}
        if (sigma == NULL){fprintf(stderr, "%s: sigma is NULL\n", __func__);}
        return -1;
    }
    // Determine if eigenvalue/eigenvector decomposition is requested
    lwantLam = false;
    if (lam != NULL){lwantLam = true;}
    lwantU = false;
    if (U != NULL){lwantU = true;}
    // Determine the desired fault vectors
    lwantK = false;
    lwantN = false;
    lwantS = false;
    if (K != NULL){lwantK = true;}
    if (N != NULL){lwantN = true;}
    if (S != NULL){lwantS = true;}
    // Loop on moment tensor chunks
    for (jmt=0; jmt<nmt; jmt=jmt+CE_CHUNKSIZE)
    {
        nmtLoc = MIN(CE_CHUNKSIZE, nmt - jmt);
        // This is the numerically expensive part b/c of eigendecomposition
        ierrAll = 0;
        for (imt=0; imt<nmtLoc; imt++)
        {
            // KEY: Convert M into another basis.
            // YOU MUST ALSO CHANGE north AND zenith IN fault2vecang BELOW
            // --> U will be with respect to this basis (from CMTdecom.m)
            // ierr = compearth_convertMT(1, CE_USE, CE_NWU, &Min[6*imt], M);
            ierr = compearth_convertMT(1, CE_USE, CE_SEU,
                                       &Min[6*(jmt+imt)], M);
            if (ierr != 0)
            {
                fprintf(stderr, "%s: Error switching basis\n", __func__);
                ierrAll = ierrAll + 1; //break;
            }
            // PART 1: moment tensor source type (or pattern)
            // Decompose moment tensor into eigenvalues + basis (M = U*lam*U')
            // NOTE: ordering of eigenvalues is important.
            ierr = compearth_CMTdecom(1, M, isort, &lamWork[3*imt],
                                      &Uwork[9*imt]);
            if (ierr != 0)
            {
                fprintf(stderr, "%s: Error decomposing CMT\n", __func__);
                ierrAll = ierrAll + 1; //break;
            }
        }
        if (ierrAll != 0)
        {
            fprintf(stderr, "%s: Error during eigendecomposition\n", __func__);
            ierr = 1;
            goto ERROR;
        }
        // Compute the lune coordinates and magnitude from eigenvalues
        lamdev = NULL;
        lamiso = NULL;
        thetadcWork = NULL;
        if (thetadc != NULL){thetadcWork = &thetadc[jmt];}
        ierr = compearth_lam2lune(nmtLoc, lamWork, &gamma[jmt], &delta[jmt],
                                  &M0[jmt], thetadcWork,
                                  lamdev, lamiso);
        // Part 2: moment tensor orientation; TT2012, Section 6.3
        ierr = compearth_eulerUtil_rotmat(1, &rotAngle, 2, Yrot);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error computing rotation matrix\n", __func__);
            return -1;
        }
        // Compute candidate fault vectors
        for (imt=0; imt<nmtLoc; imt++)
        {
            cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                        3, 3, 3, 1.0, &Uwork[9*imt], 3, Yrot, 3,
                        0.0, Vwork, 3); // V = U*Yrot (TT2012, p. 487)
            Swork[3*imt+0] = Vwork[0];
            Swork[3*imt+1] = Vwork[1];
            Swork[3*imt+2] = Vwork[2];
            Nwork[3*imt+0] = Vwork[6];
            Nwork[3*imt+1] = Vwork[7];
            Nwork[3*imt+2] = Vwork[8];
        }
        // Save lambda and U
        if (lwantLam){cblas_dcopy(3*nmtLoc, lamWork, 1, &lam[3*jmt], 1);}
        if (lwantU){cblas_dcopy(9*nmtLoc, Uwork, 1, &U[9*jmt], 1);}
        // Reassign ~0 elements to 0; ~1 elements to 1, and ~-1 elements to -1.
        setZero(nmtLoc, tol, Swork);
        setZero(nmtLoc, tol, Nwork);
        // Compute fault angles for four possible combinations (TT2012 Fig 15)
        for (imt=0; imt<nmtLoc; imt++)
        {
            for (j=0; j<3; j++)
            {
                Sloc[3*0+j] = Swork[3*imt+j]; Nloc[3*0+j] = Nwork[3*imt+j];
                Sloc[3*1+j] =-Swork[3*imt+j]; Nloc[3*1+j] =-Nwork[3*imt+j];
                Sloc[3*2+j] = Nwork[3*imt+j]; Nloc[3*2+j] = Swork[3*imt+j];
                Sloc[3*3+j] =-Nwork[3*imt+j]; Nloc[3*3+j] =-Swork[3*imt+j];
            }
            faultVec2Ang(&Sloc[0], &Nloc[0], &thetaL[0], &sigmaL[0],
                         &kappaL[0], &Kloc[0], &ierr);
            faultVec2Ang(&Sloc[3], &Nloc[3], &thetaL[1], &sigmaL[1],
                         &kappaL[1], &Kloc[3], &ierr);
            faultVec2Ang(&Sloc[6], &Nloc[6], &thetaL[2], &sigmaL[2],
                         &kappaL[2], &Kloc[6], &ierr);
            faultVec2Ang(&Sloc[9], &Nloc[9], &thetaL[3], &sigmaL[3],
                         &kappaL[3], &Kloc[9], &ierr);
            // There are four combinations of N and S that represent a double
            // copule moment tensor, as shown in Figure 15 of TT2012.
            // From these four combinations, there are two possible fault
            // planes.  We want to isoate the combination that is within the
            // boundingregion shown in Figures 16 and B1.
            memset(itemp, 0, 4*sizeof(int));
            nMatch = 0;
            for (j=0; j<4; j++)
            {
                if (thetaL[j] <= 90.0 + tol && fabs(sigmaL[j]) <= 90.0 + tol)
                {
                    itemp[nMatch] = j; //bmatch[j] = true;
                    nMatch = nMatch + 1;
                }
            }
            if (nMatch == 1)
            {
                match = itemp[0];
            }
            else if (nMatch == 2)
            {
                match = pickP1(thetaL[itemp[0]], sigmaL[itemp[0]], kappaL[itemp[0]],
                               thetaL[itemp[1]], sigmaL[itemp[1]], kappaL[itemp[1]],
                               tol, itemp[0], itemp[1]);
                if (match < 0)
                {
                    fprintf(stderr, "%s: Failed to pick a fault plane\n",
                             __func__);
                    ierr = 1;
                    goto ERROR;
                }
            }
            else if (nMatch == 3)
            {
                fprintf(stdout,
                  "%s: Warning mt on bdry of orientation domain 3 candidates\n",
                  __func__);
                fprintf(stdout, "%s: thetas: %e %e %e\n", __func__,
                        thetaL[0], thetaL[1], thetaL[2]);
                fprintf(stdout, "%s: sigmas: %e %e %e\n", __func__,
                        sigmaL[0], sigmaL[1], sigmaL[2]);
                fprintf(stdout, "%s: kappas: %e %e %e\n", __func__,
                        kappaL[0], kappaL[1], kappaL[2]);
                // Just take the first one
                match = itemp[0];
            }
            else if (nMatch == 4)
            {
                fprintf(stderr, "%s: Error not yet programmed\n", __func__);
                ierr = 1;
                goto ERROR;
            }
            else
            {
                fprintf(stderr, "%s: Error no match\n", __func__);
                ierr = 1;
                goto ERROR;
            }
            // Select the angle
            kappa[jmt+imt] = kappaL[match];
            sigma[jmt+imt] = sigmaL[match];
            theta[jmt+imt] = thetaL[match];
            // Fault vectors
            if (lwantK)
            {
                for (j=0; j<3; j++){K[3*(jmt+imt)+j] = Kloc[3*match+j];}
            }
            if (lwantN)
            {
                for (j=0; j<3; j++){N[3*(jmt+imt)+j] = Nloc[3*match+j];}
            }
            if (lwantS)
            {
                for (j=0; j<3; j++){S[3*(jmt+imt)+j] = Sloc[3*match+j];}
            }
        }
    } // Loop on moment tensor chunks
    if (ldisplay)
    {
        fprintf(stderr, "%s: ldisply not yet supported\n", __func__);
    }
ERROR:;
/*
    Uwork = NULL;
    lamWork = NULL;
*/
    return ierr;
}
/*!
 * @brief Returns fault angles in degrees.  Assumes input vectors are in
 *        the South-East-Up basis.
 *
 * @author Carl Tape and converted to C by Ben Baker
 *
 * @copyright MIT
 *
 */
static void faultVec2Ang(const double *__restrict__ S,
                         const double *__restrict__ N,
                         double *theta, double *sigma,
                         double *kappa, double *__restrict__ K, int *ierr)
{
    double v[3], costh, vnorm;
    int ierr1;
    const double deg = 180.0/M_PI;
    // South-East-Up (as In TT2012)
    const double zenith[3] = {0, 0, 1};
    const double negZenith[3] = {0, 0, -1};
    const double north[3] = {-1, 0, 0};
    *ierr = 0;
    *kappa = (double) NAN;
    *theta = (double) NAN;
    *sigma = (double) NAN;
    // Strike vector from TT2012, Eqn 29
    cross3(zenith, N, v);
    vnorm = norm3(v);
    if (vnorm < DBL_EPSILON) //== 0.0)
    {
        fprintf(stderr,
              "%s: Horizontal fault -- strike vector is same as slip vector\n",
                __func__);
        K[0] = S[0];
        K[1] = S[1];
        K[2] = S[2];
    }
    else
    {
        K[0] = v[0]/vnorm;
        K[1] = v[1]/vnorm;
        K[2] = v[2]/vnorm;
    }
    // Figure 14
    *kappa = compearth_eulerUtil_fangleSigned(3, north, K, negZenith, &ierr1);
    if (ierr1 != 0){*ierr = *ierr + 1;}
    *kappa = wrap360(*kappa);
    // Figure 14
    costh = dot3(N, zenith);
    *theta = acos(costh)*deg;
    // Figure 14
    *sigma = compearth_eulerUtil_fangleSigned(3, K, S, N, &ierr1);
    if (ierr1 != 0){*ierr = *ierr + 1;}
    return;
}

static void setZero(const int nmt, const double tol, double *__restrict__ X)
{
   double dmax;
   int i, idmax;
   // Compute the largest element of abs(X)
   idmax = (int) cblas_idamax(3*nmt, X, 1);
   dmax = X[idmax];
   // Elements near zero whilst trying to eliminate round-off errors
   #pragma omp simd
   for (i=0; i<3*nmt; i++)
   {
       if (fabs(X[i]/dmax) < tol){X[i] = 0.0;}
   }
   #pragma omp simd
   for (i=0; i<3*nmt; i++)
   {
       if (fabs(X[i] - 1.0) < tol){X[i] =-1.0;}
   }
   #pragma omp simd
   for (i=0; i<3*nmt; i++)
   {
       if (fabs(X[i] + 1.0) < tol){X[i] = 1.0;}
   }
   return;
}

static int pickP1(const double thetaA, const double sigmaA, const double kappaA,
                  const double thetaB, const double sigmaB, const double kappaB,
                  const double tol, const int p1, const int p2)
{
    int ipick;
    ipick =-1;
    if (fabs(thetaA - 90.0) < tol)
    {
        if (kappaA < 180.0){ipick = p1;}
        if (kappaB < 180.0){ipick = p2;}
        return ipick;
    }
    if (fabs(sigmaA - 90.0) < tol)
    {
        if (kappaA < 180.0){ipick = p1;}
        if (kappaB < 180.0){ipick = p2;}
        return ipick;
    }
    if (fabs(sigmaA + 90.0) < tol)
    {
        if (kappaA < 180.0){ipick = p1;}
        if (kappaB < 180.0){ipick = p2;}
        return ipick;
    }
    fprintf(stderr, "%s: Error no selection criterion was met\n", __func__);
    fprintf(stderr, "thetaA,sigmaA,kappaA=%f,%f,%f\n", thetaA, sigmaA, kappaA);
    fprintf(stderr, "thetaB,sigmaB,kappaB=%f,%f,%f\n", thetaB, sigmaB, kappaB);
    return ipick;
}

max_stars_repo_name: OUCyf/mtbeach

max_issues_repo_name: carltape/mtbeach

max_forks_repo_name: carltape/mtbeach


text:
#pragma once

#include <gsl.h>

#include <QString>

namespace gsl {
    inline QString to_QString(gsl::cstring_span<> view) { return QString::fromLatin1(view.data(), view.size()); }
}

max_stars_repo_name: carthurs/CRIMSONGUI

max_issues_repo_name: carthurs/CRIMSONGUI

max_forks_repo_name: carthurs/CRIMSONGUI


text:
#include "jobqueue.h"
#include "ance_degnome.h"
#include "fitfunc.h"
#include "flagparse.c"
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <time.h>
#include <pthread.h>
#include <unistd.h>
#include <limits.h>

typedef struct JobData JobData;
struct JobData {
	Degnome* child;
	Degnome* p1;
	Degnome* p2;
};

void usage(void);
void help_menu(void);
int jobfunc(void* p, void* tdat);
void calculate_diversity(Degnome* generation, double** percent_decent, double* diversity);

const char* usageMsg =
	"Usage: devosim [-bhrv] [-s | -u] [-c chromosome_length]\n"
	"\t\t  [-e mutation_effect] [-g num_generations]\n"
	"\t\t  [-m mutation_rate] [-o crossover_rate]\n"
	"\t\t  [-p population_size] [-t num_threads]\n"
	"\t\t  [--seed rngseed] [--target hat_height target]\n"
	"\t\t  [--sqrt | --linear | --close | --ceiling | --log]\n";

const char* helpMsg =
	"OPTIONS\n"
	"\t -b\t Simulation will stop when all degnomes are identical.\n\n"
	"\t -c chromosome_length\n"
	"\t\t Set chromosome length for the current simulation.\n"
	"\t\t Default chromosome length is 10.\n\n"
	"\t -e mutation_effect\n"
	"\t\t Set how much a mutation will effect a gene on average.\n"
	"\t\t Default mutation effect is 2.\n\n"
	"\t -g num_generations\n"
	"\t\t Set how many generations this simulation will run for.\n"
	"\t\t Default number of generations is 1000.\n\n"
	"\t -h\t Display this help menu.\n\n"
	"\t -m mutation_rate\n"
	"\t\t Set the mutation rate for the current simulation.\n"
	"\t\t Default mutation rate is 1.\n\n"
	"\t -o crossover_rate\n"
	"\t\t Set the crossover rate for the current simulation.\n"
	"\t\t Default crossover rate is 2.\n\n"
	"\t -p population_size\n"
	"\t\t Set the population size for the current simulation.\n"
	"\t\t Default population size is 10.\n\n"
	"\t -r\t Only show percentages of descent from the original genomes.\n\n"
	"\t -s\t Degnome selection will occur.\n\n"
	"\t -u\t All degnomes contribute to two offspring.\n\n"
	"\t -v\t Output will be given for every generation.\n\n"
	"\t -t num_threads\n"
	"\t\t Select the number of threads to be used in the current run.\n"
	"\t\t Default is 0 (which will result in 3/4 of cores being used).\n"
	"\t\t Must be 1 if a seed is used in order to prevent race conditions.\n\n"
	"\t --seed rngseed\n"
	"\t\t Select the seed used by the RNG in the current run.\n"
	"\t\t Default seed is 0 (which will result in a random seed).\n\n"
	"\t --target hat_height target\n"
	"\t\t Sets the ideal hat height for the current simulation\n"
	"\t\t Used for fitness functions that have an \"ideal\" value.\n\n"
	"\t --sqrt\t\t fitness will be sqrt(hat_height)\n\n"
	"\t --linear\t fitness will be hat_height\n\n"
	"\t --close\t fitness will be (target - abs(target - hat_height))\n\n"
	"\t --ceiling\t fitness will quickly level off after passing target\n\n";

pthread_mutex_t seedLock = PTHREAD_MUTEX_INITIALIZER;
unsigned long rngseed=0;

//	there is no need for the line 'int chrom_size' as it is declared as a global variable in degnome.h
int pop_size;
int num_gens;
int mutation_rate;
int mutation_effect;
int crossover_rate;
int selective;
int uniform;
int verbose;
int reduced;
int break_at_zero_diversity;

void usage(void) {
	fputs(usageMsg, stderr);
	exit(EXIT_FAILURE);
}

void help_menu(void) {
	fputs(helpMsg, stderr);
	exit(EXIT_FAILURE);
}

int num_threads = 0;
JobQueue* jq;

void *ThreadState_new(void *notused);
void ThreadState_free(void *rng);

void *ThreadState_new(void *notused) {
	// Lock seed, initialize random number generator, increment seed,
	// and unlock.
	gsl_rng *rng = gsl_rng_alloc(gsl_rng_taus);

	pthread_mutex_lock(&seedLock);
	gsl_rng_set(rng, rngseed);
	rngseed = (rngseed == ULONG_MAX ? 0 : rngseed + 1);
	pthread_mutex_unlock(&seedLock);

	return rng;
}

void ThreadState_free(void *rng) {
	gsl_rng_free((gsl_rng *) rng);
}

int jobfunc(void* p, void* tdat) {
	gsl_rng* rng = (gsl_rng*) tdat;
	JobData* data = (JobData*) p;																				//get data out
	Degnome_mate(data->child, data->p1, data->p2, rng, mutation_rate, mutation_effect, crossover_rate);			//mate

	return 0;		//exited without error
}

void calculate_diversity(Degnome* generation, double** percent_decent, double* diversity) {
	*diversity = 0;
	for (int i = 0; i < pop_size; i++) {			//calculate percent decent for each degnome
		for (int j = 0; j < pop_size; j++) {
			percent_decent[i][j] = 0;
			for (int k = 0; k < chrom_size; k++) {
				if (generation[i].GOI_array[k] == j) {
					percent_decent[i][j]++;
				}
			}
			percent_decent[i][j] /= chrom_size;
		}
	}
	for (int j = 0; j < pop_size; j++) {			//sum and average
		percent_decent[pop_size][j] = 0;
		for (int k = 0; k < pop_size; k++) {
			percent_decent[pop_size][j] += percent_decent[k][j];
		}
		percent_decent[pop_size][j] /= pop_size;
	}

	for (int i = 0; i < pop_size; i++) {			//calculate percent diversity for the entire generation
		for (int j = 0; j < pop_size; j++) {
			if (i == j) {
				continue;
			}
			for (int k = 0; k < chrom_size; k++) {
				if (generation[i].GOI_array[k] != generation[j].GOI_array[k]) {
					(*diversity)++;
				}
			}
		}
	}
	*diversity /= ((pop_size-1) * pop_size * chrom_size);
}

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

	int * flags = NULL;

	if (parse_flags(argc, argv, 3, &flags) == -1) {
		free(flags);
		usage();
	}

	if (flags[2] == 1) {
		free(flags);
		help_menu();
	}

	break_at_zero_diversity = flags[1];
	reduced = flags[3];
	verbose = flags[4];
	if (flags[5] == 1) {
		selective = 1;
	}
	else if (flags[5] == 2) {
		uniform = 1;
	}
	chrom_size = flags[6];
	mutation_effect = flags[7];
	num_gens = flags[8];
	mutation_rate = flags[9];
	crossover_rate = flags[10];
	pop_size = flags[11];

	num_threads = flags[12];

	if(flags[13] == 0){
		set_function("linear");
	}
	else if(flags[13] == 1){
		set_function("sqrt");
	}
	else if(flags[13] == 2){
		set_function("close");
	}
	else if(flags[13] == 3){
		set_function("ceiling");
	}
	else if(flags[13] == 4){
		set_function("log");
	}

	target_num = flags[14];

	if(flags[15] <= 0) {
		time_t currtime = time(NULL);                  // time
		unsigned long pid = (unsigned long) getpid();  // process id
		rngseed = currtime ^ pid;                      // random seed
	}
	else{
		rngseed = flags[15];
	}
	gsl_rng* rng = gsl_rng_alloc(gsl_rng_taus);    // rand generator
	gsl_rng_set(rng, rngseed);

	free(flags);

	if (num_threads <= 0) {
		if (num_threads < 0) {
			#ifdef DEBUG_MODE
				fprintf(stderr, "Error invalid number of threads: %u\n", num_threads);
			#endif
		}
		num_threads = (3*getNumCores()/4);
	}
	#ifdef DEBUG_MODE
		fprintf(stderr, "Final number of threads: %u\n", num_threads);
	#endif

	Degnome* parents;
	Degnome* children;
	Degnome* temp;

	printf("%u, %u, %u\n", chrom_size, pop_size, num_gens);

	parents = malloc(pop_size*sizeof(Degnome));
	children = malloc(pop_size*sizeof(Degnome));

	for (int i = 0; i < pop_size; i++) {
		parents[i].dna_array = malloc(chrom_size*sizeof(double));
		parents[i].GOI_array = malloc(chrom_size*sizeof(int));

		children[i].dna_array = malloc(chrom_size*sizeof(double));
		children[i].GOI_array = malloc(chrom_size*sizeof(int));

		parents[i].hat_size = 0;

		for (int j = 0; j < chrom_size; j++) {
			parents[i].dna_array[j] = 10;	//children aren't initialized
			parents[i].hat_size += 10;
			parents[i].GOI_array[j] = (i);	//track ancestries
		}
	}

	double* diversity;
	double** percent_decent;

	diversity = malloc(sizeof(double));
	*diversity = 1;
	percent_decent = malloc((pop_size+1)*sizeof(double*));
	for (int i = 0; i < pop_size+1; i++) {
		percent_decent[i] = malloc(pop_size*sizeof(double));
		if (i == pop_size) {
			continue;
		}
		for (int j = 0; j < pop_size; j++) {
			if (i == j) {
				percent_decent[i][j] = 1;
			}
			else {
				percent_decent[i][j] = 0;
			}
		}
	}

	if (!reduced && !verbose) {
		printf("\nGeneration 0:\n\n");
		for (int i = 0; i < pop_size; i++) {
			printf("Degnome %u allele values:\n", i);
			if (!reduced) {
				for (int j = 0; j < chrom_size; j++) {
					printf("%lf\t", parents[i].dna_array[j]);
				}
				printf("\n");
			}
			else {
				printf("%lf\n", parents[i].dna_array[0]);
			}

			printf("Degnome %u ancestries:\n", i);
			if (!reduced) {
				for (int j = 0; j < chrom_size; j++) {
					printf("%u\t", parents[i].GOI_array[j]);
				}
				printf("\n");
			}
			else {
				printf("%u\n", parents[i].GOI_array[0]);
			}
		}
	}
	printf("\n\n");

	int final_gen;
	int broke_early = 0;

	jq = JobQueue_new(num_threads, NULL, ThreadState_new, ThreadState_free);

	JobData* dat = malloc(pop_size*sizeof(JobData));

	for (int i = 0; i < num_gens; i++) {
		if (break_at_zero_diversity) {
			calculate_diversity(parents, percent_decent, diversity);
			if ((*diversity) <= 0) {
				final_gen = i;
				broke_early = 1;
				break;
			}
		}
		if (!uniform) {
			double fit;
			if (selective) {
				fit = get_fitness(parents[0].hat_size);
			}
			else {
				fit = 100;			//in runs withoutslection, everybody is equally fit
			}

			double total_hat_size = fit;
			double cum_hat_size[pop_size];
			cum_hat_size[0] = fit;

			for (int j = 1; j < pop_size; j++) {
				if (selective) {
					fit = get_fitness(parents[j].hat_size);
				}
				else {
					fit = 100;
				}

				total_hat_size += fit;
				cum_hat_size[j] = (cum_hat_size[j-1] + fit);
			}

			for (int j = 0; j < pop_size; j++) {

				pthread_mutex_lock(&seedLock);
				gsl_rng_set(rng, rngseed);
				rngseed = (rngseed == ULONG_MAX ? 0 : rngseed + 1);
				pthread_mutex_unlock(&seedLock);

				int m, d;

				double win_m = gsl_rng_uniform(rng);
				win_m *= total_hat_size;
				double win_d = gsl_rng_uniform(rng);
				win_d *= total_hat_size;

				// printf("win_m:%lf, wind:%lf, max: %lf\n", win_m,win_d,total_hat_size);

				for (m = 0; cum_hat_size[m] < win_m; m++) {
					continue;
				}

				for (d = 0; cum_hat_size[d] < win_d; d++) {
					continue;
				}

				// printf("m:%u, d:%u\n", m,d);
				dat[j].child = (children + j);
				dat[j].p1 = (parents + m);
				dat[j].p2 = (parents + d);

				JobQueue_addJob(jq, jobfunc, dat + j);			}
		}
		else {
			// printf("uniform!!!\n");

			int moms[pop_size];
			int dads[pop_size];
			int mom_max = pop_size;
			int dad_max = pop_size;

			int m, d;

			for (int j = 0; j < pop_size; j++) {
				moms[j] = j;
				dads[j] = j;
			}

			for (int j = 0; j < pop_size; j++) {
				pthread_mutex_lock(&seedLock);
				gsl_rng_set(rng, rngseed);
				rngseed = (rngseed == ULONG_MAX ? 0 : rngseed + 1);

				pthread_mutex_unlock(&seedLock);

				int index_m = (int) gsl_rng_uniform_int (rng, mom_max);
				int index_d = (int) gsl_rng_uniform_int (rng, dad_max);

				m = moms[index_m];
				d = dads[index_d];

				// printf("m:\t%u\nd:\t%u\n", m, d);

				//reduce the pool of available degnomes
				//in order to make sure everybody get's two chances to mate
				//one as a dad and one as a mom

				int temp_m = moms[index_m];
				int temp_d = dads[index_d];
				moms[index_m] = moms[mom_max-1];
				dads[index_d] = dads[dad_max-1];
				moms[mom_max-1] = temp_m;
				dads[dad_max-1] = temp_d;

				mom_max--;
				dad_max--;

				dat[j].child = (children + j);
				dat[j].p1 = (parents + m);
				dat[j].p2 = (parents + d);

				JobQueue_addJob(jq, jobfunc, dat + j);
			}
		}

		JobQueue_waitOnJobs(jq);

		temp = children;
		children = parents;
		parents = temp;
		if (verbose) {
			calculate_diversity(parents, percent_decent, diversity);
			printf("\nGeneration %u:\n", i);
			if (!reduced) {
				for (int k = 0; k < pop_size; k++) {
					printf("\n\nDegnome %u allele values:\n", k);

					for (int j = 0; j < chrom_size; j++) {
						printf("%lf\t", parents[k].dna_array[j]);
					}
					if (selective) {
						printf("\nTOTAL HAT SIZE: %lg\n\n", parents[k].hat_size);
					}
					else {
						printf("\n");
					}

					printf("\n\nDegnome %u ancestries:\n", k);
					for (int j = 0; j < chrom_size; j++) {
						printf("%u\t", parents[k].GOI_array[j]);
					}
					printf("\n");

					for (int j = 0; j < pop_size; j++) {
						if (percent_decent[k][j] > 0) {
							printf("%lf%% Degnome %u\t", (100*percent_decent[k][j]), j);
						}
					}
				}
			}
			printf("\nAverage population descent percentages:\n");
			for (int j = 0; j < pop_size; j++) {
				if (percent_decent[pop_size][j] > 0) {
					printf("%lf%% Degnome %u\t", (100*percent_decent[pop_size][j]), j);
				}
			}
			printf("\nPercent diversity: %lf\n", (100* (*diversity)));
		printf("\n\n");
		}
	}

	JobQueue_noMoreJobs(jq);

	if (verbose) {
		printf("\n");
	}

	calculate_diversity(parents, percent_decent, diversity);
	// printf("\n\n DIVERSITY%lf\n\n\n", *diversity);
	if (broke_early) {
		printf("Generation %u:\n", final_gen);
	}
	else {
		printf("Generation %u:\n", num_gens);
	}
	if (!reduced) {
		for (int i = 0; i < pop_size; i++) {
			printf("\n\nDegnome %u allele values:\n", i);
			if (!reduced) {
				for (int j = 0; j < chrom_size; j++) {
					printf("%lf\t", parents[i].dna_array[j]);
				}
			}

			printf("\n\nDegnome %u ancestries:\n", i);
			if (!reduced) {
				for (int j = 0; j < chrom_size; j++) {
					printf("%u\t", parents[i].GOI_array[j]);
				}
				printf("\n");
			}

			for (int j = 0; j < pop_size; j++) {
				if (percent_decent[i][j] > 0) {
					printf("%lf%% Degnome %u\t", (100*percent_decent[i][j]), j);
				}
			}
			printf("\n");

			if (selective) {
				printf("\nTOTAL HAT SIZE: %lg\n\n", parents[i].hat_size);
			}
			else {
				printf("\n\n");
			}
		}
	}
	printf("Average population decent percentages:\n");
	for (int j = 0; j < pop_size; j++) {
		if (percent_decent[pop_size][j] > 0) {
			printf("%lf%% Degnome %u\t", (100*percent_decent[pop_size][j]), j);
		}
	}
	printf("\nPercent diversity: %lf\n", (100* (*diversity)));
	printf("\n\n\n");

	//free everything
	JobQueue_free(jq);
	free(dat);

	for (int i = 0; i < pop_size; i++) {
		free(parents[i].dna_array);
		free(children[i].dna_array);
		free(parents[i].GOI_array);
		free(children[i].GOI_array);
		parents[i].hat_size = 0;

		free(percent_decent[i]);
	}
	free(percent_decent[pop_size]);

	free(parents);
	free(children);

	free(percent_decent);
	free(diversity);

	gsl_rng_free (rng);
}

max_stars_repo_name: masalemi/PopGenSim

max_issues_repo_name: masalemi/PopGenSim

max_forks_repo_name: masalemi/PopGenSim


text:
/* multifit/lmpar.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
 *
 * 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 3 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <gsl/gsl_permute_vector_double.h>

#include "qrsolv.c"

static size_t
count_nsing (const gsl_matrix * r)
{
  /* Count the number of nonsingular entries. Returns the index of the
     first entry which is singular. */

  size_t n = r->size2;
  size_t i;

  for (i = 0; i < n; i++)
    {
      double rii = gsl_matrix_get (r, i, i);

      if (rii == 0)
        {
          break;
        }
    }

  return i;
}

static void
compute_newton_direction (const gsl_matrix * r, const gsl_permutation * perm,
                          const gsl_vector * qtf, gsl_vector * x)
{

  /* Compute and store in x the Gauss-Newton direction. If the
     Jacobian is rank-deficient then obtain a least squares
     solution. */

  const size_t n = r->size2;
  size_t i, j, nsing;

  for (i = 0 ; i < n ; i++)
    {
      double qtfi = gsl_vector_get (qtf, i);
      gsl_vector_set (x, i,  qtfi);
    }

  nsing = count_nsing (r);

#ifdef DEBUG
  printf("nsing = %d\n", nsing);
  printf("r = "); gsl_matrix_fprintf(stdout, r, "%g"); printf("\n");
  printf("qtf = "); gsl_vector_fprintf(stdout, x, "%g"); printf("\n");
#endif

  for (i = nsing; i < n; i++)
    {
      gsl_vector_set (x, i, 0.0);
    }

  if (nsing > 0)
    {
      for (j = nsing; j > 0 && j--;)
        {
          double rjj = gsl_matrix_get (r, j, j);
          double temp = gsl_vector_get (x, j) / rjj;

          gsl_vector_set (x, j, temp);

          for (i = 0; i < j; i++)
            {
              double rij = gsl_matrix_get (r, i, j);
              double xi = gsl_vector_get (x, i);
              gsl_vector_set (x, i, xi - rij * temp);
            }
        }
    }

  gsl_permute_vector_inverse (perm, x);
}

static void
compute_newton_correction (const gsl_matrix * r, const gsl_vector * sdiag,
                           const gsl_permutation * p, gsl_vector * x,
                           double dxnorm,
                           const gsl_vector * diag, gsl_vector * w)
{
  size_t n = r->size2;
  size_t i, j;

  for (i = 0; i < n; i++)
    {
      size_t pi = gsl_permutation_get (p, i);

      double dpi = gsl_vector_get (diag, pi);
      double xpi = gsl_vector_get (x, pi);

      gsl_vector_set (w, i, dpi * (dpi * xpi) / dxnorm);
    }

  for (j = 0; j < n; j++)
    {
      double sj = gsl_vector_get (sdiag, j);
      double wj = gsl_vector_get (w, j);

      double tj = wj / sj;

      gsl_vector_set (w, j, tj);

      for (i = j + 1; i < n; i++)
        {
          double rij = gsl_matrix_get (r, i, j);
          double wi = gsl_vector_get (w, i);

          gsl_vector_set (w, i, wi - rij * tj);
        }
    }
}

static void
compute_newton_bound (const gsl_matrix * r, const gsl_vector * x,
                      double dxnorm,  const gsl_permutation * perm,
                      const gsl_vector * diag, gsl_vector * w)
{
  /* If the jacobian is not rank-deficient then the Newton step
     provides a lower bound for the zero of the function. Otherwise
     set this bound to zero. */

  size_t n = r->size2;

  size_t i, j;

  size_t nsing = count_nsing (r);

  if (nsing < n)
    {
      gsl_vector_set_zero (w);
      return;
    }

  for (i = 0; i < n; i++)
    {
      size_t pi = gsl_permutation_get (perm, i);

      double dpi = gsl_vector_get (diag, pi);
      double xpi = gsl_vector_get (x, pi);

      gsl_vector_set (w, i, dpi * (dpi * xpi / dxnorm));
    }

  for (j = 0; j < n; j++)
    {
      double sum = 0;

      for (i = 0; i < j; i++)
        {
          sum += gsl_matrix_get (r, i, j) * gsl_vector_get (w, i);
        }

      {
        double rjj = gsl_matrix_get (r, j, j);
        double wj = gsl_vector_get (w, j);

        gsl_vector_set (w, j, (wj - sum) / rjj);
      }
    }
}

/* compute scaled gradient g = D^{-1} J^T f (see More' eq 7.2) */
static void
compute_gradient_direction (const gsl_matrix * r, const gsl_permutation * p,
                            const gsl_vector * qtf, const gsl_vector * diag,
                            gsl_vector * g)
{
  const size_t n = r->size2;

  size_t i, j;

  for (j = 0; j < n; j++)
    {
      double sum = 0;

      for (i = 0; i <= j; i++)
        {
          sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
        }

      {
        size_t pj = gsl_permutation_get (p, j);
        double dpj = gsl_vector_get (diag, pj);

        gsl_vector_set (g, j, sum / dpj);
      }
    }
}

/* compute gradient g = J^T f */
static void
compute_gradient (const gsl_matrix * r, const gsl_vector * qtf,
                  gsl_vector * g)
{
  const size_t n = r->size2;

  size_t i, j;

  for (j = 0; j < n; j++)
    {
      double sum = 0;

      for (i = 0; i <= j; i++)
        {
          sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
        }

      gsl_vector_set (g, j, sum);
    }
}

static int
lmpar (gsl_matrix * r, const gsl_permutation * perm, const gsl_vector * qtf,
       const gsl_vector * diag, double delta, double * par_inout,
       gsl_vector * newton, gsl_vector * gradient, gsl_vector * sdiag,
       gsl_vector * x, gsl_vector * w)
{
  double dxnorm, gnorm, fp, fp_old, par_lower, par_upper, par_c;

  double par = *par_inout;

  size_t iter = 0;

#ifdef DEBUG
  printf("ENTERING lmpar\n");
#endif

  compute_newton_direction (r, perm, qtf, newton);

#ifdef DEBUG
  printf ("newton = ");
  gsl_vector_fprintf (stdout, newton, "%g");
  printf ("\n");

  printf ("diag = ");
  gsl_vector_fprintf (stdout, diag, "%g");
  printf ("\n");
#endif

  /* Evaluate the function at the origin and test for acceptance of
     the Gauss-Newton direction. */

  dxnorm = scaled_enorm (diag, newton);

  fp = dxnorm - delta;

#ifdef DEBUG
  printf ("dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp);
#endif

  if (fp <= 0.1 * delta)
    {
      gsl_vector_memcpy (x, newton);
#ifdef DEBUG
      printf ("took newton (fp = %g, delta = %g)\n", fp, delta);
#endif

      *par_inout = 0;

      return GSL_SUCCESS;
    }

#ifdef DEBUG
  printf ("r = ");
  gsl_matrix_fprintf (stdout, r, "%g");
  printf ("\n");

  printf ("newton = ");
  gsl_vector_fprintf (stdout, newton, "%g");
  printf ("\n");

  printf ("dxnorm = %g\n", dxnorm);
#endif

  compute_newton_bound (r, newton, dxnorm, perm, diag, w);

#ifdef DEBUG
  printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");

  printf ("diag = ");
  gsl_vector_fprintf (stdout, diag, "%g");
  printf ("\n");

  printf ("w = ");
  gsl_vector_fprintf (stdout, w, "%g");
  printf ("\n");
#endif

  {
    double wnorm = enorm (w);
    double phider = wnorm * wnorm;

    /* w == zero if r rank-deficient,
       then set lower bound to zero form MINPACK, lmder.f
       Hans E. Plesser 2002-02-25 (hans.plesser@itf.nlh.no) */
    if ( wnorm > 0 )
      par_lower = fp / (delta * phider);
    else
      par_lower = 0.0;
  }

#ifdef DEBUG
  printf("par       = %g\n", par      );
  printf("par_lower = %g\n", par_lower);
#endif

  compute_gradient_direction (r, perm, qtf, diag, gradient);

  gnorm = enorm (gradient);

#ifdef DEBUG
  printf("gradient = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
  printf("gnorm = %g\n", gnorm);
#endif

  par_upper =  gnorm / delta;

  if (par_upper == 0)
    {
      par_upper = GSL_DBL_MIN / GSL_MIN_DBL(delta, 0.1);
    }

#ifdef DEBUG
  printf("par_upper = %g\n", par_upper);
#endif

  if (par > par_upper)
    {
#ifdef DEBUG
  printf("set par to par_upper\n");
#endif

      par = par_upper;
    }
  else if (par < par_lower)
    {
#ifdef DEBUG
  printf("set par to par_lower\n");
#endif

      par = par_lower;
    }

  if (par == 0)
    {
      par = gnorm / dxnorm;
#ifdef DEBUG
      printf("set par to gnorm/dxnorm = %g\n", par);
#endif

    }

  /* Beginning of iteration */

iteration:

  iter++;

#ifdef DEBUG
  printf("lmpar iteration = %d\n", iter);
#endif

#ifdef BRIANSFIX
  /* Seems like this is described in the paper but not in the MINPACK code */

  if (par < par_lower || par > par_upper)
    {
      par = GSL_MAX_DBL (0.001 * par_upper, sqrt(par_lower * par_upper));
    }
#endif

  /* Evaluate the function at the current value of par */

  if (par == 0)
    {
      par = GSL_MAX_DBL (0.001 * par_upper, GSL_DBL_MIN);
#ifdef DEBUG
      printf("par = 0, set par to  = %g\n", par);
#endif

    }

  /* Compute the least squares solution of [ R P x - Q^T f, sqrt(par) D x]
     for A = Q R P^T */

#ifdef DEBUG
  printf ("calling qrsolv with par = %g\n", par);
#endif

  {
    double sqrt_par = sqrt(par);

    qrsolv (r, perm, sqrt_par, diag, qtf, x, sdiag, w);
  }

  dxnorm = scaled_enorm (diag, x);

  fp_old = fp;

  fp = dxnorm - delta;

#ifdef DEBUG
  printf ("After qrsolv dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp);
  printf ("sdiag = ") ; gsl_vector_fprintf(stdout, sdiag, "%g"); printf("\n");
  printf ("x = ") ; gsl_vector_fprintf(stdout, x, "%g"); printf("\n");
  printf ("r = ") ; gsl_matrix_fprintf(stdout, r, "%g"); printf("\nXXX\n");
#endif

  /* If the function is small enough, accept the current value of par */

  if (fabs (fp) <= 0.1 * delta)
    goto line220;

  if (par_lower == 0 && fp <= fp_old && fp_old < 0)
    goto line220;

  /* Check for maximum number of iterations */

  if (iter == 10)
    goto line220;

  /* Compute the Newton correction */

  compute_newton_correction (r, sdiag, perm, x, dxnorm, diag, w);

#ifdef DEBUG
  printf ("newton_correction = ");
  gsl_vector_fprintf(stdout, w, "%g"); printf("\n");
#endif

  {
    double wnorm = enorm (w);
    par_c = fp / (delta * wnorm * wnorm);
  }

#ifdef DEBUG
  printf("fp = %g\n", fp);
  printf("par_lower = %g\n", par_lower);
  printf("par_upper = %g\n", par_upper);
  printf("par_c = %g\n", par_c);
#endif

  /* Depending on the sign of the function, update par_lower or par_upper */

  if (fp > 0)
    {
      if (par > par_lower)
        {
          par_lower = par;
#ifdef DEBUG
      printf("fp > 0: set par_lower = par = %g\n", par);
#endif

        }
    }
  else if (fp < 0)
    {
      if (par < par_upper)
        {
#ifdef DEBUG
      printf("fp < 0: set par_upper = par = %g\n", par);
#endif
          par_upper = par;
        }
    }

  /* Compute an improved estimate for par */

#ifdef DEBUG
      printf("improved estimate par = MAX(%g, %g) \n", par_lower, par+par_c);
#endif

  par = GSL_MAX_DBL (par_lower, par + par_c);

#ifdef DEBUG
      printf("improved estimate par = %g \n", par);
#endif

  goto iteration;

line220:

#ifdef DEBUG
  printf("LEAVING lmpar, par = %g\n", par);
#endif

  *par_inout = par;

  return GSL_SUCCESS;
}

max_stars_repo_name: ielomariala/Hex-Game

max_issues_repo_name: Brian-ning/HMNE

max_forks_repo_name: Brian-ning/HMNE


text:
/* this manages and calls function to execute function
* from beginning to end
*/
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_statistics.h>

#include "fileio.h"
#include "data_process.h"
#include "sensor_history.h"
#include "sensor_validation.h"

#include "externs.h"

void handle_files(char *, double*, double*);
void sensor_validation(double*);
double data_processing(double*);

int sensor_number;
int temperature_min = 30;
int temperature_max = 60;
int data_number;
int group_number;
float q_percent;

/* path and names of the files used in the program
* which include input file, output file and sensor history file */
char input_file_name[255] = "../data/input_data/sample_input.csv";
char history_file_name[255] = "../data/sensor_history/sensor_history.csv";
char output_file_name[255] = "../data/output_data/output.csv";

int main() {

    printf("\nApplication is running....\n");

    /* user input for the number of sensors to process */
    printf("Insert number of sensors:");
    scanf("%d", &sensor_number);

    /* determining the time intervals for the sensors */
    int time_interval;

    printf("Insert collected time intervals:");
    scanf("%d", &time_interval);

    printf("Insert q percent(Should be 0~1):");
    scanf("%f", &q_percent);

    if ((q_percent<0) || (q_percent>1)) {
        printf("Invalid q_percent! It will be regarded as 0.7 of default value.\n");
        q_percent = 0.7;
    }

    double values[256];
    double time_value[256];
    double group_values[sensor_number];

    /* function performs reading of sensor data
    * @input : file name
    * @output : sensor time and associated values
    */
    handle_files(input_file_name, &time_value[0], &values[0]);

    /* number of calculations to be performed based on the user input */
    group_number = data_number / sensor_number;

    for (int i = 0; i < group_number; i++) {
        time_value[i] = time_value[i * sensor_number];
    }

    /* check whether the user input and actual file data matches */
    if (group_number != time_interval) {
        printf("ERROR: Time interval numbers, sensor numbers do not match with your input file!\n");
        exit(0);
    }

    sensor_validation(&values[0]);

    /* seperating multiple data computations in the output file*/
    FILE *fpout = fopen(output_file_name, "a");
    fprintf(fpout, "--------------\n");
    fclose(fpout);

    FILE *fphis = fopen(history_file_name, "a");
    fprintf(fphis, "--------------\n");
    fclose(fphis);

    for (int i = 0 ; i < group_number; i++) {
        for (int j = 0; j < sensor_number; j++) {
            group_values[j]  = values[i*sensor_number + j];
        }
        printf("\n---------------------Time interval %d---------------------\n\n", i);

        double fused = data_processing(&group_values[0]);
        float time_val_file = (float)time_value[i];

        write_data(time_val_file, fused, output_file_name);
    }

    return 1;
}

/* function checks whether an attempt made to open a file is succuessful or not */
void handle_files(char* input_file_name, double* time_value, double* values) {
    printf("\n");
    if (input_file_name != NULL) {
        data_number = read_data(&time_value[0], &values[0], input_file_name);

        if ( data_number == -1 ) {
            printf("ERROR: File open failed!\n");
            exit(0);
        } else if ( data_number == 0 ) {
            printf("ERROR: Sensor numbers inserted and file are different!\n");
            exit(0);
        } else if ( data_number > 0) {
            printf("Read data successfully!\n");
        }
    } else {
        printf("ERROR: Input file does not exist!\n");
        exit(0);
    }
}

/* function performs sensor validation for group values
* @input : group Values
*/
void sensor_validation(double* group_values) {

    int res = reading_validation(&group_values[0]);
    if (res == 1) {
        frozen_value_check(&group_values[0]);

    }

    if (res == 0) {
        printf("ERROR: Temperature values are out of range! Check history file.\n");
        frozen_value_check(&group_values[0]);
    }

}

/* function performs sensor fusion algo using group Values
* @input : group Values
* @output : fused result
*/
double data_processing(double* group_values) {

    printf("Sensor Values:\n");

    for (int i = 0; i < sensor_number; i++) {
        printf("x%d=%f, ", i, group_values[i]);
    }

    /* Step 1: Calc the Support Degree Matrix */
    gsl_matrix* D = gsl_matrix_alloc(sensor_number,sensor_number);
    support_degree_generator(D, &group_values[0]);    //function in data_process.c to get D - Support Degree Matrix

    /* Step 2: Calc eigenval & eigenvec */
    gsl_matrix* T = gsl_matrix_alloc(sensor_number,sensor_number);
    gsl_vector* evec = gsl_vector_alloc(sensor_number);
    gsl_matrix* Temp = gsl_matrix_alloc(sensor_number,sensor_number);

    gsl_matrix_memcpy(Temp, D);
    eigenvec_calc(evec, T, Temp);    //function in data_process.c to get evec - eigen group_values & T - vectors

    /* Step 3: Principal Comp Calc */
    gsl_matrix* y = gsl_matrix_alloc(sensor_number,sensor_number);
    principal_comp_calc(T, D, y);    //function in data_process.c to get T - Principal Components

    /* Step 4: Calc the contri rate of the kth principal comp */
    double alpha[sensor_number];
    contri_rate_calc_kth(evec, &alpha[0]);    //function in data_process.c to get alpha

    /* Step 5: Calc the contri rate of the m principal comp */
    double phi[sensor_number];
    major_contri_calc(&alpha[0], &phi[0]);    //function in data_process.c to get phi

    /* Step 6: Compute the integrated support degree score */
    gsl_vector* Z = gsl_vector_alloc(sensor_number);
    integ_supp_score_calc(&alpha[0], y, Z);    //function in data_process.c to get z_i

    /* Step 7-1: Eliminate incorrect data */
    int sensor_correction[sensor_number];
    elliminate_incorrect_data(Z, &sensor_correction[0]);    //function in data_process.c to elliminate incorrect datas

    /* Step 7-2: Compute the weight coefficient for each sensor */
    double omega[sensor_number];
    weight_coeff_calc(Z, &sensor_correction[0], &omega[0]);     //function in data_process.c to get omega

    /* Step 7-3: Compute the fused output */
    double fused;
    fused = fused_output(&omega[0], &group_values[0]);    //function in data_process.c to get fused output
    printf("FINAL STEP: \nThe fused output is %f\n", fused);

    /* Free memory */
    gsl_matrix_free(D);
    gsl_matrix_free(Temp);
    gsl_matrix_free(T);
    gsl_vector_free(evec);
    gsl_matrix_free(y);
    gsl_vector_free(Z);

    return fused;

}

max_stars_repo_name: karanbirsandhu/nu-sense

max_issues_repo_name: karanbirsandhu/nu-sense

max_forks_repo_name: karanbirsandhu/nu-sense


text:
// directdebye.c
// Computes the Debye sum from raytracing data.
// See man page for usage information.
#include <assert.h>
#include <complex.h>
#include <errno.h>
#include <error.h>
#include <gsl/gsl_const_mksa.h>
#include <malloc.h>
#include <math.h>
#include <matheval.h>
#include <mpi.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include "hdfio.h"
#include "miscprint.h"
#include "parseargs.h"
#include "rays.h"

int main(int argc, char **argv) {
  /* MPI initialization */
  int rank, nprocs;
  MPI_Init(&argc, &argv);
  MPI_Comm_size(MPI_COMM_WORLD, &nprocs);
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);

  /* parse command line arguments */
  args_t *a = malloc(sizeof(args_t));
  parse_args(argc, argv, a);

  /* read the rays */
  unsigned int nrays; /* number of rays */
  ray_t *ray = NULL;
  ray = readrays(a->filename_in, &nrays);
  if (ray == NULL) {
    error_at_line(-1, errno, __FILE__, __LINE__, "Problem reading ray file.\n");
  }

  /* pulse function */
  void *pfcn_intensity = NULL;
  pfcn_intensity = evaluator_create(a->pfcn_intensity_buffer);
  if (pfcn_intensity == NULL) {
    error_at_line(-1, EINVAL, __FILE__, __LINE__,
                  "Problem creating pulse evaluator function.");
  }

  /* apodization function */
  void *pfcn_g = NULL;
  pfcn_g = evaluator_create(a->pfcn_g_buffer);
  if (pfcn_g == NULL) {
    error_at_line(-1, EINVAL, __FILE__, __LINE__,
                  "Problem creating apodization evaluator function.");
  }

  /* root node prints some useful information */
  if (rank == 0) {
    print_args(a, nrays);
  }

  /* Before allocating what could potentially be a lot of memory, check to
   * see if it's even possible.  */
  unsigned int i;
  a->efield_ss_out = 1;
  for (i = 0; i < 4; ++i) {
    a->efield_ss_out *= a->dim_efield_out[i];
  }

  long page_size = sysconf(_SC_PAGE_SIZE);
  // long pages_avail = sysconf(_SC_AVPHYS_PAGES);
  long pages_tot = sysconf(_SC_PHYS_PAGES);
  if (pages_tot * page_size < 8 * a->efield_ss_out * sizeof(complex double)) {
    error_at_line(-1, ENOMEM, __FILE__, __LINE__,
                  "Size of output arrays exceed avaliable memory.\n");
  }

  /* Allocate memory for the fields.  The _rcv is memory for nodes. */
  complex double *efield_x = NULL;
  complex double *efield_x_rcv = NULL;
  complex double *efield_y = NULL;
  complex double *efield_y_rcv = NULL;
  complex double *efield_z = NULL;
  complex double *efield_z_rcv = NULL;

  double *efield_r = NULL;
  double *efield_r_rcv = NULL;

  /* The root node will be receiving all of the data, so it gets the whole
   * thing.  Calloc here to pre-zero out everything. */
  if (rank == 0) {
    efield_x =
        (complex double *)calloc(a->efield_ss_out, sizeof(complex double));
    efield_y =
        (complex double *)calloc(a->efield_ss_out, sizeof(complex double));
    efield_z =
        (complex double *)calloc(a->efield_ss_out, sizeof(complex double));
    efield_r = (double *)calloc(4 * (a->efield_ss_out), sizeof(double));
  }

  /* The root node and all the other nodes also get this little chunk to
   * work on.  The root node really has nothing better to do while waiting
   * for information so there's no real waste. */
  efield_x_rcv = (complex double *)calloc(a->efield_ss_out / nprocs,
                                          sizeof(complex double));
  efield_y_rcv = (complex double *)calloc(a->efield_ss_out / nprocs,
                                          sizeof(complex double));
  efield_z_rcv = (complex double *)calloc(a->efield_ss_out / nprocs,
                                          sizeof(complex double));
  efield_r_rcv =
      (double *)calloc(4 * (a->efield_ss_out / nprocs), sizeof(double));

  /* Catch some possible malloc errors*/
  if (errno) {
    fprintf(stderr, "Node %d\n", rank);
    error_at_line(-1, errno, __FILE__, __LINE__, "Error allocating memory.");
  }

  unsigned int n;
  double w;
  /* precompute some ray information */
  for (n = 0; n < nrays; ++n, ++ray) {
    w = 2 * M_PI * GSL_CONST_MKSA_SPEED_OF_LIGHT * 1000 / ray->lambda;
    /* ray intensity for gaussian pulse */
    ray->intensity *= evaluator_evaluate_x(pfcn_intensity, w);
    if (isnan(ray->intensity)) {
      error_at_line(-1, errno, __FILE__, __LINE__,
                    "Error with intensity function: got nan.");
    }

    /* analytic apodization factor for parabola */
    ray->g *= evaluator_evaluate_x(pfcn_g, acos(ray->p0[2] / a->fs_radius));
    if (isnan(ray->g)) {
      error_at_line(-1, errno, __FILE__, __LINE__,
                    "Error with apodization function: got nan.");
    }

    /* all of the intensity information is stored in ray->g in order to
     * save a few multiplications in the inner loop */
    ray->g =
        (1.0 / (a->fs_radius * ray->lambda * 0.001)) * ray->g * ray->intensity;

    ray->e0[0] /= ray->lambda;
    ray->e0[1] /= ray->lambda;
    ray->e0[2] /= ray->lambda;
  }
  ray -= nrays;

  /* precompute the position vectors in the efield we'll be dealing with */
  if (rank == 0) {
    double dr[4];

    /* get the spacings of the electric field */
    for (i = 0; i < 4; ++i) {
      if (a->dim_efield_out[i] == 1) {
        dr[i] = (a->dim_efield_ub[i] - a->dim_efield_lb[i]) /
                ((double)a->dim_efield_out[i]);
      } else {
        dr[i] = (a->dim_efield_ub[i] - a->dim_efield_lb[i]) /
                ((double)a->dim_efield_out[i] - 1);
      }
    }

    /* set them */
    unsigned int r_i[4] = {};
    for (r_i[0] = 0; r_i[0] < a->dim_efield_out[0]; r_i[0]++) {
      for (r_i[1] = 0; r_i[1] < a->dim_efield_out[1]; r_i[1]++) {
        for (r_i[2] = 0; r_i[2] < a->dim_efield_out[2]; r_i[2]++) {
          for (r_i[3] = 0; r_i[3] < a->dim_efield_out[3]; r_i[3]++) {
            *efield_r++ = a->dim_efield_lb[0] + dr[0] * (double)r_i[0];
            *efield_r++ = a->dim_efield_lb[1] + dr[1] * (double)r_i[1];
            *efield_r++ = a->dim_efield_lb[2] + dr[2] * (double)r_i[2];
            *efield_r++ = a->dim_efield_lb[3] + dr[3] * (double)r_i[3];
          }
        }
      }
    }
    efield_r -= 4 * a->efield_ss_out;
  }

  /* send this spacing information out to everyone */
  MPI_Scatter(efield_r, 4 * (a->efield_ss_out / nprocs), MPI_DOUBLE,
              efield_r_rcv, 4 * (a->efield_ss_out / nprocs), MPI_DOUBLE, 0,
              MPI_COMM_WORLD);

  /* Main loop over ray data.  This should be fast as possible. */
  /* pre-computing the three efield_r_rcv lines doesn't appear to have a *
   * significant effect on performance */
  complex double tmp;
  for (i = 0; i < a->efield_ss_out / nprocs; ++i) {
    for (n = 0; n < nrays; ++n, ++ray) {
      tmp = -1.0i * ray->g *
            cexp(2.0i * M_PI *
                 ((*(efield_r_rcv + 0) - ray->p0[0]) * ray->e0[0] +
                  (*(efield_r_rcv + 1) - ray->p0[1]) * ray->e0[1] +
                  (*(efield_r_rcv + 2) - ray->p0[2]) * ray->e0[2] - ray->OPL -
                  *(efield_r_rcv + 3) * GSL_CONST_MKSA_SPEED_OF_LIGHT * 1000.0 /
                      ray->lambda +
                  a->temporal_offset * GSL_CONST_MKSA_SPEED_OF_LIGHT * 1000.0 /
                      ray->lambda));
      /* three polarization components */
      efield_x_rcv[i] += ray->pol[0] * tmp;
      efield_y_rcv[i] += ray->pol[1] * tmp;
      efield_z_rcv[i] += ray->pol[2] * tmp;
    }
    ray -= nrays;
    efield_r_rcv += 4;
  }
  efield_r_rcv -= 4 * i;

  /* gather all data from nodes */
  MPI_Gather(efield_x_rcv, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX,
             efield_x, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX, 0,
             MPI_COMM_WORLD);
  MPI_Gather(efield_y_rcv, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX,
             efield_y, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX, 0,
             MPI_COMM_WORLD);
  MPI_Gather(efield_z_rcv, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX,
             efield_z, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX, 0,
             MPI_COMM_WORLD);

  /* write electric field information to HDF file */
  if (rank == 0) {
    write_efield(a, efield_x, efield_y, efield_z);
  }

  /* clean up bit */
  free(efield_x);
  free(efield_y);
  free(efield_z);
  free(a);

  free(efield_x_rcv);
  free(efield_y_rcv);
  free(efield_z_rcv);
  free(efield_r_rcv);
  if (rank == 0) free(efield_r);
  free(ray);
  evaluator_destroy(pfcn_intensity);
  evaluator_destroy(pfcn_g);
  MPI_Finalize();
  return 0;
}

max_stars_repo_name: AaronWebster/directdebye

max_issues_repo_name: AaronWebster/directdebye

max_forks_repo_name: AaronWebster/directdebye


text:
#ifndef __GSL_HELPER_H__
#define __GSL_HELPER_H__

#if defined _WIN32 || defined _WIN64
 #include <crtdbg.h>
 #define _gsl_asserte(expr,msg) _ASSERT_EXPR(expr,_CRT_WIDE(msg))
#else
 #include <assert.h>
 #define _gsl_asserte(expr,msg) assert(expr)
#endif
#define _gsl_assert(expr) _gsl_asserte(expr, #expr)

extern "C"
{
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector_double.h>
#include <gsl/gsl_matrix_double.h>
}
#include <limits>
#include <memory>
#include <functional>
#include <iostream>

namespace gsl
{

struct Precondition
{
	template<typename E, typename Condition>
	void check(Condition cond) {
		if (!cond) throw E();
	}

	template<typename E, typename Condition, typename... ConditionList>
	void check(Condition cond, ConditionList... conds) {
		if (!cond) throw E();
		else check<E>(conds...);
	}

	template<typename E, typename Condition, typename... ConditionList>
	void operator () (Condition cond, ConditionList... conds) {
		return check<E>(cond, conds...);
	}

	const char* file;
	int line;

	Precondition(const char* file, int line)
		: file(file), line(line) {}
};
#define precondition Precondition(__FILE__, __LINE__).operator()

struct out_of_range: public std::exception {};
struct bad_pointer: public std::exception {};
struct bad_size: public std::exception {};
struct zero_devide: public std::exception {};

/**
 * base_vector
 * объектно-ориентированная оболочка для gsl_vector*
 */
class base_const_vector
{
protected:
	base_const_vector() = default;
	base_const_vector(base_const_vector&&) = default;
	base_const_vector(const base_const_vector&) = default;

public:
	double at(size_t i) const throw(std::exception) {
		precondition<out_of_range>(i >= 0);
		precondition<out_of_range>(i < ptr()->size);
		return gsl_vector_get(ptr(), i);
	}

	double operator [] (size_t i) const throw (std::exception) {
		return at(i);
	}

	const double* data() const {
		return ptr()->data;
	}

	size_t size() const {
		return ptr()->size;
	}

	virtual ~base_const_vector() {}
	virtual const gsl_vector* ptr() const = 0;
};

class base_vector: public base_const_vector
{
protected:
	base_vector() = default;
	base_vector(base_vector&&) = default;
	base_vector(const base_vector&) = default;

public:
	double& at(size_t i) throw(std::exception) {
		precondition<out_of_range>(i >= 0);
		precondition<out_of_range>(i < ptr()->size);
		return *gsl_vector_ptr(ptr(), i);
	}

	double& operator [] (size_t i) throw (std::exception) {
		return at(i);
	}

	double* data() {
		return ptr()->data;
	}

	void set_zero() {
		gsl_vector_set_zero(ptr());
	}

	void set_all(double value) {
		gsl_vector_set_all(ptr(), value);
	}

	virtual ~base_vector() {}
	virtual gsl_vector* ptr() = 0;
};

class vector: public base_vector
{
public:
	vector(vector&&) = default;

	explicit vector(size_t n)
	{
		precondition<bad_size>(n > 0);
		myvec = gsl_vector_calloc(n);
		precondition<bad_pointer>(myvec);
	}

	vector(size_t n, double value)
	{
		precondition<bad_size>(n > 0);
		myvec = gsl_vector_alloc(n);
		gsl_vector_set_all(myvec, value);
		precondition<bad_pointer>(myvec);
	}

    vector(const double* array, size_t n)
	{
    	precondition<bad_pointer>(array);
    	precondition<bad_size>(n > 0);
		gsl_vector_const_view view = gsl_vector_const_view_array(array, n);
		myvec = gsl_vector_alloc(n);
		gsl_vector_memcpy(myvec, &view.vector);
		precondition<bad_pointer>(myvec);
	}

	explicit vector(const gsl_vector* vec)
	{
		precondition<bad_pointer>(vec);
		myvec = gsl_vector_alloc(vec->size);
		gsl_vector_memcpy(myvec, vec);
		precondition<bad_pointer>(myvec);
	}

	explicit vector(const gsl_vector_view& view)
	{
		myvec = gsl_vector_alloc(view.vector.size);
		gsl_vector_memcpy(myvec, &view.vector);
		precondition<bad_pointer>(myvec);
	}

	explicit vector(const gsl_vector_const_view& view)
	{
		myvec = gsl_vector_alloc(view.vector.size);
		gsl_vector_memcpy(myvec, &view.vector);
		precondition<bad_pointer>(myvec);
	}

	vector(const base_const_vector& other)
		: vector(other.ptr()) {}

	vector(const vector& other)
		: vector(other.myvec) {}

	~vector() {
		gsl_vector_free(myvec);
	}

	vector& operator = (const vector& other) {
		if( this != &other ) {
			precondition<bad_pointer>(other.myvec);
			precondition<bad_size>(myvec->size == other.myvec->size);
			gsl_vector_memcpy(myvec, other.myvec);
		}
		return *this;
	}

	gsl_vector* ptr() override {
		return myvec;
	}

	const gsl_vector* ptr() const override {
		return myvec;
	}

private:
	gsl_vector* myvec;
};

template<class V>
struct view_traits;

template<>
struct view_traits<gsl_vector_view> {

	using value_t = double;
	using vector_t = gsl_vector;
	using view_t = gsl_vector_view;

	inline static view_t view_array(value_t* arr, size_t n) {
		return gsl_vector_view_array(arr, n);
	}

	inline static view_t view_array_with_stride(value_t* arr, size_t n, size_t stride) {
		return gsl_vector_view_array_with_stride(arr, n, stride);
	}

	inline static view_t subvector(vector_t* v, size_t i, size_t n) {
		return gsl_vector_subvector(v, i, n);
	}

	inline static view_t subvector_with_stride(vector_t* v, size_t i, size_t stride, size_t n) {
		return gsl_vector_subvector_with_stride(v, i, stride, n);
	}
};

template<>
struct view_traits<gsl_vector_const_view> {

	using value_t = const double;
	using vector_t = const gsl_vector;
	using view_t = gsl_vector_const_view;

	inline static view_t view_array(value_t* arr, size_t n) {
		return gsl_vector_const_view_array(arr, n);
	}

	inline static view_t view_array_with_stride(value_t* arr, size_t n, size_t stride) {
		return gsl_vector_const_view_array_with_stride(arr, n, stride);
	}

	inline static view_t subvector(vector_t* v, size_t i, size_t n) {
		return gsl_vector_const_subvector(v, i, n);
	}

	inline static view_t subvector_with_stride(vector_t* v, size_t i, size_t stride, size_t n) {
		return gsl_vector_const_subvector_with_stride(v, i, stride, n);
	}
};

template<class View>
class base_view_t
{
protected:
	typedef view_traits<View> traits;
	typedef typename traits::value_t value_t;
	typedef typename traits::vector_t vector_t;
	typedef typename traits::view_t view_t;

	view_t make_view(vector_t* v, size_t i, size_t stride, size_t n) {
		precondition<bad_pointer>(v);
		precondition<out_of_range>(i >= 0 && i < v->size);
		precondition<out_of_range>(i + stride * (n - 1) < v->size);
		precondition<bad_size>(n > 0 && n <= v->size - i);

		return traits::subvector_with_stride(v, i, stride, n);
	}

public:
	base_view_t(const base_view_t& other)
		: view(other.view) {}

	base_view_t(const view_t& _view)
		: view(_view) {}

	base_view_t(vector_t* v)
		: view( make_view(v, 0, 1, v->size) ) {}

	base_view_t(vector_t* vector, size_t i, size_t n)
		: view( make_view(vector, i, 1, n) ) {}

	base_view_t(vector_t* vector, size_t i, size_t stride, size_t n)
		: view( make_view(vector, i, stride, n) ) {}

protected:
	view_t view;
};

class vector_const_view: public base_const_vector, public base_view_t<gsl_vector_const_view>
{
public:
	using base_view_t<gsl_vector_const_view>::base_view_t;

	vector_const_view(const base_const_vector& v)
		: base_view_t(v.ptr()) {}

	vector_const_view(const base_const_vector& v, size_t i, size_t n)
		: base_view_t(v.ptr(), i, n) {}

	vector_const_view(const base_const_vector& v, size_t i, size_t stride, size_t n)
		: base_view_t(v.ptr(), i, stride, n) {}

	const gsl_vector* ptr() const override {
		return &view.vector;
	}
};

class vector_view: public base_vector, public base_view_t<gsl_vector_view>
{
public:
	using base_view_t<gsl_vector_view>::base_view_t;

	vector_view(base_vector& v)
		: base_view_t(v.ptr()) {}

	vector_view(base_vector& v, size_t i, size_t n)
		: base_view_t(v.ptr(), i, n) {}

	vector_view(base_vector& v, size_t i, size_t stride, size_t n)
		: base_view_t(v.ptr(), i, stride, n) {}

	vector_view& operator = (const vector_view& other) {
		if (this != &other)
			view = other.view;
		return *this;
	}

	const gsl_vector* ptr() const override {
		return &view.vector;
	}

	gsl_vector* ptr() override {
		return &view.vector;
	}
};

inline std::ostream& operator << (std::ostream& output, const base_const_vector& v) {
	output << '{';
	for( size_t i=0; i<v.size(); ++i ) {
		if( i > 0 ) output << ", ";
		output << v[i];
	}
	output << '}';
	return output;
}

}//namespace gsl

#endif //__GSL_HELPER_H__


text:
// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#pragma once

#include "arcana/finally_scope.h"
#include "arcana/functional/inplace_function.h"
#include "arcana/utils/algorithm.h"
#include "sorted_vector.h"

#include <gsl/gsl>
#include <iterator>
#include <map>
#include <mutex>
#include <stdint.h>

namespace mira
{
    /*
        A collection that mimics a coat check. You insert something into it and it gives you a ticket.
        Once you're done with the ticket, your item gets removed from the collection.
    */
    template<typename T, typename MutexT = std::mutex>
    class ticketed_collection
    {
        using seed_t = int64_t;

        struct compare
        {
            bool operator()(const std::pair<seed_t, T>& left, const std::pair<seed_t, T>& right) const
            {
                return left.first < right.first;
            }

            bool operator()(const std::pair<seed_t, T>& left, const seed_t& right) const
            {
                return left.first < right;
            }
        };

        using storage_t = sorted_vector<std::pair<seed_t, T>, compare>;

        struct ticket_functor
        {
            ticket_functor(seed_t id, MutexT& mutex, storage_t& items)
                : m_id{ id }
                , m_mutex{ mutex }
                , m_items{ items }
            {}

            void operator()()
            {
                std::lock_guard<MutexT> guard{ m_mutex };

                auto found = m_items.find(
                    m_id, [](const std::pair<seed_t, T>& left, const seed_t& right) { return left.first == right; });

                assert(found != m_items.end() && "ticketed item wasn't found in the collection");

                m_items.erase(found);
            }

            seed_t m_id;
            MutexT& m_mutex;
            storage_t& m_items;
        };

    public:
        struct iterator
        {
            using iter_t = typename storage_t::const_iterator;

            using difference_type = typename std::iterator_traits<iter_t>::difference_type;
            using value_type = T;
            using pointer = const value_type*;
            using reference = const value_type&;
            using iterator_category = std::input_iterator_tag;

            explicit iterator(const iter_t& itr)
                : m_iter{ itr }
            {}

            bool operator==(const iterator& other) const
            {
                return m_iter == other.m_iter;
            }

            bool operator!=(const iterator& other) const
            {
                return !(*this == other);
            }

            iterator& operator++()
            {
                ++m_iter;
                return *this;
            }

            iterator operator++(int)
            {
                iterator pre{ *this };
                ++m_iter;
                return pre;
            }

            const T* operator->() const
            {
                return &m_iter->second;
            }

            const T& operator*() const
            {
                return m_iter->second;
            }

        private:
            typename storage_t::const_iterator m_iter;
        };

        using ticket = gsl::final_action<
            stdext::inplace_function<void(), sizeof(ticket_functor), alignof(ticket_functor)>>;

        using ticket_scope = finally_scope<ticket>;

        ticketed_collection() = default;
        ticketed_collection(const ticketed_collection&) = delete;
        ticketed_collection& operator=(const ticketed_collection&) = delete;

        iterator begin() const
        {
            return iterator{ m_items.cbegin() };
        }

        iterator end() const
        {
            return iterator{ m_items.cend() };
        }

        auto size() const
        {
            return m_items.size();
        }

        auto empty() const
        {
            return m_items.empty();
        }

        //
        // Inserts an element into the collection and returns a ticket
        // that will remove the item on its destruction. The mutex passed
        // in should be already locked when calling this method. That same
        // mutex will get locked when removing the item from the collection.
        //
        template<typename ElementT>
        ticket insert(ElementT&& element, MutexT& mutex)
        {
            seed_t id = m_seed++;
            m_items.insert(std::pair<seed_t, T>{ id, std::forward<ElementT>(element) });

            return ticket{ ticket_functor(id, mutex, m_items) };
        }

    private:
        seed_t m_seed = 0;
        storage_t m_items;
    };
}

max_stars_repo_name: syntheticmagus/mageslam

max_issues_repo_name: syntheticmagus/mageslam

max_forks_repo_name: syntheticmagus/mageslam