// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "PointwiseFunctions/AnalyticSolutions/GeneralRelativity/Tov.hpp"

// Need Boost MultiArray because it is used internally by ODEINT
#include "DataStructures/BoostMultiArray.hpp"  // IWYU pragma: keep

#include <algorithm>
#include <array>
#include <boost/numeric/odeint.hpp>  // IWYU pragma: keep
#include <cmath>
#include <cstddef>
#include <functional>
#include <ostream>
#include <pup.h>
#include <string>
#include <type_traits>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/EquationOfState.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/ContainerHelpers.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/ErrorHandling/Error.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeWithValue.hpp"

namespace gr::Solutions {
std::ostream& operator<<(std::ostream& os, const TovCoordinates coords) {
  switch (coords) {
    case TovCoordinates::Schwarzschild:
      return os << "Schwarzschild";
    case TovCoordinates::Isotropic:
      return os << "Isotropic";
    default:
      ERROR("Unknown TovCoordinates");
  }
}
}  // namespace gr::Solutions

template <>
gr::Solutions::TovCoordinates
Options::create_from_yaml<gr::Solutions::TovCoordinates>::create<void>(
    const Options::Option& options) {
  const auto type_read = options.parse_as<std::string>();
  if ("Schwarzschild" == type_read) {
    return gr::Solutions::TovCoordinates::Schwarzschild;
  } else if ("Isotropic" == type_read) {
    return gr::Solutions::TovCoordinates::Isotropic;
  }
  PARSE_ERROR(options.context(),
              "Failed to convert '"
                  << type_read
                  << "' to gr::Solutions::TovCoordinates. Must be "
                     "'Schwarzschild' or 'Isotropic'.");
}

namespace gr::Solutions {
namespace {

// In Schwarzschild coords we integrate u=r^2 and v=m/r (2 vars), and in
// isotropic coords we also integrate w=ln(psi) (3 vars).
template <TovCoordinates CoordSystem>
using TovVars =
    std::conditional_t<CoordSystem == TovCoordinates::Schwarzschild,
                       std::array<double, 2>, std::array<double, 3>>;

template <TovCoordinates CoordSystem>
void lindblom_rhs(
    const gsl::not_null<TovVars<CoordSystem>*> dvars,
    const TovVars<CoordSystem>& vars, const double log_enthalpy,
    const EquationsOfState::EquationOfState<true, 1>& equation_of_state) {
  const double& radius_squared = vars[0];
  const double& mass_over_radius = vars[1];
  double& d_radius_squared = (*dvars)[0];
  double& d_mass_over_radius = (*dvars)[1];
  const double specific_enthalpy = std::exp(log_enthalpy);
  const double rest_mass_density =
      get(equation_of_state.rest_mass_density_from_enthalpy(
          Scalar<double>{specific_enthalpy}));
  const double pressure = get(equation_of_state.pressure_from_density(
      Scalar<double>{rest_mass_density}));
  const double energy_density =
      specific_enthalpy * rest_mass_density - pressure;

  // At the center of the star: (u,v) = (0,0)
  if (UNLIKELY((radius_squared == 0.0) and (mass_over_radius == 0.0))) {
    d_radius_squared = -3.0 / (2.0 * M_PI * (energy_density + 3.0 * pressure));
    d_mass_over_radius =
        -2.0 * energy_density / (energy_density + 3.0 * pressure);
    if constexpr (CoordSystem == TovCoordinates::Isotropic) {
      double& d_log_conformal_factor = (*dvars)[2];
      d_log_conformal_factor = -0.25 * d_mass_over_radius;
    }
  } else {
    const double one_minus_two_m_over_r = 1.0 - 2.0 * mass_over_radius;
    const double denominator =
        4.0 * M_PI * radius_squared * pressure + mass_over_radius;
    const double common_factor = one_minus_two_m_over_r / denominator;
    d_radius_squared = -2.0 * radius_squared * common_factor;
    d_mass_over_radius =
        -(4.0 * M_PI * radius_squared * energy_density - mass_over_radius) *
        common_factor;
    if constexpr (CoordSystem == TovCoordinates::Isotropic) {
      double& d_log_conformal_factor = (*dvars)[2];
      d_log_conformal_factor = sqrt(one_minus_two_m_over_r) /
                               (1.0 + sqrt(one_minus_two_m_over_r)) *
                               mass_over_radius / denominator;
    }
  }
}

template <TovCoordinates CoordSystem>
class IntegralObserver {
 public:
  void operator()(const TovVars<CoordSystem>& vars,
                  const double current_log_enthalpy) {
    radius.push_back(std::sqrt(vars[0]));
    mass_over_radius.push_back(vars[1]);
    if constexpr (CoordSystem == TovCoordinates::Isotropic) {
      conformal_factor.push_back(exp(vars[2]));
    }
    log_enthalpy.push_back(current_log_enthalpy);
  }
  std::vector<double> radius;
  std::vector<double> mass_over_radius;
  std::vector<double> conformal_factor;
  std::vector<double> log_enthalpy;
};

}  // namespace

template <TovCoordinates CoordSystem>
void TovSolution::integrate(
    const EquationsOfState::EquationOfState<true, 1>& equation_of_state,
    const double central_mass_density,
    const double log_enthalpy_at_outer_radius, const double absolute_tolerance,
    const double relative_tolerance) {
  using Vars = TovVars<CoordSystem>;
  Vars vars{};
  // Initial integration variables at the center of the star
  vars[0] = 0.;  // u = r^2 = 0
  vars[1] = 0.;  // v = m / r = 0
  if constexpr (CoordSystem == TovCoordinates::Isotropic) {
    vars[2] = 0.;  // w = ln(psi) = 0 (rescaled later)
  }
  Vars dvars{};
  const double central_log_enthalpy =
      std::log(get(equation_of_state.specific_enthalpy_from_density(
          Scalar<double>{central_mass_density})));
  lindblom_rhs<CoordSystem>(make_not_null(&dvars), vars, central_log_enthalpy,
                            equation_of_state);
  double initial_step =
      -std::min(std::abs(1.0 / dvars[0]), std::abs(1.0 / dvars[1]));
  if constexpr (CoordSystem == TovCoordinates::Isotropic) {
    initial_step = -std::min(std::abs(initial_step), std::abs(1.0 / dvars[2]));
  }
  using StateDopri5 = boost::numeric::odeint::runge_kutta_dopri5<Vars>;
  boost::numeric::odeint::dense_output_runge_kutta<
      boost::numeric::odeint::controlled_runge_kutta<StateDopri5>>
      dopri5 = make_dense_output(absolute_tolerance, relative_tolerance,
                                 StateDopri5{});
  IntegralObserver<CoordSystem> observer{};
  boost::numeric::odeint::integrate_adaptive(
      dopri5,
      [&equation_of_state](const Vars& local_vars, Vars& local_dvars,
                           const double local_enthalpy) {
        return lindblom_rhs<CoordSystem>(&local_dvars, local_vars,
                                         local_enthalpy, equation_of_state);
      },
      vars, central_log_enthalpy, log_enthalpy_at_outer_radius, initial_step,
      std::ref(observer));
  outer_radius_ = observer.radius.back();
  const double total_mass_over_radius = observer.mass_over_radius.back();
  total_mass_ = total_mass_over_radius * outer_radius_;
  injection_energy_ = sqrt(1. - 2. * total_mass_ / outer_radius_);

  if constexpr (CoordSystem == TovCoordinates::Isotropic) {
    // Transform outer radius to isotropic
    const double outer_areal_radius = outer_radius_;
    outer_radius_ = 0.5 * (outer_areal_radius - total_mass_ +
                           sqrt(square(outer_areal_radius) -
                                2. * total_mass_ * outer_areal_radius));

    // Match conformal factor to exterior solution
    const double outer_conformal_factor =
        1.0 + 0.5 * total_mass_ / outer_radius_;
    const double matching_constant =
        outer_conformal_factor / observer.conformal_factor.back();
    const size_t num_points = observer.radius.size();
    for (size_t i = 0; i < num_points; ++i) {
      observer.conformal_factor[i] *= matching_constant;
      // Transform observed radius to isotropic, so we use the isotropic radius
      // for all interpolations below
      observer.radius[i] /= square(observer.conformal_factor[i]);
    }
    conformal_factor_interpolant_ = intrp::BarycentricRational(
        observer.radius, observer.conformal_factor, 5);
  }

  mass_over_radius_interpolant_ =
      intrp::BarycentricRational(observer.radius, observer.mass_over_radius, 5);
  // log_enthalpy(radius) is almost linear so an interpolant of order 3
  // maximizes precision
  log_enthalpy_interpolant_ =
      intrp::BarycentricRational(observer.radius, observer.log_enthalpy, 3);
}

TovSolution::TovSolution(
    const EquationsOfState::EquationOfState<true, 1>& equation_of_state,
    const double central_mass_density, const TovCoordinates coordinate_system,
    const double log_enthalpy_at_outer_radius, const double absolute_tolerance,
    const double relative_tolerance)
    : coordinate_system_(coordinate_system) {
  if (coordinate_system_ == TovCoordinates::Schwarzschild) {
    integrate<TovCoordinates::Schwarzschild>(
        equation_of_state, central_mass_density, log_enthalpy_at_outer_radius,
        absolute_tolerance, relative_tolerance);
  } else {
    integrate<TovCoordinates::Isotropic>(
        equation_of_state, central_mass_density, log_enthalpy_at_outer_radius,
        absolute_tolerance, relative_tolerance);
  }
}

template <typename DataType>
DataType TovSolution::mass_over_radius(const DataType& r) const {
  // Possible optimization: Support DataVector in intrp::BarycentricRational
  auto result = make_with_value<DataType>(r, 0.);
  for (size_t i = 0; i < get_size(r); ++i) {
    ASSERT(
        get_element(r, i) >= 0.0 and get_element(r, i) <= outer_radius_,
        "Invalid radius: " << r << " not in [0.0, " << outer_radius_ << "]\n");
    get_element(result, i) = mass_over_radius_interpolant_(get_element(r, i));
  }
  return result;
}

template <typename DataType>
DataType TovSolution::log_specific_enthalpy(const DataType& r) const {
  // Possible optimization: Support DataVector in intrp::BarycentricRational
  auto result = make_with_value<DataType>(r, 0.);
  for (size_t i = 0; i < get_size(r); ++i) {
    ASSERT(
        get_element(r, i) >= 0.0 and get_element(r, i) <= outer_radius_,
        "Invalid radius: " << r << " not in [0.0, " << outer_radius_ << "]\n");
    get_element(result, i) = log_enthalpy_interpolant_(get_element(r, i));
  }
  return result;
}

template <typename DataType>
DataType TovSolution::conformal_factor(const DataType& r) const {
  ASSERT(coordinate_system_ == TovCoordinates::Isotropic,
         "The conformal factor is computed only for isotropic coordinates.");
  // Possible optimization: Support DataVector in intrp::BarycentricRational
  auto result = make_with_value<DataType>(r, 0.);
  for (size_t i = 0; i < get_size(r); ++i) {
    ASSERT(
        get_element(r, i) >= 0.0 and get_element(r, i) <= outer_radius_,
        "Invalid radius: " << r << " not in [0.0, " << outer_radius_ << "]\n");
    get_element(result, i) = conformal_factor_interpolant_(get_element(r, i));
  }
  return result;
}

void TovSolution::pup(PUP::er& p) {  // NOLINT
  p | coordinate_system_;
  p | outer_radius_;
  p | total_mass_;
  p | injection_energy_;
  p | mass_over_radius_interpolant_;
  p | log_enthalpy_interpolant_;
  p | conformal_factor_interpolant_;
}

#define DTYPE(data) BOOST_PP_TUPLE_ELEM(0, data)

#define INSTANTIATE(_, data)                                                \
  template DTYPE(data) TovSolution::mass_over_radius(const DTYPE(data) & r) \
      const;                                                                \
  template DTYPE(data)                                                      \
      TovSolution::log_specific_enthalpy(const DTYPE(data) & r) const;      \
  template DTYPE(data) TovSolution::conformal_factor(const DTYPE(data) & r) \
      const;

GENERATE_INSTANTIATIONS(INSTANTIATE, (double, DataVector))

#undef DTYPE

}  // namespace gr::Solutions


