/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** 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.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/

/*

Performs y = ( G^T G )^{-1} ( G^T K G ) ( G^T G )^{-1} x
This preconditioner is defined as a PC rather than a MatShell
as we can define and compute the inverse easily without the
need of an additional KSP. Compare this to Q_S = G^T Q_K^-1 G,
where Q_S^-1 can only be performed implicitly via an iterative
method.

*/
#if 1

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <petsc.h>
#include <petscmat.h>
#include <petscvec.h>
#include <petscksp.h>
#include <petscpc.h>

#include "common-driver-utils.h"

#include <petscversion.h>
#if ( (PETSC_VERSION_MAJOR >= 3) && (PETSC_VERSION_MINOR >=3) )
  #if (PETSC_VERSION_MINOR >=6)
     #include "petsc/private/pcimpl.h"
     #include "petsc/private/kspimpl.h"
  #else
     #include "petsc-private/pcimpl.h"
     #include "petsc-private/kspimpl.h"
  #endif
#else
  #include "private/pcimpl.h"
  #include "private/kspimpl.h"
#endif

#include "pc_GtKG.h"

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

#define PCTYPE_GtKG "gtkg"

/* private data */

typedef struct {
	Mat         G, K;   /* G \in [M x N], K \in [M x M] */
	Mat         M;      /* the velocity mass matrix */
	Vec         inv_diag_M;
	PetscTruth  form_GtG; /* don't allow anything else yet */
	Mat         GtG;
	KSP         ksp;
	Vec         s,t,X;  /* s \in [M], t \in [N], X \in [M] */
	PetscTruth  monitor_activated;
	PetscTruth  monitor_rhs_consistency;
} _PC_GtKG;

typedef _PC_GtKG* PC_GtKG;

/* private prototypes */
PetscErrorCode BSSCR_PCApply_GtKG( PC pc, Vec x, Vec y );
PetscErrorCode BSSCR_PCApplyTranspose_GtKG( PC pc, Vec x, Vec y );
PetscErrorCode BSSCR_BSSCR_PCApply_GtKG_diagonal_scaling( PC pc, Vec x, Vec y );
PetscErrorCode BSSCR_BSSCR_PCApplyTranspose_GtKG_diagonal_scaling( PC pc, Vec x, Vec y );
PetscErrorCode BSSCR_PCSetUp_GtKG( PC pc );

PetscErrorCode BSSCR_pc_warn( PC pc, const char func_name[] )
{
	const PCType type;
	PCGetType( pc, &type );

	if( strcmp(type,PCTYPE_GtKG)!=0 ) {
		printf("Warning(%s): PC type (%s) should be gtkg \n",func_name, type );
		PetscFunctionReturn(0);
	}
	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_pc_error( PC pc, const char func_name[] )
{
	const PCType type;
	PCGetType( pc, &type );

	if( strcmp(type,PCTYPE_GtKG)!=0 ) {
		printf("Error(%s): PC type (%s) should be gtkg \n",func_name, type );
		PetscFinalize();
		exit(0);
	}
	PetscFunctionReturn(0);
}

void BSSCR_get_number_nonzeros_AIJ( Mat A, PetscInt *nnz )
{
	MatInfo info;

	MatGetInfo( A, MAT_GLOBAL_SUM, &info );
	*nnz = info.nz_used;

}

/*
I should not modify setup called!!
This is handled via petsc.
*/
PetscErrorCode BSSCR_PCSetUp_GtKG( PC pc )
{
	PC_GtKG    ctx = (PC_GtKG)pc->data;
	PetscReal  fill;
	Mat        Ident;
	Vec        diag;
	PetscInt   M,N, m,n;
	MPI_Comm   comm;
	PetscInt   nnz_I, nnz_G;
	const MatType mtype;
	const char *prefix;
	PetscTruth wasSetup;

	if( ctx->K == PETSC_NULL ) {
		Stg_SETERRQ( PETSC_ERR_SUP, "gtkg: K not set" );
	}
	if( ctx->G == PETSC_NULL ) {
		Stg_SETERRQ( PETSC_ERR_SUP, "gtkg: G not set" );
	}

	PetscObjectGetComm( (PetscObject)ctx->K, &comm );

	/* Check for existence of objects and trash any which exist */
	if( ctx->form_GtG == PETSC_TRUE && ctx->GtG != PETSC_NULL ) {
		Stg_MatDestroy(&ctx->GtG );
		ctx->GtG = PETSC_NULL;
	}

	if( ctx->s != PETSC_NULL ) {
		Stg_VecDestroy(&ctx->s );
		ctx->s = PETSC_NULL;
	}
	if( ctx->X != PETSC_NULL ) {
		Stg_VecDestroy(&ctx->X );
		ctx->X = PETSC_NULL;
	}
	if( ctx->t != PETSC_NULL ) {
		Stg_VecDestroy(&ctx->t );
		ctx->t = PETSC_NULL;
	}
	if( ctx->inv_diag_M != PETSC_NULL ) {
		Stg_VecDestroy(&ctx->inv_diag_M );
		ctx->inv_diag_M = PETSC_NULL;
	}

	/* Create vectors */
	MatGetVecs( ctx->K, &ctx->s, &ctx->X );
	MatGetVecs( ctx->G, &ctx->t, PETSC_NULL );

	if( ctx->M != PETSC_NULL ) {
		MatGetVecs( ctx->K, &ctx->inv_diag_M, PETSC_NULL );
		MatGetDiagonal( ctx->M, ctx->inv_diag_M );
		VecReciprocal( ctx->inv_diag_M );

		/* change the pc_apply routines */
		pc->ops->apply          = BSSCR_BSSCR_PCApply_GtKG_diagonal_scaling;
		pc->ops->applytranspose = BSSCR_BSSCR_PCApplyTranspose_GtKG_diagonal_scaling;
	}

	/* Assemble GtG */
	MatGetSize( ctx->G, &M, &N );
	MatGetLocalSize( ctx->G, &m, &n );

	MatGetVecs( ctx->G, PETSC_NULL, &diag );
	VecSet( diag, 1.0 );

	MatCreate( comm, &Ident );
	MatSetSizes( Ident, m,m , M, M );
#if (((PETSC_VERSION_MAJOR==3) && (PETSC_VERSION_MINOR>=3)) || (PETSC_VERSION_MAJOR>3) )
        MatSetUp(Ident);
#endif

	MatGetType( ctx->G, &mtype );
	MatSetType( Ident, mtype );

	if( ctx->M == PETSC_NULL ) {
		MatDiagonalSet( Ident, diag, INSERT_VALUES );
	}
	else {
		MatDiagonalSet( Ident, ctx->inv_diag_M, INSERT_VALUES );
	}

	BSSCR_get_number_nonzeros_AIJ( Ident, &nnz_I );
	BSSCR_get_number_nonzeros_AIJ( ctx->G, &nnz_G );
	//fill = 1.0;
	/*
	Not sure the best way to estimate the fill factor.
	GtG is a laplacian on the pressure space.
	This might tell us something useful...
	*/
	fill = (PetscReal)(nnz_G)/(PetscReal)( nnz_I );
	MatPtAP( Ident, ctx->G, MAT_INITIAL_MATRIX, fill, &ctx->GtG );

	Stg_MatDestroy(&Ident);
	Stg_VecDestroy(&diag );

	Stg_KSPSetOperators( ctx->ksp, ctx->GtG, ctx->GtG, SAME_NONZERO_PATTERN );

	if (!pc->setupcalled) {
		wasSetup = PETSC_FALSE;

		PCGetOptionsPrefix( pc,&prefix );
		KSPSetOptionsPrefix( ctx->ksp, prefix );
		KSPAppendOptionsPrefix( ctx->ksp, "pc_gtkg_" ); /* -pc_GtKG_ksp_type <type>, -ksp_GtKG_pc_type <type> */
	}
	else {
		wasSetup = PETSC_TRUE;
	}

//	if (!wasSetup && pc->setfromoptionscalled) {
	if (!wasSetup) {
		KSPSetFromOptions(ctx->ksp);
	}

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCDestroy_GtKG( PC pc )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	if( ctx == PETSC_NULL ) {	PetscFunctionReturn(0); }

	if( ctx->form_GtG == PETSC_TRUE && ctx->GtG != PETSC_NULL ) {
		Stg_MatDestroy(&ctx->GtG );
	}
	if( ctx->ksp != PETSC_NULL ) {	Stg_KSPDestroy(&ctx->ksp );		}
	if( ctx->s != PETSC_NULL ) {	Stg_VecDestroy(&ctx->s );		}
	if( ctx->X != PETSC_NULL ) {	Stg_VecDestroy(&ctx->X );		}
	if( ctx->t != PETSC_NULL ) {	Stg_VecDestroy(&ctx->t );		}
	if( ctx->inv_diag_M != PETSC_NULL ) {
		Stg_VecDestroy(&ctx->inv_diag_M );
		ctx->inv_diag_M = PETSC_NULL;
	}

	PetscFree( ctx );

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCView_GtKG( PC pc, PetscViewer viewer )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	PetscViewerASCIIPushTab(viewer); //1

	if( ctx->M == PETSC_NULL ) {
		PetscViewerASCIIPrintf( viewer, "gtkg: Standard \n" );
	}else {
		PetscViewerASCIIPrintf( viewer, "gtkg: Least Squares Commutator \n" );
	}

	PetscViewerASCIIPrintf( viewer, "gtkg-ksp \n" );
	PetscViewerASCIIPrintf(viewer,"---------------------------------\n");
	PetscViewerASCIIPushTab(viewer);
		KSPView( ctx->ksp, viewer );
	PetscViewerASCIIPopTab(viewer);
	PetscViewerASCIIPrintf(viewer,"---------------------------------\n");

	PetscViewerASCIIPopTab(viewer); //1

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_Lp_monitor( KSP ksp, PetscInt index )
{
	PetscInt max_it;
	PetscReal rnorm;
	KSPConvergedReason reason;

	KSPGetIterationNumber( ksp, &max_it );
	KSPGetResidualNorm( ksp, &rnorm );
	KSPGetConvergedReason( ksp, &reason );
	if (ksp->reason > 0) {
		PetscPrintf(((PetscObject)ksp)->comm,"\t<Lp(%d)>: Linear solve converged. its.=%.4d ; |r|=%5.5e ; Reason=%s\n",
				index, max_it, rnorm, KSPConvergedReasons[reason] );
	} else {
		PetscPrintf(((PetscObject)ksp)->comm,"\t<Lp(%d)>: Linear solve did not converge. its.=%.4d ; |r|=%5.5e ; Reason=%s\n",
				index, max_it, rnorm, KSPConvergedReasons[reason]);
	}

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCBFBTSubKSPMonitor( KSP ksp, PetscInt index, PetscLogDouble time )
{
        PetscInt max_it;
        PetscReal rnorm;
        KSPConvergedReason reason;

        KSPGetIterationNumber( ksp, &max_it );
        KSPGetResidualNorm( ksp, &rnorm );
        KSPGetConvergedReason( ksp, &reason );
        PetscPrintf(((PetscObject)ksp)->comm,"    PCBFBTSubKSP (%d): %D Residual norm; r0 %12.12e, r %12.12e: Reason %s: Time %5.5e \n",
		index, max_it, ksp->rnorm0, rnorm, KSPConvergedReasons[reason], time );

        PetscFunctionReturn(0);
}

/*
Checks rhs of Lp systems is in the null space of Lp, i.e. {rhs} \centerdot {null_space} = 0
Since Lp should contain the null space {1}, we just check the \sum_i rhs_i = 0
*/
PetscErrorCode BSSCRBSSCR_Lp_monitor_check_rhs_consistency( KSP ksp, Vec rhs, PetscInt index )
{
	PetscScalar dot;
#if 0
	Vec one;

	VecDuplicate( rhs, &one );
	VecSet( one, 1.0 );
	VecDot( rhs, one, &dot );
	if( PetscAbsReal(dot) > 1.0e-8 ) {
		PetscPrintf(ksp->comm,"    ($D) Lp z = r: ******* WARNING ******* RHS is not consistent. {b}.{1} = %5.5e \n", index, dot );
	}

	Stg_VecDestroy(&one );
	PetscFunctionReturn(0);
#endif
#if 0
	VecSum( rhs, &dot );
	if( PetscAbsReal(dot) > 1.0e-8 ) {
		PetscPrintf(((PetscObject)ksp)->comm,"    (%D) Lp z = r: ******* WARNING ******* RHS is not consistent. {b}.{1} = %5.5e \n",
				index, dot );
		BSSCR_VecRemoveConstNullspace( rhs, PETSC_NULL );
	}
#endif
	PetscFunctionReturn(0);
}

/*
Performs y <- S^{-1} x
S^{-1} = ( G^T G )^{-1} G^T K G ( G^T G )^{-1}
*/
PetscErrorCode BSSCR_PCApply_GtKG( PC pc, Vec x, Vec y )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	KSP ksp;
	Mat K, G;
	Vec s,t,X;
	PetscLogDouble t0,t1;

	ksp = ctx->ksp;
	K = ctx->K;
	G = ctx->G;
	s = ctx->s;
	t = ctx->t;
	X = ctx->X;

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,x,1);		}
	PetscGetTime(&t0);
	KSPSolve( ksp, x, t ); /* t <- GtG_inv x */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {	BSSCR_PCBFBTSubKSPMonitor(ksp,1,(t1-t0));		}

	MatMult( G, t, s ); /* s <- G t */
	MatMult( K, s, X ); /* X <- K s */
	MatMultTranspose( G, X, t ); /* t <- Gt X */

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,t,2);		}
	PetscGetTime(&t0);
	KSPSolve( ksp, t, y ); /* y <- GtG_inv t */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {	BSSCR_PCBFBTSubKSPMonitor(ksp,2,(t1-t0));		}

	PetscFunctionReturn(0);
}

