using RobotNet.MapShares;
using RobotNet.MapShares.Dtos;
using RobotNet.MapShares.Enums;
using RobotNet.RobotShares.Enums;
using RobotNet.Shares;

namespace RobotNet.RobotManager.Services.Planner.Space;

public class MapCompute
{
    public static double DistanceToCurveEdge(double x, double y, EdgeDto edge, NodeDto startNode, NodeDto endNode)
    {
        double dMin = Math.Sqrt(Math.Pow(x - startNode.X, 2) + Math.Pow(y - startNode.Y, 2));
        var length = MapEditorHelper.GetEdgeLength(new()
        {
            X1 = startNode.X,
            Y1 = startNode.Y,
            X2 = endNode.X,
            Y2 = endNode.Y,
            ControlPoint1X = edge.ControlPoint1X,
            ControlPoint1Y = edge.ControlPoint1Y,
            ControlPoint2X = edge.ControlPoint2X,
            ControlPoint2Y = edge.ControlPoint2Y,
            TrajectoryDegree = edge.TrajectoryDegree,
        });
        double step = 0.1 / (length == 0 ? 0.1 : length);

        for (double t = 0; t <= 1; t += step)
        {
            (double curveX, double curveY) = MapEditorHelper.Curve(t, new()
            {
                TrajectoryDegree = edge.TrajectoryDegree,
                ControlPoint1X = edge.ControlPoint1X,
                ControlPoint1Y = edge.ControlPoint1Y,
                ControlPoint2X = edge.ControlPoint2X,
                ControlPoint2Y= edge.ControlPoint2Y,
                X1 = startNode.X,
                Y1 = startNode.Y,
                X2 = endNode.X,
                Y2 = endNode.Y,
            });
            double d = Math.Sqrt(Math.Pow(x - curveX, 2) + Math.Pow(y - curveY, 2));
            if (d < dMin) dMin = d;
        }

        return dMin;
    }

    public static MessageResult<double> DistanceToEdge(double x, double y, EdgeDto edge, NodeDto startNode, NodeDto endNode)
    {
        if (edge.TrajectoryDegree == TrajectoryDegree.One)
        {
            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, ((x - startNode.X) * (endNode.X - startNode.X) + (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 new(true) { Data = Math.Sqrt(Math.Pow(x - nearestX, 2) + Math.Pow(y - nearestY, 2)) };
        }
        else
        {
            return new(true) { Data = DistanceToCurveEdge(x, y, edge, startNode, endNode) };
        }
    }

    public static Direction GetNodeDirection(RobotDirection robotDirection) =>
        robotDirection switch
        {
            RobotDirection.FORWARD => Direction.FORWARD,
            RobotDirection.BACKWARD => Direction.BACKWARD,
            _ => Direction.NONE
        };

    public static RobotDirection GetRobotDirection(Direction nodeDirection) =>
        nodeDirection switch
        {
            Direction.FORWARD => RobotDirection.FORWARD,
            Direction.BACKWARD => RobotDirection.BACKWARD,
            _ => RobotDirection.NONE
        };
}