Pathfinder

// pathfinding;

public class Cell {

    private int value;
    private int row,col;
    public float  globalG=0.0f;
    public Cell() {
        direction=null;
    }

    public int getRow() {
        return row;
    }

    public void setRow(int row) {
        this.row = row;
    }

    public int getCol() {
        return col;
    }

    public void setCol(int col) {
        this.col = col;
    }

    public void setValue(int value) {
        this.value=value;
    }

    public int getValue() {
        return value;
    }

    }

   ///the Class function


import java.util.ArrayList;
import java.io.PrintStream;;
public class main {

    static PrintStream out=System.out;

    static class Node{

        // holds the current array position data on the graph
        Cell cell;
        // gets the next adjacent node
        Node next;
        // gets the parent node of the current node
        Node parent;
        // gets all the possible adjacent Nodes from the current node
        ArrayList< Node> Neigbours;
        public Node() {
            next= null;
            cell= null;
            parent=null;
            Neigbours= null;
        }
        // first heuristic test
         float distance(Node a ,Node b) {

            return (float)Math.sqrt(((a.cell.getRow()-b.cell.getRow())*
                    (a.cell.getRow()-b.cell.getRow())+
                    (a.cell.getCol()-b.cell.getCol())*
                    (a.cell.getCol()-b.cell.getCol())));
        }

         //second heuristics test
        float distance2(Node a,Node b) {

         float x=(a.cell.getRow())-(b.cell.getRow());
         float y=(a.cell.getCol())-(b.cell.getCol());
         float cost=(float)(Math.pow(x, 2)+Math.pow(y, 2));

          return cost;

         }

         // get first prediction
        public float heuristic(Node a,Node b) {
            return distance(a,b);
        }

        // get second prediction
        public float heuristic2(Node a,Node b) {
            return distance2(a,b);
        }
    }

    static Node map_Ref[][];
    //start point where the robot is located
    static Node start;
    // the end destination where the robot is to arrive
    static Node end;
    // our map where by 0 represents a walkable path
    //    1 represents a wall(non- walkable path)
    // 2 represents the starting location of the robots position
    //4 represents  the destination
    // 5 represents an enemy
    static int obstacles[]= {1,5};
    //map
    static int map[][]= {{2,0,1,1,1,1,1},
                         {1,0,1,0,1,0,1},
                         {1,0,0,0,0,0,1},
                         {1,1,1,1,0,0,1},
                         {1,0,0,0,0,0,1},
                         {1,0,1,1,1,0,1},
                         {1,0,0,4,0,0,1}};
    //add map as a linked list
    private static Node addNode(Node Head,Node ptr) {
        map_Ref= new Node[map.length][map.length];
        for(int i=0;i<map.length;i++) {
            for(int j=0;j<map.length;j++) {

        ptr= new Node();
        ptr.cell= new Cell();
        // gets the value from the map array
        ptr.cell.setValue(map[i][j]);
        // gets the column position
        ptr.cell.setCol(j);
        // gets the row position
        ptr.cell.setRow(i);
        // set the next node pointer with the current node
        ptr.next=Head;
        Head=ptr;
        // our reference to the actual map
         map_Ref[i][j]= Head;

         // initialize the start and end values from our map
         if(map[i][j]==2) {
             start=ptr;
         }
         if(map[i][j]==4) {
             end=ptr;
         }

            }}
     return ptr;
    }

