/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <ql/instruments/makecapfloor.hpp>
#include <ql/option.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/pricingengines/capfloor/bacheliercapfloorengine.hpp>
#include <ql/pricingengines/capfloor/blackcapfloorengine.hpp>
#include <ql/utilities/dataformatters.hpp>
#include <qle/termstructures/optionletstripper1.hpp>

#include <boost/make_shared.hpp>

using boost::shared_ptr;

namespace QuantExt {

OptionletStripper1::OptionletStripper1(const shared_ptr<QuantExt::CapFloorTermVolSurface>& termVolSurface,
                                       const shared_ptr<IborIndex>& index, Rate switchStrike, Real accuracy,
                                       Natural maxIter, const Handle<YieldTermStructure>& discount,
                                       const VolatilityType type, const Real displacement, bool dontThrow,
                                       const optional<VolatilityType> targetVolatilityType,
                                       const optional<Real> targetDisplacement, Real dontThrowMinVol)
    : OptionletStripper(termVolSurface, index, discount, targetVolatilityType ? *targetVolatilityType : type,
                        targetDisplacement ? *targetDisplacement : displacement),
      volQuotes_(nOptionletTenors_, std::vector<shared_ptr<SimpleQuote> >(nStrikes_)),
      floatingSwitchStrike_(switchStrike == Null<Rate>() ? true : false), capFlooMatrixNotInitialized_(true),
      switchStrike_(switchStrike), accuracy_(accuracy), maxIter_(maxIter), dontThrow_(dontThrow),
      dontThrowMinVol_(dontThrowMinVol), inputVolatilityType_(type), inputDisplacement_(displacement) {

    capFloorPrices_ = Matrix(nOptionletTenors_, nStrikes_);
    optionletPrices_ = Matrix(nOptionletTenors_, nStrikes_);
    capletVols_ = Matrix(nOptionletTenors_, nStrikes_);
    capFloorVols_ = Matrix(nOptionletTenors_, nStrikes_);

    Real firstGuess = 0.14; // guess is only used for shifted lognormal vols
    optionletStDevs_ = Matrix(nOptionletTenors_, nStrikes_, firstGuess);

    capFloors_ = CapFloorMatrix(nOptionletTenors_);
    capFloorEngines_ = std::vector<std::vector<boost::shared_ptr<PricingEngine> > >(nOptionletTenors_);
}

void OptionletStripper1::performCalculations() const {

    // update dates
    const Date& referenceDate = termVolSurface_->referenceDate();
    const DayCounter& dc = termVolSurface_->dayCounter();
    shared_ptr<BlackCapFloorEngine> dummy(new BlackCapFloorEngine( // discounting does not matter here
        iborIndex_->forwardingTermStructure(), 0.20, dc));
    for (Size i = 0; i < nOptionletTenors_; ++i) {
        CapFloor temp = MakeCapFloor(CapFloor::Cap, capFloorLengths_[i], iborIndex_,
                                     0.04, // dummy strike
                                     0 * Days)
                            .withPricingEngine(dummy);
        shared_ptr<FloatingRateCoupon> lFRC = temp.lastFloatingRateCoupon();
        optionletDates_[i] = lFRC->fixingDate();
        optionletPaymentDates_[i] = lFRC->date();
        optionletAccrualPeriods_[i] = lFRC->accrualPeriod();
        optionletTimes_[i] = dc.yearFraction(referenceDate, optionletDates_[i]);
        atmOptionletRate_[i] = lFRC->indexFixing();
    }

    if (floatingSwitchStrike_) {
        Rate averageAtmOptionletRate = 0.0;
        for (Size i = 0; i < nOptionletTenors_; ++i) {
            averageAtmOptionletRate += atmOptionletRate_[i];
        }
        switchStrike_ = averageAtmOptionletRate / nOptionletTenors_;
    }

    const Handle<YieldTermStructure>& discountCurve =
        discount_.empty() ? iborIndex_->forwardingTermStructure() : discount_;

    const std::vector<Rate>& strikes = termVolSurface_->strikes();
    // initialize CapFloorMatrix
    if (capFlooMatrixNotInitialized_) {
        for (Size i = 0; i < nOptionletTenors_; ++i) {
            capFloors_[i].resize(nStrikes_);
            capFloorEngines_[i].resize(nStrikes_);
        }
        // construction might go here
        for (Size j = 0; j < nStrikes_; ++j) {
            for (Size i = 0; i < nOptionletTenors_; ++i) {
                volQuotes_[i][j] = shared_ptr<SimpleQuote>(new SimpleQuote());
                if (inputVolatilityType_ == ShiftedLognormal) {
                    capFloorEngines_[i][j] = boost::make_shared<BlackCapFloorEngine>(
                        discountCurve, Handle<Quote>(volQuotes_[i][j]), dc, inputDisplacement_);
                } else if (inputVolatilityType_ == Normal) {
                    capFloorEngines_[i][j] =
                        boost::make_shared<BachelierCapFloorEngine>(discountCurve, Handle<Quote>(volQuotes_[i][j]), dc);
                } else {
                    QL_FAIL("unknown volatility type: " << volatilityType_);
                }
            }
        }
        capFlooMatrixNotInitialized_ = false;
    }

    for (Size j = 0; j < nStrikes_; ++j) {
        // using out-of-the-money options
        CapFloor::Type capFloorType = strikes[j] < switchStrike_ ? CapFloor::Floor : CapFloor::Cap;
        Option::Type optionletType = strikes[j] < switchStrike_ ? Option::Put : Option::Call;

        Real previousCapFloorPrice = 0.0;
        for (Size i = 0; i < nOptionletTenors_; ++i) {

            capFloorVols_[i][j] = termVolSurface_->volatility(capFloorLengths_[i], strikes[j], true);
            volQuotes_[i][j]->setValue(capFloorVols_[i][j]);
            capFloors_[i][j] = MakeCapFloor(capFloorType, capFloorLengths_[i], iborIndex_, strikes[j], -0 * Days)
                                   .withPricingEngine(capFloorEngines_[i][j]);
            capFloorPrices_[i][j] = capFloors_[i][j]->NPV();
            optionletPrices_[i][j] = capFloorPrices_[i][j] - previousCapFloorPrice;
            previousCapFloorPrice = capFloorPrices_[i][j];
            DiscountFactor d = discountCurve->discount(optionletPaymentDates_[i]);
            DiscountFactor optionletAnnuity = optionletAccrualPeriods_[i] * d;
            try {
                if (volatilityType_ == ShiftedLognormal) {
                    optionletStDevs_[i][j] = blackFormulaImpliedStdDev(
                        optionletType, strikes[j], atmOptionletRate_[i], optionletPrices_[i][j], optionletAnnuity,
                        displacement_, optionletStDevs_[i][j], accuracy_, maxIter_);
                } else if (volatilityType_ == Normal) {
                    optionletStDevs_[i][j] =
                        std::sqrt(optionletTimes_[i]) *
                        bachelierBlackFormulaImpliedVol(optionletType, strikes[j], atmOptionletRate_[i],
                                                        optionletTimes_[i], optionletPrices_[i][j], optionletAnnuity);
                } else {
                    QL_FAIL("Unknown target volatility type: " << volatilityType_);
                }
            } catch (std::exception& e) {
                if (dontThrow_)
                    optionletStDevs_[i][j] = dontThrowMinVol_; // really need a way to log this
                else
                    QL_FAIL("could not bootstrap optionlet:"
                            "\n type:    "
                            << optionletType << "\n strike:  " << io::rate(strikes[j])
                            << "\n atm:     " << io::rate(atmOptionletRate_[i])
                            << "\n price:   " << optionletPrices_[i][j] << "\n annuity: " << optionletAnnuity
                            << "\n expiry:  " << optionletDates_[i] << "\n error:   " << e.what());
            }
            optionletVolatilities_[i][j] = optionletStDevs_[i][j] / std::sqrt(optionletTimes_[i]);
        }
    }
}

const Matrix& OptionletStripper1::capletVols() const {
    calculate();
    return capletVols_;
}

const Matrix& OptionletStripper1::capFloorPrices() const {
    calculate();
    return capFloorPrices_;
}

const Matrix& OptionletStripper1::capFloorVolatilities() const {
    calculate();
    return capFloorVols_;
}

const Matrix& OptionletStripper1::optionletPrices() const {
    calculate();
    return optionletPrices_;
}

Rate OptionletStripper1::switchStrike() const {
    if (floatingSwitchStrike_)
        calculate();
    return switchStrike_;
}
} // namespace QuantExt


#include "opt.hh"
#include "io.hh"
#include "key.hh"
#include "random.hh"

#include <boost/numeric/odeint.hpp>

namespace boost {
  //
  namespace numeric {
    //
    namespace odeint {
      //
      template<>
      struct is_resizeable<Coord::Cartesian> {
	//
	typedef boost::true_type type;

	static const bool value = type::value;
      };
    }
  }
}

/**********************************************************************************************************
 *************************************** HARDING POTENTIAL WRAPPER ****************************************
 **********************************************************************************************************/

double Opt::HardPot::evaluate (const Cartesian& x) const
{
  const char funame [] = "Opt::HardPot::evaluate: ";

  if(atom_size() != x.atom_size()) {
    //
    std::cerr << funame << "dimensions mismatch: " << atom_size() << " vs. " << x.atom_size() << "\n";

    throw Error::Logic();
  }

  return potential(x);
}

/**********************************************************************************************************
 ***************************** CONSTRAINED OPTIMIZATION IN CARTESIAN SPACE ********************************
 **********************************************************************************************************/

int Opt::CcOpt::_grad_count;

void Opt::CcOpt::execute (Coord::Cartesian& x) const
{
  const char funame [] = "Opt::CcOpt::execute: ";

  namespace odeint = boost::numeric::odeint;

  double dtemp;
  int    itemp;

  IO::Marker fumarker(funame);

  _grad_count = 0;

  Cartesian grad(x.size());

  (*this)(x, grad);

  dtemp = vlength(grad);

  IO::log << IO::log_offset << "initial gradient projection length = " << dtemp << "\n";

  if(dtemp <= _grad_tol) {
    //
    IO::log << IO::log_offset << "gradient projection is small enough: no optimization necessary\n";

    return;
  }

  if(_constrain.size() && use_constrain) {
    //
    IO::log << IO::log_offset << "initial constrain values:";

    for(_con_t::const_iterator cit = _constrain.begin(); cit != _constrain.end(); ++cit) {
      //
      dtemp = (*cit)->evaluate(x);

      switch((*cit)->type()) {
	//
      case Coord::DISTANCE:
	//
	dtemp /= Phys_const::angstrom;

	break;

      case Coord::ANGLE:
	//
	dtemp *= 180. / M_PI;

	break;

      case Coord::DIHEDRAL:
	//
	dtemp *= 180. / M_PI;

	break;
      }

      IO::log << "   " << dtemp;
    }

    IO::log << std::endl;
  }

  try {
    //
    double time = 0.;

    while(1) {
      //
      IO::log << IO::log_offset
	      << std::setw(13) << "time"
	      << std::setw(13) << "gradient"
	      << std::setw(13) << "target"
	      << "\n";

      double tmax = time + _time_length;

      itemp = odeint::integrate(*this, x, time, tmax, _time_step);

      time = tmax;

      IO::log << IO::log_offset << "steps # = " << itemp << std::endl;
    }
  } catch(_Fin) {
    //
    (*this)(x, grad);

    dtemp = vlength(grad);

    IO::log << IO::log_offset << "final gradient projection length  = " << std::setw(13) << dtemp  << "\n";

    IO::log << IO::log_offset << "gradient calls #       = " << std::setw(13) << _grad_count << "\n";

    if(_constrain.size() && use_constrain) {
      //
      IO::log << IO::log_offset << "final constrain values:";

      for(_con_t::const_iterator cit = _constrain.begin(); cit != _constrain.end(); ++cit) {
	//
	dtemp = (*cit)->evaluate(x);

	switch((*cit)->type()) {
	  //
	case Coord::DISTANCE:
	  //
	  dtemp /= Phys_const::angstrom;

	  break;

	case Coord::ANGLE:
	  //
	  dtemp *= 180. / M_PI;

	  break;

	case Coord::DIHEDRAL:
	  //
	  dtemp *= 180. / M_PI;

	  break;
	}

	IO::log << "   " << dtemp;
      }

      IO::log << std::endl;
    }
  }
}

