Проблемы с А-Стар

[Devdev12]

Где тут ошибка?

enum Visit
{ 
          Undefined,
        Open,
        Closed
}

struct FPoint
{
    public int F;
    public int G;
    public int PX;
    public int PY;
    public Visit visited;
}

    struct Point
    {
        public int X, Y;
        public int F, G, H, PX, PY;

        public Point(int x, int y)
        {
            X = x;
            Y = y;
            F = G = H = 0;
            PX = PY = 0;
        }

        public static bool operator ==(Point p1, Point p2)
        {
            return p1.X == p2.X && p1.Y == p2.Y;
        }

        public static bool operator !=(Point p1, Point p2)
        {
            return p1.X != p2.X || p1.Y != p2.Y;
        }
    }

static class PathFinding
{
    static Stack<int> open = new Stack<int>();
    static List<int> closed = new List<int>();
    static bool success = false;
    static int locx = 0;
    static int locy = 0;
    static int newlocx = 0;
    static int newlocy = 0;
    static int newloc = 0;
    static int heuristic = 0;
    static int G = 0;
    static int loc = 0;
    static int endloc = 0;
    static FPoint[] grid;


    static public List<int> FindPath(System.Drawing.Point start, System.Drawint.Point end, Field field)
    {
        loc = (start.Y * field.Width) + start.X;
        endloc = (end.Y * field.Width) + end.X;

        if (loc == endloc) return new List<int>() { loc };

        grid = new FPoint[field.Width * field.Height];
        grid[loc].G = 0;
        grid[loc].F = 4;
        grid[loc].PX = start.X;
        grid[loc].PY = start.Y;
        grid[loc].visited = Visit.Open;

        open.Push(loc);

        sbyte[,] dir = new sbyte[8, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 } };

        while (open.Count > 0)
        {
            loc = open.Pop();

            if (grid[loc].visited == Visit.Closed)
                continue;

            locx = (loc % field.Width);
            locy = (loc / field.Width);

            if (loc == endloc)
            {
                grid[loc].visited = Visit.Closed;
                success = true;
                break;
            }

            for (int i = 0; i < 8; i++)
            {
                newlocx = locx + dir[i, 0];
                newlocy = locy + dir[i, 1];
                newloc = (newlocy * (field.Width)) + newlocx;

                if (newlocy < 0 || newlocx < 0 || newlocx >= field.Width || newlocy >= field.Height) continue;

                if (field.inarr[newloc] == 255) continue;

                if (i > 3) G = (grid[loc].G + (int)(field.inarr[newloc] * 2));
                else G = (grid[loc].G + field.inarr[newloc]);

                if (grid[newloc].visited != Visit.Undefined)
                    if (grid[newloc].G <= G) continue;

                grid[newloc].PX = locx;
                grid[newloc].PY = locy;
                grid[newloc].G = G;
                heuristic =  (Math.Abs(newlocx - end.X) + Math.Abs(newlocy end.Y));
                grid[newloc].F = G + heuristic;
                open.Push(newloc);
                grid[newloc].visited = Visit.Open;
            }

            grid[loc].visited = Visit.Closed;
        }

        if (success)
        {
            int posX = end.X;
            int posY = end.Y;

            FPoint fNodeTmp = grid[(end.Y * field.Width) + end.X];
            Point Node;
            Node.F = fNodeTmp.F;
            Node.G = fNodeTmp.G;
            Node.H = 0;
            Node.PX = fNodeTmp.PX;
            Node.PY = fNodeTmp.PY;
            Node.X = end.X;
            Node.Y = end.Y;

            while (Node.X != Node.PX || Node.Y != Node.PY)
            {
                closed.Add((Node.Y * field.Width) +Node.X);

                posX = Node.PX;
                posY = Node.PY;
                fNodeTmp = grid[(posY * field.Width) + posX];
                Node.F = fNodeTmp.F;
                Node.G = fNodeTmp.G;
                Node.H = 0;
                Node.PX = fNodeTmp.PX;
                Node.PY = fNodeTmp.PY;
                Node.X = posX;
                Node.Y = posY;
            }

            closed.Add((Node.Y * field.Width) + Node.X);
            return closed;
        }
        return new List<int>();
    }
}

Field[byte] - по сути массив, характеризующий поле. Width, Height - ширина и высота его соотв.
если значение ячейки равно 255, то она непроходима.
алгоритм для восьми направлений, путь находит, но уж очень неадекватный.