//
// Copyright (c) 2009, Markus Rickert
// 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.
//
// 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 <algorithm>
#include <stack>
#include <boost/graph/graphviz.hpp>
#include <rl/math/Quaternion.h>
#include <rl/math/Rotation.h>
#include <rl/math/Spatial.h>

#include "Exception.h"
#include "JacobianInverseKinematics.h"
#include "Kinematic.h"
#include "Prismatic.h"
#include "Revolute.h"

namespace rl
{
	namespace mdl
	{
		Kinematic::Kinematic() :
			Metric(),
			invJ(),
			J(),
			Jdqd()
		{
		}

		Kinematic::~Kinematic()
		{
		}

		bool
		Kinematic::calculateInversePosition(const ::rl::math::Transform& x, const ::std::size_t& leaf, const ::rl::math::Real& delta, const ::rl::math::Real& epsilon, const ::std::size_t& iterations)
		{
			JacobianInverseKinematics ik(this);
			ik.setDelta(delta);
			ik.setEpsilon(epsilon);
			ik.setIterations(iterations);
			ik.addGoal(x, leaf);
			return ik.solve();
		}

		void
		Kinematic::calculateJacobian(const bool& inWorldFrame)
		{
			this->calculateJacobian(this->J, inWorldFrame);
		}

		void
		Kinematic::calculateJacobian(::rl::math::Matrix& J, const bool& inWorldFrame)
		{
			assert(J.rows() == this->getOperationalDof() * 6);
			assert(J.cols() == this->getDof());

			::rl::math::Vector tmp(this->getDof());

			for (::std::size_t i = 0; i < this->getDof(); ++i)
			{
				for (::std::size_t j = 0; j < this->getDof(); ++j)
				{
					tmp(j) = i == j ? 1 : 0;
				}

				this->setVelocity(tmp);
				this->forwardVelocity();

				for (::std::size_t j = 0; j < this->getOperationalDof(); ++j)
				{
					if (inWorldFrame)
					{
						J.block(j * 6, i, 3, 1) = this->getOperationalPosition(j).linear() * this->getOperationalVelocity(j).linear();
						J.block(j * 6 + 3, i, 3, 1) = this->getOperationalPosition(j).linear() * this->getOperationalVelocity(j).angular();
					}
					else
					{
						J.block(j * 6, i, 3, 1) = this->getOperationalVelocity(j).linear();
						J.block(j * 6 + 3, i, 3, 1) = this->getOperationalVelocity(j).angular();
					}
				}
			}
		}

		void
		Kinematic::calculateJacobianDerivative(const bool& inWorldFrame)
		{
			this->calculateJacobianDerivative(this->Jdqd, inWorldFrame);
		}

		void
		Kinematic::calculateJacobianDerivative(::rl::math::Vector& Jdqd, const bool& inWorldFrame)
		{
			::rl::math::Vector tmp = ::rl::math::Vector::Zero(this->getDof());

			this->setAcceleration(tmp);
			this->forwardVelocity();
			this->forwardAcceleration();

			for (::std::size_t j = 0; j < this->getOperationalDof(); ++j)
			{
				if (inWorldFrame)
				{
					::rl::math::Matrix33 wR = this->getOperationalVelocity(j).angular().cross33() * this->getOperationalPosition(j).linear();
					Jdqd.segment(j * 6, 3) = this->getOperationalPosition(j).linear() * this->getOperationalAcceleration(j).linear() + wR * this->getOperationalVelocity(j).linear();
					Jdqd.segment(j * 6 + 3, 3) = this->getOperationalPosition(j).linear() * this->getOperationalAcceleration(j).angular() + wR * this->getOperationalVelocity(j).angular();
				}
				else
				{
					Jdqd.segment(j * 6, 3) = this->getOperationalAcceleration(j).linear();
					Jdqd.segment(j * 6 + 3, 3) = this->getOperationalAcceleration(j).angular();
				}
			}
		}

		void
		Kinematic::calculateJacobianInverse(const ::rl::math::Real& lambda, const bool& doSvd)
		{
			this->calculateJacobianInverse(this->J, this->invJ, lambda, doSvd);
		}

		void
		Kinematic::calculateJacobianInverse(const ::rl::math::Matrix& J, ::rl::math::Matrix& invJ, const ::rl::math::Real& lambda, const bool& doSvd) const
		{
			if (doSvd)
			{
				invJ.setZero();

				::Eigen::JacobiSVD<::rl::math::Matrix> svd(J, ::Eigen::ComputeFullU | ::Eigen::ComputeFullV);

				::rl::math::Real wMin = svd.singularValues().minCoeff();
				::rl::math::Real lambdaSqr = wMin < static_cast<::rl::math::Real>(1.0e-9) ? (1 - ::std::pow((wMin / static_cast<::rl::math::Real>(1.0e-9)), 2)) * ::std::pow(lambda, 2) : 0;

				for (::std::ptrdiff_t i = 0; i < svd.nonzeroSingularValues(); ++i)
				{
					invJ.noalias() += (
						svd.singularValues()(i) / (::std::pow(svd.singularValues()(i), 2) + lambdaSqr) *
						svd.matrixV().col(i) * svd.matrixU().col(i).transpose()
					);
				}
			}
			else
			{
				invJ = J.transpose() * (
					J * J.transpose() + ::std::pow(lambda, 2) *
					::rl::math::Matrix::Identity(this->getOperationalDof() * 6, this->getOperationalDof() * 6)
				).inverse();
			}
		}

		::rl::math::Real
		Kinematic::calculateManipulabilityMeasure() const
		{
			return calculateManipulabilityMeasure(this->J);
		}

		::rl::math::Real
		Kinematic::calculateManipulabilityMeasure(const ::rl::math::Matrix& J) const
		{
			return ::std::sqrt((J * J.transpose()).determinant());
		}

		void
		Kinematic::forwardAcceleration()
		{
			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->forwardAcceleration();
			}
		}

		void
		Kinematic::forwardPosition()
		{
			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->forwardPosition();
			}
		}

		void
		Kinematic::forwardVelocity()
		{
			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->forwardVelocity();
			}
		}

		const ::rl::math::Matrix&
		Kinematic::getJacobian() const
		{
			return this->J;
		}

		const ::rl::math::Vector&
		Kinematic::getJacobianDerivative() const
		{
			return this->Jdqd;
		}

		const ::rl::math::Matrix&
		Kinematic::getJacobianInverse() const
		{
			return this->invJ;
		}

		bool
		Kinematic::isSingular() const
		{
			return this->isSingular(this->J);
		}

		bool
		Kinematic::isSingular(const ::rl::math::Matrix& J) const
		{
			::Eigen::JacobiSVD<::rl::math::Matrix> svd(J);
			return (::std::abs(svd.singularValues()(svd.singularValues().size() - 1)) > ::std::numeric_limits<::rl::math::Real>::epsilon()) ? false : true;
		}

		void
		Kinematic::update()
		{
			Metric::update();

			this->invJ = ::rl::math::Matrix::Identity(this->getDof(), 6 * this->getOperationalDof());
			this->J = ::rl::math::Matrix::Identity(6 * this->getOperationalDof(), this->getDof());
			this->Jdqd = ::rl::math::Vector::Zero(6 * this->getOperationalDof());
		}
	}
}


// This file is part of MeshUtility, a collection of python mesh processing
// utilities.
//
// Copyright (C) 2021 Zishun Liu <liuzishun@gmail.com>
//
// This Source Code Form is subject to the terms of the 3-Clause BSD License.
// If a copy of the BSD-3-Clause was not distributed with this file, You can
// obtain one at https://opensource.org/licenses/BSD-3-Clause.
#include <iostream>

#include <Eigen/Sparse>

#include "isocurve.h"

isocurve::isocurve(const Eigen::MatrixXd& V,
                   const Eigen::MatrixXi& F,
                   const Eigen::VectorXd& field)
{
    if (V.rows() != field.size())
        throw std::invalid_argument( "V.rows() != field.size()" );
    if (F.cols() != 3)
        throw std::invalid_argument( "Not a triangular mesh" );

    V_ = V;
    F_ = F;
    field_ = field;

    mesh_.clean();
    std::vector<EigenTriMesh::VertexHandle> vhandles(V.rows());
    for (Eigen::Index i = 0; i < V.rows(); ++i)
    {
        //vhandles[i] = mesh_.add_vertex(EigenTriMesh::Point(V(i,0), V(i,1), V(i,2)));
        vhandles[i] = mesh_.add_vertex(V.row(i));
    }

    std::vector<EigenTriMesh::VertexHandle> face_vhandles(3);
    for (Eigen::Index i = 0; i < F.rows(); ++i)
    {
        face_vhandles[0] = vhandles[F(i,0)];
        face_vhandles[1] = vhandles[F(i,1)];
        face_vhandles[2] = vhandles[F(i,2)];
        mesh_.add_face(face_vhandles);
    }
}

isocurve::~isocurve(){}

void isocurve::extract(const double val, const double eqlTol,
                       Eigen::MatrixXd& _pts,
                       Eigen::MatrixXi& _on_edge,
                       Eigen::VectorXd& _ratio,
                       std::vector<std::deque<int>>& _curve_all) const
{
    std::vector<Eigen::Vector3d> pts;
    std::vector<Eigen::Vector2i> on_edge;
    std::vector<double> ratio;
    std::vector<Eigen::Vector2i> curve_segments;
    std::vector<CurveSegmentDir> curve_segments_dir;

    Eigen::VectorXi idxOnVerts(mesh_.n_vertices());
    Eigen::VectorXi idxOnEdges(mesh_.n_edges());
    idxOnVerts.fill(-1);
    idxOnEdges.fill(-1);

    collect_pts(idxOnVerts, idxOnEdges, pts, on_edge, ratio, val, eqlTol);

    for (Eigen::Index i = 0; i < F_.rows(); ++i) {
        Eigen::VectorXi fv = F_.row(i);
        int cnt = 0;
        for (int j = 0; j < 3; ++j) {
            if (idxOnVerts[fv[j]] >= 0)
                cnt++;
        }
        switch (cnt) {
        case 3:
            std::cout << "[Warning!] Not handled: a whole triangle lays on the iso value." << std::endl;
            break;
        case 2:
            extract_on_face_through2(static_cast<unsigned int>(i), val,
                                     idxOnVerts, idxOnEdges,
                                     curve_segments, curve_segments_dir);
            break;
        case 1:
            extract_on_face_through1(static_cast<unsigned int>(i), val,
                                     idxOnVerts, idxOnEdges,
                                     curve_segments, curve_segments_dir);
            break;
        case 0:
            extract_on_face_through0(static_cast<unsigned int>(i), val,
                                     idxOnVerts, idxOnEdges,
                                     curve_segments, curve_segments_dir);
            break;
        default:
            break;
        }
    }

    // collect results
    _pts.resize(pts.size(), 3);
    for (size_t i = 0; i < pts.size(); ++i) {
        _pts.row(i) = pts[i];
    }
    _on_edge.resize(on_edge.size(), 2);
    for (size_t i = 0; i < on_edge.size(); ++i) {
        _on_edge.row(i) = on_edge[i];
    }
    _ratio = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(ratio.data(), ratio.size());

    // collect results: topology
    construct_topology(_curve_all, _pts.rows(), curve_segments, curve_segments_dir);
}

