/* $Revision: 2.0 $ $Date: 2002/10/11 18:54:54 $ */
/*=========================================================
 * csminwel.c
 *
 * Unconstrained minimization.  Uses a quasi-Newton method with BFGS update of
 * the estimated inverse hessian.  It is robust against certain pathologies
 * common on likelihood functions.  It attempts to be robust against "cliffs",
 * i.e. hyperplane discontinuities, though it is not really clear whether what
 * it does in such cases succeeds reliably.
 *
 * function [fhat,xhat,ghat,Hhat,itct,fcount,retcodehat] = csminwel(fcn,x0,H0,grad,crit,nit,varargin)
 * fcn:   string naming the objective function to be minimized
 * x0:    initial value of the parameter vector
 * H0:    initial value for the inverse Hessian.  Must be positive definite.
 * grad:  Either a string naming a function that calculates the gradient, or the null matrix.
 *        If it's null, the program calculates a numerical gradient.  In this case fcn must
 *        be written so that it can take a matrix argument and produce a row vector of values.
 * crit:  Convergence criterion.  Iteration will cease when it proves impossible to improve the
 *        function value by more than crit.
 * nit:   Maximum number of iterations.
 * varargin: A list of optional length of additional parameters that get handed off to fcn each
 *        time it is called.
 *        Note that if the program ends abnormally, it is possible to retrieve the current x,
 *        f, and H from the files g1.mat and H.mat that are written at each iteration and at each
 *        hessian update, respectively.  (When the routine hits certain kinds of difficulty, it
 *        write g2.mat and g3.mat as well.  If all were written at about the same time, any of them
 *        may be a decent starting point.  One can also start from the one with best function value.)
 *
 * Note: the level of display output depends on the value of the variable
 *       "verbose" defined in the base, caller's or global workspaces
 *       0  -  no output
 *       1  -  only warnings
 *       2  -  all output
 *
 * MATLAB algorithm by Christopher Sims
 * C implementation by Iskander Karibzhanov
 * Master of Science in Computational Finance
 * Georgia Institute of Technology
 *
 * This is a MEX-file for MATLAB.
 * Copyright 1999 Christopher Sims
 * Copyright 2002 Federal Reserve Bank of Atlanta
 *=======================================================
 * Revision history:
 *
 *  10/3/2002  -  1. corrected problem with memory corruption in C-MEX-file (csminwelmex.c)
 *                   (needed to switch fcnRhs[0] back to x[0] before destroying it.
 *                   If we don't do this, we will later clear previously destroyed array
 *                   (one of x[1], x[2] or x[3]) which causes memory fault.
 *                   The reason why fcnRhs[0] pointed to array other than x[0] is
 *                   because we use mxSetPr in feval and gfeval.
 *                   This was not a problem in C-file (csminwel.c).
 *
 * 10/11/2002  -  1. changed csminit function to avoid using fPeak without first initializing it
 *                2. added two switches in csminit function to assign retcode to 7 for lambda>=4
 *                3. added one more verbose level to display only warnings or all output
 *
 *=======================================================*/

/* numgrad parameters */
#define SCALE 1
/* csminit parameters */
#define ANGLE .005
#define THETA .3
#define FCHANGE 1000
#define MINLAMB 1e-9
#define MINDFAC .01

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

static int n, nn, npar;
static char fcn[100]="";
static char gfcn[100]="";
static mxArray **fcnRhs, *fcnLhs, *gfcnLhs[2];
static bool ngrad=true;
static int verbose=0;

void numgrad(double *g, bool *badg, double *x);
double feval(double *x); /* x must point to the heap */
void gfeval(double *g, bool *badg, double *x); /* x must point to the heap */
void csminit(double *fhat, double *xhat, int *fcount, int *retcode,
             double *x0, double f0, double *g, bool badg, double *H0);
double times(const double *x, const double *y);
void peakwall(double *g, bool *badg, int retcode, double *x);
void bfgsi(double *H, const double *dg, const double *dx);
double *mtimes(const double *x, const double *y);
double *mminus(const double *x, const double *y);

