import java.lang.Math;
import Javaclient.src.ColorChannel;
import Javaclient.src.ColorBlob;
import Javaclient.src.Beacon;

/** This extends the Robot class in order to provide specific
    functionality for this project.  The Rover object has a role and a
    lot of functionality that relies on which specific role the Rover
    plays at a given time.  Rovers also analyze the robot input in
    order to report world status to the Prodigy in order to help it
    make complex role-changing decisions. */
public class Rover extends Robot {

    // Variables for all Rovers.  
    private Prodigy prodigy;
    private int myID;
    private int myRole;
    private int cycles;
    public boolean keepGoing, debug;
    
  /** Connects the rover to the Stage and initializes itself with the
      Prodigy . */
    public Rover (Prodigy prodigy, int myID, int port) {
	super("localhost", port);
	this.prodigy = prodigy;
	this.myID = myID;
    }
    
    private String idstr () { return "[Bot "+myID+" ("+Roles.getString(myRole)+")]"; }
    private void debug (String str) { if (debug) System.out.println(str); }    
    private int getID () { return myID; }
  //private int getRole () { return myRole; }

  /** Called by the Prodigy to set the role and thus change the
      behavior of the Rover. */
    public void setRole (int newRole) { 
	debug(""+this+" received a role "+newRole);
	if (myRole == newRole) return;
	ppd.setSpeed(0,0);
	myRole = newRole;
	if (newRole == Roles.OBSERVER) {
	    justStarted = true;
	    backup = false;
	    scoutCycles = 0;
	}
	else if (newRole == Roles.SCOUT) {
	    scoutCycles = 0;
	    backup = false;
	    wallwalk = 0;
	    lost = false;
	}
	System.out.println(""+this+" received new role.");
    }
    
    private Pose getPose () { 
	return prodigy.getPose(myID); 
    }
    
  /** inheirited from the Robot class in order to provide a concrete
      lifecycle.  Loops forever until the Prodigy tells us to shut
      down. The functionality forks depending on the role. */
    public void lifeCycle () {
	pc.readAll();
	keepGoing = true;
	ppd.setSpeed(0,0);
	System.out.println("Robot "+myID+" lifecycle started.");
	while (keepGoing) {
	    debug("Robot "+myID+" role: "+myRole);
	    pc.readAll();
	    if (myRole == Roles.SCOUT)
		scout();
	    else if (myRole == Roles.OBSERVER) {
		observe();
	    }
	    cycles++;
	}
    }
    
    // VARIABLES FOR OBSERVER STATE
    private static int lostDistance = 8000;
    private boolean lost = false;
    private boolean justStarted = true;
    private boolean hybrid = false;

  /** Signal from the Prodigy to tell us to become a hybrid
      Observer/Scout (for 3+ robot implementations only) */
    public void setHybrid () { hybrid = true; }

  /** Called by the lifeCycle method to provide the bulk of the
      observer's functionality. */
    private void observe() {
	if (justStarted) {
	    if (findTarget()) {
		System.out.println(idstr()+" found scout, initiating coordinates.");
		trackTarget();
		prodigy.readyToObserve(myID);
		justStarted = false;
	    }
	}
	else {
	    
	    if (!trackTarget()) {
		if (!lost) System.out.println(""+this+" lost the scout.  Notifying...");
		ppd.setSpeed(0,0);
		// We lost somebody!!!
		prodigy.lostScout(myID);
		lost = true;
	    }
	    else {
		if (hybrid)
		    findTarget(100);
		// If we were lost, now we're found.  Notify the prodigy of the reunion.
		if (lost) {
		    prodigy.switchOff(myID);
		}
	    }
	}
    }

  /** Makes our Observer look toward our target */
    private boolean findTarget () {
	return findTarget(0);
    }

  /** Makes our hybrid Observer look & move at a given speed toward
      our target */
    private boolean findTarget (int fwdSpeed) {
	int rotation=0;
	boolean foundTarget = false;
	Beacon target = getActiveBeacon();
	if (target != null) {
	    if (Math.abs(target.bearing) <= 4) {
		foundTarget = true;
		rotation = 0;
	    }
	    else
		rotation = target.bearing;
	}
	else {
	    rotation = 120;
	    fwdSpeed = 0;
	}
	ppd.setSpeed(fwdSpeed,(int)rotation);
	return foundTarget;
    }

  /** Observes the target and tells the Prodigy of the change in
      direction or pose. */
    private boolean trackTarget () {
	Beacon targetBeacon = getActiveBeacon();
	if (targetBeacon == null) {
	    System.out.println("We lost the scout.");
	    return false;
	}
	if (targetBeacon.range >= lostDistance) return false;
	observeChange(targetBeacon);
	return true;
    }
    
