/*
Copyright 2020 Dennis Rohde

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#pragma once

#include <unordered_map>

#include <boost/chrono/include.hpp>

#include "random.hpp"
#include "curve.hpp"
#include "frechet.hpp"
#include "dynamic_time_warping.hpp"
#include "simplification.hpp"

namespace Clustering {

struct Distance_Matrix : public std::vector<std::vector<distance_t>> {
    Distance_Matrix() {}
    Distance_Matrix(const curve_number_t n, const curve_number_t m) : std::vector<std::vector<distance_t>>(n, std::vector<distance_t>(m, -1.0)) {}

    void print() const {
        for (const auto &row : *this) {
            for (const auto elem : row) {
                std::cout << elem << " ";
            }
            std::cout << std::endl;
        }
    }
};

class Cluster_Assignment : public std::vector<std::vector<curve_number_t>> {
public:
    inline Cluster_Assignment(const curve_number_t k = 0) : std::vector<std::vector<curve_number_t>>(k, std::vector<curve_number_t>()) {}

    inline curve_number_t count(const curve_number_t i) const {
        return operator[](i).size();
    }

    inline curve_number_t get(const curve_number_t i, const curve_number_t j) const {
        return operator[](i)[j];
    }

};

inline distance_t _cheap_dist(const curve_number_t i, const curve_number_t j, const Curves &in, const Curves &simplified_in, Distance_Matrix &distances) {
    if (distances[i][j] < 0) {
        const auto dist = Frechet::Continuous::distance(in[i], simplified_in[j]);
        distances[i][j] = dist.value;
    }
    return distances[i][j];
}

inline curve_number_t _nearest_center(const curve_number_t i, const Curves &in, const Curves &simplified_in, const std::vector<curve_number_t> &centers, Distance_Matrix &distances) {
    const auto infty = std::numeric_limits<distance_t>::infinity();
    // cost for curve is infinity
    auto min_cost = infty;
    curve_number_t nearest = 0;

    // except there is a center with smaller cost, then choose the one with smallest cost
    for (curve_number_t j = 0; j < centers.size(); ++j) {
        if (_cheap_dist(i, centers[j], in, simplified_in, distances) < min_cost) {
            min_cost = _cheap_dist(i, centers[j], in, simplified_in, distances);
            nearest = j;
        }
    }
    return nearest;
}

inline distance_t _curve_cost(const curve_number_t i, const Curves &in, const Curves &simplified_in, const std::vector<curve_number_t> &centers, Distance_Matrix &distances) {
    return _cheap_dist(i, centers[_nearest_center(i, in, simplified_in, centers, distances)], in, simplified_in, distances);
}

inline distance_t _center_cost_sum(const Curves &in, const Curves &simplified_in, const std::vector<curve_number_t> &centers, Distance_Matrix &distances) {
    distance_t cost = 0;

    // for all curves
    for (curve_number_t i = 0; i < in.size(); ++i) {
        const auto min_cost_elem = _curve_cost(i, in, simplified_in, centers, distances);
        cost += min_cost_elem;
    }
    return cost;
}

inline Cluster_Assignment _cluster_assignment(const Curves &in, const Curves &simplified_in, const std::vector<curve_number_t> &centers, Distance_Matrix &distances) {
    const auto k = centers.size();
    Cluster_Assignment result(centers.size());

    if (k == 0) return result;

    for (curve_number_t i = 0; i < in.size(); ++i) result[_nearest_center(i, in, simplified_in, centers, distances)].push_back(i);

    return result;
}

struct Clustering_Result {
    Curves centers;
    distance_t value;
    double running_time;
    Cluster_Assignment assignment;

    inline Curve& get(const curve_number_t i) {
        return centers[i];
    }
    inline curve_number_t size() const {
        return centers.size();
    }

    inline Curves::const_iterator cbegin() const {
        return centers.cbegin();
    }

    inline Curves::const_iterator cend() const {
        return centers.cend();
    }

    inline void compute_assignment(const Curves &in) {
        Distance_Matrix distances(in.size(), centers.size());
        std::vector<curve_number_t> center_indices = std::vector<curve_number_t>(centers.size(), 0);
        for (curve_size_t i = 1; i < centers.size(); ++i) center_indices[i] = i;
        assignment = _cluster_assignment(in, centers, center_indices, distances);
    }
};

Clustering_Result gonzalez(const curve_number_t num_centers, const curve_size_t ell, const Curves &in, Distance_Matrix &distances, const bool arya = false, const Curves &center_domain = Curves(), const bool random_start_center = true) {

    const auto start = boost::chrono::process_real_cpu_clock::now();
    Clustering_Result result;

    if (in.empty()) return result;

    std::vector<curve_number_t> centers;
    const Curves &simplified_in = center_domain;

    if (center_domain.empty()) {
        Curves simplified_in_self(in.number(), ell, in.dimensions());

        for (curve_number_t i = 0; i < in.size(); ++i) {
            Simplification::Subcurve_Shortcut_Graph graph(const_cast<Curve&>(in[i]));
            auto simplified_curve = graph.weak_minimum_error_simplification(ell);
            simplified_curve.set_name("Simplification of " + in[i].get_name());
            simplified_in_self[i] = simplified_curve;
        }
        const_cast<Curves&>(simplified_in) = simplified_in_self;
    }

    if (random_start_center) {

        Random::Uniform_Random_Generator<double> ugen;
        const curve_number_t r =  std::floor(simplified_in.size() * ugen.get());
        centers.push_back(r);

    } else centers.push_back(0);

    distance_t curr_maxdist = 0;
    curve_number_t curr_maxcurve = 0;
    distance_t curr_curve_cost;

    if (distances.empty()) distances = Distance_Matrix(in.size(), simplified_in.size());

    {
        // remaining centers
        for (curve_number_t i = 1; i < num_centers; ++i) {

            curr_maxdist = 0;
            curr_maxcurve = 0;
            {

                // all curves
                for (curve_number_t j = 0; j < in.size(); ++j) {

                    curr_curve_cost = _curve_cost(j, in, simplified_in, centers, distances);

                    if (curr_curve_cost > curr_maxdist) {
                        curr_maxdist = curr_curve_cost;
                        curr_maxcurve = j;
                    }

                }
                #if DEBUG
                std::cout << "found center no. " << i+1 << std::endl;
                #endif

                centers.push_back(curr_maxcurve);
            }
        }
    }

    if (arya) {

        auto cost = _center_cost_sum(in, simplified_in, centers, distances);
        auto approxcost = cost;
        auto gamma = 1/(3 * num_centers * in.size());
        auto found = true;

        // try to improve current solution
        while (found) {
            found = false;

            // go through all centers
            for (curve_number_t i = 0; i < num_centers; ++i) {
                auto curr_centers = centers;

                // check if there is a better center among all other curves
                for (curve_number_t j = 0; j < simplified_in.size(); ++j) {
                    // continue if curve is already part of center set
                    if (std::find(curr_centers.begin(), curr_centers.end(), j) != curr_centers.end()) continue;

                    // swap
                    curr_centers[i] = j;
                    // new cost
                    const auto curr_cost = _center_cost_sum(in, simplified_in, curr_centers, distances);
                    // check if improvement is done
                    if (curr_cost < cost - gamma * approxcost) {
                        cost = curr_cost;
                        centers = curr_centers;
                        found = true;
                    }
                }
            }
        }
        curr_maxdist = cost;
    }

    Curves simpl_centers;
    for (const auto center: centers) simpl_centers.push_back(simplified_in[center]);

    auto end = boost::chrono::process_real_cpu_clock::now();
    result.centers = simpl_centers;
    result.value = curr_maxdist;
    result.running_time = (end-start).count() / 1000000000.0;
    return result;
}

Clustering_Result arya(const curve_number_t num_centers, const curve_size_t ell, const Curves &in, Distance_Matrix &distances, const Curves &center_domain = Curves()) {
    return gonzalez(num_centers, ell, in, distances, true, center_domain, false);
}

Clustering_Result one_median_sampling(const curve_size_t ell, const Curves &in, const double epsilon, const Curves &center_domain = Curves()) {
    const auto start = boost::chrono::process_real_cpu_clock::now();
    Clustering_Result result;
    std::vector<curve_number_t> centers;
    const Curves &simplified_in = center_domain;

    if (center_domain.empty()) {
        Curves simplified_in_self(in.number(), ell, in.dimensions());

        for (curve_number_t i = 0; i < in.size(); ++i) {
            Simplification::Subcurve_Shortcut_Graph graph(const_cast<Curve&>(in[i]));
            auto simplified_curve = graph.weak_minimum_error_simplification(ell);
            simplified_curve.set_name("Simplification of " + in[i].get_name());
            simplified_in_self[i] = simplified_curve;
        }
        const_cast<Curves&>(simplified_in) = simplified_in_self;
    }

    const auto n = in.size();

    const auto s = std::ceil(60);
    const auto t = std::ceil(std::log(60)/(epsilon*epsilon));

    Random::Uniform_Random_Generator<double> ugen;

    const auto candidates = ugen.get(s);
    const auto witnesses = ugen.get(t);

    Distance_Matrix distances = Distance_Matrix(in.size(), in.size());

    curve_number_t best_candidate = 0;
    distance_t best_objective_value = std::numeric_limits<distance_t>::infinity();

    for (curve_number_t i = 0; i < candidates.size(); ++i) {

        const curve_number_t candidate = std::floor(candidates[i] * n);
        distance_t objective = 0;

        for (curve_number_t j = 0; j < witnesses.size(); ++j) {
            const curve_number_t witness = std::floor(witnesses[j] * n);

            _cheap_dist(witness, candidate, in, simplified_in, distances);
            objective += distances[witness][candidate];
        }

        if (objective < best_objective_value) {
            best_candidate = candidate;
            best_objective_value = objective;
        }
    }
    centers.push_back(best_candidate);

    auto end = boost::chrono::process_real_cpu_clock::now();
    result.centers.push_back(simplified_in[centers[0]]);
    result.value = _center_cost_sum(in, simplified_in, centers, distances);
    result.running_time = (end-start).count() / 1000000000.0;
    return result;
}

Clustering_Result one_median_exhaustive(const curve_size_t ell, const Curves &in, const Curves &center_domain = Curves()) {
    const auto start = boost::chrono::process_real_cpu_clock::now();
    Clustering_Result result;
    std::vector<curve_number_t> centers;
    const Curves &simplified_in = center_domain;

    if (center_domain.empty()) {
        Curves simplified_in_self(in.number(), ell, in.dimensions());

        for (curve_number_t i = 0; i < in.size(); ++i) {
            Simplification::Subcurve_Shortcut_Graph graph(const_cast<Curve&>(in[i]));
            auto simplified_curve = graph.weak_minimum_error_simplification(ell);
            simplified_curve.set_name("Simplification of " + in[i].get_name());
            simplified_in_self[i] = simplified_curve;
        }
        const_cast<Curves&>(simplified_in) = simplified_in_self;
    }

    const auto n = in.size();

    Distance_Matrix distances = Distance_Matrix(in.size(), in.size());

    curve_number_t best_candidate = 0;
    distance_t best_objective_value = std::numeric_limits<distance_t>::infinity();

    for (curve_number_t j = 0; j < in.size(); ++j) {

        distance_t objective = 0;

        for (curve_number_t i = 0; i < in.size(); ++i) {
            _cheap_dist(i, j, in, simplified_in, distances);
            objective += distances[i][j];
        }

        if (objective < best_objective_value) {
            best_candidate = j;
            best_objective_value = objective;
        }
    }
    centers.push_back(best_candidate);

    auto end = boost::chrono::process_real_cpu_clock::now();
    result.centers.push_back(simplified_in[centers[0]]);
    result.value = best_objective_value;
    result.running_time = (end-start).count() / 1000000000.0;
    return result;
}

Clustering_Result two_two_dtw_one_two_median(const Curves &in, const bool with_assignment = false) {
    const auto start = boost::chrono::process_real_cpu_clock::now();
    Clustering_Result result;

    const auto n = in.size();

    std::vector<std::vector<bool>> markings = std::vector<std::vector<bool>>(n, std::vector<bool>(in.get_m(), false));
    std::vector<curve_size_t> svert = std::vector<curve_size_t>(n, 0), evert = std::vector<curve_size_t>(n, 0);

    Points S1(in.dimensions()), S2(in.dimensions());
    Point mu1(in.dimensions()), mu2(in.dimensions());

    for (curve_number_t i = 0; i < n; ++i) {
        S1.push_back(in[i][svert[i]]);
        markings[i][svert[i]] = true;
        ++svert[i];
        evert[i] = in[i].size() - 1;
        S2.push_back(in[i][evert[i]]);
        markings[i][evert[i]] = true;
        --evert[i];
    }

    mu1 = S1.centroid();
    mu2 = S2.centroid();

    const auto infty = std::numeric_limits<distance_t>::infinity();

    bool done = false;
    distance_t d1 = infty, d2 = infty, dist = infty;
    curve_number_t c1 = 0, c2 = 0;

    while (not done) {
        d1 = infty;
        d2 = infty;

        for (curve_size_t i = 0; i < in.size(); ++i) {
            if (not markings[i][svert[i]]) {
                dist = in[i][svert[i]].dist_sqr(mu1);
                if (dist < d1) {
                    d1 = dist;
                    c1 = i;
                }
            }
            if (not markings[i][evert[i]]) {
                dist = in[i][evert[i]].dist_sqr(mu2);
                if (dist < d2) {
                    d2 = dist;
                    c2 = i;
                }
            }
        }

        if (d1 < d2) {
            //std::cout << "S1 add " << c1 << "." << svert[c1] << std::endl;
            S1.push_back(in[c1][svert[c1]]);
            markings[c1][svert[c1]] = true;
            ++svert[c1];
            mu1 = S1.centroid();
            done = false;
        }
        else if (d2 < infty) {
            //std::cout << "S2 add " << c2 << "." << evert[c2] << std::endl;
            S2.push_back(in[c2][evert[c2]]);
            markings[c2][evert[c2]] = true;
            --evert[c2];
            mu2 = S2.centroid();
            done = false;
        } else done = true;

    }

    Curve center_curve(mu1.dimensions(), "center curve");
    center_curve.push_back(mu1);
    center_curve.push_back(mu2);

    distance_t cost = 0;

    for (const auto &p : S1) cost += p.dist(mu1);
    for (const auto &p : S2) cost += p.dist(mu2);

    auto end = boost::chrono::process_real_cpu_clock::now();
    result.centers.push_back(center_curve);
    result.value = cost;
    result.running_time = (end-start).count() / 1000000000.0;
    return result;
}

Clustering_Result two_two_dtw_one_two_median_exact(const Curves &in, const bool with_assignment = false) {
    const auto start = boost::chrono::process_real_cpu_clock::now();
    Clustering_Result result;
    Curve best_center(in.dimensions());
    const auto infty = std::numeric_limits<distance_t>::infinity();

    curve_size_t n = 1;
    std::vector<curve_size_t> pointers = std::vector<curve_size_t>(in.size(), 0),
                                divisors = std::vector<curve_size_t>(in.size(), 0);
    distance_t best = infty, cost = 0;
    Points S1(in.dimensions()), S2(in.dimensions());

    for (curve_number_t i = 0; i < in.size(); ++i) {
        n *= in[i].complexity() - 1;
        if (i == 0) divisors[i] = in[0].complexity() - 1;
        else if (i == 1) divisors[i] = in[0].complexity() - 1;
        else divisors[i] = divisors[i-1] * (in[i].complexity() - 1);
    }

    const auto onepercent = n / 100;

    int currperc = 0;

    for (curve_size_t i = 0; i < n; ++i) {

        if (onepercent > 0) {
            if (i / onepercent > currperc) {
                currperc = i / onepercent;
                std::cout << currperc << "% done" << std::endl;
            }
        }

        pointers[0] = i % divisors[0];
        for (curve_number_t j = 1; j < in.size(); ++j) {
            pointers[j] = (i / divisors[j]) % (in[j].complexity() - 1);
        }

        S1.clear();
        S2.clear();

        for (curve_number_t j = 0; j < in.size(); ++j) {
            for (curve_size_t k = 0; k < in[j].complexity(); ++ k) {
                if (k <= pointers[j]) {
                    S1.push_back(in[j][k]);
                    if (k == in[j].complexity() - 1) std::cerr << "error!!" << std::endl;
                }
                else S2.push_back(in[j][k]);
            }
        }

        auto mu1 = S1.centroid();
        auto mu2 = S2.centroid();

        Curve center_curve(mu1.dimensions(), "optimal center curve");
        center_curve.push_back(mu1);
        center_curve.push_back(mu2);

        cost = 0;

        for (curve_number_t j = 0; j < in.size(); ++j) {
            const auto dist = Dynamic_Time_Warping::Discrete::distance(center_curve, in[j]);
            cost += dist.value;
        }

        if (cost < best) {
            best = cost;
            best_center = center_curve;
        }
    }

    auto end = boost::chrono::process_real_cpu_clock::now();
    result.centers.push_back(best_center);
    result.value = best;
    result.running_time = (end-start).count() / 1000000000.0;
    return result;
}

}


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <climits>
//#include <iostream>

#include <boost/numeric/conversion/converter.hpp>
#include "BConverter.hh"

template<typename T, typename S> T BConverter::round_to_even(const S& x)
{
    typedef boost::numeric::conversion_traits<T, S> Traits;
    typedef boost::numeric::def_overflow_handler OverflowHandler;
    typedef boost::numeric::RoundEven<typename Traits::source_type> Rounder;
    typedef boost::numeric::converter<T, S, Traits, OverflowHandler, Rounder> Converter;
    return Converter::convert(x);
}

short BConverter::shortnorm( float v, float center, float extent ) // static
{
    float f = (v - center)/extent ;
    return std::abs(f) > 1 ? SHRT_MIN : round_to_even<short, float>( 32767.0f * f ) ;
}

#define fitsInShort(x) !(((((x) & 0xffff8000) >> 15) + 1) & 0x1fffe)
#define iround(x) ((x)>=0?(int)((x)+0.5):(int)((x)-0.5))

short BConverter::shortnorm_old( float v, float center, float extent )  // static
{
    // range of short is -32768 to 32767
    // Expect no positions out of range, as constrained by the geometry are bouncing on,
    // but getting times beyond the range eg 0.:100 ns is expected
    //
    int inorm = iround(32767.0f * (v - center)/extent ) ;    // linear scaling into -1.f:1.f * float(SHRT_MAX)
    return fitsInShort(inorm) ? short(inorm) : SHRT_MIN  ;
}

unsigned char BConverter::my__float2uint_rn_old( float f ) // static
{
    return iround(f);
}

unsigned char BConverter::my__float2uint_rn( float fv ) // static
{
    return BConverter::round_to_even<unsigned char, float>( fv ) ;
}

unsigned char BConverter::my__float2uint_rn_kludge( float fv ) // static
{
    unsigned char uc(0);
    try
    {
        uc = BConverter::my__float2uint_rn(fv ) ;
    }
    catch( boost::numeric::positive_overflow& e  )
    {
        //std::cout << e.what() << std::endl ;
    }
    catch( boost::numeric::negative_overflow& e  )
    {
        //std::cout << e.what() << std::endl ;
    }
    return uc ;
}

template BRAP_API int   BConverter::round_to_even(const float& x);
template BRAP_API short BConverter::round_to_even(const float& x);
template BRAP_API unsigned char BConverter::round_to_even(const float& x);


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2019 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include "Open3D/Geometry/Geometry3D.h"

#include <Eigen/Dense>
#include <numeric>

#include "Open3D/Utility/Console.h"

namespace open3d {
namespace geometry {

Eigen::Vector3d Geometry3D::ComputeMinBound(
        const std::vector<Eigen::Vector3d>& points) const {
    if (points.empty()) {
        return Eigen::Vector3d(0.0, 0.0, 0.0);
    }
    return std::accumulate(
            points.begin(), points.end(), points[0],
            [](const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
                return a.array().min(b.array()).matrix();
            });
}

Eigen::Vector3d Geometry3D::ComputeMaxBound(
        const std::vector<Eigen::Vector3d>& points) const {
    if (points.empty()) {
        return Eigen::Vector3d(0.0, 0.0, 0.0);
    }
    return std::accumulate(
            points.begin(), points.end(), points[0],
            [](const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
                return a.array().max(b.array()).matrix();
            });
}
Eigen::Vector3d Geometry3D::ComputeCenter(
        const std::vector<Eigen::Vector3d>& points) const {
    Eigen::Vector3d center(0, 0, 0);
    if (points.empty()) {
        return center;
    }
    center = std::accumulate(points.begin(), points.end(), center);
    center /= double(points.size());
    return center;
}

void Geometry3D::ResizeAndPaintUniformColor(
        std::vector<Eigen::Vector3d>& colors,
        const size_t size,
        const Eigen::Vector3d& color) const {
    colors.resize(size);
    Eigen::Vector3d clipped_color = color;
    if (color.minCoeff() < 0 || color.maxCoeff() > 1) {
        utility::LogWarning(
                "invalid color in PaintUniformColor, clipping to [0, 1]");
        clipped_color = clipped_color.array()
                                .max(Eigen::Vector3d(0, 0, 0).array())
                                .matrix();
        clipped_color = clipped_color.array()
                                .min(Eigen::Vector3d(1, 1, 1).array())
                                .matrix();
    }
    for (size_t i = 0; i < size; i++) {
        colors[i] = clipped_color;
    }
}

void Geometry3D::TransformPoints(const Eigen::Matrix4d& transformation,
                                 std::vector<Eigen::Vector3d>& points) const {
    for (auto& point : points) {
        Eigen::Vector4d new_point =
                transformation *
                Eigen::Vector4d(point(0), point(1), point(2), 1.0);
        point = new_point.head<3>() / new_point(3);
    }
}

void Geometry3D::TransformNormals(const Eigen::Matrix4d& transformation,
                                  std::vector<Eigen::Vector3d>& normals) const {
    for (auto& normal : normals) {
        Eigen::Vector4d new_normal =
                transformation *
                Eigen::Vector4d(normal(0), normal(1), normal(2), 0.0);
        normal = new_normal.head<3>();
    }
}

void Geometry3D::TranslatePoints(const Eigen::Vector3d& translation,
                                 std::vector<Eigen::Vector3d>& points,
                                 bool relative) const {
    Eigen::Vector3d transform = translation;
    if (!relative) {
        transform -= ComputeCenter(points);
    }
    for (auto& point : points) {
        point += transform;
    }
}

void Geometry3D::ScalePoints(const double scale,
                             std::vector<Eigen::Vector3d>& points,
                             const Eigen::Vector3d& center) const {
    for (auto& point : points) {
        point = (point - center) * scale + center;
    }
}

void Geometry3D::RotatePoints(const Eigen::Matrix3d& R,
                              std::vector<Eigen::Vector3d>& points,
                              const Eigen::Vector3d& center) const {
    for (auto& point : points) {
        point = R * (point - center) + center;
    }
}

void Geometry3D::RotateNormals(const Eigen::Matrix3d& R,
                               std::vector<Eigen::Vector3d>& normals) const {
    for (auto& normal : normals) {
        normal = R * normal;
    }
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromXYZ(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixX(rotation(0)) *
           open3d::utility::RotationMatrixY(rotation(1)) *
           open3d::utility::RotationMatrixZ(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromYZX(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixY(rotation(0)) *
           open3d::utility::RotationMatrixZ(rotation(1)) *
           open3d::utility::RotationMatrixX(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromZXY(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixZ(rotation(0)) *
           open3d::utility::RotationMatrixX(rotation(1)) *
           open3d::utility::RotationMatrixY(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromXZY(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixX(rotation(0)) *
           open3d::utility::RotationMatrixZ(rotation(1)) *
           open3d::utility::RotationMatrixY(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromZYX(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixZ(rotation(0)) *
           open3d::utility::RotationMatrixY(rotation(1)) *
           open3d::utility::RotationMatrixX(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromYXZ(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixY(rotation(0)) *
           open3d::utility::RotationMatrixX(rotation(1)) *
           open3d::utility::RotationMatrixZ(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromAxisAngle(
        const Eigen::Vector3d& rotation) {
    const double phi = rotation.norm();
    return Eigen::AngleAxisd(phi, rotation / phi).toRotationMatrix();
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromQuaternion(
        const Eigen::Vector4d& rotation) {
    return Eigen::Quaterniond(rotation(0), rotation(1), rotation(2),
                              rotation(3))
            .normalized()
            .toRotationMatrix();
}

}  // namespace geometry
}  // namespace open3d


/**

Copyright (c) 2016, Aumann Florian, Borella Jocelyn, Heller Florian, Meißner Pascal, Schleicher Ralf, Stöckle Patrick, Stroh Daniel, Trautmann Jeremias, Walter Milena, Wittenbeck Valerij
All rights reserved.

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

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

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

4. The use is explicitly not permitted to any application which deliberately try to kill or do harm to any living creature.

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.

*/

