cs *cs_rR(const cs *A, double nu, double nuR, const css *As, const cs *Roldinv, double Roldldet, const cs *pG){
    
	cs *Rnew, *Rnewinv, *Ainv;
	double Rnewldet, MH;
        int dimG = A->n;
	int cnt = 0;
	int i, j;
	
	Rnewinv = cs_spalloc (dimG, dimG, dimG*dimG, 1, 0);
	
	for (i = 0 ; i < dimG; i++){
	  Rnewinv->p[i] = i*dimG;
	  for (j = 0 ; j < dimG; j++){
		Rnewinv->i[cnt] = j;
		Rnewinv->x[cnt] = 0.0;
                A->x[i*dimG+j] -= pG->x[i*dimG+j];
 		cnt++;
	  }
	}
	Rnewinv->p[dimG] = dimG*dimG;
		
	cs_cov2cor(A);
	Ainv = cs_inv(A);
	
	Rnew = cs_rinvwishart(Ainv, nu, As);	
	cs_cov2cor(Rnew);
		
	Rnewldet = log(cs_invR(Rnew, Rnewinv));

/*****************************************************/
/*       From Eq A.4 in Liu and Daniels (2006)       */
/*       using \pi_{1} = Eq 6 in Barnard (2000)      */
/*  using \pi_{2} = Eq 3.4 in Liu and Daniels (2006) */
/*****************************************************/

        MH = Roldldet-Rnewldet;
 
	for (i = 0 ; i < dimG; i++){
          MH += log(Roldinv->x[i*dimG+i]);
          MH -= log(Rnewinv->x[i*dimG+i]);
	}

	MH *= 0.5*nuR;

	if(MH<log(runif(0.0,1.0)) || Rnewldet<log(Dtol)){
	  Rnewldet = cs_invR(Roldinv, Rnew);	// save old R	
        }

        for (i = 0 ; i < dimG; i++){
          for (j = 0 ; j < dimG; j++){
 	    Rnew->x[i*dimG+j] *= sqrt((pG->x[i*dimG+i])*(pG->x[j*dimG+j]));
          }
        }

        cs_spfree(Rnewinv);
        cs_spfree(Ainv);

    return (cs_done (Rnew, NULL, NULL, 1)) ;	/* success; free workspace, return C */

}

ACADOcsparse::~ACADOcsparse()
{
	if (index1 != 0)
		delete[] index1;
	if (index2 != 0)
		delete[] index2;
	if (x != 0)
		delete[] x;

	if (S != 0)
		cs_free(S);
	if (N != 0)
	{

		if (N->L != 0)
			cs_spfree(N->L);
		if (N->U != 0)
			cs_spfree(N->U);
		if (N->pinv != 0)
			free(N->pinv);
		if (N->B != 0)
			free(N->B);
		free(N);
	}
}

/**
 * @brief Creates the penalty matrix D tilde of order k.
 * Returns the matrix Dk premultipied by a diagonal
 * matrix of weights.
 *
 * @param n                    number of observations
 * @param k                    order of the trendfilter
 * @param x                    locations of the responses
 * @return pointer to a csparse matrix
 * @see tf_calc_dktil
 */
cs * tf_calc_dktil (int n, int k, const double * x)
{
  cs * delta_k;
  cs * delta_k_cp;
  cs * Dk;
  cs * Dktil;

  int i;

  Dk = tf_calc_dk(n, k, x);

  /* Deal with k=0 separately */
  if(k == 0)
    return Dk;

  /* Construct diagonal matrix of differences: */
  delta_k = cs_spalloc(n-k, n-k, (n-k), 1, 1);
  for(i = 0; i < n - k; i++)
  {
    delta_k->p[i] = i;
    delta_k->i[i] = i;
    delta_k->x[i] = k / (x[k + i] - x[i]);
  }
  delta_k->nz = n-k;
  delta_k_cp = cs_compress(delta_k);
  Dktil = cs_multiply(delta_k_cp, Dk);

  cs_spfree(Dk);
  cs_spfree(delta_k);
  cs_spfree(delta_k_cp);

  return Dktil;
}

