/*
 *  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;
//{
//}

//


/**
 *
 * @file core_dgbtype2cb.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Azzam Haidar
 * @date 2012-12-15
 * @generated d Tue Jan  7 11:44:50 2014
 *
 **/
#include <lapacke.h>
#include "common.h"

//#define AU(m,n) &(A[(m) + LDA*(n)])
//#define AL(m,n) &(A[(m) + LDA*(n)])
#define AL(m_, n_) (A + NB + LDA * (n_) + ((m_)-(n_)))
#define AU(m_, n_) (A + NB + LDA * (n_) + ((m_)-(n_)+NB))
#define VQ(m)     (VQ + (m))
#define VP(m)     (VP + (m))
#define TAUQ(m)   (TAUQ + (m))
#define TAUP(m)   (TAUP + (m))

/***************************************************************************//**
 *
 * @ingroup CORE_double
 *
 *  CORE_dgbtype2cb is a kernel that will operate on a region (triangle) of data
 *  bounded by st and ed. This kernel apply the right update remaining from the
 *  type1 and this later will create a bulge so it eliminate the first column of
 *  the created bulge and do the corresponding Left update.
 *
 *  All detail are available on technical report or SC11 paper.
 *  Azzam Haidar, Hatem Ltaief, and Jack Dongarra. 2011.
 *  Parallel reduction to condensed forms for symmetric eigenvalue problems
 *  using aggregated fine-grained and memory-aware kernels. In Proceedings
 *  of 2011 International Conference for High Performance Computing,
 *  Networking, Storage and Analysis (SC '11). ACM, New York, NY, USA, ,
 *  Article 8 , 11 pages.
 *  http://doi.acm.org/10.1145/2063384.2063394
 *
 *******************************************************************************
 *
 * @param[in] N
 *          The order of the matrix A.
 *
 * @param[in] NB
 *          The size of the band.
 *
 * @param[in, out] A
 *          A pointer to the matrix A of size (3*NB+1)-by-N.
 *
 * @param[in] LDA
 *          The leading dimension of the matrix A. LDA >= max(1,3*NB+1)
 *
 * @param[in, out] V
 *          double array, dimension N if eigenvalue only
 *          requested or (LDV*blkcnt*Vblksiz) if Eigenvectors requested
 *          The Householder reflectors of the previous type 1 are used here
 *          to continue update then new one are generated to eliminate the
 *          bulge and stored in this array.
 *
 * @param[in, out] TAU
 *          double array, dimension (N).
 *          The scalar factors of the Householder reflectors of the previous
 *          type 1 are used here to continue update then new one are generated
 *          to eliminate the bulge and stored in this array.
 *
 * @param[in] st
 *          A pointer to the start index where this kernel will operate.
 *
 * @param[in] ed
 *          A pointer to the end index where this kernel will operate.
 *
 * @param[in] sweep
 *          The sweep number that is eliminated. it serve to calculate the
 *          pointer to the position where to store the Vs and Ts.
 *
 * @param[in] Vblksiz
 *          constant which correspond to the blocking used when applying the Vs.
 *          it serve to calculate the pointer to the position where to store the
 *          Vs and Ts.
 *
 * @param[in] WANTZ
 *          constant which indicate if Eigenvalue are requested or both
 *          Eigenvalue/Eigenvectors.
 *
 * @param[in] WORK
 *          Workspace of size nb.
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *
 ******************************************************************************/
/***************************************************************************
 *          TYPE 2-BAND-bidiag Lower/Upper columnwise-Householder
 ***************************************************************************/
void
CORE_dgbtype2cb(PLASMA_enum uplo, int N, int NB,
                double *A, int LDA,
                double *VQ, double *TAUQ,
                double *VP, double *TAUP,
                int st, int ed, int sweep, int Vblksiz, int WANTZ,
                double *WORK)
{
    double ctmp;
    int i, J1, J2, len, lem, LDX;
    int blkid, vpos, taupos, tpos;

    LDX = LDA-1;
    J1  = ed+1;
    J2  = min(ed+NB,N-1);
    lem = ed-st+1;
    len = J2-J1+1;

    if( uplo == PlasmaUpper ) {
        /* ========================
        *       UPPER CASE
        * ========================*/
        if( len > 0 ) {
            if( WANTZ == 0 ) {
                vpos   = ((sweep+1)%2)*N + st;
                taupos = ((sweep+1)%2)*N + st;
            } else {
                findVTpos(N, NB, Vblksiz, sweep, st,
                          &vpos, &taupos, &tpos, &blkid);
            }
            /* Apply remaining Left commming from type1/3_upper */
            ctmp = (*TAUQ(taupos));
            LAPACKE_dlarfx_work(LAPACK_COL_MAJOR, lapack_const(PlasmaLeft),
                                lem, len, VQ(vpos), ctmp, AU(st, J1), LDX, WORK);
        }

        if( len > 1 ) {
            if( WANTZ == 0 ) {
                vpos   = ((sweep+1)%2)*N + J1;
                taupos = ((sweep+1)%2)*N + J1;
            } else {
                findVTpos(N,NB,Vblksiz,sweep,J1, &vpos, &taupos, &tpos, &blkid);
            }

            /* Remove the top row of the created bulge */
            *VP(vpos) = 1.;
            for(i=1; i<len; i++){
                *VP(vpos+i)     = (*AU(st, J1+i));
                *AU(st, J1+i) = 0.;
            }
            /* Eliminate the row  at st */
            ctmp = (*AU(st, J1));
            LAPACKE_dlarfg_work(len, &ctmp, VP(vpos+1), 1, TAUP(taupos) );
            *AU(st, J1) = ctmp;
            /*
             * Apply Right on A(J1:J2,st+1:ed)
             * We decrease len because we start at row st+1 instead of st.
             * row st is the row that has been revomved;
             */
            lem = lem-1;
            ctmp = *TAUP(taupos);
            LAPACKE_dlarfx_work(LAPACK_COL_MAJOR, lapack_const(PlasmaRight),
                                lem, len, VP(vpos), ctmp, AU(st+1, J1), LDX, WORK);
        }
    }else{
        /* ========================
         *       LOWER CASE
         * ========================*/
        if( len > 0 ) {
            if( WANTZ == 0 ) {
                vpos   = ((sweep+1)%2)*N + st;
                taupos = ((sweep+1)%2)*N + st;
            } else {
                findVTpos(N, NB, Vblksiz, sweep, st,
                          &vpos, &taupos, &tpos, &blkid);
            }
            /* Apply remaining Right commming from type1/3_lower */
            ctmp = (*TAUP(taupos));
            LAPACKE_dlarfx_work(LAPACK_COL_MAJOR, lapack_const(PlasmaRight),
                                len, lem, VP(vpos), ctmp, AL(J1, st), LDX, WORK);
        }
        if( len > 1 ) {
            if( WANTZ == 0 ) {
                vpos   = ((sweep+1)%2)*N + J1;
                taupos = ((sweep+1)%2)*N + J1;
            } else {
                findVTpos(N,NB,Vblksiz,sweep,J1, &vpos, &taupos, &tpos, &blkid);
            }

            /* Remove the first column of the created bulge */
            *VQ(vpos) = 1.;
            memcpy(VQ(vpos+1), AL(J1+1, st), (len-1)*sizeof(double));
            memset(AL(J1+1, st), 0, (len-1)*sizeof(double));
            /* Eliminate the col  at st */
            LAPACKE_dlarfg_work(len, AL(J1, st), VQ(vpos+1), 1, TAUQ(taupos) );
            /*
             * Apply left on A(J1:J2,st+1:ed)
             * We decrease len because we start at col st+1 instead of st.
             * col st is the col that has been revomved;
             */
            lem = lem-1;
            ctmp = (*TAUQ(taupos));
            LAPACKE_dlarfx_work(LAPACK_COL_MAJOR, lapack_const(PlasmaLeft),
                                len, lem, VQ(vpos), ctmp, AL(J1, st+1), LDX, WORK);
        }
    }
    /* end of uplo case */
    return;
}
/***************************************************************************/
#undef AU
#undef AL
#undef VQ
#undef VP
#undef TAUQ
#undef TAUP


