lib/functions.c

   1 /* functions - callbacks for Bayes routines in XLISP-STAT and S        */
   2 /* XLISP-STAT 2.1 Copyright (c) 1990, by Luke Tierney                  */
   3 /* Additions to Xlisp 2.1, Copyright (c) 1989 by David Michael Betz    */
   4 /* You may give out copies of this software; for conditions see the    */
   5 /* file COPYING included with this distribution.                       */
   6
   7 #include <stdio.h>
   8 #include "xmath.h"
   9
  10 #define PRINTSTR(s) printf(s)
  11
  12 extern void *S_alloc(), *calloc(), *realloc();
  13 extern double macheps();
  14 char *minresultstring();
  15
  16 extern void choldecomp();
  17
  18 extern void minsupplyvalues(double , double *, double **,
  19                             double *, double **);
  20 extern void minimize();
  21 extern void minresults(double *, double *, double *, double *, double **,
  22                        double *, double **, int *, int *, double *);
  23 extern void minsetup(size_t n, size_t k,
  24                      int *ffun(), int *gfun(),
  25                      double *x, double typf, double *typx, char *work);
  26                          /*     int  (*ffun)(), (*gfun)();*/
  27 extern void minsetoptions(double , double, double, int , int , int , int);
  28
  29 extern void choldecomp();
  30
  31
  32
  33 /* next 2 from cbayes.c */
  34 extern void Recover(char *, char *);
  35 extern void call_S(char *fun, long narg, char **args, char **mode, long *length,char **names,
  36                    long nvals, char **values);
  37
  38 /* next 2 from derivatives.c */
  39 /*
  40 extern void numergrad(size_t n, double *x, double *grad, double *fsum,
  41                       int *ffun(), double h, double *typx);
  42 extern void numerhess(size_t n, double *x, double **hess, double f,
  43                       double *fsum, void ffun(),
  44                       double h, double *typx);
  45 */
  46 extern void numergrad(size_t , double *, double *, double *,
  47                       int ffun(double *, double *, double *, double **),
  48                       double , double *);
  49 extern void numerhess(size_t , double *, double **, double ,
  50                       double *,
  51                       int ffun(double *, double *, double *, double **),
  52                       double , double *);
  53
  54 /* next from minimize */
  55 extern size_t minworkspacesize(size_t, size_t);
  56
  57 /************************************************************************/
  58 /**                                                                    **/
  59 /**                      Definitions and Globals                       **/
  60 /**                                                                    **/
  61 /************************************************************************/
  62
  63 /* #define NULL 0L  already defined in stddef */
  64
  65 #define nil 0L
  66 #define TRUE 1
  67 #define FALSE 0
  68
  69 #define ROOT2PI 2.50662827463100050241
  70 #define PI_INV .31830988618379067153
  71
  72 #define GRADTOL_POWER 1.0 / 3.0
  73 #define H_POWER 1.0 / 6.0
  74
  75 /*typedef double **RMatrix, *RVector;*/
  76
  77 typedef struct{
  78   char *f, **sf, **g;
  79   size_t n, k;
  80   int change_sign, fderivs;
  81   int *gderivs;
  82   double typf, h, dflt;
  83   double *typx, *fsum, *cvals, *ctarget;
  84   double **gfsum;
  85 } Fundata;
  86
  87 static Fundata func, gfuncs, cfuncs;
  88
  89 /************************************************************************/
  90 /**                                                                    **/
  91 /**                         Memory Utilities                           **/
  92 /**                                                                    **/
  93 /************************************************************************/
  94
  95 /* this function is used to maintain a statically allocated piece of    */
  96 /* memory of a specified size. If a larger piece is needed the pointer  */
  97 /* is realloced. This allows functions using memory allocation to be    */
  98 /* called repeatedly (but not recursively) from within the same call    */
  99 /* from S. It attempts to avoid the danger of dangling callocs.         */
 100
 101 void static
 102 makespace(char **pptr, size_t size)
 103 {
 104   if (size <= 0) return;
 105   if (*pptr == nil) *pptr = calloc(size, 1);
 106   else *pptr = realloc(*pptr, size);
 107   if (size > 0 && *pptr == nil) Recover("memory allocation failed", NULL);
 108 }
 109
 110 /************************************************************************/
 111 /**                                                                    **/
 112 /**                    Functions Evaluation Routines                   **/
 113 /**                                                                    **/
 114 /************************************************************************/
 115
 116 /*
 117  * All Hessian evaluations by numerical derivatives assume the gradient is
 118  * evaluated first at the same location. The results are cached away.
 119  */
 120
 121 /* install log posterior function */
 122 static void
 123 install_func(char *f, char **sf, size_t n, int change_sign, /*?? */
 124                     double typf, double h, double *typx, double dflt)
 125 {
 126   int i;
 127   static int inited = FALSE;
 128
 129   if (! inited) {
 130     func.typx = nil;
 131     func.fsum = nil;
 132     inited = TRUE;
 133   }
 134   makespace(&func.typx, n * sizeof(double));
 135   makespace(&func.fsum, n * sizeof(double));
 136
 137   func.f = f;
 138   func.sf = sf;
 139   func.n = n;
 140   func.change_sign = change_sign;
 141   func.typf = (typf > 0.0) ? typf : 1.0;
 142   func.h = (h > 0.0) ? h : pow(macheps(), H_POWER);
 143   for (i = 0; i < n; i++)
 144     func.typx[i] = (typx != nil && typx[i] > 0.0) ? typx[i] : 1.0;
 145   func.dflt = dflt;
 146   func.fderivs = 0;
 147 }
 148
 149 /* install tilt functions */
 150 static void
 151 install_gfuncs(char **g, size_t n, size_t k, int change_sign, double h, double *typx)
 152 {
 153   size_t i;
 154   static int inited = FALSE;
 155   static double *gfsumdata = nil;
 156
 157   if (! inited) {
 158     gfuncs.typx = nil;
 159     gfuncs.gfsum = nil;
 160     gfuncs.gderivs = nil;
 161     inited = TRUE;
 162   }
 163   makespace(&gfuncs.typx, n * sizeof(double));
 164   makespace(&gfuncs.gfsum, k * sizeof(double *));
 165   makespace(&gfsumdata, k * n * sizeof(double));
 166   makespace(&gfuncs.gderivs, k *sizeof(int));
 167
 168   gfuncs.g = g;
 169   gfuncs.n = n;
 170   gfuncs.k = k;
 171   gfuncs.change_sign = change_sign;
 172   gfuncs.h = (h > 0.0) ? h : pow(macheps(), H_POWER);
 173   for (i = 0; i < n; i++)
 174     gfuncs.typx[i] = (typx != nil && typx[i] > 0.0) ? typx[i] : 1.0;
 175   for (i = 0; i < k; i++) gfuncs.gfsum[i] = gfsumdata + i * n;
 176   return;
 177 }
 178
 179 /* install constraint functions */
 180 static void
 181 install_cfuncs(char **g, size_t n, size_t k, double *ctarget, double h, double *typx)
 182 {
 183   size_t i;
 184   static int inited = FALSE;
 185
 186   if (! inited) {
 187     cfuncs.typx = nil;
 188     cfuncs.fsum = nil;
 189     cfuncs.gderivs = nil;
 190     inited = TRUE;
 191   }
 192   makespace(&cfuncs.typx, n * sizeof(double));
 193   makespace(&cfuncs.fsum, n * sizeof(double));
 194   makespace(&cfuncs.gderivs, k * sizeof(int));
 195
 196   cfuncs.g = g;
 197   cfuncs.n = n;
 198   cfuncs.k = k;
 199   cfuncs.h = (h > 0.0) ? h : pow(macheps(), H_POWER);
 200   for (i = 0; i < n; i++)
 201     cfuncs.typx[i] = (typx != nil && typx[i] > 0.0) ? typx[i] : 1.0;
 202   cfuncs.ctarget = ctarget;
 203   return;
 204 }
 205
 206 static int
 207 in_support(char **ff, size_t n, double *x)
 208 {
 209   char *args[1], *values[1];
 210   int *result;
 211   char *mode[1];
 212   long length[1];
 213
 214   if (ff == nil || ff[0] == nil) {
 215     return(TRUE);
 216   } else {
 217     mode[0] = "double";
 218     length[0] =n;
 219     args[0] = (char *) x;
 220     call_S(ff[0], 1L, args, mode, length, 0L, 1L, values);
 221     result = (int *) values[0];
 222     return(result[0]);
 223   }
 224 }
 225
 226 /* callback for logposterior evaluation */
 227 static int
 228 evalfunc(double *x, double *pval, double *grad, double **hess)
 229 {
 230   char *args[1], *values[3];
 231   double *result, val;
 232   char *mode[1];
 233   long length[1];
 234   int i, j;
 235
 236   for (i = 0; i < 3; i++) values[i] = nil;
 237
 238   if (in_support(func.sf, func.n, x)) {
 239     if (pval != nil || func.fderivs > 0 || hess != nil) {
 240       mode[0] = "double";
 241       length[0] = func.n;
 242       args[0] = (char *) x;
 243       call_S(func.f, 1L, args, mode, length, 0L, 3L, values);
 244       result = (double *) values[0];
 245           val = (! func.change_sign) ? result[0] : -result[0];
 246       if (pval != nil) *pval = val;
 247       if (values[2] != nil) func.fderivs = 2;
 248       else if (values[1] != nil) func.fderivs = 1;
 249       else func.fderivs = 0;
 250     }
 251     if (grad != nil) {
 252       if (func.fderivs > 0) {
 253         result = (double *) values[1];
 254         for (i = 0; i < func.n; i++)
 255           grad[i] = (! func.change_sign) ? result[i] : -result[i];
 256       }
 257       else {
 258         numergrad(func.n, x, grad, func.fsum, evalfunc, func.h, func.typx);
 259       }
 260     }
 261     if (hess != nil) {
 262       if (func.fderivs == 2) {
 263         result = (double *) values[2];
 264         for (i = 0; i < func.n; i++)
 265           for (j = 0; j < func.n; j++)
 266             hess[i][j] = (! func.change_sign) ? result[i + j * func.n]
 267                                               : -result[i + j * func.n];
 268       }
 269       else {
 270         if (func.fderivs == 1) /* kludge to get fsum for analytic gradients */
 271           numergrad(func.n, x, func.fsum, func.fsum,
 272                     evalfunc, func.h, func.typx);
 273         numerhess(func.n, x, hess, val, func.fsum, evalfunc, func.h, func.typx);
 274       }
 275     }
 276     return(TRUE);
 277   } else {
 278     if (pval != nil) {
 279       *pval = func.dflt;
 280     }
 281     return(FALSE);
 282   }
 283   return(TRUE);
 284 }
 285
 286
 287 /* callback for tilt function evaluation */
 288 static size_t which_gfunc;
 289
 290 static int
 291 evalgfunc(double *x, double *pval, double *grad, double **hess)
 292 {
 293   char *args[1], *values[3];
 294   double *result, val;
 295   char *mode[1];
 296   long length[1];
 297   int i, j;
 298
 299   for (i = 0; i < 3; i++) values[i] = nil;
 300
 301   if (pval != nil || gfuncs.gderivs[which_gfunc] > 0 || hess != nil) {
 302     mode[0] = "double";
 303     length[0] = gfuncs.n;
 304     args[0] = (char *) x;
 305     call_S(gfuncs.g[which_gfunc], 1L, args, mode, length, 0L, 3L, values);
 306     result = (double *) values[0];
 307         val = result[0];
 308     if (pval != nil) *pval = result[0];
 309     if (values[2] != nil) gfuncs.gderivs[which_gfunc] = 2;
 310     else if (values[1] != nil) gfuncs.gderivs[which_gfunc] = 1;
 311     else gfuncs.gderivs[which_gfunc] = 0;
 312   }
 313   if (grad != nil) {
 314     if (gfuncs.gderivs[which_gfunc] > 0) {
 315       result = (double *) values[1];
 316       for (i = 0; i < gfuncs.n; i++) grad[i] = result[i];
 317     }
 318     else {
 319       numergrad(gfuncs.n, x, grad, gfuncs.gfsum[which_gfunc], evalgfunc,
 320                 gfuncs.h, gfuncs.typx);
 321     }
 322   }
 323   if (hess != nil) {
 324     if (gfuncs.gderivs[which_gfunc] == 2) {
 325       result = (double *) values[2];
 326       for (i = 0; i < gfuncs.n; i++)
 327         for (j = 0; j < gfuncs.n; j++)
 328           hess[i][j] = result[i + j * gfuncs.n];
 329     }
 330     else {
 331       /* kludge to get fsum if analytic gradient used */
 332       if (gfuncs.gderivs[which_gfunc] == 1)
 333         numergrad(gfuncs.n, x, gfuncs.gfsum[which_gfunc],
 334                   gfuncs.gfsum[which_gfunc], evalgfunc, gfuncs.h, gfuncs.typx);
 335       numerhess(gfuncs.n, x, hess, val, gfuncs.gfsum[which_gfunc], evalgfunc,
 336                 gfuncs.h, gfuncs.typx);
 337     }
 338   }
 339   return(TRUE);
 340 }
 341
 342 /* callback for constraint function evaluation */
 343 static int which_cfunc;
 344
 345 static int
 346 evalcfunc(double *x, double *pval, double *grad, double **hess)
 347 {
 348   char *args[1], *values[3];
 349   double *result, val;
 350   char *mode[1];
 351   long length[1];
 352   int i, j;
 353
 354   if (pval != nil || cfuncs.gderivs[which_cfunc] > 0 || hess != nil) {
 355     mode[0] = "double";
 356     length[0] = cfuncs.n;
 357     args[0] = (char *) x;
 358     call_S(cfuncs.g[which_cfunc], 1L, args, mode, length, 0L, 3L, values);
 359     result = (double *) values[0];
 360         val = result[0];
 361     if (pval != nil) {
 362       *pval = result[0];
 363       if (cfuncs.ctarget != nil) *pval -= cfuncs.ctarget[which_cfunc];
 364     }
 365     if (values[2] != nil) cfuncs.gderivs[which_cfunc] = 2;
 366     else if (values[1] != nil) cfuncs.gderivs[which_cfunc] = 1;
 367     else cfuncs.gderivs[which_cfunc] = 0;
 368   }
 369   if (grad != nil) {
 370     if (cfuncs.gderivs[which_cfunc] > 0) {
 371       result = (double *) values[1];
 372       for (i = 0; i <cfuncs.n; i++) grad[i] = result[i];
 373     }
 374     else {
 375       numergrad(cfuncs.n, x, grad, cfuncs.fsum, evalcfunc,
 376                 cfuncs.h, cfuncs.typx);
 377     }
 378   }
 379   if (hess != nil) {
 380     if (cfuncs.gderivs[which_cfunc] == 2) {
 381       result = (double *) values[2];
 382       for (i = 0; i <cfuncs.n; i++)
 383         for (j = 0; j <cfuncs.n; j++)
 384           hess[i][j] = result[i + j * cfuncs.n];
 385     }
 386     else {
 387       /* kludge to get fsum if analytic gradient used */
 388       if (cfuncs.gderivs[which_cfunc] == 1)
 389         numergrad(cfuncs.n, x, cfuncs.fsum, cfuncs.fsum, evalcfunc,
 390                   cfuncs.h, cfuncs.typx);
 391       numerhess(cfuncs.n, x, hess, val, cfuncs.fsum, evalcfunc,
 392                 cfuncs.h, cfuncs.typx);
 393     }
 394   }
 395   return(TRUE);
 396 }
 397
 398 /* S front end for logposterior evaluation */
 399 void
 400 evalfront(char **ff, size_t *n, double *x, double *val, double *grad,
 401           double *phess, double *h, double *typx)
 402 {
 403   size_t i;
 404   static double **hess = nil;
 405
 406   install_func(ff[0], nil, *n, FALSE, 1.0, *h, typx, 0.0);
 407   if (phess == nil) hess = nil;
 408   else {
 409     makespace(&hess, *n * sizeof(double *));
 410     for (i = 0; i < *n; i++, phess += *n) hess[i] = phess;
 411   }
 412   evalfunc(x, val, grad, hess);
 413 }
 414
 415 /* S front end for tilt function evaluation */
 416 void
 417 gevalfront(char **gg, size_t *n, size_t *m, double *x, double *h,
 418            double *typx, double *val, double *grad)
 419 {
 420   size_t i;
 421
 422   install_gfuncs(gg, *n, *m, FALSE, *h, typx);
 423   for (i = 0; i < *m; i++, val++) {
 424     which_gfunc = i;
 425     evalgfunc(x, val, grad, nil);
 426     if (grad != nil) grad += *n;
 427   }
 428   return;
 429 }
 430
 431 /************************************************************************/
 432 /**                                                                    **/
 433 /**                     Derivative Scaling Routines                    **/
 434 /**                                                                    **/
 435 /************************************************************************/
 436
 437 /*
 438 void
 439 derivscalefront(char **ff, int *n, double *x, double *h, double *typx, double *tol, int *info)
 440 {
 441   int i;
 442
 443   if (*tol <= 0.0) *tol = pow(macheps(), GRADTOL_POWER);
 444
 445   install_func(ff[0], nil, *n, TRUE, 1.0, *h, typx, 0.0);
 446   *info = check_derivs(x, *tol);
 447
 448   *h = func.h;
 449   for (i = 0; i < *n; i++) typx[i] = func.typx[i];
 450   return;
 451 }
 452
 453
 454 static int
 455 check_derivs(double *x, double drvtol)
 456 {
 457   static double *grad = nil, work = nil;
 458   static double **hess = nil;
 459   int i, error;
 460
 461   grad = (double *) S_alloc(func.n, sizeof(double));
 462   hess = (double **) S_alloc(func.n, sizeof(double *));
 463   work = (double *) S_alloc(func.n + func.n * func.n, sizeof(double));
 464
 465   for (i = 0; i < func.n; i++) {
 466     hess[i] = work;
 467     work += func.n;
 468   }
 469
 470   error = derivscale(func.n, x, grad, hess, func.fsum, evalfunc,
 471                      &func.h, func.typx, drvtol, work);
 472   return(error);
 473 }
 474 */
 475
 476 /************************************************************************/
 477 /**                                                                    **/
 478 /**                    Importance Sampling Routines                    **/
 479 /**                                                                    **/
 480 /************************************************************************/
 481
 482 /* joint density of normal-cauchy mixture */
 483 static double
 484 dncmix(double *x, size_t n, double p)
 485 {
 486   size_t i;
 487   double dens;
 488
 489   for (i = 0, dens = 1.0; i < n; i++) {
 490     dens *= p * exp(-0.5 * x[i] * x[i]) / ROOT2PI
 491       + (1 - p) * PI_INV / (1.0 + x[i] * x[i]);
 492   }
 493   return(dens);
 494 }
 495
 496 /*
 497  * S front end for computing sample from transformed normal-cauchy
 498  * mixture and importance sampling weights
 499  */
 500 void
 501 samplefront(char **ff, char **sf, char **rf,
 502             double *p, size_t *n,
 503             double *x, double *ch, int *N, double *y, double *w)
 504 {
 505   double val;
 506   int i, j, k;
 507   char *args[2], *values[1];
 508   double *result, mval, c, dens, m;
 509   char *mode[2];
 510   long length[2];
 511
 512   /* get the random variables */
 513   mode[0] = "double"; mode[1] = "double";
 514   length[0] = 1; length[1] = 1;
 515   m = *N * *n; args[0] = (char *) &m; args[1] = (char *) p;
 516   call_S(rf[0], 2L, args, mode, length, 0L, 1L, values);
 517   result = (double *) values[0];
 518   for (i = 0; i < m; i++) y[i] = result[i];
 519
 520   /* construct the sample and the weights */
 521   install_func(ff[0], sf, *n, FALSE, 1.0, -1.0, nil, 0.0);
 522   c = 1.0 / pow(ROOT2PI, (double) *n);
 523   evalfunc(x, &mval, nil, nil);
 524   for (i = 0; i < *N; i++, y += *n) {
 525     dens = dncmix(y, *n, *p);
 526     for (j = 0; j < *n; j++) {
 527       val = x[j];
 528       for (k = j; k < *n; k++) val += y[k] * ch[j + *n * k];
 529       y[j] = val;
 530     }
 531     if (evalfunc(y, &val, nil, nil)) w[i] = exp(val - mval) * c / dens;
 532     else w[i] = 0.0;
 533   }
 534   return;
 535 }
 536
 537
 538 /************************************************************************/
 539 /**                                                                    **/
 540 /**                       Maximization Routines                        **/
 541 /**                                                                    **/
 542 /************************************************************************/
 543
 544 typedef struct {
 545   int n, m, k, itnlimit, backtrack, verbose, vals_suppl, exptilt;
 546   int count, termcode;
 547 } MaxIPars;
 548
 549 typedef struct {
 550   double typf, h, gradtol, steptol, maxstep, dflt, tilt, newtilt, hessadd;
 551 } MaxDPars;
 552
 553 struct {
 554   double tilt;
 555   double *gval;
 556   double  **ggrad, **ghess;
 557   int exptilt;
 558   double *tscale;
 559 } tiltinfo;
 560
 561 static void
 562 add_tilt(double *x, double *pval, double *grad, double **hess,
 563          double tilt, int exptilt)
 564 {
 565   size_t i, j, k, n = func.n, m = gfuncs.k;
 566   double *gval, *ggrad, **ghess, etilt;
 567
 568   if (m == 0) return;
 569
 570   if (gfuncs.change_sign) tilt = -tilt;
 571
 572   for (k = 0; k < m; k++) {
 573     gval = (pval != nil) ? tiltinfo.gval + k : nil;
 574     ggrad = (grad != nil) ? tiltinfo.ggrad[k] : nil;
 575     ghess = (hess != nil) ? tiltinfo.ghess : nil;
 576
 577     which_gfunc = k;
 578     evalgfunc(x, gval, ggrad, ghess);
 579
 580     if (exptilt) {
 581       etilt = (tiltinfo.tscale != nil) ? tilt / tiltinfo.tscale[k] : tilt;
 582       if (pval != nil) *pval += etilt * *gval;
 583       if (grad != nil)
 584         for (i = 0; i < n; i++) grad[i] += etilt * ggrad[i];
 585       if (hess != nil)
 586         for (i = 0; i < n; i++)
 587           for (j = 0; j < n; j++) hess[i][j] += etilt * ghess[i][j];
 588     }
 589     else {
 590       gval = tiltinfo.gval;
 591       ggrad = tiltinfo.ggrad[k];
 592       ghess = tiltinfo.ghess;
 593       if (gval[k] <= 0.0) Recover("nonpositive function value", NULL);
 594       if (pval != nil) *pval += tilt * log(gval[k]);
 595       if (grad != nil)
 596         for (i = 0; i < n; i++) grad[i] += tilt * ggrad[i] / gval[k];
 597       if (hess != nil)
 598         for (i = 0; i < n; i++)
 599           for (j = 0; j < n; j++)
 600             hess[i][j] +=
 601               tilt * (ghess[i][j] / gval[k]
 602                       - (ggrad[i] / gval[k]) * (ggrad[j] / gval[k]));
 603     }
 604   }
 605 }
 606
 607 static void
 608 set_tilt_info(size_t n, size_t m,
 609               double tilt, int exptilt, double *tscale)
 610 {
 611   static double *hessdata = nil, *graddata = nil;
 612   size_t i;
 613   static int inited = FALSE;
 614
 615   if (! inited) {
 616     tiltinfo.gval = nil;
 617     tiltinfo.ggrad = nil;
 618     tiltinfo.ghess = nil;
 619     inited = TRUE;
 620   }
 621   makespace(&tiltinfo.gval, n * sizeof(double));
 622   makespace(&tiltinfo.ggrad, m * sizeof(double *));
 623   makespace(&tiltinfo.ghess, n * sizeof(double *));
 624   makespace(&graddata, n * m * sizeof(double));
 625   makespace(&hessdata, n * n * sizeof(double));
 626
 627   tiltinfo.tilt = tilt;
 628   tiltinfo.exptilt = exptilt;
 629   for (i = 0; i < m; i++) tiltinfo.ggrad[i] = graddata + i * n;
 630   for (i = 0; i < n; i++) tiltinfo.ghess[i] = hessdata + i * n;
 631   tiltinfo.tscale = tscale;
 632 }
 633
 634
 635 static void
 636 minfunc(double *x, double *pval, double *grad, double **hess)
 637 {
 638   size_t k = gfuncs.k;
 639
 640   if (evalfunc(x, pval, grad, hess) && (k > 0))
 641     add_tilt(x, pval, grad, hess, tiltinfo.tilt, tiltinfo.exptilt);
 642 }
 643
 644 void
 645 constfunc(double *x, double *vals, double **jac, double **hess)
 646 {
 647   int i, k = cfuncs.k;
 648   double *pvali, *jaci;
 649
 650   for (i = 0; i < k; i++) {
 651     pvali = (vals != nil) ? vals + i : nil;
 652     jaci = (jac != nil) ? jac[i] : nil;
 653     which_cfunc = i;
 654     evalcfunc(x, pvali, jaci, nil);
 655   }
 656 }
 657
 658 void
 659 maxfront(char **ff, char **gf, char **cf,
 660          double *x, double *typx, double *fvals, double *gvals, double *cvals, double *ctarget,
 661          MaxIPars *ipars, MaxDPars *dpars,
 662          double *tscale, char **msg)
 663 {
 664   static char *work = nil;
 665   static double **H = nil, **cJ = nil;
 666   double *pf, *grad, *c;
 667   size_t i, n, m, k;
 668   void (*cfun)();
 669
 670   if (ipars->verbose > 0) PRINTSTR("maximizing...\n");
 671
 672   n = ipars->n;
 673   m = ipars->m;
 674   k = ipars->k;
 675   if (k >= n) Recover("too many constraints", NULL);
 676
 677   makespace(&H, n * sizeof(double *));
 678   makespace(&work, minworkspacesize(n, k));
 679
 680   pf = fvals; fvals++;
 681   grad = fvals; fvals += n;
 682   for (i = 0; i < n; i++, fvals += n) H[i] = fvals;
 683   set_tilt_info(n, m, dpars->newtilt, ipars->exptilt, tscale);
 684
 685   if (k == 0) {
 686     c = nil;
 687     cJ = nil;
 688     cfun = nil;
 689   } else {
 690     c = cvals;
 691     cvals += k;
 692     makespace(&cJ, k * sizeof(double *));
 693     for (i = 0; i < k; i++) cJ[i] = cvals + i * n;
 694     cfun = &constfunc; /* AJR: pointer to constfunc? */
 695   }
 696
 697   install_func(ff[0], nil, n, TRUE, dpars->typf, dpars->h, typx, dpars->dflt);
 698   install_gfuncs(gf, n, m, TRUE, dpars->h, typx);
 699   install_cfuncs(cf, n, k, ctarget, dpars->h, typx);
 700
 701   minsetup(n, k, &minfunc, &cfun, x, dpars->typf, typx, work); /* AJR: FIXME arg 3,4 */
 702   minsetoptions(dpars->gradtol, dpars->steptol, dpars->maxstep,
 703                 ipars->itnlimit, ipars->verbose, ipars->backtrack, TRUE);
 704
 705   if (ipars->vals_suppl) {
 706     for (i = 0; i < k; i++) c[i] -= ctarget[i];
 707     if (dpars->newtilt != dpars->tilt) {
 708       add_tilt(x, pf, grad, H, dpars->newtilt - dpars->tilt, ipars->exptilt);
 709       dpars->tilt = dpars->newtilt;
 710     }
 711     minsupplyvalues(*pf, grad, H, c, cJ);
 712   }
 713
 714   minimize();
 715   minresults(x, pf, nil, grad, H, c, cJ, &ipars->count, &ipars->termcode,
 716              &dpars->hessadd);
 717   msg[0] = minresultstring(ipars->termcode);
 718
 719   for (i = 0; i < k; i++) c[i] += ctarget[i];
 720   ipars->vals_suppl = TRUE;
 721 }
 722
 723 /************************************************************************/
 724 /**                                                                    **/
 725 /**                     Log Laplace Approximation                      **/
 726 /**                                                                    **/
 727 /************************************************************************/
 728
 729 void
 730 loglapdet(double *fvals, double *cvals, MaxIPars *ipars, MaxDPars *dpars,
 731           double *val, int *detonly)
 732 {
 733   int i, j, l, n = ipars->n, k = ipars->k;
 734   double f = -fvals[0], *hessdata = fvals + n + 1, *cgraddata = cvals + k;
 735   double ldL, ldcv, maxadd;
 736   static double **hess = nil, **cgrad = nil;
 737
 738   if (k >= n) Recover("too many constraints", NULL);
 739
 740   makespace(&hess, n * sizeof(double *));
 741   makespace(&cgrad, k * sizeof(double *));
 742
 743   for (i = 0; i < n; i++) hess[i] = hessdata + i * n;
 744   for (i = 0; i < k; i++) cgrad[i] = cgraddata + i * n;
 745
 746   choldecomp(hess, n, 0.0, &maxadd);
 747   /**** do something if not pos. definite ****/
 748
 749   for (i = 0, ldL = 0.0; i < n; i++) ldL += log(hess[i][i]);
 750
 751   if (k > 0) {
 752     /* forward solve for (L^-1) cgrad^T */
 753     for (l = 0; l < k; l++) {
 754       for (i = 0; i < n; i++) {
 755         if (hess[i][i] != 0.0) cgrad[l][i] /= hess[i][i];
 756         for (j = i + 1; j < n; j++) cgrad[l][j] -= hess[j][i] * cgrad[l][i];
 757       }
 758     }
 759
 760     /* compute sigma and stdev */
 761     for (i = 0; i < k; i++) {
 762       for (j = i; j < k; j++) {
 763         for (l = 0, hess[i][j] = 0.0; l < n; l++)
 764           hess[i][j] += cgrad[i][l] * cgrad[j][l];
 765         hess[j][i] = hess[i][j];
 766       }
 767     }
 768
 769     choldecomp(hess, k, 0.0, &maxadd);
 770     /**** do something if not pos. definite ****/
 771     for (i = 0, ldcv = 0.0; i < k; i++) ldcv += log(hess[i][i]);
 772   }
 773   else ldcv = 0.0;
 774
 775   *val = (n - k) * log(ROOT2PI) - ldL - ldcv;
 776   if (! *detonly) *val += f;
 777 }
 778
 779 #ifdef SBAYES
 780
 781 loglapfront(fvals, cvals, ipars, dpars, val)
 782      double *fvals, *cvals;
 783      MaxIPars *ipars;
 784      MaxDPars *dpars;
 785      double *val;
 786 {
 787   int detonly = FALSE;
 788
 789   loglapdet(fvals, cvals, ipars, dpars, val, &detonly);
 790 }
 791
 792 /************************************************************************/
 793 /**                                                                    **/
 794 /**                        First Order Moments                         **/
 795 /**                                                                    **/
 796 /************************************************************************/
 797
 798 moms1front(char *gf, int *n, int *m, double *x, double *hessdata, double *mean,
 799            double *stdev, double *sigmadata, double *h, double *typx)
 800 {
 801   int i, j, k;
 802   double *hess, *sigma, *delg;
 803   double *delgdata, maxadd;
 804
 805   hess = (double **) S_alloc(*n, sizeof(double *));
 806   sigma = (double **) S_alloc(*m, sizeof(double *));
 807   delg = (double **) S_alloc(*m, sizeof(double *));
 808   delgdata = (double *) S_alloc(*m * *n, sizeof(double));
 809
 810   for (i = 0; i < *n; i++) hess[i] = hessdata + i * *n;
 811   for (i = 0; i < *m; i++) sigma[i] = sigmadata + i * *m;
 812   for (i = 0; i < *m; i++) delg[i] = delgdata + i * *n;
 813
 814   gevalfront(gf, n, m, x, h, typx, mean, delgdata);
 815
 816   /* get the cholesky decomposition L of the hessian */
 817   choldecomp(hess, *n, 0.0, &maxadd);
 818
 819   /* forward solve for (L^-1) delg^T */
 820   for (k = 0; k < *m; k++) {
 821     for (i = 0; i < *n; i++) {
 822       if (hess[i][i] != 0.0) delg[k][i] /= hess[i][i];
 823       for (j = i + 1; j < *n; j++) delg[k][j] -= hess[j][i] * delg[k][i];
 824     }
 825   }
 826
 827   /* compute sigma and stdev */
 828   for (i = 0; i < *m; i++) {
 829     for (j = i; j < *m; j++) {
 830       for (k = 0, sigma[i][j] = 0.0; k < *n; k++)
 831         sigma[i][j] += delg[i][k] * delg[j][k];
 832       sigma[j][i] = sigma[i][j];
 833     }
 834     stdev[i] = sqrt(sigma[i][i]);
 835   }
 836 }
 837
 838 /************************************************************************/
 839 /**                                                                    **/
 840 /**                        Second Order Moments                        **/
 841 /**                                                                    **/
 842 /************************************************************************/
 843
 844 typedef struct {
 845   MaxIPars max;
 846   int full, covar;
 847 } MomIPars;
 848
 849 typedef struct {
 850   MaxDPars max;
 851   double mgfdel;
 852 } MomDPars;
 853
 854 moms2front(char **ff, char **gf, int *gnum, double *x, double *typx,
 855            double *fvals, double *gvals, MomIPars *ipars, MomDPars *dpars,
 856            double *mean, double *stdev, double *sigmadata)
 857 {
 858   char *msg;
 859   double h, loglap0, loglap1, loglap2;
 860   double *tilts, *fvals1, *gvals1, *x1;
 861   MomDPars dp1, *dpars1 = &dp1;
 862   MomIPars ip1, *ipars1 = &ip1;
 863   int i, n, m;
 864
 865   n = ipars->max.n;
 866   m = *gnum;
 867   h = dpars->max.h;
 868
 869   tilts = (double *) S_alloc(m, sizeof(double));
 870   x1 = (double *) S_alloc(n, sizeof(double));
 871   fvals1 = (double *) S_alloc(1 + n + n * n, sizeof(double));
 872   gvals1 = (double *) S_alloc(m + n * m, sizeof(double));
 873
 874   maxfront(ff, nil, nil, x, typx, fvals, gvals, nil, nil,
 875            ipars, dpars, nil, &msg);
 876   copypars(x, fvals, gvals, ipars, dpars, x1, fvals1, gvals1, ipars1, dpars1);
 877   loglapfront(fvals1, nil, ipars1, dpars1, &loglap0);
 878   copypars(x, fvals, gvals, ipars, dpars, x1, fvals1, gvals1, ipars1, dpars1);
 879   moms1front(gf, &n, &m, x1, fvals1 + n + 1, mean, stdev, sigmadata, &h, typx);
 880
 881   if (ipars->full) {
 882     for (i = 0; i < m; i++) {
 883       copypars(x, fvals, gvals, ipars, dpars,
 884                x1, fvals1, gvals1, ipars1, dpars1);
 885       ipars1->max.m = 1;
 886       ipars1->max.exptilt = FALSE;
 887       dpars1->max.newtilt = 1.0;
 888       maxfront(ff, gf + i, nil, x1, typx, fvals1, gvals1, nil, nil,
 889                ipars1, dpars1, nil, &msg);
 890       loglapfront(fvals1, nil, ipars1, dpars1, &loglap1);
 891       loglap1 -= loglap0;
 892
 893       copypars(x, fvals, gvals, ipars, dpars,
 894                x1, fvals1, gvals1, ipars1, dpars1);
 895       ipars1->max.m = 1;
 896       ipars1->max.exptilt = FALSE;
 897       dpars1->max.newtilt = 2.0;
 898       maxfront(ff, gf + i, nil, x1, typx, fvals1, gvals1, nil, nil,
 899                ipars1, dpars1, nil, &msg);
 900       loglapfront(fvals1, nil, ipars1, dpars1, &loglap2);
 901       loglap2 -= loglap0;
 902
 903       mean[i] = exp(loglap1);
 904       stdev[i] = sqrt(exp(loglap2) - exp(2.0 * loglap1));
 905       if (ipars->covar) sigmadata[i + i * m] = stdev[i] * stdev[i];
 906     }
 907     if (ipars->covar) {
 908       char *cgf[2];
 909       int j;
 910
 911       for (i = 0; i < m; i++) {
 912         for (j = i + 1; j < m; j++) {
 913           cgf[0] = gf[i];
 914           cgf[1] = gf[j];
 915           copypars(x, fvals, gvals, ipars, dpars,
 916                    x1, fvals1, gvals1, ipars1, dpars1);
 917           ipars1->max.m = 2;
 918           ipars1->max.exptilt = FALSE;
 919           dpars1->max.newtilt = 1.0;
 920           maxfront(ff, gf + i, nil, x1, typx, fvals1, gvals1, nil, nil,
 921                    ipars1, dpars1, nil, &msg);
 922           loglapfront(fvals1, nil, ipars1, dpars1, &loglap1);
 923           loglap1 -= loglap0;
 924
 925           sigmadata[i + j * m] = exp(loglap1) - mean[i] * mean[j];
 926           sigmadata[j + i * m] = sigmadata[i + j * m];
 927         }
 928       }
 929     }
 930   }
 931   else {
 932     for (i = 0; i < m; i++)
 933       tilts[i] = (stdev[i] > 0.0) ? dpars->mgfdel / stdev[i] : dpars->mgfdel;
 934
 935     for (i = 0; i < m; i++) {
 936       copypars(x, fvals, gvals, ipars, dpars,
 937                x1, fvals1, gvals1, ipars1, dpars1);
 938       ipars1->max.m = 1;
 939       ipars1->max.exptilt = TRUE;
 940       dpars1->max.newtilt = tilts[i];
 941       maxfront(ff, gf + i, nil, x1, typx, fvals1, gvals1, nil, nil,
 942                ipars1, dpars1, nil, &msg);
 943       loglapfront(fvals1, nil, ipars1, dpars1, &loglap1);
 944       loglap1 -= loglap0;
 945
 946       copypars(x, fvals, gvals, ipars, dpars,
 947                x1, fvals1, gvals1, ipars1, dpars1);
 948       ipars1->max.m = 1;
 949       ipars1->max.exptilt = TRUE;
 950       dpars1->max.newtilt = -tilts[i];
 951       maxfront(ff, gf + i, nil, x1, typx, fvals1, gvals1, nil, nil,
 952                ipars1, dpars1, nil, &msg);
 953       loglapfront(fvals1, nil, ipars1, dpars1, &loglap2);
 954       loglap2 -= loglap0;
 955
 956       mean[i] = (loglap1 - loglap2) / (2.0 * tilts[i]);
 957       stdev[i] = sqrt((loglap1 + loglap2) / (tilts[i] * tilts[i]));
 958       if (ipars->covar) sigmadata[i + i * m] = stdev[i] * stdev[i];
 959     }
 960     if (ipars->covar) {
 961       char *cgf[2];
 962       double ctilt, tscale[2];
 963       int j;
 964
 965       ctilt = dpars->mgfdel;
 966       for (i = 0; i < m; i++) {
 967         for (j = i + 1; j < m; j++) {
 968           cgf[0] = gf[i];
 969           cgf[1] = gf[j];
 970           tscale[0] = stdev[i] > 0 ? stdev[i] : 1.0;
 971           tscale[1] = stdev[j] > 0 ? stdev[j] : 1.0;
 972
 973           copypars(x, fvals, gvals, ipars, dpars,
 974                    x1, fvals1, gvals1, ipars1, dpars1);
 975           ipars1->max.m = 2;
 976           ipars1->max.exptilt = TRUE;
 977           dpars1->max.newtilt = ctilt;
 978           maxfront(ff, cgf, nil, x1, typx, fvals1, gvals1, nil, nil,
 979                    ipars1, dpars1, tscale, &msg);
 980           loglapfront(fvals1, nil, ipars1, dpars1, &loglap1);
 981           loglap1 -= loglap0;
 982
 983           copypars(x, fvals, gvals, ipars, dpars,
 984                    x1, fvals1, gvals1, ipars1, dpars1);
 985           ipars1->max.m = 2;
 986           ipars1->max.exptilt = TRUE;
 987           dpars1->max.newtilt = -ctilt;
 988           maxfront(ff, cgf, nil, x1, typx, fvals1, gvals1, nil, nil,
 989                    ipars1, dpars1, tscale, &msg);
 990           loglapfront(fvals1, nil, ipars1, dpars1, &loglap2);
 991           loglap2 -= loglap0;
 992
 993           sigmadata[i + j * m] = stdev[i] * stdev[j]
 994             * ((loglap2 + loglap1) /(2 * ctilt * ctilt) - 1.0);
 995           sigmadata[j + i * m] = sigmadata[i + j * m];
 996         }
 997       }
 998     }
 999   }
1000 }
1001
1002 static copypars(double *x, double *f, double *g, MomIPars *ip, MomIPars *dp,
1003                 double *x1, double *f1, double *g1, MomIPars *ip1, MomDPars *dp1)
1004 {
1005   int i, n, m, nf, ng;
1006
1007   n = ip->max.n;
1008   m = ip->max.m;
1009   nf = 1 + n + n * n;
1010   ng = m + n * m;
1011
1012   for (i = 0; i < n; i++) x1[i] = x[i];
1013   for (i = 0; i < nf; i++) f1[i] = f[i];
1014   for (i = 0; i < ng; i++) g1[i] = g[i];
1015   *ip1 = *ip;
1016   *dp1 = *dp;
1017 }
1018
1019 /************************************************************************/
1020 /**                                                                    **/
1021 /**                          Laplace Margins                           **/
1022 /**                                                                    **/
1023 /************************************************************************/
1024
1025 lapmar1front(char **ff, char **gf, double *x, double *typx, double *fvals,
1026              MaxIPars *ipars, MaxDPars *dpars, double *xmar, double *ymar,
1027              int *nmar)
1028 {
1029   char *msg;
1030   int i, n, m, mindex;
1031   double h, loglap0, loglap1, xmode, stdev, sigmadata, ctarget[1];
1032   double *fvals1, *x1, *cvals, *cvals1, *fvals2, *x2, *cvals2;
1033   MaxDPars dp1, dp2, *dpars1 = &dp1, *dpars2 = &dp2;
1034   MaxIPars ip1, ip2, *ipars1 = &ip1, *ipars2 = &ip2;
1035
1036   n = ipars->n;
1037   m = 1;
1038   h = dpars->h;
1039
1040   x1 = (double *) S_alloc(n + 1, sizeof(double));
1041   fvals1 = (double *) S_alloc(1 + n + n * n, sizeof(double));
1042   cvals = (double *) S_alloc(m + n * m, sizeof(double));
1043   cvals1 = (double *) S_alloc(m + n * m, sizeof(double));
1044   x2 = (double *) S_alloc(n + 1, sizeof(double));
1045   fvals2 = (double *) S_alloc(1 + n + n * n, sizeof(double));
1046   cvals2 = (double *) S_alloc(m + n * m, sizeof(double));
1047
1048   maxfront(ff, nil, nil, x, typx, fvals, nil, nil, nil,
1049            ipars, dpars, nil, &msg);
1050   cpmarpars(x, fvals, cvals, ipars, dpars, x1, fvals1, cvals1, ipars1, dpars1);
1051   loglapfront(fvals1, nil, ipars1, dpars1, &loglap0);
1052   cpmarpars(x, fvals, cvals, ipars, dpars, x1, fvals1, cvals1, ipars1, dpars1);
1053   moms1front(gf, &n, &m, x1, fvals1 + n + 1, &xmode, &stdev, &sigmadata,
1054              &h, typx);
1055
1056   ipars->k = 1;
1057   ipars->vals_suppl = FALSE;
1058   ctarget[0] = xmode;
1059   maxfront(ff, nil, gf, x, typx, fvals, nil, cvals, ctarget,
1060            ipars, dpars, nil, &msg);
1061
1062   for (mindex = 0; mindex < *nmar && xmar[mindex] <= xmode; mindex++);
1063
1064   cpmarpars(x, fvals, cvals, ipars, dpars, x1, fvals1, cvals1, ipars1, dpars1);
1065   for (i = mindex; i >= 0; i--) {
1066     ctarget[0] = xmar[i];
1067     maxfront(ff, nil, gf, x1, typx, fvals1, nil, cvals1, ctarget,
1068              ipars1, dpars1, nil, &msg);
1069     cpmarpars(x1, fvals1, cvals1, ipars1, dpars1, x2,
1070               fvals2, cvals2, ipars2, dpars2);
1071     loglapfront(fvals2, cvals2, ipars2, dpars2, &loglap1);
1072     ymar[i] = exp(loglap1 - loglap0);
1073   }
1074   cpmarpars(x, fvals, cvals, ipars, dpars, x1, fvals1, cvals1, ipars1, dpars1);
1075   for (i = mindex + 1; i < *nmar; i++) {
1076     ctarget[0] = xmar[i];
1077     maxfront(ff, nil, gf, x1, typx, fvals1, nil, cvals1, ctarget,
1078              ipars1, dpars1, nil, &msg);
1079     cpmarpars(x1, fvals1, cvals1, ipars1, dpars1, x2,
1080               fvals2, cvals2, ipars2, dpars2);
1081     loglapfront(fvals2, cvals2, ipars2, dpars2, &loglap1);
1082     ymar[i] = exp(loglap1 - loglap0);
1083   }
1084 }
1085
1086 static cpmarpars(double *x, double *f, double *g, MaxIPars *ip, MaxDPars *dp,
1087                  double *x1, double *f1, double *g1, MaxIPars *ip1, MaxDPars *dp1)
1088 {
1089   int i, n, k, nf, ng;
1090
1091   n = ip->n;
1092   k = ip->k;
1093   nf = 1 + n + n * n;
1094   ng = k + n * k;
1095
1096   for (i = 0; i < n; i++) x1[i] = x[i];
1097   for (i = 0; i < nf; i++) f1[i] = f[i];
1098   for (i = 0; i < ng; i++) g1[i] = g[i];
1099   *ip1 = *ip;
1100   *dp1 = *dp;
1101 }
1102 #endif /* SBAYES */
1103
1104 /*
1105   TODO
1106
1107   get hessian from gradiant for analytical gradiants
1108
1109   avoid repeated derivative calls in mimimize.
1110
1111   2d margins
1112
1113   use pos. definiteness info in margins
1114
1115 */