/* ----------------------------------------------------------------------------

snpden.cc

Implements the Gallant-Nychka SNP density estimator for univariate, iid data.
Self documented, execute snpden with no arguments for documentation.

Reference: Gallant, A. Ronald, and Douglas W. Nychka (1987), "Seminonparametric 
Maximum Likelihood Estimation," Econometrica 55, 363-399.

Copyright (C) 1993, 1994, 1999, 2001, 2002, 2006.

A. Ronald Gallant
Post Office Box 659 
Chapel Hill NC 27514-0659 
USA   

Permission to use, copy, modify, and distribute this software and its docu-
mentation for any purpose and without fee is hereby granted, provided that 
the above copyright notice appears in all copies and that both that copyright 
notice and this permission notice appear in supporting documentation.  This 
software is provided "as is" without any expressed or implied warranty.

-----------------------------------------------------------------------------*/

#include "snpden.h"
using namespace std;
using namespace scl;

char    *progname;

void    usage_msg(void);
void    process_file(istream& cfile);
void    fit(ostream& scrdata);
void    estimate(void);
void    pred(void);
REAL    cross_validate(void);
REAL    normal(REAL z, int isw);
int     improve(realmat& theta0, REAL& logL0);
void    constrain(realmat& theta, REAL& mu, REAL& sigma);

INTEGER max_sample_size=1024;   // Can be changed with flag -m
INTEGER grid=100;               // Can be changed with flag -g
INTEGER trialmax = 5;           // Can be changed with flag -t
INTEGER itermax = 60;           // Can be changed with flag -i
REAL    scl0 = 1.e-5;           // Can be changed with flag -s
REAL    eps = 1.e-5;            // Can be changed with flag -e
REAL    tau = 1.e-5;
INTEGER Kmax;                   // Set with flag -Kmax, defaults otherwise.
REAL    xmin;                   // Set with flag -xmin, defaults otherwise.
REAL    xmax;                   // Set with flag -xmax, defaults otherwise.
char*   in = 0;                 // Set with flag -in, defaults to none.
char*   out = 0;                // Set with flag -out, defaults to none.
char*   scr = 0;                // Set with flag -scr, defaults to none.
char*   hos = 0;                // Set with flag -hos, defaults to none.
int     predict = 0;            // The flag -p sets to true, estimate otw.
int     normal_plot = 0;        // The flag -n sets to true.
int     match_moments = 0;      // The flag -d sets to true.
int     filter = 0;             // The flag -f sets to true.
int     use_aic = 0;            // The flag -aic sets to true.
int     use_bic = 0;            // The flag -bic sets to true.
int     use_cv = 0;             // The flag -bic sets to true.
int     use_rule = 0;           // The flag -aic, -bic, or -cv sets to true.
int     eformat = 0;            // The flag -ef sets to true.
int     compute_xmax  = 1;              
int     compute_xmin  = 1;
int     compute_Kmax  = 1;
int     compute_scl   = 1;

void usage_msg()
{
  cerr <<'\n'
       <<"usage: " <<progname<< " [options] filename\n"
       <<"options: -p             predict at data points only\n"
       <<"         -f             acts as a filter, takes standard input\n"
       <<"         -n             normal density points instead of SNP\n"
       <<"         -d             match mean and variance of starts to data\n"
       <<"         -cv,-aic,-bic  select best cross validated, aic, or bic fit\n"
       <<"         -ef            output in e format with full precision\n"
       <<"         -Kmax  value   largest degree to fit\n" 
       <<"         -t     value   number of random starts\n" 
       <<"         -i     value   number of iterations on final polish\n" 
       <<"         -s     value   scale factor for random starts\n" 
       <<"         -e     value   convergence tolerance\n" 
       <<"         -m     value   maximum sample size \n" 
       <<"         -g     value   number of plot points sent to cout\n"
       <<"         -xmin  value   minimum plot point\n"
       <<"         -xmax  value   maximum plot point\n"
       <<"         -in    name    read starting K and coefficients from file name\n"
       <<"         -out   name    write final K and coefficients to file name\n"
       <<"         -scr   name    write fit data to file name, not to screen\n"
       <<"         -hos   name    cross validate using hold out sample file name\n"
       <<"example: "<<progname<<" -g 200 in.dat > out.dat\n"
       <<"         The input file contains x values, one per line. The output\n"
       <<"         file contains y x pairs, one pair per line, y first.\n";
}