/* Need to check this one if correct */
/*
S^{-1} = ( G^T G )^{-1} G^T K G ( G^T G )^{-1}
       = A C A
S^{-T} = A^T (A C)^T
       = A^T C^T A^T, but A = G^T G which is symmetric
       = A C^T A
       = A G^T ( G^T K )^T A
       = A G^T K^T G A

*/
PetscErrorCode BSSCR_PCApplyTranspose_GtKG( PC pc, Vec x, Vec y )
{

	PC_GtKG ctx = (PC_GtKG)pc->data;

	KSP ksp;
	Mat K, G;
	Vec s,t,X;
	PetscLogDouble t0,t1;

	ksp = ctx->ksp;
	K = ctx->K;
	G = ctx->G;
	s = ctx->s;
	t = ctx->t;
	X = ctx->X;

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,x,1);		}
	PetscGetTime(&t0);
	KSPSolve( ksp, x, t ); /* t <- GtG_inv x */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {	BSSCR_PCBFBTSubKSPMonitor(ksp,1,(t1-t0));		}

	MatMult( G, t, s ); /* s <- G t */
	MatMultTranspose( K, s, X ); /* X <- K^T s */
	MatMultTranspose( G, X, t ); /* t <- Gt X */

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,t,2);		}
	PetscGetTime(&t0);
	KSPSolve( ksp, t, y ); /* y <- GtG_inv t */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {	BSSCR_PCBFBTSubKSPMonitor(ksp,2,(t1-t0));		}

	PetscFunctionReturn(0);
}

/*
Performs y <- S^{-1} x
S^{-1} = ( G^T Di G )^{-1} G^T Di K Di G ( G^T Di G )^{-1}
where Di = diag(M)^{-1}
*/

PetscErrorCode BSSCR_BSSCR_PCApply_GtKG_diagonal_scaling( PC pc, Vec x, Vec y )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	KSP ksp;
	Mat K, G;
	Vec s,t,X,di;

	ksp = ctx->ksp;
	K = ctx->K;
	G = ctx->G;
	di = ctx->inv_diag_M;
	s = ctx->s;
	t = ctx->t;
	X = ctx->X;

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,x,1);		}
	KSPSolve( ksp, x, t );       /* t <- GtG_inv x */
	if (ctx->monitor_activated) {	BSSCR_Lp_monitor(ksp,2);		}

	MatMult( G, t, s );          /* s <- G t */
	VecPointwiseMult( s, s,di ); /* s <- s * di */

	MatMult( K, s, X );          /* X <- K s */
	VecPointwiseMult( X, X,di ); /* X <- X * di */

	MatMultTranspose( G, X, t ); /* t <- Gt X */

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,t,2);		}
	KSPSolve( ksp, t, y );       /* y <- GtG_inv t */
	if (ctx->monitor_activated) {	BSSCR_Lp_monitor(ksp,2);		}

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_BSSCR_PCApplyTranspose_GtKG_diagonal_scaling( PC pc, Vec x, Vec y )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	KSP ksp;
	Mat K, G;
	Vec s,t,X,di;

	ksp = ctx->ksp;
	K = ctx->K;
	G = ctx->G;
	di = ctx->inv_diag_M;
	s = ctx->s;
	t = ctx->t;
	X = ctx->X;

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,x,1);		}
	KSPSolve( ksp, x, t );       /* t <- GtG_inv x */
	if (ctx->monitor_activated) {	BSSCR_Lp_monitor(ksp,1);		}

	MatMult( G, t, s );          /* s <- G t */
	VecPointwiseMult( s, s,di ); /* s <- s * di */

	MatMultTranspose( K, s, X ); /* X <- K^T s */
	VecPointwiseMult( X, X,di ); /* X <- X * di */

	MatMultTranspose( G, X, t ); /* t <- Gt X */

	if (ctx->monitor_rhs_consistency) {		BSSCRBSSCR_Lp_monitor_check_rhs_consistency(ksp,t,2);		}
	KSPSolve( ksp, t, y );       /* y <- GtG_inv t */
	if (ctx->monitor_activated) {	BSSCR_Lp_monitor(ksp,2);		}

	PetscFunctionReturn(0);
}

/*
Only the options related to GtKG should be set here.
*/
PetscErrorCode BSSCR_PCSetFromOptions_GtKG( PC pc )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;
	PetscTruth ivalue, flg;

	PetscOptionsGetTruth( PETSC_NULL, "-pc_gtkg_monitor", &ivalue, &flg );
	if( flg==PETSC_TRUE ) {
		ctx->monitor_activated = ivalue;
	}

	PetscOptionsGetTruth( PETSC_NULL, "-pc_gtkg_monitor_rhs_consistency", &ivalue, &flg );
	if( flg==PETSC_TRUE ) {
		ctx->monitor_rhs_consistency = ivalue;
	}

	PetscFunctionReturn(0);
}

/* ---- Exposed functions ---- */

PetscErrorCode BSSCR_PCCreate_GtKG( PC pc )
{
	PC_GtKG         pc_data;
	PetscErrorCode  ierr;

	/* create memory for ctx */
	ierr = Stg_PetscNew( _PC_GtKG,&pc_data);CHKERRQ(ierr);

	/* init ctx */
	pc_data->K   = PETSC_NULL;
	pc_data->G   = PETSC_NULL;
	pc_data->M   = PETSC_NULL;
	pc_data->GtG = PETSC_NULL;
	pc_data->form_GtG          = PETSC_TRUE;
	pc_data->ksp               = PETSC_NULL;
	pc_data->monitor_activated = PETSC_FALSE;
	pc_data->monitor_rhs_consistency = PETSC_FALSE;

	pc_data->s = PETSC_NULL;
	pc_data->t = PETSC_NULL;
	pc_data->X = PETSC_NULL;
	pc_data->inv_diag_M = PETSC_NULL;

	/* create internals */
	KSPCreate( ((PetscObject)pc)->comm, &pc_data->ksp );

	/* set ctx onto pc */
	pc->data  = (void*)pc_data;

	ierr = PetscLogObjectMemory(pc,sizeof(_PC_GtKG));CHKERRQ(ierr);

	/* define operations */
	pc->ops->setup   = BSSCR_PCSetUp_GtKG;
	pc->ops->view    = BSSCR_PCView_GtKG;
	pc->ops->destroy = BSSCR_PCDestroy_GtKG;
	pc->ops->setfromoptions = BSSCR_PCSetFromOptions_GtKG;

	pc->ops->apply          = BSSCR_PCApply_GtKG;
	pc->ops->applytranspose = BSSCR_PCApplyTranspose_GtKG;

	PetscFunctionReturn(0);
}

/*
K & G must different to PETSC_NULL
M can be PETSC_NULL
*/
PetscErrorCode BSSCR_PCGtKGSet_Operators( PC pc, Mat K, Mat G, Mat M )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	BSSCR_pc_error( pc, "__func__" );

	ctx->K = K;
	ctx->G = G;
	ctx->M = M;

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCGtKGSet_OperatorForAlgebraicCommutator( PC pc, Mat M )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	BSSCR_pc_error( pc, "__func__" );

	ctx->M = M;

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCGtKGAttachNullSpace( PC pc )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;
	MatNullSpace nsp;

	BSSCR_pc_error( pc, "__func__" );

	/* Attach a null space */
	MatNullSpaceCreate( PETSC_COMM_WORLD, PETSC_TRUE, PETSC_NULL, PETSC_NULL, &nsp );
#if ( (PETSC_VERSION_MAJOR >= 3) && (PETSC_VERSION_MINOR <6) )
	KSPSetNullSpace( ctx->ksp, nsp );
#else
    Mat A;
    KSPGetOperators(ctx->ksp,&A,NULL);//Note: DOES NOT increase the reference counts of the matrix, so you should NOT destroy them.
    MatSetNullSpace( A, nsp);
#endif
	/*
	NOTE: This does NOT destroy the memory for nsp, it just decrements the nsp->refct, so that
	the next time MatNullSpaceDestroy() is called, the memory will be released. The next time this
	is called will be by KSPDestroy();
	*/
	MatNullSpaceDestroy( nsp );

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCGtKGGet_KSP( PC pc, KSP *ksp )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	BSSCR_pc_error( pc, "__func__" );

	if( ksp != PETSC_NULL ) {
		(*ksp) = ctx->ksp;
	}

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCGtKGSet_KSP( PC pc, KSP ksp )
{
	PC_GtKG ctx = (PC_GtKG)pc->data;

	BSSCR_pc_error( pc, "__func__" );

	if( ctx->ksp != PETSC_NULL ) {
		Stg_KSPDestroy(&ctx->ksp);
	}
	ctx->ksp = ksp;

	PetscFunctionReturn(0);
}
#endif


#pragma once

#include <utility>
#include <initializer_list>
#include <exception>
#include <iostream>
#include <cmath>

#include <gsl/gsl_math.h>
#include <gsl/gsl_linalg.h>

#include "bits/matrix-view.h"
#include "utils/fcmp.h"
#include "vector.h"

namespace gsl_wrapper
{
  class Matrix
  {
  public:
    // Constructors and destructor
    Matrix(size_t i, size_t j);
    Matrix(size_t matrix_size);
    Matrix(std::initializer_list<std::initializer_list<double>> args);
    Matrix(const Vector &vec);

    Matrix(const Matrix &copy_from);
    Matrix(Matrix &&move_from);

    ~Matrix();

    // Member functions
    auto get_gsl_matrix() const -> gsl_matrix *;
    auto get_dimensions() const -> std::pair<size_t, size_t>;
    auto num_rows() const -> size_t;
    auto num_collumns() const -> size_t;

    // Operators
    auto operator=(const Matrix &copy_from) -> Matrix &;
    auto operator=(Matrix &&move_from) -> Matrix &;

    auto operator==(const Matrix &comparasion_matrix) const -> bool;
    auto operator!=(const Matrix &comparasion_matrix) const -> bool;