/* cs_symperm: symmetric permutation of a symmetric sparse matrix. */
void mexFunction
(
    int nargout,
    mxArray *pargout [ ],
    int nargin,
    const mxArray *pargin [ ]
)
{
    cs Amatrix, *A, *C, *D ;
    csi ignore, n, *P, *Pinv ;
    if (nargout > 1 || nargin != 2)
    {
        mexErrMsgTxt ("Usage: C = cs_symperm(A,p)") ;
    }
    A = cs_mex_get_sparse (&Amatrix, 1, 1, pargin [0]) ;
    n = A->n ;
    P = cs_mex_get_int (n, pargin [1], &ignore, 1) ;    /* get P */
    Pinv = cs_pinv (P, n) ;                             /* P=Pinv' */
    C = cs_symperm (A, Pinv, 1) ;                       /* C = A(p,p) */
    D = cs_transpose (C, 1) ;                           /* sort C */
    cs_spfree (C) ;
    C = cs_transpose (D, 1) ;
    cs_spfree (D) ;
    pargout [0] = cs_mex_put_sparse (&C) ;              /* return C */
    cs_free (P) ;
    cs_free (Pinv) ;
}

returnValue ACADOcsparse::setMatrix(double *A_)
{
	int run1;
	int order = 0;

	if (dim <= 0)
		return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
	if (nDense <= 0)
		return ACADOERROR(RET_MEMBER_NOT_INITIALISED);

	cs *C, *D;
	C = cs_spalloc(0, 0, 1, 1, 1);

	for (run1 = 0; run1 < nDense; run1++)
		cs_entry(C, index1[run1], index2[run1], A_[run1]);

	D = cs_compress(C);
	S = cs_sqr(order, D, 0);
	N = cs_lu(D, S, TOL);

	cs_spfree(C);
	cs_spfree(D);

	return SUCCESSFUL_RETURN;
}

scs_int factorize(const AMatrix * A, const Settings * stgs, Priv * p) {
	scs_float *info;
	scs_int *Pinv, amd_status, ldl_status;
	cs *C, *K = formKKT(A, stgs);
	if (!K) {
		return -1;
	}
	amd_status = LDLInit(K, p->P, &info);
	if (amd_status < 0)
		return (amd_status);
#if EXTRAVERBOSE > 0
	if(stgs->verbose) {
		scs_printf("Matrix factorization info:\n");
#ifdef DLONG
		amd_l_info(info);
#else
		amd_info(info);
#endif
	}
#endif
	Pinv = cs_pinv(p->P, A->n + A->m);
	C = cs_symperm(K, Pinv, 1);
	ldl_status = LDLFactor(C, NULL, NULL, &p->L, &p->D);
	cs_spfree(C);
	cs_spfree(K);
	scs_free(Pinv);
	scs_free(info);
	return (ldl_status);
}

/* cs_permute: permute a sparse matrix */
void mexFunction
(
    int nargout,
    mxArray *pargout [ ],
    int nargin,
    const mxArray *pargin [ ]
)
{
    cs Amatrix, *A, *C, *D ;
    int ignore, *P, *Q, *Pinv ;
    if (nargout > 1 || nargin != 3)
    {
        mexErrMsgTxt ("Usage: C = cs_permute(A,p,q)") ;
    }
    A = cs_mex_get_sparse (&Amatrix, 0, 1, pargin [0]) ;    /* get A */
    P = cs_mex_get_int (A->m, pargin [1], &ignore, 1) ;     /* get P */
    Q = cs_mex_get_int (A->n, pargin [2], &ignore, 1) ;     /* get Q */
    Pinv = cs_pinv (P, A->m) ;              /* P = Pinv' */
    C = cs_permute (A, Pinv, Q, 1) ;        /* C = A(p,q) */
    D = cs_transpose (C, 1) ;               /* sort C via double transpose */
    cs_spfree (C) ;
    C = cs_transpose (D, 1) ;
    cs_spfree (D) ;
    pargout [0] = cs_mex_put_sparse (&C) ;          /* return C */
    cs_free (Pinv) ;
    cs_free (P) ;
    cs_free (Q) ;
}

