#include <iostream>
#include <fstream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <vector>
#include <cmath>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
#include "TwoChQS.hpp"

#ifndef pi
#define pi 3.141592653589793238462643383279502884197169
#endif

int main (int argc, char* argv[]){

  //char ModelOption[]="Anderson";
  //int ModelNo=0;

  // command-line model options
#include"ModelOpt.cpp"

  CNRGarray Aeig(2);

  CNRGbasisarray AeigCut(2);

  CNRGbasisarray Abasis(2);

  CNRGbasisarray SingleSite(3);

  CNRGmatrix* MatArray;
  int NumNRGarrays=2;
  // MatArray 0 is nd
  // MatArray 1 is cd

  // Thermodynamics
  CNRGthermo Suscep;
  CNRGthermo Entropy;

  // STL vector

  CNRGmatrix Qm1fNQ[2];

  CNRGmatrix MQQp1;

  double U,ed,Gamma1,Gamma2;
  vector<double> Params;
  double Lambda;
  double HalfLambdaFactor;
  double Dband=1.0;
  int calcdens;

  double DN=0.0;
  double TM=0.0;
  double betabar=0.727;
  double Temp=0.0;
  double Sus=0.0;
  //vector<double> SuscepChain;
  char arqSus[32],arqname[32];

  double chi_m1,chi_N[2];
  double daux[4];

  int Nsites=0,Nsitesmax=2;

  int Ncutoff=700;
  int UpdateBefCut=0;
  int auxIn;

  // outstream
  ofstream OutFile;
  // instream
  ifstream InFile;

  int ii,jj,i1,i2;

  // STL iterator:

  vector<double>::iterator diter;

  // Phonon
  // Test!! Remove later
  double Nph=1.0;
  double w0=0.2;
  double lambda=0.4;
  double alpha=0.4;
  double tp=0.2;

  ///            ///
  /// Begin code ///
  ///            ///
  U=0.5;
  ed=-0.5*U;
  Gamma1=0.0282691;
  Gamma2=0.0;
  Lambda=2.5;

  // Steps: Input parameters
  InFile.open("nrg_input_TwoCh.dat");
  if (InFile.is_open())
    {
      InFile >> Nsitesmax;
      InFile >> Ncutoff;
      InFile >> U;
      InFile >> Gamma1;
      InFile >> Gamma2;
      InFile >> ed;
      InFile >> Lambda;
      InFile >> Dband;
      InFile >> auxIn;
      InFile >> UpdateBefCut;
      InFile >> calcdens;
    }
  else cout << "can't open nrg_input_TwoCh.dat" << endl;

  InFile.close();

  if (ModelNo==4)
    {
      InFile.open("Input_Phonon.dat");
      if (InFile.is_open())
	{
	  InFile >> Nph;
	  InFile >> w0;
	  InFile >> lambda;
	  InFile >> tp;
	  InFile >> alpha;
	}
      else cout << "can't open Input_Phonon.dat" << endl;
      InFile.close();
      Gamma1=pi*tp*tp;
      Gamma2=alpha*alpha*Gamma1;
    }
  // end if phonon

  ///////////////////////////
  ///////////////////////////

  // New stuff
   strcpy(Suscep.ArqName,"SuscepImp2Ch_25_726.dat");
   strcpy(Suscep.ChainArqName,"SuscepChain2Ch.dat");
   Suscep.Calc=CalcSuscep;

   strcpy(Entropy.ArqName,"EntropyImp2Ch_25_726.dat");
   strcpy(Entropy.ChainArqName,"EntropyChain2Ch.dat");
   Entropy.Calc=CalcEntropy;

   // Chain model
   if(ModelNo==3) calcdens=3;

   if (calcdens==2)
     {
       Suscep.CalcChain=false;
       Entropy.CalcChain=false;
       Suscep.Nsite0Chain=0; // Anderson chains
       Entropy.Nsite0Chain=0; // Anderson chains

       if ( (!FileExists(Suscep.ChainArqName))||
	    (!FileExists(Entropy.ChainArqName)) )
	 {
	   cout << " Can't find chain files: " << endl
		<< "   " << Suscep.ChainArqName << endl
		<< "   " << Entropy.ChainArqName << endl;
	   cout << " Exiting... " << endl;
	   exit(0);
	 }
       else
	 cout << " Found files "
	      << Suscep.ChainArqName << ", "
	      << Entropy.ChainArqName
	      << endl;
     }
   if (calcdens==3)
     {
       //strcpy(arqSus,"SuscepChain.dat");
       Suscep.CalcChain=true;
       Entropy.CalcChain=true;
       if ( (FileExists(Suscep.ChainArqName))||
	    (FileExists(Entropy.ChainArqName)) )
	 {
	   cout << " Error: Chain files already exist: " << endl
		<< "   " << Suscep.ChainArqName << endl
		<< "   " << Entropy.ChainArqName << endl;
	   cout << " Not rewriting. Exiting... " << endl;
	   exit(0);
	 }
       U=0.0;ed=0.0;Gamma1=0.0;Gamma2=0.0;
     }

  ///////////////////////////
  ///////////////////////////

  HalfLambdaFactor=0.5*(1.0+(1.0/Lambda));
  chi_m1=sqrt(2.0*Gamma1/pi)/(sqrt(Lambda)*HalfLambdaFactor);

  double U_tilde=0.5*U/HalfLambdaFactor;
  double ed_tilde=ed/HalfLambdaFactor;
  double Gamma1_tilde=Gamma1*(2.0/pi)/(HalfLambdaFactor*HalfLambdaFactor);
  double Gamma2_tilde=Gamma2*(2.0/pi)/(HalfLambdaFactor*HalfLambdaFactor);

  OutFile.open("NRG_in.txt");
  OutFile << "Begin NRG 2-ch calculation" << endl;
  OutFile << " Model : " << ModelOption << endl;
  OutFile << " Nsitesmax    = " << Nsitesmax-1 << endl;
  OutFile << " Ncutoff      = " << Ncutoff << endl;
  OutFile << " U            = " << U << endl;
  OutFile << " Gamma1        = " << Gamma1 << endl;
  OutFile << " Gamma2        = " << Gamma2 << endl;
  OutFile << " ed           = " << ed << endl;
  OutFile << " Lambda       = " << Lambda << endl;
  OutFile << " Dband        = " << Dband << endl;
  OutFile << " UpdateBefCut = " << UpdateBefCut << endl;
  OutFile << " calcdens     = " << calcdens << endl;
  OutFile << "=================================" << endl;
  OutFile << "U~ = " << U_tilde << endl;
  OutFile << "ed~ = " <<  ed_tilde << endl;
  OutFile << "Gamma1~ = " <<  Gamma1_tilde<< endl;
  OutFile << "Gamma2~ = " <<  Gamma2_tilde<< endl;
  OutFile << "=================================" << endl;
  if (ModelNo==4)
    {
      OutFile << "Nph = " <<  Nph << endl;
      OutFile << "w0 = " <<  w0 << endl;
      OutFile << "lambda = " <<  lambda << endl;
      OutFile << "tp = " <<  tp << endl;
      OutFile << "alpha = " <<  alpha << endl;
      OutFile << "chi_S_tilde = " <<  sqrt(Gamma1_tilde) << endl;
      OutFile << "chi_A_tilde = " <<  alpha*sqrt(Gamma1_tilde) << endl;
      OutFile << "=================================" << endl;
    }
  OutFile.close();

  // Define H0 (impurity + 1s site)
  // Output:
  //       Aeig,
  //       fd_{1sigma} and fd_{2sigma} matrix elements
  //

  // Set single site (use pointers in the subroutines!!)

  TwoChQS_SetSingleSite(&SingleSite);

  Params.push_back(U_tilde/sqrt(Lambda));
  Params.push_back(ed_tilde/sqrt(Lambda));
  Params.push_back(sqrt(Gamma1_tilde/Lambda));
  Params.push_back(sqrt(Gamma2_tilde/Lambda));

  Nsites=0;
  DN=HalfLambdaFactor*pow(Lambda,(-(Nsites-1)/2.0) );
  TM=DN/betabar;
  cout << "DN = " << DN << "  TM = " << TM << endl;

  switch (ModelNo)
    {
    case 0 :
      TwoChQS_SetH0Anderson(Params,&SingleSite,&Aeig,&Abasis);
      Suscep.dImpValue=1.0/8.0;
      Entropy.dImpValue=2.0*log(2.0);
      break;

    case 1 :
      cout << " Testing 2ch Kondo model. " << endl;
      Params.clear();
      Params.push_back(Gamma1);
      Params.push_back(Gamma2);
      TwoChQS_SetH0Kondo(Params,&SingleSite,&Aeig,&Abasis,MatArray);
      Suscep.dImpValue=1.0/4.0;
      Entropy.dImpValue=log(2.0);
      break;

    case 3 :
      TwoChQS_SetH0Chain(Params,&SingleSite,&Aeig,&Abasis);
      Suscep.dImpValue=0.0;
      Entropy.dImpValue=0.0; // Check this!!
      Suscep.CalcChain=true;
      Entropy.CalcChain=true;
      break;
    case 4 :
      Params.push_back(w0); //
      Params.push_back(lambda); //
      Params.push_back(alpha); //
      Params.push_back(Nph); //
      // Using the untransformed Hamiltonian
      //TwoChQS_SetH0CMphononwTransf(Params,&SingleSite,&Aeig,&Abasis,MatArray);
      TwoChQS_SetH0CMphonon(Params,&SingleSite,&Aeig,&Abasis,MatArray);
      // Qm1fQ needs special updates
      for (int ich=0;ich<=1;ich++)
	{
	  Qm1fNQ[ich].NeedOld=false;
	  Qm1fNQ[ich].CheckForMatEl=TwoChQS_cd_check;
	}

      Qm1fNQ[0].CalcMatEl=TwoChQS_cd_ich1_Phonon_MatEl;
      Qm1fNQ[1].CalcMatEl=TwoChQS_cd_ich2_Phonon_MatEl;
      /////////
      Suscep.dImpValue=0.0;
      Entropy.dImpValue=0.0; // Check this!!
      strcpy(Suscep.ArqName,"SuscepImp2Ch_Phonon.dat");
      strcpy(Entropy.ArqName,"EntropyImp2Ch_Phonon.dat");
      // Update with a new scheme
      cout << " Updating! " << endl;
      AeigCut.CNRGbasisarray::ClearAll();
      AeigCut=CutStates(&Aeig, Ncutoff);
      UpdateMatrices(&SingleSite,&AeigCut,
		     &Abasis,&Qm1fNQ[0], 2);

      break;
    default :
      cout << " Model not implemented. Exiting... " << endl;
      exit(0);
      break;
    }
  //end switch models

  Aeig.PrintEn();

  if (ModelNo!=4){TwoChQS_UpdateQm1fQ(&SingleSite,&Aeig,&Abasis,Qm1fNQ);}

  cout << " f0_ch1 : " << endl;
  //Qm1fNQ[0].PrintAllBlocks();
  Qm1fNQ[0].PrintMatBlock(1,3);
  Qm1fNQ[0].PrintMatBlock(2,3);
  cout << " f0_ch1 : " << endl;
  //Qm1fNQ[1].PrintAllBlocks();
  Qm1fNQ[1].PrintMatBlock(1,3);
  Qm1fNQ[1].PrintMatBlock(2,3);

  // Calculate Susceptibility at Nsites=0!

  if ( (calcdens==2)||(calcdens==3) )
    {
      TM=DN/betabar;
      Params.clear();
      Params.push_back(betabar);

      Suscep.ReadNChainValue(Nsites,0);
      Suscep.AddValue(Params,&Aeig,1,true,TM);
      Suscep.SaveNValue(Nsites,0);

      Entropy.ReadNChainValue(Nsites,0);
      Entropy.AddValue(Params,&Aeig,1,true,TM);
      Entropy.SaveNValue(Nsites,0);

    }
  //end if calcdens=2 or 3

  //TwoChQS_UpdateQm1fQ(&SingleSite,&Aeig,&Abasis,Qm1fNQ);

  // Check Matrix
//    for (int ich=1;ich<=2;ich++)
//      {
//        cout << " Printing Qm1fQ channel: " << ich << endl;
//        for (int ibl=0; ibl<Qm1fNQ[ich-1].NumMatBlocks();ibl++)
//  	{
//  	  Qm1fNQ[ich-1].PrintMatBlock(ibl);
//  	}
//      }

  // Loop on Nsites: start from Nsites=0 (imp+1)
  //

  Nsites=1;
  while (Nsites<=Nsitesmax)
    {

      DN=HalfLambdaFactor*pow(Lambda,(-(Nsites-1)/2.0) );
      TM=DN/betabar;
      cout << "DN = " << DN << "TM = " << TM << endl;

      // 0 - Update chi_N, eps_N

      daux[0]=(double)( 1.0-pow(Lambda,(-Nsites)) );
      daux[1]=(double)sqrt( 1.0-pow(Lambda,-(2*Nsites-1)) );
      daux[2]=(double)sqrt( 1.0-pow(Lambda,-(2*Nsites+1)) );
      daux[3]=0.5*(1.0+(1.0/Lambda))*(double)sqrt(Lambda);

      chi_N[0]=daux[0]/(daux[1]*daux[2]);
      chi_N[1]=daux[0]/(daux[1]*daux[2]);

      cout << "chi_N = " << chi_N[0] << "  " << chi_N[1] << endl;

      cout << "Nsites = " << Nsites << endl;
      cout << "BEG Eig Nshell = " << Aeig.Nshell << endl;

      // 1 - Eliminate states and Build Abasis

      cout << " Nstates: ";
      cout <<  Aeig.Nstates() << endl;

      cout << "Cutting states..." << endl;
      AeigCut.CNRGbasisarray::ClearAll();
      AeigCut=CutStates(&Aeig, Ncutoff);

      cout << "... done cutting states." << endl;

      // Calculate new matrix elements using the CUT basis:
      // update Qm1fNQ, Qm1cdQ, etc.

      if ( (UpdateBefCut==0)&&(Nsites>1) )
	{
	  cout << "Updating matrices after cutting... " << endl;
	  TwoChQS_UpdateMatrixAfterCutting(&SingleSite,
	  				    &AeigCut, &Abasis, Qm1fNQ, &MQQp1);
	  cout << "... done updating matrices. " << endl;
	}

      // Spec density calculation, other similar things woudl go HERE

      // This is where the loop really begins...

      QS_BuildBasis(&AeigCut,&Abasis,&SingleSite,UpdateBefCut);

      cout << "No blocks = " << Abasis.NumBlocks() << endl;

      cout << "No states = " << Abasis.Nstates() << endl;

      //Abasis.PrintAll();

      // 2 - Build and diagonalize H_N+1

      Params.clear();
      Params.push_back(chi_N[0]);
      Params.push_back(chi_N[1]);
      Params.push_back(Lambda);

      cout << "Diagonalizing HN... " << endl;

      TwoChQS_DiagHN(Params,&Abasis,&SingleSite,Qm1fNQ,&Aeig);

      Aeig.PrintEn();

      cout << "..done diagonalizing HN. " << endl;

      // 3 - Update Qm1f1NQ, Qm1f2Q, Qm1cdQ, etc.

      if ( (UpdateBefCut==1)&&(Nsites<Nsitesmax) )
	{
	  cout << "Updating matrices before cutting... " << endl;
	  TwoChQS_UpdateQm1fQ(&SingleSite,&Aeig,&Abasis,Qm1fNQ);
	  cout << "... done updating matrices. " << endl;

	}

      // Calculate Susceptibility/Entropy

      if ( (calcdens==2)||(calcdens==3) )
	{
	  TM=DN/betabar;
	  Params.clear();
	  Params.push_back(betabar);

	  Suscep.ReadNChainValue(Nsites,0);
	  Suscep.AddValue(Params,&Aeig,1,true,TM);
	  Suscep.SaveNValue(Nsites,0);

	  Entropy.ReadNChainValue(Nsites,0);
	  Entropy.AddValue(Params,&Aeig,1,true,TM);
	  Entropy.SaveNValue(Nsites,0);
	}
  //end if calcdens=2 or 3

      // 4 - Update Nsites

      Nsites++;

    }

  cout << "=== Calculation Finished! ==== "<< endl;
  OutFile.open("NRG_end.txt");
  OutFile << "END NRG calculation" << endl;
  OutFile.close();

  cout << "Calling destructors " << endl;

}
//END code