  /** Helper to observe changes in target pose. */
    private boolean observeChange(Beacon beacon) {
	Pose targetPose = getPoseFor(beacon);
	prodigy.observeChange(myID, targetPose);
	return true;
    }

  /** Checks for fiduciary devices in range (beacons) */
    private Beacon getActiveBeacon () {
	Beacon beacons[] = fi.getBeacons();
	for (int i=0; i<beacons.length; ++i) {
	    Beacon curr = beacons[i];
	    if (curr.range > 0) return curr;
	}
	return null;
    }

  
  /** Translates a Beacon (Fiduciary) object into one of our Pose objects. */
    private Pose getPoseFor (Beacon beacon) {
	Pose newPose = new Pose();
	shootRay(getPose(), (beacon.bearing+90)*2, beacon.range, newPose);
	newPose.heading = computeHeading(getPose().heading, beacon.orientation);
	return newPose;
    }

  /** Given a fiduciary object with a relative heading, compute the
      <i>actual</i> heading. */
    private short computeHeading (short myHeading, short beaconHeading) {
	return (short)((-1*beaconHeading)+myHeading);
    }
    

    // VARIABLES FOR SCOUT STATE

    private static int scoutReadPeriod = 20;
    private static int scoutForwardSpeed = 200;
    private static int scoutRotateSpeed = 30;
    private static int caughtUp = 900;
    private int scoutCycles = scoutReadPeriod;
    private boolean backup = false;
    private boolean catchup = false;
    private boolean scoutInit = true;
    private int wallwalk = 0;
    private static int desiredHeading = 0;

  /** Method for the Prodigy to set a target heading so the Scouts
      will move in certain directions and thus better explore the
      map. */
    public static void setDesiredHeading (int heading) {
	desiredHeading = heading;
    }
    
  /** Helper method to decide whether to read the walls or not. */
    private boolean timeToRead () {
	return (!lost && (scoutCycles == 0));
    }

  /** Signal from the Prodigy to tell a Scout that it's been lost and
      needs to return to sight of its Observer. */
    public void backup () {
	if (!backup)
	    System.out.println(""+this+" must back up to be seen again.");
	backup = true;
    }

  /** Signal for a scout to catch up to its observer so that they can
      map another direction or switch off. */
    public void catchup () {
	if (!catchup)
	    System.out.println(""+this+" must catch up to the other.");
	catchup = true;
    }

  /** A reminder from the Prodigy to a Scout so that we will
      initialize our heading to the desired direction. */
    public void rememberToReorient () {
	System.out.println(""+this+" must remember to reorient");
	scoutInit = true;
    }

  /** delegated from the lifeCycle method so that we can encapsulate
      all Scout functionality. */
    private void scout () {
	if (scoutInit) {
	    System.out.print("-Init-");
	    //System.out.println("We're initializing.");
	    if (Math.abs(getPose().heading-desiredHeading) < 3)
		scoutInit = false;
	    else {
		//int mult = (getPose().heading < 0) ? -1 : 1;
		
		//int mult =  RoboMath.leftOf(getPose().heading, desiredHeading) ? -1 : 1;
		int speed = RoboMath.degreesAway(getPose().heading, desiredHeading);
		System.out.print("["+speed+"]");
		ppd.setSpeed(0,-1*speed);
	    }
	    return;
	}
	if (catchup) {
	    System.out.print("-Catch-");
	    findTarget(50);
	    Beacon target = getActiveBeacon();
	    if (target == null) {
		System.out.println("We're not very good at catching up.");
		//catchup = false;
	    }
	    else if (target.range < caughtUp) {
		catchup = false;
		ppd.setSpeed(0,0);
		prodigy.caughtUp(myID);
	    }
	}
	if (backup) {
	    ppd.setSpeed(-3*scoutForwardSpeed,0);
	    //delay();
	}
	else {
	    if (timeToRead()) {
		ppd.setSpeed(0,0);
		delay();
		readWalls();
	    }
	    else {
		int laserData[] = gatherLaserData();
		int danger = danger(laserData);
		int fwdSpeed = decideForwardSpeed(laserData[0], danger);
		int rotSpeed = decideRotationalSpeed(danger, laserData[1]);
		ppd.setSpeed(fwdSpeed, rotSpeed);
	    }
	    scoutCycles++;
	    if (scoutCycles == scoutReadPeriod) scoutCycles = 0;
	}
    }

  /** For wall-walking - we don't care about certain degrees of our lasers. */
    private boolean isValid (int laserIndex) {
	return ((laserIndex > 45) && (laserIndex < 315));
    }

