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

namespace Assets.Scripts.Server.Pathing
{
    /// <summary>
    /// Attach this script to any obstacle with a collider to enable dynamic updates of the graphs around it.
    /// When the object has moved or rotated at least <see cref="updateError"/> world units
    /// then it will call AstarPath.UpdateGraphs and update the graph around it.
    ///
    /// Make sure that any children colliders do not extend beyond the bounds of the collider attached to the
    /// GameObject that the DynamicGridObstacle component is attached to since this script only updates the graph
    /// around the bounds of the collider on the same GameObject.
    ///
    /// An update will be triggered whenever the bounding box of the attached collider has changed (moved/expanded/etc.) by at least <see cref="updateError"/> world units or if
    /// the GameObject has rotated enough so that the outmost point of the object has moved at least <see cref="updateError"/> world units.
    ///
    /// This script works with both 2D colliders and normal 3D colliders.
    ///
    /// Note: This script works best with a GridGraph, PointGraph or LayerGridGraph
    /// You can use this with recast graphs as well. However since recast graph updates are much slower it is recommended to use the <see cref="Pathfinding.NavmeshCut"/> component if at all possible.
    ///
    /// See: AstarPath.UpdateGraphs
    /// See: graph-updates (view in online documentation for working links)
    /// See: navmeshcutting (view in online documentation for working links)
    /// </summary>
    [HelpURL("http://arongranberg.com/astar/documentation/beta/class_pathfinding_1_1_dynamic_grid_obstacle.php")]
    public class DynamicGridPenalty : GraphModifier
    {
        private CharacterController _characterController;

        /// <summary>Cached transform component</summary>
        private Transform _tr;

        /// <summary>The minimum change in world units along one of the axis of the bounding box of the collider to trigger a graph update</summary>
        public float updateError = 1;

        /// <summary>
        /// Time in seconds between bounding box checks.
        /// If AstarPath.batchGraphUpdates is enabled, it is not beneficial to have a checkTime much lower
        /// than AstarPath.graphUpdateBatchingInterval because that will just add extra unnecessary graph updates.
        ///
        /// In real time seconds (based on Time.realtimeSinceStartup).
        /// </summary>
        public float checkTime = 0.2F;
        public float centerYOffset = 0F;
        public int penalty = 1000;

        /// <summary>Bounds of the collider the last time the graphs were updated</summary>
        private Bounds _prevBounds;

        /// <summary>Rotation of the collider the last time the graphs were updated</summary>
        private Quaternion _prevRotation;

        /// <summary>True if the collider was enabled last time the graphs were updated</summary>
        private bool _prevEnabled;

        private float _lastCheckTime = -9999;
        private readonly Queue<GraphUpdateObject> _pendingGraphUpdates = new Queue<GraphUpdateObject>();

        private Bounds Bounds => _characterController.bounds;
        /// <summary>
        /// these are the bounds after we've adjust the center to be shifted by <see cref="centerYOffset"/>
        /// </summary>
        private Bounds ShiftedBounds => new Bounds(new Vector3(Bounds.center.x, Bounds.center.y + centerYOffset, Bounds.center.z), Bounds.size);

        private bool ColliderEnabled => _characterController.enabled;

        protected override void Awake()
        {
            base.Awake();

            _characterController = GetComponentInParent<CharacterController>() ?? GetComponentInChildren<CharacterController>();
            _tr = transform;
            if (_characterController == null && Application.isPlaying)
            {
                throw new System.Exception("A CharacterController must be attached to the GameObject(" + gameObject.name + ") for the DynamicGridObstacle to work");
            }

            _prevBounds = ShiftedBounds;
            _prevRotation = _tr.rotation;
            // Make sure we update the graph as soon as we find that the collider is enabled
            _prevEnabled = false;
        }

        public override void OnPostScan()
        {
            // Make sure we find the collider
            // AstarPath.Awake may run before Awake on this component
            if (_characterController == null) Awake();

            // In case the object was in the scene from the start and the graphs
            // were scanned then we ignore the first update since it is unnecessary.
            if (_characterController != null) _prevEnabled = ColliderEnabled;
        }

