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

namespace Astar_Implementation
{
    class Program
    {
        public class Node
        {
            public int PositionX, PositionY;

            public int FCost
            {
                get
                {
                    return GCost + HCost; //suma
                }
            }

            public int GCost; // koszt do punktu startowego
            public int HCost; // koszt do mety

            public Node Parent;

            public Node(int PositionX, int PositionY)
            {
                this.PositionX = PositionX;
                this.PositionY = PositionY;
            }
        }

        public class Map
        {
            public int Width, Height;

            public Map(int Width, int Height)
            {
                this.Width = Width;
                this.Height = Height;
            }
        }

        public class Blockade
        {
            public int x, y;

            public Blockade(int x, int y)
            {
                this.x = x;
                this.y = y;
            }
        }

        static public int CalculateG(Node child, Node Parent)
        {
            if (child.PositionX != Parent.PositionX && child.PositionY == Parent.PositionY || child.PositionX == Parent.PositionX && child.PositionY != Parent.PositionY) return Parent.GCost += 10;
            else return Parent.GCost + 14;
        }

        static public int CalculateH(int x, int y, int x2, int y2)
        {
            /*
            int X = x - x2;
            int Y = y - y2;
            return (int)Math.Sqrt(X * X + Y * Y);
            */
            int cost=0;
            while(x != x2 || y != y2)
            {
                if (x != x2 && y == y2 || x == x2 && y != y2) cost += 10;
                else cost += 14;

                if (x > x2) x++;
                else if (x < x2) x--;

                if (y > y2) y++;
                else if (y < y2) y--;
            }
            return cost;
        }

        public static List<Node> AllNodes = new List<Node>();
        public static List<Node> Open = new List<Node>();
        public static List<Node> Closed = new List<Node>();
        public static List<Blockade> Blockades = new List<Blockade>();

        static void Main(string[] args)
        {
            Map m = new Map(20, 10);
            Blockades.Add(new Blockade(2, 2));
            Blockades.Add(new Blockade(3, 2));
            Blockades.Add(new Blockade(4, 2));

            Node StartNode = new Node(1, 5);
            Node EndNode = new Node(20, 5);
            Node CurrNode;

            //Add nodes
            for (int y = 1; y <= m.Height; y++)
            {
                for (int x = 1; x <= m.Width; x++)
                {
                    if (!BlockadeHavePos(x, y))
                    {
                        AllNodes.Add(new Node(x, y));
                    }
                }
            }

            StartNode.HCost = CalculateH(StartNode.PositionX, StartNode.PositionY, EndNode.PositionX, EndNode.PositionY);
            StartNode.GCost = 0;

            Open.Add(StartNode);

            while (Open.Count > 0)
            {
                CurrNode = Open[0];

                for (int y = CurrNode.PositionY - 1; y <= CurrNode.PositionY + 1; y++)
                {
                    for (int x = CurrNode.PositionX - 1; x <= CurrNode.PositionX + 1; x++)
                    {
                        if (x <= m.Width && x > 0 && y <= m.Height && y > 0)
                        {
                            if (!BlockadeHavePos(x, y))
                            {
                                if (AllNodes.Find(item => item.PositionX == x && item.PositionY == y) != null)
                                {
                                    Node n = AllNodes.Find(item => item.PositionX == x && item.PositionY == y);
                                    if (!Closed.Contains(n))
                                    {
                                        #region finish
                                        if (x == EndNode.PositionX && y == EndNode.PositionY)
                                        {
                                            Console.WriteLine("Found it!");

                                            List<Node> Path = new List<Node>();
                                            EndNode.Parent = CurrNode;
                                            Path.Add(EndNode);
                                            Node ActualNode = EndNode;

                                            while (ActualNode.Parent != null)
                                            {
                                                Path.Add(ActualNode.Parent);
                                                ActualNode = ActualNode.Parent;
                                            }

                                            for (int Y = 1; Y <= m.Height; Y++)
                                            {
                                                for (int X = 1; X <= m.Width; X++)
                                                {
                                                    if (!BlockadeHavePos(X, Y))
                                                    {
                                                        Node n2 = AllNodes.Find(item => item.PositionX == X && item.PositionY == Y);
                                                        if (X == StartNode.PositionX && Y == StartNode.PositionY) Console.Write("[S]");
                                                        else if (X == EndNode.PositionX && Y == EndNode.PositionY) Console.Write("[E]");
                                                        else if (Path.Contains(n2)) Console.Write("[P]");
                                                        else
                                                            Console.Write("[ ]");
                                                    }
                                                    else
                                                    {
                                                        Console.Write("[B]");
                                                    }
                                                }
                                                Console.Write("\n");
                                            }
                                            Console.Read();
                                            return;
                                        }
                                        #endregion
                                        else {

                                            if (Open.Find(item => item.PositionX == n.PositionX && item.PositionY == n.PositionY) == null)
                                            {
                                                n.GCost = CalculateG(n, CurrNode);
                                                n.HCost = CalculateH(x, y, EndNode.PositionX, EndNode.PositionY);
                                                n.Parent = CurrNode;
                                                Open.Add(n);
                                            }
                                            else
                                            {
                                                if (n.GCost < CurrNode.GCost)
                                                {
                                                    Open.Clear();
                                                    Closed.Clear();
                                                    Closed.Add(CurrNode);
                                                    Open.Add(StartNode);
                                                    continue;
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
                Closed.Add(CurrNode);
                Open.RemoveAt(0);
                Open = Open.OrderBy(item => item.FCost).ToList();
            }
        }

        static bool BlockadeHavePos(int x, int y)
        {
            foreach (Blockade bl in Blockades)
            {
                if (bl.x == x && bl.y == y) return true;
            }
            return false;
        }
    }
}