  /** For Scouts to decide how much to rotate given the laser readings. */
    private int decideRotationalSpeed (int immediateDanger, int index) {
	int rot = 10;
	if (isDiag(index)) rot = 20;
	if (isFront(index)) rot = 30;
	if (wallwalk == 0) {
	    if (immediateDanger != 0) {
		wallwalk = -1*immediateDanger;
	    }
	    else
		return 0;
	}
	if (wallwalk != 0) {
	    if (immediateDanger == 0) {
		if (Math.abs(desiredHeading-getPose().heading)<=10) {
		    System.out.println(""+this+" stopped following the wall");
		    wallwalk = 0;
		    return 0;
		}
		else {
		    // go toward wall.
		    return rot*wallwalk;
		}
	    }
	    else {
		// go away from wall.
		return rot*-wallwalk;
	    }
	}
	return 0;
    }

  /** For Scouts to decide how fast to go given the laser readings. */
    private int decideForwardSpeed (int closest, int danger) {
	if ((danger != 0)&&(closest < 600)) return 0;
	return (closest > 800) ? 400 : closest/3;
    }

  /** Analyzes the laser readings and returns the closest danger and
      where that danger is within our view. */
    private int[] gatherLaserData () {
	pc.readAll();
	int readings[] = lpd.getRange();
	int closest = Integer.MAX_VALUE;
	int closestIndex = 0;
	for (int i=45; i<315; i+=2) {
	    if (readings[i] < closest) {
		closest = readings[i];
		closestIndex = i;
	    }
	}
	return new int[] { closest, closestIndex };
    }

    private static int frontDanger = 700;
    private static int diagDanger = 600;
    private static int sideDanger = 500;
    
  /** Tells us whether a laser index is peripheral. */
    private boolean isSide (int index) {
	return ((index <= 80)&&(index > 25)) || ((index > 280)&&(index <= 335));
    }
    
  /** Tells us whether a laser index is semi-frontal. */
    private boolean isDiag (int index) {
	return ((index <= 135)&&(index > 80)) || ((index > 225)&&(index <= 280));
    }

  /** Tells us whether a laser index is frontal. */
    private boolean isFront (int index) {
	return ((index <= 180)&&(index > 135)) || ((index > 180)&&(index <= 225));
    }

  /** Tells us whether a laser reading is dangerous or not, and which
      direction we need to turn to avoid it. */
    private int danger (int[] laserData) {
	int dir = -1;
	if (laserData[1] <= 180) {
	    dir = 1;
	}
	if (isSide(laserData[1])) return (laserData[0]<sideDanger) ? dir : 0;
	else if (isDiag(laserData[1])) return (laserData[0]<diagDanger) ? dir : 0;
	else if (isFront(laserData[1])) return (laserData[0]<frontDanger) ? dir : 0;
	return 0;
    }

  /** Delays us and gathers the world's data so we can make a good laser reading. */
    private void delay () {
	ppd.setSpeed(0,0);
	for (int i=0; i<3; ++i) pc.readAll();
    }

  /** Does the trig functions to compute a laser reading at a certain degree. */
    private Pose shootDegree (int degree) {
	int readings[] = lpd.getRange();
	Pose myPose = getPose();
	int ray = readings[(degree*2)-1];
	Pose target = new Pose();
	shootRay(myPose, (degree*2)-1, ray, target);
	return target;
    }

  /** Reads the walls, given the laser data. */
    private void readWalls () {
	System.out.println(""+this+" reading walls.");
	int readings[] = lpd.getRange();
	Pose myPose = getPose();
	Pose target = new Pose();
	for (int i=0; i<360; i++) {
	    int ray = readings[i];
	    if (ray >= 7900) continue;
	    shootRay(myPose, i, ray, target);
	    prodigy.reportHit(target);
	}
    }

  /** Given our pose, the degree, and a ray, we can convert that to
      world coordinates.  Used for wall-reading with laser rays. */
    private void shootRay (Pose myPose, int degrees, int ray, Pose target) {
	double theta = RoboMath.toRadians(myPose.heading);
	double relTheta = RoboMath.toRadians(90.0-((360-degrees)/2.0));
	double relX = ray*Math.cos(relTheta);
	double relY = ray*Math.sin(relTheta);
	target.x = (int)Math.rint((relX*Math.cos(theta))+(relY*Math.sin(theta)))
	    +myPose.x;
	target.y = (int)Math.rint((-1*relX*Math.sin(theta))+(relY*Math.cos(theta)))
	    +myPose.y;
    }

  /** Converts our Rover to a conventional Java String for output. */
    public String toString () {
	return idstr()+" "+getPose();
    }
    
}