void isocurve::collect_pts(Eigen::VectorXi& idxOnVerts,
                           Eigen::VectorXi& idxOnEdges,
                           std::vector<Eigen::Vector3d>& pts,
                           std::vector<Eigen::Vector2i>& on_edge,
                           std::vector<double>& ratio,
                           const double val, const double eqlTol) const {
    int cntPts = 0;

    for (Eigen::Index i = 0; i < V_.rows(); ++i) {
        if (std::abs(field_[i]-val) < eqlTol) {
            idxOnVerts[i] = cntPts;
            cntPts++;
            pts.push_back(V_.row(i));
            on_edge.push_back(Eigen::Vector2i(i, i));
            ratio.push_back(0.0);
        }
    }

    // std::vector<bool> face_visited(mesh_.n_faces(), false);  // actually useless

    for (EigenTriMesh::EdgeHandle eh : mesh_.all_edges()) {
        EigenTriMesh::HalfedgeHandle heh = mesh_.halfedge_handle(eh, 0);
        EigenTriMesh::VertexHandle vh0 = mesh_.from_vertex_handle(heh);
        EigenTriMesh::VertexHandle vh1 = mesh_.to_vertex_handle(heh);
        if (idxOnVerts[vh0.idx()] >= 0 || idxOnVerts[vh1.idx()] >= 0) {
            continue;
        }
        double x0 = field_[vh0.idx()];
        double x1 = field_[vh1.idx()];
        if ( (x0 < val && x1 > val) || (x0 > val && x1 < val) ) {
            double t0 = std::abs(x0-val);
            double t1 = std::abs(x1-val);
            double t = t0+t1;
            t0 /= t;
            t1 /= t;
            pts.push_back(t1*V_.row(vh0.idx())+t0*V_.row(vh1.idx()));
            on_edge.push_back(Eigen::Vector2i(vh0.idx(), vh1.idx()));
            ratio.push_back(t0);
            idxOnEdges[eh.idx()] = cntPts;
            cntPts++;
        }
    }
}

void isocurve::extract_on_face_through2(const unsigned int i,
                                        const double val,
                                        const Eigen::VectorXi& idxOnVerts,
                                        const Eigen::VectorXi& idxOnEdges,
                                        std::vector<Eigen::Vector2i>& curve_segments,
                                        std::vector<CurveSegmentDir>& curve_segments_dir
                                        ) const {
    EigenTriMesh::FaceHandle fh = mesh_.face_handle(i);
    EigenTriMesh::HalfedgeHandle heh;
    EigenTriMesh::VertexHandle vh0;
    EigenTriMesh::VertexHandle vh1;
    // find heh
    for (EigenTriMesh::HalfedgeHandle h : mesh_.fh_range(fh)) {
        vh0 = mesh_.from_vertex_handle(h);
        vh1 = mesh_.to_vertex_handle(h);
        if (idxOnVerts[vh0.idx()] >= 0 && idxOnVerts[vh1.idx()] >= 0) {
            heh = h;
            break;
        }
    }

    //        vh1
    //        /|\
    //       / | \
    //      /  |  \
    // vh2 /---|---\ vh3
    //        vh0

    EigenTriMesh::HalfedgeHandle heh2 = mesh_.next_halfedge_handle(heh);
    EigenTriMesh::VertexHandle vh2 = mesh_.to_vertex_handle(heh2);
    if (field_[vh2.idx()] < val) {
        curve_segments.push_back(Eigen::Vector2i(idxOnVerts[vh0.idx()],
                                 idxOnVerts[vh1.idx()]));
    }else{  // field_[vh2.idx()] > val. Cannot be == val
        curve_segments.push_back(Eigen::Vector2i(idxOnVerts[vh1.idx()],
                                 idxOnVerts[vh0.idx()]));
    }

    EigenTriMesh::HalfedgeHandle heh1 = mesh_.opposite_halfedge_handle(heh);
    if (mesh_.is_boundary(heh1)) {
        curve_segments_dir.push_back(CurveSegmentDir::Determined);
        return;
    }
    // not boundary
    EigenTriMesh::HalfedgeHandle heh3 = mesh_.next_halfedge_handle(heh1);
    EigenTriMesh::VertexHandle vh3 = mesh_.to_vertex_handle(heh3);
    if ( idxOnVerts[vh3.idx()] ||
         (field_[vh2.idx()] < val && field_[vh3.idx()] > val) ||
         (field_[vh2.idx()] > val && field_[vh3.idx()] < val) ) {
        curve_segments_dir.push_back(CurveSegmentDir::Determined);
        return;
    }

    std::cout << "[Warning!] Local extrema edge!" << std::endl;
    curve_segments_dir.push_back(CurveSegmentDir::Flexible);
    return;
}

void isocurve::extract_on_face_through1(const unsigned int i,
                                        const double val,
                                        const Eigen::VectorXi& idxOnVerts,
                                        const Eigen::VectorXi& idxOnEdges,
                                        std::vector<Eigen::Vector2i>& curve_segments,
                                        std::vector<CurveSegmentDir>& curve_segments_dir
                                        ) const {
    auto fh = mesh_.face_handle(i);
    for (EigenTriMesh::HalfedgeHandle heh : mesh_.fh_range(fh)) {
        // auto eh = mesh_.edge_handle(heh);
        auto vh = mesh_.from_vertex_handle(heh);
        if (idxOnVerts[vh.idx()] < 0)
        {
            continue;
        }
        auto heh1 = mesh_.next_halfedge_handle(heh);
        auto eh1 = mesh_.edge_handle(heh1);
        if (idxOnEdges[eh1.idx()] < 0) {
            // only passes a vertex, does not enter this triangle.
            return;
        }
        auto vh1 = mesh_.to_vertex_handle(heh);
        if (field_[vh1.idx()] > val) {
            curve_segments.push_back(Eigen::Vector2i(idxOnVerts[vh.idx()],
                                     idxOnEdges[eh1.idx()]));
            curve_segments_dir.push_back(CurveSegmentDir::Determined);
        } else {
            curve_segments.push_back(Eigen::Vector2i(idxOnEdges[eh1.idx()],
                                     idxOnVerts[vh.idx()]));
            curve_segments_dir.push_back(CurveSegmentDir::Determined);
        }
        return;
    }
}

void isocurve::extract_on_face_through0(const unsigned int i,
                                        const double val,
                                        const Eigen::VectorXi& idxOnVerts,
                                        const Eigen::VectorXi& idxOnEdges,
                                        std::vector<Eigen::Vector2i>& curve_segments,
                                        std::vector<CurveSegmentDir>& curve_segments_dir
                                        ) const {
    auto fh = mesh_.face_handle(i);
    for (EigenTriMesh::HalfedgeHandle heh : mesh_.fh_range(fh)) {
        auto eh = mesh_.edge_handle(heh);
        auto vh = mesh_.from_vertex_handle(heh);
        if (idxOnEdges[eh.idx()] < 0 || field_[vh.idx()] > val) {
            continue;
        }
        auto heh1 = mesh_.next_halfedge_handle(heh);
        auto eh1 = mesh_.edge_handle(heh1);
        if (idxOnEdges[eh1.idx()] >= 0) {
            curve_segments.push_back(Eigen::Vector2i(idxOnEdges[eh.idx()],
                                     idxOnEdges[eh1.idx()]));
            curve_segments_dir.push_back(CurveSegmentDir::Determined);
            return;
        }
        // actually here should have an "else"
        heh1 = mesh_.next_halfedge_handle(heh1);
        eh1 = mesh_.edge_handle(heh1);
        if (idxOnEdges[eh1.idx()] >= 0) {
            curve_segments.push_back(Eigen::Vector2i(idxOnEdges[eh.idx()],
                                     idxOnEdges[eh1.idx()]));
            curve_segments_dir.push_back(CurveSegmentDir::Determined);
            return;
        }
        // indeed should have an "else"
        throw std::invalid_argument( "Isocurve cuts through one edge. Not possible!" );
    }
}

void isocurve::construct_topology(std::vector<std::deque<int>>& curve_all,
                                  const Eigen::Index num_pts,
                                  std::vector<Eigen::Vector2i>& curve_segments,
                                  const std::vector<CurveSegmentDir>& curve_segments_dir) const {

    typedef Eigen::Triplet<int> T;

    // construct graph
    std::vector<T> triplets;
    for (size_t i = 0; i < curve_segments.size(); ++i) {
        int i0 = curve_segments[i][0];
        int i1 = curve_segments[i][1];
        switch (curve_segments_dir[i]) {
        case CurveSegmentDir::Determined:
            triplets.push_back(T(i0, i1, 1));
            triplets.push_back(T(i1, i0, -1));
            break;
        case CurveSegmentDir::Flexible:
            triplets.push_back(T(i0, i1, 2));
            triplets.push_back(T(i1, i0, 2));
            break;
        //default:
        //    break;
        }
    }

    SpMat G(num_pts,num_pts);
    G.setFromTriplets(triplets.begin(), triplets.end());

    // std::vector<int> pts_directed(num_pts, 0);
    // std::vector<int> pts_ends;

    // check manifoldness
    std::vector<int> neighbors;
    std::vector<int> neighbor_tags;
    for (int k=0; k < G.outerSize(); ++k) {
        neighbors.clear();
        neighbor_tags.clear();
        for (SpMat::InnerIterator it(G,k); it; ++it)
        {
            neighbors.push_back(it.col());
            neighbor_tags.push_back(it.value());
//            if (it.value() == 1 || it.value() == -1) {
//                pts_directed[k] = 1;
//            }
        }
        switch (neighbors.size()) {
        case 0:
            std::cout << "[Warning!] Isolated point." << std::endl;
            break;
        case 1:
//        if (neighbors.size() == 1) {
//            pts_ends.push_back(k);
//        }
            break;
        case 2:
            if (neighbor_tags[0] == neighbor_tags[1] && std::abs(neighbor_tags[1]) == 1) {
                // both 1 or both -1
                std::cout << "[Warning!] Non-manifold curve." << std::endl;
            }
            break;
        default:
            std::cout << "[Warning!] Non-manifold. more than 2 neighbors." << std::endl;
            break;
        }
    }
    // sm1.coeffRef(i,j) = v_ij;
    // heads = Eigen::Map<Eigen::VectorXi, Eigen::Unaligned>(heads_vec.data(), heads_vec.size());

    std::vector<int> pts_visited(num_pts, 0);
    //std::vector<std::deque<int>> curve_all;
    //std::cout << pts_visited[0] << std::endl;
    int start = find_zero(pts_visited);
    //std::cout << "start" << start << std::endl;
    //std::vector<std::deque<int>> curve_all_t;
    while (start >= 0) {
        std::deque<int> curve;
        curve.push_back(start);
        pts_visited[start] = 1;
        int invert = extend(curve, G, pts_visited);
        if (invert == 1)
            std::reverse(curve.begin(), curve.end());
        curve_all.push_back(curve);

        start = find_zero(pts_visited);
    }

//    triplets.clear();
//    for (size_t i = 0; i < curve_all.size(); ++i) {
//        for (size_t j = 0; j < curve_all[i].size()-1; ++j) {
//            triplets.push_back(T(curve_all_t[i][j], curve_all_t[i][j+1], 1));
//            triplets.push_back(T(curve_all_t[i][j], curve_all_t[i][j+1], -1));
//        }
//    }

//    SpMat G1(num_pts,num_pts);
//    G1.setFromTriplets(triplets.begin(), triplets.end());

//    // compare G and G1

}

inline int isocurve::find_zero(const std::vector<int>& vec) const {
    for (size_t i = 0; i < vec.size(); ++i) {
        if (vec[i] == 0)
            return i;
    }
    return -1;
}