/* plhs = [fh,xh,gh,H,itct,fcount,retcodeh] */
/* prhs = [fcn,x0,H,grad,crit,nit,varargin] */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
   bool done=false, badg[4], badgh, nogh=true, stuck=false;
   double *x[4], *g[4], f[4], *grad, crit, *dg, *dx;
   double *fh, *xh, *gh, *H, *itct, *fcount, *retcodeh;
   int retcode[3], fc=0, ih, nit;
   register int i;

   if (nrhs < 6)
      mexErrMsgTxt("At least six input arguments required.");
   if (nlhs != 7)
      mexErrMsgTxt("Seven output arguments required.");

   if ((fcnLhs=mexGetVariable("global","verbose")) ||
       (fcnLhs=mexGetVariable("caller","verbose")) ||
       (fcnLhs=mexGetVariable("base","verbose")))
      verbose = (int)mxGetScalar(fcnLhs);

   n = mxGetM(prhs[1]); nn = n*n;
   /* ngrad=0 if grad is a non-empty string */
   if (!mxIsChar(prhs[3])) {
      if (!mxIsEmpty(prhs[3]))
         grad = mxGetPr(prhs[3]);
   }
   else if (!mxIsEmpty(prhs[3])) {
      if (mxGetString(prhs[3], gfcn, mxGetM(prhs[3])*mxGetN(prhs[3])+1))
         mexWarnMsgTxt("Not enough space. Gradient function name is truncated.");
      ngrad = false;
   }

   for (i=0; i<4; i++)
      g[i] = mxCalloc(n, sizeof(double));

   if (mxGetString(prhs[0], fcn, mxGetM(prhs[0])*mxGetN(prhs[0])+1))
      mexWarnMsgTxt("Not enough space. Function name is truncated.");

   npar = nrhs-5;
   fcnRhs = mxCalloc(npar, sizeof(*fcnRhs));
   x[0] = mxGetPr(fcnRhs[0]=mxDuplicateArray(prhs[1]));
   for (i=1; i<4; i++)
      x[i] = mxCalloc(n, sizeof(double));
   for (i=6; i<nrhs; i++)
      fcnRhs[i-5] = (mxArray*)prhs[i];
   f[0] = feval(x[0]);

   fh       = mxGetPr(plhs[0]=mxCreateScalarDouble(f[0]));
   xh       = mxGetPr(plhs[1]=mxDuplicateArray(prhs[1]));
   gh       = mxGetPr(plhs[2]=mxCreateDoubleMatrix(n,1,mxREAL));
   H        = mxGetPr(plhs[3]=mxDuplicateArray(prhs[2]));
   itct     = mxGetPr(plhs[4]=mxCreateScalarDouble(0));
   fcount   = mxGetPr(plhs[5]=mxCreateScalarDouble(0));
   retcodeh = mxGetPr(plhs[6]=mxCreateScalarDouble(0));

   if (f[0] > 1e50) {
      mexWarnMsgTxt("Bad initial parameter");
      return;
   }

   crit = mxGetScalar(prhs[4]);
   nit = (int)mxGetScalar(prhs[5]);
   if (ngrad)
      /* if grad is not string, either compute it (if grad=[])
         or check whether it is fine) */
      if (mxIsEmpty(prhs[3]))
         numgrad(g[0],&badg[0],x[0]);
      else {
         for (i=0; i<n; i++)
            if (!grad[i])
               badg[0] = 1;
         memcpy(g[0],grad,n*sizeof(double));
      }
   else
      /* if grad is a string, compute it */
      gfeval(g[0],&badg[0],x[0]);
   retcode[2] = 101;
   /* iterate until done is false */
   while (!done) {
      for (i=0; i<n; i++)
         g[1][i] = g[2][i] = g[3][i] = 0;
      if (verbose==2) {
         mexPrintf("-----------------\n-----------------\n");
         mexPrintf("f at the beginning of new iteration, %20.10f\nx = ",f[0]);
         for (i=0; i<n; i++) {
            mexPrintf("%15.8g ",x[0][i]);
            if (i%4==3) mexPrintf("\n");
         }
         if (i%4<3) mexPrintf("\n");
      }
      *itct += 1;
      csminit(&f[1],x[1],&fc,&retcode[0],x[0],f[0],g[0],badg[0],H);
      *fcount += fc;
      /* if retcode1=1 gradient is zero and you are at the peak */
      if (retcode[0]!=1) {
         peakwall(g[1],&badg[1],retcode[0],x[1]);
         /* Bad gradient or back and forth on step length. */
         /* Possibly at cliff edge. Try perturbing search direction. */
         if (badg[1]) {
            double *Hcliff = mxCalloc(nn,sizeof(double));
            const double randmax=1/RAND_MAX;
            /* if stuck, give it another try by perturbing H */
            memcpy(Hcliff,H,nn*sizeof(double));
            for (i=0; i<nn; i+=n+1)
               Hcliff[i] *= 1+rand()*randmax;
            if (verbose)
               mexPrintf("Cliff.  Perturbing search direction.\n");
            csminit(&f[2],x[2],&fc,&retcode[1],x[0],f[0],g[0],badg[0],Hcliff);
            *fcount += fc;
            if  (f[2] < f[0]) {
               peakwall(g[2],&badg[2],retcode[1],x[2]);
               if (badg[2]) {
                  double *xx = mxCalloc(n,sizeof(double)), nx;
                  if (verbose)
                     mexPrintf("Cliff again.  Try traversing.\n");
                  for (i=0; i<n; i++)
                     xx[i] = x[2][i]-x[1][i];
                  nx = times(xx,xx);
                  if (sqrt(nx) < 1e-13) {
                     f[3] = f[0];
                     memcpy(x[3],x[0],n*sizeof(double));
                     badg[3] = true;
                     retcode[2] = 101;
                  } else {
                     double *gcliff = mxCalloc(n,sizeof(double)),
                            *eye = mxCalloc(n,sizeof(double));
                     const double dfnx = (f[2]-f[1])/nx;
                     for (i=0; i<n; i++) {
                        gcliff[i] = dfnx*xx[i];
                        eye[i*(n+1)] = 1;
                     }
                     csminit(&f[3],x[3],&fc,&retcode[2],x[0],f[0],gcliff,false,eye);
                     *fcount += fc;
                     peakwall(g[3],&badg[3],retcode[2],x[3]);
                     mxFree(eye);
                     mxFree(gcliff);
                  }
                  mxFree(xx);
               } else {
                  f[3] = f[0];
                  memcpy(x[3],x[0],n*sizeof(double));
                  badg[3] = true;
                  retcode[2] = 101;
               }
            } else {
               f[3] = f[0];
               memcpy(x[3],x[0],n*sizeof(double));
               badg[3] = true;
               retcode[2] = 101;
            }
            mxFree(Hcliff);
         } else {
            /* normal iteration, no walls, or else we're finished here. */
            f[2] = f[0];
            f[3] = f[0];
            badg[2] = true;
            badg[3] = true;
            retcode[1] = 101;
            retcode[2] = 101;
         }
      }
      /* how to pick gh and xh */
      if (f[3]<f[0] && badg[3]==0) {
         /* if 3 (transversing) was needed, it improved and gradient is good, take that */
         ih = 3;
         *fh = f[3];
         memcpy(xh,x[3],n*sizeof(double));
         memcpy(gh,g[3],n*sizeof(double));
         badgh = badg[3];
         *retcodeh = retcode[2];
      }
      else if (f[2]<f[0] && badg[2]==0) {
         /* if 2 (perturbig) was needed, it improved and gradient is good, take that */
         ih = 2;
         *fh = f[2];
         memcpy(xh,x[2],n*sizeof(double));
         memcpy(gh,g[2],n*sizeof(double));
         badgh = badg[2];
         *retcodeh = retcode[1];
      }
      else if (f[1]<f[0] && badg[1]==0) {
    /* if first try went fine, take that */
         ih = 1;
         *fh = f[1];
         memcpy(xh,x[1],n*sizeof(double));
         memcpy(gh,g[1],n*sizeof(double));
         badgh = badg[1];
         *retcodeh = retcode[0];
      }
      else {
         /* if nothing worked, just take the min of your attempts and compute the gradient */
         if (f[1] <= f[2])
            if (f[1] <= f[3]) ih = 1;
            else ih = 3;
         else if (f[2] <= f[3]) ih = 2;
            else ih = 3;
         *fh = f[i];
         memcpy(xh,x[ih],n*sizeof(double));
         *retcodeh = retcode[ih-1];
         if (nogh) {
            nogh = false;
            if (ngrad)
               numgrad(gh,&badgh,xh);
            else
               gfeval(gh,&badgh,xh);
         }
         badgh = true;
      }
      /* end of picking */
      stuck = fabs(*fh-f[0]) < crit;
      /* if nothing REALLY worked, too bad, you're stuck */
      if (!badg[0] && !badgh && !stuck) {
         /* if you are not stuck, update H0 matrix */
         dg = mminus(gh,g[0]);
         dx = mminus(xh,x[0]);
         bfgsi(H,dg,dx);
         mxFree(dx);
         mxFree(dg);
      }

      if (verbose==2)
         mexPrintf("----\nImprovement on iteration %d = %18.9f\n",(int)*itct,f[0]-*fh);
      if ((int)*itct > nit) {
         mexWarnMsgTxt("Iteration count termination");
         done = true;
      }
      else if (stuck) {
         if (verbose==2)
            mexPrintf("improvement < crit termination\n");
         done = true;
      }
      if (verbose)
         switch ((int)*retcodeh) {
            case 1:
               mexWarnMsgTxt("Zero gradient"); break;
            case 2:
               mexWarnMsgTxt("Back adjustment of stepsize didn't finish"); break;
            case 3:
               mexWarnMsgTxt("Smallest stepsize still improving too slow"); break;
            case 4:
               mexWarnMsgTxt("Forth adjustment of stepsize didn't finish"); break;
            case 6:
               mexWarnMsgTxt("Smallest step still improving too slow, reversed gradient"); break;
            case 5:
               mexWarnMsgTxt("Largest stepsize still improving too fast"); break;
            case 7:
               mexWarnMsgTxt("Possible inaccuracy in Hessian matrix"); break;
         }

      f[0] = *fh;
      memcpy(x[0],xh,n*sizeof(double));
      memcpy(g[0],gh,n*sizeof(double));
      badg[0] = badgh;
   }

   mxSetPr(fcnRhs[0],x[0]);    /* added to prevent destroying array other than x[0] */
   mxDestroyArray(fcnRhs[0]);
   mxFree(fcnRhs);
   for (i=0; i<4; i++)
      mxFree(g[i]);
   for (i=1; i<4; i++)
      mxFree(x[i]);
}

