#include <engine/Neighbours.hpp>

#include <Eigen/Dense>

namespace Engine
{
    namespace Neighbours
    {
        std::vector<scalar> Get_Shell_Radius(const Data::Geometry & geometry, const int n_shells)
        {
            const scalar shell_width = 1e-3;
            auto shell_radius = std::vector<scalar>(n_shells);

            Vector3 a = geometry.bravais_vectors[0];
            Vector3 b = geometry.bravais_vectors[1];
            Vector3 c = geometry.bravais_vectors[2];

            // The n_shells + 2 is a value that is big enough by experience to
            // produce enough needed shells, but is small enough to run sufficiently fast
            int tMax = n_shells + 2;
            int imax = std::min(tMax, geometry.n_cells[0]-1),
                jmax = std::min(tMax, geometry.n_cells[1]-1),
                kmax = std::min(tMax, geometry.n_cells[2]-1);

            // Abort condidions for all 3 vectors
            if (a.norm() == 0.0) imax = 0;
            if (b.norm() == 0.0) jmax = 0;
            if (c.norm() == 0.0) kmax = 0;

            int i, j, k, iatom, jatom, ishell;
            scalar current_radius=0, dx, min_distance=0;
            Vector3 x0={0,0,0}, x1={0,0,0};
            for (ishell = 0; ishell < n_shells; ++ishell)
            {
                min_distance = current_radius;
                current_radius = 1e10;
                for (iatom = 0; iatom < geometry.n_cell_atoms; ++iatom)
                {
                    x0 =  geometry.cell_atoms[iatom][0] * a
                        + geometry.cell_atoms[iatom][1] * b
                        + geometry.cell_atoms[iatom][2] * c;
                    // Note: due to symmetry we only need to check half the space
                    for (i = imax; i >= 0; --i)
                    {
                        for (j = jmax; j >= -jmax; --j)
                        {
                            for (k = kmax; k >= -kmax; --k)
                            {
                                for (jatom = 0; jatom < geometry.n_cell_atoms; ++jatom)
                                {
                                    if ( !( iatom==jatom && i==0 && j==0 && k==0 ) )
                                    {
                                        x1 =  geometry.cell_atoms[jatom][0] * a
                                            + geometry.cell_atoms[jatom][1] * b
                                            + geometry.cell_atoms[jatom][2] * c
                                            + i*a + j*b + k*c;
                                        dx = (x0-x1).norm();
                                        if (dx - min_distance > shell_width && dx < current_radius)
                                        {
                                            current_radius = dx;
                                            shell_radius[ishell] = dx;
                                        }
                                    }
                                }//endfor jatom
                            }//endfor k
                        }//endfor j
                    }//endfor i
                }//endfor iatom
            }

            return shell_radius;
        }

        void Get_Neighbours_in_Shells(const Data::Geometry & geometry, int n_shells, pairfield & neighbours, intfield & shells, bool use_redundant_neighbours)
        {
            const scalar shell_width = 1e-3;
            auto shell_radius = Get_Shell_Radius(geometry, n_shells);

            Vector3 a = geometry.bravais_vectors[0];
            Vector3 b = geometry.bravais_vectors[1];
            Vector3 c = geometry.bravais_vectors[2];

            // The n_shells + 2 is a value that is big enough by experience to
            // produce enough needed shells, but is small enough to run sufficiently fast
            int tMax = n_shells + 2;
            int imax = std::min(tMax, geometry.n_cells[0]-1),
                jmax = std::min(tMax, geometry.n_cells[1]-1),
                kmax = std::min(tMax, geometry.n_cells[2]-1);
            int imin, jmin, kmin, jatommin;
            // If redundant neighbours should not be used, we restrict the search to half of the space
            if( use_redundant_neighbours )
            {
                imin=-imax; jmin=-jmax; kmin=-kmax;
            }
            else
            {
                imin=0; jmin=-jmax; kmin=-kmax;
            }

            // Abort condidions for all 3 vectors
            if (a.norm() == 0.0) imax = 0;
            if (b.norm() == 0.0) jmax = 0;
            if (c.norm() == 0.0) kmax = 0;

            int i, j, k, iatom, jatom, ishell;
            scalar dx, radius;
            Vector3 x0={0,0,0}, x1={0,0,0};
            for (iatom = 0; iatom < geometry.n_cell_atoms; ++iatom)
            {
                if( use_redundant_neighbours )
                    jatommin=0;
                else
                    jatommin=iatom;

                x0 =  geometry.cell_atoms[iatom][0] * a
                    + geometry.cell_atoms[iatom][1] * b
                    + geometry.cell_atoms[iatom][2] * c;
                for (ishell = 0; ishell < n_shells; ++ishell)
                {
                    radius = shell_radius[ishell];
                    for (i = imax; i >= imin; --i)
                    {
                        for (j = jmax; j >= jmin; --j)
                        {
                            for (k = kmax; k >= kmin; --k)
                            {
                                for (jatom = jatommin; jatom < geometry.n_cell_atoms; ++jatom)
                                {
                                    if ((jatom > iatom) || (i>0 || (i==0 && j>0) || (i==0 && j==0 && k>0)) || use_redundant_neighbours)
                                    {
                                        x1 =  geometry.cell_atoms[jatom][0] * a
                                            + geometry.cell_atoms[jatom][1] * b
                                            + geometry.cell_atoms[jatom][2] * c
                                            + i*a + j*b + k*c;
                                        dx = (x0-x1).norm();
                                        if (std::abs(dx - radius) < shell_width)
                                        {
                                            Pair neigh;
                                            neigh.i = iatom;
                                            neigh.j = jatom;
                                            neigh.translations[0] = i;
                                            neigh.translations[1] = j;
                                            neigh.translations[2] = k;
                                            neighbours.push_back( neigh );
                                            shells.push_back(ishell);
                                        }
                                    }
                                }//endfor jatom
                            }//endfor k
                        }//endfor j
                    }//endfor i
                }//endfor ishell
            }//endfor iatom
        }

        pairfield Get_Pairs_in_Radius(const Data::Geometry & geometry, scalar radius)
        {
            auto pairs = pairfield(0);

            if (radius > 1e-6)
            {
                Vector3 a = geometry.bravais_vectors[0];
                Vector3 b = geometry.bravais_vectors[1];
                Vector3 c = geometry.bravais_vectors[2];

                Vector3 bounds_diff = geometry.bounds_max - geometry.bounds_min;
                Vector3 ratio = {
                    bounds_diff[0]/std::max(1, geometry.n_cells[0]),
                    bounds_diff[1]/std::max(1, geometry.n_cells[1]),
                    bounds_diff[2]/std::max(1, geometry.n_cells[2]) };

                // This should give enough translations to contain all DDI pairs
                int imax = 0, jmax = 0, kmax = 0;
                if ( bounds_diff[0] > 0 )
                    imax = std::min(geometry.n_cells[0], (int)(1.1 * radius * geometry.n_cells[0] / bounds_diff[0]));
                if ( bounds_diff[1] > 0 )
                    jmax = std::min(geometry.n_cells[1], (int)(1.1 * radius * geometry.n_cells[1] / bounds_diff[1]));
                if ( bounds_diff[2] > 0 )
                    kmax = std::min(geometry.n_cells[2], (int)(1.1 * radius * geometry.n_cells[2] / bounds_diff[2]));

                int i,j,k;
                scalar dx;
                Vector3 x0={0,0,0}, x1={0,0,0};

                // Abort condidions for all 3 vectors
                if (a.norm() == 0.0) imax = 0;
                if (b.norm() == 0.0) jmax = 0;
                if (c.norm() == 0.0) kmax = 0;

                for (int iatom = 0; iatom < geometry.n_cell_atoms; ++iatom)
                {
                    x0 = geometry.cell_atoms[iatom];
                    for (i = imax; i >= -imax; --i)
                    {
                        for (j = jmax; j >= -jmax; --j)
                        {
                            for (k = kmax; k >= -kmax; --k)
                            {
                                for (int jatom = 0; jatom < geometry.n_cell_atoms; ++jatom)
                                {
                                    x1 = geometry.cell_atoms[jatom] + i*a + j*b + k*c;
                                    dx = (x0-x1).norm();
                                    if (dx < radius)
                                    {
                                        pairs.push_back( {iatom, jatom, {i, j, k} } );
                                    }
                                }//endfor jatom
                            }//endfor k
                        }//endfor j
                    }//endfor i
                }//endfor iatom
            }

            return pairs;
        }

        Vector3 DMI_Normal_from_Pair(const Data::Geometry & geometry, const Pair & pair, int chirality)
        {
            Vector3 ta = geometry.bravais_vectors[0];
            Vector3 tb = geometry.bravais_vectors[1];
            Vector3 tc = geometry.bravais_vectors[2];

            int da = pair.translations[0];
            int db = pair.translations[1];
            int dc = pair.translations[2];

            Vector3 ipos = geometry.cell_atoms[pair.i];
            Vector3 jpos = geometry.cell_atoms[pair.j] + da*ta + db*tb + dc*tc;

            if (chirality == 1)
            {
                // Bloch chirality
                return (jpos - ipos).normalized();
            }
            else if (chirality == -1)
            {
                // Inverse Bloch chirality
                return (ipos - jpos).normalized();
            }
            else if (chirality == 2)
            {
                // Neel chirality (surface)
                return (jpos - ipos).normalized().cross(Vector3{0,0,1});
            }
            else if (chirality == -2)
            {
                // Inverse Neel chirality (surface)
                return Vector3{0,0,1}.cross((jpos - ipos).normalized());
            }
            else
            {
                return Vector3{ 0,0,0 };
            }
        }

        void DDI_from_Pair(const Data::Geometry & geometry, const Pair & pair, scalar & magnitude, Vector3 & normal)
        {
            Vector3 ta = geometry.bravais_vectors[0];
            Vector3 tb = geometry.bravais_vectors[1];
            Vector3 tc = geometry.bravais_vectors[2];

            int da = pair.translations[0];
            int db = pair.translations[1];
            int dc = pair.translations[2];

            Vector3 ipos = geometry.cell_atoms[pair.i];
            Vector3 jpos = geometry.cell_atoms[pair.j] + da*ta + db*tb + dc*tc;

            // Calculate positions and difference vector
            Vector3 vector_ij = jpos - ipos;

            // Length of difference vector
            magnitude = vector_ij.norm();
            normal = vector_ij.normalized();
        }

    }// end Namespace Neighbours
}// end Namespace Engine


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * 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.
 */
