/*
   Routines for calculation of the log-likelihood or restricted
   log-likelihood with mixed-effects models.

   Copyright (C) 2007-2024  The R Core Team
   Copyright (C) 1997-2005  Douglas M. Bates <bates@stat.wisc.edu>,
		            Jose C. Pinheiro, Saikat DebRoy

   This file is part of the nlme package for R and related languages
   and is made available under the terms of the GNU General Public
   License, version 2, or at your option, any later version,
   incorporated herein by reference.

   This program is distributed in the hope that it will be
   useful, but WITHOUT ANY WARRANTY; without even the implied
   warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
   PURPOSE.  See the GNU General Public License for more
   details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, a copy is available at
   http://www.r-project.org/Licenses/

*/

#include <float.h> // for DBL_EPSILON

#include <stdint.h>
#include "nlmefit.h"
#include "matrix.h"
#include "pdMat.h"

// 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
static double *_sigma_; /* This to provide msmnh out-of-band the sigma. */
extern void F77_NAME(msmnh)(void);

static int **
setOffsets(int ** base, int * ngrp, int Qp2)
{
    int i, **ptrVec = R_Calloc((size_t) Qp2, int *);
    for (i = 0; i < Qp2; i++) {
	ptrVec[i] = *base;
	*base += ngrp[i];
    }
    return ptrVec;
}

dimPTR
dims(int *pdims)
{				/* constructor for a dims object */
    dimPTR value = R_Calloc((size_t) 1, struct dim_struct);
    int *base, Qp2, *ngrp;

    value->N = (int) pdims[0];
    value->ZXrows = pdims[1];
    value->ZXcols = pdims[2];
    value->Q = pdims[3];
    Qp2 = (value->Q) + 2;
    value->Srows = pdims[4];
    value->q = pdims + 5;
    ngrp = value->ngrp = value->q + Qp2;
    value->DmOff = value->ngrp + Qp2;
    value->ncol = value->DmOff + Qp2;
    value->nrot = value->ncol + Qp2;
    base = value->nrot + Qp2;
    value->ZXoff = setOffsets(&base, ngrp, Qp2);
    value->ZXlen = setOffsets(&base, ngrp, Qp2);
    value->SToff = setOffsets(&base, ngrp, Qp2);
    value->DecOff = setOffsets(&base, ngrp, Qp2);
    value->DecLen = setOffsets(&base, ngrp, Qp2);
    return value;
}

SEXP getListElement(SEXP list, char *str)
{
    SEXP elmt = R_NilValue, names = getAttrib(list, R_NamesSymbol);
    int i;

    for (i = 0; i < length(list); i++)
	if(strcmp(CHAR(STRING_ELT(names, i)), str) == 0) {
	    elmt = VECTOR_ELT(list, i);
	    break;
	}
    return elmt;
}

dimPTR				/* create a dimensions object directly */
dimS(SEXP d)			/* from an SEXP */
{
    int i, Qp2;  SEXP tmp;
    dimPTR value = R_Calloc((size_t) 1, struct dim_struct);
    value->N = INTEGER(coerceVector(getListElement(d, "N"), INTSXP))[0];
    value->ZXrows =
	INTEGER(coerceVector(getListElement(d, "ZXrows"), INTSXP))[0];
    value->ZXcols =
	INTEGER(coerceVector(getListElement(d, "ZXcols"), INTSXP))[0];
    value->Q =
	INTEGER(coerceVector(getListElement(d, "Q"), INTSXP))[0];
    value->Srows =
	INTEGER(coerceVector(getListElement(d, "Srows"), INTSXP))[0];
    Qp2 = value->Q + 2;
    value->q = INTEGER(coerceVector(getListElement(d, "q"), INTSXP));
    value->ngrp = INTEGER(coerceVector(getListElement(d, "ngrp"), INTSXP));
    value->DmOff = INTEGER(coerceVector(getListElement(d, "DmOff"), INTSXP));
    value->ncol = INTEGER(coerceVector(getListElement(d, "ncol"), INTSXP));
    value->nrot = INTEGER(coerceVector(getListElement(d, "nrot"), INTSXP));
    value->ZXoff = R_Calloc(Qp2, int *);
    PROTECT(tmp = coerceVector(getListElement(d, "ZXoff"), VECSXP));
    for (i = 0; i < Qp2; i++) {
	(value->ZXoff)[i] = INTEGER(coerceVector(VECTOR_ELT(tmp, i), INTSXP));
    }
    UNPROTECT(1);
    value->ZXlen = R_Calloc(Qp2, int *);
    PROTECT(tmp = coerceVector(getListElement(d, "ZXlen"), VECSXP));
    for (i = 0; i < Qp2; i++) {
	(value->ZXlen)[i] = INTEGER(coerceVector(VECTOR_ELT(tmp, i), INTSXP));
    }
    UNPROTECT(1);
    value->SToff = R_Calloc(Qp2, int *);
    PROTECT(tmp = coerceVector(getListElement(d, "SToff"), VECSXP));
    for (i = 0; i < Qp2; i++) {
	(value->SToff)[i] = INTEGER(coerceVector(VECTOR_ELT(tmp, i), INTSXP));
    }
    UNPROTECT(1);
    value->DecOff = R_Calloc(Qp2, int *);
    PROTECT(tmp = coerceVector(getListElement(d, "DecOff"), VECSXP));
    for (i = 0; i < Qp2; i++) {
	(value->DecOff)[i] = INTEGER(coerceVector(VECTOR_ELT(tmp, i), INTSXP));
    }
    UNPROTECT(1);
    value->DecLen = R_Calloc(Qp2, int *);
    PROTECT(tmp = coerceVector(getListElement(d, "DecLen"), VECSXP));
    for (i = 0; i < Qp2; i++) {
	(value->DecLen)[i] = INTEGER(coerceVector(VECTOR_ELT(tmp, i), INTSXP));
    }
    UNPROTECT(1);
    return value;
}
void
dimFree(dimPTR this)
{
    R_Free(this->DecOff);
    R_Free(this->DecLen);
    R_Free(this->SToff);
    R_Free(this->ZXlen);
    R_Free(this->ZXoff);
    R_Free(this);
}

int
count_DmHalf_pars( dimPTR dd, int *pdClass )
{
    int i, result;
    for ( i = 0, result = 0; i < dd->Q; i++ ) {
	switch( pdClass[ i ] ) {
	case 0: case 4: result += ( (dd->q)[ i ] * ( (dd->q)[ i ] + 1 ) ) / 2;
	    break;
	case 1: result += (dd->q)[ i ];
	    break;
	case 2: result += 1;
	    break;
	case 3: result += 2;
	}
    }
    return result;
}