#ifndef MYMATHHELPER_HPP_
#define MYMATHHELPER_HPP_

#include "robot_model_services/typedef.hpp"
#include <boost/math/special_functions/binomial.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/foreach.hpp>
#include <ros/ros.h>
#include <set>
#include <vector>
#include <geometry_msgs/Quaternion.h>

namespace robot_model_services {
	/*!
	 * \brief MathHelper unites the generally needed math operations.
	 * \author Ralf Schleicher
	 * \date 2014
	 * \version 1.0
	 * \copyright GNU Public License
	 */
	class MathHelper {
	private:
		/*!
		 * \return the randomness generator.
		 */
		static boost::mt19937& getRandomnessGenerator();
	public:
		/*!
		 * \brief converts cartesian coordinates to sphere coordinates
		 * \param cartestian any cartesian coordinates
		 * \return sphere coordinates (radius,inclinate,azimuth)
		 */
		static SimpleSphereCoordinates convertC2S(const SimpleVector3 &cartesian);

		/*!
		 * \brief converts cartesian coordinates to sphere coordinates (lightweight)
		 * \param cartestian any cartesian coordinates
		 * \param sphere (out) sphere coordinates (radius,inclinate,azimuth)
		 */
		static void convertC2S(const SimpleVector3 &cartesian, SimpleSphereCoordinates &sphere);