NumericsSparseMatrix* freeNumericsSparseMatrix(NumericsSparseMatrix* A)
{
  if (A->linearSolverParams)
  {
    freeNumericsSparseLinearSolverParams(A->linearSolverParams);
    A->linearSolverParams = NULL;
  }
  if (A->triplet)
  {
    cs_spfree(A->triplet);
    A->triplet = NULL;
  }
  if (A->csc)
  {
    cs_spfree(A->csc);
    A->csc = NULL;
  }
  if (A->trans_csc)
  {
    cs_spfree(A->trans_csc);
    A->trans_csc = NULL;
  }
  if (A->csr)
  {
    cs_spfree(A->csr);
    A->csr = NULL;
  }
  return NULL;
}

/* cs_add: sparse matrix addition */
void mexFunction
(
    int nargout,
    mxArray *pargout [ ],
    int nargin,
    const mxArray *pargin [ ]
)
{
    double alpha, beta ;
    cs Amatrix, Bmatrix, *A, *B, *C, *D ;
    if (nargout > 1 || nargin < 2 || nargin > 4)
    {
        mexErrMsgTxt ("Usage: C = cs_add(A,B,alpha,beta)") ;
    }
    A = cs_mex_get_sparse (&Amatrix, 0, 1, pargin [0]) ;    /* get A */
    B = cs_mex_get_sparse (&Bmatrix, 0, 1, pargin [1]) ;    /* get B */
    alpha = (nargin < 3) ? 1 : mxGetScalar (pargin [2]) ;   /* get alpha */
    beta  = (nargin < 4) ? 1 : mxGetScalar (pargin [3]) ;   /* get beta */
    C = cs_add (A,B,alpha,beta) ;       /* C = alpha*A + beta *B */
    cs_dropzeros (C) ;                  /* drop zeros */
    D = cs_transpose (C, 1) ;           /* sort result via double transpose */
    cs_spfree (C) ;
    C = cs_transpose (D, 1) ;
    cs_spfree (D) ;
    pargout [0] = cs_mex_put_sparse (&C) ;      /* return C */
}

/* Modified version of Tim Davis's cs_lu_mex.c file for MATLAB */
void install_lu(SEXP Ap, int order, double tol, Rboolean err_sing)
{
    // (order, tol) == (1, 1) by default, when called from R.
    SEXP ans;
    css *S;
    csn *N;
    int n, *p, *dims;
    CSP A = AS_CSP__(Ap), D;
    R_CheckStack();

    n = A->n;
    if (A->m != n)
	error(_("LU decomposition applies only to square matrices"));
    if (order) {		/* not using natural order */
	order = (tol == 1) ? 2	/* amd(S'*S) w/dense rows or I */
	    : 1;		/* amd (A+A'), or natural */
    }
    S = cs_sqr(order, A, /*qr = */ 0);	/* symbolic ordering */
    N = cs_lu(A, S, tol);	/* numeric factorization */
    if (!N) {
	if(err_sing)
	    error(_("cs_lu(A) failed: near-singular A (or out of memory)"));
	else {
	    /* No warning: The useR should be careful :
	     * Put  NA  into  "LU" factor */
	    set_factors(Ap, ScalarLogical(NA_LOGICAL), "LU");
	    return;
	}
    }
    cs_dropzeros(N->L);		/* drop zeros from L and sort it */
    D = cs_transpose(N->L, 1);
    cs_spfree(N->L);
    N->L = cs_transpose(D, 1);
    cs_spfree(D);
    cs_dropzeros(N->U);		/* drop zeros from U and sort it */
    D = cs_transpose(N->U, 1);
    cs_spfree(N->U);
    N->U = cs_transpose(D, 1);
    cs_spfree(D);
    p = cs_pinv(N->pinv, n);	/* p=pinv' */
    ans = PROTECT(NEW_OBJECT(MAKE_CLASS("sparseLU")));
    dims = INTEGER(ALLOC_SLOT(ans, Matrix_DimSym, INTSXP, 2));
    dims[0] = n; dims[1] = n;
    SET_SLOT(ans, install("L"),
	     Matrix_cs_to_SEXP(N->L, "dtCMatrix", 0));
    SET_SLOT(ans, install("U"),
	     Matrix_cs_to_SEXP(N->U, "dtCMatrix", 0));
    Memcpy(INTEGER(ALLOC_SLOT(ans, Matrix_pSym, /* "p" */
			      INTSXP, n)), p, n);
    if (order)
	Memcpy(INTEGER(ALLOC_SLOT(ans, install("q"),
				  INTSXP, n)), S->q, n);
    cs_nfree(N);
    cs_sfree(S);
    cs_free(p);
    UNPROTECT(1);
    set_factors(Ap, ans, "LU");
}

