// GDnD wiki example
// Demonstrates: following a list of waypoints smoothly, whether they come from NavMesh.CalculatePath
//   (path.corners) or from your own grid A*:
//   - line-of-sight smoothing (drop waypoints you can already see past)
//   - track progress along the path (advance past corners you have reached or cut), and seek a lookahead point
//   - Arrive (slow down) only on the final waypoint, so the agent does not brake at every corner
// Related pages: [[unity-navmesh]], [[steering-behaviours]], [[pathfinding-algorithms]], [[navigation-mesh-construction]]

using System;
using System.Collections.Generic;
using UnityEngine;

// Plain C# class: the owner asks it for a desired velocity each frame and moves however it likes
// (transform, CharacterController, kinematic Rigidbody, or NavMeshAgent.Move).
public class WaypointPathFollower
{
    public float MaxSpeed = 4f;
    public float WaypointRadius = 0.6f;     // "close enough" to a corner to aim at the next one
    public float Lookahead = 1.5f;          // aim this far ahead along the path, not at the corner itself
    public float SlowingRadius = 2.5f;      // Arrive starts slowing inside this distance of the LAST point
    public float ArrivedRadius = 0.15f;

    private readonly List<Vector3> points = new List<Vector3>();
    private int target;                     // index of the waypoint we are heading for

    public bool IsFinished => target >= points.Count;

    // canSeeDirectly(a, b): true if a straight move from a to b is clear.
    // NavMesh: (a, b) => !NavMesh.Raycast(a, b, out _, NavMesh.AllAreas). Grid A*: walk the cells on the line.
    // Pass null to skip smoothing (NavMesh corners are already string-pulled, so they rarely need it).
    public void SetPath(IList<Vector3> waypoints, Func<Vector3, Vector3, bool> canSeeDirectly = null)
    {
        points.Clear();
        if (waypoints == null || waypoints.Count == 0) { target = 0; return; }

        // Line-of-sight pruning: from each kept point, jump to the furthest point still visible.
        int i = 0;
        points.Add(waypoints[0]);
        while (i < waypoints.Count - 1)
        {
            int furthest = i + 1;
            if (canSeeDirectly != null)
                for (int j = waypoints.Count - 1; j > i + 1; j--)
                    if (canSeeDirectly(waypoints[i], waypoints[j])) { furthest = j; break; }
            points.Add(waypoints[furthest]);
            i = furthest;
        }
        target = points.Count > 1 ? 1 : 0;  // points[0] is where we started
    }

    public Vector3 DesiredVelocity(Vector3 position)
    {
        if (IsFinished) return Vector3.zero;
        int last = points.Count - 1;

        // Where are we along the path? Segment k runs from points[k-1] to points[k]. If we are nearer
        // a later segment (we cut a corner), or inside WaypointRadius of the current corner, move on.
        // Never aim at a corner we have already passed: that is what causes U-turns and jitter.
        for (int k = target + 1; k <= Mathf.Min(target + 2, last); k++)
            if (DistanceToSegment(position, k) < DistanceToSegment(position, target)) target = k;
        while (target < last && Flat(points[target] - position).magnitude < WaypointRadius)
            target++;

        if (target == last)
        {
            // Final segment: Arrive. Only the last point gets braking, so corners are taken at speed.
            Vector3 toGoal = Flat(points[last] - position);
            float distance = toGoal.magnitude;
            if (distance < ArrivedRadius) { target = points.Count; return Vector3.zero; }
            float speed = MaxSpeed * Mathf.Min(1f, distance / SlowingRadius);
            return toGoal / distance * speed;
        }

        // Otherwise seek a point Lookahead metres further along the path from our closest point on it.
        Vector3 aim = PointAlongPath(ClosestPointOnSegment(position, target), Lookahead);
        Vector3 toAim = Flat(aim - position);
        return toAim.sqrMagnitude < 1e-6f ? Vector3.zero : toAim.normalized * MaxSpeed;
    }

    private Vector3 PointAlongPath(Vector3 from, float distance)
    {
        for (int k = target; k < points.Count; k++)
        {
            float segment = Vector3.Distance(from, points[k]);
            if (segment >= distance) return Vector3.MoveTowards(from, points[k], distance);
            distance -= segment;
            from = points[k];
        }
        return points[points.Count - 1];
    }

    private Vector3 ClosestPointOnSegment(Vector3 p, int k)
    {
        Vector3 a = points[k - 1], b = points[k], ab = b - a;
        float t = ab.sqrMagnitude < 1e-6f ? 1f : Mathf.Clamp01(Vector3.Dot(p - a, ab) / ab.sqrMagnitude);
        return a + ab * t;
    }

    private float DistanceToSegment(Vector3 p, int k) => Flat(ClosestPointOnSegment(p, k) - p).magnitude;

    private static Vector3 Flat(Vector3 v) => new Vector3(v.x, 0f, v.z);
}