// Copyright (c) 2018  Liangliang Nan. All rights reserved.
//
// This file is part of CGAL (www.cgal.org)
//
// $URL: https://github.com/CGAL/cgal/blob/v5.1/Polygonal_surface_reconstruction/include/CGAL/Polygonal_surface_reconstruction.h $
// $Id: Polygonal_surface_reconstruction.h 0779373 2020-03-26T13:31:46+01:00 Sébastien Loriot
// SPDX-License-Identifier: LGPL-3.0-or-later OR LicenseRef-Commercial
//
// Author(s) : Liangliang Nan

#ifndef CGAL_POLYGONAL_SURFACE_RECONSTRUCTION_H
#define CGAL_POLYGONAL_SURFACE_RECONSTRUCTION_H

#include <CGAL/license/Polygonal_surface_reconstruction.h>

#include <CGAL/bounding_box.h>
#include <CGAL/property_map.h>
#include <CGAL/internal/hypothesis.h>
#include <CGAL/internal/compute_confidences.h>
#include <CGAL/internal/point_set_with_planes.h>

#include <unordered_map>

/*!
\file Polygonal_surface_reconstruction.h
*/

namespace CGAL {

        /*!
        \ingroup PkgPolygonalSurfaceReconstruction

        \brief

        Implementation of the Polygonal Surface Reconstruction method.

        Given a set of 3D points with unoriented normals sampled
        from the outer boundary of a piecewise planar object, the Polygonal Surface
        Reconstruction method \cgalCite{nan2017polyfit} outputs a simplified and
        watertight surface mesh interpolating the input point set.

        The method first generates a set of face candidates by intersecting the planar
        primitives. Then an optimal subset of the candidate faces is selected through
        optimization under hard constraints that enforce the final model to be manifold
        and watertight.

        The reconstruction assumes the planar segmentation of the point cloud is
        provided in the input.

        \tparam GeomTraits a geometric traits class, model of Kernel
        */
        template <class GeomTraits>
        class Polygonal_surface_reconstruction
        {
        public:

                /// \name Types

                typedef typename GeomTraits::FT                                FT;                        ///< number type.
                typedef typename GeomTraits::Point_3                Point;                ///< point type.
                typedef typename GeomTraits::Vector_3                Vector;                ///< vector type.
                typedef typename GeomTraits::Plane_3                Plane;                ///< plane type.

        private:

                // Polygon mesh storing the candidate faces
                typedef CGAL::Surface_mesh<Point>                                Polygon_mesh;

                typedef typename Polygon_mesh::Face_index                Face_descriptor;
                typedef typename Polygon_mesh::Vertex_index                Vertex_descriptor;
                typedef typename Polygon_mesh::Edge_index                Edge_descriptor;
                typedef typename Polygon_mesh::Halfedge_index        Halfedge_descriptor;

                // Public methods
        public:

                /// \name Creation

                /*!
                Creates a Polygonal Surface Reconstruction object.
                After construction, candidate faces are generated and point/face confidence values are
                computed, allowing to reuse them in the subsequent reconstruction step with different parameters.

                \tparam PointRange is the range of input points, model of `ConstRange`.
                \tparam PointMap is a model of `ReadablePropertyMap` with value        type `GeomTraits::Point_3`.
                \tparam NormalMap is a model of `ReadablePropertyMap` with value type `GeomTraits::Vector_3`.
                \tparam IndexMap is a model of `ReadablePropertyMap` with value        type `int`.

                \param points range of input points.
                \param point_map property map: value_type of `typename PointRange::const_iterator` -> `Point_3`
                \param normal_map property map: value_type of `typename PointRange::const_iterator` -> `Vector_3`
                \param index_map property map: value_type of `typename PointRange::const_iterator` -> `int`,
                denoting the index of the plane it belongs to (-1 if the point is not assigned to a plane)
                */
                template <
                        typename PointRange,
                        typename PointMap,
                        typename NormalMap,
                        typename IndexMap
                >
                        Polygonal_surface_reconstruction(
                                const PointRange& points,
                                PointMap point_map,
                                NormalMap normal_map,
                                IndexMap index_map
                        );


                /// \name Operations

                /** Reconstructs a watertight polygonal mesh model.

                \tparam MixedIntegerProgramTraits a model of `MixedIntegerProgramTraits`

                \tparam PolygonMesh a model of `MutableFaceGraph`

                \return `true` if the reconstruction succeeded, `false` otherwise.
                */
                template <typename MixedIntegerProgramTraits, typename PolygonMesh>
                bool reconstruct(
                        PolygonMesh& output_mesh,                ///< the final reconstruction result
                        double wt_fitting = 0.43,                ///< weight for the data fitting term.
                        double wt_coverage = 0.27,                ///< weight for the point coverage term.
                        double wt_complexity = 0.30                ///< weight for the model complexity term.
                );

