#include "Pnm.h"

#include <iostream>

#include <vector>
#include <string>
#include <cmath>

#include <Eigen/Sparse>

typedef Eigen::SparseMatrix<double, Eigen::RowMajor> Matrix;
typedef Matrix::InnerIterator MatrixIterator;
typedef Eigen::BiCGSTAB <Eigen::SparseMatrix<double>> BiCGSTAB;
typedef Eigen::LeastSquaresConjugateGradient <Eigen::SparseMatrix<double>>
        LeastSqCG;
typedef Eigen::SparseLU <Eigen::SparseMatrix<double>> SparseLU;

Pnm::Pnm(const std::map <std::string, std::variant<int, double, std::string>> &paramsPnm,
         std::shared_ptr <Netgrid> netgrid) :
        _paramsPnm(paramsPnm),
        _netgrid(netgrid),
        _dim(netgrid->_poresPores.size()),
        _matrix(_dim, _dim),
        _freeVector(new double[_dim], _dim),
        _pressures(new double[_dim], _dim) {}

void Pnm::printParamsPnm() {

    for (auto const &[name, param] : _paramsPnm) {
        std::cout << name << ": ";
        if (std::get_if<int>(&param))
            std::cout << std::get<int>(param) << std::endl;
        else if (std::get_if<double>(&param))
            std::cout << std::get<double>(param) << std::endl;
        else if (std::get_if<std::string>(&param))
            std::cout << std::get<std::string>(param) << std::endl;
    }

}

void Pnm::calcConductances(const std::vector<double> &densities,
                           const std::vector<double> &viscosities) {

    for (auto &[throat, pores] : _netgrid->_throatsPores) {

        auto &height = _netgrid->_throatsDepths[throat];
        auto &width = _netgrid->_throatsWidths[throat];
        auto &length = _netgrid->_throatsLs[throat];
        auto &density = densities[throat];
        auto &viscosity = viscosities[throat];
        // 3D case
        // double resistance = 12. * viscosity * length / height / height / height / width;
        // 2D case
        auto corrFactor = 1.12;
        double resistance = corrFactor * 12. * viscosity * length / width / width / width / height;

        // _conductances[throat] = density / resistance;
        _conductances[throat] = 1. / resistance;
    }

}

void Pnm::processThroats() {
    for (auto&[throat, pores] : _netgrid->_throatsPores) {
        auto &normals = _netgrid->_normalsThroatsPores[throat];
        for (uint32_t i = 0; i < pores.size(); i++)
            _matrixCoeffs[throat][pores[i]] = normals[i] * _conductances[throat];
    }
}

void Pnm::processPores() {

    for (auto &pore: _netgrid->_inletPores)
        _freeCoeffs[pore] = 0;
    for (auto &pore: _netgrid->_outletPores)
        _freeCoeffs[pore] = 0;
    for (auto &pore: _netgrid->_deadendPores)
        _freeCoeffs[pore] = 0;
    for (auto &pore: _netgrid->_nonboundPores)
        _freeCoeffs[pore] = 0;
}

void Pnm::processNewmanPores(std::map<uint32_t, double> &poresFlows) {
    // ToDo: make sure it is ok with flow sign
    for (auto &[pore, flow]: poresFlows)
        _freeCoeffs[pore] = flow;
}

void Pnm::processDirichPores(std::map<uint32_t, double> &poresPressures) {
    for (auto &[pore, pressure]: poresPressures)
        _freeCoeffs[pore] = pressure;
}

void Pnm::fillMatrix(std::map<uint32_t, double> &poresFlows,
                     std::map<uint32_t, double> &poresPressures,
                     const std::vector<double> &capillaryPressures) {

    for (int i = 0; i < _dim; ++i)
        for (MatrixIterator it(_matrix, i); it; ++it)
            it.valueRef() = 0;

    std::set <uint32_t> groupedPores;
    groupedPores.insert(_netgrid->_nonboundPores.begin(), _netgrid->_nonboundPores.end());
    groupedPores.insert(_netgrid->_deadendPores.begin(), _netgrid->_deadendPores.end());

    for (auto &pore: groupedPores) {
        _freeVector[pore] = _freeCoeffs[pore];
        auto &throats = _netgrid->_poresThroats[pore];
        auto &normals = _netgrid->_normalsPoresThroats[pore];
        for (uint32_t i = 0; i < throats.size(); i++) {
            _freeVector[pore] += normals[i] * _conductances[throats[i]] *
                                 capillaryPressures[throats[i]];
            for (auto &poreCurr : _netgrid->_throatsPores[throats[i]])
                _matrix.coeffRef(pore, poreCurr) +=
                        normals[i] * _matrixCoeffs[throats[i]][poreCurr];
        }
    }

    for (auto &[pore, flow]: poresFlows) {
        _freeVector[pore] = _freeCoeffs[pore];
        auto &throats = _netgrid->_poresThroats[pore];
        auto &normals = _netgrid->_normalsPoresThroats[pore];
        for (uint32_t i = 0; i < throats.size(); i++) {
            _freeVector[pore] += normals[i] * _conductances[throats[i]] *
                                 capillaryPressures[throats[i]];
            for (auto &poreCurr : _netgrid->_throatsPores[throats[i]])
                _matrix.coeffRef(pore, poreCurr) +=
                        normals[i] * _matrixCoeffs[throats[i]][poreCurr];
        }
    }

    for (auto &[pore, pressure]: poresPressures) {
        _freeVector[pore] = _freeCoeffs[pore];
        _matrix.coeffRef(pore, pore) = 1;
    }

}

void Pnm::calculatePress() {

    auto &itAccuracy = std::get<double>(_paramsPnm["it_accuracy"]);
    auto &solverMethod = std::get<std::string>(_paramsPnm["solver_method"]);

    if (solverMethod == "sparseLU") {

        SparseLU sparseLU;
        sparseLU.compute(_matrix);
        _pressures = sparseLU.solve(_freeVector);

    } else if (solverMethod == "biCGSTAB") {

        BiCGSTAB biCGSTAB;
        biCGSTAB.compute(_matrix);
        biCGSTAB.setTolerance(itAccuracy);
        _pressures = biCGSTAB.solveWithGuess(_freeVector, _pressures);

    } else if (solverMethod == "leastSqCG") {

        LeastSqCG leastSqCG;
        leastSqCG.compute(_matrix);
        leastSqCG.setTolerance(itAccuracy);
        _pressures = leastSqCG.solveWithGuess(_freeVector, _pressures);

    }

}

void Pnm::cfdProcedure(const std::vector<double> &densities,
                       const std::vector<double> &viscosities,
                       const std::vector<double> &capillaryPressures,
                       std::map<uint32_t, double> &poresFlows,
                       std::map<uint32_t, double> &poresPressures) {

    calcConductances(densities, viscosities);
    processThroats();
    processPores();
    processNewmanPores(poresFlows);
    processDirichPores(poresPressures);
    fillMatrix(poresFlows, poresPressures, capillaryPressures);
    // std::cout << _matrix << std::endl;

    calculatePress();

}

void Pnm::calcThroatsVolFlows(const std::vector<double> &capillaryPressures) {

    for (auto &[throat, conductance] : _conductances) {
        auto &pores = _netgrid->_throatsPores[throat];
        auto &normals = _netgrid->_normalsThroatsPores[throat];
        _throatsVolFlows[throat] = conductance * capillaryPressures[throat];
        for (uint32_t i = 0; i < pores.size(); i++)
            _throatsVolFlows[throat] -= normals[i] * conductance * _pressures[pores[i]];
    }

}

void Pnm::calcPoresFlowRates() {

    std::set <uint32_t> groupedPores;
    groupedPores.insert(_netgrid->_nonboundPores.begin(), _netgrid->_nonboundPores.end());
    groupedPores.insert(_netgrid->_deadendPores.begin(), _netgrid->_deadendPores.end());

    for (auto &pore: groupedPores) {
        auto &poresThroats = _netgrid->_poresThroats[pore];
        auto &normals = _netgrid->_normalsPoresThroats[pore];
        for (uint32_t i = 0; i < poresThroats.size(); i++)
            _poresFlowRates[pore] += normals[i] * _throatsVolFlows[poresThroats[i]];
    }

    for (auto &pore: _netgrid->_outletPores) {
        auto &poresThroats = _netgrid->_poresThroats[pore];
        auto &normals = _netgrid->_normalsPoresThroats[pore];
        for (uint32_t i = 0; i < poresThroats.size(); i++)
            _poresFlowRates[pore] += normals[i] * _throatsVolFlows[poresThroats[i]];
    }

    for (auto &pore: _netgrid->_inletPores) {
        auto &poresThroats = _netgrid->_poresThroats[pore];
        auto &normals = _netgrid->_normalsPoresThroats[pore];
        for (uint32_t i = 0; i < poresThroats.size(); i++)
            _poresFlowRates[pore] += -normals[i] * _throatsVolFlows[poresThroats[i]];
    }

}

void Pnm::calcTotalFlowRate(const std::set <uint32_t> &pores) {

    _totFlowRate = 0;

    for (auto &pore: pores)
        _totFlowRate += _poresFlowRates[pore];
}

void Pnm::setPressures(Eigen::Ref <Eigen::VectorXd> pressures) {
    if (_pressures.data() != pressures.data())
        delete _pressures.data();
    new(&_pressures) Eigen::Map<Eigen::VectorXd>(pressures.data(),
                                                 pressures.size());
}

Eigen::Ref <Eigen::VectorXd> Pnm::getPressures() {
    return _pressures;
}


/* This file is part of PyMesh. Copyright (c) 2015 by Qingnan Zhou */
#include "SelfIntersection.h"

#include <Core/Exception.h>
#include <Math/MatrixUtils.h>

#include <boost/function.hpp>
#include <boost/bind.hpp>

#include <CGAL/box_intersection_d.h>

using namespace PyMesh;

namespace SelfIntersectionHelper {
    class Box : public CGAL::Box_intersection_d::Box_d<Float,
          3, CGAL::Box_intersection_d::ID_NONE> {
        public:
            typedef CGAL::Box_intersection_d::Box_d<Float, 3,
                    CGAL::Box_intersection_d::ID_NONE> Base;
            typedef Float                   NT;
            typedef std::size_t             ID;

            Box() {}
            Box(bool complete) : Base(complete) {}
            Box(NT l[3], NT h[3]) : Base( l, h) {}
            Box( const CGAL::Bbox_3& b) : Base( b) {}
            ID  id() const { return m_id; }
            void set_id(ID id) { m_id = id; }
        private:
            ID m_id;
    };

    void handle_intersection_candidate(
            SelfIntersection* self, const Box& a, const Box& b) {
        self->handle_intersection_candidate(a.id(), b.id());
    }

    Vector2I get_opposite_edge(const Vector3I& f, size_t v) {
        if (f[0] == v) {
            return Vector2I(f[1], f[2]);
        } else if (f[1] == v) {
            return Vector2I(f[2], f[0]);
        } else if (f[2] == v) {
            return Vector2I(f[0], f[1]);
        } else {
            std::stringstream err_msg;
            err_msg << "Vertex " << v << " does not belong to triangle ("
                << f.transpose() << ")";
            throw RuntimeError(err_msg.str());
        }
    }