    auto operator[](const size_t index) const -> gsl_wrapper::bits::MatrixRow;
    auto operator*(const Matrix &mul) const -> Matrix;
    auto operator*(const double number) const -> Matrix;
    auto operator+(const Matrix &matrix) const -> Matrix;
    auto operator+(const double numer) const -> Matrix;

    // Friend declarations
    friend auto operator<<(std::ostream &stream, const Matrix &matrix) -> std::ostream &;
    friend auto operator*(const double number, const Matrix &matrix) -> Matrix;
    friend auto operator+(const double number, const Matrix &matrix) -> Matrix;

  private:
    gsl_matrix *m_matrixPtr;
    size_t m_numRows;
    size_t m_numCollumns;
  };

  inline Matrix::Matrix(size_t i, size_t j)
      : m_matrixPtr{gsl_matrix_calloc(i, j)}, m_numRows{i}, m_numCollumns{j}
  {
  }

  inline Matrix::Matrix(size_t matrix_size)
      : Matrix(matrix_size, matrix_size)
  {
  }

  inline Matrix::Matrix(std::initializer_list<std::initializer_list<double>> args)
      : m_matrixPtr{nullptr}, m_numCollumns{0}, m_numRows{0}
  {
    if (args.size() == 0)
      return;
    size_t num_rows = args.size();
    size_t num_collumns = (*args.begin()).size();

    // Setting object properties
    m_numCollumns = num_collumns;
    m_numRows = num_rows;
    m_matrixPtr = gsl_matrix_calloc(m_numRows, m_numCollumns);

    size_t row_iterator = 0;
    size_t collumn_iterator = 0;
    for (auto &&row : args)
    {
      if (row.size() != num_collumns)
        throw std::range_error{"Diffrent number of items in diffrent rows when creating matrix"};

      for (auto &&el : row)
      {
        (*this)[row_iterator][collumn_iterator++] = el;
      }
      collumn_iterator = 0;
      ++row_iterator;
    }
  }

  inline Matrix::Matrix(const Vector &vec)
      : m_matrixPtr{gsl_matrix_calloc(vec.size(), 1)},
        m_numCollumns{1},
        m_numRows{vec.size()}
  {
    for (size_t i = 0; i < m_numRows; i++)
    {
      (*this)[i][0] = vec[i];
    }
  }

  inline Matrix::Matrix(const Matrix &copy_from)
      : m_matrixPtr{gsl_matrix_calloc(copy_from.m_numRows, copy_from.m_numCollumns)},
        m_numRows{copy_from.m_numRows},
        m_numCollumns{copy_from.m_numCollumns}
  {
    gsl_matrix_memcpy(m_matrixPtr, copy_from.m_matrixPtr);
  }

  inline Matrix::Matrix(Matrix &&move_from)
      : m_matrixPtr{std::exchange(move_from.m_matrixPtr, nullptr)},
        m_numRows{std::exchange(move_from.m_numRows, 0)},
        m_numCollumns{std::exchange(move_from.m_numCollumns, 0)}
  {
  }

  inline Matrix::~Matrix()
  {
    gsl_matrix_free(m_matrixPtr);
  }

  inline auto Matrix::get_gsl_matrix() const -> gsl_matrix *
  {
    return m_matrixPtr;
  }

  inline auto Matrix::get_dimensions() const -> std::pair<size_t, size_t>
  {
    return {m_numRows, m_numCollumns};
  }

  inline auto Matrix::num_rows() const -> size_t
  {
    return m_numRows;
  }
  inline auto Matrix::num_collumns() const -> size_t
  {
    return m_numCollumns;
  }

  inline auto Matrix::operator=(const Matrix &copy_from) -> Matrix &
  {
    // Prevent self copy
    if (m_matrixPtr == copy_from.m_matrixPtr)
      return *this;

    gsl_matrix_free(m_matrixPtr);
    m_matrixPtr = gsl_matrix_calloc(copy_from.m_numRows, copy_from.m_numCollumns);
    gsl_matrix_memcpy(m_matrixPtr, copy_from.m_matrixPtr);

    m_numCollumns = copy_from.m_numCollumns;
    m_numRows = copy_from.m_numRows;

    return *this;
  }

  inline auto Matrix::operator=(Matrix &&move_from) -> Matrix &
  {
    // Prevent self move
    if (m_matrixPtr == move_from.m_matrixPtr)
      return *this;

    gsl_matrix_free(m_matrixPtr);
    m_matrixPtr = std::exchange(move_from.m_matrixPtr, nullptr);
    m_numRows = std::exchange(move_from.m_numRows, 0);
    m_numCollumns = std::exchange(move_from.m_numCollumns, 0);

    return *this;
  }

  inline auto Matrix::operator==(const Matrix &comparasion_matrix) const -> bool
  {

    if ((m_numCollumns != comparasion_matrix.m_numCollumns) || (m_numRows != comparasion_matrix.m_numRows))
      return false;

    for (size_t i = 0; i < m_numRows; i++)
    {
      for (size_t j = 0; j < m_numCollumns; j++)
      {
        bool test = ::gsl_wrapper::utils::equal((*this)[i][j], comparasion_matrix[i][j]);
        if (!test)
          return false;
      }
    }
    return true;
  }

  inline auto Matrix::operator!=(const Matrix &comparasion_matrix) const -> bool
  {
    return !(*this == comparasion_matrix);
  }

  inline auto Matrix::operator[](const size_t index) const -> gsl_wrapper::bits::MatrixRow
  {
    using bits::MatrixRow;
    gsl_vector_view view = gsl_matrix_row(m_matrixPtr, index);
    return MatrixRow(view);
  }

  inline auto Matrix::operator*(const Matrix &mul) const -> Matrix
  {
    // Check sizes
    if (m_numCollumns != mul.m_numRows)
      throw std::runtime_error{"Wrong matrix sizes!"};

    Matrix result(m_numRows, mul.m_numCollumns);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, m_matrixPtr, mul.m_matrixPtr, 0.0, result.get_gsl_matrix());

    return result;
  }

  inline auto Matrix::operator*(const double number) const -> Matrix
  {
    Matrix result = *this;
    for (size_t i = 0; i < m_numRows; i++)
    {
      for (size_t j = 0; j < m_numCollumns; j++)
      {
        result[i][j] *= number;
      }
    }

    return result;
  }

  inline auto Matrix::operator+(const Matrix &matrix) const -> Matrix
  {
    if ((m_numCollumns != matrix.m_numCollumns) || (m_numRows != matrix.m_numRows))
      throw std::range_error{"Wrong matrix sizes when adding"};

    Matrix result = *this;
    gsl_matrix_add(result.m_matrixPtr, matrix.m_matrixPtr);

    return result;
  }

  inline auto Matrix::operator+(const double number) const -> Matrix
  {
    Matrix result = *this;
    for (size_t i = 0; i < m_numRows; i++)
    {
      for (size_t j = 0; j < m_numCollumns; j++)
      {
        result[i][j] += number;
      }
    }

    return result;
  }

  inline auto operator<<(std::ostream &stream, const Matrix &matrix) -> std::ostream &
  {
    for (size_t i = 0; i < matrix.m_numRows; i++)
    {
      for (size_t j = 0; j < matrix.m_numCollumns; j++)
      {
        stream << matrix[i][j] << " ";
      }
      stream << std::endl;
    }

    return stream;
  }

  inline auto operator*(const double number, const Matrix &matrix) -> Matrix
  {
    Matrix result = matrix;
    for (size_t i = 0; i < matrix.m_numRows; i++)
    {
      for (size_t j = 0; j < matrix.m_numCollumns; j++)
      {
        result[i][j] *= number;
      }
    }

    return result;
  }

  inline auto operator+(const double number, const Matrix &matrix) -> Matrix
  {
    Matrix result = matrix;
    for (size_t i = 0; i < matrix.m_numRows; i++)
    {
      for (size_t j = 0; j < matrix.m_numCollumns; j++)
      {
        result[i][j] += number;
      }
    }

    return result;
  }
}


#ifndef __PROJ_GAUSS_MIXTURES_H__
#define __PROJ_GAUSS_MIXTURES_H__

/* include */
#include <stdbool.h>
#include <float.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_linalg.h>

/* variable declarations and definitions */

double halflogtwopi; /* constant used in calculation */

int nthreads;
int tid;

struct gaussian{
  double alpha;
  gsl_vector *mm;
  gsl_matrix *VV;
};

struct datapoint{
  gsl_vector *ww;
  gsl_matrix *SS;
  gsl_matrix *RR;
  double logweight;
};

struct gaussian * newgaussians, * startnewgaussians;
gsl_matrix * qij;
gsl_permutation * p;
gsl_vector * wminusRm, * TinvwminusRm;
gsl_matrix * Tij,* Tij_inv, * VRT, * VRTTinv, * Rtrans, * I;
struct modelbs{
  gsl_vector *bbij;
  gsl_matrix *BBij;
};

struct modelbs * bs;

FILE *logfile,*convlogfile;

gsl_rng * randgen; /* global random number generator */

/* function declarations */

static inline bool bovy_isfin(double x){ /* true if x is finite */
  return (x > DBL_MAX || x < -DBL_MAX) ? false : true;}
void minmax(gsl_matrix * q, int row, bool isrow, double * min, double * max);
double logsum(gsl_matrix * q, int row, bool isrow);
double normalize_row(gsl_matrix * q, int row,bool isrow,bool noweight, double weight);
void bovy_randvec(gsl_vector * eps, int d, double length); /* returns random vector */
double bovy_det(gsl_matrix * A);/* determinant of matrix A */
void calc_splitnmerge(struct datapoint * data,int N,struct gaussian * gaussians, int K, gsl_matrix * qij, int * snmhierarchy);
void splitnmergegauss(struct gaussian * gaussians,int K, gsl_matrix * qij, int j, int k, int l);
void proj_EM_step(struct datapoint * data, int N, struct gaussian * gaussians, int K,bool * fixamp, bool * fixmean, bool * fixcovar, double * avgloglikedata, bool likeonly, double w,bool noproj, bool diagerrs, bool noweight);
void proj_EM(struct datapoint * data, int N, struct gaussian * gaussians, int K,bool * fixamp, bool * fixmean, bool * fixcovar, double * avgloglikedata, double tol,long long int maxiter, bool likeonly, double w,bool keeplog, FILE *logfile,FILE *tmplogfile, bool noproj, bool diagerrs, bool noweight);
void proj_gauss_mixtures(struct datapoint * data, int N, struct gaussian * gaussians, int K,bool * fixamp, bool * fixmean, bool * fixcovar, double * avgloglikedata, double tol,long long int maxiter, bool likeonly, double w, int splitnmerge, bool keeplog, FILE *logfile,FILE *convlogfile, bool noproj, bool diagerrs, bool noweight);
void calc_qstarij(double * qstarij, gsl_matrix * qij, int partial_indx[3]);

#endif /* proj_gauss_mixtures.h */


#ifndef __CNN_ACTIV_H__
#define __CNN_ACTIV_H__

#include <cblas.h>

#include "cnn.h"
#include "cnn_builtin_math.h"
#include "cnn_macro.h"
#include "cnn_types.h"

#ifdef CNN_WITH_CUDA
#include <cublas_v2.h>
#include <cuda_runtime.h>

#include "cnn_builtin_math_cu.h"
#include "cnn_init.h"
#endif

static inline void cnn_forward_activ(union CNN_LAYER* layerRef,
                                     struct CNN_CONFIG* cfgRef, int layerIndex)
{
    // Cache
    struct CNN_LAYER_ACTIV* layerPtr = &layerRef[layerIndex].activ;

    struct CNN_MAT* outData = &layerPtr->outMat.data;
    struct CNN_MAT* preOutData = &layerRef[layerIndex - 1].outMat.data;

    cnn_activ_t id = cfgRef->layerCfg[layerIndex].activ.id;