void numgrad(double *g, bool *badg, double *x) {
   static double delta=SCALE*1e-6, deltai=1e6/SCALE, f0, g0, tmp, *xp;
   register int i;
   f0 = feval(x);
   *badg = false;
   for (i=0, xp=x; i<n; i++, xp++, g++) {
      tmp = *xp;
      *xp += delta;
      g0 = (feval(x)-f0)*deltai;
      *xp = tmp;
      if (fabs(g0)<1e15)
         *g = g0;
      else {
         if (verbose)
            mexPrintf("Bad gradient.\n");
         *g = 0;
         *badg = true;
      }
   }
}

double feval(double *x) {
   /* switching pointers is less expensive than copying arrays */
   mxSetPr(*fcnRhs,x);
   if (mexCallMATLAB(1,&fcnLhs,npar,fcnRhs,fcn))
      mexErrMsgTxt("Error in the call to objective function.\n");
   return mxGetScalar(fcnLhs);
}

void gfeval(double *g, bool *badg, double *x) {
   /* switching pointers is less expensive than copying arrays */
   mxSetPr(*fcnRhs,x);
   if (mexCallMATLAB(2,gfcnLhs,npar,fcnRhs,gfcn))
      mexErrMsgTxt("Error in the call to gradient function.\n");
   /* cannot simply assign pointer since different g's can mess up */
   memcpy(g,mxGetPr(*gfcnLhs),n*sizeof(double));
   *badg = (bool)mxGetScalar(gfcnLhs[1]);
}