    size_t get_opposite_vertex(const Vector3I& f, const Vector2I& e) {
        if (f[0] != e[0] && f[0] != e[1]) return f[0];
        if (f[1] != e[0] && f[1] != e[1]) return f[1];
        if (f[2] != e[0] && f[2] != e[1]) return f[2];
        throw RuntimeError("Face must be topologically degnerated!");
    }

    std::vector<Box> get_triangle_bboxes(
            const SelfIntersection::Points& pts, const MatrixIr& faces) {
        const size_t num_faces = faces.rows();
        std::vector<Box> boxes;
        boxes.reserve(num_faces);
        for (size_t i=0; i<num_faces; i++) {
            const Vector3I f = faces.row(i);
            const std::vector<SelfIntersection::Point_3> corners{
                pts[f[0]], pts[f[1]], pts[f[2]]
            };
            if (CGAL::collinear(pts[f[0]], pts[f[1]], pts[f[2]])) {
                // Triangle is degenerated.
                continue;
            }
            boxes.emplace_back(CGAL::bbox_3(corners.begin(), corners.end()));
            boxes.back().set_id(i);
        }
        return boxes;
    }
}
using namespace SelfIntersectionHelper;

SelfIntersection::SelfIntersection(
        const MatrixFr& vertices, const MatrixIr& faces)
: m_faces(faces) {
    const size_t num_vertices = vertices.rows();
    const size_t dim = vertices.cols();
    const size_t vertex_per_face = faces.cols();

    if (dim != 3) {
        throw NotImplementedError(
                "Self intersection check only support 3D");
    }
    if (vertex_per_face != 3) {
        throw NotImplementedError(
                "Self intersection check only works with triangles");
    }

    m_points.resize(num_vertices);
    for (size_t i=0; i<num_vertices; i++) {
        m_points[i] = Point_3(
                vertices(i,0),
                vertices(i,1),
                vertices(i,2));
    }
}

void SelfIntersection::detect_self_intersection() {
    clear();
    std::vector<Box> boxes = get_triangle_bboxes(m_points, m_faces);
    boost::function<void(const Box& a, const Box& b)> cb =
        boost::bind(SelfIntersectionHelper::handle_intersection_candidate,
                this, _1, _2);
    CGAL::box_self_intersection_d(boxes.begin(), boxes.end(), cb);
}

void SelfIntersection::clear() {
    m_intersecting_pairs.clear();
}

void SelfIntersection::handle_intersection_candidate(
        size_t f_idx_1, size_t f_idx_2) {
    auto duplicated_vertices = topological_overlap(f_idx_1, f_idx_2);
    const Vector3I f1 = m_faces.row(f_idx_1);
    const Vector3I f2 = m_faces.row(f_idx_2);
    const Triangle_3 t1(m_points[f1[0]], m_points[f1[1]], m_points[f1[2]]);
    const Triangle_3 t2(m_points[f2[0]], m_points[f2[1]], m_points[f2[2]]);

    bool is_intersecting = false;
    const size_t num_duplicated_vertices = duplicated_vertices.size();
    switch (num_duplicated_vertices) {
        case 0:
            // triangles do not touch.
            {
                bool t1_degenerate = t1.is_degenerate();
                bool t2_degenerate = t2.is_degenerate();
                if (t1_degenerate || t2_degenerate) {
                    // Degenerated triangles are considered as
                    // self-intersecting.
                    is_intersecting = true;
                } else {
                    is_intersecting = CGAL::do_intersect(t1, t2);
                }
            }
            break;
        case 3:
            // duplicated face
            is_intersecting = true;
            break;
        case 1:
            {
                // touch at a vertex.
                size_t shared_vertex = duplicated_vertices[0];
                Vector2I opp_edge_1 = get_opposite_edge(f1, shared_vertex);
                Vector2I opp_edge_2 = get_opposite_edge(f2, shared_vertex);
                Segment_3 seg_1(m_points[opp_edge_1[0]], m_points[opp_edge_1[1]]);
                Segment_3 seg_2(m_points[opp_edge_2[0]], m_points[opp_edge_2[1]]);
                is_intersecting =
                    CGAL::do_intersect(t1, seg_2) ||
                    CGAL::do_intersect(t2, seg_1);
            }
            break;
        case 2:
            {
                // touch at an edge.
                Vector2I shared_edge(duplicated_vertices[0],
                        duplicated_vertices[1]);
                size_t v1 = get_opposite_vertex(f1, shared_edge);
                size_t v2 = get_opposite_vertex(f2, shared_edge);
                const auto& p1 = m_points[v1];
                const auto& p2 = m_points[v2];
                const auto& p3 = m_points[shared_edge[0]];
                const auto& p4 = m_points[shared_edge[1]];
                if (CGAL::coplanar(p1, p2, p3, p4)) {
                    if (CGAL::collinear(p3, p4, p1)) {
                        is_intersecting = true;
                    } else if (CGAL::collinear(p3, p4, p2)) {
                        is_intersecting = true;
                    } else {
                        switch (CGAL::coplanar_orientation(p3, p4, p1, p2)) {
                            case CGAL::POSITIVE:
                                is_intersecting = true;
                                break;
                            case CGAL::NEGATIVE:
                                is_intersecting = false;
                                break;
                            case CGAL::COLLINEAR:
                                throw RuntimeError(
                                        "Inconsistent CGAL predicate output");
                                break;
                        }
                    }
                } else {
                    is_intersecting = false;
                }
            }
            break;
        default:
            throw RuntimeError(
                    "Two triangles sharing more than 3 vertices? Something is very wrong");
    }

    if (is_intersecting) {
        m_intersecting_pairs.emplace_back(f_idx_1, f_idx_2);
    }
}

std::vector<size_t> SelfIntersection::topological_overlap(size_t id1, size_t id2) const {
    Vector3I f1 = m_faces.row(id1);
    Vector3I f2 = m_faces.row(id2);
    std::vector<size_t> duplicated_vertices;
    duplicated_vertices.reserve(3);
    for (size_t i=0; i<3; i++) {
        for (size_t j=0; j<3; j++) {
            if (f1[i] == f2[j]) {
                duplicated_vertices.push_back(f1[i]);
            }
        }
    }
    return duplicated_vertices;
}


#include <algorithm>
#include <array>
#include <cmath>

#include <boost/math/constants/constants.hpp>
#include <Euclid/BoundingVolume/OBB.h>
#include <Euclid/MeshUtil/CGALMesh.h>
#include <Euclid/Geometry/TriMeshGeometry.h>
#include <Euclid/Math/Vector.h>
#include <Euclid/Render/RayTracer.h>

namespace Euclid
{

template<typename Mesh, typename T>
void proxy_view(const Mesh& mesh,
                const ViewSphere<Mesh>& view_sphere,
                std::vector<T>& view_scores,
                float weight)
{
    using Point_3 = typename boost::property_traits<
        typename boost::property_map<Mesh,
                                     boost::vertex_point_t>::type>::value_type;
    using Kernel = typename CGAL::Kernel_traits<Point_3>::Kernel;
    const int proxies = 6;
    const int width = 256;
    const int height = 256;
    const float least_visible =
        std::cos(boost::math::float_constants::pi / 3.0f);
    const float w1 = weight;    // weight for projected areas
    const float w2 = 1.0f - w1; // weight for visible ratios

    auto [vbeg, vend] = vertices(mesh);
    auto mesh_vpmap = get(boost::vertex_point, mesh);
    OBB<Kernel> obb;
    obb.build(vbeg, vend, mesh_vpmap);

    // Compute projected area
    std::vector<float> positions;
    std::vector<unsigned> indices;
    extract_mesh<3>(mesh, positions, indices);
    positions.push_back(0.0f); // Embree alignment
    RayTracer raytracer;
    raytracer.attach_geometry_buffers(positions, indices);

    std::vector<float> projected_areas(proxies);
    for (size_t i = 0; i < projected_areas.size(); ++i) {
        OrthoRayCamera cam;
        auto view_dir = view_sphere.radius * obb.axis(i % 3);
        if (i >= 3) {
            view_dir = -view_dir;
        }
        Eigen::Vector3f pos, focus, up;
        cgal_to_eigen(view_sphere.center + view_dir, pos);
        cgal_to_eigen(view_sphere.center, focus);
        cgal_to_eigen(obb.axis((i + 1) % 3), up);
        cam.lookat(pos, focus, up);
        cam.set_extent(view_sphere.radius * 2.0f, view_sphere.radius * 2.0f);

        std::vector<uint8_t> pixels(width * height, 0);
        raytracer.render_silhouette(pixels, cam, width, height);
        auto proj = 0;
        for (size_t j = 0; j < pixels.size(); ++j) {
            if (pixels[j] != 0) {
                ++proj;
            }
        }
        projected_areas[i] = static_cast<float>(proj) / pixels.size();
    }
    auto max_proj_area =
        *std::max_element(projected_areas.begin(), projected_areas.end());
    for (auto& proj_area : projected_areas) {
        proj_area /= max_proj_area;
    }

    // Compute visible ratio
    std::vector<float> visible_ratios(proxies);
    std::vector<int> n_visible_facets(proxies, 0);
    for (const auto& f : faces(mesh)) {
        auto normal = face_normal(f, mesh);
        if (normal * obb.axis(0) > least_visible) {
            ++n_visible_facets[0];
        }
        if (normal * obb.axis(1) > least_visible) {
            ++n_visible_facets[1];
        }
        if (normal * obb.axis(2) > least_visible) {
            ++n_visible_facets[2];
        }
        if (-normal * obb.axis(0) > least_visible) {
            ++n_visible_facets[3];
        }
        if (-normal * obb.axis(1) > least_visible) {
            ++n_visible_facets[4];
        }
        if (-normal * obb.axis(2) > least_visible) {
            ++n_visible_facets[5];
        }
    }
    float inv_nf = 1.0f / num_faces(mesh);
    std::transform(n_visible_facets.begin(),
                   n_visible_facets.end(),
                   visible_ratios.begin(),
                   [inv_nf](int nf) { return nf * inv_nf; });
    auto max_visible_ratio =
        *std::max_element(visible_ratios.begin(), visible_ratios.end());
    for (auto& visible_ratio : visible_ratios) {
        visible_ratio /= max_visible_ratio;
    }

    // Compute final score
    view_scores.clear();
    view_scores.resize(num_vertices(view_sphere.mesh), 0.0f);
    auto sphere_vpmap = get(boost::vertex_point, view_sphere.mesh);
    auto sphere_vimap = get(boost::vertex_index, view_sphere.mesh);
    for (const auto& v : vertices(view_sphere.mesh)) {
        auto view_dir = normalized(get(sphere_vpmap, v) - obb.center());
        auto i = get(sphere_vimap, v);
        view_scores[i] += (w1 * projected_areas[0] + w2 * visible_ratios[0]) *
                          std::max(view_dir * obb.axis(0), 0.0f);
        view_scores[i] += (w1 * projected_areas[1] + w2 * visible_ratios[1]) *
                          std::max(view_dir * obb.axis(1), 0.0f);
        view_scores[i] += (w1 * projected_areas[2] + w2 * visible_ratios[2]) *
                          std::max(view_dir * obb.axis(2), 0.0f);
        view_scores[i] += (w1 * projected_areas[3] + w2 * visible_ratios[3]) *
                          std::max(-view_dir * obb.axis(0), 0.0f);
        view_scores[i] += (w1 * projected_areas[4] + w2 * visible_ratios[4]) *
                          std::max(-view_dir * obb.axis(1), 0.0f);
        view_scores[i] += (w1 * projected_areas[5] + w2 * visible_ratios[5]) *
                          std::max(-view_dir * obb.axis(2), 0.0f);
    }
}

} // namespace Euclid