void Opt::CcOpt::operator() (const Cartesian& x, Cartesian& dx, double time) const
{
  const char funame [] = "Opt::CcOpt::operator(): ";

  int    itemp;
  double dtemp;

  ++_grad_count;

  if(!x.size()) {
    //
    std::cerr << funame << "zero configuration space vector dimension\n";

    throw Error::Init();
  }

  dx.resize(x.size());

  for(int i = 0; i < dx.size(); ++i)
    //
    dx[i] = -pot()->grad(x, i);

  if(use_constrain && _constrain.size()) {
    //
    // constrain subspace
    //
    std::vector<Cartesian> cspace(_constrain.size(), x);

    int cc = 0;

    for(_con_t::const_iterator cit = _constrain.begin(); cit != _constrain.end(); ++cit, ++cc) {
      //
      for(int i = 0; i < x.size(); ++i)
	//
	cspace[cc][i] = (*cit)->grad(x, i);

      dtemp = normalize(cspace[cc]);

      if(dtemp == 0.) {
	//
	std::cerr << funame << "zero constrain vector\n";

	throw Error::Range();
      }

      for(int d = 0; d < cc; ++d)
	//
	orthogonalize(cspace[cc], cspace[d]);

      if(cc) {
	//
	dtemp = normalize(cspace[cc]);

	if(dtemp < _ci_tol) {
	  //
	  std::cerr << funame << "constrains are linearly dependent: " << dtemp << " vs " << _ci_tol << "\n";

	  throw Error::Run();
	}
      }
    }

    // potential gradient component, orthogonal to constrain subspace
    //
    for(cc = 0; cc < cspace.size(); ++cc)
      //
      orthogonalize(dx, cspace[cc]);
  }

  if(time != 0.) {
    //
    dtemp = vlength(dx);

    IO::log << IO::log_offset
	    << std::setw(13) << time
	    << std::setw(13) << dtemp
	    << std::setw(13) << _grad_tol
	    << std::endl;

    if(dtemp < _grad_tol)
      //
      throw _Fin();
  }
}

void Opt::CcOpt::set (std::istream& from)
{
  const char funame [] = "Opt::CcOpt::set:";

    int    itemp;
    double dtemp;

  IO::Marker funame_marker(funame);

  KeyGroup CcOptGroup;

  Key  tol_key("GradientTolerance[a.u.]"       );
  Key  cit_key("ConstrainIndependenceTolerance");
  Key step_key("IntegrationStep"               );
  Key time_key("IntegrationTime"               );

  std::string token, comment, stemp;

  while(from >> token) {
    //
    // end of input
    //
    if(token == IO::end_key()) {
      //
      std::getline(from, comment);
      //
      break;
    }
    // gradient tolerance
    //
    else if(token == tol_key) {
      //
      if(!(from >> _grad_tol)) {
	//
	std::cerr << funame << token << ": corrupted\n";

	throw Error::Input();
      }

      if(_grad_tol <= 0.) {
	//
	std::cerr << funame << token << ": out of range: " << _grad_tol << "\n";

	throw Error::Range();
      }
    }
    // constrain (local) independence tolerance
    //
    else if(token == cit_key) {
      //
      if(!(from >> _ci_tol)) {
	//
	std::cerr << funame << token << ": corrupted\n";

	throw Error::Input();
      }

      if(_ci_tol <= 0.) {
	//
	std::cerr << funame << token << ": out of range: " << _ci_tol << "\n";

	throw Error::Range();
      }
    }
    // initial time step
    //
    else if(token == step_key) {
      //
      if(!(from >> _time_step)) {
	//
	std::cerr << funame << token << ": corrupted\n";

	throw Error::Input();
      }

      if(_time_step <= 0.) {
	//
	std::cerr << funame << token << ": out of range: " << _time_step << "\n";

	throw Error::Range();
      }
    }
    // integration time
    //
    else if(token == time_key) {
      //
      if(!(from >> _time_length)) {
	//
	std::cerr << funame << token << ": corrupted\n";

	throw Error::Input();
      }

      if(_time_length <= 0.) {
	//
	std::cerr << funame << token << ": out of range: " << _time_length << "\n";

	throw Error::Range();
      }
    }
    // unknown keyword
    //
    else if(IO::skip_comment(token, from)) {
      //
      std::cerr << funame << "unknown keyword " << token << "\n";

      Key::show_all(std::cerr);

      std::cerr << "\n";

      throw Error::Init();
    }
  }

  if(!from) {
    //
    std::cerr << funame << "corrupted\n";

    throw Error::Input();
  }
}

/********************************************************************************************************
 *********************** OPTIMIZATION AND IMPORTANCE SAMPLING IN Z-MATRIX COORDINATES********************
 ********************************************************************************************************/

// gradient tolerance
//
double Opt::ZOpt::grad_tol = 1.e-5;

// maximal optimization step
//
double Opt::ZOpt::max_opt_step = 0.1;

// maximal number of optimization iterations
//
int Opt::ZOpt::max_opt_count = 100;

// differentiation step
//
double Opt::ZOpt::diff_step = .001;

// potential
//
ConstSharedPointer<Coord::CartFun> Opt::ZOpt::pot;

double Opt::ZOpt::_con_pot (const std::vector<double>& cpos) const
{
  const char funame [] = "Opt::ZOpt::_con_pot: ";

  int    itemp;
  double dtemp;

  if(!pot) {
    //
    std::cerr << funame << "potential not initialized\n";

    throw Error::Init();
  }

  if(cpos.size() != _con_modes.size()) {
    //
    std::cerr << funame << "number of conserved modes mismatch: " << cpos.size() << " vs " << _con_modes.size() << "\n";

    throw Error::Logic();
  }

  ZData zpos = _zmin;

  itemp = 0;

  for(mode_t::const_iterator cit = _con_modes.begin(); cit != _con_modes.end(); ++cit, ++itemp)
    //
    zpos[cit->first] = cpos[itemp];

  return pot->evaluate((Cartesian)zpos);
}

Lapack::Vector Opt::ZOpt::_con_grad () const
{
  int    itemp;
  double dtemp;

  std::vector<double> cpos(_con_modes.size());

  itemp = 0;

  for(mode_t::const_iterator cit = _con_modes.begin(); cit != _con_modes.end(); ++cit, ++itemp)
    //
    cpos[itemp] = _zmin[cit->first];

  Lapack::Vector res(_con_modes.size());

  for(int i  = 0; i < _con_modes.size(); ++i) {
    //
    cpos[i] += diff_step;

    res[i] = _con_pot(cpos);

    cpos[i] -= 2. * diff_step;

    res[i] -= _con_pot(cpos);

    cpos[i] += diff_step;

    res[i] /= 2. * diff_step;
  }

  return res;
}

Lapack::SymmetricMatrix Opt::ZOpt::_con_hess () const
{
  int    itemp;
  double dtemp;

  std::vector<double> cpos(_con_modes.size());

  itemp = 0;

  for(mode_t::const_iterator cit = _con_modes.begin(); cit != _con_modes.end(); ++cit, ++itemp)
    //
    cpos[itemp] = _zmin[cit->first];

  const double e0 = _con_pot(cpos);

  Lapack::SymmetricMatrix res(_con_modes.size());

  for(int i  = 0; i < _con_modes.size(); ++i)
    //
    for(int j  = i; j < _con_modes.size(); ++j)
      //
      if(i != j) {
	//
	cpos[i] += diff_step;

	cpos[j] += diff_step;

	res(i, j) = _con_pot(cpos);

	cpos[i] -= 2. * diff_step;

	res(i, j) -= _con_pot(cpos);

	cpos[j] -= 2. * diff_step;

	res(i, j) += _con_pot(cpos);

	cpos[i] += 2. * diff_step;

	res(i, j) -= _con_pot(cpos);

	cpos[i] -= diff_step;

	cpos[j] += diff_step;

	res(i, j) /= 4. * diff_step * diff_step;
      }
      else {
	//
	cpos[i] += diff_step;

	res(i, i) = _con_pot(cpos) - 2. * e0;

	cpos[i] -= 2. * diff_step;

	res(i, i) += _con_pot(cpos);

	cpos[i] += diff_step;

	res(i, i) /= diff_step * diff_step;
      }

  return res;
}

Opt::ZOpt::ZOpt (const ZData& zinit, const mode_t& cm) : _zmin(zinit), _con_modes(cm)
{
  const char funame [] = "Opt::ZOpt::ZOpt: ";

  IO::Marker fumark(funame);

  double dtemp;
  int    itemp;

  int count = 0;

  IO::log << IO::log_offset << std::setw(5) << "#" << std::setw(13) << "Grad[au]" << std::setw(13) << "Target" << std::endl;

  while(count++ < max_opt_count) {
    //
    _fc_eval_sqrt = _con_hess().eigenvalues(&_fc_evec);

    Lapack::Vector dx = _con_grad();

    dtemp = vlength(dx);

    IO::log << IO::log_offset << std::setw(5) << count << std::setw(13) << dtemp << std::setw(13) << grad_tol << std::endl;

    if(dtemp < grad_tol)
      //
      break;

    dx = dx * _fc_evec;

    for(int i = 0; i < dx.size(); ++i) {
      //
      if(_fc_eval_sqrt[i] <= 0.) {
	//
	IO::log << IO::log_offset << i + 1 << "-th force constant eigenvalue is negative" << std::endl;

	if(dx[i] < 0.) {
	  //
	  dx[i] = -max_opt_step;
	}
	else if(dx[i] > 0.) {
	  //
	  dx[i] = max_opt_step;
	}
      }
      else {
	//
	dtemp = dx[i] / _fc_eval_sqrt[i];

	if(dtemp < -max_opt_step) {
	  //
	  dx[i] = -max_opt_step;
	}
	else if(dtemp > max_opt_step) {
	  //
	  dx[i] = max_opt_step;
	}
	else
	  //
	  dx[i] = dtemp;
      }
    }

    dx = _fc_evec * dx;

    itemp = 0;

    for(mode_t::const_iterator cit = _con_modes.begin(); cit != _con_modes.end(); ++cit, ++itemp) {
      //
      dtemp = _zmin[cit->first] - dx[itemp];

      if(dtemp < cit->second.first) {
	//
	IO::log << IO::log_offset << cit->first << " proposed value is below lower limit, " << dtemp << ": clip it" << std::endl;

	_zmin[cit->first] = cit->second.first;
      }
      else if(dtemp > cit->second.second) {
	//
	IO::log << IO::log_offset << cit->first << " proposed value is above upper limit, " << dtemp << ": clip it" << std::endl;

	_zmin[cit->first] = cit->second.second;
      }
      else
	//
	_zmin[cit->first] = dtemp;
    }
  }

  if(count > max_opt_count) {
    //
    IO::log << IO::log_offset << "maximal number of iterations reached: no convergence" << std::endl;

    std::cerr << funame << "maximal number of iterations reached: no convergence\n";

    throw NoConv();
  }

  for(int i = 0; i < _con_modes.size(); ++i) {
    //
    dtemp = _fc_eval_sqrt[i];

    if(dtemp <= 0.) {
      //
      std::cerr << funame << "force constant matrix at minimum is not positively defined\n";

      throw Error::Run();
    }

    _fc_eval_sqrt[i] = std::sqrt(dtemp);
  }

  _mass_factor = std::sqrt(product(_zmin.inertia_moments())) / Lapack::Cholesky(_zmin.mobility_matrix()).det_sqrt();

  _ener_min = pot->evaluate((Cartesian)_zmin);
}