		/*!
		 * \brief converts sphere coordinates to cartesian coordinates (lightweight)
		 * \param sphere any sphere coordinates (radius,inclinate,azimuth)
		 * \return cartesian coordinates
		 */
		static SimpleVector3 convertS2C(const SimpleSphereCoordinates &sphere);

		/*!
		 * \brief converts sphere coordinates to cartesian coordinates (lightweight)
		 * \param sphere any sphere coordinates (radius,inclinate,azimuth)
		 * \param cartesian (out) cartesian coordinates
		 */
		static void convertS2C(const SimpleSphereCoordinates &sphere, SimpleVector3 &cartesian);

		static SimpleVector3 getVisualAxis(const SimpleQuaternion &orientation);

		static void getVisualAxis(const SimpleQuaternion &orientation, SimpleVector3 &resultAxis);

		/*!
		 * \param value [in]
		 * \return the signum of value
		 */
		static double getSignum(const double &value);

		/*!
		 * \param idx [in] the index to project to 0
		 * \param X [in] the 3d vector used for this action.
		 * \return the 3d vector after projection
		 */
		static SimpleVector3 getProjection(const std::size_t &idx, const SimpleVector3 &X);

		/*!
		 * \param X [in] the first 3d vector
		 * \param Y [in] the second 3d vector
         * \return the cosinus between the two vectors.
		 */
		static Precision getCosinus(const SimpleVector3 &X, const SimpleVector3 &Y);

        /*!
         * \param X [in] the first 3d vector
         * \param Y [in] the second 3d vector
         * \return the angle between the two vectors
         */
        static Precision getAngle(const SimpleVector3 &X, const SimpleVector3 &Y);

        /*!
		 * \param firstAngle [in] the first angle
		 * \param secondAngle [in] the second angle
		 * \return the minimum angle difference.
		 */
		static Precision getMinimumAngleDifference(const Precision &firstAngle, const Precision &secondAngle);

		/*!
		 * \param min [in] the minumum integer
		 * \param max [in] the maximum integer
		 * \return a random integer in the range [min, max]
		 */
		static int getRandomInteger(const int &min, const int &max);

		/*!
		 * \param mean [in] the mean value
		 * \param standardDeviation [in] the standard deviation to use.
		 * \return a normal distributed random number.
		 */
		static Precision getRandomNumber(const Precision &mean, const Precision &standardDeviation);

		/*!
		 * \param mean [in] the mean value in X dimensions
		 * \param standardDeviation [in] the standard deviation to use in X dimensions.
		 * \return a normal distributed random vector.
		 */
		static SimpleVectorX getRandomVector(const SimpleVectorX &mean, const SimpleVectorX &standardDeviation);

		/*!
		 * \return a random rotation quaternion
		 */
		static SimpleQuaternion getRandomQuaternion();

		/*!
		 * \param heading [in] the heading angle
		 * \param attitude [in] the attitude angle
		 * \param bank [in] the bank angle
		 * \return the corresponding quaternion
		 */
		static SimpleQuaternion getQuaternionByAngles(const Precision &heading, const Precision &attitude, const Precision &bank);

		/*!
		 * \param numberOfPoints [in] the number of points to use.
		 * \return an array of orientations.
		 */
		static SimpleQuaternionCollectionPtr getOrientationsOnUnitSphere(const int &numberOfPoints);

		/*!
		 * \param input in radians
		 * \return input in degrees
		 */
		static double radToDeg(double input);

		/*!
		 * \param input in degrees
		 * \return input in radians
		 */
		static double degToRad(double input);

		static double getDotProduct(SimpleVector3 v1, SimpleVector3 v2);

		template<typename Set> static void printSet(boost::shared_ptr<Set> &setPtr) {
			std::cout << "\t{ ";
			BOOST_FOREACH(typename Set::value_type value, *setPtr) {
				std::cout << value << ", ";
			}
			std::cout << "}" << std::endl;
		}

		template<typename Set> static void printPowerSet(boost::shared_ptr<std::set<boost::shared_ptr<Set> > > &powerSetPtr) {
			std::cout << "{ " << std::endl;
			BOOST_FOREACH(boost::shared_ptr<Set> subSetPtr, *powerSetPtr) {
				printSet(subSetPtr);
			}
			std::cout << "} " << std::endl;
			std::cout << powerSetPtr->size() << " Items" << std::endl;
		}

		template<typename Set> static boost::shared_ptr<std::set<boost::shared_ptr<Set> > > powerSet(const boost::shared_ptr<Set> &setPtr) {
			boost::shared_ptr<std::set<boost::shared_ptr<Set> > > powerSetPtr(new std::set<boost::shared_ptr<Set> >);

			std::size_t value = 0;
			assert(setPtr->size() <= 31);

			std::size_t limit = (1 << setPtr->size());
			for (std::size_t counter = 0; counter < limit; ++counter) {
				boost::shared_ptr<Set> subSetPtr(new Set);
				std::size_t idx = 0;
				for (typename Set::iterator setIter = setPtr->begin(); setIter != setPtr->end();  ++setIter, ++idx) {
					if ( (value & (1 << idx)) != 0 ) {
						subSetPtr->insert(*setIter);
					}
				}
				powerSetPtr->insert(subSetPtr);

				// the value
				++value;
			}

			return powerSetPtr;
		}

		template<typename PowerSet> static boost::shared_ptr<PowerSet> filterCardinalityPowerSet(const boost::shared_ptr<PowerSet> &powerSetPtr, const std::size_t min, const std::size_t max) {
			boost::shared_ptr<PowerSet> resultPowerSetPtr(new PowerSet);

			for (typename PowerSet::iterator powerSetIter = powerSetPtr->begin(); powerSetIter != powerSetPtr->end(); ++powerSetIter) {
				if ((*powerSetIter)->size() < min || (*powerSetIter)->size() > max) {
					continue;
				}

				resultPowerSetPtr->insert(*powerSetIter);
			}

			return resultPowerSetPtr;
		}

		template<typename PowerSet> static boost::shared_ptr<PowerSet> filterCardinalityPowerSet(const boost::shared_ptr<PowerSet> &setPtr, const std::size_t min) {
			return filterCardinalityPowerSet<PowerSet>(setPtr, min, setPtr->size());
		}
	};
}

#endif /* MYMATHHELPER_HPP_ */


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2021, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_RESIDUAL_BASE_HPP_
#define CROCODDYL_CORE_RESIDUAL_BASE_HPP_

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/data-collector-base.hpp"

