BambuStudio/src/libslic3r/GCode/SeamPlacer.cpp

#include "SeamPlacer.hpp"

#include "tbb/parallel_for.h"
#include "tbb/blocked_range.h"
#include "tbb/parallel_reduce.h"
#include <boost/log/trivial.hpp>
#include <random>
#include <algorithm>
#include <queue>

#include "libslic3r/AABBTreeLines.hpp"
#include "libslic3r/KDTreeIndirect.hpp"
#include "libslic3r/ExtrusionEntity.hpp"
#include "libslic3r/Print.hpp"
#include "libslic3r/BoundingBox.hpp"
#include "libslic3r/ClipperUtils.hpp"
#include "libslic3r/Layer.hpp"

#include "libslic3r/Geometry/Curves.hpp"
#include "libslic3r/ShortEdgeCollapse.hpp"
#include "libslic3r/TriangleSetSampling.hpp"

#include "libslic3r/Utils.hpp"

//#define DEBUG_FILES

#ifdef DEBUG_FILES
#include <boost/nowide/cstdio.hpp>
#include <SVG.hpp>
#endif

namespace Slic3r {

namespace SeamPlacerImpl {

// ************  FOR BACKPORT COMPATIBILITY ONLY ***************
// Color mapping of a value into RGB false colors.
inline Vec3f value_to_rgbf(float minimum, float maximum, float value)
{
    float ratio = 2.0f * (value - minimum) / (maximum - minimum);
    float b     = std::max(0.0f, (1.0f - ratio));
    float r     = std::max(0.0f, (ratio - 1.0f));
    float g     = 1.0f - b - r;
    return Vec3f{r, g, b};
}

// Color mapping of a value into RGB false colors.
inline Vec3i value_to_rgbi(float minimum, float maximum, float value) { return (value_to_rgbf(minimum, maximum, value) * 255).cast<int>(); }
// ***************************

template<typename T> int sgn(T val) { return int(T(0) < val) - int(val < T(0)); }

// base function: ((e^(((1)/(x^(2)+1)))-1)/(e-1))
// checkout e.g. here: https://www.geogebra.org/calculator
float gauss(float value, float mean_x_coord, float mean_value, float falloff_speed)
{
    float shifted     = value - mean_x_coord;
    float denominator = falloff_speed * shifted * shifted + 1.0f;
    float exponent    = 1.0f / denominator;
    return mean_value * (std::exp(exponent) - 1.0f) / (std::exp(1.0f) - 1.0f);
}

float compute_angle_penalty(float ccw_angle)
{
    // This function is used:
    // ((ℯ^(((1)/(x^(2)*3+1)))-1)/(ℯ-1))*1+((1)/(2+ℯ^(-x)))
    // looks scary, but it is gaussian combined with sigmoid,
    // so that concave points have much smaller penalty over convex ones
    // https://github.com/prusa3d/PrusaSlicer/tree/master/doc/seam_placement/corner_penalty_function.png
    return gauss(ccw_angle, 0.0f, 1.0f, 3.0f) + 1.0f / (2 + std::exp(-ccw_angle));
}

/// Coordinate frame
class Frame
{
public:
    Frame()
    {
        mX = Vec3f(1, 0, 0);
        mY = Vec3f(0, 1, 0);
        mZ = Vec3f(0, 0, 1);
    }

    Frame(const Vec3f &x, const Vec3f &y, const Vec3f &z) : mX(x), mY(y), mZ(z) {}

    void set_from_z(const Vec3f &z)
    {
        mZ         = z.normalized();
        Vec3f tmpZ = mZ;
        Vec3f tmpX = (std::abs(tmpZ.x()) > 0.99f) ? Vec3f(0, 1, 0) : Vec3f(1, 0, 0);
        mY         = (tmpZ.cross(tmpX)).normalized();
        mX         = mY.cross(tmpZ);
    }

    Vec3f to_world(const Vec3f &a) const { return a.x() * mX + a.y() * mY + a.z() * mZ; }

    Vec3f to_local(const Vec3f &a) const { return Vec3f(mX.dot(a), mY.dot(a), mZ.dot(a)); }

    const Vec3f &binormal() const { return mX; }

    const Vec3f &tangent() const { return mY; }

    const Vec3f &normal() const { return mZ; }

private:
    Vec3f mX, mY, mZ;
};

Vec3f sample_sphere_uniform(const Vec2f &samples)
{
    float term1 = 2.0f * float(PI) * samples.x();
    float term2 = 2.0f * sqrt(samples.y() - samples.y() * samples.y());
    return {cos(term1) * term2, sin(term1) * term2, 1.0f - 2.0f * samples.y()};
}

Vec3f sample_hemisphere_uniform(const Vec2f &samples)
{
    float term1 = 2.0f * float(PI) * samples.x();
    float term2 = 2.0f * sqrt(samples.y() - samples.y() * samples.y());
    return {cos(term1) * term2, sin(term1) * term2, abs(1.0f - 2.0f * samples.y())};
}

Vec3f sample_power_cosine_hemisphere(const Vec2f &samples, float power)
{
    float term1 = 2.f * float(PI) * samples.x();
    float term2 = pow(samples.y(), 1.f / (power + 1.f));
    float term3 = sqrt(1.f - term2 * term2);

    return Vec3f(cos(term1) * term3, sin(term1) * term3, term2);
}

std::vector<float> raycast_visibility(const AABBTreeIndirect::Tree<3, float> &raycasting_tree,
                                      const indexed_triangle_set &            triangles,
                                      const TriangleSetSamples &              samples,
                                      size_t                                  negative_volumes_start_index)
{
    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: raycast visibility of " << samples.positions.size() << " samples over " << triangles.indices.size() << " triangles: end";

    // prepare uniform samples of a hemisphere
    float              step_size = 1.0f / SeamPlacer::sqr_rays_per_sample_point;
    std::vector<Vec3f> precomputed_sample_directions(SeamPlacer::sqr_rays_per_sample_point * SeamPlacer::sqr_rays_per_sample_point);
    for (size_t x_idx = 0; x_idx < SeamPlacer::sqr_rays_per_sample_point; ++x_idx) {
        float sample_x = x_idx * step_size + step_size / 2.0;
        for (size_t y_idx = 0; y_idx < SeamPlacer::sqr_rays_per_sample_point; ++y_idx) {
            size_t dir_index                         = x_idx * SeamPlacer::sqr_rays_per_sample_point + y_idx;
            float  sample_y                          = y_idx * step_size + step_size / 2.0;
            precomputed_sample_directions[dir_index] = sample_hemisphere_uniform({sample_x, sample_y});
        }
    }

    bool model_contains_negative_parts = negative_volumes_start_index < triangles.indices.size();

    std::vector<float> result(samples.positions.size());
    tbb::parallel_for(tbb::blocked_range<size_t>(0, result.size()), [&triangles, &precomputed_sample_directions, model_contains_negative_parts, negative_volumes_start_index,
                                                                     &raycasting_tree, &result, &samples](tbb::blocked_range<size_t> r) {
        // Maintaining hits memory outside of the loop, so it does not have to be reallocated for each query.
        std::vector<igl::Hit> hits;
        for (size_t s_idx = r.begin(); s_idx < r.end(); ++s_idx) {
            result[s_idx]                 = 1.0f;
            constexpr float decrease_step = 1.0f / (SeamPlacer::sqr_rays_per_sample_point * SeamPlacer::sqr_rays_per_sample_point);

            const Vec3f &center = samples.positions[s_idx];
            const Vec3f &normal = samples.normals[s_idx];
            // apply the local direction via Frame struct - the local_dir is with respect to +Z being forward
            Frame f;
            f.set_from_z(normal);

            for (const auto &dir : precomputed_sample_directions) {
                Vec3f final_ray_dir = (f.to_world(dir));
                if (!model_contains_negative_parts) {
                    igl::Hit hitpoint;
                    // FIXME: This AABBTTreeIndirect query will not compile for float ray origin and
                    // direction.
                    Vec3d final_ray_dir_d = final_ray_dir.cast<double>();
                    Vec3d ray_origin_d    = (center + normal * 0.01f).cast<double>(); // start above surface.
                    bool  hit = AABBTreeIndirect::intersect_ray_first_hit(triangles.vertices, triangles.indices, raycasting_tree, ray_origin_d, final_ray_dir_d, hitpoint);
                    if (hit && its_face_normal(triangles, hitpoint.id).dot(final_ray_dir) <= 0) { result[s_idx] -= decrease_step; }
                } else { // TODO improve logic for order based boolean operations - consider order of volumes
                    bool casting_from_negative_volume = samples.triangle_indices[s_idx] >= negative_volumes_start_index;

                    Vec3d ray_origin_d = (center + normal * 0.01f).cast<double>(); // start above surface.
                    if (casting_from_negative_volume) {                            // if casting from negative volume face, invert direction, change start pos
                        final_ray_dir = -1.0 * final_ray_dir;
                        ray_origin_d  = (center - normal * 0.01f).cast<double>();
                    }
                    Vec3d final_ray_dir_d = final_ray_dir.cast<double>();
                    bool  some_hit        = AABBTreeIndirect::intersect_ray_all_hits(triangles.vertices, triangles.indices, raycasting_tree, ray_origin_d, final_ray_dir_d, hits);
                    if (some_hit) {
                        int counter = 0;
                        // NOTE: iterating in reverse, from the last hit for one simple reason: We know the state of the ray at that point;
                        //  It cannot be inside model, and it cannot be inside negative volume
                        for (int hit_index = int(hits.size()) - 1; hit_index >= 0; --hit_index) {
                            Vec3f face_normal = its_face_normal(triangles, hits[hit_index].id);
                            if (hits[hit_index].id >= int(negative_volumes_start_index)) { // negative volume hit
                                counter -= sgn(face_normal.dot(final_ray_dir));            // if volume face aligns with ray dir, we are leaving negative space
                                // which in reverse hit analysis means, that we are entering negative space :) and vice versa
                            } else {
                                counter += sgn(face_normal.dot(final_ray_dir));
                            }
                        }
                        if (counter == 0) { result[s_idx] -= decrease_step; }
                    }
                }
            }
        }
    });

    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: raycast visibility of " << samples.positions.size() << " samples over " << triangles.indices.size() << " triangles: end";

    return result;
}

std::vector<float> calculate_polygon_angles_at_vertices(const Polygon &polygon, const std::vector<float> &lengths, float min_arm_length)
{
    std::vector<float> result(polygon.size());

    if (polygon.size() == 1) { result[0] = 0.0f; }

    size_t idx_prev = 0;
    size_t idx_curr = 0;
    size_t idx_next = 0;

    float distance_to_prev = 0;
    float distance_to_next = 0;

    // push idx_prev far enough back as initialization
    while (distance_to_prev < min_arm_length) {
        idx_prev = Slic3r::prev_idx_modulo(idx_prev, polygon.size());
        distance_to_prev += lengths[idx_prev];
    }

    for (size_t _i = 0; _i < polygon.size(); ++_i) {
        // pull idx_prev to current as much as possible, while respecting the min_arm_length
        while (distance_to_prev - lengths[idx_prev] > min_arm_length) {
            distance_to_prev -= lengths[idx_prev];
            idx_prev = Slic3r::next_idx_modulo(idx_prev, polygon.size());
        }

        // push idx_next forward as far as needed
        while (distance_to_next < min_arm_length) {
            distance_to_next += lengths[idx_next];
            idx_next = Slic3r::next_idx_modulo(idx_next, polygon.size());
        }

        // Calculate angle between idx_prev, idx_curr, idx_next.
        const Point &p0  = polygon.points[idx_prev];
        const Point &p1  = polygon.points[idx_curr];
        const Point &p2  = polygon.points[idx_next];
        result[idx_curr] = float(angle(p1 - p0, p2 - p1));

        // increase idx_curr by one
        float curr_distance = lengths[idx_curr];
        idx_curr++;
        distance_to_prev += curr_distance;
        distance_to_next -= curr_distance;
    }

    return result;
}

struct CoordinateFunctor
{
    const std::vector<Vec3f> *coordinates;
    CoordinateFunctor(const std::vector<Vec3f> *coords) : coordinates(coords) {}
    CoordinateFunctor() : coordinates(nullptr) {}

