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

/*  Etienne Balmes, Jean Michel Leclere, Robert Cimrman */
/*  Claire Delforge                                     */

/* ------------------------------------------------------------------*/
/*     SCILAB/Matlab MACROS                                          */
/* ------------------------------------------------------------------*/

#ifdef FORMATLAB

#include  "matrix.h"
void SetPrToNull(mxArray *prhs)
{  mxSetPr(prhs,NULL); }


#else

void SetPrToNull(mxArray *prhs)
{
  double * tmp_pr;
  tmp_pr = mxGetPr(prhs);
  tmp_pr = NULL;
}
#endif

/* obsolete with scilab 3
if !defined(mxArrayToString)
 char* mxArrayToString(mxArray *prhs) {

  int buflen, status;
  char* buf;

  buflen = (mxGetM (prhs) * mxGetN(prhs)) + 1;
  buf = mxCalloc (buflen, sizeof(char));
  status = mxGetString (prhs, buf, buflen);
  return(buf);
}
*/ 
#if !defined(mexLock)&&!defined(OSTYPEmexmac)
void mexLock() {}
void mexUnlock() {}
#endif



/* ------------------------------------------------------------------*/
/*        FullRealloc                                                  */
/* ------------------------------------------------------------------*/


void FullRealloc ( mxArray *full, int newM, int newN ) {

	double *ptr;
	double   *newptr;
	int oldN, oldM, i, j;

	ptr=mxGetPr(full);
	mexPrintf("OK");
	/*	newptr = mxRealloc(ptr, newM*newN*sizeof(*ptr)); */
	oldM = mxGetM(full); oldN = mxGetN(full);
	newptr = mxMalloc(newM*newN*sizeof(*ptr));
	if (newptr==NULL) {
             mxFree(ptr);mexErrMsgTxt ("Realloc failure."); 
        }
        for (i=0;i<oldM;i++) { for (j=0;j<oldN;j++) { 
          newptr[i+j*newM] = ptr[i+j*oldM];
        }}
        mxFree(ptr);
        mxSetPr(full,newptr);
        mxSetM(full,newM);
        mxSetN(full,newN);

}



/* ------------------------------------------------------------------*/
/*      C: function :       heapsort, two *double fields are sorted  */
/* ------------------------------------------------------------------*/
void hpsort(const int n, double ra[], double rb[]) {
  unsigned long  i, ir, j, l;
  double         rra, rrb;

  ra--; rb--;                                   /* just for shifting */

  if (n<2)  return; 
  l = (n >> 1)+1; 
  ir = n;

  for (;;) {
    if (l>1) { rra = ra[--l]; rrb = rb[l];}
    else {
      rra = ra[ir];    rrb = rb[ir];
      ra[ir] = ra[1];  rb[ir] = rb[1];
      if (--ir == 1) {
        ra[1]  = rra;  rb[1]  = rrb;
        break;
      }
    }
    i = l; 
    j = l+l;
    while (j<=ir) {
      if ( j<ir && ra[j]<ra[j+1] ) j++;
      if ( rra<ra[j] ) {
        ra[i]=ra[j]; rb[i]=rb[j];
        i=j;
        j <<= 1;
      }
      else j = ir+1;
    }
    ra[i] = rra;  rb[i] = rrb;
  }
  ra++; rb++;                                   /* just for shifting */
}

/* ------------------------------------------------------------------  */
/*      C: function :       heapsort, three *double fields are sorted  */
/* ------------------------------------------------------------------  */
void hpsortc(const int n, double ra[], double rb[], double rc[]) {
  unsigned long  i, ir, j, l;
  double         rra, rrb,rrc;

  ra--; rb--; rc--;         /* just for shifting */

  if (n<2)  return; 
  l = (n >> 1)+1; 
  ir = n;

  for (;;) {
    if (l>1) { rra = ra[--l]; rrb = rb[l]; rrc = rc[l];}
    else {
      rra = ra[ir];    rrb = rb[ir];   rrc = rc[ir];
      ra[ir] = ra[1];  rb[ir] = rb[1]; rc[ir] = rc[1];
      if (--ir == 1) {
        ra[1]  = rra;  rb[1]  = rrb; rc[1]  = rrc;
        break;
      }
    }
    i = l; 
    j = l+l;
    while (j<=ir) {
      if ( j<ir && ra[j]<ra[j+1] ) j++;
      if ( rra<ra[j] ) {
        ra[i]=ra[j]; rb[i]=rb[j]; rc[i]=rc[j];
        i=j;
        j <<= 1;
      }
      else j = ir+1;
    }
    ra[i] = rra;  rb[i] = rrb; rc[i] = rrc;
  }
  ra++; rb++;rc++;         /* just for shifting */
}



#include "sp_util_subs.c"



#ifdef SP_UTIL_ADD
#include "../../sdtdev/mex50/sp_util_pre.c"
#endif

static double EPSL=1.e-6;
static double OpenFEMDIAG=1;

/* ------------------------------------------------------------------------ */
/*             The gatewayfunction                                          */
/* ------------------------------------------------------------------------ */