int cs_rechol(const cs *A, const csn *N, int *pinv, int *c, double *x) {

	double d, lki;
	double *Lx, *Cx;
	int t, i, p, k, n, *Li, *Lp, *Ui, *Up, *Cp, *Ci;
	cs *C, *E;
	if (!CS_CSC (A) || !N || !N->L || !N->U || !c || !x)
		return (0);
	n = A->n;
	C = pinv ? cs_symperm(A, pinv, 1) : ((cs *) A);
	E = pinv ? C : NULL; /* E is alias for A, or a copy E=A(p,p) */
	if (!C) {
		cs_spfree(E);
		return (0);
	}
	Cp = C->p;
	Ci = C->i;
	Cx = C->x;
	Lp = N->L->p;
	Li = N->L->i;
	Lx = N->L->x;
	Up = N->U->p;
	Ui = N->U->i;
	for (k = 0; k < n; k++) /* compute L(k,:) for L*L' = C */
	{
		x[k] = 0; /* x (0:k) is now zero */
		for (p = Cp[k]; p < Cp[k + 1]; p++) /* x = full(triu(C(:,k))) */
		{
			if (Ci[p] <= k)
				x[Ci[p]] = Cx[p];
		}
		d = x[k]; /* d = C(k,k) */
		x[k] = 0; /* clear x for k+1st iteration */
		for (t = Up[k]; t < Up[k + 1] - 1; t++) /* solve L(0:k-1,0:k-1) * x = C(:,k) */
		/* the loop does not include the diagonal element of row k of L - or column k of U - */
		/* (which is placed at the end of column k for a sorted matrix - after its transposition), */
		/* because it is treated outside the loop */
		{
			i = Ui[t]; /* pattern of L(k,:) */
			lki = x[i] / Lx[Lp[i]]; /* L(k,i) = x (i) / L(i,i) */
			x[i] = 0; /* clear x for k+1st iteration */
			for (p = Lp[i] + 1; p < c[i]; p++) {
				x[Li[p]] -= Lx[p] * lki;
			}
			d -= lki * lki; /* d = d - L(k,i)*L(k,i) */
			p = c[i]++;
			Lx[p] = lki;
		}
		if (d <= 0) {
			cs_spfree(E);
			return (0);
		} /* not pos def */
		p = c[k]++;
		Lx[p] = sqrt(d);
	}
	cs_spfree(E);
	return (1); /* success */
}

cs* cs_sorted_multiply2(const cs* a, const cs* b)
{
  cs* D = cs_multiply(a,b);
  cs* E = cs_transpose(D,1);
  cs_spfree(D);
  cs* C = cs_transpose(E,1);
  cs_spfree(E);
  return C;
}