    const float &operator()(size_t idx, size_t dim) const { return coordinates->operator[](idx)[dim]; }
};

// structure to store global information about the model - occlusion hits, enforcers, blockers
struct GlobalModelInfo
{
    TriangleSetSamples                          mesh_samples;
    std::vector<float>                          mesh_samples_visibility;
    CoordinateFunctor                           mesh_samples_coordinate_functor;
    KDTreeIndirect<3, float, CoordinateFunctor> mesh_samples_tree{CoordinateFunctor{}};
    float                                       mesh_samples_radius;

    indexed_triangle_set             enforcers;
    indexed_triangle_set             blockers;
    AABBTreeIndirect::Tree<3, float> enforcers_tree;
    AABBTreeIndirect::Tree<3, float> blockers_tree;

    bool is_enforced(const Vec3f &position, float radius) const
    {
        if (enforcers.empty()) { return false; }
        float radius_sqr = radius * radius;
        return AABBTreeIndirect::is_any_triangle_in_radius(enforcers.vertices, enforcers.indices, enforcers_tree, position, radius_sqr);
    }

    bool is_blocked(const Vec3f &position, float radius) const
    {
        if (blockers.empty()) { return false; }
        float radius_sqr = radius * radius;
        return AABBTreeIndirect::is_any_triangle_in_radius(blockers.vertices, blockers.indices, blockers_tree, position, radius_sqr);
    }

