using Pathfinding;
using Pathfinding.Util;
using Unity.Collections;
using Unity.Mathematics;
using UnityEngine;

public class AstarAINav : DAINav
{
    [SerializeField] private Seeker _seeker;
    public Seeker Seeker
    {
        get { return _seeker; }
        set { _seeker = value; }
    }

    [SerializeField] private RangeF _repathInterval = new RangeF(0.25f, 2f);
    public RangeF RepathInterval
    {
        get { return _repathInterval; }
        set { _repathInterval = value; }
    }

    [SerializeField] private float _repathDist = 5f;
    public float RepathDist
    {
        get { return _repathDist; }
        set { _repathDist = value; }
    }

    [SerializeField] private float _pathLeadRadius = 1.5f;
    public float PathLeadRadius
    {
        get { return _pathLeadRadius; }
        set { _pathLeadRadius = value; }
    }

    [SerializeField] private float _reachedPathEndDist = 0.333f;
    public float ReachedPathEndDist
    {
        get { return _reachedPathEndDist; }
        set { _reachedPathEndDist = value; }
    }

    [SerializeField] private float _autoStraightPathDist = 1f;
    public float AutoStraightPathDist
    {
        get { return _autoStraightPathDist; }
        set { _autoStraightPathDist = value; }
    }

    [SerializeField] private float _allowedStraightPenetrateDist = 0.5f;
    public float AllowedStraightPenetrateDist
    {
        get { return _allowedStraightPenetrateDist; }
        set { _allowedStraightPenetrateDist = value; }
    }
    
    public static NativeMovementPlane MovementPlane => new NativeMovementPlane(SimpleMovementPlane.XYPlane);

    public Vector2 LastPathGoal { get; private set; }
    public float LastPathTime { get; private set; }
    public Path Path { get; private set; }
    public PathTracer Tracer { get; private set; }

    public override bool HasStraightPathTo(Vector2 goalPos)
    {
        Vector2 startPos = Character.Position;
        if (Vector2.Distance(startPos, goalPos) <= AutoStraightPathDist) return true;

        if (!AstarPath.active || AstarPath.active.graphs.Length <= 0) return true;
        var graph = AstarPath.active.graphs[0];
        RecastGraph recast = (graph is RecastGraph) ? (RecastGraph)graph : null;
        if (recast == null) return true;

        GraphHitInfo hitInfo;
        if (recast.Linecast(Character.Position, goalPos, null, out hitInfo))
        {
            // Characters near walls may have their point slightly in the dead zone
            Vector2 hitPoint = Utils2D.V3toV2(hitInfo.point);
            return Vector2.Distance(goalPos, hitPoint) <= AllowedStraightPenetrateDist;
        }

        return true;
    }

