DataSetSystem.cpp

#include "DataSetSystem.h"
 
#include "Context.h"
#include "Services/Variables.h"
#include "../BeamUtils.h"
#include "Context.h"
#include "Services/TaskManager.h"
#include "Platform/Instrumentation.h"
 
#include "Foundation/Logging/Logger.h"
#include "Foundation/Platform/Timer.h"
#include "Foundation/Platform/Threads.h"
 
#include <glm/gtx/component_wise.hpp>
 
#include <algorithm>
 
using namespace Cogs::Core::EchoSounder;
 
namespace
{
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("MultibeamDataSetSystem");
 
  bool equals(const std::vector<float>& A, const std::vector<float>& B, float epsilon)
  {
    if (A.size() != B.size()) return false;
    for (size_t i = 0; i < A.size(); i++) {
      if (epsilon < std::abs(A[i] - B[i])) return false;
    }
    return true;
  }
 
  struct ResampleBufferWorkerTask
  {
    std::shared_ptr<DataSetPersistent> persistent;
    uint32_t A, B;
 
    ResampleBufferWorkerTask(std::shared_ptr<DataSetPersistent> persistent, uint32_t A, uint32_t B)
      : persistent(persistent), A(A), B(B)
    {}
 
    void operator()()
    {
      CpuInstrumentationScope(SCOPE_SYSTEMS, "ResampleBufferWorker");
 
      const auto & configNew = persistent->configNew;
      const auto & configCur = persistent->config;
      auto & bufferNew = persistent->bufferNew;
      auto & bufferCur = persistent->buffer;
      const auto padding = configCur.useDecibel ? -std::numeric_limits<float>::infinity() : 0.0f;
      const auto beamCount = configNew.beamCount;
      const auto valuesStrideCur = configCur.sampleCount*beamCount;
      const auto valuesStrideNew = configNew.sampleCount*beamCount;
 
      for (uint32_t kNew = A; kNew < B; kNew++)
      {
        uint32_t kCur = (bufferCur.sliceBegin + kNew);
        if (configCur.capacity <= kCur) kCur -= configCur.capacity;
 
        size_t valuesOffCur = kCur*valuesStrideCur;
        size_t valuesOffNew = kNew*valuesStrideNew;
 
        for (uint32_t j = 0; j < beamCount; j++) {
          for (uint32_t iNew = 0; iNew < configNew.sampleCount; iNew++) {
            auto iCur = static_cast<uint32_t>((configNew.depthOffset - configCur.depthOffset + iNew*configNew.depthStep) / configCur.depthStep);
            float value = padding;
            if (iCur < configCur.sampleCount) {
              value = bufferCur.values[valuesOffCur + iCur];
            }
            bufferNew.values[valuesOffNew + iNew] = value;
          }
 
          valuesOffCur += configCur.sampleCount;
          valuesOffNew += configNew.sampleCount;
        }
      }
 
      uint32_t ACur = bufferCur.sliceBegin + A;
      if (configCur.capacity <= ACur) ACur -= configCur.capacity;
 
 
 
      std::memcpy(bufferNew.bottomDepths.data() + A*beamCount,
                  bufferCur.bottomDepths.data() + ACur*beamCount,
                  sizeof(float)*(B - A)*beamCount);
 
      std::memcpy(bufferNew.bottomReflectivities.data() + A*beamCount,
                  bufferCur.bottomReflectivities.data() + ACur*beamCount,
                  sizeof(float)*(B - A)*beamCount);
 
      std::memcpy(bufferNew.timestamps.data() + A,
                  bufferCur.timestamps.data() + ACur,
                  sizeof(int64_t)*(B - A));
 
      std::memcpy(bufferNew.metaPings.data() + A,
                  bufferCur.metaPings.data() + ACur,
                  sizeof(PingMetaData)*(B - A));
 
      std::memcpy(bufferNew.distances.data() + A,
                  bufferCur.distances.data() + ACur,
                  sizeof(float)*(B - A));
    }
  };
 
  struct ResampleBufferMainTask
  {
    Cogs::Core::Context* context;
    std::shared_ptr<DataSetPersistent> persistent;
 
    ResampleBufferMainTask(Cogs::Core::Context* context, std::shared_ptr<DataSetPersistent> persistent)
      : context(context), persistent(persistent) {}
 
    void operator()()
    {
      CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "ResampleBufferMain");
      auto timer = Cogs::Timer::startNew();
      const auto & configNew = persistent->configNew;
      const auto & configCur = persistent->config;
      auto & bufferNew = persistent->bufferNew;
      auto & bufferCur = persistent->buffer;
 
 
      const auto Q = configNew.capacity;
      const auto M = configNew.beamCount;
      const auto N = configNew.sampleCount;
      const auto keepSlices = std::min(configNew.capacity - 1, bufferCur.sliceCount);
      const auto keepBegin = (bufferCur.sliceBegin + bufferCur.sliceCount - keepSlices) % configCur.capacity;
      const auto concurrency = std::max(1u, (uint32_t)Cogs::Threads::hardwareConcurrency());
      const auto slicesPerTask = std::max(10u, keepSlices / concurrency);
 