namespace crocoddyl {

/**
 * @brief Abstract class for residual models
 *
 * In Crocoddyl, a residual model defines a vector function \f$\mathbf{r}(\mathbf{x}, \mathbf{u})\mathbb{R}^{nr}\f$
 * where `nr` describes its dimension in the Euclidean space. This function depends on the state point
 * \f$\mathbf{x}\in\mathcal{X}\f$, which lies in the state manifold described with a `nq`-tuple, its velocity
 * \f$\dot{\mathbf{x}}\in T_{\mathbf{x}}\mathcal{X}\f$ that belongs to the tangent space with `nv` dimension, and the
 * control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$. The residual function can used across cost and constraint models.
 *
 * The main computations are carring out in `calc` and `calcDiff` routines. `calc` computes the residual vector
 * and `calcDiff` computes the Jacobians of the residual function.
 * Additionally, it is important remark that `calcDiff()` computes the Jacobians using the latest stored values by
 * `calc()`. Thus, we need to run first `calc()`.
 *
 * \sa `StateAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
 */
template <typename _Scalar>
class ResidualModelAbstractTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  /**
   * @brief Initialize the residual model
   *
   * @param[in] state        State of the system
   * @param[in] nr           Dimension of residual vector
   * @param[in] nu           Dimension of control vector
   * @param[in] q_dependent  Define if the residual function depends on q (default true)
   * @param[in] v_dependent  Define if the residual function depends on v (default true)
   * @param[in] u_dependent  Define if the residual function depends on u (default true)
   */
  ResidualModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr, const std::size_t nu,
                           const bool q_dependent = true, const bool v_dependent = true,
                           const bool u_dependent = true);

  /**
   * @copybrief ResidualModelAbstractTpl()
   *
   * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
   *
   * @param[in] state        State of the system
   * @param[in] nr           Dimension of residual vector
   * @param[in] q_dependent  Define if the residual function depends on q (default true)
   * @param[in] v_dependent  Define if the residual function depends on v (default true)
   * @param[in] u_dependent  Define if the residual function depends on u (default true)
   */
  ResidualModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr, const bool q_dependent = true,
                           const bool v_dependent = true, const bool u_dependent = true);
  virtual ~ResidualModelAbstractTpl();

  /**
   * @brief Compute the residual vector
   *
   * @param[in] data  Residual data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u);

  /**
   * @brief Compute the Jacobian of the residual vector
   *
   * It computes the Jacobian the residual function. It assumes that `calc()` has been run first.
   *
   * @param[in] data  Residual data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                        const Eigen::Ref<const VectorXs>& u);

  /**
   * @brief Create the residual data
   *
   * The default data contains objects to store the values of the residual vector and their Jacobians.
   * However, it is possible to specialized this function if we need to create additional data, for instance, to avoid
   * dynamic memory allocation.
   *
   * @param data  Data collector
   * @return the residual data
   */
  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);

  /**
   * @copybrief calc()
   *
   * @param[in] data  Residual data
   * @param[in] x     State point
   */
  void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @copybrief calcDiff()
   *
   * @param[in] data  Residual data
   * @param[in] x     State point
   */
  void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Return the state
   */
  const boost::shared_ptr<StateAbstract>& get_state() const;

  /**
   * @brief Return the dimension of the residual vector
   */
  std::size_t get_nr() const;

  /**
   * @brief Return the dimension of the control input
   */
  std::size_t get_nu() const;

  /**
   * @brief Return true if the residual function depends on q
   */
  bool get_q_dependent() const;

  /**
   * @brief Return true if the residual function depends on v
   */
  bool get_v_dependent() const;

  /**
   * @brief Return true if the residual function depends on u
   */
  bool get_u_dependent() const;

  /**
   * @brief Print information on the residual model
   */
  template <class Scalar>
  friend std::ostream& operator<<(std::ostream& os, const ResidualModelAbstractTpl<Scalar>& model);

  /**
   * @brief Print relevant information of the residual model
   *
   * @param[out] os  Output stream object
   */
  virtual void print(std::ostream& os) const;

 protected:
  boost::shared_ptr<StateAbstract> state_;  //!< State description
  std::size_t nr_;                          //!< Residual vector dimension
  std::size_t nu_;                          //!< Control dimension
  VectorXs unone_;                          //!< No control vector
  bool q_dependent_;                        //!< Label that indicates if the residual function depends on q
  bool v_dependent_;                        //!< Label that indicates if the residual function depends on v
  bool u_dependent_;                        //!< Label that indicates if the residual function depends on u
};

template <typename _Scalar>
struct ResidualDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  template <template <typename Scalar> class Model>
  ResidualDataAbstractTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
      : shared(data),
        r(model->get_nr()),
        Rx(model->get_nr(), model->get_state()->get_ndx()),
        Ru(model->get_nr(), model->get_nu()) {
    r.setZero();
    Rx.setZero();
    Ru.setZero();
  }
  virtual ~ResidualDataAbstractTpl() {}

  DataCollectorAbstract* shared;  //!< Shared data allocated by the action model
  VectorXs r;                     //!< Residual vector
  MatrixXs Rx;                    //!< Jacobian of the residual vector with respect the state
  MatrixXs Ru;                    //!< Jacobian of the residual vector with respect the control
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/residual-base.hxx"

#endif  // CROCODDYL_CORE_RESIDUAL_BASE_HPP_


/**
 *  Project: The Stock Libraries
 *
 *  File: globals.hpp
 *  Created: Jun 05, 2012
 *
 *  Author: Abhinav Sarje <abhinav.sarje@gmail.com>
 *
 *  Copyright (c) 2012-2017 Abhinav Sarje
 *  Distributed under the Boost Software License.
 *  See accompanying LICENSE file.
 */

#ifndef __GLOBALS_HPP__
#define __GLOBALS_HPP__

#include <boost/array.hpp>
#include <vector>
#include <cmath>

#include "typedefs.hpp"

namespace stock {

	typedef struct vector2_t {
		boost::array <real_t, 2> vec_;

		/* constructors */

		vector2_t() {
			vec_[0] = 0; vec_[1] = 0;
		} // vector2_t()

		vector2_t(real_t a, real_t b) {
			vec_[0] = a; vec_[1] = b;
		} // vector2_t()

		vector2_t(vector2_t& a) {
			vec_[0] = a[0]; vec_[1] = a[1];
		} // vector2_t()

		vector2_t(const vector2_t& a) {
			vec_ = a.vec_;
		} // vector2_t()

		/* operators */

		vector2_t& operator=(const vector2_t& a) {
			vec_ = a.vec_;
			return *this;
		} // operator=

		vector2_t& operator=(vector2_t& a) {
			vec_ = a.vec_;
			return *this;
		} // operator=

		real_t& operator[](int i) {
			return vec_[i];
		} // operator[]
	} vector2_t;

	typedef struct vector3_t {
		boost::array <real_t, 3> vec_;

		/* constructors */

		vector3_t() {
			vec_[0] = 0; vec_[1] = 0; vec_[2] = 0;
		} // vector3_t()

		vector3_t(real_t a, real_t b, real_t c) {
			vec_[0] = a; vec_[1] = b; vec_[2] = c;
		} // vector3_t()

		vector3_t(vector3_t& a) {
			vec_[0] = a[0]; vec_[1] = a[1]; vec_[2] = a[2];
		} // vector3_t()

		vector3_t(const vector3_t& a) {
			vec_ = a.vec_;
		} // vector3_t()

		/* operators */

		vector3_t& operator=(const vector3_t& a) {
			vec_ = a.vec_;
			return *this;
		} // operator=

		vector3_t& operator=(vector3_t& a) {
			vec_ = a.vec_;
			return *this;
		} // operator=

		real_t& operator[](int i) {
			return vec_[i];
		} // operator[]

		vector3_t operator+(int toadd) {
			return vector3_t(vec_[0] + toadd, vec_[1] + toadd, vec_[2] + toadd);
		} // operator+()

		vector3_t operator+(vector3_t toadd) {
			return vector3_t(vec_[0] + toadd[0], vec_[1] + toadd[1], vec_[2] + toadd[2]);
		} // operator+()

		vector3_t operator-(int tosub) {
			return vector3_t(vec_[0] - tosub, vec_[1] - tosub, vec_[2] - tosub);
		} // operator-()

		vector3_t operator-(vector3_t tosub) {
			return vector3_t(vec_[0] - tosub[0], vec_[1] - tosub[1], vec_[2] - tosub[2]);
		} // operator-()

		vector3_t operator/(vector3_t todiv) {
			return vector3_t(vec_[0] / todiv[0], vec_[1] / todiv[1], vec_[2] / todiv[2]);
		} // operator/()
	} vector3_t;

	typedef struct matrix3x3_t {
		typedef boost::array<real_t, 3> mat3_t;
		mat3_t mat_[3];

		/* constructors */

		matrix3x3_t() {
			for(int i = 0; i < 3; ++ i)
				for(int j = 0; j < 3; ++ j)
					mat_[i][j] = 0.0;
		} // matrix3x3_t()

		matrix3x3_t(const matrix3x3_t& a) {
			mat_[0] = a.mat_[0];
			mat_[1] = a.mat_[1];
			mat_[2] = a.mat_[2];
		} // matrix3x3_t

		/* operators */

		mat3_t& operator[](unsigned int index) {
			return mat_[index];
		} // operator[]()

		mat3_t& operator[](int index) {
			return mat_[index];
		} // operator[]()

		matrix3x3_t& operator=(matrix3x3_t& a) {
			for(int i = 0; i < 3; ++ i)
				for(int j = 0; j < 3; ++ j)
					mat_[i][j] = a[i][j];
			return *this;
		} // operstor=()

		matrix3x3_t operator+(int toadd) {
			matrix3x3_t sum;
			for(int i = 0; i < 3; ++ i)
				for(int j = 0; j < 3; ++ j)
					sum.mat_[i][j] = mat_[i][j] + toadd;
			return sum;
		} // operator+()

		matrix3x3_t operator+(matrix3x3_t& toadd) {
			matrix3x3_t sum;
			for(int i = 0; i < 3; ++ i)
				for(int j = 0; j < 3; ++ j)
					sum.mat_[i][j] = mat_[i][j] + toadd[i][j];
			return sum;
		} // operator+()

		matrix3x3_t operator*(int tomul) {
			matrix3x3_t prod;
			for(int i = 0; i < 3; ++ i)
				for(int j = 0; j < 3; ++ j)
					prod.mat_[i][j] = mat_[i][j] * tomul;
			return prod;
		} // operator*()

		matrix3x3_t operator*(matrix3x3_t& tomul) {
			matrix3x3_t prod;
			for(int i = 0; i < 3; ++ i)
				for(int j = 0; j < 3; ++ j)
					prod.mat_[i][j] = mat_[i][j] * tomul[i][j];
			return prod;
		} // operator*()

	} matrix3x3_t;

} // namespace stock

#endif /* __GLOBALS_HPP__ */


#include <algorithm>
#include <fstream>
#include <sstream>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

#include <iostream>

#include "Indices.hxx"
#include "Tensor.hxx"
#include "Epsilon.hxx"
#include "Eta.hxx"
#include "Scalar.hxx"
#include "LinearAlgebra.hxx"
#include "Algorithms.hxx"

std::string printTree (std::unique_ptr<Tree<Node>> const & tree) {
  if (tree->isEmpty()) {
    return printForest (tree->forest, 0);
  } else {
    std::stringstream ss;
    ss << tree->node->print();
    ss << "\n";
    ss << printForest (tree->forest, 1);
    return ss.str();
  }
}

std::string printTreeMaple (std::unique_ptr<Tree<Node>> const & tree) {
  std::stringstream ss;
  auto it = tree->firstLeaf();

  bool first = true;
  while (it != nullptr) {
    if (first) {
      first = false;
    } else {
      ss << "+ ";
    }
    ss << printBranchMaple (it->getBranch());
    ss << std::endl;

    it = it->nextLeaf();
  }

  return ss.str();
}

std::string printBranchMaple (std::vector <Node *> const & branch) {
  assert (branch.size() > 1);
  std::stringstream ss;

  ss << branch.back()->printMaple();

  for (size_t counter = 0; counter < branch.size() - 1; ++counter) {
    ss << " * ";
    ss << branch[counter]->printMaple();
  }

  return ss.str();
}

std::string printForest (Forest<Node> const & f, size_t depth) {
  std::stringstream ss;
  std::for_each (f.cbegin(), f.cend(),
    [depth, &ss] (auto const & t) {
      for (size_t counter = 0; counter < depth; ++counter) {
        ss << "\t";
      }
      ss << t->node->print();
      ss << "\n";
      ss << printForest (t->forest, depth + 1);
    });
  return ss.str();
}

std::unique_ptr<Tree<Node>> copyTree (std::unique_ptr<Tree<Node>> const & tree) {
  auto new_tree = std::make_unique<Tree<Node>>();
  if (!tree->isEmpty()) {
    new_tree->node = tree->node->clone();
  }
  new_tree->forest = Forest<Node>();
  new_tree->forest.reserve(tree->forest.size());

  std::for_each(tree->forest.cbegin(), tree->forest.cend(),
    [&new_tree,&tree] (auto const & t) {
      new_tree->forest.emplace_back(std::move(copyTree(t)));
      new_tree->forest.back()->parent = new_tree.get();
    });

  return new_tree;
}

void multiplyTree (std::unique_ptr<Tree<Node>> & tree, mpq_class const & factor) {
  auto it = tree->firstLeaf();

  while (it != nullptr) {
    it->node->multiply(factor);
    it = it->nextLeaf();
  }
}