// Sammlung verwendeter Funktionen
#include "global.h"
#include "targets.h"
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>
#include <math.h>
#include <stdio.h>
#include "utility.h"
#include <string.h>
#include <time.h>
#include <omp.h>

void VVerlet_Step(const int N,  double  x[N/2] , double v[N/2], double a[N/2], double *t, //mach einzelnen update Schritt nach Velocity-Verlet ohne Hinderniss
					void (*derivmethod) (double *y, double *ans, double t,int N)) {
	double temp_vec[N], abl[N];
	int i,j;
	for (j = 0; j < DIM; j++ )		// heavy particle outside parallel region
	{

		v[j] = v[j] + 0.5 * a[j] * MAXSTEPSIZE;
		x[j] = x[j] + v[j] * MAXSTEPSIZE;
		temp_vec[j] = x[j];					//temp_vec zur abl- berechnung
		temp_vec[DIM + j] = v[j] * mass;
	}
	#pragma omp parallel for 			//parallelisiere über Osszillatoren
	for (i= 0 ; i < OSSZI; i++)
	{
		for (j = 0; j < DIM; j++ )
		{
			v[DIM + j + i*DIM] = v[DIM + j + i*DIM] + 0.5 * a[DIM + j + i*DIM] * MAXSTEPSIZE;
			x[DIM + j + i*DIM] = x[DIM + j + i*DIM] + v[DIM + j + i*DIM] * MAXSTEPSIZE;
			temp_vec[2*DIM + i * 2*DIM + j] = x[DIM + j + i*DIM];	//temp_vec zur abl- berechnung
			temp_vec[3*DIM + i * 2*DIM + j] = v[DIM + j + i*DIM] ;
		}
	}

	derivmethod(temp_vec, abl, *t, N); //bilde Ableitung zu t
										// gehe wieder in x-v Form für a
	for (j = 0; j < DIM; j++ )
	{
		a[j] = abl[DIM + j] / mass;
		v[j] = v[j] + 0.5 * a[j] * MAXSTEPSIZE;
	}

	#pragma omp parallel for
	for (i= 0 ; i < OSSZI; i++)
	{
		for (j = 0; j < DIM; j++ )
		{
			a[DIM +j + i*DIM] = abl[3*DIM + j + i * 2*DIM]/massq[i];
			v[DIM +j + i*DIM] = v[DIM +j + i*DIM]\
									+ 0.5 * a[DIM +j + i*DIM] * MAXSTEPSIZE;
		}
	}
	//#pragma omp parallel for
	//for (i= 0 ; i < N/2; i++)
	//{
	//	v[i] = v[i] + 0.5 * a[i] * MAXSTEPSIZE;
	//}
	Update_Lattice_Position(lattice_position, x, LATTICE_SPACING);
	*t = *t + MAXSTEPSIZE;
}

void VVerlet_Step_deriv(const int N,  double  x[N/2] , double v[N/2], double a[N/2], double *t, //mach einzelnen update Schritt nach Velocity-Verlet ohne Hinderniss
					void (*derivmethod) (double *y, double *ans, double t,int N)) {

	int i,j;
	SETNUMTHREADS
	double sum[DIM];
	#pragma omp parallel 			// Values for Bath particles parralized///////////////////
	{
		int ID = omp_get_thread_num();
		int MAX_THREADS = omp_get_num_threads();

		if (MAX_THREADS < DIM)  // Catch case : not enough threads
		{	for (j= 0; j<DIM; j++)
			{
				sum[j] = 0.0f;				// prepare sum for heavy particle
			}
		}else if (ID < DIM)  // heavy particle dimensions go to threads
		{
			sum[ID] = 0.0f;				// prepare sum for heavy particle
		}
		#pragma omp for
		for (i= 0 ; i < N/2; i++)	// Erstes Halbupdate ////
		{
			double v_temp = v[i] = v[i] + 0.5 * a[i] * MAXSTEPSIZE;		//vnext = v + a/2 *dt
			x[i] = x[i] + v_temp * MAXSTEPSIZE;			//xnext = x + vnext *dt
		}
		#pragma omp for 		// Values for Bath particles parralized
		for (i = 0 ; i < OSSZI; i++)
		{
			double coup = coupling[i];
			double om = ommega[i];
			for(j = 0; j < DIM; j++)
			{		// Ableitung /
				double a_temp = a[DIM + j + i*DIM] = - om * om * x[DIM + j + i*DIM]\
											  + coup * x[j]; 		// dp_q/dt = -w^2 * q+ gamma * x
				v[DIM +j + i*DIM] = v[DIM +j + i*DIM]\
										+ 0.5 * a_temp * MAXSTEPSIZE; // Zweites Halbupdate ////

			}
		}
		double private_sum[DIM];
		double private_c[DIM] = {0.0,0.0};			// Kahan correction
		double private_t[DIM] = {0.0,0.0};
		double private_y[DIM] = {0.0,0.0};
		for(j = 0; j < DIM; j++)		// Values for heavy particle
		{
			private_sum[j] = 0.0; 			// JEder thread berechnet Partialsumme
		}
		#pragma omp for 			// berechne die Summe gamma_i *q_i - gamma_i^2/ommega_i^2 * x
		for (i = 0 ; i < OSSZI; i++)
		{
			double coup = coupling[i];
			double om = ommega[i];
			for (j=0; j<DIM; j++)
			{	// Kahan summation scheme
				private_y[j] = coup * x[DIM + j + i*DIM]\
						 - pow(coup,2.0)/pow(om,2.0) * x[j]  - private_c[j];
				private_t[j] = private_sum[j] + private_y[j];
				private_c[j] = (private_t[j] - private_sum[j]) - private_y[j];
				private_sum[j] = private_t[j];
			}
		}
        for(j=0; j<DIM; j++)
        {
        	#pragma omp atomic		//addiere threadsummen auf
            sum[j] =sum[j] + private_sum[j];
        }
	    #pragma omp barrier
	    if ( MAX_THREADS < DIM)  // Catch case : not enough threads
		{	for (j= 0; j<DIM; j++)
			{
				a[j] = sum[j] /mass ;   	//p-Ableitung
			}
		}else if (ID < DIM)  // heavy particle dimensions go to threads
		{
			a[ID] = sum[ID] /mass ;   	//p-Ableitung
		}
	}

	for(j = 0; j< DIM; j++)
	{
		v[j] = v[j] + 0.5 * a[j] * MAXSTEPSIZE;
	}
	*t = *t + MAXSTEPSIZE;
}

