# pragma once
//#define EIGEN_USE_MKL_ALL
#include"basis.hpp"
#include<cmath>

#include <Eigen/Sparse>
#ifdef MOM
using ValType= std::complex<double>;
#else
using ValType= double;
#endif
namespace Operators{
  using Many_Body::pi;
  using  Many_Body::GetLattice;
  using  Many_Body::Translate;
  using  Many_Body::Position;
  //   using  Many_Body::State();
  using Many_Body::RightId;
  using Many_Body::LeftId;

  using Mat= Eigen::SparseMatrix<ValType,Eigen::RowMajor>;

    template<typename T>
    size_t CheckSign(const T& state, size_t i, size_t j)
  {
    size_t m=0;
    if(i>j)
      {
  	for(size_t l=j+1; l<i; l++)
  	  {
  	    m+=size_t(state[l]);
  	  }

      }
    else
      	for(size_t l=i+1; l<j; l++)
  	  {
  	    m+=size_t(state[l]);
  	  }
    return m;
  }
    template<typename TotalBasis, typename State, typename Lattice>
  void Act(int i, int j, TotalBasis& totalBasis, State& tpState, Lattice state, Mat& op, double var)
  {
    if(state[i]==state[j])
      {

      }
    else{
      Lattice temp=state;
      //  std::cout<< "i,jx "<< i << ", "<< j << std::endl;
       state.flip(i);
       //setPartNr(j, temp[i]);
      state.flip(j);

      size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		     size_t newStateNr= Position(*it2);
		     //	         std::cout<< "old "<< temp << " at "<< Position(tpState)<< std::endl;

		     // std::cout<< "new "<< state <<" at  "<<newStateNr<<  std::endl;
	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= ValType{var};}
		    }

  }

  // carefull with changes, making one for cdag and one for ccdag
    template<typename T>
    size_t CheckSign2(const T& state, size_t i, size_t j)
  {
    size_t m=0;
    if(i>j)
      {
  	for(size_t l=j; l<i; l++)
  	  {
  	    m+=size_t(state[l]);
  	  }

      }
    else
      	for(size_t l=i; l<j; l++)
  	  {
  	    m+=size_t(state[l]);
  	  }
    return m;
  }

  size_t NextWithBC(size_t i, size_t sites, bool PB=true, size_t steps=1)
  {
    if(PB)
      {return (i+steps)%sites;}
    else{return (i+steps);}
  }
  size_t Length( size_t sites, bool PB=true)
  {
    if(PB)
      {return sites;}
    else{return sites-1;}
  }
    template<class TotalBasis>
  Mat CdagOperator(const TotalBasis& totalBasis, int i,   const bool& PB=true)
{

   using BasisIt= typename TotalBasis::BasisIt;

 using Lattice=typename TotalBasis::Lattice;
    size_t dim=totalBasis.dim;

    Mat op(dim, dim);
       op.setZero();
 for( auto& tpState :totalBasis)
	     {

	       BasisIt it2=totalBasis.find(Id(tpState));

	       Lattice state=GetLattice(*it2);
	       //	       std::cout << " stat "<< totalBasis.totalmaxPar << "\n";
	       //std::cout << " i "<< i<< "  has "<< state[i]<< std::endl;

	       if(state[i]==0 )
	         	 {

			   auto temp=state;
	         	   state.setPartNr(i, 1);
			   size_t signControl=CheckSign2(temp, i, 0);
			   // std::cout << "at i="<< i <<" changed to  "<< state << "\n"<< "with aign cont "<< signControl<< '\n';
	         	   it2= totalBasis.find(state.GetId());

	     		   size_t newStateNr= Position(*it2);
	      		              if(signControl%2==0)
   	      	    	 {
	      		   op.coeffRef(newStateNr, Position(tpState))+= ValType{1};}
   	      	       else
   	      	    	 { op.coeffRef(newStateNr, Position(tpState))-= ValType{1};}
	      	    }

	      }
      return op;

       }
  template<class TotalBasis>
  Mat COperator(const TotalBasis& totalBasis, int i,   const bool& PB=true)
{

   using BasisIt= typename TotalBasis::BasisIt;

 using Lattice=typename TotalBasis::Lattice;
    size_t dim=totalBasis.dim;

    Mat op(dim, dim);
       op.setZero();
 for( auto& tpState :totalBasis)
	     {

	        BasisIt it2=totalBasis.find(Id(tpState));

	       Lattice state=GetLattice(*it2);
	       // std::cout << " stat "<< state << "\n";
	       //std::cout << " i "<< i<< "  has "<< state[i]<< std::endl;

	       if(state[i]==1)
	         	 {

			   auto temp=state;
	         	   state.setPartNr(i, 0);
			   size_t signControl=CheckSign2(temp, i, 0);
			   // std::cout << "at i="<< i <<" changed to  "<< state << "\n"<< "with aign cont "<< signControl<< '\n';
	         	   it2= totalBasis.find(state.GetId());

	     		   size_t newStateNr= Position(*it2);
	      		              if(signControl%2==0)
   	      	    	 {
	      		   op.coeffRef(newStateNr, Position(tpState))+= ValType{1};}
   	      	       else
   	      	    	 { op.coeffRef(newStateNr, Position(tpState))-= ValType{1};}
	      	    }

	      }
      return op;

       }
  template<class TotalBasis>
  Mat NumberOperatorE(const TotalBasis& totalBasis, const double omega=1., bool PB=0, int start=0, int stop=0)
 {

    size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
 stop = (stop!=0) ? stop : sites;

      for(const auto& tpState : totalBasis)
	{

	  for(size_t i=start; i<stop; i++)
	    {

  op.coeffRef(Position(tpState), Position(tpState))+=ValType(omega*totalBasis.particlesAt(Id(tpState), i));

	  }
	}
      return op;

       }
  template<class TotalBasis, class Functor>
  Mat EKinLongRangeOperator(const TotalBasis& totalBasis,Functor f, int lmx=1, double var=1. , bool PB=0 )
     {

using Lattice=typename TotalBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
    int  stop = Operators::Length( sites, PB);

   for( auto& tpState : totalBasis)
	     {
	       	    for(int i=0; i<stop; i++)
                 {
		   for(int j=1; j<=lmx; j++)
                 {
Lattice state_1=GetLattice(tpState);
		   Lattice state_2=GetLattice(tpState);
		   int j_1=(i+j)%sites;

	   	    		    if(state_1[i]==state_1[j_1])
   	       	     {
	  //

   	   	     }
   	   	    else{

		      Lattice temp=state_1;
		      state_1.switchPartNr(i, temp[j_1], j_1, temp[i]);

  size_t signControl=CheckSign(temp, i, j_1);

		    auto it2_1 = totalBasis.find(state_1.GetId());
	     		   size_t newStateNr_1= Position(*it2_1);

	   	               if(signControl%2==0)
   	   	    	  {
			    op.coeffRef(newStateNr_1, Position(tpState))-= ValType{var}*f(i,j_1);}

		   	       	   	       else
		   	    {
			      op.coeffRef(newStateNr_1, Position(tpState))+= ValType{var}*f(i,j_1);
}

		    }

		 }

   	  }
	     }
  return op;
  }
  template<class TotalBasis>
  Mat EKinOperator(const TotalBasis& totalBasis, double var=1. , bool PB=0, int start=0, int stop=0 )
     {

using Lattice=typename TotalBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
      stop = (stop!=0) ? stop : Operators::Length( sites, PB);

   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=start; i<stop; i++)
                 {

		   Lattice state=GetLattice(tpState);
                     size_t j=Operators::NextWithBC(i, sites, PB);
	   	    		    if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[i]);
		      state.setPartNr(i, temp[j]);

	  	     size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		     size_t newStateNr= Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= ValType{var};}
		    }

	   	     }

   	  }

  return op;
  }

Mat EKinOperator(const Many_Body::OneElectronBasis& totalBasis, double var=1. , bool PB=0, int start=0, int stop=0 )
     {

       using Lattice=typename Many_Body::OneElectronBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
      stop = (stop!=0) ? stop : Operators::Length( sites, PB);

   for( auto& tpState : totalBasis)
   	     {

   	       	    for(size_t i=start; i<stop; i++)
                 {

   		   Lattice state=GetLattice(tpState);
                     size_t j=Operators::NextWithBC(i, sites, PB);
   	   	    		    if(state[i]==state[j])
   	       	     {
   	  //

   	   	     }
   	   	    else{

   		    Act(i, j, totalBasis, tpState, state, op, var);
   		    }

   	   	     }

   	  }

  return op;
  }

   template<class TotalBasis>
  Mat CurrOperator(const TotalBasis& totalBasis, double var=1. , bool PB=0, int start=0, int stop=0)
     {

using Lattice=typename TotalBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
      stop = (stop!=0) ? stop : Operators::Length( sites, PB);

   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=start; i<stop; i++)
                 {

		   Lattice state=GetLattice(tpState);
                     size_t j=Operators::NextWithBC(i, sites, PB);
	   	    		    if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[i]);
		      state.setPartNr(i, temp[j]);

	  	     size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		    double otherSign=(state[j]==1)?+1:-1;

		     size_t newStateNr= Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= otherSign*ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= otherSign*ValType{var};}
		    }

	   	     }

   	  }

  return op;
  }

  Mat CurrOperator(const  Many_Body::OneElectronBasis& totalBasis, double var=1. , bool PB=0, int start=0, int stop=0)
     {

using Lattice=typename  Many_Body::OneElectronBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
      stop = (stop!=0) ? stop : Operators::Length( sites, PB);

   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=start; i<stop; i++)
                 {

		   Lattice state=GetLattice(tpState);
                     size_t j=Operators::NextWithBC(i, sites, PB);
	   	    		    if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{

		   Lattice temp=state;

		          state.flip(i);

			  state.flip(j);
	  	     size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		    double otherSign=(temp[j]==1)?+1:-1;

		     size_t newStateNr= Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= otherSign*ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= otherSign*ValType{var};}
		    }

	   	     }

   	  }

  return op;

  }

    template<class TotalBasis>
    Mat totalHetOperator(const TotalBasis& totalBasis, double tint, double t0, double tl, double V, int Llead1,  int Llead2,  int Lchain )
     {
       bool PB=0;

using Lattice=typename TotalBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
 int L_x=Llead1+(Lchain+1)/2;
 double E=0;
 if(Lchain>1)
   {

   E=V/(Lchain+1);

   }
 std::cout<< "L x "<<L_x<< " E "<<E<<std::endl;
   for( auto& tpState : totalBasis)
	     {
const Lattice state=GetLattice(tpState);
	       	    for(size_t i=0; i<Llead1-1; i++)
                 {

		   if(state[i]==1)
		     {

	       op.coeffRef(Position(tpState), Position(tpState))-= ValType{V/2};

		     }
		   size_t j=Operators::NextWithBC(i, sites, PB);
		   //	   std::cout<< "i,j "<< i << ", "<< j << std::endl;
		     // first lead

		      Act(i, j, totalBasis, tpState, state, op, tl);

		 }
		     for(size_t i=Llead1+Lchain; i<Llead1+Lchain+Llead2-1; i++)
                 {
		   if(state[i]==1)
		     {
		       // std::cout<<" i "<<i+1<< " V "<< V <<std::endl;

		       op.coeffRef(Position(tpState), Position(tpState))+= ValType{V/2};

		     }

				   size_t j=Operators::NextWithBC(i, sites, PB);
		     // second lead

		     Act(i, j, totalBasis, tpState, state, op, tl);

	   	     }
		    	   if(state[Llead1+Llead2+Lchain-1]==1)
		     {

		       op.coeffRef(Position(tpState), Position(tpState))+= ValType{V/2};

		     }
			   if(state[Llead1-1]==1)
		     {

		       op.coeffRef(Position(tpState), Position(tpState))-= ValType{V/2};

		     }
		    // chain
		    	       	    for(size_t i=Llead1; i<Llead1+Lchain-1; i++)
                 {
		   Lattice state=GetLattice(tpState);

                     size_t j=Operators::NextWithBC(i, sites, PB);
		     // first lead
		     Act(i, j, totalBasis, tpState, state, op, t0);
		     	   if(state[i]==1)
		     {

		       		         op.coeffRef(Position(tpState), Position(tpState))+= (static_cast<double>((i+1))-L_x)*E;

		     }
	   	     }
				      	   if(state[Llead1+Lchain-1]==1)
		     {

		       	       op.coeffRef(Position(tpState), Position(tpState))+=(static_cast<double>(Llead1+Lchain)-L_x)*E;
	     }
				    Act(Llead1-1, Llead1, totalBasis, tpState, state, op, tint);
				    Act(Llead1+Lchain-1, Llead1+Lchain, totalBasis, tpState, state, op, tint);	  }

  return op;
  }
