using System;
using System.Collections.Generic;
using System.Text;
using System.Xml;
using System.IO;

/* Fetch .osm
 wget http://download.geofabrik.de/osm/europe/czech_republic.osm.bz2
bzcat czech*.osm.bz | osmosis \
	--read-xml enableDateParsing=no file=/dev/stdin \
	--way-key-value keyValueList="highway.motorway,highway.motorway_link,highway.trunk,highway.trunk_link,highway.primary,highway.primary_link,highway.secondary,highway.secondary_link,highway.tertiary,highway.unclassified,highway.road,highway.residential" \
	--used-node \
	--write-xml file=cz_silnice.osm
 */

//Openstreetmap datafile example
//<node id='343700067' lat='14.123' lon='50.123'></node>
//<way id='53696241' timestamp='2010-03-29T23:03:19Z' uid='248441' user='Jandrs' visible='true' version='1' changeset='4272445'>
//  <nd ref='343700067' />
//  <nd ref='343724018' />
//  <tag k='highway' v='residential' />
//  <tag k='oneway' v='yes' />
//  <tag k='source' v='uhul:ortofoto' />
//</way>


namespace OsmDijkstraRouter
{
    class Program
    {

        private static Dictionary<long, Vertex> nodesById;

        static void Main(string[] args)
        {
            System.Threading.Thread.CurrentThread.CurrentCulture = System.Globalization.CultureInfo.InvariantCulture;

            Console.WriteLine("Semester work in csharp at Charles University Prague");
            Console.WriteLine("   FINDING THE SHORTEST ROUTE USING DIJKSTRA ALGORITHM");
            Console.WriteLine();
            Console.WriteLine("code (c) Pavel Zbytovsky <pavel@zby.cz> 2010, GPLv3");
            Console.WriteLine("data (c) openstreetmap.org and contributors, CC-BY-SA ");
            Console.WriteLine();
            Console.WriteLine();

            //args = new String[3];
            //args[0] = "../highways-hostivice.osm";
            //args[1] = "105018767";
            //args[2] = "21312439";

            //check args and set strStartTarget + xmlreader
            #region args handling
            if (args.Length != 1 && args.Length != 3)
                CwExit("Usage: <file.osm> [<start-id>-<target-id>]");

            //todo: start and target as coordinates (getting the closest) ... easy
            string strStartTarget = "";
            if (args.Length == 3)
                strStartTarget = args[1]+" "+args[2];

            XmlTextReader reader = null;
            if(File.Exists(args[0]))
                reader = new XmlTextReader(args[0]); //read xmldokumentu  (http://support.microsoft.com/kb/307548)
            else
                CwExit("The osm datafile does not exist.");
            #endregion
            
            //nodes - creating new <Vertex>es and adding them to notVisited and nodesById; set start/target
            #region loading all <node>s

            nodesById = new Dictionary<long, Vertex>();
            bool skip = false;
            while (reader.Read())
            {
                skip = false;
                if (reader.NodeType != XmlNodeType.Element) continue;
                if (reader.Name == "way") break;
                if (reader.Name != "node") continue;

                Vertex tmpVertex = new Vertex();  //the only factory for Vertexes
                tmpVertex.distance = Double.MaxValue;

                while (reader.MoveToNextAttribute())
                { // reading attributes
                    switch (reader.Name)
                    {
                        case "id":
                            tmpVertex.id = Convert.ToInt64(reader.Value);
                            break;
                        case "lat":
                            tmpVertex.lat = double.Parse(reader.Value);
                            break;
                        case "lon":
                            tmpVertex.lon = double.Parse(reader.Value);
                            break;
                        case "action":
                            if (reader.Value == "delete")
                                skip = true;                    //NOTE: csharp cant continue outer loop?
                            break;
                    }
                }
                if (skip) continue;

                //adds to hastable
                nodesById.Add(tmpVertex.id, tmpVertex);

                //fancy output
                if (nodesById.Count % 10000 == 0)
                    Console.Write("\rLoading <node>s... {0}        ", nodesById.Count);
            }
            Console.WriteLine("\rLoading <node>s... {0}        ", nodesById.Count);
            #endregion

            //way - just adding neighbors to Vertexes
            #region loading all <way>s
            if (reader.Name != "way")
                CwExit("No <way> found in openstreetmap data file.");

            LinkedList<Vertex> way = new LinkedList<Vertex>(); //temporary way holding all nodes
            short oneway = 0;
            int waysCount = 0;
            do
            {
                switch (reader.NodeType)
                {
                    case XmlNodeType.Element:
                        //if (reader.Name == "way") nothing;
                        if (reader.Name == "nd") //<nd ref='...'> add node to temporary way if found
                        {
                            reader.MoveToNextAttribute();
                            long nodeId = long.Parse(reader.Value);

                            try { way.AddLast(nodesById[nodeId]); }
                            catch (KeyNotFoundException) { }
                        }
                        else if (reader.Name == "tag") //<tag k='oneway' v='...'> set oneway=-1/0/1
                        {
                            reader.MoveToNextAttribute();
                            if (reader.Value == "oneway")
                            {
                                reader.MoveToNextAttribute();
                                if (reader.Value == "yes" || reader.Value == "1")
                                    oneway = 1;
                                else if (reader.Value == "-1")
                                    oneway = -1;
                            }
                        }

                        break;

                    case XmlNodeType.EndElement:                //handle </way> .. add the neighbors
                        if (reader.Name == "way") //we dont like </osm>
                        {
                            Vertex u, w;
                            LinkedListNode<Vertex> item = way.First;

                            if(item != null)
                            while (item.Next != null)  //iterating through temporary way
                            {
                                u = item.Value;
                                w = item.Next.Value;
                                if (oneway != -1)
                                    u.neighbors.AddLast(w); //add the forward neighbor
                                if (oneway != 1)
                                    w.neighbors.AddLast(u); //add the backward neighbor
                                item = item.Next;
                            }

                            way.Clear();
                            oneway = 0;

                            //fancy output
                            if (++waysCount % 1000 == 0)
                                Console.Write("\rLoading <way>s... {0}        ", waysCount);
                        }
                        break;
                }
            } while (reader.Read());
            Console.WriteLine("\rLoading <way>s... {0}        ", waysCount);
            Console.WriteLine();
            #endregion

            //invoking the RoutingAlgorithm
            #region console usage & interactive mode
            if (strStartTarget.Length > 1)
            {
                RoutingAlgorithm(strStartTarget);   //console usage
            }
            else //interactive usage
            {
                while (true)
                {
                    Console.WriteLine("Please specify start and target ids on one line: (for exit hit Enter)");
                    strStartTarget = Console.ReadLine().Trim();
                    if (strStartTarget.Length == 0) break;

                    RoutingAlgorithm(strStartTarget);
                    Console.WriteLine();

                    foreach (KeyValuePair<long,Vertex> item in nodesById)
                    {
                        Vertex v = item.Value;
                        v.distance = double.MaxValue;
                        v.previous = null;
                    }

                }
            }
            #endregion  

            Console.ReadKey();
        }