    float calculate_point_visibility(const Vec3f &position) const
    {
        std::vector<size_t> points = find_nearby_points(mesh_samples_tree, position, mesh_samples_radius);
        if (points.empty()) { return 1.0f; }

        auto compute_dist_to_plane = [](const Vec3f &position, const Vec3f &plane_origin, const Vec3f &plane_normal) {
            Vec3f orig_to_point = position - plane_origin;
            return std::abs(orig_to_point.dot(plane_normal));
        };

        float total_weight     = 0;
        float total_visibility = 0;
        for (size_t i = 0; i < points.size(); ++i) {
            size_t sample_idx = points[i];

            Vec3f sample_point  = this->mesh_samples.positions[sample_idx];
            Vec3f sample_normal = this->mesh_samples.normals[sample_idx];

            float weight = mesh_samples_radius - compute_dist_to_plane(position, sample_point, sample_normal);
            weight += (mesh_samples_radius - (position - sample_point).norm());
            total_visibility += weight * mesh_samples_visibility[sample_idx];
            total_weight += weight;
        }

        return total_visibility / total_weight;
    }

#ifdef DEBUG_FILES
    void debug_export(const indexed_triangle_set &obj_mesh) const
    {
        indexed_triangle_set          divided_mesh = obj_mesh;
        Slic3r::CNumericLocalesSetter locales_setter;

        {
            auto  filename = debug_out_path("visiblity.obj");
            FILE *fp       = boost::nowide::fopen(filename.c_str(), "w");
            if (fp == nullptr) {
                BOOST_LOG_TRIVIAL(error) << "stl_write_obj: Couldn't open " << filename << " for writing";
                return;
            }

            for (size_t i = 0; i < divided_mesh.vertices.size(); ++i) {
                float visibility = calculate_point_visibility(divided_mesh.vertices[i]);
                Vec3f color      = value_to_rgbf(0.0f, 1.0f, visibility);
                fprintf(fp, "v %f %f %f  %f %f %f\n", divided_mesh.vertices[i](0), divided_mesh.vertices[i](1), divided_mesh.vertices[i](2), color(0), color(1), color(2));
            }
            for (size_t i = 0; i < divided_mesh.indices.size(); ++i)
                fprintf(fp, "f %d %d %d\n", divided_mesh.indices[i][0] + 1, divided_mesh.indices[i][1] + 1, divided_mesh.indices[i][2] + 1);
            fclose(fp);
        }

        {
            auto  filename = debug_out_path("visiblity_samples.obj");
            FILE *fp       = boost::nowide::fopen(filename.c_str(), "w");
            if (fp == nullptr) {
                BOOST_LOG_TRIVIAL(error) << "stl_write_obj: Couldn't open " << filename << " for writing";
                return;
            }

            for (size_t i = 0; i < mesh_samples.positions.size(); ++i) {
                float visibility = mesh_samples_visibility[i];
                Vec3f color      = value_to_rgbf(0.0f, 1.0f, visibility);
                fprintf(fp, "v %f %f %f  %f %f %f\n", mesh_samples.positions[i](0), mesh_samples.positions[i](1), mesh_samples.positions[i](2), color(0), color(1), color(2));
            }
            fclose(fp);
        }
    }
#endif
};

// Extract perimeter polygons of the given layer
Polygons extract_perimeter_polygons(const Layer *layer, const SeamPosition configured_seam_preference, std::vector<const LayerRegion *> &corresponding_regions_out)
{
    Polygons polygons;
    for (const LayerRegion *layer_region : layer->regions()) {
        for (const ExtrusionEntity *ex_entity : layer_region->perimeters.entities) {
            if (ex_entity->is_collection()) { // collection of inner, outer, and overhang perimeters
                for (const ExtrusionEntity *perimeter : static_cast<const ExtrusionEntityCollection *>(ex_entity)->entities) {
                    ExtrusionRole role = perimeter->role();
                    if (perimeter->is_loop()) {
                        for (const ExtrusionPath &path : static_cast<const ExtrusionLoop *>(perimeter)->paths) {
                            if (path.role() == ExtrusionRole::erExternalPerimeter) { role = ExtrusionRole::erExternalPerimeter; }
                        }
                    }

                    if (role == ExtrusionRole::erExternalPerimeter ||
                        (is_perimeter(role) && configured_seam_preference == spRandom)) { // for random seam alignment, extract all perimeters
                        Points p;
                        perimeter->collect_points(p);
                        polygons.emplace_back(std::move(p));
                        corresponding_regions_out.push_back(layer_region);
                    }
                }
                if (polygons.empty()) {
                    Points p;
                    ex_entity->collect_points(p);
                    polygons.emplace_back(std::move(p));
                    corresponding_regions_out.push_back(layer_region);
                }
            } else {
                Points p;
                ex_entity->collect_points(p);
                polygons.emplace_back(std::move(p));
                corresponding_regions_out.push_back(layer_region);
            }
        }
    }

    if (polygons.empty()) { // If there are no perimeter polygons for whatever reason (disabled perimeters .. ) insert dummy point
        // it is easier than checking everywhere if the layer is not emtpy, no seam will be placed to this layer anyway
        polygons.emplace_back(std::vector{Point{0, 0}});
        corresponding_regions_out.push_back(nullptr);
    }

    return polygons;
}

// Insert SeamCandidates created from perimeter polygons in to the result vector.
// Compute its type (Enfrocer,Blocker), angle, and position
// each SeamCandidate also contains pointer to shared Perimeter structure representing the polygon
// if Custom Seam modifiers are present, oversamples the polygon if necessary to better fit user intentions
void process_perimeter_polygon(
    const Polygon &orig_polygon, float z_coord, const LayerRegion *region, const GlobalModelInfo &global_model_info, PrintObjectSeamData::LayerSeams &result)
{
    if (orig_polygon.size() == 0) { return; }
    Polygon polygon       = orig_polygon;
    bool    was_clockwise = polygon.make_counter_clockwise();
    float   angle_arm_len = region != nullptr ? region->flow(FlowRole::frExternalPerimeter).nozzle_diameter() : 0.5f;

    std::vector<float> lengths{};
    for (size_t point_idx = 0; point_idx < polygon.size() - 1; ++point_idx) { lengths.push_back((unscale(polygon[point_idx]) - unscale(polygon[point_idx + 1])).norm()); }
    lengths.push_back(std::max((unscale(polygon[0]) - unscale(polygon[polygon.size() - 1])).norm(), 0.1));
    std::vector<float> polygon_angles = calculate_polygon_angles_at_vertices(polygon, lengths, angle_arm_len);

    result.perimeters.push_back({});
    Perimeter &perimeter = result.perimeters.back();

    std::queue<Vec3f> orig_polygon_points{};
    for (size_t index = 0; index < polygon.size(); ++index) {
        Vec2f unscaled_p = unscale(polygon[index]).cast<float>();
        orig_polygon_points.emplace(unscaled_p.x(), unscaled_p.y(), z_coord);
    }
    Vec3f             first = orig_polygon_points.front();
    std::queue<Vec3f> oversampled_points{};
    size_t            orig_angle_index = 0;
    perimeter.start_index              = result.points.size();
    perimeter.flow_width               = region != nullptr ? region->flow(FlowRole::frExternalPerimeter).width() : 0.0f;
    bool some_point_enforced           = false;
    while (!orig_polygon_points.empty() || !oversampled_points.empty()) {
        EnforcedBlockedSeamPoint type = EnforcedBlockedSeamPoint::Neutral;
        Vec3f                    position;
        float                    local_ccw_angle = 0;
        bool                     orig_point      = false;
        if (!oversampled_points.empty()) {
            position = oversampled_points.front();
            oversampled_points.pop();
        } else {
            position = orig_polygon_points.front();
            orig_polygon_points.pop();
            local_ccw_angle = was_clockwise ? -polygon_angles[orig_angle_index] : polygon_angles[orig_angle_index];
            orig_angle_index++;
            orig_point = true;
        }

        if (global_model_info.is_enforced(position, perimeter.flow_width)) { type = EnforcedBlockedSeamPoint::Enforced; }

        if (global_model_info.is_blocked(position, perimeter.flow_width)) { type = EnforcedBlockedSeamPoint::Blocked; }
        some_point_enforced = some_point_enforced || type == EnforcedBlockedSeamPoint::Enforced;

        if (orig_point) {
            Vec3f pos_of_next      = orig_polygon_points.empty() ? first : orig_polygon_points.front();
            float distance_to_next = (position - pos_of_next).norm();
            if (global_model_info.is_enforced(position, distance_to_next)) {
                Vec3f vec_to_next = (pos_of_next - position).normalized();
                float step_size   = SeamPlacer::enforcer_oversampling_distance;
                float step        = step_size;
                while (step < distance_to_next) {
                    oversampled_points.push(position + vec_to_next * step);
                    step += step_size;
                }
            }
        }

        result.points.emplace_back(position, perimeter, local_ccw_angle, type);
    }

    perimeter.end_index = result.points.size();

    if (some_point_enforced) {
        // We will patches of enforced points (patch: continuous section of enforced points), choose
        // the longest patch, and select the middle point or sharp point (depending on the angle)
        // this point will have high priority on this perimeter
        size_t     perimeter_size = perimeter.end_index - perimeter.start_index;
        const auto next_index     = [&](size_t idx) { return perimeter.start_index + Slic3r::next_idx_modulo(idx - perimeter.start_index, perimeter_size); };

        std::vector<size_t> patches_starts_ends;
        for (size_t i = perimeter.start_index; i < perimeter.end_index; ++i) {
            if (result.points[i].type != EnforcedBlockedSeamPoint::Enforced && result.points[next_index(i)].type == EnforcedBlockedSeamPoint::Enforced) {
                patches_starts_ends.push_back(next_index(i));
            }
            if (result.points[i].type == EnforcedBlockedSeamPoint::Enforced && result.points[next_index(i)].type != EnforcedBlockedSeamPoint::Enforced) {
                patches_starts_ends.push_back(next_index(i));
            }
        }
        // if patches_starts_ends are empty, it means that the whole perimeter is enforced.. don't do anything in that case
        if (!patches_starts_ends.empty()) {
            // if the first point in the patches is not enforced, it marks a patch end. in that case, put it to the end and start on next
            // to simplify the processing
            assert(patches_starts_ends.size() % 2 == 0);
            bool start_on_second = false;
            if (result.points[patches_starts_ends[0]].type != EnforcedBlockedSeamPoint::Enforced) {
                start_on_second = true;
                patches_starts_ends.push_back(patches_starts_ends[0]);
            }
            // now pick the longest patch
            std::pair<size_t, size_t> longest_patch{0, 0};
            auto                      patch_len = [perimeter_size](const std::pair<size_t, size_t> &start_end) {
                if (start_end.second < start_end.first) {
                    return start_end.first + (perimeter_size - start_end.second);
                } else {
                    return start_end.second - start_end.first;
                }
            };
            for (size_t patch_idx = start_on_second ? 1 : 0; patch_idx < patches_starts_ends.size(); patch_idx += 2) {
                std::pair<size_t, size_t> current_patch{patches_starts_ends[patch_idx], patches_starts_ends[patch_idx + 1]};
                if (patch_len(longest_patch) < patch_len(current_patch)) { longest_patch = current_patch; }
            }
            std::vector<size_t> viable_points_indices;
            std::vector<size_t> large_angle_points_indices;
            for (size_t point_idx = longest_patch.first; point_idx != longest_patch.second; point_idx = next_index(point_idx)) {
                viable_points_indices.push_back(point_idx);
                if (std::abs(result.points[point_idx].local_ccw_angle) > SeamPlacer::sharp_angle_snapping_threshold) { large_angle_points_indices.push_back(point_idx); }
            }
            assert(viable_points_indices.size() > 0);
            if (large_angle_points_indices.empty()) {
                size_t central_idx                          = viable_points_indices[viable_points_indices.size() / 2];
                result.points[central_idx].central_enforcer = true;
            } else {
                size_t central_idx                                                      = large_angle_points_indices.size() / 2;
                result.points[large_angle_points_indices[central_idx]].central_enforcer = true;
            }
        }
    }
}

// Get index of previous and next perimeter point of the layer. Because SeamCandidates of all polygons of the given layer
// are sequentially stored in the vector, each perimeter contains info about start and end index. These vales are used to
// deduce index of previous and next neigbour in the corresponding perimeter.
std::pair<size_t, size_t> find_previous_and_next_perimeter_point(const std::vector<SeamCandidate> &perimeter_points, size_t point_index)
{
    const SeamCandidate &current = perimeter_points[point_index];
    int                  prev    = point_index - 1; // for majority of points, it is true that neighbours lie behind and in front of them in the vector
    int                  next    = point_index + 1;

    if (point_index == current.perimeter.start_index) {
        // if point_index is equal to start, it means that the previous neighbour is at the end
        prev = current.perimeter.end_index;
    }

    if (point_index == current.perimeter.end_index - 1) {
        // if point_index is equal to end, than next neighbour is at the start
        next = current.perimeter.start_index;
    }

    assert(prev >= 0);
    assert(next >= 0);
    return {size_t(prev), size_t(next)};
}

// Computes all global model info - transforms object, performs raycasting
void compute_global_occlusion(GlobalModelInfo &result, const PrintObject *po, std::function<void(void)> throw_if_canceled)
{
    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: gather occlusion meshes: start";
    auto                 obj_transform = po->trafo_centered();
    indexed_triangle_set triangle_set;
    indexed_triangle_set negative_volumes_set;
    // add all parts
    for (const ModelVolume *model_volume : po->model_object()->volumes) {
        if (model_volume->type() == ModelVolumeType::MODEL_PART || model_volume->type() == ModelVolumeType::NEGATIVE_VOLUME) {
            auto                 model_transformation = model_volume->get_matrix();
            indexed_triangle_set model_its            = model_volume->mesh().its;
            its_transform(model_its, model_transformation);
            if (model_volume->type() == ModelVolumeType::MODEL_PART) {
                its_merge(triangle_set, model_its);
            } else {
                its_merge(negative_volumes_set, model_its);
            }
        }
    }
    throw_if_canceled();

    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: gather occlusion meshes: end";

    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: decimate: start";
    its_short_edge_collpase(triangle_set, 25000);
    its_short_edge_collpase(negative_volumes_set, 25000);

    size_t negative_volumes_start_index = triangle_set.indices.size();
    its_merge(triangle_set, negative_volumes_set);
    its_transform(triangle_set, obj_transform);
    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: decimate: end";

    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: Compute visibility sample points: start";

    result.mesh_samples                    = sample_its_uniform_parallel(SeamPlacer::raycasting_visibility_samples_count, triangle_set);
    result.mesh_samples_coordinate_functor = CoordinateFunctor(&result.mesh_samples.positions);
    result.mesh_samples_tree               = KDTreeIndirect<3, float, CoordinateFunctor>(result.mesh_samples_coordinate_functor, result.mesh_samples.positions.size());

    // The following code determines search area for random visibility samples on the mesh when calculating visibility of each perimeter point
    // number of random samples in the given radius (area) is approximately poisson distribution
    // to compute ideal search radius (area), we use exponential distribution (complementary distr to poisson)
    // parameters of exponential distribution to compute area that will have with probability="probability" more than given number of samples="samples"
    float probability = 0.9f;
    float samples     = 4;
    float density     = SeamPlacer::raycasting_visibility_samples_count / result.mesh_samples.total_area;
    // exponential probability distrubtion function is : f(x) = P(X > x) = e^(l*x) where l is the rate parameter (computed as 1/u where u is mean value)
    // probability that sampled area A with S samples contains more than samples count:
    //  P(S > samples in A) = e^-(samples/(density*A));   express A:
    float search_area          = samples / (-logf(probability) * density);
    float search_radius        = sqrt(search_area / PI);
    result.mesh_samples_radius = search_radius;

    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: Compute visiblity sample points: end";
    throw_if_canceled();

    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: Mesh sample raidus: " << result.mesh_samples_radius;

    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: build AABB tree: start";
    auto raycasting_tree = AABBTreeIndirect::build_aabb_tree_over_indexed_triangle_set(triangle_set.vertices, triangle_set.indices);

    throw_if_canceled();
    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: build AABB tree: end";
    result.mesh_samples_visibility = raycast_visibility(raycasting_tree, triangle_set, result.mesh_samples, negative_volumes_start_index);
    throw_if_canceled();
#ifdef DEBUG_FILES
    result.debug_export(triangle_set);
#endif
}

void gather_enforcers_blockers(GlobalModelInfo &result, const PrintObject *po)
{
    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: build AABB trees for raycasting enforcers/blockers: start";

    auto obj_transform = po->trafo_centered();

    for (const ModelVolume *mv : po->model_object()->volumes) {
        if (mv->is_seam_painted()) {
            auto model_transformation = obj_transform * mv->get_matrix();

            indexed_triangle_set enforcers = mv->seam_facets.get_facets(*mv, EnforcerBlockerType::ENFORCER);
            its_transform(enforcers, model_transformation);
            its_merge(result.enforcers, enforcers);

            indexed_triangle_set blockers = mv->seam_facets.get_facets(*mv, EnforcerBlockerType::BLOCKER);
            its_transform(blockers, model_transformation);
            its_merge(result.blockers, blockers);
        }
    }

    result.enforcers_tree = AABBTreeIndirect::build_aabb_tree_over_indexed_triangle_set(result.enforcers.vertices, result.enforcers.indices);
    result.blockers_tree  = AABBTreeIndirect::build_aabb_tree_over_indexed_triangle_set(result.blockers.vertices, result.blockers.indices);

    BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: build AABB trees for raycasting enforcers/blockers: end";
}

struct SeamComparator
{
    SeamPosition setup;
    float        angle_importance;
    explicit SeamComparator(SeamPosition setup) : setup(setup)
    {
        angle_importance = setup == spNearest ? SeamPlacer::angle_importance_nearest : SeamPlacer::angle_importance_aligned;
    }

