using libsecondlife;
using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;

namespace AStar
{

    public class Pathfinding
    {

        private class PathNode
        {
            private LLVector3 _Position;

            /// <summary>
            /// A container for a node position which includes details for calculating movement cost
            /// </summary>
            /// <param name="position"></param>
            /// <param name="parent"></param>
            public PathNode(LLVector3 position)
            {
                _Position = position;
            }

            /// <summary>
            /// The coordinates of this node
            /// </summary>
            public LLVector3 Position
            {
                get { return _Position; }
            }

            /// <summary>
            /// The parent of this node
            /// </summary>
            public LLVector3 Parent;

            /// <summary>
            /// The cost of moving from to this node from its parent (10 if adjacent, 14 if diagonal)
            /// </summary>
            public int G;

            /// <summary>
            /// The "Manhattan method" score for this node in relation to the target position
            /// </summary>
            public int H;

            /// <summary>
            /// The total cost of chosing this node
            /// </summary>
            public int F;
        }

        /// <summary>
        /// Checks the specified location on the wander map to see if we are permitted to go there
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <returns>true if we are allowed to go there, false if we are not</returns>
        private static bool MapCheckPosition(Bitmap map, LLVector3 position)
        {
            int x = (int)position.X;
            int y = 255 - (int)position.Y;
            int z = (int)position.Z;

            if (map == null || x > 255 || y > 255 || x < 0 || y < 0) return false;

            List<Color> pixels = new List<Color>();
            pixels.Add(map.GetPixel(x, y));
            //if (y + 1 <= 255) pixels.Add(map.GetPixel(x, y + 1));
            //if (y - 1 >= 0) pixels.Add(map.GetPixel(x, y - 1));
            //etc... add all 9

            foreach (Color c in pixels)
            {
                if (c.R != 00 || c.B != 00 || c.G != 00) return false;
            }
            return true;
        }

        /// <summary>
        /// Returns the an efficient path from pointA to pointB
        /// </summary>
        /// <param name="target"></param>
        /// <returns></returns>
        public static List<LLVector3> AStarPath(Bitmap map, LLVector3 pointA, LLVector3 pointB)
        {
            pointA = new LLVector3((int)pointA.X, (int)pointA.Y, (int)pointA.Z);
            pointB = new LLVector3((int)pointB.X, (int)pointB.Y, (int)pointB.Z); 
            List<LLVector3> path = new List<LLVector3>();
            if (!MapCheckPosition(map, pointA)) return path;
            if (!MapCheckPosition(map, pointB)) return path;

            Dictionary<LLVector3, PathNode> open = new Dictionary<LLVector3, PathNode>();
            Dictionary<LLVector3, PathNode> closed = new Dictionary<LLVector3, PathNode>();        

            //Add the starting square (or node) to the open list.
            PathNode current = new PathNode(pointA);
            current.Parent = pointA; //the start parent is itself (as a marker)
            current.G = 0;
            current.H = (int)Math.Abs(pointB.X - pointA.X) + (int)Math.Abs(pointB.Y - pointA.Y);
            current.F = current.G + current.H;
            open.Add(current.Position, current);

            do
            {
                //Look for the lowest F cost square on the open list.
                //We refer to this as the current square.
                Nullable<int> lowest = null;
                foreach (PathNode node in open.Values)
                {
                    if (lowest == null || node.F < lowest)
                    {
                        lowest = node.F;
                        current = node;
                    }
                }
                if (lowest == null) throw new Exception("No available path was detected.");
                PathNode parent = current;

                //Switch it to the closed list.
                open.Remove(current.Position);
                closed.Add(current.Position, current);

                if (current.Position == pointB) break;

                //For each of the 8 squares adjacent to this current square...
                int x = (int)current.Position.X;
                int y = (int)current.Position.Y;
                int z = (int)current.Position.Z;
                List<LLVector3> adjacent = new List<LLVector3>();
                adjacent.Add(new LLVector3(x + 1, y + 1, 0));
                adjacent.Add(new LLVector3(x - 1, y - 1, 0));
                adjacent.Add(new LLVector3(x + 1, y - 1, 0));
                adjacent.Add(new LLVector3(x - 1, y + 1, 0));
                adjacent.Add(new LLVector3(x + 1, y, 0));
                adjacent.Add(new LLVector3(x - 1, y, 0));
                adjacent.Add(new LLVector3(x, y + 1, 0));
                adjacent.Add(new LLVector3(x, y - 1, 0));
                foreach(LLVector3 position in adjacent)
                {
                    //If it is not walkable or if it is on the closed list, ignore it.
                    if (!MapCheckPosition(map, position) || closed.ContainsKey(position)) continue;
                    //Otherwise do the following.
                    //If it isn’t on the open list, add it to the open list.
                    if (!open.ContainsKey(position))
                    {
                        open.Add(position, new PathNode(position));
                        //Make the current square the parent of this square.
                        open[position].Parent = current.Position;
                        //Record the F, G, and H costs of the square. 
                        if (position.X == x || position.Y == y) open[position].G = parent.G + 10; //adjacent
                        else open[position].G = parent.G + 14; //diagonal
                        open[position].H = (int)Math.Abs(position.X - x) + (int)Math.Abs(position.Y - y);
                        open[position].F = open[position].G + open[position].H;
                    }
                    //If so, change the parent of the square to the current square, and
                    //recalculate the G and F scores of the square.
                    else
                    {
                        open[position].Parent = current.Position;
                        if (position.X == x || position.Y == y) open[position].G = parent.G + 10; //adjacent
                        else open[position].G = parent.G + 14; //diagonal

                        open[position].F = open[position].G + open[position].H;
                    }
                }                
            }
            while (open.Count > 0);

            if (current.Position != pointB) throw new Exception("Unable to find a path!");

            //Backtrack from the end to the beginning
            do
            {
                path.Insert(0, current.Position);
                current = closed[current.Parent];
            }
            while (current.Parent != closed[current.Parent].Parent); //the start parent is itself

            return path;
        }
    }
}