/**
 * @file math.hpp
 * @author Ondrej Prochazka <ondrej.prochazka@citationtech.net>
 *
 * Low level math helper functions
 */

#ifndef MATH_MATH_HPP
#define MATH_MATH_HPP

#include <algorithm>
#include <stdexcept>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "dbglog/dbglog.hpp"

namespace math {

namespace ublas = boost::numeric::ublas;

/** square function */
template <typename T>
T sqr( T val ) { return val * val; }

namespace detail {

// enabled for N == 1
template<int N, typename T>
typename std::enable_if<N == 1, T>::type pow( T value ) {
    return value;
}
// enabled for even N
template<int N, typename T>
typename std::enable_if<(N > 1) && (N & 1) == 0, T>::type pow( T value ) {
    return pow<N/2>(value * value);
}
// enabled for odd N
template<int N, typename T>
typename std::enable_if<(N > 1) && (N & 1) == 1, T>::type pow( T value ) {
    return pow<N/2>(value * value) * value;
}

} // namespace detail

/** Power function with integer compile-time exponent */
template<int N, typename T>
T pow( T value ) {
    return detail::pow<N>(value);
}

/** even and odd */

template <typename T>
bool even( T val ) { return ( val >> 1 << 1 == val ); }

template <typename T>
bool odd( T val ) { return ( val >> 1 << 1 != val ); }

/** interval check */

template<typename T>
bool ccinterval( const T & lb, const T  & ub, const T & value ) {
    return ( lb <= value && value <= ub );
}

/**
  * Signum function
  */

template <typename Value_t>
int sgn( const Value_t & value ) {
   if ( value > 0 ) return 1;
   if ( value < 0 ) return -1;
   return 0;
}

/** Clamp value to given range.
 */
template <typename T>
inline T clamp(T value, T min, T max)
{
    return std::max(min, std::min(value, max));
}

template <typename T>
inline bool isInteger(T value, T tolerance = T(0))
{
    T integer;
    return (std::abs(std::modf(value, &integer)) <= tolerance);
}

/**
  * Matrix inversion
  */

template <typename T, typename L, typename C>
ublas::matrix<T,L,C> matrixInvert( const ublas::matrix<T,L,C> & input ) {

    typedef ublas::permutation_matrix<std::size_t> pmatrix;

    // create a working copy of the input
    ublas::matrix<T,L,C> A(input);

    // create a permutation matrix for the LU-factorization
    pmatrix pm(A.size1());

    // perform LU-factorization
    auto res = lu_factorize(A,pm);
    if( res != 0 ) {
        LOGTHROW(warn1, std::runtime_error)
            << "Singular matrix in math::matrixInvert. Aborting.";
    }

    // create identity matrix of "inverse"
    ublas::matrix<T,L,C> inverse = ublas::identity_matrix<T>(A.size1());

    // backsubstitute to get the inverse
    lu_substitute(A, pm, inverse);
    return inverse;
}

template <typename T, typename L, typename C>
bool matrixInvertInplace( ublas::matrix<T,L,C> &input)
{
    typedef ublas::permutation_matrix<std::size_t> pmatrix;

    // create a working copy of the input
    ublas::matrix<T,L,C> A(input);

    // create a permutation matrix for the LU-factorization
    pmatrix pm(A.size1());

    // perform LU-factorization
    auto res = lu_factorize(A,pm);
    if (res != 0) { return false; }

    // backsubstitute to get the inverse
    lu_substitute(A, pm, input);
    return true;
}

/**
 * Returns the trace of a square matrix.
 */
template <class T, class L, class C>
double trace(ublas::matrix<T, L, C> m) {
    const int n = m.size1();
    ublas::matrix_vector_range<ublas::matrix<T, L, C>> diag
            (m, ublas::range(0, n), ublas::range(0, n));
    return sum(diag);
}

/**
 * Matrix determinant
 */

template <class matrix_T>
double determinant(ublas::matrix_expression<matrix_T> const& mat_r)
{
  double det = 1.0;

  matrix_T mLu(mat_r() );
  ublas::permutation_matrix<std::size_t> pivots(mat_r().size1() );

  int is_singular = lu_factorize(mLu, pivots);

  if (!is_singular)
  {
    for (std::size_t i=0; i < pivots.size(); ++i)
    {
      if (pivots(i) != i)
        det *= -1.0;

      det *= mLu(i,i);
    }
  }
  else
    det = 0.0;

  return det;
}

/**
 * Solve a 2x2 linear system
 */
template<typename MatrixType, typename VectorType>
void solve2x2(const MatrixType &mat, const VectorType &rhs, VectorType &result)
{
    double det = mat(0,0)*mat(1,1) - mat(0,1)*mat(1,0);

    // check the determinant
    if (std::abs(det) < 1e-14) {
        LOGTHROW(err1, std::runtime_error) << "Singular matrix in solve2x2.";
    }

    det = 1.0 / det;
    result(0) = (rhs(0)*mat(1,1) - mat(0,1)*rhs(1)) * det;
    result(1) = (mat(0,0)*rhs(1) - rhs(0)*mat(1,0)) * det;
}

} // namespace math

#endif // MATH_MATH_HPP


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

#include "Domain/CoordinateMaps/CubicScale.hpp"

#include <array>
#include <boost/none.hpp>
#include <ostream>
#include <pup.h>
#include <pup_stl.h>
#include <utility>

#include "ControlSystem/FunctionOfTime.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "ErrorHandling/Error.hpp"
#include "NumericalAlgorithms/RootFinding/NewtonRaphson.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/DereferenceWrapper.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/MakeWithValue.hpp"

