SinglePingIsoSurfacesTasks.cpp

#include "Resources/MeshManager.h"
#include "Platform/Instrumentation.h"
#include "Resources/VertexFormats.h"
#include "Context.h"
 
#include "../Extensions/GeometryProcessing/GeometryProcessing.h"
#include "../Extensions/IsoSurfaces/IsoSurfaces.h"
 
#include "../Components/DataSetComponent.h"
#include "../Components/PingIsoComponent.h"
#include "../Components/BeamGroupComponent.h"
#include "../Systems/PingIsoSystem.h"
#include "../Systems/DataSetSystem.h"
#include "../BeamUtils.h"
#include "../BoundingBox.h"
#include "dBToLinearTask.h"
#include "DepthResampleTask.h"
#include "BeamResampleTask.h"
#include "SinglePingIsoSurfacesTasks.h"
 
#include "Foundation/Logging/Logger.h"
#include "Foundation/Memory/MemoryBuffer.h"
#include "Foundation/Platform/Timer.h"
 
#include <glm/gtc/type_ptr.hpp>
 
using std::min;
using std::max;
using std::numeric_limits;
using std::vector;
 
using glm::vec2;
using glm::vec3;
using glm::ivec3;
using glm::angleAxis;
 
using namespace Cogs::Core;
 
using Cogs::Memory::TypedBuffer;
 
 
namespace {
 
  const int   taskSize_Linearize = 5000;
  const int   taskSize_ResampleDepth = 20;
  const int   taskSize_InterpolateDirs = 500;
  const int   taskSize_InterpolateMinor = 100;
  const int   taskSize_InterpolateMajor = 100;
  const ivec3 taskSize_Classify = ivec3(32, 16, 16);
  const int   taskSize_PopCnt = 10000;
  const int   taskSize_ExtractVtx = 10000;
  const int   taskSize_ExtractIdx = 10000;
  const int   taskSize_Normals = 10000;
  const int   taskSize_BBox = 10000;
 
  const size_t granularity = 1024;
 
  int elapsed_Count = 0;
  double elapsed_Total = 0.0;
  double elapsed_Resample = 0.0;
  double elapsed_Linearize = 0.0;
  double elapsed_DepthResample = 0.0;
  double elapsed_BeamResample = 0.0;
  double elapsed_Analyze = 0.0;
  double elapsed_Extract = 0.0;
  double elapsed_Normals = 0.0;
  double elapsed_SetupMesh = 0.0;
  double elapsed_Store = 0.0;
 
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("EchoPingIsoTasks");
 
  static const float factorA = (float)(log2(10.0) / 10.0);
  static const float factorAe = (float)(log(10.0) / 10.0);
  static const float scale_dB = 100.0f; // Constant added to dB value to utilize both positive and negative exponents.
 
 
  template<typename T>
  struct WrapFunctorPointer
  {
    T* pointer;
 
    WrapFunctorPointer(T* pointer) : pointer(pointer) {}
 
    void operator()() { (*pointer)(); }
 
  };
 
 
  struct VertexEmit
  {
    const EchoSounder::SinglePingIsoSurfacesMainTask* main = nullptr;
    uint32_t* slicesToMerge = nullptr;
    size_t slicesToMergeModulo = 0;
    glm::vec3* P = nullptr;
    glm::vec2* T = nullptr;
    size_t vertexOffset = 0;
    float linearThreshold = 0.f;
    float normalizedLayer = 0.f;
    float separation = 0.f;
    ivec3 maxFieldIndex;
 