void mexFunction (int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[]) {
  char  *buf ;

  int   i, i1;



  if (nrhs==0)  {
  double *a;
  plhs[0] = mxCreateDoubleMatrix (1, 1, mxREAL);
  a = mxGetPr (plhs[0]);  
#ifdef SP_UTIL_ADD
  *a=5.0008;
#else
  *a=1.0002;
#endif
  return; 
  }

  buf = mxArrayToString( prhs[0] );

/*----------------------------------------------------------- ismex */
  if (!strcmp("ismex",buf))  {
  double *a;

  plhs[0] = mxCreateDoubleMatrix (1, 1, mxREAL);
  a = mxGetPr (plhs[0]);
  *a=1.0;

/*----------------------------------------------------------------- profile */
}  else if (!strcmp("profile",buf)) {
  double        *prind;
  int           *ir, *jc, N, i;
  
  /* check for proper number of arguments */
  if (nrhs != 2) mexErrMsgTxt ("Two inputs required."); 
  if (!mxIsSparse(prhs[1])) mexErrMsgTxt ("Matrix must be sparse.");

  N = mxGetN (prhs[1]); 
  ir = mxGetIr(prhs[1]); jc = mxGetJc(prhs[1]);  
  /* create matrix for return arguments */
  plhs[0] = mxCreateDoubleMatrix (N, 1, mxREAL); prind = mxGetPr (plhs[0]);

  /* calculate the profile prind of sparse matrix  */
  for (i=0; i<N; i++, prind++)   *prind = (double)(i-ir[(jc[i])]+1);
  
    

}/* k = sp_util('spind',k,ind); ------------------------------*/
else if (!strcmp("spind",buf))  {

  int        i, j, k, Nk, Nind, Nc, nzmax, cj;
  double     *ind, *pr, *pI, *ir_tmp, N, nj, kR;
  int        *ir, *jc, *rind, *ip;

  /* check if input is sparse */
  if (!mxIsSparse (prhs[1])) mexErrMsgTxt ("Matrix must be sparse.");
  if (nrhs != 3) mexErrMsgTxt ("three inputs are required."); 
  if (nlhs != 1) mexErrMsgTxt ("one output is required."); 
 
  pr = mxGetPr (prhs[1]); ir = mxGetIr (prhs[1]); jc = mxGetJc (prhs[1]);  

  Nk = mxGetN (prhs[1]); N = (double)(Nk);  Nc = jc[Nk];

  /* Get the starting position of indexation vector and its size */
  ind = mxGetPr (prhs[2]); Nind = mxGetM(prhs[2])*mxGetN(prhs[2]);


  if (Nind==Nk) {                   /* all rows are kept ---------*/

  /* This is very dirty : explicit modification of input */ 

    rind = mxCalloc (Nk,sizeof(int));
    for (i=0;i<Nind;i++) {j = (int)(ind[i])-1;
     if (j>Nk) mexErrMsgTxt ("Index larger than sparse matrix size"); 
     rind[j] = i;
    }
    ir_tmp = mxCalloc (jc[Nk],sizeof(double));
    for (i=0; i<Nk; i++) { kR=(double)(rind[i])*N+.1;
      for (j=jc[i]; j<jc[i+1]; j++) {
        ir_tmp[j] = kR+(double)(rind[ir[j]]);
      }
    }
    if (mxIsComplex(prhs[1])) {
      pI = mxGetPi (prhs[1]); 
      hpsortc (jc[Nk], ir_tmp, pr,pI); }/* sort ir_tmp, rearrange pr,pI */ 
    else {
      hpsort (jc[Nk], ir_tmp, pr); }/* sort ir_tmp, rearrange pr */ 

    k=0; nj=0; for (i=0;i<jc[Nk];i++){
      j = (int)(ir_tmp[i]/N);
      while (k!=j) {k++;jc[k]=i;nj=N*(double)(j);} /* column starts */
      ir[i] = (int)(ir_tmp[i]-nj);
    }
    while (k<Nind) {k++;jc[k]=jc[Nk];}

    plhs[0]=(mxArray *)(prhs[1]);
    /*    nzmax = jc[Nk];
    plhs[0] = mxCreateSparse (Nind,Nind,nzmax,mxREAL);
    ip = mxGetJc (plhs[0]);  for (i=0;i<=Nind;i++) {ip[i]=jc[i];}
    ip = mxGetIr (plhs[0]);  ind = mxGetPr (plhs[0]);
    for (i=0;i<nzmax;i++) {ip[i]=ir[i];ind[i]=pr[i];}*/
  }
  else {                 /* some rows are deleted ------------------ */

  rind = mxCalloc (Nk,sizeof(int));
  for (i=0;i<Nk;i++) rind[i] = Nk;
  for (i=0;i<Nind;i++) {j = (int)(ind[i])-1;
   if (j>Nk) mexErrMsgTxt ("Index larger than sparse matrix size"); 
   rind[j] = i;
  }

  ir_tmp = mxCalloc (jc[Nk],sizeof(double)); nj = N*(N+1);
  for (i=0; i<Nk; i++) { 
      if (rind[i]==Nk) {for (j=jc[i]; j<jc[i+1]; j++) {ir_tmp[j]=nj;}}
      else {kR=(double)(rind[i])*N+.1;
        for (j=jc[i]; j<jc[i+1]; j++) {
          cj=rind[ir[j]]; if (cj==Nk) ir_tmp[j] = nj;
          else ir_tmp[j]=kR+(double)(cj);}
      }
  }
  if (mxIsComplex(prhs[1])) {
      pI = mxGetPi (prhs[1]); 
      hpsortc (jc[Nk], ir_tmp, pr,pI); }/* sort ir_tmp, rearrange pr,pI */ 
  else {
      hpsort (jc[Nk], ir_tmp, pr); }/* sort ir_tmp, rearrange pr */ 

  k=0;jc[0]=0;nzmax=0;nj=0; for (i=0;i<jc[Nk];i++){
      j = (int)(ir_tmp[i]/N);
      if (j>=Nk){if (nzmax==0) {nzmax=i;} i = jc[Nk];}
      else {j = (int)(ir_tmp[i]/N);
       while (k!=j) {k++;jc[k]=i;nj=N*(double)(j);} /* column starts */
       ir[i] = (int)(ir_tmp[i]-nj);
      }
      /*printf("(%i,%i,%i,%5.1f)",i,j,k,pr[i]);*/
  }
  if (nzmax==0) nzmax = jc[Nk];
  while (k<Nind) {k++;jc[k]=nzmax;}

  if (mxIsComplex(prhs[1])) {
    plhs[0] = mxCreateSparse (Nind,Nind,nzmax,mxCOMPLEX);
    ip = mxGetJc (plhs[0]);   for (i=0;i<=Nind;i++) {ip[i]=jc[i];}
    ip=mxGetIr (plhs[0]); ind = mxGetPr (plhs[0]); ir_tmp = mxGetPi (plhs[0]);
    for (i=0;i<nzmax;i++) {ip[i]=ir[i];ind[i]=pr[i];ir_tmp[i]=pI[i];}}
  else {
    plhs[0] = mxCreateSparse (Nind,Nind,nzmax,mxREAL);
    ip = mxGetJc (plhs[0]);   for (i=0;i<=Nind;i++) {ip[i]=jc[i];}
    ip = mxGetIr (plhs[0]);  ind = mxGetPr (plhs[0]);
    for (i=0;i<nzmax;i++) {ip[i]=ir[i];ind[i]=pr[i];}}

  }

}/*--------------------------------------------------------- xkx  */
  else if (!strcmp("xkx",buf))  {

    double *offset;
    int    type,ym,yn;

  mexWarnMsgTxt("Use OF_MK call instead of SP_UTIL");

  /* check for proper number of arguments */
  if (nrhs < 3) {
    mexErrMsgTxt ("Three inputs required.");
  } 
  else if(nlhs!=1) {
    mexErrMsgTxt ("One output required.");
  }
  
  /* check the inputs sizes */
  if (mxGetM (prhs[1]) != 3 || mxGetN (prhs[1]) != 3) {
   mexErrMsgTxt ("First argument X must be 3 by 3 in c = xkx(X,K)");
  }

  if (mxIsSparse(prhs[2])) mexErrMsgTxt ("k must be full");
  /* create matrix for the return argument */
  ym=mxGetM(prhs[2]);yn=mxGetN(prhs[2]);
  if (mxIsComplex(prhs[2])) { 
    plhs[0] = mxCreateDoubleMatrix (ym, yn, mxCOMPLEX);
  } else  plhs[0] = mxCreateDoubleMatrix (ym, yn, mxREAL);
  if (nrhs>3) {
   if   (mxGetM(prhs[3])==0) offset=NULL; 
   else if (mxGetM(prhs[3])!=3) { mexErrMsgTxt ("offset must be 3xN");}
   else offset=mxGetPr(prhs[3]); 
  } else offset=NULL;

  if (nrhs==5) type=(int)*mxGetPr(prhs[4]); else type=0;

  if (mxIsComplex (prhs[1])) mexErrMsgTxt ("Matrix of basis change must not be complex.");
  if (mxIsComplex (prhs[2])) { 
    x_k_x(mxGetPr(prhs[1]),mxGetPi(prhs[2]),mxGetPi(plhs[0]),offset,type,
     ym,yn);
  }
  /* call the subroutine xkx */
  x_k_x (mxGetPr (prhs[1]),mxGetPr(prhs[2]),mxGetPr(plhs[0]),offset,type,
   ym,yn);

}/* ----------------------------------------------------------------
out1 = sp_util('mind',ki, ke,length(Up.DOF),[mind]);
out1 = sp_util('mindsym',ki, ke,length(Up.DOF),[mind]); 
out1 = sp_util('mind','xxx',[],length(Up.DOF),[mind]);
out1 = sp_util('mindsym','xxx',[],length(Up.DOF),[mind]);
nees fid=fopen('xxx','wb','l');
*/
else if (!strcmp ("mind",buf) || !strcmp ("mindsym",buf))  {

  double  *mind, *ki, *ke, K,  r3, nj, cur, nex, *pr, *r1, *r2; 
  int     i, i2, j, k, l, NE, *jci, n, nzmax, nzc, *ir, *jc, *jcj, ooc;
  int     i1;

  char    *buf1;

  if (nrhs<4) mexErrMsgTxt ("Bad number of inputs.");
  if (nlhs>1) mexErrMsgTxt ("Bad number of outputs.");
  if (mxIsComplex(prhs[2])) mexErrMsgTxt ("ke must be real");
  if (mxIsSparse(prhs[1])) mexErrMsgTxt ("ki must be full");
  if (mxIsSparse(prhs[2])) mexErrMsgTxt ("ke must be full");
 
  /* deal with how ki ke is given */
  if (mxIsInt32(prhs[2])) { /* as a single matrix */
   ki = mxGetPr (prhs[1]);  ke = ki+mxGetM(prhs[1]);
   ir=mxGetData (prhs[2]); NE=ir[0]; ooc=1;
  } else if (mxIsChar(prhs[1])) { /* kie in a file */

   /*    int       fb, n[1]; */
   int n[1];
   FILE *fb;
    buf1 = mxArrayToString( prhs[1] ); fb=fopen(buf1,"rb"); mxFree(buf1);
    fread(n,sizeof(int),1,fb);NE=n[0];
    ki=mxMalloc(2*n[0]*sizeof(double));
    fread(ki,sizeof(double),2*n[0],fb);
    fclose(fb);
    ke=ki+NE;  ooc=0;

  } else { /* Ki, ke given as arguments */ 
    ke = mxGetPr (prhs[2]);  ki = mxGetPr (prhs[1]);   
    NE = mxGetM (prhs[1])*mxGetN (prhs[1]); 
    ooc=2;
    if (mxGetM (prhs[3]) == 2 || mxGetN (prhs[3]) == 2) {
     pr = mxGetPr (prhs[3]); n = (int)(pr[1]+.1); 
     if (n>NE){ mexErrMsgTxt ("NW must be <= length(ki)");}
     NE=n;
    } 
    if (mxGetM (prhs[1])!=mxGetM (prhs[2])) {
      mexErrMsgTxt ("data and index must have the same size");
    }
  }  /* format of input arguments - - - - - - - - - - - - - - - - - - -*/

  if (NE==0)  { /* safe return if empty matrix */
     K = *mxGetPr (prhs[3]); n = (int)(K+.1);
     plhs[0] = mxCreateSparse (n,n,0,mxREAL);
     mxFree(buf); return;
  } 
  if (nrhs==5)  {/* mind is given - - - - - - - - - - - - - - - - - - - -*/

    int Nmi;

    r1 = mxGetPr (prhs[4]);     Nmi = mxGetM (prhs[4]); 
    mind = mxCalloc (3*Nmi,sizeof(double));
    for (i=0; i<3*Nmi; i++)  mind[i]=r1[i];
    r1 = mind+Nmi; r2 = mind+2*Nmi; 
    hpsortc (Nmi,mind,r1,r2); /* sort mind */ 

    i=0; 
    for (j=0;j<Nmi;j++) { /* loop on elements with non-zero coefficients */
     /* printf("%i-%i ",i,(int)(mind[j]-.99)); */
     /* indices and coefficient */
     if (mind[j]<=mind[j+Nmi]) {
      i1=(int)(mind[j]-.99);  i2=(int)(mind[j+Nmi]-.99); K = mind[j+2*Nmi];
      if (i1<0)  mexErrMsgTxt ("indices must be positive");
      if (i2>NE)  mexErrMsgTxt ("mind > size(ke)");
      if (K==1.0) { /* printf("km%4i %4i %4i %e\n",i,i1,i2,K); */
        if (i>i2) {} /* empty matrix */  
        else if (i!=i1) { /* translate if zero coef before */
          for (k=i1;k<=i2;k++) { ke[i] = ke[k];ki[i] = ki[k];i++; }
        } else if (i<i2+1) {i=i2+1;}
      } else if (K!=0.0) {
          for (k=i1;k<=i2;k++) {ke[i] = K*ke[k];ki[i] = ki[k];i++; }
      }
    }}; 
    if (i<=NE) {NE = i;}
    mxFree(mind);
  } /* end of mind multiplication  - - - - - - - - - - - - - - - - - - - */

  K = *mxGetPr (prhs[3]); n = (int)(K+.1);

  /* sort ind and rearrange data in the same way */ 
  hpsort (NE, ki, ke);
  if (ki[NE-1]>((double)n)*((double)n)) {
   mexErrMsgTxt ("index exceeds matrix dimension (ki>N^2)");
  }

  if (NE==0)  { /* safe return if empty matrix, available for mind and mindsym */
     plhs[0] = mxCreateSparse (n,n,0,mxREAL);
     return;
  } 

  /* - - - -  - - - - - - - - - - - - - - - - - - - - - - - - mind - */
  if (!strcmp ("mind",buf)) {

   /* eliminate repeated values  */
   jci = mxCalloc(n+1, sizeof(int));  for(k=0;k<=n;k++){jci[k]=0;}
   nzmax=0; cur = ki[nzmax]-1;j=(int)(cur/K);nj=K*(double)(j)-0.1;
   ki[nzmax]=cur-nj;nzc = 0;

   for(k=1;k<NE;k++) {
     nex = ki[k]-1;
     if(nex==cur) { ke[nzmax]+=ke[k]; }  /* sum repeated */
     else {
       if  (ke[nzmax]!=0.0) {/* (ke[nzmax]!=0.0)accepting non-0 new values */
         jci[j+1]++;nzmax++;ke[nzmax]=ke[k];}
       else {     /* removing zero new value */
          ke[nzmax]=ke[k];/*jci[j+1]--;printf("bad");*/
       }
       cur = nex; r3=cur-nj; ki[nzmax]=r3;
       if(r3>=K){ /* new column */
          j=(int)(cur/K);nj=K*(double)(j)-0.1;r3=cur-nj; ki[nzmax]=r3;
       }
     }
   } /* loop on k */
   if (ke[nzmax]!=0.0) { jci[j+1]++; } else {nzmax--;}

   nzmax++;
   /* printf("nzmax %i",nzmax); */
   for(k=1;k<=n;k++){jci[k]+=jci[k-1];} /* cumsum for column starts */

   /* create sparse output matrix */
   plhs[0] = mxCreateSparse (n,n,nzmax,mxREAL);
   pr = mxGetPr (plhs[0]); ir = mxGetIr (plhs[0]); jc = mxGetJc (plhs[0]);
   memcpy (pr, ke, nzmax*sizeof(double)); 
   memcpy (jc, jci, (n+1)*sizeof(int));
   mxFree(jci);
   for(k=0;k<nzmax;k++) { ir[k]=(int)(ki[k]); }

  /* - - - -  - - - - - - - - - - - - - - - - - - - - - - - - mindsym - */
  } else if (!strcmp ("mindsym",buf)) {

   /* eliminate repeated values */
   K = (double)(n); 
   jci = mxCalloc(n+1, sizeof(int));  for(k=0;k<=n;k++){jci[k]=0;}
   jcj = mxCalloc(n+1, sizeof(int));  for(k=0;k<=n;k++){jcj[k]=0;}
   nzmax=0; cur = ki[nzmax]-1;j=(int)(cur/K);nj=K*(double)(j)-0.1;
   r3=cur-nj; ki[nzmax]=r3; nzc = 0;


   for(k=1;k<NE;k++) {
     nex = ki[k]-1;
     if (nex==cur) { ke[nzmax]+=ke[k]; }  /* sum repeated */
     else {
       if  (ke[nzmax]!=0.0) {/* (ke[i]!=0.0)accepting non-zero new values */
         jci[j+1]++; jcj[j+1]++; 
	 if (r3>2e9||(int)(r3)!=j) {jcj[(int)(r3)+1]++;} else {nzc++;}
	 /*	 if ((int)(r3)!=j) {jcj[(int)(r3)+1]++;} else {nzc++;}*/
         nzmax++;ke[nzmax]=ke[k];}
       else {     /* removing zero new value */
          ke[nzmax]=ke[k];
       }
       cur = nex; r3=cur-nj; ki[nzmax]=r3;
       if(r3>=K){ /* new column */
         j=(int)((cur+.01)/K);nj=K*(double)(j)-0.1;r3=cur-nj; 
	 /* if (r3>K) {
	  mexPrintf("%.1f j=%i=%.1f r3=%.1f\n",cur,j,nj,r3);
          mexErrMsgTxt("bad");
	  }*/
         ki[nzmax]=r3;
       }
     }
   } /* loop on k */

   if (ke[nzmax]!=0.0) {
    jci[j+1]++;jcj[j+1]++; 
    if(r3>2e9||(int)(r3)!=j) {jcj[(int)(r3)+1]++;} else {nzc++;}
   } else {nzmax--;}

   for(k=1;k<=n;k++){jci[k]+=jci[k-1];jcj[k]+=jcj[k-1]; } /* cumsum c. starts */

   /* create sparse output matrix */
   plhs[0] = mxCreateSparse (n,n,jcj[n],mxREAL);
   pr = mxGetPr (plhs[0]); ir = mxGetIr (plhs[0]); jc = mxGetJc (plhs[0]);
   memcpy (jc, jcj, (n+1)*sizeof(int));

   for(j=n-1;j>=0;j--) {
     /* mexPrintf("\n j %i %i,",j+1,jci[j+1]-1);*/
     for(k=jci[j+1]-1;k>=jci[j];k--) {
         i = j+1; l = jcj[i]-1;pr[l] = ke[k];ir[l] = (int)(ki[k]);jcj[i]--;
	 /* mexPrintf(" %i:%i:%i (%.1f) ",i,l,ir[l],ki[k]); */
	 i = ir[l]+1;
         if (i!=j+1){l=jcj[i]-1; pr[l] = ke[k]; ir[l] = j;jcj[i]--; }
     }
   } 
   mxFree(jci); mxFree(jcj);

  } /* if mindsym */
  if      (ooc==0) { ki[0]=-1;  /* mxFree(ki); */} 
  else if (ooc==1) { ki[0]=-1;  /* mxFree(ki);  SetPrToNull(prhs[1]); */}
  else             {ki[0]=-1; 
  /* mxFree(ki); SetPrToNull(prhs[1]);
     mxFree(ke); SetPrToNull(prhs[2]);*/
 }



}/* ----------------------------------------------------------------
sparse = sp_util('sp2st',k); */
else if (!strcmp ("sp2st",buf))  {

  double        *pr, *v;
  mxArray       *field_value; 
  int           n, i, *ir, *jc, dims[2] = {1,1};
  const char    *field_names[] = {"pr", "ir", "jc","nzmax"};

  if (!mxIsSparse(prhs[1])) mexErrMsgTxt ("Matrix must be sparse."); 
  pr = mxGetPr (prhs[1]); ir = mxGetIr (prhs[1]); jc = mxGetJc (prhs[1]);

  plhs[0] = mxCreateStructArray(2, dims, 4, field_names);  

  n = mxGetNzmax (prhs[1]);
  field_value = mxCreateDoubleMatrix(1,n,mxREAL);
  v = mxGetPr (field_value); 
  mxSetField(plhs[0], 0, "pr", field_value); 
  for (i=0;i<n;i++) { v[i] = pr[i]; }

  field_value = mxCreateDoubleMatrix(1,n,mxREAL);
  v = mxGetPr (field_value); 
  mxSetField(plhs[0], 0, "ir", field_value); 
  for (i=0;i<n;i++) { v[i] = (double)(ir[i]);}

  n = mxGetN (prhs[1]);
  field_value = mxCreateDoubleMatrix(1,n+1,mxREAL);
  v = mxGetPr (field_value); 
  mxSetField(plhs[0], 0, "jc", field_value); 
  for (i=0;i<=n;i++) { v[i] = (double)(jc[i]); }

  field_value = mxCreateDoubleMatrix(1,1,mxREAL);
  v = mxGetPr(field_value);  mxSetField(plhs[0], 0, "nzmax", field_value); 
  v[0]=(double)mxGetNzmax(prhs[1]); 


}/* ----------------------------------------------------------------
 sp_util('insertinkie',kelem,iddl,kie,ik,N); 
kelem=[1 2 3;4 5 6;7 8 9]';
iddl=int32([5 6 9]);
kie=zeros(20,2);ik=int32(9);
sp_util('insertinkie',sparse(kelem),iddl,kie,ik,100);
kie
ik

*/
else if (!strcmp ("insertinkie",buf))  {

  double        *kelem, *kie, N;
  int           NE, Nkie, *ik, j1, j2, *iddl, flag, *ir, *jc; 

  /* handle with sparse or full input */ 
  if (mxIsSparse (prhs[1])) {
    flag=1;
    kelem = mxGetPr (prhs[1]); ir = mxGetIr (prhs[1]); jc = mxGetJc (prhs[1]);
  }
  else {
    flag=0;
    kelem = mxGetPr (prhs[1]);
  }

  if (nrhs != 6) mexErrMsgTxt ("Not enough inputs."); 
#ifdef FORMATLAB
  if (nlhs != 0) mexErrMsgTxt ("No output is required."); 
#else
  if (nlhs != 2) mexErrMsgTxt ("Two output is required."); 
  plhs[0] = prhs[3];plhs[1] = prhs[4];
#endif
  /* matrix entry kelem */ 
  NE = mxGetN (prhs[1]); 
  if (NE!=mxGetM (prhs[1])) mexErrMsgTxt ("Matrix must be square.");

  iddl = mxGetData (prhs[2]);  
  kie = mxGetPr (prhs[3]); Nkie = mxGetM (prhs[3]);
  ik = mxGetData (prhs[4]); N = *mxGetPr (prhs[5]); 


  if (flag) { /* is Sparse */
    for (j1=0;j1<NE;j1++) { /* column jc */

      for (j2=jc[j1];j2<jc[j1+1];j2++) { /* row ir */
        if (iddl[j1]>=0 && iddl[ir[j2]]>=0 && kelem[j2]) {
          kie[*ik]  = (double)(1+iddl[ir[j2]])+N*(double)(iddl[j1]); /* ind */
          kie[*ik+Nkie]=    kelem[j2]; /* value */
          ik[0]++;
          if (ik[0]>Nkie) { 
            Nkie=(int)((double)Nkie*1.25)+10;
            FullRealloc(prhs[3],Nkie,2); kie = mxGetPr (prhs[3]);
          }
	}
      }
    }
  }
  else {
    for (j1=0;j1<NE;j1++) {/* column */
      for (j2=0;j2<NE;j2++) { /* row */
        if (iddl[j1]>=0 && iddl[j2]>=0 && kelem[j2+NE*j1]) {
          if (ik[0]>Nkie) { 
            Nkie=(int)((double)Nkie*1.25)+NE^2;
            FullRealloc(prhs[3],Nkie,2); kie = mxGetPr (prhs[3]);
	    mexPrintf("At %i\n",Nkie);mexErrMsgTxt("Kie buffer overflow");
          }
          kie[*ik]=(double)(1+iddl[j2])+N*(double)(iddl[j1]);/* indice */
          kie[*ik+Nkie]=kelem[j2+NE*j1]; /* value */
          ik[0]++;
        }
      }
    }
  }


}/* ----------------------------------------------------------------
 [out,out1,out2]=sp_util('basis',x,y)
 */
else if (!strcmp ("basis",buf))  {

  double     *x, *y, *z, *out2, eps=1.e-16;

  
  if (nrhs == 3) { /* sp_util('basis',x,y) */

   if (mxIsSparse(prhs[1])) mexErrMsgTxt ("X must be full");
   if (mxIsSparse(prhs[2])) mexErrMsgTxt ("Y must be full");
   if ((mxGetM (prhs[1])!=3 || mxGetM (prhs[1])!=3) &&
       (mxGetN (prhs[1])!=3 || mxGetN (prhs[1])!=3)  ) 
       mexErrMsgTxt ("Input must be 3 by 1 vectors.");
   x = mxGetPr (prhs[1]); y = mxGetPr (prhs[2]); 
   plhs[0] = mxCreateDoubleMatrix(3,3,mxREAL);  
   z = mxGetPr (plhs[0]);
   basis(x,y,z);

  } /* end nrhs == 3 */
  else if (nrhs == 2) { 
       /*
       Node=[0 0 0; 0 1 0; 1 1 0; 1 0 0]'
       [b,r,t]=sp_util('basis',Node)
       
     */
    if ((mxGetN (prhs[1])==4) || (mxGetM (prhs[1])==4)) { /* quad4 basis */
      double     *x, *x1;
      int32      Elt[4]={1,2,3,4}; 

      x = mxGetPr (prhs[1]);              
      plhs[1] = mxCreateDoubleMatrix(4,3,mxREAL);  z=mxGetPr(plhs[1]);
      plhs[2] = mxCreateDoubleMatrix(1,3,mxREAL);  out2=mxGetPr(plhs[2]);
      /* plhs[0] = mxCreateDoubleMatrix(9,1,mxREAL);  x1=mxGetPr(plhs[0]); */
      plhs[0] = mxCreateDoubleMatrix(3,3,mxREAL);  x1=mxGetPr(plhs[0]);
      basisQ4(x, 1, Elt, x1,z,out2);



    } /* end of quad4 basis */


  } /* if (nrhs == 3)*/
}/* ----------------------------------------------------------------
 % standard call
 Node=[0 0 0; 0 1 0; 1 1 0; 1 0 0;0 0 1; 0 1 1; 1 1 1; 1 0 1]';
 Elt=int32([1 2 3 4;5 6 7 8]');
 b=sp_util('basiselt',Node,Elt);
 reshape(b,3,6)

 */
else if (!strcmp ("basiselt",buf))  {

  double     *Node, *b, eps=1.e-16;
  int32      *Elt;
  int        Nelt;

  if (nrhs != 3)  
    mexErrMsgTxt ("3 input arguments needed :  b=sp_util('basiselt',Node,Elt)");
  if (mxIsSparse(prhs[1])) mexErrMsgTxt ("Node must be full");
  if (mxIsSparse(prhs[2])) mexErrMsgTxt ("Elt must be full");
  if (mxGetM (prhs[1])!=3) mexErrMsgTxt ("Node must have 3 rows");
  if (!mxIsInt32(prhs[2])) mexErrMsgTxt ("Elt connectivity must be int32");

  Nelt=mxGetN(prhs[2]);  Node = mxGetPr (prhs[1]);  Elt = mxGetData(prhs[2]);
  plhs[0] = mxCreateDoubleMatrix(9, Nelt, mxREAL);  b=mxGetPr(plhs[0]);

  if (mxGetM (prhs[2])==3) {
   basisT3(Node, Nelt, Elt, b);
  }
  else if (mxGetM (prhs[2])==4) {
   basisQ4(Node, Nelt, Elt, b, NULL, NULL);
  }
  else {
   mexErrMsgTxt ("sp_util('basiselt',Node,Elt) : unrecognized element");
  }
 

}/*---------------------------------------------------------  ERROR  */

/* 'setinput',In,Val,Offset --------------------------------------------- */
else if (!strcmp("setinput",buf))  {

  double   *in, *val, *opt;
  int     jrow, jcol, ioff, voff, typ, *irow, *icol, M;
  char    *buf1;

  typ=0;

  if (nrhs>5) {
   buf1 = mxArrayToString( prhs[5] );
   if      (!strcmp("+",buf1))   typ=1;
   else if (!strcmp("-",buf1))   typ=2;
   else if (!strcmp(".*",buf1))  typ=3;
   else if (!strcmp("./",buf1))  typ=4;
   else if (!strcmp("*",buf1))   typ=5;
   else if (!strcmp("/",buf1))   typ=6;
   else if (!strcmp("=",buf1))   typ=0; /* default if not given */
  }

 if ( nrhs<3) mexErrMsgTxt("Bad number of input arguments");

 if ( mxIsDouble(prhs[1]) & mxIsDouble(prhs[2]) )
   {
    in=mxGetPr(prhs[1]); val=mxGetPr(prhs[2]);
    irow=mxGetData(prhs[3]);icol=mxGetData(prhs[4]);
    
    /* check size */
    if (     (irow[mxGetM(prhs[3])-1]>mxGetM(prhs[1]))  
         ||  (icol[mxGetN(prhs[4])-1]>mxGetN(prhs[1]))    )         
                                            mexErrMsgTxt("Wrong indices");
    M=mxGetM(prhs[2]);
    if (typ==0) { /* set value */
      for (jcol=0;jcol<mxGetN(prhs[2]);jcol++) {
       ioff=mxGetM(prhs[1])*(icol[jcol]-1); voff=M*jcol;
       for (jrow=0;jrow<M;jrow++) in[irow[jrow]-1+ioff] = val[voff+jrow];
      }
    } else if (typ==1) {  /* add to matrix */
      for (jcol=0;jcol<mxGetN(prhs[2]);jcol++) {
       ioff=mxGetM(prhs[1])*(icol[jcol]-1); voff=M*jcol;
       for (jrow=0;jrow<M;jrow++) in[irow[jrow]-1+ioff] += val[voff+jrow];
      }
    } else if (typ==2) {  /* minus to matrix */
      for (jcol=0;jcol<mxGetN(prhs[2]);jcol++) {
       ioff=mxGetM(prhs[1])*(icol[jcol]-1); voff=M*jcol;
       for (jrow=0;jrow<M;jrow++) in[irow[jrow]-1+ioff] -= val[voff+jrow];
      }
    } else if (typ==3) {  /* multiply matrix, by coordinates  */
      for (jcol=0;jcol<mxGetN(prhs[2]);jcol++) {
       ioff=mxGetM(prhs[1])*(icol[jcol]-1); voff=M*jcol;
       for (jrow=0;jrow<M;jrow++) in[irow[jrow]-1+ioff] *= val[voff+jrow];
      }
    } else if (typ==4) {  /* divide matrix, by coordinates */
      for (jcol=0;jcol<mxGetN(prhs[2]);jcol++) {
       ioff=mxGetM(prhs[1])*(icol[jcol]-1); voff=M*jcol;
       for (jrow=0;jrow<M;jrow++) in[irow[jrow]-1+ioff] /= val[voff+jrow];
      }
    } else if (typ==5) {  /* multiply  matrix, by scalar */
      M=mxGetN(prhs[3]);
      for (jcol=0;jcol<mxGetN(prhs[4]);jcol++) {
       ioff=mxGetM(prhs[1])*(icol[jcol]-1);
       for (jrow=0;jrow<M;jrow++) in[irow[jrow]-1+ioff] *= *val; 
      }
    } else if (typ==6) {  /* divide matrix, by scalar */
      M=mxGetN(prhs[3]);
      for (jcol=0;jcol<mxGetN(prhs[4]);jcol++) {
       ioff=mxGetM(prhs[1])*(icol[jcol]-1);
       for (jrow=0;jrow<M;jrow++)  in[irow[jrow]-1+ioff] /= *val; 
      }
    } /* type of double operations */

   } else if (mxIsCell(prhs[1])) {
    opt=mxGetPr(prhs[3]);
    jrow=(int)opt[0]-1;
    mxDestroyArray(mxGetCell(prhs[1],jrow));
    mxSetCell(prhs[1],jrow,prhs[2]);
   } else mexErrMsgTxt("Not a supported setinput case");


#ifdef SP_UTIL_ADD
#include "../../sdtdev/mex50/sp_util_post.c"
#endif


} else if (!strcmp("cvs",buf)) {

    mxArray *st;
    mxArray *rhs[1], *lhs[1];
    int     dims[2];
    st=mxCreateString("$Revision: 1.55 $  $Date: 2006/04/06 15:24:55 $");

#ifdef SP_UTIL_ADD

    rhs[0] = mxCreateString("post_cvs");

    dims[0]=4; dims[1]=2;  plhs[0]=mxCreateCellArray(2,dims);

    mxSetCell(plhs[0],0,      mxCreateString("pre"));
    mxSetCell(plhs[0],1,      mxCreateString("sp_util"));
    mxSetCell(plhs[0],2,      mxCreateString("post"));
    mxSetCell(plhs[0],3,      mxCreateString("subs"));

    mxSetCell(plhs[0],4,pre_cvs ());         /* pre */
    mxSetCell(plhs[0],5,st);                 /* sp_util */
    mexCallMATLAB(1, lhs, 1, rhs, "sp_util");
    mxSetCell(plhs[0],6,lhs[0]);             /* post */
    mxSetCell(plhs[0],7,subs_cvs_sputil ());         /* subs */

    mxDestroyArray(rhs[0]);
#else
    plhs[0]=st; 
#endif

} else if (!strcmp("subscvs",buf)) {

  plhs[0]=subs_cvs_sputil ();

/*----------------------------------------------------------- epsl */
}  else if (!strcmp("epsl",buf))  {
  double *a;

  if (nrhs==1) {
   plhs[0] = mxCreateDoubleMatrix (1, 1, mxREAL);
   a = mxGetPr (plhs[0]); *a=EPSL; 
  }
  else if (nrhs==2) { a = mxGetPr (prhs[1]); EPSL = *a;}

/*----------------------------------------------------------- epsl */
}  else if (!strcmp("diag",buf))  {
  double *a;

  if (nrhs==1) {
   plhs[0] = mxCreateDoubleMatrix (1, 1, mxREAL);
   a = mxGetPr (plhs[0]); *a=OpenFEMDIAG; 
  }
  else if (nrhs==2) { a = mxGetPr (prhs[1]); OpenFEMDIAG = *a;}

/*----------------------------------------------------------- ismex */
} else  if (!strcmp("issdt",buf))  {
  double *a;

  plhs[0] = mxCreateDoubleMatrix (1, 1, mxREAL);
  a = mxGetPr (plhs[0]);
#ifdef SP_UTIL_ADD
  *a=1.0;
#else
  *a=0.0;
#endif


/* addcol ------------------------------------------------------------
   add end column to sparse matrix k1 of size nk1 x nk1
   matrix k2 is size nk1+ncol x nk1+ncol
   k2=sp_util('addcol',k1,ncol)

                 --------------
                 -        -   -
                 -  k1    -col-
             k2= -        -   -
                 --------------
                 -  col   -   -
                 --------------

   28/02/2006 ------------------------------------------------------*/
    } else if (!strcmp("addcol",buf))  {

      mxArray *out;
      int nNod, nnz, ii, jj, ncol;
      int *prow, *icol, *nprow, *nicol, aux;
      double *val;


      nNod = mxGetM( prhs[1] );
      prow = mxGetJc( prhs[1] );
      icol = mxGetIr( prhs[1] );
      ncol = (int)mxGetScalar( prhs[2] );

 
      aux=prow[nNod]-1;
      aux += ncol*(ncol+2*nNod);

      nprow = mxCalloc( nNod+ncol+1, sizeof( int ) );
      nicol = mxCalloc( aux, sizeof( int ) );

      nprow[0]=prow[0];
      nicol[0]=icol[0];


      for (ii=1;ii<nNod+1;ii++) {
	nprow[ii]=prow[ii]+ncol*ii;
	for (jj=0;jj<prow[ii]-prow[ii-1];jj++) {
	  nicol[nprow[ii-1]+jj]=icol[prow[ii-1]+jj];
	}
	for (jj=0;jj<ncol;jj++) {
	  nicol[nprow[ii]-ncol+jj]=nNod+jj;
	}
      }

      for (ii=0;ii<ncol;ii++) {
	nprow[nNod+1+ii]=nprow[nNod+ii]+nNod+ncol;
	for (jj=0;jj<nNod+ncol;jj++) {
	  nicol[jj+nprow[nNod+ii]]=jj;
	}
      }

      nnz=nprow[nNod+ncol];
      out = mxCreateSparse( nNod+ncol, nNod+ncol, nnz, mxREAL );
      val = mxGetPr( out);

      for (ii = 0; ii < nnz; ii++) {
	val[ii] = 1.0;
      }
      mxSetIr( out, nicol );
      mxSetJc( out, nprow );
      plhs[0] = out;
      

} else {
    mexErrMsgTxt ("Unknown command.");
}
  mxFree(buf);
}