        public static void RoutingAlgorithm(string strStartTarget)
        {
            //parse start & target + initialize notVisited
            #region start/target handling
            long startId=0, targetId=0;
            Vertex start = null, target = null;

            try
            {
                startId = Convert.ToInt64(strStartTarget.Split(' ')[0]);
                targetId = Convert.ToInt64(strStartTarget.Split(' ')[1]);
            }
            catch (Exception)
            {
                CwExit("start/target ids are not numbers.");
            }


            //start and target
            target = nodesById[targetId];
            start = nodesById[startId];
            start.distance = 0;
            //do we have destinations?
            if (start == null || target == null)
                CwExit("Start or target not found in openstreetmap data file.");

            GPWiki.BinaryHeap<Vertex> notVisited = new GPWiki.BinaryHeap<Vertex>();
            notVisited.Add(start);
            #endregion

            // ---- dijkstra algorithm ----
            //each Vertex in nodesById has previous=null and distance=MaxValue (start is in notVisited, distance=0)
            #region dijkstra alogirthm
            while (notVisited.Count > 0)
            {
                Vertex u = notVisited.Remove();
                if (u.distance == double.MaxValue)           // all remaining vertices are inaccessible from source
                    break;

                if (u == target)                // target reached with shortest way
                    break;

                foreach (Vertex v in u.neighbors)
                {
                    double distThroughU = u.distance + Distance(u, v);

                    if (v.distance == double.MaxValue)  //never been reached
                    {
                        v.distance = distThroughU;
                        v.previous = u;
                        notVisited.Add(v);
                    }
                    else if (distThroughU < v.distance)  //found shorter way, relaxing the edge
                    {
                        v.distance = distThroughU;
                        v.previous = u;
                        notVisited.CustomUpHeap(v);
                    }
                }



                //fancy output
                if (notVisited.Count % 50 == 0)
                    Console.Write("\rNot visited count: {0}        ", notVisited.Count);
            }
            Console.WriteLine("\rNot visited count: {0}        ", notVisited.Count);
            #endregion

            //outputs KML, GPX and console info
            #region final output
            StreamWriter swKml = new StreamWriter("output"+strStartTarget+".kml", false);
            StreamWriter swGpx = new StreamWriter("output"+strStartTarget+".gpx", false);
            swKml.WriteLine("<?xml version='1.0' encoding='UTF-8'?><kml xmlns='http://www.opengis.net/kml/2.2' xmlns:gx='http://www.google.com/kml/ext/2.2'><Placemark><LineString><tessellate>1</tessellate><coordinates>");
            swGpx.WriteLine("<?xml version='1.0' encoding='UTF-8'?><gpx><trk><trkseg>");
            Vertex x = target;
            int i = 0;
            while (x.previous != null)  //writing backwards, but doesnt matter
            {
                //Console.WriteLine("<{0}> {1} {2}", x.id, x.lat, x.lon);
                swKml.Write("{1},{0},0 ", x.lat, x.lon);
                swGpx.Write("<trkpt lat='{0}' lon='{1}' />", x.lat, x.lon);
                x = x.previous;
                i++;
            }
            Console.WriteLine("Done - {0} nodes, {1} km", i, target.distance == double.MaxValue ? 0 : target.distance);

            swKml.WriteLine("{1},{0},0", x.lat, x.lon);
            swKml.WriteLine("</coordinates></LineString><name>Length: {0} km</name></Placemark></kml>", target.distance);
            swKml.Close();

            swGpx.Write("<trkpt lat='{0}' lon='{1}' />", x.lat, x.lon);
            swGpx.WriteLine("</trkseg></trk></gpx>");
            swGpx.Close();
            #endregion

        }

