#include "snpden.h"

using namespace std;
using namespace scl;

int main()
{
  INTEGER n=25;
  static INT_32BIT ix = 01523467; 
  realmat X(n,1);

  INTEGER i,j;

  for (i=1; i<=n; i++) X[i]=unsk(&ix);

  REAL e0 = 1.0e-5;

  INTEGER K=10;  INTEGER p=K+3;
  realmat theta(p,1);

  theta[1] = 0.0;
  theta[2] = 1.0;
  double norm=0.0;
  if (n <= 25 ) {
    for (i=3; i<=p; i++) {
      theta[i]=1.0/pow((double)(i-2),2);
      norm += pow((double)theta[i],2);
    }
  }
  else {
    for (i=3; i<=p; i++) {
      theta[i]=0.0;
    }
    theta[3]=1.0;
    norm=1.0;
  }

  norm -= e0;
  norm = sqrt(norm);
  for (i=3; i<=p; i++) theta[i] = theta[i]/(REAL)norm;

  cout << starbox("/theta//") << theta;

  cout << starbox("/l/gradient/hessian//");
  realmat gradient;  realmat hessian;
  REAL l = snplgh(X,theta,e0,gradient,hessian);
  dgmpnt(cout,&l,1,1) << gradient <<hessian;

  if (n > 25 ) exit(0);

  char prtstr[LINESIZE];  REAL estimate;

  cout << starbox("/Check gradient//");
  sprintf(prtstr,"%20s %20s %20s","i","gradient","estimate");
  cout << prtstr << '\n';
  REAL eps=1.0e-5;  REAL lplus, tsave;
  for (i=1; i<=p; i++) {
    tsave = theta.check(i);
    theta.check(i) += eps;
    lplus = snpl(X,theta,e0);
    theta.check(i) = tsave;
    l = snplgh(X,theta,e0,gradient,hessian);
    estimate = (lplus-l)/eps;
    sprintf(prtstr,"%20d %20.10f %20.10f",i,gradient[i],estimate);
    cout << prtstr << '\n';
  }


  cout << starbox("/Check hessian//");
  sprintf(prtstr,"%15s %5s %20s %20s","i","j","hessian","estimate");
  cout << prtstr << '\n';
  REAL g,gplus;  
  for (j=1; j<=p; j++) {
    for (i=j; i<=p; i++) {
      tsave = theta.check(i);
      theta.check(i) += eps;
      lplus = snplgh(X,theta,e0,gradient,hessian);
      gplus = gradient[j];
      theta.check(i) = tsave;
      l = snplgh(X,theta,e0,gradient,hessian);
      g = gradient[j];
      estimate = (gplus-g)/eps;
      sprintf(prtstr,"%15d %5d %20.10f %20.10f",i,j,hessian(i,j),estimate);
      cout << prtstr << '\n';
    }
  }

  realmat A;  realmat B;
 
  cout << snpl(X,theta)-snpl(X,theta,0.0) << '\n';

  snplgh(X,theta,0.0,gradient,hessian);
  snplgh(X,theta,B,A);

  cout << gradient-B << hessian-A;

  realmat delta(p, 1, 0.1);
  realmat sc0(1,1);
  realmat sc(1,1);
  realmat tmp;

  sc0[1] = snplgh(X,theta,0.5,gradient,hessian);
  sc = sc0 + T(gradient)*delta + 0.5*T(delta)*hessian*delta;
  tmp = theta+delta;
  l = snplgh(X,tmp,0.5,gradient,hessian);
  cout << "\n\n" << l << "     " << sc[1] << "\n\n";

  sc0[1] = snplgh(X,theta,gradient,hessian);
  sc = sc0 + T(gradient)*delta + 0.5*T(delta)*hessian*delta;
  tmp = theta+delta;
  l = snplgh(X,tmp,gradient,hessian);
  cout << "\n\n" << l << "     " << sc[1] << "\n\n";

  cout << starbox("/Tests of snplgi//_");

  realmat info;

  snplgi(X,theta,0.0,gradient,info);
  cout << info;
  cout << gradient-B;
  snplgi(X,theta,B,A);
  cout << gradient-B << info-A;

  cout << starbox("/Tests of snplg//_");

  snplg(X,theta,0.0,gradient);
  cout << gradient-B;
  snplg(X,theta,B);
  cout << gradient-B;

  n=10000;
  X.resize(n,1);

  fill(theta);
  theta[1]=0.0;
  theta[2]=1.0;
  theta[3]=1.0;

  for (i=1; i<=n; i++) X[i]=unsk(&ix);

  snplgi(X,theta,0.0,gradient,info);
  snplgh(X,theta,0.0,gradient,hessian);

  cout << starbox("/gradient//_") << gradient;
  cout << starbox("/info - hessian//_") << info - hessian;
  cout << starbox("/info//_") << info;
  cout << starbox("/hessian//_") << hessian;

  cout << starbox("/Tests of snplgirmt//_");

  n=25;
  X.resize(n,1);

  for (i=1; i<=n; i++) X[i]=unsk(&ix);

  e0 = 1.0e-5;

  K=10;  
  p=K+3;

  theta.resize(p,1);
  gradient.resize(1,1);
  info.resize(1,1);

  theta[1] = 0.0;
  theta[2] = 1.0;
  norm=0.0;
  if (n <= 25 ) {
    for (i=3; i<=p; i++) {
      theta[i]=1.0/pow((double)(i-2),2);
      norm += pow((double)theta[i],2);
    }
  }
  else {
    for (i=3; i<=p; i++) {
      theta[i]=0.0;
    }
    theta[3]=1.0;
    norm=1.0;
  }

  norm -= e0;
  norm = sqrt(norm);
  for (i=3; i<=p; i++) theta[i] = theta[i]/(REAL)norm;

  realmat Xt(n-1,1);

  INTEGER t=5;

  for (i=1,j=0; i<=n; i++) {
    if (i != t) {
      j++;
      Xt[j]=X[i];
    }
  }

  l=snplgi(Xt,theta,e0,gradient,info);

  cout << '\n' << "l = " << l << '\n';

  REAL    ltmp=l;
  realmat gtmp=gradient;
  realmat itmp=info;

  REAL scale = ((REAL)(n-1))/((REAL)n);
  ltmp = scale*ltmp;
  gtmp = scale*gtmp;
  itmp = scale*itmp;
   
  l=snplgi(X,theta,e0,gradient,info);
  cout << '\n' << "l = " << l << '\n';
  l=snplgirmt(X,theta,e0,l,gradient,info,t);
  cout << '\n' << "l = " << l << '\n';

  cout << starbox("/l-ltmp/gradient-gtmp/info-itmp//");
  REAL ldiff=l-ltmp;
  dgmpnt(cout,&ldiff,1,1) << gradient-gtmp << info-itmp;



  n=25;
  K=10;  
  p=K+3;

  theta[1] = 0.0;
  theta[2] = 1.0;
  norm=0.0;
  if (n <= 25 ) {
    for (i=3; i<=p; i++) {
      theta[i]=1.0/pow((double)(i-2),2);
      norm += pow((double)theta[i],2);
    }
  }
  else {
    for (i=3; i<=p; i++) {
      theta[i]=0.0;
    }
    theta[3]=1.0;
    norm=1.0;
  }

  norm = sqrt(norm);
  for (i=3; i<=p; i++) theta[i] = theta[i]/(REAL)norm;

  t=n;

  for (i=1,j=0; i<=n; i++) {
    if (i != t) {
      j++;
      Xt[j]=X[i];
    }
  }

  l=snplgi(Xt,theta,gradient,info);

  cout << '\n' << "l = " << l << '\n';

  ltmp=l;
  gtmp=gradient;
  itmp=info;

  scale = ((REAL)(n-1))/((REAL)n);
  ltmp = scale*ltmp;
  gtmp = scale*gtmp;
  itmp = scale*itmp;
   
  l=snplgi(X,theta,gradient,info);
  cout << '\n' << "l = " << l << '\n';
  l=snplgirmt(X,theta,l,gradient,info,t);
  cout << '\n' << "l = " << l << '\n';

  cout << starbox("/l-ltmp/gradient-gtmp/info-itmp//");
  ldiff=l-ltmp;
  dgmpnt(cout,&ldiff,1,1) << gradient-gtmp << info-itmp;

  exit(0);
}








