import java.lang.Math;

/** This class is the brains behind the whole operation.  It delegates
    roles to the various rovers to elicit different responses from the
    rovers.  It also holds and outputs the 2D occupancy grid. */
public class Prodigy implements Runnable {

    private final int MAX_NUM_ROVERS = 3;
    private final int STARTING_NUM_ROVERS = 2;
    private final int ONEBLOCK = 50;
    private final int START_BUFFER = 600;
    private final int MAP_SIDE = 1200;

    public Record[] records = new Record[MAX_NUM_ROVERS];
    private boolean[] ready = new boolean[MAX_NUM_ROVERS];
    
    boolean[][] map;
    public int readings = 0;
    
    public int activeRovers = 0;
    private boolean okToRead = false;
    private boolean coordsEstablished = false;

  /** Entry point for my project. */
    public static void main (String args[]) {
	Prodigy prodigy = new Prodigy();
	new Thread(prodigy).start();
	new Thread(new Shell(prodigy)).start();
    }
    
  /** Initializes the map and all the rovers. */
    public Prodigy () {
	super();
	map = new boolean[MAP_SIDE][MAP_SIDE];
	for (int i=0; i<STARTING_NUM_ROVERS; ++i) spawnRover();
    }

  /** Implementation of the Runnable interface so that we can give the
      Prodigy its own thread. */
    public void run () {
	snore(1000);
	System.out.println("Setting to free");
	setRole(STARTING_NUM_ROVERS-1, Roles.FREE);
	System.out.println("Setting to observer");
	setObserver(0,1);
	while (!mapped()) {
	    snore(500);
	}
	shutdown();
	outputMap();
    }

  /** Called by an Observer to notify the Prodigy that it has found
      its target and it is ready to observe it. */
    public void readyToObserve(int id) {
	int observedID = records[id].getTargetID();
	setRole(observedID, Roles.SCOUT);
    }

  /** Called by an Observer to notify that it has lost a scout and the
      scout needs to back up. */
    public void lostScout (int observerID) {
	int observedID = records[observerID].getTargetID();
	records[observedID].getRover().backup();
    }

  /** Called by Rovers to signify that it has caught up to its
      counterpart.  Signals the two robots to continue mapping as
      before. */
    public void caughtUp (int scoutID) {
	System.out.println(""+scoutID+" has caught up.");
	for (int i=0; i<activeRovers; ++i) {
	    if (records[i].getTargetID() == scoutID) {
		records[i].getRover().rememberToReorient();
		setObserver(scoutID, i);
	    }
	}
    }

  /** Called by a Rover to signify that it needs to catch up to its
      counterpart.  Generally called after a back-up. */
    public void switchOff (int id) {
	int observedID = records[id].getTargetID();
	records[id].getRover().catchup();
	setObserver(observedID, id);
    }

  /** Called by an observer in order to change the Prodigy's record of
      the observed rover's Pose.. */
    public void observeChange (int id, Pose newPose) {
	int observedID = records[id].getTargetID();
	records[observedID].changePose(newPose);
    }
    
  /** Accessor method to return the pose of any Rover. */
    public Pose getPose (int id) {
	return records[id].getPose();
    }
    
  /** Helper method to see if we've mapped the entire map yet. */
    public boolean mapped () { return (readings >= 200000); }
    
    
  // These fields measure the bounds of the map that we've seen so far.
    private int maxX = Integer.MIN_VALUE;
    private int maxY = Integer.MIN_VALUE;
    private int minX = Integer.MAX_VALUE;
    private int minY = Integer.MAX_VALUE;
    
  /** Called by Rovers to report that it has hit something at this
      pose.  The trig is done on the Rover side, but the conversion to
      map-coordinates is done inside this method.  It is synchronized
      to prevent race conditions. */
    public synchronized void reportHit (Pose xy) {
	// convert to blocks from world-tiles.
	if (whichRobot(xy) != -1) return;
	int x = (int)Math.rint(xy.x/ONEBLOCK)+START_BUFFER;
	int y = (int)Math.rint(xy.y/ONEBLOCK)+START_BUFFER;
	readings++;
	if (x < minX) minX = x;
	else if (x > maxX) maxX = x;
	if (y < minY) minY = y;
	else if (y > maxY) maxY = y;
	if ((x < 0) || (y < 0)) System.out.println("("+x+","+y+") will not work!");
	map[x][y] = true;
    }

  /** A wrapper around the thread.sleep() method. */
    private void snore (int num) {
	try {
	    Thread.sleep(num);
	} catch (InterruptedException ie) {}
	
    }

  /** Helper method that sets the role of a Rover to a given role. */
    private void setRole (int id, int role) {
	records[id].setRole(role);
    }

  /** Helper method that sets a Rover to observe another. */
    private void setObserver (int obsID, int targetID) {
	records[obsID].setObserver(targetID);
    }
    
  /** Helper method that spawns a rover and a record to track it. */
  private void spawnRover () {
	int newID = (++activeRovers)-1;
	ready[newID] = false;
	records[newID] = new Record(this, newID);
	System.out.println("Spawned Record: "+records[newID]);
	records[newID].getPose();
    }
    
  /** Helper method that returns the ID of the robot at this location,
      or -1 if no robot. Used to make sure that we're not mapping a
      robot instead of a wall. */
    private int whichRobot (Pose xy) {
	for (int i=0; i<activeRovers; ++i)
	    if (RoboMath.isRobot(xy, records[i].getPose(), RoboMath.ROBO_RADIUS)) {
		return i;
	    }
	return -1;
    }

  /** Helper method that shuts down each rover and kills the
      threads. */
    private void shutdown() {
	for (int i=0; i<activeRovers; ++i) {
	    records[i].getRover().keepGoing = false;
	}
    }

  /** Helper method to convert the boolean[][] into a PGM file for
      viewing with a normal graphics viewer. */
    private void outputMap () {
	System.out.println("X {"+minX+" "+maxX+"}");
	System.out.println("Y {"+minY+" "+maxY+"}");
	int[][] out = new int[1+maxX-minX][1+maxY-minY];
	System.out.println("Outputting size "+(maxX-minX)+" "+(maxY-minY));
	for (int i=minY; i<maxY; ++i) {
	    for (int j=minX; j<maxX; ++j) {
		boolean num = map[j][i];
		out[j-minX][i-minY] = num ? 1 : 0;
	    }
	}
	try {
	    PGMGen.generateFile(out, "Scott");
	} catch (Exception e) {}
    }

}