UniformGridSystem.cpp

#define _USE_MATH_DEFINES
#ifdef _WIN32
#include <winsock2.h>
#include <intrin.h>
#elif (defined(__GNUC__) || defined(__clang__)) && !defined(EMSCRIPTEN)
#include <cpuid.h>
#endif
 
#include "UniformGridSystem.h"
 
#include "../Components/UniformGridIsoComponent.h"
 
#include "Context.h"
#include "EntityStore.h"
#include "Resources/ResourceStore.h"
#include "Resources/MaterialManager.h"
#include "Resources/MeshManager.h"
#include "Resources/TextureManager.h"
#include "Components/Core/SceneComponent.h"
#include "Components/Core/TransformComponent.h"
#include "Components/Core/MeshComponent.h"
#include "Components/Core/MeshRenderComponent.h"
#include "Components/Core/SubMeshRenderComponent.h"
 
#include "Resources/Mesh.h"
#include "Services/Features.h"
 
#include "../BeamUtils.h"
 
#include "Foundation/Logging/Logger.h"
#include "Foundation/Platform/Timer.h"
 
#include <map>
#include <queue>
#include <chrono>
 
namespace {
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("UniformGridSystem");
  struct CpuId
  {
    CpuId() = delete;
    CpuId(uint32_t level)
    {
#ifdef _MSC_VER
      int ret[4];
      __cpuid(ret, level);
      eax = ret[0];
      ebx = ret[1];
      ecx = ret[2];
      edx = ret[3];
#elif defined(__GNUC__) || defined(__clang__)
      __get_cpuid(level, &eax, &ebx, &ecx, &edx);
#else
#error "implement cpuid for this compiler"
#endif
    }
    uint32_t eax, ebx, ecx, edx;
  };
 
  float dbToLinear(float v)
  {
    float w = powf(10.0f, v*(1.0f/20.0f));
    if (!isfinite(w)) {
      w = v < 0.f ? 0.f : std::numeric_limits<float>::max();
    }
    return w;
  }
}
 
namespace Cogs::Core::EchoSounder {
 
  void sampleTile_border_sse41(float * data,
                               const float *v,
                               const glm::vec3 tileIndex,
                               const glm::uvec3 tilePos,
                               const glm::uvec3 dataSize,
                               const glm::uvec3 maxIndices,
                               const glm::vec3 tp,
                               const glm::vec3 scale,
                               const glm::vec3 arrayPositionGlobal,
                               const glm::vec4* frustum,
                               const float minDistanceSquared,
                               const float maxDistanceSquared,
                               const glm::quat inverseOrientation,
                               const uint32_t coordSys,
                               const uint32_t minorCount,
                               const uint32_t sampleCount,
                               const glm::vec3 polarScale,
                               const glm::vec3 polarShift);
 
  void sampleTile_inner_sse41(float * data,
                              const float *v,
                              const glm::vec3 tileIndex,
                              const glm::uvec3 tilePos,
                              const glm::uvec3 dataSize,
                              const glm::uvec3 maxIndices,
                              const glm::vec3 tp,
                              const glm::vec3 scale,
                              const glm::vec3 arrayPositionGlobal,
                              const glm::vec4* frustum,
                              const float minDistanceSquared,
                              const float maxDistanceSquared,
                              const glm::quat inverseOrientation,
                              const uint32_t coordSys,
                              const uint32_t minorCount,
                              const uint32_t sampleCount,
                              const glm::vec3 polarScale,
                              const glm::vec3 polarShift);
 
  void sampleTile_inner_fma_gather(float * data,
                                   const float *v,
                                   const glm::vec3 tileIndex,
                                   const glm::uvec3 tilePos,
                                   const glm::uvec3 dataSize,
                                   const glm::uvec3 maxIndices,
                                   const glm::vec3 tp,
                                   const glm::vec3 scale,
                                   const glm::vec3 arrayPositionGlobal,
                                   const glm::vec4* frustum,
                                   const float minDistanceSquared,
                                   const float maxDistanceSquared,
                                   const glm::quat inverseOrientation,
                                   const uint32_t coordSys,
                                   const uint32_t minorCount,
                                   const uint32_t sampleCount,
                                   const glm::vec3 polarScale,
                                   const glm::vec3 polarShift);
 
  void sampleTile3_avx2(float * data,
                        float& minVal,
                        float& maxVal,
                        const float *v,
                        const glm::vec3 tileIndex,
                        const glm::uvec3 tilePos,
                        const glm::uvec3 dataSize,
                        const glm::uvec3 maxIndices,
                        const glm::vec3 tp,
                        const glm::vec3 scale,
                        const glm::vec3 arrayPositionGlobal,
                        const glm::vec4* frustum,
                        const float minDistanceSquared,
                        const float maxDistanceSquared,
                        const glm::quat inverseOrientation,
                        const uint32_t coordSys,
                        const uint32_t minorCount,
                        const uint32_t sampleCount,
                        const glm::vec3 polarScale,
                        const glm::vec3 polarShift);
 
  void sampleTile3_border_avx2(float * data,
                               float& minVal,
                               float& maxVal,
                               const float *v,
                               const glm::vec3 tileIndex,
                               const glm::uvec3 tilePos,
                               const glm::uvec3 dataSize,
                               const glm::uvec3 maxIndices,
                               const glm::vec3 tp,
                               const glm::vec3 scale,
                               const glm::vec3 arrayPositionGlobal,
                               const glm::vec4* frustum,
                               const float minDistanceSquared,
                               const float maxDistanceSquared,
                               const glm::quat inverseOrientation,
                               const uint32_t coordSys,
                               const uint32_t minorCount,
                               const uint32_t sampleCount,
                               const glm::vec3 polarScale,
                               const glm::vec3 polarShift);
 
  MeshHandle createIsoSurfacesReference(Context* context,
                                        uint64_t& analyze,
                                        uint64_t& vtx,
                                        uint64_t& idx,
                                        Cogs::Memory::MemoryBuffer& scratch,
                                        const float* values,
                                        const glm::uvec3 layout,
                                        const glm::vec3 minCorner,
                                        const glm::vec3 maxCorner,
                                        const float *thresholds, size_t count);
 
  MeshHandle createIsoSurfacesAVX2(Context* context,
                                   uint64_t& analyze,
                                   uint64_t& vtx,
                                   uint64_t& idx,
                                   Cogs::Memory::MemoryBuffer& scratch,
                                   const float* values,
                                   const glm::uvec3 gridLayout,
                                   const glm::vec3 minCorner,
                                   const glm::vec3 maxCorner,
                                   const float *thresholds, size_t count);
 
  MeshHandle createIsoSurfacesSplitAVX2(Context* context,
                                        uint64_t& analyze,
                                        uint64_t& vtx,
                                        uint64_t& idx,
                                        Cogs::Memory::MemoryBuffer& scratch,
                                        const float* values,
                                        const glm::uvec3 gridLayout,
                                        const glm::vec3 minCorner,
                                        const glm::vec3 maxCorner,
                                        const float *thresholds, size_t count);
 