/* free a numeric factorization */
csn *cs_nfree (csn *N)
{
    if (!N) return (NULL) ;	/* do nothing if N already NULL */
    cs_spfree (N->L) ;
    cs_spfree (N->U) ;
    cs_free (N->pinv) ;
    cs_free (N->B) ;
    return (cs_free (N)) ;	/* free the csn struct and return NULL */
}

void computeSparseAWpB(
  double *A,
  NumericsMatrix *W,
  double *B,
  NumericsMatrix *AWpB)
{

  /* unsigned int problemSize = W->size0; */

  SparseBlockStructuredMatrix* Wb = W->matrix1;

  assert((unsigned)W->size0 >= 3);
  assert((unsigned)W->size0 / 3 >= Wb->filled1 - 1);

  double Ai[9], Bi[9], tmp[9];

  for (unsigned int row = 0, ip9 = 0, i0 = 0;
       row < Wb->filled1 - 1; ++row, ip9 += 9, i0 += 3)
  {
    assert(ip9 < 3 *  (unsigned)W->size0 - 8);

    extract3x3(3, ip9, 0, A, Ai);
    extract3x3(3, ip9, 0, B, Bi);

    for (unsigned int blockn = (unsigned int) Wb->index1_data[row];
         blockn < Wb->index1_data[row + 1]; ++blockn)
    {

      unsigned int col = (unsigned int) Wb->index2_data[blockn];

      mm3x3(Ai, Wb->block[blockn], tmp);
      if (col == row) add3x3(Bi, tmp);

      cpy3x3(tmp, AWpB->matrix1->block[blockn]);
    }
  }
  /* Invalidation of sparse storage, if any. */
  if (AWpB->matrix2)
  {
    if (AWpB->matrix2->triplet)
    {
      cs_spfree(AWpB->matrix2->triplet);
      AWpB->matrix2->triplet = NULL;
    }
    if (AWpB->matrix2->csc)
    {
      cs_spfree(AWpB->matrix2->csc);
      AWpB->matrix2->csc = NULL;
    }
    if (AWpB->matrix2->trans_csc)
    {
      cs_spfree(AWpB->matrix2->trans_csc);
      AWpB->matrix2->trans_csc = NULL;
    }
  }
}

/* free workspace for demo3 */
static int done3 (int ok, css *S, csn *N, double *y, cs *W, cs *E, int *p)
{
    cs_sfree (S) ;
    cs_nfree (N) ;
    cs_free (y) ;
    cs_spfree (W) ;
    cs_spfree (E) ;
    cs_free (p) ;
    return (ok) ;
}

/* free a problem */
problem *free_problem (problem *Prob)
{
    if (!Prob) return (NULL) ;
    cs_spfree (Prob->A) ;
    if (Prob->sym) cs_spfree (Prob->C) ;
    cs_free (Prob->b) ;
    cs_free (Prob->x) ;
    cs_free (Prob->resid) ;
    return (cs_free (Prob)) ;
}

cs* cs_sorted_multiply(const cs* a, const cs* b)
{
  cs* A = cs_transpose (a, 1) ;
  cs* B = cs_transpose (b, 1) ;
  cs* D = cs_multiply (B,A) ;   /* D = B'*A' */
  cs_spfree (A) ;
  cs_spfree (B) ;
  cs_dropzeros (D) ;      /* drop zeros from D */
  cs* C = cs_transpose (D, 1) ;   /* C = D', so that C is sorted */
  cs_spfree (D) ;
  return C;
}

/**
 * @brief Creates the admittance matrix.
 *
 *    @return 0 on success.
 */