double *
generate_DmHalf( double *DmHalf, dimPTR dd, int *pdClass, double *pars )
{				/* Expand parameters to DmHalf arrays */
    int i, j, q, Q = dd->Q; double diag;
    for (i = 0; i < Q; i++) {
	q = (dd->q)[ i ];
	switch (pdClass[i]) {
	case 0:			/* default: unstructured */
	    matrixLog_pd( DmHalf + (dd->DmOff)[ i ], dd->q + i, pars );
	    pars += (q * (q + 1))/2;
	    break;
	case 1:			/* diagonal */
	    for (j = 0; j < q; j++) {
		DmHalf[ (dd->DmOff)[i] + j * (q + 1) ] = exp( *pars++ );
	    }
	    break;
	case 2:			/* multiple of identity */
	    diag = exp( *pars );
	    for (j = 0; j < q; j++) {
		DmHalf[ (dd->DmOff)[i] + j * (q + 1) ] = diag;
	    }
	    pars++;
	    break;
	case 3:			/* compound symmetry */
	    compSymm_pd( DmHalf + (dd->DmOff)[ i ], dd->q + i, pars );
	    pars += 2;
	    break;
	case 4:                 /* unstructured with log-cholesky
				   parametrization */
	    logChol_pd(DmHalf + (dd->DmOff)[ i ], dd->q + i, pars );
	    pars += (q * (q + 1))/2;
	    break;
	}
    }
    return DmHalf;
}

#ifdef Debug
static void
print_mat( char *msg, double *x, int ldx, int nrow, int ncol )
{				/* print matrix and message */
    int i, j;
    printf( "%s\n", msg );
    for (i = 0; i < nrow; i++) {
	for (j = 0; j < ncol; j++) {
	    printf( " %10.5g", x[i + j * ldx ] );
	}
	printf( "\n" );
    }
    printf( "\n" );
}
#endif /* Debug */

static double *
scale_mat(double *y, int ldy, double a,
	  double *x, int ldx, int nrow, int ncol)
{				/* y <- a * x */
    int i, j;
    double * ret = y;

    for (j = 0; j < ncol; j++) {
	for (i = 0; i < nrow; i++) { y[i] = a * x[i]; }
	y += ldy; x += ldx;
    }
    return ret;
}

static double *
plus_equals_mat(double *y, int ldy, double *x, int ldx, int nrow, int ncol)
{				/* y <- y + x */
    double * ret = y;
    int i, j;

    for (j = 0; j < ncol; j++) {
	for (i = 0; i < nrow; i++) { y[i] += x[i]; }
	y += ldy; x += ldx;
    }
    return ret;
}

static int			/* backsolve and update */
backsolve(double *mat, int ldmat, int nupdate, int ncol, int nrot, int ny)
{
    int i, j, ONE = 1, info;
    double *y = mat + (int) ((ncol + nrot - ny) * ldmat);

    mat = mat - (int) nupdate;
    for (i = 0; i < ny; i++) {	/* usually ny = 1 but just in case ... */
	F77_CALL(dtrsl) (mat + (int) nupdate, &ldmat, &ncol, y, &ONE, &info);
	if (info != 0) {
	    return info;
	}
	for (j = 0; j < ncol; j++) {
	    d_axpy(y - (int) nupdate, - y[j], mat + (int) (j * ldmat), nupdate);
	}
	y += (int) ldmat;
    }
    return info;
}

static int			/* invert an upper-triangular matrix in place*/
invert_upper(double *mat, int ldmat, int ncol)
{
    int i, j, ONE = 1, info = 0;
    double *b = R_Calloc((size_t) ncol, double);

    for (i = ncol; i > 1; i--) {
	for (j = 0; j < (i - 1); j++) { b[j] = 0.0; }
	b[((int) i) - 1] = 1.0;
	F77_CALL(dtrsl) (mat, &ldmat, &i, b, &ONE, &info);
	if (info != 0) { R_Free(b); return info; }
	Memcpy(mat + (i - 1) * ldmat, b, (int) i);
    }
    if (*mat == 0.0) { R_Free(b); return 1; }
    *mat = 1.0 / (*mat);
    R_Free(b); return 0;
}

static int			/* invert a block in the virtual R array */
invert_block(double *mat, int ldmat, int nabove, int ncol, int nright)
{
    double * tpblk = mat - (int) nabove;
    int info = invert_upper(mat, ldmat, ncol);

    if (info != 0) return info;
    if (nright > 0) {
	double *ntri = R_Calloc((size_t) (ncol * ncol), double),
	    *rtblk = mat + ncol * ldmat;
	scale_mat(ntri, ncol, -1.0, mat, ldmat, ncol, ncol);
	mult_mat(rtblk, ldmat, ntri, ncol, ncol, ncol, rtblk, ldmat, nright);
	R_Free(ntri);
	if (nabove > 0) {
	    double *tmp = R_Calloc((size_t)(nabove * nright), double);
	    plus_equals_mat(rtblk - (size_t)nabove, ldmat,
			    mult_mat(tmp, nabove, tpblk, ldmat, nabove, ncol,
				     rtblk, ldmat, nright),
			    nabove, nabove, nright);
	    R_Free(tmp);
	}
    }
    if (nabove > 0) {
	mult_mat(tpblk, ldmat, tpblk, ldmat, nabove, ncol, mat, ldmat, ncol);
    }
    return 0;
}


void				/* return the decomposition for ZXy */
mixed_decomp(double *ZXy, int *pdims)
{
    dimPTR dd = dims(pdims);	/* Create a dimensions structure */
    internal_decomp(dd, ZXy);
    dimFree(dd);
}

void
internal_decomp(dimPTR dd, double *ZXy)
{				/* decompose ZXy and re-write the dims */
    int i, j, Qp2 = (dd->Q) + 2;
    double *dc;

    if ((dd->Srows) >= (dd->ZXrows)) /* decomposition is not worthwhile */
	return;
    dc = R_Calloc((size_t) ((dd->Srows) * (dd->ZXcols)), double);
    for (i = 0; i < Qp2; i++) {
	for(j = 0; j < (dd->ngrp)[i]; j++) {
	    QR_and_rotate(ZXy + (dd->ZXoff)[i][j], dd->ZXrows, (dd->ZXlen)[i][j],
			  (dd->ncol)[i] + (dd->nrot)[i], DNULLP, 0,
			  (dd->ncol)[i], DNULLP, dc + (dd->SToff)[i][j],
			  dd->Srows);
	}
    }
    Memcpy(ZXy, dc, dd->Srows * dd->ZXcols);
    for (i = 0; i < Qp2; i++) {	/* re-write the offsets and lengths */
	for (j = 0; j < (dd->ngrp)[i]; j++) {
	    (dd->ZXoff)[i][j] = (dd->DecOff)[i][j];
	    (dd->ZXlen)[i][j] = (dd->DecLen)[i][j];
	}
    }
    dd->ZXrows = dd->Srows;	/* and the total number of rows */
    R_Free(dc);
}

