RCP<const Thyra::VectorBase<Scalar> >
get_xdot( const InterpolationBufferBase<Scalar> &interpBuffer, const Scalar &t )
{
  using Teuchos::implicit_cast;
  Array<Scalar> time_vec;
  time_vec.push_back(t);
  Array<RCP<const Thyra::VectorBase<Scalar> > > xdot_vec;
  interpBuffer.getPoints(time_vec,0,&xdot_vec,0);
  TEUCHOS_ASSERT( 1 == implicit_cast<int>(xdot_vec.size()) );
  return xdot_vec[0];
}
void get_x_and_x_dot(
  const InterpolationBufferBase<Scalar> &interpBuffer,
  const Scalar t,
  const Ptr<RCP<const Thyra::VectorBase<Scalar> > > &x,
  const Ptr<RCP<const Thyra::VectorBase<Scalar> > > &x_dot
  )
{
  Array<Scalar> time_vec;
  time_vec.push_back(t);
  Array<RCP<const Thyra::VectorBase<Scalar> > > x_vec;
  Array<RCP<const Thyra::VectorBase<Scalar> > > x_dot_vec;
  interpBuffer.getPoints(
    time_vec,
    nonnull(x) ? &x_vec : 0,
    nonnull(x_dot) ? &x_dot_vec : 0,
    0
    );
  if (nonnull(x)) *x = x_vec[0];
  if (nonnull(x_dot)) *x_dot = x_dot_vec[0];
}
bool Rythmos::getCurrentPoints(
  const InterpolationBufferBase<Scalar> &interpBuffer,
  const Array<Scalar>& time_vec,
  Array<RCP<const Thyra::VectorBase<Scalar> > >* x_vec,
  Array<RCP<const Thyra::VectorBase<Scalar> > >* xdot_vec,
  int *nextTimePointIndex_inout
  )
{

  typedef ScalarTraits<Scalar> ST;
  using Teuchos::as;

  const int numTotalTimePoints = time_vec.size();

  // Validate input
#ifdef RYTHMOS_DEBUG
  TEST_FOR_EXCEPT(nextTimePointIndex_inout==0);
  TEUCHOS_ASSERT( 0 <= *nextTimePointIndex_inout && *nextTimePointIndex_inout < numTotalTimePoints );
  TEUCHOS_ASSERT( x_vec == 0 || as<int>(x_vec->size()) == numTotalTimePoints );
  TEUCHOS_ASSERT( xdot_vec == 0 || as<int>(xdot_vec->size()) == numTotalTimePoints );
#endif // RYTHMOS_DEBUG

  int &nextTimePointIndex = *nextTimePointIndex_inout;
  const int initNextTimePointIndex = nextTimePointIndex;

  const TimeRange<Scalar> currentTimeRange = interpBuffer.getTimeRange();
  
  if (currentTimeRange.length() >= ST::zero()) {

    // Load a temp array with all of the current time points that fall in the
    // current time range.
    Array<Scalar> current_time_vec;
    { // scope for i to remove shadow warning.
      int i;
      for ( i = 0; i < numTotalTimePoints-nextTimePointIndex; ++i ) {
        const Scalar t = time_vec[nextTimePointIndex];
#ifdef RYTHMOS_DEBUG
        TEUCHOS_ASSERT( t >= currentTimeRange.lower() );
#endif // RYTHMOS_DEBUG
        if ( currentTimeRange.isInRange(t) ) {
          ++nextTimePointIndex;
          current_time_vec.push_back(t);
        }
        else {
          break;
        }
      }
#ifdef RYTHMOS_DEBUG
      // Here I am just checking that the loop worked as expected with the data
      // in the current time range all comming first.
      TEUCHOS_ASSERT( nextTimePointIndex-initNextTimePointIndex == i );
#endif
    }

    // Get points in current time range if any such points exist

    const int numCurrentTimePoints = current_time_vec.size();

    if ( numCurrentTimePoints > 0 ) {

      // Get the state(s) for current time points from the stepper and put
      // them into temp arrays
      Array<RCP<const Thyra::VectorBase<Scalar> > > current_x_vec;
      Array<RCP<const Thyra::VectorBase<Scalar> > > current_xdot_vec;
      if (x_vec || xdot_vec) {
        interpBuffer.getPoints(
          current_time_vec,
          x_vec ? &current_x_vec : 0,
          xdot_vec ? &current_xdot_vec : 0,
          0 // accuracy_vec
          );
      }

      // Copy the gotten x and xdot vectors from the temp arrays to the output
      // arrays.
      for ( int i = initNextTimePointIndex; i < nextTimePointIndex; ++i ) {
        if (x_vec)
          (*x_vec)[i] = current_x_vec[i-initNextTimePointIndex];
        if (xdot_vec)
          (*xdot_vec)[i] = current_xdot_vec[i-initNextTimePointIndex];
      }

    }

  }

  return ( nextTimePointIndex == initNextTimePointIndex ? false : true );

}
void PointwiseInterpolationBufferAppender<Scalar>::append(
  const InterpolationBufferBase<Scalar>& interpBuffSource, 
  const TimeRange<Scalar>& appendRange,
  const Ptr<InterpolationBufferBase<Scalar> > &interpBuffSink 
  ) 
{
  TEUCHOS_ASSERT( !is_null(interpBuffSink) );
#ifdef RYTHMOS_DEBUG
  this->assertAppendPreconditions(interpBuffSource,appendRange,*interpBuffSink);
#endif // RYTHMOS_DEBUG

  RCP<Teuchos::FancyOStream> out = this->getOStream();
  Teuchos::OSTab ostab(out,1,"PointwiseInterpolationBufferAppender::append");
  if ( Teuchos::as<int>(this->getVerbLevel()) >= Teuchos::as<int>(Teuchos::VERB_HIGH) ) {
    *out << "Interpolation Buffer source range = [" << interpBuffSource.getTimeRange().lower() << "," <<
      interpBuffSource.getTimeRange().upper() << "]" << std::endl;
    *out << "Append range = [" << appendRange.lower() << "," << appendRange.upper() << "]" << std::endl;
    *out << "Interpolation Buffer sink range = [" << interpBuffSink->getTimeRange().lower() << "," <<
      interpBuffSink->getTimeRange().upper() << "]" << std::endl;
  }
  // Set up appendRange correctly to be either (] or [):
  RCP<const TimeRange<Scalar> > correctedAppendRange = Teuchos::rcp(&appendRange,false);
  if (compareTimeValues<Scalar>(interpBuffSink->getTimeRange().upper(),appendRange.lower()) == 0) {
    // adding to end of buffer 
    correctedAppendRange = Teuchos::rcp(new TimeRange_oc<Scalar>(appendRange));
    if ( Teuchos::as<int>(this->getVerbLevel()) >= Teuchos::as<int>(Teuchos::VERB_HIGH) ) {
      *out << "Corrected append range = (" << correctedAppendRange->lower() << "," << 
        correctedAppendRange->upper() << "]" << std::endl;
    }
  } 
  else if (compareTimeValues<Scalar>(interpBuffSink->getTimeRange().lower(),appendRange.upper()) == 0) {
    // adding to beginning of buffer
    correctedAppendRange = Teuchos::rcp(new TimeRange_co<Scalar>(appendRange));
    if ( Teuchos::as<int>(this->getVerbLevel()) >= Teuchos::as<int>(Teuchos::VERB_HIGH) ) {
      *out << "Corrected append range = [" << correctedAppendRange->lower() << "," << 
        correctedAppendRange->upper() << ")" << std::endl;
    }
  }

  Array<Scalar> time_vec_in;
  interpBuffSource.getNodes(&time_vec_in);

  Array<Scalar> time_vec;
  selectPointsInTimeRange(time_vec_in,*correctedAppendRange,Teuchos::outArg(time_vec));
  if ( Teuchos::as<int>(this->getVerbLevel()) >= Teuchos::as<int>(Teuchos::VERB_HIGH) ) {
    *out << "Selected points for appending to sink buffer: " << time_vec << std::endl;
  }

  Array<RCP<const Thyra::VectorBase<Scalar> > > x_vec;
  Array<RCP<const Thyra::VectorBase<Scalar> > > xdot_vec;
  Array<ScalarMag> accuracy_vec;
  interpBuffSource.getPoints(time_vec, &x_vec, &xdot_vec, &accuracy_vec);

  if ( Teuchos::as<int>(this->getVerbLevel()) >= Teuchos::as<int>(Teuchos::VERB_HIGH) ) {
    *out << "Sink buffer range before addPoints = [" << interpBuffSink->getTimeRange().lower() << "," <<
      interpBuffSink->getTimeRange().upper() << "]" << std::endl;
  }

  interpBuffSink->addPoints(time_vec, x_vec, xdot_vec);

  if ( Teuchos::as<int>(this->getVerbLevel()) >= Teuchos::as<int>(Teuchos::VERB_HIGH) ) {
    *out << "Sink buffer range after addPoints = [" << interpBuffSink->getTimeRange().lower() << "," <<
      interpBuffSink->getTimeRange().upper() << "]" << std::endl;
  }

}