/*
 *            Copyright 2009-2020 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

// Standard includes
#include <cmath>

// Third party includes
#include <boost/math/constants/constants.hpp>

// VOTCA includes
#include <votca/tools/constants.h>

// Local VOTCA includes
#include "votca/xtp/sigma_base.h"
#include "votca/xtp/threecenter.h"

namespace votca {
namespace xtp {

Eigen::MatrixXd Sigma_base::CalcExchangeMatrix() const {
  Eigen::MatrixXd result = Eigen::MatrixXd::Zero(_qptotal, _qptotal);
  Index occlevel = _opt.homo - _opt.rpamin + 1;
  Index qpmin = _opt.qpmin - _opt.rpamin;
#pragma omp parallel for schedule(dynamic)
  for (Index gw_level1 = 0; gw_level1 < _qptotal; gw_level1++) {
    const Eigen::MatrixXd& Mmn1 = _Mmn[gw_level1 + qpmin];
    for (Index gw_level2 = gw_level1; gw_level2 < _qptotal; gw_level2++) {
      const Eigen::MatrixXd& Mmn2 = _Mmn[gw_level2 + qpmin];
      double sigma_x =
          -(Mmn1.topRows(occlevel).cwiseProduct(Mmn2.topRows(occlevel))).sum();
      result(gw_level2, gw_level1) = sigma_x;
    }
  }
  result = result.selfadjointView<Eigen::Lower>();
  return result;
}

Eigen::VectorXd Sigma_base::CalcCorrelationDiag(
    const Eigen::VectorXd& frequencies) const {
  Eigen::VectorXd result = Eigen::VectorXd::Zero(_qptotal);
#pragma omp parallel for schedule(dynamic)
  for (Index gw_level = 0; gw_level < _qptotal; gw_level++) {
    result(gw_level) =
        CalcCorrelationDiagElement(gw_level, frequencies[gw_level]);
  }
  return result;
}

Eigen::MatrixXd Sigma_base::CalcCorrelationOffDiag(
    const Eigen::VectorXd& frequencies) const {
  Eigen::MatrixXd result = Eigen::MatrixXd::Zero(_qptotal, _qptotal);
#pragma omp parallel for schedule(dynamic)
  for (Index gw_level1 = 0; gw_level1 < _qptotal; gw_level1++) {
    for (Index gw_level2 = gw_level1 + 1; gw_level2 < _qptotal; gw_level2++) {
      double sigma_c = CalcCorrelationOffDiagElement(
          gw_level1, gw_level2, frequencies[gw_level1], frequencies[gw_level2]);
      result(gw_level2, gw_level1) = sigma_c;
    }
  }
  result = result.selfadjointView<Eigen::Lower>();
  return result;
}

}  // namespace xtp
}  // namespace votca


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "Ecqpp.h"
#include <Eigen/QR>
#include <algorithm>
#include <cassert>
#include <iostream>
#include <limits>

namespace Scine {
namespace Utils {

Ecqpp::Ecqpp(const Eigen::MatrixXd& B, const Eigen::VectorXd& E)
  : B_(B), E_(E), dimension_(static_cast<unsigned>(E.size())) {
  assert(B.rows() == dimension_ && B.cols() == dimension_ && "B matrix and E vector do not have compatible dimensions.");
}

Eigen::VectorXd Ecqpp::calculateOptimalCoefficients() {
  solveAllConstrainedProblems();
  return solution_;
}

void Ecqpp::solveAllConstrainedProblems() {
  bestSolutionEnergy_ = std::numeric_limits<double>::max();
  for (unsigned i = 0; i < dimension_; ++i)
    solveAllConstrainedProblemsForNumberZeros(i);
}

void Ecqpp::solveAllConstrainedProblemsForNumberZeros(unsigned int numberZeros) {
  std::vector<bool> indexConsidered(dimension_, true);
  for (unsigned i = 0; i < numberZeros; ++i)
    indexConsidered[i] = false;

  do {
    generatePreviousIndexesVector(indexConsidered, numberZeros);
    generateReducedObjects();
    solveConstrainedProblem();
    if (solutionIsValid())
      addSolution();
  } while (std::next_permutation(indexConsidered.begin(), indexConsidered.end()));
}

void Ecqpp::generatePreviousIndexesVector(const std::vector<bool>& consideredIndexes, unsigned numberZeros) {
  unsigned writeIndex = 0;
  unsigned newMatrixDimension = dimension_ - numberZeros;
  previousIndexes_.resize(newMatrixDimension);
  for (unsigned readIndex = 0; readIndex < dimension_; ++readIndex) {
    if (consideredIndexes[readIndex]) {
      previousIndexes_[writeIndex] = readIndex;
      ++writeIndex;
    }
  }
}

void Ecqpp::generateReducedObjects() {
  auto reducedDimension = static_cast<unsigned>(previousIndexes_.size());

  reducedB_.resize(reducedDimension, reducedDimension);
  reducedE_.resize(reducedDimension);

  for (unsigned i = 0; i < reducedDimension; ++i) {
    reducedE_(i) = E_(previousIndexes_[i]);
    for (unsigned j = 0; j < reducedDimension; ++j) {
      reducedB_(i, j) = B_(previousIndexes_[i], previousIndexes_[j]);
    }
  }
}

void Ecqpp::solveConstrainedProblem() {
  auto rDim = reducedB_.cols();

  Eigen::MatrixXd M(rDim + 1, rDim + 1);
  Eigen::VectorXd b(rDim + 1);

  M.block(0, 0, rDim, rDim) = reducedB_;
  M.block(rDim, 0, 1, rDim).setOnes();
  M.block(0, rDim, rDim, 1).setOnes();
  M(rDim, rDim) = 0;
  b.head(rDim) = reducedE_;
  b(rDim) = 1;

  Eigen::VectorXd solution = M.colPivHouseholderQr().solve(b);
  reducedSolution_ = solution.head(rDim);
}

bool Ecqpp::solutionIsValid() const {
  for (unsigned i = 0; i < reducedSolution_.size(); ++i)
    if (reducedSolution_[i] < 0)
      return false;
  return true;
}

void Ecqpp::addSolution() {
  generateSolutionFromReducedSolution();
  setBestSolutionIfHasLowerEnergy();
}

void Ecqpp::generateSolutionFromReducedSolution() {
  fullSolution_ = Eigen::VectorXd::Zero(dimension_);
  for (unsigned i = 0; i < previousIndexes_.size(); ++i) {
    fullSolution_[previousIndexes_[i]] = reducedSolution_[i];
  }
}

void Ecqpp::setBestSolutionIfHasLowerEnergy() {
  double currentEnergy = E_.dot(fullSolution_) - 0.5 * fullSolution_.transpose() * B_ * fullSolution_;
  if (currentEnergy < bestSolutionEnergy_) {
    solution_ = fullSolution_;
    bestSolutionEnergy_ = currentEnergy;
  }
}
} // namespace Utils
} // namespace Scine


/** $Id: EnergyDistribution.cxx 165001 2018-08-27 09:22:30Z jvansanten $
 * @file
 * @author Jakob van Santen <vansanten@wisc.edu>
 *
 * $Revision: 165001 $
 * $Date: 2018-08-27 03:22:30 -0600 (Mon, 27 Aug 2018) $
 */

#include <MuonGun/EnergyDistribution.h>
#include <MuonGun/RadialDistribution.h>
#include <phys-services/I3RandomService.h>
#include <MuonGun/EnsembleSampler.h>

#include <boost/bind.hpp>