#include "scaled_variance.h"

#include "smash/../../scatteractionsfinder.cc"

#include <Eigen/Dense>
#include <fstream>
#include <gsl/gsl_sf_bessel.h>

/// Loads particle table from SMASH
bool load_particle_types() {
  std::cout << "Loading SMASH particle types and decay modes" << std::endl;
  std::string smash_dir(std::getenv("SMASH_DIR"));
  if (smash_dir == "") {
    std::cerr << "Failed to load SMASH particle types. SMASH_DIR is not set."
              << std::endl;
    return true;
  }
  std::ifstream particles_input_file(smash_dir + "/input/particles.txt");
  std::stringstream buffer;
  if (particles_input_file) {
    buffer << particles_input_file.rdbuf();
    smash::ParticleType::create_type_list(buffer.str());
  } else {
    std::cerr << "File with SMASH particle list not found." << std::endl;
    return true;
  }
  std::ifstream decaymodes_input_file(smash_dir + "/input/decaymodes.txt");
  if (decaymodes_input_file) {
    buffer.clear();
    buffer.str(std::string());
    buffer << decaymodes_input_file.rdbuf();
    smash::DecayModes::load_decaymodes(buffer.str());
    smash::ParticleType::check_consistency();
  } else {
    std::cerr << "File with SMASH decaymodes not found." << std::endl;
    return true;
  }
  return false;
}

int ScaledVarianceCalculator::set_eos_solver_equations(
                             const gsl_vector* x, void* params,
                             gsl_vector* f) {
  smash::ParticleTypePtrList* types =
      reinterpret_cast<struct solver_params *>(params)->types;
  double e = reinterpret_cast<struct solver_params *>(params)->e;
  double nb = reinterpret_cast<struct solver_params *>(params)->nb;
  double ns = reinterpret_cast<struct solver_params *>(params)->ns;
  double nq = reinterpret_cast<struct solver_params *>(params)->nq;

  const double T = gsl_vector_get(x, 0);
  const double mub = gsl_vector_get(x, 1);
  const double mus = gsl_vector_get(x, 2);
  const double muq = gsl_vector_get(x, 3);

  double e_tot = 0.0, nb_tot = 0.0, ns_tot = 0.0, nq_tot = 0.0;
  for (const smash::ParticleTypePtr t : (*types)) {
    const double b = t->baryon_number();
    const double s = t->strangeness();
    const double q = t->charge();
    const double z = t->mass() / T;
    const double mu = mub * b + mus * s + muq * q;
    const double mu_m_over_T = (mu - t->mass()) / T;
    const double factor = t->pdgcode().spin_degeneracy() * 4.0 * M_PI *
                          std::pow(T / (2.0 * M_PI * smash::hbarc), 3);
    const double k1 = gsl_sf_bessel_Kn_scaled(1, z);
    const double k2 = gsl_sf_bessel_Kn_scaled(2, z);
    const double ex = std::exp(mu_m_over_T);
    double rho = z * z *  k2 * ex * factor;
    double edens = z * z * (3 * k2 + z * k1) * ex * T * factor;
    e_tot += edens;
    nb_tot += rho * b;
    ns_tot += rho * s;
    nq_tot += rho * q;
  }
  gsl_vector_set(f, 0, e_tot - e);
  gsl_vector_set(f, 1, nb_tot - nb);
  gsl_vector_set(f, 2, ns_tot - ns);
  gsl_vector_set(f, 3, nq_tot - nq);
  return GSL_SUCCESS;
}

std::string ScaledVarianceCalculator::print_solver_state(size_t iter,
     gsl_multiroot_fsolver* solver) const {
  std::stringstream s;
  // clang-format off
  s << "iter = " << iter << ","
    << " x = " << gsl_vector_get(solver->x, 0) << " "
               << gsl_vector_get(solver->x, 1) << " "
               << gsl_vector_get(solver->x, 2) << " "
               << gsl_vector_get(solver->x, 3) << ", "
    << "f(x) = " << gsl_vector_get(solver->f, 0) << " "
                 << gsl_vector_get(solver->f, 1) << " "
                 << gsl_vector_get(solver->f, 2) << " "
                 << gsl_vector_get(solver->f, 3) << " "
                 << std::endl;
  // clang-format on
  return s.str();

}

void ScaledVarianceCalculator::setTmu_from_conserved(double Etot, double V,
                                                double B, double S, double Q) {
  if (quantum_statistics_) {
    std::cout << "WARNING: quantum statistics requested, but not implemented "
              << "for (E, B, S, Q) -> (T, muB, muS, muQ) solver." << std::endl;
    throw std::runtime_error("");
  }
  const gsl_multiroot_fsolver_type *solver_type = gsl_multiroot_fsolver_hybrid;
  gsl_multiroot_fsolver* solver = gsl_multiroot_fsolver_alloc(solver_type, 4);
  gsl_vector* x = gsl_vector_alloc(4);
  int residual_status = GSL_SUCCESS;
  size_t iter = 0;
  std::cout << print_solver_state(iter, solver);

  struct solver_params p = {&all_types_in_the_box_, Etot/V, B/V, S/V, Q/V};
  gsl_multiroot_function f = {&set_eos_solver_equations, 4, &p};

  gsl_vector_set(x, 0, 0.15);
  gsl_vector_set(x, 1, 0.01);
  gsl_vector_set(x, 2, 0.01);
  gsl_vector_set(x, 3, 0.01);

  gsl_multiroot_fsolver_set(solver, &f, x);
  do {
    iter++;
    const auto iterate_status = gsl_multiroot_fsolver_iterate(solver);
    // std::cout << print_solver_state(iter, solver);

    // Avoiding too low temperature
    if (gsl_vector_get(solver->x, 0) < 0.015) {
      T_ = 0.0;
      mub_ = 0.0;
      mus_ = 0.0;
      muq_ = 0.0;
      return;
    }

    // check if solver is stuck
    if (iterate_status) {
      break;
    }
    residual_status = gsl_multiroot_test_residual(solver->f, 1.e-9);
  } while (residual_status == GSL_CONTINUE && iter < 1000);

  if (residual_status != GSL_SUCCESS) {
    std::stringstream solver_parameters;
    solver_parameters << "Solver run with "
                      << "e = " << Etot/V << ", nb = " << B/V
                      << ", ns = " << S/V << ", nq = " << Q/V
                      << std::endl;
    throw std::runtime_error(gsl_strerror(residual_status) +
                             solver_parameters.str() +
                             print_solver_state(iter, solver));
  }
  T_ = gsl_vector_get(solver->x, 0);
  mub_ = gsl_vector_get(solver->x, 1);
  mus_ = gsl_vector_get(solver->x, 2);
  muq_ = gsl_vector_get(solver->x, 3);
  gsl_multiroot_fsolver_free(solver);
  gsl_vector_free(x);
}

std::pair<double, double> ScaledVarianceCalculator::scaled_variance(
    std::function<bool(const smash::ParticleTypePtr)> type_of_interest) {

  constexpr unsigned int m = 5;
  Eigen::MatrixXd k2_tilde = Eigen::MatrixXd::Zero(m, m);
  double kappa1 = 0.0;

  for (const smash::ParticleTypePtr ptype : all_types_in_the_box_) {
    const double b = B_conservation_ ? ptype->baryon_number() : 0;
    const double s = S_conservation_ ? ptype->strangeness() : 0;
    const double q = Q_conservation_ ? ptype->charge() : 0;
    const double z = ptype->mass() / T_;
    const double mu = mub_ * b + mus_ * s + muq_ * q;
    const double mu_m_over_T = (mu - ptype->mass()) / T_;
    if (mu_m_over_T > 0 and quantum_statistics_) {
      std::cout << "Warning: quantum expressions for " << ptype->name() <<
                   " do not converge, m < chemical potential." << std::endl;
    }
    const double factor = ptype->pdgcode().spin_degeneracy() * 4.0 * M_PI *
                          std::pow(T_ / (2.0 * M_PI * smash::hbarc), 3);
    double EE = 0.0, EN = 0.0, NN = 0.0, N1 = 0.0;
    // std::cout << "Computing matrix for " << ptype.name() << std::endl;
    for (unsigned int k = 1; k < quantum_series_max_terms_; k++) {
      if (k > 1 and !quantum_statistics_) {
        break;
      }
      const double k1 = gsl_sf_bessel_Kn_scaled(1, z * k);
      const double k2 = gsl_sf_bessel_Kn_scaled(2, z * k);
      const double x = std::exp(mu_m_over_T * k);
      double N1_summand = z * z / k * k2 * x;
      double NN_summand = z * z * k2 * x;
      double EN_summand = z * z / k * (3 * k2 + k * z * k1) * x;
      double EE_summand = z * z / (k * k) *
                          ((z * z * k * k + 12.0) * k2 + 3.0 * z * k * k1) * x;
      // std::cout << "k = " << k
      //          << ", N1_summand*factor*1000 = " << N1_summand*factor*1000
      //          << ", NN_summand*factor*1000 = " << NN_summand*factor*1000
      //          << ", EN_summand = " << EN_summand
      //          << ", EE_summand = " << EE_summand << std::endl;
      if (k > 1 and
          EE_summand < EE * quantum_series_rel_precision_ and
          EN_summand < EN * quantum_series_rel_precision_ and
          NN_summand < NN * quantum_series_rel_precision_ and
          N1_summand < N1 * quantum_series_rel_precision_) {
        break;
      }
      if (k % 2 == 0 and ptype->pdgcode().is_baryon()) {
        NN_summand = -NN_summand;
        EN_summand = -EN_summand;
        EE_summand = -EE_summand;
        N1_summand = -N1_summand;
      }
      NN += NN_summand;
      EN += EN_summand;
      EE += EE_summand;
      N1 += N1_summand;
    }
    EE *= factor;
    EN *= factor;
    NN *= factor;
    N1 *= factor;

    Eigen::MatrixXd k2_tilde_ptype(m, m);
    // clang-format off
    k2_tilde_ptype << NN,     EN,    q*NN,    b*NN,    s*NN,
                      EN,     EE,    q*EN,    b*EN,    s*EN,
                      q*NN, q*EN,  q*q*NN,  q*b*NN,  q*s*NN,
                      b*NN, b*EN,  b*q*NN,  b*b*NN,  b*s*NN,
                      s*NN, s*EN,  s*q*NN,  s*b*NN,  s*s*NN;
    // clang-format on
    if (!type_of_interest(ptype)) {
      k2_tilde_ptype.row(0).setZero();
      k2_tilde_ptype.col(0).setZero();
      N1 = 0.0;
    }
    kappa1 += N1;
    k2_tilde += k2_tilde_ptype;
  }

  if (!energy_conservation_) {
    k2_tilde.row(1).setZero();
    k2_tilde.col(1).setZero();
  }
  // Remove zero rows and columns before asking for determinant
  // This uses the property of k2_tilde matrix being symmetric
  Eigen::Matrix<bool, 1, Eigen::Dynamic> non_zeros =
      k2_tilde.cast<bool>().colwise().any();
  const unsigned int n = non_zeros.count();
  Eigen::MatrixXd k2_tilde_nz(n, n);
  int i = 0, j;
  for (unsigned int i0 = 0; i0 < m; ++i0) {
    if (!non_zeros(i0)) {
      continue;
    }
    j = 0;
    for (unsigned int j0 = 0; j0 < m; ++j0) {
      if (non_zeros(j0)) {
        k2_tilde_nz(i, j) = k2_tilde(i0, j0);
        j++;
      }
    }
    i++;
  }

  const double rho_type_interest = kappa1;
  // No conservation laws: grand-canonical case
  if (n < 2) {
    return std::make_pair(rho_type_interest,
                          k2_tilde_nz(0,0) / kappa1);
  }

  const double det_k2_tilde = k2_tilde_nz.determinant();
  const double det_k2 = k2_tilde_nz.block(1, 1, n - 1, n - 1).determinant();
  const double expected_scaled_variance = det_k2_tilde / det_k2 /
                                          rho_type_interest;
  return std::make_pair(rho_type_interest, expected_scaled_variance);
}