      bufferNew.sliceBegin = 0;
      bufferNew.sliceCount = keepSlices;
      bufferNew.timestamps.resize(Q, false);
      bufferNew.values.resize(Q * M * N, false);
      bufferNew.bottomDepths.resize(Q * M, false);
      bufferNew.bottomReflectivities.resize(Q * M, false);
      bufferNew.metaPings.resize(Q, false);
      bufferNew.distances.resize(Q, false);
 
      auto tasks = 0u;
      if (0 < keepSlices) {
        auto group = context->taskManager->createGroup();
        if (keepBegin + keepSlices <= configCur.capacity)
        {
          for (uint32_t s = 0; s < keepSlices; s += slicesPerTask)
          {
            context->taskManager->enqueueChild(group, ResampleBufferWorkerTask(persistent, s, std::min(keepSlices, s + slicesPerTask)));
            tasks++;
          }
        }
        else {
          auto split = configCur.capacity - keepBegin;
          assert(split <= keepSlices);
          for (uint32_t s = 0; s < split; s += slicesPerTask)
          {
            context->taskManager->enqueueChild(group, ResampleBufferWorkerTask(persistent, s, std::min(split, s + slicesPerTask)));
            tasks++;
          }
          for (uint32_t s = split; s < keepSlices; s += slicesPerTask)
          {
            context->taskManager->enqueueChild(group, ResampleBufferWorkerTask(persistent, s, std::min(keepSlices, s + slicesPerTask)));
            tasks++;
          }
        }
        context->taskManager->wait(group);
        bufferNew.distances[keepSlices - 1] = 0;
      }
      LOG_DEBUG(logger, "ResampleBuffer keep=%d, stride=%d, tasks=%d, elapsed=%.2fms", keepSlices, slicesPerTask, tasks, 1000.0*timer.elapsedSeconds());
 
      persistent->dataReady = true;
      persistent->runningTasks--;
    }
 
  };
 
}
 
void Cogs::Core::EchoSounder::DataSetSystem::updateGridAngles(DataSetData& /*dataData*/)
{
  // Separate minor and major angles under the assumption that a "slice" of beams
  // lies has constant corresponding angle.
#if 0
  const auto& config = dataData.persistent->config;
  const auto minorCount = config.minorCount;
  const auto majorCount = config.majorCount;
 
  const auto & fullMinorAngles = config.directionY;
  const auto & fullMajorAngles = config.directionX;
 
  auto & separatedMinorAngles = dataData.directionY;
  auto & separatedMajorAngles = dataData.directionX;
 
  const auto iN = minorCount;
  const auto jN = majorCount;
 
  separatedMinorAngles.clear();
  separatedMinorAngles.resize(minorCount);
  for (uint32_t i = 0; i < minorCount; i++) {
    float sumAngle = 0.f;
    for (uint32_t j = 0; j < majorCount; j++) {
      sumAngle += fullMinorAngles[minorCount*j + i];
    }
    separatedMinorAngles[i] = sumAngle / majorCount;
  }
 
  separatedMajorAngles.clear();
  separatedMajorAngles.resize(majorCount);
  for (uint32_t j = 0; j < majorCount; j++) {
    float sumAngle = 0.f;
    for (uint32_t i = 0; i < minorCount; i++) {
      sumAngle += fullMajorAngles[minorCount*j + i];
    }
    separatedMajorAngles[j] = sumAngle / minorCount;
  }
  int a = 2;
#endif
}
 
 
bool Cogs::Core::EchoSounder::DataSetSystem::handleConfigTransition(Context* context, DataSetData& dataData)
{
  dataData.persistent->configValid = false;
  const auto & configNew = dataData.persistent->configNew;
 
  if (configNew.capacity < 1) return false;
  if (configNew.beamCount < 1) return false;
  if (configNew.sampleCount < 2) return false;
 
  const auto & configCurr = dataData.persistent->config;
  if ((configCurr.beamCount != configNew.beamCount) ||
      (configCurr.coordSys != configNew.coordSys) ||
      (std::numeric_limits<float>::epsilon() < glm::compMax(glm::abs(configCurr.transducerAlpha - configNew.transducerAlpha))) ||
      (std::numeric_limits<float>::epsilon() < glm::compMax(glm::abs(configCurr.transducerOffset - configNew.transducerOffset))) ||
      (configCurr.useDecibel != configNew.useDecibel) ||
      !equals(configCurr.directionX, configNew.directionX, std::numeric_limits<float>::epsilon()) ||
      !equals(configCurr.directionY, configNew.directionY, std::numeric_limits<float>::epsilon()) ||
      !equals(configCurr.beamWidthX, configNew.beamWidthX, std::numeric_limits<float>::epsilon()) ||
      !equals(configCurr.beamWidthY, configNew.beamWidthY, std::numeric_limits<float>::epsilon()))
  {
    resizeBuffers(dataData);
    return false;
  }
  if ((configCurr.capacity == configNew.capacity) &&
      (configCurr.sampleCount == configNew.sampleCount) &&
      (std::abs(configCurr.depthOffset - configNew.depthOffset) < std::numeric_limits<float>::epsilon()) &&
      (std::abs(configCurr.depthStep - configNew.depthStep) < std::numeric_limits<float>::epsilon()))
  {
    dataData.persistent->configValid = true;
    return false; // Basically identical configs, do nothing.
  }
  if (configNew.tryPreserve) {
    dataData.persistent->runningTasks++;
    dataData.persistent->configValid = true;
    context->taskManager->enqueue(context->taskManager->GlobalQueue, ResampleBufferMainTask(context, dataData.persistent));
    return true;
  }
  else {
    resizeBuffers(dataData);
    return false;
  }
}
 
 
 