realmat X;
realmat Xraw;
realmat Xhos;
REAL    xmean;
REAL    xsigma;

realmat theta;  
realmat gradient;
realmat hessian;
REAL    logL;

REAL const zero = 0.0;
REAL const half = 0.5;
REAL const one  = 1.0;
REAL const two  = 2.0;
REAL const four = 4.0;
REAL const ten  = 10.0;

int main(int argc, char * argp[], char * envp[])
{
  int      argx, flagx, filex;
  char     ch, flag;

  ch = envp[0][0]; ch++; // This supresses a compiler warning.

  progname = argp[0];

  // begin: loop over flags and input files.

  argx = 0;  filex = 1;  

  while (argx < argc) {

    if (argx==0) argx++;

    // begin: read flags, if any.

    while (argx < argc && (argp[argx])[0]=='-') {

      // Read flags, if any.  Note that flags can occur singly as -a -b 
      // or they can be grouped as -ab.  Just insert code within the
      // appropriate case and don't worry about it.

      for (flagx=1; (flag=(argp[argx])[flagx])!=0; flagx++) {

        // If a flag can have more than one letter, deal with it within
        // the appropriate case by stripping off letters and incrementing
        // flagx.

        // If a flag indicates that argp[argx+1] is a file that should be read 
        // then, within the appropriate case, open arg[argx+1] or otherwise 
        // deal with the file name arg[argx+1], assign argp[argx] to 
        // argp[argx+1], and increment argx.  

        // If a flag indicates that argp[argx+1] is a string containing the  
        // value of a parameter, etc., then, within the appropriate case, deal 
        // with the string arg[argx+1], assign argp[argx] to argp[argx+1], and 
        // increment argx.  

        switch (flag) {

        case 'p' :
          predict = 1;
          break;

        case 'a' :
          if (memcmp(&(argp[argx][flagx]),"aic",3)==0) {
            flagx += 2;
            use_aic = 1;
            use_bic = 0;
            use_cv = 0;
            use_rule = 1;
            break;
          }
          goto badflag;

        case 'b' :
          if (memcmp(&(argp[argx][flagx]),"bic",3)==0) {
            flagx += 2;
            use_aic = 0;
            use_bic = 1;
            use_cv = 0;
            use_rule = 1;
            break;
          }
          goto badflag;

        case 'c' :
          if (memcmp(&(argp[argx][flagx]),"cv",2)==0) {
            flagx += 1;
            use_aic = 0;
            use_bic = 0;
            use_cv = 1;
            use_rule = 1;
            break;
          }
          goto badflag;

        case 'f' :
          filter = 1;
          break;

        case 'n' :
          normal_plot = 1;
          break;

        case 'd' :
          match_moments = 1;
          break;

        case 'm' :
          max_sample_size = (INTEGER) atoi(argp[argx+1]); 
          max_sample_size++;
          argp[argx+1] = argp[argx];
          argx++;
          break;

        case 't' :
          trialmax = (INTEGER) atoi(argp[argx+1]); 
          argp[argx+1] = argp[argx];
          argx++;
          break;

        case 's' :
          if (memcmp(&(argp[argx][flagx]),"scr",3)==0) {
            flagx += 2;
            scr = argp[argx+1]; 
            argp[argx+1] = argp[argx];
            argx++;
            break;
          }
          else {
            scl0 = (REAL) atof(argp[argx+1]); 
            argp[argx+1] = argp[argx];
            argx++;
            compute_scl = 0;
            break;
          }

        case 'e' :
          if (memcmp(&(argp[argx][flagx]),"ef",2)==0) { 
            flagx += 1;
            eformat = 1;
            cerr.setf(ios::scientific,ios::floatfield); 
            cerr.precision(17); 
            break;
          }
          else {
            eps = (REAL) atof(argp[argx+1]); 
            argp[argx+1] = argp[argx];
            argx++;
            break;
          }

        case 'g' :
          grid = (INTEGER) atoi(argp[argx+1]); 
          argp[argx+1] = argp[argx];
          argx++;
          break;

        case 'x' : 
          if (memcmp(&(argp[argx][flagx]),"xmin",4)==0) {
            flagx += 3;
            xmin = atof(argp[argx+1]); 
            argp[argx+1] = argp[argx];
            compute_xmin = 0;
            argx++;
            break;
          }
          if (memcmp(&(argp[argx][flagx]),"xmax",4)==0) {
            flagx += 3;
            xmax = atof(argp[argx+1]); 
            argp[argx+1] = argp[argx];
            compute_xmax = 0;
            argx++;
            break;
          }
          goto badflag;

        case 'K' : 
          if (memcmp(&(argp[argx][flagx]),"Kmax",4)==0) {
            flagx += 3;
            Kmax = (INTEGER) atoi(argp[argx+1]); 
            argp[argx+1] = argp[argx];
            compute_Kmax = 0;
            argx++;
            break;
          }
          goto badflag;

        case 'i' : 
          if (memcmp(&(argp[argx][flagx]),"in",2)==0) {
            flagx += 1;
            in = argp[argx+1]; 
            argp[argx+1] = argp[argx];
            argx++;
            break;
          }
          else {
            itermax = (INTEGER) atoi(argp[argx+1]); 
            argp[argx+1] = argp[argx];
            argx++;
            break;
          }

        case 'o' : 
          if (memcmp(&(argp[argx][flagx]),"out",3)==0) {
            flagx += 2;
            out = argp[argx+1]; 
            argp[argx+1] = argp[argx];
            argx++;
            break;
          }
          goto badflag;

        case 'h' : 
          if (memcmp(&(argp[argx][flagx]),"hos",3)==0) {
            flagx += 2;
            hos = argp[argx+1]; 
            argp[argx+1] = argp[argx];
            argx++;
            break;
          }
          goto badflag;

        badflag :
        default :
          cerr << "Flag was ";
          while ((flag=(argp[argx])[flagx++])!=0) 
            cerr << flag;
          cerr << "\nError: unknown flag.\n";
          exit(1);
          break;

        } // end: switch

      } // end: for 

    argx++;

    } // end: while

    // end: read flags, if any.

    if (filter) {
      process_file(cin);
    }
    else {
      if (argx >= argc) {
        usage_msg();
        exit(1);
      }

      ifstream infile(argp[argx]);
      if (!infile) {
        cerr << "Error, "<< progname <<", Cannot open " << argp[argx] << "\n";
        exit(1);
      } 
      istream& from = infile;

      process_file(from);
      infile.close();
    }

    if (scr) {
      ofstream scrdata(scr);
      if (!scrdata) {
        cerr << "Error, " << progname << ", Cannot open " << scr << "\n";
        exit(1);
      } 
      if (eformat) {
        scrdata.setf(ios::scientific,ios::floatfield); 
        scrdata.precision(17);
      }
      if (!normal_plot) fit(scrdata);
    }
    else {
      if (!normal_plot) fit(cerr);
    }

    if (predict) pred();
    else estimate();
   

    filex++;
    argx++;
  } 

  // end: loop over flags and input files.

  exit(0);

}