///////////////////////
//       else
// 	{
// 	  while (!InFile.eof())
// 	    {
// 	      InFile >> Temp >> daux[0] >> daux[0] >> Sus;
// 	      SuscepChain.push_back(Sus);
// 	    }
// 	  SuscepChain.pop_back();
// 	  if (SuscepChain.size()<Nsitesmax-1)
// 	    {
// 	      cout << "Nsuscep = " << SuscepChain.size() << " Nsitesmax = " << Nsitesmax << endl;
// 	      cout << " Longer SuscepChain needed! Doing it all over..." << endl;
// 	      SuscepChain.clear();
// 	      strcpy(arqSus,"SuscepChain.dat");
// 	      calcdens=3;
// 	    }
// 	}
//       InFile.close();
//////////////////////////

      //SuscepChain.clear();
//       strcpy(arqSus,"SuscepImp_25_726.dat");

//       strcpy(arqname,"SuscepChain.dat");
//       InFile.open(arqname);
//       if (InFile.fail())
// 	{
// 	  cout << "Can't find " << arqname << endl;
// 	  strcpy(arqSus,"SuscepChain.dat");
// 	  calcdens=3;
// 	}

//       Sus=CalcSuscep(Params,&Aeig,1,true);
//       double Sus0=0.0;
//       int nlines=0;
//       if (calcdens==2)
// 	{
// 	  InFile.open("SuscepChain.dat");
// 	  InFile.clear();
// 	  InFile.seekg(0, ios::beg); // rewind
// 	  if (InFile.fail())
// 	    {
// 	      cout << "Can't open  SuscepChain.dat"<< endl;
// 	    }
// 	  else
// 	    {
// 	      while ( (!InFile.eof())&&(nlines<=Nsites) )
// 		{
// 		  InFile >> daux[0] >> daux[0] >> daux[0] >> Sus0;
// 		  nlines++;
// 		}
// 	    }
// 	  InFile.close();
// 	}
//        else
// 	Sus-=1.0/8.0; // exclude dot site in the chain.
//       if (Nsites==0) OutFile.open(arqSus);
//       else OutFile.open(arqSus,ofstream::app);
//       OutFile.precision(20);
//       OutFile << scientific << TM << " " << Sus << " " << Sus0 << " " << Sus-Sus0 << endl;
//       OutFile.close();

// 	  Sus=CalcSuscep(Params,&Aeig,1,true);
// 	  double Sus0=0.0;
// 	  int nlines=0;
// 	  // Gets SuscepChain iteration by iteration
// 	  if (calcdens==2)
// 	    {
// 	      InFile.open("SuscepChain.dat");
// 	      InFile.clear();
// 	      InFile.seekg(0, ios::beg); // rewind
// 	      if (InFile.fail())
// 		{
// 		  cout << "Can't find " << arqname << endl;
// 		}
// 	      else
// 		{
// 		  while ( (!InFile.eof())&&(nlines<=Nsites) )
// 		    {
// 		      InFile >> daux[0] >> daux[0] >> daux[0] >> Sus0;
// 		      nlines++;
// 		    }
// 		}
// 	      InFile.close();
// 	    }
// 	  //if (calcdens==2) Sus0=SuscepChain[Nsites];
// 	  else Sus-=1.0/8.0; // exclude dot site in the chain.

// 	  if (Nsites==0) OutFile.open(arqSus);
// 	  else OutFile.open(arqSus,ofstream::app);
// 	  OutFile.precision(20);
// 	  OutFile << scientific << TM << " " << Sus << " " << Sus0 << " " << Sus-Sus0 << endl;
// 	  OutFile.close();


// Copyright 2017 Nicolas Mellado
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// -------------------------------------------------------------------------- //
//
// Authors: Dror Aiger, Yoni Weill, Nicolas Mellado
//
// This file is part of the implementation of the 4-points Congruent Sets (4PCS)
// algorithm presented in:
//
// 4-points Congruent Sets for Robust Surface Registration
// Dror Aiger, Niloy J. Mitra, Daniel Cohen-Or
// ACM SIGGRAPH 2008 and ACM Transaction of Graphics.
//
// Given two sets of points in 3-space, P and Q, the algorithm applies RANSAC
// in roughly O(n^2) time instead of O(n^3) for standard RANSAC, using an
// efficient method based on invariants, to find the set of all 4-points in Q
// that can be matched by rigid transformation to a given set of 4-points in P
// called a base. This avoids the need to examine all sets of 3-points in Q
// against any base of 3-points in P as in standard RANSAC.
// The algorithm can use colors and normals to speed-up the matching
// and to improve the quality. It can be easily extended to affine/similarity
// transformation but then the speed-up is smaller because of the large number
// of congruent sets. The algorithm can also limit the range of transformations
// when the application knows something on the initial pose but this is not
// necessary in general (though can speed the runtime significantly).

// Home page of the 4PCS project (containing the paper, presentations and a
// demo): http://graphics.stanford.edu/~niloy/research/fpcs/fpcs_sig_08.html
// Use google search on "4-points congruent sets" to see many related papers
// and applications.

#include "super4pcs/algorithms/match4pcsBase.h"
#include "super4pcs/shared4pcs.h"
#include "super4pcs/sampling.h"
#include "super4pcs/accelerators/kdtree.h"

#include <vector>
#include <atomic>

#include <Eigen/Core>
#include <Eigen/Geometry>

const double pi = std::acos(-1);

// Compute the closest points between two 3D line segments and obtain the two
// invariants corresponding to the closet points. This is the "intersection"
// point that determines the invariants. Since the 4 points are not exactly
// planar, we use the center of the line segment connecting the two closest
// points as the "intersection".
template < typename VectorType, typename Scalar>
static Scalar
distSegmentToSegment(const VectorType& p1, const VectorType& p2,
                     const VectorType& q1, const VectorType& q2,
                     Scalar& invariant1, Scalar& invariant2) {

  static const Scalar kSmallNumber = 0.0001;
  VectorType u = p2 - p1;
  VectorType v = q2 - q1;
  VectorType w = p1 - q1;
  Scalar a = u.dot(u);
  Scalar b = u.dot(v);
  Scalar c = v.dot(v);
  Scalar d = u.dot(w);
  Scalar e = v.dot(w);
  Scalar f = a * c - b * b;
  // s1,s2 and t1,t2 are the parametric representation of the intersection.
  // they will be the invariants at the end of this simple computation.
  Scalar s1 = 0.0;
  Scalar s2 = f;
  Scalar t1 = 0.0;
  Scalar t2 = f;

  if (f < kSmallNumber) {
    s1 = 0.0;
    s2 = 1.0;
    t1 = e;
    t2 = c;
  } else {
    s1 = (b * e - c * d);
    t1 = (a * e - b * d);
    if (s1 < 0.0) {
      s1 = 0.0;
      t1 = e;
      t2 = c;
    } else if (s1 > s2) {
      s1 = s2;
      t1 = e + b;
      t2 = c;
    }
  }

  if (t1 < 0.0) {
    t1 = 0.0;
    if (-d < 0.0)
      s1 = 0.0;
    else if (-d > a)
      s1 = s2;
    else {
      s1 = -d;
      s2 = a;
    }
  } else if (t1 > t2) {
    t1 = t2;
    if ((-d + b) < 0.0)
      s1 = 0;
    else if ((-d + b) > a)
      s1 = s2;
    else {
      s1 = (-d + b);
      s2 = a;
    }
  }
  invariant1 = (std::abs(s1) < kSmallNumber ? 0.0 : s1 / s2);
  invariant2 = (std::abs(t1) < kSmallNumber ? 0.0 : t1 / t2);

  return ( w + (invariant1 * u) - (invariant2 * v)).norm();
}

