PotatoPlan/Game1/Sources/Pathing/A-Star/Astar.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Xna.Framework;

class Astar
{

    private Vector2 tractorPos;
    private Vector2 housePos;
    private Crops[,] crops;
    private Vector2 Size;
    private PriorityQueue allPaths;
    private Vector2 targetPos;

    public void update(Crops[,] newCrops, Vector2 newSize, Vector2 newTractorPos, Vector2 newHousePos, Vector2 newtargetPos)
    {
        tractorPos = new Vector2((int)newTractorPos.X, (int)newTractorPos.Y);
        housePos = new Vector2((int)newHousePos.X, (int)newHousePos.Y);
        targetPos = newtargetPos;
        crops = newCrops;
        Size = newSize;

    }

    public Nodes getOptimalPath()
    {
        return allPaths.Peek();
    }

    private List <Nodes> GetAdjacentNodes(Vector2 currentPos)
    {
        var adjacentNodes = new List<Nodes>()

        {
            new Nodes(new Vector2(currentPos.X, currentPos.Y+1)),
            new Nodes(new Vector2(currentPos.X + 1, currentPos.Y)),
            new Nodes(new Vector2(currentPos.X, currentPos.Y - 1)),
            new Nodes(new Vector2(currentPos.X - 1, currentPos.Y)),
        };

        for (int i = 3; i >=0; i--)
        {
            if (adjacentNodes[i].getCords().X < 0 || adjacentNodes[i].getCords().Y < 0)
                adjacentNodes.Remove(adjacentNodes[i]);
            else
            {
                if (adjacentNodes[i].getCords().X > Size.X || adjacentNodes[i].getCords().Y > Size.Y)
                    adjacentNodes.Remove(adjacentNodes[i]);
            }
        }

        return adjacentNodes.Where(
            item => crops[(int)item.getCords().X, (int)item.getCords().Y].Status != 0).ToList();
    }


    public int ComputeHScore(Vector2 currentNode, Vector2 endNote)
    {
        return (int)(Math.Abs(endNote.X - currentNode.X) + Math.Abs(endNote.Y - currentNode.Y));
    }

    public Path FindPath()
    {
        Path path = new Path();
        PriorityQueue openList = new PriorityQueue();
        PriorityQueue closedList = new PriorityQueue();
        Nodes startPos = new Nodes (tractorPos);
        Nodes current = null;
        int g = 0;

        openList.Enqueue(startPos);

        while (openList.Count > 0)
        {
            current = openList.Peek();
            closedList.Enqueue(current);
            openList.Dequeue();

            if (closedList.Peek().getCords() == targetPos)
                break;

            var adjacentNodes = GetAdjacentNodes(current.getCords());
            g++;

            foreach(var adjacentNode in adjacentNodes)
            {
                if (closedList.Exists(adjacentNode.getCords()))
                    continue;
                if (!(openList.Exists(adjacentNode.getCords())))
                {
                    adjacentNode.setG(g);
                    adjacentNode.setH(ComputeHScore(adjacentNode.getCords(), targetPos));
                    adjacentNode.calculateF();
                    adjacentNode.setParent(current);
                    openList.Enqueue(adjacentNode);
                }
                else
                {
                    if(g + adjacentNode.getH() < adjacentNode.getF())
                    {
                        adjacentNode.setG(g);
                        adjacentNode.calculateF();
                        adjacentNode.setParent(current);
                    }
                }
            }
        }

        while (current.getParent() != null)
        {
            path.AddNode(current);
            current = current.getParent();
        }
        path.FlipArray();
        return path;
    }

}