    private Vector2 _lastNavigationVec;
    public override Vector2 NavigateTowards(Vector2 goalPos)
    {
        Vector2 pos = Character.Position;
#if UNITY_EDITOR
        Debug.DrawLine(Utils2D.V2toV3(pos, -0.5f), Utils2D.V2toV3(goalPos, -0.5f), new Color(1, 0.5f, 0f, 0.5f), 1/30f);
#endif

        if (AstarPath.active && Vector2.Distance(goalPos, pos) > ReachedPathEndDist)
        {
            float delta = Time.time - LastPathTime;
            float dist = Vector2.Distance(goalPos, LastPathGoal);
            bool repath = false;
            if (delta > ((dist > RepathDist) ? RepathInterval.Min : RepathInterval.Max))
            {
                repath = true;
            }
            if (Path != null && Tracer.hasPath)
            {
                Tracer.UpdateStart(Utils2D.V2toV3(pos), PathTracer.RepairQuality.High, MovementPlane, Path.traversalProvider, Path);
                Tracer.UpdateEnd(Utils2D.V2toV3(pos), PathTracer.RepairQuality.High, MovementPlane, Path.traversalProvider, Path);
                if (Tracer.isStale) repath = true;
            }
            if (Seeker != null && repath)
            {
                LastPathGoal = goalPos;
                LastPathTime = Time.time;
                Seeker.StartPath(pos, goalPos, OnPathComplete);
            }
        }
        else ClearPath();

        if (Path != null && Tracer.hasPath)
        {
            var buffer = new NativeList<float3>(Allocator.Temp);
            NativeArray<int> scratchArray = default;
            Tracer.GetNextCorners(buffer, 5, ref scratchArray, Allocator.Temp, Path.traversalProvider, Path);
            scratchArray.Dispose();

            float leadRadius = PathLeadRadius;
            Vector2 pPos = pos;
            for(int i = 0; i < buffer.Length && leadRadius > 0; i++)
            {
                Vector2 bPos = Utils2D.V3toV2(Utils2D.ToV3(buffer[i]));
                float dist = Vector2.Distance(pPos, bPos);
                pPos = dist > leadRadius ? Vector2.Lerp(pPos, bPos, leadRadius / dist) : bPos;
                leadRadius -= dist;
            }
            buffer.Dispose();
            _lastNavigationVec = (pPos - pos).normalized;

#if UNITY_EDITOR
            Debug.DrawLine(Utils2D.V2toV3(pos, -0.5f), Utils2D.V2toV3(pos + _lastNavigationVec, -0.5f), new Color(0, 1f, 0f, 1f), 1 / 30f);
#endif
            return _lastNavigationVec;
        }

        _lastNavigationVec = Vector2.ClampMagnitude(goalPos - pos, PathLeadRadius) / PathLeadRadius;
#if UNITY_EDITOR
        Debug.DrawLine(Utils2D.V2toV3(pos, -0.5f), Utils2D.V2toV3(pos + _lastNavigationVec, -0.5f), new Color(1, 1f, 0f, 1f), 1 / 30f);
#endif
        return _lastNavigationVec;
    }

    public virtual void OnPathComplete(Path p)
    {
        if (Path != null)
        {
            Path.Release(this);
            Tracer.Clear();
        }
        Path = null;
        if (!isActiveAndEnabled) return;

        if (p.error || p.vectorPath.Count <= 0) return;
        if (!(p is ABPath))
        {
            GameLog.Errorf("Path incorrect type (not ABPath): {0}", p.GetType());
            return;
        }
        ABPath abP = p as ABPath;

        Path = p;
        Path.Claim(this);

        Vector2 pos = Character.Position;
        // If only one point, add current pos as start
        if (p.vectorPath != null && p.vectorPath.Count == 1) p.vectorPath.Insert(0, pos);
        
        var parts = Funnel.SplitIntoParts(Path);
        Tracer.SetPath(parts, abP.path, abP.originalStartPoint, abP.originalEndPoint, MovementPlane, Path.traversalProvider, Path);
        ListPool<Funnel.PathPart>.Release(ref parts);
    }

    public virtual void ClearPath()
    {
        if (Path != null) Path.Release(this);
        Path = null;
        Tracer.Clear();

        if (Seeker) Seeker.CancelCurrentPathRequest();
    }

    public override void OnEnable()
    {
        base.OnEnable();
        Tracer = new PathTracer(Allocator.Persistent);
        LastPathTime = Time.time - (RepathInterval.Max + 1);
    }

    public override void OnDisable()
    {
        ClearPath();
        Tracer.Dispose();
        base.OnDisable();
    }

    public virtual void OnDrawGizmos()
    {
        if (Path != null)
        {
            Gizmos.color = Color.green;
            var vecs = Path.vectorPath;
            if(vecs != null && vecs.Count >= 2)
            {
                for (int i = 1; i < vecs.Count; i++) Gizmos.DrawLine(vecs[i - 1], vecs[i]);
            }
        }
        Gizmos.color = Color.yellow;
        Gizmos.DrawLine(transform.position, transform.position + Utils2D.V2toV3(_lastNavigationVec));
    }
}