namespace CoordMapsTimeDependent {

CubicScale::CubicScale(const double outer_boundary) noexcept
    : outer_boundary_(outer_boundary) {
  if (outer_boundary_ <= 0.0) {
    ERROR("For invertability, we require outer_boundary to be positive.\n");
  }
}

template <typename T>
std::array<tt::remove_cvref_wrap_t<T>, 1> CubicScale::operator()(
    const std::array<T, 1>& source_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  const auto a_of_t = map_list.at(f_of_t_a_).func(time)[0][0];
  const auto b_of_t = map_list.at(f_of_t_b_).func(time)[0][0];
  return {{source_coords[0] *
           (a_of_t +
            (b_of_t - a_of_t) * square(source_coords[0] / outer_boundary_))}};
}

template <typename T>
boost::optional<std::array<tt::remove_cvref_wrap_t<T>, 1>> CubicScale::inverse(
    const std::array<T, 1>& target_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  // the original coordinates are found by solving for the roots
  // of (b-a)/X^2*\xi^3 + a*\xi - x = 0, where a and b are the FunctionsOfTime,
  // X is the outer_boundary, and x represents the mapped coordinates
  const auto a_of_t = map_list.at(f_of_t_a_).func(time)[0][0];
  const auto b_of_t = map_list.at(f_of_t_b_).func(time)[0][0];

  // these checks ensure that the function is monotonically increasing
  // and that there is one real root in the domain of \xi, [0,X]
  if (a_of_t <= 0.0) {
    ERROR("We require expansion_a > 0 for invertibility, however expansion_a = "
          << a_of_t << ".");
  }
  if (b_of_t < 2.0 / 3.0 * a_of_t or b_of_t <= 0.0) {
    ERROR("The map is invertible only if 0 < expansion_b < expansion_a*2/3, "
          << " but expansion_b = " << b_of_t << " and expansion_a = " << a_of_t
          << ".");
  }

  // Make the coordinates dimensionless
  const tt::remove_cvref_wrap_t<T> x_bar = target_coords[0] / outer_boundary_;

  // Check if x_bar is outside of the range of the map
  if (x_bar < 0.0 or x_bar > b_of_t) {
    return boost::none;
  }

  // with the assumptions above:
  // x_bar lies within the range [0,b]
  // and \xi_bar = \xi/X is restricted to the domain [0,1]
  // For an initial guess, we provide a linearly approximated solution for
  // \xi_bar, which is just x_bar/b.
  const tt::remove_cvref_wrap_t<T> initial_guess = x_bar / b_of_t;
  const double cubic_coef_a = (b_of_t - a_of_t);

  const auto cubic_and_deriv =
      [&cubic_coef_a, &a_of_t, &x_bar ](double x) noexcept {
    return std::make_pair(x * (cubic_coef_a * square(x) + a_of_t) - x_bar,
                          3.0 * cubic_coef_a * square(x) + a_of_t);
  };

  // The original implementation of this inverse function used a cubic
  // equation solver. However, given that the problem of finding the inverse
  // in this case is well constrained -- using a Newton-Raphson root find with
  // a linearly approximated guess is almost twice as fast.
  // Using google benchmark,
  // the CubicEquation solver: ~ 480 ns
  // boost implemented Newton-Raphson: ~ 280 ns
  // minimal Newton-Raphson from Numerical Recipes: ~ 255 ns
  // Despite the minimal Newton-Raphson being more efficient than the boost
  // version, here we utilize the boost implementation, as it includes
  // additional checks for zero derivative, checks on bounds, and can implement
  // bisection if necessary.
  return {
      {{outer_boundary_ * RootFinder::newton_raphson(
                              cubic_and_deriv, initial_guess, 0.0, 1.0, 14)}}};
}

template <typename T>
std::array<tt::remove_cvref_wrap_t<T>, 1> CubicScale::frame_velocity(
    const std::array<T, 1>& source_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  const auto dt_a_of_t = map_list.at(f_of_t_a_).func_and_deriv(time)[1][0];
  const auto dt_b_of_t = map_list.at(f_of_t_b_).func_and_deriv(time)[1][0];
  const auto frame_vel =
      source_coords[0] *
      (dt_a_of_t +
       (dt_b_of_t - dt_a_of_t) * square(source_coords[0] / outer_boundary_));

  return {{frame_vel}};
}

template <typename T>
tnsr::Ij<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame> CubicScale::jacobian(
    const std::array<T, 1>& source_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  const auto a_of_t = map_list.at(f_of_t_a_).func(time)[0][0];
  const auto b_of_t = map_list.at(f_of_t_b_).func(time)[0][0];
  auto jac{
      make_with_value<tnsr::Ij<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame>>(
          dereference_wrapper(source_coords[0]), 0.0)};

  get<0, 0>(jac) =
      a_of_t +
      3.0 * (b_of_t - a_of_t) * square(source_coords[0] / outer_boundary_);

  return jac;
}

template <typename T>
tnsr::Ij<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame>
CubicScale::inv_jacobian(
    const std::array<T, 1>& source_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  const auto a_of_t = map_list.at(f_of_t_a_).func(time)[0][0];
  const auto b_of_t = map_list.at(f_of_t_b_).func(time)[0][0];
  auto inv_jac{
      make_with_value<tnsr::Ij<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame>>(
          dereference_wrapper(source_coords[0]), 0.0)};

  get<0, 0>(inv_jac) = 1.0 / (a_of_t +
                              3.0 * (b_of_t - a_of_t) *
                                  square(source_coords[0] / outer_boundary_));

  return inv_jac;
}

template <typename T>
tnsr::Iaa<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame> CubicScale::hessian(
    const std::array<T, 1>& source_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept {
  const auto a_of_t_and_derivs = map_list.at(f_of_t_a_).func_and_2_derivs(time);
  const auto b_of_t_and_derivs = map_list.at(f_of_t_b_).func_and_2_derivs(time);

  auto result{
      make_with_value<tnsr::Iaa<tt::remove_cvref_wrap_t<T>, 1, Frame::NoFrame>>(
          dereference_wrapper(source_coords[0]), 0.0)};
  // time-time
  get<0, 0, 0>(result) = a_of_t_and_derivs[2][0] * source_coords[0] +
                         (b_of_t_and_derivs[2][0] - a_of_t_and_derivs[2][0]) *
                             cube(source_coords[0]) / square(outer_boundary_);
  // time-space
  get<0, 0, 1>(result) =
      a_of_t_and_derivs[1][0] +
      3.0 * (b_of_t_and_derivs[1][0] - a_of_t_and_derivs[1][0]) *
          square(source_coords[0] / outer_boundary_);
  // space-space
  get<0, 1, 1>(result) = 6.0 *
                         (b_of_t_and_derivs[0][0] - a_of_t_and_derivs[0][0]) *
                         source_coords[0] / square(outer_boundary_);

  return result;
}

void CubicScale::pup(PUP::er& p) noexcept {
  p | f_of_t_a_;
  p | f_of_t_b_;
  p | outer_boundary_;
}

bool operator==(const CoordMapsTimeDependent::CubicScale& lhs,
                const CoordMapsTimeDependent::CubicScale& rhs) noexcept {
  return lhs.f_of_t_a_ == rhs.f_of_t_a_ and lhs.f_of_t_b_ == rhs.f_of_t_b_ and
         lhs.outer_boundary_ == rhs.outer_boundary_;
}

// Explicit instantiations
/// \cond
template boost::optional<std::array<tt::remove_cvref_wrap_t<double>, 1>>
CubicScale::inverse(
    const std::array<double, 1>& target_coords, const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept;
template boost::optional<std::array<
    tt::remove_cvref_wrap_t<std::reference_wrapper<const double>>, 1>>
CubicScale::inverse(
    const std::array<std::reference_wrapper<const double>, 1>& target_coords,
    const double time,
    const std::unordered_map<std::string, FunctionOfTime&>& map_list) const
    noexcept;

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

#define INSTANTIATE(_, data)                                                   \
  template std::array<tt::remove_cvref_wrap_t<DTYPE(data)>, 1> CubicScale::    \
  operator()(const std::array<DTYPE(data), 1>& source_coords,                  \
             const double time,                                                \
             const std::unordered_map<std::string, FunctionOfTime&>& map_list) \
      const noexcept;                                                          \
  template std::array<tt::remove_cvref_wrap_t<DTYPE(data)>, 1>                 \
  CubicScale::frame_velocity(                                                  \
      const std::array<DTYPE(data), 1>& source_coords, const double time,      \
      const std::unordered_map<std::string, FunctionOfTime&>& map_list)        \
      const noexcept;                                                          \
  template tnsr::Ij<tt::remove_cvref_wrap_t<DTYPE(data)>, 1, Frame::NoFrame>   \
  CubicScale::jacobian(                                                        \
      const std::array<DTYPE(data), 1>& source_coords, const double time,      \
      const std::unordered_map<std::string, FunctionOfTime&>& map_list)        \
      const noexcept;                                                          \
  template tnsr::Ij<tt::remove_cvref_wrap_t<DTYPE(data)>, 1, Frame::NoFrame>   \
  CubicScale::inv_jacobian(                                                    \
      const std::array<DTYPE(data), 1>& source_coords, const double time,      \
      const std::unordered_map<std::string, FunctionOfTime&>& map_list)        \
      const noexcept;                                                          \
  template tnsr::Iaa<tt::remove_cvref_wrap_t<DTYPE(data)>, 1, Frame::NoFrame>  \
  CubicScale::hessian(                                                         \
      const std::array<DTYPE(data), 1>& source_coords, const double time,      \
      const std::unordered_map<std::string, FunctionOfTime&>& map_list)        \
      const noexcept;

GENERATE_INSTANTIATIONS(INSTANTIATE, (double, DataVector,
                                      std::reference_wrapper<const double>,
                                      std::reference_wrapper<const DataVector>))
#undef DTYPE
#undef INSTANTIATE
/// \endcond
}  // namespace CoordMapsTimeDependent


//-----------------------------------------------------------------------------
#include <utility>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>
#include <queue>
#include <set>
#include <map>
#include <string>
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <cstring>
#include <omp.h>
//-----------------------------------------------------------------------------
#include <ilcplex/ilocplex.h>
//-----------------------------------------------------------------------------
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
//-----------------------------------------------------------------------------
#define pb push_back
#define mp make_pair
#define MAXSTR 1024
#define MAX 100
#define EPS 1e-6

using namespace std;
using namespace boost;

#include "combinadic.h"

typedef pair<int, int> ii;
typedef adjacency_list< vecS, vecS, undirectedS,
    property<vertex_index_t, int>,
    property<edge_index_t, int> > Graph;
typedef vector< graph_traits<Graph>::edge_descriptor > vec_t;

bool found = false;
char errmsg[MAXSTR];

int nNodes = 0;
int nCols  = 0;
int cbSize = 4;

vector<vector<int>> graphIn;
map<vector<int>, int> colsId;

int sgn(double a) { return ((a > EPS) ? (1) : ((a < -EPS) ? (-1) : (0))); }
int cmp(double a, double b = 0.0) { return sgn(a - b); }

/*
    Print elapsed time.
    */
void printElapsedTime(double start, double stop)
{
    double elapsed = stop - start;
    printf("Elapsed time: %.3lfs.\n", elapsed);
}
//-----------------------------------------------------------------------------
// Mac
#ifdef __MACH__
#include <mach/clock.h>
#include <mach/mach.h>
#endif
//-----------------------------------------------------------------------------
/*  Get clock time.
    */
void current_utc_time(struct timespec *ts)
{
    #ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time
        clock_serv_t cclock;
        mach_timespec_t mts;
        host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
        clock_get_time(cclock, &mts);
        mach_port_deallocate(mach_task_self(), cclock);
        ts->tv_sec = mts.tv_sec;
        ts->tv_nsec = mts.tv_nsec;
    #else
        clock_gettime(CLOCK_REALTIME, ts);
    #endif
}

double getTime()
{
    timespec ts;
    current_utc_time(&ts);
    return double(ts.tv_sec) + double(ts.tv_nsec) / 1e9;
}
//-----------------------------------------------------------------------------
int faces_count = 0;
map< vector<int>, int > hasFace;
// Some planar face traversal visitors that will
// print the vertices and edges on the faces
struct output_visitor : public planar_face_traversal_visitor
{
    vector<int> faceVertices;
    void begin_face(){ printf("New face: "); }
    void end_face(){
        printf("\n");
        sort(faceVertices.begin(), faceVertices.end());
        hasFace[faceVertices] = 1;
        faceVertices.clear();
        faces_count++;
    }
};

struct vertex_output_visitor : public output_visitor
{
    template <typename Vertex>
    void next_vertex(Vertex v)
    {
        std::cout << v + 1 << " ";
        faceVertices.pb((int)v);
    }
};

struct edge_output_visitor : public output_visitor
{
    template <typename Edge>
    void next_edge(Edge e)
    {
        std::cout << e << " ";
    }
};
//-----------------------------------------------------------------------------
CPXENVptr env = NULL;
CPXLPptr  lp  = NULL;
//-----------------------------------------------------------------------------
void readData()
{
    printf("---------- Input graph ----------\n");
    //- - - - - - - - - - - - - - - - - - -
    scanf("%d", &nNodes);
    printf("%d\n", nNodes);
    // cbSize = nNodes-1;
    //- - - - - - - - - - - - - - - - - - -

    graphIn.resize( nNodes + 1 );
    for (int i = 1; i <= nNodes; i++)
    {
        graphIn[i].resize( nNodes + 1 );
    }

    //- - - - - - - - - - - - - - - - - - -
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++)
        {
            scanf("%d", &graphIn[i][j]);
            graphIn[j][i] = graphIn[i][j];
            printf("%d ", graphIn[i][j]);
        }
        printf("\n");
    }
    printf("---------------------------------\n\n");
    //- - - - - - - - - - - - - - - - - - -
}
//-----------------------------------------------------------------------------
int findColID( int i, int j, int k )
{
    map< vector<int>, int >::iterator itm;

    vector< int > v(3);
    v[0] = i;
    v[1] = j;
    v[2] = k;

    itm = colsId.find( v );
    if (itm == colsId.end())
    {
        return -1;
        // cout << "Cannot find colId x_" << j << '(' << h << ')' << '_' << k << '\n';
        // abort();
    }
    else
    {
        return itm->second;
    }
}
//-----------------------------------------------------------------------------
void buildModel()
{
    int status = 0;

    //- - - - - - - - - - - - - - - - - - -
    CPXchgobjsen( env, lp, CPX_MAX );

    nCols  = nNodes * (nNodes - 1) / 2;
    printf("nCols = %d\n", nCols);
    nCols += nNodes * (nNodes * nNodes - 3 * nNodes + 2) / 6;
    printf("nCols = %d\n", nCols);
    //- - - - - - - - - - - - - - - - - - -

    //- - - - - - - - - - - - - - - - - - -
    char    sense[2];
    int     rmatbeg[2];
    double  rhs[2];

    char*   ctype    = new char[nCols + 1];
    char**  cname    = new char*[nCols + 1];
    char**  rname    = new char*[1];
    rname[0] = new char[20];
    int*    rmatind  = new int[nCols + 1];
    double* rmatval  = new double[nCols + 1];
    double* obj      = new double[nCols + 1];
    double* lb       = new double[nCols + 1];
    double* ub       = new double[nCols + 1];
    //- - - - - - - - - - - - - - - - - - -

    //- - - - - - - - - - - - - - - - - - -
    int col = 0;
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++, col++)
        {
            vector<int> v(3);
            v[0] = i;
            v[1] = j;
            v[2] = 0;
            colsId[v] = col;

            cname[col] = new char[20];
            sprintf(cname[col], "x_%d_%d", i, j);
            lb[col]    = 0.0;
            ub[col]    = 1.0;
            ctype[col] = 'B';
            obj[col]   = graphIn[i][j];
        }
    }
    //- - - - - - - - - - - - - - - - - - -
    printf("col = %d\n", col);

    //- - - - - - - - - - - - - - - - - - -
    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++, col++)
            {
                vector<int> v(3);
                v[0] = i;
                v[1] = j;
                v[2] = k;
                colsId[v] = col;

                cname[col] = new char[20];
                sprintf(cname[col], "f_%d_%d_%d", i, j, k);
                lb[col]    = 0.0;
                ub[col]    = 1.0;
                ctype[col] = 'B';
                obj[col]   = 0.0;
            }
        }
    }
    //- - - - - - - - - - - - - - - - - - -
    printf("col = %d\n", col);

    //- - - - - - - - - - - - - - - - - - -
    if (status = CPXnewcols( env, lp, nCols, obj, lb, ub, ctype, (char **) cname ))
    {
        printf("CPXnewcols: Could not add new columns, error %d\n", status);
        abort();
    }
    //- - - - - - - - - - - - - - - - - - -
    printf("CCCCCC\n");

    //R7 - - - - - - - - - - - - - - - - - - -
    rhs[0]     = 3.0 * (nNodes - 2);
    sense[0]   = 'E';
    rmatbeg[0] = 0;

    int p = 0;
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++, p++)
        {
            rmatind[p] = p;
            rmatval[p] = 1.0;
        }
    }
    sprintf(rname[0], "R7");

    if (status = CPXaddrows( env, lp,
                            (int) 0, (int) 1, p,
                            (const double *) rhs, (const char *) sense,
                            (const int *) rmatbeg, (const int *) rmatind,
                            (const double *) rmatval,
                            NULL, (char **) rname ))
    {
        printf("CPXaddrows: Could not add new rows (%s)\n", rname);
        abort();
    }
    printf("777777\n");
    //R7 - - - - - - - - - - - - - - - - - - -

    //R8 - - - - - - - - - - - - - - - - - - -
    rhs[0]   = 0.0;
    sense[0] = 'E';
    rmatbeg[0] = 0;

    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++)
        {
            p = 0;
            for (int k = 1; k < i; k++, p++)
            {
                rmatind[p] = findColID( k, i, j );;
                rmatval[p] = 1.0;
            }
            for (int k = i + 1; k < j; k++, p++)
            {
                rmatind[p] = findColID( i, k, j );;
                rmatval[p] = 1.0;
            }
            for (int k = j + 1; k <= nNodes; k++, p++)
            {
                rmatind[p] = findColID( i, j, k );;
                rmatval[p] = 1.0;
            }
            rmatind[p] = findColID( i, j, 0 );;
            rmatval[p] = -2.0;
            p++;

            sprintf(rname[0], "R8_%d_%d", i, j);

            if (status = CPXaddrows( env, lp,
                                    (int) 0, (int) 1, p,
                                    (const double *) rhs, (const char *) sense,
                                    (const int *) rmatbeg, (const int *) rmatind,
                                    (const double *) rmatval,
                                    NULL, (char **) rname ))
            {
                printf("CPXaddrows: Could not add new rows (%s)\n", rname);
                abort();
            }
        }
    }
    printf("888888\n");
    //R8 - - - - - - - - - - - - - - - - - - -

    //R9 - - - - - - - - - - - - - - - - - - -
    rhs[0]     = 3.0;
    sense[0]   = 'G';
    rmatbeg[0] = 0;

    for (int i = 1; i <= nNodes; i++)
    {
        p = 0;
        for (int j = 1; j < i; j++, p++)
        {
            rmatind[p] = findColID( j, i, 0 );
            rmatval[p] = 1.0;
        }
        for (int j = nNodes; j > i; j--, p++)
        {
            rmatind[p] = findColID( i, j, 0 );
            rmatval[p] = 1.0;
        }
        sprintf(rname[0], "R9_%d", i);

        if (status = CPXaddrows( env, lp,
                                (int) 0, (int) 1, p,
                                (const double *) rhs, (const char *) sense,
                                (const int *) rmatbeg, (const int *) rmatind,
                                (const double *) rmatval,
                                NULL, (char **) rname ))
        {
            printf("CPXaddrows: Could not add new rows (%s)\n", rname);
            abort();
        }
    }
    printf("999999\n");
    //R9  - - - - - - - - - - - - - - - - - - -

    //R10 - - - - - - - - - - - - - - - - - - -
    rhs[0]     = 0.0;
    sense[0]   = 'L';
    rmatbeg[0] = 0;

    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++)
            {
                p = 0;
                rmatind[p] = findColID( i, j, k );;
                rmatval[p] = 1.0;
                p++;
                rmatind[p] = findColID( i, j, 0 );;
                rmatval[p] = -1.0;
                p++;

                sprintf(rname[0], "R10_%d_%d_%d", i, j, k);

                if (status = CPXaddrows( env, lp,
                                        (int) 0, (int) 1, p,
                                        (const double *) rhs, (const char *) sense,
                                        (const int *) rmatbeg, (const int *) rmatind,
                                        (const double *) rmatval,
                                        NULL, (char **) rname ))
                {
                    printf("CPXaddrows: Could not add new rows (%s)\n", rname);
                    abort();
                }
            }
        }
    }
    printf("101010\n");
    //R10 - - - - - - - - - - - - - - - - - - -

    //R11 - - - - - - - - - - - - - - - - - - -
    rhs[0]     = 0.0;
    sense[0]   = 'L';
    rmatbeg[0] = 0;

    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++)
            {
                p = 0;
                rmatind[p] = findColID( i, j, k );;
                rmatval[p] = 1.0;
                p++;
                rmatind[p] = findColID( j, k, 0 );;
                rmatval[p] = -1.0;
                p++;

                sprintf(rname[0], "R11_%d_%d_%d", i, j, k);

                if (status = CPXaddrows( env, lp,
                                        (int) 0, (int) 1, p,
                                        (const double *) rhs, (const char *) sense,
                                        (const int *) rmatbeg, (const int *) rmatind,
                                        (const double *) rmatval,
                                        NULL, (char **) rname ))
                {
                    printf("CPXaddrows: Could not add new rows (%s)\n", rname);
                    abort();
                }
            }
        }
    }
    printf("111111\n");
    //R11 - - - - - - - - - - - - - - - - - - -

    //R12 - - - - - - - - - - - - - - - - - - -
    rhs[0]     = 0.0;
    sense[0]   = 'L';
    rmatbeg[0] = 0;

    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++)
            {
                p = 0;
                rmatind[p] = findColID( i, j, k );;
                rmatval[p] = 1.0;
                p++;
                rmatind[p] = findColID( i, k, 0 );;
                rmatval[p] = -1.0;
                p++;

                sprintf(rname[0], "R12_%d_%d_%d", i, j, k);

                if (status = CPXaddrows( env, lp,
                                        (int) 0, (int) 1, p,
                                        (const double *) rhs, (const char *) sense,
                                        (const int *) rmatbeg, (const int *) rmatind,
                                        (const double *) rmatval,
                                        NULL, (char **) rname ))
                {
                    printf("CPXaddrows: Could not add new rows (%s)\n", rname);
                    abort();
                }
            }
        }
    }
    printf("121212\n");
    //R12 - - - - - - - - - - - - - - - - - - -

    //- - - - - - - - - - - - - - - - - - -
    delete[] rmatind;

    delete[] rmatval;
    delete[] obj;
    delete[] lb;
    delete[] ub;
    delete[] ctype;

    for (int i = 0; i <= nCols; i++)
    {
        delete[] cname[i];
    }
    delete[] cname;

    delete[] rname[0];
    delete[] rname;
    //- - - - - - - - - - - - - - - - - - -
}
//-----------------------------------------------------------------------------
void openCplex()
{
    int status = 0;

    if (!(env = CPXopenCPLEX( &status )))
    {
        printf("Could not open CPLEX environment.\n");
        CPXgeterrorstring( env, status, errmsg );
        printf("%s\n", errmsg);
        abort();
    }

    if (status = CPXsetintparam( env, CPX_PARAM_SCRIND, CPX_ON ))
    {
        printf("PARAM_SCRIND: Failure to turn on screen indicator, error %d\n",
            status);
        abort();
    }

    if (status = CPXsetintparam (env, CPX_PARAM_SIMDISPLAY, 2))
    {
        printf("PARAM_SIMDISPLAY: Failed to turn up simplex display level.\n");
        abort();
    }

    if (status = CPXsetintparam( env, CPX_PARAM_DATACHECK, CPX_ON ))
    {
        printf("PARAM_DATACHECK: Failure to turn on data checking, error %d\n",
            status);
        abort();
    }

    if (!(lp = CPXcreateprob( env, &status, "PLANAR" )))
    {
        printf("Failed to create LP PLANAR.\n");
        abort();
    }
}
//-----------------------------------------------------------------------------
void writeSolutions()
{
    int    solStat  = 0;
    double objValue = 0.0;

    double* x = new double[nCols + 1];
    if (x == NULL)
    {
        printf("Could not allocate memory for solution.\n");
        abort();
    }

    int status = CPXsolution( env, lp, &solStat, &objValue, x, NULL, NULL, NULL);
    if (status)
    {
        printf("Failed to obtain cplex solution.\n");
        abort();
    }

    printf("\nSolution status = %d\n", solStat);
    printf("Solution value  = %.0lf\n\n", objValue);

    //- - - - - - - - - - - - - - - - - - -
    int col = 0;
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++, col++)
        {
            if (cmp(x[col]) == 1)
            {
                printf("x(%d,%d) = %0.lf\n", i, j, x[col]);
            }
        }
    }

    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++, col++)
            {
                if (cmp(x[col]) == 1)
                {
                    printf("f(%d,%d, %d) = %0.lf\n", i, j, k, x[col]);
                }
            }
        }
    }
    //- - - - - - - - - - - - - - - - - - -
    delete[] x;
    //- - - - - - - - - - - - - - - - - - -
}
//-----------------------------------------------------------------------------
void addCut( vector<int>& S )
{
    int status = 0;
    static int r13 = 1;

    //- - - - - - - - - - - - - - - - - - -
    char    sense[2];
    int     rmatbeg[2];
    double  rhs[2];

    char**  rname    = new char*[1];
    rname[0]         = new char[20];
    int*    rmatind  = new int[nCols + 1];
    double* rmatval  = new double[nCols + 1];
    //- - - - - - - - - - - - - - - - - - -

    //- - - - - - - - - - - - - - - - - - -
    rhs[0]   = 2 * (S.size() - 2) - 1;
    sense[0] = 'L';
    rmatbeg[0] = 0;

    int p = 0;
    for (int i = 1; i < S.size() - 1; i++)
    {
        for (int j = i + 1; j < S.size(); j++)
        {
            for (int k = j + 1; k <= S.size(); k++)
            {
                vector<int> tmpVertices;
                tmpVertices.pb(S[i-1]); tmpVertices.pb(S[j-1]); tmpVertices.pb(S[k-1]);
                if (hasFace.count(tmpVertices))
                {
                    rmatind[p] = findColID( S[i-1]+1, S[j-1]+1, S[k-1]+1 );
                    rmatval[p] = 1.0;
                    p++;
                }
            }
        }
    }

    sprintf(rname[0], "R13_%d", r13++);

    if (status = CPXaddrows( env, lp,
                            (int) 0, (int) 1, p,
                            (const double *) rhs, (const char *) sense,
                            (const int *) rmatbeg, (const int *) rmatind,
                            (const double *) rmatval,
                            NULL, (char **) rname ))
    {
        printf("CPXaddrows: Could not add new rows (%s)\n", rname);
        abort();
    }
    //- - - - - - - - - - - - - - - - - - -

    //- - - - - - - - - - - - - - - - - - -
    delete[] rmatind;
    delete[] rmatval;
    delete[] rname[0];
    delete[] rname;
    //- - - - - - - - - - - - - - - - - - -

}
//-----------------------------------------------------------------------------
//vector<ii> getEdges()
map<ii, bool> getEdges()
{
    int    solStat  = 0;
    double objValue = 0.0;

    double* x = new double[nCols + 1];
    if (x == NULL)
    {
        printf("Could not allocate memory for solution.\n");
        abort();
    }

    int status = CPXsolution(env, lp, &solStat, &objValue, x, NULL, NULL, NULL);
    if (status)
    {
        printf("Failed to obtain cplex solution.\n");
        abort();
    }

    // vector<ii> resp;
    map<ii, bool> resp;
    //- - - - - - - - - - - - - - - - - - -
    int col = 0;
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++, col++)
        {
            if (cmp(x[col]) == 1)
            {
                // resp.pb(mp(i, j));
                resp[mp(i, j)] = true;
            }
        }
    }
    delete [] x;
    return resp;
}
//-----------------------------------------------------------------------------
//Solution obtained from the relaxed model.
map<ii, bool> sol;
//Was this combination of edges used as a restriction already?
map<vector<ii>, int> cutFound;
//A list having MPGs used as restrictions.
map<int64, vector<ii>> edgesListM;    //smallest
vector<vector<ii>> edgesListV;        //smpg