double Opt::ZOpt::anharmonic_correction (double temperature, int count_max, double* rerr, int* fail) const
{
  const char funame [] = "Opt::ZOpt::anharmonic_correction: ";

  static const double exp_pow_max = 100.;

  double dtemp;
  int    itemp;
  bool   btemp;

  const double tsqrt = std::sqrt(temperature);

  Lapack::Vector vtemp(_con_modes.size());

  double res = 0., var =  0.;

  int skip = 0;

  // sampling cycle
  //
  for(int count = 0; count < count_max; ++count) {
    //
    double eref = 0.;

    for(int i = 0; i < _con_modes.size(); ++i) {
      //
      dtemp = Random::norm();

      eref += dtemp * dtemp;

      vtemp[i] = dtemp / _fc_eval_sqrt[i] * tsqrt;
    }

    eref *= temperature / 2.;

    vtemp = _fc_evec * vtemp;

    ZData ztemp = _zmin;

    itemp = 0;

    btemp = false;

    for(mode_t::const_iterator cit = _con_modes.begin(); cit != _con_modes.end(); ++cit, ++itemp) {
      //
      dtemp = ztemp[cit->first] + vtemp[itemp];

      // check that the non-fluxional coordinate is in the allowed window
      //
      if(dtemp < cit->second.first || dtemp > cit->second.second) {
	//
	switch(ztemp.type(cit->first)) {
	  //
	case DISTANCE:
	  //
	  dtemp /= Phys_const::angstrom;

	  break;

	default:
	  //
	  dtemp *= 180. / M_PI;
	}

	IO::log << funame << "WARNING: T = " << temperature / Phys_const::kelv
		<< "K, z-matrix variable " << cit->first << " out of limits: " << dtemp << "\n";

	++skip;

	btemp = true;

	break;
      }

      ztemp[cit->first] = dtemp;
    }

    if(btemp)
      //
      continue;

    dtemp = (pot->evaluate((Cartesian)ztemp) - eref - _ener_min) / temperature;

    if(dtemp > exp_pow_max)
      //
      continue;

    if(dtemp < -exp_pow_max) {
      //
      IO::log << funame << "WARNING: anharmonic correction too negative at T = " << temperature / Phys_const::kelv << "K: "
	      << dtemp * temperature / Phys_const::kcal << " kcal/mol: skipping the point" << std::endl;

      ++skip;

      continue;
    }

    dtemp = std::exp(-dtemp) / Lapack::Cholesky(ztemp.mobility_matrix()).det_sqrt()
      * std::sqrt(product(ztemp.inertia_moments())) / mass_factor();

    res += dtemp;

    var += dtemp * dtemp;
    //
  }// sampling cycle
  //

  res /= (double)count_max;

  var /= (double)count_max;

  // relative error
  //
  if(rerr)
    //
    *rerr = std::sqrt(var - res * res) / res / std::sqrt((double)count_max);

  // failed points #
  //
  if(fail)
    //
    *fail = skip;

  return res;
}


#include <NTL/ZZ_p.h>
#include <NTL/FFT.h>

#include <NTL/new.h>

NTL_START_IMPL

ZZ_pInfoT::ZZ_pInfoT(const ZZ& NewP)
{
   if (NewP <= 1) Error("ZZ_pContext: p must be > 1");

   ref_count = 1;
   p = NewP;
   size = p.size();

   ExtendedModulusSize = 2*size +
                 (NTL_BITS_PER_LONG + NTL_ZZ_NBITS - 1)/NTL_ZZ_NBITS;

   initialized = 0;
   x = 0;
   u = 0;
   tbl = 0;
   tbl1 = 0;

   long i;
   for (i = 0; i < MAX_ZZ_p_TEMPS; i++)
      temps[i] = 0;

   temps_top = 0;
}

void ZZ_pInfoT::init()
{
   ZZ B, M, M1, M2, M3;
   long n, i;
   long q, t;

   initialized = 1;

   sqr(B, p);

   LeftShift(B, B, NTL_FFTMaxRoot+NTL_FFTFudge);

   set(M);
   n = 0;
   while (M <= B) {
      UseFFTPrime(n);
      q = FFTPrime[n];
      n++;
      mul(M, M, q);
   }

   NumPrimes = n;
   MaxRoot = CalcMaxRoot(q);

   double fn = double(n);

   if (8.0*fn*(fn+32) > NTL_FDOUBLE_PRECISION)
      Error("modulus too big");

   if (8.0*fn*(fn+32) > NTL_FDOUBLE_PRECISION/double(NTL_SP_BOUND))
      QuickCRT = 0;
   else
      QuickCRT = 1;

   if (!(x = (double *) NTL_MALLOC(n, sizeof(double), 0)))
      Error("out of space");

   if (!(u = (long *) NTL_MALLOC(n,  sizeof(long), 0)))
      Error("out of space");

   ZZ_p_rem_struct_init(&rem_struct, n, p, FFTPrime);

   ZZ_p_crt_struct_init(&crt_struct, n, p, FFTPrime);

   if (ZZ_p_crt_struct_special(crt_struct)) return;

   ZZ qq, rr;

   DivRem(qq, rr, M, p);

   NegateMod(MinusMModP, rr, p);

   for (i = 0; i < n; i++) {
      q = FFTPrime[i];

      long tt = rem(qq, q);

      mul(M2, p, tt);
      add(M2, M2, rr);
      div(M2, M2, q);  // = (M/q) rem p

      div(M1, M, q);
      t = rem(M1, q);
      t = InvMod(t, q);

      mul(M3, M2, t);
      rem(M3, M3, p);

      ZZ_p_crt_struct_insert(crt_struct, i, M3);

      x[i] = ((double) t)/((double) q);
      u[i] = t;
   }
}

ZZ_pInfoT::~ZZ_pInfoT()
{
   long i;

   for (i = 0; i < MAX_ZZ_p_TEMPS; i++)
      if (temps[i]) delete temps[i];

   if (initialized) {
      ZZ_p_rem_struct_free(rem_struct);
      ZZ_p_crt_struct_free(crt_struct);

      free(x);
      free(u);
   }
}

ZZ_pInfoT *ZZ_pInfo = 0;

typedef ZZ_pInfoT *ZZ_pInfoPtr;

static
void CopyPointer(ZZ_pInfoPtr& dst, ZZ_pInfoPtr src)
{
   if (src == dst) return;

   if (dst) {
      dst->ref_count--;

      if (dst->ref_count < 0)
         Error("internal error: negative ZZ_pContext ref_count");

      if (dst->ref_count == 0) delete dst;
   }

   if (src) {
      if (src->ref_count == NTL_MAX_LONG)
         Error("internal error: ZZ_pContext ref_count overflow");

      src->ref_count++;
   }

   dst = src;
}

void ZZ_p::init(const ZZ& p)
{
   ZZ_pContext c(p);
   c.restore();
}

ZZ_pContext::ZZ_pContext(const ZZ& p)
{
   ptr = NTL_NEW_OP ZZ_pInfoT(p);
}

ZZ_pContext::ZZ_pContext(const ZZ_pContext& a)
{
   ptr = 0;
   CopyPointer(ptr, a.ptr);
}

ZZ_pContext& ZZ_pContext::operator=(const ZZ_pContext& a)
{
   CopyPointer(ptr, a.ptr);
   return *this;
}

ZZ_pContext::~ZZ_pContext()
{
   CopyPointer(ptr, 0);
}

void ZZ_pContext::save()
{
   CopyPointer(ptr, ZZ_pInfo);
}

void ZZ_pContext::restore() const
{
   CopyPointer(ZZ_pInfo, ptr);
}

ZZ_pBak::~ZZ_pBak()
{
   if (MustRestore)
      CopyPointer(ZZ_pInfo, ptr);

   CopyPointer(ptr, 0);
}

void ZZ_pBak::save()
{
   MustRestore = 1;
   CopyPointer(ptr, ZZ_pInfo);
}

void ZZ_pBak::restore()
{
   MustRestore = 0;
   CopyPointer(ZZ_pInfo, ptr);
}

ZZ_pTemp::ZZ_pTemp()
{
   if (ZZ_pInfo->temps_top == MAX_ZZ_p_TEMPS)
      Error("ZZ_p temporary: out of temps");

   pos = ZZ_pInfo->temps_top;
   ZZ_pInfo->temps_top++;
}

ZZ_pTemp::~ZZ_pTemp()
{
   ZZ_pInfo->temps_top--;
}

ZZ_p& ZZ_pTemp::val() const
{
   if (!ZZ_pInfo->temps[pos])
      ZZ_pInfo->temps[pos] = NTL_NEW_OP ZZ_p;

   return *(ZZ_pInfo->temps[pos]);
}

const ZZ_p& ZZ_p::zero()
{
   static ZZ_p z(ZZ_p_NoAlloc);
   return z;
}

ZZ_p::DivHandlerPtr ZZ_p::DivHandler = 0;

ZZ_p::ZZ_p()
{
   _ZZ_p__rep.SetSize(ModulusSize());
}

ZZ_p::ZZ_p(INIT_VAL_TYPE, const ZZ& a)
{
   _ZZ_p__rep.SetSize(ModulusSize());
   conv(*this, a);
}

ZZ_p::ZZ_p(INIT_VAL_TYPE, long a)
{
   _ZZ_p__rep.SetSize(ModulusSize());
   conv(*this, a);
}

void conv(ZZ_p& x, long a)
{
   if (a == 0)
      clear(x);
   else if (a == 1)
      set(x);
   else {
      static ZZ y;

      conv(y, a);
      conv(x, y);
   }
}

istream& operator>>(istream& s, ZZ_p& x)
{
   static ZZ y;

   s >> y;
   conv(x, y);

   return s;
}

void div(ZZ_p& x, const ZZ_p& a, const ZZ_p& b)
{
   ZZ_pTemp TT; ZZ_p& T = TT.val();

   inv(T, b);
   mul(x, a, T);
}

void inv(ZZ_p& x, const ZZ_p& a)
{
   if (InvModStatus(x._ZZ_p__rep, a._ZZ_p__rep, ZZ_p::modulus())) {
      if (IsZero(a._ZZ_p__rep))
         Error("ZZ_p: division by zero");
      else if (ZZ_p::DivHandler)
         (*ZZ_p::DivHandler)(a);
      else
         Error("ZZ_p: division by non-invertible element");
   }
}

long operator==(const ZZ_p& a, long b)
{
   if (b == 0)
      return IsZero(a);

   if (b == 1)
      return IsOne(a);

   ZZ_pTemp TT; ZZ_p& T = TT.val();
   conv(T, b);
   return a == T;
}

void add(ZZ_p& x, const ZZ_p& a, long b)
{
   ZZ_pTemp TT; ZZ_p& T = TT.val();
   conv(T, b);
   add(x, a, T);
}

void sub(ZZ_p& x, const ZZ_p& a, long b)
{
   ZZ_pTemp TT; ZZ_p& T = TT.val();
   conv(T, b);
   sub(x, a, T);
}

void sub(ZZ_p& x, long a, const ZZ_p& b)
{
   ZZ_pTemp TT; ZZ_p& T = TT.val();
   conv(T, a);
   sub(x, T, b);
}

void mul(ZZ_p& x, const ZZ_p& a, long b)
{
   ZZ_pTemp TT; ZZ_p& T = TT.val();
   conv(T, b);
   mul(x, a, T);
}

void div(ZZ_p& x, const ZZ_p& a, long b)
{
   ZZ_pTemp TT; ZZ_p& T = TT.val();
   conv(T, b);
   div(x, a, T);
}

void div(ZZ_p& x, long a, const ZZ_p& b)
{
   if (a == 1) {
      inv(x, b);
   }
   else {
      ZZ_pTemp TT; ZZ_p& T = TT.val();
      conv(T, a);
      div(x, T, b);
   }
}

NTL_END_IMPL


/* Copyright (c) 2020 C. Pattison
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once
#include <cassert>
#include <algorithm>
#include <utility>
#include <complex>
#include <cstdint>
#include <exception>

#include <Eigen/Dense>
#include "syk_types.hpp"
#include "util.hpp"

/** Utilities for working with a Hamiltonian as a sum of Pauli operators
 */