    if (id > 0)
    {
        cnn_activ_list[id](outData->mat, preOutData->mat,
                           outData->rows * outData->cols, layerPtr->buf.mat);
    }
    else
    {
#ifdef CNN_WITH_CUDA
        float alpha = 1.0;
        float beta = 0.0;

        cnn_assert_cudnn(cudnnSoftmaxForward(                     //
            cnnInit.cudnnHandle,                                  //
            CUDNN_SOFTMAX_ACCURATE, CUDNN_SOFTMAX_MODE_INSTANCE,  //
            &alpha,                                               //
            layerPtr->ten, preOutData->mat,                       //
            &beta,                                                //
            layerPtr->ten, outData->mat));
#else
        for (int j = 0; j < cfgRef->batch; j++)
        {
            int srcShift = j * preOutData->cols;
            int dstShift = j * outData->cols;

            float* srcPtr = preOutData->mat + srcShift;
            float* dstPtr = outData->mat + dstShift;
            float* bufPtr = layerPtr->buf.mat + dstShift;

            cnn_activ_list[id](dstPtr, srcPtr, outData->cols, bufPtr);
        }
#endif
    }
}

static inline void cnn_backward_activ(union CNN_LAYER* layerRef,
                                      struct CNN_CONFIG* cfgRef, int layerIndex)
{
    // Cache
    struct CNN_LAYER_ACTIV* layerPtr = &layerRef[layerIndex].activ;

    struct CNN_MAT* outData = &layerPtr->outMat.data;
    struct CNN_MAT* preOutData = &layerRef[layerIndex - 1].outMat.data;

    cnn_activ_t id = cfgRef->layerCfg[layerIndex].activ.id;

    // Find layer gradient
    if (layerIndex > 1)
    {
        if (id > 0)
        {
            cnn_activ_grad_list[id](
                preOutData->grad, outData->grad, preOutData->mat,
                outData->rows * outData->cols, outData->mat, layerPtr->buf.mat);
        }
        else
        {
#ifdef CNN_WITH_CUDA
            float alpha = 1.0;
            float beta = 0.0;

            cnn_assert_cudnn(cudnnSoftmaxBackward(                    //
                cnnInit.cudnnHandle,                                  //
                CUDNN_SOFTMAX_ACCURATE, CUDNN_SOFTMAX_MODE_INSTANCE,  //
                &alpha,                                               //
                layerPtr->ten, outData->mat,                          //
                layerPtr->ten, outData->grad,                         //
                &beta,                                                //
                layerPtr->ten, preOutData->grad));

#else
            for (int j = 0; j < cfgRef->batch; j++)
            {
                int shift = j * outData->cols;

                // Gradient calculation
                cnn_activ_grad_list[id](
                    preOutData->grad + shift, outData->grad + shift,
                    preOutData->mat + shift, outData->cols,
                    outData->mat + shift, layerPtr->buf.mat);
            }
#endif
        }
    }
}

#endif


#ifndef DELTAFREADER_H
#define DELTAFREADER_H

#include "ParameterReader.h"
#include "readindata.h"
#include "GaussThermal.h"
#include <fstream>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp.h>

using namespace std;

class Deltaf_Data
{
    private:
        ParameterReader * paraRdr;

        int hrg_eos; // type of pdg file for hadron resonance gas EoS
        int mode; //type of freezeout surface, VH or VAH
        int df_mode; // type of delta-f correction (e.g. 14-moment, CE, or modified distribution)
        int include_baryon;

        string hrg_eos_path;
        string urqmd = "deltaf_coefficients/vh/urqmd/"; // directories of df coefficient tables
        string smash = "deltaf_coefficients/vh/smash/";
        string smash_box = "deltaf_coefficients/vh/smash_box/";
    public:
        int points_T;
        int points_muB;

        double T_min;
        double muB_min;

        double dT;
        double dmuB;

        double * T_array;
        double * muB_array;
        //  Coefficients of 14 moment approximation (vhydro)
        // df ~ ((c0-c2)m^2 + b.c1(u.p) + (4c2-c0)(u.p)^2).Pi + (b.c3 + c4(u.p))p_u.V^u + c5.p_u.p_v.pi^uv
        double ** c0_data;
        double ** c1_data;
        double ** c2_data;
        double ** c3_data;
        double ** c4_data;

        //  Coefficients of Chapman Enskog expansion (vhydro)
        // df ~ ((c0-c2)m^2 + b.c1(u.p) + (4c2-c0)(u.p)^2).Pi + (b.c3 + c4(u.p))p_u.V^u + c5.p_u.p_v.pi^uv
        double ** F_data;
        double ** G_data;
        double ** betabulk_data;
        double ** betaV_data;
        double ** betapi_data;

        // cubic splines of the coefficients as function of temperature only (neglect muB, nB, Vmu)
        // (c1, G = 0) for muB = 0 and (c3, c4, betaV) aren't needed since they couple to baryon diffusion
        // so in the cubic spline evaluation: just set (G, c1, c3, c4) = 0 and betaV = 1 (betaV is in denominator)

        gsl_spline * c0_spline;
        gsl_spline * c2_spline;
        gsl_spline * c3_spline;

        gsl_spline * F_spline;
        gsl_spline * betabulk_spline;
        gsl_spline * betaV_spline;
        gsl_spline * betapi_spline;

        // Jonah coefficients
        const int jonah_points = 301;       // # lambda interpolation points
        const double lambda_min = -1.0;     // lambda min / max values
        const double lambda_max = 2.0;
        const double delta_lambda = (lambda_max - lambda_min) / ((double)jonah_points - 1.0);

        double * lambda_squared_array;      // squared isotropic momentum scale
        double * z_array;                   // renormalization factor (apart from detLambda)
        double * bulkPi_over_Peq_array;     // bulk pressure output
        double bulkPi_over_Peq_max;         // the maximum bulk pressure in the array

        gsl_spline * lambda_squared_spline; // cubic splines for lambda^2(bulkPi/Peq) and z(bulkPi/Peq)
        gsl_spline * z_spline;

        Deltaf_Data(ParameterReader * paraRdr_in);
        ~Deltaf_Data();

        void load_df_coefficient_data();    // read the data files in /deltaf_coefficients/vh

        void construct_cubic_splines();

        // I skip the photon because I think it breaks down for lambda = -1
        void compute_jonah_coefficients(particle_info * particle_data, int Nparticle);

        deltaf_coefficients evaluate_df_coefficients(double T, double muB, double E, double P, double bulkPi);

        deltaf_coefficients cubic_spline(double T, double E, double P, double bulkPi);

        double calculate_bilinear(double ** f_data, double T, double muB, double TL, double TR, double muBL, double muBR, int iTL, int iTR, int imuBL, int imuBR);

        deltaf_coefficients bilinear_interpolation(double T, double muB, double E, double P, double bulkPi);

        void test_df_coefficients(double bulkPi_over_P);

        void compute_particle_densities(particle_info * particle_data, int Nparticle);
};

#endif


/*
** LISA single subject statistical inference using 1st level GLM (general linear modeling).
** The prewhitening approach is adapted from Worsley et al (2002), Neuroimage 15(1).
**
** G.Lohmann, MPI-KYB, 2018
*/
#include <viaio/Vlib.h>
#include <viaio/VImage.h>
#include <viaio/mu.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

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

extern gsl_matrix *PseudoInv(gsl_matrix *A,gsl_matrix *B);
extern void printmat(gsl_matrix *R,char *str);
extern void printvec(gsl_vector *x,char *str);

extern void prewhite(gsl_matrix_float** pinvX,gsl_matrix_float** invM,gsl_matrix_float* X,int);

extern void dfs(VFloat** Dfs,gsl_matrix_float* pinvX,gsl_matrix_float* X,gsl_matrix_float* con,int);

extern void whitecov(VImage rho_vol,gsl_matrix_float* Y,gsl_matrix_float* invM,
		     gsl_matrix_float* pinvX,gsl_matrix_float* X,int,int);

extern void whitecov2(VImage effect_image, VImage rho_vol,
		      gsl_matrix_float* Y, gsl_matrix_float* X, gsl_matrix_float* con,
		      VFloat* Dfs, int, int slice);

void CopySlice(gsl_matrix *Data,gsl_matrix_float *Y,VImage map,int slice)
{
  size_t nvox;
  size_t i=0,j=0;
  double u=0;
  int ncols = VPixel(map,0,3,2,VShort);

  gsl_matrix_float_set_zero(Y);

  for (nvox=0; nvox < Data->size1; nvox++) {
    int b = VPixel(map,0,0,nvox,VShort);
    if (b != slice) continue;
    int r = VPixel(map,0,1,nvox,VShort);
    int c = VPixel(map,0,2,nvox,VShort);
    i = c + ncols*r;
    if (i >= Y->size2) continue;

    for (j=0; j<Data->size2; j++) {
      u = gsl_matrix_get(Data,nvox,j);
      gsl_matrix_float_set(Y,j,i,(float)u);
    }
  }
}

gsl_matrix_float *GslFloatCpy(gsl_matrix *X)
{
  int i,j;
  double u;
  gsl_matrix_float *Z = gsl_matrix_float_calloc(X->size2,X->size1);
  for (i=0; i<X->size1; i++) {
    for (j=0; j<X->size2; j++) {
      u = gsl_matrix_get(X,i,j);
      gsl_matrix_float_set(Z,j,i,(float)u);
    }
  }
  return Z;
}