double			/* evaluate the log-likelihood pieces */
internal_loglik(dimPTR dd, double *ZXy, double *DmHalf, int *RML,
		double *dc, double *lRSS,
		// 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Sol.
		double *sigma)
{
    int Q = dd->Q,  Qp2 = Q + 2;
    double *lglk = R_Calloc( Qp2, double );

    for (int i = 0; i < Qp2; i++) {
	int qi = (dd->q)[i];
	// we could pull things out of this loop, but assume
	// optimizing compiler can do that for us.
	for (int j = 0; j < (dd->ngrp)[i]; j++) {
	    int ldstr;
	    double *store;
	    /* if dc is NULL, don't attempt storage */
	    if (dc != DNULLP) {
		ldstr = dd->Srows;
		store = dc + (dd->SToff)[i][j];
	    } else {
		ldstr = 0;
		store = DNULLP;
	    }
	    if (qi >
		QR_and_rotate(ZXy + (dd->ZXoff)[i][j], dd->ZXrows,
			      (dd->ZXlen)[i][j], (dd->ncol)[i] + (dd->nrot)[i],
			      DmHalf + (dd->DmOff)[i], qi, (dd->ncol)[i],
			      lglk + i, store, ldstr))
	    {
		warning("Singular precision matrix in level %ld, block %ld",
			(long int) (i - Q), (long int) (j + 1));
		return -DBL_MAX;
	    }
	}
    }
    double accum = 0;
    for(int i = 0; i < Q; i++) {
	int qi = (dd->q)[i];
	double *dmHlf = R_Calloc( (size_t) qi * qi, double );
	QRptr dmQR = QR( copy_mat( dmHlf, qi, DmHalf + (dd->DmOff)[i],
				   qi, qi, qi ), qi, qi, qi);
	accum += (dd->ngrp)[i] * QRlogAbsDet( dmQR ) - lglk[i];
	QRfree( dmQR ); R_Free( dmHlf );
    }
    // 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
    if (*sigma > 0) { // Fixed sigma
	double h = 0;
	if (*RML == 1) {
	    h += (lglk[Q] - dd->ncol[Q]*lglk[Q+1]) - 1;
	}
	accum -= pow(exp(lglk[Q+1]),2)/(2*pow(*sigma,2));
	accum -= (dd->N - dd->ncol[Q]) * log(*sigma);
	accum -= h;
    } else {	// Free sigma
	accum -= *RML * lglk[Q] + (dd->N - *RML * dd->ncol[Q]) * lglk[Q + 1];
    }
    if (lRSS != DNULLP) {
	*lRSS = lglk[Q+1];
    }
    R_Free(lglk);
    return accum;
}

void
internal_estimate(dimPTR dd, double *dc)
{				/* solve for Beta and b_i estimates */
    int Qp1 = (dd->Q) + 1;

    for (int i = (dd->Q); i >= 0; i--) {
	for (int j = 0; j < (dd->ngrp)[i]; j++) {
	    if (backsolve(dc + (dd->SToff)[i][j], dd->Srows,
			  (dd->SToff)[i][j] - (dd->DecOff)[i][j],
			  (dd->ncol)[i], (dd->nrot)[i], (dd->ncol)[Qp1]) != 0)
	    {
		error(_("Singularity in backsolve at level %ld, block %ld"),
		      (long int) (i - (dd->Q)), (long int) (j + 1));
	    }
	}
    }
}

static void
internal_R_invert(dimPTR dd, double *dc)
{				/* Invert the virtual R matrix in place */
    int i, j;
    for (i = (dd->Q); i >= 0; i--) {
	for (j = 0; j < (dd->ngrp)[i]; j++) {
	    invert_block(dc + (dd->SToff)[i][j], dd->Srows,
			 (dd->SToff)[i][j] - (dd->DecOff)[i][j],
			 (dd->ncol)[i], (dd->nrot)[i] - 1);
	}
    }
}

static double cube_root_eps = 0.;

static void
pt_prod( double *prod, double *a, double *b, size_t len )
{				/* prod <- a * b */
    for (int i = 0; i < len; i++) {
	*prod++ = *a++ * *b++;
    }
}