int isocurve::extend(std::deque<int>& curve, const SpMat& G, std::vector<int>& pts_visited) const {
    // return value:
    // 1: invert; 0: flexible; -1: not invert.

    std::vector<int> neighbors;
    //std::vector<int> neighbor_tags;
    for (SpMat::InnerIterator it(G, curve[0]); it; ++it) {
        neighbors.push_back(it.col());
        //neighbor_tags.push_back(it.value());
    }
    if (neighbors.size() == 0) {
        // curve[0] already marked visited.
        return 0;
    }

    // not really front/back
    int invert = extend_front(curve, neighbors[0], G, pts_visited);
    //if (neighbors.size() == 2 && curve[0] != *curve.end()) {
    if (neighbors.size() == 2 && curve[0] != curve.back()) {
        // i.e. not a ring
        int invert1 = extend_back(curve, neighbors[1], G, pts_visited);
        if (invert1 + invert == 0 && invert != 0) {
            // 1 and -1
            printf("[Warning!] Complex direction!");
        }
        invert = std::max(invert, invert1);
    }

    return invert;
}

int isocurve::extend_front(std::deque<int>& curve, int front, const SpMat& G, std::vector<int>& visited) const {
    int invert = 0;

    //int prev = *curve.end();
    int prev = curve[0];
    int now = front;
    if (visited[now] == 1)
        return invert;
    while (true) {
        visited[now] = 1;
        curve.push_back(now);
        bool found_next = false;
        for (SpMat::InnerIterator it(G, now); it; ++it) {
            int n = it.col();
            if (n != prev) {
                prev = now;
                now = n;
                found_next = true;
                if (it.value() == 1) {
                    if (invert == 1)
                        printf("[Warning!] Complex direction!");
                    invert = -1;  // not invert
                }
                if (it.value() == -1) {
                    if (invert == -1)
                        printf("[Warning!] Complex direction!");
                    invert = 1;
                }
                break;
            }
        }
        if (!found_next)
            break;
        if (visited[now] == 1) {  // ring
            curve.push_back(now);
            break;
        }
    }
    return invert;
}

int isocurve::extend_back(std::deque<int>& curve, int back, const SpMat& G, std::vector<int>& visited) const {
    // must not be a ring

    int invert = 0;

    int prev = curve[0];
    int now = back;
    while (visited[now] != 1) {
        visited[now] = 1;
        curve.push_front(now);
        for (SpMat::InnerIterator it(G, now); it; ++it) {
            int n = it.col();
            if (n != prev) {
                prev = now;
                now = n;
                if (it.value() == 1) {
                    if (invert == -1)
                        printf("[Warning!] Complex direction!");
                    invert = 1;
                }
                if (it.value() == -1) {
                    if (invert == 1)
                        printf("[Warning!] Complex direction!");
                    invert = -1;
                }
                break;
            }
        }
    }
    return invert;
}

std::tuple<Eigen::MatrixXd, Eigen::MatrixXi, Eigen::VectorXd, std::vector<std::deque<int>>>
isocurve::extract(const double val, const double eqlTol) {
    Eigen::MatrixXd pts;
    Eigen::MatrixXi on_edge;
    Eigen::VectorXd ratio;
    std::vector<std::deque<int>> curves_all;
    extract(val, eqlTol, pts, on_edge, ratio, curves_all);
    // std::cout << curves_all.size() << std::endl;
    auto res = std::make_tuple(pts, on_edge, ratio, curves_all);
    return res;
}


//   GAMBIT: Global and Modular BSM Inference Tool
//   *********************************************
///  \file
///
//
///  *********************************************
///
///  Authors:
///  <!-- add name and date if you modify -->
///
///  \author Ben Farmer
///          (benjamin.farmer@fysik.su.se)
///  \date 2015 Apr
///
///  \author Pat Scott
///          (p.scott@imperial.ac.uk)
///  \date 2016 Oct
///
///  *********************************************

#include "gambit/Models/SimpleSpectra/MSSMSimpleSpec.hpp"
#include "gambit/Elements/mssm_slhahelp.hpp"
#include "gambit/Utils/util_functions.hpp"
#include "gambit/Utils/variadic_functions.hpp"
#include "gambit/Logs/logger.hpp"

#include <math.h>
#include <boost/lexical_cast.hpp>

using namespace SLHAea;

namespace Gambit
{

      /// Helper function for sorting int, double pairs according to the double
      bool orderer (std::pair<int, double> a, std::pair<int, double> b) { return a.second < b.second; }

      /// @{ Member functions for SLHAeaModel class

      /// Default Constructor
      MSSMea::MSSMea()
        : SLHAeaModel()
      {}

      /// Constructor via SLHAea object
      MSSMea::MSSMea(const SLHAea::Coll& input)
        : SLHAeaModel(input)
      {
        std::map<int, int>& slha1to2 = PDG_translation_map;
        str blocks[4] = {"DSQMIX", "USQMIX", "SELMIX", "SNUMIX"};
        str gen3mix[3] = {"SBOTMIX", "STOPMIX", "STAUMIX"};
        logger() << LogTags::utils;

        // Work out if this SLHAea object is SLHA1 or SLHA2
        if (data.find(blocks[0]) == data.end() or
            data.find(blocks[1]) == data.end() or
            data.find(blocks[2]) == data.end() or
            data.find(blocks[3]) == data.end() )
        {
          if (data.find(gen3mix[0]) == data.end() or
              data.find(gen3mix[1]) == data.end() or
              data.find(gen3mix[2]) == data.end() )
          {
            utils_error().raise(LOCAL_INFO, "Input SLHA data appears to be neither SLHA1 nor SLHA2.");
          }
          logger() << "Input SLHA for setting up simple spectrum is SLHA1.  You old dog." << EOM;

          // Get scale, needed for specifying SLHA2 blocks
          /// TODO: Currently assumes all blocks at same scale. Should check if this
          /// is true.
          double scale = 0.0;
          try
          {
            scale = SLHAea::to<double>(data.at("GAUGE").find_block_def()->at(3));
          }
          catch (const std::out_of_range& e)
          {
            std::ostringstream errmsg;
            errmsg << "Could not find block \"GAUGE\" in SLHAea object (required to retrieve scale Q). Received out_of_range error with message: " << e.what();
            utils_error().raise(LOCAL_INFO,errmsg.str());
          }

          //Looks like it is SLHA1, so convert it to SLHA2.
          int lengths[4] = {6, 6, 6, 3};
          str names[4] = {"~d_", "~u_", "~l_", "~nu_"};
          std::vector<int> pdg[4];
          std::vector< std::pair<int, double> > masses[4];
          pdg[0] = initVector<int>(1000001, 1000003, 1000005, 2000001, 2000003, 2000005); // d-type squarks
          pdg[1] = initVector<int>(1000002, 1000004, 1000006, 2000002, 2000004, 2000006); // u-type squarks
          pdg[2] = initVector<int>(1000011, 1000013, 1000015, 2000011, 2000013, 2000015); // sleptons
          pdg[3] = initVector<int>(1000012, 1000014, 1000016);                            // sneutrinos
          for (int j = 0; j < 4; j++)
          {
            // Get the masses
            for (int i = 0; i < lengths[j]; i++) masses[j].push_back(std::pair<int, double>(pdg[j][i], getdata("MASS",pdg[j][i])));

            // Sort them
            std::sort(masses[j].begin(), masses[j].end(), orderer);

            // Rewrite them in correct order, and populate the pdg-pdg maps
            for (int i = 0; i < lengths[j]; i++)
            {
              //data["MASS"][pdg[j][i]][1] = boost::lexical_cast<str>(masses[j][i].second);
              //data["MASS"][pdg[j][i]][2] = "# "+names[j]+boost::lexical_cast<str>(i+1);
              str masspdg = boost::lexical_cast<str>(masses[j][i].second);
              str comment = "# "+names[j]+boost::lexical_cast<str>(i+1);
              SLHAea_add(data, "MASS", pdg[j][i], masspdg, comment, true);
              slha1to2[masses[j][i].first] = pdg[j][i];
            }

            // Write the mixing block.  i is the SLHA2 index, k is the SLHA1 index.
            //data[blocks[j]][""] << "BLOCK" << blocks[j];
            SLHAea_check_block(data, blocks[j]);
            for (int i = 0; i < lengths[j]; i++) for (int k = 0; k < lengths[j]; k++)
            {
              double datum;
              if (lengths[j] == 3 or (k != 2 and k != 5)) // first or second generation (or neutrinos)
              {
                datum = (slha1to2.at(pdg[j][k]) == pdg[j][i]) ? 1.0 : 0.0;
              }
              else // third generation => need to use the 2x2 SLHA1 mixing matrices.
              {
                double family_index = 0;
                if (k == 2)
                {
                  if (slha1to2.at(pdg[j][k]) == pdg[j][i]) family_index = 1;
                  else if (slha1to2.at(pdg[j][5]) == pdg[j][i]) family_index = 2;
                }
                else if (k == 5)
                {
                  if (slha1to2.at(pdg[j][k]) == pdg[j][i]) family_index = 2;
                  else if (slha1to2.at(pdg[j][2]) == pdg[j][i]) family_index = 1;
                }
                if (family_index > 0)
                {
                  datum = getdata(gen3mix[j], family_index, (k+1)/3);
                }
                else datum = 0.0;
              }
              //data[blocks[j]][""] << i+1 << k+1 << datum << "# "+blocks[j]+boost::lexical_cast<str>(i*10+k+11);
              SLHAea_add(data, blocks[j], i+1, k+1, datum, "# "+blocks[j]+boost::lexical_cast<str>(i*10+k+11), true);
            }

          }
          // Now deal with MSOFT --> SLHA2 soft mass matrix blocks
          // (inverse of retrieval code in add_MSSM_spectrum_to_SLHAea)
          sspair M[5] = {sspair("MSL2","ml2"), sspair("MSE2","me2"), sspair("MSQ2","mq2"), sspair("MSU2","mu2"), sspair("MSD2","md2")};
          for (int k=0;k<5;k++)
          {
            std::string block(M[k].first);
            if(not SLHAea_block_exists(data, block)) SLHAea_add_block(data, block, scale); //TODO: maybe just always delete and replace
            for(int i=1;i<4;i++) for(int j=1;j<4;j++)
            {
              std::ostringstream comment;
              comment << block << "(" << i << "," << j << ")";
              double entry;
              if(i==j)
              {
                entry = getdata("MSOFT",30+3*k+i+(k>1?4:0)); // black magic to get correct index in MSOFT matching diagonal elements
              }
              else
              {
                // Everything off-diagonal is zero in SLHA1
                entry = 0;
              }
              //data[block][""] << i << j << entry*entry << "# "+comment.str();
              SLHAea_add(data, block, i, j, entry*entry, "# "+comment.str(), true);
            }
          }

          // Yukawa and trilinear blocks.  YU, YD and YE, plus [YU, YD and YE; SLHA1 only], or [TU, TD and TE; SLHA2 only].
          sspair A[3] = {sspair("AU","Au"), sspair("AD","Ad"), sspair("AE","Ae")};
          sspair Y[3] = {sspair("YU","Yu"), sspair("YD","Yd"), sspair("YE","Ye")};
          sspair T[3] = {sspair("TU","TYu"), sspair("TD","TYd"), sspair("TE","TYe")};
          for (int k=0;k<3;k++)
          {
            SLHAea_check_block(data, A[k].first);
            SLHAea_check_block(data, Y[k].first);
            SLHAea_check_block(data, T[k].first); // TODO: should delete superceded slha1 "A" blocks?
            for(int i=1;i<4;i++)
            {
              for(int j=1;j<4;j++)
              {
                std::ostringstream comment;
                comment << "(" << i << "," << j << ")";
                // SLHA1 has only diagonal elements in Y and A. We should fill them out fully.
                // Assume missing diagonal elements are also zero.
                double Yentry;
                double Aentry;
                if(i==j)
                {
                  if(SLHAea_check_block(data,Y[k].first,i,j))
                  {
                    Yentry = getdata(Y[k].first,i,j);
                  }
                  else
                  {
                    Yentry = 0;
                  }

                  if(SLHAea_check_block(data,A[k].first,i,j))
                  {
                    Aentry = getdata(A[k].first,i,j);
                  }
                  else
                  {
                    Aentry = 0;
                  }
                }
                else
                {
                  Yentry = 0;
                  Aentry = 0;
                }

                double Tentry = Aentry * Yentry;
                SLHAea_add(data, Y[k].first, i, j, Yentry, "# "+Y[k].first+"_"+comment.str(), true);
                SLHAea_add(data, A[k].first, i, j, Aentry, "# "+A[k].first+"_"+comment.str(), true);
                SLHAea_add(data, T[k].first, i, j, Tentry, "# "+T[k].first+"_"+comment.str(), true);
              }
            }
          }

        }

        else logger() << "Input SLHA data for setting up simple spectrum is SLHA2.  *living in the future*" << EOM;
        // In the end, we should always have converted to SLHA2 by now, so set this as the internally-tracked version.
        wrapped_slha_version = 2;
      }