                /*! Gives the user the possibility to access the intermediate candidate faces
                        (i.e., the faces induced by the intersection of the supporting planes).
                \tparam PolygonMesh a model of `MutableFaceGraph`.
                */
                template <typename PolygonMesh>
                void output_candidate_faces(PolygonMesh& candidate_faces) const;

                /// Gets the error message (if reconstruction failed).
                const std::string& error_message() const { return error_message_; }

                // Data members.
        private:
                internal::Hypothesis<GeomTraits> hypothesis_;

                // The generated candidate faces stored as a polygon mesh
                Polygon_mesh        candidate_faces_;

                std::string                error_message_;

        private: // Copying is not allowed
                Polygonal_surface_reconstruction(const Polygonal_surface_reconstruction& psr);

        }; // end of Polygonal_surface_reconstruction


        //////////////////////////////////////////////////////////////////////////s

        // implementations

        template <class GeomTraits>

        template <
                typename PointRange,
                typename PointMap,
                typename NormalMap,
                typename IndexMap
        >
                Polygonal_surface_reconstruction<GeomTraits>::Polygonal_surface_reconstruction(
                        const PointRange& points,
                        PointMap point_map,
                        NormalMap normal_map,
                        IndexMap index_map
                ) : error_message_("")
        {
                if (points.empty()) {
                        error_message_ = "empty input points";
                        return;
                }

                typedef internal::Planar_segment<GeomTraits>                        Planar_segment;
                typedef internal::Point_set_with_planes<GeomTraits>                Point_set_with_planes;

                Point_set_with_planes point_set(points, point_map, normal_map, index_map);

                const std::vector< Planar_segment* >& planar_segments = point_set.planar_segments();
                if (planar_segments.size() < 4) {
                        error_message_ = "at least 4 planes required to reconstruct a closed surface mesh (only "
                                + std::to_string(planar_segments.size()) + " provided)";
                        return;
                }

                hypothesis_.generate(point_set, candidate_faces_);

                typedef internal::Candidate_confidences<GeomTraits>                Candidate_confidences;
                Candidate_confidences conf;
                conf.compute(point_set, candidate_faces_);
        }


        template <class GeomTraits>
        template <typename PolygonMesh>
        void Polygonal_surface_reconstruction<GeomTraits>::output_candidate_faces(PolygonMesh& candidate_faces) const {
                candidate_faces.clear();        // make sure it is empty.
                CGAL::copy_face_graph(candidate_faces_, candidate_faces);
        }