        #region other methods
        //http://www.movable-type.co.uk/scripts/latlong.html
        public static double Distance(Vertex c1, Vertex c2)
        {
            double R = 6371; // km
            double dLat = ToRad(c2.lat - c1.lat);
            double dLon = ToRad(c2.lon - c1.lon); //c2.lon - c2.lon -> acts funny - taking short detours
            double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
                    Math.Cos(ToRad(c1.lat)) * Math.Cos(ToRad(c2.lat)) *
                    Math.Sin(dLon / 2) * Math.Sin(dLon / 2);
            double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
            return R * c;
        }

        public static double ToRad(double deg)
        {
            return deg * (Math.PI / 180);
        }

        static public void CwExit(string s)
        {
            Console.WriteLine();
            Console.WriteLine(s);
            Console.ReadKey();
            Environment.Exit(-1);
        }
        #endregion

    }

    class Vertex : IComparable<Vertex>, GPWiki.IBinaryHeapIndexed
    {
        public double lat;
        public double lon;
        public long id;
        public LinkedList<Vertex> neighbors;
        public double distance;// = double.MaxValue;
        public Vertex previous;

        public Vertex()
        {
            this.neighbors = new LinkedList<Vertex>();
        }

        #region IComparable Members
        public int CompareTo(Vertex obj)
        {
            return distance.CompareTo(obj.distance);
        }
        #endregion