    void operator()(int32_t index, ivec3 aSample, ivec3 bSample)
    {
      ivec3 I[2] = { aSample, bSample };
 
      const size_t beamMinorCount = main->beamMinorCount;
      const size_t sampleCount = main->sampleCount;
 
      float linearValue[2];
      vec3 positions[2];
      for (int m = 0; m < 2; m++) {
        ivec3 Ir = I[m];
        ivec3 Ic = glm::clamp(Ir, ivec3(0), maxFieldIndex);
 
        size_t linearIndex = (Ic.z*beamMinorCount + Ic.y)*sampleCount + Ic.x;
 
        linearValue[m] = main->persistent->linearValues[linearIndex] - linearThreshold;
 
        float depth = Ic.x*main->depthStep + main->depthOffset;
        vec3 o = main->metaPing.arrayPositionVessel;
        vec3 d = main->rayDir[Ic.z*beamMinorCount + Ic.y];
        vec3 dd = d;
 
        if (Ic.x != Ir.x) {
          int signed_one = Ic.x - Ir.x < 0 ? -1 : 1;
          depth = max(0.f, depth - separation * signed_one);
        }
 
        if (Ic.y != Ir.y) {
          int signed_one = Ic.y - Ir.y < 0 ? -1 : 1;
          // Verify that signed_one pushes the value inside the valid range
          assert(0 <= Ic.y + signed_one);
          assert(Ic.y + signed_one <= maxFieldIndex.y);
          d += separation * glm::normalize(dd - main->rayDir[Ic.z * beamMinorCount + Ic.y + signed_one]);
        }
        if (Ic.z != Ir.z) {
          int signed_one = Ic.z - Ir.z < 0 ? -1 : 1;
          int ic_z = Ic.z + signed_one;
          // Verify that signed_one pushes the value inside the valid range
          assert(0 <= Ic.z + signed_one);
          assert(Ic.z + signed_one <= maxFieldIndex.z);
          d += separation * glm::normalize(dd - main->rayDir[ic_z * beamMinorCount + Ic.y]);
        }
 
        positions[m] = o + depth*d;
      }
 
      float den = (linearValue[1] - linearValue[0]);
      float t = abs(den) < numeric_limits<float>::min() ? 0.5f : -linearValue[0] / den;
 
      size_t vo = vertexOffset + index;
      if (slicesToMerge && (I[0].y == I[1].y) && (I[0].y == 0 || I[0].y == maxFieldIndex.y)) {
 
        // If we are wrapping along-Y (slicesToMerge != nullptr), and we're on a vertex that
        // lies on an Y-side, record vertex index.
 
        const size_t slice = (I[0].y == 0) ? 0 : 1;
        const size_t axis = (I[0].x + 1 == I[1].x) ? 0 : 2;
 
        assert((!axis && (I[0].z == I[1].z)) || ((I[0].z + 1 == I[1].z) && (I[0].x == I[1].x)));
        assert((I[0].x + 1) <= sampleCount);
 
        assert(0 <= I[0].z + 1);
        assert(0 <= I[0].x + 1);
        const size_t cell = (I[0].z + 1) * (sampleCount + 1) + (I[0].x + 1);
        const size_t ix = cell * 4 + slice + axis;
 
        assert(ix < slicesToMergeModulo);
        assert(slicesToMerge[ix] == uint32_t(~0));
 
        slicesToMerge[ix] = uint32_t(vo);
      }
      P[vo] = (1.f - t)*positions[0] + t*positions[1];
      T[vo] = vec2(normalizedLayer, 0.5f);
    }
 
    void operator()(int32_t ixIndex, glm::ivec4* samples, const uint32_t ixN)
    {
      for (uint32_t k = 0; k < ixN; k++) {
        this->operator()(ixIndex + k,
                         glm::ivec3(samples[0][0],
                                    samples[1][0],
                                    samples[2][0]),
                         glm::ivec3(samples[0][1 + k],
                                    samples[1][1 + k],
                                    samples[2][1 + k]));
      }
    }
  };
 
 
  struct VerticalMaskTask
  {
    const EchoSounder::SinglePingIsoSurfacesMainTask* parent;
    const size_t a;
    const size_t b;
 
    VerticalMaskTask(EchoSounder::SinglePingIsoSurfacesMainTask* parent, size_t a, size_t b) : parent(parent), a(a), b(b) { }
   
    void operator()()
    {
      CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "VertMask");
 
      const size_t minorCount = parent->beamMinorCount;
      //const size_t majorCount = parent->beamMajorCount;
      const size_t sampleCount = parent->sampleCount;
      const float doff = parent->depthOffset;
      const float dstp = parent->depthStep;
      const float maxRange = doff + dstp*sampleCount;
 
      float minVerticalDepth = std::isfinite(parent->minVerticalDepth) ? std::max(0.f, parent->minVerticalDepth) : 0.f;
      float maxVerticalDepth = std::isfinite(parent->maxVerticalDepth) ? std::max(minVerticalDepth, parent->maxVerticalDepth) : 10000.f;
 
      EchoSounder::clipVertical(parent->persistent->linearValues, a * minorCount * sampleCount,
                                parent->metaPing.arrayPositionVessel, parent->rayDir, a * minorCount,
                                minVerticalDepth, maxVerticalDepth,
                                parent->depthOffset, parent->depthStep, maxRange,
                                minorCount * (b - a), sampleCount);
    }
 
  };
}
 
struct ClipToSeabedTask
{
  const EchoSounder::SinglePingIsoSurfacesMainTask* parent;
  const size_t majorIndexStart;
  const size_t majorIndexEnd;
 
  ClipToSeabedTask(EchoSounder::SinglePingIsoSurfacesMainTask* parent, size_t majorIndexStart, size_t majorIndexEnd) : parent(parent), majorIndexStart(majorIndexStart), majorIndexEnd(majorIndexEnd) { }
 
  void operator()()
  {
    CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "ClipToSeabedTask");
 
    const size_t minorCount = parent->beamMinorCount;
    //const size_t majorCount = parent->beamMajorCount;
    const size_t sampleCount = parent->sampleCount;
    const float depthOffset = parent->depthOffset;
    const float depthStep = parent->depthStep;
 
