/*
 *  vbHmmTs.c
 *  Model-specific core functions for VB-HMM-TS.
 *
 *  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 "vbHmmTs.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

//// Uncomment one/both of the following defenitions to activate constraint on I or/and K.
//#define  INTENSITY_CAP
//#define  TRANSITION_RATE_CAP
#ifdef INTENSITY_CAP
#define  maxIntensityRatio  10.0
#endif
#ifdef TRANSITION_RATE_CAP
#define  minPhotonNumPerState  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_ts(){
    commonFunctions funcs;
    funcs.newModelParameters    = newModelParameters_ts;
    funcs.freeModelParameters   = freeModelParameters_ts;
    funcs.newModelStats         = newModelStats_ts;
    funcs.freeModelStats        = freeModelStats_ts;
    funcs.initializeVbHmm       = initializeVbHmm_ts;
    funcs.pTilde_z1             = pTilde_z1_ts;
    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_ts;
    funcs.pTilde_xn_zn          = pTilde_xn_zn_ts;
    funcs.calcStatsVars         = calcStatsVars_ts;
    funcs.maximization          = maximization_ts;
    funcs.varLowerBound         = varLowerBound_ts;
    funcs.reorderParameters     = reorderParameters_ts;
    funcs.outputResults         = outputResults_ts;
    setFunctions( funcs );
}

//void setGFunctions_ts(){
//    gCommonFunctions funcs;
//    funcs.newModelParameters    = newModelParameters_ts;
//    funcs.freeModelParameters   = freeModelParameters_ts;
//    funcs.newModelStats         = newModelStats_ts;
//    funcs.freeModelStats        = freeModelStats_ts;
//    funcs.newModelStatsG        = newModelStatsG_ts;
//    funcs.freeModelStatsG       = freeModelStatsG_ts;
//    funcs.initializeVbHmmG      = initializeVbHmmG_ts;
//    funcs.pTilde_z1             = pTilde_z1_ts;
//    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_ts;
//    funcs.pTilde_xn_zn          = pTilde_xn_zn_ts;
//    funcs.calcStatsVarsG        = calcStatsVarsG_ts;
//    funcs.maximizationG         = maximizationG_ts;
//    funcs.varLowerBoundG        = varLowerBoundG_ts;
//    funcs.reorderParametersG    = reorderParametersG_ts;
//    funcs.outputResultsG        = outputResultsG_ts;
//    setGFunctions( funcs );
//    isGlobalAnalysis = 1;
//}

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

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

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

    p->uPiArr = (double*)malloc( sNo * sizeof(double) );
    p->sumUPi = 0.0;
    // hyperparameter for p( k(i,j) ) (i != j)
    p->uKMat = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->uKMat[i] = (double*)malloc( sNo * sizeof(double) );
    }
    p->sumUKArr = (double*)malloc( sNo * sizeof(double) );
    // hyperparameter for p( I(k) )
    p->aIArr = (double*)malloc( sNo * sizeof(double) );
    p->bIArr = (double*)malloc( sNo * sizeof(double) );

    p->avgPi = (double *)malloc( sNo * sizeof(double) );
    p->avgLnPi = (double *)malloc( sNo * sizeof(double) );
    p->avgI = (double *)malloc( sNo * sizeof(double) );
    p->avgLnI = (double *)malloc( sNo * sizeof(double) );
    p->avgK = (double **)malloc( sNo * sizeof(double*) );
    p->avgLnK = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->avgK[i] = (double *)malloc( sNo * sizeof(double) );
        p->avgLnK[i] = (double *)malloc( sNo * sizeof(double) );
    }
    p->avgLnKI = (double *)malloc( sNo * sizeof(double) );

    return p;
}

void freeModelParameters_ts( p, xn, sNo )
void **p;
xnDataSet *xn;
int sNo;
{
    tsParameters *gp = *p;
    int i;

    free( gp->uPiArr );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->uKMat[i] );
    }
    free( gp->uKMat );
    free( gp->sumUKArr );
    free( gp->aIArr );
    free( gp->bIArr );

    free( gp->avgPi );
    free( gp->avgLnPi );
    free( gp->avgI );
    free( gp->avgLnI );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->avgK[i] );
        free( gp->avgLnK[i] );
    }
    free( gp->avgK );
    free( gp->avgLnK );
    free( gp->avgLnKI );

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

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

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

        return s;

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

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

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

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

//void freeModelStatsG_ts( gs, xns, gv, ivs )
//void **gs;
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//
//    free( *gs );
//    *gs = NULL;
//}

void initializeVbHmm_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsData *d = xn->data;
    int sNo = gv->sNo;
    tsParameters *p = gv->params;

    int i, j;

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

    // hyperparameter for p( k(i,j) ) (i != j)
    for( i = 0 ; i < sNo ; i++ ){
        p->sumUKArr[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            p->uKMat[i][j] = 1.0;
            if( j != i ){
                p->sumUKArr[i] += p->uKMat[i][j];
            }
        }
    }

    double meanI = (double)xn->N / d->T;

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

    initialize_indVars_ts( xn, gv, iv );

    calcStatsVars_ts( xn, gv, iv );
    maximization_ts( xn, gv, iv );
}

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

void initialize_indVars_ts( 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_ts( 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 = (tsData*)malloc( sizeof(tsData) );
    tsData *d = (tsData*)xn->data;
    d->T = 0.0;
    d->dt = NULL;
    d->time = NULL;
    return xn;
}

void freeXnDataSet_ts( xn )
xnDataSet **xn;
{
    tsData *d = (tsData*)(*xn)->data;
    free( d->dt );
    free( d->time );
    free( (*xn)->data );
    free( (*xn)->name );
    free( *xn );
    *xn = NULL;
}

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

double pTilde_zn_zn1_ts( i, j, params )
int i, j;
void *params;
{
    tsParameters *p = (tsParameters*)params;
    if( i == j ){
        return exp( p->avgLnI[i] - p->avgLnKI[i] );
    } else {
        return exp( p->avgLnK[i][i] + p->avgLnK[i][j] - p->avgLnKI[i] );
    }
}

double pTilde_xn_zn_ts( xnWv, n, i, params )
xnDataSet *xnWv;
size_t n;
int i;
void *params;
{
    tsParameters *p = (tsParameters*)params;
    tsData *d = (tsData*)xnWv->data;
    return exp( p->avgLnI[i] - p->avgI[i] * d->dt[n] );
}

void calcStatsVars_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsData *d = (tsData*)xn->data;
    tsStats *s = (tsStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *Ni = s->Ni, *Ti = s->Ti;
    double *Nii = s->Nii, *Nij = s->Nij, **Mij = s->Mij;
    size_t n;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        Ni[i] = 1e-10;
        Ti[i] = 1e-10;
        Nii[i] = 0.0;
        Nij[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            Mij[i][j] = 1e-10;
        }
    }
    for( n = 0 ; n < dLen ; n++ ){
        for( i = 0 ; i < sNo ; i++ ){
            Ni[i]  += gmMat[n][i];
            Ti[i]  += gmMat[n][i] * d->dt[n];
            Nii[i] += xiMat[n][i][i];
            for( j = 0 ; j < sNo ; j++ ){
                if( j != i ){
                    Mij[i][j] += xiMat[n][i][j];
                    Nij[i]    += xiMat[n][i][j];
                }
            }
        }
    }
    for( i = 0 ; i < sNo ; i++ ){
        Nii[i] = MAX( Nii[i], 1.0 );
        Nij[i] = MAX( Nij[i], 1.0 );
    }
}

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

void maximization_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsParameters *p = (tsParameters*)gv->params;
    tsStats *s = (tsStats*)iv->stats;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgK = p->avgK, **avgLnK = p->avgLnK;
    double *avgLnKI = p->avgLnKI, *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uKMat = p->uKMat, *sumUKArr = p->sumUKArr;
    double *Ni = s->Ni, *Ti = s->Ti;
    double *Nii = s->Nii, *Nij = s->Nij, **Mij = s->Mij;
    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 );

        avgK[i][i] = (Ni[i] + aIArr[i]) * Nij[i] / (Nii[i] - 1.0) / (Ti[i] + bIArr[i]);
        avgLnK[i][i]  = gsl_sf_psi(Ni[i] + aIArr[i]) + gsl_sf_psi(Nij[i]);
        avgLnK[i][i] += -gsl_sf_psi(Nii[i]) - log(Ti[i] + bIArr[i]);

        avgI[i] = (Ni[i] + aIArr[i]) / (Ti[i] + bIArr[i]);
        avgLnI[i] = gsl_sf_psi(Ni[i] + aIArr[i]) - log(Ti[i] + bIArr[i]);

        avgLnKI[i]  = gsl_sf_psi(Ni[i] + aIArr[i]) + gsl_sf_psi(Nii[i] + Nij[i]);
        avgLnKI[i] += -gsl_sf_psi(Nii[i]) - log(Ti[i] + bIArr[i]);

#ifdef INTENSITY_CAP
        double meanI = (double)xnWv->N / xnWv->T;
        avgI[i]  = MIN( avgI[i], maxIntensityRatio * meanI );
        avgLnI[i]  = MIN( avgLnI[i], log(maxIntensityRatio * meanI) );
#endif
#ifdef TRANSITION_RATE_CAP
        avgK[i][i] = MIN( avgK[i][i], avgI[i]/minPhotonNumPerState );
        avgLnK[i][i]  = MIN( avgLnK[i][i], avgLnI[i] - log(minPhotonNumPerState) );
        avgLnKI[i]  = MIN( avgLnKI[i], avgLnI[i] + log(1.0 + 1.0/minPhotonNumPerState) );
#endif

        for( j = 0 ; j < sNo ; j++ ){
            if( i != j ){
                avgK[i][j] = ( uKMat[i][j] + Mij[i][j] ) / ( sumUKArr[i] + Nij[i] );
                avgLnK[i][j] = gsl_sf_psi( uKMat[i][j] + Mij[i][j] ) - gsl_sf_psi( sumUKArr[i] + Nij[i] );
            }
        }
    }
}

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

double varLowerBound_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsParameters *p = (tsParameters*)gv->params;
    tsStats *s = (tsStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, *cn = iv->cn;
    double *avgLnPi = p->avgLnPi, **avgLnK = p->avgLnK;
    double *avgLnKI = p->avgLnKI, *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uKMat = p->uKMat, *sumUKArr = p->sumUKArr;
    double *Ni = s->Ni, *Ti = s->Ti;
    double *Nii = s->Nii, *Nij = s->Nij, **Mij = s->Mij;
    size_t n;
    int i, j;

    double lnpPi;
    lnpPi = gsl_sf_lngamma(sumUPi);
    double Ck = 1.0, lnpKii = (double)sNo * log(Ck);
    double lnpKij = 0.0;
    double lnpI = 0.0;
    double lnqPi;
    lnqPi = gsl_sf_lngamma(sumUPi + 1.0);
    double lnqKiiI = 0.0;
    double lnqKij = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnpKii -= avgLnK[i][i];

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

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

        lnqKiiI += gsl_sf_lngamma(Nii[i] + Nij[i]) + (Ni[i] + aIArr[i]) * log(Ti[i] + bIArr[i]);
        lnqKiiI += -gsl_sf_lngamma(Ni[i] + aIArr[i]) -gsl_sf_lngamma(Nii[i]);
        lnqKiiI += -gsl_sf_lngamma(Nij[i]) + (Nij[i]-1.0)*avgLnK[i][i];
        lnqKiiI += (Ni[i] + Nii[i] + aIArr[i] - 1.0)*avgLnI[i];
        lnqKiiI += -(Nii[i] + Nij[i])*avgLnKI[i] - (Ti[i] + bIArr[i])*avgI[i];

        lnpKij += gsl_sf_lngamma(sumUKArr[i]);
        lnqKij += gsl_sf_lngamma(sumUKArr[i] + Nij[i]);
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                lnpKij += (uKMat[i][j]-1)*avgLnK[i][j] - gsl_sf_lngamma(uKMat[i][j]);
                lnqKij += (uKMat[i][j]+Mij[i][j]-1)*(gsl_sf_psi(uKMat[i][j]+Mij[i][j])-gsl_sf_psi(sumUKArr[i]+Nij[i]));
                lnqKij -= gsl_sf_lngamma( uKMat[i][j] + Mij[i][j] );
            }
        }
    }

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

    double val;
    val  = lnpPi + lnpKii + lnpKij + lnpI;
    val -= lnqPi + lnqKiiI + lnqKij;
    val += lnpX;
    val += log(gsl_sf_fact(sNo));

    return val;
}

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

void reorderParameters_ts( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsParameters *p = (tsParameters*)gv->params;
    tsStats *s = (tsStats*)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, **avgK = p->avgK;
    double **avgLnK = p->avgLnK, *avgLnKI = p->avgLnKI;
    double *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *Ni = s->Ni, *Ti = s->Ti;
    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( sNo * sizeof(double) );   }

    // index indicates order of avgI values (0=biggest avgI -- sNo=smallest avgI).
    for( i = 0 ; i < sNo ; i++ ){
        index[i] = sNo - 1;
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                if( avgI[i] < avgI[j] ){
                    index[i]--;
                } else if( avgI[i] == avgI[j] ){
                    if( i < j )
                    {   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]] = avgK[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgK[i][j] = s2D[i][j];   }
    }

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

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

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

void outputTsResults( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    tsParameters *p = (tsParameters*)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, "   pi: ( %g", p->avgPi[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgPi[i]);
    }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   k_matrix: [");
    for( i = 0 ; i < sNo ; i++ ){
        fprintf(logFP, " ( %g", p->avgK[i][0]);
        for( j = 1 ; j < sNo ; j++ )
        {   fprintf(logFP, ", %g", p->avgK[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, pi");
        for( i = 0 ; i < sNo ; i++ )
        {   fprintf(fp, ", K%dx", i);   }
        fprintf(fp, "\n");

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

//


#include <stdio.h>
#include <string.h>
#include "erl_nif.h"
#include <cblas.h>

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

static ERL_NIF_TERM atom_ok;
static ERL_NIF_TERM atom_true;

static ERL_NIF_TERM atom_rowmaj;
static ERL_NIF_TERM atom_colmaj;

static ERL_NIF_TERM atom_notransp;
static ERL_NIF_TERM atom_transpose;
static ERL_NIF_TERM atom_conjugatet;

static ERL_NIF_TERM atom_upper;
static ERL_NIF_TERM atom_lower;

static ERL_NIF_TERM atom_left;
static ERL_NIF_TERM atom_right;

static ERL_NIF_TERM atom_nonunit;
static ERL_NIF_TERM atom_unit;

static ERL_NIF_TERM make_cont(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM from_list(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM to_values(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM to_idx_list(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM cont_size(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM cont_update(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);

static ERL_NIF_TERM drotg(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM drot(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM dcopy(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM l1d_1cont(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM l1d_2cont(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM dgemv(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM dtrmv(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);
static ERL_NIF_TERM dgemm(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[]);

static ErlNifFunc nif_funcs[] = {
    {"make_cont", 2, make_cont},
    {"from_list", 1, from_list},
    {"to_tuple_list_impl", 4, to_values},
    {"cont_size", 1, cont_size},
    {"values", 2, to_idx_list},
    {"update", 2, cont_update},
    {"update", 3, cont_update},

    {"rotg", 2, drotg},
    {"rot",  9, drot},
    {"copy", 4, dcopy},
    {"copy", 7, dcopy},
    {"one_vec", 6, l1d_1cont},
    {"two_vec", 9, l1d_2cont},

    {"gemv_impl", 15, dgemv},
    {"trmv_impl", 11, dtrmv},
    {"trmv_impl", 12, dtrmv},

    {"gemm_impl", 15, dgemm},
    {"trmm_impl", 15, dgemm},
};

static ErlNifResourceType *avec_r;

typedef struct {
    double v[1];
} Avec;

static ERL_NIF_TERM mk_avec(ErlNifEnv *env, unsigned int n, Avec **avec) {
    ERL_NIF_TERM term;
    *avec = enif_alloc_resource(avec_r, n*sizeof(double));
    term = enif_make_resource(env, *avec);
    enif_release_resource(*avec);
    return term;
}

#define AVEC_SIZE(AV) 	(enif_sizeof_resource(AV) / 8)

/* ---------------------------------------------------*/