    // Standard comparator, must respect the requirements of comparators (e.g. give same result on same inputs) for sorting usage
    // should return if a is better seamCandidate than b
    bool is_first_better(const SeamCandidate &a, const SeamCandidate &b, const Vec2f &preffered_location = Vec2f{0.0f, 0.0f}) const
    {
        if (setup == SeamPosition::spAligned && a.central_enforcer != b.central_enforcer) { return a.central_enforcer; }

        // Blockers/Enforcers discrimination, top priority
        if (a.type != b.type) { return a.type > b.type; }

        // avoid overhangs
        if (a.overhang > 0.0f || b.overhang > 0.0f) { return a.overhang < b.overhang; }

        // prefer hidden points (more than 0.5 mm inside)
        if (a.embedded_distance < -0.5f && b.embedded_distance > -0.5f) { return true; }
        if (b.embedded_distance < -0.5f && a.embedded_distance > -0.5f) { return false; }

        if (setup == SeamPosition::spRear && a.position.y() != b.position.y()) { return a.position.y() > b.position.y(); }

        float distance_penalty_a = 0.0f;
        float distance_penalty_b = 0.0f;
        if (setup == spNearest) {
            distance_penalty_a = 1.0f - gauss((a.position.head<2>() - preffered_location).norm(), 0.0f, 1.0f, 0.005f);
            distance_penalty_b = 1.0f - gauss((b.position.head<2>() - preffered_location).norm(), 0.0f, 1.0f, 0.005f);
        }

        // the penalites are kept close to range [0-1.x] however, it should not be relied upon
        float penalty_a = a.overhang + a.visibility + angle_importance * compute_angle_penalty(a.local_ccw_angle) + distance_penalty_a;
        float penalty_b = b.overhang + b.visibility + angle_importance * compute_angle_penalty(b.local_ccw_angle) + distance_penalty_b;

        return penalty_a < penalty_b;
    }

    // Comparator used during alignment. If there is close potential aligned point, it is compared to the current
    // seam point of the perimeter, to find out if the aligned point is not much worse than the current seam
    // Also used by the random seam generator.
    bool is_first_not_much_worse(const SeamCandidate &a, const SeamCandidate &b) const
    {
        // Blockers/Enforcers discrimination, top priority
        if (setup == SeamPosition::spAligned && a.central_enforcer != b.central_enforcer) {
            // Prefer centers of enforcers.
            return a.central_enforcer;
        }

        if (a.type == EnforcedBlockedSeamPoint::Enforced) { return true; }

        if (a.type == EnforcedBlockedSeamPoint::Blocked) { return false; }

        if (a.type != b.type) { return a.type > b.type; }

        // avoid overhangs
        if ((a.overhang > 0.0f || b.overhang > 0.0f) && abs(a.overhang - b.overhang) > (0.1f * a.perimeter.flow_width)) { return a.overhang < b.overhang; }

        // prefer hidden points (more than 0.5 mm inside)
        if (a.embedded_distance < -0.5f && b.embedded_distance > -0.5f) { return true; }
        if (b.embedded_distance < -0.5f && a.embedded_distance > -0.5f) { return false; }

        if (setup == SeamPosition::spRandom) { return true; }

        if (setup == SeamPosition::spRear) { return a.position.y() + SeamPlacer::seam_align_score_tolerance * 5.0f > b.position.y(); }

        float penalty_a = a.overhang + a.visibility + angle_importance * compute_angle_penalty(a.local_ccw_angle);
        float penalty_b = b.overhang + b.visibility + angle_importance * compute_angle_penalty(b.local_ccw_angle);

        return penalty_a <= penalty_b || penalty_a - penalty_b < SeamPlacer::seam_align_score_tolerance;
    }