void process_file(istream& from)
{ 
  REAL * x;

  INTEGER n = 0;

  REAL * Xinput = new REAL[max_sample_size];
  if (Xinput == 0) {cerr << "Error, snpden, operator new failed\n"; exit(1);}

  REAL * top = Xinput;
  while( (from>>*top) && n < max_sample_size ) { 
    top++; n++;
  }

  if ( n == max_sample_size ) {
    cerr << "\nError: The maximum sample size = " 
         << max_sample_size 
         << " is too small.\n"
         << "       Give a larger value with the -m flag.\n\n";
    exit(1);
  }

  INTEGER i; 

  heapsort(Xinput,n);

  Xraw.resize(n,1);  

  //Xraw is a misnomer now that the heapsort call preceeds this copy
  for (i=1, x=Xinput; i<=n; i++, x++) Xraw[i]=*x;

  if (compute_xmin) xmin=*Xinput;
  if (compute_xmax) xmax=*(Xinput+n-1);

  for (x=Xinput, xmean=0.0; x < top; x++) {
    xmean += *x;
  }
  xmean /= (REAL)n;

  for (x=Xinput, xsigma=0.0; x < top; x++) {
    xsigma += (*x-xmean)*(*x-xmean);
  }
  xsigma = sqrt(xsigma/(REAL)n);  


  X.resize(n,1); 
  for (i=1; i<=n; i++)  X[i] = (Xraw[i]-xmean)/xsigma;
 
  if (hos) {
    
    ifstream hosfile(hos);
    if (!hosfile) {
      cerr << "Error, " << progname << ", Cannot open " << hos << "\n";
      exit(1);
    }

    INTEGER nhos = 0;

    top = Xinput;
    while( (hosfile>>*top) && nhos < max_sample_size ) {
      top++; nhos++;
    }

    if ( nhos == max_sample_size ) {
      cerr << "\nError: The maximum sample size = " 
           << max_sample_size 
           << " is too small.\n"
           << "       Give a larger value with the -m flag.\n\n";
      exit(1);
    }

    Xhos.resize(nhos,1);
    for (i=1, x=Xinput; i<=nhos; i++, x++) Xhos[i]=(*x-xmean)/xsigma;

  }

  delete [] Xinput; 
}