template<class TotalBasis>
Mat totCurrOperator(const TotalBasis& totalBasis, double var ,  int Llead1, int Llead2, int Lchain)
     {

       using Lattice=typename TotalBasis::Lattice;

   size_t dim=totalBasis.dim;
   size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
   for( auto& tpState : totalBasis)
	     {

	       int i=Llead1-1;

	       int j=i+1;

		   Lattice state=GetLattice(tpState);

	   	    		    if(state[i]==state[j])
   	       	     {

   	   	     }
   	   	    else{

		      Lattice temp=state;

		          state.flip(i);

			  state.flip(j);
	  	     size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		    double otherSign=(temp[j]==1)?+1:-1;

		     size_t newStateNr= Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= otherSign*ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= otherSign*ValType{var};}
		    }
				    state=GetLattice(tpState);

	 i=Lchain+Llead1-1;

    j=i+1;

		if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{

		      Lattice temp=state;
		    state.flip(i);
       //setPartNr(j, temp[i]);
      state.flip(j);

	  	     size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		    double otherSign=(temp[j]==1)?+1:-1;

		     size_t newStateNr= Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= otherSign*ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= otherSign*ValType{var};}
		    }

   	  }

   op*=0.5;

  return op;
  }
}


/*
    BSD 3-Clause License

    Copyright (c) 2018, Roboy
            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 copyright holder 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 HOLDER 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: Simon Trendel ( simon.trendel@tum.de ), 2018
    description: helper class for handling triangulation from lighthouse angles
*/

#pragma once

// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
#include "darkroom/Sensor.hpp"

struct LighthouseCalibration{
    double phase = 0;
    double curve = 0;
    double tilt = 0;
    double gibmag = 0;
    double gibphase = 0;
    void reset(){ phase = 0; curve = 0; tilt = 0; gibmag = 0; gibphase = 0; }
};

// offset from center of laser rotation
#define AXIS_OFFSET 0.015

using namespace Eigen;

class Triangulation{
public:
    /**
   * get the 3D minimum distance between 2 lines
   * following http://geomalgorithms.com/a07-_distance.html#Distance-between-Lines
   * @param pos0 origin line0
   * @param dir1 direction line0
   * @param pos1 origin line1
   * @param dir2 direction line1
   * @param tri0 point on line0 closest to line1
   * @param tri1 point on line1 closest to line0
   * @return distance between the lines
   */
    double dist3D_Line_to_Line( Vector3d &pos0, Vector3d &dir1,
                                Vector3d &pos1, Vector3d &dir2,
                                Vector3d &tri0, Vector3d &tri1);

/**
    * This function triangulates the position of a sensor using the horizontal and vertical angles from two ligthouses
    * @param angles0 vertical/horizontal angles form first lighthouse
    * @param angles1 vertical/horizontal angles form second lighthouse
    * @param RT_0 pose matrix of first lighthouse
    * @param RT_1 pose matrix of second lighthouses
    * @param triangulated_position the triangulated position
    * @param ray0 ligthhouse ray
    * @param ray1 ligthhouse ray
    */
    double triangulateFromLighthouseAngles(Vector2d &angles0, Vector2d &angles1, Matrix4d &RT_0, Matrix4d &RT_1,
                                           Vector3d &triangulated_position, Vector3d &ray0, Vector3d &ray1);

    double triangulateFromRays(Vector3d &ray0, Vector3d &ray1, Matrix4d &RT_0, Matrix4d &RT_1, Vector3d &triangulated_position);

    void rayFromLighthouseAngles(Vector2d &angles, Vector3d &ray, int lighthouse);
    void rayFromLighthouseAngles(double elevation, double azimuth, Vector3d &ray, int lighthouse);

    // for each motor and each lighthouse
    LighthouseCalibration calibration[2][2];
};


#include "drake/solvers/fbstab/components/dense_linear_solver.h"

#include <cmath>

#include <Eigen/Dense>

#include "drake/solvers/fbstab/components/dense_data.h"
#include "drake/solvers/fbstab/components/dense_residual.h"
#include "drake/solvers/fbstab/components/dense_variable.h"

namespace drake {
namespace solvers {
namespace fbstab {
namespace {
// Solves the system A*A' x = b in place
// where A is lower triangular and invertible.
void CholeskySolve(const Eigen::MatrixXd& A, Eigen::VectorXd* b) {
  A.triangularView<Eigen::Lower>().solveInPlace(*b);
  A.triangularView<Eigen::Lower>().transpose().solveInPlace(*b);
}
}  //  namespace

DenseLinearSolver::DenseLinearSolver(int nz, int nv) {
  if (nz <= 0 || nv <= 0) {
    throw std::runtime_error(
        "In DenseLinearSolver::DenseLinearSolver: inputs must be positive.");
  }
  nz_ = nz;
  nv_ = nv;

  K_.resize(nz_, nz_);
  r1_.resize(nz_);
  r2_.resize(nv_);
  Gamma_.resize(nv_);
  mus_.resize(nv_);
  gamma_.resize(nv_);
  B_.resize(nv_, nz_);
}

void DenseLinearSolver::SetAlpha(double alpha) { alpha_ = alpha; }

bool DenseLinearSolver::Initialize(const DenseVariable& x,
                                   const DenseVariable& xbar, double sigma) {
  const DenseData* const data = x.data();
  if (xbar.data() != data) {
    throw std::runtime_error(
        "In DenseLinearSolver::Factor: x and xbar have mismatched problem "
        "data.");
  }
  if (xbar.nz_ != x.nz_ || xbar.nv_ != x.nv_) {
    throw std::runtime_error(
        "In DenseLinearSolver::Factor: inputs must be the same size");
  }
  if (xbar.nz_ != nz_ || xbar.nv_ != nv_) {
    throw std::runtime_error(
        "In DenseLinearSolver::Factor: inputs must match object size.");
  }
  if (sigma <= 0) {
    throw std::runtime_error(
        "In DenseLinearSolver::Factor: sigma must be positive.");
  }
  const Eigen::MatrixXd& H = data->H();
  const Eigen::MatrixXd& A = data->A();

  K_ = H + sigma * Eigen::MatrixXd::Identity(nz_, nz_);

  // K <- K + A'*diag(Gamma(x))*A
  Eigen::Vector2d pfb_gradient;
  for (int i = 0; i < nv_; i++) {
    const double ys = x.y()(i) + sigma * (x.v()(i) - xbar.v()(i));
    pfb_gradient = PFBGradient(ys, x.v()(i));
    gamma_(i) = pfb_gradient(0);
    mus_(i) = pfb_gradient(1) + sigma * pfb_gradient(0);
    Gamma_(i) = gamma_(i) / mus_(i);
  }
  // B is used to avoid temporaries
  B_.noalias() = Gamma_.asDiagonal() * A;
  K_.noalias() += A.transpose() * B_;

  // Factor K = LL' in place
  Eigen::LLT<Eigen::Ref<Eigen::MatrixXd> > L(K_);

  Eigen::ComputationInfo status = L.info();
  if (status != Eigen::Success) {
    return false;
  } else {
    return true;
  }
}

bool DenseLinearSolver::Solve(const DenseResidual& r, DenseVariable* x) const {
  if (x == nullptr) {
    throw std::runtime_error("In DenseLinearSolver::Solve: x cannot be null.");
  }
  if (r.nz_ != x->nz_ || r.nv_ != x->nv_) {
    throw std::runtime_error(
        "In DenseLinearSolver::Solve residual and variable objects must be the "
        "same size");
  }
  if (x->nz_ != nz_ || x->nv_ != nv_) {
    throw std::runtime_error(
        "In DenseLinearSolver::Factor: inputs must match object size.");
  }
  const DenseData* const data = x->data();
  const Eigen::MatrixXd& A = data->A();
  const Eigen::VectorXd& b = data->b();

  // This method solves the system:
  // KK'z = rz - A'*diag(1/mus)*rv
  // diag(mus) v = rv + diag(gamma)*A*z
  // Where K has been precomputed by the factor routine.
  // See (28) and (29) in https://arxiv.org/pdf/1901.04046.pdf

  // Compute rz - A'*(rv./mus) and store it in r1_.
  r2_ = r.v_.cwiseQuotient(mus_);
  r1_.noalias() = r.z_ - A.transpose() * r2_;

  // Solve KK'*z = rz - A'*(rv./mus)
  // where K = chol(H + sigma*I + A'*Gamma*A)
  // is assumed to have been computed during the factor phase.
  x->z() = r1_;
  CholeskySolve(K_, x->z_);

  // Compute v = diag(1/mus) * (rv + diag(gamma)*A*z)
  // written so as to avoid temporary creation
  r2_.noalias() = A * x->z();
  r2_.noalias() = gamma_.asDiagonal() * r2_;
  r2_.noalias() += r.v_;

  // v = r2./mus
  x->v() = r2_.cwiseQuotient(mus_);

  // y = b - Az
  x->y() = b - A * x->z();

  return true;
}

Eigen::Vector2d DenseLinearSolver::PFBGradient(double a, double b) const {
  const double r = sqrt(a * a + b * b);
  const double d = 1.0 / sqrt(2.0);

  Eigen::Vector2d v;
  if (r < zero_tolerance_) {
    v(0) = alpha_ * (1.0 - d);
    v(1) = alpha_ * (1.0 - d);

  } else if ((a > 0) && (b > 0)) {
    v(0) = alpha_ * (1.0 - a / r) + (1.0 - alpha_) * b;
    v(1) = alpha_ * (1.0 - b / r) + (1.0 - alpha_) * a;

  } else {
    v(0) = alpha_ * (1.0 - a / r);
    v(1) = alpha_ * (1.0 - b / r);
  }

  return v;
}

}  // namespace fbstab
}  // namespace solvers
}  // namespace drake


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/opencv.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches );

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
            (
                    ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
                    ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
            );
}

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<" usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );

    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        //计算3D坐标
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];

        if ( d == 0 )   // bad depth
            continue;
        float dd = d/5000.0;
        //调用计数坐标函数
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );

        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;

    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    //solvePnPRansac ( pts_3d, pts_2d, K, Mat(), r, t, false );
    Mat R;
    //旋转向量到旋转矩阵
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    vector<cv::Point2f> next_keypoints;
    vector<cv::Point3f> prev_keypoints_3d;
    vector<cv::Point2f> prev_keypoints;
   // list< cv::Point2f > keypoints;      // 因为要删除跟踪失败的点，使用list
    vector<cv::KeyPoint> kps;
    cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
    detector->detect( img_1, kps );
    for ( auto kp:kps )
    {
        prev_keypoints.push_back( kp.pt ); //将坐标放入keypoints链表中
    }

//    for ( auto kp:keypoints )
//    {
//        prev_keypoints.push_back(kp); //prev_keypoints 赋值为keypoints
//    }
    vector<unsigned char> status;
    vector<float> error;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    cv::calcOpticalFlowPyrLK( img_1, img_2, prev_keypoints, next_keypoints, status, error );

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"LK Flow use time："<<time_used.count()<<" seconds."<<endl;

    cout<<"prev:"<<prev_keypoints.size() <<"  next:"<<next_keypoints.size()<<endl;

    vector<cv::KeyPoint> n_Keypoints;
    KeyPoint::convert(next_keypoints, n_Keypoints, 1, 1, 0, -1);

    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( kps, n_Keypoints, match );

    // 把跟丢的点删掉，keypoint 放当前帧

