//Copyright (c) 2022 Ultimaker B.V.
//CuraEngine is released under the terms of the AGPLv3 or higher.

#include "LinePolygonsCrossings.h"
#include "communication/sliceDataStorage.h"
#include <algorithm>

namespace cura52 {

    LinePolygonsCrossings::Crossing::Crossing(const size_t poly_idx, const coord_t x, const size_t point_idx)
        : poly_idx(poly_idx)
        , x(x)
        , point_idx(point_idx)
    {
    }

    bool LinePolygonsCrossings::calcScanlineCrossings(bool fail_on_unavoidable_obstacles)
    {
        for (unsigned int poly_idx = 0; poly_idx < boundary.size(); poly_idx++)
        {
            ConstPolygonRef poly = boundary[poly_idx];
            Point p0 = transformation_matrix.apply(poly[poly.size() - 1]);
            for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++)
            {
                Point p1 = transformation_matrix.apply(poly[point_idx]);
                if ((p0.Y >= transformed_startPoint.Y && p1.Y <= transformed_startPoint.Y) || (p1.Y >= transformed_startPoint.Y && p0.Y <= transformed_startPoint.Y))
                { // if line segment crosses the line through the transformed start and end point (aka scanline)
                    if (p1.Y == p0.Y) //Line segment is parallel with the scanline. That means that both endpoints lie on the scanline, so they will have intersected with the adjacent line.
                    {
                        p0 = p1;
                        continue;
                    }
                    const coord_t x = p0.X + (p1.X - p0.X) * (transformed_startPoint.Y - p0.Y) / (p1.Y - p0.Y); // intersection point between line segment and the scanline

                    if (x >= transformed_startPoint.X && x <= transformed_endPoint.X)
                    {
                        if (!((p1.Y == transformed_startPoint.Y && p1.Y < p0.Y) || (p0.Y == transformed_startPoint.Y && p0.Y < p1.Y)))
                        { // perform edge case only for line segments on and below the scanline, not for line segments on and above.
                            // \/ will be no crossings and /\ two, but most importantly | will be one crossing.
                            crossings.emplace_back(poly_idx, x, point_idx);
                        }
                    }
                }
                p0 = p1;
            }

            if (fail_on_unavoidable_obstacles && crossings.size() % 2 == 1)
            { // if start area and end area are not the same
                return false;
            }
        }
        // order crossings by increasing x
        std::sort(crossings.begin(), crossings.end(), [](const Crossing& a, const Crossing& b) -> bool { return a.x < b.x; });
        return true;
    }


    bool LinePolygonsCrossings::lineSegmentCollidesWithBoundary()
    {
        Point diff = endPoint - startPoint;

        transformation_matrix = PointMatrix(diff);
        transformed_startPoint = transformation_matrix.apply(startPoint);
        transformed_endPoint = transformation_matrix.apply(endPoint);

        for (ConstPolygonRef poly : boundary)
        {
            Point p0 = transformation_matrix.apply(poly.back());
            for (Point p1_ : poly)
            {
                Point p1 = transformation_matrix.apply(p1_);
                // when the boundary just touches the line don't disambiguate between the boundary moving on to actually cross the line
                // and the boundary bouncing back, resulting in not a real collision - to keep the algorithm simple.
                //
                // disregard overlapping line segments; probably the next or previous line segment is not overlapping, but will give a collision
                // when the boundary line segment fully overlaps with the line segment this edge case is not viewed as a collision
                if (p1.Y != p0.Y && ((p0.Y >= transformed_startPoint.Y && p1.Y <= transformed_startPoint.Y) || (p1.Y >= transformed_startPoint.Y && p0.Y <= transformed_startPoint.Y)))
                {
                    int64_t x = p0.X + (p1.X - p0.X) * (transformed_startPoint.Y - p0.Y) / (p1.Y - p0.Y);

                    if (x > transformed_startPoint.X && x < transformed_endPoint.X)
                    {
                        return true;
                    }
                }
                p0 = p1;
            }
        }

        return false;
    }


    bool LinePolygonsCrossings::generateCombingPath(CombPath& combPath, int64_t max_comb_distance_ignored, bool fail_on_unavoidable_obstacles)
    {
        if (shorterThen(endPoint - startPoint, max_comb_distance_ignored) || !lineSegmentCollidesWithBoundary())
        {
            //We're not crossing any boundaries. So skip the comb generation.
            combPath.push_back(startPoint);
            combPath.push_back(endPoint);
            return true;
        }

        bool success = calcScanlineCrossings(fail_on_unavoidable_obstacles);
        if (!success)
        {
            return false;
        }

        CombPath basicPath;
        generateBasicCombingPath(basicPath);
        optimizePath(basicPath, combPath);
        //    combPath = basicPath; // uncomment to disable comb path optimization
        return true;
    }


    void LinePolygonsCrossings::generateBasicCombingPath(CombPath& combPath)
    {
        // crossings are ordered by increasing x
        for (unsigned i = 0; i < crossings.size(); ++i)
        {
            // find the next crossing that belongs to the same polygon
            for (unsigned j = i + 1; j < crossings.size(); ++j)
            {
                if (crossings[i].poly_idx == crossings[j].poly_idx)
                {
                    // comb between the two crossings
                    generateBasicCombingPath(crossings[i], crossings[j], combPath);
                    // update outer loop variable to skip any crossings on other polygons
                    i = j;
                    break;
                }
            }
        }
        combPath.push_back(endPoint);
    }

    void LinePolygonsCrossings::generateBasicCombingPath(const Crossing& min, const Crossing& max, CombPath& combPath)
    {
        // minimise the path length by measuring the length of both paths around the polygon so we can determine the shorter path

        ConstPolygonRef poly = boundary[min.poly_idx];
        combPath.push_back(transformation_matrix.unapply(Point(min.x - std::abs(dist_to_move_boundary_point_outside), transformed_startPoint.Y)));

        // follow the path in the same direction as the winding order of the boundary polygon
        std::vector<Point> fwd_points;
        Point prev = combPath.back();
        coord_t fwd_len = 0;
        for (unsigned int point_idx = min.point_idx
            ; point_idx != max.point_idx
            ; point_idx = (point_idx < poly.size() - 1) ? (point_idx + 1) : (0))
        {
            const Point p = PolygonUtils::getBoundaryPointWithOffset(poly, point_idx, dist_to_move_boundary_point_outside);
            fwd_points.push_back(p);
            fwd_len += vSize(p - prev);
            prev = p;
        }

        const Point last = transformation_matrix.unapply(Point(max.x + std::abs(dist_to_move_boundary_point_outside), transformed_startPoint.Y));

        if (fwd_points.size() > 0)
        {
            fwd_len += vSize(last - fwd_points.back());
        }

        // follow the path in the opposite direction of the winding order of the boundary polygon
        std::vector<Point> rev_points;
        prev = combPath.back();
        coord_t rev_len = 0;
        unsigned int min_idx = (min.point_idx == 0) ? poly.size() - 1 : min.point_idx - 1;
        unsigned int max_idx = (max.point_idx == 0) ? poly.size() - 1 : max.point_idx - 1;
        for (unsigned int point_idx = min_idx
            ; point_idx != max_idx
            ; point_idx = (point_idx > 0) ? (point_idx - 1) : (poly.size() - 1))
        {
            const Point p = PolygonUtils::getBoundaryPointWithOffset(poly, point_idx, dist_to_move_boundary_point_outside);
            rev_points.push_back(p);
            rev_len += vSize(p - prev);
            prev = p;
            if (rev_len > fwd_len)
            {
                // this path is already longer than the forward path so there's no point in carrying on
                break;
            }
        }

        if (rev_points.size() > 0)
        {
            rev_len += vSize(last - rev_points.back());
        }

        // use the points from the shortest path
        for (auto& p : (fwd_len < rev_len) ? fwd_points : rev_points)
        {
            combPath.push_back(p);
        }
        combPath.push_back(last);
    }

    bool LinePolygonsCrossings::optimizePath(CombPath& comb_path, CombPath& optimized_comb_path)
    {
        optimized_comb_path.push_back(startPoint);
        for (unsigned int point_idx = 1; point_idx < comb_path.size(); point_idx++)
        {
            if (comb_path[point_idx] == comb_path[point_idx - 1]) //Two points are the same. Skip the second.
            {
                continue;
            }
            Point& current_point = optimized_comb_path.back();
            if (PolygonUtils::polygonCollidesWithLineSegment(current_point, comb_path[point_idx], loc_to_line_grid))
            {
                if (PolygonUtils::polygonCollidesWithLineSegment(current_point, comb_path[point_idx - 1], loc_to_line_grid))
                {
                    comb_path.cross_boundary = true;
                }
                // before we add this point, can we discard some of the previous points and still avoid clashing with the combing boundary?
                const int max_short_circuit_len = 1 << 3; // test distances of 8, 4, 2, 1
                for (unsigned n = std::min(max_short_circuit_len, (int)optimized_comb_path.size()); n > 0; n >>= 1)
                {
                    if (optimized_comb_path.size() > n && !PolygonUtils::polygonCollidesWithLineSegment(optimized_comb_path[optimized_comb_path.size() - n - 1], comb_path[point_idx - 1], loc_to_line_grid))
                    {
                        // we can remove n points from the path without it clashing with the combing boundary
                        for (unsigned i = 0; i < n; ++i)
                        {
                            optimized_comb_path.pop_back();
                        }
                        break;
                    }
                }
                optimized_comb_path.push_back(comb_path[point_idx - 1]);

                if (point_idx == 1)
                {
                    // we have just inserted the first point in the path that touches the boundary
                    // and as the first line in the comb is pointing from the start point towards the
                    // end point, we can maybe save some travel by replacing this first point with another
                    // that also lies on the second path of the comb, like this:
                    //
                    //         1
                    //        /|
                    //       / |
                    //      /  |           ===>  ----1
                    //     S   |                 S   |
                    //         |                     |
                    //         2----3 ...            2----3 ...
                    //

                    Point p = optimized_comb_path.back();
                    for (float frac : { 0.9, 0.9, 0.7, 0.5 })
                    {
                        // slide p towards the second point in the comb path
                        p = comb_path[1] + (p - comb_path[1]) * frac;
                        if (!PolygonUtils::polygonCollidesWithLineSegment(startPoint, p, loc_to_line_grid))
                        {
                            // using the new corner doesn't cause a conflict
                            optimized_comb_path.back() = p;
                        }
                        else
                        {
                            // quit loop and keep what we know to be good
                            break;
                        }
                    }
                }
            }
            else
            {
                // : dont add the newest point

                // TODO: add the below extra optimization? (+/- 7% extra computation time, +/- 2% faster print for Dual_extrusion_support_generation.stl)
                while (optimized_comb_path.size() > 1)
                {
                    if (PolygonUtils::polygonCollidesWithLineSegment(optimized_comb_path[optimized_comb_path.size() - 2], comb_path[point_idx], loc_to_line_grid))
                    {
                        break;
                    }
                    else
                    {
                        optimized_comb_path.pop_back();
                    }
                }
            }
        }

        if (optimized_comb_path.size() > 1)
        {
            const unsigned n = optimized_comb_path.size();
            // the penultimate corner may be deleted if the resulting path doesn't conflict with the boundary
            if (!PolygonUtils::polygonCollidesWithLineSegment(optimized_comb_path[n - 2], comb_path.back(), loc_to_line_grid))
            {
                optimized_comb_path.pop_back();
            }
            else {
                // that wasn't possible so try and move the penultimate corner without conficting with the boundary
                // in exactly the same way as we did at the start of the path
                for (float frac : { 0.9, 0.9, 0.7, 0.5 })
                {
                    // make a new point between the penultimate corner and the corner before that
                    Point p = optimized_comb_path[n - 2] + (optimized_comb_path[n - 1] - optimized_comb_path[n - 2]) * frac;
                    if (!PolygonUtils::polygonCollidesWithLineSegment(p, comb_path.back(), loc_to_line_grid))
                    {
                        // using the new corner doesn't cause a conflict
                        optimized_comb_path[n - 1] = p;
                    }
                    else
                    {
                        // quit loop and keep what we know to be good
                        break;
                    }
                }
            }
        }

        optimized_comb_path.push_back(comb_path.back());
        return true;
    }

}//namespace cura52
