/*
%          [L.L, L.d, skip, diagadd] = blkchol(L,ADA [,pars [,absd]])
% BLKCHOL  Computes sparse lower-triangular Cholesky factor L,
%            L*L' = P(perm,perm)
%   Input Parameter L is typically generated by symbchol.
%   Parameters: pars.canceltol and pars.maxu.
%   Optional: absd to force adding diag if drops below canceltol*absd.
%
%   There are important differences with standard CHOL(P(L.perm,L.perm))':
%
%   -  BLKCHOL uses the supernodal partition XSUPER, possibly splitted
%    by SPLIT, to use dense linear algebra on dense subblocks.
%    Much faster than CHOL.
%
%   -  BLKCHOL never fails. It only sees the lower triangular part of P;
%    if during the elimination, a diagonal entry becomes negative
%    (due to massive cancelation), the corresponding d[k] is set to 0.
%    If d[k] suffers from cancelation and norm(L(:,k)) becomes too big, then
%    the column is skipped, and listed in (skip, Lskip).
%
% SEE ALSO sparchol, fwblkslv, bwblkslv   

    This file is part of SeDuMi 1.05
    Copyright (C) 2001 Jos F. Sturm
      Dept. Econometrics & O.R., Tilburg University, the Netherlands.
      Supported by the Netherlands Organization for Scientific Research (NWO).
    Affiliation SeDuMi 1.03 and 1.04Beta (2000):
      Dept. Quantitative Economics, Maastricht University, the Netherlands.
    Affiliations up to SeDuMi 1.02 (AUG1998):
      CRL, McMaster University, Canada.
      Supported by the Netherlands Organization for Scientific Research (NWO).
  
    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
  
    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, write to the Free Software
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

*/
#include <string.h>
#include "mex.h"
#include "blksdp.h"

#define L_OUT    myplhs[0]
#define D_OUT    myplhs[1]
#define SKIP_OUT  myplhs[2]
#define DIAGADD_OUT  myplhs[3]
#define NPAROUT 4

#define L_IN      prhs[0]
#define P_IN      prhs[1]
#define NPARINMIN 2
#define PARS_IN   prhs[2]
#define ABSD_IN   prhs[3]
#define NPARIN 4

/* ------------------------------------------------------------
   PROTOTYPES:
   ------------------------------------------------------------ */
int blkLDL(const int neqns, const int nsuper, const int *xsuper,
           const int *snode,  const int *xlindx, const int *lindx,
           double *lb,
           const int *ljc, double *lpr, double *d, const int *perm,
           const double ub, const double maxu, int *skipIr,
           int iwsiz, int *iwork, int fwsiz, double *fwork);

/* ============================================================
   SUBROUTINES:
   ============================================================*/
/* ------------------------------------------------------------
   PERMUTEP - Let L = tril(P(perm,perm))
   INPUT
     Ljc, Lir - sparsity structure of output matrix L = tril(P(perm,perm)).
     Pjc, Pir, Ppr - Input matrix, before ordering.
     perm     - length m pivot ordering.
     m        - order: P is m x m.
   WORKING ARRAY
      Pj  - Length m float work array.
   IMPORTANT: L, P and PERM in C style.
   ------------------------------------------------------------ */
void permuteP(const int *Ljc,const int *Lir,double *Lpr,
              const int *Pjc,const int *Pir,const double *Ppr,
              const int *perm, double *Pj, const int m)
{
  int j,inz,jcol;
/* ------------------------------------------------------------
   Let Pj = all-0
   ------------------------------------------------------------ */
  fzeros(Pj,m);
/* ------------------------------------------------------------
   For each column j, let
    Pj(:) = P(:,PERM(j))   and    L(:,j) = Pj(PERM(:))  (L sparse)
   ------------------------------------------------------------ */
  for(j = 0; j < m; j++){
    jcol = perm[j];
    for(inz = Pjc[jcol]; inz < Pjc[jcol+1]; inz++)
      Pj[Pir[inz]] = Ppr[inz];
    for(inz = Ljc[j]; inz < Ljc[j+1]; inz++)
      Lpr[inz] = Pj[perm[Lir[inz]]];
/* ------------------------------------------------------------
   Let Pj = all-0
   ------------------------------------------------------------ */
    for(inz = Pjc[jcol]; inz < Pjc[jcol+1]; inz++)
      Pj[Pir[inz]] = 0.0;
  }
}