void ScaledVarianceCalculator::prepare_decays() {
  std::vector<smash::ParticleTypePtr> all_stable_hadrons;
  for (const smash::ParticleType &ptype : smash::ParticleType::list_all()) {
    if (ptype.is_stable()) {
      all_stable_hadrons.push_back(&ptype);
    }
  }

  for (const smash::ParticleTypePtr res : all_types_in_the_box_) {
    smash::decaytree::Node tree(res->name(), 1.0, {res}, {res}, {res}, {});
    constexpr double sqrts = 5.0;  // This is sufficient to add all possible decays
    smash::decaytree::add_decays(tree, sqrts);
    std::vector<smash::FinalStateCrossSection> fs = tree.final_state_cross_sections();
    smash::deduplicate(fs);
    double wsum = 0.0;
    for (const smash::FinalStateCrossSection &xs : fs) {
      wsum += xs.cross_section_;
    }
    assert(std::abs(wsum - 1.0) < 1.e-9);
    std::cout << res->name() << std::endl;
    for (const smash::FinalStateCrossSection &xs : fs) {
      std::map<smash::ParticleTypePtr, int> decay_final_states;
      // For each stable particle count how many of it one finds in the final state
      for (const smash::ParticleTypePtr ptype : all_stable_hadrons) {
        std::string little_str = ptype->name(),
                    big_str = xs.name_;
        // How many times little_str occurs in big_str?
        int nPos = big_str.find(little_str, 0);
        int count = 0;
        while (nPos != std::string::npos) {
          count++;
          nPos = big_str.find(little_str, nPos + little_str.size());
        }
        if (count > 0) {
          decay_final_states[ptype] = count;
        }
      }
      all_decay_final_states_[res].emplace_back(
          std::make_pair(xs.cross_section_, decay_final_states));
      std::cout << "    " << xs.name_ << " " << xs.cross_section_ << ";   ";
      for (const auto &hadron_count : decay_final_states) {
        std::cout << hadron_count.second << hadron_count.first->name() << " ";
      }
      std::cout << std::endl;
    }
  }
}

std::ostream& operator<< (std::ostream& out,
                          const ScaledVarianceCalculator &svc) {
  out << "Scaled variance calculator" << std::endl;
  out << "T [GeV] = " << svc.T_
      << ", muB [GeV] = " << svc.mub_
      << ", muS [GeV] = " << svc.mus_
      << ", muQ [GeV] = " << svc.muq_ << std::endl;
  out << "Quantum statistics = " << svc.quantum_statistics_ << std::endl;
  out << "Included conservation laws: ";
  if (svc.energy_conservation_) {
    out << " energy;";
  }
  if (svc.B_conservation_) {
    out << "  baryon number;";
  }
  if (svc.S_conservation_) {
    out << " strangeness;";
  }
  if (svc.Q_conservation_) {
    out << " electric charge;";
  }
  out << std::endl;
  out << "Species in the box:" << std::endl;
  for (const smash::ParticleTypePtr t : svc.all_types_in_the_box_) {
    out << t->name() << " ";
  }
  out << std::endl;
  return out;
}

int main() {
  load_particle_types();

  // Prepare the set of particle species in the box
  std::vector<smash::ParticleTypePtr> hadrons_in_the_box;
  for (const smash::ParticleType &t : smash::ParticleType::list_all()) {
    if (t.is_hadron() && t.mass() < 2.0) {
      hadrons_in_the_box.push_back(&t);
    }
  }
  std::sort(hadrons_in_the_box.begin(), hadrons_in_the_box.end(),
            [&](smash::ParticleTypePtr ta, smash::ParticleTypePtr tb) {
              return ta->mass() < tb->mass();
            });

  const double Temperature = 0.2;  // [GeV]
  const double muB = 0.0;  // [GeV]
  const double muS = 0.0;  // [GeV]
  const double muQ = 0.0;  // [GeV]
  const bool E_conservation = true;
  const bool B_conservation = true;
  const bool S_conservation = true;
  const bool Q_conservation = true;
  const bool quantum_statistics = false;
  ScaledVarianceCalculator svc(hadrons_in_the_box,
                               Temperature, muB, muS, muQ,
                               E_conservation, B_conservation,
                               S_conservation, Q_conservation,
                               quantum_statistics);
  svc.prepare_decays();
/*
  const double V = 1762.1897;  // [fm^3]
  const double E_tot = 972.4227;  // [GeV]
  const double B_tot = 0.0;
  const double S_tot = 0.0;
  const double Q_tot = 0.0;
  svc.setTmu_from_conserved(E_tot, V, B_tot, S_tot, Q_tot);
  std::cout << svc;

  // Variance of each specie
  for (const smash::ParticleTypePtr t : hadrons_in_the_box) {
    const auto density_and_variance = svc.scaled_variance(
          [&](const smash::ParticleTypePtr t0) { return t0 == t; });
    std::cout << t->name() << " " << density_and_variance.first * V << " "
              << density_and_variance.second << std::endl;
  }

  // Variance of total number
  const auto density_and_variance = svc.scaled_variance(
      [&](const smash::ParticleTypePtr) { return true; });
  std::cout << "Ntot " << density_and_variance.first * V << " "
            << density_and_variance.second << std::endl;
*/
}


/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkBoostBrandesCentrality.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------*/
#include "vtkBoostBrandesCentrality.h"

#include "vtkCellArray.h"
#include "vtkCellData.h"
#include "vtkMath.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"
#include "vtkFloatArray.h"
#include "vtkDataArray.h"
#include "vtkSmartPointer.h"
#include "vtkStringArray.h"

#include "vtkBoostGraphAdapter.h"
#include "vtkDirectedGraph.h"
#include "vtkUndirectedGraph.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/betweenness_centrality.hpp>
#include <boost/graph/properties.hpp>

using namespace boost;

vtkStandardNewMacro(vtkBoostBrandesCentrality);

//-----------------------------------------------------------------------------
vtkBoostBrandesCentrality::vtkBoostBrandesCentrality() :
  UseEdgeWeightArray    (false),
  InvertEdgeWeightArray (false),
  EdgeWeightArrayName   (NULL)
{
}

//-----------------------------------------------------------------------------
vtkBoostBrandesCentrality::~vtkBoostBrandesCentrality()
{
  this->SetEdgeWeightArrayName(0);
}

//-----------------------------------------------------------------------------
void vtkBoostBrandesCentrality::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "UseEdgeWeightArray: " << this->UseEdgeWeightArray << endl;

  os << indent << "InvertEdgeWeightArray: " << this->InvertEdgeWeightArray
    << endl;

  os << indent << "this->EdgeWeightArrayName: " <<
    (this->EdgeWeightArrayName ?  this->EdgeWeightArrayName : "NULL") << endl;
}