void VVerlet_Step_hardsphere_reflect(const int N,  double  x[] , double v[], double a[], double *t, //mach einzelnen update Schritt nach Velocity-Verlet ohne Hinderniss
					void (*derivmethod) (double *y, double *ans, double t,int N)) {

	int i,j;
	SETNUMTHREADS
	double sum[DIM];						// variable stepsize maximum MAXSTEPSIZE
	double target_vec[20];
	int reflec_flag = 0; 					// set to 1of target if root found
	int target_nr_hit = 0;					// number of actual target hit
	double contact_stepsize;
	// upper search bound
	int number_targets;
	double lower_t_global[2*DIM]; 			// array containing contact times for all targets in cell
	int reflec_flag_global[2*DIM];			// global array tro indicate reflection on target
	for (i=0; i < 2*DIM; i++)
	{
		lower_t_global[i] = MAXSTEPSIZE;	// all times a equal or smaller stepsize, so search for minimum later
		reflec_flag_global[i] = 0;

	}
	// determine time to collsion --------------------------------------------------------------------------------------------
	contact_stepsize = MAXSTEPSIZE;
	Update_Lattice_Position(lattice_position, x, LATTICE_SPACING);
	if 	(	(number_targets =  TARGET_METHOD(lattice_position, target_vec, 	// Find Targets
		LATTICE_SPACING, ESCAPE_CELLS)\
		)>0
		)
	{
		#pragma omp parallel for
		for (int target_i = 0; target_i < number_targets; target_i++)
		{
			double lower_t; 						// lower search bound
			double upper_t;
			int signold; int signnew;
			lower_t = 0.0; 							// lower search bound
			upper_t = MAXSTEPSIZE;					// upper search bound
			double steps = 500.0;
			int root_flag = 1;						// set to one if root found and also one at fitst loop
			int root_loop = 0;						// number of current loop
			int root_loop_max = 3;
			while ((root_flag == 1)	&&	(root_loop < root_loop_max))
			{
				root_flag = 0;
				root_loop += 1;
			// get sign lower bound before loop
				double eval_value = 0.0;
				for(i = 0; i< DIM; i++)
				{
					double v_temp = v[i] + 0.5 * a[i] * lower_t;
					double x_temp = x[i] + v_temp * lower_t;
					eval_value += pow( (x_temp - target_vec[i + DIM * target_i]) , 2.0);		// f = sum (x-R)^2
				}
				eval_value = eval_value - pow(TARGET_LENGTH , 2.0);
				if (eval_value > 0)
				{
					signold = 1;
				}else
				{
					signold = -1;
				}
				double dt = (upper_t - lower_t)/ steps;
			// check all signs in intervall till first change
				for (double t_check = lower_t; t_check < upper_t; t_check += dt)
				{
					eval_value = 0.0;
					for(i = 0; i< DIM; i++)
					{
						double v_temp = v[i] + 0.5 * a[i] * t_check;
						double x_temp = x[i] + v_temp * t_check;
						eval_value += pow( (x_temp - target_vec[i + DIM * target_i]) , 2.0);		// f = sum (x-R)^2
					}
					eval_value = eval_value - pow(TARGET_LENGTH , 2.0);
					//printf("%2.2e\n", eval_value);
					if (eval_value > 0)
					{
						signnew = 1;
					}else
					{
						signnew = -1;
					}
					if (signnew != signold)				// at first sign change adjust intervall
					{
						lower_t = t_check - dt;
						steps = 10000.0;				// refine only 4 orders of magnitude per loop
						upper_t = t_check;
						root_flag = 1;					// redo loop to refine root
						lower_t_global[target_i] = lower_t;
						break;
					}
					signold = signnew;
				}
			}
		}
	}// collision detection end--------------------------------------------------------------------------------------------------
	double minimum_t = MAXSTEPSIZE;
	for (i=0; i < number_targets; i++)				//find smallest reflection time
	{
		if(lower_t_global[i] < minimum_t)
		{
			reflec_flag = 1;
			target_nr_hit = i;
			minimum_t = lower_t_global[i];
		}
	}
	if (reflec_flag ==1 )
	{
		if (TARGET_FLAG)				// check for first contact
		{
			time_first_contact += *t + minimum_t;
			TARGET_FLAG = 0;// no need to test again after 1st contact
		}
		contact_stepsize =  minimum_t;				// pick lower boud so particle is JUST outside the boundary - Else zero search converges badly
	}else
	{
		contact_stepsize = MAXSTEPSIZE;
	}

	#pragma omp parallel 							// Values for Bath particles parralized//
	{
		int ID = omp_get_thread_num();
		int MAX_THREADS = omp_get_num_threads();

		if (MAX_THREADS < DIM) 						// Catch case : not enough threads
		{	for (j= 0; j<DIM; j++)
			{
				sum[j] = 0.0f;						// prepare sum for heavy particle
			}
		}else if (ID < DIM)  						// heavy particle dimensions go to threads
		{
			sum[ID] = 0.0f;							// prepare sum for heavy particle
		}
		#pragma omp for
		for (i= 0 ; i < N/2; i++)					// Erstes Halbupdate ////
		{
			double v_temp = v[i] = v[i] + 0.5 * a[i] * contact_stepsize;		//vnext = v + a/2 *dt
			x[i] = x[i] + v_temp * contact_stepsize;			//xnext = x + vnext *dt
		}
		#pragma omp for 		// Values for Bath particles parralized
		for (i = 0 ; i < OSSZI; i++)
		{
			double coup = coupling[i];
			double om = ommega[i];
			for(j = 0; j < DIM; j++)
			{		// Ableitung /
				double a_temp = a[DIM + j + i*DIM] = - om * om * x[DIM + j + i*DIM]\
											  + coup * x[j]; 		// dp_q/dt = -w^2 * q+ gamma * x
				v[DIM +j + i*DIM] = v[DIM +j + i*DIM]\
										+ 0.5 * a_temp * contact_stepsize; // Zweites Halbupdate ////

			}
		}
		double private_sum[DIM];
		double private_c[DIM] = {0.0,0.0};			// Kahan correction
		double private_t[DIM] = {0.0,0.0};
		double private_y[DIM] = {0.0,0.0};
		for(j = 0; j < DIM; j++)					// Values for heavy particle
		{
			private_sum[j] = 0.0; 					// ech thread calcs partial sum
		}
		#pragma omp for 							// calc sum gamma_i *q_i - gamma_i^2/ommega_i^2 * x
		for (i = 0 ; i < OSSZI; i++)
		{
			double coup = coupling[i];
			double om = ommega[i];
			for (j=0; j<DIM; j++)
			{										// Kahan summation scheme
				private_y[j] = coup * x[DIM + j + i*DIM]\
						 - pow(coup,2.0)/pow(om,2.0) * x[j]  - private_c[j];
				private_t[j] = private_sum[j] + private_y[j];
				private_c[j] = (private_t[j] - private_sum[j]) - private_y[j];
				private_sum[j] = private_t[j];
			}
		}
        for(j=0; j<DIM; j++)
        {
        	#pragma omp atomic		//addiere threadsummen auf
            sum[j] =sum[j] + private_sum[j];
        }
	    #pragma omp barrier
	    if ( MAX_THREADS < DIM)  // Catch case : not enough threads
		{	for (j= 0; j<DIM; j++)
			{
				a[j] = sum[j] /mass ;   	//p-Ableitung
			}
		}else if (ID < DIM)  // heavy particle dimensions go to threads
		{
			a[ID] = sum[ID] /mass ;   	//p-Ableitung
		}
	}

	for(j = 0; j< DIM; j++)
	{
		v[j] = v[j] + 0.5 * a[j] * contact_stepsize;
	}
	*t = *t + contact_stepsize;
	double norm = 0.0;
	for(j = 0; j< DIM; j++)
	{
			norm  += pow(target_vec[j + DIM * target_nr_hit] - x[j],2.0);
	}
	norm = sqrt(norm);
	if (number_targets)
	{
		if(norm < TARGET_LENGTH * 0.98)									// catch case particle entered sphere, only nonempty cells
		{
			FLAG_DUMP_VALUE  = 1;
			printf("\nsphere entered at t=%2.2e, lattice point %d %d", *t, lattice_position[0], lattice_position[1]);
		}
	}

	if ((reflec_flag))												// reflect on surface normal. Assume particle is on the surface
																	// prevent case of inner reflection in case particle still got in
	{
		double surface_normal[DIM];
		double vec_norm = 0.0;
		double scalar_prod = 0.0;
		// find vector towards target
		for(j = 0; j< DIM; j++)
		{
			surface_normal[j] = target_vec[j + DIM * target_nr_hit] - x[j];
		}
		for(j = 0; j< DIM; j++)
		{
			vec_norm += surface_normal[j] * surface_normal[j];
		}
		// point surface vector outwards and normalize
		vec_norm = sqrt(vec_norm);
		for(j = 0; j< DIM; j++)
		{
			surface_normal[j] = surface_normal[j]/vec_norm;
		}
		// find part of v in surface direction
		for(j = 0; j< DIM; j++)
		{
			scalar_prod += surface_normal[j] * v[j];
		}
		// reflect that part
		for(j = 0; j< DIM; j++)
		{
			v[j] = v[j] - 2.0 * scalar_prod * surface_normal[j];
		}

	}
}

