using RobotNet10.Common;
using RobotNet10.Common.Models;
using RobotNet10.RobotApp.Services.Simulation;
using RobotDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection;

namespace RobotNet10.RobotApp.Services.Navigation.CSharp;

public class DockToConfig
{
    #region Core Stanley Parameters

    /// <summary>
    /// Cross-track error gain (K)
    /// Default: 2.5
    ///
    /// Meaning: How aggressively to correct lateral position error
    /// Formula: δ = ψ + arctan(K × e / (v + Ks))
    ///
    /// ↑ Increase (3.0-5.0):
    ///   ✓ Faster correction of cross-track error
    ///   ✓ Tighter path following
    ///   ✗ May cause oscillation
    ///   ✗ Less smooth on noisy paths
    ///
    /// ↓ Decrease (1.5-2.0):
    ///   ✓ Smoother motion
    ///   ✓ Less oscillation
    ///   ✗ Slower error correction
    ///   ✗ Larger cross-track error
    ///
    /// Tuning Tips:
    /// - Start: 2.5 for general use
    /// - High precision: 3.0-4.0
    /// - Smooth priority: 1.5-2.0
    /// - Check stability by observing steering oscillation
    /// </summary>
    public double K { get; set; } = 2.5;

    /// <summary>
    /// Softening constant (Ks) - meters/second
    /// Default: 0.1 m/s
    ///
    /// Meaning: Added to velocity denominator to prevent division by zero at low speeds
    /// Formula: δ = ψ + arctan(K × e / (v + Ks))
    ///
    /// ↑ Increase (0.15-0.2):
    ///   ✓ Less aggressive correction at low speed
    ///   ✓ Smoother motion when starting
    ///   ✗ Slower error correction at low speed
    ///
    /// ↓ Decrease (0.05-0.08):
    ///   ✓ More responsive at low speed
    ///   ✗ May cause oscillation when slow
    ///   ✗ Risk of instability near zero velocity
    ///
    /// Tuning Tips:
    /// - Should be ~10% of typical operating velocity
    /// - If robot oscillates when slow: increase to 0.15-0.2
    /// - If too sluggish at startup: decrease to 0.05-0.08
    /// </summary>
    public double Ks { get; set; } = 0.1;

    #endregion

    #region Vehicle Parameters

    /// <summary>
    /// Wheelbase (L) - distance between front and rear axles (meters)
    /// Default: 0.5m
    ///
    /// Meaning: Distance from rear axle (robot center) to virtual front axle
    /// Used to calculate front axle position and convert steering angle to angular velocity
    ///
    /// IMPORTANT: Must match actual robot geometry
    ///
    /// Formula: ω = (v × tan(δ)) / L
    /// </summary>
    public double WheelBase { get; set; } = 0.6;

    /// <summary>
    /// Maximum steering angle (radians)
    /// Default: 0.5 rad (≈28.6°)
    ///
    /// Meaning: Physical limit of equivalent steering angle
    ///
    /// ↑ Increase (0.6-0.8 rad ≈ 34-46°):
    ///   ✓ Sharper turns possible
    ///   ✗ May exceed robot's turning capability
    ///
    /// ↓ Decrease (0.3-0.4 rad ≈ 17-23°):
    ///   ✓ Safer, gentler turns
    ///   ✗ Cannot track sharp curves
    ///
    /// Tuning Tips:
    /// - Test robot's max practical turn rate
    /// - Calculate: δ_max = arctan(L × ω_max / v_typical)
    /// - Example: L=0.5m, ω_max=2rad/s, v=1m/s → δ_max = 0.785 rad (45°)
    /// - Conservative: 0.4-0.5 rad
    /// </summary>
    public double MaxSteeringAngle { get; set; } = 0.5;

    #endregion

    #region Goal Approach Parameters