namespace I3MuonGun {

EnergyDistribution::~EnergyDistribution() {};

double
EnergyDistribution::operator()(double d, double ct,
    unsigned m, double r, double e) const
{
	return std::exp(GetLog(d, ct, m, r, log_value(std::log(e))));
}

double
EnergyDistribution::Integrate(double d, double ct,
    unsigned m, double r_min, double r_max, double e_min, double e_max) const
{
	// restrict integration to range where density can be nonzero
	double loge_min = std::max(minLog_, std::log(e_min));
	double loge_max = std::min(maxLog_, std::log(e_max));
	r_min = std::max(0., r_min);
	r_max = std::min(GetMaxRadius(), r_max);
	if (m > 1) {
		// Integrate dP/(dlogE dr^2) for numerical stability
		auto integrand = [this,d,ct,m](double r2, double loge)
		{
			double r = std::sqrt(r2);
			return std::exp(GetLog(d, ct, m, r, log_value(loge))
			    - std::log(2*r) + loge);
		};
		boost::array<double, 2> lo = {{r_min*r_min, loge_min}};
		boost::array<double, 2> hi = {{r_max*r_max, loge_max}};
		return I3MuonGun::Integrate(boost::function<double(double,double)>(integrand),
		    lo, hi, 1e-12, 1e-6, 10000);
	} else {
		// For single muons, dP/dr is a delta function at 0
		auto integrand = [this,d,ct,m](double loge)
		{
			return std::exp(GetLog(d, ct, m, 0, log_value(loge)) + loge);
		};
		return I3MuonGun::Integrate(boost::function<double(double)>(integrand), loge_min, loge_max);
	}
}

bool
SplineEnergyDistribution::operator==(const EnergyDistribution &o) const
{
	const SplineEnergyDistribution *other = dynamic_cast<const SplineEnergyDistribution*>(&o);
	if (!other)
		return false;
	else
		return (singles_ == other->singles_ && bundles_ == other->bundles_);
}

SplineEnergyDistribution::SplineEnergyDistribution(const std::string &singles, const std::string &bundles)
    : singles_(singles), bundles_(bundles)
{
	if (singles_.GetNDim() != 3u)
		log_fatal("'%s' does not appear to be a single-muon energy distribution", singles.c_str());
	if (bundles_.GetNDim() != 5u)
		log_fatal("'%s' does not appear to be a muon bundle energy distribution", bundles.c_str());
	SetMin(std::exp(std::max(singles_.GetExtents(2).first, bundles_.GetExtents(2).first)));
	SetMax(std::exp(std::min(singles_.GetExtents(2).second, bundles_.GetExtents(2).second)));
}

double
SplineEnergyDistribution::GetMaxRadius() const
{
	return bundles_.GetExtents(3).second;
}

double
SplineEnergyDistribution::GetLog(double depth, double cos_theta,
    unsigned multiplicity, double radius, log_value log_energy) const
{
	double coords[5] = {cos_theta, depth, static_cast<double>(multiplicity),
	    radius, log_energy};
	double logprob;

	if (radius < 0 || radius > GetMaxRadius() ||
	    log_energy < minLog_ || log_energy > maxLog_) {
		return -std::numeric_limits<double>::infinity();
	} else if (multiplicity < 2) {
		coords[2] = coords[4];
		if (singles_.Eval(coords, &logprob) != 0)
			return -std::numeric_limits<double>::infinity();
	} else if (bundles_.Eval(coords, &logprob) != 0)
		return -std::numeric_limits<double>::infinity();

	// Bundle spline is fit to log(dP/dr^2 dlogE)
	if (multiplicity > 1)
		logprob += std::log(2*radius);

	return logprob;
}

std::vector<std::pair<double,double> >
SplineEnergyDistribution::Generate(I3RandomService &rng, double depth,
    double cos_theta, unsigned multiplicity, unsigned nsamples) const
{
	typedef double (Signature)(double, double);
	typedef EnsembleSampler<Signature> Sampler;

	// the number of walkers must be even, and at least twice the
	// dimensionality of the space
	const unsigned walkers = std::max(4u, nsamples + (nsamples % 2));
	std::vector<Sampler::array_type> initial_ensemble(walkers);
	{
		// Draw starting positions from the MUPAGE parameterization
		BMSSEnergyDistribution proposal;
		proposal.SetMin(GetMin());
		proposal.SetMax(GetMax());
		std::vector<std::pair<double, double> > samples =
		    proposal.Generate(rng, depth, cos_theta, multiplicity, walkers);
		for (unsigned i=0; i < walkers; i++) {
			initial_ensemble[i][0] = samples.at(i).first;
			initial_ensemble[i][1] = samples.at(i).second;
		}
	}

	auto log_posterior = [this,depth,cos_theta,multiplicity](double r, double e)
	{
		return this->GetLog(depth, cos_theta, multiplicity, r,
		    EnergyDistribution::log_value(std::log(e)));
	};
	Sampler sampler(log_posterior, initial_ensemble);

	// Run the sampler for a few cycles to make it independent of the initial
	// ensemble. Fewer than 50 or so burn-in steps is too small to reach the
	// stationary distribution, while more than 100 is a waste of time, as
	// measured with resources/test/test_sampling.py
	for (unsigned i=0; i < 64; i++)
		sampler.Sample(rng);

	// copy the current ensemble into the output
	std::vector<std::pair<double,double> > samples;
	samples.reserve(nsamples);
	const std::vector<Sampler::sample> &ensemble = sampler.Sample(rng);
	unsigned todo = std::min(walkers, nsamples - unsigned(samples.size()));
	for (unsigned j=0; j < todo; j++) {
		samples.push_back(std::make_pair(ensemble[j].point[0], ensemble[j].point[1]));
	}

	// check the acceptance rate for sanity.
	double acceptance_rate = sampler.GetAcceptanceRate();
	if (acceptance_rate < 0.2) {
		log_warn("Ensemble sampler accepted only %.0f%% of samples (too low). It may be stuck in a local maximum.", 100*acceptance_rate);
	} else if (acceptance_rate > 0.9) {
		log_warn("Ensemble sampler accepted %.0f%% of samples (too high). This is an expensive random walk.", 100*acceptance_rate);
	}

	return samples;
}

BMSSEnergyDistribution::BMSSEnergyDistribution() :
    beta_(0.42), g0_(-0.232), g1_(3.961), e0a_(0.0304), e0b_(0.359), e1a_(-0.0077), e1b_(0.659),
    a0_(0.0033), a1_(0.0079), b0a_(0.0407), b0b_(0.0283), b1a_(-0.312), b1b_(6.124),
    q0_(0.0543), q1_(-0.365), c0a_(-0.069), c0b_(0.488), c1_(-0.117),
    d0a_(-0.398), d0b_(3.955), d1a_(0.012), d1b_(-0.35)
{}

bool
BMSSEnergyDistribution::operator==(const EnergyDistribution &o) const
{
	return dynamic_cast<const BMSSEnergyDistribution*>(&o);
}

OffsetPowerLaw
BMSSEnergyDistribution::GetSpectrum(double depth, double cos_theta, unsigned m, double r) const
{
	// Convert to water-equivalent depth
	double h = (200*I3Units::m/I3Units::km)*0.832 + (depth-(200*I3Units::m/I3Units::km))*0.917;
	double bX = beta_*h/cos_theta;
	double g, eps;
	if (m == 1) {
		g = g0_*log(h) + g1_;
		eps = (e0a_*exp(e0b_*h)/cos_theta + e1a_*h + e1b_)*I3Units::TeV;
	} else {
		m = std::min(m, 4u);
		double a = a0_*h + a1_;
		double b = (b0a_*m + b0b_)*h + (b1a_*m + b1b_);
		double q = q0_*h + q1_;
		g = a*r + b*(1 - 0.5*exp(q*r));
		double c = (c0a_*h + c0b_)*exp(c1_*r);
		double d = (d0a_*h + d0b_)*pow(r, d1a_*h + d1b_);
		eps = (c*acos(cos_theta) + d)*I3Units::TeV;
	}

	return OffsetPowerLaw(g, eps*(1-exp(-bX)), GetMin(), GetMax());
}

double
BMSSEnergyDistribution::GetLog(double depth, double cos_theta,
    unsigned multiplicity, double radius, log_value log_energy) const
{

	return BMSSRadialDistribution().GetLog(depth, cos_theta, multiplicity, radius) +
	    GetSpectrum(depth, cos_theta, multiplicity, radius).GetLog(log_energy);
}

double
BMSSEnergyDistribution::GetMaxRadius() const
{
	return 250*I3Units::m;
}

std::vector<std::pair<double,double> >
BMSSEnergyDistribution::Generate(I3RandomService &rng, double depth,
    double cos_theta, unsigned multiplicity, unsigned samples) const
{
	std::vector<std::pair<double,double> > values;
	values.reserve(samples);
	std::pair<double, double> val;
	for (unsigned i=0; i < samples; i++) {
		val.first = BMSSRadialDistribution().Generate(rng, depth, cos_theta, multiplicity);
		val.second = GetSpectrum(depth, cos_theta, multiplicity, val.first).Generate(rng);
		values.push_back(val);
	}

	return values;
}

OffsetPowerLaw::OffsetPowerLaw() : gamma_(NAN), offset_(NAN), emin_(NAN), emax_(NAN)
{}

OffsetPowerLaw::OffsetPowerLaw(double gamma, double offset, double emin, double emax)
    : gamma_(gamma), offset_(offset), emin_(emin), emax_(emax)
{
	if (gamma <= 0)
		log_fatal("Power law index must be > 0");
	else if (gamma == 1) {
		nmin_ = std::log(emin + offset);
		nmax_ = std::log(emax + offset);
		norm_ = 1./(nmax_ - nmin_);
	} else {
		nmin_ = std::pow(emin + offset, 1-gamma);
		nmax_ = std::pow(emax + offset, 1-gamma);
		norm_ = (1-gamma)/(nmax_ - nmin_);
	}
	lognorm_ = std::log(norm_);
}

bool
OffsetPowerLaw::operator==(const OffsetPowerLaw &other) const
{
	return (gamma_ == other.gamma_ && offset_ == other.offset_
	    && emin_ == other.emin_ && emax_ == other.emax_ );
}

double
OffsetPowerLaw::operator()(double energy) const
{
	if (energy <= emax_ && energy >= emin_)
		return norm_*std::pow(energy + offset_, -gamma_);
	else
		return 0.;
}

double
OffsetPowerLaw::GetLog(double energy) const
{
	if (energy <= emax_ && energy >= emin_)
		return lognorm_ - gamma_*std::log(energy + offset_);
	else
		return -std::numeric_limits<double>::infinity();
}

double
OffsetPowerLaw::GetLog(EnergyDistribution::log_value log_energy) const
{
	return GetLog(std::exp(log_energy));
}

double
OffsetPowerLaw::Generate(I3RandomService &rng) const
{
	return InverseSurvivalFunction(rng.Uniform());
}

double
OffsetPowerLaw::InverseSurvivalFunction(double p) const
{
	if (gamma_ == 1)
		return std::exp((1-p)*(nmax_ - nmin_) + nmin_) - offset_;
	else
		return std::pow((1-p)*(nmax_ - nmin_) + nmin_, 1./(1.-gamma_)) - offset_;
}

template <typename Archive>
void
EnergyDistribution::serialize(Archive &ar __attribute__ ((unused)), unsigned version __attribute__ ((unused)))
{}

template <typename Archive>
void
SplineEnergyDistribution::serialize(Archive &ar, unsigned version)
{
	if (version > 0)
		log_fatal_stream("Version "<<version<<" is from the future");

	ar & make_nvp("EnergyDistribution", base_object<EnergyDistribution>(*this));
	ar & make_nvp("SingleEnergy", singles_);
	ar & make_nvp("BundleEnergy", bundles_);
}

template <typename Archive>
void
BMSSEnergyDistribution::serialize(Archive &ar, unsigned version)
{
	if (version > 0)
		log_fatal_stream("Version "<<version<<" is from the future");

	ar & make_nvp("EnergyDistribution", base_object<EnergyDistribution>(*this));
}

template <typename Archive>
void
OffsetPowerLaw::serialize(Archive &ar, unsigned version)
{
	if (version > 0)
		log_fatal_stream("Version "<<version<<" is from the future");

	ar & make_nvp("Gamma", gamma_);
	ar & make_nvp("Offset", offset_);
	ar & make_nvp("MinEnergy", emin_);
	ar & make_nvp("MaxEnergy", emax_);

	*this = OffsetPowerLaw(gamma_, offset_, emin_, emax_);
}

}

I3_SERIALIZABLE(I3MuonGun::EnergyDistribution);
I3_SERIALIZABLE(I3MuonGun::SplineEnergyDistribution);
I3_SERIALIZABLE(I3MuonGun::BMSSEnergyDistribution);
I3_SERIALIZABLE(I3MuonGun::OffsetPowerLaw);


#include <thread>
#include <vector>
#include <armadillo>
#include "RHF.hpp"
#include "esp.hpp"

//
// Electrostatic Potential (ESP)
// : calculates partial atomic charges that fit
//   the quantum mechanical electrostatic potential on the selected grid points
//

using namespace std;
using namespace libint2;