/*
    Add a set of edges that generates a cut.
    edgesIdx    ---> Set of edges
    t           ---> Iteration number
*/
void findCut(vector<ii> edgesIdx, int t)
{
    //get num of vertices
    //m = 3n - 6; n = (m+6)/3
    int graphSize = (edgesIdx.size()+6)/3;
    vector<int> S;
    set<int> V;

    //create the graph which will generate a restriction 13
    Graph tmp_planar(graphSize);
    for (int i = 0; i < edgesIdx.size(); ++i)
    {
        int u = edgesIdx[i].first, v = edgesIdx[i].second;
        if (!V.count(u))
        {
            S.pb(u);
            V.insert(u);
        }
        if (!V.count(v))
        {
            S.pb(v);
            V.insert(v);
        }
        add_edge(u, v, tmp_planar);
    }
    //Necessary.
    sort(S.begin(), S.end());

    //Initialize the interior edge index; necessary for face traversal.
    property_map<Graph, edge_index_t>::type e_index = get(edge_index, tmp_planar);
    graph_traits<Graph>::edges_size_type edge_count = 0;
    graph_traits<Graph>::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(tmp_planar); ei != ei_end; ++ei)
        put(e_index, *ei, edge_count++);

    vector<vec_t> embedding(num_vertices(tmp_planar));
    if (boyer_myrvold_planarity_test(tmp_planar, &embedding[0]))
    {
        //Clear the face set.
        hasFace.clear();

        vertex_output_visitor v_vis;
        printf("---------- Adding cut ----------\n");
        planar_face_traversal(tmp_planar, &embedding[0], v_vis);
        printf("Number of faces = %d\n", faces_count);
        printf("Number of edges = %d\n", num_edges(tmp_planar));
        printf("--------------------------------\n");
        faces_count = 0;

        cutFound[edgesIdx]++;
        addCut(S);
    }
}
//-----------------------------------------------------------------------------
/*
    n       ---> Size of the input array
    k       ---> Size of the combination
    app     ---> Approach option
*/
void combine(int n, int k, int app)
{
    Combination c(n, k);
    int64 ub = c.choose(n, k);
    #pragma omp parallel for shared(edgesListM, edgesListV)
    for (int64 i = 0; i < ub; i++)
    {
        Graph tmp_planar(k);

        vector<int> S = c.element(i).getArray();
        vector<ii> edges;

        for (int vertex = 0; vertex < S.size()-1; vertex++)
        {
            int u = S[vertex];
            for (int adj = vertex+1; adj < S.size(); adj++)
            {
                int v = S[adj];
                //add an edge if (u, v) belongs to the solution found
                if (!sol.count(mp(u+1, v+1))) continue;
                edges.pb(mp(u, v));
                add_edge(u, v, tmp_planar);
            }
        }
        //Was this restriction inserted already?
        if (cutFound.count(edges)) continue;

        //Cannot be maximal
        if (num_edges(tmp_planar) != 3*S.size()-6) continue;

        //Is it planar?
        if (boyer_myrvold_planarity_test(tmp_planar))
        {
            #pragma omp critical
            {
                if (app) edgesListM[i] = edges;
                else edgesListV.pb(edges);
            }
        }
    }
}
//-----------------------------------------------------------------------------
void bcSmallest(int app)
{
    // mipBasis();
    int t = 0;
    while (true)
    {
        if (CPXmipopt( env, lp ))
        {
            printf("CPXmipopt: Failed to optimize LP.\n");
            abort();
        }
        char name[20];
        sprintf(name, "planar-%d.lp", t);
        CPXwriteprob(env, lp, name, NULL);

        sol = getEdges();
        map<ii, bool>::iterator ed;
        writeSolutions();

        //Build the solution graph.
        Graph planar;
        for (ed = sol.begin(); ed != sol.end(); ed++)
        {
            ii at = ed->first;
            int a = at.first-1, b = at.second-1;
            add_edge(a, b, planar);
        }

        if (boyer_myrvold_planarity_test(planar))
        {
            printf("Maximal Planar Subgraph found!\n");
            break;
        }
        else
        {
            //S initial size must be at least 4.
            edgesListM.clear();
            cbSize = 4;
            found = true;
            while (cbSize < nNodes)
            {
                double st = getTime();
                printf("Combination size = %d\n", cbSize);
                combine(nNodes, cbSize, app);
                double fn = getTime();
                printElapsedTime(st, fn);

                if (edgesListM.size()) break;
                cbSize++;
            }
            if (edgesListM.size() == 0)
            {
                printf("There is no MPG for restriction 13.\n");
                return;
            }
            //Use the first that was found.
            findCut(edgesListM.begin()->second, t);
        }
        // char m_name[20];
        // sprintf(m_name, "MIPS-%d", t);
        // CPXwritemipstarts(env, lp, m_name, 0, 0);
        t++;
    }
}
//-----------------------------------------------------------------------------
void bcSMPG(int app)
{
    // mipBasis();
    int t = 0;
    while (true)
    {
        if (CPXmipopt( env, lp ))
        {
            printf("CPXmipopt: Failed to optimize LP.\n");
            abort();
        }
        char name[20];
        sprintf(name, "planar-%d.lp", t);
        CPXwriteprob(env, lp, name, NULL);

        sol = getEdges();
        map<ii, bool>::iterator ed;
        writeSolutions();

        //Build the solution graph.
        Graph planar;
        for (ed = sol.begin(); ed != sol.end(); ed++)
        {
            ii at = ed->first;
            int a = at.first-1, b = at.second-1;
            add_edge(a, b, planar);
        }

        if (boyer_myrvold_planarity_test(planar))
        {
            printf("Maximal Planar Subgraph found!\n");
            break;
        }
        else
        {
            edgesListV.clear();
            cbSize = 4;
            found = true;
            while (cbSize < nNodes)
            {
                double st = getTime();
                printf("Combination size = %d\n", cbSize);
                combine(nNodes, cbSize, app);
                double fn = getTime();
                printElapsedTime(st, fn);
                cbSize++;
            }
            if (edgesListV.size() == 0)
            {
                printf("There is no MPG for restriction 13.\n");
                return;
            }

            //Having all MPGs generated, we have to get the smallest one
            //which is not a subgraph of any other.
            int idx = edgesListV.size()-1;
            vector<ii>::iterator vit;
            for (int i = 0; i < edgesListV.size()-1; i++)
            {
                found = true;
                for (int j = i+1; j < edgesListV.size(); j++)
                {
                    if (edgesListV[i].size() == edgesListV[j].size()) continue;

                    vector<ii> tmp(edgesListV[j].size());
                    vit = set_intersection(edgesListV[i].begin(), edgesListV[i].end(),
                        edgesListV[j].begin(), edgesListV[j].end(), tmp.begin());

                    //If the smallest subgraph is a subgraph of any other,
                    //then it cannot be a restriction.
                    int sz = (int)(vit-tmp.begin());
                    if (sz == edgesListV[i].size())
                    {
                        found = false;
                        break;
                    }
                }
                if (found)
                {
                    idx = i;
                    break;
                }
            }
            //Use the smallest as the best cut.
            findCut(edgesListV[idx], t);
        }
        t++;
    }
}
//-----------------------------------------------------------------------------
int main(int argv, char** argc)
{
    // ios::sync_with_stdio(false);
    if (argv < 2)
    {
        printf("ERROR! Required num. of arguments: 2\n");
        printf("Try:\nsmallest - ./a.out 1\nsmpg     - ./a.out 0\n");
        return 0;
    }

    int status;

    readData();
    openCplex();

    if (argv == 2)
    {
        buildModel();
    }
    else if (argv == 3)
    {
        if (status = CPXreadcopyprob(env, lp, argc[2], "LP"))
        {
            printf("Error! Could not read file!\n");
            exit(1);
        }
    }

    int option = atoi(argc[1]);

    if (option) bcSmallest(option);
    else bcSMPG(option);

    if (status = CPXwriteprob(env, lp, "planar.lp", NULL))
    {
        printf("CPXwriteprob: Failed to write LP to disk, error %d\n",
            status);
    }

    writeSolutions();

    if (status = CPXfreeprob(env, &lp))
    {
        printf("CPXfreeprob failed, error code %d\n", status);
    }

    if (status = CPXcloseCPLEX(&env))
    {
        CPXgeterrorstring(env, status, errmsg);
        printf("CPXcloseCPLEX: Could not close CPLEX environment.\n");
        printf("%s\n", errmsg);
    }
    //- - - - - - - - - - - - - - - - - - -

    return 0;
}
//-----------------------------------------------------------------------------
void mipBasis()
{
    int status = 0;

    // int cstat[nCols + 1];
    // int rstat[nRows + 1];

    int nFaces, x, nzcnt = 0, effortlevel = 0;
    int beg = 0;

    vector<int> varindices;
    vector<double> values;
    for (int i = 1; i < nNodes; i++)
    {
        for (int j = i + 1; j <= nNodes; j++)
        {
            scanf("%d", &x);
            vector<int> tmp;
            tmp.pb(i); tmp.pb(j); tmp.pb(0);

            varindices.pb(colsId[tmp]);
            values.pb(x ? 1.0 : 0.0);
            nzcnt++;
        }
    }

    map< vector<int>, int > sFaces;
    scanf("%d", &nFaces);
    for (int i = 0; i < nFaces; i++)
    {
        vector<int> sF;
        for (int j = 0; j < 3; j++)
        {
            scanf("%d", &x);
            sF.pb(x+1);
        }
        sort(sF.begin(), sF.end());
        sFaces[sF] = 1;
    }

    for (int i = 1; i < nNodes - 1; i++)
    {
        for (int j = i + 1; j < nNodes; j++)
        {
            for (int k = j + 1; k <= nNodes; k++)
            {
                vector<int> tmp;
                tmp.pb(i); tmp.pb(j); tmp.pb(k);

                varindices.pb(colsId[tmp]);
                values.pb(sFaces.count(tmp) ? 1.0 : 0.0);
                nzcnt++;
            }
        }
    }
    printf("Result: %d\n", nzcnt);

    int arr_varindices[nzcnt];
    double arr_values[nzcnt];
    copy(varindices.begin(), varindices.end(), arr_varindices);
    copy(values.begin(), values.end(), arr_values);

    /* Now copy the mip start */

    CPXsetintparam( env, CPX_PARAM_ADVIND, 1 );

    if (status = CPXaddmipstarts (env, lp, 1, nzcnt, &beg, arr_varindices,
        arr_values, &effortlevel, NULL))
    {
        printf("CPXaddmipstarts: Could not add mip start.\n");
        abort();
    }

    CPXwritemipstarts(env, lp, "MIPS", 0, 0);
}
//-----------------------------------------------------------------------------