void Cogs::Core::EchoSounder::DataSetSystem::resizeBuffers(DataSetData& dataData)
{
  const auto & configNew = dataData.persistent->configNew;
  dataData.persistent->config = configNew;
  dataData.persistent->configValid = true;
  dataData.configGen++;
 
  const auto Q = dataData.persistent->config.capacity;
  const auto M = dataData.persistent->config.beamCount;
  const auto N = dataData.persistent->config.sampleCount;
 
  dataData.persistent->buffer.sliceBegin = 0;
  dataData.persistent->buffer.sliceCount = 0;
  dataData.persistent->buffer.timestamps.resize(Q, false);
  dataData.persistent->buffer.values.resize(Q * M * N, false);
  dataData.persistent->buffer.bottomDepths.resize(Q * M, false);
  dataData.persistent->buffer.bottomReflectivities.resize(Q * M, false);
  dataData.persistent->buffer.metaPings.resize(Q, false);
  dataData.persistent->buffer.distances.resize(Q, false);
 
  updateGridAngles(dataData);
  dataData.slicePreservedBegin = 0;
  dataData.slicePreservedCount = 0;
  dataData.configGen++;
  dataData.dataGen++;
 
}
 
void Cogs::Core::EchoSounder::DataSetSystem::handleNewPing(DataSetData& dataData, Ping& ping)
{
  if (!dataData.persistent->configValid) return;
 
  const auto & config = dataData.persistent->config;
 
  if ((ping.beamCount != config.beamCount) ||
      (ping.sampleCount != config.sampleCount))
  {
    LOG_WARNING(logger, "handlePing: numBeams/numSamples mismatch, ignoring slice.");
    return;
  }
 
  auto & buf = dataData.persistent->buffer;
  if (0 < buf.sliceCount) {
    auto prevSlice = (buf.sliceBegin + buf.sliceCount - 1u) % config.capacity;
    if (ping.timestamp <= buf.timestamps[prevSlice]) {
      LOG_WARNING(logger, "handlePing: Detected non-monotonic increasing timestamps, ignoring slice.");
      return;
    }
  }
 
  if (buf.sliceCount == config.capacity) {    // Make room for a new slice
    if (dataData.slicePreservedBegin == buf.sliceBegin) {
      dataData.slicePreservedBegin = (dataData.slicePreservedBegin + 1u) % config.capacity;
      dataData.slicePreservedCount = std::max(1u, dataData.slicePreservedCount) - 1u;
    }
    buf.sliceBegin = (buf.sliceBegin + 1u) % config.capacity;
    buf.sliceCount = std::max(1u, buf.sliceCount) - 1u;
  }
 
  int slice = (buf.sliceBegin + buf.sliceCount) % config.capacity;
 
  // --- Copy timestamp, values, bottomDepth, bottomReflectivites.
  const auto M = config.beamCount;
  const auto N = config.sampleCount;
 
  buf.timestamps[slice] = ping.timestamp;
 
  std::memcpy(buf.values.data() + M*N*slice, ping.values(), ping.valuesSize());
 
  // Copy bottom depths and replace non-finites with zero (which is no-data marker for depths).
  {
    const auto * s = ping.bottomDepths();
    auto * d = buf.bottomDepths.data() + M * slice;
    for (size_t i = 0; i < M; i++) {
      d[i] = std::isfinite(s[i]) ? s[i] : 0.f;
    }
  }
 
  std::memcpy(buf.bottomReflectivities.data() + M*slice, ping.bottomReflectivities(), ping.bottomReflectivitiesSize());
 
 
 
  // --- Update metaPings
  auto & metaPing = buf.metaPings[slice];
  metaPing.vesselOrientationGlobal = ping.orientation;
  metaPing.vesselPositionGlobal = ping.position;
  getArrayToVesselTransform(buf.metaPings[slice].arrayOrientationVessel,
                            buf.metaPings[slice].arrayPositionVessel,
                            config.transducerAlpha,
                            config.transducerOffset);
  buf.metaPings[slice].arrayOrientationGlobal = metaPing.vesselOrientationGlobal * buf.metaPings[slice].arrayOrientationVessel;
  buf.metaPings[slice].arrayPositionGlobal = metaPing.vesselPositionGlobal + metaPing.vesselOrientationGlobal * buf.metaPings[slice].arrayPositionVessel;
 
  // --- Update distances
  if (0 < buf.sliceCount) {
    auto minSliceDistance = std::max(0.001f, context->variables->get("echo.minSliceDistance")->getFloat());
 
    auto prevSlice = (buf.sliceBegin + buf.sliceCount - 1u) % config.capacity;
    buf.distances[prevSlice] = std::max(minSliceDistance,
                                        glm::distance(buf.metaPings[prevSlice].vesselPositionGlobal,
                                                      buf.metaPings[slice].vesselPositionGlobal));
  }
  buf.distances[slice] = 0;
  buf.sliceCount++;
  dataData.dataGen++;
}
 