void exchangeTensorIndices (std::unique_ptr<Tree<Node>> & tree, std::map<char, char> const & exchange_map) {
  if (!tree->isEmpty()) {
    tree->node->exchangeTensorIndices(exchange_map);
  }

  std::for_each(tree->forest.begin(), tree->forest.end(),
    [&exchange_map] (auto & t) {
      exchangeTensorIndices (t, exchange_map);
    });
}

void exchangeSymmetrizeTree (std::unique_ptr<Tree<Node>> & tree, std::map<char, char> const & exchange_map, int parity) {
  multiplyTree(tree, mpq_class(1, 2));

  auto new_tree = copyTree(tree);

  mpq_class factor = (parity < 0 ? -1 : 1);
  multiplyTree (new_tree, factor);

  exchangeTensorIndices (new_tree, exchange_map);

  applyTensorSymmetries (new_tree);
  removeEmptyBranches(new_tree);

  sortTreeAndMerge (tree, new_tree);
  removeEmptyBranches(tree);
}

void multiExchangeSymmetrizeTree (std::unique_ptr<Tree<Node>> & tree, std::vector<std::pair<std::map<char, char>, int>> const & exchange_map_set) {
  multiplyTree (tree, mpq_class(1, exchange_map_set.size() + 1));

  auto tree_copy = copyTree (tree);

  std::for_each (exchange_map_set.cbegin(), exchange_map_set.cend(),
    [&tree_copy,&tree] (auto const & p) {
      auto new_tree = copyTree (tree_copy);
      auto exchange_map = p.first;
      int parity = p.second;
      mpq_class factor = (parity < 0 ? -1 : 1);
      multiplyTree (new_tree, factor);
      exchangeTensorIndices (new_tree, exchange_map);
      applyTensorSymmetries (new_tree);
      removeEmptyBranches (new_tree);
      sortTreeAndMerge (tree, new_tree);
      removeEmptyBranches (tree);
    });
}

void redefineScalarsSym (std::unique_ptr<Tree<Node>> & tree) {
  auto leaf_it = tree->firstLeaf();
  std::map<size_t, std::pair<size_t, mpq_class>> substitution_map;
  size_t var_counter = 0;

  while (leaf_it != nullptr) {
    Scalar * scalar = static_cast<Scalar *>(leaf_it->node.get());
    auto first = scalar->getCoefficientMap()->begin();
    auto it = substitution_map.find(first->first);
    if (it == substitution_map.end()) {
      std::unique_ptr<Node> new_scalar = std::make_unique<Scalar>(++var_counter, first->second);
      std::swap(new_scalar, leaf_it->node);
      substitution_map[first->first] = std::make_pair(var_counter, first->second);
    } else {
      std::unique_ptr<Node> new_scalar = std::make_unique<Scalar>(it->second.first, first->second);
      std::swap(new_scalar, leaf_it->node);
    }

    leaf_it = leaf_it->nextLeaf();
  }
}

void sortBranch (std::vector<Node *> & branch) {
  std::sort (branch.begin(), branch.end(),
    [] (auto & n, auto & m) {
      return (*n) < m;
    });
}

void sortForest (Forest<Node> & forest) {
  std::sort (forest.begin(), forest.end(),
    [] (auto const & n, auto const & m) {
      return *(n->node) < m->node.get();
    });
}

void sortTree (std::unique_ptr<Tree<Node>> & tree) {
  sortForest (tree->forest);

  std::for_each(tree->forest.begin(), tree->forest.end(),
    [] (auto & t) {
      sortTree (t);
    });
}

void sortTreeAndMerge (std::unique_ptr<Tree<Node>> & dst, std::unique_ptr<Tree<Node>> const & src) {
  auto src_it = src->firstLeaf();

  while (src_it != nullptr) {
    auto branch = src_it->getBranch();
    sortBranch (branch);

    insertBranch (dst, branch);

    src_it = src_it->nextLeaf();
  }
}

void mergeTrees (std::unique_ptr<Tree<Node>> & dst, std::unique_ptr<Tree<Node>> const & src) {
  auto src_it = src->firstLeaf();

  while (src_it != nullptr) {
    auto branch = src_it->getBranch();

    insertBranch (dst, branch);

    src_it = src_it->nextLeaf();
  }
}

void canonicalizeTree (std::unique_ptr<Tree<Node>> & tree) {
  applyTensorSymmetries (tree);

  auto ret = std::make_unique<Tree<Node>>();

  sortTreeAndMerge (ret, tree);
  removeZeroScalars (ret);
  removeEmptyBranches (ret);
  sortTree (ret);

  std::swap (ret, tree);
}

void insertBranch (std::unique_ptr<Tree<Node>> & dst, std::vector<Node *> & branch, size_t const node_number) {
  if (branch.size() == node_number) {
    return;
  }

  bool const branch_node_is_scalar = (typeid(*(branch[node_number])) == typeid(Scalar));

  auto node_it = std::find_if (dst->forest.begin(), dst->forest.end(),
    [&branch,branch_node_is_scalar,node_number] (auto & t) {
      if (t->node == nullptr) {
        return false;
      } else if (branch_node_is_scalar && (typeid(*(t->node)) == typeid(Scalar))) {
        return true;
      } else if (t->node->equals(branch[node_number])){
        return true;
      } else {
        return false;
      }
    });

  if (node_it == dst->forest.end()) {
    dst->forest.push_back (std::make_unique<Tree<Node>>());
    dst->forest.back()->node = branch[node_number]->clone();
    dst->forest.back()->parent = dst.get();
    insertBranch(dst->forest.back(), branch, node_number + 1);
  } else if (branch_node_is_scalar && (typeid(*((*node_it)->node)) == typeid(Scalar))) {
    static_cast<Scalar *>((*node_it)->node.get())->addOther(static_cast<Scalar *>(branch[node_number]));
    insertBranch (*node_it, branch, node_number + 1);
  } else {
    insertBranch (*node_it, branch, node_number + 1);
  }
}

void removeEmptyBranches (std::unique_ptr<Tree<Node>> & tree) {
  std::for_each(tree->forest.begin(), tree->forest.end(),
    [] (auto & t) {
      removeEmptyBranches (t);
    });

  tree->forest.erase(std::remove_if(tree->forest.begin(), tree->forest.end(),
    [] (auto & t) {

      if (!t->isEmpty()) {
        auto is_node_scalar = (typeid(*(t->node)) == typeid(Scalar));
        if (is_node_scalar && static_cast<Scalar *>(t->node.get())->isZero()) {
          return true;
        } else if (!is_node_scalar && t->isLeaf()) {
          return true;
        }
      } else {
        if (t->isLeaf()) {
          return true;
        }
      }

  return false;

  }), tree->forest.end());
}

void removeZeroScalars (std::unique_ptr<Tree<Node>> & tree) {
  auto it = tree->firstLeaf();

  while (it != nullptr) {
    static_cast<Scalar *>(it->node.get())->removeZeros();
    it = it->nextLeaf();
  }
}

bool isTreeSorted (std::unique_ptr<Tree<Node>> const & tree) {
  return (std::is_sorted(tree->forest.cbegin(), tree->forest.cend(),
    [] (auto const & a, auto const & b) {
      return *(a->node) < b->node.get();
    }) && std::all_of(tree->forest.cbegin(), tree->forest.cend(),
      [] (auto const & t) {
        return isTreeSorted (t);
      }));
}

std::map<size_t, mpq_class> evaluateTree (std::unique_ptr<Tree<Node>> const & tree, std::map<char, char> const & eval_map, mpq_class prefactor) {
  std::map<size_t, mpq_class> ret;

  if (tree->isLeaf()) {
    std::map<size_t, mpq_class> const * leaf_map = tree->node->getCoefficientMap();
    std::for_each(leaf_map->cbegin(), leaf_map->cend(),
      [&ret,&prefactor] (auto const & p) {
        ret.insert(std::make_pair(p.first, prefactor * p.second));
      });
  } else {
    if (!tree->isRoot()) {
      prefactor *= static_cast<Tensor const *>(tree->node.get())->evaluate(eval_map);
      if (prefactor == 0) {
        return ret;
      }
    }

    std::for_each(tree->forest.cbegin(), tree->forest.cend(),
      [&ret, &eval_map, &prefactor] (auto const & t) {
        auto _map = evaluateTree (t, eval_map, prefactor);
        std::for_each(_map.cbegin(), _map.cend(),
          [&ret] (auto const & a) {
            auto it = ret.find(a.first);
            if (it == ret.end()) {
              ret.insert(std::make_pair(a.first, a.second));
            } else {
              it->second += a.second;
              if (it->second == 0) {
                ret.erase(it);
              }
            }
          });
      });
  }

  return ret;
}

void applyTensorSymmetries(std::unique_ptr<Tree<Node>> & tree, int parity) {
  std::vector<Tree<Node>*> erase_vec;
  std::for_each(tree->forest.begin(), tree->forest.end(),
    [parity,&erase_vec] (auto & t) {
      int new_parity = t->node->applyTensorSymmetries(parity);
      if (new_parity == 0) {
        erase_vec.push_back(t.get());
      } else {
        applyTensorSymmetries (t, new_parity);
      }
    });

  tree->forest.erase(std::remove_if(tree->forest.begin(), tree->forest.end(),
    [&erase_vec] (auto & t) {
      return (std::find(erase_vec.begin(), erase_vec.end(), t.get()) != erase_vec.end());
    }), tree->forest.end());
}

std::map<size_t, size_t> getVariableMap (std::unique_ptr<Tree<Node>> const & tree) {
  auto variables = getVariableSet(tree);
  std::map<size_t, size_t> ret;
  std::for_each(variables.cbegin(), variables.cend(),
    [&ret,n=0] (auto const & v) mutable {
      ret[v] = n++;
    });
  return ret;
}

std::set<size_t> getVariableSet (std::unique_ptr<Tree<Node>> const & tree) {
  std::set<size_t> ret;

  auto leaf_it = tree->firstLeaf();

  while (leaf_it != nullptr) {
    ret.merge (leaf_it->node->getVariableSet());
    leaf_it = leaf_it->nextLeaf();
  }

  return ret;
}

void shrinkForest (Forest<Node> & forest) {
  forest.shrink_to_fit();
  std::for_each(forest.begin(), forest.end(),
    [] (auto & t) {
      t->forest.shrink_to_fit();
    });
}

void substituteVariables (std::unique_ptr<Tree<Node>> & tree, std::map<size_t, size_t> const & subs_map) {
  auto leaf_it = tree->firstLeaf();

  while (leaf_it != nullptr) {
    leaf_it->node->substituteVariables (subs_map);
    leaf_it = leaf_it->nextLeaf();
  }
}

void setVariablesToZero (std::unique_ptr<Tree<Node>> & tree, std::set<size_t> const & variables) {
  auto leaf_it = tree->firstLeaf();

  while (leaf_it != nullptr) {
    leaf_it->node->removeVariables(variables);
    leaf_it = leaf_it->nextLeaf();
  }
}

