package BraitenbergSolution;

import MotorControlSolution.*;
import BraitenbergSolution.LightSensors.Behaviors;
//import MotorControlSolution.RobotBase;
//import MotorControlSolution.RobotVelocityController;

/**
 * <p>Implementation of Braitenberg and related behaviors.</p>
 **/
public class Behavior {

  /**
   * <p>Update period between behavior iterations in ms.</p>
   **/
  public static final long BEHAVIOR_UPDATE_PERIOD_MS = 100;

  /** // (Solution)
   * <p>Light value threshold for {@link #search}.</p> // (Solution)
   **/ // (Solution)
  public static final double SEARCH_THRESHOLD = 30.0; // (Solution)
  /** // (Solution)
   * <p>Angular rotation speed in rad/s for {@link #search}.</p> // (Solution)
   **/ // (Solution)
  public static final double SEARCH_SPEED = 0.4; // (Solution)
  /** // (Solution)
   * <p>Max rotation speed in rad/s for {@link #goToLight}.</p> // (Solution)
   **/ // (Solution)
  public static final double GOTO_SPEED = 2.0; // (Solution)
  /** // (Solution)
   * <p>Light value threshold for {@link #goToLight}.</p> // (Solution)
   **/ // (Solution)
  public static final double GOTO_THRESHOLD = 60.0; // (Solution)
  /** // (Solution)
   * <p>Max rotation speed in rad/s for the Braitenberg vehicles.</p> // (Solution)
   **/ // (Solution)
  public static final double VEHICLE_SPEED = 3.0; // (Solution)

  /**
   * <p>The robot.</p>
   **/
  protected RobotBase robot;

  /**
   * <p>The robot velocity controller.</p>
   **/
  protected RobotVelocityController robotVelocityController;

  /**
   * <p>(robot) left photocell.</p>
   **/
  protected Photocell leftPhotocell;

  /**
   * <p>(robot) right photocell.</p>
   **/
  protected Photocell rightPhotocell;
  
  /**
   * <p>The current behavior.</p>
   */
  protected Behaviors currentBehavior;
   
  /**
   * <p>Create a new behavior object.</p>
   *
   * @param robot the robot
   * @param leftPhotocell the (robot) left light sensor, already calibrated
   * @param rightPhotocell the (robot) right light sensor, already calibrated
   **/
  public Behavior(RobotBase robot,
                  Photocell leftPhotocell, Photocell rightPhotocell,
                  Behaviors behavior){
    this.robot = robot;
    this.robotVelocityController = robot.getRobotVelocityController();
    this.leftPhotocell = leftPhotocell;
    this.rightPhotocell = rightPhotocell;
    this.currentBehavior = behavior;
  }
  
  /**
   * <p>Sets the current behavior of the robot.</p>
   */
  public void setBehavior(Behaviors b) {
	  currentBehavior = b;
  }

  /**
   * <p>Convenience method to make a beep sound.</p>
   **/
  protected void beep() {
    java.awt.Toolkit.getDefaultToolkit().beep();
  }

  /**
   * <p>Convenience method to {@link
   * RobotVelocityController#setDesiredAngularVelocity} on {@link
   * #robotVelocityController}.</p>
   *
   * @param left the desired left wheel angular velocity in rad/s, positive
   * means corresp side moves forward
   * @param right the desired right wheel angular velocity in rad/s, positive
   * means corresp side moves forward
   **/
  protected void setDesiredAngularVelocity(double left, double right) {
    robotVelocityController.setDesiredAngularVelocity(left, right);
  }

  /**
   * <p>Execute the behavior.</p>
   *
   * <p>Repeatedly runs a selected behavior until the robot is estopped.</p>
   *
   * <p>Waits {@link #BEHAVIOR_UPDATE_PERIOD_MS} between updates.</p>
   **/
  public void go() {

    robot.enableMotors(true);

    boolean found = false;

    for (;;) {
      
      if (robot.estopped())
        return;
      
      double l = leftPhotocell.getValue();
      double r = rightPhotocell.getValue();

      //select your behavior here
      if (currentBehavior == Behaviors.SEARCH) {
	      if (!found)
	        found |= search(l, r);
	
	      if (found) 
	        goToLight(l, r);    	  
      }
      else if (currentBehavior == Behaviors.TWO_A) {
    	  vehicle2a(l, r);    	  
      }
      else if (currentBehavior == Behaviors.TWO_B) {
    	  vehicle2b(l, r);
      }
      else if (currentBehavior == Behaviors.THREE_A) {
    	  vehicle3a(l, r);
      }
      else if (currentBehavior == Behaviors.THREE_B) {
    	  vehicle3b(l, r);
      }
      else if (currentBehavior == Behaviors.CREATIVE) {
      // Begin Student Code
    	  loveThenExplore(l, r); // ( Solution )
      // End Student Code
      }

      try {
        Thread.sleep(BEHAVIOR_UPDATE_PERIOD_MS);
      } catch (InterruptedException e) {
        robot.estop();
        break;
      }
    }
  }
   