    // function to convert a linked list to an array of Nodes
    private static void getPaths(Node Head){
         //holds the values of the tested nodes in the linked list
         ArrayList <Node> closedList=new ArrayList<Node>();
         // holds the values of the non- tested nodes from the linked list
         ArrayList<Node>  openList= new ArrayList<Node>();

         // gets all available neighboring nodes form a linked list
         get_neigbours();



         // our open list must not be empty we must initialize it with
         // the values neighbors of the end node
        openList.add(start);
        while(!openList.isEmpty()) {
            // set the cost value to +infinity
            float cost=Float.MAX_VALUE;
            for(int i=0;i<openList.size();i++) {

                closedList.add(openList.get(i));
                for(int j=0;j<openList.get(i).Neigbours.size();j++) {
                    Node nd=openList.get(i).Neigbours.get(j);
                    /*the open list should not contain the node and the closed list should not
                     contain the node
                     if the cell contains the value 1 is a wall
                     only add nodes containing the value 0 since its a walkable path
                     */
                    if(!closedList.contains(nd)&&!openList.contains(nd)&& !isObstacle(nd.cell.getValue())) {
                        if(!openList.contains(nd))
                        {

                        // get the global goal from the start to the end
                        nd.cell.globalG= nd.heuristic2(openList.get(i), nd);

                        if(nd.cell.globalG<=cost) {
                            // add the target neighbor to the OpenList for testing
                        openList.add(nd);
                        //initialize the parent node
                        nd.parent=openList.get(i);
                        // reset the cost to the current neighbor
                         cost=nd.cell.globalG;

                             }

                        }
                    }


                }



            }

            // since we have tested the nodes in the open list  remove them
            for(int i=0;i<closedList.size();i++) {
                if(openList.contains(closedList.get(i))){
                    openList.remove(closedList.get(i));
                }
            }

            // reached the target
            if(openList.contains(end)) {
                break;
            }

        }


    }


    // returns the size of a Node list
    private static int ListSize(Node Head) {
        int size=0;
        while(Head!=null) {
            size++;
            Head=Head.next;
        }

        return size-1;
    }



    // get the list of neighbors from a particular  node
    private static void get_neigbours() {

         for(int i=0;i<map_Ref.length;i++) {
             for(int j=0;j<map_Ref.length;j++) {

                 ArrayList<Node> ngbrs= new ArrayList<Node>();
                 Node nd= map_Ref[i][j];
                 // get the neighbors on the left hand side
                 if(i>0) {
                     ngbrs.add(map_Ref[i-1][j]);
                 }
                 // gets the neighboring node on the right hand side
                 if(i!=map_Ref.length-1) {
                     ngbrs.add(map_Ref[i+1][j]);
                 }
                 // get the neighboring node on the bottom part adjacent to the current node
                 if(j>0) {
                     ngbrs.add(map_Ref[i][j-1]);
                 }
                 // get the top neighboring node on the top part of the adjacent node
                 if(j!=map_Ref.length-1) {
                     ngbrs.add(map_Ref[i][j+1]);
                 }
                 // set the neighbors to the current node
                 nd.Neigbours=ngbrs;
             }
         }


    }

    // returns true if there's an obstacles on the graph
    // using the obstacles array
    private static  boolean isObstacle(int in ) {
        for(int i=0;i<obstacles.length;i++) {
            if(obstacles[i]==in) {

                return true;
            }
        }
        return false;
    }
    //prints all nodes in the linked list
    private static void PrintNode(Node Head) {
        while(Head!=null) {
        out.print(Head.cell.getValue());
        Head=Head.next;
        }

    }

    private static void PrintMatrix() {
        // prints our matrix on the console
          for(int i=0;i<map.length;i++)
          {
              for(int j=0;j<map.length;j++) {
                  out.print(map[i][j]);

              }
              out.print("\n");
            ;
          }
    }

    public static void main(String[] args) {
        // TODO Auto-generated method stub
      Node Path= new Node();
      Path.cell=new Cell();
      Path.cell.setValue(1);
      Path= addNode(Path,new Node());
      out.print("The Original Graph Matrix\n");
      PrintMatrix();
      //gets the shortest path
      getPaths(Path);
      //Gets shortest path parent nodes
      Node t=end;
      while(t!=null) {
          map[t.cell.getRow()][t.cell.getCol()]=3;
          t=t.parent;
      }
      //state whether a path was found on the graph
      if(map[start.cell.getRow()][start.cell.getCol()]==3)
      out.print("\nA Path was Found\n");
      else
      out.print("A Path wasn't Found\n");
      out.print("The Shortest Path On the graph\n");
      PrintMatrix();

    }




}