namespace GlobalRegistration{

Match4PCSBase::Match4PCSBase(  const Match4PCSOptions& options
                             , const Utils::Logger& logger
#ifdef SUPER4PCS_USE_OPENMP
                             , const int omp_nthread_congruent
#endif
                               )
  :number_of_trials_(0)
  , max_base_diameter_(-1)
  , P_mean_distance_(1.0)
  , best_LCP_(0.0)
  , options_(options)
  , randomGenerator_(options.randomSeed)
  , logger_(logger)
#ifdef SUPER4PCS_USE_OPENMP
  , omp_nthread_congruent_(omp_nthread_congruent)
#endif
{
  base_3D_.resize(4);
}

Match4PCSBase::~Match4PCSBase(){}

Match4PCSBase::Scalar
Match4PCSBase::MeanDistance() {
  const Scalar kDiameterFraction = 0.2;
  using RangeQuery = GlobalRegistration::KdTree<Scalar>::RangeQuery<>;

  int number_of_samples = 0;
  Scalar distance = 0.0;

  for (size_t i = 0; i < sampled_P_3D_.size(); ++i) {

    RangeQuery query;
    query.sqdist = P_diameter_ * kDiameterFraction;
    query.queryPoint = sampled_P_3D_[i].pos().cast<Scalar>();

    GlobalRegistration::KdTree<Scalar>::Index resId =
        kd_tree_.doQueryRestrictedClosestIndex(query , i);

    if (resId != GlobalRegistration::KdTree<Scalar>::invalidIndex()) {
      distance += (sampled_P_3D_[i].pos() - sampled_P_3D_[resId].pos()).norm();
      number_of_samples++;
    }
  }

  return distance / number_of_samples;
}

bool Match4PCSBase::SelectRandomTriangle(int &base1, int &base2, int &base3) {
      int number_of_points = sampled_P_3D_.size();
      base1 = base2 = base3 = -1;

      // Pick the first point at random.
      int first_point = randomGenerator_() % number_of_points;

      const Scalar sq_max_base_diameter_ = max_base_diameter_*max_base_diameter_;

      // Try fixed number of times retaining the best other two.
      Scalar best_wide = 0.0;
      for (int i = 0; i < kNumberOfDiameterTrials; ++i) {
        // Pick and compute
        const int second_point = randomGenerator_() % number_of_points;
        const int third_point = randomGenerator_() % number_of_points;
        const VectorType u =
                sampled_P_3D_[second_point].pos() -
                sampled_P_3D_[first_point].pos();
        const VectorType w =
                sampled_P_3D_[third_point].pos() -
                sampled_P_3D_[first_point].pos();
        // We try to have wide triangles but still not too large.
        Scalar how_wide = (u.cross(w)).norm();
        if (how_wide > best_wide &&
                u.squaredNorm() < sq_max_base_diameter_ &&
                w.squaredNorm() < sq_max_base_diameter_) {
          best_wide = how_wide;
          base1 = first_point;
          base2 = second_point;
          base3 = third_point;
        }
      }
      return base1 != -1 && base2 != -1 && base3 != -1;
}

// Try the current base in P and obtain the best pairing, i.e. the one that
// gives the smaller distance between the two closest points. The invariants
// corresponding the the base pairing are computed.
bool Match4PCSBase::TryQuadrilateral(Scalar &invariant1, Scalar &invariant2,
                                     int& id1, int& id2, int& id3, int& id4) {

  Scalar min_distance = std::numeric_limits<Scalar>::max();
  int best1, best2, best3, best4;
  best1 = best2 = best3 = best4 = -1;
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      if (i == j) continue;
      int k = 0;
      while (k == i || k == j) k++;
      int l = 0;
      while (l == i || l == j || l == k) l++;
      double local_invariant1;
      double local_invariant2;
      // Compute the closest points on both segments, the corresponding
      // invariants and the distance between the closest points.
      Scalar segment_distance = distSegmentToSegment(
                  base_3D_[i].pos(), base_3D_[j].pos(),
                  base_3D_[k].pos(), base_3D_[l].pos(),
                  local_invariant1, local_invariant2);
      // Retail the smallest distance and the best order so far.
      if (segment_distance < min_distance) {
        min_distance = segment_distance;
        best1 = i;
        best2 = j;
        best3 = k;
        best4 = l;
        invariant1 = local_invariant1;
        invariant2 = local_invariant2;
      }
    }
  }

  if(best1 < 0 || best2 < 0 || best3 < 0 || best4 < 0 ) return false;

  std::vector<Point3D> tmp = base_3D_;
  base_3D_[0] = tmp[best1];
  base_3D_[1] = tmp[best2];
  base_3D_[2] = tmp[best3];
  base_3D_[3] = tmp[best4];

  std::array<int, 4> tmpId = {id1, id2, id3, id4};
  id1 = tmpId[best1];
  id2 = tmpId[best2];
  id3 = tmpId[best3];
  id4 = tmpId[best4];

  return true;
}

// Selects a good base from P and computes its invariants. Returns false if
// a good planar base cannot can be found.
bool Match4PCSBase::SelectQuadrilateral(Scalar& invariant1, Scalar& invariant2,
                                        int& base1, int& base2, int& base3,
                                        int& base4) {

  const Scalar kBaseTooSmall (0.2);
  int current_trial = 0;

  // Try fix number of times.
  while (current_trial < kNumberOfDiameterTrials) {
    // Select a triangle if possible. otherwise fail.
    if (!SelectRandomTriangle(base1, base2, base3)){
      return false;
    }

    base_3D_[0] = sampled_P_3D_[base1];
    base_3D_[1] = sampled_P_3D_[base2];
    base_3D_[2] = sampled_P_3D_[base3];

    // The 4th point will be a one that is close to be planar to the other 3
    // while still not too close to them.
    const double x1 = base_3D_[0].x();
    const double y1 = base_3D_[0].y();
    const double z1 = base_3D_[0].z();
    const double x2 = base_3D_[1].x();
    const double y2 = base_3D_[1].y();
    const double z2 = base_3D_[1].z();
    const double x3 = base_3D_[2].x();
    const double y3 = base_3D_[2].y();
    const double z3 = base_3D_[2].z();

    // Fit a plan.
    Scalar denom = (-x3 * y2 * z1 + x2 * y3 * z1 + x3 * y1 * z2 - x1 * y3 * z2 -
                    x2 * y1 * z3 + x1 * y2 * z3);

    if (denom != 0) {
      Scalar A =
          (-y2 * z1 + y3 * z1 + y1 * z2 - y3 * z2 - y1 * z3 + y2 * z3) / denom;
      Scalar B =
          (x2 * z1 - x3 * z1 - x1 * z2 + x3 * z2 + x1 * z3 - x2 * z3) / denom;
      Scalar C =
          (-x2 * y1 + x3 * y1 + x1 * y2 - x3 * y2 - x1 * y3 + x2 * y3) / denom;
      base4 = -1;
      Scalar best_distance = std::numeric_limits<Scalar>::max();
      // Go over all points in P.
      const Scalar too_small = std::pow(max_base_diameter_ * kBaseTooSmall, 2);
      for (unsigned int i = 0; i < sampled_P_3D_.size(); ++i) {
        if ((sampled_P_3D_[i].pos()- sampled_P_3D_[base1].pos()).squaredNorm() >= too_small &&
            (sampled_P_3D_[i].pos()- sampled_P_3D_[base2].pos()).squaredNorm() >= too_small &&
            (sampled_P_3D_[i].pos()- sampled_P_3D_[base3].pos()).squaredNorm() >= too_small) {
          // Not too close to any of the first 3.
          const Scalar distance =
              std::abs(A * sampled_P_3D_[i].x() + B * sampled_P_3D_[i].y() +
                   C * sampled_P_3D_[i].z() - 1.0);
          // Search for the most planar.
          if (distance < best_distance) {
            best_distance = distance;
            base4 = int(i);
          }
        }
      }
      // If we have a good one we can quit.
      if (base4 != -1) {
        base_3D_[3] = sampled_P_3D_[base4];
        if(TryQuadrilateral(invariant1, invariant2, base1, base2, base3, base4))
            return true;
      }
    }
    current_trial++;
  }

  // We failed to find good enough base..
  return false;
}

void Match4PCSBase::initKdTree(){
  size_t number_of_points = sampled_P_3D_.size();

  // Build the kdtree.
  kd_tree_ = GlobalRegistration::KdTree<Scalar>(number_of_points);

  for (size_t i = 0; i < number_of_points; ++i) {
    kd_tree_.add(sampled_P_3D_[i].pos());
  }
  kd_tree_.finalize();
}

bool Match4PCSBase::ComputeRigidTransformation(
        const std::array<Point3D, 4>& ref,
        const std::array<Point3D, 4>& candidate,
        const Eigen::Matrix<Scalar, 3, 1>& centroid1,
        Eigen::Matrix<Scalar, 3, 1> centroid2,
        Scalar max_angle,
        Eigen::Ref<MatrixType> transform,
        Scalar& rms_,
        bool computeScale ) const {

  rms_ = kLargeNumber;

  Scalar kSmallNumber = 1e-6;

  // We only use the first 3 pairs. This simplifies the process considerably
  // because it is the planar case.

  const VectorType& p0 = ref[0].pos();
  const VectorType& p1 = ref[1].pos();
  const VectorType& p2 = ref[2].pos();
        VectorType  q0 = candidate[0].pos();
        VectorType  q1 = candidate[1].pos();
        VectorType  q2 = candidate[2].pos();

  Scalar scaleEst (1.);

  // Compute scale factor if needed
  if (computeScale){
      const VectorType& p3 = ref[3].pos();
      const VectorType& q3 = candidate[3].pos();

      const Scalar ratio1 = (p1 - p0).norm() / (q1 - q0).norm();
      const Scalar ratio2 = (p3 - p2).norm() / (q3 - q2).norm();

      const Scalar ratioDev  = std::abs(ratio1/ratio2 - Scalar(1.));  // deviation between the two
      const Scalar ratioMean = (ratio1+ratio2)/Scalar(2.);            // mean of the two

      if ( ratioDev > Scalar(0.1) )
          return kLargeNumber;

      //Log<LogLevel::Verbose>( ratio1, " ", ratio2, " ", ratioDev, " ", ratioMean);
      scaleEst = ratioMean;

      // apply scale factor to q
      q0 = q0*scaleEst;
      q1 = q1*scaleEst;
      q2 = q2*scaleEst;
      centroid2 *= scaleEst;
  }

  VectorType vector_p1 = p1 - p0;
  if (vector_p1.squaredNorm() == 0) return kLargeNumber;
  vector_p1.normalize();
  VectorType vector_p2 = (p2 - p0) - ((p2 - p0).dot(vector_p1)) * vector_p1;
  if (vector_p2.squaredNorm() == 0) return kLargeNumber;
  vector_p2.normalize();
  VectorType vector_p3 = vector_p1.cross(vector_p2);

  VectorType vector_q1 = q1 - q0;
  if (vector_q1.squaredNorm() == 0) return kLargeNumber;
  vector_q1.normalize();
  VectorType vector_q2 = (q2 - q0) - ((q2 - q0).dot(vector_q1)) * vector_q1;
  if (vector_q2.squaredNorm() == 0) return kLargeNumber;
  vector_q2.normalize();
  VectorType vector_q3 = vector_q1.cross(vector_q2);

  //cv::Mat rotation = cv::Mat::eye(3, 3, CV_64F);
  Eigen::Matrix<Scalar, 3, 3> rotation = Eigen::Matrix<Scalar, 3, 3>::Identity();

  Eigen::Matrix<Scalar, 3, 3> rotate_p;
  rotate_p.row(0) = vector_p1;
  rotate_p.row(1) = vector_p2;
  rotate_p.row(2) = vector_p3;

  Eigen::Matrix<Scalar, 3, 3> rotate_q;
  rotate_q.row(0) = vector_q1;
  rotate_q.row(1) = vector_q2;
  rotate_q.row(2) = vector_q3;

  rotation = rotate_p.transpose() * rotate_q;

  // Discard singular solutions. The rotation should be orthogonal.
  if (((rotation * rotation).diagonal().array() - Scalar(1) > kSmallNumber).any())
      return false;

  //FIXME
  if (max_angle >= 0) {
      // Discard too large solutions (todo: lazy evaluation during boolean computation
      if (! (
                  std::abs(std::atan2(rotation(2, 1), rotation(2, 2)))
                  <= max_angle &&

                  std::abs(std::atan2(-rotation(2, 0),
                                      std::sqrt(std::pow(rotation(2, 1),2) +
                                                std::pow(rotation(2, 2),2))))
                  <= max_angle &&

                  std::abs(atan2(rotation(1, 0), rotation(0, 0)))
                  <= max_angle
             ))
          return false;
  }

  //FIXME
  // Compute rms and return it.
  rms_ = Scalar(0.0);
  {
      VectorType first, transformed;

      //cv::Mat first(3, 1, CV_64F), transformed;
      for (int i = 0; i < 3; ++i) {
          first = scaleEst*candidate[i].pos() - centroid2;
          transformed = rotation * first;
          rms_ += (transformed - ref[i].pos() + centroid1).norm();
      }
  }

  rms_ /= Scalar(ref.size());

  Eigen::Transform<Scalar, 3, Eigen::Affine> etrans (Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity());
  transform = etrans
      .scale(scaleEst)
      .translate(centroid1)
      .rotate(rotation)
      .translate(-centroid2)
      .matrix();

  return true;
}

