/*
 * Copyright 1996-2006 Catherine Loader.
 */

#include "mex.h"
/*
 * Copyright 1996-2006 Catherine Loader.
 */
#include <math.h>
#include "mut.h"

/* stirlerr(n) = log(n!) - log( sqrt(2*pi*n)*(n/e)^n ) */

#define S0 0.083333333333333333333       /* 1/12 */
#define S1 0.00277777777777777777778     /* 1/360 */
#define S2 0.00079365079365079365079365  /* 1/1260 */
#define S3 0.000595238095238095238095238 /* 1/1680 */
#define S4 0.0008417508417508417508417508 /* 1/1188 */

/*
  error for 0, 0.5, 1.0, 1.5, ..., 14.5, 15.0.
*/
static double sferr_halves[31] = {
0.0, /* n=0 - wrong, place holder only */
0.1534264097200273452913848,  /* 0.5 */
0.0810614667953272582196702,  /* 1.0 */
0.0548141210519176538961390,  /* 1.5 */
0.0413406959554092940938221,  /* 2.0 */
0.03316287351993628748511048, /* 2.5 */
0.02767792568499833914878929, /* 3.0 */
0.02374616365629749597132920, /* 3.5 */
0.02079067210376509311152277, /* 4.0 */
0.01848845053267318523077934, /* 4.5 */
0.01664469118982119216319487, /* 5.0 */
0.01513497322191737887351255, /* 5.5 */
0.01387612882307074799874573, /* 6.0 */
0.01281046524292022692424986, /* 6.5 */
0.01189670994589177009505572, /* 7.0 */
0.01110455975820691732662991, /* 7.5 */
0.010411265261972096497478567, /* 8.0 */
0.009799416126158803298389475, /* 8.5 */
0.009255462182712732917728637, /* 9.0 */
0.008768700134139385462952823, /* 9.5 */
0.008330563433362871256469318, /* 10.0 */
0.007934114564314020547248100, /* 10.5 */
0.007573675487951840794972024, /* 11.0 */
0.007244554301320383179543912, /* 11.5 */
0.006942840107209529865664152, /* 12.0 */
0.006665247032707682442354394, /* 12.5 */
0.006408994188004207068439631, /* 13.0 */
0.006171712263039457647532867, /* 13.5 */
0.005951370112758847735624416, /* 14.0 */
0.005746216513010115682023589, /* 14.5 */
0.005554733551962801371038690  /* 15.0 */
};

double stirlerr(n)
double n;
{ double nn;

  if (n<15.0)
  { nn = 2.0*n;
    if (nn==(int)nn) return(sferr_halves[(int)nn]);
    return(mut_lgamma(n+1.0) - (n+0.5)*log((double)n)+n - HF_LG_PIx2);
  }

  nn = (double)n;
  nn = nn*nn;
  if (n>500) return((S0-S1/nn)/n);
  if (n>80) return((S0-(S1-S2/nn)/nn)/n);
  if (n>35) return((S0-(S1-(S2-S3/nn)/nn)/nn)/n);
  return((S0-(S1-(S2-(S3-S4/nn)/nn)/nn)/nn)/n);
}

double bd0(x,np)
double x, np;
{ double ej, s, s1, v;
  int j;
  if (fabs(x-np)<0.1*(x+np))
  {
    s = (x-np)*(x-np)/(x+np);
    v = (x-np)/(x+np);
    ej = 2*x*v; v = v*v;
    for (j=1; ;++j)
    { ej *= v;
      s1 = s+ej/((j<<1)+1);
      if (s1==s) return(s1);
      s = s1;
    }
  }
  return(x*log(x/np)+np-x);
}

/*
  Raw binomial probability calculation.
  (1) This has both p and q arguments, when one may be represented
      more accurately than the other (in particular, in df()).
  (2) This should NOT check that inputs x and n are integers. This
      should be done in the calling function, where necessary.
  (3) Does not check for 0<=p<=1 and 0<=q<=1 or NaN's. Do this in
      the calling function.
*/
double dbinom_raw(x,n,p,q,give_log)
double x, n, p, q;
int give_log;
{ double f, lc;

  if (p==0.0) return((x==0) ? D_1 : D_0);
  if (q==0.0) return((x==n) ? D_1 : D_0);

  if (x==0)
  { lc = (p<0.1) ? -bd0(n,n*q) - n*p : n*log(q);
    return( DEXP(lc) );
  }

  if (x==n)
  { lc = (q<0.1) ? -bd0(n,n*p) - n*q : n*log(p);
    return( DEXP(lc) );
  }

  if ((x<0) | (x>n)) return( D_0 );

  lc = stirlerr(n) - stirlerr(x) - stirlerr(n-x)
         - bd0(x,n*p) - bd0(n-x,n*q);
  f = (PIx2*x*(n-x))/n;

  return( FEXP(f,lc) );
}

double dbinom(x,n,p,give_log)
int x, n;
double p;
int give_log;
{ 
  if ((p<0) | (p>1) | (n<0)) return(INVALID_PARAMS);
  if (x<0) return( D_0 );
  
  return( dbinom_raw((double)x,(double)n,p,1-p,give_log) );
}

/*
  Poisson probability  lb^x exp(-lb) / x!.
  I don't check that x is an integer, since other functions
  that call dpois_raw() (i.e. dgamma) may use a fractional
  x argument.
*/
double dpois_raw(x,lambda,give_log)
int give_log;
double x, lambda;
{
  if (lambda==0) return( (x==0) ? D_1 : D_0 );
  if (x==0) return( DEXP(-lambda) );
  if (x<0) return( D_0 );

  return(FEXP( PIx2*x, -stirlerr(x)-bd0(x,lambda) ));
}

double dpois(x,lambda,give_log)
int x, give_log;
double lambda;
{
  if (lambda<0) return(INVALID_PARAMS);
  if (x<0) return( D_0 );

  return( dpois_raw((double)x,lambda,give_log) );
}

double dbeta(x,a,b,give_log)
double x, a, b;
int give_log;
{ double f, p;

  if ((a<=0) | (b<=0)) return(INVALID_PARAMS);
  if ((x<=0) | (x>=1)) return(D_0);

  if (a<1)
  { if (b<1)                                    /* a<1, b<1 */
    { f = a*b/((a+b)*x*(1-x));
      p = dbinom_raw(a,a+b,x,1-x,give_log);
    }
    else                                        /* a<1, b>=1 */
    { f = a/x;
      p = dbinom_raw(a,a+b-1,x,1-x,give_log);
    }
  }
  else
  { if (b<1)                                    /* a>=1, b<1 */
    { f = b/(1-x);
      p = dbinom_raw(a-1,a+b-1,x,1-x,give_log);
    }
    else                                        /* a>=1, b>=1 */
    { f = a+b-1;
      p = dbinom_raw(a-1,(a-1)+(b-1),x,1-x,give_log);
    }
  }

  return( (give_log) ? p + log(f) : p*f );
}

/*
 *   To evaluate the F density, write it as a Binomial probability
 *   with p = x*m/(n+x*m). For m>=2, use the simplest conversion.
 *   For m<2, (m-2)/2<0 so the conversion will not work, and we must use
 *   a second conversion. Note the division by p; this seems unavoidable
 *   for m < 2, since the F density has a singularity as x (or p) -> 0.
 */
double df(x,m,n,give_log)
double x, m, n;
int give_log;
{ double p, q, f, dens;

  if ((m<=0) | (n<=0)) return(INVALID_PARAMS);
  if (x <= 0.0) return(D_0);

  f = 1.0/(n+x*m);
  q = n*f;
  p = x*m*f;

  if (m>=2)
  { f = m*q/2;
    dens = dbinom_raw((m-2)/2.0, (m+n-2)/2.0, p, q, give_log);
  }
  else
  { f = m*m*q / (2*p*(m+n));
    dens = dbinom_raw(m/2.0, (m+n)/2.0, p, q, give_log);
  }

  return((give_log) ? log(f)+dens : f*dens);
}

/*
 * Gamma density,
 *                  lb^r x^{r-1} exp(-lb*x)
 *      p(x;r,lb) = -----------------------
 *                          (r-1)!
 *
 * If USE_SCALE is defined below, the lb argument will be interpreted
 * as a scale parameter (i.e. replace lb by 1/lb above). Otherwise,
 * it is interpreted as a rate parameter, as above.
 */

/* #define USE_SCALE */

double dgamma(x,r,lambda,give_log)
int give_log;
double x, r, lambda;
{ double pr;

  if ((r<=0) | (lambda<0)) return(INVALID_PARAMS);
  if (x<=0.0) return( D_0 );

#ifdef USE_SCALE
  lambda = 1.0/lambda;
#endif

  if (r<1)
  { pr = dpois_raw(r,lambda*x,give_log);
    return( (give_log) ?  pr + log(r/x) : pr*r/x );
  }

  pr = dpois_raw(r-1.0,lambda*x,give_log);
  return( (give_log) ? pr + log(lambda) : lambda*pr);
}

double dchisq(x, df, give_log)
double x, df;
int give_log;
{
 return(dgamma(x, df/2.0,
  0.5
  ,give_log));
/*
#ifdef USE_SCALE
  2.0
#else
  0.5
#endif
  ,give_log));
*/
}

/*
 * Given a sequence of r successes and b failures, we sample n (\le b+r)
 * items without replacement. The hypergeometric probability is the
 * probability of x successes:
 *
 *                dbinom(x,r,p) * dbinom(n-x,b,p)
 *   p(x;r,b,n) = ---------------------------------
 *                          dbinom(n,r+b,p)
 *
 * for any p. For numerical stability, we take p=n/(r+b); with this choice,
 * the denominator is not exponentially small.
 */
double dhyper(x,r,b,n,give_log)
int x, r, b, n, give_log;
{ double p, q, p1, p2, p3;

  if ((r<0) | (b<0) | (n<0) | (n>r+b))
    return( INVALID_PARAMS );

  if (x<0) return(D_0);

  if (n==0) return((x==0) ? D_1 : D_0);

  p = ((double)n)/((double)(r+b));
  q = ((double)(r+b-n))/((double)(r+b));

  p1 = dbinom_raw((double)x,(double)r,p,q,give_log);
  p2 = dbinom_raw((double)(n-x),(double)b,p,q,give_log);
  p3 = dbinom_raw((double)n,(double)(r+b),p,q,give_log);

  return( (give_log) ? p1 + p2 - p3 : p1*p2/p3 );
}

/*
  probability of x failures before the nth success.
*/
double dnbinom(x,n,p,give_log)
double n, p;
int x, give_log;
{ double prob, f;

  if ((p<0) | (p>1) | (n<=0)) return(INVALID_PARAMS);

  if (x<0) return( D_0 );

  prob = dbinom_raw(n,x+n,p,1-p,give_log);
  f = n/(n+x);

  return((give_log) ? log(f) + prob : f*prob);
}

