SwathPathResamplingPositions.cpp

#include <algorithm>
#include "Components/WindowComponent.h"
#include "Systems/DataSetSystem.h"
#include "SwathPathResamplingPositions.h"
 
#include <iostream>
#include <sstream>
//#define VERBOSE_WRAP(a) do {(a);} while(0) 
#define VERBOSE_WRAP(a) do ; while(0) 
 
bool Cogs::Core::EchoSounder::SwathPathResamplingPositions::clear(float spacing, float maxPathUpsample)
{
  this->spacing = spacing;
  this->maxPathUpsample = std::max(1.f, maxPathUpsample);
  bufferFront = 0;
  bufferBack = 0;
 
  auto N = entries.count();
  entries.clear();
  if (N != 0) { VERBOSE_WRAP(std::cerr << "Removed all entities [clear]\n"); }
  return N != 0;
}
 
//Remove all endtries older than validTimeStart
bool Cogs::Core::EchoSounder::SwathPathResamplingPositions::retireEvictedSlices(int64_t validTimeStart)
{
  bool rv = false;
  while (!entries.empty() && (entries.front().bufferTime < validTimeStart)) {
    VERBOSE_WRAP(std::cerr << "Popped " << entries.frontIndex() << ":" << entries.front().bufferIndex << "[retireEvict]\n");
    entries.popFront();
    rv = true;
  }
  return rv;
}
 
bool Cogs::Core::EchoSounder::SwathPathResamplingPositions::seedIfQueueEmpty(const Config& config, const DataSetBuffers& buffer)
{
  if (!entries.empty()) return false;
 
  //we want to add the newest slice from the buffer.
  //if we are resampling, we can't add an entry with reference to last slice, since the position
  //of an entry is defined as a linear combination between the referenced slice and the next one in buffer
  bool isResampling = spacing != 0.f;
  float fractional = isResampling ? 1.f : 0.f;
  auto ix = (buffer.sliceBegin + buffer.sliceCount - (isResampling ? 2 : 1)) % config.capacity;
 
  entries.pushBack(Entry{ buffer.timestamps[ix], ix, fractional });
  VERBOSE_WRAP(std::cerr << "pushBack: " << entries.backIndex() << ": ix=" << ix << ", t=" << fractional << "[seed]\n");
 
  generation++;
  return true;
}
 
//remove entries such that we are not storing more than maxSlices
bool Cogs::Core::EchoSounder::SwathPathResamplingPositions::trimEntries()
{
  bool rv = false;
  while (maxSlices < entries.count()) {
    VERBOSE_WRAP(std::cerr << "Popped " << entries.frontIndex() << ":" << entries.front().bufferIndex << " [trimEntries]\n");
    entries.popFront();
    rv = true;
  }
  return rv;
}
 
//checks if we can include entries with newer timestamps than the newest entry in the entries-queue
bool Cogs::Core::EchoSounder::SwathPathResamplingPositions::includeMostRecentSlices(const Config& config, const DataSetBuffers& buffer)
{
  assert(!entries.empty());
 
  auto sliceHead1 = (buffer.sliceBegin + buffer.sliceCount - 1u) % config.capacity;
 
  bool rv = false;
  if (spacing == 0.0f) {
 
    while (entries.back().bufferIndex != sliceHead1)
    {
      auto ix = (entries.back().bufferIndex + 1) % config.capacity;
      entries.pushBack(Entry{ buffer.timestamps[ix], ix, 0.0f });
      VERBOSE_WRAP(std::cerr << "pushBack: " << entries.backIndex() << ": ix=" << ix << ", t=0.0 [includeMostRecentSlices]\n");
      rv = true;
    }
 
  }
  else {
 
    assert(entries.back().bufferIndex != sliceHead1); // If fails => logical error somewhere.
 
    while (true) {
      auto ep = entryPosition(config, buffer, entries.backIndex());
      auto currPos = ep.first;
      auto distanceToCover = std::max(ep.second/maxPathUpsample, spacing);
      for (uint32_t k = 0; k < maxSliceSkip; k++) {
        auto currIx = (entries.back().bufferIndex + k) % config.capacity;
        if (currIx == sliceHead1) {
          return rv; // Nothing to do.
        }
        auto nextIx = (entries.back().bufferIndex + k + 1) % config.capacity;
 
        auto distanceToNextSlice =   glm::distance(currPos, buffer.metaPings[nextIx].vesselPositionGlobal);
        if (distanceToCover < distanceToNextSlice) {
          //next wanted sample entry lies somewhere between current entry and next slice
          float fractional;
          if (entries.back().bufferIndex == currIx) { // We haven't swapped interval, so take previous fractional into account.
            auto distBetweenSlices = distanceToNextSlice / (1 - entries.back().fractional);
            fractional = entries.back().fractional + distanceToCover / distBetweenSlices;
          } 
          else {
            fractional = distanceToCover / distanceToNextSlice;
          }
          assert(fractional - 1e-5f < 1.f);
          entries.pushBack(Entry{ buffer.timestamps[currIx], currIx, fractional });
          VERBOSE_WRAP(std::cerr << "pushBack: " << entries.backIndex() << ": ix=" << currIx << ", t=" << fractional << "[includeMostRecentSlices]\n");
          rv = true;
          goto done;
        }
        distanceToCover -= distanceToNextSlice;
      } //end for-loop
      {
        // We have skipped so many slices that we force a sample point.
        auto forceIx = (entries.back().bufferIndex + maxSliceSkip - 1) % config.capacity;
        entries.pushBack(Entry{ buffer.timestamps[forceIx], forceIx, 0.0 });
        VERBOSE_WRAP(std::cerr << "pushBack: " << entries.backIndex() << ": ix=" << forceIx << ", t=0.f" << "[includeMostRecentSlices, forced]\n");
        rv = true;
      }
 
    done:
      ;
    }
 
  }
  return rv;
}
 
