////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2012 Zach Byerly
//  Copyright (c) 2012 Bryce Adelstein-Lelbach
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
////////////////////////////////////////////////////////////////////////////////

#if !defined(OCTOPUS_9BA6055C_E7A9_4A16_8A24_B8B410AA1A14)
#define OCTOPUS_9BA6055C_E7A9_4A16_8A24_B8B410AA1A14

// http://www.vistrails.org/index.php/User:Tohline/Apps/PapaloizouPringleTori

#include <octopus/state.hpp>
#include <octopus/vector2d.hpp>
#include <octopus/driver.hpp>
#include <octopus/science.hpp>
#include <octopus/engine/engine_interface.hpp>
#include <octopus/engine/ini.hpp>
#include <octopus/octree/octree_reduce.hpp>
#include <octopus/octree/octree_apply_leaf.hpp>
#include <octopus/math.hpp>
#include <octopus/global_variable.hpp>
#include <octopus/io/multi_writer.hpp>
#include <octopus/io/fstream.hpp>

#if defined(OCTOPUS_HAVE_SILO)
    #include <octopus/io/silo.hpp>
#endif

#include <boost/format.hpp>
#include <boost/atomic.hpp>
#include <boost/math/constants/constants.hpp>

#include <hpx/include/plain_actions.hpp>

// FIXME: Move shared code from the drivers into a shared object/headers.
// FIXME: Names.
// FIXME: Proper configuration.
// FIXME: Globals are bad mkay.

enum rotational_direction
{
    rotate_clockwise,
    rotate_counterclockwise
};

enum momentum_conservation
{
    angular_momentum_conservation,
    cartesian_momentum_conservation
};

///////////////////////////////////////////////////////////////////////////////
double const initial_cfl_factor = 1.0e-2;

double const cfl_factor = 0.4;

/// Gravitation constant.
double const G = 1.0;

/// Mass of the central object.
double const M_C = 1.0;

/// Outer radius of the torus.
double const R_outer = 1.0;

/// Polytropic index.
double const gamma_ = 1.333;

double const polytropic_n = 3.0;

/// Amplitude of the perturbation.
double const kick_amplitude = 1e-2;

/// Angular speed of the frame, e.g. how fast the grid rotates.
OCTOPUS_GLOBAL_VARIABLE((double), omega);

/// Direction of the torus' rotation.
OCTOPUS_GLOBAL_VARIABLE((rotational_direction), rot_dir);

/// Advection scheme.
OCTOPUS_GLOBAL_VARIABLE((momentum_conservation), mom_cons);

/// If true, a rotating frame of reference is used.
OCTOPUS_GLOBAL_VARIABLE((bool), rotating_grid);

/// Polytropic constant.
OCTOPUS_GLOBAL_VARIABLE((double), kappa);

/// The ratio of the inner radius to the outer radius, e.g. the thinnness of the
/// torus.
OCTOPUS_GLOBAL_VARIABLE((double), X_in);

/// Mode of the perturbation.
OCTOPUS_GLOBAL_VARIABLE((boost::uint64_t), kick_mode);

///////////////////////////////////////////////////////////////////////////////
/// Mass density
double&       rho(octopus::state& u)       { return u[0]; }
double const& rho(octopus::state const& u) { return u[0]; }

/// Momentum density (X-axis)
double&       momentum_x(octopus::state& u)       { return u[1]; }
double const& momentum_x(octopus::state const& u) { return u[1]; }

/// Momentum density (Y-axis)
double&       momentum_y(octopus::state& u)       { return u[2]; }
double const& momentum_y(octopus::state const& u) { return u[2]; }

/// Momentum density (Z-axis)
double&       momentum_z(octopus::state& u)       { return u[3]; }
double const& momentum_z(octopus::state const& u) { return u[3]; }

/// Total energy of the gas
double&       total_energy(octopus::state& u)       { return u[4]; }
double const& total_energy(octopus::state const& u) { return u[4]; }

/// Entropy tracer
double&       tau(octopus::state& u)       { return u[5]; }
double const& tau(octopus::state const& u) { return u[5]; }

enum { radial_momentum_idx = 6 };

double&       angular_momentum(octopus::state& u)       { return u[7]; }
double const& angular_momentum(octopus::state const& u) { return u[7]; }

double radius(octopus::array<double, 3> const& v)
{
    return std::sqrt(v[0]*v[0] + v[1]*v[1]);
}

double radius(double x, double y)
{
    return std::sqrt(x*x + y*y);
}

double radial_momentum(
    octopus::state const& u
  , octopus::array<double, 3> const& v
    )
{
    switch (mom_cons)
    {
        case angular_momentum_conservation:
            return u[radial_momentum_idx];
        case cartesian_momentum_conservation:
        {
            double const R = radius(v);
            return momentum_x(u)*v[0]/R + momentum_y(u)*v[1]/R;
        }
        default: break;
    }

    OCTOPUS_ASSERT(false);
    return 0.0;
}

double tangential_momentum(
    octopus::state const& u
  , octopus::array<double, 3> const& v
    )
{
    double const R = radius(v);

    switch (mom_cons)
    {
        case angular_momentum_conservation:
            return angular_momentum(u)/R - rho(u)*R*omega;
        case cartesian_momentum_conservation:
            return momentum_y(u)*v[0]/R
                 - momentum_x(u)*v[1]/R
                 - rho(u)*R*omega;
        default: break;
    }

    OCTOPUS_ASSERT(false);
    return 0.0;
}

template <octopus::axis Axis>
double gravity(octopus::array<double, 3> const& v)
{
    double const x = v[0];
    double const y = v[1];
    double const z = v[2];

    double const r = std::sqrt(x*x + y*y + z*z);
    double const F = -G*M_C/(r*r);

    switch (Axis)
    {
        case octopus::x_axis:
            return F*(x/r);
        case octopus::y_axis:
            return F*(y/r);
        case octopus::z_axis:
            return F*(z/r);
        default: break;
    }

    OCTOPUS_ASSERT(false);
    return 0.0;
}

double radial_gravity(octopus::array<double, 3> const& v)
{
    double const x = v[0];
    double const y = v[1];
    double const z = v[2];

    double const r = std::sqrt(x*x + y*y + z*z);
    double const R = radius(v);
    double const F = -G*M_C/(r*r);

    return F*(R/r);
}

template <octopus::axis Axis>
double velocity(
    octopus::state const& u
  , octopus::array<double, 3> const& v
    )
{
    double const x = v[0];
    double const y = v[1];

    double const R = radius(v);
    double const st = tangential_momentum(u, v);
    double const sr = radial_momentum(u, v);

    switch (Axis)
    {
        case octopus::x_axis:
            return (x*sr-y*st)/R/rho(u);
        case octopus::y_axis:
            return (y*sr+x*st)/R/rho(u);
        case octopus::z_axis:
            return momentum_z(u)/rho(u);
        default: break;
    }

    OCTOPUS_ASSERT(false);
    return 0.0;
}

double kinetic_energy(
    octopus::state const& u
  , octopus::array<double, 3> const& v
    )
{
    double const sr = radial_momentum(u, v);
    double const st = tangential_momentum(u, v);
    return 0.5 * (sr*sr + st*st + momentum_z(u)*momentum_z(u)) / rho(u);
}

/// Gas pressure - polytropic equation of state.
double pressure(octopus::state const& u)
{
    return kappa * std::pow(rho(u), gamma_);
}

double speed_of_sound(octopus::state const& u)
{
    OCTOPUS_ASSERT(rho(u) > 0.0);
    OCTOPUS_ASSERT(pressure(u) >= 0.0);
    return std::sqrt(gamma_ * pressure(u) / rho(u));
}

// Omega at the radius with the highest pressure.
double omega_R_0()
{
    double const j_H = std::sqrt(2.0*X_in/(1.0+X_in));
    double const j_here = j_H*std::sqrt(G*M_C*R_outer);
    return (G*M_C)*(G*M_C)/(j_here*j_here*j_here);
}

void initialize_omega()
{
    if (rotating_grid)
        omega = omega_R_0();
    else
        omega = 0.0;
}

double orbital_period()
{
    double const pi = boost::math::constants::pi<double>();
    return 2.0*pi/omega_R_0();
}

double z_max(double R)
{
    using std::pow;
    using std::sqrt;

    double const X = R/R_outer;
    double const j_H = sqrt(2.0*X_in/(1.0+X_in));
    double const C = 1.0/(1.0+X_in);
    double const tmp = pow(C+0.5*(j_H/X)*(j_H/X), -2.0) - X*X;

    if (tmp <= 0.0)
        return 0.0;

    return R_outer*sqrt(tmp);
}

double rho_max()
{
    using std::pow;
    using std::sqrt;

    double const n = polytropic_n;

    double const R_0 = R_outer*2.0*X_in/(1.0+X_in);
    double const j_H = sqrt(2.0*X_in/(1.0+X_in));
    double const X_max = R_0/R_outer;
    double const C = 1.0/(1.0+X_in);
    double const H_max = 1.0/sqrt(X_max*X_max) - 0.5*(j_H/X_max)*(j_H/X_max)-C;

    return pow(H_max/((n+1)*kappa), n);
}

double density_floor()
{
    return 1e-10 * rho_max();
}

