SwathPathChunks.cpp

#include <string>
#include <iostream>
#include <algorithm>
#include "SwathPathChunks.h"
#include "SwathPathResamplingPositions.h"
 
//#define VERBOSE_WRAP(a) do {(a);} while(0) 
#define VERBOSE_WRAP(a) do ; while(0) 
 
namespace {
 
  const std::string package = "SwathPathChunks: ";
 
}
 
Cogs::Core::EchoSounder::SwathPathChunks::SwathPathChunks(uint32_t preferredExtent, uint32_t skirtSize)
  : preferredExtent(preferredExtent),
    skirtSize(skirtSize)
{
}
 
void Cogs::Core::EchoSounder::SwathPathChunks::clear()
{
  chunks.clear();
 
  VERBOSE_WRAP(std::cerr << package << "Clear\n");
}
 
bool Cogs::Core::EchoSounder::SwathPathChunks::retireEvictedPositions(const SwathPathResamplingPositions& positions)
{
  bool rv = false;
  const auto & entries = positions.getEntries();
 
  while (!chunks.empty() && !entries.validIndex(chunks.front().support.a)) {
    VERBOSE_WRAP(std::cerr << package << "Popped chunk " << chunks.frontIndex() << " [" << chunks.front().support.a << ", " << chunks.front().support.b << "]\n");
    chunks.popFront();
    rv = true;
  }
  return rv;
}
 
 
bool Cogs::Core::EchoSounder::SwathPathChunks::seedIfQueueEmpty(const SwathPathResamplingPositions& positions)
{
  if (!chunks.empty()) return false;
 
  const auto leastRecentIndex = positions.getEntries().frontIndex();
  const auto mostRecentIndex = positions.getEntries().backIndex();
 
  uint32_t m = mostRecentIndex - leastRecentIndex;
 
  SwathPathChunk chunk;
  chunk.extent.b = mostRecentIndex;
  chunk.extent.a = mostRecentIndex - std::min(preferredExtent, m);
  chunk.support.b = mostRecentIndex;
  chunk.support.a = mostRecentIndex - std::min(preferredExtent + skirtSize, m);
  assert(chunk.extent.a - chunk.support.a <= chunk.support.b - chunk.support.a);
  assert(chunk.extent.b - chunk.support.a <= chunk.support.b - chunk.support.a);
 
  chunks.pushBack(chunk);
  VERBOSE_WRAP(std::cerr << package << "seed: " <<
    chunks.back().support.a << "/" << chunks.back().extent.a << "..." <<
    chunks.back().extent.b << "/" << chunks.back().support.b << " [" << leastRecentIndex << ":" << mostRecentIndex << "\n");
 
  generation++;
 
  return true;
}
 
bool Cogs::Core::EchoSounder::SwathPathChunks::tryGrowMoreRecent(const SwathPathResamplingPositions& positions)
{
  if (chunks.empty()) return false;
 
  const auto & entries = positions.getEntries();
  const auto leastRecentIndex = entries.frontIndex();
  const auto mostRecentIndex = entries.backIndex();
 
  if (chunks.back().extent.b == mostRecentIndex) return false;  // Fully covered.
 
  bool rv = false;
 
  // Try make most recent chunk fully sized.
  {
    auto & c = chunks.back();
 
    auto extent_b = c.extent.a + std::min(preferredExtent, mostRecentIndex - c.extent.a);
    auto support_b = c.extent.a + std::min(preferredExtent + skirtSize, mostRecentIndex - c.extent.a);
    rv = rv || (c.extent.b != extent_b) || (c.support.b != support_b);
    c.extent.b = extent_b;
    c.support.b = support_b;
 
    assert(c.extent.a - c.support.a <= c.support.b - c.support.a);
    assert(c.extent.b - c.support.a <= c.support.b - c.support.a);
  }
 
 
  // Try to let chunks have full support towards recent.
  for (uint32_t i = 0; i < chunks.count(); i++) {
    auto & c = chunks[chunks.frontIndex() + i];
    if (c.support.b - c.extent.b < skirtSize) {
      auto support_b = c.extent.b + std::min(skirtSize, mostRecentIndex - c.extent.b);
      rv = rv || (c.support.b != support_b);
      c.support.b = support_b;
 
      assert(c.extent.a - c.support.a <= c.support.b - c.support.a);
      assert(c.extent.b - c.support.a <= c.support.b - c.support.a);
    }
  }
 
  // And add new chunks
  while (static_cast<int32_t>(mostRecentIndex - chunks.back().extent.b) > 0) {
 
    SwathPathChunk chunk;
    chunk.extent.a = chunks.back().extent.b;
    chunk.support.a = chunk.extent.a - std::min(skirtSize, chunk.extent.a - leastRecentIndex);
 
    chunk.extent.b = chunk.extent.a + std::min(preferredExtent, mostRecentIndex - chunk.extent.a);
    chunk.support.b = chunk.extent.b + std::min(skirtSize, mostRecentIndex - chunk.extent.b);
 
    chunk.flags = SwathPathChunkFlag::None;
 
    assert(chunk.extent.a - chunk.support.a <= chunk.support.b - chunk.support.a);
    assert(chunk.extent.b - chunk.support.a <= chunk.support.b - chunk.support.a);
 
    // Distance to most recent index should shrink, otherwise something is really broken.
    assert(static_cast<int32_t>(mostRecentIndex - chunk.extent.b) < static_cast<int32_t>(mostRecentIndex - chunks.back().extent.b));
 
    chunks.pushBack(chunk);
    rv = true;
  }
 
  return rv;
}
 