/* ************************************************************
   SPCHOL - calls the block cholesky blkLDL.
   INPUT:
      m       - Order of L: L is m x m, ne.At is N x m.
      nsuper  - Number of supernodes (blocks).
      xsuper  - Length nsuper+1: first simple-node of each supernode
      snode   - Length neqns: snode(node) is the supernode containing "node".
      ljc     - Length neqns+1: start of the columns of L.
      abstol  - minimum diagonal value. If 0, then no absolute threshold.
      canceltol  - Force d >= canceltol * orgd (by adding low-rank diag).
      maxu    - Maximal allowed max(abs(L(:,k)))..
         If L gets to big in these columns, we skip the pivots.
      iwsiz, fwsiz - size of integer and floating-point working storage.
               See "WORKING ARRAYS" for required amount.
   UPDATED:
      lindx   - row indices. On INPUT: for each column (by ljc),
          on OUTPUT: for each supernode (by xlindx).
      Lpr     - On input, contains tril(X), on output
	  such that   X = L*diag(d)*L'.
      lb    - Length neqns. INPUT:  diag entries BEFORE cancelation;
          OUTPUT: lb(skipIr) are values of low rank diag. matrix that is
          added before factorization.
   OUTPUT
      xlindx  - Length nsuper+1: Start of sparsity structure in lindx,
              for each supernode (all simple nodes in a supernode have the
              same nonzero-structure).
      snode  - Length m: snode(node) is the supernode containing "node".
      d      - length neqns vector, diagonal in L*diag(d)*L'.
      skipIr - length nskip (<= neqns) array. skipIr(1:nskip) lists the
        columns that have been skipped in the Cholesky. d[skipIr] = 0.
   WORKING ARRAYS:
      iwork  - Length iwsiz working array; iwsiz = 2*m + 2 * nsuper.
      fwork  - Length fwsiz working vector; fwsiz = L.tmpsiz.
   RETURNS - nskip (<=neqns), number of skipped nodes. Length of skipIr.
   *********************************************************************** */
int spchol(const int m, const int nsuper, const int *xsuper,
           int *snode,	int *xlindx, int *lindx, double *lb,
           const int *ljc, double *lpr, double *d, const int *perm,
           const double abstol,
           const double canceltol, const double maxu, int *skipIr,
           int *pnadd,
           const int iwsiz, int *iwork, const int fwsiz, double *fwork)
{
  int jsup,j,ix,jcol,collen, nskip, nadd;
  double ub, dj;
  
/* ------------------------------------------------------------
   Let ub = max(diag(L)) / maxu^2
   ------------------------------------------------------------ */
  ub = 0.0;
  for(j = 0; j < m; j++)
    if((dj = lpr[ljc[j]]) > ub)
      ub = dj;
  ub /= SQR(maxu);
/* ------------------------------------------------------------
   Let lb = MAX(abstol, canceltol * lbIN), where lbIN is diag(L),
   or those quantities before being affected by cancelation.
   ------------------------------------------------------------ */
  for(j = 0; j < m; j++)
    if((dj = canceltol * lb[j]) > abstol)
      lb[j] = dj;
    else
      lb[j] = abstol;
/* ------------------------------------------------------------
   SNODE: map each column to the supernode containing it
   ------------------------------------------------------------ */
  j = xsuper[0];
  for(jsup = 0; jsup < nsuper; jsup++){
    while(j < xsuper[jsup + 1])
      snode[j++] = jsup;
  }
/* ------------------------------------------------------------
   COMPRESS SUBSCRIPTS:
    Let (xlindx,lindx) = ljc(xsuper(:)), i.e store only once
    for each snode, instead of once per column.
   ------------------------------------------------------------ */
  for(ix = 0, jsup = 0; jsup < nsuper; jsup++){
    xlindx[jsup] = ix;
    jcol = xsuper[jsup];
    collen = ljc[jcol+1] - ljc[jcol];
    memmove(lindx + ix, lindx + ljc[jcol], collen * sizeof(int));
    ix += collen;
  }
  xlindx[nsuper] = ix;
/* ------------------------------------------------------------
   Do the block sparse Cholesky L*D*L'
   ------------------------------------------------------------ */
  nskip = blkLDL(m, nsuper, xsuper, snode, xlindx, lindx, lb,
                ljc, lpr, d, perm,
                ub, maxu, skipIr, iwsiz, iwork, fwsiz, fwork);
  if(nskip < 0)
    return nskip;
/* ------------------------------------------------------------
   Let iwork = diag-adding indices. Viz. where d(skipIr)>0.0.
   Let skipIr = skipIr except diag-adding indices. Hence d(skipIr)=0.
   ------------------------------------------------------------ */
  if(iwsiz < nskip)
    return -1;
  ix = 0;
  nadd = 0;
  for(j = 0; j < nskip; j++){
    jsup = skipIr[j];
    if(d[jsup] > 0.0)
      iwork[nadd++] = jsup;       /* diagonal adding */
    else
      skipIr[ix++] = jsup;          /* pivot skipping */
  }
  *pnadd = nadd;
  return ix;
}