    bool are_similar(const SeamCandidate &a, const SeamCandidate &b) const { return is_first_not_much_worse(a, b) && is_first_not_much_worse(b, a); }
};

#ifdef DEBUG_FILES
void debug_export_points(const std::vector<PrintObjectSeamData::LayerSeams> &layers, const BoundingBox &bounding_box, const SeamComparator &comparator)
{
    for (size_t layer_idx = 0; layer_idx < layers.size(); ++layer_idx) {
        std::string angles_file_name = debug_out_path(("angles_" + std::to_string(layer_idx) + ".svg").c_str());
        SVG         angles_svg{angles_file_name, bounding_box};
        float       min_vis = 0;
        float       max_vis = min_vis;

        float min_weight = std::numeric_limits<float>::min();
        float max_weight = min_weight;

        for (const SeamCandidate &point : layers[layer_idx].points) {
            Vec3i       color = value_to_rgbi(-PI, PI, point.local_ccw_angle);
            std::string fill  = "rgb(" + std::to_string(color.x()) + "," + std::to_string(color.y()) + "," + std::to_string(color.z()) + ")";
            angles_svg.draw(scaled(Vec2f(point.position.head<2>())), fill);
            min_vis = std::min(min_vis, point.visibility);
            max_vis = std::max(max_vis, point.visibility);

            min_weight = std::min(min_weight, -compute_angle_penalty(point.local_ccw_angle));
            max_weight = std::max(max_weight, -compute_angle_penalty(point.local_ccw_angle));
        }

        std::string visiblity_file_name = debug_out_path(("visibility_" + std::to_string(layer_idx) + ".svg").c_str());
        SVG         visibility_svg{visiblity_file_name, bounding_box};
        std::string weights_file_name = debug_out_path(("weight_" + std::to_string(layer_idx) + ".svg").c_str());
        SVG         weight_svg{weights_file_name, bounding_box};
        std::string overhangs_file_name = debug_out_path(("overhang_" + std::to_string(layer_idx) + ".svg").c_str());
        SVG         overhangs_svg{overhangs_file_name, bounding_box};

        for (const SeamCandidate &point : layers[layer_idx].points) {
            Vec3i       color           = value_to_rgbi(min_vis, max_vis, point.visibility);
            std::string visibility_fill = "rgb(" + std::to_string(color.x()) + "," + std::to_string(color.y()) + "," + std::to_string(color.z()) + ")";
            visibility_svg.draw(scaled(Vec2f(point.position.head<2>())), visibility_fill);

            Vec3i       weight_color = value_to_rgbi(min_weight, max_weight, -compute_angle_penalty(point.local_ccw_angle));
            std::string weight_fill  = "rgb(" + std::to_string(weight_color.x()) + "," + std::to_string(weight_color.y()) + "," + std::to_string(weight_color.z()) + ")";
            weight_svg.draw(scaled(Vec2f(point.position.head<2>())), weight_fill);

            Vec3i       overhang_color = value_to_rgbi(-0.5, 0.5, std::clamp(point.overhang, -0.5f, 0.5f));
            std::string overhang_fill  = "rgb(" + std::to_string(overhang_color.x()) + "," + std::to_string(overhang_color.y()) + "," + std::to_string(overhang_color.z()) + ")";
            overhangs_svg.draw(scaled(Vec2f(point.position.head<2>())), overhang_fill);
        }
    }
}
#endif

// Pick best seam point based on the given comparator
void pick_seam_point(std::vector<SeamCandidate> &perimeter_points, size_t start_index, const SeamComparator &comparator)
{
    size_t end_index = perimeter_points[start_index].perimeter.end_index;

    size_t seam_index = start_index;
    for (size_t index = start_index; index < end_index; ++index) {
        if (comparator.is_first_better(perimeter_points[index], perimeter_points[seam_index])) { seam_index = index; }
    }
    perimeter_points[start_index].perimeter.seam_index = seam_index;
}

size_t pick_nearest_seam_point_index(const std::vector<SeamCandidate> &perimeter_points, size_t start_index, const Vec2f &preffered_location)
{
    size_t         end_index = perimeter_points[start_index].perimeter.end_index;
    SeamComparator comparator{spNearest};

    size_t seam_index = start_index;
    for (size_t index = start_index; index < end_index; ++index) {
        if (comparator.is_first_better(perimeter_points[index], perimeter_points[seam_index], preffered_location)) { seam_index = index; }
    }
    return seam_index;
}

// picks random seam point uniformly, respecting enforcers blockers and overhang avoidance.
void pick_random_seam_point(const std::vector<SeamCandidate> &perimeter_points, size_t start_index)
{
    SeamComparator comparator{spRandom};

    // algorithm keeps a list of viable points and their lengths. If it finds a point
    // that is much better than the viable_example_index (e.g. better type, no overhang; see is_first_not_much_worse)
    // then it throws away stored lists and starts from start
    // in the end, the list should contain points with same type (Enforced > Neutral > Blocked) and also only those which are not
    // big overhang.
    size_t viable_example_index = start_index;
    size_t end_index            = perimeter_points[start_index].perimeter.end_index;
    struct Viable
    {
        // Candidate seam point index.
        size_t index;
        float  edge_length;
        Vec3f  edge;
    };
    std::vector<Viable> viables;

    const Vec3f pseudornd_seed = perimeter_points[viable_example_index].position;
    float       rand           = std::abs(sin(pseudornd_seed.dot(Vec3f(12.9898f, 78.233f, 133.3333f))) * 43758.5453f);
    rand                       = rand - (int) rand;

    for (size_t index = start_index; index < end_index; ++index) {
        if (comparator.are_similar(perimeter_points[index], perimeter_points[viable_example_index])) {
            // index ok, push info into viables
            Vec3f edge_to_next{perimeter_points[index == end_index - 1 ? start_index : index + 1].position - perimeter_points[index].position};
            float dist_to_next = edge_to_next.norm();
            viables.push_back({index, dist_to_next, edge_to_next});
        } else if (comparator.is_first_not_much_worse(perimeter_points[viable_example_index], perimeter_points[index])) {
            // index is worse then viable_example_index, skip this point
        } else {
            // index is better than viable example index, update example, clear gathered info, start again
            // clear up all gathered info, start from scratch, update example index
            viable_example_index = index;
            viables.clear();

            Vec3f edge_to_next = (perimeter_points[index == end_index - 1 ? start_index : index + 1].position - perimeter_points[index].position);
            float dist_to_next = edge_to_next.norm();
            viables.push_back({index, dist_to_next, edge_to_next});
        }
    }

    // now pick random point from the stored options
    float len_sum    = std::accumulate(viables.begin(), viables.end(), 0.0f, [](const float acc, const Viable &v) { return acc + v.edge_length; });
    float picked_len = len_sum * rand;

    size_t point_idx = 0;
    while (picked_len - viables[point_idx].edge_length > 0) {
        picked_len = picked_len - viables[point_idx].edge_length;
        point_idx++;
    }

    Perimeter &perimeter          = perimeter_points[start_index].perimeter;
    perimeter.seam_index          = viables[point_idx].index;
    perimeter.final_seam_position = perimeter_points[perimeter.seam_index].position + viables[point_idx].edge.normalized() * picked_len;
    perimeter.finalized           = true;
}

class PerimeterDistancer
{
    std::vector<Linef>                lines;
    AABBTreeIndirect::Tree<2, double> tree;

public:
    PerimeterDistancer(const Layer *layer)
    {
        ExPolygons layer_outline = layer->lslices;
        for (const ExPolygon &island : layer_outline) {
            assert(island.contour.is_counter_clockwise());
            for (const auto &line : island.contour.lines()) { lines.emplace_back(unscale(line.a), unscale(line.b)); }
            for (const Polygon &hole : island.holes) {
                assert(hole.is_clockwise());
                for (const auto &line : hole.lines()) { lines.emplace_back(unscale(line.a), unscale(line.b)); }
            }
        }
        tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(lines);
    }