  /**
   * <p>Rotate in place to search for the light.</p>
   *
   * <p>Rotates until a light is found (according to some criteria).</p>
   *
   * @param l the (robot) left calibrated sensor reading
   * @param r the (robot) right calibrated sensor reading
   *
   * @return true if the light has been found
   **/
  public boolean search(double l, double r) {
    
    boolean found = false;

    // Begin Student Code
    double avg = (l+r)/2.0; // (Solution)
    double diff = (l-r); // (Solution)
    if (avg < SEARCH_THRESHOLD) { // (Solution)
      System.err.println("search: " + l + ", " + r); // (Solution)
      setDesiredAngularVelocity(-SEARCH_SPEED, SEARCH_SPEED); // (Solution)
    } else if (Math.abs(diff) > 10.0) { // (Solution)
      System.err.println("search: " + l + ", " + r); // (Solution)
      setDesiredAngularVelocity( // (Solution)
        -SEARCH_SPEED*diff/100.0, // (Solution)
        SEARCH_SPEED*diff/100.0); // (Solution)
    } else { // (Solution)
      System.err.println("found: " + l + ", " + r + " (" + avg + ")"); // (Solution)
      setDesiredAngularVelocity(0.0, 0.0); // (Solution)
      found = true; // (Solution)
    } // (Solution)
    // End Student Code

    return found;
  }

  /**
   * <p>Move in a straight line towards the light.</p>
   *
   * <p>Speed should decrease in proportion to distance from light.</p>
   *
   * @param l the (robot) left calibrated sensor reading
   * @param r the (robot) right calibrated sensor reading
   **/
  public void goToLight(double l, double r) {
    // Begin Student Code
    double avg = (l+r)/2.0; // (Solution) 
    double speed = GOTO_SPEED*(1.0-avg/100.0); // (Solution)
    if (Math.max(l, r) > GOTO_THRESHOLD) { // (Solution)
      speed = 0.0; // (Solution)
      System.err.println("got there: " + l + ", " + r + " (" + avg + "): " + speed); // (Solution)
      beep(); // (Solution)
    } // (Solution)
    System.err.println("goto: " + l + ", " + r + " (" + avg + "): " + speed); // (Solution)
    setDesiredAngularVelocity(speed, speed); // (Solution)
    // End Student Code
  }

  /**
   * <p>Braitenberg's vehicle 2a.</p>
   *
   * @param l the (robot) left calibrated sensor reading
   * @param r the (robot) right calibrated sensor reading
   **/
  public void vehicle2a(double l, double r) {
    // Begin Student Code
    System.err.println("2a: " + l + ", " + r); // (Solution)
    setDesiredAngularVelocity(VEHICLE_SPEED*l/100.0, VEHICLE_SPEED*r/100.0); //(Solution)
    // End Student Code
  }

  /**
   * <p>Braitenberg's vehicle 2b.</p>
   *
   * @param l the (robot) left calibrated sensor reading
   * @param r the (robot) right calibrated sensor reading
   **/
  public void vehicle2b(double l, double r) {
    // Begin Student Code
    System.err.println("2b: " + l + ", " + r); // (Solution)
    setDesiredAngularVelocity(VEHICLE_SPEED*r/100.0, VEHICLE_SPEED*l/100.0); //(Solution)
    // End Student Code
  }

  /**
   * <p>Braitenberg's vehicle 3a.</p>
   *
   * @param l the (robot) left calibrated sensor reading
   * @param r the (robot) right calibrated sensor reading
   **/
  public void vehicle3a(double l, double r) {
    // Begin Student Code
    System.err.println("3a: " + l + ", " + r); // (Solution)
    setDesiredAngularVelocity(VEHICLE_SPEED*(1.0-l/70.0), //(Solution)
                              VEHICLE_SPEED*(1.0-r/70.0)); //(Solution)
    // End Student Code
  }

  /**
   * <p>Braitenberg's vehicle 3b.</p>
   *
   * @param l the (robot) left calibrated sensor reading
   * @param r the (robot) right calibrated sensor reading
   **/
  public void vehicle3b(double l, double r) {
    // Begin Student Code
    System.err.println("3b: " + l + ", " + r); // (Solution)
    setDesiredAngularVelocity(VEHICLE_SPEED*(1.0-r/70.0), //(Solution)
                              VEHICLE_SPEED*(1.0-l/70.0)); //(Solution)
    // End Student Code
  }

  /** // (Solution)
   * <p>"love" (vehicle3a) a light source until we get to 50 percent intensity, // (Solution)
   * then "explore" (vehicle3b) until we get below 50 again.</p> // (Solution)
   **/ // (Solution)
  public void loveThenExplore(double l, double r) { // (Solution)
    double avg = (l+r)/2.0; // (Solution) 
    if (avg < 50.0) // (Solution)
      vehicle3a(l, r); // (Solution)
    else // (Solution)
      vehicle3b(l, r); // (Solution)
  } // (Solution)
}