using System;
using System.Collections.Generic;
using System.Text;

namespace OpenTraffic.Model.Graph
{
    class ShortestPath<V, E>
        where V : Vertex
        where E : GraphEdge<V>
    {
        private class VertexInfo
        {
            public VertexInfo(float d, float param,  E prev)
            {
                distance = d;
                previous = prev;
                this.param = param;
            }

            public float distance;
            public float param;
            public E previous;
        }

        private VertexInfo[] vertexs;

        private static void AddNearVisited(Dictionary<V, VertexInfo> q, VertexInfo vi, V v)
        {
            q.Add(v, vi);
        }

        private static void Update(Dictionary<V, VertexInfo> q, VertexInfo vi, V v)
        {
            if (!q.ContainsKey(v)) q.Add(v, vi);
        }

        private static void Update(Collections.TrafficHeapWithUpdate<float, V> q, VertexInfo vi, V v)
        {
            q.Update(-vi.distance, v);
        }

        private static void AddNearVisited(Collections.TrafficHeapWithUpdate<float, V> q, VertexInfo vi, V v)
        {
            q.Add(-vi.distance, v);
        }

        private static V GetMin(Dictionary<V, VertexInfo> q)
        {
            float min = float.PositiveInfinity;
            V v = null;
            foreach (KeyValuePair<V, VertexInfo> kv in q)
            {
                if (kv.Value.distance < min)
                {
                    v = kv.Key;
                    min = kv.Value.distance;
                }
            }
            if (v != null) q.Remove(v);
            return v;

        }

        private static V GetMin(Collections.TrafficHeapWithUpdate<float, V> q)
        {
            return q.Pop().Value;

        }
        public delegate float CalcCost(E e, float f, ref float param);

        // Will not work with negative ids and will allocate alot of extra memory for sparse ids.
        //
        public void Proceed(Graph.Graph<V, E> g, V v0, V stop, CalcCost calcCost, float startParam)
        {
            TrafficProfile.GetProfile().Start("init");
            int maxId = 0;
            foreach (V v in g.Nodes)
            {
                maxId = Math.Max(v.Id, maxId);
            }
            vertexs = new VertexInfo[maxId+1];
            E[][] adjLinks = new E[maxId+1][];
            //Dictionary<V, VertexInfo> Q = new Dictionary<V, VertexInfo>();
            Collections.TrafficHeapWithUpdate<float, V> Q = new Collections.TrafficHeapWithUpdate<float, V>(g.Nodes.Count);


            foreach (V v in g.Nodes)
            {
                VertexInfo vi = new VertexInfo(float.PositiveInfinity, float.NaN, null);
                vertexs[v.Id] = vi;
            }
            TrafficProfile.GetProfile().SStart("adjlinks");
            foreach (KeyValuePair<V, List<E>> links in g.AdjLinks)
            {
                adjLinks[links.Key.Id] = new E[links.Value.Count];
                int i = -1;
                foreach (E e in links.Value)
                {
                    adjLinks[links.Key.Id][++i] = e;
                }
            }

            TrafficProfile.GetProfile().SStart("iter");

            vertexs[v0.Id].distance = 0;
            vertexs[v0.Id].param = startParam;
            AddNearVisited(Q, vertexs[v0.Id], v0);
            while (Q.Count > 0)
            {
                TrafficProfile.GetProfile().Start("find min");
                V u = GetMin(Q);
                TrafficProfile.GetProfile().Stop();
                if (u == null) return;
                if (u == stop) return;
                VertexInfo vi1 = vertexs[u.Id];
                float du = vi1.distance;
                if (float.IsPositiveInfinity(du)) return;
                
                foreach (E e in adjLinks[u.Id])
                {
                    TrafficProfile.GetProfile().Start("cost calculation");
                    float param = vi1.param;
                    float f = calcCost(e, du, ref param);
                    TrafficProfile.GetProfile().SStart("updating");
                    VertexInfo vi2 = vertexs[e.Target.Id];
                    if (du + f < vi2.distance)
                    {
                        vi2.distance = du + f;
                        vi2.previous = e;
                        vi2.param = param;
                        if (float.IsPositiveInfinity(vi2.distance))
                        {
                            AddNearVisited(Q, vi2, e.Target);
                        }
                        else
                        {                         
                           Update(Q, vi2, e.Target);
                        }
                    }
                    TrafficProfile.GetProfile().Stop();
                }
            }
            TrafficProfile.GetProfile().Stop();
        }

        public List<E> ExtractRoute(V start, V stop)
        {
            List<E> route = new List<E>();
            V v = stop;
            while (v != start)
            {
                E e = vertexs[v.Id].previous;
                route.Insert(0, e);
                v = e.Source;
            }
            return route;
        }
    }
}