//-----------------------------------------------------------------------------
int vtkBoostBrandesCentrality::RequestData(
  vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector,
  vtkInformationVector *outputVector)
{
  // get the info objects
  vtkInformation *inInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation *outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkGraph *input = vtkGraph::SafeDownCast(
    inInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkGraph *output = vtkGraph::SafeDownCast(
    outInfo->Get(vtkDataObject::DATA_OBJECT()));

  // Send the data to output.
  output->ShallowCopy(input);

  // Compute betweenness centrality

  // Property map for vertices
  vtkFloatArray* vertexCMap = vtkFloatArray::New();
  vertexCMap->SetName("centrality");
  identity_property_map imap;

  // Property map for edges
  vtkFloatArray* edgeCMap = vtkFloatArray::New();
  edgeCMap->SetName("centrality");
  vtkGraphEdgePropertyMapHelper<vtkFloatArray*> helper(edgeCMap);

  vtkSmartPointer<vtkDataArray> edgeWeight (0);
  if(this->UseEdgeWeightArray && this->EdgeWeightArrayName)
  {
    if(!this->InvertEdgeWeightArray)
    {
      edgeWeight = input->GetEdgeData()->GetArray(this->EdgeWeightArrayName);
    }
    else
    {
      vtkDataArray* weights =
          input->GetEdgeData()->GetArray(this->EdgeWeightArrayName);

      if(!weights)
      {
        vtkErrorMacro(<<"Error: Edge weight array " << this->EdgeWeightArrayName
                      << " is set but not found or not a data array.\n");
        return 0;
      }

      edgeWeight.TakeReference(
        vtkDataArray::CreateDataArray(weights->GetDataType()));

      double range[2];
      weights->GetRange(range);

      if(weights->GetNumberOfComponents() > 1)
      {
        return 0;
      }

      for(int i=0; i < weights->GetDataSize(); ++i)
      {
        edgeWeight->InsertNextTuple1(range[1] - weights->GetTuple1(i));
      }
    }

    if(!edgeWeight)
    {
      vtkErrorMacro(<<"Error: Edge weight array " << this->EdgeWeightArrayName
                    << " is set but not found or not a data array.\n");
      return 0;
    }
  }

  // Is the graph directed or undirected
  if (vtkDirectedGraph::SafeDownCast(output))
  {
    vtkDirectedGraph *g = vtkDirectedGraph::SafeDownCast(output);
    if(edgeWeight)
    {
      vtkGraphEdgePropertyMapHelper<vtkDataArray*> helper2(edgeWeight);
      brandes_betweenness_centrality(g,
        centrality_map(vertexCMap).edge_centrality_map(
          helper).vertex_index_map(imap).weight_map(helper2));
    }
    else
    {
      brandes_betweenness_centrality(g,
        centrality_map(vertexCMap).edge_centrality_map(
          helper).vertex_index_map(imap));
    }
  }
  else
  {
    vtkUndirectedGraph *g = vtkUndirectedGraph::SafeDownCast(output);
    if(edgeWeight)
    {
      vtkGraphEdgePropertyMapHelper<vtkDataArray*> helper2(edgeWeight);
      brandes_betweenness_centrality(g,
             centrality_map(vertexCMap).edge_centrality_map(
               helper).vertex_index_map(imap).weight_map(helper2));
    }
    else
    {
      brandes_betweenness_centrality(g,
             centrality_map(vertexCMap).edge_centrality_map(
               helper).vertex_index_map(imap));
    }
  }

  // Add the arrays to the output and dereference
  output->GetVertexData()->AddArray(vertexCMap);
  vertexCMap->Delete();
  output->GetEdgeData()->AddArray(edgeCMap);
  edgeCMap->Delete();

  return 1;
}


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/*
 * color.cpp
 */

#include "color.hpp"

#include <boost/numeric/ublas/matrix.hpp>
#include <cmath>

namespace imgproc {

/* class RGBColor */

RGBColor::RGBColor( float r, float g, float b )
    : ublas::vector<float>(3) {
    (*this)(0) = r; (*this)(1) = g; (*this)(2) = b;
}

RGBColor::RGBColor( const ublas::vector<float> & c )
    : ublas::vector<float>( c ) {}

RGBColor::RGBColor( const YCCColor & c )
    : ublas::vector<float>(3) {

    //assert( c(0) <= 1.0 && c(0) >= 0.0 );

    ublas::matrix<float> a( 3, 3 );
    a( 0, 0 ) = 1.f; a( 0, 1 ) = 9.2674E-4f; a( 0, 2 ) = 1.4017f;
    a( 1, 0 ) = 1.f; a( 1, 1 ) =  -0.34370f; a( 1, 2 ) = -0.7142f;
    a( 2, 0 ) = 1.f; a( 2, 1 ) = 1.7722f; a( 2, 2 ) = 9.9022E-4f;

    *this = RGBColor( ublas::prod( a, c ) );

    // clip to fit into rgb gamut
    ublas::vector<float> yccnochroma( 3 );
    yccnochroma(0) = c(0); yccnochroma(1) = yccnochroma(2) = 0.f;
    ublas::vector<float> nochroma = ublas::prod( a, yccnochroma );

    if ( (*this)(0) < 0.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 0.f - nochroma(0) ) / ( (*this)(0) - nochroma(0) );
        *this = RGBColor( nochroma + u * diff );
    }

    if ( (*this)(0) > 1.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 1.f - nochroma(0) ) / ( (*this)(0) - nochroma(0) );
        *this = RGBColor( nochroma + u * diff );
    }

    if ( (*this)(1) < 0.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 0.f - nochroma(1) ) / ( (*this)(1) - nochroma(1) );
        *this = RGBColor( nochroma + u * diff );
    }

    if ( (*this)(1) > 1.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 1.f - nochroma(1) ) / ( (*this)(1) - nochroma(1) );
        *this = RGBColor( nochroma + u * diff );
    }

    if ( (*this)(2) < 0.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 0.f - nochroma(2) ) / ( (*this)(2) - nochroma(2) );
        *this = RGBColor( nochroma + u * diff );
    }

    if ( (*this)(2) > 1.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 1.f - nochroma(2) ) / ( (*this)(2) - nochroma(2) );
        *this = RGBColor( nochroma + u * diff );
    }
}

gil::rgb8_pixel_t RGBColor::rgbpixel() const {

    return gil::rgb8_pixel_t(
        (unsigned char)(round( 0xff * (*this)(0) )),
        (unsigned char)(round( 0xff * (*this)(1) )),
        (unsigned char)(round( 0xff * (*this)(2) )));
}

RGBColor::RGBColor( const gil::rgb8_pixel_t & p )
    : ublas::vector<float>(3) {

    (*this)(0) = (float) p[0] / 0xff;
    (*this)(1) = (float) p[1] / 0xff;
    (*this)(2) = (float) p[2] / 0xff;
}

/* class YCCColor */

YCCColor::YCCColor( float y, float cb, float cr )
    : ublas::vector<float>(3) {
    (*this)(0) = y; (*this)(1) = cb; (*this)(2) = cr;
}

YCCColor::YCCColor( const ublas::vector<float> & c )
    : ublas::vector<float>( c ) {}

YCCColor::YCCColor( const RGBColor & c )
    : ublas::vector<float>(3) {

    /*assert( c(0) <= 1.0 && c(0) >= 0.0 );
    assert( c(1) <= 1.0 && c(1) >= 0.0 );
    assert( c(2) <= 1.0 && c(2) >= 0.0 );*/

    ublas::matrix<float> a( 3, 3 );
    a( 0, 0 ) = 0.299f; a( 0, 1 ) = 0.587f; a( 0, 2 ) = 0.114f;
    a( 1, 0 ) = -0.169f; a( 1, 1 ) = -0.331f; a( 1, 2 ) = 0.500f;
    a( 2, 0 ) = 0.500f; a( 2, 1 ) = -0.419f; a( 2, 2 ) = -0.081f;

    //std::cout << ublas::prod( a, c );

    *this = YCCColor( ublas::prod( a, c ) );

    //std::cout << " =? " << *this << std::endl;
}

YCCColor::YCCColor( const gil::rgb8_pixel_t & yccpixel )
    : ublas::vector<float>(3) {

    (*this)(0) = ( (float) yccpixel[0] / 0xff ) - 0.5f;
    (*this)(1) = ( (float) yccpixel[1] / 0xff ) - 0.5f;
    (*this)(2) = ( (float) yccpixel[2] / 0xff ) - 0.5f;
}

YCCColor::YCCColor( const gil::rgb32f_pixel_t & yccpixel )
    : ublas::vector<float>(3) {

    (*this)(0) = ( yccpixel[0] / 0xff ) - 0.5f;
    (*this)(1) = ( yccpixel[1] / 0xff ) - 0.5f;
    (*this)(2) = ( yccpixel[2] / 0xff ) - 0.5f;
}

gil::rgb8_pixel_t YCCColor::yccpixel() const {

    return gil::rgb8_pixel_t(
        (int) round( 0xff * (*this)(0) ),
        (int) round( 0xff * ( (*this)(1) + 0.5f ) ),
        (int) round( 0xff * ( (*this)(2) + 0.5f ) ) );
}

float ccDiff( const YCCColor & color1, const YCCColor & color2 ) {

    float hue1, hue2;

    hue1 = atan2( color1( 2 ), color1( 1 ) );

    hue2 = atan2( color2( 2 ), color2( 1 ) );

    return std::min( fabs( hue1 - hue2 ), float(2 * M_PI) - fabs( hue1 - hue2 ) );

    //return sqr( color2( 1 ) - color1( 1 ) ) + sqr( color2( 2 ) - color1( 2 ) );
}

} // namespace imgproc


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/global_pose_estimation/least_unsquared_deviation_position_estimator.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <limits>
#include <unordered_map>
#include <vector>

#include "theia/math/qp_solver.h"
#include "theia/sfm/global_pose_estimation/pairwise_translation_and_scale_error.h"
#include "theia/sfm/types.h"
#include "theia/util/map_util.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

Vector3d GetRotatedTranslation(const Vector3d& rotation_angle_axis,
                               const Vector3d& translation) {
  Matrix3d rotation;
  ceres::AngleAxisToRotationMatrix(
      rotation_angle_axis.data(),
      ceres::ColumnMajorAdapter3x3(rotation.data()));
  return rotation.transpose() * translation;
}

}  // namespace

LeastUnsquaredDeviationPositionEstimator::
    LeastUnsquaredDeviationPositionEstimator(
        const LeastUnsquaredDeviationPositionEstimator::Options& options)
    : options_(options) {
  CHECK_GT(options_.max_num_iterations, 0);
  CHECK_GT(options_.max_num_reweighted_iterations, 0);
}

bool LeastUnsquaredDeviationPositionEstimator::EstimatePositions(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  CHECK_NOTNULL(positions)->clear();

  InitializeIndexMapping(view_pairs, orientations);

  // Set up the linear system.
  SetupConstraintMatrix(view_pairs, orientations);
  solution_.setZero(constraint_matrix_.cols());
  weights_.setConstant(constraint_matrix_.rows(), 1.0);

  // Set the lower bounds for the QP. The positions should be unbounded while
  // the scales havea lower bound of 1.0.
  Eigen::VectorXd lower_bound(constraint_matrix_.cols());
  lower_bound.head(3 * orientations.size())
      .setConstant(-std::numeric_limits<double>::infinity());
  lower_bound.tail(view_pairs.size()).setConstant(1.0);

  // Solve for the camera positions using an IRLS scheme. The values p and r are
  // constant at zero.
  Eigen::SparseMatrix<double> P(constraint_matrix_.rows(),
                                constraint_matrix_.rows());
  Eigen::VectorXd q(constraint_matrix_.cols());
  q.setZero();
  const double r = 0;
  QPSolver::Options qp_solver_options;
  qp_solver_options.max_num_iterations = 10;
  for (int i = 0; i < options_.max_num_reweighted_iterations; i++) {
    if (i > 0) {
      UpdateConstraintWeights();
    }

    // Compute P = A^t * W * A, the quadratic matrix term for our QP.
    P = constraint_matrix_.transpose() * weights_.matrix().asDiagonal() *
        constraint_matrix_;

    // Solve the quadratic program. Increase the number of possible iterations
    // each time.
    qp_solver_options.max_num_iterations = std::min(
        options_.max_num_iterations, 2 * qp_solver_options.max_num_iterations);
    QPSolver qp_solver(qp_solver_options, P, q, r);
    const Eigen::VectorXd prev_solution = solution_;
    qp_solver.SetLowerBound(lower_bound);
    if (!qp_solver.Solve(&solution_)) {
      LOG(WARNING) << "Could not solve the Quadratic Program for the least "
                      "unsquared deviations position solver.";
      return false;
    }
  }

  // Set the estimated positions.
  for (const auto& view_id_index : view_id_to_index_) {
    const int index = view_id_index.second;
    const ViewId view_id = view_id_index.first;
    if (index == kConstantViewIndex) {
      (*positions)[view_id] = Eigen::Vector3d::Zero();
    } else {
      (*positions)[view_id] = solution_.segment<3>(index);
    }
  }

  return true;
}