  MeshHandle UniformGridData::createIsoSurfaces(Tile & tile, Cogs::Memory::MemoryBuffer& scratch, const glm::vec3 minCorner)
  {
    Context* context = system->context;
 
    MeshHandle mesh = MeshHandle::NoHandle;
    float *thresholds = this->thresholds.data();
    size_t count = this->thresholds.size();
    std::vector<float> t(count);
    for(uint32_t i=0; i<count; i++) t[i] = dbToLinear(thresholds[i]);
    if (!count || tile.maxValue <= t[0] || t[count-1] <= tile.minValue) return mesh;
 
    const glm::vec3 maxCorner = minCorner + glm::vec3(size)*scale;
 
    uint64_t analyze = 0;
    uint64_t vtx = 0;
    uint64_t idx = 0;
    switch (compute) {
    case ComputeType::Reference:
    case ComputeType::SSE41:
    case ComputeType::FMA:
      mesh = createIsoSurfacesReference(context, analyze, vtx, idx,
                                        scratch,
                                        tile.data, size,
                                        minCorner, maxCorner,
                                        t.data(), count);
      break;
    case ComputeType::AVX2:
#ifdef USE_SPLIT
      mesh = createIsoSurfacesSplitAVX2(context, analyze, vtx, idx,
                                        scratch,
                                        tile.data, size,
                                        minCorner, maxCorner,
                                        t.data(), count);
      break;
#endif
      mesh = createIsoSurfacesAVX2(context, analyze, vtx, idx,
                                   scratch,
                                   tile.data, size,
                                   minCorner, maxCorner,
                                   t.data(), count);
      break;
    }
    isoSurf.analyze.fetch_add(uint32_t(analyze));
    isoSurf.vtx.fetch_add(uint32_t(vtx));
    isoSurf.idx.fetch_add(uint32_t(idx));
    isoSurf.N.fetch_add(1);
    return mesh;
  }
  glm::vec3 getSamplePosRef(const glm::uvec3 /*dataSize*/,
                            const glm::vec3 tilePos,
                            const glm::vec3 scale,
                            const glm::vec3 arrayPositionGlobal,
                            const glm::vec4* frustum,
                            const float minDistanceSquared,
                            const float maxDistanceSquared,
                            const glm::quat inverseOrientation,
                            const uint32_t coordSys,
                            const glm::vec3 polarScale,
                            const glm::vec3 polarShift,
                            const uint32_t x,
                            const uint32_t y,
                            const uint32_t z)
  {
    glm::vec3 p = scale * glm::vec3(x, y, z);
    glm::vec3 q = (tilePos - arrayPositionGlobal) + p; // Sample pos with origin in beam bundle origin.
    float r2 = glm::dot(q, q);
 
    bool in_0 = 0.f <= glm::dot(glm::vec3(frustum[0]), q);
    bool in_1 = 0.f <= glm::dot(glm::vec3(frustum[1]), q);
    bool in_2 = 0.f <= glm::dot(glm::vec3(frustum[2]), q);
    bool in_3 = 0.f <= glm::dot(glm::vec3(frustum[3]), q);
    bool in_4 = minDistanceSquared <= r2;
    bool in_5 = r2 <= maxDistanceSquared;
    if (!in_0 || !in_1 || !in_2 || !in_3 || !in_4 || !in_5) return glm::vec3(-1.f);
 
    glm::vec3 a = inverseOrientation * q;
    float r = std::sqrt(r2);
 
    glm::vec3 polarPos;
    if (coordSys == 0) {
      float dirX = std::asin(a.x / r);
      float dirY = std::asin(a.y / r);
      polarPos = glm::vec3(dirY, dirX, r);
    }
    else if (coordSys == 1) {
      float dirX = std::asin(a.x / r);
      float dirY = std::atan(a.y / a.z);
      polarPos = glm::vec3(dirY, dirX, a.z);
    }
    glm::vec3 xi = glm::max(glm::vec3(0.f), polarScale*(polarPos - polarShift));
 
    return xi;
  }
}// namespace ...
void Cogs::Core::EchoSounder::UniformGridSystem::initialize(Context * context)
{
  ComponentSystemWithDataPool::initialize(context);
 
  CpuId level1(0x00000001u);
  has_sse41 = level1.ecx & (1u << 19u);
  bool osxsave = level1.ecx & (1u << 27u);
  has_avx2 = false;
  if(osxsave){
    CpuId level7(0x00000007u);
    has_avx2 = level7.ebx & (1u << 5u);
  }
  if(has_sse41) LOG_INFO(logger, "Has SSE41");
  if(has_avx2) LOG_INFO(logger, "Has AVX2");
 
  Queue = context->taskManager->createQueue("EchoSounder", Threads::hardwareConcurrency()-1);
 
  point_material = context->materialManager->loadMaterial("UniformGridPoint.material");
  context->materialManager->processLoading();
  point_instance = context->materialInstanceManager->createMaterialInstance(point_material);
  float pointSize = 16.0f;
  point_instance->setProperty("pointSize", &pointSize, sizeof(float));
  point_instance->setOption("BlendMode", "Blend");
  point_instance->setupInstance(point_material.resolve());
}
 
