/*
 *  vbHmmGauss.c
 *  Model-specific core functions for VB-HMM-GAUSS.
 *
 *  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 2016.11.04
 */

#include "vbHmmGauss.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  MAX(a,b)  ((a)>(b)?(a):(b))
#define  MIN(a,b)  ((a)<(b)?(a):(b))

static int isGlobalAnalysis = 0;

void setFunctions_gauss(){
    commonFunctions funcs;
    funcs.newModelParameters    = newModelParameters_gauss;
    funcs.freeModelParameters   = freeModelParameters_gauss;
    funcs.newModelStats         = newModelStats_gauss;
    funcs.freeModelStats        = freeModelStats_gauss;
    funcs.initializeVbHmm       = initializeVbHmm_gauss;
    funcs.pTilde_z1             = pTilde_z1_gauss;
    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_gauss;
    funcs.pTilde_xn_zn          = pTilde_xn_zn_gauss;
    funcs.calcStatsVars         = calcStatsVars_gauss;
    funcs.maximization          = maximization_gauss;
    funcs.varLowerBound         = varLowerBound_gauss;
    funcs.reorderParameters     = reorderParameters_gauss;
    funcs.outputResults         = outputResults_gauss;
    setFunctions( funcs );
}

void setGFunctions_gauss(){
    gCommonFunctions funcs;
    funcs.newModelParameters    = newModelParameters_gauss;
    funcs.freeModelParameters   = freeModelParameters_gauss;
    funcs.newModelStats         = newModelStats_gauss;
    funcs.freeModelStats        = freeModelStats_gauss;
    funcs.newModelStatsG        = newModelStatsG_gauss;
    funcs.freeModelStatsG       = freeModelStatsG_gauss;
    funcs.initializeVbHmmG      = initializeVbHmmG_gauss;
    funcs.pTilde_z1             = pTilde_z1_gauss;
    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_gauss;
    funcs.pTilde_xn_zn          = pTilde_xn_zn_gauss;
    funcs.calcStatsVarsG        = calcStatsVarsG_gauss;
    funcs.maximizationG         = maximizationG_gauss;
    funcs.varLowerBoundG        = varLowerBoundG_gauss;
    funcs.reorderParametersG    = reorderParametersG_gauss;
    funcs.outputResultsG        = outputResultsG_gauss;
    setGFunctions( funcs );
    isGlobalAnalysis = 1;
}

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

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

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

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

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

    p->uBtArr = (double *)malloc( sNo * sizeof(double) );
    p->uMuArr = (double *)malloc( sNo * sizeof(double) );
    p->uAArr = (double *)malloc( sNo * sizeof(double) );
    p->uBArr = (double *)malloc( sNo * sizeof(double) );
    p->btMu = (double *)malloc( sNo * sizeof(double) );
    p->aLm = (double *)malloc( sNo * sizeof(double) );
    p->bLm = (double *)malloc( sNo * sizeof(double) );
    p->mu0 = (double *)malloc( sNo * sizeof(double) );

    return p;
}

void freeModelParameters_gauss( p, xn, sNo )
void **p;
xnDataSet *xn;
int sNo;
{
    gaussParameters *gp = *p;
    int i;

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

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

    free( gp->uBtArr );
    free( gp->uMuArr );
    free( gp->uAArr );
    free( gp->uBArr );
    free( gp->btMu );
    free( gp->aLm );
    free( gp->bLm );
    free( gp->mu0 );

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

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

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

        return s;

    } else {

        return NULL;

    }
}

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

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

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

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

    return gs;
}

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

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

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

    int i, j;
    double totalX = 0.0, precX, varX = 0.0;
    for( i = 0 ; i < dLen ; i++ ){
        totalX += d->v[i];
    }
    double meanX = totalX / (double)dLen;
    for( i = 0 ; i < dLen ; i++ ){
        varX += pow(d->v[i] - meanX, 2.0);
    }
    varX /= (double)(dLen - 1);
    precX = 1.0 / varX;

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

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

    // hyper parameter for p( mu(k), lm(k) )
    for( i = 0 ; i < sNo ; i++ ){
        p->uBtArr[i] = 0.25;
        p->uMuArr[i] = meanX;
        p->uAArr[i] = 0.01 * varX;
        p->uBArr[i] = 0.01;
    }

    initialize_indVars_gauss( xn, gv, iv );

    calcStatsVars_gauss( xn, gv, iv );
    maximization_gauss( xn, gv, iv );
}

void initializeVbHmmG_gauss( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    gaussData *d;
    size_t dLen, totalN;
    int sNo = gv->sNo, rNo = xns->R;
    gaussParameters *p = gv->params;

    int i, j, r;
    double totalX, precX, varX, meanX;

    totalX = 0.0;
    totalN = 0;
    for( r = 0 ; r < rNo ; r++ ){
        d = xns->xn[r]->data;
        dLen = xns->xn[r]->N;
        for( i = 0 ; i < dLen ; i++ ){
            totalX += d->v[i];
        }
        totalN += dLen;
    }
    meanX = totalX / (double)totalN;
    varX = 0.0;
    for( r = 0 ; r < rNo ; r++ ){
        d = xns->xn[r]->data;
        dLen = xns->xn[r]->N;
        for( i = 0 ; i < dLen ; i++ ){
            varX += pow(d->v[i] - meanX, 2.0);
        }
    }
    varX /= (double)(totalN - 1);
    precX = 1.0 / varX;

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

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

    // hyper parameter for p( mu(k), lm(k) )
    for( i = 0 ; i < sNo ; i++ ){
        p->uBtArr[i] = 0.25;
        p->uMuArr[i] = meanX;
        p->uAArr[i] = 0.01 * varX;
        p->uBArr[i] = 0.01;
    }

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

    calcStatsVarsG_gauss( xns, gv, ivs );
    maximizationG_gauss( xns, gv, ivs );
}