      /// @{ Getters for MSSM information
      double MSSMea::get_Mu()      const{ return getdata("HMIX",1); } // mu(Q) DRbar
      double MSSMea::get_tanbeta() const{ return getdata("HMIX",2); } // tan beta(Q) DRbar ( = vu/vd)
      double MSSMea::get_tanbeta_mZ() const{ return getdata("MINPAR",3); } // tan beta(mZ) DRbar
      double MSSMea::get_v()       const{ return getdata("HMIX",3); } // v = sqrt(vd^2 + vu^2) DRbar
      double MSSMea::get_mA2()     const{ return getdata("HMIX",4); } // m^2_A=[m3^2/cosBsinB](Q) DRbar, tree
      ////// THESE ARE NOT SLHA! Therefore cannot rely on them being in the SLHAea object.
      ////// But we can compute them instead.
      // double MSSMea::get_BMu()     const { return getdata("HMIX",101); }
      // double MSSMea::get_vd()      const { return getdata("HMIX",102); }
      // double MSSMea::get_vu()      const { return getdata("HMIX",103); }
      ///////
      double MSSMea::get_BMu() const
      {
        double tb = get_tanbeta();
        double cb = cos(atan(tb));
        double sb = sin(atan(tb));
        return get_mA2() * (sb * cb);
      }
      double MSSMea::get_vd() const
      {
        double v = get_v();
        double tb = get_tanbeta();
        return sqrt(abs( v*v / ( tb*tb + 1 ) ));
      }
      double MSSMea::get_vu() const
      {
        double v = get_v();
        double itb = 1./get_tanbeta();
        return sqrt(abs( v*v / ( itb*itb + 1 ) ));
      }

      double MSSMea::get_MassB () const { return getdata("MSOFT",1); }
      double MSSMea::get_MassWB() const { return getdata("MSOFT",2); }
      double MSSMea::get_MassG () const { return getdata("MSOFT",3); }
      double MSSMea::get_mHd2() const { return getdata("MSOFT",21); }
      double MSSMea::get_mHu2() const { return getdata("MSOFT",22); }

      double MSSMea::get_mq2(int i, int j) const { return getdata("MSQ2",i,j); }
      double MSSMea::get_ml2(int i, int j) const { return getdata("MSL2",i,j); }
      double MSSMea::get_md2(int i, int j) const { return getdata("MSD2",i,j); }
      double MSSMea::get_mu2(int i, int j) const { return getdata("MSU2",i,j); }
      double MSSMea::get_me2(int i, int j) const { return getdata("MSE2",i,j); }

      double MSSMea::get_TYd(int i, int j) const { return getdata("Td",i,j); }
      double MSSMea::get_TYu(int i, int j) const { return getdata("Tu",i,j); }
      double MSSMea::get_TYe(int i, int j) const { return getdata("Te",i,j); }

      double MSSMea::get_Yd(int i, int j) const { return getdata("Yd",i,j); }
      double MSSMea::get_Yu(int i, int j) const { return getdata("Yu",i,j); }
      double MSSMea::get_Ye(int i, int j) const { return getdata("Ye",i,j); }

      double MSSMea::get_g1() const { return getdata("GAUGE",1)/sqrt(3./5.); } // Convert from gy (in SLHAea object) to g1
      double MSSMea::get_g2() const { return getdata("GAUGE",2); }
      double MSSMea::get_g3() const { return getdata("GAUGE",3); }
      double MSSMea::get_sinthW2_DRbar() const
      {
        double sg1 = 0.6 * Utils::sqr(get_g1());
        return sg1 / (sg1 + Utils::sqr(get_g2()));
      }

      double MSSMea::get_MGlu_pole() const { return getdata("MASS",1000021); }

      double MSSMea::get_Mhh_pole_slha(int i) const
      {
         if      (i==1){ return getdata("MASS",25); } // Neutral Higgs(1)
         else if (i==2){ return getdata("MASS",35); } // Neutral Higgs(2)
         else { utils_error().raise(LOCAL_INFO,"Invalid index input to get_Mhh_pole_slha! Please check index range limits in wrapper SubSpectrum class!"); return -1; } // Should not return.
      }
      double MSSMea::get_MAh_pole () const { return getdata("MASS",36); }
      double MSSMea::get_MHpm_pole() const { return getdata("MASS",37); }
      double MSSMea::get_MW_pole()    const { return getdata("MASS",24); } // REQUIRED output of MSSM-compatible subspectrum

      double MSSMea::get_MCha_pole_slha(int i) const
      {
         if      (i==1){ return getdata("MASS",1000024); } // Chargino(1)
         else if (i==2){ return getdata("MASS",1000037); } // Chargino(2)
         else { utils_error().raise(LOCAL_INFO,"Invalid index input to get_MCha_pole_slha! Please check index range limits in wrapper SubSpectrum class!"); return -1; } // Should not return.
      }
      double MSSMea::get_MSd_pole_slha(int i) const
      {
         static std::map<int,int> match;

         if      (i==1){ return getdata("MASS",1000001); } // d-type squark(1)
         else if (i==2){ return getdata("MASS",1000003); } // d-type squark(2)
         else if (i==3){ return getdata("MASS",1000005); } // d-type squark(3)
         else if (i==4){ return getdata("MASS",2000001); } // d-type squark(4)
         else if (i==5){ return getdata("MASS",2000003); } // d-type squark(5)
         else if (i==6){ return getdata("MASS",2000005); } // d-type squark(6)
         else { utils_error().raise(LOCAL_INFO,"Invalid index input to get_MSd_pole_slha! Please check index range limits in wrapper SubSpectrum class!"); return -1; } // Should not return.
      }
      double MSSMea::get_MSu_pole_slha(int i) const
      {
         if      (i==1){ return getdata("MASS",1000002); } // u-type squark(1)
         else if (i==2){ return getdata("MASS",1000004); } // u-type squark(2)
         else if (i==3){ return getdata("MASS",1000006); } // u-type squark(3)
         else if (i==4){ return getdata("MASS",2000002); } // u-type squark(4)
         else if (i==5){ return getdata("MASS",2000004); } // u-type squark(5)
         else if (i==6){ return getdata("MASS",2000006); } // u-type squark(6)
         else { utils_error().raise(LOCAL_INFO,"Invalid index input to get_MSd_pole_slha! Please check index range limits in wrapper SubSpectrum class!"); return -1; } // Should not return.
      }
      double MSSMea::get_MSe_pole_slha(int i) const
      {
         if      (i==1){ return getdata("MASS",1000011); } // charged slepton(1)
         else if (i==2){ return getdata("MASS",1000013); } // charged slepton(2)
         else if (i==3){ return getdata("MASS",1000015); } // charged slepton(3)
         else if (i==4){ return getdata("MASS",2000011); } // charged slepton(4)
         else if (i==5){ return getdata("MASS",2000013); } // charged slepton(5)
         else if (i==6){ return getdata("MASS",2000015); } // charged slepton(6)
         else { utils_error().raise(LOCAL_INFO,"Invalid index input to get_MSd_pole_slha! Please check index range limits in wrapper SubSpectrum class!"); return -1; } // Should not return.
      }
      double MSSMea::get_MSv_pole_slha(int i) const
      {
         if      (i==1){ return getdata("MASS",1000012); } // Sneutrino(1)
         else if (i==2){ return getdata("MASS",1000014); } // Sneutrino(2)
         else if (i==3){ return getdata("MASS",1000016); } // Sneutrino(3)
         else { utils_error().raise(LOCAL_INFO,"Invalid index input to get_MSd_pole_slha! Please check index range limits in wrapper SubSpectrum class!"); return -1; } // Should not return.
      }
      double MSSMea::get_MChi_pole_slha(int i) const
      {
         if      (i==1){ return getdata("MASS",1000022); } // Neutralino(1)
         else if (i==2){ return getdata("MASS",1000023); } // Neutralino(2)
         else if (i==3){ return getdata("MASS",1000025); } // Neutralino(3)
         else if (i==4){ return getdata("MASS",1000035); } // Neutralino(4)
         else { utils_error().raise(LOCAL_INFO,"Invalid index input to get_MChi_pole_slha! Please check index range limits in wrapper SubSpectrum class!"); return -1; } // Should not return.
      }

      // Pole Mixings
      double MSSMea::get_ZD_pole_slha(int i, int j) const { return getdata("DSQMIX",i,j); }
      double MSSMea::get_ZU_pole_slha(int i, int j) const { return getdata("USQMIX",i,j); }

      double MSSMea::get_ZV_pole_slha(int i, int j) const { return getdata("SNUMIX",i,j); }
      double MSSMea::get_ZE_pole_slha(int i, int j) const { return getdata("SELMIX",i,j); }

      double MSSMea::get_ZH_pole_slha(int i, int j) const { return getdata("SCALARMIX",i,j); }
      double MSSMea::get_ZA_pole_slha(int i, int j) const { return getdata("PSEUDOSCALARMIX",i,j); }

      double MSSMea::get_ZP_pole_slha(int i, int j) const { return getdata("CHARGEMIX",i,j); }
      double MSSMea::get_ZN_pole_slha(int i, int j) const { return getdata("NMIX",i,j); }

      double MSSMea::get_UM_pole_slha(int i, int j) const { return getdata("UMIX",i,j); }
      double MSSMea::get_UP_pole_slha(int i, int j) const { return getdata("VMIX",i,j); }

      /// @}

      /// @{ Member functions for MSSMSimpleSpec class

      /// @{ Constructors

      /// Default Constructor
      MSSMSimpleSpec::MSSMSimpleSpec(double uncert)
      {
        set_pole_mass_uncertainties(uncert);
      }

      /// Constructor via SLHAea object
      MSSMSimpleSpec::MSSMSimpleSpec(const SLHAea::Coll& input, double uncert)
        : SLHASimpleSpec(input)
      {
        set_pole_mass_uncertainties(uncert);
      }

      /// Copy constructor: needed by clone function.
      MSSMSimpleSpec::MSSMSimpleSpec(const MSSMSimpleSpec& other, double uncert)
        : SLHASimpleSpec(other)
      {
        set_pole_mass_uncertainties(uncert);
      }

      /// @}

      /// Ofset from user-input indices (user assumes 1,2,3 indexed, e.g. use offset=-1 for zero-indexing)
      int MSSMSimpleSpec::get_index_offset() const {return 0.;} // we use indices starting from 1 in this file, matching user assumptions. (because Peter is god, he knows user assumptions before they do.)

      /// Retrieve SLHAea object
      // NOTE! No need to write this function, SubSpectrum base class can handle it if add_to_SLHAea exists.
      //SLHAea::Coll MSSMSimpleSpec::getSLHAea(int slha_version) const