namespace willow { namespace qcmol {

static unsigned int num_threads;

static void esp_esp (const int thread_id,
		     const vector<Atom>& atoms,
		     const BasisSet& bs,
		     const arma::mat& Dm,
		     const vector<arma::vec3>& grids,
		     arma::vec& results);

ESP::ESP (const vector<Atom>& atoms,
	  const BasisSet& bs,
	  const Integrals& ints,
	  const int qm_chg,
	  const bool l_print,
	  const vector<QAtom>& atoms_Q)
  : RHF (atoms, bs, ints, qm_chg, l_print, atoms_Q)
{

  num_threads = std::thread::hardware_concurrency();

  if (num_threads == 0)
    num_threads = 1;

  const arma::mat Dm = densityMatrix();

  esp_grid (atoms);

  // ---- Thread
  vector<arma::vec>   grid_t(num_threads);
  vector<std::thread> t_esp (num_threads);

  for (auto id = 0; id < num_threads; ++id) {
    t_esp[id] = std::thread (esp_esp, id,
			     std::cref(atoms),
			     std::cref(bs),
			     std::cref(Dm),
			     std::cref(m_grids),
			     std::ref(grid_t[id]));
  }

  // Join
  for (auto id = 0; id < num_threads; ++id) {
    t_esp[id].join();
  }

  m_grids_val = grid_t[0];

  for (auto id = 1; id < num_threads; ++id) {
    m_grids_val += grid_t[id];
  }
  //---- (done: Thread) ---

  m_qf = esp_fit (atoms);

  if (l_print) {
    for (auto i = 0; i < atoms.size(); i++)
      cout << "ESP " << i+1 << "  " << m_qf(i) << endl;
  }

}

void esp_esp (const int thread_id,
	      const vector<Atom>& atoms,
	      const BasisSet& bs,
	      const arma::mat& Dm,
	      const vector<arma::vec3>& grids,
	      arma::vec& result)
{

  const auto nsize   = grids.size();

  result = arma::vec(nsize, arma::fill::zeros);

  const auto nshells = bs.size();
  const auto nbf     = bs.nbf();

  libint2::Engine engine (libint2::Operator::nuclear,
			  bs.max_nprim(),
			  bs.max_l(),
			  0);

  const auto& buf = engine.results();

  const auto shell2bf = bs.shell2bf ();

  arma::mat Vm(nbf,nbf);

  for (auto ig = 0; ig < grids.size(); ++ig) {

    if (ig % num_threads != thread_id) continue;

    const auto xg = grids[ig](0);
    const auto yg = grids[ig](1);
    const auto zg = grids[ig](2);

    // Interaction with electron density
    vector<pair<double,array<double,3>>> q;
    q.push_back ( {1.0, {{xg, yg, zg}}} );

    engine.set_params (q);

    // calc Vm
    Vm.zeros();

    for (auto s1 = 0; s1 != nshells; ++s1) {
      auto bf1 = shell2bf[s1];
      auto nbf1 = bs[s1].size();

      for (auto s2 = 0; s2 != nshells; ++s2) {
	auto bf2 = shell2bf[s2];
	auto nbf2 = bs[s2].size();

	engine.compute (bs[s1], bs[s2]);
	const auto* buf0 = buf[0];

	for (auto ib = 0, ij = 0; ib < nbf1; ib++) {
	  for (auto jb = 0; jb < nbf2; jb++, ij++) {
	    const double val = buf0[ij];
	    Vm(bf1+ib,bf2+jb) = val;
	    Vm(bf2+jb,bf1+ib) = val;
	  }
	} // ib

      } // s2
    }  // s1

    //
    // get electrostatic potential on the grid points
    // -- from electron density
    double gval = 0.0;

    for (auto i = 0; i < nbf; i++)
      for (auto j = 0; j < nbf; j++)
	gval += Dm(i,j)*Vm(i,j);

    // get electrostatic potential on the grid points
    // -- from nuclei
    auto zval = 0.0;

    for (auto ia = 0; ia < atoms.size(); ++ia) {
      auto xij = atoms[ia].x - xg;
      auto yij = atoms[ia].y - yg;
      auto zij = atoms[ia].z - zg;
      auto r2  = xij*xij + yij*yij + zij*zij;
      auto r   = sqrt(r2);
      zval += static_cast<double>(atoms[ia].atomic_number)/r;
    }

    result(ig) = gval + zval;
  }

}

arma::vec ESP::esp_fit (const vector<Atom>& atoms)
{

  // set up matrix of linear coefficients
  auto natoms = atoms.size();
  auto ndim   = natoms+1;

  arma::mat am(ndim,ndim);
  arma::vec bv(ndim);

  am.zeros();
  bv.zeros();

  for (auto i = 0; i < atoms.size(); ++i) {
    auto xi = atoms[i].x;
    auto yi = atoms[i].y;
    auto zi = atoms[i].z;

    for (auto j = i; j < atoms.size(); ++j) {
      auto xj = atoms[j].x;
      auto yj = atoms[j].y;
      auto zj = atoms[j].z;

      auto sum = 0.0;

      for (auto k = 0; k < m_grids.size(); ++k) {
	auto xg = m_grids[k](0);
	auto yg = m_grids[k](1);
	auto zg = m_grids[k](2);

	auto rig2 = (xi-xg)*(xi-xg) + (yi-yg)*(yi-yg) + (zi-zg)*(zi-zg);
	auto rjg2 = (xj-xg)*(xj-xg) + (yj-yg)*(yj-yg) + (zj-zg)*(zj-zg);

	sum += 1.0/sqrt(rig2*rjg2);
      }

      am(i,j) = sum;
      am(j,i) = sum;
    }

    am(i,natoms) = 1.0;
    am(natoms,i) = 1.0;
  }

  // construct column vector b

  for (auto i = 0; i < atoms.size(); ++i) {
    auto xi = atoms[i].x;
    auto yi = atoms[i].y;
    auto zi = atoms[i].z;

    auto sum = 0.0;
    for (auto k = 0; k < m_grids.size(); ++k) {
      auto xg = m_grids[k](0);
      auto yg = m_grids[k](1);
      auto zg = m_grids[k](2);
      auto val = m_grids_val(k);

      auto rig2 = (xi-xg)*(xi-xg) + (yi-yg)*(yi-yg) + (zi-zg)*(zi-zg);

      sum += val/sqrt(rig2);
    }
    bv(i) = sum;
  }
  bv(natoms) = 0.0; // b(natoms) = charge;

  arma::mat am_inv = am.i();

  arma::vec qf(natoms);
  qf.zeros();

  for (auto i = 0; i < natoms; ++i) {
    auto sum = 0.0;

    for (auto j = 0; j < ndim; ++j) {
    //for (auto j = 0; j < atoms.size(); ++j) {
      sum = sum + am_inv(i,j)*bv(j);
    }
    qf(i) = sum;
    //cout << " qf " << sum << endl;
  }

  return qf;

}

void ESP::esp_grid (const vector<Atom>& atoms)
{

  vector<double> radius(11);

  // A
  radius[0] = 0.0; // Ghost
  radius[1] = 0.3; // H
  radius[2] = 1.22;// He
  radius[3] = 1.23;// Li
  radius[4] = 0.89; // Be
  radius[5] = 0.88; // B
  radius[6] = 0.77; // C
  radius[7] = 0.70; // N
  radius[8] = 0.66; // O
  radius[9] = 0.58; // F
  radius[10]= 1.60; // Ne

  for (auto i = 1; i <= 10; i++) {
    radius[i] = (radius[i] + 0.7)*ang2bohr;
  }
  double grd_min_x = atoms[0].x;
  double grd_min_y = atoms[0].y;
  double grd_min_z = atoms[0].z;

  double grd_max_x = atoms[0].x;
  double grd_max_y = atoms[0].y;
  double grd_max_z = atoms[0].z;

  for (auto ia = 1; ia < atoms.size(); ++ia) {

    grd_min_x = min(grd_min_x, atoms[ia].x);
    grd_min_y = min(grd_min_y, atoms[ia].y);
    grd_min_z = min(grd_min_z, atoms[ia].z);

    grd_max_x = max(grd_max_x, atoms[ia].x);
    grd_max_y = max(grd_max_y, atoms[ia].y);
    grd_max_z = max(grd_max_z, atoms[ia].z);

  }
  //
  // calculate the grid size
  //
  // A --> au
  const double rcut = 3.0*ang2bohr;
  const double rcut2= rcut*rcut;
  const double spac = 0.5*ang2bohr;

  const int ngrid_x = int ((grd_max_x - grd_min_x + 2.0*rcut)/spac) + 1;
  const int ngrid_y = int ((grd_max_y - grd_min_y + 2.0*rcut)/spac) + 1;
  const int ngrid_z = int ((grd_max_z - grd_min_z + 2.0*rcut)/spac) + 1;

  const auto small = 1.0e-8;
  arma::vec3 vg;

  for(auto iz = 0; iz <  ngrid_z; ++iz)
    for(auto iy = 0; iy <  ngrid_y; ++iy)
      for(auto ix = 0; ix <  ngrid_x; ++ix) {

	vg.zeros();

	vg(0) = grd_min_x - rcut + ix*spac;
	vg(1) = grd_min_y - rcut + iy*spac;
	vg(2) = grd_min_z - rcut + iz*spac;

	double dmin = rcut2;

	bool lupdate = true;

	for (auto ia = 0; ia < atoms.size(); ++ia) {
	  int iz    = atoms[ia].atomic_number;
	  auto rad2 = radius[iz]*radius[iz];

	  auto dx = vg(0) - atoms[ia].x;
	  auto dy = vg(1) - atoms[ia].y;
	  auto dz = vg(2) - atoms[ia].z;

	  auto d2  = dx*dx + dy*dy + dz*dz;

	  if ( (rad2 - d2) > small) {
	    lupdate = false;
	    break;
	  }

	  if ( (dmin-d2) > small) dmin = d2;

	}

	if (lupdate) {
	  if ( (rcut2 - dmin) > small) {
	    m_grids.push_back (vg);
	  }
	}

      }

}

} } // namespace willow::qcmol


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014, 2015.
// Modifications copyright (c) 2014-2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef TTMATH_STUB
#define TTMATH_STUB

#include <boost/math/constants/constants.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/coordinate_cast.hpp>

#include <ttmath.h>
namespace ttmath
{
    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> sqrt(Big<Exponent, Mantissa> const& v)
    {
        return Sqrt(v);
    }

    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> abs(Big<Exponent, Mantissa> const& v)
    {
        return Abs(v);
    }

    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> ceil(Big<Exponent, Mantissa> const& v)
    {
        return Ceil(v);
    }

    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> floor(Big<Exponent, Mantissa> const& v)
    {
        return Floor(v);
    }

    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> asin(Big<Exponent, Mantissa> const& v)
    {
        return ASin(v);
    }

    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> sin(Big<Exponent, Mantissa> const& v)
    {
        return Sin(v);
    }

    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> cos(Big<Exponent, Mantissa> const& v)
    {
        return Cos(v);
    }

    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> tan(Big<Exponent, Mantissa> const& v)
    {
        return Tan(v);
    }

    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> atan(Big<Exponent, Mantissa> const& v)
    {
        return ATan(v);
    }

    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> acos(Big<Exponent, Mantissa> const& v)
    {
        return ACos(v);
    }

    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> atan2(Big<Exponent, Mantissa> const& y, Big<Exponent, Mantissa> const& x)
    {
        // return ATan2(y, 2); does not (yet) exist in ttmath...

        // See http://en.wikipedia.org/wiki/Atan2

        Big<Exponent, Mantissa> const zero(0);
        Big<Exponent, Mantissa> const two(2);

        if (y == zero)
        {
            // return x >= 0 ? 0 : pi and pi=2*arccos(0)
            return x >= zero ? zero : two * ACos(zero);
        }

        return two * ATan((sqrt(x * x + y * y) - x) / y);
    }