    /// <summary>
    /// Distance to start increasing K gain near goal (meters)
    /// Default: 1.0m
    ///
    /// Meaning: When within this distance, K gain increases linearly
    /// to improve tracking accuracy during final approach
    ///
    /// ↑ Increase (1.5-2.0m):
    ///   ✓ Earlier tightening, smoother transition
    ///   ✗ May be too aggressive on long approach
    ///
    /// ↓ Decrease (0.5-0.8m):
    ///   ✓ Only tighten very close to goal
    ///   ✗ Less time to correct errors
    ///
    /// Tuning Tips:
    /// - Should be larger than GoalTolerance × 10
    /// - Typical: 0.8-1.5m
    /// </summary>
    public double GoalApproachDistance { get; set; } = 1.0;

    /// <summary>
    /// K gain multiplier at goal position
    /// Default: 2.0 (K doubles when at goal)
    ///
    /// Meaning: At goal, effective K = K × GoalGainMultiplier
    /// Linearly interpolated from 1.0 at GoalApproachDistance to this value at goal
    ///
    /// ↑ Increase (2.5-3.0):
    ///   ✓ Much tighter tracking near goal
    ///   ✗ Risk of oscillation
    ///
    /// ↓ Decrease (1.3-1.5):
    ///   ✓ Gentler increase
    ///   ✗ Less improvement near goal
    ///
    /// Tuning Tips:
    /// - Start at 2.0
    /// - If oscillating near goal: decrease to 1.5
    /// - If still drifting: increase to 2.5
    /// </summary>
    public double GoalGainMultiplier { get; set; } = 2.0;

    public double ReachedRadius { get; set; } = 0.03;
    #endregion

    #region Angular Velocity Limit

    /// <summary>
    /// Maximum angular velocity during dock-to approach (rad/s)
    /// Default: 0.8 rad/s
    ///
    /// Meaning: Clamps the angular velocity output of Stanley controller
    /// to prevent excessive rotation during docking.
    ///
    /// ↑ Increase (1.0-1.5):
    ///   ✓ Faster heading correction
    ///   ✗ May overshoot or oscillate during docking
    ///
    /// ↓ Decrease (0.3-0.5):
    ///   ✓ Smoother, more precise docking
    ///   ✗ Slower heading correction
    ///
    /// Tuning Tips:
    /// - Should be lower than PurePursuit/Stanley MaxAngularVelocity for precise docking
    /// - Start at 0.8, decrease if robot oscillates during dock
    /// </summary>
    public double MaxAngularVelocity { get; set; } = 0.05;

    #endregion

    #region Path Resolution

    /// <summary>
    /// Waypoint spacing for path sampling (meters)
    /// Default: 0.05m (5cm)
    ///
    /// Meaning: Distance between interpolated path points
    /// Same as PurePursuit.ResolutionSplit for consistency
    /// </summary>
    public double ResolutionSplit { get; set; } = 0.05;

    #endregion

    public RobotDirection DockToDirection { get; set; } = RobotDirection.FORWARD;
    public double DockToLength { get; set; } = 3;

    #region Fine Positioning

    /// <summary>
    /// Timeout per FinePositioning attempt (milliseconds).
    /// Default: 6000ms (6s)
    /// </summary>
    public int FinePositioningTimeoutMs { get; set; } = 6000;

    /// <summary>
    /// Maximum retries for FinePositioning before transitioning to Error.
    /// Default: 3
    /// </summary>
    public int FinePositioningMaxRetries { get; set; } = 3;

    #endregion

    #region Docking Overshoot & FinePositioning Tuning

    /// <summary>
    /// Distance (meters) from goal at which overshoot detection begins during Docking.
    /// Default: 0.2m
    ///
    /// Meaning: Similar to MovingOvershootDetectionRadius but for Docking phase.
    /// Smaller default because docking requires higher precision.
    ///
    /// ↑ Increase (0.3-0.5):
    ///   ✓ Earlier overshoot detection
    ///   ✗ May false-trigger during normal approach deceleration
    ///
    /// ↓ Decrease (0.1-0.15):
    ///   ✓ Fewer false triggers
    ///   ✗ Robot may overshoot further before detection
    ///
    /// Tuning Tips:
    /// - Should be > ReachedRadius (default 0.03m)
    /// - If robot frequently enters FinePositioning unnecessarily: increase
    /// - If robot overshoots too far before FP kicks in: decrease
    /// </summary>
    public double DockingOvershootDetectionRadius { get; set; } = 0.2;

