#define ADAPTIVE

namespace ADAPTPlanning.UnityTest
{
    using System.Collections.Generic;
    using UnityEngine;
    using ADAPTPlanning.Planners;

    #if ADAPTIVE
    using TS = ADAPTPlanning.Domains.AdaptiveHighwayState;
    #else
    using TS = State;
    #endif

    public partial class PlannerGizmoAdaptive
    {
        /// <summary>
        /// Whether to show the resulting plan gizmos.
        /// </summary>
        public bool ShowPlan = true;

        /// <summary>
        /// Whether to show the endpoint state gizmos.
        /// </summary>
        public bool ShowEndpoints = true;

        /// <summary>
        /// Whether to show the open state transitions.
        /// </summary>
        public bool ShowOpen = true;

        /// <summary>
        /// Whether to show the inconsistent state transitions.
        /// </summary>
        public bool ShowIncons = true;

        /// <summary>
        /// Whether to show the invalid state transitions.
        /// </summary>
        public bool ShowInvalid = true;

        /// <summary>
        /// Whether to show the visited state transitions.
        /// </summary>
        public bool ShowVisited = true;

        /// <summary>
        /// Whether to show cost debug text.
        /// </summary>
        public bool ShowText;

        /// <summary>
        /// Draw all of the gizmos that show the current state of the planner.
        /// </summary>
        void OnDrawGizmos()
        {
            var cubesize = new UnityEngine.Vector3(0.5f, 0.5f, 0.5f);
            if (Planner != null) {
                DrawVisited();
                DrawIncons();
                DrawOpen(cubesize);
                DrawInvalid(cubesize);
                DrawText();
                DrawPlan();
            }
            DrawEndpoints(cubesize);
        }

        void DrawVisited()
        {
            var visited = Planner.Visited;
            if (!ShowVisited || visited == null) {
                return;
            }

            Gizmos.color = Color.magenta;
            foreach (var t in visited) {
                if (t == null) {
                    continue;
                }
                Gizmos.DrawLine(t.From.Position.ToUnity(),
                    t.To.Position.ToUnity());
            }

            var arastar = Planner as ARAStar<TS>;
            if (arastar != null) {
                Gizmos.color = Color.red;
                foreach (var n in arastar.VisitedNodes) {
                    var t = n.Transition;
                    if (t != null && n.Closed) {
                        Gizmos.DrawLine(t.From.Position.ToUnity(),
                            t.To.Position.ToUnity());
                    }
                }
            }

            var dijkstra = Planner as Dijkstra<TS>;
            if (dijkstra != null) {
                Gizmos.color = Color.red;
                foreach (var n in dijkstra.VisitedNodes) {
                    var t = n.Transition;
                    if (t != null && dijkstra.Closed.Contains(n)) {
                        Gizmos.DrawLine(t.From.Position.ToUnity(),
                            t.To.Position.ToUnity());
                    }
                }
            }

            var adstar = Planner as ADStar<TS>;
            if (adstar != null) {
                Gizmos.color = Color.red;
                foreach (var n in adstar.VisitedNodes) {
                    var t = n.Transition;
                    if (t != null && adstar.Closed.Contains(n)) {
                        Gizmos.DrawLine(t.From.Position.ToUnity(),
                            t.To.Position.ToUnity());
                    }
                }
            }
        }

        void DrawIncons()
        {
            if (!ShowIncons) {
                return;
            }

            var arastar = Planner as ARAStar<TS>;
            if (arastar != null && arastar.InconsNodes != null) {
                Gizmos.color = Color.yellow;
                foreach (var n in arastar.InconsNodes) {
                    var t = n.Transition;
                    Gizmos.DrawLine(t.From.Position.ToUnity(),
                        t.To.Position.ToUnity());
                }
            }

            var adstar = Planner as ADStar<TS>;
            if (adstar != null && adstar.Incons != null && adstar.Incons.Count != 0) {
                Gizmos.color = Color.yellow;
                foreach (var n in adstar.Incons) {
                    var t = n.Transition;
                    Gizmos.DrawLine(t.From.Position.ToUnity(),
                        t.To.Position.ToUnity());
                }
            }
        }

        void DrawInvalid(UnityEngine.Vector3 cubesize)
        {
            if (!ShowInvalid) {
                return;
            }

            var pl = Planner as PlannerDynamic<Domain<TS>, TS>;
            if (pl != null && pl.InvalidStates != null && pl.InvalidStates.Count != 0) {
                Gizmos.color = Color.gray;
                foreach (var s in pl.InvalidStates) {
                    Gizmos.DrawCube(s.Position.ToUnity(), cubesize);
                }
            }
        }

        void DrawOpen(UnityEngine.Vector3 cubesize)
        {
            var open = Planner.Open;
            if (!ShowOpen || open == null) {
                return;
            }

            Gizmos.color = Color.green;
            foreach (var s in open) {
                Gizmos.DrawCube(s.Position.ToUnity(), cubesize);
            }
        }

        void DrawText()
        {
            if (!(ShowText && ShowVisited)) {
                return;
            }

            var dijkstra = Planner as Dijkstra<TS>;
            if (dijkstra != null && dijkstra.VisitedNodes != null) {
                foreach (var n in dijkstra.VisitedNodes) {
                    var ce = float.IsInfinity(n.CostEstimate) ?
                        "" : n.CostEstimate.ToString("00.0");
                    TextGizmo.Draw(n.State.Position.ToUnity(), ce);
                }
                return;
            }

            var astar = Planner as AStar<TS>;
            if (astar != null && astar.VisitedNodes != null) {
                foreach (var n in astar.VisitedNodes) {
                    var pc = float.IsInfinity(n.PastCost) ?
                        "" : n.PastCost.ToString("00.0");
                    var ce = float.IsInfinity(n.CostEstimate) ?
                        "" : n.CostEstimate.ToString("00.0");
                    var s = string.Format("{0}\n{1}", pc, ce);
                    TextGizmo.Draw(n.State.Position.ToUnity(), s);
                }
                return;
            }

            var adstar = Planner as ADStar<TS>;
            if (adstar != null && adstar.VisitedNodes != null) {
                foreach (var n in adstar.VisitedNodes) {
                    var pc = float.IsInfinity(n.PastCost) ?
                        "" : n.PastCost.ToString("00.0");
                    var ce = float.IsInfinity(n.OneStepLookaheadCost) ?
                        "" : n.OneStepLookaheadCost.ToString("00.0");
                    var s = string.Format("{0}\n{1}", pc, ce);
                    TextGizmo.Draw(n.State.Position.ToUnity(), s);
                }
                return;
            }
        }

        void DrawPlan()
        {
            var plan = Planner.Plan;
            if (!ShowPlan || plan == null) {
                return;
            }
            Gizmos.color = Planner.Status.Path == Status.PathStatus.Optimal ?
                Color.blue : Color.cyan;
            foreach (var t in plan) {
                Gizmos.DrawLine(t.From.Position.ToUnity(),
                    t.To.Position.ToUnity());
            }
        }

        void DrawEndpoints(UnityEngine.Vector3 cubesize)
        {
            if (!ShowEndpoints) {
                return;
            }
            Gizmos.color = Color.blue;
            if (S1 != null) {
                Gizmos.DrawCube(S1.Position.ToUnity(), cubesize);
            }
            if (S2 != null) {
                Gizmos.DrawCube(S2.Position.ToUnity(), cubesize);
            }
        }
    }
}