      /// Add SLHAea object to another
      void MSSMSimpleSpec::add_to_SLHAea(int slha_version, SLHAea::Coll& slha) const
      {
        // Add SPINFO data if not already present
        SLHAea_add_GAMBIT_SPINFO(slha);

        // All MSSM blocks
        slhahelp::add_MSSM_spectrum_to_SLHAea(*this, slha, slha_version);
      }

      /// Retrieve the PDG translation map
      const std::map<int, int>& MSSMSimpleSpec::PDG_translator() const { return slhawrap.PDG_translator(); }

      /// Set pole mass uncertainties
      void MSSMSimpleSpec::set_pole_mass_uncertainties(double uncert)
      {
        const std::vector<int> i12        = initVector(1,2);
        const std::vector<int> i123       = initVector(1,2,3);
        const std::vector<int> i1234      = initVector(1,2,3,4);
        const std::vector<int> i123456    = initVector(1,2,3,4,5,6);
        const std::vector<str> sbosons1   = initVector<str>("~g","A0","H+","H-","W+","W-");
        const std::vector<str> sbosons2   = initVector<str>("~chi+","~chi-","h0");
        const std::vector<str> sfermions1 = initVector<str>("~u","~d","~e-","~ubar","~dbar","~e+");
        const std::vector<str> sfermions2 = initVector<str>("~nu","~nubar");
        set_override_vector(Par::Pole_Mass_1srd_high, uncert, sfermions1, i123456, true);
        set_override_vector(Par::Pole_Mass_1srd_low,  uncert, sfermions1, i123456, true);
        set_override_vector(Par::Pole_Mass_1srd_high, uncert, sfermions2, i123, true);
        set_override_vector(Par::Pole_Mass_1srd_low,  uncert, sfermions2, i123, true);
        set_override_vector(Par::Pole_Mass_1srd_high, uncert, sbosons1, true);
        set_override_vector(Par::Pole_Mass_1srd_low,  uncert, sbosons1, true);
        set_override_vector(Par::Pole_Mass_1srd_high, uncert, sbosons2, i12, true);
        set_override_vector(Par::Pole_Mass_1srd_low,  uncert, sbosons2, i12, true);
        set_override_vector(Par::Pole_Mass_1srd_high, uncert, "~chi0", i1234, true);
        set_override_vector(Par::Pole_Mass_1srd_low,  uncert, "~chi0", i1234, true);
      }

      // Map fillers

      MSSMSimpleSpec::GetterMaps MSSMSimpleSpec::fill_getter_maps()
      {
         GetterMaps map_collection;

         typedef MTget::FInfo1 FInfo1;
         typedef MTget::FInfo2 FInfo2;

         // Can't use c++11 initialiser lists, se have to initialise the index sets like this.
         static const int i12v[] = {1,2};
         static const std::set<int> i12(i12v, Utils::endA(i12v));

         static const int i123v[] = {1,2,3};
         static const std::set<int> i123(i123v, Utils::endA(i123v));

         static const int i1234v[] = {1,2,3,4};
         static const std::set<int> i1234(i1234v, Utils::endA(i1234v));

         static const int i123456v[] = {1,2,3,4,5,6};
         static const std::set<int> i123456(i123456v, Utils::endA(i123456v));

         // Running parameters
         {
            MTget::fmap0 tmp_map;
            tmp_map["BMu"] = &Model::get_BMu;
            tmp_map["mA2"] = &Model::get_mA2;
            tmp_map["mHd2"] = &Model::get_mHd2;
            tmp_map["mHu2"] = &Model::get_mHu2;
            map_collection[Par::mass2].map0 = tmp_map;
         }
         {
            MTget::fmap2 tmp_map;
            tmp_map["mq2"] = FInfo2( &Model::get_mq2, i123, i123);
            tmp_map["ml2"] = FInfo2( &Model::get_ml2, i123, i123);
            tmp_map["md2"] = FInfo2( &Model::get_md2, i123, i123);
            tmp_map["mu2"] = FInfo2( &Model::get_mu2, i123, i123);
            tmp_map["me2"] = FInfo2( &Model::get_me2, i123, i123);
            map_collection[Par::mass2].map2 = tmp_map;
         }
         {
            MTget::fmap0 tmp_map;
            tmp_map["M1"]= &Model::get_MassB;
            tmp_map["M2"]= &Model::get_MassWB;
            tmp_map["M3"]= &Model::get_MassG;
            tmp_map["Mu"]= &Model::get_Mu;
            tmp_map["vu"]= &Model::get_vu;
            tmp_map["vd"]= &Model::get_vd;
            map_collection[Par::mass1].map0 = tmp_map;
         }
         {
            MTget::fmap2 tmp_map;
            tmp_map["TYd"]= FInfo2( &Model::get_TYd, i123, i123);
            tmp_map["TYe"]= FInfo2( &Model::get_TYe, i123, i123);
            tmp_map["TYu"]= FInfo2( &Model::get_TYu, i123, i123);
            tmp_map["ad"] = FInfo2( &Model::get_TYd, i123, i123);
            tmp_map["ae"] = FInfo2( &Model::get_TYe, i123, i123);
            tmp_map["au"] = FInfo2( &Model::get_TYu, i123, i123);
            map_collection[Par::mass1].map2 = tmp_map;
         }
         {
            MTget::fmap0 tmp_map;
            tmp_map["g1"]= &Model::get_g1;
            tmp_map["g2"]= &Model::get_g2;
            tmp_map["g3"]= &Model::get_g3;
            tmp_map["tanbeta"]= &Model::get_tanbeta;
            tmp_map["tanbeta(mZ)"]= &Model::get_tanbeta_mZ; // Special entry for reproducing MINPAR entry in SLHA
            tmp_map["sinW2"]= &Model::get_sinthW2_DRbar;
            map_collection[Par::dimensionless].map0 = tmp_map;
         }
         {
            MTget::fmap2 tmp_map;
            tmp_map["Yd"]= FInfo2( &Model::get_Yd, i123, i123);
            tmp_map["Yu"]= FInfo2( &Model::get_Yu, i123, i123);
            tmp_map["Ye"]= FInfo2( &Model::get_Ye, i123, i123);
            map_collection[Par::dimensionless].map2 = tmp_map;
         }

         // "Physical" parameters
         {
            MTget::fmap0 tmp_map;
            tmp_map["~g"] = &Model::get_MGlu_pole;
            tmp_map["A0"] = &Model::get_MAh_pole;
            tmp_map["H+"] = &Model::get_MHpm_pole;
            // Antiparticle label
            tmp_map["H-"] = &Model::get_MHpm_pole;
            tmp_map["W+"] = &Model::get_MW_pole;
            map_collection[Par::Pole_Mass].map0 = tmp_map;
         }
         {
            MTget::fmap1 tmp_map;
            tmp_map["~d"] =    FInfo1( &Model::get_MSd_pole_slha, i123456 );
            tmp_map["~u"] =    FInfo1( &Model::get_MSu_pole_slha, i123456 );
            tmp_map["~e-"] =   FInfo1( &Model::get_MSe_pole_slha, i123456 );
            tmp_map["~nu"] =   FInfo1( &Model::get_MSv_pole_slha, i123 );
            tmp_map["h0"] =    FInfo1( &Model::get_Mhh_pole_slha, i12 );
            tmp_map["~chi+"] = FInfo1( &Model::get_MCha_pole_slha, i12 );
            tmp_map["~chi0"] = FInfo1( &Model::get_MChi_pole_slha, i1234 );

            // Antiparticles (same getters, just different string name)
            tmp_map["~dbar"] = FInfo1( &Model::get_MSd_pole_slha, i123456 );
            tmp_map["~ubar"] = FInfo1( &Model::get_MSu_pole_slha, i123456 );
            tmp_map["~e+"]   = FInfo1( &Model::get_MSe_pole_slha, i123456 );
            tmp_map["~nubar"]= FInfo1( &Model::get_MSv_pole_slha, i123 );
            tmp_map["~chi-"] = FInfo1( &Model::get_MCha_pole_slha, i12 );
            map_collection[Par::Pole_Mass].map1 = tmp_map;
         }
         {
            MTget::fmap2 tmp_map;
            tmp_map["~d"] =    FInfo2( &Model::get_ZD_pole_slha, i123456, i123456);
            tmp_map["~nu"] =   FInfo2( &Model::get_ZV_pole_slha, i123, i123);
            tmp_map["~u"] =    FInfo2( &Model::get_ZU_pole_slha, i123456, i123456);
            tmp_map["~e-"]=    FInfo2( &Model::get_ZE_pole_slha, i123456, i123456);
            tmp_map["h0"] =    FInfo2( &Model::get_ZH_pole_slha, i12, i12);
            tmp_map["A0"] =    FInfo2( &Model::get_ZA_pole_slha, i12, i12);
            tmp_map["H+"] =    FInfo2( &Model::get_ZP_pole_slha, i12, i12);
            tmp_map["~chi0"] = FInfo2( &Model::get_ZN_pole_slha, i1234, i1234);
            tmp_map["~chi-"] = FInfo2( &Model::get_UM_pole_slha, i12, i12);
            tmp_map["~chi+"] = FInfo2( &Model::get_UP_pole_slha, i12, i12);
            map_collection[Par::Pole_Mixing].map2 = tmp_map;
         }

         return map_collection;
      }

} // end Gambit namespace


/********************************************************************************
 *
 * Stub for the low-level API
 *
 * Author: Maxime Arthaud (maxime@arthaud.me)
 *
 * Copyright (c) 2014 Carnegie Mellon University
 *
 * 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.
 *
 *******************************************************************************/

#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>

#include <api/api.hpp>

using namespace boost::python;
namespace python = boost::python;
using namespace ikos;
using namespace ikos::api;

/* imported class */
object fraction;

/*
 * Conversion class, from Python to IKOS
 */
class CFGFactory {
  private:
    string_factory V;

  public:
    z_var_t get_z_var(object py_var) {
      std::string name = extract< std::string >(py_var.attr("name"));
      return z_var_t(V[name]);
    }

    q_var_t get_q_var(object py_var) {
      std::string name = extract< std::string >(py_var.attr("name"));
      return q_var_t(V[name]);
    }

    z_linear_expression_t get_z_linear_expression(object py_expr) {
      z_linear_expression_t ikos_expr;

      object py_tuple = py_expr.attr("normalize")();
      long cst = extract< long >(py_tuple[1]);

      stl_input_iterator< object > it(py_tuple[0]), end;
      for(; it != end; ++it) {
        object py_term = *it;
        long factor = extract< long >(py_term[0]);
        ikos_expr = ikos_expr + z_number(factor) * get_z_var(py_term[1]);
      }

      ikos_expr = ikos_expr + z_number(cst);
      return ikos_expr;
    }

    q_linear_expression_t get_q_linear_expression(object py_expr) {
      q_linear_expression_t ikos_expr;

      object py_tuple = py_expr.attr("normalize")();
      long cst_num = extract< long >(py_tuple[1].attr("numerator"));
      long cst_den = extract< long >(py_tuple[1].attr("denominator"));

      stl_input_iterator< object > it(py_tuple[0]), end;
      for(; it != end; ++it) {
        object py_term = *it;
        long factor_num = extract< long >(py_term[0].attr("numerator"));
        long factor_den = extract< long >(py_term[0].attr("denominator"));
        ikos_expr = ikos_expr + q_number(factor_num, factor_den) * get_q_var(py_term[1]);
      }

      ikos_expr = ikos_expr + q_number(cst_num, cst_den);
      return ikos_expr;
    }