        void Update()
        {
            if (!Application.isPlaying) return;

            if (_characterController == null)
            {
                Debug.LogError("No CharacterController attached to this GameObject. The DynamicGridObstacle component requires a collider.", this);
                enabled = false;
                return;
            }

            // Check if the previous graph updates have been completed yet.
            // We don't want to update the graph again until the last graph updates are done.
            // This is particularly important for recast graphs for which graph updates can take a long time.
            while (_pendingGraphUpdates.Count > 0 && _pendingGraphUpdates.Peek().stage != GraphUpdateStage.Pending)
            {
                _pendingGraphUpdates.Dequeue();
            }

            if (AstarPath.active == null || AstarPath.active.isScanning || Time.realtimeSinceStartup - _lastCheckTime < checkTime || !Application.isPlaying || _pendingGraphUpdates.Count > 0)
            {
                return;
            }

            _lastCheckTime = Time.realtimeSinceStartup;
            if (ColliderEnabled)
            {
                // The current bounds of the collider
                Bounds newBounds = ShiftedBounds;
                var newRotation = _tr.rotation;

                Vector3 minDiff = _prevBounds.min - newBounds.min;
                Vector3 maxDiff = _prevBounds.max - newBounds.max;

                var extents = newBounds.extents.magnitude;
                // This is the distance that a point furthest out on the bounding box
                // would have moved due to the changed rotation of the object
                var errorFromRotation = extents * Quaternion.Angle(_prevRotation, newRotation) * Mathf.Deg2Rad;

                // If the difference between the previous bounds and the new bounds is greater than some value, update the graphs
                if (minDiff.sqrMagnitude > updateError * updateError || maxDiff.sqrMagnitude > updateError * updateError ||
                    errorFromRotation > updateError || !_prevEnabled)
                {
                    // Update the graphs as soon as possible
                    DoUpdateGraphs();
                }
            }
            else
            {
                // Collider has just been disabled
                if (_prevEnabled)
                {
                    DoUpdateGraphs();
                }
            }
        }

        /// <summary>
        /// Revert graphs when disabled.
        /// When the DynamicObstacle is disabled or destroyed, a last graph update should be done to revert nodes to their original state
        /// </summary>
        protected override void OnDisable()
        {
            base.OnDisable();
            if (AstarPath.active != null && Application.isPlaying)
            {
                var guo = new GraphUpdateObject(_prevBounds);
                _pendingGraphUpdates.Enqueue(guo);
                AstarPath.active.UpdateGraphs(guo);
                _prevEnabled = false;
            }

            // Stop caring about pending graph updates if this object is disabled.
            // This avoids a memory leak since `Update` will never be called again to remove pending updates
            // that have been completed.
            _pendingGraphUpdates.Clear();
        }

        /// <summary>
        /// Update the graphs around this object.
        /// Note: The graphs will not be updated immediately since the pathfinding threads need to be paused first.
        /// If you want to guarantee that the graphs have been updated then call <see cref="AstarPath.FlushGraphUpdates"/>
        /// after the call to this method.
        /// </summary>
        public void DoUpdateGraphs()
        {
            if (_characterController == null) return;

            // Required to ensure we get the most up to date bounding box from the physics engine
            UnityEngine.Physics.SyncTransforms();
            UnityEngine.Physics2D.SyncTransforms();

            if (!ColliderEnabled)
            {
                // If the collider is not enabled, then col.bounds will empty
                // so just update prevBounds
                var guo = CreateGraphUpdateObject(_prevBounds);
                _pendingGraphUpdates.Enqueue(guo);
                AstarPath.active.UpdateGraphs(guo);
            }
            else
            {
                Bounds newBounds = ShiftedBounds;

                Bounds merged = newBounds;
                merged.Encapsulate(_prevBounds);
                _oldMergedBounds = merged;

                // Check what seems to be fastest, to update the union of prevBounds and newBounds in a single request
                // or to update them separately, the smallest volume is usually the fastest
                if (BoundsVolume(merged) < BoundsVolume(newBounds) + BoundsVolume(_prevBounds))
                {
                    // Send an update request to update the nodes inside the 'merged' volume
                    var guo = CreateGraphUpdateObject(merged);
                    _pendingGraphUpdates.Enqueue(guo);
                    AstarPath.active.UpdateGraphs(guo);
                }
                else
                {
                    // Send two update request to update the nodes inside the 'prevBounds' and 'newBounds' volumes
                    var guo1 = CreateGraphUpdateObject(_prevBounds);
                    var guo2 = CreateGraphUpdateObject(newBounds);
                    _pendingGraphUpdates.Enqueue(guo1);
                    _pendingGraphUpdates.Enqueue(guo2);
                    AstarPath.active.UpdateGraphs(guo1);
                    AstarPath.active.UpdateGraphs(guo2);
                }
                _prevBounds = newBounds;
            }

            _prevEnabled = ColliderEnabled;
            _prevRotation = _tr.rotation;

            // Set this here as well since the DoUpdateGraphs method can be called from other scripts
            _lastCheckTime = Time.realtimeSinceStartup;
        }

        private Bounds _oldMergedBounds;
        private void OnDrawGizmos()
        {
            Gizmos.DrawWireCube(_oldMergedBounds.center, _oldMergedBounds.size);
        }

        private GraphUpdateObject CreateGraphUpdateObject(Bounds bounds)
        {
            return new GraphUpdateObject(bounds) { addPenalty = penalty, resetPenaltyOnPhysics = true, updateErosion = true, updatePhysics = true };
        }

        /// <summary>Volume of a Bounds object. X*Y*Z</summary>
        static float BoundsVolume(Bounds b)
        {
            return System.Math.Abs(b.size.x * b.size.y * b.size.z);
        }
    }
}