static ERL_NIF_TERM make_cont(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    unsigned int n;
    ERL_NIF_TERM res;
    Avec *avec = NULL;
    double *data;

    if(!enif_get_uint(env, argv[0], &n)) return enif_make_badarg(env);
    res = mk_avec(env, n, &avec);

    if(enif_is_binary(env, argv[1])) {
	ErlNifBinary bin;
	enif_inspect_binary(env, argv[1], &bin);
	if(n*sizeof(double) != bin.size)
	    return enif_make_badarg(env);
	memcpy(avec->v, bin.data, sizeof(double)*n);
    } else if(enif_is_identical(argv[1], atom_true)) {
	data = avec->v;
	memset(data, 0, sizeof(double)*n);
    }

    return res;
}

static ERL_NIF_TERM from_list(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    unsigned int len;
    int dim, i,j, tmp;
    ERL_NIF_TERM hd, tail, res;
    const ERL_NIF_TERM *current;
    Avec *avec = NULL;
    double *data;

    if(!enif_get_list_length(env, argv[0], &len) || len == 0)
	return enif_make_badarg(env);

    enif_get_list_cell(env, argv[0], &hd, &tail);
    if(!enif_get_tuple(env, hd, &dim, &current))
	dim = 1;
    res = mk_avec(env, len*dim, &avec);

    enif_consume_timeslice(env, (len*dim)/1000);

    data = avec->v;
    if(dim == 1) {
	for(i=0; i<len; i++) {
	    if(!enif_get_double(env, hd, data))
		return enif_make_badarg(env);
	    data++;
	    enif_get_list_cell(env, tail, &hd, &tail);
	}
    } else {
	for(i=0; i<len; i++) {
	    if(!enif_get_tuple(env, hd, &tmp, &current) || tmp != dim)
		return enif_make_badarg(env);
	    for(j=0; j<dim; j++) {
		if(!enif_get_double(env, current[j], data))
		    return enif_make_badarg(env);
		data++;
	    }
	    enif_get_list_cell(env, tail, &hd, &tail);
	}
    }
    return res;
}

static ERL_NIF_TERM to_values(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    ERL_NIF_TERM tail, *tmp;
    Avec *avec = NULL;
    double *arr;
    unsigned int idx, n, dim, i, j, max;

    if(!enif_get_uint(env, argv[0], &idx)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[1], &n))   return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[2], &dim))  return enif_make_badarg(env);

    if(!enif_get_resource(env, argv[3], avec_r, (void **) &avec))
	return enif_make_badarg(env);
    max = AVEC_SIZE(avec);

    enif_consume_timeslice(env, max/1000);

    if(idx+n > max) return enif_make_badarg(env);

    if(n == 1 && dim == 0)  /* get_value() */
	return enif_make_double(env, avec->v[idx]);

    if(dim == 0) { /* to_list */
	tail = enif_make_list(env, 0);
	arr = avec->v + idx + n-1;
	for(i=0; i < max; i++) {
	    tail = enif_make_list_cell(env, enif_make_double(env, *arr), tail);
	    arr -= 1;
	}
	return tail;
    }
    if(dim == n) { /* to_tuple */
	tmp = (ERL_NIF_TERM*) malloc(sizeof(ERL_NIF_TERM)*n);
	arr = avec->v;
	for(i=0; i < n; i++) {
	    tmp[i] = enif_make_double(env, arr[idx+i]);
	}
	tail = enif_make_tuple_from_array(env, tmp, n);
	free(tmp);
	return tail;
    }

    /* list of tuples */
    if(n % dim != 0)
	return enif_make_badarg(env);

    arr = avec->v + idx + n-dim;
    tmp = (ERL_NIF_TERM*) malloc(sizeof(ERL_NIF_TERM)*dim);
    tail = enif_make_list(env, 0);

    for(i=0; i < max / dim; i++) {
	for(j=0; j < dim; j++, arr++) {
	    tmp[j] = enif_make_double(env, *arr);
	}
	tail = enif_make_list_cell(env, enif_make_tuple_from_array(env, tmp, dim), tail);
	arr -= 2*dim;
    }
    free(tmp);
    return tail;
}

static ERL_NIF_TERM to_idx_list(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    ERL_NIF_TERM hd, tail, *tmp;
    Avec *avec = NULL;
    double *arr;
    unsigned int idx, n, i;

    if(!enif_get_list_length(env, argv[0], &n) || n == 0)
	return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[1], avec_r, (void **) &avec))
	return enif_make_badarg(env);

    enif_consume_timeslice(env, n/1000);
    tmp = (ERL_NIF_TERM*) malloc(sizeof(ERL_NIF_TERM)*n);
    arr = avec->v;
    tail = argv[0];
    for(i=0; i < n; i++) {
	enif_get_list_cell(env, tail, &hd, &tail);
	if(!enif_get_uint(env, hd, &idx) || idx >= AVEC_SIZE(avec))
	    return enif_make_badarg(env);
	tmp[i] = enif_make_tuple2(env, hd, enif_make_double(env, arr[idx]));
    }
    return enif_make_list_from_array(env, tmp, n);
}

static ERL_NIF_TERM cont_size(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    Avec *avec = NULL;

    if(!enif_get_resource(env, argv[0], avec_r, (void **) &avec)) {
	return enif_make_badarg(env);
    }

    return enif_make_uint(env, AVEC_SIZE(avec));
}