/*
** general linear model (GLM) using whitening
*/
void VWhiteGLM(gsl_matrix *Data,VImage map,gsl_matrix *X0,gsl_vector *con,int numlags,VImage zmap)
{
  int i,slice;
  int numcon=1;
  int nslices = VPixel(map,0,3,0,VShort);
  int nrows = VPixel(map,0,3,1,VShort);
  int ncols = VPixel(map,0,3,2,VShort);
  int npix = (nrows*ncols);
  int ntimesteps = Data->size2;

  gsl_set_error_handler_off();
  VFillImage(zmap,VAllBands,0);

  /* contrast vector */
  gsl_matrix_float *contrast = gsl_matrix_float_alloc(1,con->size);
  for (i=0; i<con->size; i++) gsl_matrix_float_set(contrast,0,i,(float)con->data[i]);

  /* copy design matrix */
  gsl_matrix_float *X = GslFloatCpy(X0);

  /* initialize rho_vol */
  VImage rho_vol = VCreateImage(npix,nslices,numlags,VFloatRepn);
  VFillImage(rho_vol,VAllBands,0);

  /* prewhitening */
  gsl_matrix_float *pinvX=NULL,*invM=NULL;
  prewhite(&pinvX,&invM,X,numlags);

  /* degrees of freedom */
  VFloat *dfs_gsl = (VFloat *)VMalloc(sizeof(VFloat)* (numcon+1));
  dfs(&dfs_gsl,pinvX,X,contrast,numlags);

  /* first pass */
  gsl_matrix_float *Y = gsl_matrix_float_calloc(ntimesteps,npix);
  for (slice=0; slice<nslices; slice++) {
    CopySlice(Data,Y,map,(int)slice);
    whitecov(rho_vol,Y,invM,pinvX,X,numlags,slice);
  }

  /* second pass */
  VImage dest = VCreateImage(nslices,nrows,ncols,VFloatRepn);
  VFillImage(dest,VAllBands,0);
  for (slice=0; slice<nslices; slice++) {
    CopySlice(Data,Y,map,(int)slice);
    whitecov2(dest,rho_vol,Y,X,contrast,dfs_gsl,numlags,slice);
  }

  /* output */
  zmap = VCopyImagePixels(dest,zmap,VAllBands);

  /* cleanup */
  gsl_matrix_float_free(contrast);
  gsl_matrix_float_free(X);
  gsl_matrix_float_free(Y);
  gsl_matrix_float_free(pinvX);
  gsl_matrix_float_free(invM);
  VFree(dfs_gsl);
  VDestroyImage(rho_vol);
  VDestroyImage(dest);
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     local_trajectory_regression.h
* \author   Jong Jin Park and Collin Johnson
*
* Declaration of LocalTrajectoryRegression.
*/

#ifndef TRACKER_MOTIONS_LOCAL_TRAJECTORY_REGRESSION_H
#define TRACKER_MOTIONS_LOCAL_TRAJECTORY_REGRESSION_H

#include <tracker/object_state.h>
#include <core/point.h>
#include <boost/optional.hpp>
#include <nlopt.h>
#include <deque>
#include <vector>
#include <memory>

namespace vulcan
{
namespace tracker
{

struct regression_data_t
{
    int64_t timestamp;
    float   x;
    float   y;

    explicit regression_data_t(int64_t timestamp = 0, float x = 0.0f, float y = 0.0f)
    : timestamp(timestamp)
    , x(x)
    , y(y)
    {
    }
};

/**
* LocalTrajectoryRegression
*/
class LocalTrajectoryRegression
{
public:

    static const int kRegressionDimension = 4;
    static const int kIdxX  = 0;
    static const int kIdxY  = 1;
    static const int kIdxVX = 2;
    static const int kIdxVY = 3;

    // constructor
    LocalTrajectoryRegression(void);

    // Copy constructor
    LocalTrajectoryRegression(const LocalTrajectoryRegression& rhs);

    // destructor
    ~LocalTrajectoryRegression(void);

    // fit trajectory. Estimated velocity and trajectory are the outputs
    boost::optional<velocity_t> fitTrajectory(const std::deque<regression_data_t>& recentTrajectory,
                                              std::vector<Position>*               estimatedTrajectory = nullptr);

    // cost funtion for fitting the trajectory
    double evaluate(const double x[kRegressionDimension], double* grad);

private:

    void initializeData (const std::deque<regression_data_t>& recentTrajectory);

    // construct estimated trajectory using series of timestamps from data
    void predictTrajectory(double x0, double y0, double vx, double vy, std::vector<Position>& estimatedTrajectory);

    // computing energy and gradient
    double computeCost(void);
    void   computeGradient(double* grad);

    // setting up the optimizer
    void setupNLOpt(void);

    // optimizer
    nlopt_opt optimizer_;

    // data members
    std::vector<regression_data_t> data_;
    std::vector<Position>          prediction_;
    Position                       prior_;
    Point<double>            priorWeights_;

    int  numIter_;
    bool haveEnoughData_;

    // parameters
//     regression_params_t params_; // NOTE: hard coded for now
};

} // namespace mpepc
} // namespace vulcan

#endif // TRACKER_MOTIONS_LOCAL_TRAJECTORY_REGRESSION_H


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

#define PROGRAM_NAME "mergemt"

static void printUsage(void);
static int parseArguments(int argc, char *argv[],
                          char iniFile[PATH_MAX]);

struct mergemtParms_struct
{
    char **files;
    double *wts;
    char archiveFile[PATH_MAX];
    int nfiles;
};

int mergemt_readIni(const char *iniFile, struct mergemtParms_struct *parms);

int main(int argc, char *argv[])
{
    struct mergemtParms_struct parms;
    char iniFile[PATH_MAX], programNameIn[256];
    double *beta0, *beta1, *betaLoc, *dep0, *dep1, *depLoc,
           *gamma0, *gamma1, *gammaLoc, *kappa0, *kappa1, *kappaLoc,
           *M0, *M1, *M0loc, *phi0, *phi1,
           *phiLoc, *sigma0, *sigma1, *sigmaLoc, *theta0, *theta1,
           *thetaLoc, xsum;
    int i, ierr, il, im, indx, jndx, nb, nb0, nb1, ncopy, ng, ng0, ng1,
        nk, nk0, nk1, nlocs, nlocs0, nlocs1, nm, nm0, nm1, nmt, nmt0, nmt1,
        ns, ns0, ns1, nt, nt0, nt1;
    const int jm = 0;
    ierr = parseArguments(argc, argv, iniFile);
    if (ierr != 0)
    {
        if (ierr ==-2){return EXIT_SUCCESS;}
        printf("%s: Error parsing arguments\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    if (!os_path_isfile(iniFile))
    {
        printf("%s: Error ini file doesn't exist\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    ierr = mergemt_readIni(iniFile, &parms);
    if (ierr != 0)
    {
        printf("%s: Error reading ini file\n", PROGRAM_NAME);
        return EXIT_FAILURE;
    }
    xsum = array_sum64f(parms.nfiles, parms.wts, &ierr);
    if (fabs(xsum) < 1.e-14)
    {
        printf("%s: Division by zero - setting to 1\n", PROGRAM_NAME);
        xsum = 1.0;
    }
    cblas_dscal(parms.nfiles, 1.0/xsum, parms.wts, 1);
    // Load the files
    nlocs0 =-1;
    nlocs1 =-1;
    nm0 =-1;
    nm1 =-1;
    nb0 =-1;
    nb1 =-1;
    ng0 =-1;
    ng1 =-1;
    nk0 =-1;
    nk1 =-1;
    ns0 =-1;
    ns1 =-1;
    nt0 =-1;
    nt1 =-1;
    nmt0 =-1;
    nmt1 =-1;
    dep0 = NULL;
    dep1 = NULL;
    M0 = NULL;
    M1 = NULL;
    beta0 = NULL;
    beta1 = NULL;
    gamma0 = NULL;
    gamma1 = NULL;
    kappa0 = NULL;
    kappa1 = NULL;
    sigma0 = NULL;
    sigma1 = NULL;
    theta0 = NULL;
    theta1 = NULL;
    phi0 = NULL;
    phi1 = NULL;
    for (i=0; i<parms.nfiles; i++)
    {
        if (!os_path_isfile(parms.files[i])){continue;}
        printf("%s: Loading archive: %s\n", PROGRAM_NAME, parms.files[i]);
        ierr = parmt_io_readObjfnArchive64f(parms.files[i],
                                            programNameIn,
                                            &nlocs, &depLoc,
                                            &nm, &M0loc,
                                            &nb, &betaLoc,
                                            &ng, &gammaLoc,
                                            &nk, &kappaLoc,
                                            &ns, &sigmaLoc,
                                            &nt, &thetaLoc,
                                            &nmt, &phiLoc);
        if (ierr != 0)
        {
            printf("%s: Error loading %s; skipping...\n",
                   PROGRAM_NAME, parms.files[i]);
            continue;
        }
        if (strcasecmp("parmt", programNameIn) == 0)
        {
            if (nm0 ==-1)
            {
                nlocs0 = nlocs;
                nm0 = nm;
                nb0 = nb;
                ng0 = ng;
                nk0 = nk;
                ns0 = ns;
                nt0 = nt;
                nmt0 = nmt;
                phi0 = array_zeros64f(nmt, &ierr);
                dep0 = array_copy64f(nlocs, depLoc, &ierr);
                M0 = array_copy64f(nm, M0loc, &ierr);
                beta0 = array_copy64f(nb, betaLoc, &ierr);
                gamma0 = array_copy64f(ng, gammaLoc, &ierr);
                kappa0 = array_copy64f(nk, kappaLoc, &ierr);
                sigma0 = array_copy64f(ns, sigmaLoc, &ierr);
                theta0 = array_copy64f(nt, thetaLoc, &ierr);
            }
            if (nm0 != nm || nb0 != nb || ng0 != ng || nk0 != nk ||
                ns0 != ns || nt0 != nt || nmt0 != nmt || nlocs0 != nlocs)
            {
                printf("%s: Size mismatch in parmt\n", PROGRAM_NAME);
                continue;
            }
            cblas_daxpy(nmt, parms.wts[i], phiLoc, 1, phi0, 1);
        }
        else if (strcasecmp("polarmt", programNameIn) == 0)
        {
            if (nm1 ==-1)
            {
                nlocs1 = nlocs;
                nm1 = nm;
                nb1 = nb;
                ng1 = ng;
                nk1 = nk;
                ns1 = ns;
                nt1 = nt;
                nmt1 = nmt;
                phi1 = array_zeros64f(nmt, &ierr);
                dep1 = array_copy64f(nlocs, depLoc, &ierr);
                M1 = array_copy64f(nm, M0loc, &ierr);
                beta1 = array_copy64f(nb, betaLoc, &ierr);
                gamma1 = array_copy64f(ng, gammaLoc, &ierr);
                kappa1 = array_copy64f(nk, kappaLoc, &ierr);
                sigma1 = array_copy64f(ns, sigmaLoc, &ierr);
                theta1 = array_copy64f(nt, thetaLoc, &ierr);
            }
printf("%f\n",parms.wts[i]);
            cblas_daxpy(nmt, parms.wts[i], phiLoc, 1, phi1, 1);
        }
        else
        {
            printf("%s: Unkown origin: %s\n", PROGRAM_NAME, programNameIn);
            goto NEXT;
        }

NEXT:;
        memory_free64f(&M0loc);
        memory_free64f(&betaLoc);
        memory_free64f(&gammaLoc);
        memory_free64f(&kappaLoc);
        memory_free64f(&sigmaLoc);
        memory_free64f(&thetaLoc);
        memory_free64f(&phiLoc);
    }
    if (phi0 != NULL && phi1 != NULL)
    {
        if (nb0 != nb1 || ng0 != ng1 || nk0 != nk1 ||
            ns0 != ns1 || nt0 != nt1 || nlocs0 != nlocs)
        {
            printf("%s: Inconsistent grid search sizes\n", PROGRAM_NAME);
            return -1;
        }
        printf("%f\n", array_max64f(nmt0, phi0, &ierr));
        printf("%f\n", array_max64f(nmt1, phi1, &ierr));
        printf("%s: Stacking polarities into waveforms...\n", PROGRAM_NAME);
/*
        for (i=0; i<nm0; i++)
        {
            //printf("%d %d\n", nmt, nmt0);
            cblas_daxpy(nmt1, 1.0, phi1, 1, &phi0[nmt1*i], 1);
        }
*/
        for (il=0; il<nlocs0; il++)
        {
            for (im=0; im<nm0; im++)
            {
                indx = il*nm0*nb0*ng0*nk0*ns0*nt0
                     + im*nb0*ng0*nk0*ns0*nt0;
                jndx = il*nm1*nb1*ng1*nk1*ns1*nt1
                     + jm*nb1*ng1*nk1*ns1*nt1;
                ncopy = nb0*ng0*nk0*ns0*nt0;
                cblas_daxpy(ncopy,  1.0, &phi1[jndx], 1, &phi0[indx], 1);
            }
        }
printf("%f\n", array_max64f(nmt0, phi0, &ierr));
        printf("%s: Writing joint archive\n", PROGRAM_NAME);
        ierr = parmt_io_createObjfnArchive64f(PROGRAM_NAME, parms.archiveFile,
                                              1, //data.nobs, TODO - read and fix
                                              nlocs0, dep0,
                                              nm0, M0,
                                              nb0, beta0,
                                              ng0, gamma0,
                                              nk0, kappa0,
                                              ns0, sigma0,
                                              nt0, theta0);
        ierr = parmt_io_writeObjectiveFunction64f(parms.archiveFile,
                                                  nmt0, phi0);
    }
    else
    {
        if (phi0 != NULL)
        {
            printf("%s: Writing joint waveform archive\n", PROGRAM_NAME);
            ierr = parmt_io_createObjfnArchive64f(PROGRAM_NAME, parms.archiveFile,
                                              1, //data.nobs, TODO - read and fix
                                              nlocs0, dep0,
                                              nm0, M0,
                                              nb0, beta0,
                                              ng0, gamma0,
                                              nk0, kappa0,
                                              ns0, sigma0,
                                              nt0, theta0);
            ierr = parmt_io_writeObjectiveFunction64f(parms.archiveFile,
                                                      nmt0, phi0);
        }
        else if (phi1 != NULL)
        {
            printf("%s: Writing joint waveform archive\n", PROGRAM_NAME);
            ierr = parmt_io_createObjfnArchive64f(PROGRAM_NAME, parms.archiveFile,
                                              1, //data.nobs, TODO - read and fix
                                              nlocs1, dep1,
                                              nm1, M1,
                                              nb1, beta1,
                                              ng1, gamma1,
                                              nk1, kappa1,
                                              ns1, sigma1,
                                              nt1, theta1);
        }
    }
    memory_free64f(&dep0);
    memory_free64f(&dep1);
    memory_free64f(&M0);
    memory_free64f(&M1);
    memory_free64f(&beta0);
    memory_free64f(&beta1);
    memory_free64f(&gamma0);
    memory_free64f(&gamma1);
    memory_free64f(&kappa0);
    memory_free64f(&kappa1);
    memory_free64f(&sigma0);
    memory_free64f(&sigma1);
    memory_free64f(&theta0);
    memory_free64f(&theta1);
    memory_free64f(&phi0);
    memory_free64f(&phi1);
    return EXIT_SUCCESS;
}
//============================================================================//
int mergemt_readIni(const char *iniFile,
                    struct mergemtParms_struct *parms)
{
    const char *fcnm = "mergemt_readIni\0";
    const char *s;
    char vname[256];
    dictionary *ini;
    int i, ierr;
    memset(parms, 0, sizeof(struct mergemtParms_struct));
    if (!os_path_isfile(iniFile))
    {
        printf("%s: Error ini file doesn't exist\n", fcnm);
        return -1;
    }
    ini = iniparser_load(iniFile);

    parms->nfiles = iniparser_getint(ini, "general:nmerge\0", 0);
    if (parms->nfiles < 1)
    {
        printf("%s: No files to merge\n", fcnm);
        return -1;
    }

    parms->files = (char **) calloc((size_t) parms->nfiles, sizeof(char *));
    parms->wts = array_set64f(parms->nfiles, 1.0, &ierr);
    for (i=0; i<parms->nfiles; i++)
    {
        parms->files[i] = (char *) calloc(PATH_MAX, sizeof(char));
        memset(vname, 0, 256*sizeof(char));
        sprintf(vname, "general:mergeFile_%d", i+1);
        s = iniparser_getstring(ini, vname, NULL);
        if (!os_path_isfile(s))
        {
            printf("%s: File %s doesn't exist\n", fcnm, s);
            continue;
        }
        strcpy(parms->files[i], s);

        memset(vname, 0, 256*sizeof(char));
        sprintf(vname, "general:wtFile_%d", i+1);
        parms->wts[i] = iniparser_getdouble(ini, vname, 1.0);
        if (parms->wts[i] < 0.0)
        {
            printf("%s: Weight can't be negative: %f\n", fcnm, parms->wts[i]);
            return -1;
        }
    }

    memset(vname, 0, 256*sizeof(char));
    strcpy(vname, "general:mergeArchive");
    s = iniparser_getstring(ini, vname, "mergemt.h5");
    strcpy(parms->archiveFile, s);

    iniparser_freedict(ini);
    return 0;
}
//============================================================================//
static int parseArguments(int argc, char *argv[],
                          char iniFile[PATH_MAX])
{
    bool linFile;
    int prod;
    linFile = false;
    memset(iniFile, 0, PATH_MAX*sizeof(char));
    while (true)
    {
        static struct option longOptions[] =
        {
            {"help", no_argument, 0, '?'},
            {"help", no_argument, 0, 'h'},
            {"ini_file", required_argument, 0, 'i'},
            {0, 0, 0, 0}
        };
        int c, optionIndex;
        c = getopt_long(argc, argv, "?hi:m:l:",
                        longOptions, &optionIndex);
        if (c ==-1){break;}
        if (c == 'i')
        {
            strcpy(iniFile, (const char *) optarg);
            linFile = true;
        }
        else if (c == 'h' || c == '?')
        {
            printUsage();
            return -2;
        }
        else
        {
            printf("%s: Unknown options: %s\n",
                   PROGRAM_NAME, argv[optionIndex]);
        }
    }
    if (!linFile)
    {
        printf("%s: Error must specify ini file\n\n", PROGRAM_NAME);
        printUsage();
        return -1;
    }
    return 0;
}
//============================================================================//
static void printUsage(void)
{
    printf("Usage:\n   mergemt -i input_file\n\n");
    printf("Required arguments:\n");
    printf("   -i input_file specifies the initialization file\n");
    printf("\n");
    printf("Optional arguments:\n");
    printf("   -h displays this message\n");
    return;
}


#include <stdio.h>
#include <stddef.h>
#include <stdbool.h>
#include <stdlib.h>
#include <limits.h>
#include <string.h>

#include <cpuid.h>

#include <perf_counter.h>

#if defined(USE_MKL)
	#include <mkl_cblas.h>
#elif defined(USE_OPENBLAS)
	#include <cblas.h>
#elif defined(USE_BLIS)
	#include <blis/blis.h>
#else
	#error No BLAS library enabled
#endif

extern unsigned long long median(unsigned long long array[], size_t length);
extern void read_memory(const void* memory, size_t length);

enum gemm_type {
	gemm_type_unknown,
	gemm_type_cgemm,
	gemm_type_sgemm,
};

unsigned long long benchmark_gemm(
	const void* memory, size_t cache_size,
	enum gemm_type type,
	size_t m, size_t n, size_t k,
	const float a[], const float b[], float c[],
	size_t max_iterations)
{
	unsigned long long computation_time[max_iterations];
	size_t computation_samples = 0;
	for (size_t iteration = 0; iteration < max_iterations; iteration++) {
		read_memory(memory, cache_size);

		unsigned long long start_time, end_time;
		if (!read_timer(&start_time))
			continue;

		switch (type) {
			case gemm_type_sgemm:
			{
#if defined(USE_MKL) || defined(USE_OPENBLAS)
				cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
					m, n, k,
					1.0f, a, k, b, k, 0.0f, c, n);
#elif defined(USE_BLIS)
				float alpha = 1.0f;
				float beta = 0.0f;
				bli_sgemm(BLIS_NO_TRANSPOSE, BLIS_TRANSPOSE,
					m, n, k,
					&alpha,
					(float*) a, k, 1,
					(float*) b, k, 1,
					&beta,
					c, n, 1);
#endif
				break;
			}
			case gemm_type_cgemm:
			{
#if defined(USE_MKL) || defined(USE_OPENBLAS)
				float alpha[2] = { 1.0f, 0.0f };
				float beta[2] = { 0.0f, 0.0f };
				cblas_cgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
					m, n, k,
					alpha, a, k, b, k, beta, c, n);
#elif defined(USE_BLIS)
				scomplex alpha = { 1.0f, 0.0f };
				scomplex beta = { 0.0f, 0.0f };
				bli_cgemm(BLIS_NO_TRANSPOSE, BLIS_TRANSPOSE,
					m, n, k,
					&alpha,
					(scomplex*) a, k, 1,
					(scomplex*) b, k, 1,
					&beta,
					(scomplex*) c, n, 1);
#endif
				break;
			}
		}

		if (!read_timer(&end_time))
			continue;

		computation_time[computation_samples++] = end_time - start_time;
	}

	return median(computation_time, computation_samples);
}

struct options {
	enum gemm_type type;
	size_t m;
	size_t n;
	size_t k;
	size_t iterations;
};

static void print_options_help(const char* program_name) {
	printf(
"%s parameters...\n"
"Required parameters:\n"
"  -g   --gemm               The type of GEMM operation (sgemm, cgemm)\n"
"  -m                        The M dimension\n"
"  -n                        The N dimension\n"
"  -k                        The K dimension\n"
"Optional parameters:\n"
"  -i   --iterations         # iterations (default: 151)\n",
		program_name);
}

static struct options parse_options(int argc, char** argv) {
	struct options options = {
		.type = gemm_type_unknown,
		.k = 0,
		.m = 0,
		.n = 0,
		.iterations = 151
	};
	for (int argi = 1; argi < argc; argi += 1) {
		if ((strcmp(argv[argi], "-g") == 0) || (strcmp(argv[argi], "--gemm") == 0)) {
			if (argi + 1 == argc) {
				fprintf(stderr, "Error: expected k value\n");
				exit(EXIT_FAILURE);
			}
			if (strcmp(argv[argi + 1], "sgemm") == 0) {
				options.type = gemm_type_sgemm;
			} else if (strcmp(argv[argi + 1], "cgemm") == 0) {
				options.type = gemm_type_cgemm;
			} else {
				fprintf(stderr, "Error: invalid value %s for the gemm type: expected \"sgemm\", \"cgemm\"\n", argv[argi + 1]);
				exit(EXIT_FAILURE);
			}
			argi += 1;
		} else if (strcmp(argv[argi], "-k") == 0) {
			if (argi + 1 == argc) {
				fprintf(stderr, "Error: expected K value\n");
				exit(EXIT_FAILURE);
			}
			if (sscanf(argv[argi + 1], "%zu", &options.k) != 1) {
				fprintf(stderr, "Error: can not parse %s as an unsigned integer\n", argv[argi + 1]);
				exit(EXIT_FAILURE);
			}
			if (options.k == 0) {
				fprintf(stderr, "Error: invalid value %s for the K parameter: positive value expected\n", argv[argi + 1]);
				exit(EXIT_FAILURE);
			}
			argi += 1;
		} else if (strcmp(argv[argi], "-m") == 0) {
			if (argi + 1 == argc) {
				fprintf(stderr, "Error: expected M value\n");
				exit(EXIT_FAILURE);
			}
			if (sscanf(argv[argi + 1], "%zu", &options.m) != 1) {
				fprintf(stderr, "Error: can not parse %s as an unsigned integer\n", argv[argi + 1]);
				exit(EXIT_FAILURE);
			}
			if (options.m == 0) {
				fprintf(stderr, "Error: invalid value %s for the M parameter: positive value expected\n", argv[argi + 1]);
				exit(EXIT_FAILURE);
			}
			argi += 1;
		} else if (strcmp(argv[argi], "-n") == 0) {
			if (argi + 1 == argc) {
				fprintf(stderr, "Error: expected N value\n");
				exit(EXIT_FAILURE);
			}
			if (sscanf(argv[argi + 1], "%zu", &options.n) != 1) {
				fprintf(stderr, "Error: can not parse %s as an unsigned integer\n", argv[argi + 1]);
				exit(EXIT_FAILURE);
			}
			if (options.n == 0) {
				fprintf(stderr, "Error: invalid value %s for the N parameter: positive value expected\n", argv[argi + 1]);
				exit(EXIT_FAILURE);
			}
			argi += 1;
		} else if ((strcmp(argv[argi], "--iterations") == 0) || (strcmp(argv[argi], "-i") == 0)) {
			if (argi + 1 == argc) {
				fprintf(stderr, "Error: expected iterations value\n");
				exit(EXIT_FAILURE);
			}
			if (sscanf(argv[argi + 1], "%zu", &options.iterations) != 1) {
				fprintf(stderr, "Error: can not parse %s as an unsigned integer\n", argv[argi + 1]);
				exit(EXIT_FAILURE);
			}
			if (options.iterations == 0) {
				fprintf(stderr, "Error: invalid value %s for the number of iterations: positive value expected\n", argv[argi + 1]);
				exit(EXIT_FAILURE);
			}
			argi += 1;
		} else if ((strcmp(argv[argi], "--help") == 0) || (strcmp(argv[argi], "-h") == 0)) {
			print_options_help(argv[0]);
			exit(EXIT_SUCCESS);
		} else {
			fprintf(stderr, "Error: unknown argument '%s'\n", argv[argi]);
			print_options_help(argv[0]);
			exit(EXIT_FAILURE);
		}
	}
	if (options.type == gemm_type_unknown) {
		fprintf(stderr, "Error: gemm kernel type is not specified\n");
		print_options_help(argv[0]);
		exit(EXIT_FAILURE);
	}
	if (options.m == 0) {
		fprintf(stderr, "Error: M is not specified\n");
		print_options_help(argv[0]);
		exit(EXIT_FAILURE);
	}
	if (options.n == 0) {
		fprintf(stderr, "Error: N is not specified\n");
		print_options_help(argv[0]);
		exit(EXIT_FAILURE);
	}
	if (options.k == 0) {
		fprintf(stderr, "Error: K is not specified\n");
		print_options_help(argv[0]);
		exit(EXIT_FAILURE);
	}
	return options;
}

int main(int argc, char** argv) {
	const struct options options = parse_options(argc, argv);
	size_t components = 0;
	switch (options.type) {
		case gemm_type_sgemm:
			components = 1;
			break;
		case gemm_type_cgemm:
			components = 2;
			break;
		default:
			__builtin_unreachable();
	}

	const size_t cache_size = 128 * 1024 * 1024;
	void* memory = valloc(cache_size);

#ifdef USE_BLIS
	err_t blis_status = bli_init();
	if (blis_status != BLIS_SUCCESS) {
		fprintf(stderr, "BLIS initialization failed: error code %d\n", blis_status);
		exit(EXIT_FAILURE);
	}
#endif

	float* c = (float*) valloc(options.m * options.n * components * sizeof(float));
	float* a = (float*) valloc(options.m * options.k * components * sizeof(float));
	float* b = (float*) valloc(options.k * options.n * components * sizeof(float));

	memset(c, 0, options.m * options.n * components * sizeof(float));
	memset(a, 0, options.m * options.k * components * sizeof(float));
	memset(b, 0, options.k * options.n * components * sizeof(float));

	{
		printf("Iterations: %zu\n", options.iterations);

		const unsigned long long gemm_nanoseconds = benchmark_gemm(
			memory, cache_size,
			options.type,
			options.m, options.n, options.k,
			a, b, c,
			options.iterations);
		const double gemm_gflops = 2.0 * components * components * options.m * options.n * options.k / ((double) gemm_nanoseconds);

		printf("Time: %5.3lf ms\n", gemm_nanoseconds * 1.0e-6);
		printf("Performance: %5.3lf GFLOPs/s\n", gemm_gflops);
	}
}


/************************************************************/
/*********  Interval Censoring data analysis   ***********/
/*********  by Hao Liu on 6/7/07                  **********/
/************************************************************/

/*** accepted data file: first p colmuns are covariates, last 2 columns are observed time intervals, 9999=censored ***/

#include <stdio.h>
#include <math.h>
#include <time.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sort_double.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_vector.h>

#define MAX_ITER 10000
#define TOL 1.0e-6
#define sim_num 300

/** sim_num is the bootstrap replicates **/

#define machine_p 1.0e-15
#define myPHO 0.5
#define myPI 0.01
#define Sigma 20.0

struct fun_params {
    double *dij;
    double *zij;
    double *pj;
    int nn;
    int np;
    int mk;
};

double fdf_eplike(int, const double *, double *, void *);
void myxij(double *, const double *, const double *, void *);
void deveplike(double *, double *, const double *, double *, double *, void *);

double maxp(double *, double *, double, const double *, double *, void *);
double normf(double *, double *, double, double, const double *, double *, void *);

extern int conmin(int, double *, double *, double *, int *, int *, double, int, double *, int, double, int,
                  double (*)(int, const double *, double *, void *), void *);

int main(int argc, char *argv[])
{
    FILE *fpi, *fpo1, *fpo2;
    if(argc<=4) {
        printf("need: 1) data file name, 2) sample size n, 3) number of covariate p, and 4) the output file1 5) files2...\n");
        return 0;
    }
    fpi=fopen(argv[1], "r");
    fpo1=fopen(argv[4], "w");
    fpo2=fopen(argv[5], "w");

    int nn=0, np=0, i, j;
    char *cs=malloc(1000*sizeof(char));

    sscanf(argv[2], "%i", &nn);
    sscanf(argv[3], "%i", &np);

    if (np>nn) {
        printf("sample size should be larger than the number of covariates.");
        return 0;
    }

    double *li= malloc(nn*sizeof(double)), *lia= malloc(nn*sizeof(double)), *li0=malloc(nn*sizeof(double));
    double *ri= malloc(nn*sizeof(double)), *ria= malloc(nn*sizeof(double)), *ri0=malloc(nn*sizeof(double));
    double *zij= malloc(nn*np*sizeof(double)), *zij0= malloc(nn*np*sizeof(double)), *dij=malloc(nn*nn*sizeof(double));
    double *q=malloc(nn*sizeof(double)), *p=malloc(nn*sizeof(double)), *gam= malloc(nn*nn*sizeof(double));

    double *pj=calloc(nn, sizeof(double)), *mypj=calloc(nn, sizeof(double));

    double alpha0=1.0, theta0=1.0, nvold;
    register double *tau= calloc(nn, sizeof(double));
    double past_time, now_time, mysize, tmp00;

    double curt, wt, u, likval1, likval2, likval;

	int j1, k, mk, ss, iter, status, iter2, cure;
    int *bsi=malloc(nn*sizeof(int)), *bsi2=malloc(nn*sizeof(int));

    double *ww=calloc(2*(np+1)*(np+8), sizeof(double)),
        *xx=calloc(2*(np+1), sizeof(double)), *gg=calloc(2*(np+1), sizeof(double)), fval;
    double acc=1e-11, eps=1e-5;;
    int nflag=-1, ifun=-1, ifunter=-1;

    struct fun_params *mydata=malloc(sizeof(struct fun_params));

    const gsl_rng_type *Ty;
    gsl_rng *rn;

    gsl_rng_env_setup();
    Ty = gsl_rng_default;
    rn = gsl_rng_alloc (Ty);
    gsl_rng_set(rn, 100);

    /** read the data, first line is the hearder and thus removed ***/
    fgets(cs, 1000, fpi);

    for (i = 0; i<nn; i++) {
        for (j=0;j<np;j++) fscanf(fpi, "%lf,", zij0+np*i+j);
        fscanf(fpi, "%lf,%lf", li0+i, ri0+i);
        bsi[i]=i;
    }
    fclose(fpi);
    mydata->nn = nn;
    mydata->np = np;

    /** sim_num is the bootstrap replicates **/

    for (ss=0;ss<sim_num;ss++){

        /** select the bootstrap sample ***/
        /*** random sample with replacement **/

        gsl_ran_sample (rn, bsi2, nn, bsi, nn, sizeof(int));
        for(i=0;i<nn;i++) {
            for (j=0;j<np;j++) zij[np*i+j] = zij0[np*bsi2[i]+j];
            lia[i]=li0[bsi2[i]];
            li[i]=li0[bsi2[i]];
            ria[i]=ri0[bsi2[i]];
            ri[i]=ri0[bsi2[i]];
        }

        mydata->zij = zij;
        gsl_sort(ria,1,nn);
        gsl_sort(lia,1,nn);

        /*** generate p, q data ***/
        p[0]=ria[0];
        i=j=k=0;
        do {
            while (lia[i]<=p[k] && i<nn) i++;
            q[k]=lia[i-1];
            k++;
            while (ria[j]<lia[i] && j<nn) j++;
            if (j<nn) p[k]=ria[j];
            else p[k]=ria[j-1];
        } while (i<nn && j<nn);

        mk=k-1;
        mydata->mk=mk;
        for (i=0; i<nn; i++)
            for (j = 0; j <= mk; j++)
                dij[i*(mk+1)+j]=(li[i]<=q[j] && p[j]<=ri[i]) ? 1.0: 0.0;
        mydata->dij= dij;

        /** Initial values **/

        for (j=0; j<mk; j++) pj[j] = 1.0/mk;
        (mydata->pj)=pj;

        for (j=0; j<=np; j++) xx[j] = 0.0;

        nflag=2;
        fval=9999.0;
        iter2=0;
        do {
            iter2++;
            likval2=fval;

            for (j = 0; j < mk; j++) tau[j]=1.0/(mydata->pj)[j];
            nvold = 0.1;

            /*** calculate expected Xij ***/
            myxij(gam, xx, pj, mydata);

            /*** Primal-dual interior-point method ***/

            nvold = maxp(pj, tau, nvold, xx, gam, mydata);

            /** minimize to get theta **/

            if (nflag!=0) for (j=0; j<=np; j++) xx[j] = 0.1;
            nflag= conmin(np+1, xx, &fval, gg, &ifun, &ifunter, 0.001, 300, ww, 400, 1e-18, 1, fdf_eplike, mydata);

            likval1=fval;

        } while(fabs(likval1-likval2)>0.0001 & iter2 < MAX_ITER);

        now_time = (double) clock ();

        /*** outputs ****/

        for(j=0; j<=np; j++) fprintf(fpo1, "%.10f, ", xx[j]);
        fprintf(fpo1, "%.10f, %.2f \n", fval, now_time/(double) CLOCKS_PER_SEC);
        for(j=0; j<mk; j++) fprintf(fpo2, "%.10f,", (mydata->pj)[j]);
        fprintf(fpo2, "%i\n", mk);
    }

    fclose(fpo1);
    fclose(fpo2);

    free(li);
    free(ri);
    free(zij);

    free(li0);
    free(ri0);
    free(zij0);

    free(lia);
    free(ria);
    free(p);
    free(q);
    free(mypj);

    free(gam);
    free(tau);

    free(xx);
    free(ww);
    free(gg);

    free(mydata);

    return 0;
}

/*** the expected likelihood, 0=alpha, 1=theta and the derivative ***/

double fdf_eplike(int np1, const double *theta, double *df, void * par)
{
    struct fun_params * param = (struct fun_params *) par;
    int i,j,j1, m=(param->mk), nn=(param->nn), tmpi;

    double alp0, sz, likval, msumval, tmpz, tmpz1, tmpa, tmpc, tmpd, tmp0, dfz1, dfz2;
    double *xij, *cij, *pps, pp;

    xij=calloc(m+1, sizeof(double));
    pps=calloc(m+1, sizeof(double));
    cij=calloc(m+1, sizeof(double));

    alp0=theta[0];

    for(j=0;j<m;j++) {
        pps[j]=0.0;
        for(j1=0;j1<j;j1++) pps[j] += ((param->pj)[j1]) ;
    }

    for(j=0;j<np1;j++) df[j]=0.0;

    likval=0.0;
    for(i=0;i<nn;i++) {
        tmpz1=0.0;
        for(j=1;j<np1;j++) tmpz1 += theta[j] * (param->zij)[i*(np1-1)+j-1];
        tmpz=exp(alp0 + tmpz1);

        tmpc=0.0;
        for(j=0;j<m;j++) {
            pp = (param->pj)[j];
            xij[j] = ((param->dij)[i*(m+1)+j]) * exp( -tmpz* pps[j]) *(1.0- exp(-pp*tmpz));
            tmpc += xij[j];
        }
        xij[m]= (param -> dij)[i*(m+1)+m] * exp(-tmpz);
        tmpc += xij[m];

        if (tmpc == 0.0) {
            for(j=0;j<np1;j++) df[j]=-9999999.0;
            return 9999999.0;
        }

        for(j=0;j<=m;j++) {
            cij[j]=0.0;
            for(j1=j+1; j1<m; j1++) cij[j] += xij[j1];
            xij[j] /=tmpc;
        }

        msumval=0.0;
        for(j=0;j<m;j++) {
            cij[j] /=tmpc;
            pp= (param -> pj)[j];
            likval += ( xij[j]* log(1.0- exp(- pp* tmpz )) - cij[j] * pp * tmpz);
            msumval += (( xij[j] *exp(-pp* tmpz) /(1.0- exp(-pp* tmpz)) - cij[j] )*pp);
        }
        df[0] += ((msumval-xij[m])*tmpz);

        for(j=1;j<np1;j++) df[j] += ((msumval-xij[m])*(param->zij)[i*(np1-1)+j-1]*tmpz);
        likval -= xij[m]*tmpz;
    }

    for(j=0;j<np1;j++) df[j] = - df[j]/((double) nn);

    free(cij);
    free(xij);
    free(pps);

    return -likval/((double) nn);
}

/*** Calculate the updated Xij ***/

void myxij(double *lamij, const double *the, const double *mypj, void * parm)
{
    struct fun_params * para = (struct fun_params *) parm;

    double alp0, tmp0, tmpc, pp, tmpz, tmpz1;
    int i,j,j1, tmpi, m=(para->mk), nn=(para->nn), np=(para->np);
    double *gj=calloc(m+1, sizeof(double)), *xij=calloc(m+1, sizeof(double));
    double *dij=(para->dij), *zij=(para->zij);

    alp0=the[0];

    for(j=0;j<m;j++) {
        gj[j]=0.0;
        for(j1=0;j1<j;j1++) gj[j] += mypj[j1];
    }
    for(i=0;i<nn;i++) {
        tmpz1=0.0;
        for(j=0;j<np;j++) tmpz1 += the[j+1] * zij[j+i*np];
        tmpz=exp(alp0+ tmpz1);

        tmpc=0.0;
        for(j=0;j<m;j++) {
            pp = mypj[j];
            xij[j] = dij[i*(m+1)+j] * exp( -tmpz* gj[j]) *(1.0- exp(-pp*tmpz));
            tmpc += xij[j];
        }
        xij[m]= dij[i*(m+1)+m] * exp(-tmpz);
        tmpc += xij[m];

        for(j=0;j<m;j++)  lamij[i*m+j] = xij[j] / tmpc;
    }

    free(xij);
    free(gj);
}

/*** Calculate the first derivative and second derivative wrt p of the expected liklihood ***/
void deveplike(double *df, double *ddf, const double *the, double *gam, double * myp, void *parm)
{
    struct fun_params * para = (struct fun_params *) parm;
    double alp0, tmp1, tmpz1, tmp2, tmpa, pp;
    int i,j,k,j1, m=(para->mk), nn=(para->nn), np=(para->np);
    double *zij=(para->zij);

    alp0=the[0];
    for(j=0;j<m;j++) {

        df[j] = ddf[j] = 0.0;
        pp = myp[j];

        for(i=0;i<nn;i++) {

            tmpz1=0.0;
            for(k=0;k<np;k++) tmpz1 += the[k+1] * zij[k+i*np];
            tmp1=exp(alp0+ tmpz1);
            tmp2=exp(tmp1 * pp);

            tmpa =0.0;
            for(j1=j+1; j1<m; j1++) tmpa += gam[i*m+j1];
            df[j] += tmp1*( gam[i*m+j] /( tmp2- 1.0) - tmpa);
            ddf[j] -= (gam[i*m+j]*tmp1*tmp1*tmp2 / (tmp2 - 1.0) / (tmp2 - 1.0));
        }
    }
}

/*** NormF ***/

double normf(double *p, double *tau, double nv, double eta, const double *the, double *gam, void *parm)
{
    struct fun_params * para = (struct fun_params *) parm;
	double *fj, *ffj, sum1, tmp1, tmp2;
	int j, m=(para->mk);

    fj=calloc(m, sizeof(double));
    ffj=calloc(m, sizeof(double));

    deveplike(fj, ffj, the, gam, p, parm);

    sum1=0.0;
    for(j=0;j<m;j++) {
        tmp1 = (fj[j] + tau[j] - nv);
        tmp2 = p[j] * tau[j] - 1.0/eta;
        sum1 += (tmp1*tmp1 + tmp2*tmp2);
    }

    free(fj);
    free(ffj);
    return sqrt(sum1);
}

/**** Maximize for p ****/

double maxp(double *pj, double *tau, double nvold, const double *the, double *gam, void *parm)
{
    struct fun_params * para = (struct fun_params *) parm;

	int j, iter, m=(para->mk), nn=(para->nn), np=(para->np);
    double eta, nv, curnv, steps, sum1, sum2, tmp1, tmp2;
    double *fj, *ffj, *delp, *curp, *taunew, *curtau;

    fj = calloc(m, sizeof(double));
    ffj = calloc(m, sizeof(double));
    delp = calloc(m, sizeof(double));
    curp= calloc(m, sizeof(double));
    taunew = calloc(m, sizeof(double));
    curtau = calloc(m, sizeof(double));

    iter=0;
    do {
        iter++;

        // calculate the df=fj and diag(ddf)=ffj

        for(j=0;j<m;j++) eta += pj[j]*tau[j];
        eta = m*Sigma/eta;
        deveplike(fj, ffj, the, gam, pj, parm);

        sum1 = sum2 = 0.0;
        for(j=0;j<m;j++) {
            sum1 += ((1.0/eta + pj[j] * fj[j]) / (tau[j] - pj[j] * ffj[j]));
            sum2 += (pj[j] / (tau[j] - pj[j] * ffj[j]));
        }
        nv=sum1/sum2;

        // backtracking: find the largest positive step = steps

        steps=1.0;
        for(j=0;j<m;j++) {
            delp[j] =  (pj[j] * (fj[j]-nv) + 1.0/eta) / (tau[j] - pj[j] * ffj[j]);
            taunew[j] =   nv - ffj[j] * delp[j] - fj[j];
            tmp1 = taunew[j] - tau[j];
            if (tmp1 <0) steps = fmin(steps, -tau[j]/tmp1);
        }
        steps *= 0.99;

        // backtracking: to keep the minimal p > 0;
        tmp2 = 1.0;
        for(j=0;j<m;j++) tmp2=fmin(tmp2,  pj[j] + steps * delp[j]);
        while(tmp2 <= 0) {
            steps *= myPHO;
            tmp2 = 1.0;
            for(j=0;j<m;j++) tmp2=fmin(tmp2, pj[j] + steps * delp[j]);
        }

        // backtracking:

        for(j=0;j<m;j++) {
            curp[j] = pj[j] + steps * delp[j];
            curtau[j] = tau[j] + steps * (taunew[j]-tau[j]);
        }
        curnv=nvold + steps * (nv-nvold);
        tmp2 = normf(curp, curtau, curnv, eta, the, gam, parm);
        tmp1 = normf(pj, tau, nvold, eta, the, gam, parm);

        while(tmp2 > (1.0-myPI*steps)*tmp1) {
            steps *= myPHO;
            for(j=0;j<m;j++) {
                curp[j] = pj[j] + steps * delp[j];
                curtau[j] = tau[j] + steps * (taunew[j]-tau[j]);
            }
            curnv=nvold + steps * (nv-nvold);
            tmp2=normf(curp, curtau, curnv, eta, the, gam, parm);
        }

        deveplike(fj, ffj, the, gam, curp, parm);

        sum1=sum2=0.0;
        for(j=0;j<m;j++) {
            tmp1 = (fj[j] + curtau[j] - curnv);
            sum1 += (tmp1*tmp1);
            sum2 += curp[j] * curtau[j];
            pj[j] = curp[j];
            tau[j] = curtau[j];
        }

        nvold=curnv;
//         printf("iter=%i: sum1=%.20f sum2=%f \n", iter, sqrt(sum1), sum2/m);

    } while ( (sqrt(sum1)> TOL)& iter < MAX_ITER*10 );

    free(fj);
    free(ffj);
    free(delp);
    free(curtau);
    free(taunew);
    free(curp);

    return nvold;
}


/*
 * Copyright 2021 The DAPHNE Consortium
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#pragma once

#include <runtime/local/context/DaphneContext.h>
#include <runtime/local/datastructures/DataObjectFactory.h>
#include <runtime/local/datastructures/DenseMatrix.h>

#include <cblas.h>

#include <cassert>
#include <cstddef>

// ****************************************************************************
// Struct for partial template specialization
// ****************************************************************************

template<class DTRes, class DTLhs, class DTRhs>
struct MatMul {
    static void apply(DTRes *& res, const DTLhs * lhs, const DTRhs * rhs, DCTX(ctx)) = delete;
};

// ****************************************************************************
// Convenience function
// ****************************************************************************

template<class DTRes, class DTLhs, class DTRhs>
void matMul(DTRes *& res, const DTLhs * lhs, const DTRhs * rhs, DCTX(ctx)) {
    MatMul<DTRes, DTLhs, DTRhs>::apply(res, lhs, rhs, ctx);
}

// ****************************************************************************
// (Partial) template specializations for different data/value types
// ****************************************************************************

// ----------------------------------------------------------------------------
// DenseMatrix <- DenseMatrix, DenseMatrix
// ----------------------------------------------------------------------------

template<>
struct MatMul<DenseMatrix<float>, DenseMatrix<float>, DenseMatrix<float>> {
    static void apply(DenseMatrix<float> *& res, const DenseMatrix<float> * lhs, const DenseMatrix<float> * rhs, DCTX(ctx)) {
        const auto nr1 = static_cast<int>(lhs->getNumRows());
        const auto nc1 = static_cast<int>(lhs->getNumCols());
        const auto nc2 = static_cast<int>(rhs->getNumCols());
        assert((nc1 == static_cast<int>(rhs->getNumRows())) && "#cols of lhs and #rows of rhs must be the same");

        if(res == nullptr)
            res = DataObjectFactory::create<DenseMatrix<float>>(nr1, nc2, false);

        if(nr1 == 1 && nc2 == 1) // Vector-Vector
            res->set(0, 0, cblas_sdot(nc1, lhs->getValues(), 1, rhs->getValues(),
                    static_cast<int>(rhs->getRowSkip())));
        else if(nc2 == 1)        // Matrix-Vector
            cblas_sgemv(CblasRowMajor, CblasNoTrans, nr1, nc1, 1, lhs->getValues(),
                    static_cast<int>(lhs->getRowSkip()), rhs->getValues(),
                    static_cast<int>(rhs->getRowSkip()), 0,res->getValues(),
                    static_cast<int>(res->getRowSkip()));
        else                     // Matrix-Matrix
            cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, nr1, nc2, nc1,
                    1, lhs->getValues(), static_cast<int>(lhs->getRowSkip()), rhs->getValues(),
                static_cast<int>(rhs->getRowSkip()), 0, res->getValues(), static_cast<int>(res->getRowSkip()));
    }
};

template<>
struct MatMul<DenseMatrix<double>, DenseMatrix<double>, DenseMatrix<double>> {
    static void apply(DenseMatrix<double> *& res, const DenseMatrix<double> * lhs, const DenseMatrix<double> * rhs, DCTX(ctx)) {
        const auto nr1 = static_cast<int>(lhs->getNumRows());
        const auto nc1 = static_cast<int>(lhs->getNumCols());
        const auto nc2 = static_cast<int>(rhs->getNumCols());
        assert((nc1 == static_cast<int>(rhs->getNumRows())) && "#cols of lhs and #rows of rhs must be the same");

        if(res == nullptr)
            res = DataObjectFactory::create<DenseMatrix<double>>(nr1, nc2, false);

        if(nr1 == 1 && nc2 == 1) // Vector-Vector
            res->set(0, 0, cblas_ddot(nc1, lhs->getValues(), 1, rhs->getValues(),
                static_cast<int>(rhs->getRowSkip())));
        else if(nc2 == 1)        // Matrix-Vector
            cblas_dgemv(CblasRowMajor, CblasNoTrans, nr1, nc1, 1, lhs->getValues(),
            static_cast<int>(lhs->getRowSkip()), rhs->getValues(),static_cast<int>(rhs->getRowSkip()), 0,
                res->getValues(), static_cast<int>(res->getRowSkip()));
        else                     // Matrix-Matrix
            cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, nr1, nc2, nc1,
                    1, lhs->getValues(), static_cast<int>(lhs->getRowSkip()), rhs->getValues(),
                    static_cast<int>(rhs->getRowSkip()), 0, res->getValues(), static_cast<int>(res->getRowSkip()));
    }
};