void csminit(double *fhat, double *xhat, int *fcount, int *retcode,
             double *x0, double f0, double *g, bool badg, double *H0) {
   double lambda=1, gnorm=0, dxnorm=0, factor=3, lambdaPeak=0;
   double f, dfhat, a, tmp, fPeak=f0, lambdaMax=mxGetInf();
   double *dx, *dxtest;
   bool done=false, shrink=true, shrinkSignal, growSignal;
   register int i;

   *fhat = f0;
   *fcount = 0;
   *retcode = 0;
   gnorm = sqrt(times(g,g));
   if ((gnorm < 1.e-12) && !badg)
      *retcode = 1;  /* gradient convergence */
   else {
      /* with badg true, we don't try to match rate of improvement to directional */
      /* derivative.  We're satisfied just to get some improvement in f. */
      dx = mxCalloc(n, sizeof(double));
      if (!dx) mexErrMsgTxt("Dynamic memory allocation error.\n");
      for (i=0; i<n; i++)
         dx[i] = -times(&H0[i*n],g);
      dxnorm = sqrt(times(dx,dx));
      if (dxnorm > 1e12) {
         mexPrintf("Near-singular H problem.\n");
         for (i=0; i<n; i++)
            dx[i] *= FCHANGE/dxnorm;
      }
      dfhat = times(dx,g);
      if (!badg) {
         /* test for alignment of dx with gradient and fix if necessary */
         a = -dfhat/(gnorm*dxnorm);
         if (a<ANGLE) {
            tmp = (ANGLE*dxnorm+dfhat/gnorm)/gnorm;
            for (i=0; i<n; i++)
               dx[i] -= tmp*g[i];
            dfhat = times(dx,g);
            dxnorm = sqrt(times(dx,dx));
            if (verbose)
               mexPrintf("Correct for low angle: %g\n",a);
         }
      }
      if (verbose==2)
          mexPrintf("Predicted improvement: %18.9f, Norm of gradient: %18.9f\n",-dfhat/2,gnorm);

      dxtest = mxCalloc(n, sizeof(double));
      while (!done) {
         for (i=0; i<n; i++)
            dxtest[i] = x0[i]+dx[i]*lambda;
         f = feval(dxtest);
         if (verbose==2)
            mexPrintf("lambda = %10.5g; f = %.7f",lambda,f);
         if (f<*fhat) {
            *fhat = f;
            memcpy(xhat,dxtest,n*sizeof(double));
         }
         (*fcount)++;
         tmp = -THETA*dfhat*lambda;

         /* the optimal lambda should be such that f0-f > -THETA*dfhat*lambda (see Berndt et al.) */
         /* If that's not the case and grad is good, OR if grad is bad and f is not going down, shrinkSignal = true */
         shrinkSignal = ( !badg && (f0-f <= (tmp>0?tmp:0)) ) ||
                         ( badg && (f0-f < 0 ) );

         /* the optimal lambda should also be such that f0-f<-(1-THETA)*dfhat*lambda */
         /* If that's not the case with lambda>0, AND grad is good, growthSignal = 1 */
         growSignal = !badg && (lambda > 0) && (f0-f >= -(1-THETA)*dfhat*lambda);

         if (verbose==2)
            mexPrintf("\tS-G = %d-%d",shrinkSignal,growSignal);

         /* If shrinkSignal=true AND ( lambda>lambdaPeak or lambda negative ) */
         /* (note when lambdaPeak=0 the second part only excludes lambda=0) */
         /* try shrinking lambda */
         if ( shrinkSignal && ( (lambda>lambdaPeak) || (lambda<0) ) ) {
       /* if shrink=false OR lambda/factor is already smaller than lambdaPeak, increase factor */
            if ( (lambda>0) && ((!shrink) || (lambda/factor <= lambdaPeak)) ) {
               shrink = true;
               factor = pow(factor,.6);
               while (lambda/factor <= lambdaPeak)
                  factor = pow(factor,.6);
               if (fabs(factor-1)<MINDFAC) {
                  if (fabs(lambda) < 4)
                     *retcode = 2;
                  else
                     *retcode = 7;
                  done = true;
               }
            }
            if ((lambda<lambdaMax) && (lambda>lambdaPeak))
               lambdaMax=lambda;
            /* shrink lambda */
            lambda /= factor;
            /* if lambda has already been shrunk as much as possible */
            if (fabs(lambda) < MINLAMB)
          /* if lambda is positive AND you have not made any improvement */
               /* try going against gradient, which may be inaccurate */
               if ((lambda > 0) && (f0 <= *fhat))
                  lambda = -lambda*pow(factor,6);
               else {
        /* if lambda is negative: let it be known and quit trying */
                  if (lambda < 0)
                     *retcode = 6;
                  /* if you have not made any imporvement: */
                  /* let it be known and quit trying */
                  else
                     *retcode = 3;
                  done = true;
               }
         }
         /* If growSignal=true and lambda positive OR ( lambda>lambdaPeak or lambda negative ) */
         /* (note when lambdaPeak=0 the second part only excludes lambda=0) */
         /* try increase lambda */
         else
            if ( (growSignal && (lambda > 0) ) ||
               ( shrinkSignal && (lambda <= lambdaPeak) && (lambda > 0) ) ) {
               if (shrink) {
                  shrink = false;
                  factor = pow(factor,.6);
                  if (fabs(factor-1) < MINDFAC) {
                     if (fabs(lambda) < 4)
                        *retcode = 4;
                     else
                        *retcode = 7;
                     done = true;
                  }
               }
               if ( (f<fPeak) && (lambda>0) ) {
                  fPeak = f;
                  lambdaPeak = lambda;
                  if (lambdaMax <= lambdaPeak)
                     lambdaMax = lambdaPeak*factor*factor;
               }
               /* increase lambda (up to 1e20) */
               lambda *= factor;
               /* if lambda has been increased up to the limit and */
               /* you have not made any imporvement: */
               /* let it be known and quit trying */
               if (fabs(lambda) > 1e20) {
                  *retcode = 5;
                  done = true;
               }
            }
       /* If growthSignal=shrinkSignal=false you found a good lambda, you are done */
            else {
               done = true;
               *retcode = factor<1.2 ? 7 : 0;
            }
         if (verbose==2)
            mexPrintf("\tfactor = %f\n",factor);
      }
      mxFree(dxtest);
      mxFree(dx);
   }
   if (verbose==2)
      mexPrintf("Norm of dx %10.5g; retcode = %d\n", dxnorm, *retcode);
}