        #region IBinaryHeapIndexed Members
        private int binaryHeapIndex;
        public int BinaryHeapIndex
        {
            get { return binaryHeapIndex; }
            set { binaryHeapIndex = value; }
        }
        #endregion
    }

}



namespace GPWiki
{
    //source http://gpwiki.org/index.php/C_sharp:BinaryHeapOfT

    #region IBinaryHeapIndexed interface
    /// <summary>
    ///  For instant access of CustomUpHeap of any item  [by Pavel Zbytovský]
    /// </summary>
    public interface IBinaryHeapIndexed
    {
        int BinaryHeapIndex { get; set; }
    }
    #endregion


    /// <summary>
    /// A binary heap, useful for sorting data and priority queues.
    /// </summary>
    /// <typeparam name="T"><![CDATA[IComparable<T> type of item in the heap]]>.</typeparam>
    public class BinaryHeap<T> : ICollection<T>
                      where T : IBinaryHeapIndexed, IComparable<T>
    {
        // Constants
        private const int DEFAULT_SIZE = 4;
        // Fields
        private T[] _data = new T[DEFAULT_SIZE];
        private int _count = 0;
        private int _capacity = DEFAULT_SIZE;
        private bool _sorted;

        // Properties
        /// <summary>
        /// Gets the number of values in the heap. 
        /// </summary>
        public int Count
        {
            get { return _count; }
        }
        /// <summary>
        /// Gets or sets the capacity of the heap.
        /// </summary>
        public int Capacity
        {
            get { return _capacity; }
            set
            {
                int previousCapacity = _capacity;
                _capacity = Math.Max(value, _count);
                if (_capacity != previousCapacity)
                {
                    T[] temp = new T[_capacity];
                    Array.Copy(_data, temp, _count);
                    _data = temp;
                }
            }
        }
        // Methods
        /// <summary>
        /// Creates a new binary heap.
        /// </summary>
        public BinaryHeap()
        {
        }
        private BinaryHeap(T[] data, int count)
        {
            Capacity = count;
            _count = count;
            Array.Copy(data, _data, count);
        }
        /// <summary>
        /// Gets the first value in the heap without removing it.
        /// </summary>
        /// <returns>The lowest value of type TValue.</returns>
        public T Peek()
        {
            return _data[0];
        }

        /// <summary>
        /// Removes all items from the heap.
        /// </summary>
        public void Clear()
        {
            this._count = 0;
            _data = new T[_capacity];
        }

        private void UpdateItemIndex(int index)
        //Updates BinaryHeapIndex in item T via IBinaryHeapIndexed interface; needed for CustomUpHeap()
        {
            _data[index].BinaryHeapIndex = index;
        }

        /// <summary>
        /// Adds a key and value to the heap.
        /// </summary>
        /// <param name="item">The item to add to the heap.</param>
        public void Add(T item)
        {
            if (_count == _capacity)
            {
                Capacity *= 2;
            }
            _data[_count] = item;
            UpdateItemIndex(_count);
            UpHeap();
            _count++;
        }

        /// <summary>
        /// Removes and returns the first item in the heap.
        /// </summary>
        /// <returns>The next value in the heap.</returns>
        public T Remove()
        {
            if (this._count == 0)
            {
                throw new InvalidOperationException("Cannot remove item, heap is empty.");
            }
            T v = _data[0];
            _count--;
            _data[0] = _data[_count];
            _data[_count] = default(T); //Clears the Last Node
            if(_count > 0)
                DownHeap();
            return v;
        }


        /// <summary>
        /// Performs up-heap bubbling from supplied item. [by Pavel Zbytovský]
        /// </summary>
        public void CustomUpHeap(T item)
        {
            //int p = _count;
            //T item = _data[p];
            int p = item.BinaryHeapIndex; // n-times slower: Array.IndexOf<T>(_data, item);

            int par = Parent(p);
            while (par > -1 && item.CompareTo(_data[par]) < 0) //item < parent
            {
                _data[p] = _data[par]; //Swap nodes
                UpdateItemIndex(p);
                p = par;
                par = Parent(p);
            }
            _data[p] = item;
            UpdateItemIndex(p);
            _sorted = false;
        }


        private void UpHeap()
        //helper function that performs up-heap bubbling
        {
            _sorted = false;
            int p = _count;
            T item = _data[p];
            int par = Parent(p);
            while (par > -1 && item.CompareTo(_data[par]) < 0)
            {
                _data[p] = _data[par]; //Swap nodes
                UpdateItemIndex(p);
                p = par;
                par = Parent(p);
            }
            _data[p] = item;
            UpdateItemIndex(p);

        }
        private void DownHeap()
        //helper function that performs down-heap bubbling
        {
            _sorted = false;
            int n;
            int p = 0;
            T item = _data[p];
            while (true)
            {
                int ch1 = Child1(p);
                if (ch1 >= _count) break;
                int ch2 = Child2(p);
                if (ch2 >= _count)
                {
                    n = ch1;
                }
                else
                {
                    n = _data[ch1].CompareTo(_data[ch2]) < 0 ? ch1 : ch2;
                }
                if (item.CompareTo(_data[n]) > 0)
                {
                    _data[p] = _data[n]; //Swap nodes
                    UpdateItemIndex(p);
                    p = n;
                }
                else
                {
                    break;
                }
            }
            _data[p] = item;
            UpdateItemIndex(p);
        }
        public void EnsureSort()
        {
            throw new NotImplementedException("in EnsureSort");
            //if (_sorted) return;
            //Array.Sort(_data, 0, _count);
            //!!! neeeds sth like this:     foreach p: UpdateItemIndex(p);
            //_sorted = true;
        }
        private static int Parent(int index)
        //helper function that calculates the parent of a node
        {
            return (index - 1) >> 1;
        }
        private static int Child1(int index)
        //helper function that calculates the first child of a node
        {
            return (index << 1) + 1;
        }
        private static int Child2(int index)
        //helper function that calculates the second child of a node
        {
            return (index << 1) + 2;
        }

        /// <summary>
        /// Creates a new instance of an identical binary heap.
        /// </summary>
        /// <returns>A BinaryHeap.</returns>
        public BinaryHeap<T> Copy()
        {
            return new BinaryHeap<T>(_data, _count);
        }

        /// <summary>
        /// Gets an enumerator for the binary heap.
        /// </summary>
        /// <returns>An IEnumerator of type T.</returns>
        public IEnumerator<T> GetEnumerator()
        {
            EnsureSort();
            for (int i = 0; i < _count; i++)
            {
                yield return _data[i];
            }
        }
        System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator()
        {
            return GetEnumerator();
        }

        /// <summary>
        /// Checks to see if the binary heap contains the specified item.
        /// </summary>
        /// <param name="item">The item to search the binary heap for.</param>
        /// <returns>A boolean, true if binary heap contains item.</returns>
        public bool Contains(T item)
        {
            EnsureSort();
            return Array.BinarySearch<T>(_data, 0, _count, item) >= 0;
        }
        /// <summary>
        /// Copies the binary heap to an array at the specified index.
        /// </summary>
        /// <param name="array">One dimensional array that is the destination of the copied elements.</param>
        /// <param name="arrayIndex">The zero-based index at which copying begins.</param>
        public void CopyTo(T[] array, int arrayIndex)
        {
            EnsureSort();
            Array.Copy(_data, array, _count);
        }
        /// <summary>
        /// Gets whether or not the binary heap is readonly.
        /// </summary>
        public bool IsReadOnly
        {
            get { return false; }
        }
        /// <summary>
        /// [SLOW] Removes an item from the binary heap. This utilizes the type T's Comparer and will not remove duplicates.
        /// </summary>
        /// <param name="item">The item to be removed.</param>
        /// <returns>Boolean true if the item was removed.</returns>
        public bool Remove(T item)
        {
            EnsureSort();
            int i = Array.BinarySearch<T>(_data, 0, _count, item);
            if (i < 0) return false;
            Array.Copy(_data, i + 1, _data, i, _count - i);
            _data[_count] = default(T);
            _count--;
            return true;
        }

    }
}