using RobotNet.MapShares;
using RobotNet.MapShares.Dtos;
using RobotNet.RobotShares.Dtos;

namespace RobotNet.RobotManager.Services.Traffic;

public class TrafficMath
{
    public static bool Edge1IntersectEdge(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
    {
        if (edge.TrajectoryDegree != MapShares.Enums.TrajectoryDegree.One) return true;

        double dx = edge.X2 - edge.X1;
        double dy = edge.Y2 - edge.Y1;
        var length = Math.Sqrt(Math.Pow(dx, 2) + Math.Pow(dy, 2));
        if (length == 0) return Math.Sqrt(Math.Pow(edge.X1 - centerCircle.X, 2) + Math.Pow(edge.Y1 - centerCircle.Y, 2)) <= radius;

        double distance = Math.Abs(dy * centerCircle.X - dx * centerCircle.Y + (edge.X2 * edge.Y1 - edge.X1 * edge.Y2)) / length;
        if (distance > radius) return false;

        double a = dx * dx + dy * dy;
        double b = 2 * (dx * (edge.X1 - centerCircle.X) + dy * (edge.Y1 - centerCircle.Y));
        double c = (edge.X1 - centerCircle.X) * (edge.X1 - centerCircle.X) + (edge.Y1 - centerCircle.Y) * (edge.Y1 - centerCircle.Y) - radius * radius;

        double delta = b * b - 4 * a * c;
        if (delta < 0) return false;

        double t1 = (-b + Math.Sqrt(delta)) / (2 * a);
        double t2 = (-b - Math.Sqrt(delta)) / (2 * a);

        return (t1 >= 0 && t1 <= 1) || (t2 >= 0 && t2 <= 1);
    }

    public static bool Edge2IntersectEdge(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
    {
        if (edge.TrajectoryDegree != MapShares.Enums.TrajectoryDegree.Two) return true;

        var length = Math.Sqrt(Math.Pow(edge.X2 - edge.X1, 2) + Math.Pow(edge.Y2 - edge.Y1, 2));
        if (length == 0) return Math.Sqrt(Math.Pow(edge.X1 - centerCircle.X, 2) + Math.Pow(edge.Y1 - centerCircle.Y, 2)) <= radius;

        double step = 0.1 / length;
        for (double t = 0; t <= 1.001; t += step)
        {
            (double x1, double y1) = CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
            if (Math.Sqrt(Math.Pow(x1 - centerCircle.X, 2) + Math.Pow(y1 - centerCircle.Y, 2)) < radius) return true;
        }
        return false;
    }

    public static bool Edge3IntersectEdge(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
    {
        if (edge.TrajectoryDegree != MapShares.Enums.TrajectoryDegree.Three) return true;

        var length = Math.Sqrt(Math.Pow(edge.X2 - edge.X1, 2) + Math.Pow(edge.Y2 - edge.Y1, 2));
        if (length == 0) return Math.Sqrt(Math.Pow(edge.X1 - centerCircle.X, 2) + Math.Pow(edge.Y1 - centerCircle.Y, 2)) <= radius;

        double step = 0.1 / length;
        for (double t = 0; t <= 1.001; t += step)
        {
            (double x1, double y1) = CurveDegreeThree(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
            if (Math.Sqrt(Math.Pow(x1 - centerCircle.X, 2) + Math.Pow(y1 - centerCircle.Y, 2)) < radius) return true;
        }
        return false;
    }

    public static bool EdgeIntersectCircle(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
    {
        if (edge.TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One) return Edge1IntersectEdge(centerCircle, radius, edge);
        else if (edge.TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two) return Edge2IntersectEdge(centerCircle, radius, edge);
        else if (edge.TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Three) return Edge3IntersectEdge(centerCircle, radius, edge);
        return true;
    }

    public static (double x, double y) CurveDegreeTwo(double t, double x1, double y1, double controlPointX, double controlPointY, double x2, double y2)
    {
        var x = (1 - t) * (1 - t) * x1 + 2 * t * (1 - t) * controlPointX + t * t * x2;
        var y = (1 - t) * (1 - t) * y1 + 2 * t * (1 - t) * controlPointY + t * t * y2;
        return (x, y);
    }

    public static (double x, double y) CurveDegreeThree(double t, double x1, double y1, double controlPoint1X, double controlPoint1Y, double controlPoint2X, double controlPoint2Y, double x2, double y2)
    {
        var x = Math.Pow(1 - t, 3) * x1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2X + Math.Pow(t, 3) * x2; ;
        var y = Math.Pow(1 - t, 3) * y1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2Y + Math.Pow(t, 3) * y2;
        return (x, y);
    }

    public static double CalTurningRadius(AgentModel model, double offsetLength, double offsetWidth)
    {
        var navigationPointX = offsetLength - model.NavigationPointX;
        var navigationPointY = offsetWidth - model.NavigationPointY;
        var length = model.Length + 2 * offsetLength;
        var width = model.Width + 2 * offsetWidth;
        double d1 = Math.Sqrt(navigationPointX * navigationPointX + navigationPointY * navigationPointY);
        double d2 = Math.Sqrt((length - navigationPointX) * (length - navigationPointX) + navigationPointY * navigationPointY);
        double d3 = Math.Sqrt(navigationPointX * navigationPointX + (width - navigationPointY) * (width - navigationPointY));
        double d4 = Math.Sqrt((length - navigationPointX) * (length - navigationPointX) + (width - navigationPointY) * (width - navigationPointY));

        return Math.Max(Math.Max(d1, d2), Math.Max(d3, d4));
    }

    public static TrafficLockedShapeDto CalRobotRectShape(AgentModel model, double offsetLength, double offsetWidth, double x, double y, double theta)
    {
        double x1 = model.NavigationPointX - offsetLength;
        double y1 = model.NavigationPointY - offsetWidth;
        double x2 = model.Length + model.NavigationPointX + offsetLength;
        double y2 = model.NavigationPointY - offsetWidth;
        double x3 = model.Length + model.NavigationPointX + offsetLength;
        double y3 = model.Width + model.NavigationPointY + offsetWidth;
        double x4 = model.NavigationPointX - offsetLength;
        double y4 = model.Width + model.NavigationPointY + offsetWidth;

        return new()
        {
            Type = TrafficLockedShapeType.Rectangle,
            X1 = x1 * Math.Cos(theta) - y1 * Math.Sin(theta) + x,
            Y1 = x1 * Math.Sin(theta) + y1 * Math.Cos(theta) + y,
            X2 = x2 + Math.Cos(theta) - y2 * Math.Sin(theta) + x,
            Y2 = x2 * Math.Sin(theta) + y2 * Math.Cos(theta) + y,
            X3 = x3 + Math.Cos(theta) - y3 * Math.Sin(theta) + x,
            Y3 = x3 * Math.Sin(theta) + y3 * Math.Cos(theta) + y,
            X4 = x4 + Math.Cos(theta) - y4 * Math.Sin(theta) + x,
            Y4 = x4 * Math.Sin(theta) + y4 * Math.Cos(theta) + y
        };
    }

    public static bool RectIntersectCircle(TrafficNodeDto centerCircle, double radius, double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
    {
        double xMin = Math.Min(Math.Min(x1, x2), Math.Min(x3, x4));
        double xMax = Math.Max(Math.Max(x1, x2), Math.Max(x3, x4));
        double yMin = Math.Min(Math.Min(y1, y2), Math.Min(y3, y4));
        double yMax = Math.Max(Math.Max(y1, y2), Math.Max(y3, y4));

        double xClosest = Math.Max(xMin, Math.Min(centerCircle.X, xMax));
        double yClosest = Math.Max(yMin, Math.Min(centerCircle.Y, yMax));

        double distance = Math.Sqrt((centerCircle.X - xClosest) * (centerCircle.X - xClosest) + (centerCircle.Y - yClosest) * (centerCircle.Y - yClosest));
        return distance <= radius;
    }

    public static bool RectIntersectRect(TrafficLockedShapeDto rect1, TrafficLockedShapeDto rect2)
    {
        double x1Min = Math.Min(Math.Min(rect1.X1, rect1.X2), Math.Min(rect1.X3, rect1.X4));
        double x1Max = Math.Max(Math.Max(rect1.X1, rect1.X2), Math.Max(rect1.X3, rect1.X4));
        double y1Min = Math.Min(Math.Min(rect1.Y1, rect1.Y2), Math.Min(rect1.Y3, rect1.Y4));
        double y1Max = Math.Max(Math.Max(rect1.Y1, rect1.Y2), Math.Max(rect1.Y3, rect1.Y4));

        double x2Min = Math.Min(Math.Min(rect2.X1, rect2.X2), Math.Min(rect2.X3, rect2.X4));
        double x2Max = Math.Max(Math.Max(rect2.X1, rect2.X2), Math.Max(rect2.X3, rect2.X4));
        double y2Min = Math.Min(Math.Min(rect2.Y1, rect2.Y2), Math.Min(rect2.Y3, rect2.Y4));
        double y2Max = Math.Max(Math.Max(rect2.Y1, rect2.Y2), Math.Max(rect2.Y3, rect2.Y4));

        bool xOverlap = x1Min <= x2Max && x2Min <= x1Max;
        bool yOverlap = y1Min <= y2Max && y2Min <= y1Max;

        return xOverlap && yOverlap;
    }
}