/*! \file
  \brief Functors to convert data to doubles.
  \details SVG plot assumes all data are convertible to double or uncertain value type unc before being plotted.
    The functors are used to convert both 1D and 2D (pairs of data values) to be converted.
    Note that uncertain value class unc only holds double precision so higher precision data type
    will therefore lose information.  This seems a reasonable design decision as any real data
    to be plotted is unlikely to have more than double precision (about 16 decimal digits).

    "svg_plot\example\convertible_to_double.cpp" demonstrates that built-in types
    @c float, @c double, @c long double @ Boost.Multiprecision @c cpp_bin_float_quad
    work as expected, as well as a sample User Defined Type (UDT) a fixed-point type.

    Types that cannot be converted to double nor constructible from double provoke a compile-time message:
    "Uncertain types must be convertible to double!"

    (This uses checks using http://www.cplusplus.com/reference/type_traits/is_constructible/
      @c BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");
     ).
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2009, 2012, 2013, 2018, 2021

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

#ifndef BOOST_SVG_DETAIL_FUNCTORS_HPP
#define BOOST_SVG_DETAIL_FUNCTORS_HPP

#include <boost/quan/unc.hpp>
#include <boost/quan/meas.hpp>

namespace boost {
namespace svg {
namespace detail
{
  using namespace boost::quan;

  /*! \class boost::svg::detail::double_1d_convert
     \brief This functor allows any 1D data that can be converted to @c double to be plotted.
 */