    // needed in order to work with boost::geometry::math::mod
    template <uint Exponent, uint Mantissa>
    inline Big<Exponent, Mantissa> mod(Big<Exponent, Mantissa> const& x,
                                       Big<Exponent, Mantissa> const& y)
    {
        return Mod(x, y);
    }
}

// Specific structure implementing constructor
// (WHICH IS NECESSARY FOR Boost.Geometry because it enables using T() !! )
struct ttmath_big : ttmath::Big<1,4>
{
    ttmath_big(double v = 0)
        : ttmath::Big<1,4>(v)
    {}
    ttmath_big(ttmath::Big<1,4> const& v)
        : ttmath::Big<1,4>(v)
    {}

    // unary operator+() is implemented for completeness
    inline ttmath_big const& operator+() const
    {
        return *this;
    }

    // needed in order to work with boost::geometry::math::abs
    inline ttmath_big operator-() const
    {
        return ttmath::Big<1,4>::operator-();
    }

    /*
    inline operator double() const
    {
        return atof(this->ToString().c_str());
    }

    inline operator int() const
    {
        return atol(ttmath::Round(*this).ToString().c_str());
    }
    */
};

// arithmetic operators for ttmath_big objects, defined as free functions
inline ttmath_big operator+(ttmath_big const& x, ttmath_big const& y)
{
    return static_cast<ttmath::Big<1,4> const&>(x).operator+(y);
}

inline ttmath_big operator-(ttmath_big const& x, ttmath_big const& y)
{
    return static_cast<ttmath::Big<1,4> const&>(x).operator-(y);
}

inline ttmath_big operator*(ttmath_big const& x, ttmath_big const& y)
{
    return static_cast<ttmath::Big<1,4> const&>(x).operator*(y);
}

inline ttmath_big operator/(ttmath_big const& x, ttmath_big const& y)
{
    return static_cast<ttmath::Big<1,4> const&>(x).operator/(y);
}

namespace boost{ namespace geometry { namespace math
{

namespace detail
{
    // Workaround for boost::math::constants::pi:
    // 1) lexical cast -> stack overflow and
    // 2) because it is implemented as a function, generic implementation not possible

    // Partial specialization for ttmath
    template <ttmath::uint Exponent, ttmath::uint Mantissa>
    struct define_half_pi<ttmath::Big<Exponent, Mantissa> >
    {
        static inline ttmath::Big<Exponent, Mantissa> apply()
        {
            static ttmath::Big<Exponent, Mantissa> const half_pi(
                "1.57079632679489661923132169163975144209858469968755291048747229615390820314310449931401741267105853399107404325664115332354692230477529111586267970406424055872514205135096926055277982231147447746519098");
            return half_pi;
        }
    };

    // Partial specialization for ttmath
    template <ttmath::uint Exponent, ttmath::uint Mantissa>
    struct define_pi<ttmath::Big<Exponent, Mantissa> >
    {
        static inline ttmath::Big<Exponent, Mantissa> apply()
        {
            static ttmath::Big<Exponent, Mantissa> const the_pi(
                "3.14159265358979323846264338327950288419716939937510582097494459230781640628620899862803482534211706798214808651328230664709384460955058223172535940812848111745028410270193852110555964462294895493038196");
            return the_pi;
        }
    };

    // Partial specialization for ttmath
    template <ttmath::uint Exponent, ttmath::uint Mantissa>
    struct define_two_pi<ttmath::Big<Exponent, Mantissa> >
    {
        static inline ttmath::Big<Exponent, Mantissa> apply()
        {
            static ttmath::Big<Exponent, Mantissa> const two_pi(
                "6.28318530717958647692528676655900576839433879875021164194988918461563281257241799725606965068423413596429617302656461329418768921910116446345071881625696223490056820540387704221111928924589790986076392");
            return two_pi;
        }
    };

    template <>
    struct define_half_pi<ttmath_big>
            : public define_half_pi<ttmath::Big<1,4> >
    {};

    template <>
    struct define_pi<ttmath_big>
            : public define_pi<ttmath::Big<1,4> >
    {};

    template <>
    struct define_two_pi<ttmath_big>
            : public define_two_pi<ttmath::Big<1,4> >
    {};

    template <ttmath::uint Exponent, ttmath::uint Mantissa>
    struct equals_with_epsilon<ttmath::Big<Exponent, Mantissa>, false>
    {
        static inline bool apply(ttmath::Big<Exponent, Mantissa> const& a, ttmath::Big<Exponent, Mantissa> const& b)
        {
            // See implementation in util/math.hpp
            // But here borrow the tolerance for double, to avoid exact comparison
            ttmath::Big<Exponent, Mantissa> const epsilon = std::numeric_limits<double>::epsilon();
            return ttmath::Abs(a - b) <= epsilon * ttmath::Abs(a);
        }
    };

    template <>
    struct equals_with_epsilon<ttmath_big, false>
            : public equals_with_epsilon<ttmath::Big<1, 4>, false>
    {};

} // detail

} // ttmath

namespace detail
{

template <ttmath::uint Exponent, ttmath::uint Mantissa>
struct coordinate_cast<ttmath::Big<Exponent, Mantissa> >
{
    static inline ttmath::Big<Exponent, Mantissa> apply(std::string const& source)
    {
        return ttmath::Big<Exponent, Mantissa> (source);
    }
};

template <>
struct coordinate_cast<ttmath_big>
{
    static inline ttmath_big apply(std::string const& source)
    {
        return ttmath_big(source);
    }
};

} // namespace detail

}} // boost::geometry

// Support for boost::numeric_cast to int and to double (necessary for SVG-mapper)
namespace boost { namespace numeric
{

template
<
    ttmath::uint Exponent, ttmath::uint Mantissa,
    typename Traits,
    typename OverflowHandler,
    typename Float2IntRounder,
    typename RawConverter,
    typename UserRangeChecker
>
struct converter<int, ttmath::Big<Exponent, Mantissa>, Traits, OverflowHandler, Float2IntRounder, RawConverter, UserRangeChecker>
{
    static inline int convert(ttmath::Big<Exponent, Mantissa> arg)
    {
        int v;
        arg.ToInt(v);
        return v;
    }
};

template
<
    ttmath::uint Exponent, ttmath::uint Mantissa,
    typename Traits,
    typename OverflowHandler,
    typename Float2IntRounder,
    typename RawConverter,
    typename UserRangeChecker
>
struct converter<double, ttmath::Big<Exponent, Mantissa>, Traits, OverflowHandler, Float2IntRounder, RawConverter, UserRangeChecker>
{
    static inline double convert(ttmath::Big<Exponent, Mantissa> arg)
    {
        double v;
        arg.ToDouble(v);
        return v;
    }
};

template
<
    typename Traits,
    typename OverflowHandler,
    typename Float2IntRounder,
    typename RawConverter,
    typename UserRangeChecker
>
struct converter<int, ttmath_big, Traits, OverflowHandler, Float2IntRounder, RawConverter, UserRangeChecker>
{
    static inline int convert(ttmath_big arg)
    {
        int v;
        arg.ToInt(v);
        return v;
    }
};

template
<
    typename Traits,
    typename OverflowHandler,
    typename Float2IntRounder,
    typename RawConverter,
    typename UserRangeChecker
>
struct converter<double, ttmath_big, Traits, OverflowHandler, Float2IntRounder, RawConverter, UserRangeChecker>
{
    static inline double convert(ttmath_big arg)
    {
        double v;
        arg.ToDouble(v);
        return v;
    }
};

}}

#endif


/*
 * img_align.cpp
 *
 *  Created on: Aug 22, 2012
 *      Author: cforster
 */

#include <vector>
#include <string>
#include <stdint.h>
#include <stdio.h>
#include <iostream>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <opencv2/opencv.hpp>
#include <vikit/math_utils.h>
#include <vikit/vision.h>
#include <vikit/pinhole_camera.h>
#include <vikit/nlls_solver.h>
#include <vikit/performance_monitor.h>
#include <vikit/img_align.h>
#include <sophus/se3.h>