void initialize_indVars_gauss( 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_gauss( 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 = (gaussData*)malloc( sizeof(gaussData) );
    gaussData *d = (gaussData*)xn->data;
    d->v = NULL;
    return xn;
}

void freeXnDataSet_gauss( xn )
xnDataSet **xn;
{
    gaussData *d = (gaussData*)(*xn)->data;
    free( d->v );
    free( (*xn)->data );
    free( (*xn)->name );
    free( *xn );
    *xn = NULL;
}

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

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

double pTilde_xn_zn_gauss( xn, n, i, params )
xnDataSet *xn;
size_t n;
int i;
void *params;
{
    gaussParameters *p = (gaussParameters*)params;
    gaussData *d = (gaussData*)xn->data;
    double val;
    val  = p->avgLnLm[i] - log(2.0 * M_PI);
    val -= 1.0/p->btMu[i] + p->aLm[i] / p->bLm[i] * pow( d->v[n] - p->avgMu[i], 2.0);
    return exp(val / 2.0);
}

void calcStatsVars_gauss( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    gaussData *d = (gaussData*)xn->data;
    gaussStats *s = (gaussStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *Nii = s->Nii, **Nij = s->Nij, *Ni = s->Ni;
    double *barX = s->barX, *NiSi = s->NiSi;
    size_t n;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        Ni[i]   = 1e-10;
        Nii[i]  = 1e-10;
        barX[i] = 1e-10;
        NiSi[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];
            barX[i] += gmMat[n][i] * d->v[n];
            for( j = 0 ; j < sNo ; j++ ){
                Nii[i]    += xiMat[n][i][j];
                Nij[i][j] += xiMat[n][i][j];
            }
        }
        barX[i] /= Ni[i];
        for( n = 0 ; n < dLen ; n++ ){
            NiSi[i] += gmMat[n][i] * pow( d->v[n] - barX[i], 2.0);
        }
    }
}

void calcStatsVarsG_gauss( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    gaussData *d;
    gaussGlobalStats *gs = (gaussGlobalStats*)ivs->stats;
    int sNo = gv->sNo, rNo = xns->R;
    double **gmMat, ***xiMat;
    double *NiiR = gs->NiiR, **NijR = gs->NijR, *NiR = gs->NiR;
    double *barXR = gs->barXR, *NiSiR = gs->NiSiR, *z1iR = gs->z1iR;
    size_t dLen, n;
    int i, j, r;

    for( i = 0 ; i < sNo ; i++ ){
        NiR[i]   = 1e-10;
        NiiR[i]  = 1e-10;
        barXR[i] = 1e-10;
        NiSiR[i] = 1e-10;
        for( j = 0 ; j < sNo ; j++ ){
            NijR[i][j] = 1e-10;
        }
        z1iR[i] = 1e-10;
    }
    for( r = 0 ; r < rNo ; r++ ){
        d = (gaussData*)xns->xn[r]->data;
        dLen = xns->xn[r]->N;
        gmMat = ivs->indVars[r]->gmMat;
        xiMat = ivs->indVars[r]->xiMat;
        for( i = 0 ; i < sNo ; i++ ){
            z1iR[i] += gmMat[0][i];
            for( n = 0 ; n < dLen ; n++ ){
                NiR[i]   += gmMat[n][i];
                barXR[i] += gmMat[n][i] * d->v[n];
                for( j = 0 ; j < sNo ; j++ ){
                    NiiR[i]  += xiMat[n][i][j];
                    NijR[i][j] += xiMat[n][i][j];
                }
            }
        }
    }
    for( i = 0 ; i < sNo ; i++ ){
        barXR[i] /= NiR[i];
        for( r = 0 ; r < rNo ; r++ ){
            d = (gaussData*)xns->xn[r]->data;
            dLen = xns->xn[r]->N;
            gmMat = ivs->indVars[r]->gmMat;
            for( n = 0 ; n < dLen ; n++ ){
                NiSiR[i] += gmMat[n][i] * pow( d->v[n] - barXR[i], 2.0);
            }
        }
    }
}

void maximization_gauss( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    gaussParameters *p = (gaussParameters*)gv->params;
    gaussStats *s = (gaussStats*)iv->stats;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uBtArr = p->uBtArr, *uMuArr = p->uMuArr, *uAArr = p->uAArr, *uBArr = p->uBArr;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgMu = p->avgMu, *avgLm = p->avgLm, *avgLnLm = p->avgLnLm;
    double *mu0 = p->mu0, *btMu = p->btMu, *aLm = p->aLm, *bLm = p->bLm;
    double *Ni = s->Ni, *Nii = s->Nii, **Nij = s->Nij, *barX = s->barX, *NiSi = s->NiSi;
    int i, j;

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

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

        btMu[i] = uBtArr[i] + Ni[i];
        mu0[i]  = (uBtArr[i] * uMuArr[i] + Ni[i] * barX[i]) / btMu[i];
        aLm[i]  = uAArr[i] + Ni[i] / 2.0;
        bLm[i]  = uBArr[i] + (NiSi[i] / 2.0);
        bLm[i] += uBtArr[i] * Ni[i] * pow( barX[i] - uMuArr[i], 2.0) / 2.0 / (uBtArr[i] + Ni[i]);

        avgMu[i]    = mu0[i];
        avgLm[i]    = aLm[i] / bLm[i];
        avgLnLm[i]  = gsl_sf_psi( aLm[i] ) - log( bLm[i] );
    }
}

void maximizationG_gauss( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    gaussParameters *p = (gaussParameters*)gv->params;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uBtArr = p->uBtArr, *uMuArr = p->uMuArr, *uAArr = p->uAArr, *uBArr = p->uBArr;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgMu = p->avgMu, *avgLm = p->avgLm, *avgLnLm = p->avgLnLm;
    double *mu0 = p->mu0, *btMu = p->btMu, *aLm = p->aLm, *bLm = p->bLm;
    gaussGlobalStats *gs = (gaussGlobalStats*)ivs->stats;
    double *NiR = gs->NiR, *NiiR = gs->NiiR, **NijR = gs->NijR, *barXR = gs->barXR, *NiSiR = gs->NiSiR;
    double *z1iR = gs->z1iR, dR = (double)(xns->R);
    int sNo = gv->sNo;
    int i, j;

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

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

        btMu[i] = uBtArr[i] + NiR[i];
        mu0[i]  = (uBtArr[i] * uMuArr[i] + NiR[i] * barXR[i]) / btMu[i];
        aLm[i]  = uAArr[i] + NiR[i] / 2.0;
        bLm[i]  = uBArr[i] + (NiSiR[i] / 2.0);
        bLm[i] += uBtArr[i] * NiR[i] * pow( barXR[i] - uMuArr[i], 2.0) / 2.0 / (uBtArr[i] + NiR[i]);

        avgMu[i]    = mu0[i];
        avgLm[i]    = aLm[i] / bLm[i];
        avgLnLm[i]  = gsl_sf_psi( aLm[i] ) - log( bLm[i] );
    }
}