static ERL_NIF_TERM cont_update(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    ERL_NIF_TERM hd, tail;
    const ERL_NIF_TERM *curr;
    Avec *avec = NULL;
    double *arr, temp;
    int i=0, n=0;
    unsigned int idx, max;

    if(argc == 2) { /* tuple list of values */
	if(!enif_is_list(env, argv[0]))
	    return enif_make_badarg(env);
	if(!enif_get_resource(env, argv[1], avec_r, (void **) &avec))
	    return enif_make_badarg(env);
	arr = avec->v;
	max = AVEC_SIZE(avec);
	tail = argv[0];
	while(!enif_is_empty_list(env, tail)) {
	    if(!enif_get_list_cell(env, tail, &hd, &tail))
		return enif_make_badarg(env);
	    if(!enif_get_tuple(env, hd, &i, &curr) || i != 2)
		return enif_make_badarg(env);
	    if(!enif_get_uint(env, curr[0], &idx) || idx >= max)
		return enif_make_badarg(env);
	    if(!enif_get_double(env, curr[1], &temp))
		return enif_make_badarg(env);
	    arr[idx] = temp;
	    n++;
	}
	enif_consume_timeslice(env, n/1000);
	return atom_ok;
    }
    /* update(Idx, V|[Vs], Vec) */
    if(!enif_get_uint(env, argv[0], &idx))
	return enif_make_badarg(env);
    if(!enif_is_list(env, argv[1])) {
	/* update(Idx, Double, Vec) */
	if(!enif_get_double(env, argv[1], &temp))
	    return enif_make_badarg(env);
	if(!enif_get_resource(env, argv[2], avec_r, (void **) &avec))
	    return enif_make_badarg(env);
	max = AVEC_SIZE(avec);
	if(idx >= max)
	    return enif_make_badarg(env);
	arr = avec->v;
	arr[idx] = temp;
	return atom_ok;
    }
    /* update(Idx, [Values], Vec) */
    tail = argv[1];
    if(!enif_get_resource(env, argv[2], avec_r, (void **) &avec))
	return enif_make_badarg(env);
    max = AVEC_SIZE(avec);
    arr = avec->v+idx;
    while(!enif_is_empty_list(env, tail)) {
	if(!enif_get_list_cell(env, tail, &hd, &tail))
	    return enif_make_badarg(env);
	if(!enif_get_double(env, hd, &temp))
	    return enif_make_badarg(env);
	*arr = temp;
	arr++;
	idx++;
	n++;
	if(idx > max)
	    return enif_make_badarg(env);
    }
    enif_consume_timeslice(env, n/1000);
    return atom_ok;
}

/* ---------------------------------------------------*/

static ERL_NIF_TERM drotg(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{/* (a, b) */
    double a,b,c,s;

    if(!enif_get_double(env, argv[0], &a) || !enif_get_double(env, argv[1], &b)) {
	return enif_make_badarg(env);
    }

    cblas_drotg(&a,&b,&c,&s);
    return enif_make_tuple4(env,
			    enif_make_double(env, a), enif_make_double(env, b),
			    enif_make_double(env, c), enif_make_double(env, s));

}

static ERL_NIF_TERM drot(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    unsigned int n, xs, ys;
    int xi, yi;
    Avec *ax, *ay;
    double *x, *y, c, s;

    if(!enif_get_uint(env, argv[0], &n)) return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[1], avec_r, (void **) &ax)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[2], &xs)) return enif_make_badarg(env);
    if(!enif_get_int(env, argv[3], &xi)) return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[4], avec_r, (void **) &ay)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[5], &ys)) return enif_make_badarg(env);
    if(!enif_get_int(env, argv[6], &yi)) return enif_make_badarg(env);
    if(!enif_get_double(env, argv[7], &c)) return enif_make_badarg(env);
    if(!enif_get_double(env, argv[8], &s)) return enif_make_badarg(env);

    x = ax->v + xs;
    y = ay->v + ys;
    /* array limit checks */
    if((xs+n*abs(xi)-1) > AVEC_SIZE(ax)) return enif_make_badarg(env);
    if((ys+n*abs(yi)-1) > AVEC_SIZE(ax)) return enif_make_badarg(env);

    enif_consume_timeslice(env, n/1000);
    cblas_drot(n, x, xi, y, xi, c, s);

    return atom_ok;
}

static ERL_NIF_TERM l1d_1cont(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    unsigned int op, n, xs;
    int xi, tmp;
    Avec *ax;
    double *x, alpha;
    ERL_NIF_TERM res = atom_ok;

    if(!enif_get_uint(env, argv[0], &n)) return enif_make_badarg(env);
    if(!enif_get_double(env, argv[1], &alpha)) return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[2], avec_r, (void **) &ax)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[3], &xs)) return enif_make_badarg(env);
    if(!enif_get_int(env, argv[4], &xi)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[5], &op)) return enif_make_badarg(env);
    x = ax->v + xs;
    /* array limit checks */
    if((xs+n*abs(xi)-1) > AVEC_SIZE(ax)) return enif_make_badarg(env);

    switch(op) {
    case 0:
	cblas_dscal(n, alpha, x, xi);
	break;
    case 1:
	res = enif_make_double(env, cblas_dnrm2(n, x, xi));
	break;
    case 2:
	res = enif_make_double(env, cblas_dasum(n, x, xi));
	break;
    case 3:
	tmp = cblas_idamax(n, x, xi);
	res = enif_make_tuple2(env,
			       enif_make_uint(env, tmp),
			       enif_make_double(env, x[tmp]));
	break;
    }
    enif_consume_timeslice(env, n/1000);
    return res;
}

static ERL_NIF_TERM dcopy(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    unsigned int n, xs, ys = 0;
    int xi, yi=1;
    Avec *ax, *ay;
    double *x, *y;
    ERL_NIF_TERM res;

    if(!enif_get_uint(env, argv[0], &n)) return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[1], avec_r, (void **) &ax)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[2], &xs)) return enif_make_badarg(env);
    if(!enif_get_int(env, argv[3], &xi)) return enif_make_badarg(env);
    /* array limit checks */
    if((xs+n*abs(xi)-1) > AVEC_SIZE(ax)) return enif_make_badarg(env);

    if(argc == 4) {  /* copy to new vector */
	res = mk_avec(env, n, &ay);
    } else {
	if(!enif_get_resource(env, argv[4], avec_r, (void **) &ay)) return enif_make_badarg(env);
	if(!enif_get_uint(env, argv[5], &ys)) return enif_make_badarg(env);
	if(!enif_get_int(env, argv[6], &yi)) return enif_make_badarg(env);
	if((ys+n*abs(yi)-1) > AVEC_SIZE(ay)) return enif_make_badarg(env);
	res = atom_ok;
    }

    x = ax->v + xs;
    y = ay->v + ys;
    cblas_dcopy(n, x, xi, y, yi);
    enif_consume_timeslice(env, n/1000);

    return res;
}

static ERL_NIF_TERM l1d_2cont(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    unsigned int op, n, xs, ys;
    int xi, yi;
    Avec *ax, *ay;
    double *x, *y, alpha, res;

    if(!enif_get_uint(env, argv[0], &n)) return enif_make_badarg(env);
    if(!enif_get_double(env, argv[1], &alpha)) return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[2], avec_r, (void **) &ax)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[3], &xs)) return enif_make_badarg(env);
    if(!enif_get_int(env, argv[4], &xi)) return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[5], avec_r, (void **) &ay)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[6], &ys)) return enif_make_badarg(env);
    if(!enif_get_int(env, argv[7], &yi)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[8], &op)) return enif_make_badarg(env);
    x = ax->v + xs;
    y = ay->v + ys;
    /* array limit checks */
    if((xs+n*abs(xi)-1) > AVEC_SIZE(ax)) return enif_make_badarg(env);
    if((ys+n*abs(yi)-1) > AVEC_SIZE(ay)) return enif_make_badarg(env);

    enif_consume_timeslice(env, n/1000);
    switch(op) {
    case 0:
	cblas_daxpy(n, alpha, x, xi, y, yi);
	break;
    case 1:
	cblas_dswap(n, x, xi, y, yi);
	break;
    case 2:
	res = cblas_ddot(n, x, xi, y, yi);
	return enif_make_double(env, res);
    }
    return atom_ok;
}

/* ---------------------------------------------------*/

static ERL_NIF_TERM dgemv(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    unsigned int m, n, lda, xs, ys, op;
    int xi, yi;
    Avec *aa, *ax, *ay;
    double *a, *x, *y, alpha, beta;
    enum CBLAS_ORDER order = 0;
    enum CBLAS_TRANSPOSE trans = 0;
    enum CBLAS_UPLO uplo = 0;

    if(enif_is_identical(argv[0], atom_rowmaj)) order = CblasRowMajor;
    else if(enif_is_identical(argv[0], atom_colmaj)) order = CblasColMajor;
    else return enif_make_badarg(env);

    if(enif_is_identical(argv[1], atom_notransp)) trans = CblasNoTrans;
    else if(enif_is_identical(argv[1], atom_transpose)) trans = CblasTrans;
    else if(enif_is_identical(argv[1], atom_conjugatet)) trans = CblasConjTrans;
    else if(enif_is_identical(argv[1], atom_lower)) uplo = CblasLower;
    else if(enif_is_identical(argv[1], atom_upper)) uplo = CblasUpper;
    else return enif_make_badarg(env);

    if(!enif_get_uint(env, argv[2], &m)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[3], &n)) return enif_make_badarg(env);
    if(!enif_get_double(env, argv[4], &alpha)) return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[5], avec_r, (void **) &aa)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[6], &lda)) return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[7], avec_r, (void **) &ax)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[8], &xs)) return enif_make_badarg(env);
    if(!enif_get_int(env, argv[9], &xi)) return enif_make_badarg(env);
    if(!enif_get_double(env, argv[10], &beta)) return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[11], avec_r, (void **) &ay)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[12], &ys)) return enif_make_badarg(env);
    if(!enif_get_int(env, argv[13], &yi)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[14], &op)) return enif_make_badarg(env);

    a = aa->v;
    x = ax->v + xs;
    y = ay->v + ys;

    /* array limit checks */
    if((xs+n*abs(xi)-1) > AVEC_SIZE(ax)) return enif_make_badarg(env);
    if((ys+n*abs(yi)-1) > AVEC_SIZE(ay)) return enif_make_badarg(env);

    switch(op) {
    case 0:
	cblas_dgemv(order, trans, m, n, alpha, a, lda, x, xi, beta, y, yi); break;
    case 1:
	cblas_dsymv(order, uplo, n, alpha, a, lda, x, xi, beta, y, yi); break;
    case 2:
	cblas_dspmv(order, uplo, n, alpha, a, x, xi, beta, y, yi); break;
    case 3:
	cblas_dger(order, m, n, alpha, x, xi, y, yi, a, lda); break;
    case 4:
	cblas_dsyr2(order, uplo, n, alpha, x, xi, y, yi, a, lda); break;
    case 5:
	cblas_dspr2(order, uplo, n, alpha, x, xi, y, yi, a); break;
    }
    enif_consume_timeslice(env, MAX(1,m)*n/1000);
    return atom_ok;
}