void VVerlet_Step_hardsphere_reflect_all(const int N,  double  x[] , double v[], double a[], double *t, //mach einzelnen update Schritt nach Velocity-Verlet ohne Hinderniss
					void (*derivmethod) (double *y, double *ans, double t,int N)) {

	int i,j;
	SETNUMTHREADS
	double sum[DIM];						// variable stepsize maximum MAXSTEPSIZE
	double target_vec[20];
	int reflec_flag = 0; 					// set to 1of target if root found
	int target_nr_hit = 0;					// number of actual target hit
	double contact_stepsize;
	// upper search bound
	int number_targets;
	double lower_t_global[2*DIM]; 			// array containing contact times for all targets in cell
	int reflec_flag_global[2*DIM];			// global array tro indicate reflection on target
	for (i=0; i < 2*DIM; i++)
	{
		lower_t_global[i] = MAXSTEPSIZE;	// all times a equal or smaller stepsize, so search for minimum later
		reflec_flag_global[i] = 0;

	}
	// determine time to collsion --------------------------------------------------------------------------------------------
	contact_stepsize = MAXSTEPSIZE;
	Update_Lattice_Position(lattice_position, x, LATTICE_SPACING);
	if 	(	(number_targets =  TARGET_METHOD(lattice_position, target_vec, 	// Find Targets
		LATTICE_SPACING, ESCAPE_CELLS)\
		)>0
		)
	{
		#pragma omp parallel for
		for (int target_i = 0; target_i < number_targets; target_i++)
		{
			double lower_t; 						// lower search bound
			double upper_t;
			int signold; int signnew;
			lower_t = 0.0; 							// lower search bound
			upper_t = MAXSTEPSIZE;					// upper search bound
			double steps = 500.0;
			int root_flag = 1;						// set to one if root found and also one at fitst loop
			int root_loop = 0;						// number of current loop
			int root_loop_max = 3;
			while ((root_flag == 1)	&&	(root_loop < root_loop_max))
			{
				root_flag = 0;
				root_loop += 1;
			// get sign lower bound before loop
				double eval_value = 0.0;
				for(i = 0; i< DIM; i++)
				{
					double v_temp = v[i] + 0.5 * a[i] * lower_t;
					double x_temp = x[i] + v_temp * lower_t;
					eval_value += pow( (x_temp - target_vec[i + DIM * target_i]) , 2.0);		// f = sum (x-R)^2
				}
				eval_value = eval_value - pow(TARGET_LENGTH , 2.0);
				if (eval_value > 0)
				{
					signold = 1;
				}else
				{
					signold = -1;
				}
				double dt = (upper_t - lower_t)/ steps;
			// check all signs in intervall till first change
				for (double t_check = lower_t; t_check < upper_t; t_check += dt)
				{
					eval_value = 0.0;
					for(i = 0; i< DIM; i++)
					{
						double v_temp = v[i] + 0.5 * a[i] * t_check;
						double x_temp = x[i] + v_temp * t_check;
						eval_value += pow( (x_temp - target_vec[i + DIM * target_i]) , 2.0);		// f = sum (x-R)^2
					}
					eval_value = eval_value - pow(TARGET_LENGTH , 2.0);
					//printf("%2.2e\n", eval_value);
					if (eval_value > 0)
					{
						signnew = 1;
					}else
					{
						signnew = -1;
					}
					if (signnew != signold)				// at first sign change adjust intervall
					{
						lower_t = t_check - dt;
						steps = 10000.0;				// refine only 4 orders of magnitude per loop
						upper_t = t_check;
						root_flag = 1;					// redo loop to refine root
						lower_t_global[target_i] = lower_t;
						break;
					}
					signold = signnew;
				}
			}
		}
	}// collision detection end--------------------------------------------------------------------------------------------------
	double minimum_t = MAXSTEPSIZE;
	for (i=0; i < number_targets; i++)				//find smallest reflection time
	{
		if(lower_t_global[i] < minimum_t)
		{
			reflec_flag = 1;
			target_nr_hit = i;
			minimum_t = lower_t_global[i];
		}
	}
	if (reflec_flag ==1 )
	{
		if (TARGET_FLAG)				// check for first contact
		{
			time_first_contact += *t + minimum_t;
			TARGET_FLAG = 0;// no need to test again after 1st contact
		}
		contact_stepsize =  minimum_t;				// pick lower boud so particle is JUST outside the boundary - Else zero search converges badly
	}else
	{
		contact_stepsize = MAXSTEPSIZE;
	}

	#pragma omp parallel 							// Values for Bath particles parralized//
	{
		int ID = omp_get_thread_num();
		int MAX_THREADS = omp_get_num_threads();

		if (MAX_THREADS < DIM) 						// Catch case : not enough threads
		{	for (j= 0; j<DIM; j++)
			{
				sum[j] = 0.0f;						// prepare sum for heavy particle
			}
		}else if (ID < DIM)  						// heavy particle dimensions go to threads
		{
			sum[ID] = 0.0f;							// prepare sum for heavy particle
		}
		#pragma omp for
		for (i= 0 ; i < N/2; i++)					// Erstes Halbupdate ////
		{
			double v_temp = v[i] = v[i] + 0.5 * a[i] * contact_stepsize;		//vnext = v + a/2 *dt
			x[i] = x[i] + v_temp * contact_stepsize;			//xnext = x + vnext *dt
		}
		#pragma omp for 		// Values for Bath particles parralized
		for (i = 0 ; i < OSSZI; i++)
		{
			double coup = coupling[i];
			double om = ommega[i];
			for(j = 0; j < DIM; j++)
			{		// Ableitung /
				double a_temp = a[DIM + j + i*DIM] = - om * om * x[DIM + j + i*DIM]\
											  + coup * x[j]; 		// dp_q/dt = -w^2 * q+ gamma * x
				v[DIM +j + i*DIM] = v[DIM +j + i*DIM]\
										+ 0.5 * a_temp * contact_stepsize; // Zweites Halbupdate ////

			}
		}
		double private_sum[DIM];
		double private_c[DIM] = {0.0,0.0};			// Kahan correction
		double private_t[DIM] = {0.0,0.0};
		double private_y[DIM] = {0.0,0.0};
		for(j = 0; j < DIM; j++)					// Values for heavy particle
		{
			private_sum[j] = 0.0; 					// ech thread calcs partial sum
		}
		#pragma omp for 							// calc sum gamma_i *q_i - gamma_i^2/ommega_i^2 * x
		for (i = 0 ; i < OSSZI; i++)
		{
			double coup = coupling[i];
			double om = ommega[i];
			for (j=0; j<DIM; j++)
			{										// Kahan summation scheme
				private_y[j] = coup * x[DIM + j + i*DIM]\
						 - pow(coup,2.0)/pow(om,2.0) * x[j]  - private_c[j];
				private_t[j] = private_sum[j] + private_y[j];
				private_c[j] = (private_t[j] - private_sum[j]) - private_y[j];
				private_sum[j] = private_t[j];
			}
		}
        for(j=0; j<DIM; j++)
        {
        	#pragma omp atomic		//addiere threadsummen auf
            sum[j] =sum[j] + private_sum[j];
        }
	    #pragma omp barrier
	    if ( MAX_THREADS < DIM)  // Catch case : not enough threads
		{	for (j= 0; j<DIM; j++)
			{
				a[j] = sum[j] /mass ;   	//p-Ableitung
			}
		}else if (ID < DIM)  // heavy particle dimensions go to threads
		{
			a[ID] = sum[ID] /mass ;   	//p-Ableitung
		}
	}

	for(j = 0; j< DIM; j++)
	{
		v[j] = v[j] + 0.5 * a[j] * contact_stepsize;
	}
	*t = *t + contact_stepsize;
	double norm = 0.0;
	for(j = 0; j< DIM; j++)
	{
			norm  += pow(target_vec[j + DIM * target_nr_hit] - x[j],2.0);
	}
	norm = sqrt(norm);
	if (number_targets)
	{
		if(norm < TARGET_LENGTH * 0.98)									// catch case particle entered sphere, only nonempty cells
		{
			FLAG_DUMP_VALUE  = 1;
			printf("\nsphere entered at t=%2.2e, lattice point %d %d", *t, lattice_position[0], lattice_position[1]);
		}
	}

	if ((reflec_flag))												// reflect on surface normal. Assume particle is on the surface
																	// prevent case of inner reflection in case particle still got in
	{
		double surface_normal[DIM];
		double vec_norm = 0.0;
		double scalar_prod = 0.0;
		// find vector towards target
		for(j = 0; j< DIM; j++)
		{
			surface_normal[j] = target_vec[j + DIM * target_nr_hit] - x[j];
		}
		for(j = 0; j< DIM; j++)
		{
			vec_norm += surface_normal[j] * surface_normal[j];
		}
		// point surface vector outwards and normalize
		vec_norm = sqrt(vec_norm);
		for(j = 0; j< DIM; j++)
		{
			surface_normal[j] = surface_normal[j]/vec_norm;
		}
		// find part of v in surface direction
		for(j = 0; j< DIM; j++)
		{
			scalar_prod += surface_normal[j] * v[j];
		}
		// reflect that part for ALL
		#pragma omp parallel for
		for (i = 0; i < OSSZI; i++ )
		{
			for(j = 0; j< DIM; j++)
			{
				v[i*DIM + j] = v[i*DIM + j] - 2.0 * scalar_prod * surface_normal[j];
			}
		}
	}
}