void solveNumerical (std::vector<std::pair<std::unique_ptr<Tree<Node>> &, std::function< void (std::unique_ptr<Tree<Node>> const &, std::set<std::map<size_t, mpq_class>> &)>>> const & equations) {
  std::set<std::map<size_t, mpq_class>> eval_res_set;
  std::set<size_t> var_set;

  std::for_each (equations.cbegin(), equations.cend(),
      [&eval_res_set,&var_set] (auto const & eq) {
        auto & fun = eq.second;
        auto & tree = eq.first;

        var_set.merge (getVariableSet (tree));
        fun (tree, eval_res_set);
      });

    std::cout << "number of equations : " << eval_res_set.size() << std::endl;

    std::cout << "number of coefficients : " << var_set.size() << std::endl;

    std::map<size_t, size_t> var_map;
    std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> eval_res_mat;
    std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> eval_res_mat_mapped;

    std::for_each(eval_res_set.cbegin(), eval_res_set.cend(),
      [row_counter=0,&eval_res_mat] (auto const & m) mutable {
        std::for_each(m.cbegin(), m.cend(),
          [&row_counter,&eval_res_mat] (auto const & p) {
            eval_res_mat.insert(std::make_pair(std::make_pair(row_counter, p.first), p.second));
          });
        ++row_counter;
      });

    eval_res_set.clear();

    std::for_each(eval_res_mat.cbegin(), eval_res_mat.cend(),
      [&eval_res_mat_mapped,&var_map,var=0] (auto const & v) mutable {
        auto it = var_map.find(v.first.second);
        if (it == var_map.end()) {
          var_map.insert(std::make_pair(v.first.second, var));
          ++var;
        }
        eval_res_mat_mapped.insert(std::make_pair(std::make_pair(v.first.first, var_map.at(v.first.second)), v.second));
      });

    eval_res_mat.clear();

  solveLinearSystem (eval_res_mat_mapped, eval_res_mat_mapped.rbegin()->first.first + 1, var_map.size());
}

void reduceNumerical (std::unique_ptr<Tree<Node>> & tree, std::function< void (std::unique_ptr<Tree<Node>> const &, std::set<std::map<size_t, mpq_class>> &)> fun) {
  std::set<std::map<size_t, mpq_class>> eval_res_set;

  fun (tree, eval_res_set);

  std::set<size_t> var_set = getVariableSet (tree);
  std::map<size_t, size_t> var_map;
  std::set<size_t> erase_set = var_set;
  std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> eval_res_mat;
  std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> eval_res_mat_mapped;

  std::for_each(eval_res_set.cbegin(), eval_res_set.cend(),
    [row_counter=0,&eval_res_mat] (auto const & m) mutable {
      std::for_each(m.cbegin(), m.cend(),
        [&row_counter,&eval_res_mat] (auto const & p) {
          eval_res_mat.insert(std::make_pair(std::make_pair(row_counter, p.first), p.second));
        });
      ++row_counter;
    });

  eval_res_set.clear();

  std::for_each(eval_res_mat.cbegin(), eval_res_mat.cend(),
    [&eval_res_mat_mapped,&var_map,&erase_set,var=0] (auto const & v) mutable {
      erase_set.erase(v.first.second);
      auto it = var_map.find(v.first.second);
      if (it == var_map.end()) {
        var_map.insert(std::make_pair(v.first.second, var));
        ++var;
      }
      eval_res_mat_mapped.insert(std::make_pair(std::make_pair(v.first.first, var_map.at(v.first.second)), v.second));
    });

  eval_res_mat.clear();

  std::cout << "These equations contain " << var_map.size() << " variables." << std::endl;
  std::cout << "That is, " << erase_set.size() << " variables do not contribute at all." << std::endl;
  std::cout << std::endl;
  std::cout << "Computing LU decomposition of the linear system using Eigen subroutines ..." << std::endl;

  auto erase_set_2_mapped = findDependentVariables (eval_res_mat_mapped, eval_res_mat_mapped.rbegin()->first.first + 1, var_map.size());
  std::cout << "... done! " << erase_set_2_mapped.size() << " variables are dependent." << std::endl;

  eval_res_mat_mapped.clear();

  std::map<size_t, size_t> var_rmap;
  std::transform (var_map.cbegin(), var_map.cend(), std::inserter(var_rmap, var_rmap.begin()),
    [] (auto const & p) {
      return std::make_pair(p.second, p.first);
    });

  std::set<size_t> erase_set_2;
  std::transform(erase_set_2_mapped.cbegin(), erase_set_2_mapped.cend(), std::inserter(erase_set_2, erase_set_2.begin()),
    [&var_rmap] (auto const & v) {
      return var_rmap.at(v);
    });

  erase_set_2_mapped.clear();

  erase_set.merge(erase_set_2);
  erase_set_2.clear();

  std::cout << "Removing all " << erase_set.size() << " variables from the ansatz." << std::endl;

  setVariablesToZero (tree, erase_set);
  removeEmptyBranches (tree);

  var_set = getVariableSet (tree);

  var_map.clear();

  std::transform (var_set.cbegin(), var_set.cend(), std::inserter (var_map, var_map.begin()),
    [counter=0] (auto const & v) mutable {
      return std::make_pair (v, ++counter);
    });

  substituteVariables (tree, var_map);
}

bool compareTrees (std::unique_ptr<Tree<Node>> const & tree1, std::unique_ptr<Tree<Node>> const & tree2) {
  bool nodes_equal = false;
  if (tree1->isEmpty() != tree2->isEmpty()) {
    nodes_equal = false;
  } else if (tree1->isEmpty() && tree2->isEmpty()) {
    nodes_equal = true;
  } else {
    nodes_equal = tree1->node->equals(tree2->node.get());
  }

  return (nodes_equal && std::equal(tree1->forest.cbegin(), tree1->forest.cend(),
                                    tree2->forest.cbegin(), tree2->forest.cend(),
                                    [] (auto const & t1, auto const & t2) {
                                      return compareTrees (t1, t2);
                                    }));
}

void contractTreeWithEta (std::unique_ptr<Tree<Node>> & tree, char i1, char i2) {
  std::vector<std::unique_ptr<Tree<Node>>> new_forest;
  std::for_each (tree->forest.begin(), tree->forest.end(),
    [i1, i2, &new_forest] (auto & t) {
      auto parent = t->parent;
      auto forest = contractTreeWithEtaInner (t, i1, i2);
      std::for_each (forest.begin(), forest.end(),
        [&t,parent] (auto & t2) {
          t2->parent = parent;
        });
      new_forest.insert (new_forest.end(), std::make_move_iterator (forest.begin()), std::make_move_iterator (forest.end()));
    });
  std::swap (tree->forest, new_forest);
}

Forest<Node> contractTreeWithEtaInner (std::unique_ptr<Tree<Node>> & tree, char i1, char i2) {
  tree->parent = nullptr;
  Forest<Node> ret;
  auto & node_type = typeid (*(tree->node));
  if (node_type == typeid (Epsilon)) {
    bool const contains_i1 = static_cast<Epsilon *>(tree->node.get())->containsIndex (i1);
    bool const contains_i2 = static_cast<Epsilon *>(tree->node.get())->containsIndex (i2);
    if (contains_i1) {
      if (contains_i2) {
        return ret;
      } else {
        auto _tree = std::make_unique<Tree<Node>>();
        _tree->forest.emplace_back (std::move(tree));
        return std::move(eliminateSecondEta(_tree, i1, i2)->forest);
      }
    } else {
      if (contains_i2) {
        auto _tree = std::make_unique<Tree<Node>>();
        _tree->forest.emplace_back (std::move(tree));
        return std::move(eliminateSecondEta(_tree, i2, i1)->forest);
      } else {
        contractTreeWithEta (tree, i1, i2);
        ret.emplace_back (std::move(tree));
        return ret;
      }
    }
  } else if (node_type == typeid (Eta)) {
    bool const contains_i1 = static_cast<Eta *>(tree->node.get())->containsIndex (i1);
    bool const contains_i2 = static_cast<Eta *>(tree->node.get())->containsIndex (i2);
    if (contains_i1) {
      if (contains_i2) {
        multiplyTree (tree, mpq_class (4, 1));
        std::for_each (tree->forest.begin(), tree->forest.end(),
          [&ret] (auto & t) {
            ret.emplace_back(std::move(t));
          });
        return ret;
      } else {
        auto _tree = std::make_unique<Tree<Node>>();
        _tree->forest.emplace_back (std::move(tree));
        return std::move(eliminateSecondEta(_tree, i1, i2)->forest);
      }
    } else {
      if (contains_i2) {
        auto _tree = std::make_unique<Tree<Node>>();
        _tree->forest.emplace_back (std::move(tree));
        return std::move(eliminateSecondEta(_tree, i2, i1)->forest);
      } else {
        contractTreeWithEta (tree, i1, i2);
        ret.emplace_back (std::move(tree));
        return ret;
      }
    }
  } else {
    contractTreeWithEta (tree, i1, i2);
    ret.emplace_back (std::move(tree));
    return ret;
  }
}

std::unique_ptr<Tree<Node>> eliminateSecondEta (std::unique_ptr<Tree<Node>> & tree, char i1, char i2) {
  auto ret = std::make_unique<Tree<Node>>();
  auto it = tree->firstLeaf ();

  while (it != nullptr) {
    auto branch = it->getBranch();
    assert (branch[0] != nullptr);

    auto eta_it = std::find_if (branch.cbegin(), branch.cend(),
      [i2] (auto const & n) {
        return (n != nullptr &&
                typeid (*n) == typeid (Eta) &&
                static_cast<Eta *>(n)->containsIndex (i2));
      });
    char i_new = static_cast<Eta *>(*eta_it)->getOther(i2);

    branch.erase (eta_it);
    auto node_cpy = branch[0]->clone();
    branch[0] = node_cpy.get();
    static_cast<Tensor *>(branch[0])->exchangeTensorIndices({{i1, i_new}});

    sortBranch (branch);
    insertBranch (ret, branch);

    it = it->nextLeaf();
  }

  return ret;
}