void LeastUnsquaredDeviationPositionEstimator::InitializeIndexMapping(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations) {
  std::unordered_set<ViewId> views;
  for (const auto& view_pair : view_pairs) {
    if (ContainsKey(orientations, view_pair.first.first)) {
      views.insert(view_pair.first.first);
    }
    if (ContainsKey(orientations, view_pair.first.second)) {
      views.insert(view_pair.first.second);
    }
  }

  // Create a mapping from the view id to the index of the linear system.
  int index = kConstantViewIndex;
  view_id_to_index_.reserve(orientations.size());
  for (const ViewId view_id : views) {
    view_id_to_index_[view_id] = index;
    index += 3;
  }

  // Create a mapping from the view id pair to the index of the linear system.
  view_id_pair_to_index_.reserve(view_pairs.size());
  for (const auto& view_pair : view_pairs) {
    view_id_pair_to_index_[view_pair.first] = index;
    ++index;
  }
}

void LeastUnsquaredDeviationPositionEstimator::SetupConstraintMatrix(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations) {
  constraint_matrix_.resize(3 * view_pairs.size(),
                            3 * orientations.size() + view_pairs.size());

  // Add the camera to camera constraints.
  std::vector<Eigen::Triplet<double> > triplet_list;
  triplet_list.reserve(9 * view_pairs.size());
  int row = 0;
  for (const auto& view_pair : view_pairs) {
    const ViewIdPair view_id_pair = view_pair.first;

    const int view1_index = FindOrDie(view_id_to_index_, view_id_pair.first);
    const int view2_index = FindOrDie(view_id_to_index_, view_id_pair.second);
    const int scale_index =
        FindOrDieNoPrint(view_id_pair_to_index_, view_id_pair);

    // Rotate the relative translation so that it is aligned to the global
    // orientation frame.
    const Vector3d translation_direction =
        GetRotatedTranslation(FindOrDie(orientations, view_id_pair.first),
                              view_pair.second.position_2);

    // Add the constraint for view 1 in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    if (view1_index != kConstantViewIndex) {
      triplet_list.emplace_back(row + 0, view1_index + 0, -1.0);
      triplet_list.emplace_back(row + 1, view1_index + 1, -1.0);
      triplet_list.emplace_back(row + 2, view1_index + 2, -1.0);
    }

    // Add the constraint for view 2 in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    if (view2_index != kConstantViewIndex) {
      triplet_list.emplace_back(row + 0, view2_index + 0, 1.0);
      triplet_list.emplace_back(row + 1, view2_index + 1, 1.0);
      triplet_list.emplace_back(row + 2, view2_index + 2, 1.0);
    }

    // Add the constraint for scale in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    triplet_list.emplace_back(row + 0, scale_index, -translation_direction[0]);
    triplet_list.emplace_back(row + 1, scale_index, -translation_direction[1]);
    triplet_list.emplace_back(row + 2, scale_index, -translation_direction[2]);

    row += 3;
  }

  constraint_matrix_.setFromTriplets(triplet_list.begin(), triplet_list.end());

  VLOG(2) << view_pairs.size() << " camera to camera constraints were added "
                                    "to the position estimation problem.";
}

// Compute the error:
//     err_i_j = || c_j - c_i - scale_i_j * t_i_j ||^2
//
// For each pairwise constraint, set w_i_j = (err_i_j + delta)^(-1/2) to
// reweight the QP solver for robustness.
void LeastUnsquaredDeviationPositionEstimator::UpdateConstraintWeights() {
  static const double delta = 1e-12;
  // Compute the errors with a simple matrix multiplication.
  const Eigen::VectorXd errors = constraint_matrix_ * solution_;
  weights_ = (errors.array().square() + delta).sqrt().inverse();
}

}  // namespace theia


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/global_pose_estimation/least_unsquared_deviation_position_estimator.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <ceres/rotation.h>

#include <limits>
#include <unordered_map>
#include <vector>

#include "theia/math/constrained_l1_solver.h"
#include "theia/sfm/types.h"
#include "theia/util/map_util.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

Vector3d GetRotatedTranslation(const Vector3d& rotation_angle_axis,
                               const Vector3d& translation) {
  Matrix3d rotation;
  ceres::AngleAxisToRotationMatrix(
      rotation_angle_axis.data(),
      ceres::ColumnMajorAdapter3x3(rotation.data()));
  return rotation.transpose() * translation;
}

}  // namespace

LeastUnsquaredDeviationPositionEstimator::
    LeastUnsquaredDeviationPositionEstimator(
        const LeastUnsquaredDeviationPositionEstimator::Options& options)
    : options_(options) {
  CHECK_GT(options_.max_num_iterations, 0);
  CHECK_GT(options_.max_num_reweighted_iterations, 0);
}

bool LeastUnsquaredDeviationPositionEstimator::EstimatePositions(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  CHECK_NOTNULL(positions)->clear();

  InitializeIndexMapping(view_pairs, orientations);
  const int num_views = view_id_to_index_.size();
  const int num_view_pairs = view_id_pair_to_index_.size();

  // Set up the linear system.
  SetupConstraintMatrix(view_pairs, orientations);
  Eigen::VectorXd solution;
  solution.setZero(constraint_matrix_.cols());

  // Create the lower bound constraint enforcing that all scales are > 1.
  Eigen::SparseMatrix<double> geq_mat(num_view_pairs,
                                      constraint_matrix_.cols());
  for (int i = 0; i < num_view_pairs; i++) {
    geq_mat.insert(i, 3 * (num_views - 1) + i) = 1.0;
  }
  Eigen::VectorXd geq_vec(num_view_pairs);
  geq_vec.setConstant(1.0);

  Eigen::VectorXd b(constraint_matrix_.rows());
  b.setZero();

  // Solve for camera positions by solving a constrained L1 problem to enforce
  // all relative translations scales > 1.
  ConstrainedL1Solver::Options l1_options;
  ConstrainedL1Solver solver(
      l1_options, constraint_matrix_, b, geq_mat, geq_vec);
  solver.Solve(&solution);

  // Set the estimated positions.
  for (const auto& view_id_index : view_id_to_index_) {
    const int index = view_id_index.second;
    const ViewId view_id = view_id_index.first;
    if (index == kConstantViewIndex) {
      (*positions)[view_id] = Eigen::Vector3d::Zero();
    } else {
      (*positions)[view_id] = solution.segment<3>(index);
    }
  }

  return true;
}

void LeastUnsquaredDeviationPositionEstimator::InitializeIndexMapping(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations) {
  std::unordered_set<ViewId> views;
  for (const auto& view_pair : view_pairs) {
    if (ContainsKey(orientations, view_pair.first.first) &&
        ContainsKey(orientations, view_pair.first.second)) {
      views.insert(view_pair.first.first);
      views.insert(view_pair.first.second);
    }
  }

  // Create a mapping from the view id to the index of the linear system.
  int index = kConstantViewIndex;
  view_id_to_index_.reserve(views.size());
  for (const ViewId view_id : views) {
    view_id_to_index_[view_id] = index;
    index += 3;
  }

  // Create a mapping from the view id pair to the index of the linear system.
  view_id_pair_to_index_.reserve(view_pairs.size());
  for (const auto& view_pair : view_pairs) {
    if (ContainsKey(view_id_to_index_, view_pair.first.first) &&
        ContainsKey(view_id_to_index_, view_pair.first.second)) {
      view_id_pair_to_index_[view_pair.first] = index;
      ++index;
    }
  }
}

void LeastUnsquaredDeviationPositionEstimator::SetupConstraintMatrix(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations) {
  constraint_matrix_.resize(
      3 * view_id_pair_to_index_.size(),
      3 * (view_id_to_index_.size() - 1) + view_pairs.size());

  // Add the camera to camera constraints.
  std::vector<Eigen::Triplet<double> > triplet_list;
  triplet_list.reserve(9 * view_pairs.size());
  int row = 0;
  for (const auto& view_pair : view_pairs) {
    const ViewIdPair view_id_pair = view_pair.first;
    if (!ContainsKey(view_id_to_index_, view_id_pair.first) ||
        !ContainsKey(view_id_to_index_, view_id_pair.second)) {
      continue;
    }

    const int view1_index = FindOrDie(view_id_to_index_, view_id_pair.first);
    const int view2_index = FindOrDie(view_id_to_index_, view_id_pair.second);
    const int scale_index =
        FindOrDieNoPrint(view_id_pair_to_index_, view_id_pair);

    // Rotate the relative translation so that it is aligned to the global
    // orientation frame.
    const Vector3d translation_direction =
        GetRotatedTranslation(FindOrDie(orientations, view_id_pair.first),
                              view_pair.second.position_2);

    // Add the constraint for view 1 in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    if (view1_index != kConstantViewIndex) {
      triplet_list.emplace_back(row + 0, view1_index + 0, -1.0);
      triplet_list.emplace_back(row + 1, view1_index + 1, -1.0);
      triplet_list.emplace_back(row + 2, view1_index + 2, -1.0);
    }

    // Add the constraint for view 2 in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    if (view2_index != kConstantViewIndex) {
      triplet_list.emplace_back(row + 0, view2_index + 0, 1.0);
      triplet_list.emplace_back(row + 1, view2_index + 1, 1.0);
      triplet_list.emplace_back(row + 2, view2_index + 2, 1.0);
    }

    // Add the constraint for scale in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    triplet_list.emplace_back(row + 0, scale_index, -translation_direction[0]);
    triplet_list.emplace_back(row + 1, scale_index, -translation_direction[1]);
    triplet_list.emplace_back(row + 2, scale_index, -translation_direction[2]);

    row += 3;
  }

  constraint_matrix_.setFromTriplets(triplet_list.begin(), triplet_list.end());

  VLOG(2) << view_pairs.size() << " camera to camera constraints were added "
                                  "to the position estimation problem.";
}

std::unordered_map<ViewId, Eigen::Vector3d> LeastUnsquaredDeviationPositionEstimator::EstimatePositionsWrapper(
      const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
      const std::unordered_map<ViewId, Eigen::Vector3d>& orientation) {
  std::unordered_map<ViewId, Eigen::Vector3d> positions;
  EstimatePositions(view_pairs, orientation, &positions);
  return positions;
}

}  // namespace theia


/**
BSD 3-Clause License

This file is part of the code accompanying the paper
From Planes to Corners: Multi-Purpose Primitive Detection in Unorganized 3D Point Clouds
by C. Sommer, Y. Sun, L. Guibas, D. Cremers and T. Birdal,
accepted for Publication in IEEE Robotics and Automation Letters (RA-L) 2020.

Copyright (c) 2019, Christiane Sommer.
All rights reserved.

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

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

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

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

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

// includes
#include <iostream>
#include <fstream>
#include "definitions.h"
// libraries
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <sophus/so3.hpp>
#include <CLI/CLI.hpp>
// classes
#include "Timer.h"
#include "Plane.h"
#include "graph/PlaneGraph.h"
#include "graph/ParallelPlaneGraph.h"
#include "PPF/PairDetector.h"
#include "corners/Corner.h"
// function includes
#include "io/load_ply_cloud.h"
#include "visualize/pcshow.h"

/*
 * main
 */
