#include "vde/core/cam_mesh.h"
#include "vde/curves/nurbs_curve.h"
#include "vde/core/aabb.h"
#include <cmath>
#include <cstdio>
#include <algorithm>
#include <limits>
#include <map>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace vde::core {

// ── Internal helpers ─────────────────────────────────────────────────────

namespace {

/// Format a double to 4 decimal places
std::string fmt(double v) {
    char buf[32];
    std::snprintf(buf, sizeof(buf), "%.4f", v);
    return buf;
}

/// 2D cross product
inline double cross2d(const Vector2D& a, const Vector2D& b) {
    return a.x() * b.y() - a.y() * b.x();
}

/// Segment-segment intersection (2D)
bool segment_intersect(const Point2D& a1, const Point2D& a2,
                       const Point2D& b1, const Point2D& b2, Point2D& out) {
    Vector2D da = a2 - a1;
    Vector2D db = b2 - b1;
    double cross = cross2d(da, db);
    if (std::abs(cross) < 1e-12) return false;
    Vector2D dc = b1 - a1;
    double t = cross2d(dc, db) / cross;
    double u = cross2d(dc, da) / cross;
    if (t >= -1e-12 && t <= 1.0 + 1e-12 && u >= -1e-12 && u <= 1.0 + 1e-12) {
        out = a1 + t * da;
        return true;
    }
    return false;
}

/// Ray-casting point-in-polygon (2D)
bool point_in_polygon(const std::vector<Point2D>& poly, const Point2D& p) {
    int n = static_cast<int>(poly.size());
    if (n < 3) return false;
    bool inside = false;
    for (int i = 0, j = n - 1; i < n; j = i++) {
        const auto& pi = poly[i];
        const auto& pj = poly[j];
        if (((pi.y() > p.y()) != (pj.y() > p.y())) &&
            (p.x() < (pj.x() - pi.x()) * (p.y() - pi.y()) / (pj.y() - pi.y()) + pi.x())) {
            inside = !inside;
        }
    }
    return inside;
}

/// Fit a clamped NURBS curve through given control points
curves::NurbsCurve fit_clamped(const std::vector<Point3D>& pts) {
    int n = static_cast<int>(pts.size());
    if (n < 2) {
        return curves::NurbsCurve({pts[0], pts[0]}, {0,0,1,1}, {1,1}, 1);
    }
    int p = (n <= 2) ? 1 : std::min(3, n - 1);
    std::vector<double> weights(n, 1.0);
    std::vector<double> knots(n + p + 1);
    for (int i = 0; i <= p; ++i)      knots[i] = 0.0;
    for (int i = n; i < n + p + 1; ++i) knots[i] = 1.0;
    int interior = n - p - 1;
    if (interior > 0) {
        for (int i = 0; i <= interior; ++i) {
            knots[p + i] = static_cast<double>(i) / static_cast<double>(interior + 1);
        }
    }
    return curves::NurbsCurve(pts, std::move(knots), std::move(weights), p);
}

/// Sample a NURBS curve at `count` evenly-spaced parameter values
std::vector<Point3D> sample_curve(const curves::NurbsCurve& curve, int count) {
    std::vector<Point3D> pts;
    pts.reserve(count);
    auto [t0, t1] = curve.domain();
    for (int i = 0; i < count; ++i) {
        double t = t0 + (t1 - t0) * static_cast<double>(i) / static_cast<double>(count - 1);
        pts.push_back(curve.evaluate(t));
    }
    return pts;
}

/// Rotate a 2D point around origin by angle (radians)
Point2D rotate_2d(const Point2D& p, double angle_rad) {
    double c = std::cos(angle_rad);
    double s = std::sin(angle_rad);
    return Point2D(c * p.x() - s * p.y(), s * p.x() + c * p.y());
}

}  // anonymous namespace