//    int i=0;
//    for ( auto iter=keypoints.begin(); iter!=keypoints.end(); i++)
//    {
//        if ( status[i] == 0 ) //表示状态
//        {
//            iter = keypoints.erase(iter);
//            continue;
//        }
//        *iter = next_keypoints[i];
//        iter++;
//    }
//    for ( Point2f m:prev_keypoints )
//    {
//        //计算3D坐标
//        ushort d = d1.ptr<unsigned short> (int (m.y)) [ int (m.x) ];
//        if ( d == 0 )   // bad depth
//            continue;
//        float dd = d/5000.0;
//        //调用计数坐标函数
//        prev_keypoints_3d.push_back ( Point3f ( m.x*dd, m.y*dd, dd ) );
//    }
//
//
//    Mat r2, t22;
//
//    solvePnP ( prev_keypoints_3d, next_keypoints, K, Mat(), r2, t22, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
//    //solvePnPRansac ( pts_3d, pts_2d, K, Mat(), r, t, false );
//    Mat R2;
//    //旋转向量到旋转矩阵
//    cv::Rodrigues ( r2, R2 ); // r为旋转向量形式，用Rodrigues公式转换为矩阵
//
//    cout<<"R="<<endl<<R2<<endl;
//    cout<<"t="<<endl<<t22<<endl;
    return 0;

}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}


#include <vector>
#include <tuple>
#include <iostream>
#include <fstream>
#include <cmath>
#include <string>
#include <random>
#include <nlopt.hpp>
#include <Eigen/Dense>
#include <boost/math/special_functions/binomial.hpp>

#define MAXBUFSIZE  ((int) 1e6)

unsigned int function_calls =0;
unsigned long int mcmc_steps = 10000;

std::vector< std::pair<int,std::vector<double> > > read_prior(const char*
	filename)
	{
		std::ifstream infile;
		infile.open(filename);

		std::vector< std::pair<int,std::vector<double> > > acc;

		while(!infile.eof())
		{
			std::string line;
			std::getline(infile,line);

			std::stringstream stream(line);
			int size_household;
			stream >> size_household;

			std::vector<double> priors;
			double probab;
			while(stream >> probab)
			{
				priors.push_back(probab);
			}
			acc.push_back(std::make_pair(size_household,priors));
		}
		return acc;
	}

int find_maximum(const std::vector< std::pair<int,
	std::vector<double> > > &priors)
{
	int curr_max = 0;
	for(auto household : priors)
	{
		int temp = household.first;
		if(temp > curr_max) curr_max = temp;
	}
	return curr_max;
}

Eigen::MatrixXd proposal(const std::vector< std::pair<int,
	std::vector<double> > > &priors,int max_n,std::mt19937 &rng)
	{
		Eigen::MatrixXd final_sizes = Eigen::MatrixXd::Zero(max_n+1,max_n);

		for(auto household : priors)
		{
			int number_sick=0;

			for(double ind_prob : household.second) // We go through all members
				// of the household and assign them an individual status
			{
				std::bernoulli_distribution sick(ind_prob);
				if(sick(rng))
				{
					number_sick++;
				}
			}

		final_sizes(number_sick,household.first-1)++;
		}
		return final_sizes;
	}

Eigen::MatrixXd readMatrix(const char *filename)
    {
    int cols = 0, rows = 0;
    double buff[MAXBUFSIZE];

    // Read numbers from file into buffer.
    std::ifstream infile;
    infile.open(filename);
    while (! infile.eof())
        {
        std::string line;
        std::getline(infile, line);

        int temp_cols = 0;
        std::stringstream stream(line);
        while(! stream.eof())
            stream >> buff[cols*rows+temp_cols++];

        if (temp_cols == 0)
            continue;

        if (cols == 0)
            cols = temp_cols;

        rows++;
        }

    infile.close();

    // rows--;

    // Populate matrix with numbers.
    Eigen::MatrixXd result(rows,cols);
    for (int i = 0; i < rows; i++)
        for (int j = 0; j < cols; j++)
            result(i,j) = buff[ cols*i+j ];

    return result;
    }

double Phi(const double &x, const double &alpha, const double &beta, const
	double &k, const int n)
{
	double mean = beta/pow(n,alpha);
	return pow(k/(k+x*mean),k);
}

/* Returns a (n+1)*(n+1) matrix such that F(i,j) = F_i^{n,j}. Columns correspond
to different values of s0, rows are the m values. As such, F is upper
triangular, and F(i,j) for i>j is undefined. */

std::pair<Eigen::MatrixXd,Eigen::MatrixXd> solve_triangular(const
	std::vector<double> &v, const int n)
	{
		Eigen::MatrixXd result1(n+1,n+1);
		Eigen::MatrixXd result2(n+1,n+1);
		for (int s0=0; s0 <= n; ++s0)
		{
			Eigen::MatrixXd A1(s0+1,s0+1);
			Eigen::MatrixXd A2(s0+1,s0+1);
			Eigen::VectorXd B(s0+1);
			for(int i=0; i<=s0; ++i)
			{
				B(i) = boost::math::binomial_coefficient<double>(s0,i);
				for (int j = 0; j <= i; ++j)
				{
					A1(i,j) = boost::math::binomial_coefficient<double>
					(s0-j,i-j)
					/(pow(Phi(s0-i,v[6],v[5],v[4],n),j)*(pow(v[0],s0-i)));
					A2(i,j) = boost::math::binomial_coefficient<double>
					(s0-j,i-j)
					/(pow(Phi(s0-i,v[6],v[5],v[4],n),j)*(pow(v[3],s0-i)));
				}
			}
			Eigen::VectorXd sol1 = A1.triangularView<Eigen::Lower>().solve(B);
			sol1.conservativeResize(n+1);
			result1.col(s0)=sol1;
			Eigen::VectorXd sol2 = A2.triangularView<Eigen::Lower>().solve(B);
			sol2.conservativeResize(n+1);
			result2.col(s0)=sol2;
		}

		for (int i = 0; i <= n; ++i)
		{
			for (int j = 0; j < i; ++j)
			{
				result1(i,j)=1;
				result2(i,j)=0;
			}
		}
 		return std::pair<Eigen::MatrixXd,Eigen::MatrixXd> {result1, result2};
	}

/* Returns the value of T_{(m,n)}, the expected frequency of households of size
n with m infected individuals. Arguments are a tuple (n,m), the running
parameter vector and the precomputed matrix containing the F_m^{n,s0}. */
double expected_prob(const std::tuple<int,int> &sizes, const
	std::vector<double> &parameters, const
	std::pair<Eigen::MatrixXd,Eigen::MatrixXd> &distributions)
	{
	const double n = std::get<0>(sizes); // n \ge 1
	const double m = std::get<1>(sizes); // 0 \le m \le s0

	double pasx = parameters[1];
	double ppr = parameters[2];

	Eigen::MatrixXd dis_current = distributions.first;
	Eigen::MatrixXd dis_prior = distributions.second;

	double sum=0.;

	for(int t=0; t<=(n-m); ++t)
	{
		for(int r=0; r<=(n-m-t); ++r)
		{
			for (int l = 0; l<= (n-m-r-t); ++l)
			{
				double prob1 = boost::math::binomial_coefficient<double>
				(m+t,t)*pow(pasx,t)*pow(1-pasx,m);
				double prob2 = boost::math::binomial_coefficient<double>
				(n,r)*pow(ppr,r)*pow(1-ppr,n-r);
				if(m+t>n-r-l) std::cout << "n=" << n << " m=" << m << " t=" << t
					<< " r=" << r << " l=" << l << std::endl;
				double expected_prob1 = dis_current(m+t,n-r-l);
				double expected_prob2 = dis_prior(l,n-r);
				sum += prob1*prob2*expected_prob1*expected_prob2;
			}
		}
	}
	return sum;
}

double Dev(const std::vector<double> &v, std::vector<double> &grad, void*
	my_func_data)
	{
		/* Contains the observed final sizes k_{(m,n)}. Be careful :
		households(i,j) contains k_{(i,j+1)}. Also, households(i,j)=0
		if i>j. */
		const Eigen::MatrixXd *households =
			static_cast<Eigen::MatrixXd *>(my_func_data);

		int max_n = (*households).cols();

		std::vector< std::pair<Eigen::MatrixXd,Eigen::MatrixXd > >
		distribution_matrices;

		/*	Computes the expected final size distributions F(n; s0,m) for 1
		\le n \le n_max and m\le s0 \le n. Output is a vector whose (n+1)-th
		element contains the matrix of the F(n;s0,m) for a fixed n. */

		for (int n = 1; n <= max_n; ++n)
		{
			std::pair<Eigen::MatrixXd,Eigen::MatrixXd> temp = solve_triangular
			(v,n);
			distribution_matrices.push_back(temp);
		}

		double sum = 0.;

		auto colSums = (*households).colwise().sum();

		for (int n = 1; n <= max_n; ++n)
		{
			double col_sum = colSums(n-1);
			for(int m=0; m<=n; ++m)
			{
				double observed = (*households)(m,n-1);
				if(observed==0.) continue;
				std::tuple<int,int> obs_tuple {n,m};
				double expected = expected_prob(obs_tuple, v,
					distribution_matrices[n-1]);
				sum += observed*(std::log10(observed/col_sum)-
					std::log10(expected));
			}
		}
		return sum;
	}

int main(int argc, char const *argv[])
{
	const auto priors = read_prior("test_prior.txt");

	const int max_n = find_maximum(priors);

	std::vector<Eigen::MatrixXd> households; /* Contiendra les valeurs de
	tailles finales */

	nlopt::opt opt(nlopt::LN_BOBYQA, 7);

	void* f_data;

	opt.set_min_objective(Dev,f_data); // Définit Dev comme fonction à minimiser

	/* Les paramètres sont dans l'ordre (cf Fraser et al. 2011) :

	- La proba d'échappement Q
	- La proba d'infection asymptomatique et infectieuse p_asx
	- La proba d'infection asymptomatique et non-infectieuse p_r
	- La proba d'échappement à une infection antérieure Q_prior
	- Hétérogénéité des infectivités k
	- Facteur de taille du domicile beta
	- Exposant de la taille du domicile alpha

	*/

	std::vector<double> lb { 0.,0.,0.,0.,0.,0.,0. };
	std::vector<double> ub { 1.,1.,1.,1.,5.,5.,5. };
	opt.set_lower_bounds(lb);
	opt.set_upper_bounds(ub);

	std::vector<std::string> var_names
		{"Q","p_asx","p_pr","Q_prior","k","beta","alpha"};
	std::vector<double> init_guess { 0.5,0.5,0.5,0.5,1.,1.,1. } ;

	Eigen::MatrixXd test_matrix = readMatrix("sim_matrix.txt");

	std::vector<std::vector<double> > variables;

	// Eigen::MatrixXd prop = proposal(priors,max_n,rd);
	// std::cout << prop << std::endl;
	void* my_func_data = static_cast<void *>(&test_matrix);
	opt.set_min_objective(Dev,my_func_data);
	double minf =0.;
	try
	{
		nlopt::result result = opt.optimize(init_guess,minf);
	}	catch(const nlopt::roundoff_limited& e)
	{
		for(int i=0; i<7; ++i)
		{
			std::cout << var_names[i] << " : " << init_guess[i] << std::endl;
		}
		std::cout << "Vraisemblance : " << minf << std::endl;
	}

	// std::random_device rd;
	// std::mt19937 rng(rd());

	// for (unsigned long int i = 0; i < mcmc_steps; ++i)
	// {
	// 	Eigen::MatrixXd prop = proposal(priors,max_n,rng);
	// 	std::vector<double> init_guess { 0.5,0.5,0.5,0.5,1.,1.,1. } ;
	// 	void* my_func_data = static_cast<void *>(&prop);
	// 	opt.set_min_objective(Dev,my_func_data);
	// 	opt.set_maxeval(10000);
	// 	double minf = 0.;
	// 	try
	// 	{
	// 		nlopt::result result = opt.optimize(init_guess,minf);
	// 	}	catch(const nlopt::roundoff_limited& e)
	// 	{
	// 		if(isnan(minf))
	// 		{
	// 			std::cout << prop << std::endl;
	// 			break;
	// 		}
	// 		init_guess.push_back(minf);
	// 		variables.push_back(init_guess);
	// 	}
	// }

	// std::ofstream outfile("mcmc_output.csv");
 //    std::ostream_iterator<double> output_iterator(outfile, " ");
 //    for(auto iteration : variables)
 //    {
	// 	std::copy(iteration.begin(),iteration.end(),output_iterator);
	// 	outfile << "\n";
 //    }

	return 0;
}