    auto & output = parent->persistent->linearValues;
    for (size_t k = majorIndexStart; k < majorIndexEnd; k++) {
      for (size_t j = 0; j < minorCount; j++) {
 
        size_t out = (k*minorCount + j)*sampleCount;
 
        auto bottomDepth = parent->depths[k*minorCount +j];
        auto clipDist = bottomDepth - parent->seabedClipOffset;
        auto N = (bottomDepth != 0.f && std::isfinite(parent->seabedClipOffset)) ? std::min(static_cast<size_t>(std::max(0.f, std::floor((clipDist - depthOffset) / depthStep))), sampleCount) : sampleCount;
        for (size_t i = N; i < sampleCount; ++i) {
          output[out + i] = 0.f;
        }
      }
    }
  }
};
 
EchoSounder::SinglePingIsoSurfacesMainTask::SinglePingIsoSurfacesMainTask(Context* context,
                                                              const PingIsoComponent* isoComp, const SinglePingIsoSurfacesData* isoData,
                                                              const BeamGroupComponent* groupComp,
                                                              const DataSetComponent* /*dataComp*/, const DataSetData* dataData)
  :
  persistent(isoData->persistent),
  context(context),
  index(isoData->index),
  thresholds(isoData->thresholds),
  flipOrientation(isoComp->flipOrientation),
  beamMinorClosed(groupComp->minorClosed),
  metaPing(dataData->persistent->buffer.metaPings[index]),
  layers(static_cast<int>(isoData->thresholds.size())),
  sampleCount(dataData->persistent->config.sampleCount),
  coordSys(dataData->persistent->config.coordSys),
  capSeparation(isoComp->capSeparation),
  seabedClipOffset(isoData->seabedClipOffset),
  depthOffset(dataData->persistent->config.depthOffset),
  depthStep(dataData->persistent->config.depthStep),
  logarithmicUnit(dataData->persistent->config.useDecibel),
  depthStepResample(isoComp->depthSpacing),
  beamStepResample(isoComp->beamSpacing),
  maxBeamUpsample(isoComp->beamMaxUpsample),
  overflowThreshold(isoComp->overflowThreshold),
  minVerticalDepth(isoComp->minVerticalDepth),
  maxVerticalDepth(isoComp->maxVerticalDepth)
{
  const auto srcMajNum = groupComp->majorCount;
  const auto srcMinNum = groupComp->minorCount;
 
  beamMajorCount = max(2u, srcMajNum);
  beamMinorCount = max(2u, srcMinNum + (beamMinorClosed ? 1u : 0u));
  beamCount = beamMajorCount*beamMinorCount;
  persistent->values.resize(beamCount*sampleCount + beamMajorCount);
  depths.resize(beamCount);
 
  directionX.clear();
  directionX.resize(beamCount);
  directionY.clear();
  directionY.resize(beamCount);
 
  const auto & config = dataData->persistent->config;
  auto * src = dataData->persistent->buffer.values.data() + index*static_cast<size_t>(dataData->persistent->config.beamCount*sampleCount);
  for (uint32_t major = 0; major < beamMajorCount; major++) {
    for (uint32_t minor = 0; minor < beamMinorCount; minor++) {
 
      unsigned k = minor;
      if (srcMinNum <= minor) {
        if (beamMinorClosed) {
          k -= srcMinNum;
        }
        else {
          k = srcMinNum - 1;
        }
      }
      const auto ixDst = major*(beamMinorCount) + minor;
      const auto ixSrc = groupComp->beams[min(srcMajNum - 1, major)*beamMinorCount + k];
 
      directionX[ixDst] = dataData->persistent->config.directionX[ixSrc];
      directionY[ixDst] = dataData->persistent->config.directionY[ixSrc];
      depths[ixDst] = dataData->persistent->buffer.bottomDepths[ixSrc];
 
      memcpy(persistent->values.data() + sampleCount*ixDst, src + sampleCount*ixSrc, sizeof(float)*sampleCount);
    }
  }
 
  if (srcMajNum == 1) {
 
    unsigned srcMinNumPadded = srcMinNum + (beamMinorClosed ? 1u : 0u);
 
    std::vector<uint32_t> beams(srcMinNumPadded);
    for (unsigned i = 0; i < srcMinNum; i++) {
      beams[i] = groupComp->beams[i];
    }
    if (beamMinorClosed) {
      beams[srcMinNum] = beams[0];
    }
 
    inflateFans(directionX, directionY,
                config.directionX, config.beamWidthX,
                config.directionY, config.beamWidthY,
                beams, groupComp->topology, srcMinNumPadded);
 
  }
}
 