namespace syk {
using namespace std::complex_literals;

/** Pauli group
 */

enum class AbstractPauli : short {I = 0, X = 1, Y = 2, Z = 3};
enum class Sign : short {P = 1, M = -1};
struct Pauli { AbstractPauli m; Sign s; };

Sign operator*(Sign a, Sign b) {
    return (a == b) ? Sign::P : Sign::M;
}

Sign operator!(Sign a) {
    return a * Sign::M;
}

Pauli operator*(Pauli a, Pauli b) {
    auto new_sign = a.s * b.s;
    if( !(static_cast<int>(a.m) <= static_cast<int>(b.m)) ) {
        std::swap(a, b);
        new_sign = (
                a.m == AbstractPauli::I
            ||  b.m == AbstractPauli::I
            ||  a.m == b.m) ? new_sign : !new_sign;
    }

    if (a.m == AbstractPauli::I) { return Pauli {b.m, new_sign}; }
    if (a.m == b.m) { return Pauli {AbstractPauli::I, new_sign}; }
    return Pauli {
        static_cast<AbstractPauli>(3 - (static_cast<int>(a.m) + static_cast<int>(b.m))%3),
        new_sign};
}

/** Utilities for construction Hamiltonian as a sum of products of Paulis
 * This has a hard coded maximum number of qubits (20) for performance
 * If/When increased it should be a multiple of 4 to allow vectorization
 *
 * Constructing the full Hamiltonian is done coefficient-wise by going through each term in the sum
 * For each term we stop when we hit a 0
 * This is branch heavy but was about 2x faster than without the branch
 */
struct PauliRep {
    static constexpr int kmax_qubits = 20;
    using PauliString = std::array<Pauli, kmax_qubits>;
    using PauliMatrix = Eigen::Matrix<std::complex<short>, 2, 2>;

    struct Term {
        PauliString pauli_op;
        double weight;
    };

protected:
    int num_qubits_;

public:
    PauliMatrix m_[4];

    PauliRep(int num_qubits) : num_qubits_(num_qubits) {
        if(num_qubits > kmax_qubits) { throw std::runtime_error("Number of qubits exceeds maximum support. Increase the compile time value"); }

        m_[0] << 1, 0,
                0, 1;

        m_[1] << 0, 1,
                1, 0;

        m_[2] << 0, -1i,
                1i, 0;

        m_[3] << 1, 0,
                0, -1;
    }

    /** Get sign
     */
    short repp(Sign a) const __attribute__((always_inline)) {
        return static_cast<short>(a);
    }

    /** Get matrix for a one qubit Pauli
     */
    const PauliMatrix& repp(AbstractPauli a) const __attribute__((always_inline)) {
        return m_[static_cast<short>(a)];
    }

    auto get_repp_factor(Pauli a) const __attribute__((always_inline)) {
        return repp(a.m) * repp(a.s);
    }

    Pauli PauliI() const { return Pauli {AbstractPauli::I, Sign::P}; }
    Pauli PauliX() const { return Pauli {AbstractPauli::X, Sign::P}; }
    Pauli PauliY() const { return Pauli {AbstractPauli::Y, Sign::P}; }
    Pauli PauliZ() const { return Pauli {AbstractPauli::Z, Sign::P}; }

    /** Single Pauli matrix element
     */
    std::complex<short> get_pauli_element(int i, int j, Pauli p) const __attribute__((always_inline)) {
        return repp(p.m)(i,j) * repp(p.s);
    }

    /** Convert a short taking values 0, +/- 1 to double
     */
    std::complex<double> zero_one_short_to_double(std::complex<short> a) const {
        return std::complex<double>(
            a.real() == 0 ? 0.0 : (a.real() > 0 ? 1.0 : -1.0),
            a.imag() == 0 ? 0.0 : (a.imag() > 0 ? 1.0 : -1.0)
        );
    }

    /** Get matrix element of a string of Paulis
     * shorts everywhere to hopefully let the compiler do its thing
     */
    std::complex<double> get_matrix_element(int i, int j, const PauliString& string) {
        std::complex<short> acc(1,0);
        int max_level = num_qubits_ - 1;

        #pragma GCC unroll 4
        for(int level = 0; level <= max_level; ++level) {
            int mask = 1 << level;
            int sub_i = (i & mask) >> level;
            int sub_j = (j & mask) >> level;
            acc *= get_pauli_element(sub_i, sub_j, string[level]);
            if(acc.real() == 0 && acc.imag() == 0) { break; }
        }
        return zero_one_short_to_double(acc);
    }

