using RobotNet10.Common.Models;
using System.ComponentModel.DataAnnotations;

namespace RobotNet10.Common;

public class SpaceCompute
{
    public static SpaceNode BezierPoint([Range(0, 1)] double t, SpaceEdge edge)
    {
        t = Math.Clamp(t, 0.0, 1.0);
        if (edge.Degree == 1) return new SpaceNode(edge.StartX + t * (edge.EndX - edge.StartX), edge.StartY + t * (edge.EndY - edge.StartY));
        else if (edge.Degree == 2)
        {
            return new((1 - t) * (1 - t) * edge.StartX + 2 * t * (1 - t) * edge.ControlPoint1X + t * t * edge.EndX,
                       (1 - t) * (1 - t) * edge.StartY + 2 * t * (1 - t) * edge.ControlPoint1Y + t * t * edge.EndY);
        }
        else if (edge.Degree == 3)
        {
            return new(Math.Pow(1 - t, 3) * edge.StartX + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2X + Math.Pow(t, 3) * edge.EndX,
                       Math.Pow(1 - t, 3) * edge.StartY + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2Y + Math.Pow(t, 3) * edge.EndY);
        }
        return new(edge.EndX, edge.EndY);
    }

    // Giải phương trình bậc 2: ax² + bx + c = 0
    public static double[] SolveQuadratic(double a, double b, double c)
    {
        var roots = new List<double>();

        if (Math.Abs(a) < 1e-10)
        {
            if (Math.Abs(b) > 1e-10)
            {
                roots.Add(-c / b);
            }
            return [.. roots];
        }

        double discriminant = b * b - 4 * a * c;

        if (discriminant >= 0)
        {
            double sqrtD = Math.Sqrt(discriminant);
            roots.Add((-b + sqrtD) / (2 * a));
            roots.Add((-b - sqrtD) / (2 * a));
        }

        return [.. roots];
    }

    // Giải phương trình bậc 3: ax³ + bx² + cx + d = 0
    public static double[] SolveCubic(double a, double b, double c, double d)
    {
        if (Math.Abs(a) < 1e-10)
        {
            // Phương trình bậc 2
            return SolveQuadratic(b, c, d);
        }

        // Chuẩn hóa về dạng x³ + px + q = 0 (depressed cubic)
        b /= a;
        c /= a;
        d /= a;

        double p = (3 * c - b * b) / 3;
        double q = (2 * b * b * b - 9 * b * c + 27 * d) / 27;

        double discriminant = q * q / 4 + p * p * p / 27;

        var roots = new List<double>();

        if (discriminant >= 0)
        {
            // Một nghiệm thực
            double sqrtD = Math.Sqrt(discriminant);
            double term1 = -q / 2 + sqrtD;
            double term2 = -q / 2 - sqrtD;
            
            // Tính căn bậc 3, xử lý số âm
            double u = term1 >= 0 
                ? Math.Pow(term1, 1.0 / 3) 
                : -Math.Pow(-term1, 1.0 / 3);
            double v = term2 >= 0 
                ? Math.Pow(term2, 1.0 / 3) 
                : -Math.Pow(-term2, 1.0 / 3);
            
            double root = u + v - b / 3;
            
            if (!double.IsNaN(root) && !double.IsInfinity(root))
            {
                roots.Add(root);
            }
        }
        else
        {
            // Ba nghiệm thực (trường hợp lượng giác)
            double r = Math.Sqrt(-p * p * p / 27);
            
            if (r > 1e-10)
            {
                double acosArg = -q / (2 * r);
                // Clamp acosArg vào [-1, 1] để tránh NaN
                acosArg = Math.Max(-1.0, Math.Min(1.0, acosArg));
                double phi = Math.Acos(acosArg);
                double temp = 2 * Math.Pow(r, 1.0 / 3);

                roots.Add(temp * Math.Cos(phi / 3) - b / 3);
                roots.Add(temp * Math.Cos((phi + 2 * Math.PI) / 3) - b / 3);
                roots.Add(temp * Math.Cos((phi + 4 * Math.PI) / 3) - b / 3);
            }
        }

        return [.. roots];
    }