// ═══════════════════════════════════════════════════════════════════════════
//  extract_contour_from_mesh — 截面轮廓提取
// ═══════════════════════════════════════════════════════════════════════════

std::vector<curves::NurbsCurve> extract_contour_from_mesh(
    const mesh::HalfedgeMesh& mesh,
    double z_height)
{
    const double kEps = 1e-10;

    // ── Step 1: Collect intersection segments ────────────────────────
    // Each (p0, p1) represents a segment where a triangular face intersects
    // the z-plane.  A triangle intersecting the plane yields exactly 2 edge-hit
    // points (general position).
    struct Seg {
        Point2D p0, p1;
    };
    std::vector<Seg> segs;

    for (size_t fi = 0; fi < mesh.num_faces(); ++fi) {
        auto fv = mesh.face_vertices(static_cast<int>(fi));
        if (fv.size() < 3) continue;

        std::vector<Point2D> hits;  // 2D intersection points on this face

        for (size_t j = 0; j < fv.size(); ++j) {
            size_t k = (j + 1) % fv.size();
            Point3D p0 = mesh.vertex(static_cast<size_t>(fv[j]));
            Point3D p1 = mesh.vertex(static_cast<size_t>(fv[k]));

            double z0 = p0.z();
            double z1 = p1.z();

            // Check if edge straddles the z plane
            if ((z0 > z_height && z1 < z_height) ||
                (z0 < z_height && z1 > z_height)) {
                double t = (z_height - z0) / (z1 - z0);
                if (t >= 0.0 && t <= 1.0) {
                    double x = p0.x() + t * (p1.x() - p0.x());
                    double y = p0.y() + t * (p1.y() - p0.y());
                    hits.emplace_back(x, y);
                }
            }
            // Edge lying exactly on the plane — take the whole edge
            else if (std::abs(z0 - z_height) < kEps && std::abs(z1 - z_height) < kEps) {
                hits.emplace_back(p0.x(), p0.y());
                hits.emplace_back(p1.x(), p1.y());
            }
        }

        // A triangle intersecting the plane should give exactly 2 points
        // (unless the plane passes through a vertex or edge — those are degenerate)
        if (hits.size() == 2) {
            segs.push_back({hits[0], hits[1]});
        } else if (hits.size() > 2) {
            // Multiple hits: pair them up to form segments
            // (e.g. plane passes through a vertex)
            for (size_t a = 0; a + 1 < hits.size(); a += 2) {
                segs.push_back({hits[a], hits[a + 1]});
            }
        }
    }

    if (segs.size() < 3) return {};

    // ── Step 2: Chain segments into closed loops ─────────────────────
    const double kMergeTol = 1e-8;
    std::vector<std::vector<Point2D>> loops;

    while (!segs.empty()) {
        std::vector<Point2D> loop;
        Seg seed = segs.back();
        segs.pop_back();

        loop.push_back(seed.p0);
        Point2D target = seed.p1;

        bool growing = true;
        while (growing && !segs.empty()) {
            // Check if we've closed the loop
            if ((target - loop.front()).norm() < kMergeTol * 10.0) {
                growing = false;
                break;
            }

            growing = false;
            for (auto it = segs.begin(); it != segs.end(); ++it) {
                if ((target - it->p0).norm() < kMergeTol) {
                    loop.push_back(it->p0);
                    target = it->p1;
                    segs.erase(it);
                    growing = true;
                    break;
                }
                if ((target - it->p1).norm() < kMergeTol) {
                    loop.push_back(it->p1);
                    target = it->p0;
                    segs.erase(it);
                    growing = true;
                    break;
                }
            }
        }

        // Close the loop
        loop.push_back(loop.front());

        if (loop.size() >= 4) {  // at least 3 unique points + closure
            loops.push_back(std::move(loop));
        }
    }

    // ── Step 3: Fit NURBS curves ─────────────────────────────────────
    std::vector<curves::NurbsCurve> result;
    result.reserve(loops.size());

    for (const auto& loop : loops) {
        std::vector<Point3D> pts3d;
        pts3d.reserve(loop.size());
        for (const auto& p : loop) {
            pts3d.emplace_back(p.x(), p.y(), z_height);
        }
        result.push_back(fit_clamped(pts3d));
    }

    return result;
}