static void
finite_diff_Hess(double (*func)(double*,double*), double *pars, int npar,
		 double *vals,
		 // 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Sol.
		 double *sigma)
{				/* use Koshal design for finite-differences */
    double nT = 1 + npar + (npar * ((double) npar + 1)) / 2;// against integer overflow
    if((nT * nT) > (double) SIZE_MAX)
	error(_("Too many parameters for finite-difference Hessian; npar = %d, nTot = %g."),
	      npar, nT);
    size_t nTot = (size_t) nT;
    double *ppt, *xpt, *dpt,
	*incr   = R_Calloc( npar,        double),
	*parray = R_Calloc( nTot * npar, double), /* array of parameters */
	*div    = R_Calloc( nTot,        double), /* divisors */
	*Xmat   = R_Calloc( nTot * nTot, double); /* regressor matrix */
    QRptr xQR;

    if (cube_root_eps == 0.0) cube_root_eps = exp( log( DBL_EPSILON ) / 3.);
    div[ 0 ] = 1.0;
    ppt = parray + npar * ( 2 * npar + 1 ); /* location of first cross term */
    xpt = Xmat   + nTot * ( 2 * npar + 1 ); /* location of first cross column */
    dpt = div + 2 * npar + 1;
    size_t np_i = npar;
    for (int i = 0; i < npar; i++, np_i++) { // np_i === npar + i
	size_t np1 = (size_t) npar + 1;
	incr[i] = (pars[ i ] != 0.0) ? cube_root_eps * pars[ i ] : cube_root_eps;
	div[ i + 1 ] = 1.0 / incr[ i ];
	div[ np_i + 1 ] = 2.0 / ( incr[ i ] * incr[ i ] );
	parray[ npar + i * np1 ] =  1.;
	parray[   np_i   * np1 ] = -1.;
	for (int j = i + 1; j < npar; j++) {
	    ppt[ i ] = ppt[ j ] = 1;
	    ppt += npar;
	}
	for (size_t j = 0; j < nTot; j++) {
	    Xmat[ j + (i + 1) * nTot ] = parray[ i + j * npar ];
	}
	pt_prod( Xmat + (np_i + 1) * nTot, Xmat + (i + 1) * nTot,
		 Xmat + (i + 1) * nTot, nTot );
	for (int j = 0; j < i; j++) {
	    pt_prod( xpt, Xmat + (i + 1) * nTot, Xmat + (j + 1) * nTot, nTot );
	    xpt += nTot;
	    *dpt++ = 1.0 / ( incr[ i ] * incr[ j ] );
	}
    }
#ifdef Debug
    print_mat( "parray", parray, npar, npar, nTot );
#endif /* Debug */
    vals[ 0 ] = (*func)( pars, sigma ); // 17-11-2015; Fixed sigma patch ...
    Xmat[ 0 ] = 1.0;
    for (size_t i = 1; i < nTot; i++) {
	Xmat[i] = 1.0;		/* column of 1's for constant */
	Memcpy( parray, pars, npar );
	for (int j = 0; j < npar; j++) {
	    parray[ j ] += parray[ j + i * npar ] * incr[ j ];
	}
	vals[i] = (*func)( parray, sigma ); // 17-11-2015; Fixed sigma patch ...
    }
#ifdef Debug
    print_mat( "Xmat", Xmat, nTot, nTot, nTot );
#endif /* Debug */
    xQR = QR( Xmat, (int) nTot, (int) nTot, (int) nTot );
    QRsolve( xQR, vals, (int) nTot, 1, vals, (int) nTot );
    pt_prod( vals, vals, div, nTot );
    /* re-arrange the Hessian terms */
    xpt = vals + npar + 1;
    Memcpy( div, vals + npar + 1, nTot - ( npar + 1 ) );
    dpt = div + npar;		/* first off-diagonal */
    for (int i = 0; i < npar; i++) {
	xpt[ i * ( npar + 1 ) ] = div[ i ];	/* diagonals */
	for (int j = 0; j < i; j++) {
	    xpt[ i + j * npar ] = xpt[ j + i * npar ] = *dpt++;
	}
    }
    QRfree( xQR ); R_Free( incr ); R_Free( parray ); R_Free( div ); R_Free( Xmat );
    return;
}

/* objective function for optif9(), itself called from
 * nlme_increment() [../nlme.c] and mixed_combined() below */
void
mixed_fcn(int n, double *pars, double *g, void *state)
{
    statePTR st = (statePTR) state;
    double *zxcopy = R_Calloc(st->dd->ZXrows * st->dd->ZXcols, double),
	*Delta = R_Calloc(st->dd->DmOff[st->dd->Q], double);

    Memcpy(zxcopy, st->ZXy, st->dd->ZXrows * st->dd->ZXcols);
    *g = -internal_loglik(st->dd, zxcopy,
			  generate_DmHalf(Delta, st->dd, st->pdClass, pars),
			  st->RML, DNULLP, DNULLP, st->sigma);// 17-11-2015; Fixed sigma ..
    if (!R_FINITE(g[0])) { // guard optif9 against NaN etc (PR#18433)
#ifdef Debug
        Rprintf("Parameters:"); for (int i = 0; i < n; i++) Rprintf(" %#8g", pars[i]);
        Rprintf("\nFunction Value: %g\n", g[0]);
#endif
        warning("Non-finite log-likelihood replaced by maximally negative value");
        *g = DBL_MAX;
    }

    R_Free(Delta); R_Free(zxcopy);
}

void // gradient for optif9() of objective function mixed_fcn() see above
mixed_grad(int n, double *pars, double *g, void *state)
{
    statePTR st = (statePTR) state;
    double *zxcopy = R_Calloc(st->dd->ZXrows * st->dd->ZXcols, double),
	*Delta = R_Calloc(st->dd->DmOff[st->dd->Q], double),
	*dc = R_Calloc((size_t) ((st->dd->Srows) * (st->dd->ZXcols)), double),
	*DmHalf, sigmainv, *pt, *res;
    double  sqrtDF = sqrt((double) (st->dd->N -
				    *(st->RML)*(st->dd->ncol[st->dd->Q])));
    int i, j, offset;

    DmHalf = generate_DmHalf(Delta, st->dd, st->pdClass, pars),
	Memcpy(zxcopy, st->ZXy, st->dd->ZXrows * st->dd->ZXcols);
    /* needed ? */
    internal_loglik(st->dd, zxcopy, DmHalf, st->RML, dc, DNULLP, st->sigma);// Fixed sigma
    internal_estimate(st->dd, dc);
    internal_R_invert(st->dd, dc);
    // 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
    if (*st->sigma > 0) {
	sigmainv = 1.0/(*st->sigma);
    } else {
	sigmainv = *(dc + (size_t)((st->dd->Srows) * (st->dd->ZXcols)) - 1)/sqrtDF;
	if (sigmainv == 0) {
	    error(_("Overfitted model!"));
	} else {
	    sigmainv = 1.0/((sigmainv < 0.0) ? - sigmainv : sigmainv);
	}
    }
    offset = ((st->dd->ZXcols) - 1) * (st->dd->Srows);
    for (i = 0; i < (st->dd->Q); i++) {
	int ncol = (st->dd->q)[i],
	    nright = (st->dd->nrot)[i] - (st->dd->nrot)[(st->dd->Q) - ( (*(st->RML)) ? 0 : 1 )];
	int nrow = (ncol + nright + 1) * (st->dd->ngrp)[i];
	QRptr qq;
	pt = res = R_Calloc((size_t) (ncol * nrow), double);
	for (j = 0; j < (st->dd->ngrp)[i]; j++) {
	    copy_trans(pt, nrow, dc + (st->dd->SToff)[i][j], st->dd->Srows,
		       ncol, ncol + nright);
	    pt += ncol + nright;
	    scale_mat(pt++, nrow, sigmainv, dc + offset + (st->dd->SToff)[i][j],
		      1, 1, ncol);
	}
	offset -= (st->dd->Srows) * ncol;
	qq = QR(res, nrow, nrow, ncol);
	QRstoreR(qq, res, ncol);
	QRfree(qq);
	switch (st->pdClass[i]) {
	case 0:			/* unstructured with matrix-logarithm
				   parametrization */
	    error(_("analytic gradient is not available with matrix logarithm"));
	    break;
	case 1:			/* diagonal */
	    for (j = 0; j < ncol; j++) {
		double tmp = DmHalf[ (st->dd->DmOff)[i] + j * (ncol + 1)];
		*g++ = st->dd->ngrp[i] - tmp*tmp*d_sum_sqr(res + j * ncol, j + 1);
	    }
	    break;
	case 2:			/* multiple of identity */
	{
	    double tmp = 0.0;
	    for(j = 0; j < ncol; j++) {
		tmp += d_sum_sqr( res + j * nrow, j + 1 );
	    }
	    *g = tmp;
	    tmp = DmHalf[ (st->dd->DmOff)[i] + j * (ncol + 1)];
	    *g *= tmp * tmp;
	    *g = ncol*st->dd->ngrp[i] - *g;
	    g++;
	    break;
	}
	case 3:			/* compound symmetry */
	{
	    error(_("analytic gradient is not available with compound symmetry"));
	    break;
	}
	case 4:			/* unstructured with log-cholesky
				   parametrization */
	{
	    int j1;
	    double *col_j = R_Calloc(ncol, double);
	    for (j1 = 0; j1 < ncol; j1++) {
		int i1;
		for(i1 = 0; i1 < j1; i1++)
		    col_j[i1] = d_dot_prod(res + i1*ncol, 1, res +
					   j1*ncol, 1, 1+i1);
		for(i1 = j1; i1 < ncol; i1++)
		    col_j[i1] = d_dot_prod(res + i1*ncol, 1, res +
					   j1*ncol, 1, 1+j1);
		for (i1 = 0; i1 <= j1; i1++) {
		    int k1;
		    double sum = 0.0;

		    for (k1 = i1; k1 < ncol; k1++) {
			sum += DmHalf[(st->dd->DmOff)[i] + i1*ncol + k1] *
			    col_j[k1];
		    }
		    if (i1 == j1)
			*g++ = st->dd->ngrp[i] -
			    sum*DmHalf[(st->dd->DmOff)[i] + i1*(ncol + 1)];
		    else
			*g++ = -sum;
		}
	    }
	    break;
	}
	}
	R_Free(res);
    }
    R_Free(dc); R_Free(Delta); R_Free(zxcopy);
}