/* ============================================================
   MAIN: MEXFUNCTION
   ============================================================ */
/* ************************************************************
   PROCEDURE mexFunction - Entry for Matlab
   ************************************************************ */
void mexFunction(const int nlhs, mxArray *plhs[],
  const int nrhs, const mxArray *prhs[])
{
  const mxArray *L_FIELD;
  mxArray *myplhs[NPAROUT];
  int    m, i, j, inz, iwsiz, nsuper, tmpsiz, fwsiz, nskip, nadd, m1;
  double *fwork, *d, *skipPr, *orgd;
  const double *permPr,*xsuperPr,*Ppr,*absd;
  int    *perm, *snode, *xsuper, *iwork, *xlindx, *skip, *skipJc;
  const int *LINir, *Pjc, *Pir;
  double canceltol, maxu, abstol;
  jcir   L;
  char useAbsd, useDelay;
/* ------------------------------------------------------------
   Check for proper number of arguments
   blkchol(L,P, pars,absd) with nparinmin=2.
   ------------------------------------------------------------ */
  if(nrhs < NPARINMIN)
    mexErrMsgTxt("blkchol requires more input arguments");
  if(nlhs > NPAROUT)
    mexErrMsgTxt("blkchol produces less output arguments");
/* ------------------------------------------------------------
   Get input matrix P to be factored.
   ------------------------------------------------------------ */
  if( (m = mxGetM(P_IN)) != mxGetN(P_IN))
    mexErrMsgTxt("P must be square");
  if(!mxIsSparse(P_IN))
    mexErrMsgTxt("P must be sparse");
  Pjc    = mxGetJc(P_IN);
  Pir    = mxGetIr(P_IN);
  Ppr    = mxGetPr(P_IN);
/* ------------------------------------------------------------
   Disassemble block Cholesky structure L
   ------------------------------------------------------------ */
  if(!mxIsStruct(L_IN))
    mexErrMsgTxt("Parameter `L' should be a structure.");
  if( (L_FIELD = mxGetField(L_IN,0,"perm")) == NULL)        /* L.perm */
    mexErrMsgTxt("Missing field L.perm.");
  if(m != mxGetM(L_FIELD) * mxGetN(L_FIELD))
    mexErrMsgTxt("perm size mismatch");
  permPr = mxGetPr(L_FIELD);
  if( (L_FIELD = mxGetField(L_IN,0,"L")) == NULL)           /* L.L */
    mexErrMsgTxt("Missing field L.L.");
  if( m != mxGetM(L_FIELD) || m != mxGetN(L_FIELD) )
    mexErrMsgTxt("Size L.L mismatch.");
  if(!mxIsSparse(L_FIELD))
    mexErrMsgTxt("L.L should be sparse.");
  L.jc = mxGetJc(L_FIELD);
  LINir = mxGetIr(L_FIELD);
  if( (L_FIELD = mxGetField(L_IN,0,"xsuper")) == NULL)      /* L.xsuper */
    mexErrMsgTxt("Missing field L.xsuper.");
  nsuper = mxGetM(L_FIELD) * mxGetN(L_FIELD) - 1;
  if( nsuper > m )
    mexErrMsgTxt("Size L.xsuper mismatch.");
  xsuperPr = mxGetPr(L_FIELD);
  if( (L_FIELD = mxGetField(L_IN,0,"tmpsiz")) == NULL)      /* L.tmpsiz */
    mexErrMsgTxt("Missing field L.tmpsiz.");
  tmpsiz   = mxGetScalar(L_FIELD);
/* ------------------------------------------------------------
   Disassemble pars structure: canceltol, maxu
   ------------------------------------------------------------ */
  canceltol = 1E-15;           /* supply with defaults */
  maxu = 5E5;
  abstol = 1E-20;
  useAbsd = 0;
  useDelay = 0;
  if(nrhs >= NPARINMIN + 1){       /* 3rd argument = pars */
    if(!mxIsStruct(PARS_IN))
      mexErrMsgTxt("Parameter `pars' should be a structure.");
    if( (L_FIELD = mxGetField(PARS_IN,0,"canceltol")) != NULL)
      canceltol  = mxGetScalar(L_FIELD);  /* pars.canceltol */
    if( (L_FIELD = mxGetField(PARS_IN,0,"maxu")) != NULL)
      maxu = mxGetScalar(L_FIELD);  /* pars.maxu */
    if( (L_FIELD = mxGetField(PARS_IN,0,"abstol")) != NULL){
      abstol = mxGetScalar(L_FIELD);  /* pars.abstol */
      abstol = MAX(abstol, 0.0);
    }
    if( (L_FIELD = mxGetField(PARS_IN,0,"delay")) != NULL)
      useDelay = mxGetScalar(L_FIELD);  /* pars.delay */
/* ------------------------------------------------------------
   Get optional vector absd
   ------------------------------------------------------------ */
    if(nrhs >= NPARIN){
      useAbsd = 1;
      absd = mxGetPr(ABSD_IN);
      if(m != mxGetM(ABSD_IN) * mxGetN(ABSD_IN))
        mexErrMsgTxt("absd size mismatch");
    }
  }
/* ------------------------------------------------------------
   Create sparse output matrix L(m x m).
   ------------------------------------------------------------ */
  L_OUT = mxCreateSparse(m,m, L.jc[m],mxREAL);
  L.ir  = mxGetIr(L_OUT);
  L.pr  = mxGetPr(L_OUT);
  memcpy(mxGetJc(L_OUT), L.jc, (m+1) * sizeof(int));
  memcpy(L.ir, LINir, L.jc[m] * sizeof(int));
/* ------------------------------------------------------------
   Create ouput vector d(m).
   ------------------------------------------------------------ */
  D_OUT = mxCreateDoubleMatrix(m,1,mxREAL);
  d     = mxGetPr(D_OUT);
/* ------------------------------------------------------------
   Compute required sizes of working arrays:
   iwsiz = 2*(m + nsuper).
   fwsiz = tmpsiz.
   ------------------------------------------------------------ */
  iwsiz = MAX(2*(m+nsuper), 1);
  fwsiz = MAX(tmpsiz, 1);
/* ------------------------------------------------------------
   Allocate working arrays:
   integer: perm(m), snode(m), xsuper(nsuper+1),
      iwork(iwsiz), xlindx(m+1), skip(m),
   double: orgd(m), fwork(fwsiz).
   ------------------------------------------------------------ */
  m1 = MAX(m,1);                  /* avoid alloc to 0 */
  perm      = (int *) mxCalloc(m1,sizeof(int)); 
  snode     = (int *) mxCalloc(m1,sizeof(int)); 
  xsuper    = (int *) mxCalloc(nsuper+1,sizeof(int));
  iwork     = (int *) mxCalloc(iwsiz,sizeof(int));
  xlindx    = (int *) mxCalloc(m+1,sizeof(int));
  skip      = (int *) mxCalloc(m1, sizeof(int));
  orgd    = (double *) mxCalloc(m1,sizeof(double)); 
  fwork   = (double *) mxCalloc(fwsiz,sizeof(double)); 
/* ------------------------------------------------------------
   Convert PERM, XSUPER to integer and C-Style
   ------------------------------------------------------------ */
  for(i = 0; i < m; i++){
    j = permPr[i];
    perm[i] = --j;
  }
  for(i = 0; i <= nsuper; i++){
    j =  xsuperPr[i];
    xsuper[i] = --j;
  }
/* ------------------------------------------------------------
   Let L = tril(P(PERM,PERM)), uses orgd(m) as temp working storage.
   ------------------------------------------------------------ */
  permuteP(L.jc,L.ir,L.pr, Pjc,Pir,Ppr, perm, orgd, m);
/* ------------------------------------------------------------
   If no orgd has been supplied, take orgd = diag(L on input)
   Otherwise, let orgd = absd(perm).
   ------------------------------------------------------------ */
  if(useAbsd)
    for(j = 0; j < m; j++)
      orgd[j] = absd[perm[j]];
  else
    for(j = 0; j < m; j++)
      orgd[j] = L.pr[L.jc[j]];
/* ------------------------------------------------------------
   Create "snode" and "xlindx"; change L.ir to the compact subscript
   array (with xlindx), and do BLOCK SPARSE CHOLESKY.
   ------------------------------------------------------------ */
  nskip = spchol(m, nsuper, xsuper, snode, xlindx,
                 L.ir, orgd, L.jc, L.pr, d, perm, abstol,
                 canceltol, maxu, skip, &nadd, iwsiz, iwork, fwsiz, fwork);
  if(nskip < 0)
    mexErrMsgTxt("Insufficient workspace in pblkchol");
/* ------------------------------------------------------------
   Copy original row-indices from LINir to L.ir.
   ------------------------------------------------------------ */
  memcpy(L.ir, LINir, L.jc[m] * sizeof(int));
/* ------------------------------------------------------------
   Create output matrices skip = sparse([],[],[],m,1,nskip),
   diagadd = sparse([],[],[],m,1,nadd),
   ------------------------------------------------------------ */
  SKIP_OUT = mxCreateSparse(m,1, MAX(1,nskip),mxREAL);
  memcpy(mxGetIr(SKIP_OUT), skip, nskip * sizeof(int));
  skipJc = mxGetJc(SKIP_OUT);
  skipJc[0] = 0; skipJc[1] = nskip;
  skipPr   = mxGetPr(SKIP_OUT);
/* ------------------------------------------------------------
   useDelay = 1 then L(:,i) is i-th column before ith pivot; useful
     for pivot-delaying strategy. (Fwslv(L, L(:,i)) still required.)
   ------------------------------------------------------------ */
  if(useDelay == 1)
    for(j = 0; j < nskip; j++)
      skipPr[j] = 1.0;
  else
    for(j = 0; j < nskip; j++){
      i = skip[j];
      skipPr[j] = L.pr[L.jc[i]];             /* Set skipped l(:,i)=ei. */
      L.pr[L.jc[i]] = 1.0;
      fzeros(L.pr+L.jc[i]+1,L.jc[i+1]-L.jc[i]-1);
    }
  DIAGADD_OUT = mxCreateSparse(m,1, MAX(1,nadd),mxREAL);
  memcpy(mxGetIr(DIAGADD_OUT), iwork, nadd * sizeof(int));
  skipJc = mxGetJc(DIAGADD_OUT);
  skipJc[0] = 0; skipJc[1] = nadd;
  skipPr   = mxGetPr(DIAGADD_OUT);
  for(j = 0; j < nadd; j++)
    skipPr[j] = orgd[iwork[j]];
/* ------------------------------------------------------------
   Release working arrays.
   ------------------------------------------------------------ */
  mxFree(fwork);
  mxFree(orgd);
  mxFree(skip);
  mxFree(xlindx);
  mxFree(iwork);
  mxFree(xsuper);
  mxFree(snode);
  mxFree(perm);
/* ------------------------------------------------------------
   Copy requested output parameters (at least 1), release others.
   ------------------------------------------------------------ */
  i = MAX(nlhs, 1);
  memcpy(plhs,myplhs, i * sizeof(mxArray *));
  for(; i < NPAROUT; i++)
    mxDestroyArray(myplhs[i]);
}