class double_1d_convert
{
public:
    typedef double result_type; //!< result type is @c double.

    //! To convert a single data value to double.
    //! \tparam T Any type that can be converted to double.
    //! \returns A single @c double data value.
    template <class T>
    double operator()(T val) const
    {
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");

       return static_cast<double>(val); //! \return Value that has been converted to double.
    }
}; // class double_1d_convert

 /*! \class boost::svg::detail::unc_1d_convert
      \brief This functor allows any 1D data that can be converted to @c boost::quan::unc (uncertain double) to be plotted.
      \details Defaults provided by the @c unc class constructor ensure that
        uncertainty, degrees of freedom information, and uncertain type are suitably set too.
*/
template <bool correlated>
class unc_1d_convert
{
public:
    typedef unc<correlated> result_type; //!< @c result_type is an uncertain floating-point type.

    /*!< Convert to uncertain type,
      providing defaults for uncertainty,  degrees of freedom information, and type (meaning undefined).
      \return value including uncertainty information.
       \tparam T Any data type with a value that can be converted to double, for example: @c double, @c uncun, @c Meas.
      */
    template <class T>
    unc<correlated> operator()(T val) const
    {
      return (result_type)val;
      /*! \return uncertain type (uncertainty, degrees of freedom information, and type meaning undefined). */
    }
}; // template <bool correlated> class default_1d_convert

/*! \class boost::svg::detail::meas_1d_convert
      \brief This functor allows any 1D data that can be converted to measurements
      (with uncertain doubles) to be plotted.
      \details Defaults provided by the meas class constructor ensure that
        uncertainty, degrees of freedom information, type, and order, timestamp and id are suitably set too.
    \tparam T Any data type with a value that can be converted to double, for example: double, unc, Meas.
    \return uncertain type (uncertainty, degrees of freedom information, and type meaning undefined).
*/
class meas_1d_convert
{
public:
    typedef Meas result_type; //!< result type includes an uncertain floating-point type.

    template <class T>
    Meas operator()(T val) const
    /*!< Convert to Meas type,
      providing defaults for uncertainty, degrees of freedom information, and uncertain type.
      \return value including uncertainty and other information.
    */
    {
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be that can be converted to double!");
      return static_cast<result_type>(val);
    }
}; // class default_1d_convert

class pair_double_2d_convert
{ /*! \class boost::svg::detail::pair_double_2d_convert
      \brief This functor allows any 2-D data that can be converted to type @c std::pair<double, double> to be plotted.
     Convert a pair of X and Y (whose types can be converted to double values) to a pair of @c doubles.
     \tparam T type whose value can be converted to @c double.
     \tparam U type whose value can be converted to @c  double.
     \returns @c std::pair of @c double data point values.
  */
public:
    typedef std::pair<double, double> result_type; //!< result type is a pair (X and Y) of doubles.

    double i; //!< Current value, 1st set by start(double i0).
    void start(double i0)
    { //! Set a start value.
      i = i0;
    }

    template <typename T, typename U>
    std::pair<double, double> operator()(const std::pair<T, U>& a) const
    { //! Assumes that a conversion from double yields just the value component of the uncertain value.
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<U, double>::value, "Uncertain types must be convertible to double!");
      return std::pair<double, double>(static_cast<double>(a.first), static_cast<double>(a.second));
    }

    template <typename T>
    std::pair<double, double> operator()(T a)
    {  //! Convert a pair of X and Y values to a @c std::pair of @c doubles.
       //! \return @c std::pair of doubles.
        return std::pair<double, double>(i++, static_cast<double>(a));
    }
}; // class pair_double_2d_convert

template <bool correlated>
class pair_unc_2d_convert
{ /*! \class boost::svg::detail::pair_unc_2d_convert
      \brief This functor allows any 2D data that can be converted to type @c std::pair<unc, unc> to be plotted.
*/
public:
    typedef std::pair<unc<correlated>, unc<correlated> > result_type; //!< result type is pair of uncertain values.

    unc<correlated> i;  //!< Current uncertain value, 1st set by start(double i0).

    void start(unc<correlated> i0)
    { //!< Set a start value.
       i = i0;
    }