static ERL_NIF_TERM dtrmv(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    unsigned int n, lda, xs, op;
    int xi;
    Avec *aa, *ax;
    double *a, *x, alpha;
    enum CBLAS_ORDER order = 0;
    enum CBLAS_TRANSPOSE trans = 0;
    enum CBLAS_UPLO uplo = 0;
    enum CBLAS_DIAG diag = 0;

    if(enif_is_identical(argv[0], atom_rowmaj)) order = CblasRowMajor;
    else if(enif_is_identical(argv[0], atom_colmaj)) order = CblasColMajor;
    else return enif_make_badarg(env);

    if(enif_is_identical(argv[1], atom_lower)) uplo = CblasLower;
    else if(enif_is_identical(argv[1], atom_upper)) uplo = CblasUpper;
    else return enif_make_badarg(env);

    if(!enif_get_uint(env, argv[10], &op)) return enif_make_badarg(env);
    if(op < 4) {
	if(enif_is_identical(argv[2], atom_notransp)) trans = CblasNoTrans;
	else if(enif_is_identical(argv[2], atom_transpose)) trans = CblasTrans;
	else if(enif_is_identical(argv[2], atom_conjugatet)) trans = CblasConjTrans;
	else return enif_make_badarg(env);

	if(enif_is_identical(argv[3], atom_nonunit)) diag = CblasNonUnit;
	else if(enif_is_identical(argv[3], atom_unit)) diag = CblasUnit;
	else return enif_make_badarg(env);
    }

    if(!enif_get_uint(env, argv[4], &n)) return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[5], avec_r, (void **) &aa)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[6], &lda)) return enif_make_badarg(env);
    if(!enif_get_resource(env, argv[7], avec_r, (void **) &ax)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[8], &xs)) return enif_make_badarg(env);
    if(!enif_get_int(env, argv[9], &xi)) return enif_make_badarg(env);
    if(argc > 11)
	if(!enif_get_double(env, argv[11], &alpha)) return enif_make_badarg(env);

    a = aa->v;
    x = ax->v + xs;

    /* array limit checks */
    if((xs+n*abs(xi)-1) > AVEC_SIZE(ax)) return enif_make_badarg(env);

    switch(op) {
    case 0:
	cblas_dtrmv(order, uplo, trans, diag, n, a, lda, x, xi);break;
    case 1:
	cblas_dtpmv(order, uplo, trans, diag, n, a, x, xi); break;
    case 2:
	cblas_dtrsv(order, uplo, trans, diag, n, a, lda, x, xi);break;
    case 3:
	cblas_dtpsv(order, uplo, trans, diag, n, a, x, xi);break;
    case 4:
	cblas_dsyr(order, uplo, n, alpha, x, xi, a, lda); break;
    case 5:
	cblas_dspr(order, uplo, n, alpha, x, xi, a); break;
    }

    enif_consume_timeslice(env, n*n/1000);
    return atom_ok;
}

/* ---------------------------------------------------*/

static ERL_NIF_TERM dgemm(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    unsigned int m, n, k, lda, ldb, ldc, op;
    Avec *aa, *ab, *ac;
    double alpha, beta, *a, *b, *c = NULL;
    enum CBLAS_ORDER order = 0;
    enum CBLAS_TRANSPOSE ta = 0, tb = 0;
    enum CBLAS_UPLO uplo = 0;
    enum CBLAS_SIDE side = 0;
    enum CBLAS_DIAG diag = 0;

    if(enif_is_identical(argv[0], atom_rowmaj)) order = CblasRowMajor;
    else if(enif_is_identical(argv[0], atom_colmaj)) order = CblasColMajor;
    else return enif_make_badarg(env);

    if(!enif_get_uint(env, argv[3], &m)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[4], &n)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[5], &k)) return enif_make_badarg(env);

    if(!enif_get_double(env, argv[6], &alpha)) return enif_make_badarg(env);

    if(!enif_get_resource(env, argv[7], avec_r, (void **) &aa)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[8], &lda)) return enif_make_badarg(env);

    if(!enif_get_resource(env, argv[9], avec_r, (void **) &ab)) return enif_make_badarg(env);
    if(!enif_get_uint(env, argv[10], &ldb)) return enif_make_badarg(env);

    if(!enif_get_double(env, argv[11], &beta)) return enif_make_badarg(env);

    if(!enif_get_uint(env, argv[12], &op)) return enif_make_badarg(env);
    if(op < 3) {
	if(!enif_get_resource(env, argv[13], avec_r, (void **) &ac))
	    return enif_make_badarg(env);
	if(!enif_get_uint(env, argv[14], &ldc)) return enif_make_badarg(env);
	c = ac->v;
    }
    a = aa->v;
    b = ab->v;
    switch(op) {
    case 0: {
	if(enif_is_identical(argv[1], atom_notransp)) ta = CblasNoTrans;
	else if(enif_is_identical(argv[1], atom_transpose)) ta = CblasTrans;
	else if(enif_is_identical(argv[1], atom_conjugatet)) ta = CblasConjTrans;
	else return enif_make_badarg(env);

	if(enif_is_identical(argv[2], atom_notransp)) tb = CblasNoTrans;
	else if(enif_is_identical(argv[2], atom_transpose)) tb = CblasTrans;
	else if(enif_is_identical(argv[2], atom_conjugatet)) tb = CblasConjTrans;
	else return enif_make_badarg(env);

	cblas_dgemm(order, ta,tb, m,n,k, alpha, a,lda, b,ldb, beta, c,ldc);
	break;}
    case 1: {
	if(enif_is_identical(argv[1], atom_left)) side = CblasLeft;
	else if(enif_is_identical(argv[1], atom_right)) side = CblasRight;
	else return enif_make_badarg(env);

	if(enif_is_identical(argv[2], atom_lower)) uplo = CblasLower;
	else if(enif_is_identical(argv[2], atom_upper)) uplo = CblasUpper;
	else return enif_make_badarg(env);

	cblas_dsymm(order, side, uplo, m,n, alpha, a,lda, b,ldb, beta, c,ldc);
	break; }
    case 2: {
	if(enif_is_identical(argv[1], atom_lower)) uplo = CblasLower;
	else if(enif_is_identical(argv[1], atom_upper)) uplo = CblasUpper;
	else return enif_make_badarg(env);

	if(enif_is_identical(argv[2], atom_notransp)) tb = CblasNoTrans;
	else if(enif_is_identical(argv[2], atom_transpose)) tb = CblasTrans;
	else if(enif_is_identical(argv[2], atom_conjugatet)) tb = CblasConjTrans;

	// fprintf(stderr, "%d %d,%d,%d %.2f %.2f %d,%d,%d\r\n", __LINE__, m,n,k, alpha, beta, lda,ldb,ldc);
	cblas_dsyr2k(order, uplo, tb, n, k, alpha, a,lda, b,ldb, beta, c,ldc);
	break; }
    case 3: {
	if(enif_is_identical(argv[1], atom_lower)) uplo = CblasLower;
	else if(enif_is_identical(argv[1], atom_upper)) uplo = CblasUpper;
	else return enif_make_badarg(env);

	if(enif_is_identical(argv[2], atom_notransp)) tb = CblasNoTrans;
	else if(enif_is_identical(argv[2], atom_transpose)) tb = CblasTrans;
	else if(enif_is_identical(argv[2], atom_conjugatet)) tb = CblasConjTrans;

	if(enif_is_identical(argv[13], atom_left)) side = CblasLeft;
	else if(enif_is_identical(argv[13], atom_right)) side = CblasRight;
	else return enif_make_badarg(env);

	if(enif_is_identical(argv[14], atom_nonunit)) diag = CblasNonUnit;
	else if(enif_is_identical(argv[14], atom_unit)) diag = CblasUnit;
	else return enif_make_badarg(env);

	cblas_dtrmm(order,side,uplo,tb,diag, m,n, alpha, a,lda, b, ldb);
	break; }
    case 4: {
	if(enif_is_identical(argv[1], atom_lower)) uplo = CblasLower;
	else if(enif_is_identical(argv[1], atom_upper)) uplo = CblasUpper;
	else return enif_make_badarg(env);

	if(enif_is_identical(argv[2], atom_notransp)) tb = CblasNoTrans;
	else if(enif_is_identical(argv[2], atom_transpose)) tb = CblasTrans;
	else if(enif_is_identical(argv[2], atom_conjugatet)) tb = CblasConjTrans;

	if(enif_is_identical(argv[13], atom_left)) side = CblasLeft;
	else if(enif_is_identical(argv[13], atom_right)) side = CblasRight;
	else return enif_make_badarg(env);

	if(enif_is_identical(argv[14], atom_nonunit)) diag = CblasNonUnit;
	else if(enif_is_identical(argv[14], atom_unit)) diag = CblasUnit;
	else return enif_make_badarg(env);

	cblas_dtrsm(order,side,uplo,tb,diag, m,n, alpha, a,lda, b, ldb);
	break; }
    case 5: {
	if(enif_is_identical(argv[1], atom_lower)) uplo = CblasLower;
	else if(enif_is_identical(argv[1], atom_upper)) uplo = CblasUpper;
	else return enif_make_badarg(env);

	if(enif_is_identical(argv[2], atom_notransp)) tb = CblasNoTrans;
	else if(enif_is_identical(argv[2], atom_transpose)) tb = CblasTrans;
	else if(enif_is_identical(argv[2], atom_conjugatet)) tb = CblasConjTrans;

	cblas_dsyrk(order,uplo,tb, n,k, alpha, a,lda, beta, b,ldb);
	break; }
    }

    enif_consume_timeslice(env, MAX(m,k)*n/1000);
    return atom_ok;
}

/* ---------------------------------------------------*/

static int load(ErlNifEnv* env, void** priv_data, ERL_NIF_TERM load_info)
{
    atom_ok = enif_make_atom(env,"ok");
    atom_true = enif_make_atom(env,"true");

    atom_notransp = enif_make_atom(env,"no_transp");
    atom_transpose = enif_make_atom(env,"transp");
    atom_conjugatet = enif_make_atom(env,"conj_transp");

    atom_rowmaj = enif_make_atom(env,"row_maj");
    atom_colmaj = enif_make_atom(env,"col_maj");

    atom_upper = enif_make_atom(env,"upper");
    atom_lower = enif_make_atom(env,"lower");

    atom_left = enif_make_atom(env,"left");
    atom_right = enif_make_atom(env,"right");

    atom_nonunit = enif_make_atom(env,"non_unit");
    atom_unit = enif_make_atom(env,"unit");

    avec_r  = enif_open_resource_type(env, "eblas", "avec", NULL, ERL_NIF_RT_CREATE, NULL);
    return 0;
}