namespace vk {

/*******************************************************************************
 * Forward Compositional
 */
ForwardCompositionalSE3::
ForwardCompositionalSE3( vector<PinholeCamera>& cam_pyr,
                         vector<cv::Mat>& depth_pyr,
                         vector<cv::Mat>& img_pyr,
                         vector<cv::Mat>& tpl_pyr,
                         vector<cv::Mat>& img_pyr_dx,
                         vector<cv::Mat>& img_pyr_dy,
                         SE3& init_model,
                         int n_levels,
                         int n_iter,
                         float res_thresh,
                         bool display,
                         Method method,
                         int test_id) :
      cam_pyr_(cam_pyr),
      depth_pyr_(depth_pyr),
      img_pyr_(img_pyr),
      tpl_pyr_(tpl_pyr),
      img_pyr_dx_(img_pyr_dx),
      img_pyr_dy_(img_pyr_dy),
      display_(display),
      log_(test_id < 0),
      res_thresh_(res_thresh)
{
  n_iter_ = n_iter;
  method_ = method;

  // Init Performance Monitor
#if 0
  if(log_)
  {
    permon_.init("forward", ros::package::getPath("rpl_examples") + "/trace/img_align/data",
                 test_id, true);
    permon_.addLog("iter");
    permon_.addLog("level");
    permon_.addLog("mu");
    permon_.addLog("chi2");
    permon_.addLog("trials");
  }
#endif

  runOptimization(init_model);

}

ForwardCompositionalSE3::
ForwardCompositionalSE3( vector<PinholeCamera>& cam_pyr,
                         vector<cv::Mat>& depth_pyr,
                         vector<cv::Mat>& img_pyr,
                         vector<cv::Mat>& tpl_pyr,
                         vector<cv::Mat>& img_pyr_dx,
                         vector<cv::Mat>& img_pyr_dy,
                         int n_levels,
                         int n_iter,
                         float res_thresh,
                         bool display,
                         Method method,
                         int test_id) :
      cam_pyr_(cam_pyr),
      depth_pyr_(depth_pyr),
      img_pyr_(img_pyr),
      tpl_pyr_(tpl_pyr),
      img_pyr_dx_(img_pyr_dx),
      img_pyr_dy_(img_pyr_dy),
      display_(display),
      log_(test_id < 0),
      res_thresh_(res_thresh)
{
  n_iter_ = n_iter;
  method_ = method;

  // Init Performance Monitor
#if 0
  if(log_)
  {
    permon_.init("forward", ros::package::getPath("rpl_examples") + "/trace/img_align/data",
                 test_id, true);
    permon_.addLog("iter");
    permon_.addLog("level");
    permon_.addLog("mu");
    permon_.addLog("chi2");
    permon_.addLog("trials");
  }
#endif

}

void ForwardCompositionalSE3::
runOptimization(SE3& model, int levelBegin, int levelEnd)
{
  if(levelBegin < 0 || levelBegin > n_levels_-1)
    levelBegin = n_levels_-1;
  if(levelEnd < 0)
    levelEnd = 1;

  // Perform Pyramidal optimization
  for(level_=levelBegin; level_>=levelEnd; --level_)
  {
    mu_ = 0.1;
    cout << endl << "PYRAMID LEVEL " << level_
         << endl << "---------------" << endl;
    optimize(model);
  }
}

double ForwardCompositionalSE3::
computeResiduals (const SE3& model, bool linearize_system, bool compute_weight_scale)
{
  // Warp the image such that it aligns with the template image
  double chi2 = 0;
  size_t n_pixels = 0;

  if(linearize_system)
    resimg_ = cv::Mat(tpl_pyr_[level_].size(), CV_32F, cv::Scalar(1));

  for( int v=0; v<depth_pyr_[level_].rows; ++v )
  {
    for( int u=0; u<depth_pyr_[level_].cols; ++u )
    {
      // compute pixel location in new img
      cv::Vec3f cv_float3 = depth_pyr_[level_].at<cv::Vec3f>(v,u);
      Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]);
      Vector3d xyz_img(model*xyz_tpl);
      Vector2f uv_img_pyr = cam_pyr_[level_].world2cam(xyz_img).cast<float>(); // apply cam model
      if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast<int>(), 2) )
      {
        // compare image values
        float intensity_tpl =
            tpl_pyr_[level_].at<float>(v,u);
        float intensity_img =
            interpolateMat_32f(img_pyr_[level_], uv_img_pyr[0], uv_img_pyr[1]);

        // compute residual (opposite to 2d case because of other jacobian)
        float res = intensity_tpl-intensity_img;

        // robustification
        if(res > res_thresh_)  res = res_thresh_;
        if(res < -res_thresh_) res = -res_thresh_;
        chi2 += res*res;
        n_pixels++;

        if(linearize_system)
        {
          // get gradient of warped image (~gradient at warped position)
          float dx = 0.5*interpolateMat_32f(img_pyr_dx_[level_], uv_img_pyr[0], uv_img_pyr[1]);
          float dy = 0.5*interpolateMat_32f(img_pyr_dy_[level_], uv_img_pyr[0], uv_img_pyr[1]);

          // evaluate jacobian
          Matrix<double,2,6> frame_jac;
          frameJac_xyz2uv(xyz_img, cam_pyr_[level_].fx(), frame_jac);

          // compute steppest descent images
          Vector6d J = dx*frame_jac.row(0) + dy*frame_jac.row(1);

          // compute Hessian and
          H_ += J*J.transpose();
          Jres_ += J*res;

          resimg_.at<float>(v,u) = -res;
        }
      }
    }
  }
  chi2 /= n_pixels;
  return chi2;
}

int ForwardCompositionalSE3::
solve()
{
  x_ = H_.ldlt().solve(-Jres_);
  if((bool) std::isnan((double) x_[0]))
    return 0;
  return 1;
}

void ForwardCompositionalSE3::
update(const ModelType& old_model,  ModelType& new_model)
{
  new_model = SE3::exp(x_)*(old_model);
}

void ForwardCompositionalSE3::
startIteration()
{
#if 0
  if(log_)
    permon_.newMeasurement();
#endif
}

void ForwardCompositionalSE3::
finishIteration()
{
#if 0
  if(log_)
  {
    permon_.log("iter", iter_);
    permon_.log("level", level_);
    permon_.log("mu", mu_);
    permon_.log("chi2", chi2_);
    permon_.log("trials", n_trials_);
  }
#endif

  if(display_)
  {
    cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE);
    cv::imshow("residuals", resimg_*3);
    cv::waitKey(0);
  }
}

/*******************************************************************************
 * Efficient Second Order Minimization (ESM)
 */
SecondOrderMinimisationSE3::
SecondOrderMinimisationSE3( vector<PinholeCamera>& cam_pyr,
                            vector<cv::Mat>& depth_pyr,
                            vector<cv::Mat>& img_pyr,
                            vector<cv::Mat>& tpl_pyr,
                            vector<cv::Mat>& img_pyr_dx,
                            vector<cv::Mat>& img_pyr_dy,
                            vector<cv::Mat>& tpl_pyr_dx,
                            vector<cv::Mat>& tpl_pyr_dy,
                            SE3& init_model,
                            int n_levels,
                            int n_iter,
                            float res_thresh,
                            bool display,
                            Method method,
                            int test_id) :
      cam_pyr_(cam_pyr),
      depth_pyr_(depth_pyr),
      img_pyr_(img_pyr),
      tpl_pyr_(tpl_pyr),
      img_pyr_dx_(img_pyr_dx),
      img_pyr_dy_(img_pyr_dy),
      tpl_pyr_dx_(tpl_pyr_dx),
      tpl_pyr_dy_(tpl_pyr_dy),
      display_(display),
      log_(test_id < 0),
      res_thresh_(res_thresh)
{
  n_iter_ = n_iter;
  method_ = method;
  verbose_ = false;

#if 0
  if(log_)
  {
    // Init Performance Monitor
    permon_.init("esm", ros::package::getPath("rpl_examples") + "/trace/img_align/data",
                 test_id, true);
    permon_.addLog("iter");
    permon_.addLog("level");
    permon_.addLog("mu");
    permon_.addLog("chi2");
    permon_.addLog("trials");
  }
#endif

  // perform pyramidal optimization
  for(level_=n_levels-1; level_>2; --level_)
  //level_ = n_levels-1;
  {
    // Optimize
    mu_ = 0.01f;
    if(display_)
    {
      cout << endl << "PYRAMID LEVEL " << level_
           << endl << "patch-width = " << img_pyr_[level_].cols
           << endl << "---------------" << endl;
    }
    optimize(init_model);
  }
}

double SecondOrderMinimisationSE3::
computeResiduals (const SE3& model, bool linearize_system, bool compute_weight_scale)
{
  // Warp the image such that it aligns with the template image
  double chi2 = 0;
  size_t n_pixels = 0;

  // TODO: to improve access speed, use a pointer and increment every iteration

  // Compute Warp
  cv::Mat mask = cv::Mat_<bool>(tpl_pyr_[level_].rows, tpl_pyr_[level_].cols, false);
  cv::Mat img_warped = cv::Mat_<float>(tpl_pyr_[level_].rows, tpl_pyr_[level_].cols, 1.0);

  for( int v=0; v<depth_pyr_[level_].rows; ++v )
  {
    for( int u=0; u<depth_pyr_[level_].cols; ++u )
    {
      // compute pixel location in new img
      cv::Vec3f cv_float3 = depth_pyr_[level_].at<cv::Vec3f>(v,u);
      Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]);
      Vector3d xyz_img(model*xyz_tpl);
      Vector2f uv_img_pyr = cam_pyr_[level_].world2cam(xyz_img).cast<float>(); // apply cam model
      if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast<int>(), 1) )
      {
        img_warped.at<float>(v,u) = interpolateMat_32f(img_pyr_[level_], uv_img_pyr[0], uv_img_pyr[1]);

        if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast<int>(), 2) )
          mask.at<bool>(v,u) = true;
      }
    }
  }

  // Compute Warp derivative
  cv::Mat img_warped_dx, img_warped_dy;
  cv::Sobel(img_warped, img_warped_dx, CV_32F, 1, 0, 1);
  cv::Sobel(img_warped, img_warped_dy, CV_32F, 0, 1, 1);

  // Compute Jacobian
  if(linearize_system)
    resimg_ = cv::Mat_<float>(tpl_pyr_[level_].size(), 1.0);

  for( int v=0; v<depth_pyr_[level_].rows; ++v )
  {
    for( int u=0; u<depth_pyr_[level_].cols; ++u )
    {
      if (mask.at<bool>(v,u))
      {
        // compare image values
        float intensity_tpl = tpl_pyr_[level_].at<float>(v,u);
        float intensity_img = img_warped.at<float>(v,u);

        // compute residual  (opposite to 2d case because of other jacobian)
        float res = intensity_tpl-intensity_img;

        // robustification
        if(res > res_thresh_)  res = res_thresh_;
        if(res < -res_thresh_) res = -res_thresh_;
        chi2 += res*res;
        n_pixels++;

        if(linearize_system)
        {
          // 0.25 because we have two 0.5 factors. First from adding the two gradients
          // and the second when we compute the sobel mask
          float dx = 0.25*(tpl_pyr_dx_[level_].at<float>(v,u) + img_warped_dx.at<float>(v,u));
          float dy = 0.25*(tpl_pyr_dy_[level_].at<float>(v,u) + img_warped_dy.at<float>(v,u));

          // evaluate jacobian
          cv::Vec3f cv_float3 = depth_pyr_[level_].at<cv::Vec3f>(v,u);
          Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]);
          Vector3d xyz_img(model*xyz_tpl);
          Matrix<double,2,6> frame_jac;
          frameJac_xyz2uv(xyz_tpl, cam_pyr_[level_].fx(), frame_jac);

          // compute steppest descent images
          Vector6d J = dx*frame_jac.row(0) + dy*frame_jac.row(1);

          // compute Hessian
          H_ += J*J.transpose();
          Jres_ += J*res;
          resimg_.at<float>(v,u) = res;
        }
      }
    }
  }
  chi2 /= n_pixels;
  return chi2;
}

int SecondOrderMinimisationSE3::
solve()
{
  x_ = H_.ldlt().solve(-Jres_);
  if((bool) std::isnan((double) x_[0]))
    return 0;
  return 1;
}

void SecondOrderMinimisationSE3::
update(const ModelType& old_model,  ModelType& new_model)
{
  new_model = SE3::exp(x_)*old_model;
}

void SecondOrderMinimisationSE3::
startIteration()
{
#if 0
  if(log_)
    permon_.newMeasurement();
#endif
}

void SecondOrderMinimisationSE3::
finishIteration()
{
#if 0
  if(log_)
  {
    permon_.log("iter", iter_);
    permon_.log("level", level_);
    permon_.log("mu", mu_);
    permon_.log("chi2", chi2_);
    permon_.log("trials", n_trials_);
  }
#endif

  if(display_)
  {
    cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE);
    cv::imshow("residuals", resimg_*3);
    cv::waitKey(0);
  }
}

} // end namespace vk


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
//
#ifndef BOOST_GRAPH_MST_PRIM_HPP
#define BOOST_GRAPH_MST_PRIM_HPP

