#include <iostream>

#include <stdlib.h>   //abs

#include "ap.h"
#include "ap.cpp"

#include "linalg.h"
#include "linalg.cpp"


#include "alglibinternal.h"
#include "alglibmisc.h"
#include "alglibinternal.cpp"
#include "alglibmisc.cpp"

using namespace std;
void svd(alglib::real_2d_array& a, alglib::real_2d_array& pseudoi, int m, int n)
{
      alglib::real_1d_array w;
      alglib::real_2d_array u, vt;

      alglib::rmatrixsvd(a, m, n, 2, 2, 2, w, u, vt);

      //create E_invers
      alglib::real_2d_array ei;
      ei.setlength(n, m);
      for(int i = 0; i < n; i++)
         for(int j = 0; j < m; j++)
            ei(i, j) = 0;
      for(int i = 0; i < w.length(); i++)
         if(abs(w(i)) > 10. * std::numeric_limits<double>::epsilon() )
            ei(i, i) = 1. / w(i);

      //pseudoinverse
      alglib::real_2d_array ei_ut;
      ei_ut.setlength(n, m);

      rmatrixgemm(n, m, m, 1, ei,0,0,0, u,0,0,1, 0, ei_ut,0,0);
      rmatrixgemm(n, m, n, 1, vt,0,0,1, ei_ut,0,0,0, 0, pseudoi,0,0);
}

void test()
{
   int m = 5;
   int n = 5;
   alglib::real_2d_array a;
   double _r2[]   ={
               10,1,0,0,0,
               0,10,1,0,0,
               0,0,10,1,0,
               0,0,0,10,1,
               0,0,0,0,1};
   a.setcontent(m, n,_r2);

   alglib::real_2d_array pseudoi;
   pseudoi.setlength(n, m);
   svd(a, pseudoi, m, n);

   //test
   alglib::real_2d_array eye;
   eye.setlength(n, n);
   rmatrixgemm(n, n, m, 1, pseudoi,0,0,0, a,0,0,0, 0, eye,0,0);

   cout << " eye: " << endl << eye.tostring(1) << endl;
}