    // Phương pháp chính xác hơn sử dụng giải phương trình bậc 3
    public static (double distance, double time) DistanceToQuadraticBezier(SpaceNode nodeRef, SpaceEdge edge)
    {
        // Đạo hàm của hàm khoảng cách bình phương theo t
        // Giải phương trình bậc 3: d/dt[|P(t) - G|²] = 0

        // Quadratic Bezier: P(t) = (1-t)²P₀ + 2t(1-t)P₁ + t²P₂
        // Đạo hàm: P'(t) = 2(1-t)(P₁-P₀) + 2t(P₂-P₁)
        // Khoảng cách bình phương: D(t) = |P(t) - G|²
        // Đạo hàm: D'(t) = 2(P(t) - G) · P'(t) = 0
        
        // Viết lại: P(t) = P₀ + 2t(P₁-P₀) + t²(P₂-2P₁+P₀)
        // P'(t) = 2(P₁-P₀) + 2t(P₂-2P₁+P₀)
        // (P(t) - G) · P'(t) = 0
        
        double ax = edge.StartX - 2 * edge.ControlPoint1X + edge.EndX;
        double ay = edge.StartY - 2 * edge.ControlPoint1Y + edge.EndY;
        double bx = 2 * (edge.ControlPoint1X - edge.StartX);
        double by = 2 * (edge.ControlPoint1Y - edge.StartY);
        double cx = edge.StartX - nodeRef.X;
        double cy = edge.StartY - nodeRef.Y;

        // Hệ số của phương trình bậc 3: At³ + Bt² + Ct + D = 0
        // Từ: (P(t) - G) · P'(t) = 0
        // 
        // Với: P(t) = P₀ + bt + at², P'(t) = b + 2at
        //      c = P₀ - G
        //      (c + bt + at²) · (b + 2at) = 0
        //
        // Khai triển tích vô hướng:
        // c·b + c·2at + bt·b + bt·2at + at²·b + at²·2at = 0
        // = c·b + 2c·a·t + b·b·t + 2a·b·t² + a·b·t² + 2a·a·t³ = 0
        // = c·b + (2c·a + b·b)·t + 3a·b·t² + 2a·a·t³ = 0
        //
        // Vậy: A = 2a·a = 2(aₓ² + aᵧ²)
        //      B = 3a·b = 3(aₓbₓ + aᵧbᵧ)
        //      C = 2c·a + b·b = 2(cₓaₓ + cᵧaᵧ) + (bₓ² + bᵧ²)
        //      D = c·b = cₓbₓ + cᵧbᵧ
        double A = 2 * (ax * ax + ay * ay);
        double B = 3 * (ax * bx + ay * by);
        double C = 2 * (ax * cx + ay * cy) + (bx * bx + by * by);
        double D = bx * cx + by * cy;

        // Tìm các nghiệm của phương trình bậc 3
        var roots = SolveCubic(A, B, C, D);

        // Tính khoảng cách tại 2 đầu mút
        double distStart = nodeRef.DistanceTo(new(edge.StartX, edge.StartY));
        double distEnd = nodeRef.DistanceTo(new(edge.EndX, edge.EndY));
        
        // Khởi tạo với giá trị tại 2 đầu mút
        double minDist = Math.Min(distStart, distEnd);
        double time = distStart < distEnd ? 0 : 1;

        // Kiểm tra khoảng cách tại các điểm tới hạn (nghiệm của đạo hàm)
        foreach (double t in roots)
        {
            // Bỏ qua NaN và Infinity
            if (double.IsNaN(t) || double.IsInfinity(t)) continue;
            
            // Chỉ xét nghiệm trong khoảng [0, 1]
            if (t >= 0 && t <= 1)
            {
                SpaceNode p = BezierPoint(t, edge);
                double dist = nodeRef.DistanceTo(p);
                
                // Cập nhật minDist và time nếu tìm thấy khoảng cách nhỏ hơn
                if (dist < minDist)
                {
                    minDist = dist;
                    time = t;
                }
            }
        }

        // Kiểm tra lại khoảng cách tại 2 đầu mút (chỉ cập nhật nếu nhỏ hơn)
        // Lưu ý: Không ghi đè nếu minDist đã được cập nhật từ nghiệm trong (0,1)
        if (distStart < minDist)
        {
            minDist = distStart;
            time = 0;
        }
        if (distEnd < minDist)
        {
            minDist = distEnd;
            time = 1;
        }

        return (minDist, time);
    }

    // Phương pháp lấy mẫu - Đơn giản nhưng chậm hơn
    public static (double distance, double time) DistanceToCubicBezier(SpaceNode nodeRef, SpaceEdge edge)
    {
        double bestT = 0;
        double minDistance = Math.Sqrt(Math.Pow(nodeRef.X - edge.StartX, 2) + Math.Pow(nodeRef.Y - edge.StartY, 2));
        var length = GetEdgeLength(edge, 0.3);
        double step = 0.3 / (length == 0 ? 0.1 : length);

        // Bước 1: Lấy mẫu thô
        for (double t = 0; t <= 1; t += step)
        {
            SpaceNode p = BezierPoint(t, edge);
            double dist = nodeRef.DistanceTo(p);

            if (dist < minDistance)
            {
                minDistance = dist;
                bestT = t;
            }
        }

        // Bước 2: Tối ưu hóa chính xác hơn
        double epsilon = 1e-6;
        step = 0.01;

        for (int iter = 0; iter < 10; iter++)
        {
            double t1 = Math.Max(0, bestT - step);
            double t2 = Math.Min(1, bestT + step);

            double d0 = nodeRef.DistanceTo(BezierPoint(t1, edge));
            double d1 = nodeRef.DistanceTo(BezierPoint(bestT, edge));
            double d2 = nodeRef.DistanceTo(BezierPoint(t2, edge));

            if (d0 < d1)
            {
                bestT = t1;
                minDistance = d0;
            }
            else if (d2 < d1)
            {
                bestT = t2;
                minDistance = d2;
            }
            else
            {
                step *= 0.5;
            }

            if (step < epsilon) break;
        }

        return (minDistance, bestT);
    }