void Cogs::Core::EchoSounder::DataSetSystem::update(Context * context)
{
  for (auto & dataSetComp : pool) {
    auto & dataData = getData(&dataSetComp);
 
    if (dataData.persistent->runningTasks != 0) {
      continue; // Re-sample job running, hands off.
    }
 
    if (dataData.persistent->dataReady) {
      dataData.persistent->dataReady = false;
 
      dataData.persistent->config = dataData.persistent->configNew;
      dataData.persistent->buffer.sliceBegin = dataData.persistent->bufferNew.sliceBegin;
      dataData.persistent->buffer.sliceCount = dataData.persistent->bufferNew.sliceCount;
      dataData.persistent->buffer.timestamps.swap(dataData.persistent->bufferNew.timestamps);
      dataData.persistent->buffer.values.swap(dataData.persistent->bufferNew.values);
      dataData.persistent->buffer.bottomDepths.swap(dataData.persistent->bufferNew.bottomDepths);
      dataData.persistent->buffer.bottomReflectivities.swap(dataData.persistent->bufferNew.bottomReflectivities);
      dataData.persistent->buffer.metaPings.swap(dataData.persistent->bufferNew.metaPings);
      dataData.persistent->buffer.distances.swap(dataData.persistent->bufferNew.distances);
 
      dataData.persistent->bufferNew.timestamps.clear();
      dataData.persistent->bufferNew.values.clear();
      dataData.persistent->bufferNew.bottomDepths.clear();
      dataData.persistent->bufferNew.bottomReflectivities.clear();
      dataData.persistent->bufferNew.metaPings.clear();
      dataData.persistent->bufferNew.distances.clear();
 
      updateGridAngles(dataData);
      dataData.slicePreservedBegin = 0;
      dataData.slicePreservedCount = 0;
      dataData.configGen++;
      dataData.dataGen++;
    }
 
    while (!dataSetComp.messages.empty()) {
      dataSetComp.setChangedTransient();
 
      auto * baseMessage = dataSetComp.messages.front();
 
      auto * clearMessage = dynamic_cast<ClearMessage*>(baseMessage);
      if (clearMessage) {
        dataData.persistent->buffer.sliceBegin = 0;
        dataData.persistent->buffer.sliceCount = 0;
        dataData.slicePreservedBegin = 0;
        dataData.slicePreservedCount = 0;
        dataData.dataGen++;
        dataData.clearGen++;
      }
 
      auto * configMessage = dynamic_cast<ConfigMessage*>(baseMessage);
      if (configMessage) {
        dataData.persistent->configNew = configMessage->config;
        dataData.persistent->configReady = true;
      }
 
      auto * pingMessage = dynamic_cast<PingMessage*>(baseMessage);
      if (pingMessage) {
        if (dataData.persistent->configReady) {
          dataData.persistent->configReady = false;
 
          if (handleConfigTransition(context, dataData)) {
            break; // Resample job has started, hands off.
          }
        }
 
        handleNewPing(dataData, pingMessage->ping);
      }
 
      dataSetComp.messages.pop_front();
      delete baseMessage;
    }
 
  }
}