    //!< \tparam T type that can be converted to double.
    //!< \tparam U type that can be converted to double.
    //! \returns A @c std::pair of double data point values.
    template <class T, class U>
    std::pair<unc<correlated>, unc<correlated> > operator()(const std::pair<T, U>& a) const
    { //!< Convert a pair of X and Y uncertain type values to a pair of @c doubles.
      //! Cast to double so that can use with float, long double and UDTs.
      //! \return @c std::pair of uncs.
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<U, double>::value, "Uncertain types must be convertible to double!");
       return std::pair<unc<correlated>, unc<correlated> >(
         (unc<correlated>)(a.first), (unc<correlated>)(static_cast<double>(a.second))
         );
    }

    template <typename T>    //!< \tparam T Any type that can be converted to double.
    std::pair<unc<correlated>, unc<correlated> > operator()(T a)
    {  //!< Convert a pair of X and Y uncertain type values to a @c std::pair of @c unc.
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");
      return std::pair<unc <correlated>, unc<correlated> >(i++, (unc<correlated>)a); //! \return pair of unc.
    }
}; // class pair_unc_2d_convert

 /*! This functor allows any 2D data that can be converted to type @c std::pair<Meas, unc> to be plotted.
   \tparam correlated @c true if the uncertainties are correlated (for example, adding to a constant value).
   */
template <bool correlated>
class pair_Meas_2d_convert
{
public:
    typedef std::pair<Meas, unc<correlated> > result_type;
    //!< result type is pair of Meas (uncertain including datetime etc) and an uncertain value.

    Meas i; //!< Current Meas (uncun + datetime etc) value.

    void start(Meas i0)
    { //!< Set a start value.
       i = i0;
    }
    //!< Convert a pair of X and Y uncertain type values to a pair of uncertain types uncun.
    //! \return pair of Meas (an unc including time, ID and order info) & an unc.
    //! Cast to double so that can potentially use with @c float, @c long double.
    //!< \tparam T type that can be converted to double.
    //!< \tparam U type that can be converted to uncertain type @c unc.
    //! \returns A @c std::pair of double precision data-point values.
    template <typename T, typename U>
    std::pair<Meas, unc<correlated> > operator()(const std::pair<T, U>& a) const
    {
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to uncun!");
      BOOST_STATIC_ASSERT_MSG(std::is_constructible<U, double>::value, "Uncertain types must be convertible to uncun!");
      return std::pair<Meas, unc<correlated> >((Meas)(static_cast<Meas>(a.first)), (unc<correlated>)(static_cast<unc<correlated>>(a.second)));
    }

    template <typename T>  //!< \tparam T Any type that can be converted to uncun.
    std::pair<Meas, unc<correlated> > operator()(T a)
    {  //!< Convert a pair of X and Y uncertain type values to a pair of Meas & unc.
        return std::pair<Meas, unc<correlated> >(i++, (unc<correlated>)(static_cast<double>(a)));
        //! \return pair of Meas & unc.
    }
}; // class pair_Meas_2d_convert

 /*! This functor allows any 2D data that can be converted to type double to be plotted.
   \tparam correlated @c true if the uncertainties are correlated (for example, adding up to a constant value).
   */
template <bool correlated>
class pair_Meas_2d_double_convert
{
public:
 //typedef std::pair<Meas, unc<correlated> > result_type;
  typedef std::pair<double, double > result_type;
  //!< result type is pair of double values.

  Meas i; //!< Current Meas (uncun + datetime etc) value.

  void start(Meas i0)
  { //!< Set a start value.
    i = i0;
  }
  //!< Convert a pair of X and Y uncertain type values to a pair of doubles.
  //! \return pair of Meas (unc including time, ID and order info) & an unc.
  //! Cast to double so that can potentially use with float, long double.
  //!< \tparam T type that can be converted to double.
  //!< \tparam U type that can be converted to double.
  //! \returns A @c std::pair of @c double precision data-point values.
  template <typename T, typename U>
  std::pair<Meas, unc<correlated> > operator()(const std::pair<T, U>& a) const
  {
    BOOST_STATIC_ASSERT_MSG(std::is_constructible<T, double>::value, "Uncertain types must be convertible to double!");
    BOOST_STATIC_ASSERT_MSG(std::is_constructible<U, double>::value, "Uncertain types must be convertible to double!");
     return std::pair<Meas, unc<correlated> >((Meas)(static_cast<Meas>(a.first)), (unc<correlated>)(static_cast<uncun>(a.second))); // OK uncertain values.
     // but fails for \boost\libs\svg_plot\example\convertible_to_double.cpp and this compiles
     //return std::pair<Meas, unc<correlated> >((Meas)(static_cast<double>(a.first)), (unc<correlated>)(static_cast<double>(a.second)));
     // but does not work for uncertainty displays.  Not fully understood.
 }

  //! Convert a pair of X and Y uncertain type values to a pair of @c Meas & @c unc.
  //! \tparam T Any type that can be converted to @c double.
  template <typename T>
  std::pair<Meas, unc<correlated> > operator()(T a)
  {
    return std::pair<Meas, unc<correlated> >(i++, (unc<correlated>)(static_cast<double>(a)));
    //! \return pair of Meas & unc.
  }
}; // class pair_Meas_2d_double_convert

} // namespace detail
} // namespace svg
} // namespace boost

#endif // BOOST_SVG_DETAIL_FUNCTORS_HPP


// -*- Mode: c++; c-basic-offset: 4 -*-

// This C++ script reads a CFN with enumerated variables and table cost
// functions only and computes a possibly improved starting upper
// bound (the sum of all the maximum finite costs in tables where
// finite means less than the initially provided upper bound).
// Author: George Katsirelos

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <tuple>
#include <map>
#include <unordered_map>
#include <boost/format.hpp>

using namespace std;
using boost::format;

const bool debug = false;

template<typename T>
ostream& operator<<(ostream& os, vector<T> const& v)
{
    os << "[";
    for(auto& t : v)
        os << t << ' ';
    os << "]";
    return os;
}

typedef long long Cost;

struct wcsptuple {
    vector<size_t> tup;
    Cost cost;
};

struct wcspfunc {
    Cost defcost;
    vector<size_t> scope;
    vector< wcsptuple > specs;

    size_t arity() const { return scope.size(); }
};

struct wcsp {
    string name;
    Cost ub;

    vector<size_t> domains;
    size_t nvars() const { return domains.size(); }

    vector<wcspfunc> functions;
};

template<typename T>
vector<T> read_vec(istream& is)
{
    vector<T> r;
    T s;
    is >> s;
    while(is) {
        r.push_back(s);
        is >> s;
    }
    return r;
}

template<typename T>
vector<T> read_vec(string const& line)
{
    istringstream iss(line);
    return read_vec<T>(iss);
}

tuple<string, size_t, size_t, size_t, Cost> read_header(string const& line)
{
    istringstream iss(line);

    string name;
    size_t nvars;
    size_t domsize;
    size_t nfun;
    Cost ub;

    iss >> name >> nvars >> domsize >> nfun >> ub;
    return make_tuple(name, nvars, domsize, nfun, ub);
}

wcspfunc read_fun(istream& is)
{
    string line;
    getline(is, line);
    vector<Cost> hd = read_vec<Cost>(line);
    size_t arity = hd[0];
    Cost defcost = hd[hd.size()-2];
    size_t nspec = hd[hd.size()-1];

    vector<wcsptuple> specs;
    for(size_t i = 0; i != nspec; ++i) {
        getline(is, line);
        vector<Cost> v = read_vec<Cost>(line);
        specs.push_back( {vector<size_t>(v.begin(), v.begin()+arity), v[v.size()-1]} );
    }

    return { defcost, vector<size_t>(hd.begin()+1, hd.begin()+1+arity),
            specs };
}

wcsp readwcsp(istream& is)
{
    wcsp w;

    size_t nvars;
    size_t domsize;
    size_t nfun;

    string line;

    getline(is, line);
    tie(w.name, nvars, domsize, nfun, w.ub) = read_header(line);

    getline(is, line);
    w.domains = read_vec<size_t>(line);

    for(size_t i = 0; i != nfun; ++i)
        w.functions.push_back(read_fun(is));

    return w;
}

void write_wcsp(wcsp const& w, ostream &ofs)
{
    size_t maxd = *max_element(w.domains.begin(), w.domains.end());
    ofs << w.name << ' ' << w.nvars()
        << ' ' << maxd
        << ' ' << w.functions.size() << ' ' << w.ub << "\n";

    for(auto& d : w.domains)
        ofs << d << ' ';
    ofs << "\n";

    for(auto& f: w.functions) {
        ofs << f.arity() << ' ';
        for(auto& v : f.scope)
            ofs << v << ' ';
        ofs << f.defcost << ' ' << f.specs.size() << "\n";
        for(auto& s : f.specs) {
            for(auto& v : s.tup)
                ofs << v << ' ';
            ofs << min(s.cost, w.ub) << "\n";
        }
    }
}

int main(int argc, char* argv[])
{
    if( argc != 3 ) {
        cout << "usage: " << argv[0] << " <wcsp-input> <wcnf-output>\n";
        return 1;
    }

    ifstream ifs(argv[1]);
    ofstream ofs(argv[2]);

    if( !ifs ) {
        cout << "could not open " << argv[1] << "\n";
        return 1;
    }

    if( !ofs ) {
        cout << "could not open " << argv[2] << "\n";
        return 1;
    }

    wcsp w = readwcsp(ifs);
    //cout << "initial top " << w.ub << "\n";
    Cost newtop = 0;
    for(auto const& f : w.functions) {
        auto me = std::max_element(f.specs.begin(), f.specs.end(),
                                   [&](wcsptuple const& m, wcsptuple const& c){
                                       return c.cost < w.ub &&
                                       (c.cost > m.cost ||
                                        m.cost >= w.ub);
                                   });
        //cout << "max of function " << f.scope << ": " << me->cost << "\n";
        newtop += me->cost;
    }
    cout << "new top " << newtop << "\n";
    w.ub = newtop;
    write_wcsp(w, ofs);

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_PINV_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_PINV_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <nt2/sdk/memory/container.hpp>
#include <nt2/sdk/meta/size_as.hpp>
#include <nt2/sdk/meta/value_as.hpp>
#include <nt2/core/container/dsl/value_type.hpp>
#include <nt2/core/container/dsl/size.hpp>
#include <nt2/include/functions/issquare.hpp>

/*!
 * @brief inv  pseudo-inverse of matrix.
 * pinv(x) is the pseudo-inverse of the matrix expression x.
 *
 * x = pinv(a) produces a matrix x of the same dimensions
 * as trans(a) so that a*x*a = a, x*a*x = x and a*x and x*a
 * are hermitian. the computation is based on svd(a) and any
 * singular values less than a tolerance are treated as zero.
 * the default tolerance is max(size(a)) * norm(a) * eps<class(a)> .
 *
 * pinv(a,tol) uses the tolerance tol instead of the default.
 *
 **/

namespace nt2 { namespace tag
  {
    /*!
     * \brief Define the tag pinv_ of functor pinv
     *        in namespace nt2::tag for toolbox algebra
    **/
    struct pinv_ :  ext::unspecified_<pinv_> { typedef ext::unspecified_<pinv_> parent; };
  }

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::pinv_, pinv, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::pinv_, pinv, 2)

}