/* In gcc we can use nested function
   definitions but not for other compilers */
static double *zxcopy, *zxcopy2, *Delta, *values;
static dimPTR dd;
static int *setngs, *pdC;
size_t zxdim;

static double
logLik_fun( double *pars, double *sigma) // 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
{				/* defined for finite differences */
    Memcpy( zxcopy2, zxcopy, zxdim );
    return internal_loglik(dd, zxcopy2, generate_DmHalf( Delta, dd, pdC, pars ),
			   setngs, DNULLP, DNULLP, sigma ); // 17-11-2015; Fixed sigma ...
}

static double
negLogLik_fun( double *pars, double *sigma) // 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
{				/* defined for finite differences */
    Memcpy( zxcopy2, zxcopy, zxdim );
    return - internal_loglik(dd, zxcopy2, generate_DmHalf( Delta, dd, pdC, pars ),
			     setngs, DNULLP, DNULLP, sigma ); // 17-11-2015; Fixed sigma ...
}

void
mixed_loglik(double *ZXy, int *pdims, double *pars, int *settings,
	     double *logLik, double *lRSS, double *sigma)
// 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
{				/* evaluate the log-likelihood */
    dd = dims(pdims);
    /* settings gives RML, asDelta, gradHess, and pdClass in that order */
    if (settings[ 1 ]) {		/* gradHess not used and pdClass ignored */
	*logLik = internal_loglik( dd, ZXy, pars, settings, DNULLP, lRSS, sigma); // 17-11-2015; ...
    } else {			/* generate the Delta arrays from pars */
	setngs = settings;
	pdC = setngs + 3; // pointer to pdClass (integer code)
	Delta = R_Calloc( (dd->DmOff)[ dd->Q ], double );

	if (settings[ 2 ] == 0) {	/* no gradient or Hessian */
	    *logLik =
		internal_loglik( dd, ZXy, generate_DmHalf( Delta, dd, pdC, pars ),
				 settings, DNULLP, lRSS, sigma ); // 17-11-2015; ...
	} else {
	    int npar = count_DmHalf_pars( dd, pdC );
	    zxdim = (dd->ZXrows) * (dd->ZXcols);
	    zxcopy = R_Calloc( zxdim, double );
	    zxcopy2 = ZXy;

	    Memcpy( zxcopy, ZXy, zxdim );
	    finite_diff_Hess( logLik_fun, pars, npar, logLik, sigma); // 17-11-2015; ...
	    R_Free( zxcopy );
	}
	R_Free( Delta );
    }
    dimFree( dd );
}

void				/* loglikelihood and parameter estimates */
mixed_estimate(double *ZXy, int *pdims, double *DmHalf, int *RML,
	       double *logLik, double *dc, int *invert,
	       // 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Sol.:
	       double *sigma)
{				/* dc receives the decomposed ZXy array */
    dimPTR dd = dims(pdims);

    *logLik = internal_loglik(dd, ZXy, DmHalf, RML, dc, DNULLP, sigma);
    internal_estimate(dd, dc);
    if (*invert != 0) { internal_R_invert( dd, dc ); }
    dimFree(dd);
}

