package VisualServoSolution;

import Carmen.*;

/**
 * 
 * @author previous TA's, prentice, vona
 *
 */
public class VisualServo implements CameraHandler, ParamChangeHandler {

  /**
   * <p>The blob tracker.</p>
   **/
  private BlobTracking blobTrack = null;

  /**
   * <p>Set default central host here.</p>
   **/
  public static String centralHost = "localhost";

  private boolean paramChanged = false;
  
  /**
   * <p>Create a new VisualServo object.</p>
   */
  public VisualServo() {

    setInitialParams();

    // subscribe to receive camera and param change messages
    CameraMessage.subscribe(this);
    ParamChangeMessage.subscribe(this);
  }

  protected void setInitialParams() {

    // Begin Student Code

    // set initial parameter values as you desire

      //EXAMPLE
//    Param.set("visualservo", "foo", "value");


    Param.set("visualservo", "target_hue_level", Double.toString(0.5)); //(Solution)
    Param.set("visualservo", "hue_threshold", Double.toString(0.4)); //(Solution)
    Param.set("visualservo", "saturation_level", Double.toString(0.25)); //(Solution)
    // Units are fraction of total number of pixels detected in blob // (Solution)
    Param.set("visualservo", "blob_size_threshold", Double.toString(0.025)); //(Solution)
    Param.set("visualservo", "target_radius", Double.toString(0.13)); //(Solution)
    Param.set("visualservo", "desired_fixation_distance", //(Solution)
              Double.toString(0.5)); //(Solution)
    Param.set("visualservo", "translation_error_tolerance", //(Solution)
              Double.toString(0.05)); //(Solution)
    Param.set("visualservo", "translation_velocity_gain", //(Solution)
              Double.toString(0.25)); //(Solution)
    Param.set("visualservo", "translation_velocity_max", //(Solution)
              Double.toString(0.05)); //(Solution)
    Param.set("visualservo", "rotation_error_tolerance", Double.toString(0.2)); //(Solution)
    Param.set("visualservo", "rotation_velocity_gain", Double.toString(0.25)); //(Solution)
    Param.set("visualservo", "rotation_velocity_max", Double.toString(0.05)); //(Solution)
    Param.set("visualservo", "use_gaussian_blur", "on"); //(Solution)

    // End Student Code
  }
  
