#include "libscl.h"

using namespace scl;
using namespace std;

int main(int argc, char** argp, char** envp)
{
  
  cout << '\n';

  const REAL root_two_pi = sqrt(8.0*atan(1.0));

  REAL pi = 4.0*atan(1.0);

  REAL rtp = sqrt(2.0*pi);

  cout << '\t'<<'\t' << "root_two_pi = " << fmt('f',27,17,rtp) << '\n';
  cout << '\t'<<'\t' << "root_two_pi = " << fmt('f',27,17,root_two_pi) << '\n';

  realmat X(3,1);
  X[1] = 1;
  X[2] = 2;
  X[3] = 3;

  realmat Mu(3,1,1.0);

  realmat Sigma(3,3,0.1);
  Sigma(1,1) = 2.0;
  Sigma(2,2) = 3.0;
  Sigma(3,3) = 4.0;

  realmat Rsig;
  cholesky(Sigma,Rsig);

  cout << '\n';
  cout << '\t'<<'\t' << "Chlesky factor of Sigma" << Rsig;

  REAL Peter_Rossi_dMvn_ans = 0.006763676;

  denval ans = mvn(X,Mu,Sigma);

  cout << starbox("/Peter_Rossi_dMvn vs. mvn//");

  cout << '\n';
  cout << '\t'<<'\t' << Peter_Rossi_dMvn_ans <<' '<< exp(ans.log_den) << '\n';
  

  INTEGER m=4;
  INTEGER n=100;

  realmat sig(m,m,0.0);

  realmat mu(m,1,1.0);

  realmat x(m,1);

  INT_32BIT seed = 100542;
  for (INTEGER t=1; t<=n; ++t) {
    for (INTEGER i=1; i<=m; ++i) x[i]=unsk(seed);
    sig += x*T(x);
    x += mu;
  }

  sig = sig/n;

  cout << starbox("/x, mu, sig//");

  cout << x << mu << sig;

  cout << '\n';

  denval dv = mvn(x,mu,sig);

  cout << '\t' << '\t' << "dv.log_den = " << dv.log_den << '\n';;

  cout << starbox("/Intermediate mvn computations//");

  cout << '\n';

  const REAL tolerance = 16.0*EPS;

  INTEGER M = mu.size();

  realmat R;

  INTEGER rank = cholesky(sig,R,tolerance);

  cout << '\t' << '\t' << "R = " << R << '\n';

  cout << '\t' << '\t' << "rank = " << rank << '\n';

  realmat Rinv;

  rinv(R,Rinv);

  cout << '\n';

  cout << '\t' << '\t' << "Rinv = " << Rinv << '\n';

  REAL log_det_R = logdetR(R,tolerance);

  cout << '\t' << '\t' << "log_det_R = " << log_det_R << '\n';

  REAL logscale = -REAL(M)*log(root_two_pi) - log_det_R;

  cout << '\t' << '\t' << "logscale = " << logscale << '\n';

  cout << '\n';

  realmat y = x - mu;

  cout << '\t' << '\t' << "y = " << y << '\n';

  cout << '\n';

  REAL q = 0.0;
  for (INTEGER j=1; j<=M; ++j) {
    REAL sum = 0.0;
    for (INTEGER i=1; i<=j; ++i) {
      sum += y[i]*Rinv(i,j);
    }
    cout << '\t' << '\t' << "sum = " << sum << '\n';
    q += pow(sum,2);
  }

  cout << '\n';

  cout << '\t' << '\t' << "q = " << q << '\n';

  REAL log_den = logscale - 0.5*q;

  cout << '\t' << '\t' << "log_den = " << log_den << '\n';

  return 0;
}
