using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using AStar;
using AStar.Exceptions;

namespace AStar
{
    public class AStarPathFinder
    {
        private int[,] walkableMap;//only 0 and 1 allowed
        private Node[,] checkedNodes;
        public List<Node> finalPath = new List<Node>();
        public bool PathFound = false;

        private Node startPos = new Node();
        private Node endPos = new Node();

        private int[,] checkOffset = new int[8,2];

        private int mapWidth = 0;
        private int mapHeight = 0;

        private bool startSet = false;
        private bool endSet = false;
        private bool mapSet = false;

        public AStarPathFinder()
        {
            checkOffset[0, 0] = 1; //right
            checkOffset[0, 1] = 0;

            checkOffset[1, 0] = 1; //down right
            checkOffset[1, 1] = 1;

            checkOffset[2, 0] = 0; //down
            checkOffset[2, 1] = 1;

            checkOffset[3, 0] = -1; //down left
            checkOffset[3, 1] = 1;

            checkOffset[4, 0] = -1; //left
            checkOffset[4, 1] = 0;

            checkOffset[5, 0] = -1; //up left
            checkOffset[5, 1] = -1;

            checkOffset[6, 0] = 0; //up
            checkOffset[6, 1] = -1;

            checkOffset[7, 0] = 1; //up right
            checkOffset[7, 1] = -1;
        }

        public void Add(List<List<int>> walkableMap)
        {
            int[,] map = new int[walkableMap.Count, walkableMap.ElementAt(0).Count];

            int i = 0;
            int j = 0;

            foreach (List<int> row in walkableMap)
            {
                foreach (int id in row)
                {
                    if (id != 0 && id != 1)
                    {
                        throw (new IllegalMapException("Walkable map may only contain 0's and 1's"));
                    }
                    else
                    {
                        map[i, j] = id;
                    }
                    i++;
                }
                i = 0;
                j++;
            }

            mapWidth = walkableMap.Count;
            mapHeight = walkableMap.ElementAt(0).Count;

            checkedNodes = new Node[walkableMap.Count, walkableMap.ElementAt(0).Count];
            mapSet = true;

            this.walkableMap = map;
        }

        public void SetStartingPosition(Node node)
        {
            startPos = node;
            startSet = true;
        }
        public void SetStartingPosition(int x, int y)
        {
            startPos = new Node(x, y);
            startSet = true;
        }

        public void SetEndingPosition(Node node)
        {
            endPos = node;
            endSet = true;
        }
        public void SetEndingPosition(int x, int y)
        {
            endPos = new Node(x, y);
            endSet = true;
        }

        public void searchPath()
        {
            if (startSet && endSet && mapSet)
            {
                int curX = startPos.x;
                int curY = startPos.y;

                bool pathFound = false;
                while (!pathFound)
                {
                    if (curX == endPos.x && curY == endPos.y)
                    {
                        pathFound = true;
                        break;
                    }

                    int closestX = 0;
                    int closestY = 0;
                    double closestDistance = 9999999;

                    int newX = 0;
                    int newY = 0;

                    for (int i = 0; i < 7; i++)
                    {
                        int xOffset = checkOffset[i, 0];
                        int yOffset = checkOffset[i, 1];

                        if (curX + xOffset >= 0 && curX + xOffset <= mapWidth && curY + yOffset >= 0 && curY + yOffset <= mapHeight)
                        {
                            if (checkedNodes[curX + xOffset, curY + yOffset] == null)
                            {
                                Node n = new Node();
                                n.x = curX + xOffset;
                                n.y = curY + yOffset;

                                int w = 0;//width length
                                int h = 0;//height length

                                //make sure no negative values are used
                                if (endPos.x - n.x >= 0)
                                {
                                    w = endPos.x - n.x;
                                }
                                else
                                {
                                    w = n.x - endPos.x;
                                }

                                if (endPos.y - n.y >= 0)
                                {
                                    h = endPos.y - n.y;
                                }
                                else
                                {
                                    h = n.y - endPos.y;
                                }
                                //calculate the scores

                                n.h = w + h;
                                n.g = Math.Sqrt(w * w + h * h);
                                n.f = n.g + n.h;

                                if (closestDistance > n.f)
                                {
                                    closestDistance = n.f;
                                    closestX = n.x;
                                    closestY = n.y;

                                    newX = closestX;
                                    newY = closestY;

                                    n.setDirectionForOffset(xOffset, yOffset);
                                }
                                checkedNodes[curX + xOffset, curY + yOffset] = n;
                            }
                        }
                    }

                    curX = newX;
                    curY = newY;

                }
            }
            else
            {
                throw (new IllegalArgumentException("Not enough data to search a path."));
            }

            //get the actual path

            bool finalPathFound = false;
            finalPath = new List<Node>();
            //finalPath.Add(checkedNodes[endPos.x, endPos.y]);

            int myX = endPos.x;
            int myY = endPos.y;

            Console.WriteLine("starting");

            while (!finalPathFound)
            {
                if(myX == startPos.x && myY == startPos.y)
                {
                    finalPathFound = true;
                    break;
                }

                int [] offset = checkedNodes[myX, myY].getOffsetForDirection();
                if (myX + offset[0] >= 0 && myX + offset[0] <= mapWidth && myY + offset[1] >= 0 && myY + offset[1] <= mapHeight)
                {
                    myX += offset[0];
                    myY += offset[1];

                    finalPath.Add(checkedNodes[myX + offset[0], myY + offset[1]]);
                }
            }

            Console.WriteLine("done");

            PathFound = true;
        }
    }
}