private static double getAvgVelocityToNextStop(Observation obs, BlockState blockState) {

    StopTimeEntry stop = blockState.getBlockLocation().getNextStop().getStopTime();
    final double distToDest =
        TurboButton.distance(obs.getLocation(), stop.getStop().getStopLocation());
    final int currSchedTime =
        (int) (obs.getTime() - blockState.getBlockInstance().getServiceDate()) / 1000;

    final double expectedVelocity;
    if (currSchedTime >= stop.getArrivalTime()) {
      /*
       * Assumption here is that the driver will speed up to get to the stop on time.
       * TODO: check this.
       */
      expectedVelocity = _avgVelocity * 3d / 2d;
    } else {
      expectedVelocity = distToDest / (stop.getArrivalTime() - currSchedTime);
    }

    return expectedVelocity;
  }
  public RUN_TRANSITION_STATE getRunTransitionState(Context context) {
    final VehicleState state = context.getState();
    final VehicleState parentState = context.getParentState();
    final Observation obs = context.getObservation();
    final Observation prevObs = obs.getPreviousObservation();
    final BlockStateObservation blockStateObs = state.getBlockStateObservation();

    if (parentState != null
        && prevObs != null
        && MotionModelImpl.hasRunChanged(
            parentState.getBlockStateObservation(), state.getBlockStateObservation())) {
      if (!Objects.equal(obs.getRecord().getRunId(), prevObs.getRecord().getRunId())
          || !Objects.equal(obs.getRecord().getOperatorId(), prevObs.getRecord().getOperatorId())) {

        return RUN_TRANSITION_STATE.RUN_CHANGE_INFO_DIFF;
      } else {
        EVehiclePhase parentPhase = parentState.getJourneyState().getPhase();

        /*
         * TODO clean up this hack We are really in-progress, but because of the
         * out-of-service headsign, we can't report it as in-progress
         */
        if (context.getObservation().hasOutOfServiceDsc()
            && EVehiclePhase.DEADHEAD_DURING == parentPhase
            && (blockStateObs != null && blockStateObs.isOnTrip()))
          parentPhase = EVehiclePhase.IN_PROGRESS;

        if (EVehiclePhase.isLayover(parentPhase)
            || EVehiclePhase.DEADHEAD_AFTER == parentPhase
            || EVehiclePhase.DEADHEAD_BEFORE == parentPhase
            || EVehiclePhase.DEADHEAD_DURING == parentPhase) {
          if (parentState.getBlockStateObservation() != null
              && !parentState.getBlockStateObservation().isRunFormal()
              && state.getJourneyState().getPhase() == EVehiclePhase.IN_PROGRESS)
            return RUN_TRANSITION_STATE.RUN_CHANGE_FROM_OOS_NORUN_TO_IN;
          else if (state.getJourneyState().getPhase() == EVehiclePhase.IN_PROGRESS)
            return RUN_TRANSITION_STATE.RUN_CHANGE_FROM_OOS_TO_IN;
          else return RUN_TRANSITION_STATE.RUN_CHANGE_FROM_OOS_TO_OSS;
        } else {
          return RUN_TRANSITION_STATE.RUN_CHANGE_FROM_IS;
        }
      }
    } else {
      if (blockStateObs == null
          && parentState != null
          && parentState.getBlockStateObservation() == null)
        return RUN_TRANSITION_STATE.RUN_CHANGE_FROM_OOS_TO_OSS;
      else return RUN_TRANSITION_STATE.RUN_NOT_CHANGED;
    }
  }
  public DSC_STATE getDscState(final Context context) {
    final VehicleState state = context.getState();
    final Observation obs = context.getObservation();

    final JourneyState js = state.getJourneyState();
    EVehiclePhase phase = js.getPhase();

    final String observedDsc = obs.getLastValidDestinationSignCode();

    if (observedDsc == null || !obs.hasValidDsc() || obs.hasOutOfServiceDsc()) {
      /** If we haven't yet seen a valid DSC, or it's out of service */
      if (!obs.hasValidDsc()) {
        return DSC_STATE.DSC_NOT_VALID;
      } else if (EVehiclePhase.IN_PROGRESS == phase && obs.hasOutOfServiceDsc()) {
        return DSC_STATE.DSC_OOS_IP;
      } else {
        return DSC_STATE.DSC_OOS_NOT_IP;
      }
    } else {
      final BlockState bs = state.getBlockState();
      if (bs == null) {
        return DSC_STATE.DSC_IS_NO_BLOCK;
      } else {
        final Set<String> dscs = Sets.newHashSet();
        dscs.add(bs.getDestinationSignCode());
        final Set<AgencyAndId> routes = Sets.newHashSet();
        routes.add(bs.getBlockLocation().getActiveTrip().getTrip().getRouteCollection().getId());

        /*
         * dsc changes occur between parts of a block, so account for that
         */
        if (EVehiclePhase.LAYOVER_DURING == phase || EVehiclePhase.DEADHEAD_DURING == phase) {
          final BlockTripEntry nextTrip = bs.getBlockLocation().getActiveTrip().getNextTrip();

          if (nextTrip != null) {
            final String dsc =
                _destinationSignCodeService.getDestinationSignCodeForTripId(
                    nextTrip.getTrip().getId());
            if (dsc != null) dscs.add(dsc);
            routes.add(nextTrip.getTrip().getRouteCollection().getId());
          }
        }

        /*
         * Check if it's active, since deadhead-after/before's
         * can/should have o.o.s. dsc's, although they often don't.
         * TODO perhaps check the last non-o.o.s. dsc to give
         * higher weight to deadhead-after's that match (when
         * we have good run-info, perhaps).
         */
        if (!EVehiclePhase.isActiveAfterBlock(phase)
            && !EVehiclePhase.isActiveBeforeBlock(phase)
            && dscs.contains(observedDsc)) {
          if (EVehiclePhase.IN_PROGRESS == phase) return DSC_STATE.DSC_MATCH;
          else return DSC_STATE.DSC_DEADHEAD_MATCH;
        } else {
          /*
           * a dsc implies a route. even though the reported dsc may not match,
           * we expect the general route to be the same...
           */
          boolean routeMatch = false;
          for (final AgencyAndId dscRoute : obs.getDscImpliedRouteCollections()) {
            if (routes.contains(dscRoute)) {
              routeMatch = true;
              break;
            }
          }

          if (routeMatch) return DSC_STATE.DSC_ROUTE_MATCH;
          else return DSC_STATE.DSC_NO_ROUTE_MATCH;
        }
      }
    }
  }
  @Override
  public SensorModelResult likelihood(Context context)
      throws BadProbabilityParticleFilterException {

    final VehicleState state = context.getState();
    final VehicleState parentState = context.getParentState();
    final Observation obs = context.getObservation();
    final EVehiclePhase phase = state.getJourneyState().getPhase();
    final BlockStateObservation blockStateObs = state.getBlockStateObservation();

    //    /*-
    //     * TODO clean up this hack
    //     * We are really in-progress, but because of the
    //     * out-of-service headsign, we can't report it as in-progress
    //     */
    //    if (obs.hasOutOfServiceDsc() && EVehiclePhase.DEADHEAD_DURING == phase
    //        && (blockStateObs != null && blockStateObs.isOnTrip()))
    //      phase = EVehiclePhase.IN_PROGRESS;

    final SensorModelResult result = new SensorModelResult("pEdge", 1d);

    if (obs.getPreviousObservation() == null || parentState == null) {
      if (EVehiclePhase.isActiveDuringBlock(phase)) {
        result.addResultAsAnd("no prev. obs./vehicle-state(in-progress)", 1d);
      } else {
        result.addResultAsAnd("no prev. obs./vehicle-state", 1d);
      }
      return result;
    }

    if (obs.isAtBase()) {
      result.addResultAsAnd("pNotInProgress(base)", 1d);
      return result;
    }

    if (blockStateObs == null) {

      result.addResultAsAnd(computeNoEdgeMovementLogProb(state, obs));
      return result;
    }

    /*
     * Edge Movement
     */
    final boolean previouslyInactive = parentState.getBlockState() == null;
    final boolean newRun =
        MotionModelImpl.hasRunChanged(
            parentState.getBlockStateObservation(), state.getBlockStateObservation());

    final boolean hasMoved = !state.getMotionState().hasVehicleNotMoved();
    if (!previouslyInactive && !newRun) {

      /*
       * This could be a transition from being on a trip geom to off, and vice
       * versa.
       */
      if (EVehiclePhase.IN_PROGRESS != phase) {
        /*
         * We're currently not in-progress
         */
        if (EVehiclePhase.isActiveDuringBlock(phase)) {
          if (state
              .getBlockState()
              .getBlockInstance()
              .equals(parentState.getBlockState().getBlockInstance())) {
            result.addResultAsAnd(
                computeEdgeMovementLogProb(obs, state, parentState, hasMoved, true));
          } else {
            result.addResultAsAnd(computeNoEdgeMovementLogProb(state, parentState, obs));
          }
        } else {
          result.addResultAsAnd(computeNoEdgeMovementLogProb(state, obs));
        }
      } else if (EVehiclePhase.IN_PROGRESS != parentState.getJourneyState().getPhase()) {
        /*
         * We were previously not in-progress, and now we are.
         */
        if (EVehiclePhase.isActiveDuringBlock(parentState.getJourneyState().getPhase())) {
          result.addResultAsAnd(
              computeEdgeMovementLogProb(obs, state, parentState, hasMoved, true));
        } else {
          result.addResultAsAnd(computeNoEdgeMovementLogProb(state, parentState, obs));
        }
      } else {
        /*
         * We're in-progress and were before.
         */
        result.addResultAsAnd(computeEdgeMovementLogProb(obs, state, parentState, hasMoved, false));
      }

    } else {
      /*
       * We have a new run, or just got one after having nothing.
       */

      if (EVehiclePhase.IN_PROGRESS == phase) {
        if (EVehiclePhase.isActiveDuringBlock(parentState.getJourneyState().getPhase())) {
          result.addResultAsAnd(
              computeEdgeMovementLogProb(obs, state, parentState, hasMoved, false));
        } else {
          result.addResultAsAnd(computeNoEdgeMovementLogProb(state, parentState, obs));
        }

      } else {
        result.addResultAsAnd(computeNoEdgeMovementLogProb(state, obs));
      }
    }

    return result;
  }
  private static final SensorModelResult computeEdgeMovementLogProb(
      Observation obs,
      VehicleState state,
      VehicleState parentState,
      boolean hasMoved,
      boolean isDuring)
      throws BadProbabilityParticleFilterException {

    final BlockState blockState = state.getBlockState();
    final BlockState parentBlockState = parentState.getBlockState();
    final double currentDab;
    final double prevDab;
    if (!blockState.getBlockInstance().equals(parentBlockState.getBlockInstance())) {
      /*
       * Note: this isn't really working like we want it to, since there are
       * separate shapes that share the same segments of geometry.
       */
      if (!Objects.equal(
          parentBlockState.getBlockLocation().getActiveTrip().getTrip().getShapeId(),
          blockState.getBlockLocation().getActiveTrip().getTrip().getShapeId())) {
        return computeNoEdgeMovementLogProb(state, parentState, obs);
      }
      currentDab =
          blockState.getBlockLocation().getDistanceAlongBlock()
              - blockState.getBlockLocation().getActiveTrip().getDistanceAlongBlock();
      prevDab =
          parentBlockState.getBlockLocation().getDistanceAlongBlock()
              - parentBlockState.getBlockLocation().getActiveTrip().getDistanceAlongBlock();
    } else {
      currentDab = blockState.getBlockLocation().getDistanceAlongBlock();
      prevDab = parentBlockState.getBlockLocation().getDistanceAlongBlock();
    }

    final SensorModelResult result = new SensorModelResult("edge-move");
    final double dabDelta = currentDab - prevDab;
    if (ParticleFilter.getDebugEnabled()) result.addResult("dist: " + dabDelta, 0d);

    final double obsTimeDelta = (obs.getTime() - obs.getPreviousObservation().getTime()) / 1000d;
    final double expAvgDist;
    if (hasMoved) {
      if (!isDuring || blockState.getBlockLocation().getNextStop() == null) {
        expAvgDist = getAvgVelocityBetweenStops(blockState) * obsTimeDelta;
      } else {
        expAvgDist = getAvgVelocityToNextStop(obs, blockState) * obsTimeDelta;
      }
    } else {
      expAvgDist = 0d;
    }
    if (ParticleFilter.getDebugEnabled()) result.addResult("expAvgDist: " + expAvgDist, 0d);

    final double x = dabDelta - expAvgDist;

    /*
     * Now, we need to measure the state transition likelihood
     * wrt. heading.
     * For the in-progress -> in-progress transition, we need
     * to calculate the expected turn-rate/orientation between the two states.
     */
    double obsOrientation = Math.toRadians(obs.getOrientation());
    if (Double.isNaN(obsOrientation)) {
      obsOrientation = Math.toRadians(blockState.getBlockLocation().getOrientation());
    }
    if (ParticleFilter.getDebugEnabled()) result.addResult("obsOrient:" + obsOrientation, 0d);

    final double orientDiff;
    final double logpOrient;
    if (hasMoved) {
      if (!isDuring || blockState.getBlockLocation().getNextStop() == null) {
        /*
         * TODO
         * We could use a polar velocity model and the turn rate...
         */
        final double prevOrientation =
            Math.toRadians(parentBlockState.getBlockLocation().getOrientation());
        final double currentOrientation =
            Math.toRadians(blockState.getBlockLocation().getOrientation());
        final double angleDiff = Angle.diff(prevOrientation, currentOrientation);
        final double avgExpOrientation =
            Angle.normalizePositive(
                prevOrientation
                    + (Angle.getTurn(prevOrientation, currentOrientation) == Angle.CLOCKWISE
                            ? -1d
                            : 1d)
                        * angleDiff
                        / 2d);
        orientDiff = Angle.diff(obsOrientation, avgExpOrientation);
        if (ParticleFilter.getDebugEnabled())
          result.addResult("expOrient: " + avgExpOrientation, 0d);
        logpOrient = _inProgressProb * logVonMisesPdf(orientDiff, _inProgressConcParam);
      } else {
        /*
         * The orientation is wrt. the next stop destination, i.e.
         * we want to check that we're heading toward it.
         */
        double pathOrDestOrientation =
            Math.toRadians(
                SphericalGeometryLibrary.getOrientation(
                    obs.getLocation().getLat(),
                    obs.getLocation().getLon(),
                    blockState
                        .getBlockLocation()
                        .getNextStop()
                        .getStopTime()
                        .getStop()
                        .getStopLocation()
                        .getLat(),
                    blockState
                        .getBlockLocation()
                        .getNextStop()
                        .getStopTime()
                        .getStop()
                        .getStopLocation()
                        .getLon()));
        if (Double.isNaN(pathOrDestOrientation)) {
          pathOrDestOrientation = obsOrientation;
        }
        orientDiff = Angle.diff(obsOrientation, pathOrDestOrientation);
        if (ParticleFilter.getDebugEnabled())
          result.addResult("expOrient: " + pathOrDestOrientation, 0d);
        logpOrient = _inProgressProb * logVonMisesPdf(orientDiff, _deadheadEntranceConcParam);
      }
    } else {
      //      /*
      //       * What if we're switching trips while in the middle of doing one?
      //       * We assume that it's unlikely to do a complete 180 when we're
      //       * stopped.
      //       */
      //      if (parentState.getJourneyState().getPhase() == EVehiclePhase.IN_PROGRESS
      //          && state.getJourneyState().getPhase() == EVehiclePhase.IN_PROGRESS) {
      //        orientDiff = Math.toRadians(blockState.getBlockLocation().getOrientation()
      //            - parentBlockState.getBlockLocation().getOrientation());
      //      } else {
      orientDiff =
          Angle.diff(
              obsOrientation, Math.toRadians(blockState.getBlockLocation().getOrientation()));
      //      }
      if (ParticleFilter.getDebugEnabled())
        result.addResult("orientDiff (stopped):" + orientDiff, 0d);
      logpOrient = _inProgressProb * logVonMisesPdf(orientDiff, _stoppedConcParam);
    }

    final double logpMove =
        UnivariateGaussian.PDF.logEvaluate(x, 0d, Math.pow(obsTimeDelta, 4) / 4d) + logpOrient;

    result.addLogResultAsAnd("lik", logpMove);

    return result;
  }
  private static final SensorModelResult computeNoEdgeMovementLogProb(
      VehicleState state, VehicleState prevState, Observation obs)
      throws BadProbabilityParticleFilterException {

    final SensorModelResult result = new SensorModelResult("no-edge");
    final Observation prevObs = obs.getPreviousObservation();
    final double obsDistDelta = obs.getDistanceMoved();

    if (ParticleFilter.getDebugEnabled()) result.addResult("dist: " + obsDistDelta, 0d);

    final double expAvgDist;
    final double logpDistAlong;
    if (prevObs != null) {

      /*
       * Trying to get away with not using a real tracking filter.
       * FIXME really lame. use a Kalman filter.
       */
      final double obsTimeDelta = obs.getTimeDelta();
      if (prevObs.getTimeDelta() != null) {
        final double prevObsDistDelta = prevObs.getDistanceMoved();
        final double prevObsTimeDelta = prevObs.getTimeDelta();
        final double velocityEstimate = prevObsDistDelta / prevObsTimeDelta;
        expAvgDist =
            state.getMotionState().hasVehicleNotMoved() ? 0d : velocityEstimate * obsTimeDelta;
      } else {
        expAvgDist = state.getMotionState().hasVehicleNotMoved() ? 0d : _avgVelocity * obsTimeDelta;
      }

      if (ParticleFilter.getDebugEnabled()) result.addResult("expAvgDist: " + expAvgDist, 0d);

      /*
       * TODO: Is this needed?
       */
      final boolean prevInProgress;
      double prevOrientation;
      if (prevState != null
          && prevState.getBlockState() != null
          && prevState.getJourneyState().getPhase() == EVehiclePhase.IN_PROGRESS) {
        prevOrientation = prevState.getBlockState().getBlockLocation().getOrientation();
        prevInProgress = true;
        if (ParticleFilter.getDebugEnabled())
          result.addResult("prevOrient (state): " + prevOrientation, 0d);
      } else {
        prevOrientation = prevObs.getOrientation();
        prevInProgress = false;
        if (ParticleFilter.getDebugEnabled())
          result.addResult("prevOrient: " + prevOrientation, 0d);
      }

      final boolean isOff;
      final double currentOrientation;
      if (state.getBlockState() != null
          && state.getJourneyState().getPhase() == EVehiclePhase.IN_PROGRESS) {
        /*
         * We're entering an edge in progress, so we want to compare the
         * entrance orientation to the direction of the edge
         */
        isOff = false;
        currentOrientation = state.getBlockState().getBlockLocation().getOrientation();
        /*
         * We don't want to use the obs orientation when we're considered stopped
         * since gps error can reverse the orientation, making reverse
         * trip transitions look reasonable.
         * Similarly, if we were on a trip, use that orientation.
         * TODO FIXME: we should have the orientation in the motion state, then
         * we wouldn't need to evaluate this logic here.
         */
        if (!state.getMotionState().hasVehicleNotMoved()) {
          if (!prevInProgress) {
            prevOrientation = obs.getOrientation();
          }

          if (ParticleFilter.getDebugEnabled())
            result.addResult("new prevOrient (state): " + prevOrientation, 0d);
        }
      } else {
        isOff = true;
        currentOrientation = obs.getOrientation();
      }

      if (ParticleFilter.getDebugEnabled())
        result.addResult("curOrient: " + currentOrientation, 0d);

      final double orientDiff;
      if (Double.isNaN(prevOrientation) || Double.isNaN(currentOrientation)) {
        orientDiff = 0d;
      } else {
        orientDiff =
            Angle.diff(Math.toRadians(prevOrientation), Math.toRadians(currentOrientation));
      }

      logpDistAlong =
          UnivariateGaussian.PDF.logEvaluate(
                  obsDistDelta, expAvgDist, Math.pow(obsTimeDelta, 4) / 4d)
              + (isOff
                  ? _deadheadOffProb * logVonMisesPdf(orientDiff, _deadheadConcParam)
                  : _inProgressProb * logVonMisesPdf(orientDiff, _deadheadEntranceConcParam));

    } else {
      /*
       * No movement
       */
      logpDistAlong = 1d;
    }

    result.addLogResultAsAnd("lik", logpDistAlong);

    return result;
  }