#pragma once

// system includes --------------------------------------------------
#include <Eigen/Dense>

// own includes -----------------------------------------------------
#include <ridgelet/ridgelet_cell_array.hpp>
#include <ridgelet/rt.hpp>

template <typename FRAME>
std::vector<double>
make_inv_diagonal_preconditioner(const FRAME &frame, double vx, double vy)
{
  auto &lambdas = frame.lambdas();
  unsigned int N = lambdas.size();
  std::vector<double> d(N);

  Eigen::Vector2d sjk;
  Eigen::Vector2d v;
  v(0) = vx;
  v(1) = vy;

  for (unsigned int i = 0; i < N; ++i) {
    int j = lambdas[i].j;
    double p2mn = std::pow(2., j - 1);
    int k = lambdas[i].k;
    if (lambdas[i].t == rt_type::S) {
      d[i] = 1.0;
    } else if (lambdas[i].t == rt_type::X) {
      sjk(0) = 1;
      sjk(1) = double(k) / p2mn;
      sjk.normalize();
      d[i] = 1 + std::pow(2., j) * std::abs(sjk.dot(v));
    } else if (lambdas[i].t == rt_type::Y) {
      sjk(0) = double(k) / p2mn;
      sjk(1) = 1;
      sjk.normalize();
      d[i] = 1 + std::pow(2., j) * std::abs(sjk.dot(v));
    } else if (lambdas[i].t == rt_type::D) {
      k = (k < 0) ? -1 : 1;
      sjk(0) = 1;
      sjk(1) = k;
      sjk.normalize();
      d[i] = 1 + std::pow(2., j) * std::abs(sjk.dot(v));
    } else {
      assert(false);
    }
  }

  return d;
}

template <typename ARRAY_T>
class DiagonalOperator
{
 private:
  typedef RidgeletCellArray<ARRAY_T> rca_t;

 public:
  DiagonalOperator(const std::vector<double> &d)
      : d_(d)
  { /* empty  */
  }

  DiagonalOperator() {}

  DiagonalOperator &operator=(const DiagonalOperator &other) { d_ = other.d_; }

  DiagonalOperator(std::vector<double> &&d)
      : d_(std::forward<std::vector<double>>(d))
  { /* empty  */
  }

  void apply(rca_t &dst) const;
  void apply(rca_t &dst, const rca_t &src) const;

  void invert();

 private:
  std::vector<double> d_;
};

template <typename ARRAY_T>
void
DiagonalOperator<ARRAY_T>::apply(rca_t &dst) const
{
  assert(d_.size() == dst.coeffs().size());

  for (unsigned int i = 0; i < d_.size(); ++i) {
    dst[i] *= d_[i];
  }
}

template <typename ARRAY_T>
void
DiagonalOperator<ARRAY_T>::apply(rca_t &dst, const rca_t &src) const
{
  assert(d_.size() == dst.coeffs().size());
  assert(d_.size() == src.coeffs().size());

  for (unsigned int i = 0; i < d_.size(); ++i) {
    dst[i] = d_[i] * src[i];
  }
}

template <typename ARRAY_T>
void
DiagonalOperator<ARRAY_T>::invert()
{
  for (unsigned int i = 0; i < d_.size(); ++i) {
    d_[i] = 1. / d_[i];
  }
}

// ================================================================================
// ================================================================================
// ================================================================================
class TransportOperator
{
 protected:
  typedef Eigen::ArrayXXcd complex_array_t;
  typedef Eigen::ArrayXd col_t;  // (column vector)

 public:
  TransportOperator(double vx, double vy, double Lx, double Ly, int Nx, int Ny, double dt = 1.0)
      : vx_(vx)
      , vy_(vy)
      , Lx_(Lx)
      , Ly_(Ly)
      , Nx_(Nx)
      , Ny_(Ny)
      , dt_(dt)
  {
    Eigen::Vector2d v;
    v(0) = vx;
    v(1) = vy;

    // init xi_x
    if (Nx % 2 == 0)
      xi_x_ = col_t::LinSpaced(Nx, -Nx / 2, Nx / 2 - 1) / Lx;
    else
      xi_x_ = col_t::LinSpaced(Nx, -Nx / 2, Nx / 2) / Lx;

    // init xi_y
    if (Ny % 2 == 0)
      xi_y_ = col_t::LinSpaced(Ny, -Ny / 2, Ny / 2 - 1) / Ly;
    else
      xi_y_ = col_t::LinSpaced(Ny, -Ny / 2, Ny / 2) / Ly;
  }

  template <typename COMPLEX_ARRAY>
  void apply(COMPLEX_ARRAY &dst, const COMPLEX_ARRAY &src, bool conj = false) const;

  template <typename COMPLEX_ARRAY>
  void apply_bckwrd_euler(COMPLEX_ARRAY &dst) const;

 protected:
  double vx_;
  double vy_;
  double Lx_;
  double Ly_;
  unsigned int Nx_;
  unsigned int Ny_;

  col_t xi_x_;
  col_t xi_y_;
  const double twoPI = 2 * 3.141592653589793238462643;
  const std::complex<double> I = std::complex<double>(0, 1);
  double dt_;
};

template <typename COMPLEX_ARRAY>
void
TransportOperator::apply(COMPLEX_ARRAY &dst, const COMPLEX_ARRAY &src, bool conj) const
{
  typedef typename COMPLEX_ARRAY::Scalar numeric_t;

  const double f = conj ? -1 : 1;
  dst = src +
        f * (twoPI * dt_ * I) *
            (vx_ * xi_x_.transpose().replicate(Ny_, 1) + vy_ * xi_y_.replicate(1, Nx_))
                .cast<numeric_t>()
                .array() *
            src.array();
}

template <typename COMPLEX_ARRAY>
void
TransportOperator::apply_bckwrd_euler(COMPLEX_ARRAY &dst) const
{
  typedef typename COMPLEX_ARRAY::Scalar numeric_t;
  dst = dst.cwiseQuotient(
      COMPLEX_ARRAY::Ones(Ny_, Nx_) +
      twoPI * dt_ * I *
          (vx_ * xi_x_.transpose().replicate(Ny_, 1) + vy_ * xi_y_.replicate(1, Nx_))
              .cast<numeric_t>()
              .array());
}

// ================================================================================
/**
 *   @brief \f$ T' T  \f$
 */
class AhAOp : public TransportOperator
{
 private:
  typedef Eigen::ArrayXXcd complex_array_t;
  typedef Eigen::ArrayXd col_t;

 public:
  AhAOp(double vx, double vy, double Lx, double Ly, int Nx, int Ny, double dt = 1.0)
      : TransportOperator(vx, vy, Lx, Ly, Nx, Ny, dt)
  {
    dt2_ = dt_ * dt_;
  }

  template <typename DERIVED1, typename DERIVED2>
  void apply(Eigen::ArrayBase<DERIVED1> &dst, const Eigen::ArrayBase<DERIVED2> &src) const;

 private:
  using TransportOperator::xi_x_;
  using TransportOperator::xi_y_;
  double dt2_;
};

template <typename DERIVED1, typename DERIVED2>
void
AhAOp::apply(Eigen::ArrayBase<DERIVED1> &dst, const Eigen::ArrayBase<DERIVED2> &src) const
{
  static_assert(std::is_same<typename DERIVED1::Scalar, typename DERIVED2::Scalar>::value,
                "type mismatch");
  typedef typename DERIVED1::Scalar numeric_t;

  dst = src +
        twoPI * twoPI * dt2_ *
            (vx_ * xi_x_.transpose().replicate(Ny_, 1) + vy_ * xi_y_.replicate(1, Nx_))
                .cwiseAbs2()
                .cast<numeric_t>()
                .array() *
            src;
}

// ================================================================================
// ================================================================================
// ================================================================================
// transport operator with boundary conditions
class TransportOperatorBC : public TransportOperator
{
 public:
  template <typename DERIVED>
  TransportOperatorBC(const Eigen::DenseBase<DERIVED> &sigma,
                      double vx,
                      double vy,
                      double Lx,
                      double Ly,
                      int Nx,
                      int Ny,
                      double dt = 1.0)
      : TransportOperator(vx, vy, Lx, Ly, Nx, Ny, dt)
  {
    sigma_ = sigma;
  }

  template <typename COMPLEX_ARRAY>
  void apply(COMPLEX_ARRAY &dst, const COMPLEX_ARRAY &src, bool conj = false) const;

 private:
#ifdef USE_PLANNED_FFT
  typedef FFTr2c<PlannerR2C> fft_t;
#else
  typedef FFTr2c<PlannerR2COD> fft_t;
#endif

  typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> array_t;

 private:
  Eigen::ArrayXXd sigma_;
};

template <typename COMPLEX_ARRAY>
void
TransportOperatorBC::apply(COMPLEX_ARRAY &dst, const COMPLEX_ARRAY &src, bool conj) const
{
  typedef COMPLEX_ARRAY complex_array_t;
  typedef typename complex_array_t::Scalar numeric_t;
  const double f = conj ? -1 : 1;

  // TODO: (inefficient) memory allocation
  array_t fn(src.rows(), src.cols());
  double r = std::sqrt(vx_ * vx_ + vy_ * vy_);
  fft_t fft;
  fft.ift(fn, src);
  fn *= sigma_ * r;
  fft.ft(dst, fn, false);
  dst *= dt_;
  dst += src +
         f * (twoPI * dt_ * I) *
             (vx_ * xi_x_.transpose().replicate(Ny_, 1) + vy_ * xi_y_.replicate(1, Nx_))
                 .cast<numeric_t>()
                 .array() *
             src.array();
}

/**
 * @brief \f$ T'T \f$  * where T is of type \a TransportOperatorBC an absorption
 * term (otherwise
 * identical to \a AhAOp)
 *
 *
 */
class AhAOpsigma : public TransportOperatorBC
{
 private:
  typedef TransportOperatorBC transport_op_t;

 public:
  template <typename DERIVED>
  AhAOpsigma(const Eigen::DenseBase<DERIVED> &sigma,
             double vx,
             double vy,
             double Lx,
             double Ly,
             int Nx,
             int Ny,
             double dt = 1.0)
      : TransportOperatorBC(sigma, vx, vy, Lx, Ly, Nx, Ny, dt)
  { /* empty  */
  }

  template <typename COMPLEX_ARRAY>
  void apply(COMPLEX_ARRAY &dst, const COMPLEX_ARRAY &src) const
  {
    typedef COMPLEX_ARRAY complex_array_t;
    typedef typename complex_array_t::Scalar numeric_t;

    complex_array_t tmp(dst.rows(), dst.cols());

    transport_op_t::apply(tmp, src);
    transport_op_t::apply(dst, tmp, true);
  }
};

// ================================================================================
// ================================================================================
// ================================================================================
/// Preconditioned transport operator \f$ D^{-1} T D\f$
template <typename RT_TYPE, typename OP_T>
class PTransportOp_Base
{
 private:
  typedef RT_TYPE rt_t;
  typedef typename rt_t::complex_array_t complex_array_t;
  typedef OP_T op_t;
  typedef typename rt_t::rt_coeff_t rt_coeff_t;

 public:
  /**
   *
   * @param rt    ridgelet transform object
   * @param aha   A^T A
   * @param vx    velocity in x direction
   * @param vy    velocity in y direction
   */
  PTransportOp_Base(const rt_t &rt, const op_t &aha, double vx, double vy)
      : rt_(rt)
      , aha_(aha)
      , D_(make_inv_diagonal_preconditioner(rt.frame(), vx, vy))
      , rt_coeffs_(rt.frame())
  {
    fi.resize(rt.frame().Ny(), rt.frame().Nx());
    fo.resize(rt.frame().Ny(), rt.frame().Nx());

    // prepare preconditioner
    D_.invert();
  }

