#include <unsupported/Eigen/Polynomials>
#include <vector>
#include <iostream>

using namespace Eigen;
using namespace std;

int main()
{
  typedef Matrix<double,5,1> Vector5d;

  Vector5d roots = Vector5d::Random();
  cout << "Roots: " << roots.transpose() << endl;
  Eigen::Matrix<double,6,1> polynomial;
  roots_to_monicPolynomial( roots, polynomial );

  PolynomialSolver<double,5> psolve( polynomial );
  cout << "Complex roots: " << psolve.roots().transpose() << endl;

  std::vector<double> realRoots;
  psolve.realRoots( realRoots );
  Map<Vector5d> mapRR( &realRoots[0] );
  cout << "Real roots: " << mapRR.transpose() << endl;

  cout << endl;
  cout << "Illustration of the convergence problem with the QR algorithm: " << endl;
  cout << "---------------------------------------------------------------" << endl;
  Eigen::Matrix<float,7,1> hardCase_polynomial;
  hardCase_polynomial <<
  -0.957, 0.9219, 0.3516, 0.9453, -0.4023, -0.5508, -0.03125;
  cout << "Hard case polynomial defined by floats: " << hardCase_polynomial.transpose() << endl;
  PolynomialSolver<float,6> psolvef( hardCase_polynomial );
  cout << "Complex roots: " << psolvef.roots().transpose() << endl;
  Eigen::Matrix<float,6,1> evals;
  for( int i=0; i<6; ++i ){ evals[i] = std::abs( poly_eval( hardCase_polynomial, psolvef.roots()[i] ) ); }
  cout << "Norms of the evaluations of the polynomial at the roots: " << evals.transpose() << endl << endl;

  cout << "Using double's almost always solves the problem for small degrees: " << endl;
  cout << "-------------------------------------------------------------------" << endl;
  PolynomialSolver<double,6> psolve6d( hardCase_polynomial.cast<double>() );
  cout << "Complex roots: " << psolve6d.roots().transpose() << endl;
  for( int i=0; i<6; ++i )
  {
    std::complex<float> castedRoot( psolve6d.roots()[i].real(), psolve6d.roots()[i].imag() );
    evals[i] = std::abs( poly_eval( hardCase_polynomial, castedRoot ) );
  }
  cout << "Norms of the evaluations of the polynomial at the roots: " << evals.transpose() << endl << endl;

  cout.precision(10);
  cout << "The last root in float then in double: " << psolvef.roots()[5] << "\t" << psolve6d.roots()[5] << endl;
  std::complex<float> castedRoot( psolve6d.roots()[5].real(), psolve6d.roots()[5].imag() );
  cout << "Norm of the difference: " << std::abs( psolvef.roots()[5] - castedRoot ) << endl;
}