OctProviderSystem.cpp

#include <thread>
#include "Context.h"
#include "ExtensionRegistry.h"
#include "Services/TaskManager.h"
#include "Components/Geometry/MarkerPointSetComponent.h"
#include "Components/Core/SceneComponent.h"
 
#include "../Components/DataRefComponent.h"
#include "../Components/BeamGroupComponent.h"
#include "../Tasks/dBToLinearTask.h"
#include "../Tasks/OctProviderBuildTileTask.h"
 
#include "OctProviderSystem.h"
#include "DataSetSystem.h"
#include "../BeamUtils.h"
#include "../../../Volumetric/Source/Components/OctComponent.h"
 
#include "Foundation/Logging/Logger.h"
 
namespace {
  using namespace Cogs::Core;
 
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("EchoOctProvider");
 
  struct OctDummyTask
  {
    Context* context;
    std::shared_ptr<EchoSounder::OctProviderPersistent> persistent;
 
    void operator()()
    {
      if (!persistent->needsLinearization.empty()) {
        auto linearizeGroup = context->taskManager->createGroup(Cogs::Core::TaskManager::ResourceQueue);
 
        for (auto * item : persistent->needsLinearization) {
          assert(item->linear.size() == item->decibel.size());
          EchoSounder::dBToLinearTask task;
          task.overflowThreshold = std::numeric_limits<float>::quiet_NaN();
          task.in = item->decibel.data();
          task.out = item->linear.data();
          task.N = item->linear.size();
          context->taskManager->enqueueChild(linearizeGroup, task);
          item->linearized = true;
        }
        context->taskManager->wait(linearizeGroup);
      }
 
      // Sanity checks.
      for (auto & it : persistent->pingCache) {
        assert(it.second->linearized);
      }
      persistent->needsLinearization.clear();
 
      auto group = context->taskManager->createGroup(Cogs::Core::TaskManager::ResourceQueue);
 
      for (size_t i = 0; i < persistent->responses.size(); i++) {
 
        if (persistent->responses[i]->regionKeys.empty()) continue;
 
        context->taskManager->enqueueChild(group, EchoSounder::OctProviderBuildTileTask{ persistent, i });
      }
      context->taskManager->wait(group);
      persistent->tasksRunning = false;
    }
 
  };
 
 
  void populatePingCache(EchoSounder::DataSetSystem* dataSystem, EchoSounder::OctProviderData& providerData)
  {
    auto * persistent = providerData.persistent.get();
    persistent->needsLinearization.clear();
 
    // If discarded, discard cached slices.
    if (persistent->tainted) {
      for (auto & it : persistent->pingCache) {
        persistent->pingCacheUnused.push_back(std::move(it.second));
      }
      persistent->pingCache.clear();
    }
 
    if(persistent->responses.empty()) return;
 
    // Update group cache which stores relevant group info while task is running.
 
    persistent->groupCache.resize(providerData.sources.size());
    for (size_t s = 0; s < providerData.sources.size(); s++) {
      auto source = providerData.sources[s];
      const auto * dataEntity = source.entity->getComponent<EchoSounder::DataRefComponent>()->data.get();
      const auto * grpComp = source.entity->getComponent<EchoSounder::BeamGroupComponent>();
      assert(dataEntity != nullptr);
 
      persistent->groupCache[s].dataId = uint32_t(dataEntity->getId());
      persistent->groupCache[s].majorCount = grpComp->majorCount;
      persistent->groupCache[s].minorCount = grpComp->minorCount;
      persistent->groupCache[s].directionX = grpComp->directionX;
      persistent->groupCache[s].directionY = grpComp->directionY;
      persistent->groupCache[s].minDirectionX = grpComp->minDirectionX;
      persistent->groupCache[s].maxDirectionX = grpComp->maxDirectionX;
      persistent->groupCache[s].minDirectionY = grpComp->minDirectionY;
      persistent->groupCache[s].maxDirectionY = grpComp->maxDirectionY;
      persistent->groupCache[s].maxBeamWidthX = grpComp->maxBeamWidthX;
      persistent->groupCache[s].maxBeamWidthY = grpComp->maxBeamWidthY;
    }
 
 
    // Let slices age.
    for (auto & it : persistent->pingCache) {
      it.second->age++;
    }
 
    // Pull in data not present in the cache, reset age of stuff already present.
    unsigned newSlices = 0;
    unsigned recycledSlices = 0;
 
 
    for (auto * resp : persistent->responses) {
      for (auto & regionKey : resp->regionKeys) {
        uint32_t sourceIndex, pingIndex;
        EchoSounder::decodeKey2(sourceIndex, pingIndex, regionKey);
        assert(sourceIndex < providerData.sources.size());
 
        auto source = providerData.sources[sourceIndex];
        auto * dataEntity = source.entity->getComponent<EchoSounder::DataRefComponent>()->data.get();
 
        uint64_t cacheKey = EchoSounder::encodeKey2(persistent->groupCache[sourceIndex].dataId, pingIndex);
        auto it = persistent->pingCache.find(cacheKey);
        if (it != persistent->pingCache.end()) {
          it->second->age = 0;
        }
        else {
          std::unique_ptr<EchoSounder::CachedPing> entry;
          if (!persistent->pingCacheUnused.empty()) {
            entry = std::move(persistent->pingCacheUnused.back());
            persistent->pingCacheUnused.pop_back();
            recycledSlices++;
          }
          else {
            entry = std::make_unique<EchoSounder::CachedPing>();
          }
 
          const auto & config = dataSystem->getData(dataEntity->getComponent<EchoSounder::DataSetComponent>()).persistent->config;
          const auto & buffer = dataSystem->getData(dataEntity->getComponent<EchoSounder::DataSetComponent>()).persistent->buffer;
          const auto & metaPing = buffer.metaPings[pingIndex];
 
          entry->timestamp = buffer.timestamps[pingIndex];
          entry->metaPing = metaPing;
          entry->beamCount = config.beamCount;
          entry->sampleCount = config.sampleCount;
          entry->depthOffset = config.depthOffset;
          entry->depthStep = config.depthStep;
          entry->transducerAlpha = config.transducerAlpha;
          entry->transducerOffset = config.transducerOffset;
 
          entry->age = 0;
          entry->linearized = false;
          entry->decibel.resize(config.beamCount*config.sampleCount, false);
          std::memcpy(entry->decibel.data(), buffer.values.data() + pingIndex * config.beamCount * config.sampleCount, sizeof(float)*config.beamCount*config.sampleCount);
          entry->bottomDepths.resize(config.beamCount, false);
          std::memcpy(entry->bottomDepths.data(), buffer.bottomDepths.data() + pingIndex * config.beamCount, sizeof(float)*config.beamCount);
          entry->linear.resize(config.beamCount*config.sampleCount, false);
          persistent->needsLinearization.push_back(entry.get());
 
          persistent->pingCache[cacheKey] = std::move(entry);
          newSlices++;
        }
      }
    }
 
    // Discard expired cache entries.
    unsigned expiredSlices = 0;
    for (auto it = persistent->pingCache.begin(); it != persistent->pingCache.end(); ) {
      if constexpr (false && 10 < it->second->age) {
        persistent->pingCacheUnused.push_back(std::move(it->second));
        it = persistent->pingCache.erase(it); // Should be safe since C++-14.
        expiredSlices++;
      }
      else {
        ++it;
      }
    }
 
    // Sanity checks.
    for (auto & it : persistent->pingCache) {
      if (!it.second->linearized) {
        bool found = false;
        for (auto jt : persistent->needsLinearization) {
          if (jt == it.second.get()) { found = true; break; }
        }
        assert(found && "ping is neither already linearized nor is queued for linearization.");
      }
    }
 
    if (newSlices || expiredSlices) {
      //LOG_DEBUG(logger, "Updated slice cache, %d new slices (%d recycled) and %d expired.", newSlices, recycledSlices, expiredSlices);
    }
  }
 
 
  void addAndRemoveRegions(Context* context, const EchoSounder::OctProviderComponent* providerComp, EchoSounder::OctProviderData& providerData)
  {
    assert(providerData.callbackCount == 0);
 
    auto * dataSystem = ExtensionRegistry::getExtensionSystem<EchoSounder::DataSetSystem>(context);
    auto * octComp = providerComp->getComponent<Volumetric::OctComponent>();
 
    for (auto & src : providerData.sources) {
      auto * grpComp = src.entity->getComponent<EchoSounder::BeamGroupComponent>();
      if (!grpComp->sane) {
        if (!src.sane) providerData.doFlush = true;
        src.sane = false;
        continue;
      }
      src.sane = true;
 
      if (src.grpGen != grpComp->gen) {
        src.grpGen = grpComp->gen;
        providerData.doFlush = true;
      }
    }
 
    if (providerData.doFlush) {
      providerData.doFlush = false;
      providerData.persistent->tainted = true;
      for (auto & src : providerData.sources) {
        auto * dataRefComp = src.entity->getComponent<EchoSounder::DataRefComponent>();
        auto * dataComp = dataRefComp->data->getComponent<EchoSounder::DataSetComponent>();
        auto & dataData = dataSystem->getData(dataComp);
        src.dataGen = dataData.dataGen - 1;
        src.sliceBegin = dataData.persistent->buffer.sliceBegin;
        src.sliceCount = 0;
      }
 
      LOG_DEBUG(logger, "Performing flush");
      octComp->clearAllRegions = true;
      octComp->setChanged();
    }
 
    providerData.mostRecentTimestamp = 0;
    for (size_t s = 0; s < providerData.sources.size(); s++) {
      auto & src = providerData.sources[s];
 
      if (!src.sane) continue;
 
      auto * dataRefComp = src.entity->getComponent<EchoSounder::DataRefComponent>();
      auto * dataComp = dataRefComp->data->getComponent<EchoSounder::DataSetComponent>();
      auto & dataData = dataSystem->getData(dataComp);
      if (src.dataGen == dataData.dataGen) continue;
 
      const auto & config = dataData.persistent->config;
      const auto & buffer = dataData.persistent->buffer;
 
      if (0 < buffer.sliceCount) {
        auto last = buffer.sliceBegin + buffer.sliceCount - 1;
        last = std::min(last, last - config.capacity);
        providerData.mostRecentTimestamp = std::max(providerData.mostRecentTimestamp,
                                                    uint64_t(buffer.timestamps[last]));
      }
 
      auto sliceCount = std::min(providerData.history, buffer.sliceCount);
      auto sliceBegin = buffer.sliceBegin + buffer.sliceCount - sliceCount;
      sliceBegin = std::min(sliceBegin, sliceBegin - config.capacity);  // Wrap.
 
                                                                        // Delete pings/regions
      if (!providerData.doFlush) {
        src.sliceBegin = std::min(src.sliceBegin, src.sliceBegin - config.capacity);
        while (src.sliceBegin != sliceBegin) {
          //LOG_DEBUG(logger, "%llx: Discarding slice at %d", (intptr_t)&src, src.sliceBegin);
          octComp->regionsToRemove.push_back(EchoSounder::encodeKey2(uint32_t(s), src.sliceBegin));
          octComp->setChanged();
          src.sliceBegin = std::min(src.sliceBegin + 1, src.sliceBegin + 1 - config.capacity);
          src.sliceCount = std::max(1u, src.sliceCount) - 1u;
        }
      }
      src.sliceBegin = sliceBegin;
 
      const auto * grpComp = src.entity->getComponent<EchoSounder::BeamGroupComponent>();
 
      for (; src.sliceCount < sliceCount; src.sliceCount++) {
        auto slice = src.sliceCount + src.sliceBegin;
        slice = std::min(slice, slice - config.capacity);
 
        const auto & metaPing = buffer.metaPings[slice];
 
        glm::vec3 minCorner, maxCorner;
        EchoSounder::getAxisAlignedBoundingBox(minCorner, maxCorner,
                                               metaPing.arrayOrientationGlobal,
                                               metaPing.arrayPositionGlobal,
                                               0, // FIXME - Assume EK/ME/MS for now.
                                               grpComp->minDirectionX, grpComp->maxDirectionX,
                                               grpComp->minDirectionY, grpComp->maxDirectionY,
                                               grpComp->minDistance, grpComp->maxDistance);
        //LOG_DEBUG(logger, "%llx: Adding slice at %d", (intptr_t)&src, slice);
        octComp->regionsToAdd.push_back(Volumetric::Region{ EchoSounder::encodeKey2(uint32_t(s), slice), minCorner, maxCorner });
        octComp->setChanged();
      }
    }
  }
 
 
 
}
 
 
void Cogs::Core::EchoSounder::OctProviderSystem::preUpdate(Context * context)
{
  //const unsigned L = 300;
 
  // Update set of tiles before OctSystem:Update(@PostView) runs.
  for (auto & providerComp : pool) {
    auto & providerData = getData(&providerComp);
    if (!providerData.persistent) {
      providerData.persistent = std::make_shared<OctProviderPersistent>();
      providerData.persistent->context = context;
    }
 
    if (providerData.history != providerComp.history) {
      providerData.history = providerComp.history;
      providerData.doFlush = true;
    }
 
    auto * octComp = providerComp.getComponent<Volumetric::OctComponent>();
    octComp->valueMin = dBToLinear(providerComp.decibelMin);
    octComp->valueMax = dBToLinear(providerComp.decibelMax);
    octComp->source = Volumetric::OctSource::ValueAge;
    octComp->alphaCallback.func = alphaCallback;
    octComp->alphaCallback.data = &providerData;
 
    const auto * sceneComp = providerComp.getComponent<SceneComponent>();
    if (sceneComp->hasFieldChanged(&SceneComponent::children)) {
      LOG_DEBUG(logger, "Repopulating list of providers");
      providerData.sources.clear();
      providerData.persistent->tainted = true;
      for (auto child : sceneComp->children) {
        if (child->getComponent<DataRefComponent>() == nullptr) continue;
        if (child->getComponent<BeamGroupComponent>() == nullptr) continue;
        providerData.sources.emplace_back(OctProviderSource{ child });
      }
      providerData.doFlush = true;
    }
 
    for (auto & source : providerData.sources) {
      source.entity->getComponent<BeamGroupComponent>()->requestCallback([that = this, comp = &providerComp](Context* context) { that->callback(context, comp); });
    }
    providerData.callbackCount = static_cast<unsigned>(providerData.sources.size());
  }
}
 