  void apply(RidgeletCellArray<rt_coeff_t> &dst, const RidgeletCellArray<rt_coeff_t> &src) const
  {
    // apply D_
    D_.apply(rt_coeffs_, src);
    rt_.irt(fi, rt_coeffs_.coeffs());
    assert(!fi.hasNaN());
    aha_.apply(fo, fi);
    assert(!fo.hasNaN());
    rt_.rt(dst.coeffs(), fo);
    D_.apply(dst);
  }

 private:
  const rt_t &rt_;
  const op_t &aha_;
  DiagonalOperator<rt_coeff_t> D_;

  mutable RidgeletCellArray<rt_coeff_t> rt_coeffs_;
  mutable complex_array_t fi;
  mutable complex_array_t fo;
};

template <typename RT_TYPE = RT<>>
using PTransportOp = PTransportOp_Base<RT_TYPE, AhAOp>;

template <typename RT_TYPE = RT<>>
using PTransportOpBC = PTransportOp_Base<RT_TYPE, AhAOpsigma>;

// ================================================================================
/// Preconditioned transport operator \f$ D^{-1} T D\f$
template <typename RT_TYPE, typename OP_T>
class PTransportOp_BaseId
{
 private:
  typedef RT_TYPE rt_t;
  typedef typename rt_t::complex_array_t complex_array_t;
  typedef OP_T op_t;
  typedef typename rt_t::rt_coeff_t rt_coeff_t;

 public:
  /**
   *
   * @param rt    ridgelet transform object
   * @param aha   A^T A
   * @param vx    velocity in x direction
   * @param vy    velocity in y direction
   */
  PTransportOp_BaseId(const rt_t &rt, const op_t &aha, double vx, double vy)
      : rt_(rt)
      , aha_(aha)
  {
    fi.resize(rt.frame().Ny(), rt.frame().Nx());
    fo.resize(rt.frame().Ny(), rt.frame().Nx());
  }

  void apply(RidgeletCellArray<rt_coeff_t> &dst, const RidgeletCellArray<rt_coeff_t> &src) const
  {
    rt_.irt(fi, src.coeffs());
    assert(!fi.hasNaN());
    aha_.apply(fo, fi);
    assert(!fo.hasNaN());
    rt_.rt(dst.coeffs(), fo);
  }

 private:
  const rt_t &rt_;
  const op_t &aha_;
  mutable complex_array_t fi;
  mutable complex_array_t fo;
};

template <typename RT_TYPE = RT<>>
using PTransportOpId = PTransportOp_BaseId<RT_TYPE, AhAOp>;


#pragma once
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include <iostream>
#include <limits>

#define DBG_MACRO_NO_WARNING
#include <dbg.h>

namespace addn {
struct DN {
  double v;
  double a;

  DN(double _v, double _a) : v(_v), a(_a) {}
  DN(double _v) : DN(_v, 0.0) {}
  DN(const DN& x) : DN(x.v, x.a) {}
  DN() : DN(0.0, 0.0) {}
  inline DN& operator=(const DN& x) {
    v = x.v;
    a = x.a;
    return *this;
  }

  inline DN& operator=(const double x) {
    v = x;
    a = 0;
    return *this;
  }

  inline DN& operator+=(const DN& x) {
    v += x.v;
    a += x.a;
    return *this;
  }

  inline DN& operator/=(const DN& x) {
    a = (a * x.v - x.a * v) / std::pow(x.v, 2.0);
    v /= x.v;
    return *this;
  }

  operator double() const { return v; }
};

inline std::ostream& operator<<(std::ostream& out, const DN& n) {
  out << "(" << n.v << ", " << n.a << ")";
  return out;
}

inline DN operator-(const DN& x) { return DN{-x.v, -x.a}; }
inline DN operator+(const DN& x, const DN& y) { return DN{x.v + y.v, x.a + y.a}; }
inline DN operator-(const DN& x, const DN& y) { return DN{x.v - y.v, x.a - y.a}; }
inline DN operator*(const DN& x, const DN& y) { return DN{x.v * y.v, x.a * y.v + y.a * x.v}; }
inline DN operator/(const DN& x, const DN& y) {
  return DN{x.v / y.v, (x.a * y.v - y.a * x.v) / std::pow(y.v, 2.0)};
}

inline bool operator==(const DN& x, const DN& y) { return x.v == y.v && x.a == y.a; }
inline bool operator==(const DN& x, const double y) { return x.v == y && x.a == 0.0; }
inline bool operator==(const double x, const DN& y) { return x == y.v && 0.0 == y.a; }
inline bool operator!=(const DN& x, const DN& y) { return x.v != y.v || x.a != y.a; }
inline bool operator!=(const DN& x, const double y) { return x.v != y || x.a != 0.0; }
inline bool operator!=(const double x, const DN& y) { return x != y.v || 0.0 != y.a; }
inline bool operator<(const DN& x, const DN& y) { return x.v < y.v; }
inline bool operator<(const DN& x, const double y) { return x.v < y; }
inline bool operator<(const double x, const DN& y) { return x < y; }
inline bool operator>(const DN& x, const DN& y) { return x.v > y.v; }
inline bool operator>(const DN& x, const double y) { return x.v > y; }
inline bool operator>(const double x, const DN& y) { return x > y.v; }
inline bool operator<=(const DN& x, const DN& y) { return x.v <= y.v; }
inline bool operator<=(const DN& x, const double y) { return x.v <= y; }
inline bool operator<=(const double x, const DN& y) { return x <= y.v; }
inline bool operator>=(const DN& x, const DN& y) { return x.v >= y.v; }
inline bool operator>=(const DN& x, const double y) { return x.v >= y; }
inline bool operator>=(const double x, const DN& y) { return x >= y.v; }

inline DN pow(const DN& x, const DN& y) {
  const auto powv = std::pow(x.v, y.v);
  return DN{powv, powv * (std::log(x.v) * y.a + y.v / x.v * x.a)};
}

inline DN pow(const DN& x, const double y) {
  return DN{std::pow(x.v, y), y * std::pow(x.v, y - 1.0) * x.a};
}

inline DN pow(const double x, const DN& y) {
  const auto powv = std::pow(x, y.v);
  return DN{powv, powv * std::log(x) * y.a};
}

inline DN sin(const DN& x) { return DN{std::sin(x.v), x.a * std::cos(x.v)}; }
inline DN cos(const DN& x) { return DN{std::cos(x.v), -x.a * std::sin(x.v)}; }
inline DN tan(const DN& x) {
  const auto tanv = std::tan(x.v);
  return DN{tanv, x.a * (1 + std::pow(tanv, 2.0))};
}

inline DN asin(const DN& x) { return DN{std::asin(x.v), x.a / std::sqrt(1 - std::pow(x.v, 2.0))}; }

inline DN acos(const DN& x) {
  return DN{std::acos(x.v), -x.a / std::sqrt(1 - std::pow(x.v, 2.0))};
}

inline DN atan(const DN& x) {
  return DN{std::atan(x.v), x.a * (1 - std::pow(std::tanh(x.v), 2.0))};
}

inline DN atan2(const DN& y, const DN& x) {
  const DN q = y / x;
  return DN{std::atan2(y.v, x.v), q.a * (1 - std::pow(std::tanh(q.v), 2.0))};
}

inline DN atan2(const DN& y, const double x) {
  return DN{std::atan2(y.v, x), y.a * (1 - std::pow(std::tanh(y.v / x), 2.0))};
}

inline DN atan2(const double x, const DN& y) { return atan2(y, x); }

inline DN exp(const DN& x) { return DN{std::exp(x.v), x.a * std::exp(x.v)}; }
inline DN log(const DN& x) { return DN{std::log(x.v), x.a / x.v}; }
inline DN sqrt(const DN& x) {
  const double v_sqrt = std::sqrt(x.v);
  const double denom  = 2 * v_sqrt;
  return DN{v_sqrt, denom == 0 ? 0.0 : x.a / denom};
}

inline DN abs(const DN& x) { return DN{std::fabs(x.v), std::copysign(x.a, x.v)}; }
inline DN abs2(const DN& x) { return x * x; }
inline DN min(const DN& x, const DN& y) {
  if (std::max(std::fabs(x.v), std::fabs(y.v)) == std::numeric_limits<double>::infinity()) {
    return x <= y ? x : y;
  }

  const auto z = x >= y ? 1.0 : 0.0;
  return DN{z * y.v + (1.0 - z) * x.v, z * y.a + (1.0 - z) * x.a};
}

inline DN max(const DN& x, const DN& y) {
  if (std::max(std::fabs(x.v), std::fabs(y.v)) == std::numeric_limits<double>::infinity()) {
    return x >= y ? x : y;
  }

  const auto z = y >= x ? 1.0 : 0.0;
  return DN{z * y.v + (1.0 - z) * x.v, z * y.a + (1.0 - z) * x.a};
}

inline DN operator+(const DN& x, const double y) { return DN{x.v + y, x.a}; }
inline DN operator+(const double x, const DN& y) { return DN{x + y.v, y.a}; }
inline DN operator-(const DN& x, const double y) { return DN{x.v - y, x.a}; }
inline DN operator-(const double x, const DN& y) { return DN{x - y.v, -y.a}; }
inline DN operator*(const DN& x, const double y) { return DN{x.v * y, x.a * y}; }
inline DN operator*(const double x, const DN& y) { return DN{x * y.v, y.a * x}; }
inline DN operator/(const DN& x, const double y) {
  return DN{x.v / y, (x.a * y) / std::pow(y, 2.0)};
}

inline DN operator/(const double x, const DN& y) {
  return DN{x / y.v, (-y.a * x) / std::pow(y.v, 2.0)};
}

inline DN ceil(const DN& x) { return DN{std::ceil(x.v), 0.0}; }
inline DN floor(const DN& x) { return DN{std::floor(x.v), 0.0}; }

inline DN cosh(const DN& x) { return DN{std::cosh(x.v), x.a * std::sinh(x.v)}; }
inline DN sinh(const DN& x) { return DN{std::sinh(x.v), x.a * std::cosh(x.v)}; }
inline DN tanh(const DN& x) {
  return DN{std::tanh(x.v), x.a * (1.0 - std::pow(std::tanh(x.v), 2))};
}
}  // namespace addn

namespace Eigen {
template <> struct NumTraits<addn::DN> : NumTraits<double> {
  using Real       = addn::DN;
  using NonInteger = addn::DN;
  using Literal    = addn::DN;
  using Nested     = addn::DN;

  enum {
    IsComplex             = 0,
    IsInteger             = 0,
    IsSigned              = 1,
    RequireInitialization = 1,
    ReadCost              = 1,
    AddCost               = 2,
    MulCost               = 4
  };
};
}  // namespace Eigen


/**
 * @file solver_builder.hpp
 * @author François Hamonic (francois.hamonic@gmail.com)
 * @brief OSI_Builder class declaration
 * @version 0.1
 * @date 2020-10-27
 *
 * @copyright Copyright (c) 2020
 */
#ifndef SOLVER_BUILDER_HPP
#define SOLVER_BUILDER_HPP

#include <cmath>
#include <functional>
#include <memory>
#include <numeric>
#include <string>
#include <vector>

#include <boost/iterator/zip_iterator.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/combine.hpp>

#include <range/v3/all.hpp>

namespace SolverBuilder_Utils {
enum InequalitySense { LESS = -1, EQUAL = 0, GREATER = 1 };
enum OptimizationSense { MIN = -1, MAX = 1 };
constexpr double INFTY = std::numeric_limits<double>::max();

class LinearExpression {
private:
    double _constant;
    std::vector<int> _indices;
    std::vector<double> _coefficients;

public:
    LinearExpression() : _constant{0} {}

    void add(double c) { _constant += c; }
    void add(int id, double coef = 1) {
        _indices.push_back(id);
        _coefficients.push_back(coef);
    }