double varLowerBound_gauss( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    gaussParameters *p = (gaussParameters*)gv->params;
    gaussStats *s = (gaussStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, *cn = iv->cn;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uBtArr = p->uBtArr, *uMuArr = p->uMuArr, *uAArr = p->uAArr, *uBArr = p->uBArr;
    double *avgLnPi = p->avgLnPi, **avgLnA = p->avgLnA;
    double *avgLm = p->avgLm, *avgLnLm = p->avgLnLm;
    double *mu0 = p->mu0, *btMu = p->btMu, *aLm = p->aLm, *bLm = p->bLm;
    double *Nii = s->Nii, **Nij = s->Nij;
    size_t n;
    int i, j;

    double lnpPi = gsl_sf_lngamma(sumUPi);
    double lnpA = 0.0;
    double lnpMuLm = 0.0;
    double lnqPi = gsl_sf_lngamma(sumUPi + 1.0);
    double lnqA = 0.0;
    double lnqMuLm = - sNo / 2.0;
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1.0) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnpMuLm += log(uBtArr[i]) / 2.0;
        lnpMuLm -= uBtArr[i] / 2.0 *( 1.0 / btMu[i] + aLm[i] / bLm[i] * pow(mu0[i] - uMuArr[i], 2.0) );
        lnpMuLm += - gsl_sf_lngamma(uAArr[i]) + uAArr[i] * log(uBArr[i]);
        lnpMuLm += (uAArr[i] - 0.5) * avgLnLm[i] - uBArr[i] * avgLm[i];

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

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

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

        lnqMuLm += log(btMu[i]) / 2.0 - gsl_sf_lngamma(aLm[i]) + aLm[i] * log(bLm[i]);
        lnqMuLm += (aLm[i] - 0.5) * avgLnLm[i] - aLm[i];
    }

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

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

    return val;
}

double varLowerBoundG_gauss( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    int sNo = gv->sNo, rNo = xns->R;
    gaussParameters *p = (gaussParameters*)gv->params;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uBtArr = p->uBtArr, *uMuArr = p->uMuArr, *uAArr = p->uAArr, *uBArr = p->uBArr;
    double *avgLnPi = p->avgLnPi, **avgLnA = p->avgLnA;
    double *avgLm = p->avgLm, *avgLnLm = p->avgLnLm;
    double *mu0 = p->mu0, *btMu = p->btMu, *aLm = p->aLm, *bLm = p->bLm;
    gaussGlobalStats *gs = (gaussGlobalStats*)ivs->stats;
    double *NiiR = gs->NiiR, **NijR = gs->NijR, *z1iR = gs->z1iR, dR = (double)xns->R;
    size_t n;
    int i, j, r;

    double lnpPi = gsl_sf_lngamma(sumUPi);
    double lnpA = 0.0;
    double lnpMuLm = 0.0;
    double lnqPi = gsl_sf_lngamma(sumUPi + dR);
    double lnqA = 0.0;
    double lnqMuLm = - sNo / 2.0;
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1.0) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnpMuLm += log(uBtArr[i]) / 2.0;
        lnpMuLm -= uBtArr[i] / 2.0 *( 1.0 / btMu[i] + aLm[i] / bLm[i] * pow(mu0[i] - uMuArr[i], 2.0) );
        lnpMuLm += - gsl_sf_lngamma(uAArr[i]) + uAArr[i] * log(uBArr[i]);
        lnpMuLm += (uAArr[i] - 0.5) * avgLnLm[i] - uBArr[i] * avgLm[i];

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

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

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

        lnqMuLm += log(btMu[i]) / 2.0 - gsl_sf_lngamma(aLm[i]) + aLm[i] * log(bLm[i]);
        lnqMuLm += (aLm[i] - 0.5) * avgLnLm[i] - aLm[i];
    }

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

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

    return val;
}