#include <functional>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

namespace boost {

  namespace detail {
    // this should be somewhere else in boost...
    template <class U, class V> struct _project2nd {
      V operator()(U, V v) const { return v; }
    };
  }

  namespace detail {

    // This is Prim's algorithm to calculate the Minimum Spanning Tree
    // for an undirected graph with weighted edges.

    template <class Graph, class P, class T, class R, class Weight>
    inline void
    prim_mst_impl(const Graph& G,
                  typename graph_traits<Graph>::vertex_descriptor s,
                  const bgl_named_params<P,T,R>& params,
                  Weight)
    {
      typedef typename property_traits<Weight>::value_type W;
      std::less<W> compare;
      detail::_project2nd<W,W> combine;
      dijkstra_shortest_paths(G, s, params.distance_compare(compare).
                              distance_combine(combine));
    }
  } // namespace detail

  template <class VertexListGraph, class DijkstraVisitor,
            class PredecessorMap, class DistanceMap,
            class WeightMap, class IndexMap>
  inline void
  prim_minimum_spanning_tree
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     DijkstraVisitor vis)
  {
    typedef typename property_traits<WeightMap>::value_type W;
    std::less<W> compare;
    detail::_project2nd<W,W> combine;
    dijkstra_shortest_paths(g, s, predecessor, distance, weight, index_map,
                            compare, combine, std::numeric_limits<W>::max(), 0,
                            vis);
  }

  template <class VertexListGraph, class PredecessorMap,
            class P, class T, class R>
  inline void prim_minimum_spanning_tree
    (const VertexListGraph& g,
     PredecessorMap p_map,
     const bgl_named_params<P,T,R>& params)
  {
    detail::prim_mst_impl
      (g,
       choose_param(get_param(params, root_vertex_t()), *vertices(g).first),
       params.predecessor_map(p_map),
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight));
  }

  template <class VertexListGraph, class PredecessorMap>
  inline void prim_minimum_spanning_tree
    (const VertexListGraph& g, PredecessorMap p_map)
  {
    detail::prim_mst_impl
      (g, *vertices(g).first, predecessor_map(p_map).
       weight_map(get(edge_weight, g)),
       get(edge_weight, g));
  }

} // namespace boost

#endif // BOOST_GRAPH_MST_PRIM_HPP


//
//=======================================================================
// Copyright 2007 Stanford University
// Authors: David Gleich
//
// 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_CORE_NUMBERS_HPP
#define BOOST_GRAPH_CORE_NUMBERS_HPP

#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/iterator/reverse_iterator.hpp>
#include <boost/concept/assert.hpp>

/*
 * core_numbers
 *
 * Requirement: IncidenceGraph
 */

// History
//
// 30 July 2007
// Added visitors to the implementation
//
// 8 February 2008
// Fixed headers and missing typename

namespace boost {

    // A linear time O(m) algorithm to compute the indegree core number
    // of a graph for unweighted graphs.
    //
    // and a O((n+m) log n) algorithm to compute the in-edge-weight core
    // numbers of a weighted graph.
    //
    // The linear algorithm comes from:
    // Vladimir Batagelj and Matjaz Zaversnik, "An O(m) Algorithm for Cores
    // Decomposition of Networks."  Sept. 1 2002.

    template <typename Visitor, typename Graph>
    struct CoreNumbersVisitorConcept {
        void constraints()
        {
            BOOST_CONCEPT_ASSERT(( CopyConstructibleConcept<Visitor> ));
            vis.examine_vertex(u,g);
            vis.finish_vertex(u,g);
            vis.examine_edge(e,g);
        }
        Visitor vis;
        Graph g;
        typename graph_traits<Graph>::vertex_descriptor u;
        typename graph_traits<Graph>::edge_descriptor e;
    };

    template <class Visitors = null_visitor>
    class core_numbers_visitor : public bfs_visitor<Visitors> {
        public:
        core_numbers_visitor() {}
        core_numbers_visitor(Visitors vis)
            : bfs_visitor<Visitors>(vis) {}

        private:
        template <class Vertex, class Graph>
        void initialize_vertex(Vertex, Graph&) {}

        template <class Vertex, class Graph>
        void discover_vertex(Vertex , Graph&) {}

        template <class Vertex, class Graph>
        void gray_target(Vertex, Graph&) {}

        template <class Vertex, class Graph>
        void black_target(Vertex, Graph&) {}

        template <class Edge, class Graph>
        void tree_edge(Edge, Graph&) {}

        template <class Edge, class Graph>
        void non_tree_edge(Edge, Graph&) {}
    };

    template <class Visitors>
    core_numbers_visitor<Visitors> make_core_numbers_visitor(Visitors vis)
    { return core_numbers_visitor<Visitors>(vis); }

    typedef core_numbers_visitor<> default_core_numbers_visitor;

    namespace detail {

        // implement a constant_property_map to simplify compute_in_degree
        // for the weighted and unweighted case
        // this is based on dummy property map
        template <typename ValueType>
        class constant_value_property_map
          : public boost::put_get_helper<ValueType,
              constant_value_property_map<ValueType>  >
        {
        public:
            typedef void key_type;
            typedef ValueType value_type;
            typedef const ValueType& reference;
            typedef boost::readable_property_map_tag category;
            inline constant_value_property_map(ValueType cc) : c(cc) { }
            inline constant_value_property_map(const constant_value_property_map<ValueType>& x)
              : c(x.c) { }
            template <class Vertex>
            inline reference operator[](Vertex) const { return c; }
        protected:
            ValueType c;
        };

        // the core numbers start as the indegree or inweight.  This function
        // will initialize these values
        template <typename Graph, typename CoreMap, typename EdgeWeightMap>
        void compute_in_degree_map(Graph& g, CoreMap d, EdgeWeightMap wm)
        {
            typename graph_traits<Graph>::vertex_iterator vi,vi_end;
            typename graph_traits<Graph>::out_edge_iterator ei,ei_end;
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                put(d,*vi,0);
            }
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                for (boost::tie(ei,ei_end) = out_edges(*vi,g); ei!=ei_end; ++ei) {
                    put(d,target(*ei,g),get(d,target(*ei,g))+get(wm,*ei));
                }
            }
        }

        // the version for weighted graphs is a little different
        template <typename Graph, typename CoreMap,
            typename EdgeWeightMap, typename MutableQueue,
            typename Visitor>
        typename property_traits<CoreMap>::value_type
        core_numbers_impl(Graph& g, CoreMap c, EdgeWeightMap wm,
            MutableQueue& Q, Visitor vis)
        {
            typename property_traits<CoreMap>::value_type v_cn = 0;
            typedef typename graph_traits<Graph>::vertex_descriptor vertex;
            while (!Q.empty())
            {
                // remove v from the Q, and then decrease the core numbers
                // of its successors
                vertex v = Q.top();
                vis.examine_vertex(v,g);
                Q.pop();
                v_cn = get(c,v);
                typename graph_traits<Graph>::out_edge_iterator oi,oi_end;
                for (boost::tie(oi,oi_end) = out_edges(v,g); oi!=oi_end; ++oi) {
                    vis.examine_edge(*oi,g);
                    vertex u = target(*oi,g);
                    // if c[u] > c[v], then u is still in the graph,
                    if (get(c,u) > v_cn) {
                        // remove the edge
                        put(c,u,get(c,u)-get(wm,*oi));
                        if (Q.contains(u))
                          Q.update(u);
                    }
                }
                vis.finish_vertex(v,g);
            }
            return (v_cn);
        }

        template <typename Graph, typename CoreMap, typename EdgeWeightMap,
            typename IndexMap, typename CoreNumVisitor>
        typename property_traits<CoreMap>::value_type
        core_numbers_dispatch(Graph&g, CoreMap c, EdgeWeightMap wm,
            IndexMap im, CoreNumVisitor vis)
        {
            typedef typename property_traits<CoreMap>::value_type D;
            typedef std::less<D> Cmp;
            // build the mutable queue
            typedef typename graph_traits<Graph>::vertex_descriptor vertex;
            std::vector<std::size_t> index_in_heap_data(num_vertices(g));
            typedef iterator_property_map<std::vector<std::size_t>::iterator, IndexMap>
              index_in_heap_map_type;
            index_in_heap_map_type index_in_heap_map(index_in_heap_data.begin(), im);
            typedef d_ary_heap_indirect<vertex, 4, index_in_heap_map_type, CoreMap, Cmp> MutableQueue;
            MutableQueue Q(c, index_in_heap_map, Cmp());
            typename graph_traits<Graph>::vertex_iterator vi,vi_end;
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                Q.push(*vi);
            }
            return core_numbers_impl(g, c, wm, Q, vis);
        }

        // the version for the unweighted case
        // for this functions CoreMap must be initialized
        // with the in degree of each vertex
        template <typename Graph, typename CoreMap, typename PositionMap,
            typename Visitor>
        typename property_traits<CoreMap>::value_type
        core_numbers_impl(Graph& g, CoreMap c, PositionMap pos, Visitor vis)
        {
            typedef typename graph_traits<Graph>::vertices_size_type size_type;
            typedef typename graph_traits<Graph>::degree_size_type degree_type;
            typedef typename graph_traits<Graph>::vertex_descriptor vertex;
            typename graph_traits<Graph>::vertex_iterator vi,vi_end;

            // store the vertex core numbers
            typename property_traits<CoreMap>::value_type v_cn = 0;

            // compute the maximum degree (degrees are in the coremap)
            typename graph_traits<Graph>::degree_size_type max_deg = 0;
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                max_deg = (std::max<typename graph_traits<Graph>::degree_size_type>)(max_deg, get(c,*vi));
            }

            // store the vertices in bins by their degree
            // allocate two extra locations to ease boundary cases
            std::vector<size_type> bin(max_deg+2);
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                ++bin[get(c,*vi)];
            }

            // this loop sets bin[d] to the starting position of vertices
            // with degree d in the vert array for the bucket sort
            size_type cur_pos = 0;
            for (degree_type cur_deg = 0; cur_deg < max_deg+2; ++cur_deg) {
                degree_type tmp = bin[cur_deg];
                bin[cur_deg] = cur_pos;
                cur_pos += tmp;
            }

            // perform the bucket sort with pos and vert so that
            // pos[0] is the vertex of smallest degree
            std::vector<vertex> vert(num_vertices(g));
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                vertex v=*vi;
                size_type p=bin[get(c,v)];
                put(pos,v,p);
                vert[p]=v;
                ++bin[get(c,v)];
            }
            // we ``abused'' bin while placing the vertices, now,
            // we need to restore it
            std::copy(boost::make_reverse_iterator(bin.end()-2),
                boost::make_reverse_iterator(bin.begin()),
                boost::make_reverse_iterator(bin.end()-1));
            // now simulate removing the vertices
            for (size_type i=0; i < num_vertices(g); ++i) {
                vertex v = vert[i];
                vis.examine_vertex(v,g);
                v_cn = get(c,v);
                typename graph_traits<Graph>::out_edge_iterator oi,oi_end;
                for (boost::tie(oi,oi_end) = out_edges(v,g); oi!=oi_end; ++oi) {
                    vis.examine_edge(*oi,g);
                    vertex u = target(*oi,g);
                    // if c[u] > c[v], then u is still in the graph,
                    if (get(c,u) > v_cn) {
                        degree_type deg_u = get(c,u);
                        degree_type pos_u = get(pos,u);
                        // w is the first vertex with the same degree as u
                        // (this is the resort operation!)
                        degree_type pos_w = bin[deg_u];
                        vertex w = vert[pos_w];
                        if (u!=v) {
                            // swap u and w
                            put(pos,u,pos_w);
                            put(pos,w,pos_u);
                            vert[pos_w] = u;
                            vert[pos_u] = w;
                        }
                        // now, the vertices array is sorted assuming
                        // we perform the following step
                        // start the set of vertices with degree of u
                        // one into the future (this now points at vertex
                        // w which we swapped with u).
                        ++bin[deg_u];
                        // we are removing v from the graph, so u's degree
                        // decreases
                        put(c,u,get(c,u)-1);
                    }
                }
                vis.finish_vertex(v,g);
            }
            return v_cn;
        }

    } // namespace detail

    // non-named parameter version for the unweighted case
    template <typename Graph, typename CoreMap, typename CoreNumVisitor>
    typename property_traits<CoreMap>::value_type
    core_numbers(Graph& g, CoreMap c, CoreNumVisitor vis)
    {
        typedef typename graph_traits<Graph>::vertices_size_type size_type;
        detail::compute_in_degree_map(g,c,
            detail::constant_value_property_map<
                typename property_traits<CoreMap>::value_type>(1) );
        return detail::core_numbers_impl(g,c,
            make_iterator_property_map(
                std::vector<size_type>(num_vertices(g)).begin(),get(vertex_index, g)),
            vis
        );
    }

    // non-named paramter version for the unweighted case
    template <typename Graph, typename CoreMap>
    typename property_traits<CoreMap>::value_type
    core_numbers(Graph& g, CoreMap c)
    {
        return core_numbers(g, c, make_core_numbers_visitor(null_visitor()));
    }

    // non-named parameter version for the weighted case
    template <typename Graph, typename CoreMap, typename EdgeWeightMap,
        typename VertexIndexMap, typename CoreNumVisitor>
    typename property_traits<CoreMap>::value_type
    core_numbers(Graph& g, CoreMap c, EdgeWeightMap wm, VertexIndexMap vim,
        CoreNumVisitor vis)
    {
        typedef typename graph_traits<Graph>::vertices_size_type size_type;
        detail::compute_in_degree_map(g,c,wm);
        return detail::core_numbers_dispatch(g,c,wm,vim,vis);
    }

    // non-named parameter version for the weighted case