std::pair<glm::vec3, float> Cogs::Core::EchoSounder::SwathPathResamplingPositions::entryPosition(const Config& config, const DataSetBuffers& buffer, uint32_t entryIx)
{
  auto bufferIx = entries[entryIx].bufferIndex;
  const auto & pc = buffer.metaPings[bufferIx].vesselPositionGlobal;
  const auto & pn = buffer.metaPings[(bufferIx + 1) % config.capacity].vesselPositionGlobal;
  return std::make_pair(glm::mix(pc, pn, entries[entryIx].fractional), glm::distance(pc, pn));
}
 
 
//checks if we can add entries with older timestamps then the oldest entry in the entries-queue. Usually only happens after seeding an empty queue.
bool Cogs::Core::EchoSounder::SwathPathResamplingPositions::includeLessRecentSlices(const Config& config, const DataSetBuffers& buffer, int64_t validTimeStart)
{
  assert(!entries.empty());
 
  bool rv = false;
  if (spacing == 0.0f) {
 
    while ((entries.count() < maxSlices) && (entries.front().bufferIndex != buffer.sliceBegin))
    {
      auto ix = (entries.front().bufferIndex - 1u + config.capacity) % config.capacity;
      if (buffer.timestamps[ix] < validTimeStart) break;
      entries.pushFront(Entry{ buffer.timestamps[ix], ix, 0.0f });
      VERBOSE_WRAP(std::cerr << "pushFront: " << entries.frontIndex() << ": ix=" << ix << ", t=0.0 [includeLessRecentSlices]\n");
      rv = true;
    }
 
  }
  else {
 
    while (entries.count() < maxSlices) {
      auto ep = entryPosition(config, buffer, entries.frontIndex());
      auto currPos = ep.first;
      auto distanceToCover = std::max(ep.second / maxPathUpsample, spacing);
      auto currIx = entries.front().bufferIndex;
      for (uint32_t k = 0; k < maxSliceSkip; k++) {
        auto nextIx = (entries.front().bufferIndex - k + config.capacity) % config.capacity;
 
        if (((currIx == buffer.sliceBegin) && (currIx != nextIx)) || (buffer.timestamps[nextIx] < validTimeStart) ) {
          return rv; // Trying to iterate past the oldest slice boundary of the buffer, give up.
        }
 
        auto distanceToNextSlice = glm::distance(currPos, buffer.metaPings[nextIx].vesselPositionGlobal);
        if (distanceToCover < distanceToNextSlice) {  // Sample point is in current interval.
 
          float fractional;
          if (nextIx == entries.front().bufferIndex) {  // We haven't swapped interval, so take previous fractional into account.
            auto distBetweenSlices = distanceToNextSlice / (entries.front().fractional);
            fractional = entries.front().fractional - distanceToCover / distBetweenSlices;
          }
          else {
            fractional = 1.f - distanceToCover / distanceToNextSlice;
          }
          assert(fractional - 1e-5 <= 1.f);
          entries.pushFront(Entry{ buffer.timestamps[nextIx], nextIx,  glm::max(0.f, fractional) });
          VERBOSE_WRAP(std::cerr << "pushFront: " << entries.frontIndex() << ": ix=" << entries.front().bufferIndex << ", t=" << entries.front().fractional << "[includeLessRecentSlices]\n");
          rv = true;
          goto done; // Jump out of for-loop and go to next while-iteration.
        }
 
        // Skip to next interval
        currIx = nextIx;
        distanceToCover -= distanceToNextSlice;
        currPos = buffer.metaPings[currIx].vesselPositionGlobal;
      }
 
      {
        // Spacing is so large that we have violated max-slices-to-skip, force insert a sample point.
        auto forceIx = (entries.front().bufferIndex - maxSliceSkip + config.capacity + 1u) % config.capacity;
        entries.pushFront(Entry{ buffer.timestamps[forceIx], forceIx, 0.0 });
        VERBOSE_WRAP(std::cerr << "pushFront: " << entries.backIndex() << ": ix=" << forceIx << ", t=0.f" << "[includeLessRecentSlices, forced]\n");
        rv = true;
#ifdef _DEBUG
        // If forceIx is past the oldest slice boundary, the test in the loop should have kicked in.
        for (uint32_t k = entries.front().bufferIndex; k != forceIx; k = (k - 1u) % config.capacity) {
          assert(k != buffer.sliceBegin);
        }
#endif
      }
    done:
      ;
    }
  }
  VERBOSE_WRAP(std::cerr << "Done includeLessRecentSlices\n");
  return rv;
}
 