    public static (double x, double y, double distance, double time) GetProjectionOnEdge(double x, double y, SpaceEdge edge)
    {
        if (edge.Degree == 2)
        {
            (double distance, double time) = DistanceToQuadraticBezier(new(x, y), edge);
            var node = BezierPoint(time, edge);
            return (node.X, node.Y, distance, time);
        }
        else if (edge.Degree == 3)
        {
            (double distance, var time) = DistanceToCubicBezier(new(x, y), edge);
            var node = BezierPoint(time, edge);
            return (node.X, node.Y, distance, time);
        }
        else
        {
            double time = 0;
            var edgeLengthSquared = Math.Pow(edge.StartX - edge.EndX, 2) + Math.Pow(edge.StartY - edge.EndY, 2);
            if (edgeLengthSquared > 0)
            {
                time = Math.Max(0, Math.Min(1, ((x - edge.StartX) * (edge.EndX - edge.StartX) + (y - edge.StartY) * (edge.EndY - edge.StartY)) / edgeLengthSquared));
            }

            double nearestX = edge.StartX + time * (edge.EndX - edge.StartX);
            double nearestY = edge.StartY + time * (edge.EndY - edge.StartY);

            return (nearestX, nearestY, Math.Sqrt(Math.Pow(x - nearestX, 2) + Math.Pow(y - nearestY, 2)), time);
        }
    }

    public static double GetEdgeLength(SpaceEdge edge, double resolution)
    {
        var lineLength = Math.Sqrt(Math.Pow(edge.StartX - edge.EndX, 2) + Math.Pow(edge.StartY - edge.EndY, 2));
        if (edge.Degree == 1)
        {
            return lineLength;
        }
        else if (edge.Degree == 2)
        {
            if (lineLength <= 0) return 0;
            double step = resolution / lineLength;
            double distance = 0;

            for (double t = step; t <= 1.001; t += step)
            {
                var timePoint = BezierPoint(t - step, edge);
                var lastTimePoint = BezierPoint(t, edge);
                distance += Math.Sqrt(Math.Pow(timePoint.X - lastTimePoint.X, 2) + Math.Pow(timePoint.Y - lastTimePoint.Y, 2));
            }

            return Math.Round(distance, 3);
        }
        else
        {
            if (lineLength <= 0) return 0;
            double step = resolution / lineLength;
            double distance = 0;
            for (double t = step; t <= 1.001; t += step)
            {
                var sTime = t - step;
                var timePoint = BezierPoint(1 - sTime,  edge);
                sTime = t;
                var lastTimePoint = BezierPoint(1 - sTime, edge);

                distance += Math.Sqrt(Math.Pow(timePoint.X - lastTimePoint.X, 2) + Math.Pow(timePoint.Y - lastTimePoint.Y, 2));
            }
            return Math.Round(distance, 3);
        }
    }

    public static double GetVectorAngle(double originNodeX, double originNodeY, double vector1X, double vector1Y, double vector2X, double vector2Y)
    {
        double BA_x = vector1X - originNodeX;
        double BA_y = vector1Y - originNodeY;
        double BC_x = vector2X - originNodeX;
        double BC_y = vector2Y - originNodeY;
        // Tính độ dài của các vector AB và BC
        double lengthAB = Math.Sqrt(BA_x * BA_x + BA_y * BA_y);
        double lengthBC = Math.Sqrt(BC_x * BC_x + BC_y * BC_y);
        // Tính tích vô hướng của AB và BC
        double dotProduct = BA_x * BC_x + BA_y * BC_y;
        if (lengthAB * lengthBC == 0) return 0;
        if (dotProduct / (lengthAB * lengthBC) > 1) return 0;
        if (dotProduct / (lengthAB * lengthBC) < -1) return 180;
        return Math.Acos(dotProduct / (lengthAB * lengthBC)) * (180.0 / Math.PI);
    }

    public static double NormalizeDegreeAngle(double angle)
    {
        angle = angle % 360;
        if (angle > 180) angle -= 360;
        else if (angle < -180) angle += 360;
        return angle;
    }

    public static double NormalizeRadianAngle(double angle)
    {
        angle = angle % (2 * Math.PI);
        if (angle > Math.PI) angle -= (2 * Math.PI);
        else if (angle < -Math.PI) angle += (2 * Math.PI);
        return angle;
    }
}