static int upgrade(ErlNifEnv* env, void** priv_data, void** old_priv_data,
		   ERL_NIF_TERM load_info)
{
    return 0;
}

static void unload(ErlNifEnv* env, void* priv_data)
{

}

ERL_NIF_INIT(blasd_raw,nif_funcs,load,NULL,upgrade,unload)


// 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>

int Get_Lattice_Targets_Center_Circle( int *pos, double *targets, 	// leaves esc_cell radius free
						 double lattice_spacing,  int escape_cells)	// targets one in cell center
{
	int ret_value = 0; //leave starting cell empty, else particle starts in Target
	if 	(	(pos[0] == 0)&&
			(pos[1] == 0)
		)

	{
		ret_value = 0; //catch mistake if zero cell was still supposed to have target
		return (ret_value);
	}
	if 	(pos[0] * pos[0] + pos[1] * pos[1] > escape_cells * escape_cells ) //If outside circle
	{
		ret_value = 1;    //single target center cell
		targets[0] = pos[0] * lattice_spacing;
		targets[1] = pos[1] * lattice_spacing;
		return (ret_value);
	}
	return (ret_value);
}

int Get_Lattice_Targets_PORE_3( int *pos, double *targets, 	// leaves esc_cell radius free
						 double lattice_spacing,  int escape_cells)	// targets one in cell center
{
	double corner_vec[2];
	int corner = 0;
	//corner one oben links
	for (double i_x = -1.0; i_x < 2.0; i_x+= 2.0)
	{
		for (double i_y = -1.0; i_y < 2.0; i_y+= 2.0)
		{
			int stride = 2 * corner * (NUMBER_PORE_OBSTACLES + 1);
			corner_vec[0] =  (pos[0] + i_x *0.5) * LATTICE_SPACING;
			corner_vec[1] =  (pos[1] + i_y *0.5) * LATTICE_SPACING;
			targets[0 + stride] = corner_vec[0];
			targets[1 + stride] = corner_vec[1];
			for (int i_pore = 0; i_pore < NUMBER_PORE_OBSTACLES; i_pore++)
			{
				targets[2 + stride + i_pore * 4] = corner_vec[0] - i_x * TARGET_LENGTH * (i_pore + 1);
				targets[3 + stride + i_pore * 4] = corner_vec[1];

				targets[4 + stride + i_pore * 4] = corner_vec[0];
				targets[5 + stride + i_pore * 4] = corner_vec[1] - i_y * TARGET_LENGTH * (i_pore + 1);
			}
			corner++;
		}
		corner++;
	}
	return (4 *(1 + 2* NUMBER_PORE_OBSTACLES));
}

int Get_Lattice_Targets_Center_Square( int *pos, double *targets,	// leaves esc_cell^2 square free
						 double lattice_spacing,  int escape_cells)	// targets one in cell center
{
	int ret_value = 0; //leave starting cell empty, else particle starts in Target
	if 	(	(pos[0] == 0)&&
			(pos[1] == 0)
		)

	{
		ret_value = 0; //catch mistake if zero cell was still supposed to have target
		return (ret_value);
	}
	if 	( 	(abs(pos[0]) > escape_cells)||
			(abs(pos[1]) > escape_cells)
		)
	{
		ret_value = 1;    //single target center cell
		targets[0] = pos[0] * lattice_spacing;
		targets[1] = pos[1] * lattice_spacing;
		return (ret_value);
	}
	return (ret_value);
}

int Get_Lattice_Targets_Corner_Square( int *pos, double *targets,
						 double lattice_spacing,  int escape_cells)