void Cogs::Core::EchoSounder::SwathPathResamplingPositions::printBuffer(const Config& config, const DataSetBuffers& buffer) 
{
  std::stringstream bufferIndexes;
  std::stringstream bufferTimes;
  bufferIndexes << buffer.sliceBegin;
  bufferTimes << buffer.timestamps[buffer.sliceBegin];
  //verify that slices in the buffer are in correct order
  auto sliceTimeFirst = buffer.timestamps[buffer.sliceBegin];
  for (uint32_t i = 1; i < buffer.sliceCount; ++i) {
    auto index = (buffer.sliceBegin + i) % config.capacity;
    auto sliceTimeSecond = buffer.timestamps[index];
    assert(sliceTimeSecond >= sliceTimeFirst);
    bufferIndexes << " " << index;
    bufferTimes << " " << buffer.timestamps[index];
    sliceTimeFirst = sliceTimeSecond;
  }
 
  VERBOSE_WRAP(std::cerr << "Buffer indexes " << bufferIndexes.str() << std::endl);
  VERBOSE_WRAP(std::cerr << "Buffer time " << bufferTimes.str() << std::endl);
}
 
 
bool Cogs::Core::EchoSounder::SwathPathResamplingPositions::update(const WindowComponent* winComp,
                                                                   const DataSetComponent* /*dataComp*/,
                                                                   const DataSetData* dataData)
{
  const auto & config = dataData->persistent->config;
  const auto & buffer = dataData->persistent->buffer;
 
  /* General idea for resampling :
    Original slices are stored in buffer. This is a ring buffer, so we always need to modulo the index into this
    buffer with config.capacity.
    The resampled path is defined by the entries queue. On this queue we can push/pop to both the front and the end. Newest slices are at the end.
    To avoid duplicating data, an entry is defined by an index into the slice buffer together with
    a fraction to determine how far this entry is between the slice referenced by the index and the next slice in the buffer.
    See entryPosition() for how the position of an entry is defined.
    If we are not resampling (spacing == 0), then we chose one entry with fraction 0 for each slice in the buffer.
  */
  
  uint32_t bufferFrontNew = buffer.sliceBegin;
  uint32_t bufferBackNew = buffer.sliceBegin + buffer.sliceCount;
  uint32_t dataSetClearGenNew = dataData->clearGen;
 
  // Check if buffer has changed since last time we inspected it.
  if ((bufferFrontNew == bufferFront) && (bufferBackNew == bufferBack) && (dataSetClearGenNew == dataSetClearGen)) return false;
  bufferFront = bufferFrontNew;
  bufferBack = bufferBackNew;
 
  bool clearedNonEmpty = false;
  if (dataSetClearGen != dataSetClearGenNew) {
    auto count = entries.count();
    entries.clear();
    dataSetClearGen = dataSetClearGenNew;
    VERBOSE_WRAP(std::cerr << "Cleared " << count << " entites due to data set cleared " << dataSetClearGenNew << std::endl);
    clearedNonEmpty = count != 0;
  }
 
  if (buffer.sliceCount < 2) {
    auto count = entries.count(); 
    entries.clear();
    VERBOSE_WRAP(std::cerr << "Cleared " << count << " entites due to sliceCount " << buffer.sliceCount << std::endl);
    return count != 0 || clearedNonEmpty;
  }
 
#ifdef _DEBUG //Print current state of buffer and check that they are in correct order
  VERBOSE_WRAP(std::cerr << "start resample update " << std::endl);
  if (buffer.sliceCount < 50) {
    printBuffer(config, buffer);
  }
  VERBOSE_WRAP(std::cerr << "Num entries " << entries.count() << std::endl);
#endif
 
  if (entries.count() > 0) {
    assert(buffer.timestamps[(buffer.sliceBegin + buffer.sliceCount - 1) % config.capacity] > entries.back().bufferTime && "got older slices than current resample path");
  }
 
  // Note: If we start to discard based on winComp->head, we can end up in the situation that
  // indices change content, and this must be percolated up via generation or something clever.
  bool rv0 = retireEvictedSlices(std::max(buffer.timestamps[buffer.sliceBegin], winComp->tail));
  bool rv1 = seedIfQueueEmpty(config, buffer);
  bool rv2 = includeMostRecentSlices(config, buffer);
  bool rv3 = false;
  bool rv4 = false;
  if (maxSlices < entries.count()) {
    rv3 = trimEntries();
  }
  else if (entries.count() < maxSlices) {
    rv4 = includeLessRecentSlices(config, buffer, std::max(buffer.timestamps[buffer.sliceBegin], winComp->tail));
  }
 
  bool rv = rv0 || rv1 || rv2 || rv3 || rv4;
#ifdef _DEBUG
  // Sanity checks
 
  auto tsBufferBegin = buffer.timestamps[buffer.sliceBegin] - 1;
  auto tsBufferEnd = buffer.timestamps[(buffer.sliceBegin + buffer.sliceCount - 1) % config.capacity] + 1;
 
  size_t n = 0;
 
  VERBOSE_WRAP(std::cerr << "--- SamplingPositions"
    << " r0=" << rv0
    << " r1=" << rv1
    << " r2=" << rv2
    << " r3=" << rv3
    << " r4=" << rv4 << "\n");
 
  std::string entriesStr = "";
  for (auto i = entries.frontIndex(); i != entries.backIndex() + 1; i++) {
    entriesStr += "{ix = " + std::to_string(entries[i].bufferIndex) + ", frac = " + std::to_string(entries[i].fractional) + "}";
  }
  VERBOSE_WRAP(std::cerr << "Entries = " << entriesStr << std::endl);
  auto tsPreviousEntry = tsBufferBegin;
 
  for (auto i = entries.frontIndex(); i != entries.backIndex() + 1; i++) {
    VERBOSE_WRAP(std::cerr << i << ": {ix=" << entries[i].bufferIndex << ", frac=" << entries[i].fractional
              <<", ts=" << entries[i].bufferTime << "}\n");
    auto tsCurrentIndex = buffer.timestamps[entries[i].bufferIndex];
   
    //tsBufferBgin < tsCurrentEntry < tsBufferEnd
    assert(tsBufferBegin < tsCurrentIndex);
    assert(tsCurrentIndex < tsBufferEnd);
 
    if (spacing == 0.0f) {
      assert(entries[i].fractional == 0.0f);
 
    }
    else {
      auto  tsNextIndex = buffer.timestamps[(entries[i].bufferIndex + 1) % config.capacity];
      //tsBufferStart < tsNextIndex < tsBufferEnd
      assert(tsBufferBegin < tsNextIndex);
      assert(tsNextIndex < tsBufferEnd);
 
      //time of slices should be strict increasing
      assert(tsCurrentIndex < tsNextIndex);
 
      auto tsCurrentEntry = tsCurrentIndex + (int64_t)(entries[i].fractional*(tsNextIndex - tsCurrentIndex));
 
      assert(tsPreviousEntry <= tsCurrentEntry); //don't have '<' since we are casting to int.
 
      tsPreviousEntry = tsCurrentEntry;
    }
    n++;
  }
  assert(n == entries.count());
  VERBOSE_WRAP(std::cerr << "Done update" << std::endl);
#endif
  return rv;
}