namespace nt2 { namespace ext
{
  template<class Domain, class Expr,  int N>
  struct  size_of<tag::pinv_, Domain, N, Expr>
  {
    typedef typename boost::proto::result_of::child_c<Expr&,0>::value_type  c0_t;
    typedef typename c0_t::extent_type                               result_type;
    BOOST_FORCEINLINE result_type operator()(Expr& e) const
    {
      result_type sizee = boost::proto::child_c<0>(e).extent();
      std::swap(sizee[0], sizee[1]);
      return sizee;
    }
  };

 template <class Domain, class Expr,  int N>
 struct value_type < tag::pinv_, Domain,N,Expr>
  : meta::value_as<Expr,0>
 {};
} }

#endif


//
//  Jacobian.cpp
//  Eigen_test
//
//  Created by Emil Iliev on 18.10.19.
//  Copyright © 2019 Emil Iliev. All rights reserved.
//

#include "Jacobian.hpp"
#include <iostream>
#include <Eigen/SVD>

Jacobian::Jacobian() : mtxf(MatrixFactory::getInstance()){
}

void Jacobian::setJacobianConfiguration( unsigned int row , unsigned int col ) {
    //We don't know configuration
    _jacobian.resize(row , col);
}

Eigen::MatrixXf& Jacobian::getJacobian() {
    //Get calculated Jacobian
    return _jacobian;
}

// Transformation matrix representation
//
//|r11 r12 r13 d14|
//|r21 r22 r23 d24|
//|r31 r32 r33 d34|
//|  0   0   0   1|
//

/************************************************************************/
/* Main routine for J calculation                                       */
/************************************************************************/
void Jacobian::calculateJacobian( HomMatrixHolder& hom_matrix_handler , JointHandler& jhandler , Eigen::Matrix4f& full) {
    unsigned int col_num = jhandler.size();

    if (!col_num) {
        //Zero size not allowed
        return;
    }

    unsigned int row_num = NUMBEROFSETPARAMETERS;
    unsigned int size = hom_matrix_handler.size();
    //Set J confiruration
    setJacobianConfiguration(row_num , col_num);

    std::vector<Eigen::Matrix4f> trans_matrix_holder;
    for (int index = 0; index < size + 1; ++index) {
        trans_matrix_holder.push_back(Eigen::Matrix4f::Identity());
    }

    for (int index = 1; index < size + 1; ++index) {
        trans_matrix_holder[index] = trans_matrix_holder[index - 1] * hom_matrix_handler[index - 1];
    }

    for (unsigned int i = 1 ; i < col_num + 1 ; ++i) {
        calculateColumnOfJacobian_New(trans_matrix_holder[i - 1],DHINDEX(i),jhandler[DHINDEX(i)].getJointType(),full);
    }
}

Jacobian* Jacobian::_instance = NULL;

Jacobian* Jacobian::getInstance() {
    if (!_instance)
        _instance = new Jacobian();

    return _instance;
}

Eigen::MatrixXf Jacobian::psevdoInverse() {
    Eigen::MatrixXf inv;
    Eigen::JacobiSVD<Eigen::MatrixXf> svd;
    svd.compute(_jacobian , Eigen::ComputeThinU | Eigen::ComputeThinV);
//    svd.pinv(inv);
    double epsilon = std::numeric_limits<double>::epsilon();
    double tolerance = epsilon * std::max(_jacobian.cols(), _jacobian.rows()) *svd.singularValues().array().abs()(0);
    inv = svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
    return inv;
}

void Jacobian::calculateColumnOfJacobian_New(Eigen::Matrix4f& hom_matrix_handler, unsigned int ind, JointT jt, Eigen::Matrix4f& fullm) {
    Eigen::Vector3f z0(0.0f , 0.0f , 1.0f);
    Eigen::Vector3f zi;
    Eigen::Matrix4f transf_matrix = hom_matrix_handler;
    Eigen::Matrix3f rot_m;

    Eigen::Vector3f p_end_effector;
    Eigen::Vector3f pi;

    //Position of end effector
    p_end_effector<< fullm(0,3), fullm(1,3), fullm(2,3);
    rot_m = transf_matrix.block(0,0,3,3);

    //
    //  Zi-1
    //
    zi = rot_m * z0;
    pi << transf_matrix(0,3) , transf_matrix(1,3) , transf_matrix(2,3);

    //
    //  (Pe - Pi-1)
    //
    Eigen::Vector3f delta_vec = p_end_effector - pi;

    //
    //  Zi x (Pe - Pi-1)
    //
    Eigen::Vector3f d_rev = zi.cross(delta_vec);

    //We should get type of joint and go further
    switch(jt) {
    case PRISMATIC:
        //For prismatic joint everything is simple :
        //        | z | <--- calculated vector z
        // Cind = |   |
        //        | 0 | <--- zero vector3f
        _jacobian(0,ind) = zi(0);
        _jacobian(1,ind) = zi(1);
        _jacobian(2,ind) = zi(2);
        _jacobian(3,ind) = 0.0f;
        _jacobian(4,ind) = 0.0f;
        _jacobian(5,ind) = 0.0f;
        break;
    case REVOLUTE:
        //For revolute joint everything is harder :
        //        | z * d | <--- calculated vector z * vector d
        // Cind = |       |
        //        |   z   | <--- calculated vector z
        _jacobian(0,ind) = d_rev(0);
        _jacobian(1,ind) = d_rev(1);
        _jacobian(2,ind) = d_rev(2);
        _jacobian(3,ind) = zi(0);
        _jacobian(4,ind) = zi(1);
        _jacobian(5,ind) = zi(2);
        break;
    default:
        break;
    }
}


#include "Product.h"
#include "Config.h"
#include "State.h"
#include <boost/lexical_cast.hpp>
#include <ctpp2/CDT.hpp>
#include <macgyver/TimeParser.h>
#include <macgyver/Exception.h>
#include <spine/HTTP.h>

namespace
{
// ----------------------------------------------------------------------
/*!
 * \brief Distance between two points along earth surface
 *
 * \param theLon1 Longitude of point 1
 * \param theLat1 Latitude of point 1
 * \param theLon2 Longitude of point 2
 * \param theLat2 Latitude of point 2
 * \return The distance in kilometers
 *
 *  Haversine Formula (from R.W. Sinnott, "Virtues of the Haversine",
 *  Sky and Telescope, vol. 68, no. 2, 1984, p. 159)
 *  will give mathematically and computationally exact results. The
 *  intermediate result c is the great circle distance in radians. The
 *  great circle distance d will be in the same units as R.
 *
 *  When the two points are antipodal (on opposite sides of the Earth),
 *  the Haversine Formula is ill-conditioned, but the error, perhaps
 *  as large as 2 km (1 mi), is in the context of a distance near
 *  20,000 km (12,000 mi). Further, there is a possibility that roundoff
 *  errors might cause the value of sqrt(a) to exceed 1.0, which would
 *  cause the inverse sine to crash without the bulletproofing provided by
 *  the min() function.
 *
 * The code was taken from NFmiLocation::Distance
 */
// ----------------------------------------------------------------------

double torad(double theValue)
{
  return theValue * 3.14159265358979323846 / 180.0;
}
double geodistance(double theLon1, double theLat1, double theLon2, double theLat2)
{
  double lo1 = torad(theLon1);
  double la1 = torad(theLat1);

  double lo2 = torad(theLon2);
  double la2 = torad(theLat2);

  double dlon = lo2 - lo1;
  double dlat = la2 - la1;
  double sindlat = sin(dlat / 2);
  double sindlon = sin(dlon / 2);

  double a = sindlat * sindlat + cos(la1) * cos(la2) * sindlon * sindlon;
  double help1 = sqrt(a);
  double c = 2. * asin(std::min(1., help1));

  return 6371.220 * c;
}
}  // namespace

namespace SmartMet
{
namespace Plugin
{
namespace CrossSection
{
// ----------------------------------------------------------------------
/*!
 * \brief Initialize the product from JSON
 */
// ----------------------------------------------------------------------

void Product::init(const Json::Value& theJson, const Config& theConfig)
{
  try
  {
    if (!theJson.isObject())
      throw Fmi::Exception(BCP, "Product JSON is not a JSON object (name-value pairs)");

    // Iterate through all the members

    const auto members = theJson.getMemberNames();
    for (const auto& name : members)
    {
      const Json::Value& json = theJson[name];

      if (name == "layers")
        layers.init(json, theConfig);
      else
        throw Fmi::Exception(BCP,
                                         "Product does not have a setting named '" + name + "'");
    }
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Generate the product into the template hash tables
 *
 */
// ----------------------------------------------------------------------

void Product::generate(CTPP::CDT& theGlobals,
                       State& theState,
                       const SmartMet::Spine::TimeSeriesGenerator::LocalTimeList& theTimes)
{
  try
  {
    // Initialize the structure

    theGlobals["layers"] = CTPP::CDT(CTPP::CDT::HASH_VAL);

    // Process all times

    for (const auto& time : theTimes)
    {
      theState.time(time);
      layers.generate(theGlobals, theState);
    }

    // Generate bounding box

    const auto& env = theState.envelope();
    if (env.IsInit() != 0)
    {
      theGlobals["bbox"] = CTPP::CDT(CTPP::CDT::HASH_VAL);
      theGlobals["bbox"]["xmin"] = env.MinX;
      theGlobals["bbox"]["xmax"] = env.MaxX;
      theGlobals["bbox"]["ymin"] = env.MinY;
      theGlobals["bbox"]["ymax"] = env.MaxY;
    }

    // Distance between the two points

    theGlobals["distance"] = geodistance(theState.query().longitude1,
                                         theState.query().latitude1,
                                         theState.query().longitude2,
                                         theState.query().latitude2);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace CrossSection
}  // namespace Plugin
}  // namespace SmartMet