#include <chain_process.hpp>

static constexpr auto near_same_dir_epsilon = 10 * std::numeric_limits<double>::epsilon();

void simplify_parametric_chain(pcurve_relation_graph_t& pcurve_relation_graph)
{
    for (auto subface_node : pcurve_relation_graph.nodes<0>()) {
        for (auto edge_to_chain : subface_node.edges<edge_bidirection::to_prev>()) {
            auto& property = edge_to_chain.property();

            // for each subchain, we try to merge continuous edges which have almost same direction
            // hint: subchain cannot have adjacent edges in opposite direction
            for (auto& [vertices, _] : property.subchains) {
                std::vector<Eigen::Vector2d> simplified_vertices{};
                simplified_vertices.reserve(vertices.size());

                // CAUTION: at least start and end vertex should be in the list
                auto vertex_iter = vertices.begin();
                auto prev_vertex = *vertex_iter;
                simplified_vertices.emplace_back(prev_vertex);
                vertex_iter++;
                auto            cur_vertex = *vertex_iter;
                Eigen::Vector2d prev_delta = cur_vertex - prev_vertex;
                prev_vertex                = cur_vertex;
                vertex_iter++;
                for (; vertex_iter != vertices.end(); ++vertex_iter) {
                    auto            cur_vertex = *vertex_iter;
                    Eigen::Vector2d cur_delta  = cur_vertex - prev_vertex;

                    if ((cur_delta - prev_delta).cwiseAbs().minCoeff() > near_same_dir_epsilon)
                        simplified_vertices.emplace_back(prev_vertex);

                    prev_vertex = cur_vertex;
                    prev_delta  = cur_delta;
                }
                simplified_vertices.emplace_back(prev_vertex);

                simplified_vertices.shrink_to_fit();
                std::swap(vertices, simplified_vertices);
            }
        }
    }
}