void VVerlet_Step_deriv_Kupf(const int N,  double  x[N/2] , double v[N/2], double a[N/2], double *t,
					void (*derivmethod) (double *y, double *ans, double t,int N)) {
	//mach einzelnen update Schritt nach Velocity-Verlet ohne Hinderniss
	// use fully mobile Kupferman bath that is coupling U = sum gamma_i (q_i-X)^2

	int i,j;
	SETNUMTHREADS
	double sum[DIM];
	#pragma omp parallel 			// Values for Bath particles parralized///////////////////
	{
		int ID = omp_get_thread_num();
		int MAX_THREADS = omp_get_num_threads();

		if (MAX_THREADS < DIM)  // Catch case : not enough threads
		{	for (j= 0; j<DIM; j++)
			{
				sum[j] = 0.0f;				// prepare sum for heavy particle
			}
		}else if (ID < DIM)  // heavy particle dimensions go to threads
		{
			sum[ID] = 0.0f;				// prepare sum for heavy particle
		}
		#pragma omp for
		for (i= 0 ; i < N/2; i++)	// Erstes Halbupdate ////
		{
			double v_temp = v[i] = v[i] + 0.5 * a[i] * MAXSTEPSIZE;		//vnext = v + a/2 *dt
			x[i] = x[i] + v_temp * MAXSTEPSIZE;			//xnext = x + vnext *dt
		}
		#pragma omp for 		// Values for Bath particles parralized
		for (i = 0 ; i < OSSZI; i++)
		{
			double coup = coupling[i];
			double om = ommega[i];
			for(j = 0; j < DIM; j++)
			{		// Ableitung /
				double a_temp = a[DIM + j + i*DIM] = -coup * (x[DIM + j + i*DIM] - x[j])/massq[i];
				v[DIM +j + i*DIM] = v[DIM +j + i*DIM]\
										+ 0.5 * a_temp * MAXSTEPSIZE; // Zweites Halbupdate ////

			}
		}
		double private_sum[DIM];
		double private_c[DIM] = {0.0,0.0};			// Kahan correction
		double private_t[DIM] = {0.0,0.0};
		double private_y[DIM] = {0.0,0.0};
		for(j = 0; j < DIM; j++)		// Values for heavy particle
		{
			private_sum[j] = 0.0; 			// JEder thread berechnet Partialsumme
		}
		#pragma omp for 			// berechne die Summe gamma_i *q_i - gamma_i^2/ommega_i^2 * x
		for (i = 0 ; i < OSSZI; i++)
		{
			double coup = coupling[i];
			double om = ommega[i];
			for (j=0; j<DIM; j++)
			{	// Kahan summation scheme
				private_y[j] = coup * (x[DIM + j + i*DIM] - x[j]) - private_c[j];
				private_t[j] = private_sum[j] + private_y[j];
				private_c[j] = (private_t[j] - private_sum[j]) - private_y[j];
				private_sum[j] = private_t[j];
			}
		}
        for(j=0; j<DIM; j++)
        {
        	#pragma omp atomic		//addiere threadsummen auf
            sum[j] =sum[j] + private_sum[j];
        }
	    #pragma omp barrier
	    if ( MAX_THREADS < DIM)  // Catch case : not enough threads
		{	for (j= 0; j<DIM; j++)
			{
				a[j] = sum[j] /mass ;   	//p-Ableitung
			}
		}else if (ID < DIM)  // heavy particle dimensions go to threads
		{
			a[ID] = sum[ID] /mass ;   	//p-Ableitung
		}
	}

	for(j = 0; j< DIM; j++)
	{
		v[j] = v[j] + 0.5 * a[j] * MAXSTEPSIZE;
		VIRTUAL_V[j] = v[j];
		VIRTUAL_X[j] = x[j];
	}
	*t = *t + MAXSTEPSIZE;
}