void				/* EM iterations for mixed-effects models */
internal_EM(dimPTR dd, double *ZXy, double *DmHalf, int nn, int *pdClass,
	    int *RML, double *logLik, double *Ra, double *lRSS,
	    // 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions:
	    double *sigma)
{
    double sigmainv, *res, *pt,
	*dc = R_Calloc((size_t) ((dd->Srows) * (dd->ZXcols)), double),
	*zxcopy = R_Calloc((size_t) ((dd->ZXrows) * (dd->ZXcols)), double);
    double  sqrtDF = sqrt((double) (dd->N - *RML * (dd->ncol[dd->Q])));
    int i, j, k, offset;

    while (nn-- > 0) {
	copy_mat(zxcopy, dd->ZXrows, ZXy, dd->ZXrows, dd->ZXrows, dd->ZXcols);
	*logLik = internal_loglik(dd, zxcopy, DmHalf, RML, dc, DNULLP, sigma);
	internal_estimate( dd, dc );
	internal_R_invert( dd, dc );
	// 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
	if (*sigma > 0) {
	    sigmainv = 1.0/(*sigma);
	} else {
	    sigmainv = *(dc + (size_t)((dd->Srows) * (dd->ZXcols)) - 1)/sqrtDF;
	    if (sigmainv == 0.) {
		error(_("Overfitted model!"));
	    } else {
		sigmainv = 1.0/((sigmainv < 0.0) ? - sigmainv : sigmainv);
	    }
	}
	offset = ((dd->ZXcols) - 1) * (dd->Srows);
	for (i = 0; i < (dd->Q); i++) {
	    int ncol = (dd->q)[i],
		nright = (dd->nrot)[i] - (dd->nrot)[(dd->Q) - ( (*RML) ? 0 : 1 )];
	    int nrow = (ncol + nright + 1) * (dd->ngrp)[i];
	    QRptr qq;
	    pt = res = R_Calloc((size_t) (ncol * nrow), double);
	    for (j = 0; j < (dd->ngrp)[i]; j++) {
		copy_trans(pt, nrow, dc + (dd->SToff)[i][j], dd->Srows,
			   ncol, ncol + nright);
		pt += ncol + nright;
		scale_mat(pt++, nrow, sigmainv, dc + offset + (dd->SToff)[i][j],
			  1, 1, ncol);
	    }
	    offset -= (dd->Srows) * ncol;
	    qq = QR(res, nrow, nrow, ncol);
	    QRstoreR(qq, Ra + (dd->DmOff)[i], ncol);
	    QRfree(qq);
	    scale_mat(res, nrow, sqrt(1.0/((dd->ngrp)[i])),
		      Ra + (dd->DmOff)[i], ncol, ncol, ncol);
	    switch (pdClass[i]) {
	    case 0: case 4:			/* default: unstructured */
		invert_upper(res, nrow, ncol);
		copy_trans(DmHalf + (dd->DmOff)[i], ncol, res, nrow, ncol, ncol);
		break;
	    case 1:			/* diagonal */
		for (j = 0; j < ncol; j++) {
		    DmHalf[ (dd->DmOff)[i] + j * (ncol + 1)] =
			1. / sqrt( d_sum_sqr( res + j * nrow, j + 1 ) );
		}
		break;
	    case 2:			/* multiple of identity */
	    {
		double aux = 0.0;
		for(j = 0; j < ncol; j++) {
		    aux += d_sum_sqr( res + j * nrow, j + 1 );
		}
		aux = sqrt(ncol / aux);
		for(j = 0; j < ncol; j++) {
		    DmHalf[(dd->DmOff)[i] + j * (ncol + 1)] = aux;
		}
	    }
	    break;
	    case 3:			/* compound symmetry */
	    {
		double trA = 0.0, trAJ = 0.0, *auxRes;
		int l;
		for(j = 0; j < ncol; j++) {
		    for(k = 0; k <= j; k++) {
			trA += res[k + j * nrow] * res[k + j * nrow];
			for(l = j + 1; l < ncol; l++) {
			    trAJ += res[k + j * nrow] * res[k + l * nrow];
			}
		    }
		}
		trAJ = 2 * trAJ + trA;
		trA = (ncol - 1) / (ncol * trA - trAJ);
		trAJ = 1/trAJ - trA;
		trA = ncol * trA + trAJ;
		auxRes = DmHalf + (dd->DmOff[i]);
		for(j = 0; j < ncol; j++) {
		    auxRes[j * (ncol + 1)] = trA;
		    for(k = (j + 1); k < ncol; k++) {
			auxRes[j * ncol + k] = auxRes[j + k * ncol] = trAJ;
		    }
		}
		F77_CALL(chol)(auxRes, &ncol, &ncol, auxRes, &l);
	    }
	    break;
	    }
	    R_Free(res);
	}
    }
    copy_mat(zxcopy, dd->ZXrows, ZXy, dd->ZXrows, dd->ZXrows, dd->ZXcols);
    *logLik = internal_loglik(dd, zxcopy, DmHalf, RML, dc, lRSS,
			      // 17-11-2015; Fixed sigma patch :
			      sigma);
    R_Free(dc); R_Free(zxcopy);
}

void
mixed_EM(double *ZXy, int *pdims, double *DmHalf, int *nIter, int *pdClass,
	 int *RML, double *logLik, double *Ra, double *lRSS,
	 // 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
	 double *sigma)
{
    dimPTR dd = dims(pdims);
    internal_EM(dd, ZXy, DmHalf, *nIter, pdClass, RML, logLik, Ra, lRSS, sigma);
    dimFree(dd);
}

void				/* to be called by Fortran msmnh */
mixed_calcf(int *n, double *theta, int *nf, double *f, int *uiparm,
	    double *urparm, void (*ufparm)(void))
{
    Memcpy( zxcopy2, zxcopy, zxdim );
    *f = - internal_loglik(dd, zxcopy2, generate_DmHalf( Delta, dd, pdC, theta ),
			   setngs, DNULLP, DNULLP,
			   // 17-11-2015; Fixed sigma patch; E van Willigen; Quant.Sol.
			   _sigma_ );
}

void				/* to be called by Fortran msmnh */
mixed_calcgh(int *n, double *theta, int *nf,
	     double *g, double *h, int *uiparm,
	     double *urparm, void (*ufparm)(void))
{
    int i, nn = *n;
    double *hpt = values + nn + 1;

    finite_diff_Hess(negLogLik_fun, theta, (int) nn, values,
		     // 17-11-2015; Fixed sigma patch; E van Willigen; Quant.Sol.
		     _sigma_ );
    Memcpy( g, values + 1, nn );
    for( i = 1; i <= nn; i++ ) {	/* copy upper triangle of Hessian */
	Memcpy( h, hpt, i );
	h += i;
	hpt += nn;
    }
}

static double *
crossprod_mat(double *y, int ldy, double *x, int ldx,
	      int nrow, int ncol) /* y <- t(x) %*% x */
{
    int i, j;

    for( i = 0; i < ncol; i++ ) {
	y[ i * ldy + i ] = d_dot_prod( x + i * ldx, 1, x + i * ldx, 1, nrow );
	for( j = 0; j < i; j++) {
	    y[ i * ldy + j ] = y[ j * ldy + i ] =
		d_dot_prod( x + i * ldx, 1, x + j * ldx, 1, nrow );
	}
    }
    return y;
}

/* Forming the parameter structure from the Delta matrix */
/*  Not sure if these will ever be called from S. */
/*  Will leave open the possibility. */