void Cogs::Core::EchoSounder::UniformGridSystem::preUpdate(Context * context)
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::preUpdate");
  static std::vector<uint32_t> pointTransferData = {
    0x00888888, 0xff888888, 0xff888888, 0xff888888,
    0xffFF3300, 0xffFF3300, 0xffFF3300, 0xffFF3300,
    0xff00DD00, 0xff00DD00, 0xff00DD00, 0xff00DD00,
    0xFF0000FF, 0xFF0000FF, 0xFF0000FF, 0xFF0000FF,
  };
  for (auto & component : pool) {
    UniformGridData &data = getData(&component);
    bool update_all = data.decibelMin != component.decibelMin || data.decibelMax != component.decibelMax;
    if(!data.visualizer_group){
      data.visualizer_group = context->store->createChildEntity("Group", component.getContainer(), "VisualizerGroup");
    }
    if(update_all){
      data.decibelMin = component.decibelMin;
      data.decibelMax = component.decibelMax;
    }
    update_outline(context, &component);
 
    data.compute = component.compute; // TODO
 
    // UniformGridIsoComponent
    UniformGridIsoComponent *iso_component = component.getContainer()->getComponent<UniformGridIsoComponent>();
    if(iso_component){
      if(iso_component->thresholds.size() != data.thresholds.size()){
        data.thresholds = iso_component->thresholds;
        update_all = true;
      }
      for(size_t i=0; i<iso_component->thresholds.size(); i++){
        if(iso_component->thresholds[i] != data.thresholds[i]){
          data.thresholds = iso_component->thresholds;
          update_all = true;
          break;
        }
      }
      {
        MaterialInstanceHandle &instance = iso_component->material;
        const float fmin = std::numeric_limits<float>::min();
        glm::vec4 range(iso_component->uTextureRange.x,
                        1.f / std::max(fmin, iso_component->uTextureRange.y - iso_component->uTextureRange.x),
                        iso_component->vTextureRange.x,
                        1.f / std::max(fmin, iso_component->vTextureRange.y - iso_component->vTextureRange.x));
        instance->setVec4Property(instance->material->getVec4Key("range"), range);
        instance->setFloatProperty(instance->material->getFloatKey("noiseIntensity"), 0.0f);
      }
      size_t count = iso_component->thresholds.size();
      if(!data.layerMaterialInstances.size()) data.layerMaterialInstances.resize(count);
      for(uint32_t i=0; i<count; i++){
        MaterialInstanceHandle &instance = data.layerMaterialInstances[i];
        if(!instance) instance = context->materialInstanceManager->createMaterialInstance(iso_component->material);
        instance->clone(iso_component->material.resolve());
        if(i == count-1 && iso_component->innerLayerOpaque){
          instance->options.depthWriteEnabled = true;
          instance->setOpaque();
        }
        else{
          instance->setTransparent();
          instance->options.depthWriteEnabled = false;
        }
        //instance->setTextureProperty(instance->material->getTextureKey("ColorMap"), tex);
        instance->setTextureAddressMode("ColorMap", "Clamp");
        const float fmin = std::numeric_limits<float>::min();
        glm::vec4 range(iso_component->uTextureRange.x,
                        1.f / std::max(fmin, iso_component->uTextureRange.y - iso_component->uTextureRange.x),
                        iso_component->vTextureRange.x,
                        1.f / std::max(fmin, iso_component->vTextureRange.y - iso_component->vTextureRange.x));
        instance->setVec4Property(instance->material->getVec4Key("range"), range);
        instance->setFloatProperty(instance->material->getFloatKey("noiseIntensity"), 0.0f);
      }
    }
 
    std::unique_lock<decltype(data.tiles_mu)> lock(data.tiles_mu, std::defer_lock);
    if(!lock.try_lock()) continue;
    auto group = context->taskManager->createGroup(Cogs::Core::TaskManager::GlobalQueue);
    for(auto &tmp : data.destroy)
      context->store->removeChild(data.visualizer_group.get(), tmp);
    data.destroy.clear();
    for(auto &tmp : data.tiles){
      TileKey key = tmp.first;
      Tile &tile = tmp.second;
#if 1
      if(update_all) data.change_sv = true;
      if(tile.update && HandleIsValid(tile.mesh) && tile.mesh->isActive() && iso_component){
        tile.update = false;
        if (!tile.visualizer) {
          tile.visualizer = context->store->createChildEntity("EchoUniformGridIsoVisualizer", data.visualizer_group.get(), "UniformGridIsoVisualizer");
        }
        auto *tComp = tile.visualizer->getComponent<TransformComponent>();
        glm::uvec3 tileIndex(decodeTileKey(key));
        glm::vec3 tileCoord(glm::vec3(tileIndex)-glm::vec3(1<<15));
        glm::vec3 tilePos = tileCoord*(glm::vec3(data.size-glm::uvec3(1))*data.scale);
        tComp->position = tilePos;
        tComp->scale = data.scale;
        tComp->setChanged();
        auto *src = tile.visualizer->getComponent<SubMeshRenderComponent>();
        if (src) src->materials = data.layerMaterialInstances;
        auto *mc = tile.visualizer->getComponent<MeshComponent>();
        mc->meshHandle = tile.mesh;
        mc->setChanged();
      }
      if (tile.update && !HandleIsValid(tile.mesh)) {
        if (tile.visualizer) {
          auto *mc = tile.visualizer->getComponent<MeshComponent>();
          mc->meshHandle = MeshHandle::NoHandle;
          mc->setChanged();
          // FIXME: maybe maintain a pool of inactive entities so we don't have to create & delete so much.
          context->store->removeChild(data.visualizer_group.get(), tile.visualizer.get());
          tile.visualizer.reset();
        }
      }
#else
      if (tile.update || update_all) {
        tile.update = false;
        if (!tile.visualizer) {
          tile.visualizer = context->store->createChildEntity("EchoUniformGridPointVisualizer", data.visualizer_group.get(), "UniformGridPointVisualizer");
        }
        auto task = [this, &context, &data, key, &tile]() {
                      CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::pointVizTask");
                      float valueMin = dbToLinear(data.decibelMin);
                      float valueMax = dbToLinear(data.decibelMax);
                      float valueScale = 1.0f / (valueMax - valueMin);
                      std::vector<glm::vec3> positions;
                      std::vector<glm::vec4> colors;
                      size_t t_size = pointTransferData.size();
                      float cinv = 1.0f / 255.0f;
                      for (uint32_t z = 0; z < data.size.z; z += viz_z_div) {
                        for (uint32_t y = 0; y < data.size.y; y += viz_y_div) {
                          for (uint32_t x = 0; x < data.size.x; x += viz_x_div) {
                            uint32_t index = z * data.size.y*data.size.x + y * data.size.x + x;
                            float val = glm::clamp<float>(valueScale*tile.data[index], 0.0f, 1.0f);
                            float vali = val * (t_size - 1);
                            float a = vali - floor(vali);
                            uint32_t il = glm::clamp<uint32_t>((uint32_t)floor(vali), 0, t_size);
                            uint32_t iu = glm::clamp<uint32_t>((uint32_t)ceil(vali), 0, t_size);
                            uint32_t cl = pointTransferData[il];
                            uint32_t cu = pointTransferData[iu];
                            glm::vec4 coll(((cl>>0)&0xff)*cinv, ((cl>>8)&0xff)*cinv,
                                           ((cl>>16)&0xff)*cinv, ((cl>>24)&0xff)*cinv);
                            glm::vec4 colu(((cu>>0)&0xff)*cinv, ((cu>>8)&0xff)*cinv,
                                           ((cu>>16)&0xff)*cinv, ((cu>>24)&0xff)*cinv);
                            glm::vec4 color = glm::mix(coll, colu, a);
                            colors.push_back(color);
                            positions.push_back(glm::vec3(x, y, z)*data.scale);
                          }
                        }
                      }
                      auto *tComp = tile.visualizer->getComponent<TransformComponent>();
                      glm::uvec3 tileIndex(decodeTileKey(key));
                      glm::vec3 tileCoord(glm::vec3(tileIndex)-glm::vec3(1<<15));
                      glm::vec3 tilePos = tileCoord*(glm::vec3(data.size-glm::uvec3(1))*data.scale);
                      tComp->position = tilePos;
                      auto *rc = tile.visualizer->getComponent<MeshRenderComponent>();
                      rc->material = point_instance;
                      auto *mc = tile.visualizer->getComponent<MeshComponent>();
                      auto mesh = context->meshManager->createLocked();
                      mesh->clear();
                      mesh->set(VertexDataType::Positions, &VertexFormats::Pos3f, positions.data(), positions.data() + positions.size());
                      mesh->set(VertexDataType::Colors0, &VertexFormats::Color4f, colors.data(), colors.data() + colors.size());
                      mesh->primitiveType = Cogs::PrimitiveType::PointList;
                      mc->meshHandle = mesh.getHandle();
                    };
        context->taskManager->enqueueChild(group, task);
      }
#endif
    }
    context->taskManager->wait(group);
  }
}
void Cogs::Core::EchoSounder::UniformGridSystem::update_outline(Context * context, UniformGridComponent *component)
{
  UniformGridData &data = getData(component);
  if (!component->show_frustum){
    for(auto &tmp : data.boundingFrustum){
      EntityPtr entity = tmp.second;
      auto * meshComp = entity->getComponent<MeshComponent>();
      meshComp->meshHandle = MeshHandle::NoHandle;
      meshComp->setChanged();
    }
    return;
  }
  for(auto &tmp : data.config){
    uint32_t id = tmp.first;
    UniformGridConfig &config(*tmp.second);
 
    if(!data.boundingFrustum.count(id)){
      Cogs::Core::EntityPtr entity = context->store->createChildEntity("MeshPart", component->getContainer(), "Frustum");
      data.boundingFrustum[id] = entity;
 
      auto mat = context->materialInstanceManager->createMaterialInstance(context->materialManager->getDefaultMaterial());
      mat->setPermutation("Line");
      mat->setVariant("LightModel", "BaseColor");
      mat->setVariant("EnableLighting", false);
      mat->setVariant("VertexColor", true);
 
      auto *matRndComp = entity->getComponent<MeshRenderComponent>();
      matRndComp->material = mat;
      matRndComp->setRenderFlag(RenderFlags::CustomMaterial);
      matRndComp->setChanged();
    }
    Cogs::Core::EntityPtr entity = data.boundingFrustum[id];
 
    auto * trComp = entity->getComponent<TransformComponent>();
    trComp->rotation = data.vesselOrientationGlobal;
    trComp->position = data.vesselPositionGlobal;
    trComp->setChanged();
 
    glm::quat arrayOrientationVessel;
    glm::vec3 arrayPositionVessel;
    getArrayToVesselTransform(arrayOrientationVessel,
                              arrayPositionVessel,
                              config.transducerAlpha,
                              config.transducerOffset);
 
    float minDistance = config.depthOffset;
    float maxDistance = minDistance + config.sampleCount * config.depthStep;
 
    uint32_t beamCount = config.majorCount*config.minorCount;
 
    float minDirectionX = std::numeric_limits<float>::max();
    float maxDirectionX = -std::numeric_limits<float>::max();
    float minDirectionY = std::numeric_limits<float>::max();
    float maxDirectionY = -std::numeric_limits<float>::max();
    float maxBeamWidthX = -std::numeric_limits<float>::max();
    float maxBeamWidthY = -std::numeric_limits<float>::max();
    for(uint32_t i=0; i<beamCount; i++){
      maxDirectionX = glm::max(maxDirectionX, config.directionX[i]);
      minDirectionX = glm::min(minDirectionX, config.directionX[i]);
      maxDirectionY = glm::max(maxDirectionY, config.directionY[i]);
      minDirectionY = glm::min(minDirectionY, config.directionY[i]);
      maxBeamWidthX = glm::max(maxBeamWidthX, config.beamWidthX[i]);
      maxBeamWidthY = glm::max(maxBeamWidthY, config.beamWidthY[i]);
    }
 
    if (config.majorCount == 1) {
      minDirectionX -= 0.5f*maxBeamWidthX;
      maxDirectionX += 0.5f*maxBeamWidthX;
    }
    if (config.minorCount == 1) {
      minDirectionY -= 0.5f*maxBeamWidthY;
      maxDirectionY += 0.5f*maxBeamWidthY;
    }
 
    auto * meshComp = entity->getComponent<MeshComponent>();
    glm::vec4 frustum[6];
    getBoundingFrustum(frustum,
                       arrayOrientationVessel,
                       arrayPositionVessel,
                       config.coordSys,
                       minDirectionX, maxDirectionX,
                       minDirectionY, maxDirectionY,
                       minDistance, maxDistance);
    meshComp->meshHandle = buildFrustumOutline(context, frustum);
    meshComp->setChanged();
  }
}
void Cogs::Core::EchoSounder::UniformGridSystem::config(UniformGridComponent *component, UniformGridConfig &config)
{
  UniformGridData &data = getData(component);
  std::unique_lock<decltype(data.mu)> lock(data.mu);
  uint32_t id = config.configId;
  data.config[id] = std::make_shared<UniformGridConfig>(std::move(config));
}
void Cogs::Core::EchoSounder::UniformGridSystem::clear(UniformGridComponent *component)
{
  UniformGridData &data = getData(component);
  std::unique_lock<decltype(data.mu)> lock(data.mu); // TODO: tiles_mu
  for(auto &tmp : data.tiles){
    _aligned_free(tmp.second.data);
    Entity *ptr = tmp.second.visualizer.get();
    if(ptr) context->store->removeChild(data.visualizer_group.get(), ptr);
  }
  data.tiles.clear();
}
float Sinc(float x)
{
  constexpr float PI = float(M_PI);
  if(x < 1e-5) return 1.0f;
  return sinf(PI*x)/(PI*x);
}
float Lanczos(float x, float tau)
{
  return Sinc(x) * Sinc(x/tau);
}
void Cogs::Core::EchoSounder::UniformGridSampleData::filter_ping()
{
  float w = filter_width;
  float r = w*0.5f;
  float a = filter_falloff;
  int32_t iwidth = 1 + int32_t(floor(r))*2;
  int32_t ihalf = iwidth/2;
  if(iwidth < 3 || a == 0.0f) return;
 
  std::vector<float> weights(iwidth, 0.0f);
  for(int32_t t=0; t<iwidth; t++){
    int32_t x = t-ihalf;
    x = abs(x);
    // Box:
    // weights[t] = 1.0f;
    // Triangle:
    // Gaussian:
    weights[t] = std::exp(-a*(x*x))-std::exp(-a*(r*r));
    // Mitchell:
    // Lanczos:
    // weights[t] = Lanczos((float)x/r, tau);
    // TODO: Down/Upsample
  }
  double sum = 0.0f;
  for(int32_t t=0; t<iwidth; t++)
    sum += weights[t];
  for(int32_t t=0; t<iwidth; t++)
    weights[t] = (float)(weights[t]/sum);
  assert(std::isnormal(sum));
  Memory::TypedBuffer<float> linear_resampled;
  uint32_t beamCount = config->majorCount*config->minorCount;
  uint32_t sampleCount = config->sampleCount;
  linear_resampled.resize(beamCount*sampleCount, false);
  for(uint32_t b=0; b<beamCount; b++){
    uint32_t off = b*sampleCount;
    for(uint32_t s=0; s<sampleCount; s++){
      float avg;
      {
        int32_t st = glm::clamp((int32_t)s-ihalf, (int32_t)0, (int32_t)sampleCount-1);
        avg = linear[off+st]*weights[0];
      }
      for(int32_t t=1; t<iwidth; t++){
        int32_t st = glm::clamp((int32_t)s+t-ihalf, (int32_t)0, (int32_t)sampleCount-1);
        avg += linear[off+st]*weights[t];
      }
      linear_resampled[off+s] = avg;
    }
  }
  data.sample_data->linear.swap(linear_resampled);
}
ComponentHandle Cogs::Core::EchoSounder::UniformGridSystem::createComponent()
{
  ComponentHandle handle = ComponentSystemWithDataPool::createComponent();
  UniformGridComponent *component = handle.resolveComponent<UniformGridComponent>();
  UniformGridData &data = getData(component);
  data.system = this;
  data.component = component;
  Context *con = context;
  auto task = [this, con, component](){ this->dispatch(con, component); };
  context->taskManager->enqueue(Queue, task);
  return handle;
}
void Cogs::Core::EchoSounder::UniformGridSystem::destroyComponent(ComponentHandle handle)
{
  UniformGridComponent *component = handle.resolveComponent<UniformGridComponent>();
  UniformGridData &data = getData(component);
  data.shutdown = true;
  {
    std::unique_lock<decltype(data.mu)> lock(data.mu);
    data.cv.wait(lock);
  }
  ComponentSystemWithDataPool::destroyComponent(handle);
}
void Cogs::Core::EchoSounder::UniformGridSystem::dispatch(Context *context, UniformGridComponent *component)
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::dispatch");
  UniformGridData &data = getData(component);
 
  // Update iso-surfaces
  if(data.change_sv || data.update_iso){
    std::unique_lock<decltype(data.tiles_mu)> lock(data.tiles_mu);
    if(data.change_sv){
      data.change_sv = false;
      for(auto &tmp1 : data.tiles)
        tmp1.second.update_iso = true;
      data.update_iso = true;
    }
    data.update_iso = data.update_isosurface();
  }
 
  // Add remove tiles
  if(data.sample_data){
    using clock = std::chrono::high_resolution_clock;
    auto t0 = clock::now();
    {
      std::unique_lock<decltype(data.tiles_mu)> lock(data.tiles_mu);
      if((component->size != data.size || component->scale != data.scale) &&
         component->scale.x > 0.0f && component->scale.y > 0.0f && component->scale.z > 0.0f){
        data.size = component->size;
        data.scale = component->scale;
 
        for(auto &tmp : data.tiles){
          _aligned_free(tmp.second.data);
          Entity *ptr = tmp.second.visualizer.get();
          if(ptr) data.destroy.push_back(ptr);
        }
        data.tiles.clear();
      }
      else{
        data.sample_data->evict_tiles();
      }
      data.sample_data->cull_tiles();
      data.sample_data->gen_tiles();
    }
 
    // Wait for linear filtering
    context->taskManager->wait(data.sample_data->linear_group);
    data.sample_data->filter_ping();
 
    auto group = context->taskManager->createGroup(Queue);
    size_t concurrency = Threads::hardwareConcurrency()-1;
    concurrency*=4;
    if(data.sample_data->tiles_inside.size()){
      size_t tile_count = (size_t)data.sample_data->tiles_inside.size();
      size_t task_count = std::min(tile_count, concurrency);
      size_t steps = tile_count/task_count+1;
      for(size_t i=0; i<tile_count; i+=steps){
        size_t end = std::min(i+steps, tile_count);
        auto task = [sample_data=data.sample_data, i, end](){ sample_data->sample_inside(i, end); };
        context->taskManager->enqueueChild(group, task);
      }
    }
    if(data.sample_data->tiles_intersecting.size()){
      size_t tile_count = (size_t)data.sample_data->tiles_intersecting.size();
      size_t task_count = std::min(tile_count, concurrency);
      size_t steps = tile_count/task_count+1;
      for(size_t i=0; i<tile_count; i+=steps){
        size_t end = std::min(i+steps, tile_count);
        auto task = [sample_data=data.sample_data, i, end](){ sample_data->sample_intersecting(i, end); };
        context->taskManager->enqueueChild(group, task);
      }
    }
    context->taskManager->wait(group);
 
    {
      const char* s = "REF";
      switch (data.sample_data->compute) {
      case ComputeType::SSE41: s = "SSE41"; break;
      case ComputeType::FMA: s = "FMA"; break;
      case ComputeType::AVX2: s = "AVX2"; break;
      default: break;
      }
 
      float N_tot = (float)(data.sample_intersecting.N + data.sample_inside.N);
      if (data.runs == 9) {
        auto threadTimeToWallTime = 1.f / (Threads::hardwareConcurrency() - 1);
 
        LOG_DEBUG(logger, "%s: sample_b %.2fms (%.3fms) sample_i %.2fms (%.3fms) pop=%.1f%% iso %.2fms (%.3fms/%.3fms/%.3fms)",
                  s,
                  threadTimeToWallTime*(1.f / 1000.f) * data.sample_intersecting.us,
                  ((1.f / 1000.f) * data.sample_intersecting.us) / data.sample_intersecting.N,
                  threadTimeToWallTime*(1.f / 1000.f) * data.sample_inside.us,
                  ((1.f / 1000.f) * data.sample_inside.us) / data.sample_inside.N,
                  (100.f*data.isoSurf.N) / N_tot,
                  threadTimeToWallTime*(1.f / 1000.f) *(data.isoSurf.analyze + data.isoSurf.vtx + data.isoSurf.idx),
                  ((1.f / 1000.f) * data.isoSurf.analyze) / data.isoSurf.N,
                  ((1.f / 1000.f) * data.isoSurf.vtx) / data.isoSurf.N,
                  ((1.f / 1000.f) * data.isoSurf.idx) / data.isoSurf.N);
 
        data.sample_intersecting.us = 0;
        data.sample_intersecting.N = 0;
        data.sample_inside.us = 0;
        data.sample_inside.N = 0;
        data.isoSurf.analyze = 0;
        data.isoSurf.vtx = 0;
        data.isoSurf.idx = 0;
        data.isoSurf.N = 0;
        data.runs = 0;
      }
      else data.runs++;
    }
    auto t1 = clock::now();
    LOG_DEBUG(logger, "add_point %.4f s (%.2f ms) size (%d, %d, %d) scale (%.3f, %.3f, %.3f)",
              std::chrono::duration<float>(t1-t0).count(),
              std::chrono::duration<float, std::milli>(t1-t0).count(),
              data.size.x, data.size.y, data.size.z,
              data.scale.x, data.scale.y, data.scale.z);
 
    std::unique_lock<decltype(data.mu)> lock(data.mu);
    delete data.sample_data;
    data.sample_data = nullptr;
    if(data.sample_data_backbuffer){
      data.sample_data = data.sample_data_backbuffer;
      data.vesselOrientationGlobal = data.sample_data->vesselOrientationGlobal;
      data.vesselPositionGlobal = data.sample_data->vesselPositionGlobal;
      data.sample_data_backbuffer = nullptr;
    }
  }
 
  if(!data.shutdown){
    if(!data.update_iso) std::this_thread::sleep_for(std::chrono::milliseconds(8)); // TODO conditon variable
    auto task = [this, context, component](){ this->dispatch(context, component); };
    context->taskManager->enqueue(Queue, task);
  }
  else{
    data.cv.notify_all();
  }
}
void Cogs::Core::EchoSounder::UniformGridSystem::addPing(Context * context, UniformGridComponent *component, UniformGridPing &ping)
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::addPing");
  uint64_t ping_number;
  UniformGridData &data = getData(component);
 
  std::unique_lock<decltype(data.mu)> lock(data.mu);
  ping_number = data.ping_number++;
  if(data.sample_data && data.sample_data_backbuffer){
    CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::drop_ping");
    data.pings_droped++;
    LOG_DEBUG(logger, "Dropping ping (Handled: %llu Droped: %llu)", data.pings_handled, data.pings_droped);
    return;
  }
  data.pings_handled++;
  UniformGridSampleData *sample_data = new UniformGridSampleData(context, this, component, data, ping, ping_number);
  if(!data.sample_data){
    data.sample_data = sample_data;
    data.vesselOrientationGlobal = sample_data->vesselOrientationGlobal;
    data.vesselPositionGlobal = sample_data->vesselPositionGlobal;
  }
  else{
    data.sample_data_backbuffer = sample_data;
  }
}
Cogs::Core::EchoSounder::UniformGridSampleData::UniformGridSampleData(Context *context, UniformGridSystem *system, UniformGridComponent *component, UniformGridData &data, UniformGridPing &ping, uint64_t ping_number):
  context(context),
  system(system),
  component(component),
  data(data),
  config(data.config[ping.configId]),
  average_alpha(component->average_alpha),
  filter_width(component->filter_width),
  filter_falloff(component->filter_falloff),
  ping_number(ping_number)
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::UniformGridSampleData");
 
  if(component->compute == ComputeType::AVX2 && !system->has_avx2)
    component->compute = ComputeType::SSE41;
  if(component->compute == ComputeType::AVX2 && !system->has_sse41)
    component->compute = ComputeType::Reference;
  compute = component->compute;
 
  vesselOrientationGlobal = ping.orientation;
  vesselPositionGlobal = ping.position;
  glm::quat arrayOrientationVessel;
  glm::vec3 arrayPositionVessel;
  getArrayToVesselTransform(arrayOrientationVessel,
                            arrayPositionVessel,
                            config->transducerAlpha,
                            config->transducerOffset);
  arrayOrientationGlobal = vesselOrientationGlobal * arrayOrientationVessel;
  arrayPositionGlobal = vesselPositionGlobal + vesselOrientationGlobal * arrayPositionVessel;
 
  inverseOrientation = glm::inverse(arrayOrientationGlobal);
 
  minDistance = config->depthOffset;
  maxDistance = minDistance + config->sampleCount * config->depthStep;
 
  minDistanceSquared = minDistance * minDistance;
  maxDistanceSquared = maxDistance * maxDistance;
 
  majorCount = config->majorCount;
  minorCount = config->minorCount;
  uint32_t beamCount = majorCount * minorCount;
 
  float minDirectionX = std::numeric_limits<float>::max();
  float maxDirectionX = -std::numeric_limits<float>::max();
  float minDirectionY = std::numeric_limits<float>::max();
  float maxDirectionY = -std::numeric_limits<float>::max();
  float maxBeamWidthX = -std::numeric_limits<float>::max();
  float maxBeamWidthY = -std::numeric_limits<float>::max();
  for(uint32_t i = 0; i<beamCount; i++){
    maxDirectionX = glm::max(maxDirectionX, config->directionX[i]);
    minDirectionX = glm::min(minDirectionX, config->directionX[i]);
    maxDirectionY = glm::max(maxDirectionY, config->directionY[i]);
    minDirectionY = glm::min(minDirectionY, config->directionY[i]);
    maxBeamWidthX = glm::max(maxBeamWidthX, config->beamWidthX[i]);
    maxBeamWidthY = glm::max(maxBeamWidthY, config->beamWidthY[i]);
  }
 
  if (majorCount == 1) {
    minDirectionX -= 0.5f*maxBeamWidthX;
    maxDirectionX += 0.5f*maxBeamWidthX;
  }
  if (minorCount == 1) {
    minDirectionY -= 0.5f*maxBeamWidthY;
    maxDirectionY += 0.5f*maxBeamWidthY;
  }
 
  // Compute Ping Frustrum
  getBoundingFrustum(frustum,
                     arrayOrientationGlobal,
                     arrayPositionGlobal,
                     config->coordSys,
                     minDirectionX, maxDirectionX,
                     minDirectionY, maxDirectionY,
                     minDistance, maxDistance);
 
  maxIndices = glm::uvec3(glm::max(1u, minorCount) - 1u,
                          glm::max(1u, majorCount) - 1u,
                          glm::max(1u, config->sampleCount) - 1u);
  polarShift = glm::vec3(minDirectionY, minDirectionX, minDistance);
  polarScale = glm::vec3((glm::max(2u, minorCount) - 2u) / (maxDirectionY - minDirectionY),
                         (glm::max(2u, majorCount) - 2u) / (maxDirectionX - minDirectionX),
                         (glm::max(2u, config->sampleCount) - 2u) / (maxDistance - minDistance));
 
  // Calculate linear
  {
    size_t concurrency = Threads::hardwareConcurrency()-2;
    uint32_t sampleCount = config->sampleCount;
    uint32_t steps = (uint32_t)((beamCount*sampleCount)/concurrency+1);
    linear_group = context->taskManager->createGroup(system->Queue);
    decibel = std::move(ping.storage);
    linear.resize(beamCount*sampleCount, false);
    for(uint32_t i=0; i<beamCount*sampleCount; i+=steps){
      uint32_t end = std::min(i+steps, beamCount*sampleCount);
      auto task = [this, start=i, end](){
                    CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::dBToLinearTask");
                    float *db = (float*)decibel.data();
                    for(uint32_t i=start; i<end; i++){
                      linear[i] = glm::clamp(dbToLinear(db[i]), 0.0f, 1.0f);
                    }
                  };
      context->taskManager->enqueueChild(linear_group, task);
    }
  }
}
enum{
     INSIDE,
     OUTSIDE,
     INTERSECTION
};
static int FrustrumAABBIntersect(glm::vec4 *frustum, float minDistanceSquared, float maxDistanceSquared, glm::vec3 &min, glm::vec3 &max, float r2min, float r2max)
{
  int ret = INSIDE;
  for(uint32_t i=0; i<4; i++){
    glm::vec3 vmin, vmax;
    if(frustum[i].x < 0) {
      vmin.x = min.x;
      vmax.x = max.x;
    } else {
      vmin.x = max.x;
      vmax.x = min.x;
    }
    if(frustum[i].y < 0) {
      vmin.y = min.y;
      vmax.y = max.y;
    } else {
      vmin.y = max.y;
      vmax.y = min.y;
    }
    if(frustum[i].z < 0) {
      vmin.z = min.z;
      vmax.z = max.z;
    } else {
      vmin.z = max.z;
      vmax.z = min.z;
    }
    if(glm::dot(glm::vec3(frustum[i]), vmin) < 0.0f)
      return OUTSIDE;
    if(glm::dot(glm::vec3(frustum[i]), vmax) <= 0.0f)
      ret = INTERSECTION;
  }
  {
    if(r2min > maxDistanceSquared) return OUTSIDE;
    else if(r2max >= maxDistanceSquared) ret = INTERSECTION;
  }
  {
    if(r2max < minDistanceSquared) return OUTSIDE;
    else if(r2min <= minDistanceSquared) ret = INTERSECTION;
  }
  return ret;
}
void Cogs::Core::EchoSounder::UniformGridSampleData::cull_tiles()
{
  // Calculate frustrum AABB
  glm::vec3 aabb_min(std::numeric_limits<float>::max());
  glm::vec3 aabb_max(-std::numeric_limits<float>::max());
  uint32_t beamCount = minorCount * majorCount;
  for(uint32_t i=0; i<beamCount; i++){
    glm::vec3 d = arrayOrientationGlobal * getBeamDir(config->coordSys, config->directionX[i], config->directionY[i]);
    glm::vec3 min = d * minDistance + arrayPositionGlobal;
    glm::vec3 max = d * maxDistance + arrayPositionGlobal;
    aabb_min = glm::min(aabb_min, min);
    aabb_min = glm::min(aabb_min, max);
    aabb_max = glm::max(aabb_max, min);
    aabb_max = glm::max(aabb_max, max);
  }
 
  glm::vec3 invSize = glm::vec3(1.0f)/glm::vec3(data.size-glm::uvec3(1));
  glm::vec3 invScale = glm::vec3(1.0f)/data.scale;
 
  glm::ivec3 imin = glm::floor(aabb_min*invScale*invSize);
  glm::ivec3 imax = glm::ceil(aabb_max*invScale*invSize);
  imin-=glm::ivec3(2); // Fixme
  imax+=glm::ivec3(2);
 
  // Find tiles intersection beam
  for(int32_t tx=imin.x; tx<=imax.x; tx++){
    for(int32_t ty=imin.y; ty<=imax.y; ty++){
      for(int32_t tz=imin.z; tz<=imax.z; tz++){
        glm::vec3 tileCoord(tx, ty, tz);
        glm::vec3 tilePos = tileCoord*(glm::vec3(data.size-glm::uvec3(1))*data.scale);
        glm::vec3 q_a = tilePos - arrayPositionGlobal;
        glm::vec3 q_b = q_a + glm::vec3(data.size-glm::uvec3(1)) * data.scale;
        glm::vec3 q_min = glm::min(q_a, q_b);
        glm::vec3 q_max = glm::max(q_a, q_b);
        // TODO r2min/max calculation might not be accurate
        float r2min = std::numeric_limits<float>::max();
        float r2max = -std::numeric_limits<float>::max();
        for(uint32_t i=0; i<8; i++){
          glm::vec3 q(i & 1 ? q_min.x : q_max.x,
                      i & 2 ? q_min.y : q_max.y,
                      i & 4 ? q_min.z : q_max.z);
          float r2 = glm::dot(q, q);
          r2min = glm::min(r2min, r2);
          r2max = glm::max(r2max, r2);
        }
        int qi = FrustrumAABBIntersect(frustum, minDistanceSquared, maxDistanceSquared, q_min, q_max, r2min, r2max);
        if(qi == OUTSIDE) continue;
        if(qi == INSIDE) tiles_inside.push_back(glm::ivec3(tx, ty, tz));
        else tiles_intersecting.push_back(glm::ivec3(tx, ty, tz));
      }
    }
  }
}
void Cogs::Core::EchoSounder::UniformGridSampleData::evict_tiles()
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::evict_tiles");
  if(data.tiles.size() > component->max_tiles){
    data.evict_number++;
    for(auto it = data.tiles.begin(); it != data.tiles.end(); ){
      if(it->second.ping_number < data.evict_number){
        _aligned_free(it->second.data);
        Entity *ptr = it->second.visualizer.get();
        if(ptr) data.destroy.push_back(ptr);
        it = data.tiles.erase(it);
      }
      else{
        it++;
      }
    }
  }
}
Cogs::Core::EchoSounder::Tile Cogs::Core::EchoSounder::UniformGridSampleData::GenTile()
{
  Tile tile = {};
  tile.data = (float*)_aligned_malloc(sizeof(float)*data.size.x*data.size.y*data.size.z, sizeof(float)*8);
  if(!tile.data){
    evict_tiles();
    tile.data = (float*)_aligned_malloc(sizeof(float)*data.size.x*data.size.y*data.size.z, sizeof(float)*8);
  }
  if(!tile.data){
    LOG_ERROR(logger, "UniformGrid: OutOfMemory! Clearing!\n");
    system->clear(component);
    tile.data = (float*)_aligned_malloc(sizeof(float)*data.size.x*data.size.y*data.size.z, sizeof(float)*8);
  }
  if(!tile.data){
    LOG_ERROR(logger, "UniformGrid: OutOfMemory! Exit!\n");
    exit(EXIT_FAILURE);
  }
  memset(tile.data, 0, sizeof(float)*data.size.x*data.size.y*data.size.z);
  tile.minValue = -std::numeric_limits<float>::max();
  tile.maxValue = std::numeric_limits<float>::max();
  return tile;
}
void Cogs::Core::EchoSounder::UniformGridSampleData::gen_tiles()
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::gen_tiles");
  for(auto tileIndex: tiles_inside){
    glm::uvec3 tilePos = glm::vec3(tileIndex)+glm::vec3(1<<15);
    TileKey key = encodeTileKey(tilePos);
    if(data.tiles.count(key) == 0){
      data.tiles[key] = GenTile();
    }
  }
  for(auto tileIndex: tiles_intersecting){
    glm::uvec3 tilePos = glm::vec3(tileIndex)+glm::vec3(1<<15);
    TileKey key = encodeTileKey(tilePos);
    if(data.tiles.count(key) == 0){
      data.tiles[key] = GenTile();
    }
  }
}
void Cogs::Core::EchoSounder::UniformGridSampleData::sample_ref(float *d, const float *v, uint32_t z, uint32_t y, uint32_t x, float r2, glm::vec3 q)
{
  glm::vec3 a = inverseOrientation * q;
  float r = std::sqrt(r2);
 
  // x = sin(dirX)
  // y = sin(dirY)
  // z = r = sqrt(1.0f - x*x - y*y)
  float dirX = std::asin(a.x / r);
  float dirY = std::asin(a.y / r);
  glm::vec3 polarPos = glm::vec3(dirY, dirX, r);
  glm::vec3 t;
  glm::uvec3 i;
  glm::uvec2 j;
 
  if (config->majorCount > 1 && config->minorCount > 1) {
    // Discard samples outside of beam grid
    glm::vec3 xi = polarScale * (polarPos - polarShift);
    if ((xi.x <= 0.0f || xi.y <= 0.0f || xi.z <= 0.0f ||
      xi.x > maxIndices.x || xi.y > maxIndices.y || xi.z > maxIndices.z))
      return;
 
    // Figure out interpolation parameters.
    glm::vec3 tau = glm::floor(xi);
    t = xi - tau;
    i = glm::uvec3(tau);
    j = glm::uvec2(i) + glm::uvec2(1);
  }
  else {
    // Initial float index:
    glm::vec3 xi = glm::max(glm::vec3(0.f), polarScale*(polarPos - polarShift));
 
    // Figure out interpolation parameters.
    glm::vec3 tau = glm::floor(xi);
    t = xi - tau;
    i = glm::min(maxIndices, glm::uvec3(tau));
    j = glm::min(glm::uvec2(maxIndices), glm::uvec2(i) + glm::uvec2(1));
  }
 
  float val00 = v[(i.y*minorCount + i.x)*config->sampleCount + i.z];
  float val01 = v[(j.y*minorCount + i.x)*config->sampleCount + i.z];
  float val0 = (1.f - t.y)*val00 + t.y*val01;
 
  float val10 = v[(i.y*minorCount + j.x)*config->sampleCount + i.z];
  float val11 = v[(j.y*minorCount + j.x)*config->sampleCount + i.z];
  float val1 = (1.f - t.y)*val10 + t.y*val11;
 
  float val = (1.f - t.x)*val0 + t.x*val1;
 
  uint32_t index = z * data.size.y*data.size.x + y * data.size.x + x;
#if 1
  // Exponential moving average
  float prev = d[index];
  if(prev == 0.0){
    d[index] = val;
  }
  else{
    float alpha = average_alpha;
    d[index] = val*alpha + prev*(1.0f-alpha);
  }
#else
  d[index] = val;
#endif
  //tile.age[index] = float(10e-7*((*m)[0].ping->timestamp - misc.ping->timestamp));
}
void Cogs::Core::EchoSounder::UniformGridSampleData::sample_sn90_ref(float *d, const float *v, uint32_t z, uint32_t y, uint32_t x, float r2, glm::vec3 q)
{
  glm::vec3 a = inverseOrientation * q;
  float r = std::sqrt(r2);
 
  // x = sin(dirX)
  // y = cos(dirX) * sin(dirY)
  // z = cos(dirX) * cos(dirY)
  //
  // y/z = sin(dirY)/cos(dirY) = tan(dirY)
  //
  // dirY = atan(y/z)
  float dirX = std::asin(a.x / r);
  float dirY = std::atan(a.y / a.z);
  glm::vec3 polarPos = glm::vec3(dirY, dirX, a.z);
 
  // Initial float index:
  glm::vec3 xi = glm::max(glm::vec3(0.f), polarScale*(polarPos - polarShift));
 
  // Figure out interpolation parameters.
  glm::vec3 tau = glm::floor(xi);
  glm::vec3 t = xi - tau;
 
#if 0
  glm::uvec3 i = glm::min(maxIndices, glm::uvec3(tau));
  glm::uvec2 j = glm::min(glm::uvec2(maxIndices), glm::uvec2(i) + glm::uvec2(1));
 
  float val00 = v[(i.y*minorCount + i.x)*config->sampleCount + i.z];
  float val01 = v[(j.y*minorCount + i.x)*config->sampleCount + i.z];
  float val0 = (1.f - t.y)*val00 + t.y*val01;
 
  float val10 = v[(i.y*minorCount + j.x)*config->sampleCount + i.z];
  float val11 = v[(j.y*minorCount + j.x)*config->sampleCount + i.z];
  float val1 = (1.f - t.y)*val10 + t.y*val11;
 
  float val = (1.f - t.x)*val0 + t.x*val1;
#elif 1
  glm::uvec2 i = glm::min(glm::uvec2(maxIndices), glm::uvec2(tau));
  glm::uvec2 j = glm::min(glm::uvec2(maxIndices), glm::uvec2(i) + glm::uvec2(1));
 
  float s = t.y;
  float scale = -2.0f*cos(dirY)*polarScale.y;
 
  uint32_t z00 = (uint32_t)std::clamp(tau.z + s*scale, 0.0f, (float)maxIndices.z);
  uint32_t z01 = (uint32_t)std::clamp(tau.z + (s-1)*scale, 0.0f, (float)maxIndices.z);
  uint32_t z10 = (uint32_t)std::clamp(tau.z + s*scale, 0.0f, (float)maxIndices.z);
  uint32_t z11 = (uint32_t)std::clamp(tau.z + (s-1)*scale, 0.0f, (float)maxIndices.z);
 
  float val00 = v[(i.y*minorCount + i.x)*config->sampleCount + z00];
  float val01 = v[(j.y*minorCount + i.x)*config->sampleCount + z01];
  float val0 = (1.f - t.y)*val00 + t.y*val01;
 
  float val10 = v[(i.y*minorCount + j.x)*config->sampleCount + z10];
  float val11 = v[(j.y*minorCount + j.x)*config->sampleCount + z11];
  float val1 = (1.f - t.y)*val10 + t.y*val11;
 
  float val = (1.f - t.x)*val0 + t.x*val1;
#elif 0
  glm::uvec2 i = glm::min(glm::uvec2(maxIndices), glm::uvec2(tau));
  glm::uvec2 j = glm::min(glm::uvec2(maxIndices), glm::uvec2(i) + glm::uvec2(1));
 
  float s = t.y;
  float scale = -2.0f*cos(dirY)*polarScale.y;
 
  uint32_t z000 = std::min<uint32_t>(maxIndices.z, tau.z + s*scale);
  uint32_t z010 = std::min<uint32_t>(maxIndices.z, tau.z + (s-1)*scale);
  uint32_t z100 = std::min<uint32_t>(maxIndices.z, tau.z + s*scale);
  uint32_t z110 = std::min<uint32_t>(maxIndices.z, tau.z + (s-1)*scale);
 
  uint32_t z001 = std::min<uint32_t>(maxIndices.z, tau.z + s*scale + 1);
  uint32_t z011 = std::min<uint32_t>(maxIndices.z, tau.z + (s-1)*scale + 1);
  uint32_t z101 = std::min<uint32_t>(maxIndices.z, tau.z + s*scale + 1);
  uint32_t z111 = std::min<uint32_t>(maxIndices.z, tau.z + (s-1)*scale + 1);
 
  float val000 = v[(i.y*minorCount + i.x)*config->sampleCount + z000];
  float val010 = v[(j.y*minorCount + i.x)*config->sampleCount + z010];
  float val00 = (1.f - t.y)*val000 + t.y*val010;
 
  float val100 = v[(i.y*minorCount + j.x)*config->sampleCount + z100];
  float val110 = v[(j.y*minorCount + j.x)*config->sampleCount + z110];
  float val10 = (1.f - t.y)*val100 + t.y*val110;
 
  float val0 = (1.f - t.x)*val00 + t.x*val10;
 
  float val001 = v[(i.y*minorCount + i.x)*config->sampleCount + z001];
  float val011 = v[(j.y*minorCount + i.x)*config->sampleCount + z011];
  float val01 = (1.f - t.y)*val001 + t.y*val011;
 
  float val101 = v[(i.y*minorCount + j.x)*config->sampleCount + z101];
  float val111 = v[(j.y*minorCount + j.x)*config->sampleCount + z111];
  float val11 = (1.f - t.y)*val101 + t.y*val111;
 
  float val1 = (1.f - t.x)*val01 + t.x*val11;
 
  float val = (1.f - t.z)*val0 + t.z*val1;
#endif
 
  uint32_t index = z * data.size.y*data.size.x + y * data.size.x + x;
  d[index] = val;
  //age[index] = float(10e-7*((*m)[0].ping->timestamp - misc.ping->timestamp));
}
void Cogs::Core::EchoSounder::UniformGridSampleData::sample_intersecting(size_t start, size_t end)
{
  Cogs::Memory::MemoryBuffer scratch;
 
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::sampleIntersectingTask");
  float *v = linear.data();
  for (size_t ti = start; ti < end; ti++) {
    Timer timer = Timer::startNew();
    glm::vec3 tileCoord = tiles_intersecting[ti];
    glm::vec3 tilePos = tileCoord*(glm::vec3(data.size-glm::uvec3(1))*data.scale);
    glm::uvec3 tileIndex = tileCoord+glm::vec3(1<<15);
    TileKey key = encodeTileKey(tileIndex);
    Tile &tile = data.tiles[key];
    if (config->coordSys == 0) {
      for (uint32_t z = 0; z < data.size.z; z++) {
        for (uint32_t y = 0; y < data.size.y; y++) {
          for (uint32_t x = 0; x < data.size.x; x++) {
            glm::vec3 p = data.scale * glm::vec3(x, y, z);
            glm::vec3 q = (tilePos - arrayPositionGlobal) + p; // Sample pos with origin in beam bundle origin.
            float r2 = glm::dot(q, q);
 
            bool in_0 = 0.f <= glm::dot(glm::vec3(frustum[0]), q);
            bool in_1 = 0.f <= glm::dot(glm::vec3(frustum[1]), q);
            bool in_2 = 0.f <= glm::dot(glm::vec3(frustum[2]), q);
            bool in_3 = 0.f <= glm::dot(glm::vec3(frustum[3]), q);
            bool in_4 = minDistanceSquared <= r2;
            bool in_5 = r2 <= maxDistanceSquared;
            if (!in_0 || !in_1 || !in_2 || !in_3 || !in_4 || !in_5) continue;
            sample_ref(tile.data, v, z, y, x, r2, q);
          }
        }
      }
    }
    else if (config->coordSys == 1) {
      switch (compute) {
      case ComputeType::SSE41:
        sampleTile_border_sse41(tile.data, v, tileCoord, tileIndex, data.size, maxIndices, tilePos, data.scale, arrayPositionGlobal, frustum,
                                minDistanceSquared, maxDistanceSquared, inverseOrientation,
                                config->coordSys, minorCount, config->sampleCount, polarScale, polarShift);
        break;
      case ComputeType::FMA:
        sampleTile_inner_fma_gather(tile.data, v, tileCoord, tileIndex, data.size, maxIndices, tilePos, data.scale, arrayPositionGlobal, frustum,
                                    minDistanceSquared, maxDistanceSquared, inverseOrientation,
                                    config->coordSys, minorCount, config->sampleCount, polarScale, polarShift);
        break;
      case ComputeType::AVX2:
        sampleTile3_border_avx2(tile.data, tile.minValue, tile.maxValue, v, tileCoord, tileIndex, data.size, maxIndices, tilePos, data.scale, arrayPositionGlobal, frustum,
                                minDistanceSquared, maxDistanceSquared, inverseOrientation,
                                config->coordSys, minorCount, config->sampleCount, polarScale, polarShift);
        break;
      default:
        for (uint32_t z = 0; z < data.size.z; z++) {
          for (uint32_t y = 0; y < data.size.y; y++) {
            for (uint32_t x = 0; x < data.size.x; x++) {
              glm::vec3 p = data.scale * glm::vec3(x, y, z);
              glm::vec3 q = (tilePos - arrayPositionGlobal) + p; // Sample pos with origin in beam bundle origin.
              float r2 = glm::dot(q, q);
 
              bool in_0 = 0.f <= glm::dot(glm::vec3(frustum[0]), q);
              bool in_1 = 0.f <= glm::dot(glm::vec3(frustum[1]), q);
              bool in_2 = 0.f <= glm::dot(glm::vec3(frustum[2]), q);
              bool in_3 = 0.f <= glm::dot(glm::vec3(frustum[3]), q);
              bool in_4 = minDistanceSquared <= r2;
              bool in_5 = r2 <= maxDistanceSquared;
              if (!in_0 || !in_1 || !in_2 || !in_3 || !in_4 || !in_5) continue;
              sample_sn90_ref(tile.data, v, z, y, x, r2, q);
            }
          }
        }
        break;
      }
    }
    else { assert(false && "Unhandled coordinate system"); }
    data.sample_intersecting.us.fetch_add(uint32_t(1e6*timer.elapsedSeconds()));
    data.sample_intersecting.N.fetch_add(1);
    tile.mesh = data.createIsoSurfaces(tile, scratch, tilePos);
    tile.ping_number = ping_number;
    tile.update = true;
  }
}
void Cogs::Core::EchoSounder::UniformGridSampleData::sample_inside(size_t start, size_t end)
{
  Cogs::Memory::MemoryBuffer scratch;
 
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::sampleInsideTask");
  float *v = linear.data();
  for(size_t ti=start; ti<end; ti++){
    Timer timer = Timer::startNew();
    glm::vec3 tileCoord = tiles_inside[ti];
    glm::vec3 tilePos = tileCoord*(glm::vec3(data.size-glm::uvec3(1))*data.scale);
    glm::uvec3 tileIndex = tileCoord+glm::vec3(1<<15);
    TileKey key = encodeTileKey(tileIndex);
    Tile &tile = data.tiles[key];
    if(config->coordSys == 0){
      for(uint32_t z=0; z<data.size.z; z++){
        for(uint32_t y=0; y<data.size.y; y++){
          for(uint32_t x=0; x<data.size.x; x++){
            glm::vec3 p = data.scale * glm::vec3(x, y, z);
            glm::vec3 q = (tilePos - arrayPositionGlobal) + p; // Sample pos with origin in beam bundle origin.
            float r2 = glm::dot(q, q);
            sample_ref(tile.data, v, z, y, x, r2, q);
          }
        }
      }
    }
    else if(config->coordSys == 1){
      switch (compute) {
      case ComputeType::SSE41:
        sampleTile_border_sse41(tile.data, v, tileCoord, tileIndex, data.size, maxIndices, tilePos, data.scale, arrayPositionGlobal, frustum,
                                minDistanceSquared, maxDistanceSquared, inverseOrientation,
                                config->coordSys, minorCount, config->sampleCount, polarScale, polarShift);
        break;
      case ComputeType::FMA:
        sampleTile_inner_fma_gather(tile.data, v, tileCoord, tileIndex, data.size, maxIndices, tilePos, data.scale, arrayPositionGlobal, frustum,
                                    minDistanceSquared, maxDistanceSquared, inverseOrientation,
                                    config->coordSys, minorCount, config->sampleCount, polarScale, polarShift);
        break;
      case ComputeType::AVX2:
        sampleTile3_border_avx2(tile.data, tile.minValue, tile.maxValue, v, tileCoord, tileIndex, data.size, maxIndices, tilePos, data.scale, arrayPositionGlobal, frustum,
                                minDistanceSquared, maxDistanceSquared, inverseOrientation,
                                config->coordSys, minorCount, config->sampleCount, polarScale, polarShift);
        break;
      default:
        for (uint32_t z = 0; z < data.size.z; z++) {
          for (uint32_t y = 0; y < data.size.y; y++) {
            for (uint32_t x = 0; x < data.size.x; x++) {
              glm::vec3 p = data.scale * glm::vec3(x, y, z);
              glm::vec3 q = (tilePos - arrayPositionGlobal) + p; // Sample pos with origin in beam bundle origin.
              float r2 = glm::dot(q, q);
              sample_sn90_ref(tile.data, v, z, y, x, r2, q);
            }
          }
        }
        break;
      }
    }
    else { assert(false && "Unhandled coordinate system"); }
    data.sample_inside.us.fetch_add(uint32_t(1e6*timer.elapsedSeconds()));
    data.sample_inside.N.fetch_add(1);
    tile.mesh = data.createIsoSurfaces(tile, scratch, tilePos);
    tile.ping_number = ping_number;
    tile.update = true;
  }
}
namespace Cogs::Core::EchoSounder{
  typedef std::pair<TileKey, Tile*> Tmp;
  struct TileOrder{
    bool operator() (const Tmp &lhs, const Tmp &rhs) const
    {
      return lhs.second->ping_number<rhs.second->ping_number;
    }
};
}// namespace ...
bool Cogs::Core::EchoSounder::UniformGridData::update_isosurface()
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::update_isosurface");
  std::priority_queue<Tmp, std::vector<Tmp>, TileOrder> queue; // TODO just sort an array
  for(auto &tmp : tiles){
    if(tmp.second.update_iso){
      queue.push(std::pair<TileKey, Tile*>(tmp.first, &tmp.second));
    }
  }
  if(!queue.size()) return false;
 
  bool ret = false;
  std::vector<Tmp> update_tiles;
  uint32_t k = 0;
  while(!queue.empty()){
    update_tiles.push_back(queue.top());
    queue.pop();
    if(++k >= 512){
      ret = true;
      break;
    }
  }
 
  auto group = system->context->taskManager->createGroup(system->Queue);
  size_t concurrency = Threads::hardwareConcurrency()-1;
  size_t tile_count = update_tiles.size();
  size_t task_count = std::min(tile_count, concurrency);
  size_t steps = tile_count/task_count+1;
  for(size_t i=0; i<tile_count; i+=steps){
    size_t end = std::min(i+steps, tile_count);
    auto task = [this, &update_tiles, i, end]()
                {
                  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "UniformGrid::update_isosurface_task");
                  Cogs::Memory::MemoryBuffer scratch;
                  for(size_t j=i; j<end; j++){
                    Tmp tmp = update_tiles[j];
                    Tile &tile = *tmp.second;
                    glm::uvec3 tileIndex(decodeTileKey(tmp.first));
                    glm::vec3 tileCoord(glm::vec3(tileIndex)-glm::vec3(1<<15));
                    glm::vec3 tilePos = tileCoord*(glm::vec3(size-glm::uvec3(1))*scale);
                    tile.mesh = createIsoSurfaces(tile, scratch, tilePos);
                    tile.update = true;
                    tile.update_iso = false;
                  }
                };
    system->context->taskManager->enqueueChild(group, task);
  }
  system->context->taskManager->wait(group);
  return ret;
}