    float distance_from_perimeter(const Vec2f &point) const
    {
        Vec2d  p = point.cast<double>();
        size_t hit_idx_out{};
        Vec2d  hit_point_out = Vec2d::Zero();
        auto   distance      = AABBTreeLines::squared_distance_to_indexed_lines(lines, tree, p, hit_idx_out, hit_point_out);
        if (distance < 0) { return std::numeric_limits<float>::max(); }

        distance          = sqrt(distance);
        const Linef &line = lines[hit_idx_out];
        Vec2d        v1   = line.b - line.a;
        Vec2d        v2   = p - line.a;
        if ((v1.x() * v2.y()) - (v1.y() * v2.x()) > 0.0) { distance *= -1; }
        return distance;
    }
};

} // namespace SeamPlacerImpl

// Parallel process and extract each perimeter polygon of the given print object.
// Gather SeamCandidates of each layer into vector and build KDtree over them
// Store results in the SeamPlacer variables m_seam_per_object
void SeamPlacer::gather_seam_candidates(const PrintObject *po, const SeamPlacerImpl::GlobalModelInfo &global_model_info, const SeamPosition configured_seam_preference)
{
    using namespace SeamPlacerImpl;
    PrintObjectSeamData &seam_data = m_seam_per_object.emplace(po, PrintObjectSeamData{}).first->second;
    seam_data.layers.resize(po->layer_count());

    tbb::parallel_for(tbb::blocked_range<size_t>(0, po->layers().size()), [po, configured_seam_preference, &global_model_info, &seam_data](tbb::blocked_range<size_t> r) {
        for (size_t layer_idx = r.begin(); layer_idx < r.end(); ++layer_idx) {
            PrintObjectSeamData::LayerSeams &layer_seams = seam_data.layers[layer_idx];
            const Layer *                    layer       = po->get_layer(layer_idx);
            auto                             unscaled_z  = layer->slice_z;
            std::vector<const LayerRegion *> regions;
            // NOTE corresponding region ptr may be null, if the layer has zero perimeters
            Polygons polygons = extract_perimeter_polygons(layer, configured_seam_preference, regions);
            for (size_t poly_index = 0; poly_index < polygons.size(); ++poly_index) {
                process_perimeter_polygon(polygons[poly_index], unscaled_z, regions[poly_index], global_model_info, layer_seams);
            }
            auto functor                            = SeamCandidateCoordinateFunctor{layer_seams.points};
            seam_data.layers[layer_idx].points_tree = std::make_unique<PrintObjectSeamData::SeamCandidatesTree>(functor, layer_seams.points.size());
        }
    });
}

void SeamPlacer::calculate_candidates_visibility(const PrintObject *po, const SeamPlacerImpl::GlobalModelInfo &global_model_info)
{
    using namespace SeamPlacerImpl;

    std::vector<PrintObjectSeamData::LayerSeams> &layers = m_seam_per_object[po].layers;
    tbb::parallel_for(tbb::blocked_range<size_t>(0, layers.size()), [&layers, &global_model_info](tbb::blocked_range<size_t> r) {
        for (size_t layer_idx = r.begin(); layer_idx < r.end(); ++layer_idx) {
            for (auto &perimeter_point : layers[layer_idx].points) { perimeter_point.visibility = global_model_info.calculate_point_visibility(perimeter_point.position); }
        }
    });
}

void SeamPlacer::calculate_overhangs_and_layer_embedding(const PrintObject *po)
{
    using namespace SeamPlacerImpl;

    std::vector<PrintObjectSeamData::LayerSeams> &layers = m_seam_per_object[po].layers;
    tbb::parallel_for(tbb::blocked_range<size_t>(0, layers.size()), [po, &layers](tbb::blocked_range<size_t> r) {
        std::unique_ptr<PerimeterDistancer> prev_layer_distancer;
        if (r.begin() > 0) { // previous layer exists
            prev_layer_distancer = std::make_unique<PerimeterDistancer>(po->layers()[r.begin() - 1]);
        }

        for (size_t layer_idx = r.begin(); layer_idx < r.end(); ++layer_idx) {
            size_t regions_with_perimeter = 0;
            for (const LayerRegion *region : po->layers()[layer_idx]->regions()) {
                if (region->perimeters.entities.size() > 0) { regions_with_perimeter++; }
            };
            bool                                should_compute_layer_embedding = regions_with_perimeter > 1;
            std::unique_ptr<PerimeterDistancer> current_layer_distancer        = std::make_unique<PerimeterDistancer>(po->layers()[layer_idx]);

            for (SeamCandidate &perimeter_point : layers[layer_idx].points) {
                Vec2f point = Vec2f{perimeter_point.position.head<2>()};
                if (prev_layer_distancer.get() != nullptr) {
                    perimeter_point.overhang = prev_layer_distancer->distance_from_perimeter(point) + 0.6f * perimeter_point.perimeter.flow_width -
                                               tan(SeamPlacer::overhang_angle_threshold) * po->layers()[layer_idx]->height;
                    perimeter_point.overhang = perimeter_point.overhang < 0.0f ? 0.0f : perimeter_point.overhang;
                }

                if (should_compute_layer_embedding) { // search for embedded perimeter points (points hidden inside the print ,e.g. multimaterial join, best position for seam)
                    perimeter_point.embedded_distance = current_layer_distancer->distance_from_perimeter(point) + 0.6f * perimeter_point.perimeter.flow_width;
                }
            }

            prev_layer_distancer.swap(current_layer_distancer);
        }
    });
}

// Estimates, if there is good seam point in the layer_idx which is close to last_point_pos
// uses comparator.is_first_not_much_worse method to compare current seam with the closest point
// (if current seam is too far away )
// If the current chosen stream is close enough, it is stored in seam_string. returns true and updates last_point_pos
// If the closest point is good enough to replace current chosen seam, it is stored in potential_string_seams, returns true and updates last_point_pos
// Otherwise does nothing, returns false
// Used by align_seam_points().
std::optional<std::pair<size_t, size_t>> SeamPlacer::find_next_seam_in_layer(const std::vector<PrintObjectSeamData::LayerSeams> &layers,
                                                                             const Vec3f &                                       projected_position,
                                                                             const size_t                                        layer_idx,
                                                                             const float                                         max_distance,
                                                                             const SeamPlacerImpl::SeamComparator &              comparator) const
{
    using namespace SeamPlacerImpl;
    std::vector<size_t> nearby_points_indices = find_nearby_points(*layers[layer_idx].points_tree, projected_position, max_distance);

    if (nearby_points_indices.empty()) { return {}; }

    size_t best_nearby_point_index = nearby_points_indices[0];
    size_t nearest_point_index     = nearby_points_indices[0];

    // Now find best nearby point, nearest point, and corresponding indices
    for (const size_t &nearby_point_index : nearby_points_indices) {
        const SeamCandidate &point = layers[layer_idx].points[nearby_point_index];
        if (point.perimeter.finalized) {
            continue; // skip over finalized perimeters, try to find some that is not finalized
        }
        if (comparator.is_first_better(point, layers[layer_idx].points[best_nearby_point_index], projected_position.head<2>()) ||
            layers[layer_idx].points[best_nearby_point_index].perimeter.finalized) {
            best_nearby_point_index = nearby_point_index;
        }
        if ((point.position - projected_position).squaredNorm() < (layers[layer_idx].points[nearest_point_index].position - projected_position).squaredNorm() ||
            layers[layer_idx].points[nearest_point_index].perimeter.finalized) {
            nearest_point_index = nearby_point_index;
        }
    }

    const SeamCandidate &best_nearby_point = layers[layer_idx].points[best_nearby_point_index];
    const SeamCandidate &nearest_point     = layers[layer_idx].points[nearest_point_index];

    if (nearest_point.perimeter.finalized) {
        // all points are from already finalized perimeter, skip
        return {};
    }

    // from the nearest_point, deduce index of seam in the next layer
    const SeamCandidate &next_layer_seam = layers[layer_idx].points[nearest_point.perimeter.seam_index];

    // First try to pick central enforcer if any present
    if (next_layer_seam.central_enforcer && (next_layer_seam.position - projected_position).squaredNorm() < sqr(3 * max_distance)) {
        return {std::pair<size_t, size_t>{layer_idx, nearest_point.perimeter.seam_index}};
    }

    // First try to align the nearest, then try the best nearby
    if (comparator.is_first_not_much_worse(nearest_point, next_layer_seam)) { return {std::pair<size_t, size_t>{layer_idx, nearest_point_index}}; }
    // If nearest point is not good enough, try it with the best nearby point.
    if (comparator.is_first_not_much_worse(best_nearby_point, next_layer_seam)) { return {std::pair<size_t, size_t>{layer_idx, best_nearby_point_index}}; }

    return {};
}

std::vector<std::pair<size_t, size_t>> SeamPlacer::find_seam_string(const PrintObject *                   po,
                                                                    std::pair<size_t, size_t>             start_seam,
                                                                    const SeamPlacerImpl::SeamComparator &comparator) const
{
    const std::vector<PrintObjectSeamData::LayerSeams> &layers    = m_seam_per_object.find(po)->second.layers;
    int                                                 layer_idx = start_seam.first;

    // initialize searching for seam string - cluster of nearby seams on previous and next layers
    int                                    next_layer       = layer_idx + 1;
    int                                    step             = 1;
    std::pair<size_t, size_t>              prev_point_index = start_seam;
    std::vector<std::pair<size_t, size_t>> seam_string{start_seam};

    auto reverse_lookup_direction = [&]() {
        step             = -1;
        prev_point_index = start_seam;
        next_layer       = layer_idx - 1;
    };

    while (next_layer >= 0) {
        if (next_layer >= int(layers.size())) {
            reverse_lookup_direction();
            if (next_layer < 0) { break; }
        }
        float max_distance       = SeamPlacer::seam_align_tolerable_dist_factor * layers[start_seam.first].points[start_seam.second].perimeter.flow_width;
        Vec3f prev_position      = layers[prev_point_index.first].points[prev_point_index.second].position;
        Vec3f projected_position = prev_position;
        projected_position.z()   = float(po->get_layer(next_layer)->slice_z);

        std::optional<std::pair<size_t, size_t>> maybe_next_seam = find_next_seam_in_layer(layers, projected_position, next_layer, max_distance, comparator);

        if (maybe_next_seam.has_value()) {
            // For old macOS (pre 10.14), std::optional does not have .value() method, so the code is using operator*() instead.
            seam_string.push_back(maybe_next_seam.operator*());
            prev_point_index = seam_string.back();
            // String added, prev_point_index updated
        } else {
            if (step == 1) {
                reverse_lookup_direction();
                if (next_layer < 0) { break; }
            } else {
                break;
            }
        }
        next_layer += step;
    }
    return seam_string;
}

// clusters already chosen seam points into strings across multiple layers, and then
// aligns the strings via polynomial fit
// Does not change the positions of the SeamCandidates themselves, instead stores
// the new aligned position into the shared Perimeter structure of each perimeter
// Note that this position does not necesarilly lay on the perimeter.
void SeamPlacer::align_seam_points(const PrintObject *po, const SeamPlacerImpl::SeamComparator &comparator)
{
    using namespace SeamPlacerImpl;

    // Prepares Debug files for writing.
#ifdef DEBUG_FILES
    Slic3r::CNumericLocalesSetter locales_setter;
    auto                          clusters_f = debug_out_path("seam_clusters.obj");
    FILE *                        clusters   = boost::nowide::fopen(clusters_f.c_str(), "w");
    if (clusters == nullptr) {
        BOOST_LOG_TRIVIAL(error) << "stl_write_obj: Couldn't open " << clusters_f << " for writing";
        return;
    }
    auto  aligned_f = debug_out_path("aligned_clusters.obj");
    FILE *aligns    = boost::nowide::fopen(aligned_f.c_str(), "w");
    if (aligns == nullptr) {
        BOOST_LOG_TRIVIAL(error) << "stl_write_obj: Couldn't open " << clusters_f << " for writing";
        return;
    }
#endif

    // gather vector of all seams on the print_object - pair of layer_index and seam__index within that layer
    const std::vector<PrintObjectSeamData::LayerSeams> &layers = m_seam_per_object[po].layers;
    std::vector<std::pair<size_t, size_t>>              seams;
    for (size_t layer_idx = 0; layer_idx < layers.size(); ++layer_idx) {
        const std::vector<SeamCandidate> &layer_perimeter_points = layers[layer_idx].points;
        size_t                            current_point_index    = 0;
        while (current_point_index < layer_perimeter_points.size()) {
            seams.emplace_back(layer_idx, layer_perimeter_points[current_point_index].perimeter.seam_index);
            current_point_index = layer_perimeter_points[current_point_index].perimeter.end_index;
        }
    }

    // sort them before alignment. Alignment is sensitive to initializaion, this gives it better chance to choose something nice
    std::stable_sort(seams.begin(), seams.end(), [&comparator, &layers](const std::pair<size_t, size_t> &left, const std::pair<size_t, size_t> &right) {
        return comparator.is_first_better(layers[left.first].points[left.second], layers[right.first].points[right.second]);
    });

    // align the seam points - start with the best, and check if they are aligned, if yes, skip, else start alignment
    // Keeping the vectors outside, so with a bit of luck they will not get reallocated after couple of for loop iterations.
    std::vector<std::pair<size_t, size_t>> seam_string;
    std::vector<std::pair<size_t, size_t>> alternative_seam_string;
    std::vector<Vec2f>                     observations;
    std::vector<float>                     observation_points;
    std::vector<float>                     weights;

    int global_index = 0;
    while (global_index < int(seams.size())) {
        size_t layer_idx  = seams[global_index].first;
        size_t seam_index = seams[global_index].second;
        global_index++;
        const std::vector<SeamCandidate> &layer_perimeter_points = layers[layer_idx].points;
        if (layer_perimeter_points[seam_index].perimeter.finalized) {
            // This perimeter is already aligned, skip seam
            continue;
        } else {
            seam_string      = this->find_seam_string(po, {layer_idx, seam_index}, comparator);
            size_t step_size = 1 + seam_string.size() / 20;
            for (size_t alternative_start = 0; alternative_start < seam_string.size(); alternative_start += step_size) {
                size_t start_layer_idx  = seam_string[alternative_start].first;
                size_t seam_idx         = layers[start_layer_idx].points[seam_string[alternative_start].second].perimeter.seam_index;
                alternative_seam_string = this->find_seam_string(po, std::pair<size_t, size_t>(start_layer_idx, seam_idx), comparator);
                if (alternative_seam_string.size() > seam_string.size()) { seam_string = std::move(alternative_seam_string); }
            }
            if (seam_string.size() < seam_align_minimum_string_seams) {
                // string NOT long enough to be worth aligning, skip
                continue;
            }

            // String is long enough, all string seams and potential string seams gathered, now do the alignment
            // sort by layer index
            std::sort(seam_string.begin(), seam_string.end(),
                      [](const std::pair<size_t, size_t> &left, const std::pair<size_t, size_t> &right) { return left.first < right.first; });

            // repeat the alignment for the current seam, since it could be skipped due to alternative path being aligned.
            global_index--;

            // gather all positions of seams and their weights
            observations.resize(seam_string.size());
            observation_points.resize(seam_string.size());
            weights.resize(seam_string.size());

            auto angle_3d = [](const Vec3f &a, const Vec3f &b) { return std::abs(acosf(a.normalized().dot(b.normalized()))); };

            auto angle_weight = [](float angle) { return 1.0f / (0.1f + compute_angle_penalty(angle)); };

            // gather points positions and weights
            float total_length   = 0.0f;
            Vec3f last_point_pos = layers[seam_string[0].first].points[seam_string[0].second].position;
            for (size_t index = 0; index < seam_string.size(); ++index) {
                const SeamCandidate &current     = layers[seam_string[index].first].points[seam_string[index].second];
                float                layer_angle = 0.0f;
                if (index > 0 && index < seam_string.size() - 1) {
                    layer_angle = angle_3d(current.position - layers[seam_string[index - 1].first].points[seam_string[index - 1].second].position,
                                           layers[seam_string[index + 1].first].points[seam_string[index + 1].second].position - current.position);
                }
                observations[index]       = current.position.head<2>();
                observation_points[index] = current.position.z();
                weights[index]            = angle_weight(current.local_ccw_angle);
                float sign                = layer_angle > 2.0 * std::abs(current.local_ccw_angle) ? -0.8f : 1.0f;
                if (current.type == EnforcedBlockedSeamPoint::Enforced) {
                    sign = 1.0f;
                    weights[index] += 3.0f;
                }
                total_length += sign * (last_point_pos - current.position).norm();
                last_point_pos = current.position;
            }

            // Curve Fitting
            size_t number_of_segments = std::max(size_t(1), size_t(std::max(0.0f, total_length) / SeamPlacer::seam_align_mm_per_segment));
            auto   curve              = Geometry::fit_cubic_bspline(observations, observation_points, weights, number_of_segments);

            // Do alignment - compute fitted point for each point in the string from its Z coord, and store the position into
            // Perimeter structure of the point; also set flag aligned to true
            for (size_t index = 0; index < seam_string.size(); ++index) {
                const auto &pair = seam_string[index];
                float       t    = std::min(1.0f, std::pow(std::abs(layers[pair.first].points[pair.second].local_ccw_angle) / SeamPlacer::sharp_angle_snapping_threshold, 3.0f));
                if (layers[pair.first].points[pair.second].type == EnforcedBlockedSeamPoint::Enforced) { t = std::max(0.4f, t); }

                Vec3f current_pos = layers[pair.first].points[pair.second].position;
                Vec2f fitted_pos  = curve.get_fitted_value(current_pos.z());

                // interpolate between current and fitted position, prefer current pos for large weights.
                Vec3f final_position = t * current_pos + (1.0f - t) * to_3d(fitted_pos, current_pos.z());

                Perimeter &perimeter          = layers[pair.first].points[pair.second].perimeter;
                perimeter.seam_index          = pair.second;
                perimeter.final_seam_position = final_position;
                perimeter.finalized           = true;
            }

#ifdef DEBUG_FILES
            auto  randf = []() { return float(rand()) / float(RAND_MAX); };
            Vec3f color{randf(), randf(), randf()};
            for (size_t i = 0; i < seam_string.size(); ++i) {
                auto orig_seam = layers[seam_string[i].first].points[seam_string[i].second];
                fprintf(clusters, "v %f %f %f %f %f %f \n", orig_seam.position[0], orig_seam.position[1], orig_seam.position[2], color[0], color[1], color[2]);
            }

            color = Vec3f{randf(), randf(), randf()};
            for (size_t i = 0; i < seam_string.size(); ++i) {
                const Perimeter &perimeter = layers[seam_string[i].first].points[seam_string[i].second].perimeter;
                fprintf(aligns, "v %f %f %f %f %f %f \n", perimeter.final_seam_position[0], perimeter.final_seam_position[1], perimeter.final_seam_position[2], color[0],
                        color[1], color[2]);
            }
#endif
        }
    }

#ifdef DEBUG_FILES
    fclose(clusters);
    fclose(aligns);
#endif
}

void SeamPlacer::init(const Print &print, std::function<void(void)> throw_if_canceled_func)
{
    using namespace SeamPlacerImpl;
    m_seam_per_object.clear();

    for (const PrintObject *po : print.objects()) {
        throw_if_canceled_func();
        SeamPosition   configured_seam_preference = po->config().seam_position.value;
        SeamComparator comparator{configured_seam_preference};

        {
            GlobalModelInfo global_model_info{};
            gather_enforcers_blockers(global_model_info, po);
            throw_if_canceled_func();
            if (configured_seam_preference == spAligned || configured_seam_preference == spNearest) { compute_global_occlusion(global_model_info, po, throw_if_canceled_func); }
            throw_if_canceled_func();
            BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: gather_seam_candidates: start";
            gather_seam_candidates(po, global_model_info, configured_seam_preference);
            BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: gather_seam_candidates: end";
            throw_if_canceled_func();
            if (configured_seam_preference == spAligned || configured_seam_preference == spNearest) {
                BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: calculate_candidates_visibility : start";
                calculate_candidates_visibility(po, global_model_info);
                BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: calculate_candidates_visibility : end";
            }
        } // destruction of global_model_info (large structure, no longer needed)
        throw_if_canceled_func();
        BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: calculate_overhangs and layer embdedding : start";
        calculate_overhangs_and_layer_embedding(po);
        BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: calculate_overhangs and layer embdedding: end";
        throw_if_canceled_func();
        if (configured_seam_preference != spNearest) { // For spNearest, the seam is picked in the place_seam method with actual nozzle position information
            BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: pick_seam_point : start";
            // pick seam point
            std::vector<PrintObjectSeamData::LayerSeams> &layers = m_seam_per_object[po].layers;
            tbb::parallel_for(tbb::blocked_range<size_t>(0, layers.size()), [&layers, configured_seam_preference, comparator](tbb::blocked_range<size_t> r) {
                for (size_t layer_idx = r.begin(); layer_idx < r.end(); ++layer_idx) {
                    std::vector<SeamCandidate> &layer_perimeter_points = layers[layer_idx].points;
                    for (size_t current = 0; current < layer_perimeter_points.size(); current = layer_perimeter_points[current].perimeter.end_index)
                        if (configured_seam_preference == spRandom)
                            pick_random_seam_point(layer_perimeter_points, current);
                        else
                            pick_seam_point(layer_perimeter_points, current, comparator);
                }
            });
            BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: pick_seam_point : end";
        }
        throw_if_canceled_func();
        if (configured_seam_preference == spAligned || configured_seam_preference == spRear) {
            BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: align_seam_points : start";
            align_seam_points(po, comparator);
            BOOST_LOG_TRIVIAL(debug) << "SeamPlacer: align_seam_points : end";
        }

#ifdef DEBUG_FILES
        debug_export_points(m_seam_per_object[po].layers, po->bounding_box(), comparator);
#endif
    }
}

void SeamPlacer::place_seam(const Layer *layer, ExtrusionLoop &loop, bool external_first, const Point &last_pos) const
{
    using namespace SeamPlacerImpl;
    const PrintObject *po = layer->object();
    // Must not be called with supprot layer.
    assert(dynamic_cast<const SupportLayer *>(layer) == nullptr);
    // Object layer IDs are incremented by the number of raft layers.
    assert(layer->id() >= po->slicing_parameters().raft_layers());
    const size_t layer_index = layer->id() - po->slicing_parameters().raft_layers();
    const double unscaled_z  = layer->slice_z;

    const PrintObjectSeamData::LayerSeams &layer_perimeters = m_seam_per_object.find(layer->object())->second.layers[layer_index];

    // Find the closest perimeter in the SeamPlacer to the first point of this loop.
    size_t closest_perimeter_point_index;
    {
        const Point &fp               = loop.first_point();
        Vec2f        unscaled_p       = unscaled<float>(fp);
        closest_perimeter_point_index = find_closest_point(*layer_perimeters.points_tree.get(), to_3d(unscaled_p, float(unscaled_z)));
    }

    Vec3f  seam_position;
    size_t seam_index;
    if (const Perimeter &perimeter = layer_perimeters.points[closest_perimeter_point_index].perimeter; perimeter.finalized) {
        seam_position = perimeter.final_seam_position;
        seam_index    = perimeter.seam_index;
    } else {
        seam_index    = po->config().seam_position == spNearest ? pick_nearest_seam_point_index(layer_perimeters.points, perimeter.start_index, unscaled<float>(last_pos)) :
                                                                  perimeter.seam_index;
        seam_position = layer_perimeters.points[seam_index].position;
    }

    Point seam_point = Point::new_scale(seam_position.x(), seam_position.y());

    if (const SeamCandidate &perimeter_point = layer_perimeters.points[seam_index];
        (po->config().seam_position == spNearest || po->config().seam_position == spAligned) && loop.role() == ExtrusionRole::erPerimeter && // Hopefully internal perimeter
        (seam_position - perimeter_point.position).squaredNorm() < 4.0f &&                                                                   // seam is on perimeter point
        perimeter_point.local_ccw_angle < -EPSILON                                                                                           // In concave angles
    ) { // In this case, we are at internal perimeter, where the external perimeter has seam in concave angle. We want to align
        // the internal seam into the concave corner, and not on the perpendicular projection on the closest edge (which is what the split_at function does)
        size_t index_of_prev = seam_index == perimeter_point.perimeter.start_index ? perimeter_point.perimeter.end_index - 1 : seam_index - 1;
        size_t index_of_next = seam_index == perimeter_point.perimeter.end_index - 1 ? perimeter_point.perimeter.start_index : seam_index + 1;

        Vec2f dir_to_middle = ((perimeter_point.position - layer_perimeters.points[index_of_prev].position).head<2>().normalized() +
                               (perimeter_point.position - layer_perimeters.points[index_of_next].position).head<2>().normalized()) *
                              0.5;

        ExtrusionLoop::ClosestPathPoint projected_point = loop.get_closest_path_and_point(seam_point, true);
        // get closest projected point, determine depth of the seam point.
        float depth        = (float) unscale(Point(seam_point - projected_point.foot_pt)).norm();
        float angle_factor = cos(-perimeter_point.local_ccw_angle / 2.0f); // There are some nice geometric identities in determination of the correct depth of new seam point.
        // overshoot the target depth, in concave angles it will correctly snap to the corner; TODO: find out why such big overshoot is needed.
        Vec2f final_pos = perimeter_point.position.head<2>() + (1.4142 * depth / angle_factor) * dir_to_middle;
        seam_point      = Point::new_scale(final_pos.x(), final_pos.y());
    }

    // Because the G-code export has 1um resolution, don't generate segments shorter than 1.5 microns,
    // thus empty path segments will not be produced by G-code export.
    if (!loop.split_at_vertex(seam_point, scaled<double>(0.0015))) {
        // The point is not in the original loop.
        // Insert it.
        loop.split_at(seam_point, true);
    }
}

} // namespace Slic3r