///////////////////////////////////////////////////////////////////////////////
// Kernels.
struct initialize : octopus::trivial_serialization
{ // {{{
    void operator()(octopus::octree_server& U) const
    {
/*
        boost::uint64_t const gnx = octopus::config().grid_node_length;

        for (boost::uint64_t i = 0; i < gnx; ++i)
            for (boost::uint64_t j = 0; j < gnx; ++j)
                for (boost::uint64_t k = 0; k < gnx; ++k)
                {
                    if (U.x_center(i) > 0.0 && U.y_center(j) > 0.0)
                        rho(U(i, j, k)) = 1.0;
                    else
                        rho(U(i, j, k)) = 1.0e-10;

                    momentum_x(U(i, j, k))       = 0.0;
                    momentum_y(U(i, j, k))       = 0.0;
                    momentum_z(U(i, j, k))       = 0.0;
                    total_energy(U(i, j, k))     = 0.0;
                    tau(U(i, j, k))              = 0.0;
                    angular_momentum(U(i, j, k)) = 0.0;
                    U(i, j, k)[radial_momentum_idx] = 0.0;
                }
*/
        using std::pow;
        using std::sqrt;
        using std::atan2;
        using std::cos;

        double const ei0 = 1.0;
        double const tau0 = pow(ei0, 1.0 / gamma_);
        double const rho1 = density_floor();
        double const ei1 = density_floor();
        double const tau1 = pow(ei1, 1.0 / gamma_);

        double const R_inner = X_in * R_outer;

        double const C = 1.0/(1.0+X_in);

        double const j_H = sqrt(2.0*X_in/(1.0+X_in));
        // Conversion to "real" units (REVIEW: What does this mean?)
        double const j_here = j_H*sqrt(G*M_C*R_outer);

        boost::uint64_t const gnx = octopus::config().grid_node_length;

        for (boost::uint64_t i = 0; i < gnx; ++i)
        {
            for (boost::uint64_t j = 0; j < gnx; ++j)
            {
                for (boost::uint64_t k = 0; k < gnx; ++k)
                {
                    double const x_here = U.x_center(i);
                    double const y_here = U.y_center(j);
                    // REVIEW: Why do we do std::abs() here?
                    double const z_here = std::fabs(U.z_center(k));

                    double const R = radius(x_here, y_here);

                    // DEBUGGING
                    //std::cout << "r       = " << R       << "\n"
                    //          << "R_inner = " << R_inner << "\n"
                    //          << "R_outer = " << R_outer << "\n";

                    if ((R_inner <= R) && (R_outer >= R))
                    {
                        // DEBUGGING
                        //std::cout << "z     = " << z_here << "\n"
                        //          << "z_max = " << z_max  << "\n";

                        if (z_here <= z_max(R))
                        {
                            double const X = R/R_outer;

                            double const z = z_here/R_outer;
                            double const H_here = 1.0/sqrt(X*X+z*z)
                                                - 0.5*(j_H/X)*(j_H/X) - C;

                            double const n = polytropic_n;
                            double rho_here = (pow(H_here/((n+1)*kappa), n))
                                            * (G*M_C/R_outer);

                            OCTOPUS_ASSERT(rho_here > 0.0);

                            double const theta_here = atan2(y_here, x_here);

                            if (kick_mode != 0)
                                rho_here *= 1.0 + ( kick_amplitude
                                                  * cos(kick_mode*theta_here));

                            OCTOPUS_ASSERT(rho_here > 0.0);

                            rho(U(i, j, k)) = rho_here;

                            double& mom_x = momentum_x(U(i, j, k));
                            double& mom_y = momentum_y(U(i, j, k));

                            switch (rot_dir)
                            {
                                case rotate_counterclockwise:
                                {
                                    mom_x = (-y_here)*rho_here*j_here/pow(R, 2);
                                    mom_y = (+x_here)*rho_here*j_here/pow(R, 2);
                                    break;
                                }

                                case rotate_clockwise:
                                {
                                    mom_x = (+y_here)*rho_here*j_here/pow(R, 2);
                                    mom_y = (-x_here)*rho_here*j_here/pow(R, 2);
                                    break;
                                }

                                default: OCTOPUS_ASSERT(false);
                            }

                            total_energy(U(i, j, k))     = ei0;
                            tau(U(i, j, k))              = tau0;
                            angular_momentum(U(i, j, k)) = j_here*rho_here;
                        }

                        else
                        {
                            rho(U(i, j, k))              = rho1;
                            momentum_x(U(i, j, k))       = 0.0;
                            momentum_y(U(i, j, k))       = 0.0;
                            total_energy(U(i, j, k))     = ei1;
                            tau(U(i, j, k))              = tau1;
                            angular_momentum(U(i, j, k)) = 0.0;
                        }
                    }

                    else
                    {
                        rho(U(i, j, k))              = rho1;
                        momentum_x(U(i, j, k))       = 0.0;
                        momentum_y(U(i, j, k))       = 0.0;
                        total_energy(U(i, j, k))     = ei1;
                        tau(U(i, j, k))              = tau1;
                        angular_momentum(U(i, j, k)) = 0.0;
                    }

                    // DEBUGGING
                    //std::cout << "(" << x_here
                    //          << ", " << y_here
                    //          << ", " << z_here << ") == "
                    //          << rho(U(i, j, k)) << "\n";

                    momentum_z(U(i, j, k))          = 0.0;
                    U(i, j, k)[radial_momentum_idx] = 0.0;

//                    rho(U(i, j, k)) = (std::max)(rho(U(i, j, k))
//                                               , density_floor());
                }
            }
        }
    }
}; // }}}

struct enforce_outflow : octopus::trivial_serialization
{
    void operator()(
        octopus::octree_server& U
      , octopus::state& u
      , octopus::array<double, 3> const& loc
      , octopus::face f
        ) const
    {
/*
        std::cout << "ENFORCE OUTFLOW: (" << loc[0]
                                  << ", " << loc[1]
                                  << ", " << loc[2]
                                  << ") " << f;
*/
        switch (invert(f))
        {
            case octopus::XU:
            {
                if (velocity<octopus::x_axis>(u, loc) > 0.0)
                {
//                    std::cout << " OUTFLOW";
                    total_energy(u) -= 0.5*momentum_x(u)*momentum_x(u)/rho(u);
                    momentum_x(u) = 0.0;

                    //double const vy = velocity<octopus::y_axis>(u, loc);
                    double const R = radius(loc);
                    angular_momentum(u) =
                        loc[0]*velocity<octopus::y_axis>(u, loc)*rho(u);
                    u[radial_momentum_idx] =
                        loc[1]*velocity<octopus::y_axis>(u, loc)*rho(u)/R;
                }
                break;
            }
            case octopus::XL:
            {
                if (velocity<octopus::x_axis>(u, loc) < 0.0)
                {
//                    std::cout << " OUTFLOW";
                    total_energy(u) -= 0.5*momentum_x(u)*momentum_x(u)/rho(u);
                    momentum_x(u) = 0.0;

                    //double const vy = velocity<octopus::y_axis>(u, loc);
                    double const R = radius(loc);
                    angular_momentum(u) =
                        loc[0]*velocity<octopus::y_axis>(u, loc)*rho(u);
                    u[radial_momentum_idx] =
                        loc[1]*velocity<octopus::y_axis>(u, loc)*rho(u)/R;
                }
                break;
            }

            case octopus::YU:
            {
                if (velocity<octopus::y_axis>(u, loc) > 0.0)
                {
//                    std::cout << " OUTFLOW";
                    total_energy(u) -= 0.5*momentum_y(u)*momentum_y(u)/rho(u);
                    momentum_y(u) = 0.0;

                    //double const vx = velocity<octopus::x_axis>(u, loc);
                    double const R = radius(loc);
                    angular_momentum(u) =
                        -loc[1]*velocity<octopus::x_axis>(u, loc)*rho(u);
                    u[radial_momentum_idx] =
                        loc[0]*velocity<octopus::x_axis>(u, loc)*rho(u)/R;
                }
                break;
            }
            case octopus::YL:
            {
                if (velocity<octopus::y_axis>(u, loc) < 0.0)
                {
//                    std::cout << " OUTFLOW";
                    total_energy(u) -= 0.5*momentum_y(u)*momentum_y(u)/rho(u);
                    momentum_y(u) = 0.0;

                    //double const vx = velocity<octopus::x_axis>(u, loc);
                    double const R = radius(loc);
                    angular_momentum(u) =
                        -loc[1]*velocity<octopus::x_axis>(u, loc)*rho(u);
                    u[radial_momentum_idx] =
                        loc[0]*velocity<octopus::x_axis>(u, loc)*rho(u)/R;
                }
                break;
            }

            case octopus::ZU:
            {
                if (momentum_z(u) > 0.0)
                {
//                    std::cout << " OUTFLOW";
                    total_energy(u) -= 0.5*momentum_z(u)*momentum_z(u)/rho(u);
                    momentum_z(u) = 0.0;
                }
                break;
            }
            case octopus::ZL:
            {
                if (momentum_z(u) < 0.0)
                {
//                    std::cout << " OUTFLOW";
                    total_energy(u) -= 0.5*momentum_z(u)*momentum_z(u)/rho(u);
                    momentum_z(u) = 0.0;
                }
                break;
            }

            default: OCTOPUS_ASSERT(false); break;
        }

//        std::cout << "\n";
    }
};

struct enforce_lower_limits : octopus::trivial_serialization
{
    void operator()(
        octopus::state& u
      , octopus::array<double, 3> const& v
        ) const
    {
//        OCTOPUS_ASSERT(rho(u) >= 0.0);

        rho(u) = (std::max)(rho(u), density_floor());

        double const internal_energy = total_energy(u) - kinetic_energy(u, v);

        if (internal_energy > 0.1 * total_energy(u))
        {
            tau(u) = std::pow((std::max)(internal_energy, density_floor())
                                       , 1.0 / gamma_);
        }

        // Floor everything in the center of the grid.
        double const R_inner = X_in * R_outer;

        if (radius(v) < 0.5 * R_inner)
        {
            // REVIEW: Why don't we floor tau and energy?
            rho(u)                 = density_floor();
            momentum_x(u)          = 0.0;
            momentum_y(u)          = 0.0;
            momentum_z(u)          = 0.0;
            angular_momentum(u)    = 0.0;
            u[radial_momentum_idx] = 0.0;
        }

        // If we're not conserving angular momentum, define angular momentum
        // in terms of x and y momentum.
        // FIXME: Not sure this belongs in this particular function, or perhaps
        // this hook should be renamed to be something more generic than
        // enforce_lower_limits.
        else if (mom_cons != angular_momentum_conservation)
            angular_momentum(u) = momentum_y(u)*v[0] - momentum_x(u)*v[1];
    }
};

struct reflect_z : octopus::trivial_serialization
{
    void operator()(octopus::state& s) const
    {
        momentum_z(s) = -momentum_z(s);
    }
};

struct max_eigenvalue : octopus::trivial_serialization
{
    double operator()(
        octopus::octree_server& U
      , octopus::state const& u
      , octopus::array<double, 3> const& v
      , octopus::axis a
        ) const
    {
        switch (a)
        {
            case octopus::x_axis:
                return std::fabs(velocity<octopus::x_axis>(u, v))
                     + speed_of_sound(u);

            case octopus::y_axis:
                return std::fabs(velocity<octopus::y_axis>(u, v))
                     + speed_of_sound(u);

            case octopus::z_axis:
                return std::fabs(velocity<octopus::z_axis>(u, v))
                     + speed_of_sound(u);

            default: { OCTOPUS_ASSERT(false); break; }
        }

        return 0.0;
    }
};

// FIXME: Refactor reconstruction harness (nearly identical code is used in
// octree_server for computing fluxes).
struct cfl_treewise_compute_dt : octopus::trivial_serialization
{
    cfl_treewise_compute_dt() {}