void contractTreeWithEpsilon3 (std::unique_ptr<Tree<Node>> & tree, char m, char i1, char i2, char i3) {
  auto new_tree = std::make_unique<Tree<Node>> ();
  auto it = tree->firstLeaf ();

  while (it != nullptr) {
    auto branch = it->getBranch ();
    auto & type = typeid(*(branch[0]));
    if (type == typeid(Eta)) {
      if (std::any_of (branch.cbegin(), branch.cend(),
           [i1, i2, i3] (auto const & n) {
             if (typeid (*n) == typeid (Eta)) {
               auto eta = static_cast<Eta *>(n);
               int count = 0;
               if (eta->containsIndex(i1)) { ++count; }
               if (eta->containsIndex(i2)) { ++count; }
               if (eta->containsIndex(i3)) { ++count; }
               if (count == 2) {
                 return true;
               } else {
                 assert (count < 2);
                 return false;
               }
             } else {
               return false;
             }
           })) {
        // do nothing
      } else {
        std::vector<Node *> new_branch;
        char j1 = 0;
        char j2 = 0;
        char j3 = 0;
        new_branch.push_back (nullptr);
        std::for_each (branch.begin(), branch.end(),
            [&j1,&j2,&j3,i1,i2,i3,&new_branch] (auto n) {
              if (typeid (*n) == typeid (Eta)) {
                auto eta = static_cast<Eta *>(n);
                if (eta->containsIndex(i1)) {
                  assert (j1 == 0);
                  j1 = eta->getOther (i1);
                }
                else if (eta->containsIndex(i2)) {
                  assert (j2 == 0);
                  j2 = eta->getOther (i2);
                }
                else if (eta->containsIndex(i3)) {
                  assert (j3 == 0);
                  j3 = eta->getOther(i3);
                } else {
                  new_branch.push_back (n);
                }
              } else {
                new_branch.push_back (n);
              }
            });
        assert (j1 != 0 && j2 != 0 && j3 != 0);
        assert (branch.size() == new_branch.size() + 2);
        std::unique_ptr<Node> epsilon = std::make_unique<Epsilon> (m, j1, j2, j3);
        new_branch[0] = epsilon.get();
        insertBranch (new_tree, new_branch);
      }
    } else if (type == typeid(Epsilon)) {
      auto epsilon = static_cast<Epsilon *> (branch[0]);
      auto mult_res = epsilon->multiplyWithOther3 (i1, i2, i3);
      auto new_tree_2 = std::make_unique<Tree<Node>>();
      new_tree_2->forest.emplace_back (std::make_unique<Tree<Node>>());
      new_tree_2->forest[0]->node = std::make_unique<Eta>(m, std::get<1>(mult_res));
      new_tree_2->forest[0]->parent = new_tree_2.get();
      std::for_each (branch.cbegin() + 1, branch.cend(),
          [&mult_res,&new_tree_2] (auto const & n) {
            auto leaf = new_tree_2->firstLeaf();
            if (typeid(*n) == typeid(Scalar)) {
              auto scalar_clone = n->clone();
              scalar_clone->multiply (std::get<0>(mult_res));
              leaf->forest.emplace_back (std::make_unique<Tree<Node>>());
              std::swap (leaf->forest[0]->node, scalar_clone);
              leaf->forest[0]->parent = leaf;
            } else if (typeid(*n) == typeid(Eta)) {
              auto eta_clone = n->clone();
              eta_clone->exchangeTensorIndices (std::get<2>(mult_res));
              leaf->forest.emplace_back (std::make_unique<Tree<Node>>());
              std::swap (leaf->forest[0]->node, eta_clone);
              leaf->forest[0]->parent = leaf;
            } else {
              assert (false);
            }
          });
      std::vector<char> indices_to_symmetrize;
      indices_to_symmetrize.push_back (std::get<1>(mult_res));
      std::transform (std::get<2>(mult_res).cbegin(), std::get<2>(mult_res).cend(),
          std::back_inserter (indices_to_symmetrize), [] (auto const & p) { return p.second; });
      auto to_sym_size = indices_to_symmetrize.size();
      assert (to_sym_size > 0);
      assert (to_sym_size < 5);
      if (to_sym_size == 2) {
        exchangeSymmetrizeTree (new_tree_2, {{indices_to_symmetrize[0], indices_to_symmetrize[1]},
                                          {indices_to_symmetrize[1], indices_to_symmetrize[0]}}, -1);
      } else if (to_sym_size == 3) {
        auto & iv = indices_to_symmetrize;
        multiExchangeSymmetrizeTree (new_tree_2,
            {{{{iv[1], iv[2]}, {iv[2], iv[1]}}, -1},
             {{{iv[0], iv[1]}, {iv[1], iv[0]}}, -1},
             {{{iv[0], iv[1]}, {iv[1], iv[2]}, {iv[2], iv[0]}}, 1},
             {{{iv[0], iv[2]}, {iv[1], iv[0]}, {iv[2], iv[1]}}, 1},
             {{{iv[0], iv[2]}, {iv[2], iv[0]}}, -1}});
      } else if (to_sym_size == 4) {
        auto & iv = indices_to_symmetrize;
        multiExchangeSymmetrizeTree (new_tree_2,
            {{{{iv[2], iv[3]}, {iv[3], iv[2]}}, -1},                                  // 0 1 3 2
             {{{iv[1], iv[2]}, {iv[2], iv[1]}}, -1},                                  // 0 2 1 3
             {{{iv[1], iv[2]}, {iv[2], iv[3]}, {iv[3], iv[1]}}, 1},                   // 0 2 3 1
             {{{iv[1], iv[3]}, {iv[2], iv[1]}, {iv[3], iv[2]}}, 1},                   // 0 3 1 2
             {{{iv[1], iv[3]}, {iv[3], iv[1]}}, -1},                                  // 0 3 2 1
             {{{iv[0], iv[1]}, {iv[1], iv[0]}}, -1},                                  // 1 0 2 3
             {{{iv[0], iv[1]}, {iv[1], iv[0]}, {iv[2], iv[3]}, {iv[3], iv[2]}}, 1},   // 1 0 3 2
             {{{iv[0], iv[1]}, {iv[1], iv[2]}, {iv[2], iv[0]}}, 1},                   // 1 2 0 3
             {{{iv[0], iv[1]}, {iv[1], iv[2]}, {iv[2], iv[3]}, {iv[3], iv[0]}}, -1},  // 1 2 3 0
             {{{iv[0], iv[1]}, {iv[1], iv[3]}, {iv[2], iv[0]}, {iv[3], iv[2]}}, -1},  // 1 3 0 2
             {{{iv[0], iv[1]}, {iv[1], iv[3]}, {iv[3], iv[0]}}, 1},                   // 1 3 2 0
             {{{iv[0], iv[2]}, {iv[1], iv[0]}, {iv[2], iv[1]}}, 1},                   // 2 0 1 3
             {{{iv[0], iv[2]}, {iv[1], iv[0]}, {iv[2], iv[3]}, {iv[3], iv[1]}}, -1},  // 2 0 3 1
             {{{iv[0], iv[2]}, {iv[2], iv[0]}}, -1},                                  // 2 1 0 3
             {{{iv[0], iv[2]}, {iv[2], iv[3]}, {iv[3], iv[0]}}, 1},                   // 2 1 3 0
             {{{iv[0], iv[2]}, {iv[1], iv[3]}, {iv[2], iv[0]}, {iv[3], iv[1]}}, 1},   // 2 3 0 1
             {{{iv[0], iv[2]}, {iv[1], iv[3]}, {iv[2], iv[1]}, {iv[3], iv[0]}}, -1},  // 2 3 1 0
             {{{iv[0], iv[3]}, {iv[1], iv[0]}, {iv[2], iv[1]}, {iv[3], iv[2]}}, -1},  // 3 0 1 2
             {{{iv[0], iv[3]}, {iv[1], iv[0]}, {iv[3], iv[1]}}, 1},                   // 3 0 2 1
             {{{iv[0], iv[3]}, {iv[2], iv[0]}, {iv[3], iv[2]}}, 1},                   // 3 1 0 2
             {{{iv[0], iv[3]}, {iv[3], iv[0]}}, -1},                                  // 3 1 2 0
             {{{iv[0], iv[3]}, {iv[1], iv[2]}, {iv[2], iv[0]}, {iv[3], iv[1]}}, -1},  // 3 2 0 1
             {{{iv[0], iv[3]}, {iv[1], iv[2]}, {iv[2], iv[1]}, {iv[3], iv[0]}}, 1}}); // 3 2 1 0
      }
      mergeTrees (new_tree, new_tree_2);
    } else {
      assert(false);
    }
    it = it->nextLeaf();
  }

  std::swap (tree, new_tree);
}

void saveTree (std::unique_ptr<Tree<Node>> const & tree, std::string const & filename) {
  std::ofstream file {filename};
  boost::archive::text_oarchive oa {file};
  oa << tree;
}

std::unique_ptr<Tree<Node>> loadTree (std::string const & filename) {
  std::ifstream file {filename};
  boost::archive::text_iarchive ia {file};
  std::unique_ptr<Tree<Node>> tree;
  ia >> tree;
  return tree;
}

bool checkSaveAndLoad (std::unique_ptr<Tree<Node>> const & tree) {
  saveTree (tree, "check.prs");
  auto tree2 = loadTree ("check.prs");
  return (compareTrees (tree, tree2));
}

void shiftVariables (std::unique_ptr<Tree<Node>> & tree, int i) {
  auto it = tree->firstLeaf();

  while (it != nullptr) {
    assert (typeid(*(it->node.get())) == typeid(Scalar));
    static_cast<Scalar *>(it->node.get())->shiftVariables (i);

    it = it->nextLeaf();
  }
}

void multiplyTreeWithEta (std::unique_ptr<Tree<Node>> & tree, char i1, char i2) {
  auto new_tree = std::make_unique<Tree<Node>>();

  new_tree->forest.emplace_back (std::make_unique<Tree<Node>>());
  new_tree->forest.back()->parent = new_tree.get();
  new_tree->forest.back()->node = std::make_unique<Eta> (i1, i2);

  std::transform (tree->forest.begin(), tree->forest.end(), std::back_inserter (new_tree->forest.back()->forest),
      [&new_tree] (auto & t) {
        auto new_subtree = std::make_unique<Tree<Node>>();
        std::swap (t, new_subtree);
        new_subtree->parent = new_tree->forest.back().get();
        return std::move (new_subtree);
      });

  std::swap (tree, new_tree);
}


// json parser
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <utility>
namespace pt = boost::property_tree;

#include "../MemoryKernel/MemoryKernelFactory.h"
#include "PermittivityLorentz.h"

PermittivityLorentz::PermittivityLorentz(
    double eps_inf, double omega_p, double omega_0,
    std::shared_ptr<MemoryKernel> memory_kernel)
    : eps_inf(eps_inf), omega_p(omega_p), omega_0(omega_0),
      memory_kernel(std::move(memory_kernel)) {}

// constructor for drude model from .json file
PermittivityLorentz::PermittivityLorentz(const std::string &input_file) {

  // Create a root
  pt::ptree root;

  // Load the json file in this ptree
  pt::read_json(input_file, root);

  // check if type is right
  std::string type = root.get<std::string>("Permittivity.type");
  assert(type == "lorentz");

  // read parameters
  this->eps_inf = root.get<double>("Permittivity.eps_inf");
  this->omega_p = root.get<double>("Permittivity.omega_p");
  this->omega_0 = root.get<double>("Permittivity.omega_0");

  this->memory_kernel =
      MemoryKernelFactory::create(input_file, "Permittivity.MemoryKernel");
}

// calculate the permittivity
std::complex<double> PermittivityLorentz::calculate(double omega) const {
  // dummies for result and complex unit
  std::complex<double> result;
  std::complex<double> I(0.0, 1.0);

  // calculate the result
  result = eps_inf - omega_p * omega_p /
                         (omega_0 * omega_0 - omega * omega -
                          I * omega * memory_kernel->calculate(omega));

  return result;
}

// calculate the permittivity scaled by omega
std::complex<double>
PermittivityLorentz::calculate_times_omega(double omega) const {
  // dummies for result and complex unit
  std::complex<double> result;
  std::complex<double> I(0.0, 1.0);

  // calculate the result
  result = eps_inf * omega - omega_p * omega_p * omega /
                                 (omega_0 * omega_0 - omega * omega -
                                  I * omega * memory_kernel->calculate(omega));

  return result;
}

void PermittivityLorentz::print_info(std::ostream &stream) const {
  stream << "# PermittivityLorentz\n#\n"
         << "# eps_inf = " << eps_inf << "\n"
         << "# omega_p = " << omega_p << "\n"
         << "# omega_0 = " << omega_0 << "\n";
  memory_kernel->print_info(stream);
}


/*
Copyright (c) 2016 Bastien Durix

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/**
 *  \brief 2D skeletonization
 *  \author Bastien Durix
 */

#include <boost/program_options.hpp>
#include <time.h>
#include <iostream>
#include <iomanip>
#include <chrono>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>

#include <shape/DiscreteShape.h>
#include <boundary/DiscreteBoundary.h>
#include <skeleton/Skeletons.h>

#include <algorithm/extractboundary/NaiveBoundary.h>
#include <algorithm/skeletonization/propagation/SpherePropagation2D.h>
#include <algorithm/skeletonization/voronoi/VoronoiSkeleton2D.h>
#include <algorithm/skinning/Filling.h>
#include <algorithm/evaluation/ReprojError.h>
#include <algorithm/pruning/ScaleAxisTransform.h>
#include <algorithm/pruning/LambdaMedialAxis.h>
#include <algorithm/pruning/ThetaMedialAxis.h>

#include <displayopencv/DisplayShapeOCV.h>
#include <displayopencv/DisplayBoundaryOCV.h>
#include <displayopencv/DisplaySkeletonOCV.h>