void EchoSounder::SinglePingIsoSurfacesMainTask::operator()()
{
  auto & tm = context->taskManager;
 
  Timer timerTotal, timerPass;
 
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "EchoGridMain");
  timerTotal.start();
 
  {
    CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "PassResize1");
    timerPass.start();
 
    linearThresholds.resize(thresholds.size());
    persistent->linearValues.resize(persistent->values.size());
    if (depthStepResample != 0.0f) {
      sampleCountResample = min(10000, static_cast<int>(floor((depthStep * sampleCount) / depthStepResample)));
      persistent->tmpValues.resize(beamCount*sampleCountResample);
    }
 
    elapsed_Resample += timerPass.elapsedSeconds();
  }
 
  if (logarithmicUnit) {
    CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "PassLinearize");
    timerPass.start();
 
    auto group = context->taskManager->createGroup();
 
    transform(thresholds.begin(), thresholds.end(), linearThresholds.begin(), dBToLinear);
    
    size_t N = persistent->values.size();
    for (size_t o = 0; o < N; o += taskSize_Linearize) {
      dBToLinearTask task;
      task.overflowThreshold = overflowThreshold;
      task.in = persistent->values.data() + o;
      task.out = persistent->linearValues.data() + o;
      task.N = min(N, o + taskSize_Linearize) - o;
      context->taskManager->enqueueChild(group, task);
    }
    context->taskManager->wait(group);
    context->taskManager->destroy(group);
 
    elapsed_Linearize += timerPass.elapsedSeconds();
  }
  else {
    linearThresholds = thresholds;
    persistent->linearValues.swap(persistent->values);
  }
 
  
  if (true) {
    assert(directionX.size() == beamMajorCount*beamMinorCount);
    assert(directionY.size() == beamMajorCount*beamMinorCount);
 
    // Calculate ray directions.
    int total = beamMajorCount*beamMinorCount;
    rayDir.resize(total);
    auto group = context->taskManager->createGroup();
    for (int b = 0; b < total; b += taskSize_InterpolateDirs) {
      context->taskManager->enqueueChild(group,
                                         CalculateBeamDirectionsTask(rayDir.data(),
                                                                     coordSys,
                                                                     directionX.data(), directionY.data(),
                                                                     beamMajorCount, beamMinorCount, metaPing.arrayOrientationVessel,
                                                                     b, min(b + taskSize_InterpolateDirs, total)));
    }
    context->taskManager->wait(group);
    context->taskManager->destroy(group);
  }
 
  
  if (std::isfinite(seabedClipOffset)) {
    //clip againts bottom detection  
    int step = (beamMajorCount + 7) / 8; //just to divide task into subtask. Step is how many major beams that are treated in one task
    auto group = context->taskManager->createGroup();
 
    for (uint32_t b = 0; b < beamMajorCount; b += step) {
      context->taskManager->enqueueChild(group,
        ClipToSeabedTask(this,
          b,
          min(b + step, beamMajorCount)));
 
    }
    context->taskManager->wait(group);
    context->taskManager->destroy(group);
  }
 
 
  // PASS: Resample along rays
  if (depthStepResample != 0.0f) {
    CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "DepthResample");
    timerPass.start();
 
    auto depthResampleGroup = context->taskManager->createGroup();
    for (uint32_t b = 0; b < beamCount; b += taskSize_ResampleDepth) {
      context->taskManager->enqueueChild(depthResampleGroup, DepthResampleTask(persistent->tmpValues.data(),
                                                                               depthStepResample,
                                                                               sampleCountResample,
                                                                               persistent->linearValues.data(),
                                                                               depthStep,
                                                                               sampleCount,
                                                                               b, min(b + taskSize_ResampleDepth, beamCount)));
    }
    context->taskManager->wait(depthResampleGroup);
    context->taskManager->destroy(depthResampleGroup);
 
    persistent->linearValues.swap(persistent->tmpValues);
    depthStep = depthStepResample;
    sampleCount = sampleCountResample;
 
    elapsed_DepthResample += timerPass.elapsedSeconds();
  }
 
  // PASS: Resample between beams
  if(beamStepResample != 0.0f) {
    CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "BeamResample");
    timerPass.start();
 
    float angularStep = std::atan(beamStepResample / (depthOffset + sampleCount*depthStep));
 
    std::vector<InterpolationSamplePos> iSamplesOut;
    std::vector<InterpolationSamplePos> jSamplesOut;
    SetupBeamFilter(iSamplesOut, jSamplesOut, rayDir,
                    angularStep, maxBeamUpsample,
                    beamMinorCount, beamMajorCount);
 
    uint32_t beamMajorCountNew = static_cast<uint32_t>(jSamplesOut.size());
    uint32_t beamMinorCountNew = static_cast<uint32_t>(iSamplesOut.size());
 
    auto interpolateGroup1 = context->taskManager->createGroup();
    int total = beamMajorCountNew*beamMinorCountNew;
 
    // FIXME: Check if the following needs to be split into tasks.
    std::vector<glm::vec3> rayDirNew(total);
    for (uint32_t j = 0; j < beamMajorCountNew; j++) {
      for (uint32_t i = 0; i < beamMinorCountNew; i++) {
        const auto & d00 = rayDir[(jSamplesOut[j].i + 0)*beamMinorCount + (iSamplesOut[i].i + 0)];
        const auto & d01 = rayDir[(jSamplesOut[j].i + 0)*beamMinorCount + (iSamplesOut[i].i + 1)];
        const auto & d10 = rayDir[(jSamplesOut[j].i + 1)*beamMinorCount + (iSamplesOut[i].i + 0)];
        const auto & d11 = rayDir[(jSamplesOut[j].i + 1)*beamMinorCount + (iSamplesOut[i].i + 1)];
 
        // Bi-spherical linear interpolation
        const auto t0 = iSamplesOut[i].t;
        const auto G0 = std::acos(glm::dot(d00, d01));
        const auto d0 = (1.f / std::sin(G0))*(std::sin(G0*(1.f - t0))*d00 + std::sin(G0*t0)*d01);
 
        const auto G1 = std::acos(glm::dot(d10, d11));
        const auto d1 = (1.f / std::sin(G1))*(std::sin(G1*(1.f - t0))*d10 + std::sin(G1*t0)*d11);
 
        const auto G = std::acos(glm::dot(d0, d1));
        const auto t = jSamplesOut[j].t;
        const auto d = (1.f / std::sin(G))*(std::sin(G*(1.f - t))*d0 + std::sin(G*t)*d1);
 
        rayDirNew[j*beamMinorCountNew + i] = d;
      }
    }
    rayDir.swap(rayDirNew);
 
    // Separable pass 1, interpolate values between minor beams.
    persistent->tmpValues.resize(beamMajorCount * beamMinorCountNew * sampleCount);
    total = beamMajorCount*beamMinorCountNew;
    for (int b = 0; b < total; b += taskSize_InterpolateMinor) {
      context->taskManager->enqueueChild(interpolateGroup1,
                                         InterpolateBeamDataMinorTask(persistent->tmpValues.data(),
                                                                      beamMajorCount, beamMinorCountNew, beamMinorCount, sampleCount,
                                                                      persistent->linearValues.data(), iSamplesOut.data(),
                                                                      b, min(b + taskSize_InterpolateMinor, total)));
    }
    context->taskManager->wait(interpolateGroup1);
    context->taskManager->destroy(interpolateGroup1);
 
    persistent->linearValues.swap(persistent->tmpValues);
    beamMinorCount = beamMinorCountNew;
 
    // Separable pass 2, interpolate values between major beams.
    auto interpolateGroup2 = context->taskManager->createGroup();
    persistent->tmpValues.resize(beamMajorCountNew * beamMinorCount * sampleCount);
    total = beamMajorCountNew * beamMinorCount;
    for (int b = 0; b < total; b += taskSize_InterpolateMajor) {
      context->taskManager->enqueueChild(interpolateGroup2,
                                         InterpolateBeamDataMajorTask(persistent->tmpValues.data(),
                                                                      beamMajorCountNew, beamMinorCount, beamMinorCount, sampleCount,
                                                                      persistent->linearValues.data(), jSamplesOut.data(),
                                                                      b, min(b + taskSize_InterpolateMajor, total)));
    }
 
    context->taskManager->wait(interpolateGroup2);
    context->taskManager->destroy(interpolateGroup2);
    persistent->linearValues.swap(persistent->tmpValues);
    beamMajorCount = beamMajorCountNew;
 
    elapsed_BeamResample += timerPass.elapsedSeconds();
  }
  
 
  //not sure why clipping agains vertical depth is not done before resampling (we clip agains bottom before resampling), philipb
  if (std::isfinite(minVerticalDepth) || std::isfinite(maxVerticalDepth)) {
    // Mask according to vertical depth.
    int step = (beamMajorCount + 7) / 8;
    auto group = context->taskManager->createGroup();
 
    for (uint32_t b = 0; b < beamMajorCount; b += step) {
      context->taskManager->enqueueChild(group,
                                         VerticalMaskTask(this,
                                                          b,
                                                          min(b + step, beamMajorCount)));
 
    }
    context->taskManager->wait(group);
    context->taskManager->destroy(group);
  }
 
 
  timerPass.start();
  ivec3 Ns(sampleCount, beamMinorCount, beamMajorCount);
  persistent->volumeCases.resize(layers * Ns.x*Ns.y*Ns.z);
  persistent->volumeOffsets.resize(persistent->volumeCases.size());
  persistent->wallCases.resize(layers * 2 * (Ns.x*Ns.y + Ns.x*Ns.z + Ns.y*Ns.z));
  persistent->wallOffsets.resize(persistent->wallCases.size());
  elapsed_Resample += timerPass.elapsedSeconds();
 
 
  ivec3 fieldDim(sampleCount, beamMinorCount, beamMajorCount);
  ivec3 gridA(-1, beamMinorClosed ? 0 : -1, -1);
  ivec3 gridB = fieldDim + ivec3(1, beamMinorClosed ? 0 : 1, 1);
 
  const auto M = gridB - gridA;
 
  vector<int32_t> cellOffsets;
  TypedBuffer<uint8_t> activeCellCases;
  TypedBuffer<int32_t> cellMap;
  TypedBuffer<int32_t> activeCellVertexIndexOffsets;
  TypedBuffer<int32_t> activeCellTriangleIndexOffsets;
  TypedBuffer<int32_t> activeCellIndices;
  TypedBuffer<IsoSurfaces::Scratch::ijk_t> activeCellIJK;
  std::vector<TaskId> tasks;
 
 
  // Fire away initializing memory needed in vertexExtract.
  TaskId initSlicesToMergeGroup = NoTask;
  const size_t slicesToMergeModulo = beamMinorClosed ? 4 * (beamMajorCount + 1) * (sampleCount + 1) : 0;
  TypedBuffer<uint32_t> slicesToMerge(layers* slicesToMergeModulo);
  if (beamMinorClosed) {
    initSlicesToMergeGroup = context->taskManager->createGroup();
    const size_t chunk = std::max(slicesToMerge.size() / Threads::hardwareConcurrency(), 64 * granularity);
    for (size_t i = 0; i < slicesToMerge.size(); i += chunk) {
      size_t n = std::min(chunk, slicesToMerge.size() - i);
      context->taskManager->enqueueChild(initSlicesToMergeGroup,
                                         [ptr = slicesToMerge.data() + i, n]()
      {
        CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "InitSlicesToMerge");
        std::memset(ptr, ~0u, sizeof(uint32_t)* n);
      });
    }
  }
 
  // Analyze pass
  {
    CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "Analyze");
    timerPass.start();
 
    analyze(context,
            vertexOffsets, indexOffsets, cellOffsets,
            cellMap,
            activeCellCases,
            activeCellVertexIndexOffsets,
            activeCellTriangleIndexOffsets,
            activeCellIndices,
            activeCellIJK,
            linearThresholds,
            true,
            persistent->linearValues.data(),
            fieldDim, gridA, gridB,
            nullptr);
 
    elapsed_Analyze += timerPass.elapsedSeconds();
  }
 
  auto vertexCount = vertexOffsets.back();
  auto indexCount = indexOffsets.back();
 
  auto mesh = context->meshManager->createLocked();
 
 
  // Initialize beamMinorClosed-remapping array
  TaskId initIndexRemappingGroup = NoTask;
  TypedBuffer<uint32_t> indexRemapping;
  if (vertexCount && beamMinorClosed) {
    initIndexRemappingGroup = context->taskManager->createGroup();
    indexRemapping.resize(vertexCount, false);
    const size_t chunk = std::max(indexRemapping.size() / Threads::hardwareConcurrency(), 64 * granularity);
    for (size_t i = 0; i < indexRemapping.size(); i += chunk) {
      size_t j = std::min(i + chunk, indexRemapping.size());
      tm->enqueueChild(initIndexRemappingGroup,
                       [ptr = indexRemapping.data(), i, j]()
      {
        CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "InitIndexRemapping");
        for (size_t k = i; k < j; k++) {
          ptr[k] = static_cast<uint32_t>(k);
        }
      });
    }
  }
 
  // Wait for memory slicesToMerge is finished with initializion.
  if (initSlicesToMergeGroup.isValid()) {
    tm->wait(initSlicesToMergeGroup);
    tm->destroy(initSlicesToMergeGroup);
  }
 
  if (0 < vertexCount) {
    TypedBuffer<uint32_t> indices;
    Geometry::BoundingBox bbox;
 
    auto P = mesh->mapPositions(0, vertexCount+1); // TODO: Fix out of bound SSE access (+1)
    auto N = mesh->map<vec3>(VertexDataType::Normals, VertexFormats::Norm3f, 0, vertexCount);
    auto T = mesh->map<vec2>(VertexDataType::TexCoords0, VertexFormats::Tex2f, 0, vertexCount);
 
    indices.resize(indexCount, false);
 
    
 
    // PASS: Extract vertices and indices
    {
      CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "Extract");
      timerPass.start();
 
      TaskId vertexExtractGroup = context->taskManager->createGroup();
      for (size_t i = 0; i < layers; i++) {
        VertexEmit vertexEmit;
        vertexEmit.main = this;
        vertexEmit.slicesToMerge = beamMinorClosed ? slicesToMerge.data() + i * slicesToMergeModulo : nullptr;
        vertexEmit.slicesToMergeModulo = slicesToMergeModulo;
        vertexEmit.vertexOffset = vertexOffsets[i];
        vertexEmit.linearThreshold = linearThresholds[i];
        vertexEmit.normalizedLayer = (i + 0.5f) / layers;
        vertexEmit.maxFieldIndex = glm::max(ivec3(0), fieldDim - ivec3(1));
        vertexEmit.P = P.data;
        vertexEmit.T = T.data;
        vertexEmit.separation = 2.f * capSeparation * (layers - i - 1); // Factor 2 is due to we put the vertex on edge midpoint.
        extractVertices(context,
                        vertexExtractGroup,
                        IsoSurfaces::VertexEmitFunc(vertexEmit),
                        activeCellCases.data() + cellOffsets[i],
                        activeCellVertexIndexOffsets.data() + cellOffsets[i] + i,
                        activeCellIndices.data() + cellOffsets[i],
                        activeCellIJK.data() + cellOffsets[i],
                        cellOffsets[i + 1] - cellOffsets[i],
                        gridA, gridB,
                        nullptr);
      }
 
      TaskId indexExtractGroup = context->taskManager->createGroup();
      for (size_t i = 0; i < layers; i++) {
        Cogs::Core::IsoSurfaces::IndexEmitFunc emitter;
        if (flipOrientation == true) {
          emitter =
            [vertexOffset = vertexOffsets[i],
            indices = indices.data() + indexOffsets[i]]
          (uint32_t ixIndex, glm::ivec3 /*cell*/, const uint32_t* cellIndices, const uint32_t ixN)
          {
            for (uint32_t k = 0; k < ixN; k += 3) {
              indices[ixIndex + k + 0] = cellIndices[k + 0] + vertexOffset;
              indices[ixIndex + k + 1] = cellIndices[k + 1] + vertexOffset;
              indices[ixIndex + k + 2] = cellIndices[k + 2] + vertexOffset;
            }
          };
        }
        else {
          emitter =
            [vertexOffset = vertexOffsets[i],
            indices = indices.data() + indexOffsets[i]]
          (uint32_t ixIndex, glm::ivec3 /*cell*/, const uint32_t* cellIndices, const uint32_t ixN)
          {
            for (uint32_t k = 0; k < ixN; k += 3) {
              indices[ixIndex + k + 0] = cellIndices[k + 0] + vertexOffset;
              indices[ixIndex + k + 1] = cellIndices[k + 2] + vertexOffset;
              indices[ixIndex + k + 2] = cellIndices[k + 1] + vertexOffset;
            }
          };
        }
        extractIndices(context,
                       indexExtractGroup,
                       emitter,
                       cellMap.data() + i * M.x*M.y*M.z,
                       activeCellCases.data() + cellOffsets[i],
                       activeCellVertexIndexOffsets.data() + cellOffsets[i] + i,
                       activeCellTriangleIndexOffsets.data() + cellOffsets[i] + i,
                       activeCellIndices.data() + cellOffsets[i],
                       activeCellIJK.data() + cellOffsets[i],
                       cellOffsets[i + 1] - cellOffsets[i],
                       gridA, gridB,
                       nullptr);
 
      }
      tm->wait(vertexExtractGroup);
      tm->destroy(vertexExtractGroup);
      if (initIndexRemappingGroup.isValid()) {
        tm->wait(initIndexRemappingGroup);
        tm->destroy(initIndexRemappingGroup);
      }
      if (beamMinorClosed) {
        size_t NN = slicesToMerge.size() / 2;
        const size_t chunk = std::max(NN / Threads::hardwareConcurrency(), 64 * granularity);
 
        for (size_t i = 0; i < NN; i += chunk) {
          size_t j = std::min(i + chunk, NN);
          tm->enqueueChild(indexExtractGroup,
                           [&slicesToMerge, &indexRemapping, i, j]() {
                             CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "SetIndexRemapping");
                             for (size_t k = i; k < j; k++) {
                               if ((slicesToMerge[2 * k + 0] != ~0u) && (slicesToMerge[2 * k + 1] != ~0u)) {
                                 indexRemapping[slicesToMerge[2 * k + 1]] = slicesToMerge[2 * k + 0];
                               }
                             }
                           });
        }
      }
 
      tm->wait(indexExtractGroup);
      tm->destroy(indexExtractGroup);
 
      elapsed_Extract += timerPass.elapsedSeconds();
    }
 
 
    if (beamMinorClosed) {
      TaskId remapGroup = tm->createGroup();
      const size_t NN = indexCount;
      const size_t chunk = std::max(indexCount / Threads::hardwareConcurrency(), size_t(16 * granularity));
      for (size_t i = 0; i < NN; i += chunk) {
        size_t j = std::min(i + chunk, NN);
        tm->enqueueChild(remapGroup,
                         [&indices, &indexRemapping, i, j]() {
                           CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "DoIndexRemapping");
                           for (size_t k = i; k < j; k++) {
                             indices[k] = indexRemapping[indices[k]];
                           }
                         });
      }
      tm->wait(remapGroup);
      tm->destroy(remapGroup);
    }
 
    // PASS: Normals and bounding box
    {
      CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "NormalsBBox");
      timerPass.start();
 
      TaskId group = tm->createGroup();
      tm->enqueueChild(group, [&]
      {
        auto * indicesPtr = indices.data();
        GeometryProcessing::normalsFromIndexedTriangles(context,
                                                        reinterpret_cast<float*>(N.data), 3,
                                                        reinterpret_cast<const float*>(P.data), 3,
                                                        vertexOffsets.back(),
                                                        indicesPtr,
                                                        indexOffsets.back(),
                                                        taskSize_Normals);
      });
 
      tm->enqueueChild(group, [&]
      {
        bbox = Cogs::Geometry::makeEmptyBoundingBox<Cogs::Geometry::BoundingBox>();
        includeInBBox(context,
                      bbox,
                      reinterpret_cast<const float*>(P.data), 3,
                      vertexOffsets.back(),
                      taskSize_BBox);
      });
 
      tm->wait(group);
      tm->destroy(group);
 
      elapsed_Normals += timerPass.elapsedSeconds();
    }
 
    // PASS: Setup mesh
    {
      CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "SetupMesh");
      timerPass.start();
 
      //mesh->set(VertexDataType::Interleaved0, VertexFormats::Pos3fNorm3fTex2f, PNT.data(), PNT.data() + PNT.size());
      //mesh->setCount(PNT.size());
      mesh->setBounds(bbox);
      mesh->setIndexData(indices.data(), indexOffsets.back());
      auto subMeshes = mesh->mapSubMeshes(layers);
      for (uint32_t l = 0; l < layers; l++) {
        auto & subMesh = subMeshes[l];
        subMesh.startIndex = indexOffsets[l];
        subMesh.numIndexes = indexOffsets[l + 1] - indexOffsets[l];
        subMesh.primitiveType = Cogs::PrimitiveType::TriangleList;
      }
      mesh->setMeshFlag(MeshFlags::ClockwiseWinding);
 
      elapsed_SetupMesh += timerPass.elapsedSeconds();
    }
  }
 
 
  // PASS: Store mesh.
  {
    CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "Store");
    timerPass.start();
 
    std::lock_guard<std::mutex> lock(persistent->mutex);
    persistent->values.clear();
    persistent->linearValues.clear();
    persistent->volumeCases.clear();
    persistent->volumeOffsets.clear();
    persistent->wallCases.clear();
    persistent->wallOffsets.clear();
    persistent->tmpValues.clear();
    persistent->issuedMesh = mesh.getHandle();
    persistent->issuedPosition = metaPing.vesselPositionGlobal;
    persistent->issuedOrientation = metaPing.vesselOrientationGlobal;
    persistent->runningTasks--;
 
    elapsed_Store += timerPass.elapsedSeconds();
  }
 
  elapsed_Total += timerTotal.elapsedSeconds();
  elapsed_Count++;
 
