JoelForkTest/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;
    private int Rotation;

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

    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), 0),
            new Nodes(new Vector2(currentPos.X + 1, currentPos.Y), 1),
            new Nodes(new Vector2(currentPos.X, currentPos.Y - 1), 2),
            new Nodes(new Vector2(currentPos.X - 1, currentPos.Y), -1),
        };

        //check if out of range
        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 - 1 || adjacentNodes[i].getCords().Y > Size.Y - 1)
                    adjacentNodes.Remove(adjacentNodes[i]);
            }
        }

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

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

    public int CalculateRotationCost(int currDir, int newDir)
    {
        if (currDir == newDir)
            return 0;
        else if (Math.Abs(currDir - newDir) == 1 || Math.Abs(currDir - newDir) == 3)
            return 3;
        else if (Math.Abs(currDir - newDir) == 0 || Math.Abs(currDir - newDir) == 2) //its for turning back, so it never should happen anyway, remove this check then?
            return 9;
        return 0;
    }

    public int ConvertRotation()
    {
        int rotation = 0;
        if (Rotation == 180)
            rotation = 0;
        else if (Rotation == 270)
            rotation = 1;
        else if (Rotation == 0)
            rotation = 2;
        else if (Rotation == 90)
            rotation = -1;
        return rotation;
    }

    public Path FindPath()
    {
        Path path = new Path();
        //PriorityQueue openList = new PriorityQueue();
        MinHeap openList = new MinHeap();
        //PriorityQueue closedList = new PriorityQueue();
        MinHeap closedList = new MinHeap();
        Nodes target = new Nodes(targetPos);
        int direction = ConvertRotation();
        Nodes startPos = new Nodes(tractorPos, direction);
        Nodes current = null;
        int g = 0;
        //int direction = SmartTractor.update

        //openList.Enqueue(startPos);
        openList.Insert(startPos);

        //while (openList.Count > 0)
        while (openList.GetSize() > 0)
        {
            current = openList.getMin();
            //current = openList.Peek();
            //g = current.getG();
            closedList.Insert(current);
            //closedList.Enqueue(current);
            openList.removeMin();
            //openList.Dequeue();
            direction = current.getDirection();

            if (current.getCords() == target.getCords())
                break;


            //if (closedList.Exists(target.getCords()))
            //   break;

            var adjacentNodes = GetAdjacentNodes(current.getCords());
            //g++;
            foreach (var adjacentNode in adjacentNodes)
            {
                if (closedList.Exists(adjacentNode.getCords()))
                    continue;
                g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement() + CalculateRotationCost(direction, adjacentNode.getDirection());
                if (!(openList.Exists(adjacentNode.getCords())))
                {

                    adjacentNode.setG(g);
                    adjacentNode.setH(ComputeHScore(adjacentNode.getCords(), target.getCords()));
                    adjacentNode.calculateF();
                    adjacentNode.setParent(current);
                    openList.Insert(adjacentNode);
                    //openList.Enqueue(adjacentNode);
                }
                else
                {
                    if (g + adjacentNode.getH() < adjacentNode.getF())
                    {
                        adjacentNode.setG(g);
                        adjacentNode.calculateF();
                        adjacentNode.setParent(current);
                    }
                }

            }

            //openList.BuildMinHeap(openList.GetList());
        }

        while (current != null)
        {
            path.AddNode(current);
            current = current.getParent();
        }
        path = path.FlipArray();
        openList.deleteHeap();
        closedList.deleteHeap();
        //openList.Clear();
        //closedList.Clear();
        return path;
    }

}