using RobotNet10.GlobalPathPlanner.Model;
using System.ComponentModel.DataAnnotations;

namespace RobotNet10.GlobalPathPlanner;

public class MathExtensions
{
    public static GlobalNode BezierPoint([Range(0, 1)] double t, GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
    {
        t = Math.Clamp(t, 0.0, 1.0);
        if (edge.Degree == 1)
        {
            return new()
            {
                X = startNode.X + t * (endNode.X - startNode.X),
                Y = startNode.Y + t * (endNode.Y - startNode.Y)
            };
        }
        else if (edge.Degree == 2)
        {
            return new()
            {
                X = (1 - t) * (1 - t) * startNode.X + 2 * t * (1 - t) * edge.ControlPoint1X + t * t * endNode.X,
                Y = (1 - t) * (1 - t) * startNode.Y + 2 * t * (1 - t) * edge.ControlPoint1Y + t * t * endNode.Y
            };
        }
        else if (edge.Degree == 3)
        {
            return new()
            {
                X = Math.Pow(1 - t, 3) * startNode.X + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2X + Math.Pow(t, 3) * endNode.X,
                Y = Math.Pow(1 - t, 3) * startNode.Y + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2Y + Math.Pow(t, 3) * endNode.Y,
            };
        }
        return endNode;
    }

    public static double GetEdgeLength(GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
    {
        var lineLength = Math.Sqrt(Math.Pow(startNode.X - endNode.X, 2) + Math.Pow(startNode.Y - endNode.Y, 2));
        if (edge.Degree == 1)
        {
            return lineLength;
        }
        else if (edge.Degree == 2)
        {
            if (lineLength <= 0) return 0;
            double step = 0.1 / lineLength;
            double distance = 0;

            for (double t = step; t <= 1.001; t += step)
            {
                var timePoint = BezierPoint(t - step, startNode, endNode, edge);
                var lastTimePoint = BezierPoint(t, startNode, endNode, 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 = 0.1 / lineLength;
            double distance = 0;
            for (double t = step; t <= 1.001; t += step)
            {
                var sTime = t - step;
                var timePoint = BezierPoint(1 - sTime, startNode, endNode, edge);
                sTime = t;
                var lastTimePoint = BezierPoint(1 - sTime, startNode, endNode, 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 GetEdgesLength(GlobalEdge[] edges, GlobalNode[] Nodes)
    {
        if (edges.Length == 0) return -1;
        double distance = 0;
        for (int i = 0; i < edges.Length; i++)
        {
            var edge = edges[i];
            var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
            var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
            if (startNode is null || endNode is null) return 999;
            distance += GetEdgeLength(startNode, endNode, edge);
        }
        return distance;
    }

    // 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 DistanceToQuadraticBezier(GlobalNode nodeRef, GlobalNode startNode, GlobalNode endNode, GlobalEdge 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

        double ax = startNode.X - 2 * edge.ControlPoint1X + endNode.X;
        double ay = startNode.Y - 2 * edge.ControlPoint1Y + endNode.Y;
        double bx = 2 * (edge.ControlPoint1X - startNode.X);
        double by = 2 * (edge.ControlPoint1Y - startNode.Y);
        double cx = startNode.X - nodeRef.X;
        double cy = startNode.Y - 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
        // Khai triển: c·b + (2c·a + b·b)·t + 3a·b·t² + 2a·a·t³ = 0
        // Vậy: A = 2a·a, B = 3a·b, C = 2c·a + b·b, D = 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);

        double minDist = double.MaxValue;

        // Kiểm tra khoảng cách tại các điểm tới hạn
        foreach (double t in roots)
        {
            if (t >= 0 && t <= 1)
            {
                GlobalNode p = BezierPoint(t, startNode, endNode, edge);
                double dist = nodeRef.DistanceTo(p);
                minDist = Math.Min(minDist, dist);
            }
        }

        // Kiểm tra khoảng cách tại 2 đầu mút
        minDist = Math.Min(minDist, nodeRef.DistanceTo(startNode));
        minDist = Math.Min(minDist, nodeRef.DistanceTo(endNode));

        return minDist;
    }

    // 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
        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
            // Math.Pow(negative, 1.0/3) trả về NaN trong C#, cần xử lý riêng
            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];
    }

    // 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];
    }

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

        // Bước 1: Lấy mẫu thô
        for (double t = 0; t <= 1; t += step)
        {
            GlobalNode p = BezierPoint(t, startNode, endNode, 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, startNode, endNode, edge));
            double d1 = nodeRef.DistanceTo(BezierPoint(bestT, startNode, endNode, edge));
            double d2 = nodeRef.DistanceTo(BezierPoint(t2, startNode, endNode, 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;
    }

    public static double DistanceToEdge(GlobalNode nodeRef, GlobalNode startNode, GlobalNode endNode, GlobalEdge edge)
    {
        if (edge.Degree == 2)
        {
            return DistanceToQuadraticBezier(nodeRef, startNode, endNode, edge);
        }
        else if (edge.Degree == 3)
        {
            return DistanceToCubicBezier(nodeRef, startNode, endNode, edge);
        }
        else
        {
            double time = 0;
            var edgeLengthSquared = Math.Pow(startNode.X - endNode.X, 2) + Math.Pow(startNode.Y - endNode.Y, 2);
            if (edgeLengthSquared > 0)
            {
                time = Math.Max(0, Math.Min(1, ((nodeRef.X - startNode.X) * (endNode.X - startNode.X) + (nodeRef.Y - startNode.Y) * (endNode.Y - startNode.Y)) / edgeLengthSquared));
            }

            double nearestX = startNode.X + time * (endNode.X - startNode.X);
            double nearestY = startNode.Y + time * (endNode.Y - startNode.Y);

            return Math.Sqrt(Math.Pow(nodeRef.X - nearestX, 2) + Math.Pow(nodeRef.Y - nearestY, 2));
        }
    }

    public static double GetAngle(GlobalNode originNode, GlobalNode Node1, GlobalNode Node2)
    {
        double BA_x = Node1.X - originNode.X;
        double BA_y = Node1.Y - originNode.Y;
        double BC_x = Node2.X - originNode.X;
        double BC_y = Node2.Y - originNode.Y;
        // 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 GetStartAngle(GlobalNode startNode, GlobalNode endNode, GlobalEdge edge, double ratio)
    {
        GlobalNode NearNode = BezierPoint(ratio, startNode, endNode, edge);
        return Math.Atan2(NearNode.Y - startNode.Y, NearNode.X - startNode.X) * 180 / Math.PI;
    }

    public static double GetEndAngle(GlobalNode startNode, GlobalNode endNode, GlobalEdge edge, double ratio)
    {
        GlobalNode NearNode = BezierPoint(ratio, startNode, endNode, edge);
        return Math.Atan2(endNode.Y - NearNode.Y, endNode.X - NearNode.X) * 180 / Math.PI;
    }

    public static Orientation[] GetOrientations(Orientation currentDirection, GlobalNode[] nodes, GlobalEdge[] edges, double ratio, double changeOrientationAngle)
    {
        Orientation[] Orientations = new Orientation[nodes.Length];
        if (nodes.Length > 0) Orientations[0] = currentDirection;
        if (nodes.Length > 2)
        {
            for (int i = 1; i < nodes.Length - 1; i++)
            {
                GlobalNode startNode = BezierPoint(1 - ratio, nodes[i - 1], nodes[i], edges[i - 1]);
                GlobalNode endNode = BezierPoint(ratio, nodes[i], nodes[i + 1], edges[i]);
                var angle = GetAngle(nodes[i], startNode, endNode);
                if (angle < changeOrientationAngle) Orientations[i] = Orientations[i - 1] == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
                else Orientations[i] = Orientations[i - 1];
            }
        }
        if (nodes.Length > 1) Orientations[^1] = Orientations[^2];
        return Orientations;
    }

    public static Orientation GetOrientationStart(GlobalNode nodeRef, GlobalNode nearNode, GlobalEdge edge, double InNodeAngle, double ratio, double changeOrientationAngle)
    {
        GlobalNode NearNode = BezierPoint(ratio, nodeRef, nearNode, edge);

        var RobotNearNode = new GlobalNode()
        {
            X = nodeRef.X + Math.Cos(InNodeAngle * Math.PI / 180),
            Y = nodeRef.Y + Math.Sin(InNodeAngle * Math.PI / 180),
        };
        var angle = GetAngle(nodeRef, NearNode, RobotNearNode);
        return angle > changeOrientationAngle ? Orientation.BACKWARD : Orientation.FORWARD;
    }

    public static Orientation GetOrientationEnd(GlobalNode nodeRef, GlobalNode nearNode, GlobalEdge edge, double InNodeAngle, double ratio, double changeOrientationAngle)
    {
        GlobalNode NearNode = BezierPoint(1 - ratio, nearNode, nodeRef, edge);

        var RobotNearNode = new GlobalNode()
        {
            X = nodeRef.X + Math.Cos(InNodeAngle * Math.PI / 180),
            Y = nodeRef.Y + Math.Sin(InNodeAngle * Math.PI / 180),
        };
        var angle = GetAngle(nodeRef, NearNode, RobotNearNode);
        return angle > changeOrientationAngle ? Orientation.FORWARD : Orientation.BACKWARD;
    }

    public static GlobalEdge[] GetEdgesPlanning(GlobalNode[] path, GlobalEdge[] edges, GlobalEdge? closesEdge)
    {
        var EdgesPlanning = new List<GlobalEdge>();
        for (int i = 0; i < path.Length - 1; i++)
        {
            var edge = edges.FirstOrDefault(e => e.StartNodeId == path[i].Id && e.EndNodeId == path[i + 1].Id);
            if (edge is null)
            {
                if (i != 0) return [];
                EdgesPlanning.Add(new GlobalEdge()
                {
                    Id = closesEdge is null ? Guid.NewGuid() : closesEdge.Id,
                    StartNodeId = path[i].Id,
                    EndNodeId = path[i + 1].Id,
                    Degree = 1,
                });
                continue;
            }
            EdgesPlanning.Add(new()
            {
                Id = edge.Id,
                StartNodeId = path[i].Id,
                EndNodeId = path[i + 1].Id,
                Degree = edge.Degree,
                ControlPoint1X = edge.ControlPoint1X,
                ControlPoint1Y = edge.ControlPoint1Y,
                ControlPoint2X = edge.ControlPoint2X,
                ControlPoint2Y = edge.ControlPoint2Y
            });
        }
        return [.. EdgesPlanning];
    }
}