void VVerlet_Step_hardsphere_reflect_Kupf(const int N,  double  x[] , double v[], double a[], double *t, //mach einzelnen update Schritt nach Velocity-Verlet ohne Hinderniss
					void (*derivmethod) (double *y, double *ans, double t,int N)) {

	int i,j;
	SETNUMTHREADS
	double sum[DIM];						// variable stepsize maximum MAXSTEPSIZE
	double target_vec[20];
	int reflec_flag = 0; 					// set to 1of target if root found
	int target_nr_hit = 0;					// number of actual target hit
	double contact_stepsize = LASTSTEPSIZE;
	// upper search bound
	int number_targets;
	double lower_t_global[2*DIM]; 			// array containing contact times for all targets in cell
	int reflec_flag_global[2*DIM];			// global array tro indicate reflection on target
	for (i=0; i < 2*DIM; i++)
	{
		lower_t_global[i] = LASTSTEPSIZE;	// all times a equal or smaller stepsize, so search for minimum later
		reflec_flag_global[i] = 0;

	}
	// determine time to collsion --------------------------------------------------------------------------------------------
	contact_stepsize = LASTSTEPSIZE;
	if(VIRTUAL_FLAG)
	{
		Update_Lattice_Position(lattice_position, VIRTUAL_X, LATTICE_SPACING);
	}else
	{
		Update_Lattice_Position(lattice_position, x, LATTICE_SPACING);
	}

	if 	(	(number_targets =  TARGET_METHOD(lattice_position, target_vec, 	// Find Targets
		LATTICE_SPACING, ESCAPE_CELLS)\
		)>0
		)
	{
		double N_CHECK = 1000.0;
		#pragma omp parallel for
		for (int target_i = 0; target_i < number_targets; target_i++)	// check for all targets in increments of LASTSTEPSIZE/N_CHECK for contact
		{
			double dh = LASTSTEPSIZE /  N_CHECK;
			for (double h_check = 0.0; h_check < LASTSTEPSIZE; h_check += dh)
			{
				double r_check = 0.0;
				for (int j = 0; j < DIM; j++)
				{
					double v_temp = v[j] + 0.5 * a[j] * h_check;
					double x_temp = x[j] + v_temp * h_check;
					if(VIRTUAL_FLAG)
					{
						v_temp = VIRTUAL_V[j] + 0.5 * a[j] * h_check;
						x_temp = VIRTUAL_X[j] + v_temp * h_check;
					}
					r_check += pow( (x_temp - target_vec[j + target_i * DIM] ) ,2.0 );
				}
				r_check = sqrt(r_check);
				if (r_check < TARGET_LENGTH)
				{
					reflec_flag = 1;
					target_nr_hit = target_i;
					contact_stepsize = h_check - dh;
					if (TARGET_FLAG)				// check for first contact
					{
						time_first_contact += *t + contact_stepsize;
						TARGET_FLAG = 0;// no need to test again after 1st contact
					}
					break;
					break;
				}
			}
		}
	}
	//-------------------------------------------------------------------------------------------------------------------
	#pragma omp parallel 							// Values for Bath particles parralized//
	{
		int ID = omp_get_thread_num();
		int MAX_THREADS = omp_get_num_threads();

		if (MAX_THREADS < DIM) 						// Catch case : not enough threads
		{	for (j= 0; j<DIM; j++)
			{
				sum[j] = 0.0f;						// prepare sum for heavy particle
			}
		}else if (ID < DIM)  						// heavy particle dimensions go to threads
		{
			sum[ID] = 0.0f;							// prepare sum for heavy particle
		}
		#pragma omp for
		for (i= 0 ; i < N/2; i++)					// Erstes Halbupdate ////
		{
			double v_temp = v[i] = v[i] + 0.5 * a[i] * contact_stepsize;		//vnext = v + a/2 *dt
			x[i] = x[i] + v_temp * contact_stepsize;			//xnext = x + vnext *dt
		}
		if(VIRTUAL_FLAG)
		{
			#pragma omp for
			for (i= 0 ; i < DIM; i++)					// Erstes Halbupdate ////
			{
				double v_temp = VIRTUAL_V[i] = VIRTUAL_V[i] + 0.5 * a[i] * contact_stepsize;		//vnext = v + a/2 *dt
				VIRTUAL_X[i] = VIRTUAL_X[i] + v_temp * contact_stepsize;			//xnext = x + vnext *dt
			}
		}
		#pragma omp for 		// Values for Bath particles parralized
		for (i = 0 ; i < OSSZI; i++)
		{
			double coup = coupling[i];
			double om = ommega[i];
			for(j = 0; j < DIM; j++)
			{		// Ableitung /
				double a_temp = a[DIM + j + i*DIM] = -coup * (x[DIM + j + i*DIM] - x[j])/massq[i]; 		// dp_q/dt = -w^2 * q+ gamma * x
				v[DIM +j + i*DIM] = v[DIM +j + i*DIM]\
										+ 0.5 * a_temp * contact_stepsize; // Zweites Halbupdate ////

			}
		}
		double private_sum[DIM];
		double private_c[DIM] = {0.0,0.0};			// Kahan correction
		double private_t[DIM] = {0.0,0.0};
		double private_y[DIM] = {0.0,0.0};
		for(j = 0; j < DIM; j++)					// Values for heavy particle
		{
			private_sum[j] = 0.0; 					// ech thread calcs partial sum
		}
		#pragma omp for 							// calc sum gamma_i *q_i - gamma_i^2/ommega_i^2 * x
		for (i = 0 ; i < OSSZI; i++)
		{
			double coup = coupling[i];
			double om = ommega[i];
			for (j=0; j<DIM; j++)
			{										// Kahan summation scheme
				private_y[j] = coup * (x[DIM + j + i*DIM] - x[j]) - private_c[j];
				private_t[j] = private_sum[j] + private_y[j];
				private_c[j] = (private_t[j] - private_sum[j]) - private_y[j];
				private_sum[j] = private_t[j];
			}
		}
        for(j=0; j<DIM; j++)
        {
        	#pragma omp atomic		//addiere threadsummen auf
            sum[j] =sum[j] + private_sum[j];
        }
	    #pragma omp barrier
	    if ( MAX_THREADS < DIM)  // Catch case : not enough threads
		{	for (j= 0; j<DIM; j++)
			{
				a[j] = sum[j] /mass ;   	//p-Ableitung
			}
		}else if (ID < DIM)  // heavy particle dimensions go to threads
		{
			a[ID] = sum[ID] /mass ;   	//p-Ableitung
		}
	}

	for(j = 0; j< DIM; j++)
	{
		v[j] = v[j] + 0.5 * a[j] * contact_stepsize;
	}
	if(VIRTUAL_FLAG)
	{
		for(j = 0; j< DIM; j++)
		{
			VIRTUAL_V[j] = VIRTUAL_V[j] + 0.5 * a[j] * contact_stepsize;
		}
	}
	*t = *t + contact_stepsize;
	double norm = 0.0;
	for(j = 0; j< DIM; j++)
	{
		if(VIRTUAL_FLAG)
		{
			norm  += pow(target_vec[j + DIM * target_nr_hit] - VIRTUAL_X[j],2.0);
		}else
		{
			norm  += pow(target_vec[j + DIM * target_nr_hit] - x[j],2.0);
		}
	}
	norm = sqrt(norm);
	if (number_targets)
	{
		if(norm < TARGET_LENGTH * 0.98)									// catch case particle entered sphere, only nonempty cells
		{
			FLAG_DUMP_VALUE  = 1;
			printf("\nsphere entered at t=%2.2e, lattice point %d %d", *t, lattice_position[0], lattice_position[1]);
		}
	}

	if ((reflec_flag))												// reflect on surface normal. Assume particle is on the surface
																	// prevent case of inner reflection in case particle still got in
	{
		double surface_normal[DIM];
		double vec_norm = 0.0;
		double scalar_prod = 0.0;
		// find vector towards target
		for(j = 0; j< DIM; j++)
		{

			if(VIRTUAL_FLAG)
			{
				surface_normal[j] = target_vec[j + DIM * target_nr_hit] - VIRTUAL_X[j];
			}else
			{
				surface_normal[j] = target_vec[j + DIM * target_nr_hit] - x[j];
			}
		}
		for(j = 0; j< DIM; j++)
		{
			vec_norm += surface_normal[j] * surface_normal[j];
		}
		// point surface vector outwards and normalize
		vec_norm = sqrt(vec_norm);
		for(j = 0; j< DIM; j++)
		{
			surface_normal[j] = surface_normal[j]/vec_norm;
		}
		// find part of v in surface direction
		for(j = 0; j< DIM; j++)
		{
			if(VIRTUAL_FLAG)
			{
				scalar_prod += surface_normal[j] * VIRTUAL_V[j];
			}else
			{
				scalar_prod += surface_normal[j] * v[j];
			}
		}
		// reflect that part
		for(j = 0; j< DIM; j++)
		{
			if(VIRTUAL_FLAG)
			{
				VIRTUAL_V[j] = VIRTUAL_V[j] - 2.0 * scalar_prod * surface_normal[j];
			}else
			{
				v[j] = v[j] - 2.0 * scalar_prod * surface_normal[j];
			}
		}
	}
	if ((reflec_flag))
	{
		LASTSTEPSIZE = LASTSTEPSIZE - contact_stepsize; 		// do remaining step time
	}
	else
	{
		LASTSTEPSIZE = MAXSTEPSIZE;
	}
}