    double getConstant() const { return _constant; }
    int getNbTerms() const { return _indices.size(); }
    int * getIndicesData() { return _indices.data(); }
    double * getCoefficientsData() { return _coefficients.data(); }
    const std::vector<int> & getIndices() const { return _indices; }
    const std::vector<double> & getCoefficients() const {
        return _coefficients;
    }

    LinearExpression & simplify() {
        auto zip_view = ranges::view::zip(_indices, _coefficients);
        ranges::sort(zip_view,
                     [](auto p1, auto p2) { return p1.first < p2.first; });

        const auto begin = zip_view.begin();
        auto first = begin;
        const auto end = zip_view.end();
        for(auto next = first + 1; next != end; ++next) {
            if((*first).first != (*next).first) {
                if((*first).second != 0.0) ++first;
                *first = *next;
                continue;
            }
            (*first).second += (*next).second;
        }
        const std::size_t new_length = std::distance(begin, first + 1);
        _indices.resize(new_length);
        _coefficients.resize(new_length);
        return *this;
    }
};

struct linear_ineq_constraint {
    InequalitySense sense;
    LinearExpression linear_expression;
    linear_ineq_constraint() = default;
};

class linear_ineq_constraint_rhs_easy_init {
private:
    linear_ineq_constraint constraint_data;

public:
    linear_ineq_constraint_rhs_easy_init(linear_ineq_constraint && data)
        : constraint_data(std::move(data)) {}
    linear_ineq_constraint_rhs_easy_init & operator()(double c) {
        constraint_data.linear_expression.add(-c);
        return *this;
    }
    linear_ineq_constraint_rhs_easy_init & operator()(int id, double coef = 1) {
        constraint_data.linear_expression.add(id, -coef);
        return *this;
    }
};

class linear_ineq_constraint_lhs_easy_init {
private:
    linear_ineq_constraint constraint_data;

public:
    linear_ineq_constraint_lhs_easy_init() {}
    linear_ineq_constraint_lhs_easy_init & operator()(double c) {
        constraint_data.linear_expression.add(c);
        return *this;
    }
    linear_ineq_constraint_lhs_easy_init & operator()(int id, double coef = 1) {
        constraint_data.linear_expression.add(id, coef);
        return *this;
    }

    linear_ineq_constraint_rhs_easy_init less() {
        constraint_data.sense = LESS;
        return linear_ineq_constraint_rhs_easy_init(std::move(constraint_data));
    }
    linear_ineq_constraint_rhs_easy_init equal() {
        constraint_data.sense = EQUAL;
        return linear_ineq_constraint_rhs_easy_init(std::move(constraint_data));
    }
    linear_ineq_constraint_rhs_easy_init greater() {
        constraint_data.sense = GREATER;
        return linear_ineq_constraint_rhs_easy_init(std::move(constraint_data));
    }
};

struct linear_range_constraint {
    double lower_bound, upper_bound;
    LinearExpression linear_expression;
    linear_range_constraint()
        : lower_bound{std::numeric_limits<double>::min()}
        , upper_bound{std::numeric_limits<double>::max()} {}
};

class linear_range_constraint_lhs_easy_init {
private:
    linear_range_constraint constraint_data;

public:
    linear_range_constraint_lhs_easy_init() {}
    linear_range_constraint_lhs_easy_init & lower(double c) {
        constraint_data.lower_bound = c;
        return *this;
    }
    linear_range_constraint_lhs_easy_init & upper(double c) {
        constraint_data.upper_bound = c;
        return *this;
    }
    linear_range_constraint_lhs_easy_init & operator()(int id,
                                                       double coef = 1) {
        constraint_data.linear_expression.add(id, coef);
        return *this;
    }

    linear_range_constraint take_data() { return std::move(constraint_data); }
};

class QuadraticExpression {
private:
    LinearExpression _linear_expression;
    std::vector<int> _quad_indices_1;
    std::vector<int> _quad_indices_2;
    std::vector<double> quad_coefficients;

public:
    void add(double c) { _linear_expression.add(c); }
    void add(int id, double coef = 1) { _linear_expression.add(id, coef); }
    void add(int id_1, int id_2, double coef = 1) {
        _quad_indices_1.push_back(id_1);
        _quad_indices_2.push_back(id_2);
        quad_coefficients.push_back(coef);
    }

    const LinearExpression & getLineraExpression() const {
        return _linear_expression;
    }

    double getConstant() const { return _linear_expression.getConstant(); }

    int getNbLinearTerms() const { return _linear_expression.getNbTerms(); }
    int * getLinearIndicesData() { return _linear_expression.getIndicesData(); }
    double * getLinearCoefficientsData() {
        return _linear_expression.getCoefficientsData();
    }
    const std::vector<int> & getLinearIndices() const {
        return _linear_expression.getIndices();
    }
    const std::vector<double> & getLinearCoefficients() const {
        return _linear_expression.getCoefficients();
    }

    int getNbQuadTerms() const { return _quad_indices_1.size(); }
    int * getQuadIndices1Data() { return _quad_indices_1.data(); }
    int * getQuadIndices2Data() { return _quad_indices_2.data(); }
    double * getQuadCoefficientsData() { return quad_coefficients.data(); }
    const std::vector<int> & getQuadIndices1() const { return _quad_indices_1; }
    const std::vector<int> & getQuadIndices2() const { return _quad_indices_2; }
    const std::vector<double> & getQuadCoefficients() const {
        return quad_coefficients;
    }

    bool isLinear() const { return _quad_indices_1.empty(); }

    QuadraticExpression & simplify() {
        _linear_expression.simplify();
        return *this;
    }
};

struct quadratic_ineq_constraint {
    InequalitySense sense;
    QuadraticExpression quadratic_expression;
    quadratic_ineq_constraint() = default;
};

class quadratic_ineq_constraint_rhs_easy_init {
private:
    quadratic_ineq_constraint constraint_data;

public:
    quadratic_ineq_constraint_rhs_easy_init(quadratic_ineq_constraint && data)
        : constraint_data(std::move(data)) {}
    quadratic_ineq_constraint_rhs_easy_init & operator()(double c) {
        constraint_data.quadratic_expression.add(-c);
        return *this;
    }
    quadratic_ineq_constraint_rhs_easy_init & operator()(int id,
                                                         double coef = 1) {
        constraint_data.quadratic_expression.add(id, -coef);
        return *this;
    }
    quadratic_ineq_constraint_rhs_easy_init & operator()(int id_1, int id_2,
                                                         double coef = 1) {
        constraint_data.quadratic_expression.add(id_1, id_2, -coef);
        return *this;
    }

    quadratic_ineq_constraint take_data() { return std::move(constraint_data); }
};

class quadratic_ineq_constraint_lhs_easy_init {
private:
    quadratic_ineq_constraint constraint_data;

public:
    quadratic_ineq_constraint_lhs_easy_init() {}
    quadratic_ineq_constraint_lhs_easy_init & operator()(double c) {
        constraint_data.quadratic_expression.add(c);
        return *this;
    }
    quadratic_ineq_constraint_lhs_easy_init & operator()(int id,
                                                         double coef = 1) {
        constraint_data.quadratic_expression.add(id, coef);
        return *this;
    }
    quadratic_ineq_constraint_lhs_easy_init & operator()(int id_1, int id_2,
                                                         double coef = 1) {
        constraint_data.quadratic_expression.add(id_1, id_2, coef);
        return *this;
    }

    quadratic_ineq_constraint_rhs_easy_init less() {
        constraint_data.sense = LESS;
        return quadratic_ineq_constraint_rhs_easy_init(
            std::move(constraint_data));
    }
    quadratic_ineq_constraint_rhs_easy_init equal() {
        constraint_data.sense = EQUAL;
        return quadratic_ineq_constraint_rhs_easy_init(
            std::move(constraint_data));
    }
    quadratic_ineq_constraint_rhs_easy_init greater() {
        constraint_data.sense = GREATER;
        return quadratic_ineq_constraint_rhs_easy_init(
            std::move(constraint_data));
    }
};

}  // namespace SolverBuilder_Utils

// using namespace SolverBuilder_Utils;

// /**
//  * @brief A practical class for building OsiSolver instances
//  */
// class SolverBuilder {
//     public:
//         class VarType {
//             protected:
//                 int begin_id;
//                 int end_id;
//                 double _default_lb;
//                 double _default_ub;
//                 bool _integer;
//                 VarType(int number, double lb=0, double ub=INFTY, bool
//                 integer=false) : begin_id(0), end_id(number),
//                 _default_lb(lb), _default_ub(ub), _integer(integer) {}
//                 VarType() : VarType(0) {}
//             public:
//                 void offsetIds(int offset) { begin_id += offset; end_id+=
//                 offset; } int getNumber() const { return end_id - begin_id; }
//                 double getDefaultLB() { return _default_lb; }
//                 double getDefaultUB() { return _default_ub; }
//                 bool isInteger() { return _integer; }
//         };
//     private:
//         int nb_vars;
//         std::vector<VarType*> varTypes;

//         std::unique_ptr<double[]> objective;
//         std::unique_ptr<double[]> col_lb;
//         std::unique_ptr<double[]> col_ub;
//         std::unique_ptr<std::string[]> col_names;

//         std::vector<int> starts;
//         std::vector<int> indices;
//         std::vector<double> coefficients;

//         std::vector<double> row_lb;
//         std::vector<double> row_ub;

//         std::vector<int> integers_variables;
//     public:
//         // OSI_Builder();
//         // ~OSI_Builder();

//         // OSI_Builder & addVarType(VarType * var_type);
//         // void init();
//         // OSI_Builder & setObjective(int  var_id, double coef);
//         // OSI_Builder & setBounds(int  var_id, double lb, double ub);
//         // OSI_Builder & buffEntry(int  var_id, double coef);
//         // OSI_Builder & popEntryBuffer();
//         // OSI_Builder & clearEntryBuffer();
//         // OSI_Builder & pushRowWithoutClearing(double lb, double ub);
//         // OSI_Builder & pushRow(double lb, double ub);
//         // OSI_Builder & setColName(int var_id, std::string name);

//         // OSI_Builder & setContinuous(int var_id);
//         // OSI_Builder & setInteger(int var_id);

//         template <class OsiSolver>
//         OsiSolver * buildSolver(int sense, bool relaxed=false) {
//             OsiSolver * solver = new OsiSolver();
//             solver->loadProblem(*matrix, col_lb, col_ub, objective,
//             row_lb.data(), row_ub.data()); solver->setObjSense(sense);
//             if(relaxed)
//                 return solver;
//             for(int i : integers_variables)
//                 solver->setInteger(i);
//             solver->setColNames(colNames, 0, nb_vars, 0);
//             return solver;
//         }

//         static int nb_pairs(int n) {
//             return n*(n-1)/2;
//         };
//         static int nb_couples(int n) {
//             return 2*nb_pairs(n);
//         };

//         static int compose_pair(int i, int j) {
//             assert(i != j);
//             if(i > j) std::swap(i,j);
//             return nb_pairs(j)+i;
//         };
//         static std::pair<int,int> retrieve_pair(int id) {
//             const int j = std::round(std::sqrt(2*id+1));
//             const int i = id - nb_pairs(j);
//             return std::make_pair(i,j);
//         };
//         static int compose_couple(int i, int j) {
//             return 2*compose_pair(i, j) + (i < j ? 0 : 1);
//         };
//         static std::pair<int,int> retrieve_couple(int id) {
//             std::pair<int,int> p = retrieve_pair(id / 2);
//             if(id % 2 == 1) std::swap(p.first, p.second);
//             return p;
//         };

//         const std::vector<VarType*> & getVarTypes() { return varTypes; }

//         int getNbVars() const { return nb_vars; };

//         int getNbNonZeroVars() const {
//             std::vector<int> non_zero(nb_vars, 0);
//             const int * indices = matrix->getIndices();
//             for(int i=0; i<matrix->getNumElements(); ++i)
//                 non_zero[indices[i]] = 1;
//             return std::accumulate(non_zero.begin(), non_zero.end(), 0);
//         };
//         int getNbConstraints() const { return matrix->getNumRows(); };