void Cogs::Core::EchoSounder::OctProviderSystem::callback(Context* context, OctProviderComponent * providerComp)
{
  auto & providerData = getData(providerComp);
  assert(providerData.callbackCount != 0);
  if (--providerData.callbackCount == 0) {
    // Callback of last source, update regions.
    addAndRemoveRegions(context, providerComp, providerData);
  }
}
 
float Cogs::Core::EchoSounder::OctProviderSystem::alphaCallback(void* data, uint64_t clientData)
{
  const auto * providerData = reinterpret_cast<OctProviderData*>(data);
  return float(10e-7*(providerData->mostRecentTimestamp - clientData));
}
 
void Cogs::Core::EchoSounder::OctProviderSystem::update(Context * context)
{
  auto * dataSystem = ExtensionRegistry::getExtensionSystem<DataSetSystem>(context);
 
  for (auto & providerComp : pool) {
    auto & providerData = getData(&providerComp);
    auto * octComp = providerComp.getComponent<Volumetric::OctComponent>();
 
    // If a tile populate task is running, we keep our hands off.
    if(providerData.persistent->tasksRunning) continue;
 
    auto * persistent = providerData.persistent.get();
    auto * submitTileResponse = Reflection::TypeDatabase::getType("VolOctComponent").getMethod("submitTileResponse");
 
    // No task running, forward results to OctSystem.
    while (!providerData.persistent->responses.empty()) {
      submitTileResponse->call(octComp, std::move(persistent->responses.back()), persistent->tainted == false);
      providerData.persistent->responses.pop_back();
    }
 
    // OctSystem:Update(@PostView) has run, pull new requests and populate with data.
    auto * initTileResponse = Reflection::TypeDatabase::getType("VolOctComponent").getMethod("initTileResponse");
 
    assert(providerData.persistent->responses.empty());
    providerData.persistent->taskCount.clear();
 
    unsigned regionsPerTask = 1;
    unsigned targetParallelism = 2*std::max(1u, std::thread::hardware_concurrency());
    unsigned currentParallelism = 0;
    for (const auto reqTileKey : octComp->tileRequests) {
      if (targetParallelism < currentParallelism) break;
 
      Cogs::Core::Volumetric::TileResponse* res = nullptr;
      res = initTileResponse->callWithReturn(octComp, res, &reqTileKey);
 
      if (!res) break;
      if (res->regionKeys.empty()) {
        LOG_WARNING(logger, "Tile request with no regions.");
      }
 
      unsigned maxTasks = std::max(1u, std::min(targetParallelism, res->N.z));
 
      unsigned taskCount = std::max(1u, std::min(maxTasks,
        ((unsigned)res->regionKeys.size() + regionsPerTask - 1u) / regionsPerTask));
 
      providerData.persistent->responses.push_back(std::move(res));
      providerData.persistent->taskCount.push_back(taskCount);
      currentParallelism += taskCount;
    }
 
    populatePingCache(dataSystem, providerData);
 
    persistent->tainted = false;
    persistent->tasksRunning = true;
    context->taskManager->enqueue(Cogs::Core::TaskManager::ResourceQueue, OctDummyTask{ context, providerData.persistent });
  }
}