void VVerlet_Step_deriv_Box(const int N,  double  x[N/2] , double v[N/2], double a[N/2], double *t,
					void (*derivmethod) (double *y, double *ans, double t,int N)) {
	//macht einzelnen update schritt in Box der Länge Lattice_Spacting
	// use fully mobile Kupferman bath that is coupling U = sum gamma_i (q_i-X)^2

	int i,j;
	SETNUMTHREADS
	double sum = 0.0;
	double contact_stepsize = LASTSTEPSIZE;
	int reflec_flag = 0;
	// find contact time ------------------------------------
	double aa,b,c;
	aa=b=c=0;

	int REFLEC_WALL = 1;
	if (REFLEC_WALL)
	{											// debugging purposes, free evolution if false
		for (int i_bound = -1; i_bound < 2; i_bound += 2)
		{
			double root1 = 0.0; double  root2 = 0.0;
			if(VIRTUAL_FLAG)
			{
				c = VIRTUAL_X[0] + i_bound * LATTICE_SPACING/2.0;
				b = VIRTUAL_V[0];
				aa = 0.5 * a[0];
			}else
			{
				c = x[0] + i_bound * LATTICE_SPACING/2.0;
				b = v[0];
				aa = 0.5 * a[0];
			}

			if(b >= 0)													//  minimize rounoff err by case split
			{
				root1 = (-b - sqrt(b*b - 4.0 * aa * c)) / (2.0 * aa);
				root2 = 2.0 * c /(-b - sqrt(b*b - 4.0 * aa * c) );
			}else
			{
				root1 =  2.0 * c /(-b + sqrt(b*b - 4.0 * aa * c) );
				root2 = (-b + sqrt(b*b - 4.0 * aa * c)) / (2.0 * aa);
			}
			if( (root1 > 0) && (root1 < contact_stepsize) )
			{
				contact_stepsize = 0.995 * root1;
				reflec_flag = 1;
			}
			if( (root2 > 0) && (root2 < contact_stepsize) )
			{
				contact_stepsize = 0.995 * root2;
				reflec_flag = 1;
			}
		}
	}
	// contact time end ||||||||||||||||||||||||||||||||||||<

	#pragma omp parallel for
	for (i= 0 ; i < N/2; i++)	// Erstes Halbupdate ////
	{
		double v_temp = v[i] = v[i] + 0.5 * a[i] * contact_stepsize;		//vnext = v + a/2 *dt
		x[i] = x[i] + v_temp * contact_stepsize;							//xnext = x + vnext *dt

	}
	if(VIRTUAL_FLAG)
	{
		double v_temp = VIRTUAL_V[0] = VIRTUAL_V[0] + 0.5 * a[0] * contact_stepsize;	// virtual particle
		VIRTUAL_X[0] = VIRTUAL_X[0] + v_temp * contact_stepsize;
	}
	for (i = 0 ; i < OSSZI; i++)
	{
		double coup = coupling[i];
		double om = ommega[i];
		double a_temp = a[1 + i] = -coup * (x[1 + i] - x[0])/massq[i];
		v[1 + i] = v[1 + i]\
					+ 0.5 * a_temp * contact_stepsize; // Zweites Halbupdate ////
	}
	#pragma omp parallel for reduction(+:sum) 			// berechne die Summe gamma_i *q_i - gamma_i^2/ommega_i^2 * x
	for (i = 0 ; i < OSSZI; i++)
	{
		double coup = coupling[i];
		sum += coup * (x[1 + i] - x[0]);
	}
	a[0] = sum /mass;
	v[0] = v[0] + 0.5 * a[0] * contact_stepsize;
	if(VIRTUAL_FLAG)
	{
		VIRTUAL_V[0] = VIRTUAL_V[0] + 0.5 * a[0] * contact_stepsize;
	}
	*t = *t + contact_stepsize;
	if(reflec_flag)
	{
		LASTSTEPSIZE = LASTSTEPSIZE - contact_stepsize;
		if(REFLECTALL_FLAG)								// reflect whole bath if flag is set
		{
			for (i = 0 ; i < OSSZI; i++)
			{
				double v_rel = v[1 + i] - v[0];
				v[1 + i] = - v[0] + v_rel;						//keep relative velocity after reflection of heavy particle the same
			}
		}
		if(VIRTUAL_FLAG)
		{
			VIRTUAL_V[0] = -VIRTUAL_V[0];										// reflect heavy
		}else
		{
			v[0] = -v[0];										// reflect heavy
		}
	}
	else
	{
		LASTSTEPSIZE = MAXSTEPSIZE;
	}
}