//         int getNbElems() const { return nb_entries; };

//         double * getObjective() { return objective; }
//         CoinPackedMatrix * getMatrix() { return matrix; }

//         double * getColLB() { return col_lb; }
//         double * getColUB() { return col_ub; }

//         double * getRowLB() { return row_lb.data(); }
//         double * getRowUB() { return row_ub.data(); }
// };

#endif  // OSICLPSOLVER_BUILDER_HPP


/*
 * Copyright Nick Thompson, 2018
 * Use, modification and distribution are 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_MATH_QUADRATURE_NAIVE_MONTE_CARLO_HPP
#define BOOST_MATH_QUADRATURE_NAIVE_MONTE_CARLO_HPP
#include <sstream>
#include <algorithm>
#include <vector>
#include <atomic>
#include <memory>
#include <functional>
#include <future>
#include <thread>
#include <initializer_list>
#include <utility>
#include <random>
#include <chrono>
#include <map>
#include <type_traits>
#include <boost/math/policies/error_handling.hpp>

namespace boost { namespace math { namespace quadrature {

namespace detail {
  enum class limit_classification {FINITE,
                                   LOWER_BOUND_INFINITE,
                                   UPPER_BOUND_INFINITE,
                                   DOUBLE_INFINITE};
}

template<class Real, class F, class RandomNumberGenerator = std::mt19937_64, class Policy = boost::math::policies::policy<>,
         typename std::enable_if<std::is_trivially_copyable<Real>::value, bool>::type = true>
class naive_monte_carlo
{
public:
    naive_monte_carlo(const F& integrand,
                      std::vector<std::pair<Real, Real>> const & bounds,
                      Real error_goal,
                      bool singular = true,
                      uint64_t threads = std::thread::hardware_concurrency(),
                      uint64_t seed = 0) noexcept : m_num_threads{threads}, m_seed{seed}
    {
        using std::numeric_limits;
        using std::sqrt;
        uint64_t n = bounds.size();
        m_lbs.resize(n);
        m_dxs.resize(n);
        m_limit_types.resize(n);
        m_volume = 1;
        static const char* function = "boost::math::quadrature::naive_monte_carlo<%1%>";
        for (uint64_t i = 0; i < n; ++i)
        {
            if (bounds[i].second <= bounds[i].first)
            {
                boost::math::policies::raise_domain_error(function, "The upper bound is <= the lower bound.\n", bounds[i].second, Policy());
                return;
            }
            if (bounds[i].first == -numeric_limits<Real>::infinity())
            {
                if (bounds[i].second == numeric_limits<Real>::infinity())
                {
                    m_limit_types[i] = detail::limit_classification::DOUBLE_INFINITE;
                }
                else
                {
                    m_limit_types[i] = detail::limit_classification::LOWER_BOUND_INFINITE;
                    // Ok ok this is bad to use the second bound as the lower limit and then reflect.
                    m_lbs[i] = bounds[i].second;
                    m_dxs[i] = numeric_limits<Real>::quiet_NaN();
                }
            }
            else if (bounds[i].second == numeric_limits<Real>::infinity())
            {
                m_limit_types[i] = detail::limit_classification::UPPER_BOUND_INFINITE;
                if (singular)
                {
                    // I've found that it's easier to sample on a closed set and perturb the boundary
                    // than to try to sample very close to the boundary.
                    m_lbs[i] = std::nextafter(bounds[i].first, (std::numeric_limits<Real>::max)());
                }
                else
                {
                    m_lbs[i] = bounds[i].first;
                }
                m_dxs[i] = numeric_limits<Real>::quiet_NaN();
            }
            else
            {
                m_limit_types[i] = detail::limit_classification::FINITE;
                if (singular)
                {
                    if (bounds[i].first == 0)
                    {
                        m_lbs[i] = std::numeric_limits<Real>::epsilon();
                    }
                    else
                    {
                        m_lbs[i] = std::nextafter(bounds[i].first, (std::numeric_limits<Real>::max)());
                    }

                    m_dxs[i] = std::nextafter(bounds[i].second, std::numeric_limits<Real>::lowest()) - m_lbs[i];
                }
                else
                {
                    m_lbs[i] = bounds[i].first;
                    m_dxs[i] = bounds[i].second - bounds[i].first;
                }
                m_volume *= m_dxs[i];
            }
        }

        m_integrand = [this, &integrand](std::vector<Real> & x)->Real
        {
            Real coeff = m_volume;
            for (uint64_t i = 0; i < x.size(); ++i)
            {
                // Variable transformation are listed at:
                // https://en.wikipedia.org/wiki/Numerical_integration
                // However, we've made some changes to these so that we can evaluate on a compact domain.
                if (m_limit_types[i] == detail::limit_classification::FINITE)
                {
                    x[i] = m_lbs[i] + x[i]*m_dxs[i];
                }
                else if (m_limit_types[i] == detail::limit_classification::UPPER_BOUND_INFINITE)
                {
                    Real t = x[i];
                    Real z = 1/(1 + numeric_limits<Real>::epsilon() - t);
                    coeff *= (z*z)*(1 + numeric_limits<Real>::epsilon());
                    x[i] = m_lbs[i] + t*z;
                }
                else if (m_limit_types[i] == detail::limit_classification::LOWER_BOUND_INFINITE)
                {
                    Real t = x[i];
                    Real z = 1/(t+sqrt((numeric_limits<Real>::min)()));
                    coeff *= (z*z);
                    x[i] = m_lbs[i] + (t-1)*z;
                }
                else
                {
                    Real t1 = 1/(1+numeric_limits<Real>::epsilon() - x[i]);
                    Real t2 = 1/(x[i]+numeric_limits<Real>::epsilon());
                    x[i] = (2*x[i]-1)*t1*t2/4;
                    coeff *= (t1*t1+t2*t2)/4;
                }
            }
            return coeff*integrand(x);
        };

        // If we don't do a single function call in the constructor,
        // we can't do a restart.
        std::vector<Real> x(m_lbs.size());

        // If the seed is zero, that tells us to choose a random seed for the user:
        if (seed == 0)
        {
            std::random_device rd;
            seed = rd();
        }

        RandomNumberGenerator gen(seed);
        Real inv_denom = 1/static_cast<Real>(((gen.max)()-(gen.min)()));

        m_num_threads = (std::max)(m_num_threads, (uint64_t) 1);
        m_thread_calls.reset(new std::atomic<uint64_t>[threads]);
        m_thread_Ss.reset(new std::atomic<Real>[threads]);
        m_thread_averages.reset(new std::atomic<Real>[threads]);

        Real avg = 0;
        for (uint64_t i = 0; i < m_num_threads; ++i)
        {
            for (uint64_t j = 0; j < m_lbs.size(); ++j)
            {
                x[j] = (gen()-(gen.min)())*inv_denom;
            }
            Real y = m_integrand(x);
            m_thread_averages[i] = y; // relaxed store
            m_thread_calls[i] = 1;
            m_thread_Ss[i] = 0;
            avg += y;
        }
        avg /= m_num_threads;
        m_avg = avg; // relaxed store

        m_error_goal = error_goal; // relaxed store
        m_start = std::chrono::system_clock::now();
        m_done = false; // relaxed store
        m_total_calls = m_num_threads;  // relaxed store
        m_variance = (numeric_limits<Real>::max)();
    }

    std::future<Real> integrate()
    {
        // Set done to false in case we wish to restart:
        m_done.store(false); // relaxed store, no worker threads yet
        m_start = std::chrono::system_clock::now();
        return std::async(std::launch::async,
                          &naive_monte_carlo::m_integrate, this);
    }

    void cancel()
    {
        // If seed = 0 (meaning have the routine pick the seed), this leaves the seed the same.
        // If seed != 0, then the seed is changed, so a restart doesn't do the exact same thing.
        m_seed = m_seed*m_seed;
        m_done = true; // relaxed store, worker threads will get the message eventually
        // Make sure the error goal is infinite, because otherwise we'll loop when we do the final error goal check:
        m_error_goal = (std::numeric_limits<Real>::max)();
    }

    Real variance() const
    {
        return m_variance.load();
    }

    Real current_error_estimate() const
    {
        using std::sqrt;
        //
        // There is a bug here: m_variance and m_total_calls get updated asynchronously
        // and may be out of synch when we compute the error estimate, not sure if it matters though...
        //
        return sqrt(m_variance.load()/m_total_calls.load());
    }

    std::chrono::duration<Real> estimated_time_to_completion() const
    {
        auto now = std::chrono::system_clock::now();
        std::chrono::duration<Real> elapsed_seconds = now - m_start;
        Real r = this->current_error_estimate()/m_error_goal.load(); // relaxed load
        if (r*r <= 1) {
            return 0*elapsed_seconds;
        }
        return (r*r - 1)*elapsed_seconds;
    }

    void update_target_error(Real new_target_error)
    {
        m_error_goal = new_target_error;  // relaxed store
    }

    Real progress() const
    {
        Real r = m_error_goal.load()/this->current_error_estimate();  // relaxed load
        if (r*r >= 1)
        {
            return 1;
        }
        return r*r;
    }

    Real current_estimate() const
    {
        return m_avg.load();
    }

    uint64_t calls() const
    {
        return m_total_calls.load();  // relaxed load
    }

private:

   Real m_integrate()
   {
      uint64_t seed;
      // If the user tells us to pick a seed, pick a seed:
      if (m_seed == 0)
      {
         std::random_device rd;
         seed = rd();
      }
      else // use the seed we are given:
      {
         seed = m_seed;
      }
      RandomNumberGenerator gen(seed);
      int max_repeat_tries = 5;
      do{

         if (max_repeat_tries < 5)
         {
            m_done = false;

#ifdef BOOST_NAIVE_MONTE_CARLO_DEBUG_FAILURES
            std::cout << "Failed to achieve required tolerance first time through..\n";
            std::cout << "  variance =    " << m_variance << std::endl;
            std::cout << "  average =     " << m_avg << std::endl;
            std::cout << "  total calls = " << m_total_calls << std::endl;

            for (std::size_t i = 0; i < m_num_threads; ++i)
               std::cout << "  thread_calls[" << i << "] = " << m_thread_calls[i] << std::endl;
            for (std::size_t i = 0; i < m_num_threads; ++i)
               std::cout << "  thread_averages[" << i << "] = " << m_thread_averages[i] << std::endl;
            for (std::size_t i = 0; i < m_num_threads; ++i)
               std::cout << "  thread_Ss[" << i << "] = " << m_thread_Ss[i] << std::endl;
#endif
         }

         std::vector<std::thread> threads(m_num_threads);
         for (uint64_t i = 0; i < threads.size(); ++i)
         {
            threads[i] = std::thread(&naive_monte_carlo::m_thread_monte, this, i, gen());
         }
         do {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            uint64_t total_calls = 0;
            for (uint64_t i = 0; i < m_num_threads; ++i)
            {
               uint64_t t_calls = m_thread_calls[i].load(std::memory_order_consume);
               total_calls += t_calls;
            }
            Real variance = 0;
            Real avg = 0;
            for (uint64_t i = 0; i < m_num_threads; ++i)
            {
               uint64_t t_calls = m_thread_calls[i].load(std::memory_order_consume);
               // Will this overflow? Not hard to remove . . .
               avg += m_thread_averages[i].load(std::memory_order_relaxed)*((Real)t_calls / (Real)total_calls);
               variance += m_thread_Ss[i].load(std::memory_order_relaxed);
            }
            m_avg.store(avg, std::memory_order_release);
            m_variance.store(variance / (total_calls - 1), std::memory_order_release);
            m_total_calls = total_calls; // relaxed store, it's just for user feedback
            // Allow cancellation:
            if (m_done) // relaxed load
            {
               break;
            }
         } while (m_total_calls < 2048 || this->current_error_estimate() > m_error_goal.load(std::memory_order_consume));
         // Error bound met; signal the threads:
         m_done = true; // relaxed store, threads will get the message in the end
         std::for_each(threads.begin(), threads.end(),
            std::mem_fn(&std::thread::join));
         if (m_exception)
         {
            std::rethrow_exception(m_exception);
         }
         // Incorporate their work into the final estimate:
         uint64_t total_calls = 0;
         for (uint64_t i = 0; i < m_num_threads; ++i)
         {
            uint64_t t_calls = m_thread_calls[i].load(std::memory_order_consume);
            total_calls += t_calls;
         }
         Real variance = 0;
         Real avg = 0;

         for (uint64_t i = 0; i < m_num_threads; ++i)
         {
            uint64_t t_calls = m_thread_calls[i].load(std::memory_order_consume);
            // Averages weighted by the number of calls the thread made:
            avg += m_thread_averages[i].load(std::memory_order_relaxed)*((Real)t_calls / (Real)total_calls);
            variance += m_thread_Ss[i].load(std::memory_order_relaxed);
         }
         m_avg.store(avg, std::memory_order_release);
         m_variance.store(variance / (total_calls - 1), std::memory_order_release);
         m_total_calls = total_calls; // relaxed store, this is just user feedback

         // Sometimes, the master will observe the variance at a very "good" (or bad?) moment,
         // Then the threads proceed to find the variance is much greater by the time they hear the message to stop.
         // This *WOULD* make sure that the final error estimate is within the error bounds.
      }
      while ((--max_repeat_tries >= 0) && (this->current_error_estimate() > m_error_goal));

      return m_avg.load(std::memory_order_consume);
    }

    void m_thread_monte(uint64_t thread_index, uint64_t seed)
    {
        using std::numeric_limits;
        try
        {
            std::vector<Real> x(m_lbs.size());
            RandomNumberGenerator gen(seed);
            Real inv_denom = (Real) 1/(Real)( (gen.max)() - (gen.min)()  );
            Real M1 = m_thread_averages[thread_index].load(std::memory_order_consume);
            Real S = m_thread_Ss[thread_index].load(std::memory_order_consume);
            // Kahan summation is required or the value of the integrand will go on a random walk during long computations.
            // See the implementation discussion.
            // The idea is that the unstabilized additions have error sigma(f)/sqrt(N) + epsilon*N, which diverges faster than it converges!
            // Kahan summation turns this to sigma(f)/sqrt(N) + epsilon^2*N, and the random walk occurs on a timescale of 10^14 years (on current hardware)
            Real compensator = 0;
            uint64_t k = m_thread_calls[thread_index].load(std::memory_order_consume);
            while (!m_done) // relaxed load
            {
                int j = 0;
                // If we don't have a certain number of calls before an update, we can easily terminate prematurely
                // because the variance estimate is way too low. This magic number is a reasonable compromise, as 1/sqrt(2048) = 0.02,
                // so it should recover 2 digits if the integrand isn't poorly behaved, and if it is, it should discover that before premature termination.
                // Of course if the user has 64 threads, then this number is probably excessive.
                int magic_calls_before_update = 2048;
                while (j++ < magic_calls_before_update)
                {
                    for (uint64_t i = 0; i < m_lbs.size(); ++i)
                    {
                        x[i] = (gen() - (gen.min)())*inv_denom;
                    }
                    Real f = m_integrand(x);
                    using std::isfinite;
                    if (!isfinite(f))
                    {
                        // The call to m_integrand transform x, so this error message states the correct node.
                        std::stringstream os;
                        os << "Your integrand was evaluated at {";
                        for (uint64_t i = 0; i < x.size() -1; ++i)
                        {
                             os << x[i] << ", ";
                        }
                        os << x[x.size() -1] << "}, and returned " << f << std::endl;
                        static const char* function = "boost::math::quadrature::naive_monte_carlo<%1%>";
                        boost::math::policies::raise_domain_error(function, os.str().c_str(), /*this is a dummy arg to make it compile*/ 7.2, Policy());
                    }
                    ++k;
                    Real term = (f - M1)/k;
                    Real y1 = term - compensator;
                    Real M2 = M1 + y1;
                    compensator = (M2 - M1) - y1;
                    S += (f - M1)*(f - M2);
                    M1 = M2;
                }
                m_thread_averages[thread_index].store(M1, std::memory_order_release);
                m_thread_Ss[thread_index].store(S, std::memory_order_release);
                m_thread_calls[thread_index].store(k, std::memory_order_release);
            }
        }
        catch (...)
        {
            // Signal the other threads that the computation is ruined:
            m_done = true; // relaxed store
            std::lock_guard<std::mutex> lock(m_exception_mutex); // Scoped lock to prevent race writing to m_exception
            m_exception = std::current_exception();
        }
    }

    std::function<Real(std::vector<Real> &)> m_integrand;
    uint64_t m_num_threads;
    std::atomic<uint64_t> m_seed;
    std::atomic<Real> m_error_goal;
    std::atomic<bool> m_done;
    std::vector<Real> m_lbs;
    std::vector<Real> m_dxs;
    std::vector<detail::limit_classification> m_limit_types;
    Real m_volume;
    std::atomic<uint64_t> m_total_calls;
    // I wanted these to be vectors rather than maps,
    // but you can't resize a vector of atomics.
    std::unique_ptr<std::atomic<uint64_t>[]> m_thread_calls;
    std::atomic<Real> m_variance;
    std::unique_ptr<std::atomic<Real>[]> m_thread_Ss;
    std::atomic<Real> m_avg;
    std::unique_ptr<std::atomic<Real>[]> m_thread_averages;
    std::chrono::time_point<std::chrono::system_clock> m_start;
    std::exception_ptr m_exception;
    std::mutex m_exception_mutex;
};

}}}
#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2013 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file kahalesmilesection.hpp
    \brief Arbitrage free smile section using a C^1 inter- and extrapolation
   method proposed by Kahale, see
   http://www.risk.net/data/Pay_per_view/risk/technical/2004/0504_tech_option2.pdf
   Exponential extrapolation for high strikes can be used alternatively to avoid
   a too slowly decreasing call price function. Note that in the leftmost
   interval and right from the last grid point the input smile is always
   replaced by the extrapolating functional forms, so if you are sure that the
   input smile is globally arbitrage free and you do not want to change it in
   these strike regions you should not use this class at all.
   Input smile sections with a shift are handled accordingly, normal input
   smile section are not possible though.