// ═══════════════════════════════════════════════════════════════════════════
//  contour_toolpath (mesh) — 轮廓加工刀路
// ═══════════════════════════════════════════════════════════════════════════

Toolpath contour_toolpath(
    const mesh::HalfedgeMesh& mesh,
    double z_height,
    const Tool& /*tool*/,
    const ContourParams& params)
{
    Toolpath tp;
    tp.name = "Contour_Mesh";
    tp.safe_z = params.safe_z;
    tp.cut_z = z_height;
    tp.step_down = params.step_down;

    // Offset the cutting plane by stock_to_leave (cut slightly outside)
    double effective_z = z_height + params.stock_to_leave;

    auto contours = extract_contour_from_mesh(mesh, effective_z);
    if (contours.empty()) return tp;

    double effective_step = params.step_down;
    if (effective_step <= 0.0) effective_step = 1.0;

    // Compute Z range for multi-pass
    double z_min = std::numeric_limits<double>::max();
    double z_max = std::numeric_limits<double>::lowest();
    for (size_t vi = 0; vi < mesh.num_vertices(); ++vi) {
        auto v = mesh.vertex(static_cast<int>(vi));
        z_min = std::min(z_min, v.z());
        z_max = std::max(z_max, v.z());
    }

    // Cut from safe_z level down to z_height in step_down increments
    double start_z = z_max;
    int pass_count = 0;

    // Rapid to safe position
    tp.segments.push_back({Point3D(0, 0, params.safe_z),
                           Point3D(0, 0, params.safe_z),
                           Point3D::Zero(), PathSegmentType::Rapid});

    // For contouring, we cut at the specified z_height (can be multi-pass)
    while (start_z > z_height + 1e-9) {
        double cut_z = std::max(z_height, start_z - effective_step);

        for (size_t ci = 0; ci < contours.size(); ++ci) {
            auto sampled = sample_curve(contours[ci], 200);
            if (sampled.size() < 2) continue;

            // First contour of first pass: rapid approach
            if (pass_count == 0 && ci == 0) {
                tp.segments.push_back({Point3D(0, 0, params.safe_z),
                                       Point3D(sampled[0].x(), sampled[0].y(), params.safe_z),
                                       Point3D::Zero(), PathSegmentType::Rapid});

                // Plunge to first cut depth
                tp.segments.push_back({Point3D(sampled[0].x(), sampled[0].y(), params.safe_z),
                                       Point3D(sampled[0].x(), sampled[0].y(), cut_z),
                                       Point3D::Zero(), PathSegmentType::Linear,
                                       params.feed_rate * 0.5, cut_z});
            } else {
                // Move to start of contour at safe Z then plunge
                tp.segments.push_back({tp.segments.back().end,
                                       Point3D(sampled[0].x(), sampled[0].y(), params.safe_z),
                                       Point3D::Zero(), PathSegmentType::Rapid});

                tp.segments.push_back({Point3D(sampled[0].x(), sampled[0].y(), params.safe_z),
                                       Point3D(sampled[0].x(), sampled[0].y(), cut_z),
                                       Point3D::Zero(), PathSegmentType::Linear,
                                       params.feed_rate * 0.5, cut_z});
            }

            // Follow the contour at this Z
            for (size_t i = 1; i < sampled.size(); ++i) {
                tp.segments.push_back({sampled[i-1], sampled[i],
                                       Point3D::Zero(), PathSegmentType::Linear,
                                       params.feed_rate, cut_z});
            }

            // Retract to safe Z between contours
            tp.segments.push_back({tp.segments.back().end,
                                   Point3D(tp.segments.back().end.x(),
                                           tp.segments.back().end.y(), params.safe_z),
                                   Point3D::Zero(), PathSegmentType::Rapid});
        }

        start_z = cut_z;
        pass_count++;
    }

    // Final retract
    if (!tp.segments.empty()) {
        tp.segments.push_back({tp.segments.back().end,
                               Point3D(0, 0, params.safe_z),
                               Point3D::Zero(), PathSegmentType::Rapid});
    }

    return tp;
}