    /** Compute the Hamiltonian for some sum of terms
     * Give only the cols/rows specified by subspace
     * Subspace is usually 0 to (2^N - 1) but could be something else where symmetries exist
     */
    MatrixType get_hamiltonian(std::vector<Term> terms, std::vector<std::uint64_t> subspace) {
        MatrixType hamiltonian = MatrixType::Zero(subspace.size(), subspace.size());

        #pragma omp parallel for
        for(int j = 0; j < subspace.size(); ++j) {
            for(int i = 0; i < subspace.size(); ++i) {
                double matrix_element_re = 0.0;
                double matrix_element_im = 0.0;
                auto num_terms = terms.size();

                for(int k = 0; k < num_terms; ++k) {
                    auto single_element = get_matrix_element(subspace[i], subspace[j], terms[k].pauli_op);
                    matrix_element_re += terms[k].weight * single_element.real();
                    matrix_element_im += terms[k].weight * single_element.imag();
                }
                hamiltonian(i,j) = std::complex<double>(matrix_element_re, matrix_element_im);
            }
        }

        return hamiltonian;
    }
};
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_POLYNOM_FUNCTIONS_EXPR_POLYVAL_HPP_INCLUDED
#define NT2_POLYNOM_FUNCTIONS_EXPR_POLYVAL_HPP_INCLUDED
#include <nt2/polynom/functions/polyval.hpp>
#include <nt2/include/functions/fma.hpp>
#include <nt2/include/functions/isempty.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/repnum.hpp>
#include <nt2/include/functions/vandermonde.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/sum.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/inf.hpp>
#include <nt2/include/functions/trsolve.hpp>
#include <nt2/polynom/category.hpp>
#include <nt2/sdk/meta/fusion.hpp>
#include <boost/fusion/adapted/array.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/transpose.hpp>
#include <nt2/sdk/error/warning.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/core/container/colon/colon.hpp>

namespace nt2 { namespace ext
{
   BOOST_DISPATCH_IMPLEMENT  ( polyval_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> > )(scalar_<unspecified_<A1> > )
                            )
  {

    typedef A1 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      return a0*a1;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( polyval_, tag::cpu_
                            , (A0)(A1)
                            , (unspecified_<A0>)(scalar_<unspecified_<A1> > )
                            )
  {

    typedef typename A0::value_type value_type;
    typedef A1 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      if (isempty(a0)) return Zero<A1>();
      value_type ans = a0(1);
      for(size_t i = 2; i <= numel(a0); ++i)
      {
//       ans = fma(ans, a1, a0(i));
        ans *= a1;
        ans += a0(i);
      }
      return ans;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( polyval_, tag::cpu_
                              , (A0)(N0)(A1)(N1)
                              , ((node_<A0, nt2::tag::polyval_, N0, nt2::container::domain>))
                              ((node_<A1, nt2::tag::tie_ , N1, nt2::container::domain>))
                            )
  {
    typedef void                                                    result_type;
    typedef typename boost::proto::result_of::child_c<A1&,0>::type       v_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::type       p_type;
    typedef typename boost::proto::result_of::child_c<A0&,1>::type       x_type;
    typedef typename A0::value_type                                  value_type;
    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      // Copy data in output first
      v_type& v = boost::proto::child_c<0>(a1);
      v.resize(a0.extent());
      val(a0, a1, N1(), N0());
    }

  private:
    template < class S,  class T, class U> BOOST_FORCEINLINE
      void compute_val(S p, T x, U v)const
    {
      if (isempty(p))
      {
        v =  nt2::zeros(extent(x), meta::as_<value_type>());
      }
      else
      {
        v = repnum(p(1), size(x));
        for(size_t i = 2; i <= numel(p); ++i)
        {
            //          v = fma(v, x, p(i));
          v*= x;
          v+= p(i);
        }
      }
    }

    template < class S,  class T, class U,  class V> BOOST_FORCEINLINE
      void compute_delta(const S& x, const T& r,
                         const U& df,  const V& normr,
                         const size_t & nc,
                         container::table<value_type>& delta)const
    {
      NT2_WARNING(nt2::is_eqz(value_type(df)), "zero degree of freedom implies infinite error bounds.");
      BOOST_AUTO_TPL(vnd, nt2::vandermonde(x, nc));
      BOOST_AUTO_TPL(err, nt2::trsolve(nt2::trans(r), nt2::trans(vnd),'L') );
      value_type fact =   (normr/nt2::sqrt(value_type(df)));
      delta(nt2::_) =nt2::sqrt(oneplus(sum(sqr(err),1)))*fact;
    }

    template < class T > BOOST_FORCEINLINE
      void val(A0& a0, A1& a1,
               boost::mpl::long_<1> const &, const T&) const
    {
      BOOST_AUTO_TPL(p,  boost::proto::child_c<0>(a0));
      BOOST_AUTO_TPL(x,  boost::proto::child_c<1>(a0));
      BOOST_AUTO_TPL(v,  boost::proto::child_c<0>(a1));
      compute_val(p, x, v);
    }

    BOOST_FORCEINLINE
    void val(A0& a0, A1& a1,
             boost::mpl::long_<1> const &, boost::mpl::long_<3> const &) const
    {
      BOOST_AUTO_TPL(p,  boost::proto::child_c<0>(a0));
      BOOST_AUTO_TPL(x,  boost::proto::child_c<1>(a0));
      BOOST_AUTO_TPL(v,  boost::proto::child_c<0>(a1));
      BOOST_AUTO_TPL(mu,  boost::proto::child_c<2>(a0));
      compute_val(p, (x-mu(1))/mu(2), v);
    }
    BOOST_FORCEINLINE
    void val(A0& a0, A1& a1,
             boost::mpl::long_<1> const &, boost::mpl::long_<6> const &) const
    {
      BOOST_AUTO_TPL(p,  boost::proto::child_c<0>(a0));
      BOOST_AUTO_TPL(x,  boost::proto::child_c<1>(a0));
      BOOST_AUTO_TPL(v,  boost::proto::child_c<0>(a1));
      BOOST_AUTO_TPL(mu,  boost::proto::child_c<5>(a0));
      compute_val(p, (x-mu(1))/mu(2), v);
    }
    BOOST_FORCEINLINE
    void val(A0& a0, A1& a1,
             boost::mpl::long_<2> const &, boost::mpl::long_<5> const &) const
    {
      BOOST_AUTO_TPL(p,  boost::proto::child_c<0>(a0));
      BOOST_AUTO_TPL(x,  boost::proto::child_c<1>(a0));
      BOOST_AUTO_TPL(r,  boost::proto::child_c<2>(a0));
      BOOST_AUTO_TPL(df, boost::proto::child_c<3>(a0));
      BOOST_AUTO_TPL(nr, boost::proto::child_c<4>(a0));
      BOOST_AUTO_TPL(v,  boost::proto::child_c<0>(a1));
      compute_val(p, x, v);
      container::table<value_type> delta(extent(x));
      compute_delta(x, r, df, nr, nt2::numel(p), delta);
      boost::proto::child_c<1>(a1) = delta;
    }
    BOOST_FORCEINLINE
    void val(A0& a0, A1& a1,
             boost::mpl::long_<2> const &, boost::mpl::long_<6> const &) const
    {
      BOOST_AUTO_TPL(p,  boost::proto::child_c<0>(a0));
      BOOST_AUTO_TPL(x,  boost::proto::child_c<1>(a0));
      BOOST_AUTO_TPL(r,  boost::proto::child_c<2>(a0));
      BOOST_AUTO_TPL(df, boost::proto::child_c<3>(a0));
      BOOST_AUTO_TPL(nr, boost::proto::child_c<4>(a0));
      BOOST_AUTO_TPL(v,  boost::proto::child_c<0>(a1));
      BOOST_AUTO_TPL(mu,  boost::proto::child_c<5>(a0));
      BOOST_AUTO_TPL(xred, (x-mu(1))/mu(2));
      compute_val(p, xred, v);
      container::table<value_type> delta(of_size(extent(x)));
      delta.resize(extent(x));
      compute_delta(xred, r, df, nr, nt2::numel(p), delta);
      boost::proto::child_c<1>(a1) = delta;
    }

  };
} }

#endif


#include "sandBox.h"
#include "igl/edge_flaps.h"
#include "igl/collapse_edge.h"
#include "Eigen/dense"
#include <functional>
#include <Eigen/Core>
#include "igl/opengl/ViewerCore.h"
#include "igl/opengl/glfw/renderer.h"
#include "igl/decimate.h"
#include "igl/writeOBJ.h"
#include <igl/circulation.h>
#include <igl/shortest_edge_and_midpoint.h>
#include <igl/parallel_for.h>
#include <igl/opengl/glfw/Viewer.h>
#include <set>
#include "simplifier.h"
#include "igl/swept_volume_bounding_box.h"

SandBox::SandBox()
	:
	objectsData{ new std::vector<ObjectData*>{} },
	linksCounter{ 0 }, destIndex{ 0 },
	firstLinkIndex{ 1 },
	lastLinkIndex{ 1 },
	isCCD{ false },
	xVelocity{-0.005},
	yVelocity{ 0 }
{

}

SandBox::~SandBox()
{
    delete objectsData;
}

void SandBox::OnNewMeshLoad()
{
	if (linksCounter > 0)
	{
		data().MyTranslate(Eigen::Vector3d(0, 1.6, 0), true);
		parents.push_back(linksCounter);
	}
	else
	{
		parents.push_back(-1);
	}

	data().add_points(Eigen::RowVector3d(0, 0, 0), Eigen::RowVector3d(0, 0, 1));
	data().show_overlay_depth = false;
	data().point_size = 10;
	data().line_width = 2;
	data().set_visible(true, 2);
	data().show_faces = 3;
	data().show_texture = 2;

	data().dirty = 157; //this line prevents texture coordinates
	data().MyScale(Eigen::Vector3d(1, 1, 1));
	data().SetCenterOfRotation(Eigen::Vector3d(0, -0.8, 0));

	Eigen::MatrixXd axis(7, 3);
	axis << 0, -0.8, 0, 0, -0.8, 1.6, 0, -0.8, -1.6, 0, 0.8, 0, 0, -2.4, 0, -1.6, -0.8, 0, 1.6, -0.8, 0;
	data().add_points(axis, Eigen::RowVector3d(0, 0, 1));
 	data().add_edges(axis.row(0), axis.row(1), Eigen::RowVector3d(0, 0, 1));
	data().add_edges(axis.row(0), axis.row(2), Eigen::RowVector3d(0, 0, 1));
	data().add_edges(axis.row(0), axis.row(3), Eigen::RowVector3d(0, 1, 0));
	data().add_edges(axis.row(0), axis.row(4), Eigen::RowVector3d(0, 1, 0));
	data().add_edges(axis.row(0), axis.row(5), Eigen::RowVector3d(1, 0, 0));
	data().add_edges(axis.row(0), axis.row(6), Eigen::RowVector3d(1, 0, 0));

	ObjectData* od = new ObjectData();
	InitObjectData(*od, data().V, data().F);
	objectsData->push_back(od);
	data().AddBoundingBox(od->tree->m_box, Eigen::RowVector3d(0, 1, 0));

 	linksCounter++;
	lastLinkIndex = linksCounter;
}

void SandBox::Init(const std::string &config)
{
	std::string item_name;
	std::ifstream nameFileout;
	nameFileout.open(config);

	if (!nameFileout.is_open())
	{
		std::cout << "Can't open file " << config << std::endl;
	}
	else
	{
		while (nameFileout >> item_name)
		{
			std::cout << "openning " << item_name << std::endl;
			load_mesh_from_file(item_name);

			if (item_name.find("sphere") != std::string::npos)
			{
				parents.push_back(-1);
				data().add_points(Eigen::RowVector3d(0, 0, 0), Eigen::RowVector3d(0, 0, 1));
				data().show_overlay_depth = false;
				data().point_size = 10;
				data().line_width = 2;
				data().set_visible(false, 1);
			}
			else
			{
				if (linksCounter > 0)
				{
					data().MyTranslate(Eigen::Vector3d(0, 1.6, 0), true);
					parents.push_back(linksCounter);
				}
				else
				{
					parents.push_back(-1);
				}

				data().add_points(Eigen::RowVector3d(0, 0, 0), Eigen::RowVector3d(0, 0, 1));
				data().show_overlay_depth = false;
				data().point_size = 10;
				data().line_width = 2;
				data().set_visible(false, 1);

				data().dirty = 157; //this line prevents texture coordinates
				data().MyScale(Eigen::Vector3d(1, 1, 1));
				data().SetCenterOfRotation(Eigen::Vector3d(0, -0.8, 0));

				Eigen::MatrixXd axis(7, 3);
				axis <<
					   0, -0.8,    0,
					   0, -0.8,  1.6,
					   0, -0.8,	-1.6,
					   0,  0.8,	   0,
					   0, -2.4,    0,
					-1.6, -0.8,    0,
					 1.6, -0.8,	   0;

				data().add_points(axis, Eigen::RowVector3d(0, 0, 1));
				data().add_edges(axis.row(0), axis.row(1), Eigen::RowVector3d(0, 0, 1));
				data().add_edges(axis.row(0), axis.row(2), Eigen::RowVector3d(0, 0, 1));
				data().add_edges(axis.row(0), axis.row(3), Eigen::RowVector3d(0, 1, 0));
				data().add_edges(axis.row(0), axis.row(4), Eigen::RowVector3d(0, 1, 0));
				data().add_edges(axis.row(0), axis.row(5), Eigen::RowVector3d(1, 0, 0));
				data().add_edges(axis.row(0), axis.row(6), Eigen::RowVector3d(1, 0, 0));

				ObjectData* od = new ObjectData();
				InitObjectData(*od, data().V, data().F);
				objectsData->push_back(od);
				data().AddBoundingBox(od->tree->m_box, Eigen::RowVector3d(0, 1, 0));

				linksCounter++;
			}
		}
		lastLinkIndex = linksCounter;
		nameFileout.close();
	}
	data().set_colors(Eigen::RowVector3d(0.9, 0.1, 0.1));
	isActive = false;
}

void SandBox::InitObjectData(ObjectData& od, Eigen::MatrixXd& V, Eigen::MatrixXi& F)
{

    od.V = new Eigen::MatrixXd(V);
    od.F = new Eigen::MatrixXi(F);
    od.E = new Eigen::MatrixXi();
    od.EF = new Eigen::MatrixXi();
    od.EI = new Eigen::MatrixXi();
    od.EMAP = new Eigen::VectorXi();
    od.Q = new PriorityQueue();
    od.num_collapsed = 0;
    od.tree = new igl::AABB<Eigen::MatrixXd, 3>{};

    igl::edge_flaps(*od.F, *od.E, *od.EMAP, *od.EF, *od.EI);

    od.tree->init(*od.V, *od.F);

    od.C = new Eigen::MatrixXd(od.E->rows(), od.V->cols());

    ComputeNormals(od, data());

    ComputeQMatrices(od);

    ComputePriorityQueue(od);
}

void SandBox::ReInitObjectData(ObjectData& od)
{
    Eigen::MatrixXd V = *od.V;
    Eigen::MatrixXi F = *od.F;

    // Remove old object data
    ObjectData* odToRemove = objectsData->at(selected_data_index);
    ClearObjectData(*odToRemove);
    delete odToRemove;

    // Add new object data after collapsing edges
    ObjectData* odToAdd = new ObjectData();
    InitObjectData(*odToAdd, V, F);
    objectsData->at(selected_data_index) = odToAdd;
}

void SandBox::ClearObjectData(ObjectData& od)
{
    delete od.V;
    delete od.F;
    delete od.EMAP;
    delete od.E;
    delete od.EF;
    delete od.EI;
    delete od.Q;
    delete od.C;
    delete od.F_NORMALS;

    std::for_each(od.QMATRICES.begin(), od.QMATRICES.end(), [](Eigen::Matrix4d* m) -> void { delete m; });
    od.QMATRICES.clear();
}

void SandBox::Simplify()
{
    ObjectData* objectToSimplify = objectsData->at(selected_data_index);
    int num_to_collapse = std::ceil(0.05 * objectToSimplify->Q->size());
    Simplify(num_to_collapse, *objectToSimplify);
}

void SandBox::Simplify(int num_to_collapse, ObjectData& od)
{

    if (!od.Q->empty())
    {
        bool something_collapsed = false;
        for (int j = 0; j < num_to_collapse; j++)
        {
              if (!collapse_edge(od))
            {
                break;
            }
            something_collapsed = true;
            od.num_collapsed++;
        }

        if (something_collapsed)
        {
            data().clear();
            data().set_mesh(*od.V, *od.F);
            data().set_face_based(true);
            data().dirty = 157; //this line prevents texture coordinates
            ReInitObjectData(od);
        }
    }
}

void SandBox::MoveTo(double x, double y)
{
    data().TranslateInSystem(GetRotation(), Eigen::Vector3d(x, 0, 0));
    data().TranslateInSystem(GetRotation(), Eigen::Vector3d(0, y, 0));
    WhenTranslate();
}

void SandBox::Animate()
{
	if (isActive)
	{

		if (isCCD)
		{
			IK_CCD();
		}
		else
		{
			IK_FABRIK();
		}
	}
}

bool SandBox::ObjectsCollide(igl::AABB<Eigen::MatrixXd, 3>* firstTree, igl::AABB<Eigen::MatrixXd, 3>* secondTree)
{
	if (firstTree == nullptr || secondTree == nullptr)
	{
		return false;
	}
 	if (BoxesIntersect(firstTree->m_box, secondTree->m_box)) {
		if (firstTree->is_leaf() && secondTree->is_leaf())
		{
			data_list.at(0).AddBoundingBox(firstTree->m_box, Eigen::RowVector3d(0, 1, 0));
			data_list.at(1).AddBoundingBox(secondTree->m_box, Eigen::RowVector3d(0, 1, 0));
			return true;
		}
		else if (!firstTree->is_leaf() && secondTree->is_leaf())
		{
			return	ObjectsCollide(firstTree->m_left, secondTree) ||
					ObjectsCollide(firstTree->m_right, secondTree);
		}
		else if (firstTree->is_leaf() && !secondTree->is_leaf())
		{

			return  ObjectsCollide(firstTree, secondTree->m_left) ||
					ObjectsCollide(firstTree, secondTree->m_right);
		}
		else if (!firstTree->is_leaf() && !secondTree->is_leaf())
		{
			return  ObjectsCollide(firstTree->m_left, secondTree->m_left) ||
					ObjectsCollide(firstTree->m_left, secondTree->m_right) ||
					ObjectsCollide(firstTree->m_right, secondTree->m_left) ||
					ObjectsCollide(firstTree->m_right, secondTree->m_right);
		}
	}
	return false;
}

bool SandBox::BoxesIntersect(Eigen::AlignedBox <double, 3>& firstBox, Eigen::AlignedBox <double, 3>& secondBox)
{
	Eigen::Matrix4d firstTrans = data_list.at(0).MakeTransd();
	Eigen::Matrix4d secondTrans = data_list.at(1).MakeTransd();

	Eigen::Vector3d firstBoxCenter = firstBox.center();
	Eigen::Vector3d secondBoxCenter = secondBox.center();

	Eigen::Vector4d firstCenter = firstTrans * Eigen::Vector4d(firstBoxCenter(0), firstBoxCenter(1), firstBoxCenter(2), 1);
	Eigen::Vector4d secondCenter = secondTrans * Eigen::Vector4d(secondBoxCenter(0), secondBoxCenter(1), secondBoxCenter(2), 1);

	Eigen::Vector3d C0(firstCenter(0), firstCenter(1), firstCenter(2));
	Eigen::Vector3d C1(secondCenter(0), secondCenter(1), secondCenter(2));

	Eigen::Vector3d D = C1 - C0;

	Eigen::Matrix3d A = data_list.at(0).GetRotation();
	Eigen::Matrix3d B = data_list.at(1).GetRotation();

	Eigen::Matrix3d A_matrix;
	A_matrix << A(0, 0), A(1, 0), A(2, 0),
				A(0, 1), A(1, 1), A(2, 1),
				A(0, 2), A(1, 2), A(2, 2);

	Eigen::Matrix3d B_matrix;
	B_matrix << B(0, 0), B(1, 0), B(2, 0),
				B(0, 1), B(1, 1), B(2, 1),
				B(0, 2), B(1, 2), B(2, 2);

	Eigen::RowVector3d a;
	a << firstBox.sizes()(0) / 2, firstBox.sizes()(1) / 2, firstBox.sizes()(2) / 2;

	Eigen::RowVector3d b;
	b << secondBox.sizes()(0) / 2, secondBox.sizes()(1) / 2, secondBox.sizes()(2) / 2;

	Eigen::Matrix3d C = A.transpose() * B;

	double R0, R1, R;

	// L = A0
	R0 = a(0);
	R1 = b(0) * abs(C(0, 0)) + b(1) * abs(C(0, 1)) + b(2) * abs(C(0, 2));
	R = (A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = A1
	R0 = a(1);
	R1 = b(0) * abs(C(1, 0)) + b(1) * abs(C(1, 1)) + b(2) * abs(C(1, 2));
	R = (A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2
	R0 = a(2);
	R1 = b(0) * abs(C(2, 0)) + b(1) * abs(C(2, 1)) + b(2) * abs(C(2, 2));
	R = (A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = B0
	R0 = a(0) * abs(C(0, 0)) + a(1) * abs(C(1, 0)) + a(2) * abs(C(2, 0));
	R1 = b(0);
	R = (B_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = B1
	R0 = a(0) * abs(C(0, 1)) + a(1) * abs(C(1, 1)) + a(2) * abs(C(2, 1));
	R1 = b(1);
	R = (B_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = B2
	R0 = a(0) * abs(C(0, 2)) + a(1) * abs(C(1, 2)) + a(2) * abs(C(2, 2));
	R1 = b(2);
	R = (B_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = A0 x B0
	R0 = a(1) * abs(C(2, 0)) + a(2) * abs(C(1, 0));
	R1 = b(1) * abs(C(0, 2)) + b(2) * abs(C(0, 1));
	R = (C(1, 0) * A_matrix.row(2) * D - C(2, 0) * A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A0 x B1
	R0 = a(1) * abs(C(2, 1)) + (a(2))*abs(C(1, 1));
	R1 = b(0) * abs(C(0, 2)) + b(2) * abs(C(0, 0));
	R = (C(1, 1) * A_matrix.row(2) * D - C(2, 1) * A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A0 x B2
	R0 = a(1) * abs(C(2, 2)) + a(2) * abs(C(1, 2));
	R1 = b(0) * abs(C(0, 1)) + b(1) * abs(C(0, 0));
	R = (C(1, 2) * A_matrix.row(2) * D - C(2, 2) * A_matrix.row(1) * D).norm();
	if (R > R0 + R1) return false;

	// L = A1 x B0
	R0 = a(0) * abs(C(2, 0)) + a(2) * abs(C(0, 0));
	R1 = b(1) * abs(C(1, 2)) + b(2) * abs(C(1, 1));
	R = (C(2, 0) * A_matrix.row(0) * D - C(0, 0) * A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = A1 x B1
	R0 = a(0) * abs(C(2, 1)) + a(2) * abs(C(0, 1));
	R1 = b(0) * abs(C(1, 2)) + b(2) * abs(C(1, 0));
	R = (C(2, 1) * A_matrix.row(0) * D - C(0, 1) * A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	/// L = A1 x B2
	R0 = a(0) * abs(C(2, 2)) + a(2) * abs(C(0, 2));
	R1 = b(0) * abs(C(1, 1)) + b(1) * abs(C(1, 0));
	R = (C(2, 2) * A_matrix.row(0) * D - C(0, 2) * A_matrix.row(2) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2 x B0
	R0 = a(0) * abs(C(1, 0)) + a(1) * abs(C(0, 0));
	R1 = b(1) * abs(C(2, 2)) + b(2) * abs(C(2, 1));
	R = (C(0, 0) * A_matrix.row(1) * D - C(1, 0) * A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2 x B1
	R0 = a(0) * abs(C(1, 1)) + a(1) * abs(C(0, 1));
	R1 = b(0) * abs(C(2, 2)) + b(2) * abs(C(2, 0));
	R = (C(0, 1) * A_matrix.row(1) * D - C(1, 1) * A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;

	// L = A2 x B2
	R0 = a(0) * abs(C(1, 2)) + a(1) * abs(C(0, 2));
	R1 = b(0) * abs(C(2, 1)) + b(1) * abs(C(2, 0));
	R = (C(0, 2) * A_matrix.row(1) * D - C(1, 2) * A_matrix.row(0) * D).norm();
	if (R > R0 + R1) return false;
	return true;
}

void SandBox::SetVelocity(double x, double y)
{
	xVelocity = x;
	yVelocity = y;
}

void  SandBox::IK_CCD()
{
	Eigen::Matrix4d destTrans = data_list[destIndex].MakeTransd();
	Eigen::Matrix4d firstTrans = data_list[firstLinkIndex].MakeTransd();
	Eigen::Matrix4d lastTrans = data_list[lastLinkIndex].MakeTransd();
	Eigen::Matrix4d lastParentsTrans = CalcParentsTrans(lastLinkIndex);

	Eigen::Vector4d lowerTip(0, -0.8, 0, 1);
	Eigen::Vector4d upperTip(0, 0.8, 0, 1);

	Eigen::Vector3d dst = ExtractPosition(destTrans * Eigen::Vector4d(0, 0, 0, 1));
	Eigen::Vector3d src = ExtractPosition(firstTrans * lowerTip);
	Eigen::Vector3d tip = ExtractPosition(lastParentsTrans * lastTrans * upperTip).transpose();

	double distToDest = (dst - tip).norm();
	double absDistance = (dst - src).norm();
	double maxReach = linksCounter * 1.6;
	double threshold = 0.1;

	std::cout << distToDest << std::endl;

	if (maxReach < absDistance)
	{
		std::cout << "Cant reach" << std::endl;
		isActive = false;
		return;
	}

	if (distToDest < threshold)
	{
		std::cout << "Reached" << std::endl;
		isActive = false;
		return;
	}

	for (int i = firstLinkIndex + lastLinkIndex - 1; i >= firstLinkIndex; i--)
	{
		Eigen::Matrix4d iParentsTrans = CalcParentsTrans(i);
		Eigen::Matrix4d iTrans = data_list[i].MakeTransd();
		Eigen::Vector3d D = dst;
		Eigen::Vector3d R = ExtractPosition(iParentsTrans * iTrans * lowerTip);
		Eigen::Vector3d E = ExtractPosition(lastParentsTrans * lastTrans * upperTip);
		Eigen::Vector3d RD = D - R;
		Eigen::Vector3d RE = E - R;
		float RD_dot_RE = RD.normalized().dot(RE.normalized());
		if (abs(RD_dot_RE) > 1)
		{
			std::cout << "Dot product greater then |1|, rounding value" << std::endl;
			RD_dot_RE = RD_dot_RE > 0 ? 1 : -1;
		}
		double theta = acosf(RD_dot_RE);
		Eigen::Matrix3d m = iParentsTrans.inverse().block(0, 0, 3, 3);
		Eigen::Vector3d rotAxis = m * RD.cross(RE);
		theta = theta / 10; // Smooth the rotation
		data_list[i].MyRotate(rotAxis, -theta);
	}
}

void  SandBox::IK_FABRIK()
{
	int n = linksCounter + 1;
	float di = 1.6;

	Eigen::Matrix4d destTrans = data_list[destIndex].MakeTransd();
	Eigen::Matrix4d firstTrans = data_list[firstLinkIndex].MakeTransd();
	Eigen::Matrix4d lastTrans = data_list[lastLinkIndex].MakeTransd();
	Eigen::Matrix4d lastParentsTrans = CalcParentsTrans(lastLinkIndex);

	Eigen::Vector4d lowerTip(0, -0.8, 0, 1);
	Eigen::Vector4d upperTip(0, 0.8, 0, 1);

	Eigen::Vector3d t = ExtractPosition(destTrans * Eigen::Vector4d(0, 0, 0, 1));
	Eigen::Vector3d p1 = ExtractPosition(firstTrans * lowerTip);
	Eigen::Vector3d pn = ExtractPosition(lastParentsTrans * lastTrans * upperTip).transpose();

	float dist = (p1 - t).norm();

	if (dist > di * (n - 1))
	{
		std::cout << "The target is unreachable" << std::endl;
		isActive = false;
		return;
	}

	float difA = (pn - t).norm();
	float tol = 0.1;

	std::cout << difA << std::endl;

	if (difA <= tol)
	{
		std::cout << "Reached target" << std::endl;
		isActive = false;
		return;
	}

	std::vector<Eigen::Vector3d> p;
	p.push_back(Eigen::Vector3d::Identity()); // Dummy
	p.push_back(p1);
	for (int i = 1; i <= linksCounter; i++)
	{
		p.push_back(ExtractPosition(data_list.at(i).MakeTransd() * CalcParentsTrans(i) * upperTip));
	}

	Eigen::Vector3d b = p.at(1);

	// FORWARD REACHING
	p.at(n) = t;

	for (int i = n - 1; i >= 1; i--)
	{
		Eigen::Vector3d pi = p.at(i);
		Eigen::Vector3d pi1 = p.at(i + 1);
		float ri = (pi1 - pi).norm();
		float li = di / ri;
		p.at(i) = ((1 - li) * pi1) + (li * pi);
	}

	// BACKWARD REACHING
	p.at(1) = b;

	for (int i = 1; i <= n - 1; i++)
	{
		Eigen::Vector3d pi = p.at(i);
		Eigen::Vector3d pi1 = p.at(i + 1);
		float ri = (pi1 - pi).norm();
		float li = di / ri;
		p.at(i + 1) = ((1 - li) * pi) + (li * pi1);
	}

	// UPDATE JOINTS POSITIONS
	for (int i = 1; i <= linksCounter; i++)
	{
		Eigen::Matrix4d iParentsTrans = CalcParentsTrans(i);
		Eigen::Matrix4d iTrans = data_list.at(i).MakeTransd();
		Eigen::Vector3d D = p.at(i + 1);
		Eigen::Vector3d R = ExtractPosition(iParentsTrans * iTrans * lowerTip);
		Eigen::Vector3d E = ExtractPosition(iParentsTrans * iTrans * upperTip);
		Eigen::Vector3d RD = D - R;
		Eigen::Vector3d RE = E - R;

		Eigen::Vector3d axis = RD.cross(RE).normalized();
		float RD_dot_RE = RD.normalized().dot(RE.normalized());
		if (abs(RD_dot_RE) > 1)
		{
			std::cout << "Dot product greater then |1|, rounding value" << std::endl;
			RD_dot_RE = RD_dot_RE > 0 ? 1 : -1;
		}
		float theta = acosf(RD_dot_RE);
		Eigen::Matrix4d m = iParentsTrans.inverse();
		Eigen::Matrix3d m1;
		m1 <<
			m(0, 0), m(0, 1), m(0, 2),
			m(1, 0), m(1, 1), m(1, 2),
			m(2, 0), m(2, 1), m(2, 2);

		Eigen::Vector3d rotAxis = m1 * RD.cross(RE);
		theta = theta / 10; // Smooth the rotation
		data_list.at(i).MyRotate(rotAxis, -theta);
	}
}

Eigen::Vector3d SandBox::ExtractPosition(Eigen::Vector4d m)
{
	return Eigen::Vector3d(m(0), m(1), m(2));
}


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

// BSD 3-Clause License

// Copyright (c) 2020, Chenyu
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice,
// this
//    list of conditions and the following disclaimer.

// 2. 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.

// 3. 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.

#include "math/sparse_cholesky_llt.h"

#include <cholmod.h>
#include <glog/logging.h>

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

// UF_long is deprecated but SuiteSparse_long is only available in
// newer versions of SuiteSparse. So for older versions of
// SuiteSparse, we define SuiteSparse_long to be the same as UF_long,
// which is what recent versions of SuiteSparse do anyways.
#ifndef SuiteSparse_long
#define SuiteSparse_long UF_long
#endif

namespace DAGSfM {
namespace {

cholmod_sparse ViewAsCholmod(const Eigen::SparseMatrix<double>& const_mat) {
  Eigen::SparseMatrix<double>& mat =
      const_cast<Eigen::SparseMatrix<double>&>(const_mat);
  cholmod_sparse res;
  res.nzmax = mat.nonZeros();
  res.nrow = mat.rows();
  res.ncol = mat.cols();
  res.p = mat.outerIndexPtr();
  res.i = mat.innerIndexPtr();
  res.x = mat.valuePtr();
  res.sorted = 1;
  if (mat.isCompressed()) {
    res.packed = 1;
  } else {
    res.packed = 0;
    res.nz = mat.innerNonZeroPtr();
  }

  // Set to 0 if the matrix is not symmetric.
  res.stype = 1;
  res.itype = CHOLMOD_INT;
  res.xtype = CHOLMOD_REAL;
  res.dtype = CHOLMOD_DOUBLE;
  return res;
}

cholmod_dense ViewAsCholmod(const Eigen::VectorXd& const_vec) {
  Eigen::VectorXd& vec = const_cast<Eigen::VectorXd&>(const_vec);
  cholmod_dense res;
  res.nrow = vec.rows();
  res.ncol = vec.cols();
  res.nzmax = res.nrow * res.ncol;
  res.d = vec.size();
  res.x = reinterpret_cast<void*>(vec.data());
  res.z = 0;
  res.xtype = CHOLMOD_REAL;
  return res;
}
}  // namespace

// A class for performing the choleksy decomposition of a sparse matrix using
// CHOLMOD from SuiteSparse. This allows us to utilize the supernodal algorithms
// which are not included with Eigen. The interface is meant to mimic the Eigen
// linear solver interface except that it is not templated and requires sparse
// matrices.
SparseCholeskyLLt::SparseCholeskyLLt(const Eigen::SparseMatrix<double>& mat)
    : cholmod_factor_(nullptr),
      is_factorization_ok_(false),
      is_analysis_ok_(false),
      info_(Eigen::Success) {
  cholmod_start(&cc_);
  Compute(mat);
}

SparseCholeskyLLt::SparseCholeskyLLt()
    : cholmod_factor_(nullptr),
      is_factorization_ok_(false),
      is_analysis_ok_(false),
      info_(Eigen::Success) {
  cholmod_start(&cc_);
}

SparseCholeskyLLt::~SparseCholeskyLLt() {
  if (cholmod_factor_ != nullptr) {
    cholmod_free_factor(&cholmod_factor_, &cc_);
  }
  cholmod_finish(&cc_);
}

void SparseCholeskyLLt::AnalyzePattern(const Eigen::SparseMatrix<double>& mat) {
  // Release the current decomposition if there is one.
  if (cholmod_factor_ != nullptr) {
    cholmod_free_factor(&cholmod_factor_, &cc_);
  }

  // Get the cholmod view of the sparse matrix.
  cholmod_sparse A = ViewAsCholmod(mat);

  // Cholmod can try multiple re-ordering strategies to find a fill
  // reducing ordering. Here we just tell it use AMD with automatic
  // matrix dependence choice of supernodal versus simplicial
  // factorization.
  cc_.nmethods = 1;
  cc_.method[0].ordering = CHOLMOD_AMD;
  cc_.supernodal = CHOLMOD_AUTO;

  // Perform symbolic analysis of the matrix.
  cholmod_factor_ = cholmod_analyze(&A, &cc_);
  if (VLOG_IS_ON(2)) {
    cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_);
  }

  if (cc_.status != CHOLMOD_OK) {
    VLOG(2) << "cholmod_analyze failed. error code: %d" << cc_.status;
    info_ = Eigen::NumericalIssue;
    return;
  }

  is_analysis_ok_ = true;
  is_factorization_ok_ = false;
  info_ = Eigen::Success;
}

void SparseCholeskyLLt::Factorize(const Eigen::SparseMatrix<double>& mat) {
  CHECK_NOTNULL(cholmod_factor_);
  CHECK(is_analysis_ok_) << "Cannot call Factorize() because symbolic analysis "
                            "of the matrix (i.e. AnalyzePattern()) failed!";
  cholmod_sparse A = ViewAsCholmod(mat);

  // Save the current print level and silence CHOLMOD, otherwise
  // CHOLMOD is prone to dumping stuff to stderr, which can be
  // distracting when the error (matrix is indefinite) is not a fatal
  // failure.
  const int old_print_level = cc_.print;
  cc_.print = 0;

  cc_.quick_return_if_not_posdef = 1;
  int cholmod_status = cholmod_factorize(&A, cholmod_factor_, &cc_);
  cc_.print = old_print_level;

  // TODO(sameeragarwal): This switch statement is not consistent. It
  // treats all kinds of CHOLMOD failures as warnings. Some of these
  // like out of memory are definitely not warnings. The problem is
  // that the return value Cholesky is two valued, but the state of
  // the linear solver is really three valued. SUCCESS,
  // NON_FATAL_FAILURE (e.g., indefinite matrix) and FATAL_FAILURE
  // (e.g. out of memory).
  switch (cc_.status) {
    case CHOLMOD_NOT_INSTALLED:
      LOG(ERROR) << "CHOLMOD failure: Method not installed.";
      info_ = Eigen::InvalidInput;
      return;
    case CHOLMOD_OUT_OF_MEMORY:
      LOG(ERROR) << "CHOLMOD failure: Out of memory.";
      info_ = Eigen::NumericalIssue;
      return;
    case CHOLMOD_TOO_LARGE:
      LOG(ERROR) << "CHOLMOD failure: Integer overflow occured.";
      info_ = Eigen::NumericalIssue;
      return;
    case CHOLMOD_INVALID:
      LOG(ERROR) << "CHOLMOD failure: Invalid input.";
      info_ = Eigen::InvalidInput;
      return;
    case CHOLMOD_NOT_POSDEF:
      LOG(ERROR) << "CHOLMOD warning: Matrix not positive definite.";
      info_ = Eigen::NumericalIssue;
      return;
    case CHOLMOD_DSMALL:
      LOG(ERROR) << "CHOLMOD warning: D for LDL' or diag(L) or "
                    "LL' has tiny absolute value.";
      info_ = Eigen::NumericalIssue;
      return;
    case CHOLMOD_OK:
      // If everything is done successfully, set the appropriate flags to
      // success and exit.
      if (cholmod_status != 0) {
        info_ = Eigen::Success;
        is_factorization_ok_ = true;
        return;
      }
      LOG(ERROR) << "CHOLMOD failure: cholmod_factorize returned false "
                    "but cholmod_common::status is CHOLMOD_OK.";
      info_ = Eigen::NumericalIssue;
      return;
    default:
      LOG(ERROR) << "Unknown cholmod return code: " << cc_.status;
      info_ = Eigen::InvalidInput;
      return;
  }
}

void SparseCholeskyLLt::Compute(const Eigen::SparseMatrix<double>& mat) {
  AnalyzePattern(mat);
  Factorize(mat);
}

Eigen::ComputationInfo SparseCholeskyLLt::Info() { return info_; }

// Using the cholesky decomposition, solve for x that minimizes
//    lhs * x = rhs
// where lhs is the factorized matrix.
Eigen::VectorXd SparseCholeskyLLt::Solve(const Eigen::VectorXd& rhs) {
  CHECK_NOTNULL(cholmod_factor_);
  CHECK(is_analysis_ok_) << "Cannot call Solve() because symbolic analysis "
                            "of the matrix (i.e. AnalyzePattern()) failed!";
  CHECK(is_factorization_ok_)
      << "Cannot call Solve() because numeric factorization "
         "of the matrix (i.e. Factorize()) failed!";

  Eigen::VectorXd solution;
  if (cc_.status != CHOLMOD_OK) {
    LOG(ERROR) << "cholmod_solve failed. CHOLMOD status is not CHOLMOD_OK";
    return solution;
  }

  // returns a cholmod_dense*
  cholmod_dense b = ViewAsCholmod(rhs);
  cholmod_dense* x = cholmod_solve(CHOLMOD_A, cholmod_factor_, &b, &cc_);
  if (x == nullptr) {
    info_ = Eigen::NumericalIssue;
    return solution;
  }
  solution = Eigen::Map<Eigen::VectorXd>(reinterpret_cast<double*>(x->x),
                                         x->nrow, x->ncol);
  return solution;
}

}  // namespace DAGSfM


/**
* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
*
* Copyright (c) 2016 LIBSCAPI (http://crypto.biu.ac.il/SCAPI)
* This file is part of the SCAPI project.
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* We request that any publication and/or code referring to and/or based on SCAPI contain an appropriate citation to SCAPI, including a reference to
* http://crypto.biu.ac.il/SCAPI.
*
* Libscapi uses several open source libraries. Please see these projects for any further licensing issues.
* For more information , See https://github.com/cryptobiu/libscapi/blob/master/LICENSE.MD
*
* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
*
*/

#pragma once

#include <boost/thread/thread.hpp>
#include "../../include/comm/Comm.hpp"
#include "../../include/interactive_mid_protocols/SigmaProtocolDlog.hpp"
#include "../../include/interactive_mid_protocols/ZeroKnowledge.hpp"
#include "../../include/primitives/DlogOpenSSL.hpp"
#include "../../include/infra/Scanner.hpp"
#include "../../include/infra/ConfigFile.hpp"

struct SigmaDlogParams {
	biginteger w;
	biginteger p;
	biginteger q;
	biginteger g;
	int t;
	IpAddress proverIp;
	IpAddress verifierIp;
	int proverPort;
	int verifierPort;
	string protocolName;

	SigmaDlogParams(biginteger w, biginteger p, biginteger q, biginteger g, int t,
		IpAddress proverIp, IpAddress verifierIp, int proverPort, int verifierPort,
		string protocolName) {
		this->w = w; // witness
		this->p = p; // group order - must be prime
		this->q = q; // sub group order - prime such that p=2q+1
		this->g = g; // generator of Zq
		this->t = t; // soundness param must be: 2^t<q
		this->proverIp = proverIp;
		this->verifierIp = verifierIp;
		this->proverPort = proverPort;
		this->verifierPort = verifierPort;
		this->protocolName = protocolName;
	};
};

SigmaDlogParams readSigmaConfig(string config_file) {
	ConfigFile cf(config_file);
	string input_section = cf.Value("", "input_section");
	biginteger p = biginteger(cf.Value(input_section, "p"));
	biginteger q = biginteger(cf.Value(input_section, "q"));
	biginteger g = biginteger(cf.Value(input_section, "g"));
	biginteger w = biginteger(cf.Value(input_section, "w"));
	int t = stoi(cf.Value(input_section, "t"));
	string proverIpStr = cf.Value("", "proverIp");
	string verifierIpStr = cf.Value("", "verifierIp");
	int proverPort = stoi(cf.Value("", "proverPort"));
	int verifierPort = stoi(cf.Value("", "verifierPort"));
	auto proverIp = IpAddress::from_string(proverIpStr);
	auto verifierIp = IpAddress::from_string(verifierIpStr);
	string protocolName = cf.Value("", "protocolName");
	return SigmaDlogParams(w, p, q, g, t, proverIp, verifierIp, proverPort, verifierPort, protocolName);
};

void SigmaUsage() {
	std::cerr << "Usage: ./libscapi_examples <1(=prover)|2(=verifier)> config_file_path" << std::endl;
}

class ProverVerifierExample {
public:
	virtual void prove(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogProverComputation> proverComputation,
		shared_ptr<DlogGroup> dg,
		shared_ptr<SigmaDlogProverInput> proverinput) = 0;
	virtual bool verify(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogVerifierComputation> verifierComputation,
		shared_ptr<SigmaGroupElementMsg> msgA,
		shared_ptr<SigmaBIMsg> msgZ,
		shared_ptr<SigmaDlogCommonInput> commonInput,
		shared_ptr<DlogGroup> dg) = 0;
};

class SimpleDlogSigma : public ProverVerifierExample {
public:
	virtual void prove(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogProverComputation> proverComputation,
		shared_ptr<DlogGroup> dg,
		shared_ptr<SigmaDlogProverInput> proverinput) override {
		auto sp = new SigmaProtocolProver(server, proverComputation);
		cout << "--> running simple sigma dlog prover" << endl;
		sp->prove(proverinput);
	}
	virtual bool verify(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogVerifierComputation> verifierComputation,
		shared_ptr<SigmaGroupElementMsg> msgA,
		shared_ptr<SigmaBIMsg> msgZ,
		shared_ptr<SigmaDlogCommonInput> commonInput,
		shared_ptr<DlogGroup> dg) override{
		auto v = new SigmaProtocolVerifier(server, verifierComputation, msgA, msgZ);
		cout << "--> running simple sigma dlog verify" << endl;
		bool verificationPassed = v->verify(commonInput.get());
		delete v;
		return verificationPassed;
	}
};

class ZKFromSigma : public ProverVerifierExample {
public:
	virtual void prove(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogProverComputation> proverComputation,
		shared_ptr<DlogGroup> dg,
		shared_ptr<SigmaDlogProverInput> proverinput) {
		cout << "before creating ZK prover" << endl;
		auto receiver = make_shared<CmtPedersenReceiver>(server, dg);
		auto sp = new ZKFromSigmaProver(server, proverComputation, receiver);
		cout << "--> running ZK prover" << endl;
		sp->prove(proverinput);
	}
	virtual bool verify(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogVerifierComputation> verifierComputation,
		shared_ptr<SigmaGroupElementMsg> msgA,
		shared_ptr<SigmaBIMsg> msgZ,
		shared_ptr<SigmaDlogCommonInput> commonInput,
		shared_ptr<DlogGroup> dg) override {
		cout << "before creating ZK verifier" << endl;
		auto emptyTrap = make_shared<CmtRTrapdoorCommitPhaseOutput>();
		auto committer = make_shared<CmtPedersenCommitter>(server, dg);
		auto v = new ZKFromSigmaVerifier(server, verifierComputation, committer);
		cout << "--> running ZK verify" << endl;
		bool verificationPassed = v->verify(commonInput.get(), msgA, msgZ);
		delete v;
		return verificationPassed;
	}
};

class PedersenZKSigma : public ProverVerifierExample {
public:
	virtual void prove(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogProverComputation> proverComputation,
		shared_ptr<DlogGroup> dg,
		shared_ptr<SigmaDlogProverInput> proverinput) {
		auto sp = new ZKPOKFromSigmaCmtPedersenProver(server, proverComputation, dg);
		cout << "--> running pedersen prover" << endl;
		sp->prove(proverinput);
	}
	virtual bool verify(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogVerifierComputation> verifierComputation,
		shared_ptr<SigmaGroupElementMsg> msgA,
		shared_ptr<SigmaBIMsg> msgZ,
		shared_ptr<SigmaDlogCommonInput> commonInput,
		shared_ptr<DlogGroup> dg) override {
		auto emptyTrap = make_shared<CmtRTrapdoorCommitPhaseOutput>();
		auto v = new ZKPOKFromSigmaCmtPedersenVerifier(server, verifierComputation, emptyTrap, dg);
		cout << "--> running pedersen verify" << endl;
		bool verificationPassed = v->verify(commonInput.get(), msgA, msgZ);
		delete v;
		return verificationPassed;
	}
};

class ZKPOKFiatShamir : public ProverVerifierExample {
public:
	virtual void prove(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogProverComputation> proverComputation,
		shared_ptr<DlogGroup> dg,
		shared_ptr<SigmaDlogProverInput> proverinput) {
		auto sp = new ZKPOKFiatShamirFromSigmaProver(server, proverComputation);
		cout << "--> running Fiat Shamir prover" << endl;
		vector<byte> cont;
		auto input = make_shared<ZKPOKFiatShamirProverInput>(proverinput, cont);
		sp->prove(input);
	}
	virtual bool verify(shared_ptr<CommParty> server,
		shared_ptr<SigmaDlogVerifierComputation> verifierComputation,
		shared_ptr<SigmaGroupElementMsg> msgA,
		shared_ptr<SigmaBIMsg> msgZ,
		shared_ptr<SigmaDlogCommonInput> commonInput,
		shared_ptr<DlogGroup> dg) override {
		auto emptyTrap = make_shared<CmtRTrapdoorCommitPhaseOutput>();
		auto v = new ZKPOKFiatShamirFromSigmaVerifier(server, verifierComputation);
		cout << "--> running Fiat Shamir verify" << endl;
		vector<byte> cont;
		auto input = make_shared<ZKPOKFiatShamirCommonInput>(commonInput.get(), cont);
		bool verificationPassed = v->verify(input.get(), msgA, msgZ);
		delete v;
		return verificationPassed;
	}
};

shared_ptr<ProverVerifierExample> getProverVerifier(SigmaDlogParams sdp)
{
	shared_ptr<ProverVerifierExample> sds;
	if(sdp.protocolName=="Simple")
		sds = make_shared<SimpleDlogSigma>();
	else if (sdp.protocolName == "SimpleZK")
		sds = make_shared<ZKFromSigma>();
	else if(sdp.protocolName=="ZKPedersen")
		sds = make_shared<PedersenZKSigma>();
	else if (sdp.protocolName == "ZKFiatShamir")
		sds = make_shared<ZKPOKFiatShamir>();
	return sds;
}


// Copyright 2005 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include "graph_types.hpp"
#include <boost/python.hpp>
#include <boost/graph/iteration_macros.hpp>

namespace boost { namespace graph { namespace python {

#define BGL_PYTHON_VISITOR maybe_bellman_ford_visitor
#define BGL_PYTHON_EVENTS_HEADER <boost/graph/python/bellman_ford_events.hpp>
#include <boost/graph/python/visitor.hpp>
#undef BGL_PYTHON_EVENTS_HEADER
#undef BGL_PYTHON_VISITOR

template<typename Graph>
void
bellman_ford_shortest_paths
  (const Graph& g,
   typename graph_traits<Graph>::vertex_descriptor s,
   vector_property_map<
     typename graph_traits<Graph>::vertex_descriptor,
     typename property_map<Graph, vertex_index_t>::const_type>* in_predecessor,
   vector_property_map<
     float,
     typename property_map<Graph, vertex_index_t>::const_type>* in_distance,
   vector_property_map<
     float,
     typename property_map<Graph, edge_index_t>::const_type>* in_weight,
   boost::python::object in_visitor)
{
  using boost::python::object;

  typedef typename property_map<Graph, vertex_index_t>::const_type
    VertexIndexMap;
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  typedef vector_property_map<Vertex, VertexIndexMap> PredecessorMap;
  typedef vector_property_map<float, VertexIndexMap> DistanceMap;
  typedef typename property_map<Graph, edge_index_t>::const_type
    EdgeIndexMap;
  typedef vector_property_map<float, EdgeIndexMap> WeightMap;

  PredecessorMap predecessor =
    in_predecessor? *in_predecessor
    : PredecessorMap(num_vertices(g), get(vertex_index, g));

  DistanceMap distance =
    in_distance? *in_distance
    : DistanceMap(num_vertices(g), get(vertex_index, g));

  WeightMap weight =
    in_weight? *in_weight
    : WeightMap(num_edges(g), get(edge_index, g));

  // If no weight map was provided, initialize every weight with 1.0
  if (!in_weight) {
    BGL_FORALL_EDGES_T(e, g, Graph)
      put(weight, e, 1.0f);
  }

  if (in_visitor != object()) {
    boost::bellman_ford_shortest_paths
      (g,
       root_vertex(s).
       visitor(maybe_bellman_ford_visitor(in_visitor)).
       predecessor_map(predecessor).
       distance_map(distance).
       weight_map(weight));
  } else {
    boost::bellman_ford_shortest_paths
      (g,
       root_vertex(s).
       predecessor_map(predecessor).
       distance_map(distance).
       weight_map(weight));
  }
}

void export_bellman_ford_shortest_paths()
{
  using boost::python::arg;
  using boost::python::def;
  using boost::python::object;

#define UNDIRECTED_GRAPH(Name,Type)                                     \
  {                                                                     \
    typedef graph_traits<Type>::vertex_descriptor vertex_descriptor;    \
    typedef property_map<Type, vertex_index_t>::const_type VertexIndexMap; \
    typedef property_map<Type, edge_index_t>::const_type EdgeIndexMap;  \
    typedef vector_property_map<vertex_descriptor, VertexIndexMap>      \
      VertexPredecessorMap;                                             \
    typedef vector_property_map<float, VertexIndexMap>                  \
      VertexDistanceMap;                                                \
    typedef vector_property_map<default_color_type, VertexIndexMap>     \
      VertexColorMap;                                                   \
    typedef vector_property_map<float, EdgeIndexMap>                    \
      EdgeWeightMap;                                                    \
                                                                        \
    def("bellman_ford_shortest_paths",                                  \
        &boost::graph::python::bellman_ford_shortest_paths<Type>,       \
        (arg("graph"),                                                  \
         arg("root_vertex"),                                            \
         arg("predecessor_map") = static_cast<VertexPredecessorMap*>(0), \
         arg("distance_map") = static_cast<VertexDistanceMap*>(0),      \
         arg("weight_map") = static_cast<EdgeWeightMap*>(0),            \
         arg("visitor") = object()));                                   \
  }
#include "graphs.hpp"
}

} } } // end namespace boost::graph::python


//=======================================================================
// Copyright 2013 University of Warsaw.
// Authors: Piotr Wygocki
//
// Distributed under 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_GRAPH_AUGMENT_HPP
#define BOOST_GRAPH_AUGMENT_HPP

#include <boost/graph/filtered_graph.hpp>

namespace boost {
namespace detail {

template <class Graph, class ResCapMap>
filtered_graph<const Graph, is_residual_edge<ResCapMap> >
residual_graph(const Graph& g, ResCapMap residual_capacity) {
    return filtered_graph<const Graph, is_residual_edge<ResCapMap> >
        (g, is_residual_edge<ResCapMap>(residual_capacity));
}

template <class Graph, class PredEdgeMap, class ResCapMap,
         class RevEdgeMap>
inline void
augment(const Graph& g,
        typename graph_traits<Graph>::vertex_descriptor src,
        typename graph_traits<Graph>::vertex_descriptor sink,
        PredEdgeMap p,
        ResCapMap residual_capacity,
        RevEdgeMap reverse_edge)
{
    typename graph_traits<Graph>::edge_descriptor e;
    typename graph_traits<Graph>::vertex_descriptor u;
    typedef typename property_traits<ResCapMap>::value_type FlowValue;

    // find minimum residual capacity along the augmenting path
    FlowValue delta = (std::numeric_limits<FlowValue>::max)();
    e = get(p, sink);
    do {
        BOOST_USING_STD_MIN();
        delta = min BOOST_PREVENT_MACRO_SUBSTITUTION(delta, get(residual_capacity, e));
        u = source(e, g);
        e = get(p, u);
    } while (u != src);

    // push delta units of flow along the augmenting path
    e = get(p, sink);
    do {
        put(residual_capacity, e, get(residual_capacity, e) - delta);
        put(residual_capacity, get(reverse_edge, e), get(residual_capacity, get(reverse_edge, e)) + delta);
        u = source(e, g);
        e = get(p, u);
    } while (u != src);
}

} // namespace detail
} //namespace boost

#endif /* BOOST_GRAPH_AUGMENT_HPP */