/**
 * Project 6 solution - CS 100J 
 * 
 */

public class Robot implements Runnable {
    private Motor motor;           // robot's motor
    private int size;              // width and height of rock crater
    private Sensor gnatx, gnaty;   // sensors for gnat's position
    private DiagnosticPanel panel; // debugging panel for drawing plans and known rocks

    // constructor: fill in fields above with supplied values
    public Robot(int size, Motor m, Sensor sx, Sensor sy, DiagnosticPanel p) { 
        this.size = size; motor = m; gnatx = sx; gnaty = sy; panel = p; 
    }

    // run (control) the robot
    public void run() {
    	final int delay = 5;              // # ticks motor needs to move
    	int d;                            // direction 0, 1, 2, or 3.
    	final int[] dx = { 0, 1, -1, 0 }; // x offset of a direction
    	final int[] dy = { 1, 0, 0, -1 }; // y offset of a direction
    	int[][] map =                     // map of known rocks and distances to gnat
           new int[1+size+1][1+size+1];   //   with border of rocks (hence 1+ and +1)
   		final int INFINITY = 2*size*size; // unvisited or unreachable map locations
    	final int ROCK = INFINITY + 1;    // known rocks in map
   		int x = motor.x.value(), y = motor.y.value(); // robot location
    
    // Place rocks on border, but don't register them
    for (int i=0; i<1+size+1; i++)
        map[i][0] = map[i][1+size] = map[0][i] = map[1+size][i] = ROCK;

    while (true) {
        int gx = gnatx.value(), gy = gnaty.value(); // gnat location
        
        // Set cells in map not containing ROCKs to INFINITY
        for (int i=1; i<=size; i++)
            for (int j=1; j<=size; j++)
                if (map[i][j] != ROCK) map[i][j] = INFINITY;

        // Floodfill:
        // frontier[0..1][front..back-1] is a to-do list of places whose
        // neighbors might be unvisited; frontier[0] is y-values, frontier[1] is
        // x-values. frontier is seeded with (x,y), set at distance 0 in map
          int[][] frontier = new int[2][size*size]; 
          int front = 0, back = 1;
          frontier[0][front] = y; frontier[1][front] = x;
          map[y][x] = 0; 

        // Grow frontier until it is either empty or the gnat is reached.
          while (front<back && map[gy][gx] == INFINITY) {
            // Remove next place (px,py) from to-do list
              int py = frontier[0][front], px = frontier[1][front];
              front++;
            
            // Add unvisited neighbors (nx,ny) of (px,py)
            // to to-do list & set their distances in map.
              for (d=0; d<dx.length; d++) {
                int ny = py + dy[d], nx = px + dx[d];
                if (map[ny][nx] == INFINITY) {
                    map[ny][nx] = 1 + map[py][px];
                    frontier[0][back] = ny; 
                    frontier[1][back] = nx; back++;
                }
              }
          }

        int steps = map[gy][gx]; // number of steps to reach gnat
        if (steps == INFINITY) 
        	throw new RuntimeException("Gnat unreachable!");
        int[][] plan = frontier; // reuse frontier (no longer used) for plan

        // Traceback:
        // Move (gx,gy) backwards to 1 unit away from robot -- to where
        // robot moves in next step-- and place steps (gx,gy) into the plan
          while (map[gy][gx]>1) {
            plan[0][map[gy][gx]] = gy; plan[1][map[gy][gx]] = gx;
            // Let d be a direction where neighbor is 1 unit closer
               for (d=0; 1+map[gy+dy[d]][gx+dx[d]] != map[gy][gx]; d++)
                  // do nothing
            	  ;
            // Move to the neighbor in direction d.
               gy = gy + dy[d]; gx = gx + dx[d]; 
          }
        
         // Add (gx,gy) and robot's positions to plan and register it
           plan[0][map[gy][gx]] = gy; plan[1][map[gy][gx]] = gx;
           plan[0][0] = y; plan[1][0] = x;
           panel.registerPlan(steps,plan[1],plan[0]);
        
         // Move to (gx,gy) if not already there, and if possible.
           if (gx != x || gy != y) {
            // search for direction d from which to reach (gx,gy) from (x,y)
               for (d=0; y+dy[d] != gy || x+dx[d] != gx; d++)
               		// do nothing
               		;
            // Try to move in direction d
               motor.move(d); 
           }

         // Give motor time to move
           for (int t = delay + motor.clock(); t > motor.clock(); )
               		// do nothing
                    ;
         
         // Set (x,y) to current position of motor.
            x = motor.x.value(); y = motor.y.value();
        
         // Register a rock if robot has not moved and update map
         if (x != gx || y != gy) {
             panel.registerRock(gx,gy);
             map[gy][gx] = ROCK;
         }
      }
    }
}