    z_linear_constraint_t get_z_linear_constraint(object py_constraint) {
      z_linear_expression_t ikos_expr = get_z_linear_expression(py_constraint.attr("expression"));
      z_linear_constraint_t::kind_t kind = z_linear_constraint_t::DISEQUATION;

      if(py_constraint.attr("operator") == "<=") {
        kind = z_linear_constraint_t::INEQUALITY;
      } else if(py_constraint.attr("operator") == "=") {
        kind = z_linear_constraint_t::EQUALITY;
      } else if(py_constraint.attr("operator") == "!=") {
        kind = z_linear_constraint_t::DISEQUATION;
      } else {
        throw_error("invalid operator in LinearConstraint: %s", py_constraint.attr("operator"));
      }

      return z_linear_constraint_t(ikos_expr, kind);
    }

    q_linear_constraint_t get_q_linear_constraint(object py_constraint) {
      q_linear_expression_t ikos_expr = get_q_linear_expression(py_constraint.attr("expression"));
      q_linear_constraint_t::kind_t kind = q_linear_constraint_t::DISEQUATION;

      if(py_constraint.attr("operator") == "<=") {
        kind = q_linear_constraint_t::INEQUALITY;
      } else if(py_constraint.attr("operator") == "=") {
        kind = q_linear_constraint_t::EQUALITY;
      } else if(py_constraint.attr("operator") == "!=") {
        kind = q_linear_constraint_t::DISEQUATION;
      } else {
        throw_error("invalid operator in LinearConstraint: %s", py_constraint.attr("operator"));
      }

      return q_linear_constraint_t(ikos_expr, kind);
    }

    cfg_t get_cfg(object py_cfg) {
      stl_input_iterator< object > end;
      std::string entry_name = extract< std::string >(py_cfg.attr("entry").attr("name"));
      cfg_t cfg(entry_name);

      /* create blocks */
      for(stl_input_iterator< object > it(py_cfg.attr("blocks")); it != end; ++it) {
        object py_block = *it;
        std::string block_name = extract< std::string >(py_block.attr("name"));

        block_t& block = cfg.insert_basic_block(block_name);
        block_add_statements(block, py_block.attr("statements"));
      }

      /* create links */
      for(stl_input_iterator< object > it(py_cfg.attr("blocks")); it != end; ++it) {
        object py_block = *it;
        std::string block_name = extract< std::string >(py_block.attr("name"));

        for(stl_input_iterator< object > it2(py_block.attr("next_blocks")); it2 != end; ++it2) {
          object py_next = *it2;
          std::string next_name = extract< std::string >(py_next.attr("name"));
          cfg.get_node(nodename_t(block_name)) >> cfg.get_node(nodename_t(next_name));
        }
      }

      return cfg;
    }

  private:
    void block_add_statements(block_t& block, object py_statements) {
      stl_input_iterator< object > it(py_statements), end;

      for(; it != end; ++it) {
        object py_statement = *it;
        object class_name = py_statement.attr("__class__").attr("__name__");

        if(class_name == "BinaryOperation") {
          object type = py_statement.attr("var").attr("type");
          object op = py_statement.attr("operator");

          if(type == "int") {
            z_var_t var = get_z_var(py_statement.attr("var"));
            z_var_t left = get_z_var(py_statement.attr("left"));
            z_var_t right = get_z_var(py_statement.attr("right"));

            if(op == "+") block.add(var, left, right);
            else if(op == "+") block.sub(var, left, right);
            else if(op == "*") block.mul(var, left, right);
            else if(op == "/") block.div(var, left, right);
            else throw_error("invalid operator in BinaryStatement: %s", op);
          }
          else if(type == "rational") {
            q_var_t var = get_q_var(py_statement.attr("var"));
            q_var_t left = get_q_var(py_statement.attr("left"));
            q_var_t right = get_q_var(py_statement.attr("right"));

            if(op == "+") block.add(var, left, right);
            else if(op == "+") block.sub(var, left, right);
            else if(op == "*") block.mul(var, left, right);
            else if(op == "/") block.div(var, left, right);
            else throw_error("invalid operator in BinaryStatement: %s", op);
          }
          else {
            throw_error("invalid variable type: %s", type);
          }
        }
        else if(class_name == "Assign") {
          object type = py_statement.attr("var").attr("type");

          if(type == "int") {
            z_var_t var = get_z_var(py_statement.attr("var"));
            z_linear_expression_t expr = get_z_linear_expression(py_statement.attr("expression"));
            block.assign(var, expr);
          }
          else if(type == "rational") {
            q_var_t var = get_q_var(py_statement.attr("var"));
            q_linear_expression_t expr = get_q_linear_expression(py_statement.attr("expression"));
            block.assign(var, expr);
          }
          else {
            throw_error("invalid variable type: %s", type);
          }
        }
        else if(class_name == "Assert") {
          object type = py_statement.attr("constraint").attr("type");

          if(type == "int") {
            z_linear_constraint_t cst = get_z_linear_constraint(py_statement.attr("constraint"));
            block.assertion(cst);
          }
          else if(type == "rational") {
            q_linear_constraint_t cst = get_q_linear_constraint(py_statement.attr("constraint"));
            block.assertion(cst);
          }
          else {
            throw_error("invalid LinearConstraint type: %s", type);
          }
        }
        else if(class_name == "Checkpoint") {
          std::string name = extract< std::string >(py_statement.attr("name"));
          block.check(name);
        }
        else {
          throw_error("invalid statement: %s", class_name);
        }
      }
    }

    void throw_error(std::string format, object value) {
      object py_message = format % value.attr("__repr__")();
      std::string message = extract< std::string >(py_message);
      throw ikos::error(message);
    }

}; // class CFGFactory

/*
 * Conversion class, from IKOS to Python
 */
class PythonFactory {
  public:
    python::list get_z_linear_expression(z_linear_expression_t expr) {
      python::list result;

      for(z_linear_expression_t::iterator it = expr.begin(); it != expr.end(); ++it) {
        result.append(python::make_tuple(it->first.si(), it->second.name().str()));
      }

      if(expr.constant() != 0) {
        result.append(expr.constant().si());
      }

      return result;
    }

    python::list get_q_linear_expression(q_linear_expression_t expr) {
      python::list result;

      for(q_linear_expression_t::iterator it = expr.begin(); it != expr.end(); ++it) {
        object factor = fraction(it->first.numerator().si(), it->first.denominator().si());
        result.append(python::make_tuple(factor, it->second.name().str()));
      }

      if(expr.constant() != 0) {
        object py_cst = fraction(expr.constant().numerator().si(), expr.constant().denominator().si());
        result.append(py_cst);
      }

      return result;
    }

    python::tuple get_z_linear_constraint(z_extended_constraint_t cst) {
      python::list py_expr = get_z_linear_expression(cst.expression());

      switch(cst.kind()) {
        case z_extended_constraint_t::INF:
          return python::make_tuple(py_expr, "<");
        case z_extended_constraint_t::INF_EQ:
          return python::make_tuple(py_expr, "<=");
        case z_extended_constraint_t::SUP:
          return python::make_tuple(py_expr, ">");
        case z_extended_constraint_t::SUP_EQ:
          return python::make_tuple(py_expr, ">=");
        case z_extended_constraint_t::EQ:
          return python::make_tuple(py_expr, "=");
        case z_extended_constraint_t::NOT_EQ:
          return python::make_tuple(py_expr, "!=");
        case z_extended_constraint_t::MOD:
          return python::make_tuple(py_expr, "mod", cst.modulus().get().si());
        default:
          throw error("unreachable");
      }
    }

    python::tuple get_q_linear_constraint(q_extended_constraint_t cst) {
      python::list py_expr = get_q_linear_expression(cst.expression());

      switch(cst.kind()) {
        case q_extended_constraint_t::INF:
          return python::make_tuple(py_expr, "<");
        case q_extended_constraint_t::INF_EQ:
          return python::make_tuple(py_expr, "<=");
        case q_extended_constraint_t::SUP:
          return python::make_tuple(py_expr, ">");
        case q_extended_constraint_t::SUP_EQ:
          return python::make_tuple(py_expr, ">=");
        case q_extended_constraint_t::EQ:
          return python::make_tuple(py_expr, "=");
        case q_extended_constraint_t::NOT_EQ:
          return python::make_tuple(py_expr, "!=");
        default:
          throw error("unreachable");
      }
    }

    python::list get_z_linear_constraints(z_extended_constraint_system_t csts) {
      python::list result;

      for(z_extended_constraint_system_t::iterator it = csts.begin(); it != csts.end(); ++it) {
        result.append(get_z_linear_constraint(*it));
      }

      return result;
    }

    python::list get_q_linear_constraints(q_extended_constraint_system_t csts) {
      python::list result;

      for(q_extended_constraint_system_t::iterator it = csts.begin(); it != csts.end(); ++it) {
        result.append(get_q_linear_constraint(*it));
      }

      return result;
    }

}; // class PythonFactory

/*
 * Fix point computation
 */
template< typename ZDomain, typename QDomain>
dict compute_fixpoint_call(cfg_t& cfg) {
  typedef typename fixpoint_iterator< ZDomain, QDomain >::AbstractValue AbstractValue;

  PythonFactory py_factory;
  domain_constraints< z_number, VarName > z_domain_export;
  domain_constraints< q_number, VarName > q_domain_export;
  dict result;

  fixpoint_iterator< ZDomain, QDomain > it(cfg);
  it.run_from_top();

  std::map< CheckPointName, AbstractValue >& checkpoints = it.checkpoints();
  for(typename std::map< CheckPointName, AbstractValue >::iterator it = checkpoints.begin(); it != checkpoints.end(); ++it) {
    CheckPointName name = it->first;

    z_extended_constraint_system_t z_csts = z_domain_export.get_constraints(it->second.first());
    q_extended_constraint_system_t q_csts = q_domain_export.get_constraints(it->second.second());

    python::list py_z_csts = py_factory.get_z_linear_constraints(z_csts);
    python::list py_q_csts = py_factory.get_q_linear_constraints(q_csts);

    result[name] = python::make_tuple(py_z_csts, py_q_csts);
  }

  return result;
}

/*
 * Ikos Exceptions
 */

/* Python C-API, only to define a custom exception (impossible with boost::python) */
PyObject* create_exception_class(const char* name, PyObject* base_type = PyExc_Exception)
{
  std::string scope_name = extract< std::string >(scope().attr("__name__"));
  std::string qualified_name = scope_name + "." + name;

  PyObject* type_obj = PyErr_NewException(const_cast< char* >(qualified_name.c_str()), base_type, 0);

  if(!type_obj)
    throw_error_already_set();

  scope().attr(name) = handle<>(borrowed(type_obj));
  return type_obj;
}

PyObject* ikos_exception = NULL;

void ikos_exception_translator(ikos::error const& e) {
  /*
   * Bad design in IKOS:
   * boost::python enforces having a const& argument for exception translators,
   * and error::message() is not const, that's why I use a copy here.
   */
  ikos::error e_copy(e);
  PyErr_SetString(ikos_exception, e_copy.message().c_str());
}

/*
 * Binding functions
 */

void print_linear_expression(object py_expr) {
  if(py_expr.attr("type") == "int") {
    z_linear_expression_t expr = CFGFactory().get_z_linear_expression(py_expr);
    std::cout << expr << std::endl;
  }
  else {
    q_linear_expression_t expr = CFGFactory().get_q_linear_expression(py_expr);
    std::cout << expr << std::endl;
  }
}

void print_linear_constraint(object py_constraint) {
  if(py_constraint.attr("type") == "int") {
    z_linear_constraint_t cst = CFGFactory().get_z_linear_constraint(py_constraint);
    std::cout << cst << std::endl;
  }
  else {
    q_linear_constraint_t cst = CFGFactory().get_q_linear_constraint(py_constraint);
    std::cout << cst << std::endl;
  }
}

