#include "libscl.h"

using namespace scl;
using namespace std;

int main(int argc, char** argp, char** envp)
{
  
  INTEGER m=3;
  INTEGER lA = (m*m+m)/2;
  
  realmat A(m,m,0.0);
  realmat J(lA,lA);

  INT_32BIT seed = 100542;
  for (INTEGER t=1; t<=100; ++t) {
    realmat x(m,1);
    for (INTEGER i=1; i<=m; ++i) x[i]=unsk(seed);
    A += x*T(x);
  }

  vecwrite("factor.dat",A);

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

  cout << A;

  cout << starbox("/after gen2sym//");

  gen2sym(A.get_x(),m);

  cout << A;
 
  realmat R(lA,1);
  for (INTEGER i=1; i<=lA; ++i) R[i]=A[i];

  factor(A.get_x(),J.get_x(),m);

  cout << starbox("/after factor//");
   
  cout << A << J;

  cout << starbox("/after sym2gen//");

  sym2gen(A.get_x(),m);

  cout << A;

  cout << starbox("/Test derivatives//");

  for (INTEGER i=1; i<=lA; ++i) {
    realmat Rsave = R;
    REAL delta = 1.e-05;
    R[i] += delta;
    factor(R.get_x(),J.get_x(),m);
    realmat del = R;
    R = Rsave;
    factor(R.get_x(),J.get_x(),m);
    del -= R;
    del = (1.0/delta)*del;
    del = cbind(del,J("",i));
    cout << "i = " << i << del << '\n';
  }

  INTEGER n=5;

  realmat Rinv(n,n,0.0);
  R.resize(n,n,0.0);

  for (INTEGER j=1; j<=n; ++j) {
    for (INTEGER i=1; i<=j; ++i) {
      R(i,j) = REAL(i + j);
    }
  }

  cout << starbox("/with gen2sym & sym2gen//");

  Rinv = R;

  cout << Rinv;

  gen2sym(Rinv.get_x(),n);

  cout << Rinv;

  drinv(Rinv.get_x(),n);

  cout << Rinv;

  sym2gen(Rinv.get_x(),n);

  for (INTEGER j=1; j<=n; ++j) {
    for (INTEGER i=1; i<j; ++i) {
      Rinv(j,i) = 0.0; 
    }
  }

  cout << Rinv;

  cout << starbox("/R, Rinv//") << R << Rinv << R*Rinv << Rinv*R;

  cout << starbox("/with gen2upr & upr2gen//");

  Rinv = R;

  cout << Rinv;

  gen2upr(Rinv.get_x(),n);

  cout << Rinv;

  drinv(Rinv.get_x(),n);

  cout << Rinv;

  upr2gen(Rinv.get_x(),n);

  cout << Rinv;

  cout << starbox("/R, Rinv//") << R << Rinv << R*Rinv << Rinv*R;

  { // This checks syntax at beginning of file cholesky.cpp

    INTEGER m=3;
  
    realmat A(m,m,0.0);

    INT_32BIT seed = 100542;
    for (INTEGER t=1; t<=100; ++t) {
     realmat x(m,1);
      for (INTEGER i=1; i<=m; ++i) x[i]=unsk(seed);
      A += x*T(x);
    }

    realmat S(m,m);
    // Fill S with values
    S = A;
    realmat R = S;
    gen2sym(R.begin(),m);
    factor(R.begin(),m);
    realmat Rinv = R;
    drinv(Rinv.begin(),m);
    upr2gen(R.begin(),m);
    upr2gen(Rinv.begin(),m);

    R = S;
    gen2sym(R.begin(),m);
    realmat J0((m*m+m)/2,(m*m+m)/2);
    factor(R.begin(),J0.begin(),m);
    realmat J(m*m,m*m,0.0);
    REAL* J0x = J0.begin();
    INTEGER lr0 = (m*m+m)/2;
    for (INTEGER j=1; j<=m; ++j) {
      for (INTEGER i=1; i<=j; ++i) {
        INTEGER ij0 = (j*j-j)/2+i;
        INTEGER ij  = m*(j-1)+i;
        for (INTEGER k=1; k<=m; ++k) {
          for (INTEGER l=1; l<=k; ++l) {
            INTEGER kl0 = (k*k-k)/2+l;
            INTEGER kl  = m*(k-1)+l;
          J(ij,kl) = *(J0x + lr0*(kl0-1)+ij0-1);
          }
        }
      }
    }

    cout << S << R*T(R) << invpsd(S) << T(Rinv)*Rinv;

    cout << J0 << J;

    // Fill S with values
    S = A;
    R = S;
    gen2sym(R.begin(),m);
    INTEGER ier =factor(R.begin(),m);
    Rinv = R;
    drinv(Rinv.begin(),m);
    upr2gen(R.begin(),m);
    upr2gen(Rinv.begin(),m);

    cout << S << R*T(R) << invpsd(S) << T(Rinv)*Rinv;
    cout << '\n';

    S = A;
    R = R;
    Rinv = Rinv;

    cout << ier << ' '; 

    ier = factor(A);

    cout << ier << R - A;

    drinv(A);

    cout << Rinv - A;

  }

  realmat I;

  cout << '\n';
  cout << "Test of factor" << '\n';
  seed = 100542;

  for (INTEGER m = 10; m>=3; --m) {
    lA = (m*m+m)/2;
    A.resize(m,m,0.0);
    J.resize(lA,lA);
    for (INTEGER t=1; t<=100; ++t) {
      realmat x(m,1);
      for (INTEGER i=1; i<=m; ++i) x[i]=unsk(seed);
      A += x*T(x);
    }
    R = A;
    INTEGER ier = factor(R);
    Rinv = R;
    drinv(Rinv);
    I = R*Rinv;
    REAL error = 0.0;
    for (INTEGER j=1; j<=m; ++j) {
      for (INTEGER i=1; i<=m; ++i) {
        if (i == j) error += fabs(I(i,j) - 1.0);
        else error += fabs(I(i,j));
      }
    }
    cout << ier << ' ' << error << '\n';
  }

  cout << '\n';
  cout << "Test of drinv" << '\n';

  for (INTEGER m = 10; m>=3; --m) {
    lA = (m*m+m)/2;
    A.resize(m,m,0.0);
    J.resize(lA,lA);
    for (INTEGER t=1; t<=100; ++t) {
      realmat x(m,1);
      for (INTEGER i=1; i<=m; ++i) x[i]=unsk(seed);
      A += x*T(x);
    }
    R = A;
    INTEGER ier = factor(R);
    Rinv = R;
    drinv(Rinv);
    I = Rinv*R;
    REAL error = 0.0;
    for (INTEGER j=1; j<=m; ++j) {
      for (INTEGER i=1; i<=m; ++i) {
        if (i == j) error += fabs(I(i,j) - 1.0);
        else error += fabs(I(i,j));
      }
    }
    cout << ier << ' ' << error << '\n';
  }

  cout << '\n';
  cout << "Full rank test of cholesky" << '\n';
  seed = 100542;

  for (INTEGER m = 10; m>=3; --m) {
    A.resize(m,m,0.0);
    for (INTEGER t=1; t<=100; ++t) {
      realmat x(m,1);
      for (INTEGER i=1; i<=m; ++i) x[i]=unsk(seed);
      A += x*T(x);
    }
    REAL ldR;
    INTEGER rankR = cholesky(A, R, EPS);
    ldR = logdetR(R,EPS);
    realmat RR = T(R)*R;
    REAL error = 0.0;
    for (INTEGER j=1; j<=m; ++j) {
      for (INTEGER i=1; i<=m; ++i) {
        error += fabs(RR(i,j) - A(i,j));
      }
    }
    cout << rankR <<' '<<  ldR <<' '<< error << '\n';
    realmat U,S,V;
    rankR = svd(R,U,S,V);
    ldR = 0.0;
    for (INTEGER i=1; i<=rankR; ++i) ldR += log(S[i]);
    cout << rankR <<' '<<  ldR << '\n';
  }

  cout << '\n';
  cout << "Less than full rank test of rinv" << '\n';
  seed = 100542;

  for (INTEGER m = 10; m>=3; --m) {
    A.resize(m,m,0.0);
    for (INTEGER t=1; t<=6; ++t) {
      realmat x(m,1);
      for (INTEGER i=1; i<=m; ++i) x[i]=unsk(seed);
      if (m == 9) x[2] = 0.0;
      if (m == 8) x[6] = 1.0;
      if (m == 8) x[8] = 1.0;
      A += x*T(x);
    }
    REAL ldR;
    //cout << A;
    INTEGER rankR = cholesky(A, R, EPS);
    ldR = logdetR(R,EPS);
    //cout << R;
    rinv(R,Rinv);
    //cout << Rinv;
    I = R*Rinv*R;
    //cout << I;
    REAL error = 0.0;
    for (INTEGER j=1; j<=m; ++j) {
      for (INTEGER i=1; i<=m; ++i) {
        error += fabs(I(i,j) - R(i,j));
      }
    }
    cout << rankR <<' '<<  ldR <<' '<< error << '\n';
    I = A*Rinv*T(Rinv)*A;
    error = 0.0;
    for (INTEGER j=1; j<=m; ++j) {
      for (INTEGER i=1; i<=m; ++i) {
        error += fabs(I(i,j) - A(i,j));
      }
    }
    cout << rankR <<' '<<  ldR <<' '<< error << '\n';
  }

  cout << '\n';
  cout << "Less than full rank test of cholesky" << '\n';

  for (INTEGER m = 10; m>=3; --m) {
    A.resize(m,m,0.0);
    for (INTEGER t=1; t<=5; ++t) {
      realmat x(m,1);
      for (INTEGER i=1; i<=m; ++i) x[i]=unsk(seed);
      A += x*T(x);
    }
    REAL ldR;
    INTEGER rankR = cholesky(A, R, EPS);
    ldR = logdetR(R,EPS);
    realmat RR = T(R)*R;
    REAL error = 0.0;
    for (INTEGER j=1; j<=m; ++j) {
      for (INTEGER i=1; i<=m; ++i) {
        error += fabs(RR(i,j) - A(i,j));
      }
    }
    cout << rankR <<' '<<  ldR <<' '<< error << '\n';
    realmat U,S,V;
    rankR = svd(R,U,S,V);
    ldR = 0.0;
    for (INTEGER i=1; i<=rankR; ++i) ldR += log(S[i]);
    cout << rankR <<' '<<  ldR << '\n';
  }

  cout << '\n';
  cout << "Compare inverters" << '\n';
  seed = 100542;

  for (INTEGER m = 10; m>=3; --m) {
    A.resize(m,m,0.0);
    for (INTEGER t=1; t<=6; ++t) {
      realmat x(m,1);
      for (INTEGER i=1; i<=m; ++i) x[i]=unsk(seed);
      if (m == 9) x[2] = 0.0;
      if (m == 8) x[6] = 1.0;
      if (m == 8) x[8] = 1.0;
      A += x*T(x);
    }
    INTEGER rankR = cholesky(A,R);
    rinv(R,Rinv);
    realmat Ainv0 = Rinv*T(Rinv);
    realmat Ainv1 = invpsd(A);
    //cout << Ainv0 << Ainv1; 
    REAL error = 0.0;
    for (INTEGER j=1; j<=m; ++j) {
      for (INTEGER i=1; i<=m; ++i) {
        error += fabs(Ainv1(i,j) - Ainv0(i,j));
      }
    }
    cout << rankR <<' '<< error << '\n';
  }
  for (INTEGER m = 10; m>=3; --m) {
    A.resize(m,m,0.0);
    for (INTEGER t=1; t<=100; ++t) {
      realmat x(m,1);
      for (INTEGER i=1; i<=m; ++i) x[i]=unsk(seed);
      A += x*T(x);
    }
    INTEGER rankR = cholesky(A,R);
    rinv(R,Rinv);
    realmat Ainv0 = Rinv*T(Rinv);
    realmat Ainv1 = invpsd(A);
    //cout << Ainv0 << Ainv1; 
    REAL error = 0.0;
    for (INTEGER j=1; j<=m; ++j) {
      for (INTEGER i=1; i<=m; ++i) {
        error += fabs(Ainv1(i,j) - Ainv0(i,j));
      }
    }
    cout << rankR <<' '<< error << '\n';
  }

  return 0;
}