void estimate(void)
{
  REAL     yplt, xplt;
  REAL     u;
  REAL     xinc;
  INTEGER  j;


  xinc = (xmax - xmin) / ( (REAL) (grid-1) );

  if (normal_plot) {
    char str[80];

    for (j=0; j<grid; j++) {
      xplt = xmin + xinc*j;
      
      u = (xplt-xmean)/xsigma;

      yplt = normal(u,1)/xsigma;

      if (eformat) { 
        sprintf(str," %27.17e %27.17e\n",yplt,xplt);
        cout << str;
      }
      else {
        cout << yplt << ' ' << xplt << '\n';
      }
    }
  }
  else  {
    char str[80]; 

    INTEGER p = theta.get_rows();
    INTEGER K = p - 3;
    REAL    mu = theta[1];
    REAL    S = theta[2];

    REAL*   c = theta.get_x()+2;
    REAL    s[3];
    REAL*   w = new REAL[K+1];
    if (w == 0) {cerr << "Error, snpden, operator new failed\n"; exit(1);}

    INTEGER i;
    REAL    f, g, h, z;
    REAL    sum, tmp;  

    for (j=0; j<grid; j++) {
      xplt = xmin + xinc*j;

      u = (xplt-xmean)/xsigma;

      z = (u - mu)/S;
      g = hermite(z,K,c,w,s);
      h = g*g;
      f = h/S;
      for (i=3, sum=zero; i<=p; i++) {tmp=theta[i]; sum += tmp*tmp;}
      f /= sum;

      yplt = f/xsigma;
      if (eformat) { 
        sprintf(str," %27.17e %27.17e\n",yplt,xplt); 
        cout << str;
      }
      else {
        cout << yplt << ' ' << xplt << '\n';
      }
    }

    delete [] w; 
  }
  

}

void pred(void)
{
  REAL     yplt;
  REAL     u;
  INTEGER  n = Xraw.get_rows();
  INTEGER  i;

  if (normal_plot) {
    char str[80];
    for (i=1; i<=n; i++) {
      u = (Xraw[i]-xmean)/xsigma;
      yplt = normal(u,1)/xsigma;
      if (eformat) { 
        sprintf(str," %27.17e %27.17e\n",yplt,Xraw[i]);
        cout << str;
      }
      else {
        cout << yplt << ' ' << Xraw[i] << '\n';
      }
    }
  }
  else {
    char str[80];

    INTEGER p = theta.get_rows();
    INTEGER K = p - 3;
    REAL    mu = theta[1];
    REAL    S = theta[2];

    REAL*   c = theta.get_x()+2;
    REAL    s[3];
    REAL*   w = new REAL[K+1];
    if (w == 0) {cerr << "Error, snpden, operator new failed\n"; exit(1);}

    INTEGER i,j;
    REAL    f, g, h, z;
    REAL    sum, tmp;  

    for (j=1; j<=n; j++) {

      u = (Xraw[j]-xmean)/xsigma;

      z = (u - mu)/S;
      g = hermite(z,K,c,w,s);
      h = g*g;
      f = h/S;
      for (i=3, sum=zero; i<=p; i++) {tmp=theta[i]; sum += tmp*tmp;}
      f /= sum;

      yplt = f/xsigma;

      if (eformat) {
        sprintf(str," %27.17e %27.17e\n",yplt,Xraw[j]); 
        cout << str; 
      }
      else {
        cout << yplt << ' ' << Xraw[j] << '\n';
      }

    }

    delete [] w; 

  }
}