void print_cfg(object py_cfg) {
  cfg_t cfg = CFGFactory().get_cfg(py_cfg);
  std::cout << cfg << std::endl;
}

/* it's ugly, but there is no other way */

#define ZDOMAIN_FROM_PYTHON(var, type_name, todo)                        \
  do {                                                                   \
    if((var) == "constant") {                                            \
      typedef constant_domain< z_number, VarName > type_name;            \
      todo;                                                              \
    } else if((var) == "interval") {                                     \
      typedef interval_domain< z_number, VarName > type_name;            \
      todo;                                                              \
    } else if((var) == "octagon") {                                      \
      typedef octagon< z_number, VarName > type_name;                    \
      todo;                                                              \
    }                                                                    \
  } while (0)

#define QDOMAIN_FROM_PYTHON(var, type_name, todo)                        \
  do {                                                                   \
    if((var) == "constant") {                                            \
      typedef constant_domain< q_number, VarName > type_name;            \
      todo;                                                              \
    } else if((var) == "interval") {                                     \
      typedef interval_domain< q_number, VarName > type_name;            \
      todo;                                                              \
    } else if((var) == "octagon") {                                      \
      typedef octagon< q_number, VarName > type_name;                    \
      todo;                                                              \
    } else {                                                             \
      throw ikos::error("cannot use this domain with rational numbers"); \
    }                                                                    \
  } while (0)

dict compute_fixpoint(object py_cfg, std::string py_z_domain, std::string py_q_domain) {
  cfg_t cfg = CFGFactory().get_cfg(py_cfg);

  #define FIXPOINT_CALL return compute_fixpoint_call< ZDomain, QDomain >(cfg)
  #define FIXPOINT_CALL_QDOMAIN QDOMAIN_FROM_PYTHON(py_q_domain, QDomain, FIXPOINT_CALL)
  ZDOMAIN_FROM_PYTHON(py_z_domain, ZDomain, FIXPOINT_CALL_QDOMAIN);
  #undef FIXPOINT_CALL_QDOMAIN
  #undef FIXPOINT_CALL

  return dict(); // unreachable
}

BOOST_PYTHON_MODULE(apicore) {
  /* import */
  fraction = import("fractions").attr("Fraction");

  /* exception handling */
  ikos_exception = create_exception_class("IkosException");
  register_exception_translator< ikos::error >(ikos_exception_translator);

  /* for debugging purpose */
  def("print_linear_expression", print_linear_expression, "Print a LinearExpression");
  def("print_linear_constraint", print_linear_constraint, "Print a LinearConstraint");
  def("print_cfg", print_cfg, "Print a Cfg");

  /* IKOS call */
  def("compute_fixpoint", compute_fixpoint, "Compute a fix point");
}


// This file is part of the dune-stuff project:
//   https://github.com/wwu-numerik/dune-stuff
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_STUFF_FUNCTIONS_DERIVED_HH
#define DUNE_STUFF_FUNCTIONS_DERIVED_HH

#include <type_traits>
#include <memory>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/typetraits.hh>

#include <dune/stuff/common/exceptions.hh>
#include <dune/stuff/common/memory.hh>

#include "interfaces.hh"

namespace Dune {
namespace Stuff {
namespace Functions {
namespace internal {

enum class Derivative
{
  divergence
};

template <class FunctionType, Derivative derivative>
class SelectDerived
{
  static_assert(is_localizable_function<FunctionType>::value, "FunctionType has to be a LocalizableFunction!");

public:
  typedef typename FunctionType::EntityType E;
  typedef typename FunctionType::DomainFieldType D;
  static const size_t d = FunctionType::dimDomain;
  typedef typename FunctionType::RangeFieldType R;

private:
  template <class F>
  class Choose
  {
    template <size_t d, size_t r, size_t rC, Derivative der, bool anything = true>
    class Dimension
    {
      static_assert(!anything, "No derivative for these dimensions available!");
    };

    template <size_t d, bool anything>
    class Dimension<d, d, 1, Derivative::divergence, anything>
    {
    public:
      static const size_t r  = 1;
      static const size_t rC = 1;
    };

  public:
    static const size_t r  = Dimension<d, F::dimRange, F::dimRangeCols, derivative>::r;
    static const size_t rC = Dimension<d, F::dimRange, F::dimRangeCols, derivative>::rC;
  }; // class SelectDerived

public:
  static const size_t r  = Choose<FunctionType>::r;
  static const size_t rC = Choose<FunctionType>::rC;

  typedef typename FunctionType::LocalfunctionType FunctionLocalfunctionType;
  typedef typename LocalfunctionInterface<E, D, d, R, r, rC>::DomainType DomainType;
  typedef typename LocalfunctionInterface<E, D, d, R, r, rC>::RangeType RangeType;
  typedef typename LocalfunctionInterface<E, D, d, R, r, rC>::JacobianRangeType JacobianRangeType;

private:
  template <Derivative der, bool anything = true>
  class Call
  {
    static_assert(!anything, "Nothing available for this derivative!");
  }; // class Call

  template <bool anything>
  class Call<Derivative::divergence, anything>
  {
  public:
    static std::string type() { return "divergence"; }

    static size_t order(const size_t ord)
    {
      return boost::numeric_cast<size_t>(std::max(boost::numeric_cast<ssize_t>(ord) - 1, ssize_t(0)));
    }

    static void evaluate(const FunctionLocalfunctionType& func_local, const DomainType& xx, RangeType& ret)
    {
      typename FunctionLocalfunctionType::JacobianRangeType tmp_jac(0.0);
      func_local.jacobian(xx, tmp_jac);
      ret *= 0.0;
      for (size_t dd = 0; dd < d; ++dd)
        ret[0] += tmp_jac[dd][dd];
    } // ... evaluate(...)

    static void jacobian(const FunctionLocalfunctionType& /*func_local*/, const DomainType& /*xx*/,
                         JacobianRangeType& /*ret*/)
    {
      DUNE_THROW(NotImplemented, "for divergence!");
    }
  }; // class Call< ..., divergence >

public:
  static std::string type() { return Call<derivative>::type(); }

  static size_t order(const size_t ord) { return Call<derivative>::order(ord); }

  static void evaluate(const FunctionLocalfunctionType& func_local, const DomainType& xx, RangeType& ret)
  {
    Call<derivative>::evaluate(func_local, xx, ret);
  }

  static void jacobian(const FunctionLocalfunctionType& func_local, const DomainType& xx, JacobianRangeType& ret)
  {
    Call<derivative>::jacobian(func_local, xx, ret);
  }
}; // class SelectDerived

template <class FunctionType, Derivative derivative>
class DerivedLocalFunction
    : public LocalfunctionInterface<
          typename SelectDerived<FunctionType, derivative>::E, typename SelectDerived<FunctionType, derivative>::D,
          SelectDerived<FunctionType, derivative>::d, typename SelectDerived<FunctionType, derivative>::R,
          SelectDerived<FunctionType, derivative>::r, SelectDerived<FunctionType, derivative>::rC>
{
  typedef LocalfunctionInterface<
      typename SelectDerived<FunctionType, derivative>::E, typename SelectDerived<FunctionType, derivative>::D,
      SelectDerived<FunctionType, derivative>::d, typename SelectDerived<FunctionType, derivative>::R,
      SelectDerived<FunctionType, derivative>::r, SelectDerived<FunctionType, derivative>::rC> BaseType;

  typedef SelectDerived<FunctionType, derivative> Select;

public:
  typedef typename BaseType::EntityType EntityType;
  typedef typename BaseType::DomainType DomainType;
  typedef typename BaseType::RangeType RangeType;
  typedef typename BaseType::JacobianRangeType JacobianRangeType;

  DerivedLocalFunction(const FunctionType& func, const EntityType& ent)
    : BaseType(ent), func_local_(func.local_function(this->entity()))
  {
  }

  virtual size_t order() const override final { return Select::order(func_local_->order()); }

  virtual void evaluate(const DomainType& xx, RangeType& ret) const override final
  {
    Select::evaluate(*func_local_, xx, ret);
  }

  virtual void jacobian(const DomainType& xx, JacobianRangeType& ret) const override final
  {
    Select::jacobian(*func_local_, xx, ret);
  }

private:
  const std::unique_ptr<const typename FunctionType::LocalfunctionType> func_local_;
}; // class DerivedLocalFunction

template <class FunctionType, Derivative derivative>
class Derived
    : public LocalizableFunctionInterface<
          typename SelectDerived<FunctionType, derivative>::E, typename SelectDerived<FunctionType, derivative>::D,
          SelectDerived<FunctionType, derivative>::d, typename SelectDerived<FunctionType, derivative>::R,
          SelectDerived<FunctionType, derivative>::r, SelectDerived<FunctionType, derivative>::rC>
{
  typedef LocalizableFunctionInterface<
      typename SelectDerived<FunctionType, derivative>::E, typename SelectDerived<FunctionType, derivative>::D,
      SelectDerived<FunctionType, derivative>::d, typename SelectDerived<FunctionType, derivative>::R,
      SelectDerived<FunctionType, derivative>::r, SelectDerived<FunctionType, derivative>::rC> BaseType;
  typedef Common::ConstStorageProvider<FunctionType> FunctionStorageType;
  typedef Derived<FunctionType, derivative> ThisType;

public:
  typedef typename BaseType::EntityType EntityType;
  typedef typename BaseType::LocalfunctionType LocalfunctionType;

  Derived(const FunctionType& func, const std::string nm = "")
    : func_(Common::make_unique<FunctionStorageType>(func))
    , name_(nm.empty() ? SelectDerived<FunctionType, derivative>::type() + " of '" + func.name() + "'" : nm)
  {
  }

  Derived(const std::shared_ptr<const FunctionType> func, const std::string nm = "")
    : func_(Common::make_unique<FunctionStorageType>(func))
    , name_(nm.empty()
                ? SelectDerived<FunctionType, derivative>::type() + " of '" + func_->storage_access().name() + "'"
                : nm)
  {
  }

  Derived(ThisType&& source) = default;
  Derived(const ThisType& other) = delete;

  ThisType& operator=(const ThisType& other) = delete;
  ThisType& operator=(ThisType&& other) = delete;

  virtual std::unique_ptr<LocalfunctionType> local_function(const EntityType& entity) const override final
  {
    typedef DerivedLocalFunction<FunctionType, derivative> RealLocalFunctionType;
    assert(func_);
    return DSC::make_unique<RealLocalFunctionType>(func_->storage_access(), entity);
  } // ... local_function(...)

  virtual ThisType* copy() const { DUNE_THROW(NotImplemented, "Are you kidding me?"); }

  virtual std::string type() const override final
  {
    return SelectDerived<FunctionType, derivative>::type() + " of '" + func_->storage_access().type() + "'";
  }

  virtual std::string name() const override final { return name_; }

private:
  std::unique_ptr<const FunctionStorageType> func_;
  const std::string name_;
}; // class Derived

} // namespace internal

template <class FunctionType>
class Divergence : public internal::Derived<FunctionType, internal::Derivative::divergence>
{
  typedef internal::Derived<FunctionType, internal::Derivative::divergence> BaseType;

public:
  template <class... Args>
  Divergence(Args&&... args)
    : BaseType(std::forward<Args>(args)...)
  {
  }
}; // class Divergence

template <class T, class... Args>
std::shared_ptr<Divergence<T>> make_divergence(const T& func, Args&&... args)
{
  return std::make_shared<Divergence<T>>(func, std::forward<Args>(args)...);
}

template <class T, class... Args>
std::shared_ptr<Divergence<T>> make_divergence(std::shared_ptr<T> func, Args&&... args)
{
  return std::make_shared<Divergence<T>>(func, std::forward<Args>(args)...);
}

} // namespace Functions
} // namespace Stuff
} // namespace Dune

#endif // DUNE_STUFF_FUNCTIONS_DERIVED_HH