        template <class GeomTraits>
        template <typename MixedIntegerProgramTraits, typename PolygonMesh>
        bool Polygonal_surface_reconstruction<GeomTraits>::reconstruct(
                PolygonMesh& output_mesh,
                double wt_fitting /* = 0.43 */,
                double wt_coverage /* = 0.27 */,
                double wt_complexity /* = 0.30 */)
        {
                if (!error_message_.empty()) { // an error has occurred in the constructor
                        return false;
                }

                if (candidate_faces_.num_faces() < 4) {
                        error_message_ = "at least 4 candidate faces required to reconstruct a closed surface mesh (only "
                                + std::to_string(candidate_faces_.num_faces()) + " computed)";
                        return false;
                }

                typedef typename internal::Hypothesis<GeomTraits>::Adjacency Adjacency;
                const Adjacency& adjacency = hypothesis_.extract_adjacency(candidate_faces_);

                // Internal data structure
                Polygon_mesh target_mesh = candidate_faces_;

                // The number of supporting points of each face
                typename Polygon_mesh::template Property_map<Face_descriptor, std::size_t> face_num_supporting_points =
                        target_mesh.template add_property_map<Face_descriptor, std::size_t>("f:num_supporting_points").first;

                // The area of each face
                typename Polygon_mesh::template Property_map<Face_descriptor, FT> face_areas =
                        target_mesh.template add_property_map<Face_descriptor, FT>("f:face_area").first;

                // The point covered area of each face
                typename Polygon_mesh::template Property_map<Face_descriptor, FT> face_covered_areas =
                        target_mesh.template add_property_map<Face_descriptor, FT>("f:covered_area").first;

                // The supporting plane of each face
                typename Polygon_mesh::template Property_map<Face_descriptor, const Plane*> face_supporting_planes =
                        target_mesh.template add_property_map<Face_descriptor, const Plane*>("f:supp_plane").first;

                // Gives each face an index
                typename Polygon_mesh::template Property_map<Face_descriptor, std::size_t> face_indices =
                        target_mesh.template add_property_map<Face_descriptor, std::size_t>("f:index").first;

                double total_points = 0.0;
                std::size_t idx = 0;
                for(auto f : target_mesh.faces()) {
                        total_points += face_num_supporting_points[f];
                        face_indices[f] = idx;
                        ++idx;
                }


                typedef MixedIntegerProgramTraits                                                                MIP_Solver;
                typedef typename MixedIntegerProgramTraits::Variable                        Variable;
                typedef typename MixedIntegerProgramTraits::Linear_objective        Linear_objective;
                typedef typename MixedIntegerProgramTraits::Linear_constraint        Linear_constraint;

                MIP_Solver solver;

                // Adds variables

                // Binary variables:
                // x[0] ... x[num_faces - 1] : binary labels of all the input faces
                // x[num_faces] ... x[num_faces + num_edges - 1] : binary labels of all the intersecting edges (remain or not)
                // x[num_faces + num_edges] ... x[num_faces + num_edges + num_edges] : binary labels of corner edges (sharp edge of not)

                std::size_t num_faces = target_mesh.number_of_faces();
                std::size_t num_edges(0);

                typedef typename internal::Hypothesis<GeomTraits>::Intersection        Intersection;

                std::unordered_map<const Intersection*, std::size_t> edge_usage_status;        // keep or remove an intersecting edges
                for (std::size_t i = 0; i < adjacency.size(); ++i) {
                        const Intersection& fan = adjacency[i];
                        if (fan.size() == 4) {
                                std::size_t var_idx = num_faces + num_edges;
                                edge_usage_status[&fan] = var_idx;
                                ++num_edges;
                        }
                }

                std::size_t total_variables = num_faces + num_edges + num_edges;

                const std::vector<Variable*>& variables = solver.create_variables(total_variables);
                for (std::size_t i = 0; i < total_variables; ++i) {
                        Variable* v = variables[i];
                        v->set_variable_type(Variable::BINARY);
                }

                // Adds objective

                const typename Polygon_mesh::template Property_map<Vertex_descriptor, Point>& coords = target_mesh.points();
                std::vector<Point> vertices(target_mesh.number_of_vertices());
                idx = 0;
                for(auto v : target_mesh.vertices()) {
                        vertices[idx] = coords[v];
                        ++idx;
                }

                typedef typename GeomTraits::Iso_cuboid_3                Box;

                const Box& box = CGAL::bounding_box(vertices.begin(), vertices.end());
                FT dx = box.xmax() - box.xmin();
                FT dy = box.ymax() - box.ymin();
                FT dz = box.zmax() - box.zmin();
                FT box_area = FT(2.0) * (dx * dy + dy * dz + dz * dx);

                // Chooses a better scale: all actual values multiplied by total number of points
                double coeff_data_fitting = wt_fitting;
                double coeff_coverage = total_points * wt_coverage / box_area;
                double coeff_complexity = total_points * wt_complexity / double(adjacency.size());

                Linear_objective * objective = solver.create_objective(Linear_objective::MINIMIZE);

                std::unordered_map<const Intersection*, std::size_t> edge_sharp_status;        // the edge is sharp or not
                std::size_t num_sharp_edges = 0;
                for (std::size_t i = 0; i < adjacency.size(); ++i) {
                        const Intersection& fan = adjacency[i];
                        if (fan.size() == 4) {
                                std::size_t var_idx = num_faces + num_edges + num_sharp_edges;
                                edge_sharp_status[&fan] = var_idx;

                                // Accumulates model complexity term
                                objective->add_coefficient(variables[var_idx], coeff_complexity);
                                ++num_sharp_edges;
                        }
                }
                CGAL_assertion(num_edges == num_sharp_edges);

                for(auto f : target_mesh.faces()) {
                        std::size_t var_idx = face_indices[f];

                        // Accumulates data fitting term
                        std::size_t num = face_num_supporting_points[f];
                        objective->add_coefficient(variables[var_idx], -coeff_data_fitting * num);

                        // Accumulates model coverage term
                        double uncovered_area = (face_areas[f] - face_covered_areas[f]);
                        objective->add_coefficient(variables[var_idx], coeff_coverage * uncovered_area);
                }

                // Adds constraints: the number of faces associated with an edge must be either 2 or 0
                std::size_t var_edge_used_idx = 0;
                for (std::size_t i = 0; i < adjacency.size(); ++i) {
                        Linear_constraint* c = solver.create_constraint(0.0, 0.0);
                        const Intersection& fan = adjacency[i];
                        for (std::size_t j = 0; j < fan.size(); ++j) {
                                Face_descriptor f = target_mesh.face(fan[j]);
                                std::size_t var_idx = face_indices[f];
                                c->add_coefficient(variables[var_idx], 1.0);
                        }

                        if (fan.size() == 4) {
                                std::size_t var_idx = num_faces + var_edge_used_idx;
                                c->add_coefficient(variables[var_idx], -2.0);  //
                                ++var_edge_used_idx;
                        }
                        else { // boundary edge
                                   // will be set to 0 (i.e., we don't allow open surface)
                        }
                }

                // Adds constraints: for the sharp edges. The explanation of posing this constraint can be found here:
                // https://user-images.githubusercontent.com/15526536/30185644-12085a9c-942b-11e7-831d-290dd2a4d50c.png
                double M = 1.0;
                for (std::size_t i = 0; i < adjacency.size(); ++i) {
                        const Intersection& fan = adjacency[i];
                        if (fan.size() != 4)
                                continue;

                        // If an edge is sharp, the edge must be selected first:
                        // X[var_edge_usage_idx] >= X[var_edge_sharp_idx]
                        Linear_constraint* c = solver.create_constraint(0.0);
                        std::size_t var_edge_usage_idx = edge_usage_status[&fan];
                        c->add_coefficient(variables[var_edge_usage_idx], 1.0);
                        std::size_t var_edge_sharp_idx = edge_sharp_status[&fan];
                        c->add_coefficient(variables[var_edge_sharp_idx], -1.0);

                        for (std::size_t j = 0; j < fan.size(); ++j) {
                                Face_descriptor f1 = target_mesh.face(fan[j]);
                                const Plane* plane1 = face_supporting_planes[f1];
                                std::size_t fid1 = face_indices[f1];
                                for (std::size_t k = j + 1; k < fan.size(); ++k) {
                                        Face_descriptor f2 = target_mesh.face(fan[k]);
                                        const Plane* plane2 = face_supporting_planes[f2];
                                        std::size_t fid2 = face_indices[f2];

                                        if (plane1 != plane2) {
                                                // The constraint is:
                                                //X[var_edge_sharp_idx] + M * (3 - (X[fid1] + X[fid2] + X[var_edge_usage_idx])) >= 1
                                                // which equals to
                                                //X[var_edge_sharp_idx] - M * X[fid1] - M * X[fid2] - M * X[var_edge_usage_idx] >= 1 - 3M
                                                c = solver.create_constraint(1.0 - 3.0 * M);
                                                c->add_coefficient(variables[var_edge_sharp_idx], 1.0);
                                                c->add_coefficient(variables[fid1], -M);
                                                c->add_coefficient(variables[fid2], -M);
                                                c->add_coefficient(variables[var_edge_usage_idx], -M);
                                        }
                                }
                        }
                }

                // Optimization

                if (solver.solve()) {

                        // Marks results
                        const std::vector<double>& X = solver.solution();

                        std::vector<Face_descriptor> to_delete;
                        std::size_t f_idx(0);
                        for(auto f : target_mesh.faces()) {
                                if (static_cast<int>(std::round(X[f_idx])) == 0)
                                        to_delete.push_back(f);
                                ++f_idx;
                        }

                        for (std::size_t i = 0; i < to_delete.size(); ++i) {
                                Face_descriptor f = to_delete[i];
                                Halfedge_descriptor h = target_mesh.halfedge(f);
                                Euler::remove_face(h, target_mesh);
                        }

                        // Marks the sharp edges
                        typename Polygon_mesh::template Property_map<Edge_descriptor, bool> edge_is_sharp =
                                target_mesh.template add_property_map<Edge_descriptor, bool>("e:sharp_edges").first;
                        for (auto e : target_mesh.edges())
                                edge_is_sharp[e] = false;

                        for (std::size_t i = 0; i < adjacency.size(); ++i) {
                                const Intersection& fan = adjacency[i];
                                if (fan.size() != 4)
                                        continue;

                                std::size_t idx_sharp_var = edge_sharp_status[&fan];
                                if (static_cast<int>(X[idx_sharp_var]) == 1) {
                                        for (std::size_t j = 0; j < fan.size(); ++j) {
                                                Halfedge_descriptor h = fan[j];
                                                Face_descriptor f = target_mesh.face(h);
                                                if (f != Polygon_mesh::null_face()) { // some faces may be deleted
                                                        std::size_t fid = face_indices[f];
                                                        if (static_cast<int>(std::round(X[fid])) == 1) {
                                                                Edge_descriptor e = target_mesh.edge(h);
                                                                edge_is_sharp[e] = true;
                                                                break;
                                                        }
                                                }
                                        }
                                }
                        }

                        // Converts from internal data structure to the required `PolygonMesh`.
                        output_mesh.clear();        // make sure it is empty.
                        CGAL::copy_face_graph(target_mesh, output_mesh);
                }
                else {
                        error_message_ = "solving the binary program failed";
                        return false;
                }

                return true;
        }

} //namespace CGAL

#endif // CGAL_POLYGONAL_SURFACE_RECONSTRUCTION_H