double times(const double *x, const double *y) {
   double z = 0;
   register int i;
   for (i=0; i<n; i++, x++, y++)
      z += (*x)*(*y);
   return z;
}

void peakwall(double *g, bool *badg, int retcode, double *x) {
   /* if retcode=2 or 4 you have shrunk or increased lambda as much as you could: */
   /* exhausted search possibilities the csminit step has failed */
   if (retcode==2 || retcode==4)
      *badg = true;
   else
      /* if you are not at the peak but the csminit has improved, */
      /* compute the gradient again to update H0 */
      if (ngrad)
         numgrad(g,badg,x);
      else
         gfeval(g,badg,x);
}

void bfgsi(double *H, const double *dg, const double *dx) {
   double *Hdg, *dxdx, *dxHdg, *Hdgdx;
   double dgdx, m;
   register int i;

   Hdg = mxCalloc(n, sizeof(double));
   if (!Hdg) mexErrMsgTxt("Dynamic memory allocation error.\n");

   /* Hdg = H0*dg; */
   for (i=0; i<n; i++)
      Hdg[i] = times(&H[i*n],dg);
   /* dgdx = dg'*dx; */
   dgdx = 1/times(dg,dx);
   if (fabs(dgdx)<1e12) {
      dxdx = mtimes(dx,dx);
      dxHdg = mtimes(dx,Hdg);
      Hdgdx = mtimes(Hdg,dx);
      m = 1+times(dg,Hdg)*dgdx;
      for (i=0; i<nn; i++, H++, dxdx++, dxHdg++, Hdgdx++)
         *H += (m*(*dxdx)-(*dxHdg)-(*Hdgdx))*dgdx;
      mxFree(Hdgdx-nn);
      mxFree(dxHdg-nn);
      mxFree(dxdx-nn);
   } else {
      mexWarnMsgTxt("BFGS update failed");
      mexPrintf("|dg| = %f  |dx| = %f\n",sqrt(times(dg,dg)),sqrt(times(dx,dx)));
      mexPrintf("dg\'*dx = %f\n",dgdx);
      mexPrintf("|H*dg| = %f\n",sqrt(times(Hdg,Hdg)));
   }
   mxFree(Hdg);
}

double *mtimes(const double *x, const double *y) {
   const double *x0;
   double *z;
   register int i, j;
   z = mxCalloc(nn, sizeof(double));
   for (i=0, x0=x; i<n; i++, y++)
      for (j=0, x=x0; j<n; j++, x++, z++)
         *z = (*x)*(*y);
   return z-nn;
}

double *mminus(const double *x, const double *y) {
   double *z;
   register int i;
   z = mxCalloc(n, sizeof(double));
   for (i=0; i<n; i++, x++, y++, z++)
      *z = (*x)-(*y);
   return z-n;
}