// Verify a given transformation by computing the number of points in P at
// distance at most (normalized) delta from some point in Q. In the paper
// we describe randomized verification. We apply deterministic one here with
// early termination. It was found to be fast in practice.
Match4PCSBase::Scalar
Match4PCSBase::Verify(const Eigen::Ref<const MatrixType> &mat) const {
  using RangeQuery = GlobalRegistration::KdTree<Scalar>::RangeQuery<>;

#ifdef TEST_GLOBAL_TIMINGS
    Timer t_verify (true);
#endif

  // We allow factor 2 scaling in the normalization.
  const Scalar epsilon = options_.delta;
  std::atomic_uint good_points(0);
  const size_t number_of_points = sampled_Q_3D_.size();
  const size_t terminate_value = best_LCP_ * number_of_points;

  const Scalar sq_eps = epsilon*epsilon;

  for (size_t i = 0; i < number_of_points; ++i) {

    // Use the kdtree to get the nearest neighbor
#ifdef TEST_GLOBAL_TIMINGS
    Timer t (true);
#endif

    RangeQuery query;
    query.queryPoint = (mat * sampled_Q_3D_[i].pos().homogeneous()).head<3>();
    query.sqdist     = sq_eps;

    GlobalRegistration::KdTree<Scalar>::Index resId =
    kd_tree_.doQueryRestrictedClosestIndex( query );

#ifdef TEST_GLOBAL_TIMINGS
    kdTreeTime += Scalar(t.elapsed().count()) / Scalar(CLOCKS_PER_SEC);
#endif

    if ( resId != GlobalRegistration::KdTree<Scalar>::invalidIndex() ) {
//      Point3D& q = sampled_P_3D_[near_neighbor_index[0]];
//      bool rgb_good =
//          (p.rgb()[0] >= 0 && q.rgb()[0] >= 0)
//              ? cv::norm(p.rgb() - q.rgb()) < options_.max_color_distance
//              : true;
//      bool norm_good = norm(p.normal()) > 0 && norm(q.normal()) > 0
//                           ? fabs(p.normal().ddot(q.normal())) >= cos_dist
//                           : true;
//      if (rgb_good && norm_good) {
        good_points++;
//      }
    }

    // We can terminate if there is no longer chance to get better than the
    // current best LCP.
    if (number_of_points - i + good_points < terminate_value) {
      break;
    }
  }

#ifdef TEST_GLOBAL_TIMINGS
  verifyTime += Scalar(t_verify.elapsed().count()) / Scalar(CLOCKS_PER_SEC);
#endif
  return Scalar(good_points) / Scalar(number_of_points);
}

} // namespace Super4PCS


/*
# linalgCpp
Collection of basic tools or examples for linear algebra in c++
Original code from https://github.com/leopoldcambier/linalgCpp
*/

#ifndef MMIO_HPP
#define MMIO_HPP

#include <assert.h>

#include <iostream>
#include <fstream>
#include <algorithm>
#include <complex>
#include <string>

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace mmio {

    /** Read a line real / cplx **/
    template<typename V>
    V read_entry(std::istringstream& vals) {
        V v;
        vals >> v;
        return v;
    };
    template<>
    std::complex<double> read_entry(std::istringstream& vals) {
        double v1, v2;
        vals >> v1 >> v2;
        return std::complex<double>(v1, v2);
    };
    template<>
    std::complex<float> read_entry(std::istringstream& vals) {
        float v1, v2;
        vals >> v1 >> v2;
        return std::complex<float>(v1, v2);
    };

    /** Read a line real / cplx of coordinate values **/
    template<typename V, typename I>
    Eigen::Triplet<V,I> read_line(std::istringstream& vals) {
        I i, j;
        V v;
        vals >> i >> j >> v;
        return Eigen::Triplet<V,I>(i-1,j-1,v);
    };
    template<>
    Eigen::Triplet<std::complex<double>,int> read_line(std::istringstream& vals) {
        int i, j; double v1, v2;
        vals >> i >> j >> v1 >> v2;
        return Eigen::Triplet<std::complex<double>,int>(i-1,j-1,std::complex<double>(v1, v2));
    };
    template<>
    Eigen::Triplet<std::complex<float>,int> read_line(std::istringstream& vals) {
        int i, j; float v1, v2;
        vals >> i >> j >> v1 >> v2;
        return Eigen::Triplet<std::complex<float>,int>(i-1,j-1,std::complex<float>(v1, v2));
    };

    /** Write a line real / cplx of coordinate values **/
    template<typename V, typename I>
    std::string get_line(I i, I j, V v) {
        return std::to_string(i+1) + " " + std::to_string(j+1) + " " + std::to_string(v);
    };
    template<>
    std::string get_line(int i, int j, std::complex<double> v) {
        return std::to_string(i+1) + " " + std::to_string(j+1) + " " + std::to_string(v.real()) + " " + std::to_string(v.imag());
    };
    template<>
    std::string get_line(int i, int j, std::complex<float> v) {
        return std::to_string(i+1) + " " + std::to_string(j+1) + " " + std::to_string(v.real()) + " " + std::to_string(v.imag());
    };

    /** Symmetric (real) / hermitian (cplx) **/
    template<typename V, typename I>
    Eigen::Triplet<V,I> symmetric(Eigen::Triplet<V,I>& a) {
        return Eigen::Triplet<V,I>(a.col(), a.row(), a.value());
    }
    template<>
    Eigen::Triplet<std::complex<double>,int> symmetric(Eigen::Triplet<std::complex<double>,int>& a) {
        return Eigen::Triplet<std::complex<double>,int>(a.col(), a.row(), std::conj(a.value()));
    }
    template<>
    Eigen::Triplet<std::complex<float>,int> symmetric(Eigen::Triplet<std::complex<float>,int>& a) {
        return Eigen::Triplet<std::complex<float>,int>(a.col(), a.row(), std::conj(a.value()));
    }

    /** Skew-symmetric (real only, really) **/
    template<typename V, typename I>
    Eigen::Triplet<V,I> skew_symmetric(Eigen::Triplet<V,I>& a) {
        return Eigen::Triplet<V,I>(a.col(), a.row(), - a.value());
    }

    enum class format {coordinate, array};
    enum class type {real, integer, complex, pattern};
    enum class property {general, symmetric, hermitian, skew_symmetric};

    std::string prop2str(property p) {
        if(p == property::general) return "general";
        else if(p == property::symmetric) return "symmetric";
        else if(p == property::hermitian) return "hermitian";
        else return "skew_symmetric";
    }

    template<typename V>
    struct V2str {
        static std::string value() {
            if (std::is_same<V,std::complex<double>>::value || std::is_same<V,std::complex<float>>::value) {
                return "complex";
            } else if (std::is_integral<V>::value) {
                return "integer";
            } else {
                return "real";
            }
        }
    };

    struct Header {
        bool bannerOK;
        bool objectOK;
        format f;
        type   t;
        property p;
        Header(std::istringstream& header) {
            std::string banner, object, format, type, properties;
            header >> banner >> object >> format >> type >> properties;
            std::transform(object.begin(),      object.end(),       object.begin(),       ::tolower);
            std::transform(format.begin(),      format.end(),       format.begin(),       ::tolower);
            std::transform(type.begin(),        type.end(),         type.begin(),         ::tolower);
            std::transform(properties.begin(),  properties.end(),   properties.begin(),   ::tolower);
            this->bannerOK = ! banner.compare("%%MatrixMarket");
            this->objectOK = ! object.compare("matrix");
            assert(this->bannerOK);
            assert(this->objectOK);
            if(! format.compare("coordinate")) {
                this->f = format::coordinate;
            } else if(! format.compare("array")) {
                this->f = format::array;
            } else {
                assert(false);
            }
            if (! type.compare("real")) {
                this->t = type::real;
            } else if (! type.compare("integer")) {
                this->t = type::integer;
            } else if (! type.compare("complex")) {
                this->t = type::complex;
            } else if (! type.compare("pattern")) {
                this->t = type::pattern;
            } else {
                assert(false);
            }
            if (! properties.compare("general")) {
                this->p = property::general;
            } else if (! properties.compare("symmetric")) {
                this->p = property::symmetric;
            } else if (! properties.compare("skew-symmetric")) {
                this->p = property::skew_symmetric;
            } else if (! properties.compare("hermitian")) {
                this->p = property::hermitian;
            } else {
                assert(false);
            }
        }
    };

    /**
     * Read a sparse matrix in MM format
     */
    template<typename V, typename I>
    Eigen::SparseMatrix<V, Eigen::ColMajor, I> sp_mmread(std::string filename) {
        std::ifstream mfile(filename);
        if (mfile.is_open()) {
            std::string line;
            /** Header **/
            std::getline(mfile, line);
            std::istringstream header(line);
            Header h(header);
            assert(h.f == format::coordinate);
            // assert(h.t != type::pattern);
            /** Find M N K row **/
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                else break;
            }
            I M, N, K;
            std::istringstream MNK(line);
            MNK >> M >> N >> K;
            std::vector<Eigen::Triplet<V,I>> data;
            if(h.p != property::general) {
                data.reserve(2*K);
            } else {
                data.reserve(K);
            }
            /** Read data **/
            int lineread = 0;
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                if (h.t == type::pattern) line = line + " 1";
                std::istringstream vals(line);
                Eigen::Triplet<V,I> dataline = read_line<V,I>(vals);
                data.push_back(dataline);
                if(dataline.row() != dataline.col() && (h.p == property::symmetric || h.p == property::hermitian)) {
                    Eigen::Triplet<V,I> dataline2 = symmetric(dataline);
                    data.push_back(dataline2);
                }
                if(dataline.row() != dataline.col() && (h.p == property::skew_symmetric)) {
                    Eigen::Triplet<V,I> dataline2 = skew_symmetric(dataline);
                    data.push_back(dataline2);
                }
                if(h.p == property::skew_symmetric && dataline.row() == dataline.col()) {
                    assert(false);
                }
                if(h.p != property::general && dataline.row() < dataline.col()) {
                    assert(false);
                }
                lineread ++;
            }
            assert(lineread == K);
            Eigen::SparseMatrix<V, Eigen::ColMajor, I> A(M, N);
            A.setFromTriplets(data.begin(), data.end());
            return std::move(A);
        } else {
            throw("Couldn't open file");
        }
    }

    /**
     * Reads a dense matrix in MM format
     */
    template<typename V>
    Eigen::Matrix<V, Eigen::Dynamic, Eigen::Dynamic> dense_mmread(std::string filename) {
        std::ifstream mfile(filename);
        if (mfile.is_open()) {
            std::string line;
            /** Header **/
            std::getline(mfile, line);
            std::istringstream header(line);
            Header h(header);
            assert(h.p == property::general);
            assert(h.f == format::array);
            assert(h.t != type::pattern);
            /** Find M N row **/
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                else break;
            }
            int M, N;
            std::istringstream MNK(line);
            MNK >> M >> N;
            Eigen::Matrix<V, Eigen::Dynamic, Eigen::Dynamic> A(M, N);
            /** Read data **/
            int lineread = 0;
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                std::istringstream vals(line);
                V v = read_entry<V>(vals);
                int i = (lineread % M);
                int j = (lineread / M);
                A(i,j) = v;
                lineread ++;
            }
            assert(lineread == M*N);
            return std::move(A);
        } else {
            throw("Couldn't open file");
        }
    }

    /**
     * Reads a dense vector in MM format
     */
    template<typename V>
    Eigen::Matrix<V, Eigen::Dynamic, 1> vector_mmread(std::string filename) {
        std::ifstream mfile(filename);
        if (mfile.is_open()) {
            std::string line;
            /** Header **/
            std::getline(mfile, line);
            std::istringstream header(line);
            Header h(header);
            assert(h.p == property::general);
            assert(h.f == format::array);
            assert(h.t != type::pattern);
            /** Find M N row **/
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                else break;
            }
            int M, N;
            std::istringstream MNK(line);
            MNK >> M >> N;
            assert(N ==1);
            Eigen::Matrix<V, Eigen::Dynamic, 1> b(M);
            /** Read data **/
            int lineread = 0;
            while(std::getline(mfile, line)) {
                if(line.size() == 0 || line[0] == '%') continue;
                std::istringstream vals(line);
                V v = read_entry<V>(vals);
                int i = (lineread % M);
                // int j = (lineread / M);
                // A(i,j) = v;
                b(i) = v;
                lineread ++;
            }
            assert(lineread == M*N);
            return std::move(b);
        } else {
            throw("Couldn't open file");
        }
    }

    /**
     * Writes a sparse matrix in MM format, using the optional property p.
     * Wether the matrix satisfies or not p is not verified
     */
    template<typename V, int S, typename I>
    void sp_mmwrite(std::string filename, Eigen::SparseMatrix<V,S,I> mat, property p = property::general) {
        std::ofstream mfile;
        mfile.open (filename);
        if (mfile.is_open()) {
            std::string type = V2str<V>::value();
            std::string prop = prop2str(p);
            mfile << "%%MatrixMarket matrix coordinate " << type << " " << prop << "\n";
            int NNZ = 0;
            for (int k = 0; k < mat.outerSize(); ++k) {
                for (typename Eigen::SparseMatrix<V,S,I>::InnerIterator it(mat,k); it; ++it) {
                    if( (p == property::symmetric || p == property::hermitian) && (it.row() < it.col()) ) continue;
                    if( (p == property::skew_symmetric) && (it.row() <= it.col()) ) continue;
                    NNZ ++;
                }
            }
            mfile << mat.rows() << " " << mat.cols() << " " << NNZ << "\n";
            for (int k = 0; k < mat.outerSize(); ++k) {
                for (typename Eigen::SparseMatrix<V,S,I>::InnerIterator it(mat,k); it; ++it) {
                    if( (p == property::symmetric || p == property::hermitian) && (it.row() < it.col()) ) continue;
                    if( (p == property::skew_symmetric) && (it.row() <= it.col()) ) continue;
                    mfile << get_line(it.row(), it.col(), it.value()) << "\n";
                }
            }
        } else {
            throw("Couldn't open file");
        }
    }

    /**
     * Writes a dense matrix in MM format, using the optional property p.
     * Wether the matrix satisfies or not p is not verified
     */
    template<typename V>
    void dense_mmwrite(std::string filename, Eigen::Matrix<V, Eigen::Dynamic, Eigen::Dynamic> mat, property p = property::general) {
        std::ofstream mfile;
        mfile.open (filename);
        if (mfile.is_open()) {
            std::string type = V2str<V>::value();
            std::string prop = prop2str(p);
            mfile << "%%MatrixMarket matrix array " << type << " " << prop << "\n";
            mfile << mat.rows() << " " << mat.cols() << "\n";
            for(int j = 0; j < mat.cols(); j++) {
                for(int i = 0; i < mat.rows(); i++) {
                    if( (p == property::symmetric || p == property::hermitian) && (i < j) ) continue;
                    if( (p == property::skew_symmetric) && (i <= j) ) continue;
                    mfile << mat(i,j) << "\n";
                }
            }
        } else {
            throw("Couldn't open file");
        }
    }

}