    /// <summary>
    /// Distance (meters) from goal at which PID deceleration begins during Docking.
    /// Default: 3.0m
    ///
    /// Meaning: When distance to goal > this value, robot runs at MaxLinearVelocity.
    /// Below this distance, PID ramps velocity down proportionally.
    ///
    /// ↑ Increase (4-5):
    ///   ✓ Earlier, smoother deceleration — better for heavy robots
    ///   ✗ Slower docking approach
    ///
    /// ↓ Decrease (1-2):
    ///   ✓ Faster approach — stays at max speed longer
    ///   ✗ Risk of overshoot on high-inertia robots
    ///
    /// Tuning Tips:
    /// - Typically smaller than Moving's DecelerationDistance (docking is slower)
    /// - Should account for DockToMaxSpeed and robot braking capability
    /// </summary>
    public double DecelerationDistance { get; set; } = 3.0;

    /// <summary>
    /// Heading alignment threshold (degrees) for FinePositioning Phase 1 (Align).
    /// Default: 0.5°
    ///
    /// Meaning: Robot must align heading within this tolerance before transitioning
    /// from Align phase to Advance phase.
    ///
    /// ↑ Increase (1-2°):
    ///   ✓ Faster alignment — less time spent rotating
    ///   ✗ Robot may drift more during advance due to initial heading error
    ///
    /// ↓ Decrease (0.1-0.3°):
    ///   ✓ More precise heading before advancing
    ///   ✗ Longer alignment time, may oscillate on noisy heading sensor
    ///
    /// Tuning Tips:
    /// - Must be < ReAlignThresholdDegrees
    /// - If robot drifts during advance: decrease
    /// - If alignment takes too long or oscillates: increase
    /// </summary>
    public double FineAlignThresholdDegrees { get; set; } = 0.5;

    /// <summary>
    /// Heading drift threshold (degrees) to trigger re-alignment during FinePositioning Phase 2 (Advance).
    /// Default: 3.0°
    ///
    /// Meaning: If heading error exceeds this during advance, robot stops and re-enters Align phase.
    ///
    /// ↑ Increase (5-10°):
    ///   ✓ Fewer re-alignment interruptions
    ///   ✗ Robot may approach goal at a large angle, reducing accuracy
    ///
    /// ↓ Decrease (1-2°):
    ///   ✓ Tighter heading control during advance
    ///   ✗ Frequent re-alignment, slower convergence
    ///
    /// Tuning Tips:
    /// - Must be > FineAlignThresholdDegrees (to avoid immediate re-trigger)
    /// - Typical ratio: ReAlign ≈ 5-10x FineAlign
    /// - If robot keeps re-aligning: increase or tune AdvanceHeadingCorrectionGain
    /// </summary>
    public double ReAlignThresholdDegrees { get; set; } = 3.0;

    /// <summary>
    /// P-gain for heading correction during FinePositioning Phase 2 (Advance).
    /// Default: 1.5
    ///
    /// Meaning: Angular velocity correction = headingError × Gain × effectiveLinearVel
    /// Higher gain → stronger angular correction while advancing.
    ///
    /// ↑ Increase (2.0-3.0):
    ///   ✓ Faster heading correction during advance
    ///   ✗ May cause oscillation or jerky steering
    ///
    /// ↓ Decrease (0.5-1.0):
    ///   ✓ Smoother advance motion
    ///   ✗ Heading drift may accumulate, triggering re-alignment
    ///
    /// Tuning Tips:
    /// - Works together with DockToAdvanceMaxAngularVelocity
    /// - If robot oscillates during advance: decrease
    /// - If robot drifts and re-aligns too often: increase
    /// </summary>
    public double AdvanceHeadingCorrectionGain { get; set; } = 1.5;

