#include "CALPHADFreeEnergyFunctionsBinaryThreePhase.h"
#include "CALPHADConcSolverBinaryThreePhase.h"
#include "CALPHADFunctions.h"
#include "PhysicalConstants.h"
#include "functions.h"
#include "well_functions.h"

#include <boost/property_tree/json_parser.hpp>

#include <iomanip>
#include <string>

namespace pt = boost::property_tree;

namespace Thermo4PFM
{

CALPHADFreeEnergyFunctionsBinaryThreePhase::
    CALPHADFreeEnergyFunctionsBinaryThreePhase(pt::ptree& calphad_db,
        boost::optional<pt::ptree&> newton_db,
        const EnergyInterpolationType energy_interp_func_type,
        const ConcInterpolationType conc_interp_func_type)
    : energy_interp_func_type_(energy_interp_func_type),
      conc_interp_func_type_(conc_interp_func_type),
      newton_tol_(1.e-8),
      newton_alpha_(1.),
      newton_maxits_(20),
      newton_verbose_(false)
{
    std::string fenergy_diag_filename("energy.vtk");
    fenergy_diag_filename_ = new char[fenergy_diag_filename.length() + 1];
    strcpy(fenergy_diag_filename_, fenergy_diag_filename.c_str());

    readParameters(calphad_db);

    if (newton_db) readNewtonparameters(newton_db.get());
}

//=======================================================================
void CALPHADFreeEnergyFunctionsBinaryThreePhase::readNewtonparameters(
    pt::ptree& newton_db)
{
    newton_tol_     = newton_db.get<double>("tol", newton_tol_);
    newton_alpha_   = newton_db.get<double>("alpha", newton_alpha_);
    newton_maxits_  = newton_db.get<int>("max_its", newton_maxits_);
    newton_verbose_ = newton_db.get<bool>("verbose", newton_verbose_);
}

//=======================================================================
void CALPHADFreeEnergyFunctionsBinaryThreePhase::readParameters(
    pt::ptree& calphad_db)
{
    // print database to be read
    // std::clog << "CALPHAD database..." << std::endl;
    // pt::write_json(std::clog, calphad_db);

    pt::ptree& speciesA_db = calphad_db.get_child("SpeciesA");
    std::string dbnameL("PhaseL");
    g_species_phaseL_[0].initialize("L0", speciesA_db.get_child(dbnameL));
    // assert(checkSingleSublattice(speciesA_db.get_child(dbnameL)));
    std::string dbnameA("PhaseA");
    g_species_phaseA_[0].initialize("A0", speciesA_db.get_child(dbnameA));
    // assert(checkSingleSublattice(speciesA_db.get_child(dbnameA)));
    std::string dbnameB("PhaseB");
    g_species_phaseB_[0].initialize("B0", speciesA_db.get_child(dbnameB));
    // assert(checkSingleSublattice(speciesA_db.get_child(dbnameB)));

    pt::ptree& speciesB_db = calphad_db.get_child("SpeciesB");
    g_species_phaseL_[1].initialize("L1", speciesB_db.get_child(dbnameL));
    g_species_phaseA_[1].initialize("A1", speciesB_db.get_child(dbnameA));
    g_species_phaseB_[1].initialize("B1", speciesB_db.get_child(dbnameB));

    // read Lmix coefficients
    std::string dbnamemixL("LmixPhaseL");
    pt::ptree Lmix0_db = calphad_db.get_child(dbnamemixL);
    readLmixBinary(Lmix0_db, LmixPhaseL_);

    std::string dbnamemixA("LmixPhaseA");
    pt::ptree Lmix1_db = calphad_db.get_child(dbnamemixA);
    readLmixBinary(Lmix1_db, LmixPhaseA_);

    std::string dbnamemixB("LmixPhaseB");
    pt::ptree Lmix2_db = calphad_db.get_child(dbnamemixB);
    readLmixBinary(Lmix2_db, LmixPhaseB_);
}

//-----------------------------------------------------------------------
#ifdef HAVE_OPENMP_OFFLOAD
#pragma omp declare target
#endif
double CALPHADFreeEnergyFunctionsBinaryThreePhase::computeFreeEnergy(
    const double temperature, const double* const conc, const PhaseIndex pi,
    const bool gp)
{
    const CalphadDataType l0 = lmixPhase(0, pi, temperature);
    const CalphadDataType l1 = lmixPhase(1, pi, temperature);
    const CalphadDataType l2 = lmixPhase(2, pi, temperature);
    const CalphadDataType l3 = lmixPhase(3, pi, temperature);

    CALPHADSpeciesPhaseGibbsEnergy* g_species;

    switch (pi)
    {
        case PhaseIndex::phaseL:
            g_species = &g_species_phaseL_[0];
            break;
        case PhaseIndex::phaseA:
            g_species = &g_species_phaseA_[0];
            break;
        case PhaseIndex::phaseB:
            g_species = &g_species_phaseB_[0];
            break;
        default:
            //            std::cerr << "CALPHADFreeEnergyFunctionsBinary::"
            //                         "computeFreeEnergy(), undefined phase"
            //                      << "!!!" << std::endl;
            // abort();
            return 0.;
    }

    double fe = conc[0] * g_species[0].fenergy(temperature)
                + (1. - conc[0]) * g_species[1].fenergy(temperature)
                + CALPHADcomputeFMixBinary(l0, l1, l2, l3, conc[0])
                + CALPHADcomputeFIdealMixBinary(
                      gas_constant_R_JpKpmol * temperature, conc[0]);

    // subtract -mu*c to get grand potential
    if (gp)
    {
        double deriv;
        computeDerivFreeEnergy(temperature, conc, pi, &deriv);
        fe -= deriv * conc[0];
    }

    return fe;
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinaryThreePhase::computeDerivFreeEnergy(
    const double temperature, const double* const conc, const PhaseIndex pi,
    double* deriv)
{
    const CalphadDataType l0 = lmixPhase(0, pi, temperature);
    const CalphadDataType l1 = lmixPhase(1, pi, temperature);
    const CalphadDataType l2 = lmixPhase(2, pi, temperature);
    const CalphadDataType l3 = lmixPhase(3, pi, temperature);

    CALPHADSpeciesPhaseGibbsEnergy* g_species;

    switch (pi)
    {
        case PhaseIndex::phaseL:
            g_species = &g_species_phaseL_[0];
            break;
        case PhaseIndex::phaseA:
            g_species = &g_species_phaseA_[0];
            break;
        case PhaseIndex::phaseB:
            g_species = &g_species_phaseB_[0];
            break;
        default:
            //            std::cerr << "CALPHADFreeEnergyFunctionsBinary::"
            //                         "computeFreeEnergy(), undefined phase!!!"
            //                      << std::endl;
            // abort();
            return;
    }

    double mu = (g_species[0].fenergy(temperature)
                    - g_species[1].fenergy(temperature))
                + CALPHADcomputeFMix_derivBinary(l0, l1, l2, l3, conc[0])
                + CALPHADcomputeFIdealMix_derivBinary(
                      gas_constant_R_JpKpmol * temperature, conc[0]);

    deriv[0] = mu;
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinaryThreePhase::
    computeSecondDerivativeFreeEnergy(const double temp,
        const double* const conc, const PhaseIndex pi, double* d2fdc2)
{
    // assert(conc[0] >= 0.);
    // assert(conc[0] <= 1.);

    const CalphadDataType l0 = lmixPhase(0, pi, temp);
    const CalphadDataType l1 = lmixPhase(1, pi, temp);
    const CalphadDataType l2 = lmixPhase(2, pi, temp);
    const CalphadDataType l3 = lmixPhase(3, pi, temp);
    const double rt          = gas_constant_R_JpKpmol * temp;

    d2fdc2[0] = (CALPHADcomputeFMix_deriv2Binary(l0, l1, l2, l3, conc[0])
                 + CALPHADcomputeFIdealMix_deriv2Binary(rt, conc[0]));
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinaryThreePhase::computeTdependentParameters(
    const double temperature, CalphadDataType* Lmix_L, CalphadDataType* Lmix_A,
    CalphadDataType* Lmix_B, CalphadDataType* fA, CalphadDataType* fB)
{
    fA[0]     = g_species_phaseL_[0].fenergy(temperature);
    fB[0]     = g_species_phaseL_[1].fenergy(temperature);
    Lmix_L[0] = lmixPhase(0, PhaseIndex::phaseL, temperature);
    Lmix_L[1] = lmixPhase(1, PhaseIndex::phaseL, temperature);
    Lmix_L[2] = lmixPhase(2, PhaseIndex::phaseL, temperature);
    Lmix_L[3] = lmixPhase(3, PhaseIndex::phaseL, temperature);

    fA[1]     = g_species_phaseA_[0].fenergy(temperature);
    fB[1]     = g_species_phaseA_[1].fenergy(temperature);
    Lmix_A[0] = lmixPhase(0, PhaseIndex::phaseA, temperature);
    Lmix_A[1] = lmixPhase(1, PhaseIndex::phaseA, temperature);
    Lmix_A[2] = lmixPhase(2, PhaseIndex::phaseA, temperature);
    Lmix_A[3] = lmixPhase(3, PhaseIndex::phaseA, temperature);

    fA[2]     = g_species_phaseB_[0].fenergy(temperature);
    fB[2]     = g_species_phaseB_[1].fenergy(temperature);
    Lmix_B[0] = lmixPhase(0, PhaseIndex::phaseB, temperature);
    Lmix_B[1] = lmixPhase(1, PhaseIndex::phaseB, temperature);
    Lmix_B[2] = lmixPhase(2, PhaseIndex::phaseB, temperature);
    Lmix_B[3] = lmixPhase(3, PhaseIndex::phaseB, temperature);
}

//=======================================================================

// compute equilibrium concentrations in various phases for given temperature

bool CALPHADFreeEnergyFunctionsBinaryThreePhase::computeCeqT(
    const double temperature, double* ceq, const int maxits, const bool verbose)
{
    // Not used since the three phases are only in equilibrium at the eutectic
    // point
    return false;
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinaryThreePhase::computePhasesFreeEnergies(
    const double temperature, const double* const hphi, const double conc,
    double& fl, double& fa, double& fb)
{
    // std::cout<<"CALPHADFreeEnergyFunctionsBinary::computePhasesFreeEnergies()"<<endl;

    double c[3] = { conc, conc, conc };

    // evaluate temperature dependent parameters
    CalphadDataType fA[3];
    CalphadDataType fB[3];

    CalphadDataType Lmix_L[4];
    CalphadDataType Lmix_A[4];
    CalphadDataType Lmix_B[4];

    computeTdependentParameters(temperature, Lmix_L, Lmix_A, Lmix_B, fA, fB);

    double RTinv = 1.0 / (gas_constant_R_JpKpmol * temperature);

    CALPHADConcSolverBinaryThreePhase solver;
    solver.setup(
        conc, hphi[0], hphi[1], hphi[2], RTinv, Lmix_L, Lmix_A, Lmix_B, fA, fB);
    int ret = solver.ComputeConcentration(
        c, newton_tol_, newton_maxits_, newton_alpha_);
    if (ret < 0)
    {
#if 0
        std::cerr << "ERROR in "
                     "CALPHADFreeEnergyFunctionsBinaryThreePhase::"
                     "computePhasesFreeEnergies()"
                     " ---"
                  << "conc=" << conc << ", hphi=" << hphi[0] << std::endl;
        abort();
#endif
    }

    // assert(c[0] >= 0.);
    fl = computeFreeEnergy(temperature, &c[0], PhaseIndex::phaseL, false);

    // assert(c[1] >= 0.);
    fa = computeFreeEnergy(temperature, &c[1], PhaseIndex::phaseA, false);

    // assert(c[1] >= 0.);
    fb = computeFreeEnergy(temperature, &c[2], PhaseIndex::phaseB, false);
}

//-----------------------------------------------------------------------

int CALPHADFreeEnergyFunctionsBinaryThreePhase::computePhaseConcentrations(
    const double temperature, const double* const conc, const double* const phi,
    double* x)
{
    // assert(x[0] >= 0.);
    // assert(x[1] >= 0.);
    // assert(x[0] <= 1.);
    // assert(x[1] <= 1.);

    const double RTinv = 1.0 / (gas_constant_R_JpKpmol * temperature);

    CalphadDataType fA[3];
    CalphadDataType fB[3];
    CalphadDataType Lmix_L[4];
    CalphadDataType Lmix_A[4];
    CalphadDataType Lmix_B[4];

    computeTdependentParameters(temperature, Lmix_L, Lmix_A, Lmix_B, fA, fB);

    const double hphi0 = interp_func(conc_interp_func_type_, phi[0]);
    const double hphi1 = interp_func(conc_interp_func_type_, phi[1]);
    const double hphi2 = interp_func(conc_interp_func_type_, phi[2]);

    // conc could be outside of [0.,1.] in a trial step
    double c0 = conc[0] >= 0. ? conc[0] : 0.;
    c0        = c0 <= 1. ? c0 : 1.;
    // solve system of equations to find (cl,cs) given c0 and hphi
    // x: initial guess and solution
    CALPHADConcSolverBinaryThreePhase solver;
    solver.setup(
        c0, hphi0, hphi1, hphi2, RTinv, Lmix_L, Lmix_A, Lmix_B, fA, fB);
    int ret = solver.ComputeConcentration(
        x, newton_tol_, newton_maxits_, newton_alpha_);
#if 0
    if (ret == -1)
    {
        std::cerr << "ERROR, "
                     "CALPHADFreeEnergyFunctionsBinary::"
                     "computePhaseConcentrations() "
                     "failed for conc="
                  << conc[0] << ", hphi0=" << hphi0 << ", hphi1=" << hphi1 << ", hphi2=" << hphi2 << std::endl;
        abort();
    }
#endif

    return ret;
}
#ifdef HAVE_OPENMP_OFFLOAD
#pragma omp end declare target
#endif

//-----------------------------------------------------------------------
void CALPHADFreeEnergyFunctionsBinaryThreePhase::energyVsPhiAndC(
    const double temperature, const double* const ceq, const bool found_ceq,
    const double phi_well_scale, const int npts_phi, const int npts_c)
{
    // Not implemented because it is ill-defined for a three-phase system.
}

// Print out free energy as a function of phase
// for given composition and temperature
// File format: ASCII VTK, readble with Visit
void CALPHADFreeEnergyFunctionsBinaryThreePhase::printEnergyVsPhiHeader(
    const double temperature, const int nphi, const int nc, const double cmin,
    const double cmax, const double slopec, std::ostream& os) const
{
    // Not implemented because it is ill-defined for a three-phase system
}

//=======================================================================
void CALPHADFreeEnergyFunctionsBinaryThreePhase::printEnergyVsPhi(
    const double* const conc, const double temperature,
    const double phi_well_scale, const int npts, const double slopec,
    std::ostream& os)
{
    // Not implemented because it is ill-defined for a three-phase system
}

//=======================================================================
// compute free energy in [J/mol]

double CALPHADFreeEnergyFunctionsBinaryThreePhase::fchem(
    const double* const phi, const double* const conc, const double temperature)
{
    double hcphi[3];
    hcphi[0] = interp_func(conc_interp_func_type_, phi[0]);
    hcphi[1] = interp_func(conc_interp_func_type_, phi[1]);
    hcphi[2] = interp_func(conc_interp_func_type_, phi[2]);

    const double tol = 1.e-8;
    double fl        = 0.;
    double fa        = 0.;
    double fb        = 0.;

    bool pure_phase_0 = phi[0] > tol;
    bool pure_phase_1 = phi[1] > tol;
    bool pure_phase_2 = phi[2] > tol;

    if ((1.0 - phi[0] > tol) && (1.0 - phi[1] > tol) && (1.0 - phi[2] > tol))
    {
        computePhasesFreeEnergies(temperature, hcphi, conc[0], fl, fa, fb);
    }
    else
    {
        if (1.0 - phi[0] <= tol)
        {
            fl = computeFreeEnergy(temperature, conc, PhaseIndex::phaseL);
        }
        else if (1.0 - phi[1] <= tol)
        {
            fa = computeFreeEnergy(temperature, conc, PhaseIndex::phaseA);
        }
        else
        {
            fb = computeFreeEnergy(temperature, conc, PhaseIndex::phaseB);
        }
    }

    double hfphi[3];
    hfphi[0] = interp_func(energy_interp_func_type_, phi[0]);
    hfphi[1] = interp_func(energy_interp_func_type_, phi[1]);
    hfphi[2] = interp_func(energy_interp_func_type_, phi[2]);

    double e = hfphi[0] * fl + hfphi[1] * fa + hfphi[2];

    return e;
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinaryThreePhase::printEnergyVsComposition(
    const double temperature, std::ostream& os, const int npts)
{
    const double dc = 1.0 / (double)(npts - 1);

    os << "#phi0=1" << std::endl;
    for (int i = 0; i < npts; i++)
    {
        const double conc = i * dc;

        const double phi[3] = { 1., 0., 0. };

        double e = fchem(phi, &conc, temperature);
        os << conc << "\t" << e << std::endl;
    }
    os << std::endl << std::endl;

    os << "#phi1=1" << std::endl;
    for (int i = 0; i < npts; i++)
    {
        const double conc = i * dc;

        const double phi[3] = { 0., 1., 0. };
        double e            = fchem(phi, &conc, temperature);
        os << conc << "\t" << e << std::endl;
    }

    os << "#phi2=1" << std::endl;
    for (int i = 0; i < npts; i++)
    {
        const double conc = i * dc;

        const double phi[3] = { 0., 0., 0. };
        double e            = fchem(phi, &conc, temperature);
        os << conc << "\t" << e << std::endl;
    }
}

//=======================================================================

void CALPHADFreeEnergyFunctionsBinaryThreePhase::preRunDiagnostics(
    const double T0, const double T1)
{
    std::ofstream os1("FlC0vsT.dat", std::ios::out);
    os1 << "#Species 0, Phase L" << std::endl;
    g_species_phaseL_[0].plotFofT(os1, T0, T1);

    std::ofstream os2("FlC1vsT.dat", std::ios::out);
    os2 << "#Species 1, Phase L" << std::endl;
    g_species_phaseL_[1].plotFofT(os2, T0, T1);

    std::ofstream os3("FsC0vsT.dat", std::ios::out);
    os3 << "#Species 0, Phase A" << std::endl;
    g_species_phaseA_[0].plotFofT(os3, T0, T1);

    std::ofstream os4("FsC1vsT.dat", std::ios::out);
    os4 << "#Species 1, Phase A" << std::endl;
    g_species_phaseA_[1].plotFofT(os4, T0, T1);
}
}


/*
    Copyright (c) 2014, Philipp Krähenbühl
    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 Stanford University 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 Philipp Krähenbühl ''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 Philipp Krähenbühl 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.
*/
#include "qp.h"
#include "util.h"
#include <Eigen/Dense>

// #define VERBOSE
#ifdef VERBOSE
#include <iostream>
#endif

const int MAX_ITER = 1000;

struct ActiveSet {
	int n;       // Current size
	ArrayXb in; // in(i) : Is variable i in the active set
	ArrayXi A;  // All variables in the active set
	ActiveSet( int max_n=0 ):n(0),in(ArrayXb::Zero(max_n)),A(-ArrayXi::Ones(max_n)){}
	int add( int i ) { // Add variable i to the active set
		eassert( !in[i] );
		in[i] = true;
		A(n) = i;
		return n++;
	}
	void removeLast() { // Remove the last entry from the active set
		eassert( n > 0 );
		--n;
		eassert( in[A(n)] );
		in[A(n)] = false;
		A(n) = -1;
	}
};

template<typename T>
T dist( T a, T b ) {
	a = std::abs(a);
	b = std::abs(b);
	if (a > b) return a*std::sqrt(1 + (b/a)*(b/a));
	if (a < b) return b*std::sqrt(1 + (a/b)*(a/b));
	return a*std::sqrt(2.0);
}

template<typename T>
bool addConstraint( RMatrixX<T> & R, RMatrixX<T> & J, VectorX<T> d, ActiveSet & active_set, int l ) {
	const int n = R.rows();
	const int na = active_set.n;
	/* Givens rotation Vodoo */
	for( int j=n-1; j>na; j-- ) {
		T cc = d[j-1], ss = d[j];
		T h = dist( cc, ss );
		if( h == 0 )
			continue;
		d[j] = 0;
		ss /= h;
		cc /= h;
		if( cc < 0 ){
			cc =-cc;
			ss =-ss;
			d[j-1] = -h;
		}
		else
			d[j-1] = h;
		T xny = ss / (1. + cc);
		RowVectorX<T> t1 = J.row(j-1);
		RowVectorX<T> t2 = J.row(j);
		J.row(j-1) = t1*cc + t2*ss;
		J.row(j)   = xny * (t1 + J.row(j-1)) - t2;
	}
	active_set.add( l );
	R.col(na).head(na+1) = d.head(na+1);
	if( na > 0 && std::abs(d[na]) <= std::numeric_limits<T>::epsilon() * R.diagonal().head(na).array().abs().maxCoeff() )
		// Degenerate R
		return false;
	return true;
}

template<typename T>
void deleteConstraint( RMatrixX<T> & R, RMatrixX<T> & J, VectorX<T> & u, ActiveSet & active_set, int l ) {
	const int n = active_set.n;
	// Find the element to remove
	int qq = -1;
	for( int i=0; i<n; i++ )
		if( active_set.A[i] == l ){
			qq = i;
			break;
		}
	eassert( qq >= 0 );

	// Move all elements that follow down by ones and update the QR factorization
	for( int i=qq; i < n-1; i++ ) {
		u[i] = u[i+1];
		R.col(i) = R.col(i+1);
		active_set.A[i] = active_set.A[i+1];
	}
	// And remove the last element from the active set
	R.col(n-1).setZero();
	u[n-1] = 0;
	active_set.A[n-1] = l;
	active_set.removeLast();
	/* Some more givens rotation Vodoo to restore R and J */
	for( int j = qq; j < n-1; j++ ) {
		T cc = R(j,j), ss = R(j+1,j);
		T h = dist( cc, ss );
		if( h == 0 )
			continue;
		R(j+1,j) = 0;
		ss /= h;
		cc /= h;
		if( cc < 0 ){
			cc =-cc;
			ss =-ss;
			R(j,j) = -h;
		}
		else
			R(j,j) = h;
		T xny = ss / (1. + cc);
		const int nr = n-j-1;
		if( nr>0 ) {
			RowVectorX<T> t1 = R.row(j).segment(j+1,nr);
			RowVectorX<T> t2 = R.row(j+1).segment(j+1,nr);
			R.row(j).segment(j+1,nr)   = t1*cc + t2*ss;
			R.row(j+1).segment(j+1,nr) = xny * (t1 + R.row(j).segment(j+1,nr)) - t2;
		}
		{
			RowVectorX<T> t1 = J.row(j);
			RowVectorX<T> t2 = J.row(j+1);
			J.row(j)   = t1*cc + t2*ss;
			J.row(j+1) = xny * (t1 + J.row(j)) - t2;
		}
	}
}

// Solves a strictly convex QP of the form
//  minimize_x  x' Q x  +  c' x
//  subject to  A x <= b
template<typename T>
VectorX<T> qp( const RMatrixX<T> & Q, const VectorX<T> & c, const RMatrixX<T> & A, const VectorX<T> & b ) {
#ifdef VERBOSE
	printf("=================== SOLVING THE QP ===================\n" );
#endif
	const int n = c.size(), m = b.size();
	const T inf = std::numeric_limits<T>::infinity();

	if( Q.cols() != n || Q.rows() != n )
		throw std::invalid_argument( "Expected quadratic matrix Q of size n x n, where n=|c|!" );
	if( A.cols() != n || A.rows() != m )
		throw std::invalid_argument( "Expected inequality constraint matrix A of size |b| x n!" );

	RMatrixX<T> R = RMatrixX<T>::Zero(n,n), J = RMatrixX<T>::Zero(n,n);
	ActiveSet active_set(m);
	VectorX<T> x = VectorX<T>::Zero(n), s = VectorX<T>::Zero(m), u = VectorX<T>::Zero(m);

	/*
	 * Preprocessing phase
	 */

	/* decompose the matrix Q in the form LL^T */
	LLT<RMatrixX<T>,Lower> chol(Q);

	/* compute the inverse of the factorized matrix Q^-1, this is the initial value for J */
	// J = L^-1
	J = chol.matrixL().solve(RMatrixX<T>::Identity(n,n));

	/*
	 * Find the unconstrained minimizer x, which is a feasible point in the dual space
	 * x = -Q^-1 * c
	 */
	x = -chol.solve(c);
	for( int iter=0; iter < MAX_ITER; iter++ ) {

		/* step 1: choose a violated constraint */

		/* compute s(x) = - A * x + b for all elements of K \ A */
		s = -A * x + b;

#ifdef VERBOSE
		printf("Iteration %d   %f %f\n", iter, std::abs(s.array().min(0.f).sum())/m, std::numeric_limits<T>::epsilon() );
#endif

		/* Are there any more infeasibilities? |s|_1 < EPS*cond*100 */
		if ( -s.minCoeff() <= 100*std::numeric_limits<T>::epsilon() )
			return x;

		/* Step 2: check for feasibility and determine a new S-pair */
		int new_constraint = 0;
		T ss = VectorX<T>(s.array() * ((T)1-active_set.in.cast<T>())).minCoeff( &new_constraint );

		/* Are all constraints feasible? */
		if (ss >= 0.0) return x;

		/* Get the new constraint */
		VectorX<T> np = -A.row(new_constraint).transpose();

#ifdef VERBOSE
		printf("New constraint %d / %d\n", new_constraint, m);
#endif
		/* Step 2a: determine step direction and take partial steps */
		/* compute z = H np: the step direction in the primal space (through J, see the paper) */
		T unA = 0; // u(nA)
		while(1) {
			/* Get the number of active constraints */
			const int nA = active_set.n;

			VectorX<T> d = J * np;
			VectorX<T> z = J.bottomRows(d.size()-nA).transpose() * d.tail(d.size()-nA);

			/* compute N* np (if q > 0): the negative of the step direction in the dual space */
			VectorX<T> r = R.topLeftCorner(nA,nA).template triangularView<Upper>().solve(d.head(nA));

			/* Step 2b: compute step length t1 before violating dual feasibility */
			int l = -1;
			T t1 = inf;
			if( r.size() > 0 ) {
				t1 = (r.array() > 0).select( u.head(nA).array() / r.array(), inf ).minCoeff( &l );
				l = active_set.A[l];
			}

			/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
			T t2 = inf;
			if (std::abs(z.dot(z)) > std::numeric_limits<T>::epsilon()) // i.e. z != 0
				t2 = -ss / z.dot(np);

			/* the step is chosen as the minimum of t1 and t2 */
			T t = std::min<T>(t1, t2);

// #ifdef VERBOSE
// 			printf("  t = %f  %f %f    l = %d\n", t, t1, t2, l );
// 			std::cout<<"  ss = "<<ss<<"  z'np = "<<z.dot(np)<<std::endl;
// 			std::cout<<"  x = "<<x.transpose()<<std::endl;
// 			std::cout<<"  s = "<<s.transpose()<<std::endl;
// 			std::cout<<"  np = "<<np.transpose()<<std::endl;
// 			std::cout<<"  u = "<<u.head(nA).transpose()<<std::endl;
// 			std::cout<<"  r = "<<r.transpose()<<std::endl;
// 			std::cout<<"  z = "<<z.transpose()<<std::endl;
// 			std::cout<<"  d = "<<d.transpose()<<std::endl;
// 			std::cout<<"  R = "<<R.topLeftCorner(nA,nA)<<std::endl;
// 			std::cout<<"  J = "<<J<<std::endl;
// #endif

			/* Step 2c: determine new S-pair and take step: */

			/* case (i): no step in primal or dual space */
			if (t >= inf) /* QPP is infeasible */
				return VectorX<T>::Constant( n, inf );

			// Dual step
			u.head(nA) -= t * r;
			unA += t;

			// Primal step
			if( t2 < inf ) {
				x += t * z;
				ss = -A.row(new_constraint) * x + b(new_constraint);
			}

			/* case (ii): a patial step has taken */
			if (t2 > t1) {
#ifdef VERBOSE
				printf("  * delete %d\n", l );
#endif
				/* drop constraint l from the active set A */
				deleteConstraint<T>(R, J, u, active_set, l);
			}
			else
				break;
		}
		/* case (iii): step in primal and dual space */
		/* full step has been taken */
		/* add constraint ip to the active set*/
#ifdef VERBOSE
		printf("  * add %d\n", new_constraint );
#endif
		if (!addConstraint<T>(R, J, J*np, active_set, new_constraint))
			throw std::invalid_argument( "Degenerate R matrix, going to give up!" );
		u[active_set.n-1] = unA; // Update the lagrangian variable
	}
	return x;
}

// Solves a strictly convex sparse QP of the form
//  minimize_x  x' diag(Q) x  +  c' x
//  subject to  A x <= b
template<typename T>
VectorX<T> sparseQp( const VectorX<T> & Q, const VectorX<T> & c, const SRMatrixX<T> & A, const VectorX<T> & b ) {
	const int n = c.size(), m = b.size();
	const T inf = std::numeric_limits<T>::infinity();

	if( Q.size() != n )
		throw std::invalid_argument( "Expected vector Q of size n=|c|!" );
	if( A.cols() != n || A.rows() != m )
		throw std::invalid_argument( "Expected inequality constraint matrix A of size |b| x n!" );

	RMatrixX<T> R = RMatrixX<T>::Zero(n,n), J = RMatrixX<T>::Zero(n,n);
	ActiveSet active_set(m);
	VectorX<T> x = VectorX<T>::Zero(n), s = VectorX<T>::Zero(m), u = VectorX<T>::Zero(m);

	/*
	 * Preprocessing phase
	 */

	/* decompose the matrix Q in the form LL^T */
	const VectorX<T> L_inv = 1.0 / Q.array().sqrt();

	/* compute the inverse of the factorized matrix Q^-1, this is the initial value for J */
	// J = L^-1
	J = L_inv.asDiagonal();

	/*
	 * Find the unconstrained minimizer x, which is a feasible point in the dual space
	 * x = -Q^-1 * c
	 */
	x = -c.array() / Q.array();
	for( int iter=0; iter < MAX_ITER; iter++ ) {

		/* step 1: choose a violated constraint */

		/* compute s(x) = - A * x + b for all elements of K \ A */
		s = -A * x + b;

#ifdef VERBOSE
		printf("Iteration %d  %f %f\n", iter, std::abs(s.array().min(0.f).sum())/m, std::numeric_limits<T>::epsilon());
#endif

		/* Are there any more infeasibilities? |s|_1 < EPS*cond*100 */
		if ( -s.minCoeff() <= std::numeric_limits<T>::epsilon() )
			return x;

		/* Step 2: check for feasibility and determine a new S-pair */
		int new_constraint = 0;
		T ss = VectorX<T>(s.array() * ((T)1-active_set.in.cast<T>())).minCoeff( &new_constraint );

		/* Are all constraints feasible? */
		if (ss >= 0.0) return x;

		/* Get the new constraint */
		VectorX<T> np = -RowVectorX<T>(A.row(new_constraint)).transpose();

#ifdef VERBOSE
		printf("New constraint %d / %d\n", new_constraint, m);
#endif
		/* Step 2a: determine step direction and take partial steps */
		/* compute z = H np: the step direction in the primal space (through J, see the paper) */
		T unA = 0; // u(nA)
		while(1) {
			/* Get the number of active constraints */
			const int nA = active_set.n;

			VectorX<T> d = J * np;
			VectorX<T> z = J.bottomRows(d.size()-nA).transpose() * d.tail(d.size()-nA);

			/* compute N* np (if q > 0): the negative of the step direction in the dual space */
			VectorX<T> r = R.topLeftCorner(nA,nA).template triangularView<Upper>().solve(d.head(nA));

			/* Step 2b: compute step length t1 before violating dual feasibility */
			int l = -1;
			T t1 = inf;
			if( r.size() > 0 ) {
				t1 = (r.array() > 0).select( u.head(nA).array() / r.array(), inf ).minCoeff( &l );
				l = active_set.A[l];
			}

			/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
			T t2 = inf;
			if (std::abs(z.dot(z)) > std::numeric_limits<T>::epsilon()) // i.e. z != 0
				t2 = -ss / z.dot(np);

			/* the step is chosen as the minimum of t1 and t2 */
			T t = std::min<T>(t1, t2);

#ifdef VERBOSE
			printf("  t = %f  %f %f    l = %d\n", t, t1, t2, l );
			std::cout<<"  ss = "<<ss<<"  z'np = "<<z.dot(np)<<std::endl;
			std::cout<<"  x = "<<x.transpose()<<std::endl;
			std::cout<<"  s = "<<s.transpose()<<std::endl;
			std::cout<<"  np = "<<np.transpose()<<std::endl;
			std::cout<<"  u = "<<u.head(nA).transpose()<<std::endl;
			std::cout<<"  r = "<<r.transpose()<<std::endl;
			std::cout<<"  z = "<<z.transpose()<<std::endl;
			std::cout<<"  d = "<<d.transpose()<<std::endl;
			std::cout<<"  R = "<<R.topLeftCorner(nA,nA)<<std::endl;
			std::cout<<"  J = "<<J<<std::endl;
#endif

			/* Step 2c: determine new S-pair and take step: */

			/* case (i): no step in primal or dual space */
			if (t >= inf) /* QPP is infeasible */
				return VectorX<T>::Constant( n, inf );

			// Dual step
			u.head(nA) -= t * r;
			unA += t;

			// Primal step
			if( t2 < inf ) {
				x += t * z;
				ss = np.dot( x ) + b(new_constraint);
			}

			/* case (ii): a patial step has taken */
			if (t2 > t1) {
#ifdef VERBOSE
				printf("  * delete %d\n", l );
#endif
				/* drop constraint l from the active set A */
				deleteConstraint<T>(R, J, u, active_set, l);
			}
			else
				break;
		}
		/* case (iii): step in primal and dual space */
		/* full step has been taken */
		/* add constraint ip to the active set*/
#ifdef VERBOSE
		printf("  * add %d\n", new_constraint );
#endif
		if (!addConstraint<T>(R, J, J*np, active_set, new_constraint))
			throw std::invalid_argument( "Degenerate R matrix, going to give up!" );
		u[active_set.n-1] = unA; // Update the lagrangian variable
	}
	return x;
}

VectorXf qp( const RMatrixXf & Q, const VectorXf & c, const RMatrixXf & A, const VectorXf & b ) {
	return qp<float>( Q, c, A, b );
}
VectorXd qp( const RMatrixXd & Q, const VectorXd & c, const RMatrixXd & A, const VectorXd & b ) {
	return qp<double>( Q, c, A, b );
}
VectorXf sparseQp( const VectorXf & Q, const VectorXf & c, const SRMatrixXf & A, const VectorXf & b ) {
	return sparseQp<float>( Q, c, A, b );
}
VectorXd sparseQp( const VectorXd & Q, const VectorXd & c, const SRMatrixXd & A, const VectorXd & b ) {
	return sparseQp<double>( Q, c, A, b );
}


#include <general/nmspc_tensor_extra.h>
#include <general/nmspc_tensor_omp.h>
#include <tools/common/contraction.h>
#include <tools/common/fmt.h>

#include <iostream>
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/IterativeSolvers>

template<typename Scalar_> class MatrixReplacement;
template<typename T> using DenseMatrix = Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>;

namespace Eigen {
    namespace internal {
// MatrixReplacement looks-like a SparseMatrix, so let's inherits its traits:
        template<typename Scalar_>
        struct traits<MatrixReplacement<Scalar_>> :  public Eigen::internal::traits<DenseMatrix<Scalar_> >
        {};
    }
}

// Example of a matrix-free wrapper from a user type to Eigen's compatible type
// For the sake of simplicity, this example simply wrap a Eigen::Matrix.
template<typename Scalar_>
class MatrixReplacement : public Eigen::EigenBase<MatrixReplacement<Scalar_>> {
    public:
    // Required typedefs, constants, and method:
    typedef Scalar_ Scalar;
    typedef double RealScalar;
    typedef int StorageIndex;
    enum {
        ColsAtCompileTime = Eigen::Dynamic,
        MaxColsAtCompileTime = Eigen::Dynamic,
        IsRowMajor = false
    };

    const Scalar_ *     envL = nullptr;
    const Scalar_ *     envR = nullptr;
    const Scalar_ *     mpo = nullptr;
    std::array<long, 3> shape_mps;
    std::array<long, 4> shape_mpo;
    std::array<long, 3> shape_envL;
    std::array<long, 3> shape_envR;
    std::vector<Scalar> shift_mpo;
    long                mps_size;
//    eig::Form           form = eig::Form::SYMM;
//    eig::Side           side = eig::Side::R;
    // Profiling
//    std::unique_ptr<class_tic_toc> t_multAx;
    mutable int         counter = 0;
    mutable std::vector<Scalar> tmp;

    [[nodiscard]] Eigen::Index rows() const { return static_cast<int>(mps_size); }; /*!< Linear size\f$d^2 \times \chi_L \times \chi_R \f$  */
    [[nodiscard]] Eigen::Index cols() const { return static_cast<int>(mps_size); }; /*!< Linear size\f$d^2 \times \chi_L \times \chi_R \f$  */

    template<typename Rhs>
    Eigen::Product<MatrixReplacement,Rhs,Eigen::AliasFreeProduct> operator*(const Eigen::MatrixBase<Rhs>& x) const {
        return Eigen::Product<MatrixReplacement,Rhs,Eigen::AliasFreeProduct>(*this, x.derived());
    }

    // Custom API:
    MatrixReplacement()  = default;

    void attachTensors(
        const Scalar_ *     envL_,      /*!< The left block tensor.  */
        const Scalar_ *     envR_,      /*!< The right block tensor.  */
        const Scalar_ *     mpo_,       /*!< The Hamiltonian MPO's  */
        std::array<long, 3> shape_mps_, /*!< An array containing the shapes of the mps  */
        std::array<long, 4> shape_mpo_  /*!< An array containing the shapes of the mpo  */
    ){
        envL       = envL_;
        envR       = envR_;
        mpo        = mpo_;
        shape_mps  = shape_mps_;
        shape_mpo  = shape_mpo_;
        shape_envL = {shape_mps_[1], shape_mps_[1], shape_mpo_[0]};
        shape_envR = {shape_mps_[2], shape_mps_[2], shape_mpo_[1]};
        if(envL == nullptr) throw std::runtime_error("Lblock is a nullptr!");
        if(envR == nullptr) throw std::runtime_error("Rblock is a nullptr!");
        if(mpo == nullptr) throw std::runtime_error("mpo is a nullptr!");
        mps_size = shape_mps[0] * shape_mps[1] * shape_mps[2];
//        t_multAx = std::make_unique<class_tic_toc>(true, 5, "Time MultAx");
    }

};

// Implementation of MatrixReplacement * Eigen::DenseVector though a specialization of internal::generic_product_impl:
namespace Eigen {
    namespace internal {

        template<typename Rhs, typename ReplScalar>
        struct generic_product_impl<MatrixReplacement<ReplScalar>, Rhs, DenseShape, DenseShape, GemvProduct> // GEMV stands for matrix-vector
            : generic_product_impl_base<MatrixReplacement<ReplScalar>,Rhs,generic_product_impl<MatrixReplacement<ReplScalar>,Rhs> >
        {
            typedef typename Product<MatrixReplacement<ReplScalar>,Rhs>::Scalar Scalar;

            template<typename Dest>
            static void scaleAndAddTo(Dest& dst, const MatrixReplacement<ReplScalar>& mat, const Rhs& rhs, const Scalar& alpha)
            {
                // This method should implement "dst += alpha * lhs * rhs" inplace,
                // however, for iterative solvers, alpha is always equal to 1, so let's not worry about it.
                assert(alpha==Scalar(1) && "scaling is not implemented");
                EIGEN_ONLY_USED_FOR_DEBUG(alpha);

//                auto token = mat.t_multAx->tic_token();
                mat.tmp.resize(static_cast<size_t>(dst.size()));
                Eigen::Map<Dest> tmp_map (mat.tmp.data(), dst.size());
//                std::cout << "dst size " << dst.size() << " | rhs size " << rhs.size() << std::endl;
//                if(dst.size() != rhs.size())
//                    dst.conservativeResize(rhs.size());

                tools::common::contraction::matrix_vector_product(tmp_map.data(), rhs.data(), mat.shape_mps,
                                                                  mat.mpo,  mat.shape_mpo,
                                                                  mat.envL, mat.shape_envL,
                                                                  mat.envR, mat.shape_envR);

                dst.noalias() += tmp_map;
                mat.counter++;
//
//                // Here we could simply call dst.noalias() += lhs.my_matrix() * rhs,
//                // but let's do something fancier (and less efficient):
//                for(Index i=0; i<lhs.cols(); ++i)
//                    dst += rhs(i) * lhs.my_matrix().col(i);
            }
        };

    }
}

/* clang-format off */
template<typename Scalar>
void tools::common::contraction::matrix_inverse_vector_product(Scalar * res_ptr,
                                                       const Scalar * const mps_ptr, std::array<long,3> mps_dims,
                                                       const Scalar * const mpo_ptr, std::array<long,4> mpo_dims,
                                                       const Scalar * const envL_ptr, std::array<long,3> envL_dims,
                                                       const Scalar * const envR_ptr, std::array<long,3> envR_dims){

        // Here we return x <-- A^-1 * b
        // Where A^-1 * b is obtained by solving
        //       A*x = b
        // using an iterative matrix-free solver.
        {
            auto mps = Eigen::TensorMap<const Eigen::Tensor<const Scalar,3>>(mps_ptr,mps_dims);
            auto mpo = Eigen::TensorMap<const Eigen::Tensor<const Scalar,4>>(mpo_ptr,mpo_dims);
            auto envL = Eigen::TensorMap<const Eigen::Tensor<const Scalar,3>>(envL_ptr,envL_dims);
            auto envR = Eigen::TensorMap<const Eigen::Tensor<const Scalar,3>>(envR_ptr,envR_dims);

            if(mps.dimension(1) != envL.dimension(0))
                throw std::runtime_error(fmt::format("Dimension mismatch mps {} and envL {}", mps.dimensions(), envL.dimensions()));
            if(mps.dimension(2) != envR.dimension(0))
                throw std::runtime_error(fmt::format("Dimension mismatch mps {} and envR {}", mps.dimensions(), envR.dimensions()));
            if(mps.dimension(0) != mpo.dimension(2)) throw std::runtime_error(fmt::format("Dimension mismatch mps {} and mpo {}", mps.dimensions(), mpo.dimensions()));
            if(envL.dimension(2) != mpo.dimension(0))
                throw std::runtime_error(fmt::format("Dimension mismatch envL {} and mpo {}", envL.dimensions(), mpo.dimensions()));
            if(envR.dimension(2) != mpo.dimension(1))
                throw std::runtime_error(fmt::format("Dimension mismatch envR {} and mpo {}", envR.dimensions(), mpo.dimensions()));
        }

        // Define the "matrix-free" matrix replacement.
        MatrixReplacement<Scalar> matRepl;
        matRepl.attachTensors(envL_ptr, envR_ptr, mpo_ptr, mps_dims, mpo_dims);

        Eigen::Index MaxIters = 200000;
        double tolerance = 1e-14;
        Eigen::Map<Textra::VectorType<Scalar>> res(res_ptr, matRepl.rows());
        Eigen::Map<const Textra::VectorType<Scalar>> mps(mps_ptr, matRepl.rows());
        if constexpr (std::is_same_v<Scalar,std::complex<double>>){

            Eigen::BiCGSTAB<MatrixReplacement<Scalar>, Eigen::IdentityPreconditioner> bicg;
            bicg.compute(matRepl);
            bicg.setMaxIterations(MaxIters);
            bicg.setTolerance(tolerance);
            res = bicg.solve(mps);
//            std::cout << "BiCGSTAB: #iterations: " << bicg.iterations()
//                << ", #count: " << matRepl.counter
//                << ", estimated error: " << bicg.error()
//                << std::endl;

        }
//        {
//            Eigen::ConjugateGradient<MatrixReplacement<Scalar>, Eigen::Lower|Eigen::Upper, Eigen::IdentityPreconditioner> cg;
//            cg.setMaxIterations(20000);
//            cg.compute(matRepl);
//            res = cg.solve(mps);
//            std::cout << "CG      : #iterations: " << cg.iterations()
//                      << ", #count: " << matRepl.counter
//                      << ", estimated error: " << cg.error()
//                      << std::endl;
//        }
        if constexpr (std::is_same_v<Scalar,double>){
            Eigen::MINRES<MatrixReplacement<Scalar>, Eigen::Lower|Eigen::Upper, Eigen::IdentityPreconditioner> minres;
            minres.setMaxIterations(MaxIters);
            minres.setTolerance(tolerance);
            minres.compute(matRepl);
            res = minres.solve(mps);
            std::cout << "MINRES:   #iterations: " << minres.iterations()
                      << ", #count: " << matRepl.counter
                      << ", norm: " << res.norm()
                      << ", estimated error: " << minres.error()
                      << std::endl;
    //    std::cout << "x: \n" << x << std::endl;
        }

//        {
//            Eigen::ConjugateGradient<MatrixReplacement<Scalar>, Eigen::Lower|Eigen::Upper, Eigen::IdentityPreconditioner> cg;
//            cg.setMaxIterations(20000);
//            cg.compute(matRepl);
//            res = cg.solve(mps);
//            std::cout << "CG      : #iterations: " << cg.iterations()
//                      << ", #count: " << matRepl.counter
//                      << ", estimated error: " << cg.error()
//                      << std::endl;
//        }

}
using namespace tools::common::contraction;
template void tools::common::contraction::matrix_inverse_vector_product(
    real * res_ptr,
    const real * const mps_ptr, std::array<long,3> mps_dims,
    const real * const mpo_ptr, std::array<long,4> mpo_dims,
    const real * const envL_ptr, std::array<long,3> envL_dims,
    const real * const envR_ptr, std::array<long,3> envR_dims);
template void tools::common::contraction::matrix_inverse_vector_product(
    cplx * res_ptr,
    const cplx * const mps_ptr, std::array<long,3> mps_dims,
    const cplx * const mpo_ptr, std::array<long,4> mpo_dims,
    const cplx * const envL_ptr, std::array<long,3> envL_dims,
    const cplx * const envR_ptr, std::array<long,3> envR_dims);
/* clang-format on */