    double compute_x_dt(octopus::octree_server& U) const
    { // {{{
        double dt_inv = 0.0;

        boost::uint64_t const bw = octopus::science().ghost_zone_length;
        boost::uint64_t const gnx = octopus::config().grid_node_length;

/*
        octopus::vector2d<double> q0(gnx);
        octopus::vector2d<double> ql(gnx);
        octopus::vector2d<double> qr(gnx);
*/
        std::vector<octopus::state> q0(gnx);
        std::vector<octopus::state> ql(gnx);
        std::vector<octopus::state> qr(gnx);

        for (boost::uint64_t k = bw; k < (gnx - bw); ++k)
            for (boost::uint64_t j = bw; j < (gnx - bw); ++j)
            {
                for (boost::uint64_t i = 0; i < gnx; ++i)
                {
                    q0[i] = U(i, j, k);

                    octopus::array<double, 3> loc = U.center_coords(i, j, k);

                    octopus::science().conserved_to_primitive(q0[i], loc);
                }

                octopus::science().reconstruct(q0, ql, qr);

                for (boost::uint64_t i = bw; i < gnx - bw + 1; ++i)
                {
                    octopus::array<double, 3> loc = U.x_face_coords(i, j, k);

                    octopus::science().primitive_to_conserved(ql[i], loc);
                    octopus::science().primitive_to_conserved(qr[i], loc);

                    double const l_dt_inv =
                        (max_eigenvalue()(U, ql[i], loc, octopus::x_axis));
                    double const r_dt_inv =
                        (max_eigenvalue()(U, qr[i], loc, octopus::x_axis));

                    dt_inv = octopus::maximum(dt_inv, l_dt_inv, r_dt_inv);
                    OCTOPUS_ASSERT(0.0 < dt_inv);
                }
            }

        return cfl_factor * (1.0 / (dt_inv / (U.get_dx())));
    } // }}}

    double compute_y_dt(octopus::octree_server& U) const
    { // {{{
        double dt_inv = 0.0;

        boost::uint64_t const bw = octopus::science().ghost_zone_length;
        boost::uint64_t const gnx = octopus::config().grid_node_length;

/*
        octopus::vector2d<double> q0(gnx);
        octopus::vector2d<double> ql(gnx);
        octopus::vector2d<double> qr(gnx);
*/
        std::vector<octopus::state> q0(gnx);
        std::vector<octopus::state> ql(gnx);
        std::vector<octopus::state> qr(gnx);

        for (boost::uint64_t i = bw; i < (gnx - bw); ++i)
            for (boost::uint64_t k = bw; k < (gnx - bw); ++k)
            {
                for (boost::uint64_t j = 0; j < gnx; ++j)
                {
                    q0[j] = U(i, j, k);

                    octopus::array<double, 3> loc = U.center_coords(i, j, k);

                    octopus::science().conserved_to_primitive(q0[j], loc);
                }

                octopus::science().reconstruct(q0, ql, qr);

                for (boost::uint64_t j = bw; j < gnx - bw + 1; ++j)
                {
                    octopus::array<double, 3> loc = U.y_face_coords(i, j, k);

                    octopus::science().primitive_to_conserved(ql[j], loc);
                    octopus::science().primitive_to_conserved(qr[j], loc);

                    double const l_dt_inv =
                        (max_eigenvalue()(U, ql[j], loc, octopus::y_axis));
                    double const r_dt_inv =
                        (max_eigenvalue()(U, qr[j], loc, octopus::y_axis));

                    dt_inv = octopus::maximum(dt_inv, l_dt_inv, r_dt_inv);
                    OCTOPUS_ASSERT(0.0 < dt_inv);
                }
            }

        return cfl_factor * (1.0 / (dt_inv / (U.get_dx())));
    } // }}}

    double compute_z_dt(octopus::octree_server& U) const
    { // {{{
        double dt_inv = 0.0;

        boost::uint64_t const bw = octopus::science().ghost_zone_length;
        boost::uint64_t const gnx = octopus::config().grid_node_length;

/*
        octopus::vector2d<double> q0(gnx);
        octopus::vector2d<double> ql(gnx);
        octopus::vector2d<double> qr(gnx);
*/
        std::vector<octopus::state> q0(gnx);
        std::vector<octopus::state> ql(gnx);
        std::vector<octopus::state> qr(gnx);

        for (boost::uint64_t i = bw; i < (gnx - bw); ++i)
            for (boost::uint64_t j = bw; j < (gnx - bw); ++j)
            {
                for (boost::uint64_t k = 0; k < gnx; ++k)
                {
                    q0[k] = U(i, j, k);

                    octopus::array<double, 3> loc = U.center_coords(i, j, k);

                    octopus::science().conserved_to_primitive(q0[k], loc);
                }

                octopus::science().reconstruct(q0, ql, qr);

                for (boost::uint64_t k = bw; k < gnx - bw + 1; ++k)
                {
                    octopus::array<double, 3> loc = U.z_face_coords(i, j, k);

                    octopus::science().primitive_to_conserved(ql[k], loc);
                    octopus::science().primitive_to_conserved(qr[k], loc);

                    double const l_dt_inv =
                        (max_eigenvalue()(U, ql[k], loc, octopus::z_axis));
                    double const r_dt_inv =
                        (max_eigenvalue()(U, qr[k], loc, octopus::z_axis));

                    dt_inv = octopus::maximum(dt_inv, l_dt_inv, r_dt_inv);
                    OCTOPUS_ASSERT(0.0 < dt_inv);
                }
            }

        return cfl_factor * (1.0 / (dt_inv / (U.get_dx())));
    } // }}}

    // Compute maximum dt locally in parallel.
    double operator()(octopus::octree_server& U) const
    {
        // Do two directions in other threads.
        boost::array<hpx::future<double>, 2> xy =
        { {
            hpx::async(boost::bind
                (&cfl_treewise_compute_dt::compute_x_dt, this, boost::ref(U)))
          , hpx::async(boost::bind
                (&cfl_treewise_compute_dt::compute_y_dt, this, boost::ref(U)))
        } };

        // And do one direction here.
        double dt_limit = compute_z_dt(U);
        OCTOPUS_ASSERT(0.0 < dt_limit);

        // Wait for the x and y computations.
        dt_limit = (std::min)(dt_limit, xy[0].move());
        OCTOPUS_ASSERT(0.0 < dt_limit);

        dt_limit = (std::min)(dt_limit, xy[1].move());
        OCTOPUS_ASSERT(0.0 < dt_limit);

        return dt_limit;
    }
};

struct cfl_initial_dt : octopus::trivial_serialization
{
    double operator()(octopus::octree_server& root) const
    {
        return initial_cfl_factor
             * root.reduce<double>(cfl_treewise_compute_dt()
                                 , octopus::minimum_functor()
                                 , std::numeric_limits<double>::max());
    }
};

// IMPLEMENT: Post prediction.
struct cfl_predict_dt
{
  private:
    double max_dt_growth_;
    double fudge_factor_;

  public:
    cfl_predict_dt() : max_dt_growth_(0.0), fudge_factor_(0.0) {}

    cfl_predict_dt(
        double max_dt_growth
      , double fudge_factor
        )
      : max_dt_growth_(max_dt_growth)
      , fudge_factor_(fudge_factor)
    {}

    /// Returns the tuple (timestep N + 1 size, timestep N + gap size)
    octopus::dt_prediction operator()(
        octopus::octree_server& root
        ) const
    {
        OCTOPUS_ASSERT(0 < max_dt_growth_);
        OCTOPUS_ASSERT(0 < fudge_factor_);

        OCTOPUS_ASSERT(0 == root.get_level());

        double next_dt = root.reduce<double>(cfl_treewise_compute_dt()
                                           , octopus::minimum_functor()
                                           , std::numeric_limits<double>::max());

        return octopus::dt_prediction(next_dt, fudge_factor_ * next_dt);
    }

    template <typename Archive>
    void serialize(Archive& ar, unsigned int)
    {
        ar & max_dt_growth_;
        ar & fudge_factor_;
    }
};

// Primitive variables are mass, velocity and pressure. Conserved variables
// are mass, momentum and energy.

struct conserved_to_primitive : octopus::trivial_serialization
{
    void operator()(
        octopus::state& u
      , octopus::array<double, 3> const& v
        ) const
    {
        double const R = radius(v);

        total_energy(u)        -= kinetic_energy(u, v);
        momentum_x(u)          /= rho(u);
        momentum_y(u)          /= rho(u);
        momentum_z(u)          /= rho(u);
        u[radial_momentum_idx] /= rho(u);
        angular_momentum(u)    /= rho(u) * R;
        angular_momentum(u)    -= omega * R;
    }
};

struct primitive_to_conserved : octopus::trivial_serialization
{
    void operator()(
        octopus::state& u
      , octopus::array<double, 3> const& v
        ) const
    {
        double const R = radius(v);

        momentum_x(u)          *= rho(u);
        momentum_y(u)          *= rho(u);
        momentum_z(u)          *= rho(u);
        u[radial_momentum_idx] *= rho(u);
        angular_momentum(u)    += omega * R;
        angular_momentum(u)    *= rho(u) * R;
        total_energy(u)        += kinetic_energy(u, v);
    }
};

struct source : octopus::trivial_serialization
{
    octopus::state operator()(
        octopus::octree_server& U
      , octopus::state const& u
      , octopus::array<double, 3> const& v
        ) const
    {
        octopus::state s;

/*
        if (  octopus::compare_real(v[0], -1.26562, 1e-5)
           && octopus::compare_real(v[1], -1.45312, 1e-5)
           && octopus::compare_real(v[2], 0.046875, 1e-5))
        {
            std::stringstream ss;
            ss << ( boost::format("SOURCE U (%g, %g, %g):")
                  % v[0] % v[1] % v[2]);
            for (boost::uint64_t i = 0; i < u.size(); ++i)
                ss << ( boost::format(" %.16x")
                      % octopus::hex_real(u[i]));
            ss << "\n";
//            ss << ( boost::format("Z GRAVITY: %.17e\n")
//                  % gravity<octopus::z_axis>(v));
            std::cout << ss.str();
        }

        return s;
*/

        double const R = radius(v);
        double const p = pressure(u);
        double const lz = angular_momentum(u);

        // This is the radial momentum source term (independent of gravity).
        s[radial_momentum_idx] += (p + std::pow(lz/R, 2)/rho(u))/R;

        // Add half of the Coriolis force for rotating cartesian momentum.
        momentum_x(s) += momentum_y(u)*omega;
        momentum_y(s) -= momentum_x(u)*omega;

        // There won't be any gravity within a certain radius of the center.
        if (R < 0.5*(X_in*R_outer))
            return s;

        momentum_x(s) += rho(u)*gravity<octopus::x_axis>(v);
        momentum_y(s) += rho(u)*gravity<octopus::y_axis>(v);
        momentum_z(s) += rho(u)*gravity<octopus::z_axis>(v);

        s[radial_momentum_idx] += rho(u)*radial_gravity(v);

        return s;
    }
};