static void
Delta2MatrixLog( double *theta, int *q, double *Delta )
{
    int i, j, qq = *q, one = 1, info = 0;
    if ( qq == 1 ) {
	*theta = log(*Delta * *Delta)/2.;
    } else {
	double *vectors = R_Calloc((size_t) qq * qq, double),
	    *DtransD = R_Calloc((size_t) qq * qq, double),
	    *workmat = R_Calloc((size_t) qq * qq, double),
	    *work2 = R_Calloc((size_t) qq, double),
	    *values = R_Calloc((size_t) qq, double), *pt;
	crossprod_mat(DtransD, qq, Delta, qq, qq, qq); /* form t(Delta) %*% Delta */
	F77_CALL(rs) (q, q, DtransD, values, &one, vectors, workmat, work2, &info);
	if (info != 0) {
	    error(_("Unable to form eigenvalue-eigenvector decomposition [RS(.) ierr = %d]"), info);
	}
	copy_mat(workmat, qq, vectors, qq, qq, qq);
	for(i = 0; i < qq; i++) {
	    values[i] = log(values[i])/2;
	    for(j = 0; j < qq; j++) {
		workmat[i * qq + j] *= values[i];
	    }
	}
	copy_trans(DtransD, qq, workmat, qq, qq, qq);
	mult_mat(workmat, qq, vectors, qq, qq, qq, DtransD, qq, qq);
	for( i = 0, pt = theta; i < qq; i++ ) {
	    for( j = 0; j <= i; j++ ) {
		*pt++ = workmat[ i * qq + j ];
	    }
	}
	R_Free(vectors); R_Free(DtransD); R_Free(workmat), R_Free(work2); R_Free(values);
    }
}

static void
Delta2LogCholesky(double *theta, int *q, double *Delta )
{
    int i, qq = *q, info = 0;
    if ( qq == 1 ) {
	*theta = log(*Delta * *Delta)/2.;
    } else {
	double *ll = theta + qq,
	    *DtransD = R_Calloc((size_t) qq * qq, double);
	crossprod_mat(DtransD, qq, Delta, qq, qq, qq); /* form t(Delta) %*% Delta */
	F77_CALL(chol) (DtransD, &qq, &qq, Delta, &info); /* re-writes Delta */
	if (info != 0)
	    error(_("Unable to form Cholesky decomposition: the leading minor of order %d is not pos.def."),
		  info);
	*theta = log(Delta[0]);
	for(i = 1; i < qq; i++) {
	    theta[i] = log(Delta[i * (qq + 1)]);
	    Memcpy(ll, Delta + i * qq, i);
	    ll += i;
	}
	R_Free(DtransD);
    }
}

double *
generate_theta( double *theta, dimPTR dd, int *pdClass, double *DmHalf )
{				/* Expand parameters to DmHalf arrays */
    int i, j, q, Q = dd->Q;
    for (i = 0; i < Q; i++) {
	q = (dd->q)[ i ];
	switch (pdClass[i]) {
	case 0:			/* default: unstructured */
	    Delta2MatrixLog( theta, dd->q + i, DmHalf + (dd->DmOff)[ i ] );
	    theta += (q * (q + 1))/2;
	    break;
	case 1:			/* diagonal */
	    for (j = 0; j < q; j++) {
		*theta++ = log( DmHalf[ (dd->DmOff)[i] + j * (q + 1) ] );
	    }
	    break;
	case 2:			/* multiple of identity */
	    *theta++ = log( DmHalf[(dd->DmOff)[i]] );
	    break;
	case 3:			/* compound symmetry */
	    error(_("Haven't written the compound symmetry case for this yet"));
	    break;
	case 4:			/* default: unstructured */
	    Delta2LogCholesky( theta, dd->q + i, DmHalf + (dd->DmOff)[ i ] );
	    theta += (q * (q + 1))/2;
	    break;
	}
    }
    return theta;
}


// This is called _only_ from R's  nlme :: simulate.lme():
void				/* both EM and Newton-Raphson iterations */
mixed_combined(double *ZXy, int *pdims, double *DmHalf, int *nIter,
	       int *pdClass, int *RML, double *logLik, double *R0,
	       double *lRSS, int *info,
	       // 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
	       double *sigma)
{
    int i, j;
    double *Ra, *dc, *work;

    dd = dims(pdims);		/* Using global dd, pdC, setngs, and Delta */
    pdC = pdClass;
    setngs = RML;

    dc = R_Calloc((size_t) ((dd->Srows) * (dd->ZXcols)), double);

    Ra = R_Calloc( dd->DmOff[dd->Q], double);
    internal_decomp( dd, ZXy );	/* take decomp if useful */

				/* check for non-zero entries in DmHalf */
    if ( d_sum_sqr( DmHalf, dd->DmOff[ dd->Q ]) == 0. ) {
	work = ZXy;			/* create starting estimates */
	Delta = DmHalf;
	for( i = 0; i < dd->Q; i++ ) {
	    for ( j = 0; j < (dd->q)[i]; j++ ) {
		*Delta = 0.375 * sqrt( d_sum_sqr( work, dd->ZXrows ) / (dd->ngrp)[i]);
		Delta += (dd->q)[i] + 1;
		work += dd->ZXrows;
	    }
	    Delta -= (dd->q)[i];	/* have moved too far - step back */
	}
    }
    internal_EM(dd, ZXy, DmHalf, *nIter, pdClass, RML, logLik, Ra, lRSS,
		sigma); // 17-11-2015; Fixed sigma patch ...
    {
	statePTR st = R_Calloc(1, struct state_struct);
	int ntheta = count_DmHalf_pars( dd, pdC ), itrmcd, itncnt,
	    p = dd->ncol[dd->Q], iagflg;
	double
	    *theta = R_Calloc(ntheta, double),
	    *typsiz = R_Calloc(ntheta, double),
	    *grad = R_Calloc(ntheta, double),
	    *newtheta = R_Calloc(ntheta, double),
	    *a = R_Calloc(ntheta * ntheta, double),
	    *work = R_Calloc(ntheta * 9, double);

	st->dd = dd;
	st->ZXy = ZXy;
	st->pdClass = pdClass;
	st->RML = RML;
	st->sigma = sigma; // 17-11-2015; Fixed sigma patch; ...

	generate_theta(theta, dd, pdClass, DmHalf);

	*info = 9;			/* don't inhibit checks but suppress output */
	for (i = 0; i < ntheta; i++) { typsiz[i] = 1.0; }
/*      iagflg = 1; */
	iagflg = 0;
	for (i = 0; i < dd->Q; i++) {
	    if (pdClass[i] < 1 || pdClass[i] == 3 || pdClass[i] > 4) {
		iagflg = 0;
		break;
	    }
	}

	optif9(ntheta, ntheta, theta, (fcn_p) mixed_fcn, (fcn_p)
	       mixed_grad, (d2fcn_p) 0, st, typsiz, 1.0 /*fscale*/,
	       1 /*method*/,1 /*iexp*/, info, -1 /*ndigit*/, 50 /*itnlim*/,
	       iagflg, 0 /*iahflg*/, 1. /*dlt*/, pow(DBL_EPSILON, 0.25)
	       /*gradtl*/, 0. /*stepmx*/, sqrt(DBL_EPSILON) /*steptl*/,
	       newtheta, logLik, grad, &itrmcd, a, work, &itncnt);
	if (*info == 0) {
	    *logLik = internal_loglik( dd, ZXy,
				       generate_DmHalf( DmHalf, dd, pdC, theta ),
				       setngs, dc, lRSS,
				       // 17-11-2015; Fixed sigma patch ... :
				       sigma );
	    copy_mat(R0, p, dc + (dd->SToff)[(dd->Q)][0], (dd->Srows), p, p + 1);
	}
	R_Free(work); R_Free(a); R_Free(newtheta); R_Free(grad);
	R_Free(typsiz); R_Free(theta); R_Free(st);
    }
    dimFree( dd ); R_Free( dc ); R_Free( Ra );
}