std::tuple<double,double,int,int> EvalSkel(const shape::DiscreteShape<2>::Ptr dissh,
									   const boundary::DiscreteBoundary<2>::Ptr disbnd,
									   const skeleton::GraphSkel2d::Ptr skel)
{
	shape::DiscreteShape<2>::Ptr shp(new shape::DiscreteShape<2>(dissh->getWidth(),dissh->getHeight()));
	algorithm::skinning::Filling(shp,skel);

	double res = algorithm::evaluation::SymDiffArea(dissh,shp);
	double res2 = algorithm::evaluation::HausDist(skel,disbnd,dissh->getFrame());

	std::list<unsigned int> lnod;
	skel->getAllNodes(lnod);
	unsigned int nbbr = 0;
	for(std::list<unsigned int>::iterator it = lnod.begin(); it != lnod.end(); it++)
	{
		unsigned int deg = skel->getNodeDegree(*it);
		if(deg != 2)
			nbbr += deg;
	}
	nbbr /= 2;
	std::tuple<double,double,int,int> result = std::make_tuple(res*100.0,res2,skel->getNbNodes(),nbbr);

	return result;
}

int main(int argc, char** argv)
{
	std::string imgfile, fileskl, fileimg, filebnd;
	bool output = false;
	bool compare = false;
	bool eval = false;
	double alpha, noise;

	boost::program_options::options_description desc("OPTIONS");

	desc.add_options()
		("help", "Help message")
		("imgfile", boost::program_options::value<std::string>(&imgfile)->default_value("mask"), "Binary image file (*.png)")
		("output", boost::program_options::value<bool>(&output)->implicit_value(true), "Returns output images")
		("sigma", boost::program_options::value<double>(&noise)->default_value(1.0), "Shape noise (sigma parameter)")
		("alpha", boost::program_options::value<double>(&alpha)->default_value(2.1), "Skeleton precision (alpha parameter)")
		("fileimg", boost::program_options::value<std::string>(&fileimg)->default_value("skelpropagortho"), "Skeleton img file")
		("compare", boost::program_options::value<bool>(&compare)->implicit_value(true), "Compare result with pruning methods")
		;

	boost::program_options::variables_map vm;
	boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
	boost::program_options::notify(vm);

	if (vm.count("help")) {
		std::cout << desc << std::endl;
		return 0;
	}

	time_t start,end;
	double diff;

	cv::Mat shpimggray = cv::imread(imgfile,cv::ImreadModes::IMREAD_GRAYSCALE);
	cv::Mat shpimg;
	cv::threshold(shpimggray,shpimg,125,255,cv::THRESH_BINARY);

	// topological closure
	cv::Mat shpdil;
	cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(3,3),cv::Point(1,1));
	cv::dilate(shpimg,shpdil,element);
	cv::erode(shpdil,shpimg,element);
	shape::DiscreteShape<2>::Ptr dissh = shape::DiscreteShape<2>::Ptr(new shape::DiscreteShape<2>(shpimg.cols,shpimg.rows));
	cv::Mat cpymat(shpimg.rows,shpimg.cols,CV_8U,&dissh->getContainer()[0]);
	shpimg.copyTo(cpymat);
	cv::Mat image(shpimg.rows,shpimg.cols,CV_8UC3,cv::Scalar(255,255,255));

	displayopencv::DisplayDiscreteShape(dissh,image,dissh->getFrame(),cv::Scalar(0,0,255));

	boundary::DiscreteBoundary<2>::Ptr disbnd = algorithm::extractboundary::NaiveBoundary(dissh);

	auto start0 = std::chrono::steady_clock::now();
	algorithm::skeletonization::OptionsSphProp options(noise,alpha);
	skeleton::GraphSkel2d::Ptr grskelpropag = algorithm::skeletonization::SpherePropagation2D(disbnd,dissh,options);
	auto duration0 = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start0);
	std::tuple<double,double,int,int> respropag = EvalSkel(dissh,disbnd,grskelpropag);
	int t0 = duration0.count();
	double A0 = std::get<0>(respropag); // sym area diff
	double H0 = std::get<1>(respropag); // Hausdorff dist
	int N0 = std::get<2>(respropag); // nb nodes
	int B0 = std::get<3>(respropag); // nb branches

	if(H0 > alpha)
		std::cerr << "Problem while computing propagation" << std::endl;

	std::cout << "Propagation skeleton computation: " << duration0.count() << "ms." << std::endl;

	shape::DiscreteShape<2>::Ptr shppropag(new shape::DiscreteShape<2>(dissh->getWidth(),dissh->getHeight()));
	algorithm::skinning::Filling(shppropag,grskelpropag);

	cv::Mat imagepropag;
	image.copyTo(imagepropag);
	displayopencv::DisplayDiscreteShape(shppropag,imagepropag,shppropag->getFrame(),cv::Scalar(0,255,0));
	displayopencv::DisplayDiscreteBoundary(disbnd,imagepropag,dissh->getFrame(),cv::Scalar(0,0,0));
	displayopencv::DisplayGraphSkeleton(grskelpropag,imagepropag,dissh->getFrame(),cv::Scalar(255,0,0));

	if(output)
	{
		std::ostringstream oss;
		oss.setf( std::ios::fixed, std:: ios::floatfield );
		oss.precision(1);
		oss << fileimg << ".png";
		cv::imwrite(oss.str(), imagepropag);
	}

	if(compare)
	{
		std::cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
		std::cout.precision(2);

		auto startv = std::chrono::steady_clock::now();
		skeleton::GraphSkel2d::Ptr grskelvoro = algorithm::skeletonization::VoronoiSkeleton2d(disbnd);
		auto durationv = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - startv);
		std::tuple<double,double,int,int> resv = EvalSkel(dissh,disbnd,grskelvoro);
		int tv = durationv.count();
		double Av = std::get<0>(resv); // sym area diff
		double Hv = std::get<1>(resv); // Hausdorff dist
		int Nv = std::get<2>(resv); // nb nodes
		int Bv = std::get<3>(resv); // nb branches

		int t1;
		double A1;
		double H1;
		int N1;
		int B1;
		double sat = 1.2;
		auto start1 = std::chrono::steady_clock::now();
		skeleton::GraphSkel2d::Ptr grskelsat = algorithm::pruning::ScaleAxisTransform(grskelvoro,sat);
		auto duration1 = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start1);
		std::tuple<double,double,int,int> res1 = EvalSkel(dissh,disbnd,grskelsat);
		t1 = tv + duration1.count();
		A1 = std::get<0>(res1); // sym area diff
		H1 = std::get<1>(res1); // Hausdorff dist
		N1 = std::get<2>(res1); // nb nodes
		B1 = std::get<3>(res1); // nb branches

		if(output)
		{
			shape::DiscreteShape<2>::Ptr shpskel(new shape::DiscreteShape<2>(dissh->getWidth(),dissh->getHeight()));
			algorithm::skinning::Filling(shpskel,grskelsat);

			cv::Mat imagerec;
			image.copyTo(imagerec);
			displayopencv::DisplayDiscreteShape(shpskel,imagerec,shpskel->getFrame(),cv::Scalar(0,255,0));
			displayopencv::DisplayDiscreteBoundary(disbnd,imagerec,dissh->getFrame(),cv::Scalar(0,0,0));
			displayopencv::DisplayGraphSkeleton(grskelsat,imagerec,dissh->getFrame(),cv::Scalar(255,0,0));

			std::ostringstream oss;
			oss.setf( std::ios::fixed, std:: ios::floatfield );
			oss.precision(1);
			oss << "sat.png";
			cv::imwrite(oss.str(), imagerec);
		}

		int t2;
		double A2;
		double H2;
		int N2;
		int B2;
		double lambda = 2.0;
		auto start2 = std::chrono::steady_clock::now();
		skeleton::GraphSkel2d::Ptr grskellambda = algorithm::pruning::LambdaMedialAxis(grskelvoro,disbnd,lambda);
		auto duration2 = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start2);
		std::tuple<double,double,int,int> res2 = EvalSkel(dissh,disbnd,grskellambda);
		t2 = tv + duration2.count();
		A2 = std::get<0>(res2); // sym area diff
		H2 = std::get<1>(res2); // Hausdorff dist
		N2 = std::get<2>(res2); // nb nodes
		B2 = std::get<3>(res2); // nb branches

		if(output)
		{
			shape::DiscreteShape<2>::Ptr shpskel(new shape::DiscreteShape<2>(dissh->getWidth(),dissh->getHeight()));
			algorithm::skinning::Filling(shpskel,grskellambda);

			cv::Mat imagerec;
			image.copyTo(imagerec);
			displayopencv::DisplayDiscreteShape(shpskel,imagerec,shpskel->getFrame(),cv::Scalar(0,255,0));
			displayopencv::DisplayDiscreteBoundary(disbnd,imagerec,dissh->getFrame(),cv::Scalar(0,0,0));
			displayopencv::DisplayGraphSkeleton(grskellambda,imagerec,dissh->getFrame(),cv::Scalar(255,0,0));

			std::ostringstream oss;
			oss.setf( std::ios::fixed, std:: ios::floatfield );
			oss.precision(0);
			oss << "lambda.png";
			cv::imwrite(oss.str(), imagerec);
		}

		int t3;
		double A3;
		double H3;
		int N3;
		int B3;
		double theta = 100*M_PI/180.0;
		auto start3 = std::chrono::steady_clock::now();
		skeleton::GraphSkel2d::Ptr grskeltheta = algorithm::pruning::ThetaMedialAxis(grskelvoro,theta);
		auto duration3 = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start3);
		std::tuple<double,double,int,int> res3 = EvalSkel(dissh,disbnd,grskeltheta);
		t3 = tv + duration3.count();
		A3 = std::get<0>(res3); // sym area diff
		H3 = std::get<1>(res3); // Hausdorff dist
		N3 = std::get<2>(res3); // nb nodes
		B3 = std::get<3>(res3); // nb branches

		if(output)
		{
			shape::DiscreteShape<2>::Ptr shpskel(new shape::DiscreteShape<2>(dissh->getWidth(),dissh->getHeight()));
			algorithm::skinning::Filling(shpskel,grskeltheta);

			cv::Mat imagerec;
			image.copyTo(imagerec);
			displayopencv::DisplayDiscreteShape(shpskel,imagerec,shpskel->getFrame(),cv::Scalar(0,255,0));
			displayopencv::DisplayDiscreteBoundary(disbnd,imagerec,dissh->getFrame(),cv::Scalar(0,0,0));
			displayopencv::DisplayGraphSkeleton(grskeltheta,imagerec,dissh->getFrame(),cv::Scalar(255,0,0));

			std::ostringstream oss;
			oss.setf( std::ios::fixed, std:: ios::floatfield );
			oss.precision(2);
			oss << "theta.png";
			cv::imwrite(oss.str(), imagerec);
		}

		std::cout << "\t Time \t SymArea  Hausdorff  Branches \t Nodes" << std::endl;
		std::cout << "Propag : " << t0 << " \t " << A0 << " \t  " << H0 << " \t     " << B0 << " \t " << N0 << std::endl;
		std::cout << "Voro   : " << tv << " \t " << Av << " \t  " << Hv << " \t     " << Bv << " \t " << Nv << std::endl;
		std::cout << "SAT    : " << t1 << " \t " << A1 << " \t  " << H1 << " \t     " << B1 << " \t " << N1 << std::endl;
		std::cout << "Lambda : " << t2 << " \t " << A2 << " \t  " << H2 << " \t     " << B2 << " \t " << N2 << std::endl;
		std::cout << "Theta  : " << t3 << " \t " << A3 << " \t  " << H3 << " \t     " << B3 << " \t " << N3 << std::endl;
	}

	return 0;
}