/**
   * Similar to the annotator's `process` function but trimmed to just
   * handle a specific controller.
   *
   * @param  cas        The current common analysis system.
   * @param  controller The controller name which is also equivalent to the
   *                    mongo db collection's name.
   * @return UIMA error type id - UIMA_ERR_NONE on success.
   */
  uima::TyErrorId processController(
    uima::CAS& cas,
    const std::string& controller
  ) {
    log->logMessage("ControllerAnnotator::processController() begins");

    // Parse MongoDB data into an urdf model.
    MongoUrdf* urdf = new MongoUrdf(host);
    typedef std::vector< std::map<std::string, ModelState> > statesT;
    statesT states = urdf->getControllerStates(database, controller);
    delete urdf;

    // Initialize required indices.
    uima::FSIndexRepository& index = cas.getIndexRepository();
    uima::ANIterator jsIter = cas.getAnnotationIndex(JointStateType).iterator();
    uima::AnnotationFS ci, js;
    std::vector<std::string> names;
    int stamp, begin, end;

    for (statesT::iterator it = states.begin(); it != states.end(); it++) {
      ModelState desired = it->find("desired")->second;
      ModelState actual  = it->find("actual")->second;
      ModelState error   = it->find("error")->second;

      names = desired.getJointNames();
      stamp = desired.time;
      //begin = 0, end = 0;

      /*if (
        jsIter.isValid() && jsIter.peekPrevious().isValid() &&
        stamp < (js = jsIter.get()).getIntValue(jsTimeFtr)
      ) {
        jsIter.moveToNext();
        begin = js.getBeginPosition();
        end = js.getEndPosition();
      }*/

      ci = cas.createAnnotation(ControllerInput, stamp, stamp);
      ci.setStringValue(ciTypeFtr, utils::toUS(controller));
      ci.setIntValue(ciTimeFtr, stamp);
      ci.setFSValue(ciJnsFtr, utils::toStringArrayFS(cas, names));
      ci.setFSValue(ciDesFtr, toJtp(desired.jointStates));
      ci.setFSValue(ciActFtr, toJtp(actual.jointStates));
      ci.setFSValue(ciErrFtr, toJtp(error.jointStates));

      index.addFS(ci);
    }

    log->logMessage("ControllerAnnotator::processController() ends");
    return UIMA_ERR_NONE;
  }
  /**
   * Data processing.
   *
   * @param  cas                 The current common analysis system.
   * @param  resultSpecification The specification of expected results as given
   *                             by the annotator descriptor.
   * @return UIMA error type id - UIMA_ERR_NONE on success.
   */
  uima::TyErrorId process(
    uima::CAS& cas,
    const uima::ResultSpecification& resultSpecification
  ) {
    log->logMessage("MovementAnnotator::process() begins");

    // Save cas for use in other member functions.
    currentCas = &cas;

    // Intialize indices and the index iterator.
    uima::FSIndexRepository& index = cas.getIndexRepository();
    uima::ANIterator jsIter = cas.getAnnotationIndex(JointState).iterator();

    // Initialize reused variables.
    uima::AnnotationFS js = jsIter.get(), move;
    uima::FeatureStructure jtp;
    uima::StringArrayFS names;
    std::size_t size = js.getStringArrayFSValue(jsNameFtr).size();
    uima::ArrayFS moves = cas.createArrayFS(size);
    std::list<uima::DoubleArrayFS> prevPositions;
    std::vector<bool> movingStates(size, false);
    std::vector<double> variances;
    std::size_t id;

    // Init log stream.
    uima::LogStream& logstream = log->getLogStream(uima::LogStream::EnMessage);

    // Loop through joint states.
    while (jsIter.isValid()) {
      js = jsIter.get();
      jtp = js.getFSValue(jsJtpFtr);
      id = js.getBeginPosition();
      names = js.getStringArrayFSValue(jsNameFtr);
      prevPositions.push_back(jtp.getDoubleArrayFSValue(jtpPosFtr));

      // If the list is too long pop the first element.
      if (prevPositions.size() > observedJointStates) {
        prevPositions.pop_front();
      }

      // If the list is too short continue adding the next element.
      if (prevPositions.size() < observedJointStates) {
        continue;
      }

      // Calculate variances in every joint's positions to detect movements.
      variances = calculateVariances(prevPositions);

      // Iterate over the observed variances.
      for (std::size_t i = 0; i < variances.size(); i++) {
        move = static_cast<uima::AnnotationFS>(moves.get(i));
        icu::UnicodeString name = names.get(i).getBuffer();

        if (1 == move.getBeginPosition()) {  // Initial iteration.
          moves.set(i, moveAnnotation(name, id, id));
        } else {  // Consecutive iterations.
          bool moving = (variances[i] >= minVariance);

          if (                                 // Movement state:
            (!movingStates[i] &&  moving) ||   // wasn't + is
             (movingStates[i] && !moving)      // was    + isn't
          ) {
            index.addFS(move);
            moves.set(i, moveAnnotation(name, id, id));

          } else if (                         // Movement state:
            (!movingStates[i] && !moving) ||  // wasn't + isn't
             (movingStates[i] &&  moving)     // was    + is
          ) {
            moves.set(i, moveAnnotation(name, move.getBeginPosition(), id));
          }
        }
      }

      jsIter.moveToNext();
    }

    // Flush log.
    logstream.flush();

    // Add all remaining movements to the index - it might just be that the
    // joints didn't move at all and those are still their initial states.
    for (std::size_t i = 0; i < moves.size(); i++) {
      move = static_cast<uima::AnnotationFS>(moves.get(i));
      index.addFS(move);
    }

    log->logMessage("MovementAnnotator::process() ends");
    return UIMA_ERR_NONE;
  }
  /**
   * Data processing.
   *
   * @param  cas                 The current common analysis system.
   * @param  resultSpecification The specification of expected results as given
   *                             by the annotator descriptor.
   * @return UIMA error type id - UIMA_ERR_NONE on success.
   */
  uima::TyErrorId process(
    uima::CAS& cas,
    const uima::ResultSpecification& resultSpecification
  ) {
    log->logMessage("MovementDirectionAnnotator::process() begins");

    // Save cas for use in other member functions.
    currentCas = &cas;

    // Intialize the cas index and the movement index iterator.
    uima::FSIndexRepository& index = cas.getIndexRepository();
    uima::ANIndex jsIndex = cas.getAnnotationIndex(JointState);
    uima::ANIterator moveIter = cas.getAnnotationIndex(Movement).iterator();

    // Initialize reused variables.
    uima::AnnotationFS move, js;
    uima::UnicodeStringRef name;
    std::vector<uima::AnnotationFS> jointStates;
    double pos, posDiff, posPrev = 0, posStart = 0;
    std::size_t begin = 0, end;
    int direction;

    std::vector<int> movementStates;

    // Loop through movement annotations.
    while (moveIter.isValid()) {
      move = moveIter.get();
      jointStates = utils::selectCovered(jsIndex, move);
      direction = 0;
      end = move.getEndPosition();
      begin = move.getBeginPosition();

      // Loop through joint states related to this movement.
      for (std::size_t i = 0; i <= jointStates.size(); i++) {
        if (i < jointStates.size()) {
          js = jointStates[i];
          end = js.getEndPosition();
          name = move.getStringValue(mvNameFtr);
          pos = getPosByName(js, name);
          posStart = pos;
          posDiff = pos - posPrev;
        } else {
          // Force direction change to annotate the very last movement dir.
          posDiff = (posDiff >= 0) ? 1 : -1;
        }

        // Act acording to movement direction.
        switch (direction) {
          case POSITIVE:
            if (0 > posDiff) {  // Direction changed, now negative.
              index.addFS(posMoveAnnotation(name, begin, end, posStart, pos));
              direction = NEGATIVE;
              posStart = pos;
              begin = js.getBeginPosition();
            }
            break;
          case NEGATIVE:
            if (0 < posDiff) {  // Direction changed, now positive.
              index.addFS(negMoveAnnotation(name, begin, end, posStart, pos));
              direction = POSITIVE;
              posStart = pos;
              begin = js.getBeginPosition();
            }
            break;
          default:  // Only entered in initial iteration.
            direction = (0 <= posDiff) ? POSITIVE : NEGATIVE;
        }

        posPrev = pos;
      }

      moveIter.moveToNext();
    }

    log->logMessage("MovementDirectionAnnotator::process() ends");
    return UIMA_ERR_NONE;
  }
  /**
   * DistanceToLimit annotations.
   *
   * @return UIMA error id. UIMA_ERR_NONE on success.
   */
  uima::TyErrorId annotateLimits(
    uima::CAS& cas,
    const urdf::Model& model
  ) {
    uima::FSIndexRepository& index = cas.getIndexRepository();
    uima::ANIndex jsIndex = cas.getAnnotationIndex(JointState);
    uima::ANIterator jsIter = jsIndex.iterator();

    uima::AnnotationFS js, dst;
    uima::FeatureStructure jtp;

    uima::StringArrayFS jsNames;
    uima::DoubleArrayFS jtpPositions, jtpVelocities, jtpEfforts;

    boost::shared_ptr<const urdf::Joint> joint;
    boost::shared_ptr<urdf::JointLimits> limits;

    std::vector<std::string> dstNames;
    std::vector<double> upperLimits, lowerLimits, velocities, efforts;

    while (jsIter.isValid()) {
      js = jsIter.get();
      jtp = js.getFSValue(jsJtpFtr);

      // Get feature structure value arrays.
      jsNames       = js.getStringArrayFSValue(jsNameFtr);
      jtpPositions  = jtp.getDoubleArrayFSValue(jtpPosFtr);
      jtpVelocities = jtp.getDoubleArrayFSValue(jtpEffFtr);
      jtpEfforts    = jtp.getDoubleArrayFSValue(jtpVelFtr);

      // Clear storage vectors.
      dstNames.clear();
      upperLimits.clear();
      lowerLimits.clear();
      velocities.clear();
      efforts.clear();

      for (std::size_t i = 0; i < jsNames.size(); i++) {
        joint = model.getJoint(jsNames.get(i).asUTF8());
        limits = joint->limits;

        // Limits are only relevant for some joint types.
        if (
          joint->type == urdf::Joint::REVOLUTE ||
          joint->type == urdf::Joint::PRISMATIC
        ) {
          dstNames.push_back(joint->name);
          upperLimits.push_back(limits->upper - jtpPositions.get(i));
          lowerLimits.push_back(limits->lower - jtpPositions.get(i));
          velocities.push_back(limits->velocity - jtpVelocities.get(i));
          efforts.push_back(limits->effort - jtpEfforts.get(i));
        }
      }

      dst = cas.createAnnotation(DistanceToLimit,
                                 js.getBeginPosition(), js.getEndPosition());
      dst.setFSValue(dstNameFtr, utils::toStringArrayFS(cas, dstNames));
      dst.setFSValue(dstUppFtr, utils::toDoubleArrayFS(cas, upperLimits));
      dst.setFSValue(dstLowFtr, utils::toDoubleArrayFS(cas, lowerLimits));
      dst.setFSValue(dstVelFtr, utils::toDoubleArrayFS(cas, velocities));
      dst.setFSValue(dstEffFtr, utils::toDoubleArrayFS(cas, efforts));

      index.addFS(dst);
      jsIter.moveToNext();
    }

    return UIMA_ERR_NONE;
  }