// REVIEW: For rot_dir to work properly, do we need to make some changes here?
struct flux : octopus::trivial_serialization
{
    octopus::state operator()(
        octopus::octree_server& U
      , octopus::state& u
      , octopus::array<double, 3> const& loc
      , octopus::array<boost::uint64_t, 3> const& idx
      , octopus::axis a
        ) const
    {
        double const p = pressure(u);
        double const R = radius(loc);

        octopus::state fl;

        switch (a)
        {
            case octopus::x_axis:
            {
                // Velocity.
                double const v = velocity<octopus::x_axis>(u, loc);

                for (boost::uint64_t i = 0; i < u.size(); ++i)
                    fl[i] = u[i] * v;

                momentum_x(fl)   += p;
                total_energy(fl) += v * p;

                fl[radial_momentum_idx] += loc[0] * p / R;
                angular_momentum(fl)    -= loc[1] * p;

/*
                if (  octopus::compare_real(U.x_center(idx[0]), -1.26562, 1e-5)
                   && octopus::compare_real(U.y_center(idx[1]), -1.45312, 1e-5)
                   && octopus::compare_real(U.z_center(idx[2]), 0.046875, 1e-5))
                {
                    std::stringstream ss;
                    ss << ( boost::format("X FLUX U (%g, %g, %g):")
                          % U.x_center(idx[0])
                          % U.y_center(idx[1])
                          % U.z_center(idx[2]));
                    for (boost::uint64_t i = 0; i < fl.size(); ++i)
                        ss << ( boost::format(" %.16x")
                              % octopus::hex_real(u[i]));
                    ss << "\n";
                    ss << ( boost::format("X FLUX (%g, %g, %g):")
                          % U.x_center(idx[0])
                          % U.y_center(idx[1])
                          % U.z_center(idx[2]));
                    for (boost::uint64_t i = 0; i < fl.size(); ++i)
                        ss << ( boost::format(" %.16x")
                              % octopus::hex_real(fl[i]));
                    ss << "\n";
                    ss << (boost::format("RADIUS: %.17e\n") % R);
                    ss << (boost::format("PRESSURE: %.17e\n") % p);
                    ss << (boost::format("VELOCITY: %.17e\n") % v);
                    std::cout << ss.str();
                }
*/

                break;
            }

            case octopus::y_axis:
            {
                double const v = velocity<octopus::y_axis>(u, loc);

                for (boost::uint64_t i = 0; i < u.size(); ++i)
                    fl[i] = u[i] * v;

                momentum_y(fl)   += p;
                total_energy(fl) += v * p;

                fl[radial_momentum_idx] += loc[1] * p / R;
                angular_momentum(fl)    += loc[0] * p;

                break;
            }

            case octopus::z_axis:
            {
                double const v = velocity<octopus::z_axis>(u, loc);

                for (boost::uint64_t i = 0; i < u.size(); ++i)
                    fl[i] = u[i] * v;

                momentum_z(fl)   += p;
                total_energy(fl) += v * p;

                break;
            }

            default: { OCTOPUS_ASSERT(false); break; }
        }

        return fl;
    }
};

struct refine_by_geometry
  : octopus::elementwise_refinement_criteria_base<refine_by_geometry>
{
    /// Returns true if we should refine the region that contains this point.
    bool refine(
        octopus::octree_server& U
      , octopus::state const& u
      , octopus::array<double, 3> loc
        )
    {
        using std::sqrt;
        using std::abs;

        double const dx = U.get_dx();

        double const xU = loc[0]+0.5*dx; // x upper
        double const xL = loc[0]-0.5*dx; // x lower
        double const yU = loc[1]+0.5*dx; // y upper
        double const yL = loc[1]-0.5*dx; // y lower

        double const radius0 = radius(loc[0], loc[1]);
        double const radius1 = radius(xU, yU);
        double const radius2 = radius(xU, yL);
        double const radius3 = radius(xL, yU);
        double const radius4 = radius(xL, yL);

        bool condition0 = (
            std::fabs(loc[2]+0.5*dx) < z_max(radius0) ||
            std::fabs(loc[2]-0.5*dx) < z_max(radius0)
            );

        bool condition1 = (
            ((radius1 < R_outer) && (radius1 > X_in*R_outer)) ||
            ((radius2 < R_outer) && (radius2 > X_in*R_outer)) ||
            ((radius3 < R_outer) && (radius3 > X_in*R_outer)) ||
            ((radius4 < R_outer) && (radius4 > X_in*R_outer))
            );

        return condition0 && condition1;
    }

    /// If this returns true for all regions in a point, that region will be
    /// unrefined.
    bool unrefine(
        octopus::octree_server& U
      , octopus::state const& u
      , octopus::array<double, 3> loc
        )
    {
        // Unused currently.
        return false;
    }

    template <typename Archive>
    void serialize(Archive& ar, const unsigned int)
    {
        typedef elementwise_refinement_criteria_base<refine_by_geometry>
            base_type;
        ar & hpx::util::base_object_nonvirt<base_type>(*this);
    }
};

struct output_equatorial_plane
{
    struct slicer
    {
      private:
        std::ofstream* ofs_;

      public:
        slicer() : ofs_(0) {}

        slicer(std::ofstream& ofs) : ofs_(&ofs) {}

        void operator()(
            octopus::octree_server& U
          , octopus::state& u
          , octopus::array<double, 3>& loc
            ) const
        {
            (*ofs_) << ( boost::format("%g %g %g %i")
                       % loc[0]
                       % loc[1]
                       % loc[2]
                       % U.get_level());

            for (boost::uint64_t i = 0; i < u.size(); ++i)
            {
//                (*ofs_) << ( boost::format(" %016x")
//                           % octopus::hex_real(u[i]));
                (*ofs_) << " " << u[i];
            }

            (*ofs_) << "\n";
        }

        template <typename Archive>
        void serialize(Archive& ar, const unsigned int)
        {
            OCTOPUS_ASSERT(false);
        };
    };

  private:
    octopus::axis axis_;
  public:
    output_equatorial_plane() : axis_() {}

    output_equatorial_plane(octopus::axis a) : axis_(a) {}

    void operator()(
        octopus::octree_server& U
      , std::ofstream& ofs
        ) const
    {
        U.slice_leaf(slicer(ofs), axis_, 1e-7);
    }

    template <typename Archive>
    void serialize(Archive& ar, const unsigned int)
    {
        ar & axis_;
    };
};

struct add_functor : octopus::trivial_serialization
{
    boost::uint64_t operator()(boost::uint64_t a, boost::uint64_t b) const
    {
        return a + b;
    }
};

struct one_functor : octopus::trivial_serialization
{
    boost::uint64_t operator()(octopus::octree_server&) const
    {
        return 1;
    }
};

boost::uint64_t count_nodes(octopus::octree_server& U)
{
    return U.reduce<boost::uint64_t>(one_functor(), add_functor(), 0);
}

struct slice_distribution : octopus::trivial_serialization
{
    hpx::id_type operator()(
        octopus::octree_init_data const& init
      , std::vector<hpx::id_type> const& localities
        ) const
    {
        double const pi = boost::math::constants::pi<double>();

        boost::uint64_t const bw = octopus::science().ghost_zone_length;
        double const grid_dim = octopus::config().spatial_domain;
        boost::uint64_t const gnx = octopus::config().grid_node_length;

        boost::uint64_t n = localities.size();

        boost::uint64_t i = gnx / 2;
        boost::uint64_t j = gnx / 2;
//        boost::uint64_t i = 0;
//        boost::uint64_t j = 0;

        double const dx0 = octopus::science().initial_dx();

        double const x = double(init.offset[0] + i) * init.dx - grid_dim
                            - bw * dx0 - init.origin[0];
        double const y = double(init.offset[1] + j) * init.dx - grid_dim
                            - bw * dx0 - init.origin[1];

//        double const x = x_face + 0.5 * init.dx;
//        double const y = y_face + 0.5 * init.dx;
//        double const x = x_face;
//        double const y = y_face;

        double theta = std::atan2(y, x);

        if (theta < 0)
            theta += 2*pi;

//        std::cout << ( boost::format("x=%1%, y=%2%, theta=%3%, loc[0]=%4%, loc[1]=%5%\n")
//                     % x % y % theta % init.location[0] % init.location[1]);

        boost::uint64_t l = 0;

        while (true)
        {
            OCTOPUS_ASSERT(l < n);

            double lower_bound = double(l)/double(n) * 2*pi;

//            std::cout << ( boost::format("l=%1%, lower_bound=%2%\n")
//                         % l % lower_bound);

            if (theta >= lower_bound)
            {
                double upper_bound = double(l+1)/double(n) * 2*pi;

                if (theta < upper_bound)
                    break;
            }

            ++l;
        }

        OCTOPUS_ASSERT(l < n);

        return localities[l];
    }
};

struct reset_checkpoint : octopus::trivial_serialization
{
    void operator()() const
    {
        octopus::checkpoint().seekp(0);
    }
};

#endif // OCTOPUS_9BA6055C_E7A9_4A16_8A24_B8B410AA1A14


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_MAXMAG_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MAXMAG_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns the value with the greatest absolute value among
    its arguments.

    @par Header <boost/simd/function/maxmag.hpp>

    @par Example:

      @snippet maxmag.cpp maxmag

    @par Possible output:

      @snippet maxmag.txt maxmag

  **/
  Value maxmag(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/maxmag.hpp>
#include <boost/simd/function/simd/maxmag.hpp>

#endif


/*
 * dstar.hpp
 *
 * Graph Library for Autonomous and Dynamic Systems
 *
 * author:  Arnaud Degroote <arnaud.degroote@laas.fr>
 * created: 2013-09-10
 * license: BSD
 */

#ifndef DSTAR_HPP
#define DSTAR_HPP

#include <boost/bimap/bimap.hpp>
#include <boost/bimap/unordered_set_of.hpp>
#include <boost/bimap/multiset_of.hpp>

#include "gladys/graph_astar.hpp"

namespace gladys {

    class no_path {};

    class dstar_search {
        /*
         * This is a fairly trivial implementation of dstar lite as described
         * in 'Fast replanning for navigation in unknown terrain' page 5 (fig
         * 5).
         *
         * The only non-trivial point is the use of boost::bimap to implement
         * the priority_queue, as one need to:
         *   - fastly get the smallest key
         *   - fastly find and erase an element by its vertex identifier.
         * In particular, use of std::priority_queue does not match the second
         * prerequisite.
         *
         * As possible, I keep the same variable name than the one used in the
         * algorithm description.
         *
         * Note that currently, the considered graph is not oriented, so
         * succ(v) == prev(v)
         *
         * TODO: allow to pass an user-defined heuristic. h is for moment the
         * euclidean distance between two vertices.
         * XXX: the replan part is not well tested, as the API does not allow
         * to upgrade a graph.
         */
        private:

            typedef std::pair<float, float> key_t;

            typedef boost::bimaps::bimap <
                boost::bimaps::unordered_set_of<vertex_t>,
                boost::bimaps::multiset_of<key_t, std::less<key_t>>
            > bm_type;

            typedef bm_type::value_type bm_value;

            struct cost {
                float g;
                float rhs;
            };

            const graph_t& g;
            vertex_t start, goal, last;
            time_t t;
            bm_type pq;
            std::vector<cost> costs;
            float km;

            key_t calc_key(const vertex_t& v) const;

            float h(const vertex_t& v1, const vertex_t& v2) const;

            void update_vertex(const vertex_t& v);
            void compute_shortest_path();

            void print_pq() const;

        public:
            dstar_search(const graph_t& g, const vertex_t& start, const vertex_t& goal);

            void write_graphviz(std::ostream& os) const;

            path_t get_path() const;

            void replan(const vertex_t& now);
    };

} // namespace gladys

#endif // DSTAR_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_COMMON_ILOG2_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_COMMON_ILOG2_HPP_INCLUDED

#include <boost/simd/bitwise/functions/ilog2.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/minusone.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/functions/simd/clz.hpp>
#include <boost/simd/include/functions/simd/exponent.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::ilog2_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {

    typedef typename dispatch::meta::as_integer<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::exponent(a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::ilog2_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {

    typedef typename dispatch::meta::as_integer<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type s_type;
      return boost::simd::minusone(boost::simd::splat<result_type>(sizeof(s_type)*8)-bitwise_cast<result_type>(boost::simd::clz(a0)));
    }
  };
} } }

