@Override
  public void startWaypoint(
      final UtmPose waypoint, final String controller, final WaypointObserver obs) {

    final double dt = (double) UPDATE_INTERVAL_MS / 1000.0;

    // Keep a reference to the navigation flag for THIS waypoint
    final AtomicBoolean isNavigating = new AtomicBoolean(true);
    System.out.println("\nStart Waypoint to " + waypoint.pose.position.x);
    // Set this to be the current navigation flag
    synchronized (_navigationLock) {
      if (_isNavigating != null) _isNavigating.set(false);
      _isNavigating = isNavigating;
      _waypoint = waypoint.clone();
    }

    new Thread(
            new Runnable() {

              @Override
              public void run() {
                while (isNavigating.get()) {
                  // If we are not set in autonomous mode, don't try to drive!
                  if (!_isAutonomous.get()) {
                    // TODO: probably should add a "paused" state
                    if (obs != null) obs.waypointUpdate(WaypointState.OFF);
                  } else {
                    // Report our status as moving toward target
                    if (obs != null) obs.waypointUpdate(WaypointState.GOING);

                    // Get the position of the vehicle and the waypoint
                    // TODO: fix threading issue (fast stop/start)
                    Pose pose = _pose.pose.pose.pose;
                    Pose wpPose = _waypoint.pose;

                    // TODO: handle different UTM zones!
                    // Figure out how to drive to waypoint
                    _controller.controller.update(SimpleBoatSimulator.this, dt);

                    // TODO: measure dt directly instead of approximating

                    // Check for termination condition
                    // TODO: termination conditions tested by controller
                    double dist = distToGoal(pose, wpPose);

                    if (dist <= 6.0) {
                      System.out.println("Should reach a waypoint now.");
                      break;
                    }
                  }

                  // Pause for a while
                  try {
                    Thread.sleep(UPDATE_INTERVAL_MS);
                  } catch (InterruptedException e) {
                  }
                }

                // Stop the vehicle
                _velocity.linear.x = 0.0;
                _velocity.linear.y = 0.0;
                _velocity.linear.z = 0.0;
                _velocity.angular.x = 0.0;
                _velocity.angular.y = 0.0;
                _velocity.angular.z = 0.0;

                // Upon completion, report status
                // (if isNavigating is still true, we completed on our own)
                if (isNavigating.getAndSet(false)) {
                  if (obs != null) {
                    obs.waypointUpdate(WaypointState.DONE);
                    System.out.println("Should report reaching a waypoint now.");
                  }
                } else {
                  if (obs != null) {
                    obs.waypointUpdate(WaypointState.CANCELLED);
                    System.out.println("Should report cancelling a waypoint now.");
                  }
                }
              }
            })
        .start();
  }
  @Override
  public void startWaypoint(final UtmPose waypoint, String controller, final WaypointObserver obs) {

    // Keep a reference to the navigation flag for THIS waypoint
    final AtomicBoolean isNavigating = new AtomicBoolean(true);
    System.out.println("\nStart Waypoint to " + waypoint.pose.position.x);
    // Set this to be the current navigation flag

    synchronized (_navigationLock) {
      if (_isNavigating != null) {
        _isNavigating.set(false);
      }
      _isNavigating = isNavigating;
      _waypoint = waypoint.clone();
    }

    new Thread(
            new Runnable() {

              @Override
              public void run() {
                while (isNavigating.get()) {

                  // If we are not set in autonomous mode, don't try to drive!
                  // if (_isAutonomous.get()) {

                  // Get the position of the vehicle and the waypoint
                  UtmPoseWithCovarianceStamped state = getState();
                  Pose pose = state.pose.pose.pose;
                  Pose wpPose = waypoint.pose;

                  // TODO: handle different UTM zones!

                  // Compute the distance and angle to the waypoint
                  // TODO: compute distance more efficiently
                  double distance =
                      Math.sqrt(
                          Math.pow((wpPose.position.x - pose.position.x), 2)
                              + Math.pow((wpPose.position.y - pose.position.y), 2));
                  double angle =
                      Math.atan2(
                              (wpPose.position.y - pose.position.y),
                              (wpPose.position.x - pose.position.x))
                          - QuaternionUtils.toYaw(pose.orientation);
                  angle = normalizeAngle(angle);

                  // Choose driving behavior depending on direction and
                  // where we are
                  if (Math.abs(angle) > 1.0) {

                    // If we are facing away, turn around first
                    _velocity.linear.x = 1.0;
                    _velocity.angular.z = Math.max(Math.min(angle / 1.0, 5.0), -5.0);
                  } else if (distance >= 3.0) {

                    // If we are far away, drive forward and turn
                    _velocity.linear.x = Math.min(distance / 5.0, 400.0);
                    _velocity.angular.z = Math.max(Math.min(angle / 10.0, 5.0), -5.0);
                  } else /* (distance < 3.0) */ {
                    break;
                  }
                  // }

                  // Pause for a while
                  try {
                    Thread.sleep(LOCAL_UPDATE_INTERVAL);
                    if (obs != null) {
                      obs.waypointUpdate(WaypointState.GOING);
                    }
                  } catch (InterruptedException e) {
                    e.printStackTrace();
                  }
                }

                // Stop the vehicle
                _velocity.linear.x = 0.0;
                _velocity.angular.z = 0.0;

                // Upon completion, report status
                // (if isNavigating is still true, we completed on our own)
                if (isNavigating.getAndSet(false)) {
                  if (obs != null) {
                    obs.waypointUpdate(WaypointState.DONE);
                  }
                } else {
                  if (obs != null) {
                    obs.waypointUpdate(WaypointState.CANCELLED);
                  }
                }
              }
            })
        .start();
  }