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

namespace AStar {
    public class AStar {

        public AStarNode _nodeStart;
        public AStarNode _nodeGoal;

        public List<AStarNode> openList;
        public List<AStarNode> closedList;
        public List<AStarNode> neighbors;
        public List<AStarNode> finalPath;

        public AStar() {
            openList = new List<AStarNode>();
            closedList = new List<AStarNode>();
            neighbors = new List<AStarNode>();
            finalPath = new List<AStarNode>();
        }


        public void FindPath(AStarNode start, AStarNode goal) {
            openList = new List<AStarNode>();
            closedList = new List<AStarNode>();
            neighbors = new List<AStarNode>();

            _nodeStart = start;
            _nodeGoal = goal;

            openList.Add(start);
            while(openList.Count > 0) {

                openList.Sort(SortNodes);
                AStarNode currentNode = openList[0];
                openList.RemoveAt(0);

                if (currentNode.IsEqualTo(goal)) {
                    finalPath = new List<AStarNode>();
                    while(currentNode.parent != null) {
                        finalPath.Insert(0, currentNode);
                        currentNode = currentNode.parent;
                    }
                    break;

                } else {
                    currentNode.GetNeighbors(neighbors);
                    for(int iNB = 0; iNB < neighbors.Count; iNB++) {
                        AStarNode neighbor = neighbors[iNB];

                        int existsInOpenList = -1;
                        int existsInClosedList = -1;

                        for (int ioi = 0; ioi < openList.Count; ioi++) {
                            if (openList[ioi].IsEqualTo(neighbor)) {
                                existsInOpenList = ioi;
                                break;
                            }
                        }
                        for (int ici = 0; ici < closedList.Count; ici++) {
                            if (closedList[ici].IsEqualTo(neighbor)) {
                                existsInClosedList = ici;
                                break;
                            }
                        }

                        if(existsInOpenList != -1) {
                            if (openList[existsInOpenList].g > (neighbor.g + openList[existsInOpenList].nodeCost)) {
                                openList[existsInOpenList].g = (neighbor.g + openList[existsInOpenList].nodeCost);
                                continue;
                            }
                        } else if(existsInClosedList != -1) {
                            if (closedList[existsInClosedList].g > (neighbor.g + closedList[existsInClosedList].nodeCost)) {
                                closedList[existsInClosedList].g = (neighbor.g + closedList[existsInClosedList].nodeCost);
                                openList.Add(closedList[existsInClosedList]);
                                closedList.RemoveAt(existsInClosedList);
                                continue;
                            }
                        } else {
                            //AddToOpenList(openList, neighbor);
                            openList.Add(neighbor);
                        }
                    }
                }
                closedList.Add(currentNode);
            }
        }

        public int SortNodes(AStarNode n1, AStarNode n2) {
            if (n1 == null) {
                if (n2 == null) {
                    return 0;
                } else {
                    return -1;
                }
            } else {
                if (n2 == null) {
                    return 1;
                } else {
                    if (n1.f == n2.f) return 0;
                    else return ((n1.f > n2.f) ? 1 : -1);
                }
            }
        }

        private void AddToOpenList(List<AStarNode> openList, AStarNode node) {
            int i = 0;
            if (openList.Count < 1) {
                openList.Add(node);
                return;
            }
            while (i < openList.Count) {
                if (openList[i].f > node.f) {
                    openList.Insert(i, node);
                    break;
                }
                i++;
            }
        }
    }

    public class AStarNode {

        public AStarNode parent;
        public AStarNode goal;

        public int nodeCost;
        public int g;
        public double h;
        public double f;

        public AStarNode(AStarNode parent, AStarNode goal, int parentG, int nodeG) {
            this.parent = parent;
            this.goal = goal;
            this.g = parentG + nodeG;
            this.nodeCost = nodeG;
        }

        public virtual void GetNeighbors(List<AStarNode> listNeighbors) { return; }

        public virtual void Calculate() {}

        public virtual bool IsEqualTo(AStarNode node) { return false; }
    }

    public class AStarNode2D : AStarNode{
        public int x;
        public int y;

        public AStarNode2D(AStarNode parent, AStarNode goal, int parentG, int nodeG, int x, int y) : base(parent, goal, parentG, nodeG) {
            this.x = x;
            this.y = y;
            Calculate();
        }

        public override void GetNeighbors(List<AStarNode> listNeighbors) {
            listNeighbors.Clear();
            AddNeighbor(listNeighbors, x-1, y  );
            AddNeighbor(listNeighbors, x-1, y-1);
            AddNeighbor(listNeighbors, x  , y-1);
            AddNeighbor(listNeighbors, x+1, y-1);
            AddNeighbor(listNeighbors, x+1, y  );
            AddNeighbor(listNeighbors, x+1, y+1);
            AddNeighbor(listNeighbors, x  , y+1);
            AddNeighbor(listNeighbors, x-1, y+1);
        }
        private void AddNeighbor(List<AStarNode> listNeighbors, int x, int y) {
            if(x >= 0 && x < WorldData.gridWidth && y >= 0 && y < WorldData.gridHeight) {
                if(WorldData.grid[(y * WorldData.gridWidth) + x] != -1) {
                    listNeighbors.Add(new AStarNode2D(this, goal, g, WorldData.grid[(y * WorldData.gridWidth) + x], x, y));
                }
            }
        }

        public override void Calculate() {
            // Heuristics! :O
            if (goal != null) {
                this.h = Math.Sqrt(Math.Pow((this.x - ((AStarNode2D)goal).x), 2) + Math.Pow((this.y - ((AStarNode2D)goal).y), 2));
                this.f = this.g + this.h;
            }
        }

        public override bool IsEqualTo(AStarNode node) {
            return ((this.x == ((AStarNode2D)node).x) && (this.y == ((AStarNode2D)node).y));
        }
    }

}