#include <iostream>
#include "quickmatrix.hpp"


//
// Simple test program for quickmatrix.cpp
//

int main ()
{
  //
  // Test matrix SVD
  //
  double data[]={1,2,3,4,5,6,7,8};
  Matrix A=MatrixFromData(4,2,data);
  Matrix U=MatrixCreate(4,2);
  Matrix V=MatrixCreate(2,2);
  Vector S=VectorCreate(2);
  //
  // Call SVD
  //
  MatrixSVD(A,U,V,S);
  //
  // Print A (should be unchanged)
  //
  fprintf(stderr,"A:\n");
  MatrixPrint(stderr,A);
  fprintf(stderr,"\n");
  //
  // Print U
  //
  fprintf(stderr,"U:\n");
  MatrixPrint(stderr,U);
  fprintf(stderr,"\n");
  //
  // Print V
  //
  fprintf(stderr,"V:\n");
  MatrixPrint(stderr,V);
  fprintf(stderr,"\n");
  //
  // Print S
  //
  fprintf(stderr,"S:\n");
  VectorPrint(stderr,S);
  fprintf(stderr,"\n\n");
  //
  // Test singular values
  //
  if (fabs(S->d[0]-14.2691)>1e-5 || fabs(S->d[1]-0.626828)>1e-5) {
    fprintf(stderr,">>>> Singular values incorrect!\n");
  }

  fprintf(stderr,"pinv(A):\n");
  MatrixPseudoInvert(A);
  MatrixPrint(stderr,A);
  fprintf(stderr,"\n");
  //
  // Test MatrixEigenvalues
  //
  double sym[]={1,2,3,2,4,5,3,5,6};
  Matrix B=MatrixFromData(3,3,sym);
  Matrix E=MatrixCreate(3,3);
  MatrixMove(E,B);
  double lambda[3];

  fprintf(stderr,"B:\n");
  MatrixPrint(stderr,B);
  fprintf(stderr,"\n");

  MatrixEigenvalues(E, lambda, NULL);

  fprintf(stderr,"[E,lambda]=eig(B)\n\n");

  fprintf(stderr,"E:\n");
  MatrixPrint(stderr,E);
  fprintf(stderr,"\n");

  fprintf(stderr,"lambda:\n");
  for (unsigned i=0; i<3; i++) {
    fprintf(stderr,"%f ",lambda[i]);
  }
  fprintf(stderr,"\n\n");

  Matrix L=MatrixCreate(3,3);
  for (unsigned i=0; i<3; i++) {
    for (unsigned j=0; j<3; j++) {
      L->d2[i][j]=(i==j)?lambda[i]:0.0;
    }
  }

  MatrixMultiply(L,L,E);
  MatrixTranspose(E);
  MatrixMultiply(L,E,L);
  fprintf(stderr,"E'*diag(lambda)*E:\n");
  MatrixPrint(stderr,L);
  fprintf(stderr,"\n");
  //
  // Test reconstructed B
  //
  for (unsigned i=0; i<3; i++) {
    for (unsigned j=0; j<3; j++) {
      if (fabs(L->d2[i][j]-B->d2[i][j])>1e-5) {
	fprintf(stderr,">>>> Eigen value decomposition incorrect!\n");
      }
    }
  }

  return 0;
}