REAL normal(REAL z, int isw)
// Returns the value of the normal kernel at z if isw=1 and its 
// convolution with itself if isw=2.
{
  REAL const two = 2.0;
  REAL const root_two_pi = 2.5066282746310005024;
  REAL const root_two    = 1.4142135623730950488;
  REAL       h;
  REAL       tmp;

  switch (isw) {

    case 1 :
     tmp = -z*z/two;
     h = exp( tmp );
     h /= root_two_pi;
     return h;

    case 2 :
     tmp = -z*z/two;
     tmp /= two;
     h = exp( tmp );
     h /= root_two_pi;
     h /= root_two;
     return h;

    default :
      cerr << "\nError: bad data to REAL normal(REAL z, int isw)\n";
      exit(1);
   }
   //These statements can't be reached but stop a compiler warning.
   cerr << "\nError: bad data to REAL normal(REAL z, int isw)\n";
   return 1;
}    


void fit(ostream& scrdata) 
{
// tuning parameters

  REAL alpha = 0.2;

  INTEGER startmax = 1;  if (compute_scl) startmax = 4;
  INTEGER itermax0 = 4;

  INT_32BIT seed = 5748;  INT_32BIT * ix = &seed;

// local 

  REAL aic, bic, cv;
  REAL rule, rule_best;
  REAL sum, scale, tmp;
  REAL logL0, logL_best;
  REAL mu, sigma;
  REAL rn = X.get_rows();

  INTEGER i;
  INTEGER iter;  
  INTEGER start;
  INTEGER trial;
  INTEGER K, p, p0;

  realmat theta0;
  realmat theta_save;
  realmat theta_best;
  realmat theta_rule;

  if (compute_Kmax) Kmax = (K=1+(INTEGER)pow(rn,alpha)) <= 4  ?  4 : K;

  if (in) {

    ifstream inparm(in);
    if (!inparm) {
      cerr << "Error, " << progname << ", Cannot open " << in << "\n";
      exit(1);
    } 

    inparm >> K;
    if (K < 1) {
      cerr << "Error, " << progname << ", K must be positive\n"; 
      exit(1);
    }
    p = K + 3;
    theta.resize(p,1);
    for (i=1; i<=p; i++) {
      if ( !(inparm >> theta[i]) ) {
        cerr << "Error, " << progname << ", Can't read " << in << "\n";
        exit(1);
      }
    }
    if (theta[2] <= zero) {
      cerr << "Error, " << progname << ", theta[2] must be positive\n"; 
      exit(1);
    }

    logL = snpl(X,theta);
    for (i=3, sum=zero; i<=p; i++) {tmp=theta[i]; sum += tmp*tmp;}
    logL += log(sum)+log(xsigma);
    aic = logL + ((REAL)p/rn);
    bic = logL + ((REAL)p/rn)*log(rn)/two;

    if (use_aic) {
      rule_best = aic;
      scrdata << "K = " << K << "  obj = " << logL
            << "  aic = " << aic << "  bic = " << bic << '\n';
    }
    else if (use_cv) {
      cv = cross_validate();
      rule_best = cv;
      scrdata << "K = " << K << "  obj = " << logL
            << "  aic = " << aic << "  bic = " << bic
            << "  cv = " << cv <<'\n';
    }
    else {
      rule_best = bic;
      scrdata << "K = " << K << "  obj = " << logL
            << "  aic = " << aic << "  bic = " << bic << '\n';
    }
    theta_rule = theta;
    inparm.close();

  }
  else {
    K = 0;
    p = K + 3;
    theta.resize(p,1);
    theta[1] = zero;
    theta[2] = one;
    theta[3] = one;
    logL = snpl(X,theta);
    for (i=3, sum=zero; i<=p; i++) {tmp=theta[i]; sum += tmp*tmp;}
    logL += log(sum)+log(xsigma);
    aic = logL + ((REAL)p/rn);
    bic = logL + ((REAL)p/rn)*log(rn)/two;
    if (use_aic) {
      rule_best = aic;
      scrdata << "K = " << K << "  obj = " << logL
            << "  aic = " << aic << "  bic = " << bic << '\n';
    }
    else if (use_cv) {
      cv = cross_validate();
      rule_best = cv;
      scrdata << "K = " << K << "  obj = " << logL
            << "  aic = " << aic << "  bic = " << bic
            << "  cv = " << cv <<'\n';
    }
    else {
      rule_best = bic;
      scrdata << "K = " << K << "  obj = " << logL
            << "  aic = " << aic << "  bic = " << bic << '\n';
    }
    theta_rule = theta;
    K = 1;
  }

  do {

    p0 = p;
    p = K + 3;
    theta_save.resize(p,1,zero);
    for (i=1; i<=p0; i++) theta_save[i] = theta[i];
    logL_best = logL;  theta_best = theta_save;
    theta.resize(p,1);

    start = 0;  scale = scl0/ten;  
    do {

      trial = 0;  scale *= ten;
      do {

        theta[3] = one;
        for (i=4; i<=p; i++) {
          tmp = i-3; tmp *= tmp; tmp *= tmp; 
          theta[i] = theta_save[i] + scale*two*(ran(ix)-half)/tmp;
        }
        if ( (K <= 2) || (match_moments) )  {
          constrain(theta,mu,sigma);
          theta[1] = mu;
          theta[2] = sigma;
        }
        else {
          theta[1] = theta_save[1] + scale*two*(ran(ix)-half);
          theta[2] = theta_save[2] + scale*two*(ran(ix)-half);
        }

        iter = 0;  
        do {
          theta0 = theta;  
          if (!improve(theta0,logL0)) break;
        }
        while( iter++ < itermax0  &&  fabs(logL-logL0) > eps*fabs(logL0+tau) );

        if (logL_best > logL) { logL_best = logL;  theta_best = theta; }

      }
      while ( trial++ < trialmax );

    }
    while ( start++ < startmax );

    theta = theta_best;  
      
    p = theta.get_rows();  
    iter = 0;  
    do {
      theta0 = theta;  
      if (!improve(theta0,logL0)) break;
    }
    while( iter++ < itermax  &&  fabs(logL-logL0) > eps*fabs(logL0+tau) );
     
    aic = logL + ((REAL)p/rn);
    bic = logL + ((REAL)p/rn)*log(rn)/two;

    if (use_aic) {
      rule = aic;
      scrdata << "K = " << K << "  obj = " << logL
            << "  aic = " << aic << "  bic = " << bic << '\n';
    }
    else if (use_cv) {
      cv = cross_validate();
      rule = cv;
      scrdata << "K = " << K << "  obj = " << logL
            << "  aic = " << aic << "  bic = " << bic
            << "  cv = " << cv <<'\n';
    }
    else {
      rule = bic;
      scrdata << "K = " << K << "  obj = " << logL
            << "  aic = " << aic << "  bic = " << bic << '\n';
    }
      

    if (rule < rule_best) {rule_best = rule;  theta_rule = theta;}

  }
  while ( K++ < Kmax );

  if (use_rule) {
    theta = theta_rule;
    if (use_aic) {
      scrdata << "\nBest aic selected, K = " << theta.get_rows()-3 << '\n';
    }
    else if (use_cv) {
      scrdata << "\nBest cv selected, K = " << theta.get_rows()-3 << '\n';
    }
    else {
      scrdata << "\nBest bic selected, K = " << theta.get_rows()-3 << '\n';
    }
  }


  if (out) {
    ofstream outparm(out);
    if (!outparm) {
      cerr << "Error, " << progname << ", Cannot open " << out << "\n";
      exit(1);
    } 

    char str[80];

    p = theta.get_rows();
    outparm << p-3 << '\n';
    for (i=1; i<=p; i++) {
      sprintf(str," %27.17e\n",theta[i]);
      if ( !(outparm << str) ) {
        cerr << "Error, " << progname << ", Can't write " << out << "\n";
        exit(1);
      }
    }
    outparm.close();
  }
}