    /// <summary>
    /// Maximum angular velocity (rad/s) for heading correction during FinePositioning Phase 2 (Advance).
    /// Default: 0.15 rad/s
    ///
    /// Meaning: Angular correction during advance is clamped to ±this value.
    /// Independent of NavigationConfig.MaxAngularVelocity (which is tuned for Moving phase).
    ///
    /// ↑ Increase (0.2-0.3):
    ///   ✓ Stronger heading correction during advance
    ///   ✗ May cause path deviation or jerky steering
    ///
    /// ↓ Decrease (0.05-0.1):
    ///   ✓ Very smooth, nearly straight-line advance
    ///   ✗ Cannot correct heading drift effectively, may trigger re-alignment
    ///
    /// Tuning Tips:
    /// - Should be small relative to DockToRetrySpeed to keep motion smooth
    /// - If advance path is too curved: decrease
    /// - If heading correction is too weak and triggers frequent re-align: increase
    /// </summary>
    public double DockToAdvanceMaxAngularVelocity { get; set; } = 0.15;

    /// <summary>
    /// Number of consecutive distance-increasing cycles to trigger overshoot during FinePositioning Advance.
    /// Default: 3
    ///
    /// Meaning: During advance, if distance to goal increases for this many consecutive cycles,
    /// overshoot is declared and a retry is initiated.
    ///
    /// ↑ Increase (5-7):
    ///   ✓ More tolerant of temporary distance fluctuations (sensor noise)
    ///   ✗ Slower overshoot detection — robot travels further past goal
    ///
    /// ↓ Decrease (1-2):
    ///   ✓ Faster overshoot detection
    ///   ✗ May false-trigger on sensor noise or minor jitter
    ///
    /// Tuning Tips:
    /// - At 30ms cycle: 3 counts = 90ms detection delay
    /// - Noisy localization: increase to 5-7
    /// - Stable localization: 2-3 is sufficient
    /// </summary>
    public int FinePositioningOvershootCount { get; set; } = 3;

    #endregion

    #region Continuous Goal Update

    /// <summary>
    /// Maximum allowed position shift (meters) between consecutive goal updates.
    /// If new goal is farther than this from old goal, it is rejected.
    /// Default: 0.5m
    /// </summary>
    public double MaxGoalPositionShift { get; set; } = 0.2;

    /// <summary>
    /// Maximum allowed angle shift (degrees) between consecutive goal updates.
    /// If new goal orientation differs by more than this from old goal, it is rejected.
    /// Default: 15 degrees
    /// </summary>
    public double MaxGoalAngleShiftDegrees { get; set; } = 5.0;

    #endregion

    public DockToConfig Clone() => (DockToConfig)MemberwiseClone();
}

public class DockToController(DockToConfig dockConfig)
{
    public DockToConfig DockConfig { get; private set; } = dockConfig;
    public List<NavigationNode> Waypoints_Value = [];
    public NavigationNode Goal = null!;
    private int _currentWaypointAheadIndex = 0;
    public NavigationNode StartNode = null!;

    public DockToController WithPath(NavigationNode startNode, NavigationNode currentGoal)
    {
        _currentWaypointAheadIndex = 0;
        StartNode = startNode;
        Goal = currentGoal;
        Waypoints_Value = [..PathSplit(StartNode, Goal)];
        return this;
    }

    public void ResetTracking()
    {
        _currentWaypointAheadIndex = 0;
    }

    public void UpdateGoal(NavigationNode currentGoal)
    {
        Goal = currentGoal;
        Waypoints_Value = [.. PathSplit(StartNode, Goal)];
    }