bool Cogs::Core::EchoSounder::SwathPathChunks::tryGrowLessRecent(const SwathPathResamplingPositions& positions)
{
  if (chunks.empty()) return false;
 
  const auto & entries = positions.getEntries();
  const auto leastRecentIndex = entries.frontIndex();
  const auto mostRecentIndex = entries.backIndex();
 
  bool rv = false;
  while (1 < static_cast<int32_t>(chunks.front().extent.a - leastRecentIndex)) {
 
    SwathPathChunk chunk;
    chunk.extent.b = chunks.front().extent.a;
    chunk.support.b = chunk.extent.b + std::min(skirtSize, mostRecentIndex - chunk.extent.b);
 
    chunk.extent.a = chunk.extent.b - std::min(preferredExtent, chunk.extent.b - leastRecentIndex);
    chunk.support.a = chunk.extent.a - std::min(skirtSize, chunk.extent.a - leastRecentIndex);
 
    assert(chunk.extent.a - chunk.support.a <= chunk.support.b - chunk.support.a);
    assert(chunk.extent.b - chunk.support.a <= chunk.support.b - chunk.support.a);
 
    assert(static_cast<int32_t>(chunk.extent.a - leastRecentIndex) < static_cast<int32_t>(chunks.front().extent.a - leastRecentIndex));
 
    chunks.pushFront(chunk);
    rv = true;
  }
  return rv;
}
 
bool Cogs::Core::EchoSounder::SwathPathChunks::update(const SwathPathResamplingPositions& positions)
{
  if (positions.getEntries().count() < 2) {
    auto count = chunks.count();
    chunks.clear();
    return count != 0;
  }
 
  bool rv0 = false;
  if (resamplingPosGen != positions.getGeneration()) {
    resamplingPosGen = positions.getGeneration();
    chunks.clear();
    rv0 = true;
  }
 
  //const auto leastRecentIndex = positions.getEntries().frontIndex();
  //const auto mostRecentIndex = positions.getEntries().backIndex();
 
  bool rv1 = retireEvictedPositions(positions);
  bool rv2 = seedIfQueueEmpty(positions);
  bool rv3 = tryGrowMoreRecent(positions);
  bool rv4 = tryGrowLessRecent(positions);
 
  bool rv = rv0 || rv1 || rv2 || rv3 || rv4;
 
  if (rv && !chunks.empty()) {
    for (uint32_t i = 0; i < chunks.count(); i++) {
      chunks[chunks.frontIndex() + i].flags = SwathPathChunkFlag::None;
    }
    chunks[chunks.frontIndex()].flags |= SwathPathChunkFlag::Tail;
    chunks[chunks.backIndex()].flags |= SwathPathChunkFlag::Head;
  }
 
 
  if (rv) {
    VERBOSE_WRAP(std::cerr << "--- Chunks: " << rv0 << "-" << rv1 << "-" << rv2 << "-" << rv3 << "-" << rv4 << "\n");
    for (uint32_t i = 0; i < chunks.count(); i++) {
      const auto & c = chunks[chunks.frontIndex() + i];
      VERBOSE_WRAP(std::cerr << (chunks.frontIndex() + i) << ": "
                   << "extent={" << c.extent.a << ", " << c.extent.b << "}, "
                   << "suppport={" << c.support.a << ", " << c.support.b << "}, "
                   << "closed={" << ((c.flags & 1) != 0) << ", " << ((c.flags & 2) != 0) << "}\n");
 
 
      assert(c.extent.a - c.support.a <= c.support.b - c.support.a);
      assert(c.extent.b - c.support.a <= c.support.b - c.support.a);
 
      if (0 < i) {
        assert(chunks[chunks.frontIndex() + i - 1].extent.b == c.extent.a);
      }
      if (i + 1 < chunks.count()) {
        assert(c.extent.b == chunks[chunks.frontIndex() + i + 1].extent.a);
      }
 
      assert(positions.getEntries().validIndex(c.extent.a));
      assert(positions.getEntries().validIndex(c.extent.b));
      assert(positions.getEntries().validIndex(c.support.a));
      assert(positions.getEntries().validIndex(c.support.b));
    }
  }
 
  return rv;
}