/* functions for calculating df's for fixed effects tests */

static double
inner_perc(double *x, int *grp, int n)
    /* percentage of groups for which x is inner */
{
    /* x - column of X matrix to be assessed
       grp - integer vector with groups
       n - length of x and grp
       data are assumed to be ordered by grp */

    int currGrp, nn = 0, isInner;
    double nInner = 0., nGrp = 0., currVal;

    while (nn < n) {
	currGrp = grp[nn];
	currVal = x[nn];
	nGrp++;
	isInner = 0;
	do {
	    if (isInner == 0 && x[nn] != currVal) {
		nInner++;
		isInner = 1;
	    }
	    nn++;
	} while (nn < n && currGrp == grp[nn]);
    }
    return(nInner/nGrp);
}

void
inner_perc_table(double *X, int *grps, int *p, int *Q, int *n, double *pTable)
    /* constructs an p x Q "inner-percentage" table for a fixed effects
       matrix X and a set of grouping vectors grps */
{
    int i, j, pp = *p, nn = *n, ipp = 0, inn = 0;
    for(i = 0; i < *Q; i++) {
	for(j = 0; j < pp; j++) {
	    pTable[j + ipp] = inner_perc(X + j * nn, grps + inn, nn);
	}
	ipp += pp;
	inn += nn;
    }
}

/* gls functions */
void
gls_loglik(double *Xy, int *pdims, double *logLik, double *lRSS,
	   // 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
	   double *sigma)
{
    int i, N = pdims[0], p = pdims[1], RML = pdims[2],
	Np1 = N + 1, Nr = N - RML * p, rnkm1;
    QRptr dmQR;

    dmQR = QR(Xy, N, N, p + 1);
    rnkm1 = (dmQR->rank) - 1;
    if(rnkm1 != p) {
	*logLik = -DBL_MAX;
    } else {
	// 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
	double f;
	*lRSS = log(f=fabs (dmQR->mat[p * Np1]));
	if (*sigma > 0) { // fixed sigma
	    double h = 0;
	    if (RML == 1) {
		for(i = 0; i < p; i++) {
		    h += log(fabs(dmQR->mat[i * Np1]));
		}
	    }
	    *logLik -= pow(f,2)/(2*pow(*sigma,2));
	    *logLik -= Nr * log(*sigma);
	    *logLik -= h;
	} else { // estimated sigma (default)
	    *logLik -= (((double)Nr) * (*lRSS));
	    if (RML == 1) {
		for (i = 0; i < p; i++) {
		    *logLik -= log(fabs(dmQR->mat[i * Np1]));
		}
	    }
    	}
    }
    QRfree(dmQR);
}

#if 0
/* gls functions */
void
gls_loglik(double *Xy, int *pdims, double *logLik, double *lRSS)
{
    int i, N = pdims[0], p = pdims[1], RML = pdims[2],
	Np1 = N + 1, Nr = N - RML * p;
    QRptr dmQR;

    dmQR = QR(Xy, N, N, p + 1);
    *lRSS = log(fabs(dmQR->mat[p * Np1]));
    *logLik -= Nr * (*lRSS);
    if (RML == 1) {
	for(i = 0; i < p; i++) {
	    *logLik -= log(fabs(dmQR->mat[i * Np1]));
	}
    }
    QRfree(dmQR);
}
#endif

void
gls_estimate(double *Xy, int *pdims, double *beta, double *sigma,
	     double *logLik, double *varBeta, int *rank, int *pivot)
{
    int i, N = pdims[0], p = pdims[1], RML = pdims[2], pp1 = p + 1,
	Nr = N - RML * p, rk, rkm1, rkp1;
    QRptr dmQR;
    double *R = R_Calloc((size_t) (pp1 * pp1), double);

    dmQR = QR(Xy, N, N, pp1);
    *rank = rk = dmQR->rank;
    rkm1 = rk - 1;
    rkp1 = rk + 1;
    Memcpy(pivot, dmQR->pivot, pp1);
    for(i = 0; i < rk; i++) {
	Memcpy(R + i * rk, dmQR->mat + i * N, i + 1);
    }
    // 17-11-2015; Fixed sigma patch; E van Willigen; Quantitative Solutions
    if (*sigma > 0) { // fixed sigma
	double h = 0;
	*logLik = fabs(R[rk * rk - 1]);
	if (RML == 1) { // RML
	    for(i=0; i<rkm1; i++) {
		h += log(fabs(R[i * (rkp1)]));
	    }
	}
	*logLik = -pow(*logLik, 2)/(2*(pow(*sigma,2)));
	*logLik -= Nr * log(*sigma);
	*logLik -= h;
    } else {	// estimated sigma (default)
    	*sigma = fabs(R[rk * rk - 1]);
    	*logLik -= Nr * log(*sigma);
    	*sigma /= sqrt(((double) Nr));
	if (RML == 1) {
	    for(i = 0; i < rkm1; i++) {
		*logLik -= log(fabs(R[i * (rkp1)]));
	    }
	}
    }
    copy_mat(varBeta, rkm1, R, rk, rkm1, rkm1);
    invert_upper(varBeta, rkm1, rkm1);
    mult_mat(beta, rkm1, varBeta, rkm1, rkm1, rkm1, R + rkm1 * rk, rk,  1);
    QRfree(dmQR);
    R_Free(R);
}