void constrain(realmat& theta, REAL& mu, REAL& sigma) 
{
  REAL C0, C1, C2;
  REAL tmp;
  INTEGER i;

  INTEGER K=theta.get_rows()-3;

  C0=C1=C2=0;

  i=0; 
  while (i <= K) {
    tmp = theta[3+i];
    tmp *= tmp;
    C0 += tmp;
    C2 += (1 + i + i*i)*tmp;
    i++;
  }

  i=0; 
  while (i <= K-1) {
    tmp = theta[3+i];
    tmp *= theta[4+i];
    C1 += two*sqrt((REAL)(i+1))*tmp;
    i++;
  }

  i=0; 
  while (i <= K-2) {
    tmp = theta[3+i];
    tmp *= theta[5+i];
    C2 += two*sqrt((REAL)(i+1))*sqrt((REAL)(i+2))*tmp;
    i++;
  }

  C1 /= C0;
  C2 /= C0;

  sigma = one/sqrt(C2 - C1*C1);
  mu = -sigma*C1;
}

int improve(realmat& theta0, REAL& logL0)
{
  INTEGER cutmax = 6;

  realmat delta;
  INTEGER cut;  
  INTEGER i,j;
  INTEGER p;
  REAL    sum,tmp,step;

  p = theta0.get_rows();
  logL0 = snplgi(X,theta0,delta,hessian);
  for (i=3, sum=zero; i<=p; i++) {tmp=theta0[i]; sum += tmp*tmp;}
  logL0 += log(sum)+log(xsigma);
  tmp = two/sum;
  for (i=3; i<=p; i++) {
    delta[i] += tmp*theta0[i];
    hessian(i,i) += tmp;
  }
  tmp = tmp*tmp;
  for (j=3; j<=p; j++)  {
  for (i=3; i<=p; i++)  {
    hessian(i,j) -= tmp*theta0[i]*theta0[j];
  }
  }
  for (i=1; i<=p; i++) {
    hessian(3,i) = zero;
    hessian(i,3) = zero;
  }
  delta[3] = zero;
  solve(hessian,delta);
  step = one;  cut = 0;
  do {
    do {
      theta = theta0 - step*delta;
      step /= two;
    }
    while( theta[2] <= zero );
    logL = snpl(X,theta);
    for (i=3, sum=zero; i<=p; i++) {tmp=theta[i]; sum += tmp*tmp;}
    logL += log(sum)+log(xsigma);
  }
  while( logL > logL0 && cut++ < cutmax );

  if (cut == cutmax) {
    theta = theta0; 
    logL = logL0; 
    return 0;
  }
  else {
    return 1;
  }
}