double dt(x, df, give_log)
double x, df;
int give_log;
{ double t, u, f;

  if (df<=0.0) return(INVALID_PARAMS);

  /*
     exp(t) = Gamma((df+1)/2) /{ sqrt(df/2) * Gamma(df/2) }
            = sqrt(df/2) / ((df+1)/2) * Gamma((df+3)/2) / Gamma((df+2)/2).
     This form leads to a computation that should be stable for all
     values of df, including df -> 0 and df -> infinity.
  */
  t = -bd0(df/2.0,(df+1)/2.0) + stirlerr((df+1)/2.0) - stirlerr(df/2.0);

  if (x*x>df)
    u = log( 1+ x*x/df ) * df/2;
  else
    u = -bd0(df/2.0,(df+x*x)/2.0) + x*x/2.0;

  f = PIx2*(1+x*x/df);

  return( FEXP(f,t-u) );
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
 * Provides mut_erf() and mut_erfc() functions. Also mut_pnorm().
 * Had too many problems with erf()'s built into math libraries
 * (when they existed). Hence the need to write my own...
 *
 * Algorithm from Walter Kr\"{a}mer, Frithjof Blomquist.
 * "Algorithms with Guaranteed Error Bounds for the Error Function
 * and Complementary Error Function"
 * Preprint 2000/2, Bergische Universt\"{a}t GH Wuppertal
 * http://www.math.uni-wuppertal.de/wrswt/preprints/prep_00_2.pdf
 *
 * Coded by Catherine Loader, September 2006.
 */

#include "mut.h"

double erf1(double x)  /* erf; 0 < x < 0.65) */
{ double p[5] = {1.12837916709551256e0,  /* 2/sqrt(pi) */
                 1.35894887627277916e-1,
                 4.03259488531795274e-2,
                 1.20339380863079457e-3,
                 6.49254556481904354e-5};
  double q[5] = {1.00000000000000000e0,
                 4.53767041780002545e-1,
                 8.69936222615385890e-2,
                 8.49717371168693357e-3,
                 3.64915280629351082e-4};
  double x2, p4, q4;
  x2 = x*x;
  p4 = p[0] + p[1]*x2 + p[2]*x2*x2 + p[3]*x2*x2*x2 + p[4]*x2*x2*x2*x2;
  q4 = q[0] + q[1]*x2 + q[2]*x2*x2 + q[3]*x2*x2*x2 + q[4]*x2*x2*x2*x2;
  return(x*p4/q4);
}

double erf2(double x) /* erfc; 0.65 <= x < 2.2 */
{ double p[6] = {9.99999992049799098e-1,
                 1.33154163936765307e0,
                 8.78115804155881782e-1,
                 3.31899559578213215e-1,
                 7.14193832506776067e-2,
                 7.06940843763253131e-3};
  double q[7] = {1.00000000000000000e0,
                 2.45992070144245533e0,
                 2.65383972869775752e0,
                 1.61876655543871376e0,
                 5.94651311286481502e-1,
                 1.26579413030177940e-1,
                 1.25304936549413393e-2};
  double x2, p5, q6;
  x2 = x*x;
  p5 = p[0] + p[1]*x + p[2]*x2 + p[3]*x2*x + p[4]*x2*x2 + p[5]*x2*x2*x;
  q6 = q[0] + q[1]*x + q[2]*x2 + q[3]*x2*x + q[4]*x2*x2 + q[5]*x2*x2*x + q[6]*x2*x2*x2;
  return( exp(-x2)*p5/q6 );
}

double erf3(double x) /* erfc; 2.2 < x <= 6 */
{ double p[6] = {9.99921140009714409e-1,
                 1.62356584489366647e0,
                 1.26739901455873222e0,
                 5.81528574177741135e-1,
                 1.57289620742838702e-1,
                 2.25716982919217555e-2};
  double q[7] = {1.00000000000000000e0,
                 2.75143870676376208e0,
                 3.37367334657284535e0,
                 2.38574194785344389e0,
                 1.05074004614827206e0,
                 2.78788439273628983e-1,
                 4.00072964526861362e-2};
  double x2, p5, q6;
  x2 = x*x;
  p5 = p[0] + p[1]*x + p[2]*x2 + p[3]*x2*x + p[4]*x2*x2 + p[5]*x2*x2*x;
  q6 = q[0] + q[1]*x + q[2]*x2 + q[3]*x2*x + q[4]*x2*x2 + q[5]*x2*x2*x + q[6]*x2*x2*x2;
  return( exp(-x2)*p5/q6 );
}

double erf4(double x)  /* erfc; x > 6.0 */
{ double p[5] = {5.64189583547756078e-1,
                 8.80253746105525775e0,
                 3.84683103716117320e1,
                 4.77209965874436377e1,
                 8.08040729052301677e0};
  double q[5] = {1.00000000000000000e0,
                 1.61020914205869003e1,
                 7.54843505665954743e1,
                 1.12123870801026015e2,
                 3.73997570145040850e1};
  double x2, p4, q4;
  if (x>26.5432) return(0.0);
  x2 = 1.0/(x*x);
  p4 = p[0] + p[1]*x2 + p[2]*x2*x2 + p[3]*x2*x2*x2 + p[4]*x2*x2*x2*x2;
  q4 = q[0] + q[1]*x2 + q[2]*x2*x2 + q[3]*x2*x2*x2 + q[4]*x2*x2*x2*x2;
  return(exp(-x*x)*p4/(x*q4));
}

double mut_erfc(double x)
{ if (x<0.0) return(2.0-mut_erfc(-x));
  if (x==0.0) return(1.0);
  if (x<0.65) return(1.0-erf1(x));
  if (x<2.2) return(erf2(x));
  if (x<6.0) return(erf3(x));
  return(erf4(x));
}

double mut_erf(double x)
{ 
  if (x<0.0) return(-mut_erf(-x));
  if (x==0.0) return(0.0);
  if (x<0.65) return(erf1(x));
  if (x<2.2) return(1.0-erf2(x));
  if (x<6.0) return(1.0-erf3(x));
  return(1.0-erf4(x));
}

double mut_pnorm(double x)
{ if (x<0.0) return(mut_erfc(-x/SQRT2)/2);
  return((1.0+mut_erf(x/SQRT2))/2);
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
#include "mut.h"

static double lookup_gamma[21] = {
  0.0, /* place filler */
  0.572364942924699971,  /* log(G(0.5)) = log(sqrt(pi)) */
  0.000000000000000000,  /* log(G(1)) = log(0!) */
 -0.120782237635245301,  /* log(G(3/2)) = log(sqrt(pi)/2)) */
  0.000000000000000000,  /* log(G(2)) = log(1!) */
  0.284682870472919181,  /* log(G(5/2)) = log(3sqrt(pi)/4) */
  0.693147180559945286,  /* log(G(3)) = log(2!) */
  1.200973602347074287,  /* etc */
  1.791759469228054957,
  2.453736570842442344,
  3.178053830347945752,
  3.957813967618716511,
  4.787491742782045812,
  5.662562059857141783,
  6.579251212010101213,
  7.534364236758732680,
  8.525161361065414667,
  9.549267257300996903,
 10.604602902745250859,
 11.689333420797268559,
 12.801827480081469091 };

/*
 * coefs are B(2n)/(2n(2n-1))  2n(2n-1) =
 *  2n  B(2n) 2n(2n-1) coef
 *   2  1/6     2      1/12
 *   4 -1/30   12     -1/360
 *   6  1/42   30      1/1260
 *   8 -1/30   56     -1/1680
 *  10  5/66   90      1/1188
 *  12 -691/2730 132   691/360360
 */

double mut_lgamma(double x)
{ double f, z, x2, se;
  int ix;

/* lookup table for common values.
 */
  ix = (int)(2*x);
  if (((ix>=1) & (ix<=20)) && (ix==2*x)) return(lookup_gamma[ix]);

  f = 1.0;
  while (x <= 15)
  { f *= x;
    x += 1.0;
  }

  x2 = 1.0/(x*x);
  z = (x-0.5)*log(x) - x + HF_LG_PIx2;
  se = (13860 - x2*(462 - x2*(132 - x2*(99 - 140*x2))))/(166320*x);

  return(z + se - log(f));
}

double mut_lgammai(int i)  /* log(Gamma(i/2)) for integer i */
{ if (i>20) return(mut_lgamma(i/2.0));
  return(lookup_gamma[i]);
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
 * A is a n*p matrix, find the cholesky decomposition
 * of the first p rows. In most applications, will want n=p.
 *
 * chol_dec(A,n,p)       computes the decomoposition R'R=A.
 *                       (note that R is stored in the input A).
 * chol_solve(A,v,n,p)   computes (R'R)^{-1}v
 * chol_hsolve(A,v,n,p)  computes (R')^{-1}v
 * chol_isolve(A,v,n,p)  computes (R)^{-1}v
 * chol_qf(A,v,n,p)      computes ||(R')^{-1}v||^2.
 * chol_mult(A,v,n,p)    computes (R'R)v
 *
 * The solve functions assume that A is already decomposed.
 * chol_solve(A,v,n,p) is equivalent to applying chol_hsolve()
 * and chol_isolve() in sequence.
 */

#include <math.h>
#include "mut.h"

void chol_dec(A,n,p)
double *A;
int n, p;
{ int i, j, k;

  for (j=0; j<p; j++)
  { k = n*j+j;
    for (i=0; i<j; i++) A[k] -= A[n*j+i]*A[n*j+i];
    if (A[k]<=0)
    { for (i=j; i<p; i++) A[n*i+j] = 0.0; }
    else
    { A[k] = sqrt(A[k]);
      for (i=j+1; i<p; i++)
      { for (k=0; k<j; k++)
          A[n*i+j] -= A[n*i+k]*A[n*j+k];
        A[n*i+j] /= A[n*j+j];
      }
    }
  }
  for (j=0; j<p; j++)
    for (i=j+1; i<p; i++) A[n*j+i] = 0.0;
}

int chol_solve(A,v,n,p)
double *A, *v;
int n, p;
{ int i, j;

  for (i=0; i<p; i++)
  { for (j=0; j<i; j++) v[i] -= A[i*n+j]*v[j];
    v[i] /= A[i*n+i];
  }
  for (i=p-1; i>=0; i--)
  { for (j=i+1; j<p; j++) v[i] -= A[j*n+i]*v[j];
    v[i] /= A[i*n+i];
  }
  return(p);
}

int chol_hsolve(A,v,n,p)
double *A, *v;
int n, p;
{ int i, j;

  for (i=0; i<p; i++)
  { for (j=0; j<i; j++) v[i] -= A[i*n+j]*v[j];
    v[i] /= A[i*n+i];
  }
  return(p);
}

int chol_isolve(A,v,n,p)
double *A, *v;
int n, p;
{ int i, j;

  for (i=p-1; i>=0; i--)
  { for (j=i+1; j<p; j++) v[i] -= A[j*n+i]*v[j];
    v[i] /= A[i*n+i];
  }
  return(p);
}

double chol_qf(A,v,n,p)
double *A, *v;
int n, p;
{ int i, j;
  double sum;
 
  sum = 0.0;
  for (i=0; i<p; i++)
  { for (j=0; j<i; j++) v[i] -= A[i*n+j]*v[j];
    v[i] /= A[i*n+i];
    sum += v[i]*v[i];
  }
  return(sum);
}

int chol_mult(A,v,n,p)
double *A, *v;
int n, p;
{ int i, j;
  double sum;
  for (i=0; i<p; i++)
  { sum = 0;
    for (j=i; j<p; j++) sum += A[j*n+i]*v[j];
    v[i] = sum;
  }
  for (i=p-1; i>=0; i--)
  { sum = 0;
    for (j=0; j<=i; j++) sum += A[i*n+j]*v[j];
    v[i] = sum;
  }
  return(1);
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
#include <stdio.h>
#include <math.h>
#include "mut.h"
#define E_MAXIT 20
#define E_TOL 1.0e-10
#define SQR(x) ((x)*(x))

double e_tol(D,p)
double *D;
int p;
{ double mx;
  int i;
  if (E_TOL <= 0.0) return(0.0);
  mx = D[0];
  for (i=1; i<p; i++) if (D[i*(p+1)]>mx) mx = D[i*(p+1)];
  return(E_TOL*mx);
}

void eig_dec(X,P,d)
double *X, *P;
int d;
{ int i, j, k, iter, ms;
  double c, s, r, u, v;

  for (i=0; i<d; i++)
    for (j=0; j<d; j++) P[i*d+j] = (i==j);

  for (iter=0; iter<E_MAXIT; iter++)
  { ms = 0;
    for (i=0; i<d; i++)
      for (j=i+1; j<d; j++)
        if (SQR(X[i*d+j]) > 1.0e-15*fabs(X[i*d+i]*X[j*d+j]))
        { c = (X[j*d+j]-X[i*d+i])/2;
          s = -X[i*d+j];
          r = sqrt(c*c+s*s);
          c /= r;
          s = sqrt((1-c)/2)*(2*(s>0)-1);
          c = sqrt((1+c)/2);
          for (k=0; k<d; k++)
          { u = X[i*d+k]; v = X[j*d+k];
            X[i*d+k] = u*c+v*s;
            X[j*d+k] = v*c-u*s;
          }
          for (k=0; k<d; k++)
          { u = X[k*d+i]; v = X[k*d+j];
            X[k*d+i] = u*c+v*s;
            X[k*d+j] = v*c-u*s;
          }
          X[i*d+j] = X[j*d+i] = 0.0;
          for (k=0; k<d; k++)
          { u = P[k*d+i]; v = P[k*d+j];
            P[k*d+i] = u*c+v*s;
            P[k*d+j] = v*c-u*s;
          }
          ms = 1;
        }
    if (ms==0) return;
  }
  mut_printf("eig_dec not converged\n");
}

int eig_solve(J,x)
jacobian *J;
double *x;
{ int d, i, j, rank;
  double *D, *P, *Q, *w;
  double tol;

  D = J->Z;
  P = Q = J->Q;
  d = J->p;
  w = J->wk;

  tol = e_tol(D,d);

  rank = 0;
  for (i=0; i<d; i++)
  { w[i] = 0.0;
    for (j=0; j<d; j++) w[i] += P[j*d+i]*x[j];
  }
  for (i=0; i<d; i++)
    if (D[i*d+i]>tol)
    { w[i] /= D[i*(d+1)];
      rank++;
    }
  for (i=0; i<d; i++)
  { x[i] = 0.0;
    for (j=0; j<d; j++) x[i] += Q[i*d+j]*w[j];
  }
  return(rank);
}

int eig_hsolve(J,v)
jacobian *J;
double *v;
{ int i, j, p, rank;
  double *D, *Q, *w;
  double tol;

  D = J->Z;
  Q = J->Q;
  p = J->p;
  w = J->wk;

  tol = e_tol(D,p);
  rank = 0;

  for (i=0; i<p; i++)
  { w[i] = 0.0;
    for (j=0; j<p; j++) w[i] += Q[j*p+i]*v[j];
  }
  for (i=0; i<p; i++)
  { if (D[i*p+i]>tol)
    { v[i] = w[i]/sqrt(D[i*(p+1)]);
      rank++;
    }
    else v[i] = 0.0;
  }
  return(rank);
}

int eig_isolve(J,v)
jacobian *J;
double *v;
{ int i, j, p, rank;
  double *D, *Q, *w;
  double tol;

  D = J->Z;
  Q = J->Q;
  p = J->p;
  w = J->wk;

  tol = e_tol(D,p);
  rank = 0;

  for (i=0; i<p; i++)
  { if (D[i*p+i]>tol)
    { v[i] = w[i]/sqrt(D[i*(p+1)]);
      rank++;
    }
    else v[i] = 0.0;
  }

  for (i=0; i<p; i++)
  { w[i] = 0.0;
    for (j=0; j<p; j++) w[i] += Q[i*p+j]*v[j];
  }

  return(rank);
}

double eig_qf(J,v)
jacobian *J;
double *v;
{ int i, j, p;
  double sum, tol;

  p = J->p;
  sum = 0.0;
  tol = e_tol(J->Z,p);

  for (i=0; i<p; i++)
    if (J->Z[i*p+i]>tol)
    { J->wk[i] = 0.0;
      for (j=0; j<p; j++) J->wk[i] += J->Q[j*p+i]*v[j];
      sum += J->wk[i]*J->wk[i]/J->Z[i*p+i];
    }
  return(sum);
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
 *  Integrate a function f over a circle or disc.
 */

#include "mut.h"

void setM(M,r,s,c,b)
double *M, r, s, c;
int b;
{ M[0] =-r*s; M[1] = r*c;
  M[2] = b*c; M[3] = b*s;
  M[4] =-r*c; M[5] = -s;
  M[6] = -s;  M[7] = 0.0;
  M[8] =-r*s; M[9] = c;
  M[10]=   c; M[11]= 0.0;
}

void integ_circ(f,r,orig,res,mint,b)
int (*f)(), mint, b;
double r, *orig, *res;
{ double y, x[2], theta, tres[MXRESULT], M[12], c, s;
  int i, j, nr;
  
  y = 0;
  for (i=0; i<mint; i++)
  { theta = 2*PI*(double)i/(double)mint;
    c = cos(theta); s = sin(theta);
    x[0] = orig[0]+r*c;
    x[1] = orig[1]+r*s;

    if (b!=0)
    { M[0] =-r*s; M[1] = r*c;
      M[2] = b*c; M[3] = b*s;
      M[4] =-r*c; M[5] = -s;
      M[6] = -s;  M[7] = 0.0;
      M[8] =-r*s; M[9] = c;
      M[10]=   c; M[11]= 0.0;
    }

    nr = f(x,2,tres,M);
    if (i==0) setzero(res,nr);
    for (j=0; j<nr; j++) res[j] += tres[j];
  }
  y = 2 * PI * ((b==0)?r:1.0) / mint;
  for (j=0; j<nr; j++) res[j] *= y;
}

void integ_disc(f,fb,fl,res,resb,mg)
int (*f)(), (*fb)(), *mg;
double *fl, *res, *resb;
{ double x[2], y, r, tres[MXRESULT], *orig, rmin, rmax, theta, c, s, M[12];
  int ct, ctb, i, j, k, nr, nrb, w;

  orig = &fl[2];
  rmax = fl[1];
  rmin = fl[0];
  y = 0.0;
  ct = ctb = 0;

  for (j=0; j<mg[1]; j++)
  { theta = 2*PI*(double)j/(double)mg[1];
    c = cos(theta); s = sin(theta);
    for (i= (rmin>0) ? 0 : 1; i<=mg[0]; i++)
    { r = rmin + (rmax-rmin)*i/mg[0];
      w = (2+2*(i&1)-(i==0)-(i==mg[0]));
      x[0] = orig[0] + r*c;
      x[1] = orig[1] + r*s;
      nr = f(x,2,tres,NULL);
      if (ct==0) setzero(res,nr);
      for (k=0; k<nr; k++) res[k] += w*r*tres[k];
      ct++;
      if (((i==0) | (i==mg[0])) && (fb!=NULL))
      { setM(M,r,s,c,1-2*(i==0));
        nrb = fb(x,2,tres,M);
        if (ctb==0) setzero(resb,nrb);
        ctb++;
        for (k=0; k<nrb; k++) resb[k] += tres[k];
      }
    }
  }


/*  for (i= (rmin>0) ? 0 : 1; i<=mg[0]; i++)
  {
    r = rmin + (rmax-rmin)*i/mg[0];
    w = (2+2*(i&1)-(i==0)-(i==mg[0]));

    for (j=0; j<mg[1]; j++)
    { theta = 2*PI*(double)j/(double)mg[1];
      c = cos(theta); s = sin(theta);
      x[0] = orig[0] + r*c;
      x[1] = orig[1] + r*s;
      nr = f(x,2,tres,NULL);
      if (ct==0) setzero(res,nr);
      ct++;
      for (k=0; k<nr; k++) res[k] += w*r*tres[k];

      if (((i==0) | (i==mg[0])) && (fb!=NULL))
      { setM(M,r,s,c,1-2*(i==0));
        nrb = fb(x,2,tres,M);
        if (ctb==0) setzero(resb,nrb);
        ctb++;
        for (k=0; k<nrb; k++) resb[k] += tres[k];
      }
    }
  } */
  for (j=0; j<nr; j++) res[j] *= 2*PI*(rmax-rmin)/(3*mg[0]*mg[1]);
  for (j=0; j<nrb; j++) resb[j] *= 2*PI/mg[1];
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
 *  Multivariate integration of a vector-valued function
 *  using Monte-Carlo method.
 *
 *  uses drand48() random number generator. Does not seed.
 */

#include <stdlib.h>
#include "mut.h"
extern void setzero();

static double M[(1+MXIDIM)*MXIDIM*MXIDIM];

void monte(f,ll,ur,d,res,n)
int (*f)(), d, n;
double *ll, *ur, *res;
{
  int i, j, nr;
#ifdef WINDOWS
  mut_printf("Sorry, Monte-Carlo Integration not enabled.\n");
  for (i=0; i<n; i++) res[i] = 0.0;
#else
  double z, x[MXIDIM], tres[MXRESULT];

srand48(234L);

  for (i=0; i<n; i++)
  { for (j=0; j<d; j++) x[j] = ll[j] + (ur[j]-ll[j])*drand48();
    nr = f(x,d,tres,NULL);
    if (i==0) setzero(res,nr);
    for (j=0; j<nr; j++) res[j] += tres[j];
  }

  z = 1;
  for (i=0; i<d; i++) z *= (ur[i]-ll[i]);
  for (i=0; i<nr; i++) res[i] *= z/n;
#endif
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
 *  Multivariate integration of a vector-valued function
 *  using Simpson's rule.
 */

#include <math.h>
#include "mut.h"
extern void setzero();

static double M[(1+MXIDIM)*MXIDIM*MXIDIM];

/* third order corners */
void simp3(fd,x,d,resd,delta,wt,i0,i1,mg,ct,res2,index)
int (*fd)(), d, wt, i0, i1, *mg, ct, *index;
double *x, *resd, *delta, *res2;
{ int k, l, m, nrd;
  double zb;

  for (k=i1+1; k<d; k++) if ((index[k]==0) | (index[k]==mg[k]))
  {
    setzero(M,d*d);
    m = 0; zb = 1.0;
    for (l=0; l<d; l++)
      if ((l!=i0) & (l!=i1) & (l!=k))
      { M[m*d+l] = 1.0;
        m++;
        zb *= delta[l];
      }
    M[(d-3)*d+i0] = (index[i0]==0) ? -1 : 1;
    M[(d-2)*d+i1] = (index[i1]==0) ? -1 : 1;
    M[(d-1)*d+k] = (index[k]==0) ? -1 : 1;
    nrd = fd(x,d,res2,M);
    if ((ct==0) & (i0==0) & (i1==1) & (k==2)) setzero(resd,nrd);
    for (l=0; l<nrd; l++)
      resd[l] += wt*zb*res2[l];
  }
}

/* second order corners */
void simp2(fc,fd,x,d,resc,resd,delta,wt,i0,mg,ct,res2,index)
int (*fc)(), (*fd)(), d, wt, i0, *mg, ct, *index;
double *x, *resc, *resd, *delta, *res2;
{ int j, k, l, nrc;
  double zb;
  for (j=i0+1; j<d; j++) if ((index[j]==0) | (index[j]==mg[j]))
  { setzero(M,d*d);
    l = 0; zb = 1;
    for (k=0; k<d; k++) if ((k!=i0) & (k!=j))
    { M[l*d+k] = 1.0;
      l++;
      zb *= delta[k];
    }
    M[(d-2)*d+i0] = (index[i0]==0) ? -1 : 1;
    M[(d-1)*d+j] = (index[j]==0) ? -1 : 1;
    nrc = fc(x,d,res2,M);
    if ((ct==0) & (i0==0) & (j==1)) setzero(resc,nrc);
    for (k=0; k<nrc; k++) resc[k] += wt*zb*res2[k];
       
    if (fd!=NULL)
      simp3(fd,x,d,resd,delta,wt,i0,j,mg,ct,res2,index);
  }
}

/* first order boundary */
void simp1(fb,fc,fd,x,d,resb,resc,resd,delta,wt,mg,ct,res2,index)
int (*fb)(), (*fc)(), (*fd)(), d, wt, *mg, ct, *index;
double *x, *resb, *resc, *resd, *delta, *res2;
{ int i, j, k, nrb;
  double zb;
  for (i=0; i<d; i++) if ((index[i]==0) | (index[i]==mg[i]))
  { setzero(M,(1+d)*d*d);
    k = 0;
    for (j=0; j<d; j++) if (j!=i)
    { M[k*d+j] = 1;
      k++;
    }
    M[(d-1)*d+i] = (index[i]==0) ? -1 : 1;
    nrb = fb(x,d,res2,M);
    zb = 1;
    for (j=0; j<d; j++) if (i!=j) zb *= delta[j];
        if ((ct==0) && (i==0))
          for (j=0; j<nrb; j++) resb[j] = 0.0;
    for (j=0; j<nrb; j++) resb[j] += wt*zb*res2[j];

    if (fc!=NULL)
      simp2(fc,fd,x,d,resc,resd,delta,wt,i,mg,ct,res2,index);
  }
}

void simpson4(f,fb,fc,fd,ll,ur,d,res,resb,resc,resd,mg,res2)
int (*f)(), (*fb)(), (*fc)(), (*fd)(), d, *mg;
double *ll, *ur, *res, *resb, *resc, *resd, *res2;
{ int ct, i, j, nr, wt, index[MXIDIM];
  double x[MXIDIM], delta[MXIDIM], z;

  for (i=0; i<d; i++)
  { index[i] = 0;
    x[i] = ll[i];
    if (mg[i]&1) mg[i]++;
    delta[i] = (ur[i]-ll[i])/(3*mg[i]);
  }
  ct = 0;

  while(1)
  { wt = 1;
    for (i=0; i<d; i++)
      wt *= (4-2*(index[i]%2==0)-(index[i]==0)-(index[i]==mg[i]));
    nr = f(x,d,res2,NULL);
    if (ct==0) setzero(res,nr);
    for (i=0; i<nr; i++) res[i] += wt*res2[i];

    if (fb!=NULL)
      simp1(fb,fc,fd,x,d,resb,resc,resd,delta,wt,mg,ct,res2,index);

    /* compute next grid point */
    for (i=0; i<d; i++)
    { index[i]++;
      if (index[i]>mg[i])
      { index[i] = 0;
        x[i] = ll[i];
        if (i==d-1) /* done */
        { z = 1.0;
          for (j=0; j<d; j++) z *= delta[j];
          for (j=0; j<nr; j++) res[j] *= z;
          return;
        }
      }
      else
      { x[i] = ll[i] + 3*delta[i]*index[i];
        i = d;
      }
    }
    ct++;
  }
}

void simpsonm(f,ll,ur,d,res,mg,res2)
int (*f)(), d, *mg;
double *ll, *ur, *res, *res2;
{ simpson4(f,NULL,NULL,NULL,ll,ur,d,res,NULL,NULL,NULL,mg,res2);
}

double simpson(f,l0,l1,m)
double (*f)(), l0, l1;
int m;
{ double x, sum;
  int i;
  sum = 0;
  for (i=0; i<=m; i++)
  { x = ((m-i)*l0 + i*l1)/m;
    sum += (2+2*(i&1)-(i==0)-(i==m)) * f(x);
  }
  return( (l1-l0) * sum / (3*m) );
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
#include "mut.h"

static double *res, *resb, *orig, rmin, rmax;
static int ct0;

void sphM(M,r,u)
double *M, r, *u;
{ double h, u1[3], u2[3];

  /* set the orthogonal unit vectors. */
  h = sqrt(u[0]*u[0]+u[1]*u[1]);
  if (h<=0)
  { u1[0] = u2[1] = 1.0;
    u1[1] = u1[2] = u2[0] = u2[2] = 0.0;
  }
  else
  { u1[0] = u[1]/h; u1[1] = -u[0]/h; u1[2] = 0.0;
    u2[0] = u[2]*u[0]/h; u2[1] = u[2]*u[1]/h; u2[2] = -h;
  }

  /* parameterize the sphere as r(cos(t)cos(v)u + sin(t)u1 + cos(t)sin(v)u2).
   * first layer of M is (dx/dt, dx/dv, dx/dr) at t=v=0.
   */
  M[0] = r*u1[0]; M[1] = r*u1[1]; M[2] = r*u1[2];
  M[3] = r*u2[0]; M[4] = r*u2[1]; M[5] = r*u2[2];
  M[6] = u[0]; M[7] = u[1]; M[8] = u[2];

  /* next layers are second derivative matrix of components of x(r,t,v).
   * d^2x/dt^2 = d^2x/dv^2 = -ru;    d^2x/dtdv = 0;
   * d^2x/drdt = u1; d^2x/drdv = u2; d^2x/dr^2 = 0.
   */

  M[9] = M[13] = -r*u[0];
  M[11]= M[15] = u1[0];
  M[14]= M[16] = u2[0];
  M[10]= M[12] = M[17] = 0.0;

  M[18]= M[22] = -r*u[1];
  M[20]= M[24] = u1[1];
  M[23]= M[25] = u2[1];
  M[19]= M[21] = M[26] = 0.0;

  M[27]= M[31] = -r*u[1];
  M[29]= M[33] = u1[1];
  M[32]= M[34] = u2[1];
  M[28]= M[30] = M[35] = 0.0;

}

double ip3(a,b)
double *a, *b;
{ return(a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
}

void rn3(a)
double *a;
{ double s;
  s = sqrt(ip3(a,a));
  a[0] /= s; a[1] /= s; a[2] /= s;
}

double sptarea(a,b,c)
double *a, *b, *c;
{ double ea, eb, ec, yab, yac, ybc, sab, sac, sbc;
  double ab[3], ac[3], bc[3], x1[3], x2[3];

  ab[0] = a[0]-b[0]; ab[1] = a[1]-b[1]; ab[2] = a[2]-b[2];
  ac[0] = a[0]-c[0]; ac[1] = a[1]-c[1]; ac[2] = a[2]-c[2];
  bc[0] = b[0]-c[0]; bc[1] = b[1]-c[1]; bc[2] = b[2]-c[2];
 
  yab = ip3(ab,a); yac = ip3(ac,a); ybc = ip3(bc,b);

  x1[0] = ab[0] - yab*a[0]; x2[0] = ac[0] - yac*a[0];
  x1[1] = ab[1] - yab*a[1]; x2[1] = ac[1] - yac*a[1];
  x1[2] = ab[2] - yab*a[2]; x2[2] = ac[2] - yac*a[2];
  sab = ip3(x1,x1); sac = ip3(x2,x2);
  ea = acos(ip3(x1,x2)/sqrt(sab*sac));

  x1[0] = ab[0] + yab*b[0]; x2[0] = bc[0] - ybc*b[0];
  x1[1] = ab[1] + yab*b[1]; x2[1] = bc[1] - ybc*b[1];
  x1[2] = ab[2] + yab*b[2]; x2[2] = bc[2] - ybc*b[2];
  sbc = ip3(x2,x2);
  eb = acos(ip3(x1,x2)/sqrt(sab*sbc));

  x1[0] = ac[0] + yac*c[0]; x2[0] = bc[0] + ybc*c[0];
  x1[1] = ac[1] + yac*c[1]; x2[1] = bc[1] + ybc*c[1];
  x1[2] = ac[2] + yac*c[2]; x2[2] = bc[2] + ybc*c[2];
  ec = acos(ip3(x1,x2)/sqrt(sac*sbc));

/*
 * Euler's formula is a+b+c-PI, except I've cheated...
 * a=ea, c=ec, b=PI-eb, which is more stable.
 */
  return(ea+ec-eb);
}

void li(x,f,fb,mint,ar)
double *x, ar;
int (*f)(), (*fb)(), mint;
{ int i, j, nr, nrb, ct1, w;
  double u[3], r, M[36];
  double sres[MXRESULT], tres[MXRESULT];

/* divide mint by 2, and force to even (Simpson's rule...)
 * to make comparable with rectangular interpretation of mint
 */
  mint <<= 1;
  if (mint&1) mint++;

  ct1 = 0;
  for (i= (rmin==0) ? 1 : 0; i<=mint; i++)
  {
    r = rmin + (rmax-rmin)*i/mint;
    w = 2+2*(i&1)-(i==0)-(i==mint);
    u[0] = orig[0]+x[0]*r;
    u[1] = orig[1]+x[1]*r;
    u[2] = orig[2]+x[2]*r;
    nr = f(u,3,tres,NULL);
    if (ct1==0) setzero(sres,nr);
    for (j=0; j<nr; j++)
      sres[j] += w*r*r*tres[j];
    ct1++;

    if ((fb!=NULL) && (i==mint)) /* boundary */
    { sphM(M,rmax,x);
      nrb = fb(u,3,tres,M);
      if (ct0==0) for (j=0; j<nrb; j++) resb[j] = 0.0;
      for (j=0; j<nrb; j++)
        resb[j] += tres[j]*ar;
    }
  }

  if (ct0==0) for (j=0; j<nr; j++) res[j] = 0.0;
  ct0++;

  for (j=0; j<nr; j++)
    res[j] += sres[j] * ar * (rmax-rmin)/(3*mint);
}

void sphint(f,fb,a,b,c,lev,mint,cent)
double *a, *b, *c;
int (*f)(), (*fb)(), lev, mint, cent;
{ double x[3], ab[3], ac[3], bc[3], ar;
  int i;

  if (lev>1)
  { ab[0] = a[0]+b[0]; ab[1] = a[1]+b[1]; ab[2] = a[2]+b[2]; rn3(ab);
    ac[0] = a[0]+c[0]; ac[1] = a[1]+c[1]; ac[2] = a[2]+c[2]; rn3(ac);
    bc[0] = b[0]+c[0]; bc[1] = b[1]+c[1]; bc[2] = b[2]+c[2]; rn3(bc);
    lev >>= 1;
    if (cent==0)
    { sphint(f,fb,a,ab,ac,lev,mint,1);
      sphint(f,fb,ab,bc,ac,lev,mint,0);
    }
    else
    { sphint(f,fb,a,ab,ac,lev,mint,1);
      sphint(f,fb,b,ab,bc,lev,mint,1);
      sphint(f,fb,c,ac,bc,lev,mint,1);
      sphint(f,fb,ab,bc,ac,lev,mint,1);
    }
    return;
  }

  x[0] = a[0]+b[0]+c[0];
  x[1] = a[1]+b[1]+c[1];
  x[2] = a[2]+b[2]+c[2];
  rn3(x);
  ar = sptarea(a,b,c);

  for (i=0; i<8; i++)
  { if (i>0)
    { x[0] = -x[0];
      if (i%2 == 0) x[1] = -x[1];
      if (i==4) x[2] = -x[2];
    }
    switch(cent)
    { case 2: /* the reflection and its 120', 240' rotations */
        ab[0] = x[0]; ab[1] = x[2]; ab[2] = x[1]; li(ab,f,fb,mint,ar);
        ab[0] = x[2]; ab[1] = x[1]; ab[2] = x[0]; li(ab,f,fb,mint,ar);
        ab[0] = x[1]; ab[1] = x[0]; ab[2] = x[2]; li(ab,f,fb,mint,ar);
      case 1: /* and the 120' and 240' rotations */
        ab[0] = x[1]; ab[1] = x[2]; ab[2] = x[0]; li(ab,f,fb,mint,ar);
        ac[0] = x[2]; ac[1] = x[0]; ac[2] = x[1]; li(ac,f,fb,mint,ar);
      case 0: /* and the triangle itself. */
        li( x,f,fb,mint,ar);
    }
  }
}

void integ_sphere(f,fb,fl,Res,Resb,mg)
double *fl, *Res, *Resb;
int (*f)(), (*fb)(), *mg;
{ double a[3], b[3], c[3];

  a[0] = 1; a[1] = a[2] = 0;
  b[1] = 1; b[0] = b[2] = 0;
  c[2] = 1; c[0] = c[1] = 0;
  
  res = Res;
  resb=Resb;
  orig = &fl[2];
  rmin = fl[0];
  rmax = fl[1];

  ct0 = 0;
  sphint(f,fb,a,b,c,mg[1],mg[0],0);
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
 * solving symmetric equations using the jacobian structure. Currently, three
 * methods can be used: cholesky decomposition, eigenvalues, eigenvalues on
 * the correlation matrix.
 *
 * jacob_dec(J,meth)    decompose the matrix, meth=JAC_CHOL, JAC_EIG, JAC_EIGD
 * jacob_solve(J,v)     J^{-1}v
 * jacob_hsolve(J,v)    (R')^{-1/2}v
 * jacob_isolve(J,v)    (R)^{-1/2}v
 * jacob_qf(J,v)         v' J^{-1} v
 * jacob_mult(J,v)      (R'R) v   (pres. CHOL only)
 * where for each decomposition, R'R=J, although the different decomp's will
 * produce different R's.
 *
 * To set up the J matrix:
 * first, allocate storage: jac_alloc(J,p,wk)
 *   where p=dimension of matrix, wk is a numeric vector of length
 *   jac_reqd(p) (or NULL, to allocate automatically).
 * now, copy the numeric values to J->Z (numeric vector with length p*p).
 *   (or, just set J->Z to point to the data vector. But remember this
 *   will be overwritten by the decomposition).
 * finally, set:
 *    J->st=JAC_RAW;
 *    J->p = p;
 *
 * now, call jac_dec(J,meth) (optional) and the solve functions as required.
 *
 */

#include "math.h"
#include "mut.h"

#define DEF_METH JAC_EIGD

int jac_reqd(int p) { return(2*p*(p+1)); }

double *jac_alloc(J,p,wk)
jacobian *J;
int p;
double *wk;
{ if (wk==NULL)
    wk = (double *)calloc(2*p*(p+1),sizeof(double));
     if ( wk == NULL ) {
      printf("Problem allocating memory for wk\n");fflush(stdout);
    }
  J->Z = wk; wk += p*p;
  J->Q = wk; wk += p*p;
  J->wk= wk; wk += p;
  J->dg= wk; wk += p;
  return(wk);
}

void jacob_dec(J, meth)
jacobian *J;
int meth;
{ int i, j, p;

  if (J->st != JAC_RAW) return;

  J->sm = J->st = meth;
  switch(meth)
  { case JAC_EIG:
      eig_dec(J->Z,J->Q,J->p);
      return;
    case JAC_EIGD:
      p = J->p;
      for (i=0; i<p; i++)
        J->dg[i] = (J->Z[i*(p+1)]<=0) ? 0.0 : 1/sqrt(J->Z[i*(p+1)]);
      for (i=0; i<p; i++)
        for (j=0; j<p; j++)
          J->Z[i*p+j] *= J->dg[i]*J->dg[j];
      eig_dec(J->Z,J->Q,J->p);
      J->st = JAC_EIGD;
      return;
    case JAC_CHOL:
      chol_dec(J->Z,J->p,J->p);
      return;
    default: mut_printf("jacob_dec: unknown method %d",meth);
  }
}

int jacob_solve(J,v) /* (X^T W X)^{-1} v */
jacobian *J;
double *v;
{ int i, rank;

  if (J->st == JAC_RAW) jacob_dec(J,DEF_METH);

  switch(J->st)
  { case JAC_EIG:
      return(eig_solve(J,v));
    case JAC_EIGD:
      for (i=0; i<J->p; i++) v[i] *= J->dg[i];
      rank = eig_solve(J,v);
      for (i=0; i<J->p; i++) v[i] *= J->dg[i];
      return(rank);
    case JAC_CHOL:
      return(chol_solve(J->Z,v,J->p,J->p));
  }
  mut_printf("jacob_solve: unknown method %d",J->st);
  return(0);
}

int jacob_hsolve(J,v) /*  J^{-1/2} v */
jacobian *J;
double *v;
{ int i;

  if (J->st == JAC_RAW) jacob_dec(J,DEF_METH);

  switch(J->st)
  { case JAC_EIG:
      return(eig_hsolve(J,v));
    case JAC_EIGD: /* eigenvalues on corr matrix */
      for (i=0; i<J->p; i++) v[i] *= J->dg[i];
      return(eig_hsolve(J,v));
    case JAC_CHOL:
      return(chol_hsolve(J->Z,v,J->p,J->p));
  }
  mut_printf("jacob_hsolve: unknown method %d\n",J->st);
  return(0);
}

int jacob_isolve(J,v) /*  J^{-1/2} v */
jacobian *J;
double *v;
{ int i, r;

  if (J->st == JAC_RAW) jacob_dec(J,DEF_METH);

  switch(J->st)
  { case JAC_EIG:
        return(eig_isolve(J,v));
    case JAC_EIGD: /* eigenvalues on corr matrix */
        r = eig_isolve(J,v);
        for (i=0; i<J->p; i++) v[i] *= J->dg[i];
	return(r);
    case JAC_CHOL:
      return(chol_isolve(J->Z,v,J->p,J->p));
  }
  mut_printf("jacob_hsolve: unknown method %d\n",J->st);
  return(0);
}

double jacob_qf(J,v)  /* vT J^{-1} v */
jacobian *J;
double *v;
{ int i;

  if (J->st == JAC_RAW) jacob_dec(J,DEF_METH);

  switch (J->st)
  { case JAC_EIG:
      return(eig_qf(J,v));
    case JAC_EIGD:
      for (i=0; i<J->p; i++) v[i] *= J->dg[i];
      return(eig_qf(J,v));
    case JAC_CHOL:
      return(chol_qf(J->Z,v,J->p,J->p));
    default:
      mut_printf("jacob_qf: invalid method\n");
      return(0.0);
  }
}

int jacob_mult(J,v)  /* J v */
jacobian *J;
double *v;
{
  if (J->st == JAC_RAW) jacob_dec(J,DEF_METH);
  switch (J->st)
  { case JAC_CHOL:
      return(chol_mult(J->Z,v,J->p,J->p));
    default:
      mut_printf("jacob_mult: invalid method\n");
      return(0);
  }
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
 *  Routines for maximization of a one dimensional function f()
 *    over an interval [xlo,xhi]. In all cases. the flag argument
 *    controls the return:
 *      flag='x', the maximizer xmax is returned.
 *                otherwise, maximum f(xmax) is returned.
 *
 *  max_grid(f,xlo,xhi,n,flag)
 *    grid maximization of f() over [xlo,xhi] with n intervals.
 *
 *  max_golden(f,xlo,xhi,n,tol,err,flag)
 *    golden section maximization.
 *    If n>2, an initial grid search is performed with n intervals
 *      (this helps deal with local maxima).
 *    convergence criterion is |x-xmax| < tol.
 *    err is an error flag.
 *    if flag='x', return value is xmax.
 *       otherwise, return value is f(xmax).
 *
 *  max_quad(f,xlo,xhi,n,tol,err,flag)
 *    quadratic maximization.
 *
 *  max_nr()
 *    newton-raphson, handles multivariate case.
 *
 *  TODO: additional error checking, non-convergence stop.
 */

#include <math.h>
#include "mut.h"

#define max_val(a,b) ((flag=='x') ? a : b)

double max_grid(f,xlo,xhi,n,flag)
double (*f)(), xlo, xhi;
int n;
char flag;
{ int i, mi;
  double x, y, mx, my;
  for (i=0; i<=n; i++)
  { x = xlo + (xhi-xlo)*i/n;
    y = f(x);
    if ((i==0) || (y>my))
    { mx = x;
      my = y;
      mi = i;
    }
  }
  if (mi==0) return(max_val(xlo,my));
  if (mi==n) return(max_val(xhi,my));
  return(max_val(mx,my));
}

double max_golden(f,xlo,xhi,n,tol,err,flag)
double (*f)(), xhi, xlo, tol;
int n, *err;
char flag;
{ double dlt, x0, x1, x2, x3, y0, y1, y2, y3;
  *err = 0;

  if (n>2)
  { dlt = (xhi-xlo)/n;
    x0 = max_grid(f,xlo,xhi,n,'x');
    if (xlo<x0) xlo = x0-dlt;
    if (xhi>x0) xhi = x0+dlt;
  }

  x0 = xlo; y0 = f(xlo);
  x3 = xhi; y3 = f(xhi);
  x1 = gold_rat*x0 + (1-gold_rat)*x3; y1 = f(x1);
  x2 = gold_rat*x3 + (1-gold_rat)*x0; y2 = f(x2);

  while (fabs(x3-x0)>tol)
  { if ((y1>=y0) && (y1>=y2))
    { x3 = x2; y3 = y2;
      x2 = x1; y2 = y1;
      x1 = gold_rat*x0 + (1-gold_rat)*x3; y1 = f(x1);
    }
    else if ((y2>=y3) && (y2>=y1))
    { x0 = x1; y0 = y1;
      x1 = x2; y1 = y2;
      x2 = gold_rat*x3 + (1-gold_rat)*x0; y2 = f(x2);
    }
    else
    { if (y3>y0) { x0 = x2; y0 = y2; }
            else { x3 = x1; y3 = y1; }
      x1 = gold_rat*x0 + (1-gold_rat)*x3; y1 = f(x1);
      x2 = gold_rat*x3 + (1-gold_rat)*x0; y2 = f(x2);
    }
  }
  if (y0>=y1) return(max_val(x0,y0));
  if (y3>=y2) return(max_val(x3,y3));
  return((y1>y2) ? max_val(x1,y1) : max_val(x2,y2));
}

double max_quad(f,xlo,xhi,n,tol,err,flag)
double (*f)(), xhi, xlo, tol;
int n, *err;
char flag;
{ double x0, x1, x2, xnew, y0, y1, y2, ynew, a, b;
  *err = 0;

  if (n>2)
  { x0 = max_grid(f,xlo,xhi,n,'x');
    if (xlo<x0) xlo = x0-1.0/n;
    if (xhi>x0) xhi = x0+1.0/n;
  }

  x0 = xlo; y0 = f(x0);
  x2 = xhi; y2 = f(x2);
  x1 = (x0+x2)/2; y1 = f(x1);

  while (x2-x0>tol)
  {
    /* first, check (y0,y1,y2) is a peak. If not,
     * next interval is the halve with larger of (y0,y2).
     */
    if ((y0>y1) | (y2>y1))
    { 
      if (y0>y2) { x2 = x1; y2 = y1; }
            else { x0 = x1; y0 = y1; }
      x1 = (x0+x2)/2;
      y1 = f(x1);
    }
    else /* peak */
    { a = (y1-y0)*(x2-x1) + (y1-y2)*(x1-x0);
      b = ((y1-y0)*(x2-x1)*(x2+x1) + (y1-y2)*(x1-x0)*(x1+x0))/2;
      /* quadratic maximizer is b/a. But first check if a's too
       * small, since we may be close to constant.
       */
      if ((a<=0) | (b<x0*a) | (b>x2*a))
      { /* split the larger halve */
        xnew = ((x2-x1) > (x1-x0)) ? (x1+x2)/2 : (x0+x1)/2;
      }
      else
      { xnew = b/a;
        if (10*xnew < (9*x0+x1)) xnew = (9*x0+x1)/10;
        if (10*xnew > (9*x2+x1)) xnew = (9*x2+x1)/10;
        if (fabs(xnew-x1) < 0.001*(x2-x0))
        {
          if ((x2-x1) > (x1-x0))
            xnew = (99*x1+x2)/100;
          else
            xnew = (99*x1+x0)/100;
        }
      }
      ynew = f(xnew);
      if (xnew>x1)
      { if (ynew >= y1) { x0 = x1; y0 = y1; x1 = xnew; y1 = ynew; }
                   else { x2 = xnew; y2 = ynew; }
      }
      else
      { if (ynew >= y1) { x2 = x1; y2 = y1; x1 = xnew; y1 = ynew; }
                   else { x0 = xnew; y0 = ynew; }
      }
    }
  }
  return(max_val(x1,y1));
}

double max_nr(F, coef, old_coef, f1, delta, J, p, maxit, tol, err)
double *coef, *old_coef, *f1, *delta, tol;
int (*F)(), p, maxit, *err;
jacobian *J;
{ double old_f, f, lambda;
  int i, j, fr;
  double nc, nd, cut;
  int rank;

  *err = NR_OK;
  J->p = p;
  fr = F(coef, &f, f1, J->Z); J->st = JAC_RAW;

  for (i=0; i<maxit; i++)
  { memcpy(old_coef,coef,p*sizeof(double));
    old_f = f;
    rank = jacob_solve(J,f1);
    memcpy(delta,f1,p*sizeof(double));

    if (rank==0) /* NR won't move! */
      delta[0] = -f/f1[0];

    lambda = 1.0;

    nc = innerprod(old_coef,old_coef,p);
    nd = innerprod(delta, delta, p);
    cut = sqrt(nc/nd);
    if (cut>1.0) cut = 1.0;
    cut *= 0.0001;
    do
    { for (j=0; j<p; j++) coef[j] = old_coef[j] + lambda*delta[j];
      f = old_f - 1.0;
      fr = F(coef, &f, f1, J->Z); J->st = JAC_RAW;
      if (fr==NR_BREAK) return(old_f);

      lambda = (fr==NR_REDUCE) ? lambda/2 : lambda/10.0;
    } while ((lambda>cut) & (f <= old_f - 1.0e-3));

    if (f < old_f - 1.0e-3)
    { *err = NR_NDIV;
      return(f);
    }
    if (fr==NR_REDUCE) return(f);

    if (fabs(f-old_f) < tol) return(f);

  }
  *err = NR_NCON;
  return(f);
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
#include <math.h>
#include "mut.h"

/* qr decomposition of X (n*p organized by column).
 * Take w for the ride, if not NULL.
 */
void qr(X,n,p,w)
double *X, *w;
int n, p;
{ int i, j, k, mi;
  double c, s, mx, nx, t;

  for (j=0; j<p; j++)
  { mi = j;
    mx = fabs(X[(n+1)*j]);
    nx = mx*mx;

    /* find the largest remaining element in j'th column, row mi.
     * flip that row with row j.
     */
    for (i=j+1; i<n; i++)
    { nx += X[j*n+i]*X[j*n+i];
      if (fabs(X[j*n+i])>mx)
      { mi = i;
        mx = fabs(X[j*n+i]);
      }
    }
    for (i=j; i<p; i++)
    { t = X[i*n+j];
      X[i*n+j] = X[i*n+mi];
      X[i*n+mi] = t;
    }
    if (w!=NULL) { t = w[j]; w[j] = w[mi]; w[mi] = t; }

    /* want the diag. element -ve, so we do the `good' Householder reflect.
     */
    if (X[(n+1)*j]>0)
    { for (i=j; i<p; i++) X[i*n+j] = -X[i*n+j];
      if (w!=NULL) w[j] = -w[j];
    }

    nx = sqrt(nx);
    c = nx*(nx-X[(n+1)*j]);
    if (c!=0)
    { for (i=j+1; i<p; i++)
      { s = 0;
        for (k=j; k<n; k++)
          s += X[i*n+k]*X[j*n+k];
        s = (s-nx*X[i*n+j])/c;
        for (k=j; k<n; k++)
          X[i*n+k] -= s*X[j*n+k];
        X[i*n+j] += s*nx;
      }
      if (w != NULL)
      { s = 0;
        for (k=j; k<n; k++)
          s += w[k]*X[n*j+k];
        s = (s-nx*w[j])/c;
        for (k=j; k<n; k++)
          w[k] -= s*X[n*j+k];
        w[j] += s*nx;
      }
      X[j*n+j] = nx;
    }
  }
}

void qrinvx(R,x,n,p)
double *R, *x;
int n, p;
{ int i, j;
  for (i=p-1; i>=0; i--)
  { for (j=i+1; j<p; j++) x[i] -= R[j*n+i]*x[j];
    x[i] /= R[i*n+i];
  }
}

void qrtinvx(R,x,n,p)
double *R, *x;
int n, p;
{ int i, j;
  for (i=0; i<p; i++)
  { for (j=0; j<i; j++) x[i] -= R[i*n+j]*x[j];
    x[i] /= R[i*n+i];
  }
}

void qrsolv(R,x,n,p)
double *R, *x;
int n, p;
{ qrtinvx(R,x,n,p);
  qrinvx(R,x,n,p);
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
 *  solve f(x)=c by various methods, with varying stability etc...
 *    xlo and xhi should be initial bounds for the solution.
 *    convergence criterion is |f(x)-c| < tol.
 *
 *  double solve_bisect(f,c,xmin,xmax,tol,bd_flag,err)
 *  double solve_secant(f,c,xmin,xmax,tol,bd_flag,err)
 *    Bisection and secant methods for solving of f(x)=c.
 *    xmin and xmax are starting values and bound for solution.
 *    tol = convergence criterion, |f(x)-c| < tol.
 *    bd_flag = if (xmin,xmax) doesn't bound a solution, what action to take?
 *      BDF_NONE returns error.
 *      BDF_EXPRIGHT increases xmax.
 *      BDF_EXPLEFT  decreases xmin.
 *    err = error flag.
 *    The (xmin,xmax) bound is not formally necessary for the secant method.
 *    But having such a bound vastly improves stability; the code performs
 *    a bisection step whenever the iterations run outside the bounds.
 *
 *  double solve_nr(f,f1,c,x0,tol,err)
 *    Newton-Raphson solution of f(x)=c.
 *    f1 = f'(x).
 *    x0 = starting value.
 *    tol = convergence criteria, |f(x)-c| < tol.
 *    err = error flag.
 *    No stability checks at present.
 *
 *  double solve_fp(f,x0,tol)
 *    fixed-point iteration to solve f(x)=x.
 *    x0 = starting value.
 *    tol = convergence criteria, stops when |f(x)-x| < tol.
 *    Convergence requires |f'(x)|<1 in neighborhood of true solution;
 *      f'(x) \approx 0 gives the fastest convergence.
 *    No stability checks at present.
 *
 *  TODO: additional error checking, non-convergence stop.
 */

#include <math.h>
#include "mut.h"

typedef struct {
  double xmin, xmax, x0, x1;
  double ymin, ymax, y0, y1;
} solvest;

int step_expand(f,c,sv,bd_flag)
double (*f)(), c;
solvest *sv;
int bd_flag;
{ double x, y;
  if (sv->ymin*sv->ymax <= 0.0) return(0);
  if (bd_flag == BDF_NONE)
  { mut_printf("invalid bracket\n");
    return(1); /* error */
  }
  if (bd_flag == BDF_EXPRIGHT)
  { while (sv->ymin*sv->ymax > 0)
    { x = sv->xmax + 2*(sv->xmax-sv->xmin);
      y = f(x) - c;
      sv->xmin = sv->xmax; sv->xmax = x;
      sv->ymin = sv->ymax; sv->ymax = y;
    }
    return(0);
  }
  if (bd_flag == BDF_EXPLEFT)
  { while (sv->ymin*sv->ymax > 0)
    { x = sv->xmin - 2*(sv->xmax-sv->xmin);
      y = f(x) - c;
      sv->xmax = sv->xmin; sv->xmin = x;
      sv->ymax = sv->ymin; sv->ymin = y;
    }
    return(0);
  }
  mut_printf("step_expand: unknown bd_flag %d.\n",bd_flag);
  return(1);
}

int step_addin(sv,x,y)
solvest *sv;
double x, y;
{ sv->x1 = sv->x0; sv->x0 = x;
  sv->y1 = sv->y0; sv->y0 = y;
  if (y*sv->ymin > 0)
  { sv->xmin = x;
    sv->ymin = y;
    return(0);
  }
  if (y*sv->ymax > 0)
  { sv->xmax = x;
    sv->ymax = y;
    return(0);
  }  
  if (y==0)
  { sv->xmin = sv->xmax = x;
    sv->ymin = sv->ymax = 0;
    return(0);
  }
  return(1);
}

int step_bisect(f,c,sv)
double (*f)(), c;
solvest *sv;
{ double x, y;
  x = sv->x0 = (sv->xmin + sv->xmax)/2;
  y = sv->y0 = f(x)-c;
  return(step_addin(sv,x,y));
}

double solve_bisect(f,c,xmin,xmax,tol,bd_flag,err)
double (*f)(), c, xmin, xmax, tol;
int bd_flag, *err;
{ solvest sv;
  int z;
  *err = 0;
  sv.xmin = xmin; sv.ymin = f(xmin)-c;
  sv.xmax = xmax; sv.ymax = f(xmax)-c;
  *err = step_expand(f,c,&sv,bd_flag);
  if (*err>0) return(sv.xmin);
  while(1) /* infinite loop if f is discontinuous */
  { z = step_bisect(f,c,&sv);
    if (z)
    { *err = 1;
      return(sv.x0);
    }
    if (fabs(sv.y0)<tol) return(sv.x0);
  }
}

int step_secant(f,c,sv)
double (*f)(), c;
solvest *sv;
{ double x, y;
  if (sv->y0==sv->y1) return(step_bisect(f,c,sv));
  x = sv->x0 + (sv->x1-sv->x0)*sv->y0/(sv->y0-sv->y1);
  if ((x<=sv->xmin) | (x>=sv->xmax)) return(step_bisect(f,c,sv));
  y = f(x)-c;
  return(step_addin(sv,x,y));
}

double solve_secant(f,c,xmin,xmax,tol,bd_flag,err)
double (*f)(), c, xmin, xmax, tol;
int bd_flag, *err;
{ solvest sv;
  int z;
  *err = 0;
  sv.xmin = xmin; sv.ymin = f(xmin)-c;
  sv.xmax = xmax; sv.ymax = f(xmax)-c;
  *err = step_expand(f,c,&sv,bd_flag);
  if (*err>0) return(sv.xmin);
  sv.x0 = sv.xmin; sv.y0 = sv.ymin;
  sv.x1 = sv.xmax; sv.y1 = sv.ymax;
  while(1) /* infinite loop if f is discontinuous */
  { z = step_secant(f,c,&sv);
    if (z)
    { *err = 1;
      return(sv.x0);
    }
    if (fabs(sv.y0)<tol) return(sv.x0);
  }
}

double solve_nr(f,f1,c,x0,tol,err)
double (*f)(), (*f1)(), c, x0, tol;
int *err;
{ double y;
  do
  { y = f(x0)-c;
    x0 -= y/f1(x0);
  } while (fabs(y)>tol);
  return(x0);
}

double solve_fp(f,x0,tol,maxit)
double (*f)(), x0, tol;
int maxit;
{ double x1;
  int i;
  for (i=0; i<maxit; i++)
  { x1 = f(x0);
    if (fabs(x1-x0)<tol) return(x1);
    x0 = x1;
  }
  return(x1); /* although it hasn't converged */
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
#include "mut.h"

void svd(x,p,q,d,mxit)  /* svd of square matrix */
double *x, *p, *q;
int d, mxit;
{ int i, j, k, iter, ms, zer;
  double r, u, v, cp, cm, sp, sm, c1, c2, s1, s2, mx;
  for (i=0; i<d; i++)
    for (j=0; j<d; j++) p[i*d+j] = q[i*d+j] = (i==j);
  for (iter=0; iter<mxit; iter++)
  { ms = 0;
    for (i=0; i<d; i++)
      for (j=i+1; j<d; j++)
      { s1 = fabs(x[i*d+j]);
        s2 = fabs(x[j*d+i]);
        mx = (s1>s2) ? s1 : s2;
        zer = 1;
        if (mx*mx>1.0e-15*fabs(x[i*d+i]*x[j*d+j]))
        { if (fabs(x[i*(d+1)])<fabs(x[j*(d+1)]))
          { for (k=0; k<d; k++)
            { u = x[i*d+k]; x[i*d+k] = x[j*d+k]; x[j*d+k] = u;
              u = p[k*d+i]; p[k*d+i] = p[k*d+j]; p[k*d+j] = u;
            }
            for (k=0; k<d; k++)
            { u = x[k*d+i]; x[k*d+i] = x[k*d+j]; x[k*d+j] = u;
              u = q[k*d+i]; q[k*d+i] = q[k*d+j]; q[k*d+j] = u;
            }
          }
          cp = x[i*(d+1)]+x[j*(d+1)];
          sp = x[j*d+i]-x[i*d+j];
          r = sqrt(cp*cp+sp*sp);
          if (r>0) { cp /= r; sp /= r; }
              else { cp = 1.0; zer = 0;}
          cm = x[i*(d+1)]-x[j*(d+1)];
          sm = x[i*d+j]+x[j*d+i];
          r = sqrt(cm*cm+sm*sm);
          if (r>0) { cm /= r; sm /= r; }
              else { cm = 1.0; zer = 0;}
          c1 = cm+cp;
          s1 = sm+sp;
          r = sqrt(c1*c1+s1*s1);
          if (r>0) { c1 /= r; s1 /= r; }
              else { c1 = 1.0; zer = 0;}
          if (fabs(s1)>ms) ms = fabs(s1);
          c2 = cm+cp;
          s2 = sp-sm;
          r = sqrt(c2*c2+s2*s2);
          if (r>0) { c2 /= r; s2 /= r; }
              else { c2 = 1.0; zer = 0;}
          for (k=0; k<d; k++)
          { u = x[i*d+k]; v = x[j*d+k];
            x[i*d+k] = c1*u+s1*v;
            x[j*d+k] = c1*v-s1*u;
            u = p[k*d+i]; v = p[k*d+j];
            p[k*d+i] = c1*u+s1*v;
            p[k*d+j] = c1*v-s1*u;
          }
          for (k=0; k<d; k++)
          { u = x[k*d+i]; v = x[k*d+j];
            x[k*d+i] = c2*u-s2*v;
            x[k*d+j] = s2*u+c2*v;
            u = q[k*d+i]; v = q[k*d+j];
            q[k*d+i] = c2*u-s2*v;
            q[k*d+j] = s2*u+c2*v;
          }
          if (zer) x[i*d+j] = x[j*d+i] = 0.0;
          ms = 1;
        }
      }
    if (ms==0) iter=mxit+10;
  }
  if (iter==mxit) mut_printf("Warning: svd not converged.\n");
  for (i=0; i<d; i++)
    if (x[i*d+i]<0)
    { x[i*d+i] = -x[i*d+i];
      for (j=0; j<d; j++) p[j*d+i] = -p[j*d+i];
    }
}

int svdsolve(x,w,P,D,Q,d,tol) /* original X = PDQ^T; comp. QD^{-1}P^T x */
double *x, *w, *P, *D, *Q, tol;
int d;
{ int i, j, rank;
  double mx;
  if (tol>0)
  { mx = D[0];
    for (i=1; i<d; i++) if (D[i*(d+1)]>mx) mx = D[i*(d+1)];
    tol *= mx;
  }
  rank = 0;
  for (i=0; i<d; i++)
  { w[i] = 0.0;
    for (j=0; j<d; j++) w[i] += P[j*d+i]*x[j];
  }
  for (i=0; i<d; i++)
    if (D[i*d+i]>tol)
    { w[i] /= D[i*(d+1)];
      rank++;
    }
  for (i=0; i<d; i++)
  { x[i] = 0.0;
    for (j=0; j<d; j++) x[i] += Q[i*d+j]*w[j];
  }
  return(rank);
}

void hsvdsolve(x,w,P,D,Q,d,tol) /* original X = PDQ^T; comp. D^{-1/2}P^T x */
double *x, *w, *P, *D, *Q, tol;
int d;
{ int i, j;
  double mx;
  if (tol>0)
  { mx = D[0];
    for (i=1; i<d; i++) if (D[i*(d+1)]>mx) mx = D[i*(d+1)];
    tol *= mx;
  }
  for (i=0; i<d; i++)
  { w[i] = 0.0;
    for (j=0; j<d; j++) w[i] += P[j*d+i]*x[j];
  }
  for (i=0; i<d; i++) if (D[i*d+i]>tol) w[i] /= sqrt(D[i*(d+1)]);
  for (i=0; i<d; i++) x[i] = w[i];
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
 *   Includes some miscellaneous vector functions:
 *     setzero(v,p)           sets all elements of v to 0.
 *     unitvec(x,k,p)         sets x to k'th unit vector e_k.
 *     innerprod(v1,v2,p)     inner product.
 *     addouter(A,v1,v2,p,c)  A <- A + c * v_1 v2^T
 *     multmatscal(A,z,n)     A <- A*z
 *     matrixmultiply(A,B,C,m,n,p)    C(m*p) <- A(m*n) * B(n*p)
 *     transpose(x,m,n)       inline transpose
 *     m_trace(x,n)           trace
 *     vecsum(x,n)            sum elements.
 */

#include "mut.h"

void setzero(v,p)
double *v;
int p;
{ int i;
  for (i=0; i<p; i++) v[i] = 0.0;
}

void unitvec(x,k,p)
double *x;
int k, p;
{ setzero(x,p);
  x[k] = 1.0;
}

double innerprod(v1,v2,p)
double *v1, *v2;
int p;
{ int i;
  double s;
  s = 0;
  for (i=0; i<p; i++) s += v1[i]*v2[i];
  return(s);
}

void addouter(A,v1,v2,p,c)
double *A, *v1, *v2, c;
int p;
{ int i, j;
  for (i=0; i<p; i++)
    for (j=0; j<p; j++)
      A[i*p+j] += c*v1[i]*v2[j];
}

void multmatscal(A,z,n)
double *A, z;
int n;
{ int i;
  for (i=0; i<n; i++) A[i] *= z;
}

/* matrix multiply A (m*n) times B (n*p).
 * store in C (m*p).
 * all matrices stored by column.
 */
void matrixmultiply(A,B,C,m,n,p)
double *A, *B, *C;
int m, n, p;
{ int i, j, k, ij;
  for (i=0; i<m; i++)
    for (j=0; j<p; j++)
    { ij = j*m+i;
      C[ij] = 0.0;
      for (k=0; k<n; k++)
        C[ij] += A[k*m+i] * B[j*n+k];
    }
}

/*
 *  transpose() transposes an m*n matrix in place.
 *  At input, the matrix has n rows, m columns and
 *    x[0..n-1] is the is the first column.
 *  At output, the matrix has m rows, n columns and
 *    x[0..m-1] is the first column.
 */
void transpose(x,m,n)
double *x;
int m, n;
{ int t0, t, ti, tj;
  double z;
  for (t0=1; t0<m*n-2; t0++)
    { ti = t0%m; tj = t0/m;
      do
      { t = ti*n+tj;
        ti= t%m;
        tj= t/m;
      } while (t<t0);
      z = x[t];
      x[t] = x[t0];
      x[t0] = z;
    }
}

/* trace of an n*n square matrix. */
double m_trace(x,n)
double *x;
int n;
{ int i;
  double sum;
  sum = 0;
  for (i=0; i<n; i++)
    sum += x[i*(n+1)];
  return(sum);
}

double vecsum(v,n)
double *v;
int n;
{ int i;
  double sum;
  sum = 0.0;
  for (i=0; i<n; i++) sum += v[i];
  return(sum);
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
  miscellaneous functions that may not be defined in the math
  libraries. The implementations are crude.
  mut_daws(x)   -- dawson's function
  mut_exp(x)    -- exp(x), but it won't overflow.

  where required, these must be #define'd in header files.

  also includes
  ptail(x)    -- exp(x*x/2)*int_{-\infty}^x exp(-u^2/2)du for x < -6.
  logit(x)    -- logistic function.
  expit(x)    -- inverse of logit.
  factorial(n)-- factorial
 */

#include "mut.h"

double mut_exp(x)
double x;
{ if (x>700.0) return(1.014232054735004e+304);
  return(exp(x));
}

double mut_daws(x)
double x;
{ static double val[] = {
  0,  0.24485619356002, 0.46034428261948, 0.62399959848185, 0.72477845900708,
      0.76388186132749, 0.75213621001998, 0.70541701910853, 0.63998807456541,
      0.56917098836654, 0.50187821196415, 0.44274283060424, 0.39316687916687,
      0.35260646480842, 0.31964847250685, 0.29271122077502, 0.27039629581340,
      0.25160207761769, 0.23551176224443, 0.22153505358518, 0.20924575719548,
      0.19833146819662, 0.18855782729305, 0.17974461154688, 0.17175005072385 };
  double h, f0, f1, f2, y, z, xx;
  int j, m;
  if (x<0) return(-mut_daws(-x));
  if (x>6)
  { /* Tail series: 1/x + 1/x^3 + 1.3/x^5 + 1.3.5/x^7 + ...  */
    y = z = 1/x;
    j = 0;
    while (((f0=(2*j+1)/(x*x))<1) && (y>1.0e-10*z))
    { y *= f0;
      z += y;
      j++;
    }
    return(z);
  }
  m = (int) (4*x);
  h = x-0.25*m;
  if (h>0.125)
  { m++;
    h = h-0.25;
  }
  xx = 0.25*m;
  f0 = val[m];
  f1 = 1-xx*f0;
  z = f0+h*f1;
  y = h;
  j = 2;
  while (fabs(y)>z*1.0e-10)
  { f2 = -(j-1)*f0-xx*f1;
    y *= h/j;
    z += y*f2;
    f0 = f1; f1 = f2;
    j++;
  }
  return(z);
}

double ptail(x) /* exp(x*x/2)*int_{-\infty}^x exp(-u^2/2)du for x < -6 */
double x;
{ double y, z, f0;
  int j;
  y = z = -1.0/x;
  j = 0;
  while ((fabs(f0= -(2*j+1)/(x*x))<1) && (fabs(y)>1.0e-10*z))
  { y *= f0;
    z += y;
    j++;
  }
  return(z);
}

double logit(x)
double x;
{ return(log(x/(1-x)));
}

double expit(x)
double x;
{ double u;
  if (x<0)
  { u = exp(x);
    return(u/(1+u));
  }
  return(1/(1+exp(-x)));
}

int factorial(n)
int n;
{ if (n<=1) return(1.0);
  return(n*factorial(n-1));
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
 * Constrained maximization of a bivariate function.
 *   maxbvgrid(f,x,ll,ur,m0,m1)
 *     maximizes over a grid of m0*m1 points. Returns the maximum,
 *     and the maximizer through the array x. Usually this is a starter,
 *     to choose between local maxima, followed by other routines to refine.
 *
 *  maxbvstep(f,x,ymax,h,ll,ur,err)
 *     essentially multivariate bisection. A 3x3 grid of points is
 *     built around the starting value (x,ymax). This grid is moved
 *     around (step size h[0] and h[1] in the two dimensions) until
 *     the maximum is in the middle.  Then, the step size is halved.
 *     Usually, this will be called in a loop.
 *     The error flag is set if the maximum can't be centered in a
 *     reasonable number of steps.
 *
 *  maxbv(f,x,h,ll,ur,m0,m1,tol)
 *     combines the two previous functions. It begins with a grid search
 *     (if m0>0 and m1>0), followed by refinement. Refines until both h
 *     components are < tol.
 */
#include "mut.h"

#define max(a,b) ((a)>(b) ? (a) : (b))
#define min(a,b) ((a)<(b) ? (a) : (b))

double maxbvgrid(f,x,ll,ur,m0,m1,con)
double (*f)(), *x, *ll, *ur;
int m0, m1, *con;
{ int i, j, im, jm;
  double y, ymax;

  im = -1;
  for (i=0; i<=m0; i++)
  { x[0] = ((m0-i)*ll[0] + i*ur[0])/m0;
    for (j=0; j<=m1; j++)
    { x[1] = ((m1-j)*ll[1] + j*ur[1])/m1;
      y = f(x);
      if ((im==-1) || (y>ymax))
      { im = i; jm = j;
        ymax = y;
      }
    }
  }

  x[0] = ((m0-im)*ll[0] + im*ur[0])/m0;
  x[1] = ((m1-jm)*ll[1] + jm*ur[1])/m1;
  con[0] = (im==m0)-(im==0);
  con[1] = (jm==m1)-(jm==0);
  return(ymax);
}

double maxbvstep(f,x,ymax,h,ll,ur,err,con)
double (*f)(), *x, ymax, *h, *ll, *ur;
int *err, *con;
{ int i, j, ij, imax, steps, cts[2];
  double newx, X[9][2], y[9];

  imax =4; y[4] = ymax;

  for (i=(con[0]==-1)-1; i<2-(con[0]==1); i++)
    for (j=(con[1]==-1)-1; j<2-(con[1]==1); j++)
    { ij = 3*i+j+4;
      X[ij][0] = x[0]+i*h[0];
      if (X[ij][0] < ll[0]+0.001*h[0]) X[ij][0] = ll[0];
      if (X[ij][0] > ur[0]-0.001*h[0]) X[ij][0] = ur[0];
      X[ij][1] = x[1]+j*h[1];
      if (X[ij][1] < ll[1]+0.001*h[1]) X[ij][1] = ll[1];
      if (X[ij][1] > ur[1]-0.001*h[1]) X[ij][1] = ur[1];
      if (ij != 4)
      { y[ij] = f(X[ij]);
        if (y[ij]>ymax) { imax = ij; ymax = y[ij]; }
      }
    }

  steps = 0;
  cts[0] = cts[1] = 0;
  while ((steps<20) && (imax != 4))
  { steps++;
    if ((con[0]>-1) && ((imax/3)==0)) /* shift right */
    {
      cts[0]--;
      for (i=8; i>2; i--)
      { X[i][0] = X[i-3][0]; y[i] = y[i-3];
      }
      imax = imax+3;
      if (X[imax][0]==ll[0])
        con[0] = -1;
      else
      { newx = X[imax][0]-h[0];
        if (newx < ll[0]+0.001*h[0]) newx = ll[0];
        for (i=(con[1]==-1); i<3-(con[1]==1); i++)
        { X[i][0] = newx;
          y[i] = f(X[i]);
          if (y[i]>ymax) { ymax = y[i]; imax = i; }
        }
        con[0] = 0;
      }
    }

    if ((con[0]<1) && ((imax/3)==2)) /* shift left */
    {
      cts[0]++;
      for (i=0; i<6; i++)
      { X[i][0] = X[i+3][0]; y[i] = y[i+3];
      }
      imax = imax-3;
      if (X[imax][0]==ur[0])
        con[0] = 1;
      else
      { newx = X[imax][0]+h[0];
        if (newx > ur[0]-0.001*h[0]) newx = ur[0];
        for (i=6+(con[1]==-1); i<9-(con[1]==1); i++)
        { X[i][0] = newx;
          y[i] = f(X[i]);
          if (y[i]>ymax) { ymax = y[i]; imax = i; }
        }
        con[0] = 0;
      }
    }

    if ((con[1]>-1) && ((imax%3)==0)) /* shift up */
    {
      cts[1]--;
      for (i=9; i>0; i--) if (i%3 > 0)
      { X[i][1] = X[i-1][1]; y[i] = y[i-1];
      }
      imax = imax+1;
      if (X[imax][1]==ll[1])
        con[1] = -1;
      else
      { newx = X[imax][1]-h[1];
        if (newx < ll[1]+0.001*h[1]) newx = ll[1];
        for (i=3*(con[0]==-1); i<7-(con[0]==1); i=i+3)
        { X[i][1] = newx;
          y[i] = f(X[i]);
          if (y[i]>ymax) { ymax = y[i]; imax = i; }
        }
        con[1] = 0;
      }
    }

    if ((con[1]<1) && ((imax%3)==2)) /* shift down */
    {
      cts[1]++;
      for (i=0; i<9; i++) if (i%3 < 2)
      { X[i][1] = X[i+1][1]; y[i] = y[i+1];
      }
      imax = imax-1;
      if (X[imax][1]==ur[1])
        con[1] = 1;
      else
      { newx = X[imax][1]+h[1];
        if (newx > ur[1]-0.001*h[1]) newx = ur[1];
        for (i=2+3*(con[0]==-1); i<9-(con[0]==1); i=i+3)
        { X[i][1] = newx;
          y[i] = f(X[i]);
          if (y[i]>ymax) { ymax = y[i]; imax = i; }
        }
        con[1] = 0;
      }
    }
/* if we've taken 3 steps in one direction, try increasing the
 * corresponding h.
 */
    if ((cts[0]==-2) | (cts[0]==2))
    { h[0] = 2*h[0]; cts[0] = 0; }
    if ((cts[1]==-2) | (cts[1]==2))
    { h[1] = 2*h[1]; cts[1] = 0; }
  }

  if (steps==40)
    *err = 1;
  else
  {
    h[0] /= 2.0; h[1] /= 2.0;
    *err = 0;
  }

  x[0] = X[imax][0];
  x[1] = X[imax][1];
  return(y[imax]);
}

#define BQMmaxp 5

int boxquadmin(J,b0,p,x0,ll,ur)
jacobian *J;
double *b0, *x0, *ll, *ur;
int p;
{ double b[BQMmaxp], x[BQMmaxp], L[BQMmaxp*BQMmaxp], C[BQMmaxp*BQMmaxp], d[BQMmaxp];
  double f, fmin;
  int i, imin, m, con[BQMmaxp], rlx;

  if (p>BQMmaxp) mut_printf("boxquadmin: maxp is 5.\n");
  if (J->st != JAC_RAW) mut_printf("boxquadmin: must start with JAC_RAW.\n");

  m = 0;
  setzero(L,p*p);
  setzero(x,p);
  memcpy(C,J->Z,p*p*sizeof(double));
  for (i=0; i<p; i++) con[i] = 0;

  do
  {
/* first, keep minimizing and add constraints, one at a time.
 */
    do
    {
      matrixmultiply(C,x,b,p,p,1);
      for (i=0; i<p; i++) b[i] += b0[i];
      conquadmin(J,b,p,L,d,m);
/* if C matrix is not pd, don't even bother.
 * this relies on having used cholesky dec.
 */
if ((J->Z[0]==0.0) | (J->Z[3]==0.0)) return(1);
      fmin = 1.0;
      for (i=0; i<p; i++) if (con[i]==0)
      { f = 1.0;
        if (x0[i]+x[i]+b[i] < ll[i]) f = (ll[i]-x[i]-x0[i])/b[i];
        if (x0[i]+x[i]+b[i] > ur[i]) f = (ur[i]-x[i]-x0[i])/b[i];
        if (f<fmin) fmin = f;
        imin = i;
      }
      for (i=0; i<p; i++) x[i] += fmin*b[i];
      if (fmin<1.0)
      { L[m*p+imin] = 1;
        m++;
        con[imin] = (b[imin]<0) ? -1 : 1;
      }
    } while ((fmin < 1.0) & (m<p));

/* now, can I relax any constraints?
 * compute slopes at current point. Can relax if:
 *   slope is -ve on a lower boundary.
 *   slope is +ve on an upper boundary.
 */
    rlx = 0;
    if (m>0)
    { matrixmultiply(C,x,b,p,p,1);
      for (i=0; i<p; i++) b[i] += b0[i];
      for (i=0; i<p; i++)
      { if ((con[i]==-1)&& (b[i]<0)) { con[i] = 0; rlx = 1; }
        if ((con[i]==1) && (b[i]>0)) { con[i] = 0; rlx = 1; }
      }

      if (rlx) /* reconstruct the constraint matrix */
      { setzero(L,p*p); m = 0;
        for (i=0; i<p; i++) if (con[i] != 0)
        { L[m*p+i] = 1;
          m++;
        }
      }
    }
  } while (rlx);

  memcpy(b0,x,p*sizeof(double)); /* this is how far we should move from x0 */
  return(0);
}

double maxquadstep(f,x,ymax,h,ll,ur,err,con)
double (*f)(), *x, ymax, *h, *ll, *ur;
int *err, *con;
{ jacobian J;
  double b[2], c[2], d, jwork[12];
  double x0, x1, y0, y1, ym, h0, xl[2], xu[2], xi[2];
  int i, m;
  
  *err = 0;

/* first, can we relax any of the initial constraints?
 * if so, just do one step away from the boundary, and
 * return for restart.
 */
  for (i=0; i<2; i++)
    if (con[i] != 0)
    { xi[0] = x[0]; xi[1] = x[1];
      xi[i] = x[i]-con[i]*h[i];
      y0 = f(xi);
      if (y0>ymax)
      { memcpy(x,xi,2*sizeof(double));
        con[i] = 0;
        return(y0);
      }
    }

/* now, all initial constraints remain active.
 */

  m = 9;
  for (i=0; i<2; i++) if (con[i]==0)
  { m /= 3;
    xl[0] = x[0]; xl[1] = x[1];
    xl[i] = max(x[i]-h[i],ll[i]); y0 = f(xl);
    x0 = xl[i]-x[i]; y0 -= ymax;
    xu[0] = x[0]; xu[1] = x[1];
    xu[i] = min(x[i]+h[i],ur[i]); y1 = f(xu);
    x1 = xu[i]-x[i]; y1 -= ymax;
    if (x0*x1*(x1-x0)==0) { *err = 1; return(0.0); }
    b[i] = (x0*x0*y1-x1*x1*y0)/(x0*x1*(x0-x1));
    c[i] = 2*(x0*y1-x1*y0)/(x0*x1*(x1-x0));
    if (c[i] >= 0.0) { *err = 1; return(0.0); }
    xi[i] = (b[i]<0) ? xl[i] : xu[i];
  }
  else { c[i] = -1.0; b[i] = 0.0; } /* enforce initial constraints */

  if ((con[0]==0) && (con[1]==0))
  { x0 = xi[0]-x[0];
    x1 = xi[1]-x[1];
    ym = f(xi) - ymax - b[0]*x0 - c[0]*x0*x0/2 - b[1]*x1 - c[1]*x1*x1/2;
    d = ym/(x0*x1);
  }
  else d = 0.0;

/* now, maximize the quadratic.
 * y[4] + b0*x0 + b1*x1 + 0.5(c0*x0*x0 + c1*x1*x1 + 2*d*x0*x1)
 * -ve everything, to call quadmin.
 */
  jac_alloc(&J,2,jwork);
  J.Z[0] = -c[0];
  J.Z[1] = J.Z[2] = -d;
  J.Z[3] = -c[1];
  J.st = JAC_RAW;
  J.p = 2;
  b[0] = -b[0]; b[1] = -b[1];
  *err = boxquadmin(&J,b,2,x,ll,ur);
  if (*err) return(ymax);

/* test to see if this step successfully increases...
 */
  for (i=0; i<2; i++)
  { xi[i] = x[i]+b[i];
    if (xi[i]<ll[i]+1e-8*h[i]) xi[i] = ll[i];
    if (xi[i]>ur[i]-1e-8*h[i]) xi[i] = ur[i];
  }
  y1 = f(xi);
  if (y1 < ymax) /* no increase */
  { *err = 1;
    return(ymax);
  }

/* wonderful. update x, h, with the restriction that h can't decrease
 * by a factor over 10, or increase by over 2.
 */
  for (i=0; i<2; i++)
  { x[i] = xi[i];
    if (x[i]==ll[i]) con[i] = -1;
    if (x[i]==ur[i]) con[i] = 1;
    h0 = fabs(b[i]);
    h0 = min(h0,2*h[i]);
    h0 = max(h0,h[i]/10);
    h[i] = min(h0,(ur[i]-ll[i])/2.0); 
  }
  return(y1);
}

double maxbv(f,x,h,ll,ur,m0,m1,tol)
double (*f)(), *x, *h, *ll, *ur, tol;
int m0, m1;
{ double ymax;
  int err, con[2];

  con[0] = con[1] = 0;
  if ((m0>0) & (m1>0))
  {
    ymax = maxbvgrid(f,x,ll,ur,m0,m1,con);
    h[0] = (ur[0]-ll[0])/(2*m0);
    h[1] = (ur[1]-ll[1])/(2*m1);
  }
  else
  { x[0] = (ll[0]+ur[0])/2;
    x[1] = (ll[1]+ur[1])/2;
    h[0] = (ur[0]-ll[0])/2;
    h[1] = (ur[1]-ll[1])/2;
    ymax = f(x);
  }

  while ((h[0]>tol) | (h[1]>tol))
  { ymax = maxbvstep(f,x,ymax,h,ll,ur,&err,con);
    if (err) mut_printf("maxbvstep failure\n");
  }

  return(ymax);
}

double maxbvq(f,x,h,ll,ur,m0,m1,tol)
double (*f)(), *x, *h, *ll, *ur, tol;
int m0, m1;
{ double ymax;
  int err, con[2];

  con[0] = con[1] = 0;
  if ((m0>0) & (m1>0))
  {
    ymax = maxbvgrid(f,x,ll,ur,m0,m1,con);
    h[0] = (ur[0]-ll[0])/(2*m0);
    h[1] = (ur[1]-ll[1])/(2*m1);
  }
  else
  { x[0] = (ll[0]+ur[0])/2;
    x[1] = (ll[1]+ur[1])/2;
    h[0] = (ur[0]-ll[0])/2;
    h[1] = (ur[1]-ll[1])/2;
    ymax = f(x);
  }

  while ((h[0]>tol) | (h[1]>tol))
  { /* first, try a quadratric step */
    ymax = maxquadstep(f,x,ymax,h,ll,ur,&err,con);
    /* if the quadratic step fails, move the grid around */
    if (err)
    {
      ymax = maxbvstep(f,x,ymax,h,ll,ur,&err,con);
      if (err)
      { mut_printf("maxbvstep failure\n");
        return(ymax);
      }
    }
  }

  return(ymax);
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
#include "mut.h"

prf mut_printf = (prf)printf;

void mut_redirect(newprf)
prf newprf;
{ mut_printf = newprf;
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
/*
 * function to find order of observations in an array.
 *
 * mut_order(x,ind,i0,i1)
 *   x     array to find order of.
 *   ind   integer array of indexes.
 *   i0,i1 (integers) range to order.
 *
 * at output, ind[i0...i1] are permuted so that
 *   x[ind[i0]] <= x[ind[i0+1]] <= ... <= x[ind[i1]].
 * (with ties, output order of corresponding indices is arbitrary).
 * The array x is unchanged.
 *
 * Typically, if x has length n, then i0=0, i1=n-1 and
 * ind is (any permutation of) 0...n-1.
 */

#include "mut.h"

double med3(x0,x1,x2)
double x0, x1, x2;
{ if (x0<x1)
  { if (x2<x0) return(x0);
    if (x1<x2) return(x1);
    return(x2);
  }
/* x1 < x0 */
  if (x2<x1) return(x1);
  if (x0<x2) return(x0);
  return(x2);
}

void mut_order(x,ind,i0,i1)
double *x;
int *ind, i0, i1;
{ double piv;
  int i, l, r, z;

  if (i1<=i0) return;
  piv = med3(x[ind[i0]],x[ind[i1]],x[ind[(i0+i1)/2]]);
  l = i0; r = i0-1;

/* at each stage,
 * x[i0..l-1] < piv
 * x[l..r] = piv
 * x[r+1..i-1] > piv
 * then, decide where to put x[i].
 */
  for (i=i0; i<=i1; i++)
  { if (x[ind[i]]==piv)
    { r++;
      z = ind[i]; ind[i] = ind[r]; ind[r] = z;
    } 
    else if (x[ind[i]]<piv)
    { r++;
      z = ind[i]; ind[i] = ind[r]; ind[r] = ind[l]; ind[l] = z;
      l++;
    }
  }

  if (l>i0) mut_order(x,ind,i0,l-1);
  if (r<i1) mut_order(x,ind,r+1,i1);
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
#include "mut.h"

#define LOG_2           0.6931471805599453094172321214581765680755
#define IBETA_LARGE     1.0e30
#define IBETA_SMALL     1.0e-30
#define IGAMMA_LARGE    1.0e30
#define DOUBLE_EP     2.2204460492503131E-16

double ibeta(x, a, b)
double x, a, b;
{ int flipped = 0, i, k, count;
  double I = 0, temp, pn[6], ak, bk, next, prev, factor, val;
  if (x <= 0) return(0);
  if (x >= 1) return(1);
/* use ibeta(x,a,b) = 1-ibeta(1-x,b,z) */
  if ((a+b+1)*x > (a+1))
  { flipped = 1;
    temp = a;
    a = b;
    b = temp;
    x = 1 - x;
  }
  pn[0] = 0.0;
  pn[2] = pn[3] = pn[1] = 1.0;
  count = 1;
  val = x/(1.0-x);
  bk = 1.0;
  next = 1.0;
  do
  { count++;
    k = count/2;
    prev = next;
    if (count%2 == 0)
      ak = -((a+k-1.0)*(b-k)*val)/((a+2.0*k-2.0)*(a+2.0*k-1.0));
    else
      ak = ((a+b+k-1.0)*k*val)/((a+2.0*k)*(a+2.0*k-1.0));
    pn[4] = bk*pn[2] + ak*pn[0];
    pn[5] = bk*pn[3] + ak*pn[1];
    next = pn[4] / pn[5];
    for (i=0; i<=3; i++)
      pn[i] = pn[i+2];
    if (fabs(pn[4]) >= IBETA_LARGE)
      for (i=0; i<=3; i++)
        pn[i] /= IBETA_LARGE;
    if (fabs(pn[4]) <= IBETA_SMALL)
      for (i=0; i<=3; i++)
        pn[i] /= IBETA_SMALL;
  } while (fabs(next-prev) > DOUBLE_EP*prev);
  /* factor = a*log(x) + (b-1)*log(1-x);
  factor -= mut_lgamma(a+1) + mut_lgamma(b) - mut_lgamma(a+b); */
  factor = dbeta(x,a,b,1) + log(x/a);
  I = exp(factor) * next;
  return(flipped ? 1-I : I);
}

/*
 * Incomplete gamma function.
 * int_0^x u^{df-1} e^{-u} du / Gamma(df).
 */
double igamma(x, df)
double x, df;
{ double factor, term, gintegral, pn[6], rn, ak, bk;
  int i, count, k;
  if (x <= 0.0) return(0.0);

  if (df < 1.0)
    return( dgamma(x,df+1.0,1.0,0) + igamma(x,df+1.0) );

  factor = x * dgamma(x,df,1.0,0);
  /* factor = exp(df*log(x) - x - lgamma(df)); */

  if (x > 1.0 && x >= df)
  {
    pn[0] = 0.0;
    pn[2] = pn[1] = 1.0;
    pn[3] = x;
    count = 1;
    rn = 1.0 / x;
    do
    { count++;
      k = count / 2;
      gintegral = rn;
      if (count%2 == 0)
      { bk = 1.0;
        ak = (double)k - df;
      } else
      { bk = x;
        ak = (double)k;
      }
      pn[4] = bk*pn[2] + ak*pn[0];
      pn[5] = bk*pn[3] + ak*pn[1];
      rn = pn[4] / pn[5];
      for (i=0; i<4; i++)
        pn[i] = pn[i+2];
      if (pn[4] > IGAMMA_LARGE)
        for (i=0; i<4; i++)
          pn[i] /= IGAMMA_LARGE;
    } while (fabs(gintegral-rn) > DOUBLE_EP*rn);
    gintegral = 1.0 - factor*rn;
  }
  else
  { /*   For x<df, use the series
     *   dpois(df,x)*( 1 + x/(df+1) + x^2/((df+1)(df+2)) + ... )
     *   This could be slow if df large and x/df is close to 1.
     */
    gintegral = term = 1.0;
    rn = df;
    do
    { rn += 1.0;
      term *= x/rn;
      gintegral += term;
    } while (term > DOUBLE_EP*gintegral);
    gintegral *= factor/df;
  }
  return(gintegral);
}

double pf(q, df1, df2)
double q, df1, df2;
{ return(ibeta(q*df1/(df2+q*df1), df1/2, df2/2));
}
/*
 * Copyright 1996-2006 Catherine Loader.
 */
#include "mut.h"
#include <string.h>

/* quadmin: minimize the quadratic,
 *   2<x,b> + x^T A x.
 *   x = -A^{-1} b.
 *
 * conquadmin: min. subject to L'x = d   (m constraints)
 *   x = -A^{-1}(b+Ly)  (y = Lagrange multiplier)
 *   y = -(L'A^{-1}L)^{-1} (L'A^{-1}b)
 *   x = -A^{-1}b + A^{-1}L (L'A^{-1}L)^{-1} [(L'A^{-1})b + d]
 * (non-zero d to be added!!)
 *
 * qprogmin: min. subject to L'x >= 0.
 */

void quadmin(J,b,p)
jacobian *J;
double *b;
int p;
{ int i;
  jacob_dec(J,JAC_CHOL);
  i = jacob_solve(J,b);
  if (i<p) mut_printf("quadmin singular %2d %2d\n",i,p);
  for (i=0; i<p; i++) b[i] = -b[i];
}

/* project vector a (length n) onto
 * columns of X (n rows, m columns, organized by column).
 * store result in y.
 */
#define pmaxm 10
#define pmaxn 100
void project(a,X,y,n,m)
double *a, *X, *y;
int n, m;
{ double xta[pmaxm], R[pmaxn*pmaxm];
  int i;

  if (n>pmaxn) mut_printf("project: n too large\n");
  if (m>pmaxm) mut_printf("project: m too large\n");

  for (i=0; i<m; i++) xta[i] = innerprod(a,&X[i*n],n);
  memcpy(R,X,m*n*sizeof(double));
  qr(R,n,m,NULL);
  qrsolv(R,xta,n,m);

  matrixmultiply(X,xta,y,n,m,1);
}

void resproj(a,X,y,n,m)
double *a, *X, *y;
int n, m;
{ int i;
  project(a,X,y,n,m);
  for (i=0; i<n; i++) y[i] = a[i]-y[i];
}

/* x = -A^{-1}b + A^{-1}L (L'A^{-1}L)^{-1} [(L'A^{-1})b + d] */
void conquadmin(J,b,n,L,d,m)
jacobian *J;
double *b, *L, *d;
int m, n;
{ double bp[10], L0[100];
  int i, j;

  if (n>10) mut_printf("conquadmin: max. n is 10.\n");
  memcpy(L0,L,n*m*sizeof(double));
  jacob_dec(J,JAC_CHOL);
  for (i=0; i<m; i++) jacob_hsolve(J,&L[i*n]);
  jacob_hsolve(J,b);

  resproj(b,L,bp,n,m);

  jacob_isolve(J,bp);
  for (i=0; i<n; i++) b[i] = -bp[i];

  qr(L,n,m,NULL);
  qrsolv(L,d,n,m);
  for (i=0; i<n; i++)
  { bp[i] = 0;
    for (j=0; j<m; j++) bp[i] += L0[j*n+i]*d[j];
  }
  jacob_solve(J,bp);
  for (i=0; i<n; i++) b[i] += bp[i];
}

void qactivemin(J,b,n,L,d,m,ac)
jacobian *J;
double *b, *L, *d;
int m, n, *ac;
{ int i, nac;
  double M[100], dd[10];
  nac = 0;
  for (i=0; i<m; i++) if (ac[i]>0)
  { memcpy(&M[nac*n],&L[i*n],n*sizeof(double));
    dd[nac] = d[i];
    nac++;
  }
  conquadmin(J,b,n,M,dd,nac);
}

/* return 1 for full step; 0 if new constraint imposed. */
int movefrom(x0,x,n,L,d,m,ac)
double *x0, *x, *L, *d;
int n, m, *ac;
{ int i, imin;
  double c0, c1, lb, lmin;
  lmin = 1.0;
  for (i=0; i<m; i++) if (ac[i]==0)
  { c1 = innerprod(&L[i*n],x,n)-d[i];
    if (c1<0.0)
    { c0 = innerprod(&L[i*n],x0,n)-d[i];
      if (c0<0.0)
      { if (c1<c0) { lmin = 0.0; imin = 1; }
      }
      else
      { lb = c0/(c0-c1);
        if (lb<lmin) { lmin = lb; imin = i; }
      }
    }
  }
  for (i=0; i<n; i++)
    x0[i] = lmin*x[i]+(1-lmin)*x0[i];
  if (lmin==1.0) return(1);
  ac[imin] = 1;
  return(0);
}

int qstep(J,b,x0,n,L,d,m,ac,deac)
jacobian *J;
double *b, *x0, *L, *d;
int m, n, *ac, deac;
{ double x[10];
  int i;

  if (m>10) mut_printf("qstep: too many constraints.\n");
  if (deac)
  { for (i=0; i<m; i++) if (ac[i]==1)
    { ac[i] = 0;
      memcpy(x,b,n*sizeof(double));
      qactivemin(J,x,n,L,d,m,ac);
      if (innerprod(&L[i*n],x,n)>d[i]) /* deactivate this constraint; should rem. */
        i = m+10;
      else
        ac[i] = 1;
    }
    if (i==m) return(0); /* no deactivation possible */
  }

  do
  { if (!deac)
    { memcpy(x,b,n*sizeof(double));
      qactivemin(J,x,n,L,d,m,ac);
    }
    i = movefrom(x0,x,n,L,d,m,ac);

    deac = 0;
  } while (i==0);
  return(1);
}

/*
 * x0 is starting value; should satisfy constraints.
 * L is n*m constraint matrix.
 * ac is active constraint vector:
 *   ac[i]=0, inactive.
 *   ac[i]=1, active, but can be deactivated.
 *   ac[i]=2, active, cannot be deactivated.
 */

void qprogmin(J,b,x0,n,L,d,m,ac)
jacobian *J;
double *b, *x0, *L, *d;
int m, n, *ac;
{ int i;
  for (i=0; i<m; i++) if (ac[i]==0)
  { if (innerprod(&L[i*n],x0,n) < d[i]) ac[i] = 1; }
  jacob_dec(J,JAC_CHOL);
  qstep(J,b,x0,n,L,d,m,ac,0);
  while (qstep(J,b,x0,n,L,d,m,ac,1));
}

void qpm(A,b,x0,n,L,d,m,ac)
double *A, *b, *x0, *L, *d;
int *n, *m, *ac;
{ jacobian J;
  double wk[1000];
  jac_alloc(&J,*n,wk);
  memcpy(J.Z,A,(*n)*(*n)*sizeof(double));
  J.p = *n;
  J.st = JAC_RAW;
  qprogmin(&J,b,x0,*n,L,d,*m,ac);
}