int main(int argc, char** argv) {

    Timer T;

    // parse setttings from command line
    std::string ply_file;
    int min_votes = 5;
    double d_min = .1, d_max = 1.;
    int sampling = 50;

    CLI::App app{"Detect and refine orthogonal plane configurations"};
    app.add_option("--img", ply_file, "ply file path, from data/")->required();
    app.add_option("--min-votes", min_votes, "minimal number of votes to accept configuration");
    app.add_option("--dmin", d_min, "minimal distance between two points");
    app.add_option("--dmax", d_max, "maximal distance between two points");
    app.add_option("--sampling", sampling, "sampling of points");

    try {
        app.parse(argc, argv);
    } catch (const CLI::ParseError& e) {
        return app.exit(e);
    }

    // load data
    std::string filepath = "../data/";
    std::vector<Eigen::Vector3f> points;
    std::vector<Eigen::Vector3f> normals;

    T.tic();
    read_ply_file(filepath+ply_file, points, normals);
    T.toc("read data");
    std::cout << points.size() << " points loaded." << std::endl;
    std::cout << normals.size() << " normals loaded." << std::endl;

    pcshow(points);

    // pairing
    ppf::PairDetector pairDet(d_max, d_min, min_votes);
    PlaneGraph planeMap(pairDet.para_thresh(), pairDet.distance_bin());
    T.tic();
    pairDet.detect_ortho_pairs(points, normals, planeMap);
    T.toc("time pairing PPF ");
    planeMap.print_info();

    std::cout << std::endl << "Thresholds:\tAngle:\t" << pairDet.para_thresh() << "\tDistance:\t" << pairDet.distance_bin() << std::endl << std::endl;

    // clustering & filtering
    T.tic();
    planeMap.cluster_graph_vertices();
    T.toc("clustering planes");
    planeMap.print_info();
    planeMap.print_parameters();
    planeMap.print_edges();

    // reduce graph to ParallelPlaneGraph
    T.tic();
    ParallelPlaneGraph redMap = planeMap.reduce_graph();
    T.toc("graph reduction");
    redMap.print_info();
    redMap.print_edges();
    T.tic();
    redMap.triangle_reduce();
    T.toc("triangle reduction");
    redMap.print_info();
    redMap.print_parameters();
    redMap.print_edges();

    T.tic();
    redMap.filter_outliers(points, normals, pairDet.distance_bin(), .5*sampling);
    T.toc("outlier filtering");
    redMap.print_info();
    redMap.print_parameters();
    redMap.print_edges();

    // coarse refinement
    double lambda = .01 * points.size();
    T.tic();
    redMap.refine(points, normals, lambda, pairDet.distance_bin(), sampling);
    redMap.cluster_graph_vertices();
    T.toc("coarse refine and clustering");
    redMap.print_info();
    redMap.print_parameters();
    redMap.print_edges();

    T.tic();
    redMap.filter_outliers(points, normals, pairDet.distance_bin(), .5*sampling);
    T.toc("outlier filtering");
    redMap.print_info();
    redMap.print_parameters();
    redMap.print_edges();

    // find triangles
    std::vector<Graph<ParallelPlaneSet>::Triangle> triangles;
    T.tic();
    redMap.find_triangles(triangles);
    T.toc("find triangles in reduced graph");
    redMap.print_triangles(triangles);
    // if no triangles found: stop here
    if (triangles.size() < 1) {
        std::cout << "No corners found, return." << std::endl;
        return 0;
    }

    // neighborhood search: Kd-Tree
    const PointCloudAdaptor pc2kd(points);
    KDTree index(3, pc2kd, nanoflann::KDTreeSingleIndexAdaptorParams(10)); // arguments: dimensionality, point cloud adaptor, max leafs
    index.buildIndex();
    std::vector<Corner> corners;
    std::vector<size_t> indices;

    // find good corners
    T.tic();
    for (const auto& t : triangles) {
        Vec3 n1 = redMap.vertices()[t.first].n();
        Vec3 n2 = redMap.vertices()[t.second].n();
        Vec3 n3 = redMap.vertices()[t.third].n();
        for (const auto d1 : redMap.vertices()[t.first].ds()) for (const auto d2 : redMap.vertices()[t.second].ds()) for (const auto d3 : redMap.vertices()[t.third].ds()) {
            indices = Corner::eval_candidate(points, normals, n1, d1, n2, d2, n3, d3, index);
            std::cout << (-d1*n1 - d2*n2 - d3*n3).transpose() << std::endl;
            if (indices.size() > 0) {
                Corner c(n1, d1, n2, d2, n3, d3);
                c.set_indices(indices);
                if (c.refine(points)) { // only accept if refinement converges
                    corners.push_back(c);
                }
            }
        }
    }
    T.toc("evaluated corner candidates");

    for (const auto& c : corners) {
        std::cout << c << std::endl << std::endl;
    }

    pcshow_corners(points, corners);

}


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file knapsack_unbounded.hpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-09-20
 */
#ifndef PAAL_KNAPSACK_UNBOUNDED_HPP
#define PAAL_KNAPSACK_UNBOUNDED_HPP

#include "paal/dynamic/knapsack/fill_knapsack_dynamic_table.hpp"
#include "paal/dynamic/knapsack/get_bound.hpp"
#include "paal/dynamic/knapsack/knapsack_common.hpp"
#include "paal/greedy/knapsack_unbounded_two_app.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/knapsack_utils.hpp"
#include "paal/utils/less_pointees.hpp"
#include "paal/utils/type_functions.hpp"
#include "paal/utils/irange.hpp"

#include <boost/range/adaptor/reversed.hpp>
#include <boost/optional.hpp>

#include <vector>

namespace paal {

namespace detail {
/**
 * @brief For knapsack dynamic algorithm for given element the table has to be
 * traversed from the lowest to highest element
 */
struct knapsack_get_position_range {
    template <typename T>
    auto operator()(T begin, T end)->decltype(irange(begin, end)) {
        return irange(begin, end);
    }
};

template <typename KnapsackData,
          typename GetBestElement, typename ValuesComparator,
          typename ReturnType = typename KnapsackData::return_type>
ReturnType knapsack_unbounded_dynamic(
    KnapsackData knap_data,
    GetBestElement getBest, ValuesComparator compareValues) {
    using Value = typename KnapsackData::value;
    using Size  = typename KnapsackData::size;
    using ObjectsIter = typename KnapsackData::object_iter;
    using ObjIterWithValueOrNull =
        boost::optional<std::pair<ObjectsIter, Value>>;
    std::vector<ObjIterWithValueOrNull> objectOnSize(knap_data.get_capacity() + 1);

    auto compare = [ = ](const ObjIterWithValueOrNull & left,
                         const ObjIterWithValueOrNull & right) {
        return compareValues(left->second, right->second);
    };

    auto objectOnSizeBegin = objectOnSize.begin();
    auto objectOnSizeEnd = objectOnSize.end();
    fill_knapsack_dynamic_table(objectOnSizeBegin, objectOnSizeEnd, knap_data.get_objects(), knap_data.get_size(),
                                [&](ObjIterWithValueOrNull val, ObjectsIter obj)
                                    ->ObjIterWithValueOrNull{
        return std::make_pair(obj, val->second + knap_data.get_value(*obj));
    },
                                compare, [](ObjIterWithValueOrNull & val) {
        val = std::make_pair(ObjectsIter{}, Value{});
    },
                                detail::knapsack_get_position_range());

    // getting position of the max value in the objectOnSize array
    auto maxPos = getBest(objectOnSizeBegin, objectOnSizeEnd, compare);

    // setting solution
    auto remainingSpaceInKnapsack = maxPos;
    while (remainingSpaceInKnapsack != objectOnSizeBegin) {
        assert(*remainingSpaceInKnapsack);
        auto && obj = *((*remainingSpaceInKnapsack)->first);
        knap_data.out(obj);
        remainingSpaceInKnapsack -= knap_data.get_size(obj);
    }

    // returning result
    if (maxPos != objectOnSizeEnd) {
        assert(*maxPos);
        return ReturnType((*maxPos)->second, maxPos - objectOnSizeBegin);
    } else {
        return ReturnType(Value{}, Size{});
    }
}

/**
 * @brief Solution to the knapsack problem
 *
 * @tparam OutputIterator
 * @param objects given objects
 * @param out the result is returned using output iterator
 * @param size functor that for given object returns its size
 * @param value functor that for given object returns its value
 */
template <typename KnapsackData,
          typename ReturnType = typename KnapsackData::return_type,
          typename Size       = typename KnapsackData::size>
ReturnType knapsack(KnapsackData knap_data,
         unbounded_tag, integral_value_tag, retrieve_solution_tag) {
    using ValueType = typename KnapsackData::value;
    using ObjectsIter = typename KnapsackData::object_iter;
    using TableElementType = boost::optional<std::pair<ObjectsIter, ValueType>>;

    auto && objects = knap_data.get_objects();

    if (boost::empty(objects)) {
        return ReturnType{};
    }
    auto maxSize = get_value_bound(knap_data, unbounded_tag{}, upper_tag{});
    auto ret = knapsack_unbounded_dynamic(
            detail::make_knapsack_data(
        knap_data.get_objects(), maxSize, knap_data.get_value(), knap_data.get_size(), knap_data.get_output_iter()),
        get_max_element_on_value_indexed_collection<TableElementType, Size>(
            TableElementType(std::make_pair(ObjectsIter{}, knap_data.get_capacity() + 1))),
        utils::greater{});
    return ReturnType(ret.second, ret.first);
}

/**
 * @brief Solution to the knapsack problem
 *
 * @tparam OutputIterator
 * @param oBegin given objects
 * @param oEnd
 * @param out the result is returned using output iterator
 * @param size functor that for given object returns its size
 * @param value functor that for given object returns its value
 */
template <typename KnapsackData>
typename KnapsackData::return_type
knapsack(KnapsackData knap_data,
         unbounded_tag, integral_size_tag, retrieve_solution_tag) {
    using Value = typename KnapsackData::value;
    return knapsack_unbounded_dynamic(std::move(knap_data),
        detail::get_max_element_on_capacity_indexed_collection<Value>(),
        utils::less{});
}

} // detail

/**
 * @brief Solution to the knapsack problem
 *
 * @tparam Objects
 * @tparam OutputIterator
 * @tparam ObjectSizeFunctor
 * @tparam ObjectValueFunctor
 * @param oBegin given objects
 * @param oEnd
 * @param out the result is returned using output iterator
 * @param size functor that for given object returns its size
 * @param value functor that for given object returns its value
 */
template <typename Objects, typename OutputIterator,
          typename ObjectSizeFunctor,
          typename ObjectValueFunctor = utils::return_one_functor>
typename detail::knapsack_base<Objects, ObjectSizeFunctor,
                               ObjectValueFunctor>::return_type
knapsack_unbounded(Objects && objects,
         detail::FunctorOnRangePValue<ObjectSizeFunctor, Objects>
             capacity, // capacity is of size type
         OutputIterator out, ObjectSizeFunctor size,
         ObjectValueFunctor value = ObjectValueFunctor()) {
    return detail::knapsack_check_integrality(detail::make_knapsack_data(std::forward<Objects>(objects), capacity, size,
                                              value, out), detail::unbounded_tag{},
                                              detail::retrieve_solution_tag());
}

} // paal