#ifdef WATCOM_WCL386_COMPILER 
int matherr(struct exception * err)
{
  static int logcount=0;

  if ( strcmp ( err->name, "log" ) == 0 ) {
    logcount++;
    switch (logcount) {
      case 1:
      case 2:
      case 3:
      case 4:
        cerr << "Error, log, argument was " << err->arg1 << "\n";
        return 1;
      case 5:
        cerr << "Error, log, argument was " << err->arg1 << ", ";
        cerr << "further errors will not be reported\n";
        return 1;
      default:
        return 1;
    }
  }
  return 0;
}
#endif

REAL cross_validate(void)
{

  REAL const two_pi = 6.283185307179587; 

  INTEGER p = theta.get_rows();
  INTEGER K = p - 3;
  INTEGER n = X.get_rows();

  INTEGER nquad = 2*K+1;
  realmat Xquad, Wquad; 
  
  if(hquad(nquad,Xquad,Wquad)) {cerr<<"Error, snpden, hquad failed\n"; exit(1);}

  REAL*   w = new REAL[K+1];
  if (w == 0) {cerr << "Error, snpden, operator new failed\n"; exit(1);}

  INTEGER i,j;

  REAL*   c;
  REAL    s[3];
  REAL    f, g, h, z, u, poly, p2, p4;
  REAL    sum, tmp;  
  REAL    mu;
  REAL    S;

  mu = theta[1];
  S =  theta[2];

  c = theta.get_x()+2;
  for (i=3, sum=zero; i<=p; i++) {tmp=theta[i]; sum += tmp*tmp;}

  REAL    cv=zero;
  REAL    factor = one/(two_pi*S);

  for (j=1; j<=nquad; j++) {

    z = Xquad[j];
    g = hermite(z,K,c,w,s);
    poly = g/w[0];
    p2 = poly*poly;
    p2 /= sum;
    p4 = p2*p2;
    u  = factor*p4*Wquad[j];
    cv += u;

  }

  REAL cv_save = cv;

  INTEGER t;

  REAL fn;
  fn = n;

  if (hos) {  // hold out sample cross validataion

    INTEGER nhos = Xhos.get_rows();
    fn = nhos;

    mu = theta[1];
    S =  theta[2];

    c = theta.get_x()+2;
    for (i=3, sum=zero; i<=p; i++) {tmp=theta[i]; sum += tmp*tmp;}

    cv = cv_save;

    for (t=1; t<=nhos; t++) {
      z = (Xhos[t]-mu)/S;
      g = hermite(z,K,c,w,s);
      h = g*g;
      f = h/S;
      f /= sum;
      cv -= two*f/fn;
    }

  }
  else {   // leave one out cross validation

    REAL    logL0, logL1, l_save, step;
    INTEGER cutmax = 6;
    INTEGER cut;  
    realmat theta0, theta1, gradient0, hessian0;
    realmat delta;

    l_save = snplgi(X,theta,gradient,hessian);

    cv = cv_save;

    for (t=1; t<=n; t++) {
  
      theta0 = theta;
      gradient0 = gradient;
      hessian0 = hessian;
      tmp = l_save;
      logL0 = snplgirmt(X,theta0,tmp,gradient0,hessian0,t);
      for (i=3, sum=zero; i<=p; i++) {tmp=theta0[i]; sum += tmp*tmp;}
      logL0 += log(sum)+log(xsigma);
  
      delta = gradient0;
  
      tmp = two/sum;
      for (i=3; i<=p; i++) {
        delta[i] += tmp*theta0[i];
        hessian0(i,i) += tmp;
      }
      tmp = tmp*tmp;
      for (j=3; j<=p; j++)  {
      for (i=3; i<=p; i++)  {
        hessian0(i,j) -= tmp*theta0[i]*theta0[j];
      }
      }
      for (i=1; i<=p; i++) {
        hessian0(3,i) = zero;
        hessian0(i,3) = zero;
      }
  
      delta[3] = zero;
      solve(hessian0,delta);
      step = one;  cut = 0;
      do {
        do {
          theta1 = theta0 - step*delta;
          step /= two;
        }
        while( theta1[2] <= zero );
  
        tmp = snpl(X,theta1);
        gradient0 = gradient;
        hessian0 = hessian;
        logL1 = snplgirmt(X,theta1,tmp,gradient0,hessian0,t);
        for (i=3, sum=zero; i<=p; i++) {tmp=theta1[i]; sum += tmp*tmp;}
        logL1 += log(sum)+log(xsigma);
      }
      while( logL1 > logL0 && cut++ < cutmax );
  
      if (cut == cutmax) {
        theta1 = theta0; 
        logL1 = logL0; 
      }
      
      logL1 = snpl(X,theta1);
      for (i=3, sum=zero; i<=p; i++) {tmp=theta1[i]; sum += tmp*tmp;}
      logL1 += log(sum)+log(xsigma);
  
      mu = theta1[1];
      S =  theta1[2];
  
      c = theta1.get_x()+2;
      z = (X[t]-mu)/S;
      g = hermite(z,K,c,w,s);
      h = g*g;
      f = h/S;
      for (i=3, sum=zero; i<=p; i++) {tmp=theta1[i]; sum += tmp*tmp;}
      f /= sum;
  
      cv -= two*f/fn;
  
    }
  }


  delete [] w; 

  cv /= xsigma;

  return cv;

}