//    template <typename Graph, typename CoreMap, typename EdgeWeightMap>
//    typename property_traits<CoreMap>::value_type
//    core_numbers(Graph& g, CoreMap c, EdgeWeightMap wm)
//    {
//        typedef typename graph_traits<Graph>::vertices_size_type size_type;
//        detail::compute_in_degree_map(g,c,wm);
//        return detail::core_numbers_dispatch(g,c,wm,get(vertex_index,g),
//            make_core_numbers_visitor(null_visitor()));
//    }

    template <typename Graph, typename CoreMap>
    typename property_traits<CoreMap>::value_type
    weighted_core_numbers(Graph& g, CoreMap c)
    {
        return weighted_core_numbers(
            g,c, make_core_numbers_visitor(null_visitor())
        );
    }

    template <typename Graph, typename CoreMap, typename CoreNumVisitor>
    typename property_traits<CoreMap>::value_type
    weighted_core_numbers(Graph& g, CoreMap c, CoreNumVisitor vis)
    { return core_numbers(g,c,get(edge_weight,g),get(vertex_index,g),vis); }

} // namespace boost

#endif // BOOST_GRAPH_CORE_NUMBERS_HPP


/*******************************************************************************
 *
 * An implementation of discrete domains based on Patricia trees.
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#pragma once

#include <crab/common/types.hpp>
#include <crab/domains/patricia_trees.hpp>
#include <crab/domains/separate_domains.hpp>

#include <boost/range.hpp>

namespace ikos {

  template< typename Element >
  class discrete_domain: public writeable {

  private:
    typedef patricia_tree_set< Element > ptset_t;

  public:
    typedef discrete_domain< Element > discrete_domain_t;
    typedef typename ptset_t::iterator iterator;

  private:
    bool _is_top;
    ptset_t _set;

  private:
    discrete_domain(bool is_top): _is_top(is_top) { }

    discrete_domain(ptset_t set): _is_top(false), _set(set) { }

  public:
    static discrete_domain_t bottom() {
      return discrete_domain_t(false);
    }

    static discrete_domain_t top() {
      return discrete_domain_t(true);
    }

  public:
    discrete_domain(): _is_top(true) { }

    discrete_domain(const discrete_domain_t& other):
        writeable(), _is_top(other._is_top), _set(other._set) { }

    discrete_domain(Element s): _is_top(false), _set(s) { }

    template<typename Iterator>
    discrete_domain(Iterator eIt, Iterator eEt)
        : _is_top(false) {
      for (auto e: boost::make_iterator_range (eIt, eEt)) {
        this->_set += e;
      }
    }

    bool is_top() {
      return this->_is_top;
    }

    bool is_bottom() {
      return (!this->_is_top && this->_set.empty());
    }

    bool operator<=(discrete_domain_t other) {
      return other._is_top || (!this->_is_top && this->_set <= other._set);
    }

    bool operator==(discrete_domain_t other) {
      return (this->_is_top && other._is_top) || (this->_set == other._set);
    }

    void operator|=(discrete_domain_t other) {
      *this = *this | other;
    }

    discrete_domain_t operator|(discrete_domain_t other) {
      if (this->_is_top || other._is_top) {
        return discrete_domain_t(true);
      } else {
        return discrete_domain_t(this->_set | other._set);
      }
    }

    discrete_domain_t operator&(discrete_domain_t other) {
      if (this->is_bottom() || other.is_bottom()) {
        return discrete_domain_t(false);
      } else if (this->_is_top) {
        return other;
      } else if (other._is_top) {
        return *this;
      } else {
        return discrete_domain_t(this->_set & other._set);
      }
    }

    discrete_domain_t operator||(discrete_domain_t other) {
      return this->operator|(other);
    }

    discrete_domain_t operator&&(discrete_domain_t other) {
      return this->operator&(other);
    }

    discrete_domain_t& operator+=(Element s) {
      if (!this->_is_top) {
        this->_set += s;
      }
      return *this;
    }

    template<typename Range>
    discrete_domain_t& operator+=(Range es) {
      if (!this->_is_top)
        for (auto e: es)
          this->_set += e;
      return *this;
    }

    discrete_domain_t operator+(Element s) {
      discrete_domain_t r(*this);
      r.operator+=(s);
      return r;
    }

    template<typename Range>
    discrete_domain_t operator+(Range es) {
      discrete_domain_t r(*this);
      r.operator+=(es);
      return r;
    }

    discrete_domain_t& operator-=(Element s) {
      if (!this->_is_top) {
        this->_set -= s;
      }
      return *this;
    }

    template<typename Range>
    discrete_domain_t& operator-=(Range es) {
      if (!this->_is_top)
        for (auto e: es)
          this->_set -= e;
      return *this;
    }

    discrete_domain_t operator-(Element s) {
      discrete_domain_t r(*this);
      r.operator-=(s);
      return r;
    }

    template<typename Range>
    discrete_domain_t operator-(Range es) {
      discrete_domain_t r(*this);
      r.operator-=(es);
      return r;
    }

    std::size_t size() {
      if (this->_is_top) {
        CRAB_ERROR("Size for discrete domain TOP is undefined");
      } else {
        return this->_set.size();
      }
    }

    iterator begin() {
      if (this->_is_top) {
        CRAB_ERROR("Iterator for discrete domain TOP is undefined");
      } else {
        return this->_set.begin();
      }
    }

    iterator end() {
      if (this->_is_top) {
        CRAB_ERROR("Iterator for discrete domain TOP is undefined");
      } else {
        return this->_set.end();
      }
    }

    void write(crab::crab_os& o) {
      if (this->_is_top) {
	o << "{...}";
      } else if (this->_set.empty()) {
	o << "_|_";
      } else {
	o << this->_set;
      }
    }

  }; // class discrete_domain

} // namespace ikos


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2013 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2013 Bruno Lalande, Paris, France.
// Copyright (c) 2013 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_ZOOM_TO_ROBUST_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ZOOM_TO_ROBUST_HPP

#include <cstddef>

#include <boost/type_traits.hpp>

#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/expand.hpp>
#include <boost/geometry/algorithms/detail/recalculate.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace zoom_to_robust
{

template <typename Box, std::size_t Dimension>
struct get_max_size
{
    static inline typename coordinate_type<Box>::type apply(Box const& box)
    {
        typename coordinate_type<Box>::type s
            = geometry::math::abs(geometry::get<1, Dimension>(box) - geometry::get<0, Dimension>(box));

        return (std::max)(s, get_max_size<Box, Dimension - 1>::apply(box));
    }
};

template <typename Box>
struct get_max_size<Box, 0>
{
    static inline typename coordinate_type<Box>::type apply(Box const& box)
    {
        return geometry::math::abs(geometry::get<1, 0>(box) - geometry::get<0, 0>(box));
    }
};

template <typename FpPoint, typename IntPoint, typename CalculationType>
struct rescale_strategy
{
    typedef typename geometry::coordinate_type<IntPoint>::type output_ct;

    rescale_strategy(FpPoint const& fp_min, IntPoint const& int_min, CalculationType const& the_factor)
        : m_fp_min(fp_min)
        , m_int_min(int_min)
        , m_multiplier(the_factor)
    {
    }

    template <std::size_t Dimension, typename Value>
    inline output_ct apply(Value const& value) const
    {
        // a + (v-b)*f
        CalculationType const a = static_cast<CalculationType>(get<Dimension>(m_int_min));
        CalculationType const b = static_cast<CalculationType>(get<Dimension>(m_fp_min));
        CalculationType const result = a + (value - b) * m_multiplier;
        return static_cast<output_ct>(result);
    }

    FpPoint const& m_fp_min;
    IntPoint const& m_int_min;
    CalculationType m_multiplier;
};

}} // namespace detail::zoom_to_robust
#endif // DOXYGEN_NO_DETAIL

template <typename Box>
inline typename coordinate_type<Box>::type get_max_size(Box const& box)
{
    return detail::zoom_to_robust::get_max_size<Box, dimension<Box>::value - 1>::apply(box);
}

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename CoordinateType, typename IsFloatingPoint>
struct robust_type
{
};

template <typename CoordinateType>
struct robust_type<CoordinateType, boost::false_type>
{
    typedef CoordinateType type;
};

template <typename CoordinateType>
struct robust_type<CoordinateType, boost::true_type>
{
    typedef int type; // long long?
};

template <typename IsFloatingPoint>
struct zoom_to_robust
{
    template
    <
        typename Geometry1, typename Geometry2, typename Geometry3,
        typename Geometry4, typename Geometry5, typename Geometry6,
        typename GeometryOut
    >
    static inline void apply(Geometry1 const& g1, Geometry2 const& g2, Geometry3 const& g3,
          Geometry4 const& g4, Geometry5 const& g5, Geometry6 const& g6,
          GeometryOut& og1, GeometryOut& og2, GeometryOut& og3,
          GeometryOut& og4, GeometryOut& og5, GeometryOut& og6)
    {
        // By default, just convert these geometries (until now: points or maybe segments)
        geometry::convert(g1, og1);
        geometry::convert(g2, og2);
        geometry::convert(g3, og3);
        geometry::convert(g4, og4);
        geometry::convert(g5, og5);
        geometry::convert(g6, og6);
    }
};

template <>
struct zoom_to_robust<boost::true_type>
{
    template
    <
        typename Geometry1, typename Geometry2, typename Geometry3,
        typename Geometry4, typename Geometry5, typename Geometry6,
        typename GeometryOut
    >
    static inline void apply(Geometry1 const& g1, Geometry2 const& g2, Geometry3 const& g3,
          Geometry4 const& g4, Geometry5 const& g5, Geometry6 const& g6,
          GeometryOut& og1, GeometryOut& og2, GeometryOut& og3,
          GeometryOut& og4, GeometryOut& og5, GeometryOut& og6)
    {
        typedef typename point_type<Geometry1>::type point1_type;

        // Get the envelop of inputs
        model::box<point1_type> env;
        geometry::assign_inverse(env);
        geometry::expand(env, g1);
        geometry::expand(env, g2);
        geometry::expand(env, g3);
        geometry::expand(env, g4);
        geometry::expand(env, g5);
        geometry::expand(env, g6);

        // Scale this to integer-range
        typename geometry::coordinate_type<point1_type>::type diff = get_max_size(env);
        double range = 1000000000.0; // Define a large range to get precise integer coordinates
        double factor = double(int(range / double(diff)));

        // Assign input/output minimal points
        point1_type min_point1;
        detail::assign_point_from_index<0>(env, min_point1);

        typedef typename point_type<GeometryOut>::type point2_type;
        point2_type min_point2;
        assign_values(min_point2, int(-range/2.0), int(-range/2.0));

        detail::zoom_to_robust::rescale_strategy<point1_type, point2_type, double> strategy(min_point1, min_point2, factor);

        geometry::recalculate(og1, g1, strategy);
        geometry::recalculate(og2, g2, strategy);
        geometry::recalculate(og3, g3, strategy);
        geometry::recalculate(og4, g4, strategy);
        geometry::recalculate(og5, g5, strategy);
        geometry::recalculate(og6, g6, strategy);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

template <typename CoordinateType>
struct robust_type
{
    typedef typename dispatch::robust_type
        <
            CoordinateType,
            typename boost::is_floating_point<CoordinateType>::type
        >::type type;
};

template <typename Geometry1, typename Geometry2>
inline void zoom_to_robust(Geometry1 const& g1a, Geometry1 const& g1b, Geometry2& g2a, Geometry2& g2b)
{
    typedef typename point_type<Geometry1>::type point1_type;
    typedef typename point_type<Geometry2>::type point2_type;

    point1_type min_point1;
    point2_type min_point2;

    // Get the envelop of inputs
    model::box<point1_type> env;
    envelope(g1a, env);
    expand(env, g1b);

    // Scale this to integer-range
    typename coordinate_type<point1_type>::type diff = get_max_size(env);
    double range = 1000000000.0; // Define a large range to get precise integer coordinates
    double factor = range / diff;

    // Assign input/output minimal points
    detail::assign_point_from_index<0>(env, min_point1);
    assign_values(min_point2, int(-range/2.0), int(-range/2.0));

    detail::zoom_to_robust::rescale_strategy<point1_type, point2_type, double> strategy(min_point1, min_point2, factor);
    recalculate(g2a, g1a, strategy);
    recalculate(g2b, g1b, strategy);
}

template
<
    typename Geometry1, typename Geometry2, typename Geometry3,
    typename Geometry4, typename Geometry5, typename Geometry6,
    typename GeometryOut
>
void zoom_to_robust(Geometry1 const& g1, Geometry2 const& g2, Geometry3 const& g3,
          Geometry4 const& g4, Geometry5 const& g5, Geometry6 const& g6,
          GeometryOut& og1, GeometryOut& og2, GeometryOut& og3,
          GeometryOut& og4, GeometryOut& og5, GeometryOut& og6)
{
    // Make FP robust (so dispatch to true), so float and double
    // Other types as int, boost::rational, or ttmath are considered to be already robust
    dispatch::zoom_to_robust
        <
            typename boost::is_floating_point
                <
                    typename geometry::coordinate_type<Geometry1>::type
                >::type
        >::apply(g1, g2, g3, g4, g5, g6, og1, og2, og3, og4, og5, og6);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ZOOM_TO_ROBUST_HPP


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Purpose:  Implementation of the nzmg (New Zealand Map Grid) projection.
//           Very loosely based upon DMA code by Bradford W. Drew
// Author:   Gerald Evenden
// Copyright (c) 1995, Gerald Evenden

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_NZMG_HPP
#define BOOST_GEOMETRY_PROJECTIONS_NZMG_HPP

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

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_zpoly1.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct nzmg {}; // New Zealand Map Grid

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace nzmg
    {

            static const double epsilon = 1e-10;
            static const int Nbf = 5;
            static const int Ntpsi = 9;
            static const int Ntphi = 8;

            template <typename T>
            inline T sec5_to_rad() { return 0.4848136811095359935899141023; }
            template <typename T>
            inline T rad_to_sec5() { return 2.062648062470963551564733573; }

            template <typename T>
            inline const pj_complex<T> * bf()
            {
                static const pj_complex<T> result[] = {
                    {.7557853228,    0.0},
                    {.249204646,    .003371507},
                    {-.001541739,    .041058560},
                    {-.10162907,    .01727609},
                    {-.26623489,    -.36249218},
                    {-.6870983,    -1.1651967}
                };
                return result;
            }

            template <typename T>
            inline const T * tphi()
            {
                static const T result[] = { 1.5627014243, .5185406398, -.03333098,
                                            -.1052906,   -.0368594,     .007317,
                                             .01220,      .00394,      -.0013 };
                return result;
            }
            template <typename T>
            inline const T * tpsi()
            {
                static const T result[] = { .6399175073, -.1358797613, .063294409, -.02526853, .0117879,
                                           -.0055161,     .0026906,   -.001333,     .00067,   -.00034 };
                return result;
            }

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_nzmg_ellipsoid
                : public base_t_fi<base_nzmg_ellipsoid<T, Parameters>, T, Parameters>
            {
                inline base_nzmg_ellipsoid(const Parameters& par)
                    : base_t_fi<base_nzmg_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T rad_to_sec5 = nzmg::rad_to_sec5<T>();

                    pj_complex<T> p;
                    const T * C;
                    int i;

                    lp_lat = (lp_lat - this->m_par.phi0) * rad_to_sec5;
                    for (p.r = *(C = tpsi<T>() + (i = Ntpsi)); i ; --i)
                        p.r = *--C + lp_lat * p.r;
                    p.r *= lp_lat;
                    p.i = lp_lon;
                    p = pj_zpoly1(p, bf<T>(), Nbf);
                    xy_x = p.i;
                    xy_y = p.r;
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T sec5_to_rad = nzmg::sec5_to_rad<T>();

                    int nn, i;
                    pj_complex<T> p, f, fp, dp;
                    T den;
                    const T* C;

                    p.r = xy_y;
                    p.i = xy_x;
                    for (nn = 20; nn ;--nn) {
                        f = pj_zpolyd1(p, bf<T>(), Nbf, &fp);
                        f.r -= xy_y;
                        f.i -= xy_x;
                        den = fp.r * fp.r + fp.i * fp.i;
                        p.r += dp.r = -(f.r * fp.r + f.i * fp.i) / den;
                        p.i += dp.i = -(f.i * fp.r - f.r * fp.i) / den;
                        if ((fabs(dp.r) + fabs(dp.i)) <= epsilon)
                            break;
                    }
                    if (nn) {
                        lp_lon = p.i;
                        for (lp_lat = *(C = tphi<T>() + (i = Ntphi)); i ; --i)
                            lp_lat = *--C + p.r * lp_lat;
                        lp_lat = this->m_par.phi0 + p.r * lp_lat * sec5_to_rad;
                    } else
                        lp_lon = lp_lat = HUGE_VAL;
                }

                static inline std::string get_name()
                {
                    return "nzmg_ellipsoid";
                }

            };

            // New Zealand Map Grid
            template <typename Parameters>
            inline void setup_nzmg(Parameters& par)
            {
                typedef typename Parameters::type calc_t;
                static const calc_t d2r = geometry::math::d2r<calc_t>();

                /* force to International major axis */
                par.ra = 1. / (par.a = 6378388.0);
                par.lam0 = 173. * d2r;
                par.phi0 = -41. * d2r;
                par.x0 = 2510000.;
                par.y0 = 6023150.;
            }

    }} // namespace detail::nzmg
    #endif // doxygen

    /*!
        \brief New Zealand Map Grid projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Fixed Earth
        \par Example
        \image html ex_nzmg.gif
    */
    template <typename T, typename Parameters>
    struct nzmg_ellipsoid : public detail::nzmg::base_nzmg_ellipsoid<T, Parameters>
    {
        inline nzmg_ellipsoid(const Parameters& par) : detail::nzmg::base_nzmg_ellipsoid<T, Parameters>(par)
        {
            detail::nzmg::setup_nzmg(this->m_par);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::nzmg, nzmg_ellipsoid, nzmg_ellipsoid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class nzmg_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<nzmg_ellipsoid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void nzmg_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("nzmg", new nzmg_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_NZMG_HPP