  /**
   * <p>Handle a CameraMessage. Perform blob tracking and
   * servo robot towards target.</p>
   * 
   * @param a received camera message 
   */
  public void handle(CameraMessage cameraMsg) {

    System.err.print("c");

    // on first camera message, create new BlobTracking instance
    if (blobTrack == null || paramChanged) {

      blobTrack = new BlobTracking(cameraMsg.width, cameraMsg.height);

      paramChanged = false;

      // Begin Student Code
      
      // set parameters on blobTrack as you desire

      //EXAMPLE
//      blobTrack.foo =
//        Double.parseDouble(Param.query("visualservo", "foo"));

      blobTrack.targetHueLevel = //(Solution)
        Double.parseDouble(Param.query("visualservo", "target_hue_level")); //(Solution)
      blobTrack.hueThreshold = //(Solution)
        Double.parseDouble(Param.query("visualservo", "hue_threshold")); //(Solution)
      blobTrack.saturationLevel = //(Solution)
        Double.parseDouble(Param.query("visualservo", "saturation_level")); //(Solution)
      blobTrack.blobSizeThreshold = //(Solution)
        Double.parseDouble(Param.query("visualservo", "blob_size_threshold")); //(Solution)
      blobTrack.targetRadius = //(Solution)
        Double.parseDouble(Param.query("visualservo", "target_radius")); //(Solution)
      blobTrack.desiredFixationDistance = //(Solution)
        Double.parseDouble(Param.query("visualservo", //(Solution)
                                       "desired_fixation_distance")); //(Solution)
      blobTrack.translationErrorTolerance = //(Solution)
        Double.parseDouble(Param.query("visualservo", //(Solution)
                                       "translation_error_tolerance")); //(Solution)
      blobTrack.translationVelocityGain = //(Solution)
        Double.parseDouble(Param.query("visualservo", //(Solution)
                                       "translation_velocity_gain")); //(Solution)
      blobTrack.translationVelocityMax = //(Solution)
        Double.parseDouble(Param.query("visualservo", //(Solution)
                                       "translation_velocity_max")); //(Solution)
      blobTrack.rotationErrorTolerance = //(Solution)
        Double.parseDouble(Param.query("visualservo", //(Solution)
                                       "rotation_error_tolerance")); //(Solution)
      blobTrack.rotationVelocityGain = //(Solution)
        Double.parseDouble(Param.query("visualservo", //(Solution)
                                       "rotation_velocity_gain")); //(Solution)
      blobTrack.rotationVelocityMax = //(Solution)
        Double.parseDouble(Param.query("visualservo", //(Solution)
                                       "rotation_velocity_max")); //(Solution)
      blobTrack.useGaussianBlur = //(Solution)
        Param.parseOnoff(Param.query("visualservo", "use_gaussian_blur")); //(Solution)

      System.err.println("  target hue level: " + blobTrack.targetHueLevel); //(Solution)
      System.err.println("  hue threshold: " + blobTrack.hueThreshold); //(Solution)
      System.err.println("  saturation level: " + blobTrack.saturationLevel); //(Solution)
      System.err.println("  blob size threshold: " + //(Solution)
                         blobTrack.blobSizeThreshold); //(Solution)
      System.err.println("  target radius: " + blobTrack.targetRadius); //(Solution)
      System.err.println("  desired fixation distance: " + //(Solution)
                         blobTrack.desiredFixationDistance); //(Solution)
      System.err.println("  translation error tolerance: " + //(Solution)
                         blobTrack.translationErrorTolerance); //(Solution)
      System.err.println("  translation velocity gain: " + //(Solution)
                         blobTrack.translationVelocityGain); //(Solution)
      System.err.println("  translation velocity max: " +  //(Solution)
                         blobTrack.translationVelocityMax); //(Solution)
      System.err.println("  rotation error tolerance: " + //(Solution)
                         blobTrack.rotationErrorTolerance); //(Solution)
      System.err.println("  rotation velocity gain: " +  //(Solution)
                         blobTrack.rotationVelocityGain); //(Solution)
      System.err.println("  rotation velocity max: " +  //(Solution)
                         blobTrack.rotationVelocityMax); //(Solution)
      System.err.println("  use gaussian blur: " + blobTrack.useGaussianBlur); //(Solution)

      // End Student Code
    }

    VisionImageMessage visionMsg = new VisionImageMessage(cameraMsg);
    
    
//     apply vision processing to raw camera image
//    blobTrack.apply(cameraMsg.image, visionMsg.image);
    Image src = new Image(cameraMsg.image, cameraMsg.width, cameraMsg.height);
    Image dest = new Image(visionMsg.image, visionMsg.width, visionMsg.height);
    blobTrack.apply(src, dest);
    
    // publish newly formed vision message
    visionMsg.image = dest.toArray();
    visionMsg.publish();
    
    // Begin Student Code

    // move robot towards target

    Robot.setVelocity(blobTrack.translationVelocityCommand, blobTrack.rotationVelocityCommand);   //(Solution)
    // End Student Code
  }

  
  /**
   * <p>Handle a ParamChangeMessage pertaining to VisualServo 
   * and update instance variables</p>
   * 
   * @param a received param change message 
   */
  public void handle(ParamChangeMessage msg) {

    if (!msg.moduleName.equals("visualservo"))
      return;

    System.err.println("VisualServo Parameter Changed: " +
                       msg.variableName + " = " + msg.newValue);

    paramChanged = true;
  }
  
  /**
   * <p>
   * Run the VisualServo process
   * </p>
   * 
   * @param optional
   *            command-line argument containing hostname
   */
  public static void main(String argv[]) {
    // if hostname is given on command-line, update centralHost
    if (argv.length > 0) {
      centralHost = argv[0];
    }
    Robot.initialize("VisualServo", centralHost);
    VisualServo visualServo = new VisualServo();
    Robot.dispatch();
  }
}