// ═══════════════════════════════════════════════════════════════════════════
//  pocket_toolpath (mesh) — 口袋加工刀路
// ═══════════════════════════════════════════════════════════════════════════

Toolpath pocket_toolpath(
    const mesh::HalfedgeMesh& mesh,
    const std::vector<mesh::HalfedgeMesh>& islands,
    double z_height,
    const Tool& tool,
    const PocketParams& params)
{
    Toolpath tp;
    tp.name = "Pocket_Mesh";
    tp.safe_z = params.safe_z;
    tp.cut_z = z_height;
    tp.step_down = params.step_down;

    // Extract outer boundary as 2D polygon
    auto boundary_curves = extract_contour_from_mesh(mesh, z_height);
    if (boundary_curves.empty()) return tp;

    // Use the longest contour as the outer boundary
    std::vector<Point2D> boundary_poly;
    {
        size_t best = 0;
        double best_len = 0;
        for (size_t i = 0; i < boundary_curves.size(); ++i) {
            auto sampled = sample_curve(boundary_curves[i], 200);
            double len = 0;
            for (size_t j = 1; j < sampled.size(); ++j) {
                len += (Vector3D(sampled[j].x() - sampled[j-1].x(),
                                 sampled[j].y() - sampled[j-1].y(),
                                 sampled[j].z() - sampled[j-1].z())).norm();
            }
            if (len > best_len) {
                best_len = len;
                best = i;
            }
        }
        auto sampled = sample_curve(boundary_curves[best], 300);
        boundary_poly.reserve(sampled.size());
        for (auto& p : sampled) {
            boundary_poly.emplace_back(p.x(), p.y());
        }
    }

    if (boundary_poly.size() < 3) return tp;

    // Extract island polygons
    std::vector<std::vector<Point2D>> island_polys;
    for (const auto& island_mesh : islands) {
        auto island_curves = extract_contour_from_mesh(island_mesh, z_height);
        for (auto& ic : island_curves) {
            auto sampled = sample_curve(ic, 200);
            std::vector<Point2D> ipoly;
            ipoly.reserve(sampled.size());
            for (auto& p : sampled) {
                ipoly.emplace_back(p.x(), p.y());
            }
            if (ipoly.size() >= 3) {
                island_polys.push_back(std::move(ipoly));
            }
        }
    }

    // Compute spacing
    double spacing = params.step_over;
    if (spacing <= 0.0) spacing = tool.diameter * 0.5;

    // Compute bounding box
    double min_x = std::numeric_limits<double>::max();
    double max_x = std::numeric_limits<double>::lowest();
    double min_y = std::numeric_limits<double>::max();
    double max_y = std::numeric_limits<double>::lowest();
    for (const auto& p : boundary_poly) {
        min_x = std::min(min_x, p.x());
        max_x = std::max(max_x, p.x());
        min_y = std::min(min_y, p.y());
        max_y = std::max(max_y, p.y());
    }

    double expand = std::max(max_x - min_x, max_y - min_y) * 0.1;
    min_x -= expand;
    max_x += expand;
    min_y -= expand;
    max_y += expand;

    // Rotation angle for zigzag
    double angle_rad = params.cut_angle * M_PI / 180.0;

    // Determine the scanning direction based on the rotated bounding box
    // We generate lines perpendicular to the scan direction
    // For cut_angle=0: horizontal lines (vary Y), for cut_angle=90: vertical lines (vary X)

    // Generate scan lines
    // Rotate boundary to align with scan direction
    std::vector<Point2D> rot_boundary;
    rot_boundary.reserve(boundary_poly.size());
    for (const auto& p : boundary_poly) {
        rot_boundary.push_back(rotate_2d(p, -angle_rad));
    }

    // Also rotate islands
    std::vector<std::vector<Point2D>> rot_islands;
    for (const auto& ip : island_polys) {
        std::vector<Point2D> rip;
        rip.reserve(ip.size());
        for (const auto& p : ip) {
            rip.push_back(rotate_2d(p, -angle_rad));
        }
        rot_islands.push_back(std::move(rip));
    }

    // Compute rotated bounding box
    double r_min_x = std::numeric_limits<double>::max();
    double r_max_x = std::numeric_limits<double>::lowest();
    double r_min_y = std::numeric_limits<double>::max();
    double r_max_y = std::numeric_limits<double>::lowest();
    for (const auto& p : rot_boundary) {
        r_min_x = std::min(r_min_x, p.x());
        r_max_x = std::max(r_max_x, p.x());
        r_min_y = std::min(r_min_y, p.y());
        r_max_y = std::max(r_max_y, p.y());
    }
    double r_expand = (r_max_y - r_min_y) * 0.05;
    r_min_y -= r_expand;
    r_max_y += r_expand;

    // ── Generate horizontal scan lines in rotated space ─────────────
    struct CutSeg { Point2D a, b; };
    std::vector<CutSeg> cut_segs;

    int ns = static_cast<int>(rot_boundary.size());
    double line_start_x = r_min_x - spacing;
    double line_end_x   = r_max_x + spacing;

    for (double y = r_min_y; y <= r_max_y + spacing * 0.5; y += spacing) {
        Point2D l_start(line_start_x, y);
        Point2D l_end(line_end_x, y);

        // Find intersections with rotated boundary
        std::vector<double> t_hits;
        for (int i = 0; i < ns; ++i) {
            int j = (i + 1) % ns;
            Point2D out;
            if (segment_intersect(l_start, l_end, rot_boundary[i], rot_boundary[j], out)) {
                Vector2D dir = l_end - l_start;
                double dlen = dir.squaredNorm();
                if (dlen > 1e-12) {
                    double t = ((out - l_start).dot(dir)) / dlen;
                    t_hits.push_back(t);
                }
            }
        }

        std::sort(t_hits.begin(), t_hits.end());
        t_hits.erase(std::unique(t_hits.begin(), t_hits.end(),
            [](double a, double b) { return std::abs(a - b) < 1e-9; }),
            t_hits.end());

        // For each pair of intersections, check if midpoint is inside boundary
        // and outside all islands
        for (size_t k = 0; k + 1 < t_hits.size(); k += 2) {
            double ta = t_hits[k];
            double tb = t_hits[k + 1];
            Point2D mid = l_start + (ta + tb) * 0.5 * (l_end - l_start);

            // Must be inside boundary
            if (!point_in_polygon(rot_boundary, mid)) continue;

            // Must be outside all islands
            bool inside_island = false;
            for (const auto& rip : rot_islands) {
                if (point_in_polygon(rip, mid)) {
                    inside_island = true;
                    break;
                }
            }
            if (inside_island) continue;

            Point2D pa = l_start + ta * (l_end - l_start);
            Point2D pb = l_start + tb * (l_end - l_start);

            // Rotate back to original space
            Point2D orig_a = rotate_2d(pa, angle_rad);
            Point2D orig_b = rotate_2d(pb, angle_rad);
            cut_segs.push_back({orig_a, orig_b});
        }
    }

    // ── Sort segments for efficient movement ───────────────────────
    // Greedy nearest-neighbour sort
    if (!cut_segs.empty()) {
        std::vector<CutSeg> sorted;
        sorted.reserve(cut_segs.size());
        sorted.push_back(cut_segs[0]);
        cut_segs.erase(cut_segs.begin());

        while (!cut_segs.empty()) {
            const Point2D& last = sorted.back().b;
            size_t best_i = 0;
            double best_dist = std::numeric_limits<double>::max();
            for (size_t i = 0; i < cut_segs.size(); ++i) {
                double d = (cut_segs[i].a - last).norm();
                if (d < best_dist) {
                    best_dist = d;
                    best_i = i;
                }
            }
            sorted.push_back(cut_segs[best_i]);
            cut_segs.erase(cut_segs.begin() + best_i);
        }
        cut_segs = std::move(sorted);
    }

    if (cut_segs.empty()) {
        tp.segments.push_back({Point3D(0, 0, params.safe_z),
                               Point3D(0, 0, params.safe_z),
                               Point3D::Zero(), PathSegmentType::Rapid});
        return tp;
    }

    // ── Build toolpath ─────────────────────────────────────────────
    // Rapid approach to first point
    tp.segments.push_back({Point3D(cut_segs[0].a.x(), cut_segs[0].a.y(), params.safe_z),
                           Point3D(cut_segs[0].a.x(), cut_segs[0].a.y(), params.safe_z),
                           Point3D::Zero(), PathSegmentType::Rapid});

    // Plunge
    tp.segments.push_back({Point3D(cut_segs[0].a.x(), cut_segs[0].a.y(), params.safe_z),
                           Point3D(cut_segs[0].a.x(), cut_segs[0].a.y(), z_height),
                           Point3D::Zero(), PathSegmentType::Linear,
                           params.feed_rate * 0.5, z_height});

    for (size_t i = 0; i < cut_segs.size(); ++i) {
        const auto& seg = cut_segs[i];

        // Cut the segment
        tp.segments.push_back({Point3D(seg.a.x(), seg.a.y(), z_height),
                               Point3D(seg.b.x(), seg.b.y(), z_height),
                               Point3D::Zero(), PathSegmentType::Linear,
                               params.feed_rate, z_height});

        // Link to next segment
        if (i + 1 < cut_segs.size()) {
            const Point2D& next = cut_segs[i + 1].a;
            double dist = (next - seg.b).norm();
            if (dist < 5.0 * spacing) {
                // Close enough: direct cut move
                tp.segments.push_back({Point3D(seg.b.x(), seg.b.y(), z_height),
                                       Point3D(next.x(), next.y(), z_height),
                                       Point3D::Zero(), PathSegmentType::Linear,
                                       params.feed_rate * 1.5, z_height});
            } else {
                // Too far: rapid retract + reposition + plunge
                tp.segments.push_back({Point3D(seg.b.x(), seg.b.y(), z_height),
                                       Point3D(seg.b.x(), seg.b.y(), params.safe_z),
                                       Point3D::Zero(), PathSegmentType::Rapid});

                tp.segments.push_back({Point3D(seg.b.x(), seg.b.y(), params.safe_z),
                                       Point3D(next.x(), next.y(), params.safe_z),
                                       Point3D::Zero(), PathSegmentType::Rapid});

                tp.segments.push_back({Point3D(next.x(), next.y(), params.safe_z),
                                       Point3D(next.x(), next.y(), z_height),
                                       Point3D::Zero(), PathSegmentType::Linear,
                                       params.feed_rate * 0.5, z_height});
            }
        }
    }

    // Retract
    tp.segments.push_back({tp.segments.back().end,
                           Point3D(tp.segments.back().end.x(),
                                   tp.segments.back().end.y(), params.safe_z),
                           Point3D::Zero(), PathSegmentType::Rapid});

    tp.segments.push_back({Point3D(tp.segments.back().end.x(),
                                   tp.segments.back().end.y(), params.safe_z),
                           Point3D(0, 0, params.safe_z),
                           Point3D::Zero(), PathSegmentType::Rapid});

    return tp;
}

}  // namespace vde::core