void reorderParameters_gauss( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    gaussParameters *p = (gaussParameters*)gv->params;
    gaussStats *s = (gaussStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgMu = p->avgMu, *avgLm = p->avgLm, *avgLnLm = p->avgLnLm;
    double *btMu = p->btMu, *aLm = p->aLm, *bLm = p->bLm;
    double *Ni = s->Ni;
    size_t n;
    int i, j;

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

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

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

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

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

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

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

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

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

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

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

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

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

void reorderParametersG_gauss( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    size_t dLen;
    int sNo = gv->sNo, rNo = xns->R;
    gaussParameters *p = (gaussParameters*)gv->params;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgMu = p->avgMu, *avgLm = p->avgLm, *avgLnLm = p->avgLnLm;
    double *btMu = p->btMu, *aLm = p->aLm, *bLm = p->bLm;
    size_t n;
    int i, j, r;

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

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

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

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

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

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

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

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

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

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

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

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

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

void outputGaussResults( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    gaussParameters *p = (gaussParameters*)gv->params;
    int sNo = gv->sNo;

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

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

    fprintf(logFP, "   lambda: ( %g", p->avgLm[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgLm[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, "mu, lambda, 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, %g", p->avgMu[i], p->avgLm[i], p->avgPi[i]);
            for( j = 0 ; j < sNo ; j++ )
            {   fprintf(fp, ", %g", p->avgA[j][i]);   }
            fprintf(fp, "\n");
        }
        fclose(fp);
    }

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

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

    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 outputGaussResultsG( xns, gv, ivs, logFP )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
FILE *logFP;
{
    int sNo = gv->sNo, rNo = xns->R;
    gaussParameters *p = (gaussParameters*)gv->params;
    int i, j, r;

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

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

    fprintf(logFP, "   lambda: ( %g", p->avgLm[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgLm[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.g.param%03d", xns->xn[0]->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        fprintf(fp, "mu, lambda, 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, %g", p->avgMu[i], p->avgLm[i], p->avgPi[i]);
            for( j = 0 ; j < sNo ; j++ )
            {   fprintf(fp, ", %g", p->avgA[j][i]);   }
            fprintf(fp, "\n");
        }
        fclose(fp);
    }

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

    int flag;
#ifdef OUTPUT_MAX_GAMMA
    sprintf( fn, "%s.g.maxG%03d", xns->xn[0]->name, sNo );
    flag = 0;
    if( (fp = fopen( fn, "w")) != NULL ){
        n = 0;
        do{
            flag = 1;
            for( r = 0 ; r < rNo ; r++ ){
                xnDataSet *xn = xns->xn[r];
                indVars *iv = ivs->indVars[r];

                if( r > 0 ){
                    fprintf( fp, "," );
                }
                if( n < xn->N ){
                    fprintf( fp, "%d", iv->gammaTraj[n] );
                }
                flag &= (n >= (xn->N - 1));
            }
            fprintf( fp, "\n" );
            n++;
        }while( !flag );
        fclose(fp);
    }
#endif

    sprintf( fn, "%s.g.maxS%03d", xns->xn[0]->name, sNo );
    flag = 0;
    if( (fp = fopen( fn, "w")) != NULL ){
        n = 0;
        do{
            flag = 1;
            for( r = 0 ; r < rNo ; r++ ){
                xnDataSet *xn = xns->xn[r];
                indVars *iv = ivs->indVars[r];

                if( r > 0 ){
                    fprintf( fp, "," );
                }
                if( n < xn->N ){
                    fprintf( fp, "%d", iv->stateTraj[n] );
                }
                flag &= (n >= (xn->N - 1));
            }
            fprintf( fp, "\n" );
            n++;
        }while( !flag );
        fclose(fp);
    }
}

//


#ifndef PINHOLE_H_I3RZULX9
#define PINHOLE_H_I3RZULX9

#include <cmath>
#include <gsl/gsl>
#include <sens_loc/camera_models/concepts.h>
#include <sens_loc/math/constants.h>
#include <sens_loc/math/coordinate.h>
#include <type_traits>

namespace sens_loc {

/// This namespace contains all camera and projection models and implements
/// them as necessary.
namespace camera_models {

/// This struct contains all parameters for the general pinhole camera model.
/// For more information you can check the
/// OpenCV documentation (Section 'Detailed Description').
///
/// \tparam Real floating point type that determines the precision of the
/// calculations.
/// \sa is_intrinsic_v
template <typename Real = float>
class pinhole {
  public:
    static_assert(std::is_floating_point_v<Real>);
    using real_type = Real;

    pinhole() = default;

    /// Initialize the pinhole model with it's essential parameters.
    /// \param w,h image dimensions
    /// \param fx,fy focal point
    /// \param cx,cy image center
    pinhole(int w, int h, Real fx, Real fy, Real cx, Real cy) noexcept
        : _w(w)
        , _h(h)
        , _fx(fx)
        , _fy(fy)
        , _cx(cx)
        , _cy(cy) {
        Expects(w > 0);
        Expects(h > 0);
        Expects(_fx > Real(0.));
        Expects(_fy > Real(0.));
        Expects(_cx > Real(0.));
        Expects(_cy > Real(0.));
    }

    /// Return the width of the image corresponding to this calibration.
    [[nodiscard]] int w() const noexcept { return _w; }
    /// Return the height of the image corresponding to this calibration.
    [[nodiscard]] int h() const noexcept { return _h; }
    /// Return the x-component of the focal point.
    [[nodiscard]] Real fx() const noexcept { return _fx; }
    /// Return the y-component of the focal point.
    [[nodiscard]] Real fy() const noexcept { return _fy; }
    /// Return the x-component of the image center.
    [[nodiscard]] Real cx() const noexcept { return _cx; }
    /// Return the y-component of the image center.
    [[nodiscard]] Real cy() const noexcept { return _cy; }

    template <typename Number = int>
    [[nodiscard]] math::image_coord<Real>
    transform_to_image(const math::pixel_coord<Number>& p) const noexcept;

    /// This methods calculates the inverse projection of the camera model
    /// to get the direction of the lightray for the pixel at \p p.
    ///
    /// \tparam _Real either integer or floating point value for pixel or
    /// subpixel precision
    /// \param p non-negative pixel coordinates
    /// \warning This does not respect a distortion model at all!
    /// \pre \p fx > 0
    /// \pre \p fy > 0
    /// \pre \p cx > 0
    /// \pre \p cy > 0
    /// \pre \p p1 == \p p2 == \p k1 == \p k2 == \p k3 == 0!!
    /// \post \f$\lVert result \rVert_2 = 1.\f$
    /// \returns normalized vector in camera coordinates
    /// \note if \p _Real is an integer-type the value is itself backprojected,
    /// which usually means the bottom left corner of the pixel and __NOT__
    /// its center!
    template <typename _Real = int>
    [[nodiscard]] math::sphere_coord<Real>
    pixel_to_sphere(const math::pixel_coord<_Real>& p) const noexcept;

    /// The same as \p project_to_sphere but the coordinate is already
    /// in the image frame of reference.
    /// \sa pinhole::project_to_sphere
    [[nodiscard]] math::sphere_coord<Real>
    image_to_sphere(const math::image_coord<Real>& p) const noexcept;

    /// Project points in camera coordinates to pixel coordinates.
    /// \note if the point can not be projected (\c p.z() == 0) the pixel
    /// coordinate {-1, -1} is returned.
    /// \sa pinhole::project_to_sphere
    template <typename _Real = Real>
    [[nodiscard]] math::pixel_coord<_Real>
    camera_to_pixel(const math::camera_coord<Real>& p) const noexcept;

  private:
    int  _w  = 0;    ///< width of the image
    int  _h  = 0;    ///< height of the image
    Real _fx = 0.0;  ///< x-coordinate of focal length
    Real _fy = 0.0;  ///< y-corrdiante of focal length
    Real _cx = 0.0;  ///< x-coordinate of image center
    Real _cy = 0.0;  ///< y-coordinate of image center

    Real p1 = 0.0;  ///< first order tangential distortion coefficient
    Real p2 = 0.0;  ///< second order tangential distortion coefficient

    Real k1 = 0.0;  ///< first order radial distortion coefficient
    Real k2 = 0.0;  ///< second order radial distortion coefficient
    Real k3 = 0.0;  ///< third order radial distortion coefficient
};

template <typename Real>
template <typename _Real>
math::image_coord<Real>
pinhole<Real>::transform_to_image(const math::pixel_coord<_Real>& p) const
    noexcept {
    static_assert(std::is_arithmetic_v<_Real>);
    Expects(_fx > 0.);
    Expects(_fy > 0.);
    Expects(_cx > 0.);
    Expects(_cy > 0.);
    Expects(p1 == 0.);
    Expects(p2 == 0.);
    Expects(k1 == 0.);
    Expects(k2 == 0.);
    Expects(k3 == 0.);
    Expects(p.u() >= 0.);
    Expects(p.u() < w());
    Expects(p.v() >= 0.);
    Expects(p.v() < h());
    return math::image_coord<Real>((p.u() - _cx) / _fx, (p.v() - _cy) / _fy);
}

template <typename Real>
template <typename _Real>
inline math::sphere_coord<Real>
pinhole<Real>::pixel_to_sphere(const math::pixel_coord<_Real>& p) const
    noexcept {
    static_assert(std::is_arithmetic_v<_Real>);
    return image_to_sphere(transform_to_image(p));
}

template <typename Real>
inline math::sphere_coord<Real>
pinhole<Real>::image_to_sphere(const math::image_coord<Real>& p) const
    noexcept {
    const double x = p.x();
    const double y = p.y();
    const double z = 1.;

    const double cse    = 1. + x * x + y * y;
    const double factor = 1. / std::sqrt(cse);

    using gsl::narrow_cast;
    math::sphere_coord<Real> res(narrow_cast<Real>(factor * x),
                                 narrow_cast<Real>(factor * y),
                                 narrow_cast<Real>(factor * z));

    // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
    Ensures(std::abs(res.norm() - 1.) < 0.000001);

    return res;
}
template <typename Real>
template <typename _Real>
math::pixel_coord<_Real>
pinhole<Real>::camera_to_pixel(const math::camera_coord<Real>& p) const
    noexcept {
    static_assert(std::is_arithmetic_v<_Real>);
    Expects(fx() > 0.);
    Expects(fy() > 0.);
    Expects(cx() > 0.);
    Expects(cy() > 0.);
    Expects(p1 == 0.);
    Expects(p2 == 0.);
    Expects(k1 == 0.);
    Expects(k2 == 0.);
    Expects(k3 == 0.);

    if (p.Z() == Real(0.0))
        return {_Real(-1), _Real(-1)};

    const math::image_coord<Real> i{p.X() / p.Z(), p.Y() / p.Z()};
    const auto u = gsl::narrow_cast<_Real>(fx() * i.x() + cx());
    const auto v = gsl::narrow_cast<_Real>(fy() * i.y() + cy());

    if (u < _Real(0.0) || u > gsl::narrow_cast<Real>(w()) || v < _Real(0.0) ||
        v > gsl::narrow_cast<Real>(h()))
        return {_Real(-1), _Real(-1)};

    return {u, v};
}
}  // namespace camera_models
}  // namespace sens_loc

#endif /* end of include guard: PINHOLE_H_I3RZULX9 */


/*
 * C version of Diffusive Nested Sampling (DNest4) by Brendon J. Brewer
 *
 * Yan-Rong Li, liyanrong@mail.ihep.ac.cn
 * Jun 30, 2016
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <time.h>
#include <string.h>
#include <gsl/gsl_rng.h>

#include "dnestvars.h"

/*! \file dnestpostprocess.c
 *  \brief post process the sample generated by dnest.
 */

int cmp_sample(const void *pa, const void *pb);
typedef struct
{
  double logl, tiebreaker;
  int id;
}SampleType;

/*
 * This function calculates log(exp(x1) - exp(x2)).
 */
double logdiffexp(double x1, double x2)  // x1 is larger
{
  double biggest = x1;
  double xx1 = x1 - biggest, xx2 = x2 - biggest;
  return log(exp(xx1) - exp(xx2)) + biggest;
}
/*
 * This functions calculates log(exp(x1)+...+exp(xn)).
 */
double logsumexp(double *x, int n)
{
  int j;
  double sum, max;

  max = x[0];
  for(j = 0; j < n; j++)
  {
    max = fmax(max, x[j]);
  }
  sum = 0.0;
  for(j=0; j< n; j++)
  {
    sum += exp( x[j] - max);
  }
  return log(sum) + max;
}

void postprocess(double temperature)
{
  printf("# Starts postprocess.\n");
  FILE *fp, *fp_sample;

  double **levels_orig, **sample_info, *logl;
  int *sandwhich;
  double *psample;
  int i, j;
  int num_levels, num_samples;
  char buf[BUF_MAX_LENGTH];
  int moreSample = 1;

  // read number of levels and samples
  fp = fopen(options.sampler_state_file, "r");
  if(fp == NULL)
  {
    fprintf(stderr, "# Error: Cannot open file %s.\n", options.sampler_state_file);
    exit(0);
  }
  fscanf(fp, "%d %d\n", &num_levels, &num_samples);
  fclose(fp);

  // allocate memory for levels
  levels_orig = malloc(num_levels * sizeof(double *));
  for(i=0; i< num_levels; i++)
  {
    levels_orig[i] = malloc(3 * sizeof(double));
  }

  // allocate memory for sample_info
  sample_info = malloc(num_samples * sizeof(double *));
  for(i=0; i< num_samples; i++)
  {
    sample_info[i] = malloc(3 * sizeof(double));
  }

  // allocate memory for samples
  logl = (void *)malloc(num_samples * sizeof(double));
  sandwhich = malloc(num_samples * sizeof(int));
  psample = (double *)malloc(dnest_size_of_modeltype);

  // read levels
  fp = fopen(options.levels_file, "r");
  if(fp == NULL)
  {
    fprintf(stderr, "# Error: Cannot open file %s.\n", options.levels_file);
    exit(0);
  }
  fgets(buf, BUF_MAX_LENGTH, fp);
  for(i=0; i < num_levels; i++)
  {
    if(feof(fp) != 0)
    {
      fprintf(stderr, "# Error: file %s ends at %d.\n", options.levels_file, i);
      exit(0);
    }

    fgets(buf, BUF_MAX_LENGTH, fp);

    if(sscanf(buf, "%lf %lf %lf", &levels_orig[i][0], &levels_orig[i][1], &levels_orig[i][2]) < 3)
    {
      fprintf(stderr, "# Error: Cannot read file %s.\n", options.levels_file);
      exit(0);
    }
    buf[0]='\0';  // clear up buf
  }
  fclose(fp);

  // read sample_info
  if(dnest_flag_sample_info == 0) //no need to recalculate
  {
    fp = fopen(options.sample_info_file, "r");
    if(fp == NULL)
    {
      fprintf(stderr, "# Error: Cannot open file %s.\n", options.sample_info_file);
      exit(0);
    }
    fgets(buf, BUF_MAX_LENGTH, fp);
    for(i=0; i < num_samples; i++)
    {
      if(feof(fp) != 0)
      {
        fprintf(stderr, "# Error: file %s ends at %d.\n", options.sample_info_file, i);
        exit(0);
      }
      fgets(buf, BUF_MAX_LENGTH, fp);
      if(sscanf(buf, "%lf %lf %lf", &sample_info[i][0], &sample_info[i][1], &sample_info[i][2]) < 3)
      {
        fprintf(stderr, "# Error: Cannot read file %s.\n", options.sample_info_file);
        exit(0);
      }
      buf[0]='\0';  // clear buf

      /* reset level assignment for levels larger than the maximum level numbers */
      if(sample_info[i][0] > num_levels -1)
        sample_info[i][0] = num_levels - 1;
    }
    fclose(fp);
  }
  else   //sample_info file doest not exist, need to recalculate.
  {
    fp = fopen(options.sample_info_file, "w");
    if(fp == NULL)
    {
      fprintf(stderr, "# Error: Cannot open file %s.\n", options.sample_info_file);
      exit(0);
    }
    printf("# Dnest starts to recalculate the sample info.\n");
    fprintf(fp, "# level assignment, log likelihood, tiebreaker, ID.\n");

    //read sample
    fp_sample = fopen(options.sample_file, "r");
    if(fp_sample == NULL)
    {
      fprintf(stderr, "# Error: Cannot open file %s.\n", options.sample_file);
      exit(0);
    }
    fgets(buf, BUF_MAX_LENGTH, fp_sample);

    for(i=0; i < num_samples; i++)
    {
      read_particle(fp_sample, (void *)psample);

      sample_info[i][1] = log_likelihoods_cal_initial((void *)psample, dnest_arg);
      sample_info[i][2] = dnest_rand();

      for(j=0; j<num_levels; j++)
      {
        if(sample_info[i][1] < levels_orig[j][1])
          break;
      }

      /*j=num_levels-1;  // find out the highest allowed level
      while( (sample_info[i][1] < levels_orig[j][1]) && (j>=0) )
      {
        j--;
      }*/

      sample_info[i][0] = (double)dnest_rand_int(j); // randomly assign a level [0, j-1]

      fprintf(fp, "%d %e %f %d\n", (int)sample_info[i][0], sample_info[i][1], sample_info[i][2], 1);
    }
    fclose(fp);
    fclose(fp_sample);
  }
  //tempering with a temperature
  for(i=0; i<num_samples; i++)
    logl[i] = sample_info[i][1] / temperature;

  // finding sandwhiching levels for each samples
  for(i=0; i<num_samples; i++)
  {
    sandwhich[i] = (int)sample_info[i][0];

    for(j=sandwhich[i]; j < num_levels; j++)
    {
      if( sample_info[i][1] > levels_orig[j][1] )
        sandwhich[i] = j;
    }
    //printf("%f %d\n", logl[i], sandwhich[i]);
  }

  double *logx_samples, *logp_samples, *logP_samples;
  double logx_min, logx_max, Umin, U;
  int num_samples_thisLevel;
  double *logx_samples_thisLevel;
  SampleType *logl_samples_thisLevel;

  double left, right;

  logx_samples = malloc(num_samples * sizeof(double));
  logp_samples = malloc(num_samples * sizeof(double));
  logP_samples = malloc(num_samples * sizeof(double));

  logx_samples_thisLevel = malloc(num_samples * sizeof(double));
  logl_samples_thisLevel = malloc(num_samples * sizeof(SampleType));

  for(i=0; i<num_levels; i++)
  {
    logx_max = levels_orig[i][0];
    if(i == num_levels - 1)
      logx_min = -1.0E300;
    else
      logx_min = levels_orig[i+1][0];

    Umin = exp( logx_min - logx_max);

    // finding the samples sandwhiched by this levels
    num_samples_thisLevel = 0;
    for(j=0; j<num_samples; j++)
      if( sandwhich[j] == i )
      {
        logl_samples_thisLevel[num_samples_thisLevel].logl = sample_info[j][1]; // logl
        logl_samples_thisLevel[num_samples_thisLevel].tiebreaker = sample_info[j][2]; // tiebreaker
        logl_samples_thisLevel[num_samples_thisLevel].id = j; // id

        num_samples_thisLevel++;
      }

    //printf("%d\n", num_samples_thisLevel);

    for(j=0; j<num_samples_thisLevel; j++)
    {
      U = Umin + (1.0 - Umin) * ( 1.0/(1.0 + num_samples_thisLevel)
           + ( 1.0 - 2.0/(1.0 + num_samples_thisLevel) ) * (num_samples_thisLevel-1 - j)/(num_samples_thisLevel - 1.0) );
      logx_samples_thisLevel[j] = logx_max + log(U);
    }

    qsort(logl_samples_thisLevel, num_samples_thisLevel, sizeof(SampleType), cmp_sample);

    //printf("%f %f %d %f\n", logl_samples_thisLevel[0].logl, logl_samples_thisLevel[0].tiebreaker, logl_samples_thisLevel[0].id, logx_samples_thisLevel[0]);
    //printf("%f %f %d %f\n", logl_samples_thisLevel[1].logl, logl_samples_thisLevel[1].tiebreaker, logl_samples_thisLevel[1].id, logx_samples_thisLevel[1]);

    for(j = 0; j<num_samples_thisLevel; j++)
    {
      if(j != num_samples_thisLevel - 1)
        left = logx_samples_thisLevel[j+1];
      else if (i == num_levels - 1)
        left = -1.0E300;
      else
        left = levels_orig[i+1][0];

      if( j!= 0)
        right = logx_samples_thisLevel[j-1];
      else
        right = levels_orig[i][0];

      //printf("%e %e %e\n", right, left, logdiffexp(right, left));

      logx_samples[logl_samples_thisLevel[j].id] = logx_samples_thisLevel[j];
      logp_samples[logl_samples_thisLevel[j].id] = log(0.5)  + logdiffexp(right, left);
    }
  }

  double sum, max, logz_estimates, H_estimates, ESS;

  sum = logsumexp(logp_samples, num_samples);
  for(j = 0; j < num_samples; j++)
  {
    logp_samples[j] -= sum;
    //logP_samples[j] = logp_samples[j] + sample_info[j][1];
    logP_samples[j] = logp_samples[j] + logl[j];
  }

  logz_estimates = logsumexp(logP_samples, num_samples);

  H_estimates = -logz_estimates;
  ESS = 0.0;
  for(j=0; j<num_samples; j++)
  {
    logP_samples[j] -= logz_estimates;
    //H_estimates += exp(logP_samples[j]) * sample_info[j][1];
    H_estimates += exp(logP_samples[j]) * logl[j];
    ESS += -logP_samples[j]*exp(logP_samples[j]);
  }
  ESS = exp(ESS);

  printf("log(Z) = %f\n", logz_estimates);
  printf("H = %f\n", H_estimates);
  printf("Effective sample size = %f\n", ESS);

  post_logz = logz_estimates;

  // resample to uniform weight

  int num_ps = moreSample*ESS;
  void *posterior_sample;
  double *posterior_sample_info;
  int *posterior_sample_idx;
  int which;

  const gsl_rng_type * dnest_post_gsl_T;
  gsl_rng * dnest_post_gsl_r;

  dnest_post_gsl_T = (gsl_rng_type *) gsl_rng_default;
  dnest_post_gsl_r = gsl_rng_alloc (dnest_post_gsl_T);
#ifndef Debug
  gsl_rng_set(dnest_post_gsl_r, time(NULL));
#else
  gsl_rng_set(dnest_post_gsl_r, 8888);
  printf("# debugging, random seed %d\n", 8888);
#endif

  posterior_sample = malloc(num_ps * dnest_size_of_modeltype);
  posterior_sample_info = malloc(num_ps * sizeof(double));
  posterior_sample_idx = malloc(num_ps * sizeof(int)); // flag for which particle to save

  max = logP_samples[0];
  for(j=0; j<num_samples; j++)
    max = fmax(logP_samples[j], max);
  for(j=0; j<num_samples; j++)
    logP_samples[j] -= max;

  for(j=0; j<num_ps; j++)
  {
    while(true)
    {
      which =  gsl_rng_uniform_int(dnest_post_gsl_r, num_samples);
      if(log(gsl_rng_uniform(dnest_post_gsl_r)) < logP_samples[which])
      {
        posterior_sample_info[j] = logl[which];
        posterior_sample_idx[j]  = which;  // sample this particle
        break;
      }
    }
  }

  // read sample and pick out selected particles

  fp_sample = fopen(options.sample_file, "r");
  if(fp_sample == NULL)
  {
    fprintf(stderr, "# Error: Cannot open file %s.\n", options.sample_file);
    exit(0);
  }
  fgets(buf, BUF_MAX_LENGTH, fp_sample);
  for(i=0; i < num_samples; i++)
  {

    read_particle(fp_sample, (void *)psample);

    for(j=0; j < num_ps; j++)
    {
      if(posterior_sample_idx[j] == i)
      {
        memcpy(posterior_sample+j*dnest_size_of_modeltype, (void *)psample, dnest_size_of_modeltype);
      }
    }
    //printf("%f %f %f\n", sample[i].params[0], sample[i].params[1], sample[i].params[2]);
  }
  fclose(fp_sample);

  //save posterior sample
  fp = fopen(options.posterior_sample_file, "w");
  if(fp == NULL)
  {
    fprintf(stderr, "# Error: Cannot open file %s.\n", options.posterior_sample_file);
    exit(0);
  }
  fprintf(fp, "# %d\n", num_ps);

  for(i=0; i<num_ps; i++)
  {
    print_particle(fp, posterior_sample + i*dnest_size_of_modeltype, dnest_arg);
  }
  fclose(fp);

  //save posterior sample information
  fp = fopen(options.posterior_sample_info_file, "w");
  if(fp == NULL)
  {
    fprintf(stderr, "# Error: Cannot open file %s.\n", options.posterior_sample_info_file);
    exit(0);
  }
  fprintf(fp, "# %d\n", num_ps);
  for(i=0; i<num_ps; i++)
  {
    fprintf(fp, "%e\n", posterior_sample_info[i]);
  }
  fclose(fp);

  for(i=0; i<num_levels; i++)
   free(levels_orig[i]);
  free(levels_orig);

  for(i=0; i<num_samples; i++)
    free(sample_info[i]);
  free(sample_info);
  free(logl);

  free(logx_samples);
  free(logx_samples_thisLevel);
  free(logp_samples);
  free(logP_samples);
  free(logl_samples_thisLevel);
  free(sandwhich);
  free(psample);
  free(posterior_sample);
  free(posterior_sample_info);
  free(posterior_sample_idx);

  gsl_rng_free(dnest_post_gsl_r);

  printf("# Ends dnest postprocess.\n");
}

int cmp_sample(const void *pa, const void *pb)
{
  SampleType *a = (SampleType *)pa;
  SampleType *b = (SampleType *)pb;

  // in acesending order
  if(a->logl > b->logl)
    return true;
  if( a->logl == b->logl && a->tiebreaker > b->tiebreaker)
    return true;

  return false;
}


//===--- Sudoku/Board.h                                                 ---===//
//
// Data container for sudoku puzzles
//===----------------------------------------------------------------------===//
//
// accepts only squared dimensions
// gives full, row, column and block access
// no processing functionality
//
//===----------------------------------------------------------------------===//
#pragma once

#include "Board_Iterators.h"
#include "Board_Section.h"
#include "Location.h"
#include "Location_Utilities.h"
#include "Size.h"
#include "exceptions.h"

#include <gsl/gsl>

#include <array>
#include <initializer_list>

#include <algorithm>

#include "Board.fwd.h" // Forward declarations

#include <cassert>

namespace Sudoku
{
template<typename T, int N = 3>
class Board
{
	static_assert(N > 1, "Board.h base_size value too small");
	static constexpr auto Size = size_t{full_size<N>};

	using index       = gsl::index;
	using Location    = ::Sudoku::Location<N>;
	using Row         = Board_Section::Row<T, N>;
	using const_Row   = Board_Section::const_Row<T, N>;
	using Col         = Board_Section::Col<T, N>;
	using const_Col   = Board_Section::const_Col<T, N>;
	using Block       = Board_Section::Block<T, N>;
	using const_Block = Board_Section::const_Block<T, N>;

public:
	using value_type             = T;
	using size_type              = size_t;
	using difference_type        = std::ptrdiff_t;
	using reference              = value_type&;
	using const_reference        = value_type const&;
	using pointer                = value_type*;
	using const_pointer          = value_type const*;
	using iterator               = Board_iterator<T, N>;
	using const_iterator         = const_Board_iterator<T, N>;
	using reverse_iterator       = reverse_Board_iterator<T, N>;
	using const_reverse_iterator = const_reverse_Board_iterator<T, N>;

	constexpr Board() noexcept;
	explicit constexpr Board(const T& default_value);
	// NOLINTNEXTLINE(google-explicit-constructor, hicpp-explicit-conversions)
	constexpr Board(std::array<T, Size> const& list) // NOLINT(runtime/explicit)
		: board_(list)
	{
	}
	Board(std::initializer_list<T>); // construct from initializer_list

	void clear();

	// Query properties
	[[nodiscard]] static constexpr size_t size() noexcept { return Size; }
	[[nodiscard]] static constexpr size_t max_size() noexcept { return size(); }
	[[nodiscard]] static constexpr bool empty() noexcept { return false; }
	[[nodiscard]] bool operator==(const Board&) const;

	// Element access
	[[nodiscard]] constexpr T& front() noexcept { return board_.front(); }
	[[nodiscard]] constexpr T const& front() const noexcept
	{
		return board_.front();
	}
	[[nodiscard]] constexpr T& back() noexcept { return board_.back(); }
	[[nodiscard]] constexpr T const& back() const noexcept
	{
		return board_.back();
	}
	// Checked
	constexpr T& at(Location);
	[[nodiscard]] constexpr T const& at(Location) const;
	constexpr T& at(index row, index col);
	[[nodiscard]] constexpr T const& at(index row, index col) const;
	[[deprecated]] T& at(index elem);
	[[deprecated, nodiscard]] const T& at(index elem) const;
	// Unchecked
	constexpr T& operator[](Location) noexcept;
	constexpr T const& operator[](Location) const noexcept;

	// Element Selection Operator (using a proxy object)
	//   usable as [row][col] where col is processed by the (const_)Row
	Row operator[](index row_id) noexcept { return row(row_id); }
	constexpr const_Row operator[](index row_id) const noexcept
	{
		return row(row_id);
	}

	// Iterators
	constexpr iterator begin() noexcept;
	constexpr iterator end() noexcept;
	[[nodiscard]] constexpr const_iterator cbegin() const noexcept;
	[[nodiscard]] constexpr const_iterator cend() const noexcept;
	[[nodiscard]] constexpr const_iterator begin() const noexcept
	{
		return cbegin();
	}
	[[nodiscard]] constexpr const_iterator end() const noexcept
	{
		return cend();
	}
	constexpr reverse_iterator rbegin() noexcept;
	constexpr reverse_iterator rend() noexcept;
	[[nodiscard]] constexpr const_reverse_iterator crbegin() const noexcept;
	[[nodiscard]] constexpr const_reverse_iterator crend() const noexcept;
	[[nodiscard]] constexpr const_reverse_iterator rbegin() const noexcept
	{
		return crbegin();
	}
	[[nodiscard]] constexpr const_reverse_iterator rend() const noexcept
	{
		return crend();
	}

	// Sections
	// clang-format off
	Row row(index id) noexcept         { return Row(*this, id); }
	[[nodiscard]] constexpr const_Row   row(index id) const noexcept
									   { return const_Row(*this, id); }
	Row row(Location loc) noexcept     { return Row(*this, loc); }
	[[nodiscard]] constexpr const_Row   row(Location loc) const noexcept
									   { return const_Row(*this, loc); }
	Col col(index id) noexcept         { return Col(*this, id); }
	[[nodiscard]] constexpr const_Col   col(index id) const noexcept
									   { return const_Col(*this, id); }
	Col col(Location loc) noexcept     { return Col(*this, loc); }
	[[nodiscard]] constexpr const_Col   col(Location loc) const noexcept
									   { return const_Col(*this, loc); }
	Block block(index id) noexcept     { return Block(*this, id); }
	[[nodiscard]] constexpr const_Block block(index id) const noexcept
									   { return const_Block(*this, id); }
	Block block(Location loc) noexcept { return Block(*this, loc); }
	[[nodiscard]] constexpr const_Block block(Location loc) const noexcept
									   { return const_Block(*this, loc); }
	// clang-format on

private:
	std::array<T, Size> board_{};
}; // class Board

//===--- free-functions ---------------------------------------------------===//

template<typename T, int N>
constexpr bool operator!=(Board<T, N> const& left, Board<T, N> const& right)
{
	return !(left == right);
}

//===----------------------------------------------------------------------===//
// Board - member-functions
//===----------------------------------------------------------------------===//
// Board - Constructors
template<typename T, int N>
constexpr Board<T, N>::Board() noexcept
{
	valid_dimensions<N>();
}

template<typename T, int N>
constexpr Board<T, N>::Board(const T& default_value)
{
	valid_dimensions<N>();
	board_.fill(default_value);
}

template<typename T, int N>
Board<T, N>::Board(std::initializer_list<T> list)
{
	valid_dimensions<N>();
	if (list.size() != size_t{full_size<N>})
		throw std::length_error{"Invalid length initializer_list"};
	std::copy(std::begin(list), std::end(list), std::begin(board_));
}

//===----------------------------------------------------------------------===//
// properties
template<typename T, int N>
bool Board<T, N>::operator==(const Board& other) const
{
	return board_ == other.board_;
}

//===----------------------------------------------------------------------===//
// Board - Operations
template<typename T, int N>
inline void Board<T, N>::clear()
{ // all elements to the empty value
	board_.fill(T{});
}

//===----------------------------------------------------------------------===//
// Board - element access
template<typename T, int N>
constexpr T& Board<T, N>::at(const Location loc)
{
	if (!is_valid<N>(loc))
	{
		throw error::invalid_Location{"Board::at(Location)"};
	}
	// NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
	return board_[gsl::narrow_cast<size_t>(loc.element())];
}

template<typename T, int N>
constexpr T const& Board<T, N>::at(const Location loc) const
{
	if (!is_valid<N>(loc))
	{
		throw error::invalid_Location{"Board::at(Location) const"};
	}
	// NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
	return board_[gsl::narrow_cast<size_t>(loc.element())];
}

template<typename T, int N>
constexpr T& Board<T, N>::at(const index row, const index col)
{
	if (!is_valid_size<N>(row, col))
	{
		throw error::invalid_Location{"Board::at(int row, col)"}; // <stdexcept>
	}
	// NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
	return board_[gsl::narrow_cast<size_t>(Location(row, col).element())];
}

template<typename T, int N>
constexpr T const& Board<T, N>::at(const index row, const index col) const
{
	if (!is_valid_size<N>(row, col))
	{
		throw error::invalid_Location{"Board::at(row, col) const"};
	}
	// NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
	return board_[gsl::narrow_cast<size_t>(Location(row, col).element())];
}

// deprecated
template<typename T, int N>
T& Board<T, N>::at(const index elem)
{
	if (!is_valid_size<N>(elem))
	{
		throw error::invalid_Location{"Board::at(int)"};
	}
	return board_.at(gsl::narrow_cast<size_t>(elem));
}

// deprecated
template<typename T, int N>
const T& Board<T, N>::at(const index elem) const
{
	if (!is_valid_size<N>(elem))
	{
		throw error::invalid_Location{"Board::at(int) const"};
	}
	return board_.at(gsl::narrow_cast<size_t>(elem));
}

template<typename T, int N>
constexpr T& Board<T, N>::operator[](const Location loc) noexcept
{
	// NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
	return board_[gsl::narrow_cast<size_t>(loc.element())];
}

template<typename T, int N>
constexpr T const& Board<T, N>::operator[](const Location loc) const noexcept
{
	// NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
	return board_[gsl::narrow_cast<size_t>(loc.element())];
}

//===----------------------------------------------------------------------===//
// Board - Iterators
template<typename T, int N>
constexpr typename Board<T, N>::iterator Board<T, N>::begin() noexcept
{
	return Board_iterator<T, N>(gsl::not_null<Board*>{this});
}
template<typename T, int N>
constexpr typename Board<T, N>::iterator Board<T, N>::end() noexcept
{
	return Board_iterator<T, N>(
		gsl::not_null<Board*>{this}, Location{full_size<N>});
}
template<typename T, int N>
constexpr typename Board<T, N>::const_iterator
	Board<T, N>::cbegin() const noexcept
{
	return const_Board_iterator<T, N>(gsl::not_null<Board const*>{this});
}
template<typename T, int N>
constexpr typename Board<T, N>::const_iterator
	Board<T, N>::cend() const noexcept
{
	return const_Board_iterator<T, N>(
		gsl::not_null<Board const*>{this}, Location{full_size<N>});
}
template<typename T, int N>
constexpr typename Board<T, N>::reverse_iterator Board<T, N>::rbegin() noexcept
{
	return reverse_Board_iterator<T, N>(gsl::not_null<Board*>{this});
}
template<typename T, int N>
constexpr typename Board<T, N>::reverse_iterator Board<T, N>::rend() noexcept
{
	return reverse_Board_iterator<T, N>(
		gsl::not_null<Board*>{this}, Location{-1});
}
template<typename T, int N>
constexpr typename Board<T, N>::const_reverse_iterator
	Board<T, N>::crbegin() const noexcept
{
	return const_reverse_Board_iterator<T, N>(
		gsl::not_null<Board const*>{this});
}
template<typename T, int N>
constexpr typename Board<T, N>::const_reverse_iterator
	Board<T, N>::crend() const noexcept
{
	return const_reverse_Board_iterator<T, N>(
		gsl::not_null<Board const*>{this}, Location{-1});
}

} // namespace Sudoku