    private NavigationNode[] PathSplit(NavigationNode startNode, NavigationNode currentGoal)
    {
        List<NavigationNode> navigationNode = [startNode];
            var spaceEdge = new SpaceEdge()
            {
                StartX = startNode.X,
                StartY = startNode.Y,
                EndX = currentGoal.X,
                EndY = currentGoal.Y,
                Degree = 1,
            };

            double length = SpaceCompute.GetEdgeLength(spaceEdge, DockConfig.ResolutionSplit);
            if (length <= 0) return [];
            double step = DockConfig.ResolutionSplit / length;

            for (double t = step; t <= 1 - step; t += step)
            {
                (double x, double y) = SpaceCompute.BezierPoint(t, spaceEdge);
                navigationNode.Add(new()
                {
                    Id = Guid.NewGuid(),
                    NodeId = string.Empty,
                    X = x,
                    Y = y,
                    Theta = null,
                    Direction = DockConfig.DockToDirection,
                    Speed = startNode.Speed,
                });
            }
            navigationNode.Add(currentGoal);
        return [.. navigationNode];
    }

    /// <summary>
    /// Calculate distance between two points
    /// </summary>
    private static double CalculateDistance(double x1, double y1, double x2, double y2)
    {
        double dx = x2 - x1;
        double dy = y2 - y1;
        return Math.Sqrt(dx * dx + dy * dy);
    }

    public (NavigationNode node, int index) GetClosestAheadWaypoint(double x, double y)
    {
        if (Waypoints_Value.Count == 0)
            throw new InvalidOperationException("Path not set");

        double minDistance = double.MaxValue;
        int closestIndex = 0;

        // Start search from current index for efficiency
        for (int i = _currentWaypointAheadIndex; i < Waypoints_Value.Count; i++)
        {
            double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);

            if (distance < minDistance)
            {
                minDistance = distance;
                closestIndex = i;
            }
        }

        // Also check previous waypoints in case robot moved backwards
        for (int i = 0; i < _currentWaypointAheadIndex; i++)
        {
            double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);