void VVerlet_Step_deriv_Box_Ramp(const int N,  double  x[N/2] , double v[N/2], double a[N/2], double *t,
					void (*derivmethod) (double *y, double *ans, double t,int N)) {
	//macht einzelnen update schritt in Box der Länge Lattice_Spacting
	// use fully mobile Kupferman bath that is coupling U = sum gamma_i (q_i-X)^2

	int i,j;
	SETNUMTHREADS
	double sum = 0.0;
	double contact_stepsize = LASTSTEPSIZE;
	int reflec_flag = 0;

	#pragma omp parallel for
	for (i= 0 ; i < N/2; i++)	// Erstes Halbupdate ////
	{
		double v_temp = v[i] = v[i] + 0.5 * a[i] * contact_stepsize;		//vnext = v + a/2 *dt
		x[i] = x[i] + v_temp * contact_stepsize;							//xnext = x + vnext *dt
	}
	for (i = 0 ; i < OSSZI; i++)
	{
		double coup = coupling[i];
		double om = ommega[i];
		double a_temp = a[1 + i] = -coup * (x[1 + i] - x[0])/massq[i];
		v[1 + i] = v[1 + i]\
					+ 0.5 * a_temp * contact_stepsize; // Zweites Halbupdate ////
	}
	#pragma omp parallel for reduction(+:sum) 			// berechne die Summe gamma_i *q_i - gamma_i^2/ommega_i^2 * x
	for (i = 0 ; i < OSSZI; i++)
	{
		double coup = coupling[i];
		sum += coup * (x[1 + i] - x[0]);
	}
	a[0] = sum /mass;

	if(x[0] < -(LATTICE_SPACING/2.0 - L_BALL) )			//left wall;
	{
		double height_ramp = 2*KBOLTZ*TEMP;
		a[0] += height_ramp/L_BALL/mass;
	}
	if(x[0] > (LATTICE_SPACING/2.0 - L_BALL) )			//right wall;
	{
		double height_ramp = 2*KBOLTZ*TEMP;
		a[0] += -height_ramp/L_BALL/mass;
	}
	v[0] = v[0] + 0.5 * a[0] * contact_stepsize;

	*t = *t + contact_stepsize;
}