// get target vectors for corner, middle or porous region. save in targest[4*DIM], return number targets
{
	int ret_value = 0;
	if(escape_cells == 0)		// catch case no free cells
	{
		ret_value = 4; // 4 targets per lattice cell
		int count = -1;
		for (int i = -1; i< 2; i+=2) //i,j in (-1,1)
		{
			for (int j = -1; j< 2; j+=2)
			{
				count++;
				targets[0 + count * DIM] = i * lattice_spacing/2.0 + pos[0] * lattice_spacing;
				targets[1 + count * DIM] = j * lattice_spacing/2.0 + pos[1] * lattice_spacing;
			}
		}
		return (ret_value);
	}
	// check porous are
	if( (abs(pos[0]) > escape_cells)||
		(abs(pos[1]) > escape_cells) )
	{
		ret_value = 4; // 4 targets per lattice cell
		int count = -1;
		for (int i = -1; i< 2; i+=2) //i,j in (-1,1)
		{
			for (int j = -1; j< 2; j+=2)
			{
				count++;
				targets[0 + count * DIM] = i * lattice_spacing/2.0 + pos[0] * lattice_spacing;
				targets[1 + count * DIM] = j * lattice_spacing/2.0 + pos[1] * lattice_spacing;
			}
		}
		return (ret_value);
	}
	//Check corner
	for (int i = -1; i< 2; i+=2) //i,j in (-1,1)
	{
		for (int j = -1; j< 2; j+=2)
		{
			if( (pos[0] == i * escape_cells)&&
				(pos[1] == j * escape_cells))
			{
				ret_value = 3; // corner and two sides
				targets[0] = i * lattice_spacing/2.0 + pos[0] * lattice_spacing; 		//corner
				targets[1] = j * lattice_spacing/2.0 + pos[1] * lattice_spacing;

				targets[2] = i * (-1) * lattice_spacing/2.0 + pos[0] * lattice_spacing;	//first side
				targets[3] = j * lattice_spacing/2.0 + pos[1] * lattice_spacing;
				targets[4] = i * lattice_spacing/2.0 + pos[0] * lattice_spacing;	//first side
				targets[5] = j * (-1) * lattice_spacing/2.0 + pos[1] * lattice_spacing;
				return (ret_value);
			}	// corner
		}
	}
	// check sides after sure you are not in corner
	for( int d = 0; d < DIM; d++)// loop over dimension
	{
		int e = 1-d;
		for (int i = -1; i< 2; i+=2)
		{
			if(pos[d] == i * escape_cells)
			{
				ret_value = 2; //two sides
				targets[d] 			= i * lattice_spacing/2.0 + pos[d] * lattice_spacing;
				targets[e] 			= (-1) * lattice_spacing/2.0 + pos[e] * lattice_spacing;
				targets[d +DIM] 	= i * lattice_spacing/2.0 + pos[d] * lattice_spacing;
				targets[e +DIM] 	= (1) * lattice_spacing/2.0 + pos[e] * lattice_spacing;
				return (ret_value);
			}
		}
	}
	return (ret_value);
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_sf.h>
#include "adkGSL.h"
#include "AFS.h"
#include "cs.h"
#include "adkCSparse.h"
#include "AFS_ctmc.h"
#include <pthread.h>

void coalMarkovChainTopologyMatrix_pthread(afsStateSpace *S,gsl_matrix *topol, gsl_matrix_int *moveType, int *dim1, int *dim2, int start, int stop, int *nnz);
void coalMarkovChainTopologyMatrix_sparse_pthread(afsStateSpace *S,double *topol, int *moveType, int *dim1, int *dim2, int start, int stop, int *nnz);

typedef struct matrixThreadObject
{
	int nnz,start, stop;
	int *dim1,*dim2;
	struct island_lik_params *island_params;
}
matrixThreadObject;

typedef struct matrixThreadObject_sparse
{
	int nnz,start, stop, nnzA;
	int *dim1,*dim2, *moveType;
	double *topol;
	struct im_lik_params *im_params;
}
matrixThreadObject_sparse;

//gonna try to use a global declared in island.c
void *threadDoRows(void* arg){
	matrixThreadObject *obj = (matrixThreadObject*)(arg);

	coalMarkovChainTopologyMatrix_pthread(obj->island_params->stateSpace,obj->island_params->topol, obj->island_params->moveType,
	obj->dim1,obj->dim2,obj->start,obj->stop,&(obj->nnz));
	//obj->nnz -= 1;

	pthread_exit((void* )obj);
}

//sparse matrix version
void *threadDoRows_sparse(void* arg){
	matrixThreadObject_sparse *obj = (matrixThreadObject_sparse*)(arg);

	coalMarkovChainTopologyMatrix_sparse_pthread(obj->im_params->stateSpace,obj->topol, obj->moveType,
	obj->dim1,obj->dim2,obj->start,obj->stop,&(obj->nnz));
	//obj->nnz -= 1;

	pthread_exit((void* )obj);
}

//sparse matrix version on reduced state space
void *threadDoRows_sparse_reduced(void* arg){
	matrixThreadObject_sparse *obj = (matrixThreadObject_sparse*)(arg);

	coalMarkovChainTopologyMatrix_sparse_pthread(obj->im_params->reducedStateSpace,obj->topol, obj->moveType,
	obj->dim1,obj->dim2,obj->start,obj->stop,&(obj->nnzA));
	//obj->nnz -= 1;

	pthread_exit((void* )obj);
}

void coalMarkovChainTopologyMatrix_pthread(afsStateSpace *S,gsl_matrix *topol, gsl_matrix_int *moveType, int *dim1, int *dim2, int start, int stop, int *nnz){
	int i,j,k,steps,x,y;
	double top,bottom;
	afsObject *delta;
	int **activePos1, **activePos2; //first dimension here relates to 2 popns
	int nlin1,nlin2,compat;
	int nonZeroCount, tot, tCount;
	int size = 200;
	nlin1 = nlin2 = 0;
	nonZeroCount = 0;
	if(S->nstates != topol->size1 || S->nstates != moveType->size1){
		fprintf(stderr,"Error: StateSpace and matrices are not of equal size\n");
		exit(1);
	}
	*nnz = 0;
	//arrays for finding positions of entries
	activePos1 = malloc(size*sizeof(int *));
	activePos2 = malloc(size*sizeof(int *));
	for(i=0;i<size;i++){
		activePos1[i] = malloc(2*sizeof(int));
		activePos1[i][0] = activePos1[i][1] = 0;
		activePos2[i] = malloc(2*sizeof(int));
		activePos2[i][0] = activePos2[i][1] = 0;

	}

	tot = S->nstates * S->nstates;
	tCount = 0;
	delta = afsObjectNewFrom(S->states[0]);
	for(i=start;i<stop;i++){
		for(j=0;j<S->nstates;j++){
			gsl_matrix_int_set(moveType,i,j,666);

			//is ith state root state?
			if(S->states[i]->nalleles == 1){
				//set correct absorbing conditions
				if(i==j){
					gsl_matrix_set(topol,i,j,1.0);
					gsl_matrix_int_set(moveType,i,j,-1);
					dim1[nonZeroCount] = i;
					dim2[nonZeroCount] = j;
					nonZeroCount += 1;
				}
				else{
					gsl_matrix_set(topol,i,j,0.0);
					gsl_matrix_int_set(moveType,i,j,-1);
				}
			}
			else{ //ith not root; what is the move?
				steps = S->states[i]->nalleles - S->states[j]->nalleles ;
				if(steps < 2){
				//	delta = afsObjectDelta(S->states[i],S->states[j]);
					afsObjectDeltaPre(S->states[i],S->states[j],delta);
					switch(steps){
						case 0:
						// 0 "steps", so no changes in nalleles between two
						//check counts and positions
						if(abs(matrixSum(delta->popMats[0])) == 1){
							nonZeroEntries(delta->popMats[0], activePos1, &nlin1);
							nonZeroEntries(delta->popMats[1], activePos2, &nlin2);

							//migration?
							if(nlin1 == nlin2 && activePos1[0][0] == activePos2[0][0] &&
								(gsl_matrix_int_min(delta->popMats[0]) >= 0 || gsl_matrix_int_min(delta->popMats[1]) >= 0))
							{

								//migration popn1?
								if(matrixSum(delta->popMats[0]) == 1){
									gsl_matrix_set(topol,i,j,
										(double) gsl_matrix_int_get(S->states[i]->popMats[0],
										activePos1[0][0], activePos1[0][1]) / S->states[i]->aCounts[0]);
									gsl_matrix_int_set(moveType,i,j,2);
									dim1[nonZeroCount] = i;
									dim2[nonZeroCount] = j;
									nonZeroCount += 1;
								}
								else{ //migration popn2
									gsl_matrix_set(topol,i,j,
										(double) gsl_matrix_int_get(S->states[i]->popMats[1],
										activePos2[0][0],activePos2[0][1]) / S->states[i]->aCounts[1]);
									gsl_matrix_int_set(moveType,i,j,3);
									dim1[nonZeroCount] = i;
									dim2[nonZeroCount] = j;
									nonZeroCount += 1;
								}
							}
						}
						break;

						case 1://coalescent?
						//coal popn1?
						if(matrixSum(delta->popMats[0]) == 1 && countNegativeEntries(delta->popMats[1]) == 0 && S->states[j]->aCounts[0] >= 1 ){
							entriesGreaterThan(delta->popMats[0], activePos1, &nlin1,0);//old lineages
							entriesLessThan(delta->popMats[0], activePos2, &nlin2,0);//new lineages
							if(nlin2 != 0 && nlin2 < 2){

								compat=0;
								if(nlin1 == 1){
									if(2*activePos1[0][0] == activePos2[0][0] && 2*activePos1[0][1] == activePos2[0][1])
										compat = 1;
								}
								if(nlin1==2){
									if(activePos1[0][0] + activePos1[1][0] == activePos2[0][0] &&
										activePos1[0][1] + activePos1[1][1] == activePos2[0][1])
										compat = 1;
								}
								if(compat != 1){
									gsl_matrix_set(topol,i,j,0.0);
									gsl_matrix_int_set(moveType,i,j,666);
								}
								else{

									top = 1.0;
									bottom = S->states[i]->aCounts[0] * (S->states[i]->aCounts[0]-1) /2.0 ;
									for(k=0;k<nlin1;k++){
										x = activePos1[k][0];
										y = activePos1[k][1];
										top *= (float) xchoosey(gsl_matrix_int_get(S->states[i]->popMats[0],x,y),
											gsl_matrix_int_get(delta->popMats[0],x,y));
									}
									gsl_matrix_set(topol,i,j,top/bottom);
									gsl_matrix_int_set(moveType,i,j,0);
									dim1[nonZeroCount] = i;
									dim2[nonZeroCount] = j;
									nonZeroCount += 1;
								}
							}
						}
						else{
							if(matrixSum(delta->popMats[1]) == 1 && countNegativeEntries(delta->popMats[0]) == 0 && S->states[j]->aCounts[1] >= 1 ){
								entriesGreaterThan(delta->popMats[1], activePos1, &nlin1,0);//old lineages
								entriesLessThan(delta->popMats[1], activePos2, &nlin2,0);//new lineages
								if(nlin2 != 0 && nlin2 < 2){
									compat=0;
									if(nlin1 == 1){
										if(2*activePos1[0][0] == activePos2[0][0] && 2*activePos1[0][1] == activePos2[0][1])
											compat = 1;
									}
									if(nlin1==2){
										if(activePos1[0][0] + activePos1[1][0] == activePos2[0][0] &&
											activePos1[0][1] + activePos1[1][1] == activePos2[0][1])
											compat = 1;
									}
									if(compat != 1){

										gsl_matrix_set(topol,i,j,0.0);
										gsl_matrix_int_set(moveType,i,j,666);
									}
									else{
										top = 1.0;

										bottom = S->states[i]->aCounts[1] * (S->states[i]->aCounts[1]-1) /2.0 ;
										for(k=0;k<nlin1;k++){
											x = activePos1[k][0];
											y = activePos1[k][1];

											top *= xchoosey(gsl_matrix_int_get(S->states[i]->popMats[1],x,y),
												gsl_matrix_int_get(delta->popMats[1],x,y));
										}

										gsl_matrix_set(topol,i,j,top/bottom);
										gsl_matrix_int_set(moveType,i,j,1);
										dim1[nonZeroCount] = i;
										dim2[nonZeroCount] = j;
										nonZeroCount += 1;
									}
								}
							}
						}
						break;
					}
					//afsObjectFree(delta);
				}
			}
		}

	}
	//cleanup and free
	for(i=0;i<size;i++){
		free(activePos1[i]);
		free(activePos2[i]);
	}
	free(activePos1);
	free(activePos2);
	*nnz = nonZeroCount;
}

void coalMarkovChainTopologyMatrix_sparse_pthread(afsStateSpace *S,double *topol, int *moveType, int *dim1, int *dim2, int start, int stop, int *nnz){
	int i,j,k,steps,x,y;
	double top,bottom;
	afsObject *delta;
	int **activePos1, **activePos2; //first dimension here relates to 2 popns
	int nlin1,nlin2,compat;
	int nonZeroCount, tot, tCount;
	int size = 400;
	nlin1 = nlin2 = 0;

	nonZeroCount = 0;
	*nnz = 0;
	//arrays for finding positions of entries
	activePos1 = malloc(size*sizeof(int *));
	activePos2 = malloc(size*sizeof(int *));
	for(i=0;i<size;i++){
		activePos1[i] = malloc(2*sizeof(int));
		activePos1[i][0] = activePos1[i][1] = 0;
		activePos2[i] = malloc(2*sizeof(int));
		activePos2[i][0] = activePos2[i][1] = 0;
	}

	tot = S->nstates * S->nstates;
	tCount = 0;
	delta = afsObjectNewFrom(S->states[0]);
	for(i=start;i<stop;i++){
		for(j=0;j<S->nstates;j++){
		//	printf("nonzerocount=%d\n",nonZeroCount);

			if(S->states[i]->nalleles == 1){
				//set correct absorbing conditions
				if(i==j){
					topol[nonZeroCount] = 1.0;
					moveType[nonZeroCount] = -1;
					dim1[nonZeroCount] = i;
					dim2[nonZeroCount] = j;
					nonZeroCount += 1;
				}
			}

			else{ //ith not root; what is the move?
				steps = S->states[i]->nalleles - S->states[j]->nalleles ;
				if(steps < 2){
					delta = afsObjectDelta(S->states[i],S->states[j]);
					switch(steps){
						case 0:
						// 0 "steps", so no changes in nalleles between two
						//check counts and positions
						if(abs(matrixSum(delta->popMats[0])) == 1){
							nonZeroEntries(delta->popMats[0], activePos1, &nlin1);
							nonZeroEntries(delta->popMats[1], activePos2, &nlin2);

							//migration?
							if(nlin1 == nlin2 && activePos1[0][0] == activePos2[0][0] &&
								(gsl_matrix_int_min(delta->popMats[0]) >= 0 || gsl_matrix_int_min(delta->popMats[1]) >= 0)){

								//migration popn1?
								if(matrixSum(delta->popMats[0]) == 1){
									topol[nonZeroCount] = (double) gsl_matrix_int_get(S->states[i]->popMats[0],
										activePos1[0][0], activePos1[0][1]) / S->states[i]->aCounts[0];
									moveType[nonZeroCount] = 2;
									dim1[nonZeroCount] = i;
									dim2[nonZeroCount] = j;
									nonZeroCount += 1;
								}
								else{ //migration popn2
									topol[nonZeroCount] = (double) gsl_matrix_int_get(S->states[i]->popMats[1],
										activePos2[0][0],activePos2[0][1]) / S->states[i]->aCounts[1];
									moveType[nonZeroCount] = 3;

									dim1[nonZeroCount] = i;
									dim2[nonZeroCount] = j;
									nonZeroCount += 1;
								}
							}
						}
						break;

						case 1://coalescent?
								//coal popn1?
						if(matrixSum(delta->popMats[0]) == 1 && countNegativeEntries(delta->popMats[1]) == 0 && S->states[j]->aCounts[0] >= 1 ){
							entriesGreaterThan(delta->popMats[0], activePos1, &nlin1,0);//old lineages
							entriesLessThan(delta->popMats[0], activePos2, &nlin2,0);//new lineages
							if(nlin2 != 0 && nlin2 < 2){

								compat=0;
								if(nlin1 == 1){
									if(2*activePos1[0][0] == activePos2[0][0] && 2*activePos1[0][1] == activePos2[0][1])
										compat = 1;
								}
								if(nlin1==2){
									if(activePos1[0][0] + activePos1[1][0] == activePos2[0][0] &&
										activePos1[0][1] + activePos1[1][1] == activePos2[0][1])
										compat = 1;
								}
								if(compat == 1){
									top = 1.0;
									bottom = S->states[i]->aCounts[0] * (S->states[i]->aCounts[0]-1) /2.0 ;
									for(k=0;k<nlin1;k++){
										x = activePos1[k][0];
										y = activePos1[k][1];
										top *= (float) xchoosey(gsl_matrix_int_get(S->states[i]->popMats[0],x,y),
											gsl_matrix_int_get(delta->popMats[0],x,y));
									}

									topol[nonZeroCount] = top/bottom;
									moveType[nonZeroCount] = 0;
									dim1[nonZeroCount] = i;
									dim2[nonZeroCount] = j;
									nonZeroCount += 1;
								}
							}
						}
						else{
							if(matrixSum(delta->popMats[1]) == 1 && countNegativeEntries(delta->popMats[0]) == 0 && S->states[j]->aCounts[1] >= 1 ){
								entriesGreaterThan(delta->popMats[1], activePos1, &nlin1,0);//old lineages
								entriesLessThan(delta->popMats[1], activePos2, &nlin2,0);//new lineages
								if(nlin2 != 0 && nlin2 < 2){
									compat=0;
									if(nlin1 == 1){
										if(2*activePos1[0][0] == activePos2[0][0] && 2*activePos1[0][1] == activePos2[0][1])
											compat = 1;
									}
									if(nlin1==2){
										if(activePos1[0][0] + activePos1[1][0] == activePos2[0][0] &&
											activePos1[0][1] + activePos1[1][1] == activePos2[0][1])
											compat = 1;
									}
									if(compat == 1){
										top = 1.0;
										bottom = S->states[i]->aCounts[1] * (S->states[i]->aCounts[1]-1) /2.0 ;
										for(k=0;k<nlin1;k++){
											x = activePos1[k][0];
											y = activePos1[k][1];

											top *= xchoosey(gsl_matrix_int_get(S->states[i]->popMats[1],x,y),
												gsl_matrix_int_get(delta->popMats[1],x,y));
										}

										topol[nonZeroCount] = top/bottom;
										moveType[nonZeroCount] = 1;
										dim1[nonZeroCount] = i;
										dim2[nonZeroCount] = j;
										nonZeroCount += 1;
									}
								}
							}
						}
						break;
					}
					//afsObjectFree(delta);
				}
			}
		}
	}
	//cleanup and free
	for(i=0;i<size;i++){
		free(activePos1[i]);
		free(activePos2[i]);
	}
	free(activePos1);
	free(activePos2);
	*nnz = nonZeroCount;
}


/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/
#include <petsc.h>
#include <petscmat.h>
#include <petscvec.h>

#include <StGermain/StGermain.h>
#include <StgDomain/StgDomain.h>

#include "common-driver-utils.h"
#include "stokes_block_scaling.h"
#include <StgFEM/StgFEM.h>
#include <PICellerator/PICellerator.h>
#include <Underworld/Underworld.h>
#include "Solvers/SLE/SLE.h" /* to give the AugLagStokes_SLE type */
#include "Solvers/KSPSolvers/KSPSolvers.h" /* for __KSP_COMMON */

#include "BSSCR.h"
#include "writeMatVec.h"

/* creates and builds the "checker-board" null-space vectors for pressure
   and sets the t and v "Vec pointers" on the bsscr struct to point to them */
#undef __FUNCT__
#define __FUNCT__ "KSPBuildPressure_CB_Nullspace_BSSCR"
PetscErrorCode KSPBuildPressure_CB_Nullspace_BSSCR(KSP ksp)
{
    KSP_BSSCR        *bsscr = (KSP_BSSCR *)ksp->data;
    FeEquationNumber *eq_num = bsscr->solver->st_sle->pSolnVec->eqNum;
    FeMesh           *feMesh = bsscr->solver->st_sle->pSolnVec->feVariable->feMesh; /* is the pressure mesh */
    unsigned          ijk[3];
    Vec               t, v;
    int numLocalNodes, globalNodeNumber, i, j, eq;
    MatStokesBlockScaling BA = bsscr->BA;
    PetscErrorCode ierr;
    Mat            Amat,Pmat, G;
    MatStructure   pflag;

    PetscFunctionBegin;
    /* get G matrix from Amat matrix operator on ksp */
    ierr=Stg_PCGetOperators(ksp->pc,&Amat,&Pmat,&pflag);CHKERRQ(ierr);
    MatNestGetSubMat( Amat, 0,1, &G );/* G should always exist */
    /* now create Vecs t and v to match size of G: i.e. pressure */ /* NOTE: not using "h" vector from ksp->vec_rhs because this part of the block vector doesn't always exist */
    MatGetVecs( G, &t, PETSC_NULL );/* t and v are destroyed in KSPDestroy_BSSCR */
    MatGetVecs( G, &v, PETSC_NULL );/* t and v such that can do G*t */

    numLocalNodes = Mesh_GetLocalSize( feMesh,  MT_VERTEX); /* number of nodes on current proc not counting any shadow nodes */
    for(j=0;j<numLocalNodes;j++){
	i = globalNodeNumber = Mesh_DomainToGlobal( feMesh, MT_VERTEX, j);
	RegularMeshUtils_Element_1DTo3D(feMesh, i, ijk);
	eq = eq_num->mapNodeDof2Eq[j][0];/* get global equation number -- 2nd arg is always 0 because pressure has only one dof */
	if(eq != -1){
	    if( (ijk[0]+ijk[1]+ijk[2])%2 ==0 ){
		VecSetValue(t,eq,1.0,INSERT_VALUES);
	    }
	    else{
		VecSetValue(v,eq,1.0,INSERT_VALUES);	    }}}

    VecAssemblyBegin( t );
    VecAssemblyEnd( t );
    VecAssemblyBegin( v );
    VecAssemblyEnd( v );

    /* Scaling the null vectors here because it easier at the moment *//* maybe should do this in the original scaling function */
    if( BA->scaling_exists == PETSC_TRUE ){
	Vec R2;
	/* Get the scalings out the block mat data */
	VecNestGetSubVec( BA->Rz, 1, &R2 );
	VecPointwiseDivide( t, t, R2); /* x <- x * 1/R2 */
	VecPointwiseDivide( v, v, R2);
    }

    // bsscr_writeVec( t, "t", "Writing t vector");
    // bsscr_writeVec( v, "v", "Writing v vector");

    bsscr->t=t;
    bsscr->v=v;

    PetscFunctionReturn(0);
}
/* creates and builds the "constant" null-space vector for pressure
   and sets the t "Vec pointer" on the bsscr struct to point to it */
#undef __FUNCT__
#define __FUNCT__ "KSPBuildPressure_Const_Nullspace_BSSCR"
PetscErrorCode KSPBuildPressure_Const_Nullspace_BSSCR(KSP ksp)
{
    KSP_BSSCR        *bsscr = (KSP_BSSCR *)ksp->data;
    FeEquationNumber *eq_num = bsscr->solver->st_sle->pSolnVec->eqNum;
    FeMesh           *feMesh = bsscr->solver->st_sle->pSolnVec->feVariable->feMesh; /* is the pressure mesh */
    unsigned          ijk[3];
    Vec               t;
    int numLocalNodes, globalNodeNumber, i, eq;
    MatStokesBlockScaling BA = bsscr->BA;
    PetscErrorCode ierr;
    Mat            Amat,Pmat, G;
    MatStructure   pflag;

    PetscFunctionBegin;
    /* get G matrix from Amat matrix operator on ksp */
    ierr=Stg_PCGetOperators(ksp->pc,&Amat,&Pmat,&pflag);CHKERRQ(ierr);
    MatNestGetSubMat( Amat, 0,1, &G );/* G should always exist */
    /* now create Vec t to match size of G: i.e. pressure */ /* NOTE: not using "h" vector from ksp->vec_rhs because this part of the block vector doesn't always exist */
    MatGetVecs( G, &t, PETSC_NULL );/* t is destroyed in KSPDestroy_BSSCR */

    VecSet(t, 1.0);

    VecAssemblyBegin( t );
    VecAssemblyEnd( t );

    /* Scaling the null vectors here because it easier at the moment */
    if( BA->scaling_exists == PETSC_TRUE ){
	Vec R2;
	/* Get the scalings out the block mat data */
	VecNestGetSubVec( BA->Rz, 1, &R2 );
	VecPointwiseDivide( t, t, R2); /* x <- x * 1/R2 */
    }
    // bsscr_writeVec( t, "t", "Writing t vector");
    bsscr->t=t;

    PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "KSPRemovePressureNullspace_BSSCR"
PetscErrorCode KSPRemovePressureNullspace_BSSCR(KSP ksp, Vec h_hat)
{
    KSP_BSSCR            *bsscr = (KSP_BSSCR *)ksp->data;
    MatStokesBlockScaling BA    = bsscr->BA;
    PetscErrorCode ierr;
    PetscScalar norm, a, a1, a2, hnorm, pnorm, gnorm;
    Vec t2, t,v;
    Mat            Amat,Pmat, D;
    MatStructure   pflag;
    double nstol   = bsscr->nstol; /* set in KSPCreate_BSSCR */

    PetscFunctionBegin;
    t=bsscr->t;
    v=bsscr->v;
    /* get G matrix from Amat matrix operator on ksp */
    ierr=Stg_PCGetOperators(ksp->pc,&Amat,&Pmat,&pflag);CHKERRQ(ierr);
    //MatNestGetSubMat( Amat, 0,1, &G );/* G should always exist */
    MatNestGetSubMat( Amat, 1,0, &D );/* D should always exist */
    /* now create Vec t2 to match left hand size of G: i.e. velocity */
    MatGetVecs( D, &t2, PETSC_NULL );
    MatNorm(D,NORM_INFINITY,&gnorm); /* seems like not a bad estimate of the largest eigenvalue for this matrix */
    if(t){/* assumes that v and t are initially set to PETSC_NULL (in KSPCreate_BSSCR ) */
	MatMultTranspose( D, t, t2);
	VecNorm(t2, NORM_2, &norm);
	VecNorm(t, NORM_2, &a);
	norm=norm/a;/* so we are using unit null vector */
	if(norm < nstol*gnorm){/* then t in NS of G */
	    VecDot(t,h_hat, &a1);
	    VecDot(t,t, &a2);
	    a=-a1/a2;
	    VecAXPY(h_hat, a, t);
	    VecDot(t,h_hat, &a1);
	    PetscPrintf( PETSC_COMM_WORLD, "\n\t* Found t NS norm= %g : Dot = %g\n\n", norm, a1);
	}
    }
    if(v){
	MatMultTranspose( D, v, t2);
	VecNorm(t2, NORM_2, &norm);
	VecNorm(v, NORM_2, &a);
	norm=norm/a;/* so we are using unit null vector */
	if(norm < nstol*gnorm){/* then v in NS of G */
	    VecDot(v,h_hat, &a1);
	    VecDot(v,v, &a2);
	    a=-a1/a2;
	    VecAXPY(h_hat, a, v);
	    VecDot(v,h_hat, &a1);
	    PetscPrintf( PETSC_COMM_WORLD, "\n\t* Found v NS norm= %g : Dot = %g\n\n", norm, a1);
	}
    }
    // bsscr_writeVec( h_hat, "h", "Writing h_hat Vector in Solver");
    Stg_VecDestroy(&t2);

    PetscFunctionReturn(0);
}


/**
 *
 * @file core_ztsrfb.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 Hatem Ltaief
 * @author Mathieu Faverge
 * @author Azzam Haidar
 * @date 2010-11-15
 * @precisions normal z -> c d s
 *
 **/
#include <cblas.h>
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup CORE_PLASMA_Complex64_t
 *
 *  CORE_ztsrfb applies a complex block reflector H or its transpose H' to a
 *  complex rectangular matrix formed by coupling two tiles A1 and A2,
 *  from either the left or the right.
 *
 *******************************************************************************
 *
 * @param[in] side
 *         @arg PlasmaLeft  : apply Q or Q**H from the Left;
 *         @arg PlasmaRight : apply Q or Q**H from the Right.
 *
 * @param[in] trans
 *         @arg PlasmaNoTrans   : No transpose, apply Q;
 *         @arg PlasmaConjTrans : ConjTranspose, apply Q**H.
 *
 * @param[in] direct
 *         Indicates how H is formed from a product of elementary
 *         reflectors
 *         @arg PlasmaForward  : H = H(1) H(2) . . . H(k) (Forward)
 *         @arg PlasmaBackward : H = H(k) . . . H(2) H(1) (Backward)
 *
 * @param[in] storev
 *         Indicates how the vectors which define the elementary
 *         reflectors are stored:
 *         @arg PlasmaColumnwise
 *         @arg PlasmaRowwise
 *
 * @param[in] M1
 *         The number of columns of the tile A1. M1 >= 0.
 *
 * @param[in] N1
 *         The number of rows of the tile A1. N1 >= 0.
 *
 * @param[in] M2
 *         The number of columns of the tile A2. M2 >= 0.
 *
 * @param[in] N2
 *         The number of rows of the tile A2. N2 >= 0.
 *
 * @param[in] K
 *          The order of the matrix T (= the number of elementary
 *          reflectors whose product defines the block reflector).
 *
 * @param[in,out] A1
 *         On entry, the M1-by-N1 tile A1.
 *         On exit, A1 is overwritten by the application of Q.
 *
 * @param[in] LDA1
 *         The leading dimension of the array A1. LDA1 >= max(1,N1).
 *
 * @param[in,out] A2
 *         On entry, the M2-by-N2 tile A2.
 *         On exit, A2 is overwritten by the application of Q.
 *
 * @param[in] LDA2
 *         The leading dimension of the tile A2. LDA2 >= max(1,N2).
 *
 * @param[in] V
 *         (LDV,K) if STOREV = 'C'
 *         (LDV,M2) if STOREV = 'R' and SIDE = 'L'
 *         (LDV,N2) if STOREV = 'R' and SIDE = 'R'
 *         The matrix V.
 *
 * @param[in] LDV
 *         The leading dimension of the array V.
 *         If STOREV = 'C' and SIDE = 'L', LDV >= max(1,M2);
 *         if STOREV = 'C' and SIDE = 'R', LDV >= max(1,N2);
 *         if STOREV = 'R', LDV >= K.
 *
 * @param[out] T
 *         The triangular K-by-K matrix T in the representation of the
 *         block reflector.
 *         T is upper triangular by block (economic storage);
 *         The rest of the array is not referenced.
 *
 * @param[in] LDT
 *         The leading dimension of the array T. LDT >= K.
 *
 * @param[in,out] WORK
 *
 * @param[in] LDWORK
 *         The dimension of the array WORK.
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *
 ******************************************************************************/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_ztsrfb = PCORE_ztsrfb
#define CORE_ztsrfb PCORE_ztsrfb
#endif
int
CORE_ztsrfb(PLASMA_enum side, PLASMA_enum trans, int direct, int storev,
            int M1, int N1, int M2, int N2, int K,
            PLASMA_Complex64_t *A1, int LDA1,
            PLASMA_Complex64_t *A2, int LDA2,
            const PLASMA_Complex64_t *V, int LDV,
            const PLASMA_Complex64_t *T, int LDT,
            PLASMA_Complex64_t *WORK, int LDWORK)
{
    static PLASMA_Complex64_t zone  =  1.0;
    static PLASMA_Complex64_t mzone = -1.0;

    int j;

    /* Check input arguments */
    if (M1 < 0) {
        coreblas_error(5, "Illegal value of M1");
        return -5;
    }
    if (N1 < 0) {
        coreblas_error(6, "Illegal value of N1");
        return -6;
    }
    if ( (M2 < 0) ||
         ( (M2 != M1) && (side == PlasmaRight) ) ){
        coreblas_error(7, "Illegal value of M2");
        return -7;
    }
    if ( (N2 < 0) ||
         ( (N2 != N1) && (side == PlasmaLeft) ) ){
        coreblas_error(8, "Illegal value of N2");
        return -8;
    }
    if (K < 0) {
        coreblas_error(9, "Illegal value of K");
        return -9;
    }

    /* Quick return */
    if ((M1 == 0) || (N1 == 0) || (M2 == 0) || (N2 == 0) || (K == 0))
        return PLASMA_SUCCESS;

    if (storev == PlasmaColumnwise) {
        if (direct == PlasmaForward) {
            if (side == PlasmaLeft) {
                /*
                 * B = A1 + V' * A2
                 */
                LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,
                    lapack_const(PlasmaUpperLower),
                    K, N1,
                    A1, LDA1, WORK, LDWORK);

                cblas_zgemm(
                    CblasColMajor, CblasConjTrans, CblasNoTrans,
                    K, N2, M2,
                    CBLAS_SADDR(zone), V, LDV,
                    A2, LDA2,
                    CBLAS_SADDR(zone), WORK, LDWORK);
                /*
                 * A2 = A2 - V*T*B -> B = T*B, A2 = A2 - V*B
                 */
                cblas_ztrmm(
                    CblasColMajor, CblasLeft, CblasUpper,
                    (CBLAS_TRANSPOSE)trans, CblasNonUnit, K, N2,
                    CBLAS_SADDR(zone), T, LDT, WORK, LDWORK);

                cblas_zgemm(
                    CblasColMajor, CblasNoTrans, CblasNoTrans,
                    M2, N2, K,
                    CBLAS_SADDR(mzone), V, LDV,
                    WORK, LDWORK,
                    CBLAS_SADDR(zone), A2, LDA2);
                /*
                 * A1 = A1 - B
                 */
                for(j = 0; j < N1; j++) {
                    cblas_zaxpy(
                        K, CBLAS_SADDR(mzone),
                        &WORK[LDWORK*j], 1,
                        &A1[LDA1*j], 1);
                }
            }
            /*
             * Columnwise / Forward / Right
             */
            else {
                /*
                 * B = A1 + A2 * V
                 */
                LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,
                    lapack_const(PlasmaUpperLower),
                    M1, K,
                    A1, LDA1, WORK, LDWORK);

                cblas_zgemm(
                    CblasColMajor, CblasNoTrans, CblasNoTrans,
                    M2, K, N2,
                    CBLAS_SADDR(zone), A2, LDA2,
                    V, LDV,
                    CBLAS_SADDR(zone), WORK, LDWORK);
                /*
                 * A2 = A2 - B*T*V' -> B = B*T, A2 = A2 - B*V'
                 */
                cblas_ztrmm(
                    CblasColMajor, CblasRight, CblasUpper,
                    (CBLAS_TRANSPOSE)trans, CblasNonUnit, M1, K,
                    CBLAS_SADDR(zone), T, LDT, WORK, LDWORK);

                cblas_zgemm(
                    CblasColMajor, CblasNoTrans, CblasConjTrans,
                    M2, N2, K,
                    CBLAS_SADDR(mzone), WORK, LDWORK,
                    V, LDV,
                    CBLAS_SADDR(zone), A2, LDA2);
                /*
                 * A1 = A1 - B
                 */
                for(j = 0; j < K; j++) {
                    cblas_zaxpy(
                        M1, CBLAS_SADDR(mzone),
                        &WORK[LDWORK*j], 1,
                        &A1[LDA1*j], 1);
                }
            }
        }
        else {
            coreblas_error(3, "Not implemented (ColMajor / Backward / Left or Right)");
            return PLASMA_ERR_NOT_SUPPORTED;
        }
    }
    else {
        if (direct == PlasmaForward) {
            /*
             * Rowwise / Forward / Left
             */
            if (side == PlasmaLeft) {
                /*
                 * B = A1 + V * A2
                 */
                LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,
                    lapack_const(PlasmaUpperLower),
                    K, N1,
                    A1, LDA1, WORK, LDWORK);

                cblas_zgemm(
                    CblasColMajor, CblasNoTrans, CblasNoTrans,
                    K, N2, M2,
                    CBLAS_SADDR(zone), V, LDV,
                    A2, LDA2,
                    CBLAS_SADDR(zone), WORK, LDWORK);
                /*
                 * A2 = A2 - V'*T*B -> B = T*B, A2 = A2 - V'*B
                 */
                cblas_ztrmm(
                    CblasColMajor, CblasLeft, CblasUpper,
                    (CBLAS_TRANSPOSE)trans, CblasNonUnit, K, N2,
                    CBLAS_SADDR(zone), T, LDT, WORK, LDWORK);

                cblas_zgemm(
                    CblasColMajor, CblasConjTrans, CblasNoTrans,
                    M2, N2, K,
                    CBLAS_SADDR(mzone), V, LDV,
                    WORK, LDWORK,
                    CBLAS_SADDR(zone), A2, LDA2);
                /*
                 * A1 = A1 - B
                 */
                for(j=0; j<N1; j++) {
                    cblas_zaxpy(
                        K, CBLAS_SADDR(mzone),
                        &WORK[LDWORK*j], 1,
                        &A1[LDA1*j], 1);
                }
            }
            /*
             * Rowwise / Forward / Right
             */
            else {
                /*
                 * B = A1 + A2 * V'
                 */
                LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,
                    lapack_const(PlasmaUpperLower),
                    M1, K,
                    A1, LDA1, WORK, LDWORK);

                cblas_zgemm(
                    CblasColMajor, CblasNoTrans, CblasConjTrans,
                    M2, K, N2,
                    CBLAS_SADDR(zone), A2, LDA2,
                    V, LDV,
                    CBLAS_SADDR(zone), WORK, LDWORK);
                /*
                 * A2 = A2 - B*T*V -> B = B*T, A2 = A2 - B*V'
                 */
                cblas_ztrmm(
                    CblasColMajor, CblasRight, CblasUpper,
                    (CBLAS_TRANSPOSE)trans, CblasNonUnit, M1, K,
                    CBLAS_SADDR(zone), T, LDT, WORK, LDWORK);

                cblas_zgemm(
                    CblasColMajor, CblasNoTrans, CblasNoTrans,
                    M2, N2, K,
                    CBLAS_SADDR(mzone), WORK, LDWORK,
                    V, LDV,
                    CBLAS_SADDR(zone), A2, LDA2);
                /*
                 * A1 = A1 - B
                 */
                for(j = 0; j < K; j++) {
                    cblas_zaxpy(
                        M1, CBLAS_SADDR(mzone),
                        &WORK[LDWORK*j], 1,
                        &A1[LDA1*j], 1);
                }
            }
        }
        else {
            coreblas_error(3, "Not implemented (RowMajor / Backward / Left or Right)");
            return PLASMA_ERR_NOT_SUPPORTED;
        }
    }
    return PLASMA_SUCCESS;
}