            if (distance < minDistance)
            {
                minDistance = distance;
                closestIndex = i;
            }
        }

        _currentWaypointAheadIndex = closestIndex;
        return (Waypoints_Value[closestIndex], closestIndex);
    }

    /// <summary>
    /// Calculate path heading at given waypoint index (for Stanley)
    /// Uses current point and next point to determine direction
    /// </summary>
    private double CalculatePathHeading(int index)
    {
        if (index >= Waypoints_Value.Count - 1)
        {
            // Last point - use previous segment direction
            if (index > 0)
            {
                double dx = Waypoints_Value[index].X - Waypoints_Value[index - 1].X;
                double dy = Waypoints_Value[index].Y - Waypoints_Value[index - 1].Y;
                return Math.Atan2(dy, dx);
            }
            return 0;
        }

        // Use current to next point
        double dxNext = Waypoints_Value[index + 1].X - Waypoints_Value[index].X;
        double dyNext = Waypoints_Value[index + 1].Y - Waypoints_Value[index].Y;
        return Math.Atan2(dyNext, dxNext);
    }

    /// <summary>
    /// Calculate signed cross-track error (for Stanley)
    /// Positive: front axle is to the left of path
    /// Negative: front axle is to the right of path
    /// </summary>
    private static double CalculateStanleyCrossTrackError(double frontX, double frontY, NavigationNode closestPoint, double pathHeading)
    {
        // Vector from closest point to front axle
        double dx = frontX - closestPoint.X;
        double dy = frontY - closestPoint.Y;

        // Path direction vector
        double pathDx = Math.Cos(pathHeading);
        double pathDy = Math.Sin(pathHeading);

        // Cross product to get signed perpendicular distance
        // positive = left, negative = right
        double crossTrackError = dx * pathDy - dy * pathDx;

        return crossTrackError;
    }

    /// <summary>
    /// Normalize angle to [-π, π]
    /// </summary>
    private static double NormalizeAngle(double angle)
    {
        while (angle > Math.PI) angle -= 2 * Math.PI;
        while (angle < -Math.PI) angle += 2 * Math.PI;
        return angle;
    }

    /// <summary>
    /// Stanley-based final approach controller
    /// When robot enters goal region (IsApproachGoal), uses Stanley algorithm for precise CTE-based tracking
    /// </summary>
    public (double linearVel, double angularVel) FinalApproachController(
        double robotX,
        double robotY,
        double robotTheta,
        double actualLinearVelocity,
        double maxLinearVelocity)
    {
        if (Goal is null || Waypoints_Value.Count == 0) return (0, 0);

        if (DockConfig.DockToDirection == RobotDirection.BACKWARD) robotTheta += Math.PI;
        robotTheta = NormalizeAngle(robotTheta);

        // Calculate front axle position
        double frontX = robotX + DockConfig.WheelBase * Math.Cos(robotTheta);
        double frontY = robotY + DockConfig.WheelBase * Math.Sin(robotTheta);

        // Find closest point on path to front axle
        var (closestPoint, closestIndex) = GetClosestAheadWaypoint(frontX, frontY);

        // Calculate heading at closest point (path direction)
        double pathHeading = CalculatePathHeading(closestIndex);

        // Calculate cross-track error (signed distance from front axle to path)
        double crossTrackError = CalculateStanleyCrossTrackError(frontX, frontY, closestPoint, pathHeading);
        if (DockConfig.DockToDirection == RobotDirection.BACKWARD) crossTrackError = -crossTrackError;

        // Calculate heading error (path heading - robot heading)
        double headingError = NormalizeAngle(pathHeading - robotTheta);

        // Adaptive K gain: increase when close to goal for tighter tracking
        double distanceToGoal = CalculateDistance(robotX, robotY, Goal.X, Goal.Y);
        double adaptiveK = DockConfig.K;
        if (distanceToGoal < DockConfig.GoalApproachDistance)
        {
            // Linearly increase K from K to K*GoalGainMultiplier as distance decreases
            double approachRatio = 1.0 - (distanceToGoal / DockConfig.GoalApproachDistance);
            adaptiveK = DockConfig.K * (1.0 + approachRatio * (DockConfig.GoalGainMultiplier - 1.0));
        }

        // Stanley formula: δ = ψ + arctan(K × e / (v + Ks))
        double crossTrackTerm = Math.Atan2(adaptiveK * crossTrackError, Math.Abs(actualLinearVelocity) + DockConfig.Ks);

        // Total steering angle
        double steeringAngle = headingError + crossTrackTerm;

        // Clamp to maximum steering angle
        steeringAngle = Math.Clamp(steeringAngle, -DockConfig.MaxSteeringAngle, DockConfig.MaxSteeringAngle);

        // Convert steering angle to angular velocity using bicycle model
        double angularVelocity = (actualLinearVelocity * Math.Tan(steeringAngle)) / DockConfig.WheelBase;

        // Clamp angular velocity for dock-to approach
        angularVelocity = Math.Clamp(angularVelocity, -DockConfig.MaxAngularVelocity, DockConfig.MaxAngularVelocity);

        // Apply direction to velocities
        double linearVel = maxLinearVelocity;
        if (DockConfig.DockToDirection == RobotDirection.BACKWARD)
        {
            linearVel = -linearVel;
        }

        // Debug output
        Console.WriteLine($"DT-Stanley: Front=({frontX:F3},{frontY:F3}), Closest=({closestPoint.X:F3},{closestPoint.Y:F3}), " +
                         $"Pose=({robotX:F3},{robotY:F3},{robotTheta * 180 / Math.PI:F1}°), " +
                         $"CTE={crossTrackError:F3}m, HeadErr={headingError * 180 / Math.PI:F1}°, " +
                         $"SteerAng={steeringAngle * 180 / Math.PI:F1}°, " +
                         $"K={adaptiveK:F2}, DTG={distanceToGoal:F3}m, " +
                         $"LVel={linearVel:F3}, AnVel={angularVelocity:F3}");

        return (linearVel, angularVelocity);
    }
}