#endif


#define BOOST_TEST_MODULE "test_omp_position_restraint_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/math/math.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/local/HarmonicPotential.hpp>
#include <mjolnir/omp/System.hpp>
#include <mjolnir/omp/RandomNumberGenerator.hpp>
#include <mjolnir/omp/PositionRestraintInteraction.hpp>
#include <mjolnir/util/make_unique.hpp>

BOOST_AUTO_TEST_CASE(omp_PositionRestraint)
{
    namespace test = mjolnir::test;

    constexpr double tol = 1e-8;
    mjolnir::LoggerManager::set_default_logger("test_omp_position_restraint_interaction.log");

    using seq_traits_type  = mjolnir::SimulatorTraits      <double, mjolnir::UnlimitedBoundary>;
    using omp_traits_type  = mjolnir::OpenMPSimulatorTraits<double, mjolnir::UnlimitedBoundary>;

    using real_type        = typename omp_traits_type::real_type;
    using coordinate_type  = typename omp_traits_type::coordinate_type;
    using boundary_type    = typename omp_traits_type::boundary_type;

    using potential_type   = mjolnir::HarmonicPotential<real_type>;

    using omp_system_type      = mjolnir::System<omp_traits_type>;
    using omp_interaction_type = mjolnir::PositionRestraintInteraction<omp_traits_type, potential_type>;

    using seq_system_type      = mjolnir::System<seq_traits_type>;
    using seq_interaction_type = mjolnir::PositionRestraintInteraction<seq_traits_type, potential_type>;

    const int max_number_of_threads = omp_get_max_threads();
    BOOST_TEST_WARN(max_number_of_threads > 2);
    BOOST_TEST_MESSAGE("maximum number of threads = " << max_number_of_threads);

    std::mt19937 rng(123456789);

    const std::size_t N_particle = 64;
    for(int num_thread=1; num_thread<=max_number_of_threads; ++num_thread)
    {
        omp_set_num_threads(num_thread);
        BOOST_TEST_MESSAGE("maximum number of threads = " << omp_get_max_threads());

        potential_type potential(1.0, 0.0);

        omp_system_type omp_sys(N_particle, boundary_type{});
        seq_system_type seq_sys(N_particle, boundary_type{});
        test::clear_everything(omp_sys);
        test::clear_everything(seq_sys);

        using positions_type = std::vector<std::tuple<std::size_t, coordinate_type, potential_type>>;
        positions_type positions;

        for(std::size_t i=0; i<omp_sys.size(); ++i)
        {
            const auto i_x = i % 4;
            const auto i_y = i / 4;
            const auto i_z = i / 16;

            omp_sys.position(i) = mjolnir::math::make_coordinate<coordinate_type>(i_x*2.0, i_y*2.0, i_z*2.0);
            positions.emplace_back(i, omp_sys.position(i), potential);
        }
        test::apply_random_perturbation(omp_sys, rng, 0.1);

        // init sequential one with the same coordinates
        for(std::size_t i=0; i<seq_sys.size(); ++i)
        {
            seq_sys.position(i) = omp_sys.position(i);
        }

        omp_interaction_type omp_interaction{positions_type(positions)};
        seq_interaction_type seq_interaction{positions_type(positions)};

        omp_interaction.initialize(omp_sys);
        seq_interaction.initialize(seq_sys);

        test::check_force_consistency              (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_force_and_energy_consistency   (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_energy_consistency             (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
    }
}


#include <QApplication>
#include <QAction>
#include <QMainWindow>
#include <QStringList>

#include "Scene_surface_mesh_item.h"
#include "Scene_textured_surface_mesh_item.h"
#include "Scene_polyhedron_selection_item.h"
#include "SMesh_type.h"
#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>
#include <CGAL/Three/Three.h>
#include "Scene.h"
#include <QElapsedTimer>
#include <QGraphicsScene>
#include <QGraphicsItem>
#include <QPen>
#include <QDockWidget>
#include <Messages_interface.h>
#include <CGAL/Polygon_mesh_processing/border.h>
#include <CGAL/Polygon_mesh_processing/measure.h>
#include <CGAL/property_map.h>

#include <CGAL/boost/graph/Seam_mesh.h>
#include <CGAL/boost/graph/graph_traits_Seam_mesh.h>

#include <CGAL/Surface_mesh_parameterization/ARAP_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Barycentric_mapping_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Discrete_authalic_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Discrete_conformal_map_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Iterative_authalic_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Error_code.h>
#include <CGAL/Surface_mesh_parameterization/LSCM_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Two_vertices_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/internal/orbifold_cone_helper.h>
#include <CGAL/Surface_mesh_parameterization/Orbifold_Tutte_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/parameterize.h>

#include <boost/container/flat_map.hpp>

#include <algorithm>
#include <iostream>
#include <iterator>
#include <vector>
#include <unordered_map>
#include <unordered_set>

#include <CGAL/boost/graph/properties.h>
#include <CGAL/Qt/GraphicsViewNavigation.h>

#include "ui_Parameterization_widget.h"
#include "ui_OTE_dialog.h"

typedef Scene_surface_mesh_item Scene_facegraph_item;
typedef Scene_textured_surface_mesh_item Scene_textured_facegraph_item;
typedef SMesh Textured_face_graph;
typedef SMesh Base_face_graph;
typedef SMesh Face_graph;
typedef EPICK Traits;

namespace SMP = CGAL::Surface_mesh_parameterization;

typedef std::unordered_set<boost::graph_traits<Base_face_graph>::face_descriptor> Component;
typedef std::vector<Component> Components;

struct Is_selected_property_map{
  typedef boost::graph_traits<Base_face_graph>::edge_descriptor edge_descriptor;
  typedef boost::property_map<Base_face_graph, boost::halfedge_index_t>::type HIndexMap;
  std::vector<bool>* is_selected_ptr;
  Base_face_graph* graph;
  HIndexMap idmap;
  Is_selected_property_map()
    : is_selected_ptr(nullptr), graph(nullptr) {}
  Is_selected_property_map(std::vector<bool>& is_selected,
                           Base_face_graph* graph)
    : is_selected_ptr( &is_selected), graph(graph)
  {
    idmap = get(boost::halfedge_index, *graph);
  }

  std::size_t id(edge_descriptor ed) { return get(idmap, halfedge(ed, *graph))/2; }

  friend bool get(Is_selected_property_map map, edge_descriptor ed)
  {
    CGAL_assertion(map.is_selected_ptr!=NULL);
    return (*map.is_selected_ptr)[map.id(ed)];
  }

  friend void put(Is_selected_property_map map, edge_descriptor ed, bool b)
  {
    CGAL_assertion(map.is_selected_ptr!=NULL);
    (*map.is_selected_ptr)[map.id(ed)]=b;
  }
};

class Navigation : public CGAL::Qt::GraphicsViewNavigation
{
public:
  Navigation()
    :CGAL::Qt::GraphicsViewNavigation(),
      prev_pos(QPoint(0,0))
  { }

protected:
  bool eventFilter(QObject *obj, QEvent *ev)
  {
    QGraphicsView* v = qobject_cast<QGraphicsView*>(obj);
    if(v == nullptr) {
      QWidget* viewport = qobject_cast<QWidget*>(obj);
      if(viewport == nullptr) {
        return false;
      }
      v = qobject_cast<QGraphicsView*>(viewport->parent());
      if(v == nullptr) {
        return false;
      }
    }
    switch(ev->type())
    {
    case QEvent::MouseMove: {
      QMouseEvent* me = static_cast<QMouseEvent*>(ev);
      if(is_dragging)
      {
        qreal dir[2] = {v->mapToScene(me->pos()).x() - prev_pos.x(),
                        v->mapToScene(me->pos()).y() - prev_pos.y()};

        v->translate(dir[0],dir[1]);
        v->update();
      }
      prev_pos = v->mapToScene(me->pos());
      break;
    }

    case QEvent::MouseButtonPress: {
      is_dragging = true;
      break;
    }
    case QEvent::MouseButtonRelease: {
      is_dragging = false;
      break;
    }
    case QEvent::Wheel: {
      QWheelEvent* event = static_cast<QWheelEvent*>(ev);
#if QT_VERSION < QT_VERSION_CHECK(5, 14, 0)
      QPoint pos = event->pos();
#else
      QPointF pos = event->position();
#endif
      QPointF old_pos = v->mapToScene(pos.x(), pos.y());
      if(event->angleDelta().y() <0)
        v->scale(1.2, 1.2);
      else
        v->scale(0.8, 0.8);
      QPointF new_pos = v->mapToScene(pos.x(), pos.y());
      QPointF delta = new_pos - old_pos;
      v->translate(delta.x(), delta.y());
      v->update();
      break;
    }

    case QEvent::MouseButtonDblClick: {
      v->fitInView(v->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
      break;
    }
    default:
      CGAL::Qt::GraphicsViewNavigation::eventFilter(obj, ev);
    }
    return false;
  }
private:
  bool is_dragging;
  QPointF prev_pos;
};

namespace SMP = CGAL::Surface_mesh_parameterization;

typedef Traits::FT                                                  FT;
typedef boost::graph_traits<Face_graph>::vertex_descriptor          P_vertex_descriptor;
typedef Traits::Point_2                                             Point_2;
typedef boost::graph_traits<Face_graph>::edge_descriptor            P_edge_descriptor;
typedef boost::graph_traits<Face_graph>::halfedge_descriptor        P_halfedge_descriptor;

// Textured polyhedron
typedef boost::graph_traits<Base_face_graph>::
                                         edge_descriptor            T_edge_descriptor;
typedef boost::graph_traits<Base_face_graph>::
                                         halfedge_descriptor        T_halfedge_descriptor;
typedef boost::graph_traits<Base_face_graph>::
                                         vertex_descriptor          T_vertex_descriptor;

// Seam
typedef CGAL::Unique_hash_map<T_halfedge_descriptor,Point_2>        UV_uhm;
typedef CGAL::Unique_hash_map<T_edge_descriptor,bool>               Seam_edge_uhm;
typedef CGAL::Unique_hash_map<T_vertex_descriptor,bool>             Seam_vertex_uhm;

typedef boost::associative_property_map<UV_uhm>                     UV_pmap;
typedef boost::associative_property_map<Seam_edge_uhm>              Seam_edge_pmap;
typedef boost::associative_property_map<Seam_vertex_uhm>            Seam_vertex_pmap;

typedef CGAL::Seam_mesh<Base_face_graph,
                        Seam_edge_pmap, Seam_vertex_pmap>           Seam_mesh;

typedef boost::graph_traits<Seam_mesh>::vertex_descriptor           s_vertex_descriptor;
typedef boost::graph_traits<Seam_mesh>::halfedge_descriptor         s_halfedge_descriptor;
typedef boost::graph_traits<Seam_mesh>::face_descriptor             s_face_descriptor;

typedef boost::graph_traits<Seam_mesh>::edges_size_type             s_edges_size_type;

typedef std::unordered_set<boost::graph_traits<Base_face_graph>::
face_descriptor>                                                    Component;
typedef std::vector<Component>                                      Components;

typedef std::unordered_set<s_face_descriptor>                       SComponent;
typedef std::vector<SComponent>                                     SComponents;

class UVItem : public QGraphicsItem
{
public :
  UVItem(Components* components,
         Base_face_graph* graph,
         std::vector<std::vector<float> >uv_borders,
         QRectF brect)
    :
      QGraphicsItem(),
      bounding_rect(brect),
      components(components),
      graph(graph),
      m_borders(uv_borders),
      m_concatenated_borders(),
      m_current_component(0)
  {
    std::size_t total_border_size = 0;
    for(std::size_t i=0; i<m_borders.size(); ++i)
      total_border_size += m_borders[i].size();

    m_concatenated_borders.resize(total_border_size);
    for(std::size_t i=0; i<m_borders.size(); ++i)
    {
      const std::vector<float>& ith_border = m_borders[i];
      m_concatenated_borders.insert(m_concatenated_borders.end(),
                                    ith_border.begin(), ith_border.end());
    }
  }

  ~UVItem()
  {
    delete components;
  }

  const std::vector<std::vector<float> >& borders() const { return m_borders; }
  const std::vector<float>& concatenated_borders() const { return m_concatenated_borders; }

  QRectF boundingRect() const
  {
    return bounding_rect;
  }

  QString item_name()const{ return texMesh_name; }
  void set_item_name(QString s){ texMesh_name = s;}

  void paint(QPainter *painter, const QStyleOptionGraphicsItem *, QWidget *)
  {
    QPen pen;
    QBrush brush;
    brush.setColor(QColor(100, 100, 255));
    brush.setStyle(Qt::SolidPattern);
    pen.setColor(Qt::black);
    pen.setWidth(0);
    painter->setPen(pen);
    painter->setBrush(brush);
    SMesh::Property_map<halfedge_descriptor,float> u,v;

    u = graph->add_property_map<halfedge_descriptor,float>("h:u", 0.0f).first;
    v = graph->add_property_map<halfedge_descriptor,float>("h:v", 0.0f).first;

    for( Component::iterator
         fi = components->at(m_current_component).begin();
         fi != components->at(m_current_component).end();
         ++fi)
    {
      boost::graph_traits<Base_face_graph>::face_descriptor f(*fi);

      QPointF points[3];
      boost::graph_traits<Base_face_graph>::halfedge_descriptor h = halfedge(f, *graph);;
      points[0] = QPointF(get(u, h), get(v, h));
      h = next(halfedge(f, *graph), *graph);
      points[1] = QPointF(get(u, h), get(v, h));
      h = next(next(halfedge(f, *graph), *graph), *graph);
      points[2] = QPointF(get(u, h), get(v, h));
      painter->drawPolygon(points,3);
    }
  }

  int number_of_components()const{return static_cast<int>(components->size());}
  int current_component()const{return m_current_component;}
  void set_current_component(int n){m_current_component = n;}

private:
  QString texMesh_name;
  QRectF bounding_rect;
  Components* components;
  Base_face_graph* graph;
  std::vector<std::vector<float> > m_borders;
  std::vector<float> m_concatenated_borders;
  int m_current_component;
};

using namespace CGAL::Three;

class Polyhedron_demo_parameterization_plugin :
    public QObject,
    public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")

public:
  // used by Polyhedron_demo_plugin_helper
  QList<QAction*> actions() const
  {
    return _actions;
  }

  ~Polyhedron_demo_parameterization_plugin()
  {
    delete navigation;
  }
  void init(QMainWindow* mainWindow,
            Scene_interface* scene_interface,
            Messages_interface* msg)
  {
    mw = mainWindow;
    scene = scene_interface;
    messages = msg;
    Scene* true_scene = static_cast<Scene*>(scene);
    connect(true_scene, SIGNAL(itemAboutToBeDestroyed(CGAL::Three::Scene_item*)),
            this, SLOT(destroyPolyline(CGAL::Three::Scene_item*)));
    QAction* actionMVC = new QAction("Mean Value Coordinates", mw);
    QAction* actionDCP = new QAction ("Discrete Conformal Map", mw);
    QAction* actionLSC = new QAction("Least Square Conformal Map", mw);
    QAction* actionDAP = new QAction("Discrete Authalic", mw);
    QAction* actionIAP = new QAction("Iterative Authalic", mw);
    QAction* actionARAP = new QAction("As Rigid As Possible", mw);
    QAction* actionOTE = new QAction("Orbifold Tutte Embedding", mw);
    QAction* actionBTP = new QAction("Tutte Barycentric", mw);
    actionMVC->setObjectName("actionMVC");
    actionDCP->setObjectName("actionDCP");
    actionLSC->setObjectName("actionLSC");
    actionDAP->setObjectName("actionDAP");
    actionIAP->setObjectName("actionIAP");
    actionARAP->setObjectName("actionARAP");
    actionOTE->setObjectName("actionOTE");
    actionBTP->setObjectName("actionBTP");

    _actions << actionARAP
             << actionBTP
             << actionDAP
             << actionIAP
             << actionDCP
             << actionLSC
             << actionMVC
             << actionOTE;
    autoConnectActions();
    Q_FOREACH(QAction *action, _actions)
      action->setProperty("subMenuName",
                          "Triangulated Surface Mesh Parameterization"
                          );
    dock_widget = new QDockWidget(
          "UVMapping "
          , mw);
    ui_widget.setupUi(dock_widget);
    dock_widget->setWindowTitle(tr(
                                  "UVMapping "
                                  ));
    graphics_scene = new QGraphicsScene(dock_widget);
    ui_widget.graphicsView->setScene(graphics_scene);
    ui_widget.graphicsView->setRenderHints(QPainter::Antialiasing);
    navigation = new Navigation();
    ui_widget.graphicsView->installEventFilter(navigation);
    ui_widget.graphicsView->viewport()->installEventFilter(navigation);
    ui_widget.component_numberLabel->setText("Component : 1");
    connect(ui_widget.prevButton, &QPushButton::clicked, this, &Polyhedron_demo_parameterization_plugin::on_prevButton_pressed);
    connect(ui_widget.nextButton, &QPushButton::clicked, this, &Polyhedron_demo_parameterization_plugin::on_nextButton_pressed);
    addDockWidget(dock_widget);
    dock_widget->setVisible(false);
    current_uv_item = nullptr;
  }

  bool applicable(QAction*) const
  {
    if (scene->selectionIndices().size() == 1)
    {
    return qobject_cast<Scene_facegraph_item*>(scene->item(scene->mainSelectionIndex()))
        || qobject_cast<Scene_polyhedron_selection_item*>(scene->item(scene->mainSelectionIndex()));
    }

    Q_FOREACH(CGAL::Three::Scene_interface::Item_id id, scene->selectionIndices())
    {
      //if one facegraph is found in the selection, it's fine
      if (qobject_cast<Scene_facegraph_item*>(scene->item(id)))
        return true;
    }
    return false;
  }

  void closure()
  {
    dock_widget->hide();
  }

public Q_SLOTS:
  void on_actionMVC_triggered();
  void on_actionDCP_triggered();
  void on_actionLSC_triggered();
  void on_actionDAP_triggered();
  void on_actionIAP_triggered();
  void on_actionARAP_triggered();
  void on_actionOTE_triggered();
  void on_actionBTP_triggered();
  void on_prevButton_pressed();
  void on_nextButton_pressed();

  void replacePolyline()
  {
    if(current_uv_item){
      Scene_textured_facegraph_item* t_item =
          qobject_cast<Scene_textured_facegraph_item*>(projections.key(current_uv_item));
     t_item->add_border_edges(std::vector<float>(0));
    }

    int id = scene->mainSelectionIndex();

    Q_FOREACH(UVItem* pl, projections)
    {
      if(pl==nullptr || pl != projections[scene->item(id)])
        continue;
      current_uv_item = pl;
      break;
    }

    if(!current_uv_item)
    {
      dock_widget->setWindowTitle(tr("UVMapping"));
      ui_widget.component_numberLabel->setText(QString("Component :"));
    }
    else
    {
      if(!graphics_scene->items().empty())
        graphics_scene->removeItem(graphics_scene->items().first());

      graphics_scene->addItem(current_uv_item);
      ui_widget.graphicsView->fitInView(current_uv_item->boundingRect(),
                                        Qt::KeepAspectRatio);
      ui_widget.component_numberLabel->setText(
            QString("Component : %1/%2").arg(current_uv_item->current_component()+1)
            .arg(current_uv_item->number_of_components()));
      dock_widget->setWindowTitle(tr("UVMapping for %1")
                                  .arg(current_uv_item->item_name()));
      Scene_textured_facegraph_item* t_item =
          qobject_cast<Scene_textured_facegraph_item*>(projections.key(current_uv_item));
     t_item->add_border_edges(
            current_uv_item->concatenated_borders());
    }
  }

  void destroyPolyline(CGAL::Three::Scene_item* item)
  {
    Q_FOREACH(UVItem* pli, projections)
    {
      if(projections.key(pli) != item)
        continue;
      graphics_scene->removeItem(pli);
      delete pli;
      projections.remove(item);
      break;
    }

    if(projections.empty() || projections.first() == NULL)
    {
      current_uv_item = nullptr;
      dock_widget->setWindowTitle(tr("UVMapping"));
      ui_widget.component_numberLabel->setText(QString("Component :"));
    }
    else
      current_uv_item = projections.first();
  }

protected:
  enum Parameterization_method { PARAM_MVC, PARAM_DCP, PARAM_LSC, PARAM_DAP,
                                 PARAM_IAP, PARAM_ARAP, PARAM_OTE, PARAM_BTP};
  void parameterize(Parameterization_method method);

private:
  Messages_interface *messages;
  QList<QAction*> _actions;
  QDockWidget* dock_widget;
  Ui::Parameterization ui_widget;
  QGraphicsScene *graphics_scene;
  Navigation* navigation;
  QMap<Scene_item*, UVItem*> projections;
  UVItem* current_uv_item;
}; // end Polyhedron_demo_parameterization_plugin

void Polyhedron_demo_parameterization_plugin::on_prevButton_pressed()
{
  int id = scene->mainSelectionIndex();
  Q_FOREACH(UVItem* pl, projections)
  {
    if(pl==nullptr
       || pl != projections[scene->item(id)])
      continue;

    current_uv_item = pl;
    break;
  }
  if(current_uv_item == nullptr)
    return;
  current_uv_item->set_current_component((std::max)(0,current_uv_item->current_component()-1));
  replacePolyline();
}

void Polyhedron_demo_parameterization_plugin::on_nextButton_pressed()
{
  int id = scene->mainSelectionIndex();
  Q_FOREACH(UVItem* pl, projections)
  {
    if(pl==nullptr
       || pl != projections[scene->item(id)])
      continue;

    current_uv_item = pl;
    break;
  }
  if(current_uv_item == nullptr)
    return;
  current_uv_item->set_current_component((std::min)(current_uv_item->number_of_components()-1,current_uv_item->current_component()+1));
  ui_widget.component_numberLabel->setText(QString("Component : %1/%2").arg(current_uv_item->current_component()+1).arg(current_uv_item->number_of_components()));
  replacePolyline();
}

void Polyhedron_demo_parameterization_plugin::parameterize(const Parameterization_method method)
{
  // get active polyhedron
  Scene_facegraph_item* poly_item = nullptr;
  CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  Q_FOREACH(CGAL::Three::Scene_interface::Item_id id, scene->selectionIndices())
  {
    poly_item = qobject_cast<Scene_facegraph_item*>(scene->item(id));
    if(!poly_item)
    {
      continue;
    }
    else
    {
      index = id;
      break;
    }
  }

  if(!poly_item)
  {
    CGAL::Three::Three::error("Selected item is not of the right type.");
    return;
  }

  Face_graph* pMesh = poly_item->face_graph();
  if(!pMesh)
  {
    CGAL::Three::Three::error("Selected item has no valid polyhedron.");
    return;
  }
  Scene_polyhedron_selection_item* sel_item = nullptr;
  bool is_seamed = false;
  Q_FOREACH(CGAL::Three::Scene_interface::Item_id id, scene->selectionIndices())
  {
    sel_item = qobject_cast<Scene_polyhedron_selection_item*>(scene->item(id));
    if(!sel_item)
      continue;
    if(sel_item->selected_edges.empty())
      continue;
    if(method == PARAM_OTE && sel_item->selected_vertices.empty())
       continue;
    is_seamed = true;
  }

  if(method == PARAM_OTE &&
     (sel_item == nullptr || sel_item->selected_vertices.empty())) {
    std::cerr << "\nError: no cones/seam selected; Aborting parameterization." << std::endl;
    return;
  }

  // Two property maps to store the seam edges and vertices
  Seam_edge_uhm seam_edge_uhm(false);
  Seam_edge_pmap seam_edge_pm(seam_edge_uhm);

  Seam_vertex_uhm seam_vertex_uhm(false);
  Seam_vertex_pmap seam_vertex_pm(seam_vertex_uhm);

  if(!is_seamed && is_closed(*pMesh))
  {
    CGAL::Three::Three::error("The selected mesh has no (real or virtual) border.");
    return;
  }

  QApplication::setOverrideCursor(Qt::WaitCursor);

  ///////////////////////////////////
  ////////// PARAMETERIZE ///////////
  ///////////////////////////////////

  QElapsedTimer time;
  time.start();
  // add textured polyhedon to the scene

  // \todo for surface_mesh
  Base_face_graph tMesh = *pMesh;
  std::vector<bool> mark(num_halfedges(tMesh)/2,false);
  std::vector<T_edge_descriptor> seam_edges;
  typedef boost::property_map<Base_face_graph, boost::vertex_index_t>::type VIDMap;
  VIDMap vidmap = get(boost::vertex_index, tMesh);
  if(is_seamed)
  {
    //create a textured_polyhedron edges selection from the ids of the corresponding vertices
    typedef boost::property_map<Base_face_graph, boost::halfedge_index_t>::type HIDMap;
    HIDMap hidmap = get(boost::halfedge_index, tMesh);
    for(P_edge_descriptor ed : sel_item->selected_edges)
    {
      boost::graph_traits<Face_graph>::vertex_descriptor a(source(ed, *pMesh)), b(target(ed, *pMesh));

      for(boost::graph_traits<Textured_face_graph>::edge_iterator it =
          edges(tMesh).begin(); it != edges(tMesh).end();
          ++it)
      {
        boost::graph_traits<Textured_face_graph>::vertex_descriptor ta(source(*it, tMesh)), tb(target(*it, tMesh));

        if((get(vidmap, ta) == get(vidmap, a) && get(vidmap,tb) == get(vidmap,b))
           ||
           (get(vidmap,ta) == get(vidmap,b) && get(vidmap,tb) == get(vidmap,a)))
        {
          T_edge_descriptor ted(*it);
          seam_edges.push_back(ted);
          break;
        }
      }

    }
    qDebug() << sel_item->selected_edges.size() << ", " << seam_edges.size();
    //fill seam mesh pmaps
    for(T_edge_descriptor ed : seam_edges)
    {
      T_halfedge_descriptor hd = halfedge(ed, tMesh);
      T_vertex_descriptor svd(source(hd, tMesh)), tvd(target(hd, tMesh));
      if(!is_border(ed, tMesh))
      {
        put(seam_edge_pm, ed, true);
        put(seam_vertex_pm, svd, true);
        put(seam_vertex_pm, tvd, true);
        mark[get(hidmap, hd)/2] = true;
      }
    }
  }

  // map the cones from the selection plugin to the textured polyhedron
  std::unordered_set<T_vertex_descriptor> unordered_cones;
  if(method == PARAM_OTE) {
    for(P_vertex_descriptor vd : sel_item->selected_vertices) {
      boost::graph_traits<Face_graph>::vertex_descriptor pvd(vd);
      boost::graph_traits<Textured_face_graph>::vertex_iterator it = vertices(tMesh).begin(),
          end = vertices(tMesh).end();
      for(; it!=end; ++it) {
        boost::graph_traits<Textured_face_graph>::vertex_descriptor tvd(*it);
        if(get(vidmap, *it) == get(vidmap, pvd)) {
          unordered_cones.insert(tvd);
        }
      }
    }
  }
  Seam_mesh sMesh(tMesh, seam_edge_pm, seam_vertex_pm);
  sMesh.set_seam_edges_number(static_cast<s_edges_size_type>(seam_edges.size()));

  // The parameterized values
  UV_uhm uv_uhm;
  UV_pmap uv_pm(uv_uhm);

  QString new_item_name;
  //determine the different connected_components
  boost::container::flat_map<boost::graph_traits<Base_face_graph>::face_descriptor, int> face_component_map;
  boost::associative_property_map< boost::container::flat_map<s_face_descriptor, int> >
      fccmap(face_component_map);

  Is_selected_property_map edge_pmap(mark, &tMesh);

  int number_of_components =
      CGAL::Polygon_mesh_processing::connected_components(
        tMesh,
        fccmap,
        CGAL::parameters::edge_is_constrained_map(
          edge_pmap));

  // Next is the gathering of the border halfedges of the connected component.
  // It is wrong to pass the underlying mesh tMesh: a sphere split in half does
  // not have any border if border_halfedges() is run with tMesh.
  //
  // The proper way would be to completely redesign the plugin to use Seam meshes
  // everywhere. But that's not worth it. Instead, we abuse the fact that faces
  // are the same in tMesh and sMesh.

  // the SEAM MESH faces of each connected component
  SComponents s_components(number_of_components);

  for(boost::graph_traits<Base_face_graph>::face_iterator fit = faces(tMesh).begin();
      fit != faces(tMesh).end(); ++fit) {
    s_components.at(fccmap[*fit]).insert(s_face_descriptor(*fit));
  }

  // once per component
  std::vector<std::vector<float> >uv_borders;
  uv_borders.resize(number_of_components);

  // to track whether the components are successfully parameterized
  SMP::Error_code status = SMP::OK;

  for(int current_component=0; current_component<number_of_components; ++current_component)
  {
    std::vector<s_halfedge_descriptor> border;
    PMP::border_halfedges(s_components.at(current_component),
                          sMesh, std::back_inserter(border));

    std::cout << sMesh.number_of_seam_edges() << " seams" << std::endl;
    std::cout << (s_components.at(current_component)).size() << " faces" << std::endl;
    std::cout << border.size() << " border halfedges" << std::endl;

    // find longest border in the connected component
    s_halfedge_descriptor bhd; // a halfedge on the (possibly virtual) border
    std::unordered_set<s_halfedge_descriptor> visited;
    FT result_len = 0;
    for(s_halfedge_descriptor hd : border)
    {
      assert(is_border(hd, sMesh));

      if(visited.find(hd) == visited.end())
      {
        FT len = 0;
        for(s_halfedge_descriptor haf : halfedges_around_face(hd, sMesh))
        {
          len += PMP::edge_length(haf, sMesh);
          visited.insert(haf);
        }

        if(result_len < len)
        {
          result_len = len;
          bhd = hd;
        }
      }
    }
    CGAL_postcondition(bhd != s_halfedge_descriptor());
    CGAL_postcondition(is_border(bhd, sMesh));
    typedef boost::property_map<Base_face_graph, boost::vertex_point_t>::type VPMap;
    VPMap vpmap =get(boost::vertex_point, tMesh);

    // collect the border edges for that connected component
    for(s_halfedge_descriptor haf : halfedges_around_face(bhd, sMesh))
    {
        uv_borders[current_component].push_back(get(vpmap, source(haf, tMesh)).x());
        uv_borders[current_component].push_back(get(vpmap, source(haf, tMesh)).y());
        uv_borders[current_component].push_back(get(vpmap, source(haf, tMesh)).z());

        uv_borders[current_component].push_back(get(vpmap, target(haf, tMesh)).x());
        uv_borders[current_component].push_back(get(vpmap, target(haf, tMesh)).y());
        uv_borders[current_component].push_back(get(vpmap, target(haf, tMesh)).z());
    }

    switch(method)
    {
    case PARAM_MVC:
    {
      std::cout << "Parameterize (MVC)..." << std::endl;
      new_item_name = tr("%1 (parameterized (MVC))").arg(poly_item->name());
      typedef SMP::Mean_value_coordinates_parameterizer_3<Seam_mesh> Parameterizer;
      status = SMP::parameterize(sMesh, Parameterizer(), bhd, uv_pm);
      break;
    }
    case PARAM_DCP:
    {
      new_item_name = tr("%1 (parameterized (DCP))").arg(poly_item->name());
      std::cout << "Parameterize (DCP)..." << std::endl;
      typedef SMP::Discrete_conformal_map_parameterizer_3<Seam_mesh> Parameterizer;
      status = SMP::parameterize(sMesh, Parameterizer(), bhd, uv_pm);
      break;
    }
    case PARAM_LSC:
    {
      new_item_name = tr("%1 (parameterized (LSC))").arg(poly_item->name());
      std::cout << "Parameterize (LSC)..." << std::endl;
      typedef SMP::LSCM_parameterizer_3<Seam_mesh> Parameterizer;
      status = SMP::parameterize(sMesh, Parameterizer(), bhd, uv_pm);
      break;
    }
    case PARAM_DAP:
    {
      new_item_name = tr("%1 (parameterized (DAP))").arg(poly_item->name());
      std::cout << "Parameterize (DAP)..." << std::endl;
      typedef SMP::Discrete_authalic_parameterizer_3<Seam_mesh> Parameterizer;
      status = SMP::parameterize(sMesh, Parameterizer(), bhd, uv_pm);
      break;
    }
    case PARAM_IAP:
    {
      new_item_name = tr("%1 (parameterized (IAP))").arg(poly_item->name());
      std::cout << "Parameterize (IAP)..." << std::endl;
      typedef SMP::Iterative_authalic_parameterizer_3<Seam_mesh> Parameterizer;
      Parameterizer parameterizer;
      status = parameterizer.parameterize(sMesh, bhd, uv_pm, 15 /*iterations*/);
      break;
    }
    case PARAM_ARAP:
    {
      new_item_name = tr("%1 (parameterized (ARAP))").arg(poly_item->name());
      std::cout << "Parameterize (ARAP)..." << std::endl;
      FT lambda = 10000; // a big value to ensure the parameterization is ARAP (and not ASAP)
      typedef SMP::ARAP_parameterizer_3<Seam_mesh> Parameterizer;
      status = SMP::parameterize(sMesh, Parameterizer(lambda), bhd, uv_pm);
      break;
    }
    case PARAM_OTE:
    {
      new_item_name = tr("%1 (parameterized (OTE))").arg(poly_item->name());
      std::cout << "Parameterize (OTE)..." << std::endl;

      // OTE cannot handle multiple connected components right now
      // @todo (need to remove the assertions such as cones.size() == 4
      //        and check where and when cones are used (passed by ID, for ex.?))
      if(number_of_components != 1) {
        std::cerr << "Orbifold Tutte Embedding can only handle one connected component" << std::endl;
        status = SMP::ERROR_NO_TOPOLOGICAL_BALL;
        break;
      }

      typedef SMP::Orbifold_Tutte_parameterizer_3<Seam_mesh> Parameterizer;

      // Get orbifold type
      QDialog dialog(mw);
      Ui::OTE_dialog ui;
      ui.setupUi(&dialog);
      connect(ui.buttonBox, SIGNAL(accepted()), &dialog, SLOT(accept()));
      connect(ui.buttonBox, SIGNAL(rejected()), &dialog, SLOT(reject()));

      QApplication::restoreOverrideCursor();

      int i = dialog.exec();
      if (i == QDialog::Rejected)
      {
        std::cout << "Aborting parameterization" << std::endl;
        QApplication::restoreOverrideCursor();
        return;
      }

      SMP::Orbifold_type orb = static_cast<SMP::Orbifold_type>(ui.OrbComboBox->currentIndex());
      std::cout << "Selected orbifold type: " << ui.OrbComboBox->currentText().toStdString() << std::endl;

      if((unordered_cones.size() != 3 && unordered_cones.size() != 4) ||
         (unordered_cones.size() == 3 && orb == SMP::Parallelogram ) ||
         (unordered_cones.size() == 4 && orb != SMP::Parallelogram)) {
        std::cerr << "Error: incompatible orbifold type and number of cones" << std::endl;
        std::cerr << "Types I, II & III require 3 selected vertices" << std::endl;
        std::cerr << "Type IV requires 4 selected vertices" << std::endl;
        QApplication::restoreOverrideCursor();
        return;
      }

      // Now, parameterize
      Parameterizer parameterizer(orb);

      // Mark cones in the seam mesh
      std::unordered_map<s_vertex_descriptor, SMP::Cone_type> cmap;
      if(!SMP::locate_unordered_cones(sMesh, unordered_cones.begin(), unordered_cones.end(), cmap))
      {
        std::cerr << "Error: invalid cone or seam selection" << std::endl;
        QApplication::restoreOverrideCursor();
        return;
      }

      QApplication::setOverrideCursor(Qt::WaitCursor);

      // Fill the index property map
      typedef std::unordered_map<s_vertex_descriptor, int> Indices;
      Indices indices;
      CGAL::Polygon_mesh_processing::connected_component(
             face(opposite(bhd, sMesh), sMesh),
             sMesh,
             boost::make_function_output_iterator(
             SMP::internal::Index_map_filler<Seam_mesh, Indices>(sMesh, indices)));
      boost::associative_property_map<Indices> vimap(indices);

      // Call to parameterizer
      status = parameterizer.parameterize(sMesh, bhd, cmap, uv_pm, vimap);
      break;
    }
    case PARAM_BTP:
    {
      std::cout << "Parameterize (BTP)..." << std::endl;
      new_item_name = tr("%1 (parameterized (BTP))").arg(poly_item->name());
      typedef SMP::Barycentric_mapping_parameterizer_3<Seam_mesh> Parameterizer;
      status = SMP::parameterize(sMesh, Parameterizer(), bhd, uv_pm);
      break;
    }
    } //end switch

    std::cout << "Connected component " << current_component << ": ";
    if(status == SMP::OK) {
      std::cout << "success (in " << time.elapsed() << " ms)" << std::endl;
    } else {
      std::cerr << "failure: " << SMP::get_error_message(status) << std::endl;
      QApplication::restoreOverrideCursor();
      return;
    }

    if(status != SMP::OK)
      break;

  } //end for each component

  QApplication::restoreOverrideCursor();
  QPointF pmin(FLT_MAX, FLT_MAX), pmax(-FLT_MAX, -FLT_MAX);

  SMesh::Property_map<halfedge_descriptor, float> umap;
  SMesh::Property_map<halfedge_descriptor, float> vmap;

  umap = tMesh.add_property_map<halfedge_descriptor, float>("h:u", 0.0f).first;
  vmap = tMesh.add_property_map<halfedge_descriptor, float>("h:v", 0.0f).first;

  tMesh.property_stats(std::cerr);
  Base_face_graph::Halfedge_iterator it;
  for(it = tMesh.halfedges_begin();
      it != tMesh.halfedges_end();
      ++it)
  {
    Seam_mesh::halfedge_descriptor hd(*it);
    FT u = uv_pm[target(hd, sMesh)].x();
    FT v = uv_pm[target(hd, sMesh)].y();
    put(umap, *it, static_cast<float>(u));
    put(vmap, *it, static_cast<float>(v));
    if(u<pmin.x())
      pmin.setX(u);
    if(u>pmax.x())
      pmax.setX(u);
    if(v<pmin.y())
      pmin.setY(v);
    if(v>pmax.y())
      pmax.setY(v);
  }

  Components* components = new Components(0);
  components->resize(number_of_components);
  boost::graph_traits<Base_face_graph>::face_iterator bfit;

  for(bfit = faces(tMesh).begin();
      bfit != faces(tMesh).end();
      ++bfit)
  {
    components->at(fccmap[*bfit]).insert(*bfit);
  }

  Scene_textured_facegraph_item* new_item = new Scene_textured_facegraph_item(tMesh);
  UVItem *projection = new UVItem(components,new_item->textured_face_graph(), uv_borders, QRectF(pmin, pmax));
  projection->set_item_name(new_item_name);

  new_item->setName(new_item_name);
  new_item->setColor(Qt::white);
  new_item->setRenderingMode(poly_item->renderingMode());
  connect( new_item, SIGNAL(selectionChanged()),
           this, SLOT(replacePolyline()) );
  poly_item->setVisible(false);
  scene->itemChanged(index);
  scene->addItem(new_item);
  if(!graphics_scene->items().empty())
    graphics_scene->removeItem(graphics_scene->items().first());
  graphics_scene->addItem(projection);
  projections[new_item] = projection;
  if(current_uv_item){
    Scene_textured_facegraph_item* t_item =
        qobject_cast<Scene_textured_facegraph_item*>(projections.key(current_uv_item));
   t_item->add_border_edges(std::vector<float>(0));
  }
  current_uv_item = projection;
  Scene_textured_facegraph_item* t_item =
      qobject_cast<Scene_textured_facegraph_item*>(projections.key(current_uv_item));
  t_item->add_border_edges(
        current_uv_item->concatenated_borders());
  if(dock_widget->isHidden()){
    dock_widget->setVisible(true);
    dock_widget->raise();
  }
  dock_widget->setWindowTitle(tr("UVMapping for %1").arg(new_item->name()));
  ui_widget.component_numberLabel->setText(QString("Component : %1/%2").arg(current_uv_item->current_component()+1).arg(current_uv_item->number_of_components()));
  ui_widget.graphicsView->fitInView(projection->boundingRect(), Qt::KeepAspectRatio);

  QApplication::restoreOverrideCursor();

}

void Polyhedron_demo_parameterization_plugin::on_actionMVC_triggered()
{
  std::cerr << "MVC...";
  parameterize(PARAM_MVC);
}

void Polyhedron_demo_parameterization_plugin::on_actionDCP_triggered()
{
  std::cerr << "DCP...";
  parameterize(PARAM_DCP);
}

void Polyhedron_demo_parameterization_plugin::on_actionLSC_triggered()
{
  std::cerr << "LSC...";
  parameterize(PARAM_LSC);
}

void Polyhedron_demo_parameterization_plugin::on_actionDAP_triggered()
{
  std::cerr << "DAP...";
  parameterize(PARAM_DAP);
}

void Polyhedron_demo_parameterization_plugin::on_actionIAP_triggered()
{
  std::cerr << "IAP...";
  parameterize(PARAM_IAP);
}

void Polyhedron_demo_parameterization_plugin::on_actionARAP_triggered()
{
  std::cerr << "ARAP...";
  parameterize(PARAM_ARAP);
}

void Polyhedron_demo_parameterization_plugin::on_actionOTE_triggered()
{
  std::cerr << "OTE...";
  parameterize(PARAM_OTE);
}

void Polyhedron_demo_parameterization_plugin::on_actionBTP_triggered()
{
  std::cerr << "BTP...";
  parameterize(PARAM_BTP);
}

#include "Parameterization_plugin.moc"