#endif // PAAL_KNAPSACK_UNBOUNDED_HPP


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2018 INRIA.
 *
 * 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.
*/

/*!\file NE....cpp
  \brief \ref EMNE_MULTIBODY - C++ input file, Time-Stepping version - O.B.

  A multibody example.
  Direct description of the model.
  Simulation with a Time-Stepping scheme.
*/

#include "SiconosKernel.hpp"
#include "KneeJointR.hpp"
#include "PrismaticJointR.hpp"
#include <boost/math/quaternion.hpp>
using namespace std;

/* Given a position of a point in the Inertial Frame and the configuration vector q of a solid
 * returns a position in the spatial frame.
 */
void fromInertialToSpatialFrame(double *positionInInertialFrame, double *positionInSpatialFrame, SP::SiconosVector  q  )
{
double q0 = q->getValue(3);
double q1 = q->getValue(4);
double q2 = q->getValue(5);
double q3 = q->getValue(6);

::boost::math::quaternion<double>    quatQ(q0, q1, q2, q3);
::boost::math::quaternion<double>    quatcQ(q0, -q1, -q2, -q3);
::boost::math::quaternion<double>    quatpos(0, positionInInertialFrame[0], positionInInertialFrame[1], positionInInertialFrame[2]);
::boost::math::quaternion<double>    quatBuff;

//perform the rotation
quatBuff = quatQ * quatpos * quatcQ;

positionInSpatialFrame[0] = quatBuff.R_component_2()+q->getValue(0);
positionInSpatialFrame[1] = quatBuff.R_component_3()+q->getValue(1);
positionInSpatialFrame[2] = quatBuff.R_component_4()+q->getValue(2);

}
void tipTrajectories(SP::SiconosVector  q, double * traj, double length)
{
  double positionInInertialFrame[3];
  double positionInSpatialFrame[3];
  // Output the position of the tip of beam1
  positionInInertialFrame[0]=length/2;
  positionInInertialFrame[1]=0.0;
  positionInInertialFrame[2]=0.0;

  fromInertialToSpatialFrame(positionInInertialFrame, positionInSpatialFrame, q  );
  traj[0] = positionInSpatialFrame[0];
  traj[1] = positionInSpatialFrame[1];
  traj[2] = positionInSpatialFrame[2];

  // std::cout <<  "positionInSpatialFrame[0]" <<  positionInSpatialFrame[0]<<std::endl;
  // std::cout <<  "positionInSpatialFrame[1]" <<  positionInSpatialFrame[1]<<std::endl;
  // std::cout <<  "positionInSpatialFrame[2]" <<  positionInSpatialFrame[2]<<std::endl;

  positionInInertialFrame[0]=-length/2;
  fromInertialToSpatialFrame(positionInInertialFrame, positionInSpatialFrame, q  );
  traj[3]= positionInSpatialFrame[0];
  traj[4] = positionInSpatialFrame[1];
  traj[5] = positionInSpatialFrame[2];
}

int main(int argc, char* argv[])
{
  try
  {

    // ================= Creation of the model =======================

    // User-defined main parameters
    unsigned int nDof = 3;
    unsigned int qDim = 7;
    unsigned int nDim = 6;
    double t0 = 0;                   // initial computation time
    double T = 10.0;                  // final computation time
    double h = 0.01;                // time step
    int N = 1000;
    double L1 = 1.0;
    double L2 = 1.0;
    double L3 = 1.0;
    double theta = 1.0;              // theta for MoreauJeanOSI integrator
    double g = 9.81; // Gravity
    double m = 1.;

    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    FILE * pFile;
    pFile = fopen("data.h", "w");
    if (pFile == NULL)
    {
      printf("fopen exampleopen filed!\n");
      fclose(pFile);
    }

    cout << "====> Model loading ..." << endl << endl;
    // -- Initial positions and velocities --
    SP::SiconosVector q03(new SiconosVector(qDim));
    SP::SiconosVector v03(new SiconosVector(nDim));
    SP::SimpleMatrix I3(new SimpleMatrix(3, 3));
    v03->zero();
    I3->eye();
    I3->setValue(0, 0, 0.1);
    q03->zero();
    (*q03)(2) = -L1 * sqrt(2.0) - L1 / 2;

    double angle = M_PI / 2;
    SiconosVector V1(3);
    V1.zero();
    V1.setValue(0, 0);
    V1.setValue(1, 1);
    V1.setValue(2, 0);
    q03->setValue(3, cos(angle / 2));
    q03->setValue(4, V1.getValue(0)*sin(angle / 2));
    q03->setValue(5, V1.getValue(1)*sin(angle / 2));
    q03->setValue(6, V1.getValue(2)*sin(angle / 2));

    SP::NewtonEulerDS bouncingbeam(new NewtonEulerDS(q03, v03, m, I3));
    // -- Set external forces (weight) --
    SP::SiconosVector weight3(new SiconosVector(nDof));
    (*weight3)(2) = -m * g;
    bouncingbeam->setFExtPtr(weight3);

    // --------------------
    // --- Interactions ---
    // --------------------

    // Interaction with the floor
    double e = 0.9;
    SP::SimpleMatrix H(new SimpleMatrix(1, qDim));
    SP::SiconosVector eR(new SiconosVector(1));
    eR->setValue(0, 2.3);
    H->zero();
    (*H)(0, 2) = 1.0;
    SP::NonSmoothLaw nslaw0(new NewtonImpactNSL(e));
    SP::NewtonEulerR relation0(new NewtonEulerR());
    relation0->setJachq(H);
    relation0->setE(eR);
    cout << "main jacQH" << endl;
    relation0->jachq()->display();

    // Interactions
    // Building the prismatic joint for bouncingbeam
    // input  - the first concerned DS : bouncingbeam
    //        - an axis in the spatial frame (absolute frame)
    // SP::SimpleMatrix H4(new SimpleMatrix(PrismaticJointR::numberOfConstraints(), qDim));
    // H4->zero();

    SP::SiconosVector axe1(new SiconosVector(3));
    axe1->zero();
    axe1->setValue(2, 1);
    SP::PrismaticJointR relation4(new PrismaticJointR(axe1, false, bouncingbeam));
    SP::NonSmoothLaw nslaw4(new EqualityConditionNSL(relation4->numberOfConstraints()));

    SP::Interaction inter4(new Interaction(nslaw4, relation4));
    SP::Interaction interFloor(new Interaction(nslaw0, relation0));

    // -------------
    // --- Model ---
    // -------------
    SP::NonSmoothDynamicalSystem myModel(new NonSmoothDynamicalSystem(t0, T));
    // add the dynamical system in the non smooth dynamical system
    myModel->insertDynamicalSystem(bouncingbeam);
    // link the interaction and the dynamical system

    myModel->link(inter4, bouncingbeam);
    myModel->link(interFloor, bouncingbeam);
    // ------------------
    // --- Simulation ---
    // ------------------

    // -- (1) OneStepIntegrators --

    SP::MoreauJeanOSI OSI3(new MoreauJeanOSI(theta));

    // -- (2) Time discretisation --
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));

    // -- (3) one step non smooth problem
    SP::OneStepNSProblem osnspb(new MLCP());

    // -- (4) Simulation setup with (1) (2) (3)
    SP::TimeStepping s(new TimeStepping(myModel, t, OSI3, osnspb));
    s->setNewtonTolerance(5e-4);
    s->setNewtonMaxIteration(50);

    // =========================== End of model definition ===========================

    // ================================= Computation =================================

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 15 + 7;
    SimpleMatrix dataPlot(N, outputSize);
    SimpleMatrix bouncingbeamPlot(2,3*N);

    SP::SiconosVector q3 = bouncingbeam->q();
    SP::SiconosVector y= interFloor->y(0);
    SP::SiconosVector ydot= interFloor->y(1);

    // --- Time loop ---
    cout << "====> Start computation ... " << endl << endl;
    // ==== Simulation loop - Writing without explicit event handling =====
    int k = 0;
    boost::progress_display show_progress(N);

    boost::timer time;
    time.restart();
    SP::SiconosVector yAux(new SiconosVector(3));
    yAux->setValue(0, 1);
    SP::SimpleMatrix Jaux(new SimpleMatrix(3, 3));
    Index dimIndex(2);
    Index startIndex(4);
    fprintf(pFile, "double T[%d*%d]={", N + 1, outputSize);
    double beamTipTrajectories[6];

    for (k = 0; k < N; k++)
    {
      // solve ...
      s->advanceToEvent();

      // --- Get values to be plotted ---
      dataPlot(k, 0) =  s->nextTime();

      dataPlot(k, 1) = (*q3)(0);
      dataPlot(k, 2) = (*q3)(1);
      dataPlot(k, 3) = (*q3)(2);
      dataPlot(k, 4) = (*q3)(3);
      dataPlot(k, 5) = (*q3)(4);
      dataPlot(k, 6) = (*q3)(5);
      dataPlot(k, 7) = (*q3)(6);

      dataPlot(k, 8) = y->norm2();
      dataPlot(k, 9) = ydot->norm2();

      tipTrajectories(q3,beamTipTrajectories,L3);
      bouncingbeamPlot(0,3*k) = beamTipTrajectories[0];
      bouncingbeamPlot(0,3*k+1) = beamTipTrajectories[1];
      bouncingbeamPlot(0,3*k+2) = beamTipTrajectories[2];
      bouncingbeamPlot(1,3*k) = beamTipTrajectories[3];
      bouncingbeamPlot(1,3*k+1) = beamTipTrajectories[4];
      bouncingbeamPlot(1,3*k+2) = beamTipTrajectories[5];

      //printf("reaction1:%lf \n", interFloor->lambda(1)->getValue(0));

      for (unsigned int jj = 0; jj < outputSize; jj++)
      {
        if ((k || jj))
          fprintf(pFile, ",");
        fprintf(pFile, "%f", dataPlot(k, jj));
      }
      fprintf(pFile, "\n");
      s->nextStep();
      ++show_progress;
    }
    fprintf(pFile, "};");
    cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
    cout << "Computation Time " << time.elapsed()  << endl;

    // --- Output files ---
    cout << "====> Output file writing ..." << endl;
    ioMatrix::write("NE_BouncingBeam.dat", "ascii", dataPlot, "noDim");
    ioMatrix::write("NE_BouncingBeam_beam.dat", "ascii", bouncingbeamPlot, "noDim");

    double error=0.0, eps=1e-12;
    if ((error=ioMatrix::compareRefFile(dataPlot, "NE_BouncingBeam.ref", eps)) >= 0.0
        && error > eps)
      return 1;

    fclose(pFile);
  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught in NE_...cpp" << endl;
  }

}