#ifdef COGS_AUTO_PROFILE
  if (2.0 < elapsed_Total) {
    double total = elapsed_Total / elapsed_Count;
    double resize = elapsed_Resample / elapsed_Count;
    double linearize = elapsed_Linearize / elapsed_Count;
    double depthResample = elapsed_DepthResample / elapsed_Count;
    double beamResample = elapsed_BeamResample / elapsed_Count;
    double analyze = elapsed_Analyze / elapsed_Count;
    double extract = elapsed_Extract / elapsed_Count;
    double normals = elapsed_Normals / elapsed_Count;
    double setupmesh = elapsed_SetupMesh / elapsed_Count;
    double store = elapsed_Store / elapsed_Count;
    LOG_DEBUG(logger, "N=%d TOT=%.1f RS=%.1f L=%.1f DR=%.1f BR=%.1f A=%.1f E=%.1f N=%.3f M=%.1f S=%.1f",
              elapsed_Count,
              1000.0*total,
              1000.0*resize,
              1000.0*linearize,
              1000.0*depthResample,
              1000.0*beamResample,
              1000.0*analyze,
              1000.0*extract,
              1000.0*normals,
              1000.0*setupmesh,
              1000.0*store);
    elapsed_Count = 0;
    elapsed_Total = 0.0;
    elapsed_Resample = 0.0;
    elapsed_Linearize = 0.0;
    elapsed_DepthResample = 0.0;
    elapsed_BeamResample = 0.0;
    elapsed_Analyze = 0.0;
    elapsed_Extract = 0.0;
    elapsed_Normals = 0.0;
    elapsed_SetupMesh = 0.0;
    elapsed_Store = 0.0;
  }
#endif
}