*/

#ifndef quantlib_kahale_smile_section_hpp
#define quantlib_kahale_smile_section_hpp

#include <ql/termstructures/volatility/smilesection.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/termstructures/volatility/smilesectionutils.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/math/distributions/normal.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif
#include <vector>
#include <utility>

// numerical constants, still experimental
#define QL_KAHALE_FMAX QL_MAX_REAL
#define QL_KAHALE_SMAX 5.0
#define QL_KAHALE_ACC 1E-12
#define QL_KAHALE_EPS QL_EPSILON

namespace QuantLib {

    class KahaleSmileSection : public SmileSection {

      public:
        struct cFunction {
            // this is just a helper class where we do not want virtual
            // functions
            cFunction(Real f, Real s, Real a, Real b)
                : f_(f), s_(s), a_(a), b_(b), exponential_(false) {}
            cFunction(Real a, Real b) : a_(a), b_(b), exponential_(true) {}
            Real operator()(Real k) {
                if (exponential_)
                    return std::exp(-a_ * k + b_);
                if (s_ < QL_EPSILON)
                    return std::max(f_ - k, 0.0) + a_ * k + b_;
                boost::math::normal normal;
                Real d1 = std::log(f_ / k) / s_ + s_ / 2.0;
                Real d2 = d1 - s_;
                return f_ * boost::math::cdf(normal, d1) -
                       k * boost::math::cdf(normal, d2) + a_ * k + b_;
            }
            Real f_, s_, a_, b_;
            const bool exponential_;
        };

        struct aHelper {
            aHelper(Real k0, Real k1, Real c0, Real c1, Real c0p, Real c1p)
                : k0_(k0), k1_(k1), c0_(c0), c1_(c1), c0p_(c0p), c1p_(c1p) {}
            Real operator()(Real a) const {
                boost::math::normal normal;
                Real d20 = boost::math::quantile(normal, -c0p_ + a);
                Real d21 = boost::math::quantile(normal, -c1p_ + a);
                Real alpha = (d20 - d21) / (std::log(k0_) - std::log(k1_));
                Real beta = d20 - alpha * std::log(k0_);
                s_ = -1.0 / alpha;
                f_ = std::exp(s_ * (beta + s_ / 2.0));
                QL_REQUIRE(f_ < QL_KAHALE_FMAX, "dummy"); // this is caught
                cFunction cTmp(f_, s_, a, 0.0);
                b_ = c0_ - cTmp(k0_);
                cFunction c(f_, s_, a, b_);
                return c(k1_) - c1_;
            }
            Real k0_, k1_, c0_, c1_, c0p_, c1p_;
            mutable Real s_, f_, b_;
        };

        struct sHelper {
            sHelper(Real k0, Real c0, Real c0p) : k0_(k0), c0_(c0), c0p_(c0p) {}
            Real operator()(Real s) const {
                s = std::max(s, 0.0);
                boost::math::normal normal;
                Real d20 = boost::math::quantile(normal, -c0p_);
                f_ = k0_ * std::exp(s * d20 + s * s / 2.0);
                QL_REQUIRE(f_ < QL_KAHALE_FMAX, "dummy"); // this is caught
                cFunction c(f_, s, 0.0, 0.0);
                return c(k0_) - c0_;
            }
            Real k0_, c0_, c0p_;
            mutable Real f_;
        };

        struct sHelper1 {
            sHelper1(Real k1, Real c0, Real c1, Real c1p)
                : k1_(k1), c0_(c0), c1_(c1), c1p_(c1p) {}
            Real operator()(Real s) const {
                s = std::max(s, 0.0);
                boost::math::normal normal;
                Real d21 = boost::math::quantile(normal, -c1p_);
                f_ = k1_ * std::exp(s * d21 + s * s / 2.0);
                QL_REQUIRE(f_ < QL_KAHALE_FMAX, "dummy"); // this is caught
                b_ = c0_ - f_;
                cFunction c(f_, s, 0.0, b_);
                return c(k1_) - c1_;
            }
            Real k1_, c0_, c1_, c1p_;
            mutable Real f_, b_;
        };

        KahaleSmileSection(const boost::shared_ptr<SmileSection> source,
                           const Real atm = Null<Real>(),
                           const bool interpolate = false,
                           const bool exponentialExtrapolation = false,
                           const bool deleteArbitragePoints = false,
                           const std::vector<Real> &moneynessGrid =
                               std::vector<Real>(),
                           const Real gap = 1.0E-5,
                           const int forcedLeftIndex = -1,
                           const int forcedRightIndex = QL_MAX_INTEGER);

        Real minStrike() const { return 0.0; }
        Real maxStrike() const { return QL_MAX_REAL; }
        Real atmLevel() const { return f_; }
        const Date& exerciseDate() const { return source_->exerciseDate(); }
        Time exerciseTime() const { return source_->exerciseTime(); }
        const DayCounter& dayCounter() const { return source_->dayCounter(); }
        const Date& referenceDate() const { return source_->referenceDate(); }
        const VolatilityType volatilityType() const {
            return source_->volatilityType();
        }
        const Real shift() const { return source_->shift(); }

        Real leftCoreStrike() const { return k_[leftIndex_]; }
        Real rightCoreStrike() const { return k_[rightIndex_]; }

        std::pair<Size, Size> coreIndices() const {
            return std::make_pair(leftIndex_, rightIndex_);
        }

        Real optionPrice(Rate strike, Option::Type type = Option::Call,
                         Real discount = 1.0) const;

      protected:
        Volatility volatilityImpl(Rate strike) const;

      private:
        Size index(Rate strike) const;
        void compute();
        boost::shared_ptr<SmileSection> source_;
        std::vector<Real> moneynessGrid_, k_, c_;
        Real f_;
        const Real gap_;
        Size leftIndex_, rightIndex_;
        std::vector<boost::shared_ptr<cFunction> > cFunctions_;
        const bool interpolate_, exponentialExtrapolation_;
        int forcedLeftIndex_, forcedRightIndex_;
        boost::shared_ptr<SmileSectionUtils> ssutils_;
    };
}

#endif