#endif


// Boost.Geometry

// Copyright (c) 2017 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_FORMULAS_ELLIPTIC_ARC_LENGTH_HPP
#define BOOST_GEOMETRY_FORMULAS_ELLIPTIC_ARC_LENGTH_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

#include <boost/geometry/formulas/flattening.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief Compute the arc length of an ellipse.
*/

template <typename CT, unsigned int Order = 1>
class elliptic_arc_length
{

public :

    struct result
    {
        result()
            : distance(0)
            , meridian(false)
        {}

        CT distance;
        bool meridian;
    };

    template <typename T, typename Spheroid>
    static result apply(T lon1, T lat1, T lon2, T lat2, Spheroid const& spheroid)
    {
        result res;

        CT c0 = 0;
        CT pi = math::pi<CT>();
        CT half_pi = pi/CT(2);
        CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);

        if (lat1 > lat2)
        {
            std::swap(lat1, lat2);
        }

        if ( math::equals(diff, c0) ||
            (math::equals(lat2, half_pi) && math::equals(lat1, -half_pi)) )
        {
            // single meridian not crossing pole
            res.distance = apply(lat2, spheroid) - apply(lat1, spheroid);
            res.meridian = true;
        }

        if (math::equals(math::abs(diff), pi))
        {
            // meridian crosses pole
            CT lat_sign = 1;
            if (lat1+lat2 < c0)
            {
                lat_sign = CT(-1);
            }
            res.distance = math::abs(lat_sign * CT(2) * apply(half_pi, spheroid)
                               - apply(lat1, spheroid) - apply(lat2, spheroid));
            res.meridian = true;
        }
        return res;
    }

    // Distance computation on meridians using series approximations
    // to elliptic integrals. Formula to compute distance from lattitude 0 to lat
    // https://en.wikipedia.org/wiki/Meridian_arc
    // latitudes are assumed to be in radians and in [-pi/2,pi/2]
    template <typename T, typename Spheroid>
    static CT apply(T lat, Spheroid const& spheroid)
    {
        CT const a = get_radius<0>(spheroid);
        CT const f = formula::flattening<CT>(spheroid);
        CT n = f / (CT(2) - f);
        CT M = a/(1+n);
        CT C0 = 1;

        if (Order == 0)
        {
           return M * C0 * lat;
        }

        CT C2 = -1.5 * n;

        if (Order == 1)
        {
            return M * (C0 * lat + C2 * sin(2*lat));
        }

        CT n2 = n * n;
        C0 += .25 * n2;
        CT C4 = 0.9375 * n2;

        if (Order == 2)
        {
            return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat));
        }

        CT n3 = n2 * n;
        C2 += 0.1875 * n3;
        CT C6 = -0.729166667 * n3;

        if (Order == 3)
        {
            return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat)
                      + C6 * sin(6*lat));
        }

        CT n4 = n2 * n2;
        C4 -= 0.234375 * n4;
        CT C8 = 0.615234375 * n4;

        if (Order == 4)
        {
            return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat)
                      + C6 * sin(6*lat) + C8 * sin(8*lat));
        }

        CT n5 = n4 * n;
        C6 += 0.227864583 * n5;
        CT C10 = -0.54140625 * n5;

        // Order 5 or higher
        return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat)
                  + C6 * sin(6*lat) + C8 * sin(8*lat) + C10 * sin(10*lat));

    }

    // Iterative method to elliptic arc length based on
    // http://www.codeguru.com/cpp/cpp/algorithms/article.php/c5115/
    // Geographic-Distance-and-Azimuth-Calculations.htm
    // latitudes are assumed to be in radians and in [-pi/2,pi/2]
    template <typename T1, typename T2, typename Spheroid>
    CT interative_method(T1 lat1,
                         T2 lat2,
                         Spheroid const& spheroid)
    {
        CT result = 0;
        CT const zero = 0;
        CT const one = 1;
        CT const c1 = 2;
        CT const c2 = 0.5;
        CT const c3 = 4000;

        CT const a = get_radius<0>(spheroid);
        CT const f = formula::flattening<CT>(spheroid);

        // how many steps to use

        CT lat1_deg = lat1 * geometry::math::r2d<CT>();
        CT lat2_deg = lat2 * geometry::math::r2d<CT>();

        int steps = c1 + (c2 + (lat2_deg > lat1_deg) ? CT(lat2_deg - lat1_deg)
                                                     : CT(lat1_deg - lat2_deg));
        steps = (steps > c3) ? c3 : steps;

        //std::cout << "Steps=" << steps << std::endl;

        CT snLat1 = sin(lat1);
        CT snLat2 = sin(lat2);
        CT twoF   = 2 * f - f * f;

        // limits of integration
        CT x1 = a * cos(lat1) /
                sqrt(1 - twoF * snLat1 * snLat1);
        CT x2 = a * cos(lat2) /
                sqrt(1 - twoF * snLat2 * snLat2);

        CT dx = (x2 - x1) / (steps - one);
        CT x, y1, y2, dy, dydx;
        CT adx = (dx < zero) ? -dx : dx;    // absolute value of dx

        CT a2 = a * a;
        CT oneF = 1 - f;

        // now loop through each step adding up all the little
        // hypotenuses
        for (int i = 0; i < (steps - 1); i++){
            x = x1 + dx * i;
            dydx = ((a * oneF * sqrt((one - ((x+dx)*(x+dx))/a2))) -
                    (a * oneF * sqrt((one - (x*x)/a2)))) / dx;
            result += adx * sqrt(one + dydx*dydx);
        }

        return result;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_ELLIPTIC_ARC_LENGTH_HPP


#include "path_finding.h"

#include "error_handling.h"
#include "rover.h"
#include "geometry.h"
#include "robot_configuration.h"

#include <opencv2/opencv.hpp>

#include <vector>
#include <queue>

#include <boost/functional/hash.hpp>
#include <boost/optional.hpp>
#include <boost/range/algorithm/reverse.hpp>

// A* path finding algorithm
template<typename TNode, typename T, typename FMinimalNodeCost, typename FForEachNeighbor, typename FIsGoal>
boost::optional<TNode> GenericAStar(rbt::pose<T> const& poseStart,
    rbt::point<T> const& ptEnd,
    FMinimalNodeCost MinimalNodeCost,
    FForEachNeighbor ForEachNeighbor,
	FIsGoal IsGoal
) {
    auto GreaterDistance = [&](TNode const& lhs, TNode const& rhs) noexcept {
        return (lhs.Position() - ptEnd).Abs() + lhs.m_fCost > (rhs.Position() - ptEnd).Abs() + rhs.m_fCost;
    };

    std::priority_queue<TNode, std::vector<TNode>, decltype(GreaterDistance)> queue(GreaterDistance);

    TNode nodeStart(poseStart);
    queue.push(nodeStart);
    MinimalNodeCost(nodeStart) = nodeStart.m_fCost;

    while(!queue.empty()) {
        auto const nodeTop = queue.top();
        queue.pop();

        if(MinimalNodeCost(nodeTop) < nodeTop.m_fCost) continue;

        if(IsGoal(nodeTop, ptEnd)) return nodeTop;

        ForEachNeighbor(
            nodeTop,
            [&](TNode const& nodeNeighbor) {
                if(rbt::assign_min(MinimalNodeCost(nodeNeighbor), nodeNeighbor.m_fCost)) {
                    queue.push(nodeNeighbor);
                }
            });
    }

    return boost::none;
}

std::vector<rbt::point<double>> FindPath(cv::Mat matn, rbt::pose<double> const& posefStart, rbt::point<double> const& ptfEnd) {
    cv::Mat matnEroded;
    auto const nMaxExtent = std::max(c_nRobotWidth/c_nScale, c_nRobotHeight/c_nScale);
    cv::erode(
        matn,
        matnEroded,
        cv::Mat::ones(nMaxExtent, nMaxExtent, CV_8U)
    );

    // Include costs of traveling close to an obstacle in calculation
    cv::Mat matnGauss;
    auto const nMaxExtentOdd = 2*nMaxExtent + 1;
    cv::GaussianBlur(matnEroded, matnGauss, cv::Size(nMaxExtentOdd, nMaxExtentOdd), 0, 0);

    float aanMinimalCost[c_nMapExtent][c_nMapExtent];
    std::fill_n(&aanMinimalCost[0][0], c_nMapExtent*c_nMapExtent, std::numeric_limits<float>::max());

    struct node {
        node() {}
        node(rbt::pose<int> const& pose) : m_pt(pose.m_pt), m_fCost(0) {}
        node(rbt::point<int> const& pt, float fCost) : m_pt(pt), m_fCost(fCost) {}

		rbt::point<int> Position() const { return m_pt; }

        rbt::point<int> m_pt; // in grid coordinates
        float m_fCost;
    };

    auto MinimalNodeCost = [&](node const& node) noexcept -> float& {
        return aanMinimalCost[node.m_pt.x][node.m_pt.y];
    };

    auto ForEachNeighbor = [&](node const& n, auto fn) noexcept {
        for(int x = -1; x <= 1; ++x) {
            for(int y = -1; y <= 1; ++y) {
                auto const ptNext = n.m_pt + rbt::size<int>(x, y);
                if((0!=x || 0!=y)
                && 128<matnGauss.at<std::uint8_t>(ptNext.y, ptNext.x)) {
                    fn(node(
                        ptNext,
                        n.m_fCost + (0==x || 0==y ? 1.0f : M_SQRT2) * (1 + (255 - matnGauss.at<std::uint8_t>(ptNext.y, ptNext.x))/10)
                    ));
                }
            }
        }
    };
	auto IsGoal = [](node const& n, rbt::point<int> const& ptnEnd) {
		return n.m_pt == ptnEnd;
	};

	auto const posenStart = ToGridCoordinate(posefStart);

    std::vector<rbt::point<double>> vecptfResult;
    if(auto onode = GenericAStar<node>(posenStart, ToGridCoordinate(ptfEnd), MinimalNodeCost, ForEachNeighbor, IsGoal)) {
        vecptfResult.emplace_back(ptfEnd);
        auto const ptnStart = posenStart.m_pt;
        for(auto nodePrev = *onode; nodePrev.m_pt!=ptnStart; ) {
            float fMinCost = std::numeric_limits<float>::max();
            node nodeMin;
            ForEachNeighbor(
                nodePrev,
                [&](node const& node) {
                    if(rbt::assign_min(fMinCost, MinimalNodeCost(node))) {
                        nodeMin = node;
                    }
                }
            );

            nodePrev = nodeMin;
            vecptfResult.emplace_back(ToWorldCoordinate(rbt::point<double>(nodeMin.m_pt)));
        }
    }
    return vecptfResult;
}

namespace {
    struct config_space_node {
		config_space_node() {}
        config_space_node(rbt::pose<double> const& pose) : m_pose(pose) {}

		rbt::point<double> Position() const { return m_pose.m_pt; }

		int RoundedYaw() const { return (int)std::round(m_pose.m_fYaw*100); }
		rbt::point<int> RoundedPosition() const {
			return rbt::point<int>((int)std::round(m_pose.m_pt.x), (int)std::round(m_pose.m_pt.y));
		}
        rbt::pose<double> m_pose;

		// as used for robot commands, encoder ticks / s?
        int m_nSpeedLeft = 0;
        int m_nSpeedRight = 0;

        // path cost
        float m_fCost = 0;

		config_space_node const* m_pnodeParent = nullptr;

        friend bool operator==(config_space_node const& lhs, config_space_node const& rhs) {
            return lhs.RoundedPosition()==rhs.RoundedPosition()
				&& lhs.RoundedYaw()==rhs.RoundedYaw()
                && lhs.m_nSpeedLeft==rhs.m_nSpeedLeft
                && rhs.m_nSpeedRight==rhs.m_nSpeedRight;
        }
    };

	constexpr int c_nSpeedStep = 100;
	constexpr double c_fTimeStep = 0.2; // s
	const float c_fHighTravelDistance = encoderTicksToCm(c_nMaxSpeed*0.8*c_fTimeStep);
}

namespace std {
	template <> struct hash<config_space_node> {
		size_t operator()(config_space_node const& node) const {
			std::size_t seed = 0;
			auto pt = node.RoundedPosition();
			boost::hash_combine(seed, pt.x);
			boost::hash_combine(seed, pt.y);
			boost::hash_combine(seed, node.RoundedYaw());
			boost::hash_combine(seed, node.m_nSpeedLeft);
			boost::hash_combine(seed, node.m_nSpeedRight);
			return seed;
		}
	};
}

std::vector<rbt::pose<double>> PathConfigurationSpace(cv::Mat matn, rbt::pose<double> const& posefStart, rbt::point<double> const& ptfEnd) {
    auto const vecptf = FindPath(matn, posefStart, ptfEnd);

	// TODO: Share code?
	cv::Mat matnEroded;
    auto const nMaxExtent = std::max(c_nRobotWidth/c_nScale, c_nRobotHeight/c_nScale);
    cv::erode(
        matn,
        matnEroded,
        cv::Mat::ones(nMaxExtent, nMaxExtent, CV_8U)
    );

    cv::Mat matnGauss;
    auto const nMaxExtentOdd = 2*nMaxExtent + 1;
    cv::GaussianBlur(matnEroded, matnGauss, cv::Size(nMaxExtentOdd, nMaxExtentOdd), 0, 0);

	cv::Mat matnPath = cv::Mat::zeros(matn.size(), CV_8U);
	rbt::point<int> ptnPrev = ToGridCoordinate(vecptf.front());
	boost::for_each(vecptf, [&](rbt::point<double> const& ptf) {
		auto const ptnGrid = ToGridCoordinate(ptf);
        cv::line(matnPath, ptnPrev, ptnGrid, cv::Scalar(255), 50/c_nScale);
        ptnPrev = ptnGrid;
	});

	int cExpanded = 0;
	std::unordered_map<config_space_node, float> mapnodefCosts;
	auto onode = GenericAStar<config_space_node>(
		posefStart,
		// Calculating a longer path in configuration space takes too much time to compute due to large state space.
		*(vecptf.end() - 80),
		[&](config_space_node const& node) noexcept -> float& {
			return mapnodefCosts.emplace(node, std::numeric_limits<float>::max()).first->second;
		},
		[&](config_space_node const& node, auto fn) noexcept {
			++cExpanded;

			auto const pnodeParent = [&] {
				auto const itpair = mapnodefCosts.find(node);
				ASSERT(itpair!=mapnodefCosts.end());
				return &itpair->first;
			}();

			for(int nStepLeft = -1; nStepLeft <= 1; ++nStepLeft) {
				// [decelerate both, left, right, no change, accelerate right, left, both]
				for(int nStepRight = -1; nStepRight <= 1; ++nStepRight) {
					// -> Calculate new positions / angle
					auto nodeNeighbor = node;
					nodeNeighbor.m_pnodeParent = pnodeParent;
					nodeNeighbor.m_nSpeedLeft += nStepLeft * c_nSpeedStep;
					nodeNeighbor.m_nSpeedRight += nStepRight * c_nSpeedStep;

					if(std::abs(nodeNeighbor.m_nSpeedLeft)<=c_nMaxSpeed
					&& std::abs(nodeNeighbor.m_nSpeedRight)<=c_nMaxSpeed
					&& std::abs(nodeNeighbor.m_nSpeedRight-nodeNeighbor.m_nSpeedLeft)<=400)
					{
						nodeNeighbor.m_pose = UpdatePose(
							node.m_pose,
							nodeNeighbor.m_nSpeedLeft*c_fTimeStep,
							nodeNeighbor.m_nSpeedRight*c_fTimeStep
						);

						cv::LineIterator itpt(
							matn,
							ToGridCoordinate(node.Position()),
							ToGridCoordinate(nodeNeighbor.Position())
						);

						float fWeightedCost = 0;
						for(int i = 0; i < itpt.count; ++i, ++itpt) {
							auto const pt = rbt::point<int>(itpt.pos());
							// -> check if still in range of shortest path
							if(matnPath.at<std::uint8_t>(pt.y, pt.x)<255) {
								goto outside_range;
							}

							// integrate costs over node.m_pt -> nodeNeighbor.m_pt
							fWeightedCost += std::pow((255.0 - matnGauss.at<std::uint8_t>(pt.y, pt.x))/30, 2);
						}
						{
							fWeightedCost = std::max(1.f, fWeightedCost/itpt.count);
							float const fDistance = (nodeNeighbor.Position()-node.Position()).Abs();
							// Penalize several short moves, i.e., slow moves
							nodeNeighbor.m_fCost += fDistance * std::max(1.0, std::pow(c_fHighTravelDistance/fDistance, 2)) * fWeightedCost;
						}

						fn(nodeNeighbor);

outside_range:
						;
					}
				}
			}
		},
		[](config_space_node const& node, rbt::point<double> const ptfEnd) {
			return (node.m_pose.m_pt - ptfEnd).SqrAbs() < c_nScale*c_nScale;
		}
	);

	std::cout << "Discovered " << mapnodefCosts.size() << " states, " << cExpanded << " expanded." << std::endl;
	std::vector<rbt::pose<double>> vecposef;
	if(onode) {
		auto const* pnodeNext = &*onode;
		while(pnodeNext) {
			std::cout << pnodeNext->m_pose <<
			" (" << pnodeNext->m_nSpeedLeft << ", " << pnodeNext->m_nSpeedRight << ") " << std::endl;

			vecposef.emplace_back(pnodeNext->m_pose);
			pnodeNext = pnodeNext->m_pnodeParent;
		}
		boost::reverse(vecposef);
	}
	return vecposef;
}


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

// These functions are largely based off the the Ceres solver polynomial
// functions which are not available through the public interface. The license
// is below:
//
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
//   used to endorse or promote products derived from this software without
//   specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: moll.markus@arcor.de (Markus Moll)
//         sameeragarwal@google.com (Sameer Agarwal)

#include "theia/math/polynomial.h"

#include <Eigen/Core>
#include <glog/logging.h>

#include <cmath>
#include <limits>

#include "theia/math/find_polynomial_roots_companion_matrix.h"

namespace theia {

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::VectorXcd;

bool FindPolynomialRoots(const VectorXd& polynomial,
                         VectorXd* real,
                         VectorXd* imaginary) {
  return FindPolynomialRootsCompanionMatrix(polynomial, real, imaginary);
}

// Remove leading terms with zero coefficients.
VectorXd RemoveLeadingZeros(const VectorXd& polynomial_in) {
  int i = 0;
  while (i < (polynomial_in.size() - 1) && polynomial_in(i) == 0) {
    ++i;
  }
  return polynomial_in.tail(polynomial_in.size() - i);
}

VectorXd DifferentiatePolynomial(const VectorXd& polynomial) {
  const int degree = polynomial.rows() - 1;
  CHECK_GE(degree, 0);

  // Degree zero polynomials are constants, and their derivative does
  // not result in a smaller degree polynomial, just a degree zero
  // polynomial with value zero.
  if (degree == 0) {
    return VectorXd::Zero(1);
  }

  VectorXd derivative(degree);
  for (int i = 0; i < degree; ++i) {
    derivative(i) = (degree - i) * polynomial(i);
  }

  return derivative;
}

VectorXd MultiplyPolynomials(const VectorXd& poly1, const VectorXd& poly2) {
  VectorXd multiplied_poly = VectorXd::Zero(poly1.size() + poly2.size() - 1);;
  for (int i = 0; i < poly1.size(); i++) {
    for (int j = 0; j < poly2.size(); j++) {
      multiplied_poly.reverse().operator()(i + j) +=
          poly1.reverse()(i) * poly2.reverse()(j);
    }
  }
  return multiplied_poly;
}

void DividePolynomial(const VectorXd& polynomial,
                      const VectorXd& divisor,
                      VectorXd* quotient,
                      VectorXd* remainder) {
  // If the divisor is higher degree than the polynomial then it cannot be
  // divided so we simply return the remainder.
  if (polynomial.size() < divisor.size()) {
    *quotient = VectorXd::Zero(1);
    *remainder = polynomial;
    return;
  }

  VectorXd numerator = RemoveLeadingZeros(polynomial);
  VectorXd denominator;
  *quotient = VectorXd::Zero(numerator.size() - divisor.size() + 1);
  while (numerator.size() >= divisor.size()) {
    denominator = VectorXd::Zero(numerator.size());
    denominator.head(divisor.size()) = divisor;

    const double quotient_scalar = numerator(0) / denominator(0);
    quotient->reverse().operator()(numerator.size() - divisor.size()) =
        quotient_scalar;
    denominator = denominator * quotient_scalar;
    numerator = numerator - denominator;
    // Sometimes there are floating point errors that result in a non-zero first
    // value.
    numerator(0) = 0;
    numerator = RemoveLeadingZeros(numerator);
  }
  *remainder = numerator;
}

VectorXd AddPolynomials(const VectorXd& poly1, const VectorXd& poly2) {
  if (poly1.size() > poly2.size()) {
    VectorXd sum = poly1;
    sum.tail(poly2.size()) += poly2;
    return sum;
  } else {
    VectorXd sum = poly2;
    sum.tail(poly1.size()) += poly1;
    return sum;
  }
}

void FindLinearPolynomialRoots(const VectorXd& polynomial,
                               VectorXd* real,
                               VectorXd* imaginary) {
  CHECK_EQ(polynomial.size(), 2);
  if (real != NULL) {
    real->resize(1);
    (*real)(0) = -polynomial(1) / polynomial(0);
  }

  if (imaginary != NULL) {
    imaginary->setZero(1);
  }
}

void FindQuadraticPolynomialRoots(const VectorXd& polynomial,
                                  VectorXd* real,
                                  VectorXd* imaginary) {
  CHECK_EQ(polynomial.size(), 3);
  const double a = polynomial(0);
  const double b = polynomial(1);
  const double c = polynomial(2);
  const double D = b * b - 4 * a * c;
  const double sqrt_D = sqrt(fabs(D));
  if (real != NULL) {
    real->setZero(2);
  }
  if (imaginary != NULL) {
    imaginary->setZero(2);
  }

  // Real roots.
  if (D >= 0) {
    if (real != NULL) {
      // Stable quadratic roots according to BKP Horn.
      // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf
      if (b >= 0) {
        (*real)(0) = (-b - sqrt_D) / (2.0 * a);
        (*real)(1) = (2.0 * c) / (-b - sqrt_D);
      } else {
        (*real)(0) = (2.0 * c) / (-b + sqrt_D);
        (*real)(1) = (-b + sqrt_D) / (2.0 * a);
      }
    }
    return;
  }

  // Use the normal quadratic formula for the complex case.
  if (real != NULL) {
    (*real)(0) = -b / (2.0 * a);
    (*real)(1) = -b / (2.0 * a);
  }
  if (imaginary != NULL) {
    (*imaginary)(0) = sqrt_D / (2.0 * a);
    (*imaginary)(1) = -sqrt_D / (2.0 * a);
  }
}

void MinimizePolynomial(const VectorXd& polynomial,
                        const double x_min,
                        const double x_max,
                        double* optimal_x,
                        double* optimal_value) {
  // Find the minimum of the polynomial at the two ends.
  //
  // We start by inspecting the middle of the interval. Technically
  // this is not needed, but we do this to make this code as close to
  // the minFunc package as possible.
  *optimal_x = (x_min + x_max) / 2.0;
  *optimal_value = EvaluatePolynomial(polynomial, *optimal_x);

  const double x_min_value = EvaluatePolynomial(polynomial, x_min);
  if (x_min_value < *optimal_value) {
    *optimal_value = x_min_value;
    *optimal_x = x_min;
  }

  const double x_max_value = EvaluatePolynomial(polynomial, x_max);
  if (x_max_value < *optimal_value) {
    *optimal_value = x_max_value;
    *optimal_x = x_max;
  }

  // If the polynomial is linear or constant, we are done.
  if (polynomial.rows() <= 2) {
    return;
  }

  const VectorXd derivative = DifferentiatePolynomial(polynomial);
  VectorXd roots_real;
  if (!FindPolynomialRoots(derivative, &roots_real, NULL)) {
    LOG(WARNING) << "Unable to find the critical points of "
                 << "the interpolating polynomial.";
    return;
  }

  // This is a bit of an overkill, as some of the roots may actually
  // have a complex part, but its simpler to just check these values.
  for (int i = 0; i < roots_real.rows(); ++i) {
    const double root = roots_real(i);
    if ((root < x_min) || (root > x_max)) {
      continue;
    }

    const double value = EvaluatePolynomial(polynomial, root);
    if (value < *optimal_value) {
      *optimal_value = value;
      *optimal_x = root;
    }
  }
}

// An iterative solver to find the closest root based on an initial guess. We
// use Laguerre's method, which is a polynomial root finding method that
// converges to a root with very high certainty. For multiple roots, the
// convergence is linear, otherwise it is cubic.
double FindRootIterativeLaguerre(const VectorXd& polynomial,
                                 const double x0,
                                 const double epsilon,
                                 const int max_iter) {
  const double kSmallestValue = 1e-10;

  // Constant symbolic derivitives.
  const VectorXd f_prime = DifferentiatePolynomial(polynomial);
  const VectorXd f_prime_prime = DifferentiatePolynomial(f_prime);
  const double k = static_cast<double>(polynomial.size());

  double x = x0;

  for (int i = 0; i < max_iter; i++) {
    const double f_of_x = EvaluatePolynomial(polynomial, x);
    if (std::abs(f_of_x) < kSmallestValue) {
      break;
    }

    const double g = EvaluatePolynomial(f_prime, x) / f_of_x;
    const double h = g * g - EvaluatePolynomial(f_prime_prime, x) / f_of_x;
    const double denom_part = std::sqrt(std::abs((k - 1.0) * (k * h - g * g)));
    const double denom = (g < 0) ? g - denom_part : g + denom_part;
    const double delta =  k / denom;
    if (std::abs(delta) < epsilon) {
      break;
    }

    x -= delta;
  }

  return x;
}

double FindRootIterativeNewton(const Eigen::VectorXd& polynomial,
                               const double x0,
                               const double epsilon,
                               const int max_iterations) {
  double root = x0;
  const Eigen::VectorXd derivative = DifferentiatePolynomial(polynomial);
  double prev = std::numeric_limits<double>::max();
  for (int i = 0; i < max_iterations && std::abs(prev - root) > epsilon; i++) {
    prev = root;
    root -= EvaluatePolynomial(polynomial, root) /
            EvaluatePolynomial(derivative, root);
  }
  return root;
}

}  // namespace theia


#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/numerics/data_out.h>

#include <deal.II/lac/petsc_sparse_matrix.h>
#include <deal.II/lac/petsc_vector.h>
#include <deal.II/lac/petsc_solver.h>
#include <deal.II/lac/petsc_precondition.h>

#include <deal.II/base/utilities.h>

#include <deal.II/lac/slepc_solver.h>

#include <deal.II/base/timer.h>
#include <deal.II/base/conditional_ostream.h>
#include <deal.II/base/index_set.h>

#include <deal.II/lac/sparsity_tools.h>

#include <deal.II/distributed/tria.h>
#include <deal.II/distributed/grid_refinement.h>

#include <fstream>
#include <iostream>
#include <math.h>
#include <random>
#include <mpi.h>

using namespace dealii;
class ParallelKL
{
public:
  ParallelKL();
  void run();
private:
  void make_grid();
  void setup_system();
  void assemble_system();
  void solve();
  void create_random_field();
  void stats_for_random_field();
  void output_results() const;
  void output_results_parallel();

  MPI_Comm mpi_communicator;

  parallel::distributed::Triangulation<2> triangulation;

  FE_Q<2>          fe;
  DoFHandler<2>    dof_handler;

  IndexSet         locally_owned_dofs;
  IndexSet         locally_relevant_dofs;

  AffineConstraints<double> constraints;

  SparsityPattern      sparsity_pattern;

  PETScWrappers::MPI::SparseMatrix system_mass_matrix;
  PETScWrappers::MPI::SparseMatrix system_stiffness_matrix;

  PETScWrappers::MPI::Vector randomfield_vector;

  std::vector<double> eigenvalues;
  std::vector<PETScWrappers::MPI::Vector> eigenvectors;

  std::vector<double> normalized_gaussian;
  std::default_random_engine generator;
  std::normal_distribution<double> distribution;

  unsigned int this_mpi_process, n_mpi_processes;

  ConditionalOStream pcout;
  TimerOutput        computing_timer;
};

ParallelKL::ParallelKL()
  : mpi_communicator(MPI_COMM_WORLD)
  , triangulation(mpi_communicator,
                  typename Triangulation<2>::MeshSmoothing(
                  Triangulation<2>::smoothing_on_refinement |
                  Triangulation<2>::smoothing_on_coarsening))
  , fe(1)
  , dof_handler(triangulation)
  , this_mpi_process(Utilities::MPI::this_mpi_process(mpi_communicator))
  , n_mpi_processes(Utilities::MPI::n_mpi_processes(mpi_communicator))
  , pcout(std::cout,
        (Utilities::MPI::this_mpi_process(mpi_communicator) == 0))
  , computing_timer(mpi_communicator,
                    pcout,
                    TimerOutput::summary,
                    TimerOutput::wall_times)
{}

void ParallelKL::make_grid()
{
  GridGenerator::hyper_cube(triangulation, -1, 1);
  triangulation.refine_global(5);

  //parallel console output
  pcout << "Number of active cells: " << triangulation.n_active_cells()
        << std::endl;
}

void ParallelKL::setup_system()
{
  TimerOutput::Scope t(computing_timer, "setup");

  dof_handler.distribute_dofs(fe);
  pcout << "Number of degrees of freedom: " << dof_handler.n_dofs()
            << std::endl;

  locally_owned_dofs = dof_handler.locally_owned_dofs();
  DoFTools::extract_locally_relevant_dofs(dof_handler, locally_relevant_dofs);

  DynamicSparsityPattern dsp(locally_relevant_dofs);
  DoFTools::make_sparsity_pattern(dof_handler, dsp);

  SparsityTools::distribute_sparsity_pattern(dsp,
                                             locally_owned_dofs,
                                             mpi_communicator,
                                             locally_relevant_dofs);

  system_stiffness_matrix.reinit(locally_owned_dofs,
                                 locally_owned_dofs,
                                 dsp,
                                 mpi_communicator);

  system_mass_matrix.reinit(locally_owned_dofs,
                            locally_owned_dofs,
                            dsp,
                            mpi_communicator);

  randomfield_vector.reinit(locally_owned_dofs, mpi_communicator);

  constraints.clear();
  constraints.reinit(locally_relevant_dofs);
  constraints.close();
}

void ParallelKL::assemble_system()
{
  TimerOutput::Scope t(computing_timer, "assembly");

  system_stiffness_matrix = 0.;
  system_mass_matrix = 0.;

  QGauss<2> quadrature_formula(fe.degree + 1);

  FEValues<2> fe_values(fe,
                        quadrature_formula,
                        update_values | update_gradients | update_JxW_values | update_quadrature_points);

  const unsigned int dofs_per_cell = fe.dofs_per_cell;

  FullMatrix<double> cell_mass_matrix(dofs_per_cell, dofs_per_cell);
  FullMatrix<double> cell_stiffness_matrix(dofs_per_cell, dofs_per_cell);

  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

  for (const auto &cell : dof_handler.active_cell_iterators())
    { if (cell->is_locally_owned())
       {
          fe_values.reinit(cell);
          cell_mass_matrix = 0.;
          cell_stiffness_matrix = 0.;

          for (const unsigned int q_index : fe_values.quadrature_point_indices())
            {
              Point<2> quad_pointq = fe_values.quadrature_point(q_index);
              for (const unsigned int i : fe_values.dof_indices())
                for (const unsigned int j : fe_values.dof_indices())
                  cell_mass_matrix(i, j) +=
                    (fe_values.shape_value(i, q_index) * // grad phi_i(x_q)
                    fe_values.shape_value(j, q_index) * // grad phi_j(x_q)
                    fe_values.JxW(q_index));           // dx

              for (const unsigned int l_index : fe_values.quadrature_point_indices())
                {
                  Point<2> quad_pointl = fe_values.quadrature_point(l_index);
                  const double point_distance = quad_pointl.distance(quad_pointq);

                  for (const unsigned int i : fe_values.dof_indices())
                    for (const unsigned int j : fe_values.dof_indices())
                        cell_stiffness_matrix(i, j) +=
                        (exp(-0.5*point_distance/(0.05*0.05))*          // R(x,x')
                        fe_values.shape_value(i, q_index) * // grad phi_i(x_q)
                        fe_values.shape_value(j, l_index) * // grad phi_j(x_l)
                        fe_values.JxW(q_index)*fe_values.JxW(l_index));           // dx
                }
            }
          cell->get_dof_indices(local_dof_indices);

          constraints.distribute_local_to_global(cell_stiffness_matrix,
                                         local_dof_indices,
                                         system_stiffness_matrix);

          constraints.distribute_local_to_global(cell_mass_matrix,
                                         local_dof_indices,
                                         system_mass_matrix);
       }
    }

system_mass_matrix.compress(VectorOperation::add);
system_stiffness_matrix.compress(VectorOperation::add);

//system_mass_matrix.print(std::cout);
//system_stiffness_matrix.print(std::cout);
}

void ParallelKL::solve()
{
  const unsigned int num_eigenpairs_requested = 100;

  eigenvalues.resize(num_eigenpairs_requested);
  eigenvectors.resize(num_eigenpairs_requested);

  normalized_gaussian.resize(num_eigenpairs_requested);

  for (unsigned int i = 0; i < num_eigenpairs_requested; ++i)
    eigenvectors[i].reinit(locally_owned_dofs, locally_relevant_dofs, mpi_communicator);

  SolverControl eigen_solver_control (10000, 1e-10);

  SLEPcWrappers::SolverKrylovSchur eigensolver(eigen_solver_control, mpi_communicator);

  eigensolver.set_which_eigenpairs(EPS_LARGEST_REAL);

  eigensolver.set_problem_type(EPS_GHEP);

  pcout << "Beginning Eigensolve..." << std::endl;
  eigensolver.solve(system_stiffness_matrix,
                    system_mass_matrix,
                    eigenvalues,
                    eigenvectors,
                    num_eigenpairs_requested);

  for (unsigned int i = 0; i < num_eigenpairs_requested; i++)
  {
    double temporary_sample = 0.0;

    if (this_mpi_process == 0)
      temporary_sample = distribution(generator);

    double temporary_sum = 0.0;

    MPI_Allreduce (&temporary_sample, &temporary_sum, 1, MPI_DOUBLE, MPI_SUM, mpi_communicator);

    normalized_gaussian[i] = temporary_sum;

  }
}

void ParallelKL::create_random_field()
{
  randomfield_vector = 0.0;

  PETScWrappers::MPI::Vector tmp_locally_owned_vector(locally_owned_dofs, mpi_communicator);

  for (unsigned int i = 0; i < eigenvalues.size(); i++)
    {
      const double multiplier = sqrt(eigenvalues[i])*normalized_gaussian[i];
      tmp_locally_owned_vector = eigenvectors[i];
      randomfield_vector.add(multiplier, tmp_locally_owned_vector);
    }
}

void ParallelKL::stats_for_random_field()
{
  PETScWrappers::MPI::Vector relevent_randomfield_vector(locally_owned_dofs, locally_relevant_dofs, mpi_communicator);
  relevent_randomfield_vector = randomfield_vector;

  QGauss<2> quadrature_formula(fe.degree + 1);

  FEValues<2> fe_values(fe,
                        quadrature_formula,
                        update_values | update_JxW_values);

  double vol_on_current_processor = 0.;
  double random_field_volume_integral_on_current_processor = 0.;

  std::vector<double> local_dof_values(fe.dofs_per_cell);
  std::vector<double> current_function_values(quadrature_formula.size());

  const FEValuesExtractors::Scalar r_field(0);

  for (const auto &cell : dof_handler.active_cell_iterators())
  { if (cell->is_locally_owned())
       {
          fe_values.reinit(cell);
          cell->get_dof_values(relevent_randomfield_vector, local_dof_values.begin(), local_dof_values.end());
          fe_values[r_field].get_function_values_from_local_dof_values(local_dof_values, current_function_values);

          for (const unsigned int q_index : fe_values.quadrature_point_indices())
            {
              vol_on_current_processor  += fe_values.JxW(q_index);
              random_field_volume_integral_on_current_processor += current_function_values[q_index]*fe_values.JxW(q_index);
            }
       }
  }

  double total_random_field_volume_integral = 0.0;
  double total_volume = 0.0;

  MPI_Allreduce (&random_field_volume_integral_on_current_processor, &total_random_field_volume_integral, 1, MPI_DOUBLE, MPI_SUM, mpi_communicator);
  MPI_Allreduce (&vol_on_current_processor, &total_volume, 1, MPI_DOUBLE, MPI_SUM, mpi_communicator);

  double volume_average_of_random_field = total_random_field_volume_integral/total_volume;

  pcout << "Volume Average of Random Field = " << volume_average_of_random_field << std::endl;
  pcout << "Total Volume = " << total_volume << std::endl;

}

void ParallelKL::output_results() const
{
  DataOut<2> data_out;
  data_out.attach_dof_handler(dof_handler);
  /*
  Vector<float> subdomain(triangulation.n_active_cells());
  for (unsigned int i = 0; i < subdomain.size(); ++i)
      subdomain(i) = triangulation.locally_owned_subdomain();
  data_out.add_data_vector(subdomain, "subdomain");
*/
  for (unsigned int i = 0; i < eigenvalues.size(); i++)
  {
    //std::cout << eigenvectors[i] << "\n" << std::endl;
    std::string tmpname = "solution";
    tmpname += Utilities::int_to_string(i, 3);
    //std::cout << "Eigen_val = " << eigenvalues[i] << " " << "Normalized_gaussian = " << normalized_gaussian[i] << "\n"  << std::endl;
    data_out.add_data_vector(eigenvectors[i], tmpname);
  }

  data_out.build_patches();
  std::ofstream output("solution.vtk");
  data_out.write_vtk(output);
}

void ParallelKL::output_results_parallel()
{
    TimerOutput::Scope t(computing_timer, "output parallel");

    PETScWrappers::MPI::Vector relevent_randomfield_vector(locally_owned_dofs, locally_relevant_dofs, mpi_communicator);
    relevent_randomfield_vector = randomfield_vector;

    DataOut<2> data_out;

    // ############################################################
    // #######                   OUTPUT                     #######
    // ############################################################

    data_out.add_data_vector(dof_handler, relevent_randomfield_vector,
                               "RandomField");

  /*
    for (unsigned int i = 0; i < eigenvalues.size(); i++)
      {
        std::string tmpname = "solution";
        tmpname += Utilities::int_to_string(i, 3);
        data_out.add_data_vector(eigenvectors[i], tmpname);
      }
      */

    std::vector<Vector<double>> buckling_eigenmodes_out;

    buckling_eigenmodes_out.resize(eigenvalues.size());
    for (unsigned int i = 0; i < eigenvalues.size(); ++i)
      {
        buckling_eigenmodes_out[i].reinit(dof_handler.n_dofs(), 0.0);
        buckling_eigenmodes_out[i] = eigenvectors[i];
        const std::string buckling_mode_string = std::string("EigenVector") + Utilities::int_to_string(i, 2);

        data_out.add_data_vector(dof_handler, buckling_eigenmodes_out[i],
                                 buckling_mode_string);
      }

    // ############################################################
    // #######                 WRITE VTU FILE               #######
    // ############################################################
    data_out.build_patches ();
    const std::string filename = "Solution." +
                                  Utilities::int_to_string (this_mpi_process, 2) + ".vtu";

    std::ofstream output (filename.c_str());
    data_out.write_vtu (output);
    output.close();

    // ############################################################
    // #######                 WRITE PVTU FILE              #######
    // ############################################################
    if (this_mpi_process == 0)
    {
      std::vector<std::string> filenames;
      for (unsigned int i=0; i < n_mpi_processes; ++i)
        filenames.push_back ("Solution." +
                                  Utilities::int_to_string (i, 2) + ".vtu");

      std::ofstream master_output ("Solution.pvtu");

      data_out.write_pvtu_record (master_output, filenames);
    }
}

void ParallelKL::run()
{
  pcout << "Running with " << "PETSc" << " on " << Utilities::MPI::n_mpi_processes(mpi_communicator)
          << " MPI rank(s)..." << std::endl;

  make_grid();

  setup_system();

  pcout << "   Number of active cells:       "
        << triangulation.n_global_active_cells() << std::endl
        << "   Number of degrees of freedom: " << dof_handler.n_dofs()
        << std::endl;

  assemble_system();
  solve();
  create_random_field();
  stats_for_random_field();

  if (Utilities::MPI::n_mpi_processes(mpi_communicator) <= 32)
    {
      TimerOutput::Scope t(computing_timer, "output");
      //output_results();
      output_results_parallel();

    }
}

int main(int argc, char* argv[])
{
  Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);

  deallog.depth_console(0);

  ParallelKL klexpansion_2d;
  klexpansion_2d.run();
  return 0;
}