static int econ_createGMatrix (void)
{
   int ret;
   int i, j;
   double R, Rsum;
   cs *M;
   StarSystem *sys;

   /* Create the matrix. */
   M = cs_spalloc( systems_nstack, systems_nstack, 1, 1, 1 );
   if (M == NULL)
      ERR("Unable to create CSparse Matrix.");

   /* Fill the matrix. */
   for (i=0; i < systems_nstack; i++) {
      sys   = &systems_stack[i];
      Rsum = 0.;

      /* Set some values. */
      for (j=0; j < sys->njumps; j++) {

         /* Get the resistances. */
         R     = econ_calcJumpR( sys, &systems_stack[sys->jumps[j]] );
         R     = 1./R; /* Must be inverted. */
         Rsum += R;

         /* Matrix is symetrical and non-diagonal is negative. */
         ret = cs_entry( M, i, sys->jumps[j], -R );
         if (ret != 1)
            WARN("Unable to enter CSparse Matrix Cell.");
         ret = cs_entry( M, sys->jumps[j], i, -R );
         if (ret != 1)
            WARN("Unable to enter CSparse Matrix Cell.");
      }

      /* Set the diagonal. */
      Rsum += 1./ECON_SELF_RES; /* We add a resistence for dampening. */
      cs_entry( M, i, i, Rsum );
   }

   /* Compress M matrix and put into G. */
   if (econ_G != NULL)
      cs_spfree( econ_G );
   econ_G = cs_compress( M );
   if (econ_G == NULL)
      ERR("Unable to create economy G Matrix.");

   /* Clean up. */
   cs_spfree(M);

   return 0;
}

/* cs_lu: sparse LU factorization, with optional fill-reducing ordering */
void mexFunction
(
    int nargout,
    mxArray *pargout [ ],
    int nargin,
    const mxArray *pargin [ ]
)
{
    css *S ;
    csn *N ;
    cs Amatrix, *A, *D ;
    csi n, order, *p ;
    double tol ;
    if (nargout > 4 || nargin > 3 || nargin < 1)
    {
        mexErrMsgTxt ("Usage: [L,U,p,q] = cs_lu (A,tol)") ;
    }
    A = cs_mex_get_sparse (&Amatrix, 1, 1, pargin [0]) ;        /* get A */
    n = A->n ;
    if (nargin == 2)                        /* determine tol and ordering */
    {
        tol = mxGetScalar (pargin [1]) ;
        order = (nargout == 4) ? 1 : 0 ;    /* amd (A+A'), or natural */
    }
    else
    {
        tol = 1 ;
        order = (nargout == 4) ? 2 : 0 ;    /* amd(S'*S) w/dense rows or I */
    }
    S = cs_sqr (order, A, 0) ;              /* symbolic ordering, no QR bound */
    N = cs_lu (A, S, tol) ;                 /* numeric factorization */
    if (!N) mexErrMsgTxt ("cs_lu failed (singular, or out of memory)") ;
    cs_dropzeros (N->L) ;                   /* drop zeros from L and sort it */
    D = cs_transpose (N->L, 1) ;
    cs_spfree (N->L) ;
    N->L = cs_transpose (D, 1) ;
    cs_spfree (D) ;
    cs_dropzeros (N->U) ;                   /* drop zeros from U and sort it */
    D = cs_transpose (N->U, 1) ;
    cs_spfree (N->U) ;
    N->U = cs_transpose (D, 1) ;
    cs_spfree (D) ;
    p = cs_pinv (N->pinv, n) ;              /* p=pinv' */
    pargout [0] = cs_mex_put_sparse (&(N->L)) ;     /* return L */
    pargout [1] = cs_mex_put_sparse (&(N->U)) ;     /* return U */
    pargout [2] = cs_mex_put_int (p, n, 1, 1) ;     /* return p */
    /* return Q */
    if (nargout == 4) pargout [3] = cs_mex_put_int (S->q, n, 1, 0) ;
    cs_nfree (N) ;
    cs_sfree (S) ;
}

void KLUSystem::clear()
{
	if (Y22) cs_spfree (Y22);

	if (T22) cs_spfree (T22);

	if (acx) delete [] acx;

	if (Numeric) klu_z_free_numeric (&Numeric, &Common);
	if (Symbolic) klu_free_symbolic (&Symbolic, &Common);

	zero_indices ();
	null_pointers ();
}