Repository: csiro-robotics/ohm
Branch: master
Commit: 4e2e76945e55
Files: 551
Total size: 3.8 MB

Directory structure:
gitextract_waem9xm7/

├── .clang-format
├── .clang-tidy
├── .gitignore
├── 3rdparty/
│   └── 3es-core/
│       └── 3esservermacros.h
├── CMakeLists.txt
├── LICENSE
├── OpenCL.md
├── clearance-performance-notes.md
├── clu/
│   ├── 3rdparty/
│   │   └── CL/
│   │       └── opencl.hpp
│   ├── CMakeLists.txt
│   ├── clu.cpp
│   ├── clu.h
│   ├── cluBuffer.h
│   ├── cluConfig.in.h
│   ├── cluConstraint.cpp
│   ├── cluConstraint.h
│   ├── cluKernel.cpp
│   ├── cluKernel.h
│   ├── cluProgram.cpp
│   └── cluProgram.h
├── cmake/
│   ├── ClangTidy.cmake
│   ├── FindLASZIP.cmake
│   ├── FindLIBLAS.cmake
│   ├── FindOctomap.cmake
│   ├── FindTBB.cmake
│   ├── Findglm.cmake
│   ├── LeakTrack.cmake
│   ├── OhmCuda.cmake
│   ├── OhmGTest.cmake
│   ├── TextFileResource.cmake
│   ├── TextFileResource.py
│   ├── compilerSetup.cmake
│   ├── doxyfile.in
│   ├── doxygen.cmake
│   ├── ohm-config.cmake
│   ├── ohm-packages.cmake
│   ├── tidy/
│   │   ├── clang-tidy-names.yaml
│   │   └── wrap-clang-tidy.py
│   └── utils.cmake
├── docs/
│   ├── docglossary.md
│   ├── docmain.h
│   ├── docusage.md
│   ├── docutils.md
│   ├── docvoxellayers.md
│   └── gpu/
│       ├── docgpualgorithm.md
│       └── docgpudetail.md
├── gputil/
│   ├── CMakeLists.txt
│   ├── cl/
│   │   ├── gpuApiExceptionCode.cpp
│   │   ├── gpuBuffer.cpp
│   │   ├── gpuBufferDetail.h
│   │   ├── gpuDevice.cpp
│   │   ├── gpuDeviceDetail.h
│   │   ├── gpuEvent.cpp
│   │   ├── gpuEventDetail.h
│   │   ├── gpuKernel.cpp
│   │   ├── gpuKernel2.h
│   │   ├── gpuKernelDetail.h
│   │   ├── gpuPinnedBuffer.cpp
│   │   ├── gpuPlatform2.h
│   │   ├── gpuProgram.cpp
│   │   ├── gpuProgramDetail.h
│   │   ├── gpuQueue.cpp
│   │   └── gpuQueueDetail.h
│   ├── cuda/
│   │   ├── cutil_atomic.h
│   │   ├── cutil_decl.h
│   │   ├── cutil_importcl.h
│   │   ├── cutil_math.h
│   │   ├── gpuApiExceptionCode.cpp
│   │   ├── gpuBuffer.cpp
│   │   ├── gpuBufferDetail.h
│   │   ├── gpuDevice.cpp
│   │   ├── gpuDeviceDetail.h
│   │   ├── gpuDirtyRegion
│   │   ├── gpuEvent.cpp
│   │   ├── gpuEventDetail.h
│   │   ├── gpuKernel.cpp
│   │   ├── gpuKernel2.h
│   │   ├── gpuKernelDetail.h
│   │   ├── gpuMemRegion.cpp
│   │   ├── gpuMemRegion.h
│   │   ├── gpuPinnedBuffer.cpp
│   │   ├── gpuPlatform2.h
│   │   ├── gpuProgram.cpp
│   │   ├── gpuProgramDetail.h
│   │   ├── gpuQueue.cpp
│   │   ├── gpuQueueDetail.h
│   │   └── ref.h
│   ├── gpuApiException.cpp
│   ├── gpuApiException.h
│   ├── gpuBuffer.h
│   ├── gpuConfig.in.h
│   ├── gpuDevice.h
│   ├── gpuDeviceInfo.h
│   ├── gpuEvent.h
│   ├── gpuEventList.cpp
│   ├── gpuEventList.h
│   ├── gpuException.cpp
│   ├── gpuException.h
│   ├── gpuKernel.h
│   ├── gpuPinMode.h
│   ├── gpuPinnedBuffer.h
│   ├── gpuPlatform.h
│   ├── gpuProgram.h
│   ├── gpuQueue.h
│   ├── gpuThrow.cpp
│   ├── gpuThrow.h
│   ├── gpuVersion.h
│   └── gpu_ext.h
├── logutil/
│   ├── CMakeLists.txt
│   ├── LogUtil.cpp
│   ├── LogUtil.h
│   ├── LogUtilConfig.in.h
│   ├── Logger.cpp
│   ├── Logger.h
│   └── LoggerDetail.h
├── ohm/
│   ├── Aabb.h
│   ├── CMakeLists.txt
│   ├── CalculateSegmentKeys.cpp
│   ├── CalculateSegmentKeys.h
│   ├── ClearingPattern.cpp
│   ├── ClearingPattern.h
│   ├── CompareMaps.cpp
│   ├── CompareMaps.h
│   ├── CopyUtil.cpp
│   ├── CopyUtil.h
│   ├── CovarianceVoxel.cpp
│   ├── CovarianceVoxel.h
│   ├── CovarianceVoxelCompute.h
│   ├── DataType.cpp
│   ├── DataType.h
│   ├── DebugDraw.cpp
│   ├── DebugDraw.h
│   ├── DebugIDs.h
│   ├── DefaultLayer.cpp
│   ├── DefaultLayer.h
│   ├── Density.cpp
│   ├── Density.h
│   ├── Key.cpp
│   ├── Key.h
│   ├── KeyHash.cpp
│   ├── KeyHash.h
│   ├── KeyList.cpp
│   ├── KeyList.h
│   ├── KeyRange.cpp
│   ├── KeyRange.h
│   ├── KeyStream.h
│   ├── LineKeysQuery.cpp
│   ├── LineKeysQuery.h
│   ├── LineQuery.cpp
│   ├── LineQuery.h
│   ├── LineWalk.h
│   ├── LineWalkCompute.h
│   ├── MapChunk.cpp
│   ├── MapChunk.h
│   ├── MapChunkFlag.h
│   ├── MapCoord.h
│   ├── MapFlag.cpp
│   ├── MapFlag.h
│   ├── MapInfo.cpp
│   ├── MapInfo.h
│   ├── MapLayer.cpp
│   ├── MapLayer.h
│   ├── MapLayout.cpp
│   ├── MapLayout.h
│   ├── MapLayoutMatch.h
│   ├── MapProbability.h
│   ├── MapRegion.cpp
│   ├── MapRegion.h
│   ├── MapRegionCache.cpp
│   ├── MapRegionCache.h
│   ├── MapSerialise.cpp
│   ├── MapSerialise.h
│   ├── Mapper.cpp
│   ├── Mapper.h
│   ├── MappingProcess.cpp
│   ├── MappingProcess.h
│   ├── Mutex.cpp
│   ├── Mutex.h
│   ├── NdtMap.cpp
│   ├── NdtMap.h
│   ├── NdtMode.cpp
│   ├── NdtMode.h
│   ├── NearestNeighbours.cpp
│   ├── NearestNeighbours.h
│   ├── OccupancyMap.cpp
│   ├── OccupancyMap.h
│   ├── OccupancyType.cpp
│   ├── OccupancyType.h
│   ├── OccupancyUtil.cpp
│   ├── OccupancyUtil.h
│   ├── OhmConfig.in.h
│   ├── Query.cpp
│   ├── Query.h
│   ├── QueryFlag.h
│   ├── RayFilter.cpp
│   ├── RayFilter.h
│   ├── RayFlag.h
│   ├── RayMapper.cpp
│   ├── RayMapper.h
│   ├── RayMapperNdt.cpp
│   ├── RayMapperNdt.h
│   ├── RayMapperOccupancy.cpp
│   ├── RayMapperOccupancy.h
│   ├── RayMapperSecondarySample.cpp
│   ├── RayMapperSecondarySample.h
│   ├── RayMapperTrace.cpp
│   ├── RayMapperTrace.h
│   ├── RayMapperTsdf.cpp
│   ├── RayMapperTsdf.h
│   ├── RayPattern.cpp
│   ├── RayPattern.h
│   ├── RayPatternConical.cpp
│   ├── RayPatternConical.h
│   ├── RaysQuery.cpp
│   ├── RaysQuery.h
│   ├── Stream.cpp
│   ├── Stream.h
│   ├── Trace.cpp
│   ├── Trace.h
│   ├── Voxel.cpp
│   ├── Voxel.h
│   ├── VoxelBlock.cpp
│   ├── VoxelBlock.h
│   ├── VoxelBlockCompressionQueue.cpp
│   ├── VoxelBlockCompressionQueue.h
│   ├── VoxelBuffer.cpp
│   ├── VoxelBuffer.h
│   ├── VoxelData.h
│   ├── VoxelIncident.h
│   ├── VoxelIncidentCompute.h
│   ├── VoxelLayout.cpp
│   ├── VoxelLayout.h
│   ├── VoxelMean.h
│   ├── VoxelMeanCompute.h
│   ├── VoxelOccupancy.h
│   ├── VoxelOccupancyCompute.h
│   ├── VoxelSecondarySample.h
│   ├── VoxelTouchTime.h
│   ├── VoxelTouchTimeCompute.h
│   ├── VoxelTsdf.cpp
│   ├── VoxelTsdf.h
│   ├── VoxelTsdfCompute.h
│   ├── private/
│   │   ├── ClearingPatternDetail.h
│   │   ├── LineKeysQueryDetail.h
│   │   ├── LineQueryDetail.h
│   │   ├── MapLayerDetail.h
│   │   ├── MapLayoutDetail.h
│   │   ├── MapperDetail.h
│   │   ├── NdtMapDetail.h
│   │   ├── NearestNeighboursDetail.h
│   │   ├── OccupancyMapDetail.cpp
│   │   ├── OccupancyMapDetail.h
│   │   ├── OccupancyQueryAlg.h
│   │   ├── QueryDetail.h
│   │   ├── RayPatternDetail.h
│   │   ├── RaysQueryDetail.h
│   │   ├── SerialiseUtil.h
│   │   ├── VoxelAlgorithms.cpp
│   │   ├── VoxelAlgorithms.h
│   │   ├── VoxelBlockCompressionQueueDetail.h
│   │   └── VoxelLayoutDetail.h
│   └── serialise/
│       ├── MapSerialiseV0.1.cpp
│       ├── MapSerialiseV0.1.h
│       ├── MapSerialiseV0.2.cpp
│       ├── MapSerialiseV0.2.h
│       ├── MapSerialiseV0.4.cpp
│       ├── MapSerialiseV0.4.h
│       ├── MapSerialiseV0.5.cpp
│       ├── MapSerialiseV0.5.h
│       ├── MapSerialiseV0.cpp
│       └── MapSerialiseV0.h
├── ohm.natvis
├── ohmapp/
│   ├── CMakeLists.txt
│   ├── DataSource.cpp
│   ├── DataSource.h
│   ├── MapHarness.cpp
│   ├── MapHarness.h
│   ├── OhmAppConfig.in.h
│   ├── OhmAppCpu.cpp
│   ├── OhmAppCpu.h
│   ├── OhmAppGpu.cpp
│   ├── OhmAppGpu.h
│   ├── OhmAppGpuConfig.in.h
│   ├── SlamIOSource.cpp
│   ├── SlamIOSource.h
│   └── ohmappmain.inl
├── ohmgpu/
│   ├── CMakeLists.txt
│   ├── ClearanceProcess.cpp
│   ├── ClearanceProcess.h
│   ├── GpuCache.cpp
│   ├── GpuCache.h
│   ├── GpuCachePostSyncHandler.h
│   ├── GpuCacheStats.h
│   ├── GpuKey.h
│   ├── GpuLayerCache.cpp
│   ├── GpuLayerCache.h
│   ├── GpuLayerCacheParams.cpp
│   ├── GpuLayerCacheParams.h
│   ├── GpuMap.cpp
│   ├── GpuMap.h
│   ├── GpuNdtMap.cpp
│   ├── GpuNdtMap.h
│   ├── GpuTransformSamples.cpp
│   ├── GpuTransformSamples.h
│   ├── GpuTsdfMap.cpp
│   ├── GpuTsdfMap.h
│   ├── LineKeysQueryGpu.cpp
│   ├── LineKeysQueryGpu.h
│   ├── LineQueryGpu.cpp
│   ├── LineQueryGpu.h
│   ├── OhmGpu.cpp
│   ├── OhmGpu.h
│   ├── OhmGpuConfig.in.h
│   ├── RayItem.h
│   ├── RaysQueryGpu.cpp
│   ├── RaysQueryGpu.h
│   ├── gpu/
│   │   ├── AdjustNdt.cl
│   │   ├── AdjustOccupancy.cl
│   │   ├── CovarianceHitNdt.cl
│   │   ├── CovarianceHitNdt.cu
│   │   ├── CovarianceHitNdt_h.cl
│   │   ├── LineKeys.cl
│   │   ├── LineKeys.cu
│   │   ├── LineWalk.cl
│   │   ├── LineWalkMarkers.cl
│   │   ├── RaysQuery.cl
│   │   ├── RaysQuery.cu
│   │   ├── RaysQueryResult.h
│   │   ├── RegionUpdate.cl
│   │   ├── RegionUpdate.cu
│   │   ├── RegionUpdateNdt.cu
│   │   ├── Regions.cl
│   │   ├── RoiRangeFill.cl
│   │   ├── RoiRangeFill.cu
│   │   ├── TransformSamples.cl
│   │   ├── TransformSamples.cu
│   │   ├── Traversal.cl
│   │   ├── TsdfUpdate.cl
│   │   ├── TsdfUpdate.cu
│   │   ├── VoxelIncident.cl
│   │   └── VoxelMean.cl
│   └── private/
│       ├── ClearanceProcessDetail.cpp
│       ├── ClearanceProcessDetail.h
│       ├── GpuMapDetail.cpp
│       ├── GpuMapDetail.h
│       ├── GpuNdtMapDetail.h
│       ├── GpuProgramRef.cpp
│       ├── GpuProgramRef.h
│       ├── GpuTransformSamplesDetail.h
│       ├── GpuTsdfMapDetail.h
│       ├── LineKeysQueryDetailGpu.h
│       ├── LineQueryDetailGpu.h
│       ├── RaysQueryDetailGpu.cpp
│       ├── RaysQueryDetailGpu.h
│       ├── RoiRangeFill.cpp
│       └── RoiRangeFill.h
├── ohmheightmap/
│   ├── 3rdparty/
│   │   └── delaunator.hpp
│   ├── CMakeLists.txt
│   ├── Heightmap.cpp
│   ├── Heightmap.h
│   ├── HeightmapMesh.cpp
│   ├── HeightmapMesh.h
│   ├── HeightmapMode.cpp
│   ├── HeightmapMode.h
│   ├── HeightmapSerialise.cpp
│   ├── HeightmapSerialise.h
│   ├── HeightmapUtil.cpp
│   ├── HeightmapUtil.h
│   ├── HeightmapVoxel.cpp
│   ├── HeightmapVoxel.h
│   ├── HeightmapVoxelType.h
│   ├── OhmHeightmapConfig.in.h
│   ├── PlaneFillLayeredWalker.cpp
│   ├── PlaneFillLayeredWalker.h
│   ├── PlaneFillWalker.cpp
│   ├── PlaneFillWalker.h
│   ├── PlaneWalkVisitMode.h
│   ├── PlaneWalker.cpp
│   ├── PlaneWalker.h
│   ├── TriangleEdge.h
│   ├── TriangleNeighbours.h
│   ├── UpAxis.h
│   └── private/
│       ├── HeightmapDetail.cpp
│       ├── HeightmapDetail.h
│       ├── HeightmapOperations.cpp
│       └── HeightmapOperations.h
├── ohmheightmapimage/
│   ├── CMakeLists.txt
│   ├── HeightmapImage.cpp
│   ├── HeightmapImage.h
│   └── OhmHeightmapImageConfig.in.h
├── ohmtools/
│   ├── CMakeLists.txt
│   ├── OhmCloud.cpp
│   ├── OhmCloud.h
│   ├── OhmGen.cpp
│   ├── OhmGen.h
│   └── OhmToolsConfig.in.h
├── ohmutil/
│   ├── 3rdparty/
│   │   ├── cxxopts/
│   │   │   └── cxxopts.hpp
│   │   └── ska/
│   │       ├── bytell_hash_map.hpp
│   │       ├── flat_hash_map.hpp
│   │       ├── ska_sort.hpp
│   │       └── unordered_map.hpp
│   ├── CMakeLists.txt
│   ├── Colour.cpp
│   ├── Colour.h
│   ├── GlmStream.h
│   ├── OhmUtilConfig.in.h
│   ├── Options.h
│   ├── PlyMesh.cpp
│   ├── PlyMesh.h
│   ├── PlyPointStream.cpp
│   ├── PlyPointStream.h
│   ├── Profile.cpp
│   ├── Profile.h
│   ├── ProfileMarker.cpp
│   ├── ProfileMarker.h
│   ├── ProgressMonitor.cpp
│   ├── ProgressMonitor.h
│   ├── SafeIO.cpp
│   ├── SafeIO.h
│   ├── ScopedTimeDisplay.cpp
│   ├── ScopedTimeDisplay.h
│   ├── VectorHash.h
│   └── p2p.h
├── package.xml
├── readme.md
├── scripts/
│   ├── ohm-parse-timing.py
│   ├── ohm-timing-run.py
│   └── setup.cfg
├── slamio/
│   ├── CMakeLists.txt
│   ├── DataChannel.h
│   ├── PointCloudReader.cpp
│   ├── PointCloudReader.h
│   ├── PointCloudReaderPdal.cpp
│   ├── PointCloudReaderPdal.h
│   ├── PointCloudReaderPly.cpp
│   ├── PointCloudReaderPly.h
│   ├── PointCloudReaderTraj.cpp
│   ├── PointCloudReaderTraj.h
│   ├── PointCloudReaderXyz.cpp
│   ├── PointCloudReaderXyz.h
│   ├── Points.cpp
│   ├── Points.h
│   ├── SlamCloudLoader.cpp
│   ├── SlamCloudLoader.h
│   ├── SlamIO.cpp
│   ├── SlamIO.h
│   ├── SlamIOConfig.in.h
│   ├── pdal/
│   │   ├── PointStream.cpp
│   │   └── PointStream.h
│   └── rply/
│       ├── LICENSE
│       ├── rply.c
│       ├── rply.h
│       └── rplyfile.h
├── tests/
│   ├── CMakeLists.txt
│   ├── data/
│   │   ├── CMakeLists.txt
│   │   ├── readme.md
│   │   ├── test-map.0.1.0.ohm
│   │   ├── test-map.0.2.0.ohm
│   │   ├── test-map.0.3.0.ohm
│   │   ├── test-map.0.4.0.ohm
│   │   └── test-map.0.ohm
│   ├── gputiltest/
│   │   ├── CMakeLists.txt
│   │   ├── GpuBufferTest.cpp
│   │   ├── GpuDeviceTest.cpp
│   │   ├── KernelTest.cpp
│   │   ├── TestMain.cpp
│   │   ├── cuda/
│   │   │   └── matrix_kernel.cu
│   │   └── matrix.cl
│   ├── ohmtest/
│   │   ├── CMakeLists.txt
│   │   ├── CompressionTests.cpp
│   │   ├── CopyTests.cpp
│   │   ├── IncidentsTests.cpp
│   │   ├── KeyTests.cpp
│   │   ├── LayoutTests.cpp
│   │   ├── LineQueryTests.cpp
│   │   ├── LineWalkTests.cpp
│   │   ├── MapTests.cpp
│   │   ├── MathsTests.cpp
│   │   ├── NdtTests.cpp
│   │   ├── OhmConfig.in.h
│   │   ├── OhmTestConfig.in.h
│   │   ├── RayPatternTests.cpp
│   │   ├── RayValidation.cpp
│   │   ├── RayValidation.h
│   │   ├── RaysQueryTests.cpp
│   │   ├── SecondarySampleTests.cpp
│   │   ├── SerialisationTests.cpp
│   │   ├── TestMain.cpp
│   │   ├── TouchTimeTests.cpp
│   │   ├── TraversalTests.cpp
│   │   ├── TsdfTests.cpp
│   │   └── VoxelMeanTests.cpp
│   ├── ohmtestcommon/
│   │   ├── CMakeLists.txt
│   │   ├── CovarianceTestUtil.cpp
│   │   ├── CovarianceTestUtil.h
│   │   ├── OhmTestUtil.cpp
│   │   ├── OhmTestUtil.h
│   │   ├── RayPatternTestUtil.h
│   │   ├── TraversalTest.cpp
│   │   ├── TraversalTest.h
│   │   ├── WalkSegmentKeysLegacy.cpp
│   │   └── WalkSegmentKeysLegacy.h
│   ├── ohmtestgpu/
│   │   ├── CMakeLists.txt
│   │   ├── GpuCopyTests.cpp
│   │   ├── GpuIncidentsTests.cpp
│   │   ├── GpuLineKeysTests.cpp
│   │   ├── GpuLineQueryTests.cpp
│   │   ├── GpuMapTest.cpp
│   │   ├── GpuMapperTests.cpp
│   │   ├── GpuNdtTests.cpp
│   │   ├── GpuRangesTests.cpp
│   │   ├── GpuRayPatternTests.cpp
│   │   ├── GpuRaysQueryTests.cpp
│   │   ├── GpuSerialisationTests.cpp
│   │   ├── GpuTestMain.cpp
│   │   ├── GpuTests.cpp
│   │   ├── GpuTouchTimeTests.cpp
│   │   ├── GpuTraversalTests.cpp
│   │   ├── GpuTsdfTests.cpp
│   │   └── GpuVoxelMeanTests.cpp
│   ├── ohmtestheightmap/
│   │   ├── CMakeLists.txt
│   │   └── HeightmapTests.cpp
│   └── slamiotest/
│       ├── CMakeLists.txt
│       ├── Loader.cpp
│       └── SlamCloudLoader.cpp
├── todo.md
├── utils/
│   ├── CMakeLists.txt
│   ├── ohm2ply/
│   │   ├── CMakeLists.txt
│   │   └── ohm2ply.cpp
│   ├── ohmcmp/
│   │   ├── CMakeLists.txt
│   │   └── ohmcmp.cpp
│   ├── ohmfilter/
│   │   ├── CMakeLists.txt
│   │   └── ohmfilter.cpp
│   ├── ohmheightmap/
│   │   ├── CMakeLists.txt
│   │   └── ohmheightmap.cpp
│   ├── ohmhm2img/
│   │   ├── CMakeLists.txt
│   │   └── ohmhm2img.cpp
│   ├── ohminfo/
│   │   ├── CMakeLists.txt
│   │   └── ohminfo.cpp
│   ├── ohmpop/
│   │   ├── CMakeLists.txt
│   │   ├── OhmPopConfig.in.h
│   │   ├── ohmpopmaincpu.cpp
│   │   └── ohmpopmaingpu.cpp
│   ├── ohmprob/
│   │   ├── CMakeLists.txt
│   │   └── ohmprob.cpp
│   ├── ohmquery/
│   │   ├── CMakeLists.txt
│   │   ├── OhmQueryConfig.in.h
│   │   └── ohmquery.cpp
│   └── ohmsubmap/
│       ├── CMakeLists.txt
│       └── ohmsubmap.cpp
└── vcpkg.json

================================================
FILE CONTENTS
================================================

================================================
FILE: .clang-format
================================================
# Expected version: clang-format-10
Language: Cpp
BasedOnStyle: Google

AccessModifierOffset: -2
AlignAfterOpenBracket: Align
AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: false
AlignConsecutiveMacros: true
AlignEscapedNewlines: Left
AlignOperands: true
AlignTrailingComments: true
AllowAllArgumentsOnNextLine: false
AllowAllConstructorInitializersOnNextLine: true
AllowAllParametersOfDeclarationOnNextLine: true
AllowShortBlocksOnASingleLine: Never
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: InlineOnly
AllowShortIfStatementsOnASingleLine: Never
AllowShortLambdasOnASingleLine: All
AllowShortLoopsOnASingleLine: false
AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: false
AlwaysBreakTemplateDeclarations: Yes
BinPackArguments: true
BinPackParameters: true
BreakBeforeBraces: Custom
BraceWrapping:
  AfterCaseLabel: true
  AfterClass: true
  AfterControlStatement: true
  AfterEnum: true
  AfterFunction: true
  AfterNamespace: true
  AfterStruct: true
  AfterUnion: true
  # AfterExternBlock: false
  BeforeCatch: true
  BeforeElse: true
  IndentBraces: false
  SplitEmptyFunction: false
  SplitEmptyRecord: true
  SplitEmptyNamespace: false
BreakBeforeBinaryOperators: None
BreakBeforeInheritanceComma: false
BreakBeforeTernaryOperators: false
BreakConstructorInitializers: BeforeComma
BreakInheritanceList: BeforeComma
BreakStringLiterals: true
ColumnLimit: 120
CompactNamespaces: false
ConstructorInitializerAllOnOneLineOrOnePerLine: false
ConstructorInitializerIndentWidth: 2
ContinuationIndentWidth: 2
Cpp11BracedListStyle: false
DerivePointerAlignment: false
FixNamespaceComments: true
IncludeBlocks: Preserve
# IncludeCategories:
#   - Regex:           '^"(llvm|llvm-c|clang|clang-c)/'
#     Priority:        2
#   - Regex:           '^(<|"(gtest|gmock|isl|json)/)'
#     Priority:        3
#   - Regex:           '.*'
#     Priority:        1
# IncludeCategories: TODO
# IncludeMainReges: TODO
IndentCaseLabels: false
IndentGotoLabels: false
# IndentPPDirectives: None
IndentWidth: 2
IndentWrappedFunctionNames: true
KeepEmptyLinesAtTheStartOfBlocks: false
# MacroBlockBegin: <strings>
# MacroBlockEnd: <strings>
MaxEmptyLinesToKeep: 2
NamespaceIndentation: None
# PenaltyBreakAssignment: <int>
# PenaltyBreakBeforeFirstCallParameter: <int>
# PenaltyBreakComment: <int>
# PenaltyBreakFirstLessLess: <int>
# PenaltyBreakString: <int>
# PenaltyExcessCharacter: <int>
# PenaltyReturnTypeOnItsOwnLine: <int>
PointerAlignment: Right
ReflowComments: true
SortIncludes: true
SortUsingDeclarations: true
SpaceAfterCStyleCast: false
SpaceAfterLogicalNot: false
SpaceAfterTemplateKeyword: true
SpaceBeforeAssignmentOperators: true
SpaceBeforeCpp11BracedList: false
SpaceBeforeCtorInitializerColon: true
SpaceBeforeInheritanceColon: true
SpaceBeforeParens: ControlStatements
SpaceBeforeRangeBasedForLoopColon: true
SpaceBeforeSquareBrackets: false
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 2
SpacesInAngles: false
SpacesInCStyleCastParentheses: false
SpacesInConditionalStatement: false
SpacesInContainerLiterals: true
SpacesInParentheses: false
SpacesInSquareBrackets: false
Standard: Auto
TabWidth: 2
UseCRLF: false
UseTab: Never


================================================
FILE: .clang-tidy
================================================
# Run clang tidy general analysis, excluding variable naming checks.
---
Checks: >
  clang-diagnostic-*,
  clang-analyzer-*,
  boost-use-to-string,
  bugprone-*,
  cert-dcl58-cpp,
  cert-env33-c,
  cert-err34-c,
  cert-err52-cpp,
  cert-err58-cpp,
  cert-mem57-cpp,
  cert-oop57-cpp,
  cert-oop58-cpp,
  cppcoreguildelines-*,
  -cppcoreguidelines-macro-usage,
  -cppcoreguidelines-pro-bounds-pointer-arithmatic,
  google-*,
  -google-runtime-references,
  llvm-include-order,
  llvm-namespace-comment,
  misc-*,
  -misc-no-recursion,
  -misc-non-private-member-variables-in-classes,
  -misc-unused-parameters,
  modernize-*,
  -modernize-use-trailing-return-type,
  performance-*,
  readability-*,
  -readability-implicit-bool-conversion,
  -readability-uppercase-literal-suffix,
  hicpp-signed-bitwise
WarningsAsErrors: ''
HeaderFilterRegex: '.*'
AnalyzeTemporaryDtors: false
FormatStyle:     none
User: csiro
CheckOptions:
  - key: bugprone-argument-comment.StrictMode
    value: '1'
  - key: bugprone-misplaced-widening-cast.CheckImplicitCasts
    value: '1'
  - key: performance-move-constructor-init.IncludeStyle
    value: 'llvm'
  - key: cppcoreguidelines-special-member-functions.AllowSoleDefaultDtor
    value: '1'
  - key: cppcoreguidelines-special-member-functions.AllowMissingMoveFunctions
    value: '1'
  - key: google-runtime-int.TypeSuffix
    value: '_t'
  - key: modernize-use-default-member-init.UseAssignment
    value: '1'
  # ---------------------------------------------------------------------------
  # Classes and structs
  # Class names
  - key:  readability-identifier-naming.ClassCase
    value: CamelCase
  # Structs
  - key:  readability-identifier-naming.StructCase
    value: CamelCase
  # static const class members
  - key:  readability-identifier-naming.ClassConstantCase
    value: CamelCase
  - key:  readability-identifier-naming.ClassConstantPrefix
    value: 'k'
  # static class members (non-const)
  - key:  readability-identifier-naming.ClassMemberCase
    value: lower_case
  # Class members which are const, but not static - omitted to control by access scope (public, private, etc)
  # - key:  readability-identifier-naming.ConstantMemberCase
  #   value: lower_case
  # Class member functions - any access modifier (public, private, etc)
  - key:  readability-identifier-naming.ClassMethodCase
    value: camelBack
  # Class member variable catchall
  - key:  readability-identifier-naming.MemberCase
    value: lower_case
  # Private class member variables
  - key:  readability-identifier-naming.PrivateMemberCase
    value: lower_case
  - key:  readability-identifier-naming.PrivateMemberSuffix
    value: '_'
  # Protected member variables
  - key:  readability-identifier-naming.ProtectedMemberCase
    value: lower_case
  - key:  readability-identifier-naming.ProtectedMemberSuffix
    value: '_'

  # ---------------------------------------------------------------------------
  # Enum declaration name case
  - key:  readability-identifier-naming.EnumCase
    value: CamelCase
  # Enum value declarations (the stuff inside the enum)
  - key:  readability-identifier-naming.EnumConstantCase
    value: CamelCase
  - key:  readability-identifier-naming.EnumConstantPrefix
    value: 'k'

  # ---------------------------------------------------------------------------
  # Templates
  - key:  readability-identifier-naming.TemplateParameterCase
    value: CamelCase
  # - key:  readability-identifier-naming.TemplateTemplateParameterCase
  #   value: CamelCase
  # - key:  readability-identifier-naming.TypeTemplateParameterCase
  #   value: CamelCase
  # - key:  readability-identifier-naming.ValueTemplateParameterCase
  #   value: CamelCase


  # ---------------------------------------------------------------------------
  # General and global
  # constexpr variable assignments
  - key:  readability-identifier-naming.ConstexprVariableCase
    value: CamelCase
  - key:  readability-identifier-naming.ConstexprVariablePrefix
    value: 'k'
  # Namespaces
  - key:  readability-identifier-naming.NamespaceCase
    value: lower_case
  # General function parameter names
  - key:  readability-identifier-naming.ParameterCase
    value: lower_case
  # Union names
  - key:  readability-identifier-naming.UnionCase
    value: CamelCase
  # General variable declarations
  - key:  readability-identifier-naming.VariableCase
    value: lower_case
  # Typedef names
  - key:  readability-identifier-naming.TypedefCase
    value: CamelCase
  # Names for type aliases: using Name = OtherThing;
  # Includes aliases declared in classes.
  - key:  readability-identifier-naming.TypeAliasCase
    value: CamelCase
  # Free function case
  - key:  readability-identifier-naming.FunctionCase
    value: camelBack
  # Global/free constant variable case. Includes anynomous namespaces.
  - key:  readability-identifier-naming.GlobalConstantCase
    value: CamelCase
  - key:  readability-identifier-naming.GlobalConstantPrefix
    value: 'k'
  # Variables in the global scope.
  - key:  readability-identifier-naming.GlobalVariableCase
    value: lower_case
  - key:  readability-identifier-naming.GlobalVariablePrefix
    value: 'g_'
  # Constants declared within local function scopes. Same as normal variables.
  - key:  readability-identifier-naming.LocalConstantCase
    value: lower_case
  - key:  readability-identifier-naming.LocalConstantPrefix
    value: ''
  # Local variable
  - key:  readability-identifier-naming.LocalVariableCase
    value: lower_case

  # Magic number ignore list
  - key: readability-magic-numbers.IgnoredIntegerValues
    value: 0;1;2;3;4;5;6;7;8;9;10;100;1000;16;32;64;128;255;512;1024;2048;4096;8192;16384;32768;32768
  - key: readability-magic-numbers.IgnorePowersOf2IntegerValues
    value: true
  - key: readability-magic-numbers.IgnoredFloatingPointValues
    value: 0.25;0.5;0.75;1.0;2.0;3.0;10.0;100.0;1000.0;0.1;0.001;0.0001;;-1.0;-2.0;-3.0;
...


================================================
FILE: .gitignore
================================================
.vscode/
.vs/
build*/
# Visual Studio output directory for CMake projects.
out/
vcpkg_installed/


================================================
FILE: 3rdparty/3es-core/3esservermacros.h
================================================
// 3rd Eye Scene macros
// This file disables all 3rd Eye Scene macros. To be used when the library is not available.

#ifndef _3ESSERVERMACROS_H
#define _3ESSERVERMACROS_H

namespace tes
{
  inline void noopohm() {}
}  // namespace tes

#define TES_STMT(statement) tes::noopohm()
#define TES_IF(condition) if (false)
#define TES_RGB(r, g, b) tes::noopohm()
#define TES_RGBA(r, g, b, a) tes::noopohm()
#define TES_COLOUR(name) tes::noopohm()
#define TES_COLOUR_I(index) tes::noopohm()
#define TES_COLOUR_A(name, a) tes::noopohm()
#define TES_BUFFER(...) tes::noopohm()

#define TES_CATEGORY(server, ...) tes::noopohm()
#define TES_SERVER_DECL(server) tes::noopohm()
#define TES_SETTINGS(server, ...) tes::noopohm()
#define TES_SERVER_INFO(server, ...) tes::noopohm()
#define TES_SERVER_INFO_TIME(server, ...) tes::noopohm()
#define TES_SERVER_CREATE(server, ...) tes::noopohm()
#define TES_SERVER_START(server, ...) tes::noopohm()
#define TES_SERVER_START_WAIT(server, ...) tes::noopohm()
#define TES_SET_CONNECTION_CALLBACK(...) tes::noopohm()
#define TES_SERVER_UPDATE(server, ...) tes::noopohm()
#define TES_SERVER_STOP(server) tes::noopohm()
#define TES_LOCAL_FILE_STREAM(server, ...) tes::noopohm()
#define TES_ACTIVE(server) false
#define TES_SET_ACTIVE(server, ...) tes::noopohm()

#define TES_FEATURE(feature) false
#define TES_FEATURE_FLAG(feature) 0
#define TES_FEATURES(featureFlags) tes::noopohm()
#define TES_IF_FEATURES(featureFlags, ...) tes::noopohm()

#define TES_REFERENCE_RESOURCE(server, ...) tes::noopohm()
#define TES_RELEASE_RESOURCE(server, ...) tes::noopohm()
#define TES_MESH_PLACEHOLDER(id) tes::noopohm()

#define TES_ARROW(server, ...) tes::noopohm()
#define TES_ARROW_T(server, ...) tes::noopohm()
#define TES_ARROW_W(server, ...) tes::noopohm()
#define TES_BOX_AABB(server, ...) tes::noopohm()
#define TES_BOX_AABB_T(server, ...) tes::noopohm()
#define TES_BOX_AABB_W(server, ...) tes::noopohm()
#define TES_BOX(server, ...) tes::noopohm()
#define TES_BOX_T(server, ...) tes::noopohm()
#define TES_BOX_W(server, ...) tes::noopohm()
#define TES_CAPSULE(server, ...) tes::noopohm()
#define TES_CAPSULE_T(server, ...) tes::noopohm()
#define TES_CAPSULE_W(server, ...) tes::noopohm()
#define TES_CONE(server, ...) tes::noopohm()
#define TES_CONE_T(server, ...) tes::noopohm()
#define TES_CONE_W(server, ...) tes::noopohm()
#define TES_CYLINDER(server, ...) tes::noopohm()
#define TES_CYLINDER_T(server, ...) tes::noopohm()
#define TES_CYLINDER_W(server, ...) tes::noopohm()
#define TES_LINES(server, ...) tes::noopohm()
#define TES_LINES_E(server, ...) tes::noopohm()
#define TES_LINE(server, ...) tes::noopohm()
#define TES_MESHSET(server, ...) tes::noopohm()
#define TES_PLANE(server, ...) tes::noopohm()
#define TES_PLANE_T(server, ...) tes::noopohm()
#define TES_PLANE_W(server, ...) tes::noopohm()
#define TES_POINTCLOUDSHAPE(server, ...) tes::noopohm()
#define TES_POINTS(server, ...) tes::noopohm()
#define TES_POINTS_C(server, ...) tes::noopohm()
#define TES_POINTS_E(server, ...) tes::noopohm()
#define TES_POSE(server, ...) tes::noopohm()
#define TES_POSE_T(server, ...) tes::noopohm()
#define TES_POSE_W(server, ...) tes::noopohm()
#define TES_SPHERE(server, ...) tes::noopohm()
#define TES_SPHERE_T(server, ...) tes::noopohm()
#define TES_SPHERE_W(server, ...) tes::noopohm()
#define TES_STAR(server, ...) tes::noopohm()
#define TES_STAR_T(server, ...) tes::noopohm()
#define TES_STAR_W(server, ...) tes::noopohm()
#define TES_TEXT2D_SCREEN(server, ...) tes::noopohm()
#define TES_TEXT2D_WORLD(server, ...) tes::noopohm()
#define TES_TEXT3D(server, ...) tes::noopohm()
#define TES_TEXT3D_FACING(server, ...) tes::noopohm()
#define TES_TRIANGLES(server, ...) tes::noopohm()
#define TES_TRIANGLES_E(server, ...) tes::noopohm()
#define TES_TRIANGLES_N(server, ...) tes::noopohm()
#define TES_TRIANGLES_NE(server, ...) tes::noopohm()
#define TES_TRIANGLES_W(server, ...) tes::noopohm()
#define TES_TRIANGLES_WE(server, ...) tes::noopohm()
#define TES_TRIANGLES_T(server, ...) tes::noopohm()
#define TES_TRIANGLES_TE(server, ...) tes::noopohm()
#define TES_TRIANGLE(server, ...) tes::noopohm()
#define TES_TRIANGLE_W(server, ...) tes::noopohm()
#define TES_TRIANGLE_T(server, ...) tes::noopohm()
#define TES_VOXELS(server, ...) tes::noopohm()

#define TES_ARROW_END(server, ...) tes::noopohm()
#define TES_BOX_END(server, ...) tes::noopohm()
#define TES_CAPSULE_END(server, ...) tes::noopohm()
#define TES_CONE_END(server, ...) tes::noopohm()
#define TES_CYLINDER_END(server, ...) tes::noopohm()
#define TES_LINES_END(server, ...) tes::noopohm()
#define TES_MESHSET_END(server, ...) tes::noopohm()
#define TES_PLANE_END(server, ...) tes::noopohm()
#define TES_POINTCLOUDSHAPE_END(server, ...) tes::noopohm()
#define TES_POINTS_END(server, ...) tes::noopohm()
#define TES_POSE_END(server, ...) tes::noopohm()
#define TES_VOXELS_END(server, ...) tes::noopohm()
#define TES_SPHERE_END(server, ...) tes::noopohm()
#define TES_STAR_END(server, ...) tes::noopohm()
#define TES_TEXT2D_END(server, ...) tes::noopohm()
#define TES_TEXT3D_END(server, ...) tes::noopohm()
#define TES_TRIANGLES_END(server, ...) tes::noopohm()
#define TES_TRIANGLE_END(server, ...) tes::noopohm()
#define TES_VOXELS_END(server, ...) tes::noopohm()

#define TES_POS_UPDATE(server, ...) tes::noopohm()
#define TES_ROT_UPDATE(server, ...) tes::noopohm()
#define TES_SCALE_UPDATE(server, ...) tes::noopohm()
#define TES_COLOUR_UPDATE(server, ...) tes::noopohm()
#define TES_COLOR_UPDATE(server, ...) tes::noopohm()
#define TES_POSROT_UPDATE(server, ...) tes::noopohm()
#define TES_POSSCALE_UPDATE(server, ...) tes::noopohm()
#define TES_ROTSCALE_UPDATE(server, ...) tes::noopohm()
#define TES_PRS_UPDATE(server, ...) tes::noopohm()
#define TES_PRC_UPDATE(server, ...) tes::noopohm()
#define TES_PSC_UPDATE(server, ...) tes::noopohm()
#define TES_RSC_UPDATE(server, ...) tes::noopohm()
#define TES_PRSC_UPDATE(server, ...) tes::noopohm()

#endif  // _3ESSERVERMACROS_H


================================================
FILE: CMakeLists.txt
================================================
cmake_minimum_required(VERSION 3.10)
project(ohm)

# Set CMAKE_CXX_STANDARD to the minimum if not externally set - e.g., from command line.
# We'll set it in such a way that it remains it the Cache and stays set.
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14 CACHE INTERNAL "C++ standard")
endif(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD_REQUIRED TRUE)
# Ensure -fPIC is added.
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

# Read version from package.xml
file(READ package.xml ohm_VERSION)
string(REGEX MATCH "\\<version\\>(.*)\\</version\\>" ohm_VERSION ${ohm_VERSION})
string(REGEX REPLACE "\\<version\\>(.*)\\</version\\>" "\\1" ohm_VERSION ${ohm_VERSION})

list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
include(utils)

# Work out if we are using VCPKG.
set(OHM_VCPKG FALSE)
if(CMAKE_TOOLCHAIN_FILE MATCHES ".*vcpkg\.cmake;?.*")
  set(OHM_VCPKG TRUE)
endif(CMAKE_TOOLCHAIN_FILE MATCHES ".*vcpkg\.cmake;?.*")

# Setup core ohm build options.
# Default to building shared libs.
set(OHM_BUILD_SHARED_DEFAULT OFF)
if(DEFINED BUILD_SHARED_LIBS)
  # But use BUILD_SHARED_LIBS as the default if it's specified (like from the command line).
  set(OHM_BUILD_SHARED_DEFAULT ${BUILD_SHARED_LIBS})
endif(DEFINED BUILD_SHARED_LIBS)
option(OHM_BUILD_SHARED "Build shared ohm libraries?" ${OHM_BUILD_SHARED_DEFAULT})
# For details on OHM_BUILD_SHARED_SUPPRESS_WARNINGS, see usage in compilerSetup.cmake
option(OHM_BUILD_SHARED_SUPPRESS_WARNINGS
  "Suppress warnings relating the DLL export symbols when building shared? Disables MSC warnings C4251 and C4275" ON)

option(OHM_PROFILE "Enable timing information messages for some queries." Off)
option(OHM_TES_DEBUG "Enable visual debugging code?" Off)
option(OHM_VALIDATION "Enable various validation tests in the occupancy map code. Has some performance impact." Off)
# OHM_FEATURE_CUDA is found in OhmCuda.cmake
option(OHM_LEAK_TRACK "Enable memory leak tracking?" OFF)

# Build options; what extensions are we going to build. These options are authoritative and flow down from here.
# For CUDA and OpenCL, we only need one, and we prefer CUDA based on what we find. However, vcpkg can guarantee we'll
# find one, so it because a dependent option.
option(OHM_BUILD_DOXYGEN "Build doxygen documentation?" Off)

ohm_feature(EIGEN "Use Eigen for more significant linear algebra algorithms (e.g., eigen decomposition)?" FIND Eigen3)
ohm_feature(HEIGHTMAP "Build heightmap library?")
ohm_feature(HEIGHTMAP_IMAGE "Build heightmap image conversion?" FIND OpenGL GLEW glfw3)
ohm_feature(PDAL "Build with PDAL reader support?" FIND pdal)
ohm_feature(THREADS "Enable CPU threading (using Thread Building Blocks)?" FIND tbb)
ohm_feature(TEST "Build unit tests?" FIND GTest)

# include CUDA config here to prime OHM_FEATURE_CUDA as we prefer CUDA over OpenCL.
# OHM_FEATURE_CUDA is in OhmCuda.cmake
include(OhmCuda)
set(OHM_FEATURE_OPENCL_DEFAULT ON)
if(OHM_FEATURE_CUDA)
  set(OHM_FEATURE_OPENCL_DEFAULT OFF)
endif(OHM_FEATURE_CUDA)
ohm_feature(OPENCL "Build with OpenCL support?" FIND OpenCL DEFAULT OHM_FEATURE_OPENCL_DEFAULT)

# # Windows vcpkg does not provide CUDA.
# add_vcpkg_feature_if("cuda" "OHM_CUDA AND NOT WIN32")

# Migrate ohm specific setting for building shared libraries to the CMake global option.
set(BUILD_SHARED_LIBS ${OHM_BUILD_SHARED})
if(BUILD_SHARED_LIBS)
  message(STATUS "Building shared ohm libraries.")
endif(BUILD_SHARED_LIBS)

# Setup clang-tidy
include(ClangTidy)

# Allow the use of folders to group targets in supporting environments.
# For example Visual Studio solution folders.
set_property(GLOBAL PROPERTY USE_FOLDERS ON)

set(CMAKE_DEBUG_POSTFIX _d)
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib")
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin")
foreach(CONF Debug;Release;MinSizeRel;RelWithDebInfo)
  string(TOUPPER ${CONF} CONFU)
  set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY_${CONFU} "${CMAKE_BINARY_DIR}/lib")
  set(CMAKE_LIBRARY_OUTPUT_DIRECTORY_${CONFU} "${CMAKE_BINARY_DIR}/bin")
  set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_${CONFU} "${CMAKE_BINARY_DIR}/bin")
endforeach(CONF)

# Add memory leak tracking support.
include(LeakTrack)

# Configure installation prefixes.
# set(OHM_PREFIX_PACKAGE lib/cmake/${CMAKE_PROJECT_NAME})
set(OHM_PREFIX_PACKAGE share/${CMAKE_PROJECT_NAME}/cmake) # For catkin interoperation
# Prefix for include directories.
set(OHM_PREFIX_INCLUDE include/ohm)

# General, required library
# We use our own FindGLM to cater for differences between Ubuntu 18.04 and 20.04.
# From 20.04 on, we can just just find_config(glm CONFIG REQUIRED)
find_package(glm MODULE REQUIRED)

if(OHM_FEATURE_OPENCL)
  find_package(OpenCL)
endif(OHM_FEATURE_OPENCL)
if(OpenCL_FOUND)
  option(OHM_EMBED_GPU_CODE "Embed GPU code into the executable (OpenCL)? Otherwise GPU code files are loaded from disk (better for development)." ON)

  set(OHM_OPENCL_SDK_VER 1.2 CACHE STRING "Select the OpenCL runtime API to use.")
  set_property(CACHE OHM_OPENCL_SDK_VER PROPERTY STRINGS 1.2 2.0 2.1 2.2 3.0)

  set(OHM_OPENCL_STD 2.0 CACHE STRING "Select the default OpenCL standard to compile GPU code against. Also runtime configurable. [max] uses the reported device allowing for 2.x requires features.")
  set_property(CACHE OHM_OPENCL_STD PROPERTY STRINGS 1.2 2.0 3.0 max)
endif(OpenCL_FOUND)

# Identify any extensions required to compile with OpenCL 2.0 std.
set(OHM_OPENCL_2_FEATURES "" CACHE STRING "Features required for OpenCL 2.x. Fallback to 1.2 otherwise.")

# Look for CUDA to setup CUDA build options and work out which CMake CUDA paradigm we'll use.
if(OHM_FEATURE_CUDA)
  if(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)
    find_package(CUDAToolkit REQUIRED)
  else(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)
    # Need to find CUDA for deprecated project configuration
    find_package(CUDA REQUIRED)
  endif(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)
endif(OHM_FEATURE_CUDA)

if(OHM_FEATURE_EIGEN)
  find_package(Eigen3 CONFIG REQUIRED)
endif(OHM_FEATURE_EIGEN)

# Manage compiler warnings.
# Use CMAKE_MODULE_PATH and include(warnings) if warnings.cmake is moved.
include(compilerSetup)

# Now setup CUDA after knowing if we are building shared libraries or not.
cuda_setup()

# Setup default memory leak tracking suppressions and options (GCC/Clang AddressSanitizer).
set(OHM_LEAK_SUPPRESS_OCL_INIT
  # OpenCL memory allocation overrides
  "__interceptor_malloc"
  "__interceptor_calloc"
  # Intel OpenCL leaks
  "libigdrcl"
  "llvm::FPPassManager::runOnFunction"
  "llvm::SmallVectorBase"
  "libigc"
  "basic_string<char, std::char_traits<char>, std::allocator<char> >::_M_mutate"
)

set(OHM_LEAK_SUPPRESS_TBB_INIT
  "libtbb"
)

set(OHM_LEAK_SUPPRESS_CUDA_INIT
)

set(OHM_ASAN_OPTIONS_CUDA_INIT "protect_shadow_gap=0")

set(OHM_LEAK_SUPPRESS_OCL "${OHM_LEAK_SUPPRESS_OCL_INIT}" CACHE STRING "Memory leak suppression list for OpenCL")
set(OHM_LEAK_SUPPRESS_TBB "${OHM_LEAK_SUPPRESS_TBB_INIT}" CACHE STRING "Memory leak suppression list for Intel Threading Building Blocks")
set(OHM_LEAK_SUPPRESS_CUDA "${OHM_LEAK_SUPPRESS_CUDA_INIT}" CACHE STRING "Memory leak suppression list for CUDA")
set(OHM_ASAN_OPTIONS_CUDA "${OHM_ASAN_OPTIONS_CUDA_INIT}" CACHE STRING "Address Sanitizer options for CUDA")

# Configure OHM_GPU as a value for configure_file
set(OHM_GPU_OPENCL 1)
set(OHM_GPU_CUDA 2)
set(OHM_GPU 0)

# Configure use of 3rd-Eye-Scene visual debugging (use for development only)
if(OHM_TES_DEBUG)
  find_package(3es)
else(OHM_TES_DEBUG)
  # Include path to 3esservermarcos.h which effectively disables all 3es macros.
  set(3ES_INCLUDE_DIRS "${CMAKE_CURRENT_LIST_DIR}/3rdparty/3es-core")
endif(OHM_TES_DEBUG)

add_subdirectory(logutil)

if(OHM_FEATURE_OPENCL)
  add_subdirectory(clu)
endif(OHM_FEATURE_OPENCL)
if(OHM_FEATURE_CUDA OR OHM_FEATURE_OPENCL)
  add_subdirectory(gputil)
endif(OHM_FEATURE_CUDA OR OHM_FEATURE_OPENCL)

add_subdirectory(ohmutil)
add_subdirectory(slamio)
add_subdirectory(ohm)
if(OHM_FEATURE_CUDA OR OHM_FEATURE_OPENCL)
  add_subdirectory(ohmgpu)
endif(OHM_FEATURE_CUDA OR OHM_FEATURE_OPENCL)
if(OHM_FEATURE_HEIGHTMAP)
  add_subdirectory(ohmheightmap)
endif(OHM_FEATURE_HEIGHTMAP)

if(OHM_FEATURE_HEIGHTMAP_IMAGE)
  # Experimental
  add_subdirectory(ohmheightmapimage)
endif(OHM_FEATURE_HEIGHTMAP_IMAGE)

add_subdirectory(ohmtools)
add_subdirectory(ohmapp)
add_subdirectory(utils)

if(OHM_FEATURE_TEST)
  enable_testing()
  add_subdirectory(tests)
endif(OHM_FEATURE_TEST)

clang_tidy_global()

export(EXPORT ${CMAKE_PROJECT_NAME}-config-targets
  FILE "${CMAKE_CURRENT_BINARY_DIR}/${CMAKE_PROJECT_NAME}-config-targets.cmake"
  NAMESPACE ohm::
)

install(EXPORT ${CMAKE_PROJECT_NAME}-config-targets
  FILE ${CMAKE_PROJECT_NAME}-config-targets.cmake
  NAMESPACE ohm::
  DESTINATION ${OHM_PREFIX_PACKAGE}
)

# Setup import scripts.
include(CMakePackageConfigHelpers)
write_basic_package_version_file(
  "${CMAKE_CURRENT_BINARY_DIR}/${CMAKE_PROJECT_NAME}-version.cmake"
  VERSION ${ohm_VERSION}
  COMPATIBILITY AnyNewerVersion
)

# Installation of the package configuration file.
configure_file(cmake/${CMAKE_PROJECT_NAME}-packages.cmake "${CMAKE_CURRENT_BINARY_DIR}/${CMAKE_PROJECT_NAME}-packages.cmake")
configure_file(cmake/${CMAKE_PROJECT_NAME}-config.cmake "${CMAKE_CURRENT_BINARY_DIR}/${CMAKE_PROJECT_NAME}-config.cmake" @ONLY)
install(FILES
    "${CMAKE_CURRENT_BINARY_DIR}/${CMAKE_PROJECT_NAME}-config.cmake"
    "${CMAKE_CURRENT_BINARY_DIR}/${CMAKE_PROJECT_NAME}-packages.cmake"
    "${CMAKE_CURRENT_BINARY_DIR}/${CMAKE_PROJECT_NAME}-version.cmake"
  DESTINATION ${OHM_PREFIX_PACKAGE}
  COMPONENT Devel)

# Install MSVC runtime libraries. This will also affect the CPack installation.
include(InstallRequiredSystemLibraries)

# Doxygen setup.
if(OHM_BUILD_DOXYGEN)
  # Include Doxygen helper functions. This also finds the Doxygen package.
  include(doxygen)

  set(DOXYGEN_DIRS
    gputil
    ohm
    ohmtools
    ohmutil
    slamio
  )

  set(DOXYGE_EXCLUDE_DIRS
    clu/3rdparty
    gputil/cl
    gputil/cuda
    ohm/cl
    ohm/gpu
    ohm/private
    ohmgpu/private
    ohmutil/3rdparty
    slamio/miniply
    slamio/pdal
  )

  if(OHM_FEATURE_CUDA OR OHM_FEATURE_OPENCL)
    list(APPEND DOXYGEN_DIRS ohmgpu)
  endif(OHM_FEATURE_CUDA OR OHM_FEATURE_OPENCL)
  if(OHM_FEATURE_OPENCL)
    list(APPEND DOXYGEN_DIRS clu)
  endif(OHM_FEATURE_OPENCL)

  list(APPEND DOXYGEN_DIRS docs)

  if(DOXYGEN_FOUND)
    # Create a target to build the documentation.
    # Here we also setup various documentation variables passed through to the doxyfile configuration.
    # Each named argument below describes the Doxygen variable it sets.
    doxygen_create(
      # DOXYFILE cmake/doxyfile.in  # Doxyfile to configure.
      PROJECT ${CMAKE_PROJECT_NAME} # PROJECT_NAME
      VERSION ${ohm_VERSION}        # PROJECT_NUMBER
      OUTPUT_DIR html               # HTML_OUPTUT
      # CSS <style>.css             # HTML_STYLESHEET
      PUBLISHER "CSIRO"             # DOCSET_PUBLISHER_NAME
      PUBLISHER_ID au.csiro         # DOCSET_PUBLISHER_ID
      PROJECT_ID au.csiro.ohm       # DOCSET_BUNDLE_ID, QHP_NAMESPACE, ECLIPSE_DOC_ID
      PATHS                         # INPUT (RECURSIVE is on)
        ${DOXYGEN_DIRS}
      EXCLUDE_PATHS                 # EXCLUDE
        ${DOXYGE_EXCLUDE_DIRS}
      # Where to find source code examples.
      # EXAMPLE_PATHS <paths>        # EXAMPLE_PATH
      # Where to find images.
      # IMAGE_PATHS <paths>          # IMAGE_PATH
    )

    # Setup installation of the generated documentation: source, destination.
    doxygen_install(${CMAKE_CURRENT_BINARY_DIR}/html ohm)
  endif(DOXYGEN_FOUND)
endif(OHM_BUILD_DOXYGEN)


================================================
FILE: LICENSE
================================================
The Software is copyright (c) Commonwealth Scientific and Industrial Research Organisation (CSIRO)
ABN 41 687 119 230.

CSIRO grants you a licence to the Software on the terms of the BSD 3-Clause Licence.

Redistribution and use in source and binary forms, with or without modification, are permitted
provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions
  and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions
  and the following disclaimer in the documentation and/or other materials provided with the
  distribution.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse
  or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

___________________________________________________________________
ZLib https://www.zlib.net/
his software is licensed under ZLib license
https://www.zlib.net/zlib_license.html
___________________________________________________________________

___________________________________________________________________
Miniply https://github.com/vilya/miniply
This software is licensed under MIT License.
___________________________________________________________________

___________________________________________________________________
Point Data Abstraction Library (PDAL) https://pdal.io/
This software is licensed under BSD Licence
___________________________________________________________________

___________________________________________________________________
OpenGL Mathematics https://glm.g-truc.net/
This software is licensed under the Happy Bunny License (Modified MIT).
___________________________________________________________________

___________________________________________________________________
Intel Threading Building Blocks https://www.threadingbuildingblocks.org/
This software is licensed under the Apache 2.0 license.
___________________________________________________________________

___________________________________________________________________
Google Test https://github.com/google/googletest
This software is licensed under the New BSD License.
___________________________________________________________________

___________________________________________________________________
CMake https://cmake.org/
This software is licensed under the BSD 3-Clause license.
___________________________________________________________________

___________________________________________________________________
cxxopts https://github.com/jarro2783/cxxopts
This software is licensed under the cxxopts licence
https://github.com/jarro2783/cxxopts/blob/master/LICENSE
___________________________________________________________________

___________________________________________________________________
Third Eye Scene
This software is licensed under a modified MIT license:
https://github.com/data61/3rdEyeScene/blob/master/license.txt
___________________________________________________________________

___________________________________________________________________
delaunator https://github.com/delfrrr/delaunator-cpp
This software is licensed under the MIT licence
https://github.com/delfrrr/delaunator-cpp/blob/master/LICENSE
___________________________________________________________________

___________________________________________________________________
libpng http://www.libpng.org/
This software is licensed under the PNG Reference Library License version
___________________________________________________________________

___________________________________________________________________
OpenGL Extension Wrangler (GLEW) Library http://glew.sourceforge.net/
This software is licensed under The OpenGL Extension Wrangler Library license
___________________________________________________________________

___________________________________________________________________
GLFW Library https://www.glfw.org/
This software is licensed under the zlib/libpng license
___________________________________________________________________

___________________________________________________________________
SKA fast hash https://github.com/skarupke/flat_hash_map
This software is licensed under the Boost Software License, Version 1.0.
___________________________________________________________________

___________________________________________________________________
SKA fast sort https://github.com/skarupke/ska_sort
This software is licensed under the Boost Software License, Version 1.0.
___________________________________________________________________

___________________________________________________________________
RPly https://github.com/lkiesow/librply
This software is license under the RPly license
___________________________________________________________________


================================================
FILE: OpenCL.md
================================================
# OpenCL Usage

OpenCL is one of the two GPU library options required for the Occupancy map (ohm). Most development has occurred on top of the Intel OpenCL implementation, but the NVIDIA implementation also works. These instructions focus on installation requirements for Intel OpenCL. For the NVIDIA support, download and install the [CUDA development kit](https://developer.nvidia.com/cuda-zone).

Both runtime drivers and an SDK need to be installed in order to get it running. Getting Intel drivers running can be an issue so the following instructions are maintained to help the installation process. The following process is focused entirely on Ubuntu 18.04.

## Windows SDK and Drvier Installation

The [Intel OpenCL SDK](https://software.intel.com/en-us/intel-opencl) is recommended for Windows development. Follow the installation instructions in the link.

Note that there are some pathing issues which may cause issues under Windows, especially when the CUDA SDK is also installed. It is recommended that the PATH environment variable is set up to find the Intel OpenCL DLLs and executables before the NVIDIA implementation is located.

OpenCL runtime drivers are installed as part of the video card driver package.

## Linux SDK Installation

The Intel OpenCL SDK is also available for Linux, however, for most Debian installations it is enough to install the appropriate apt packages provided the correct device drivers are installed.

```
sudo apt-get install opencl-headers ocl-icd-dev ocl-icd-libopencl1 ocl-icd-opencl-dev
```

### Installing Intel OpenCL Drivers

Extensive Intel CPU/GPU testing has not been made, but the following information may help address some OpenCL standard compatibility issues. The OpenCL standard defines the GPU code compilation, not the SDK version. This can be set on OHM_OPENCL_STD.

| Intel CPU Generation | Recommended OpenCL Standard |
| -------------------- | --------------------------- |
| < 6th Generation     | Unknown                     |
| 6th Generation       | 1.2                         |
| 7th Generation       | 2.0                         |
| 8th Generation       | 2.0                         |
| 9th Generation       | 2.0                         |

There are three different drivers which may be relevant to the installation process.

1. Intel Linux drivers:
    - Direct link: https://github.com/intel/compute-runtime/releases
    - General information https://software.intel.com/en-us/articles/opencl-drivers
2. beignet: apt package for Intel NEO drivers.
3. Intel Legacy Linux drivers: https://software.intel.com/en-us/articles/legacy-opencl-drivers#latest_linux_SDK_release

For general installation:

- Determine your CPU generation:
    - `grep -m 1 name /proc/cpuinfo`
    - Results should look something like: `Intel(R) Core(TM) i7-6700 CPU @ 3.40GHz` The first digit of the CPU number - in this case 6700 - indicates the generation '6'.
- Install the most appropriate driver (see below) based on your CPU generation.
    - For 6th generation use the legacy drivers (option 3).
    - For 7th+ generation use the current Intel Drives (option 1).
- Install `clinfo` apt package to confirm successful installation
    - `sudo apt-get install clinfo`
- Verify installation:
    - `clinfo | grep "Device Name"`

If installation is successful you should see a results like the following:

```
  Device Name                                     GeForce GT 640
  Device Name                                     Intel(R) HD Graphics
  Device Name                                     Intel(R) Core(TM) i7-6700 CPU @ 3.40GHz
```

Not that this example is also listing a CUDA device.

#### Intel Linux Driver

This installs the current Intel OpenCL drivers for 7th, 8th and 9th generation CPU. These are the perferred drivers as they are continually updated by Intel. Drivers may be downloaded from https://github.com/intel/compute-runtime/releases with accompanying installation instructions.

These drivers are incompatible with beignet.

#### Beignet

In 18.04 these are drivers for the NEO architecture used in 7th, 8th and 9th generation core processors. Run the following command to install the Beignet drivers:

```
sudo apt-get install beignet-dev
```

Verify the installation (above).

If this fails, you will need to `apt-get purge <package>` the following apt-packages before continuing:

- beignet
- beignet-dev
- beignet-opencl-icd

#### Intel Legacy Linux Driver

Legacy drivers are appropriate only for 6th (and 5th) generation intel chips. These are downloaded from https://software.intel.com/en-us/articles/legacy-opencl-drivers#latest_linux_SDK_release and the installation process is described at http://registrationcenter-download.intel.com/akdlm/irc_nas/11396/SRB5.0_intel-opencl-installation.pdf

These drivers are incompatible with beignet.

## NVidia OpenCL Compatibility

NVidia OpenCL requires installation of official NVidia drivers for your video card. Discussion of how to do so is beyond the scope of this document. Use `clinfo | grep "Device Name"` to determine whether NVidia drivers are correctly installed. You should see an NVidia related device name such as "GeForce XXX".

In order to build ohm to run on NVidia, the library must be build for OpenCL 1.2 (host API) and OpenCL 1.2 runtime (device standard). To do so, configure the following CMake variables to "1.2" using `ccmake`, `cmake-gui`, or editing the `CMakeCache.txt` file directly.

- `OHM_OPENCL_STD` : 1.2 (OpenCL runtime compilation)
- `OHM_OPENCL_SDK_VER` : 1.2 (OpenCL SDK selection)

Note that NVidia performance will generally be worse than Intel OpenCL because of the way ohm uses global memory.


================================================
FILE: clearance-performance-notes.md
================================================
# Performance Notes on Calculating Voxel Clearance Values

Here we note the techniques explored to general voxel clearance values on GPU and the associated performance impacts. The end goal has been to maximise performance whilst maintaining reasonable accuracy. A voxel's clearance value is the distance to its nearest occupied neighbour (or unknown neighbour depending on flags) within the specified search range.

## Techniques

Three primary techniques have been evaluated:

- Neighbour propagation
- Open list flood fill
- ROI propagation (Region of Interest propagation)


### Neighbour Propagation

This was the first technique and is essentially an iterative flood fill technique with no voxel contention. The region of interest (ROI) is defined as the map chunk/region we wish to generate clearance values for. This is padded by an appropriate number of voxels to reach the desired search range and the padded extents define the GPU working dimensions. Two buffers of voxels sized to the padded region are maintained and alternated to ensure new data do not overwrite old. The algorithm then proceeds:

1. Seed the first buffer with each voxel writing its own 3D index within the working region and 0 if it's not an obstruction (is free) or 1 if it is an obstruction (occupied or unknown and unknown flagged as obstacles).
2. Propagate obstacles with each voxel inspecting its face neighbours and selecting the nearest obstruction. Keep current value if it is a closer obstruction.
3. Iterate an appropriate number of times to fill out to the search range.
    1. Alternate read/write buffers each time.
4. Migrate results back to the CPU.

Pros:

- No contention
- Fixed iterations based on search range
- Fixed performance regardless of data content

Cons:

- Scalability: higher range searches make for higher GPU usage.
- High border overhead

### Open List Flood Fill

The next technique attempted. There an open list is generated from each obstructing voxel at processed at each iteration. This continued until the open list is empty. The ROI is padded just as in Neighbour Propagation and the padded region defines the working region on GPU.

1. Seed the working voxels similar to neighbour propagation, however the coordinates written are relative to the target voxel.
  1. Only every writes (0, 0, 0, 0) or (0, 0, 0, 1)
  2. Each obstruction pushes its face neighbours into an open list.
2. Process the open list, writing new obstructions to the open list voxels.
  1. Expand the open list with neighbours currently tracking farther obstacles or currently clear and within the search range.
3. Iterate on the open list until empty (or hard capped).

Pros:

- Reduced memory usage: working memory uses char4 rather than int4.
  - Must use 32-bit data type to support atomic CAS operations
- Lower GPU load: can fix number of workers
- Better scalability
- Less GPU thread overhead

Cons:

- CPU needed to determine whether another iteration is required and the number of workers.
- Higher number of GPU invocations, especially in worst case scenario
- Contention on writing to GPU nodes
- Limited search range because of char4 usage.

### ROI Propagation

A variant of Neighbour Propagation where we only have propagation iterations operate on the true ROI, not the padded region. This reduces the number of GPU threads to a fixed number and limits the maximum number of iterations required to cover the search range. Obstacles from outside the ROI, but within the search range have a separate seeding cycle which has memory contention, but greatly reduces the amount of work done. Uses alternating buffers just like Neighbour Propagation.

1. Seed the ROI (not padded) with each voxel writing *relative* coordinates to its nearest obstruction and flagging if it is an obstruction.
    1. Only every writes (0, 0, 0, 0) or (0, 0, 0, 1)
2. Seed obstacles from outside the ROI into nearest border voxels of the ROI
    1. Write to the nearest ROI border voxel and its face neighbour border voxels.
    2. This part has contention.
3. Propagate obstructions by having each ROI voxel inspect its face neighbours (and itself) and choose the closest obstruction.
4. Iterate an appropriate number of times to fill out to the search range or cover ROI from end to end (whichever is smaller)
    1. Alternate read/write buffers each time.
5. Migrate results back to the CPU.

Pros:

- Hybrid approach
- Far less work to do => best performance
- Reduced memory usage: working memory uses char4 rather than int4.
  - Must use 32-bit data type to support atomic CAS operations
- Fixed maximum number of iterations
- Reduce cost of scaling search range

Cons:

- Contention during seeding.
- Limited search range because of char4 usage.
- Seeding effects a down sampling which risks data loss.
  - This has been mitigated by seeding to ROI border neighbours as well as the closest ROI border voxel.

## Technique Evaluation

Evaluation using QCAT tower map data from 30-40s.

- Map resolution: 0.25
- Region size: (32, 32, 32) => (8m, 8m, 8m)

Technique             | Range | Total Time  | Average GPU Time/Region
--------------------- | ----- | ----------- | -----------------------
Neighbour propagation | 5m    | 76.6s       | 60.0ms
Neighbour propagation | 7.75m | 229.8s      | 191.7ms
Open list             | 5m    | 72.7s       | 60.7ms
Open list             | 7.75m | 108.0s      | 90.8ms
ROI propagation       | 5m    | 21.6s       | 18ms
ROI propagation       | 7.5m  | 30s         | 25ms
ROI propagation       | 15.5m | 78.5s       | 65.4ms


================================================
FILE: clu/3rdparty/CL/opencl.hpp
================================================
//
// Copyright (c) 2008-2020 The Khronos Group Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

/*! \file
 *
 *   \brief C++ bindings for OpenCL 1.0, OpenCL 1.1, OpenCL 1.2,
 *       OpenCL 2.0, OpenCL 2.1, OpenCL 2.2, and OpenCL 3.0.
 *   \author Lee Howes and Bruce Merry
 *
 *   Derived from the OpenCL 1.x C++ bindings written by
 *   Benedict R. Gaster, Laurent Morichetti and Lee Howes
 *   With additions and fixes from:
 *       Brian Cole, March 3rd 2010 and April 2012
 *       Matt Gruenke, April 2012.
 *       Bruce Merry, February 2013.
 *       Tom Deakin and Simon McIntosh-Smith, July 2013
 *       James Price, 2015-
 *   \version 2.2.0
 *   \date 2019-09-18
 *
 *   Optional extension support
 *
 *         cl_ext_device_fission
 *         #define CL_HPP_USE_CL_DEVICE_FISSION
 *         cl_khr_d3d10_sharing
 *         #define CL_HPP_USE_DX_INTEROP
 *         cl_khr_sub_groups
 *         #define CL_HPP_USE_CL_SUB_GROUPS_KHR
 *         cl_khr_image2d_from_buffer
 *         #define CL_HPP_USE_CL_IMAGE2D_FROM_BUFFER_KHR
 *
 *   Doxygen documentation for this header is available here:
 *
 *       http://khronosgroup.github.io/OpenCL-CLHPP/
 *
 *   The latest version of this header can be found on the GitHub releases page:
 *
 *       https://github.com/KhronosGroup/OpenCL-CLHPP/releases
 *
 *   Bugs and patches can be submitted to the GitHub repository:
 *
 *       https://github.com/KhronosGroup/OpenCL-CLHPP
 */

/*! \mainpage
 * \section intro Introduction
 * For many large applications C++ is the language of choice and so it seems
 * reasonable to define C++ bindings for OpenCL.
 *
 * The interface is contained with a single C++ header file \em opencl.hpp and all
 * definitions are contained within the namespace \em cl. There is no additional
 * requirement to include \em cl.h and to use either the C++ or original C
 * bindings; it is enough to simply include \em opencl.hpp.
 *
 * The bindings themselves are lightweight and correspond closely to the
 * underlying C API. Using the C++ bindings introduces no additional execution
 * overhead.
 *
 * There are numerous compatibility, portability and memory management
 * fixes in the new header as well as additional OpenCL 2.0 features.
 * As a result the header is not directly backward compatible and for this
 * reason we release it as opencl.hpp rather than a new version of cl.hpp.
 *
 *
 * \section compatibility Compatibility
 * Due to the evolution of the underlying OpenCL API the 2.0 C++ bindings
 * include an updated approach to defining supported feature versions
 * and the range of valid underlying OpenCL runtime versions supported.
 *
 * The combination of preprocessor macros CL_HPP_TARGET_OPENCL_VERSION and
 * CL_HPP_MINIMUM_OPENCL_VERSION control this range. These are three digit
 * decimal values representing OpenCL runime versions. The default for
 * the target is 200, representing OpenCL 2.0 and the minimum is also
 * defined as 200. These settings would use 2.0 API calls only.
 * If backward compatibility with a 1.2 runtime is required, the minimum
 * version may be set to 120.
 *
 * Note that this is a compile-time setting, and so affects linking against
 * a particular SDK version rather than the versioning of the loaded runtime.
 *
 * The earlier versions of the header included basic vector and string
 * classes based loosely on STL versions. These were difficult to
 * maintain and very rarely used. For the 2.0 header we now assume
 * the presence of the standard library unless requested otherwise.
 * We use std::array, std::vector, std::shared_ptr and std::string
 * throughout to safely manage memory and reduce the chance of a
 * recurrance of earlier memory management bugs.
 *
 * These classes are used through typedefs in the cl namespace:
 * cl::array, cl::vector, cl::pointer and cl::string.
 * In addition cl::allocate_pointer forwards to std::allocate_shared
 * by default.
 * In all cases these standard library classes can be replaced with
 * custom interface-compatible versions using the CL_HPP_NO_STD_ARRAY,
 * CL_HPP_NO_STD_VECTOR, CL_HPP_NO_STD_UNIQUE_PTR and
 * CL_HPP_NO_STD_STRING macros.
 *
 * The OpenCL 1.x versions of the C++ bindings included a size_t wrapper
 * class to interface with kernel enqueue. This caused unpleasant interactions
 * with the standard size_t declaration and led to namespacing bugs.
 * In the 2.0 version we have replaced this with a std::array-based interface.
 * However, the old behaviour can be regained for backward compatibility
 * using the CL_HPP_ENABLE_SIZE_T_COMPATIBILITY macro.
 *
 * Finally, the program construction interface used a clumsy vector-of-pairs
 * design in the earlier versions. We have replaced that with a cleaner
 * vector-of-vectors and vector-of-strings design. However, for backward
 * compatibility old behaviour can be regained with the
 * CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY macro.
 *
 * In OpenCL 2.0 OpenCL C is not entirely backward compatibility with
 * earlier versions. As a result a flag must be passed to the OpenCL C
 * compiled to request OpenCL 2.0 compilation of kernels with 1.2 as
 * the default in the absence of the flag.
 * In some cases the C++ bindings automatically compile code for ease.
 * For those cases the compilation defaults to OpenCL C 2.0.
 * If this is not wanted, the CL_HPP_CL_1_2_DEFAULT_BUILD macro may
 * be specified to assume 1.2 compilation.
 * If more fine-grained decisions on a per-kernel bases are required
 * then explicit build operations that take the flag should be used.
 *
 *
 * \section parameterization Parameters
 * This header may be parameterized by a set of preprocessor macros.
 *
 * - CL_HPP_TARGET_OPENCL_VERSION
 *
 *   Defines the target OpenCL runtime version to build the header
 *   against. Defaults to 200, representing OpenCL 2.0.
 *
 * - CL_HPP_NO_STD_STRING
 *
 *   Do not use the standard library string class. cl::string is not
 *   defined and may be defined by the user before opencl.hpp is
 *   included.
 *
 * - CL_HPP_NO_STD_VECTOR
 *
 *   Do not use the standard library vector class. cl::vector is not
 *   defined and may be defined by the user before opencl.hpp is
 *   included.
 *
 * - CL_HPP_NO_STD_ARRAY
 *
 *   Do not use the standard library array class. cl::array is not
 *   defined and may be defined by the user before opencl.hpp is
 *   included.
 *
 * - CL_HPP_NO_STD_UNIQUE_PTR
 *
 *   Do not use the standard library unique_ptr class. cl::pointer and
 *   the cl::allocate_pointer functions are not defined and may be
 *   defined by the user before opencl.hpp is included.
 *
 * - CL_HPP_ENABLE_EXCEPTIONS
 *
 *   Enable exceptions for use in the C++ bindings header. This is the
 *   preferred error handling mechanism but is not required.
 *
 * - CL_HPP_ENABLE_SIZE_T_COMPATIBILITY
 *
 *   Backward compatibility option to support cl.hpp-style size_t
 *   class.  Replaces the updated std::array derived version and
 *   removal of size_t from the namespace. Note that in this case the
 *   new size_t class is placed in the cl::compatibility namespace and
 *   thus requires an additional using declaration for direct backward
 *   compatibility.
 *
 * - CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY
 *
 *   Enable older vector of pairs interface for construction of
 *   programs.
 *
 * - CL_HPP_CL_1_2_DEFAULT_BUILD
 *
 *   Default to OpenCL C 1.2 compilation rather than OpenCL C 2.0
 *   applies to use of cl::Program construction and other program
 *   build variants.
 *
 * - CL_HPP_USE_CL_DEVICE_FISSION
 *
 *   Enable the cl_ext_device_fission extension.
 *
 * - CL_HPP_USE_CL_IMAGE2D_FROM_BUFFER_KHR
 *
 *   Enable the cl_khr_image2d_from_buffer extension.
 *
 * - CL_HPP_USE_CL_SUB_GROUPS_KHR
 *
 *   Enable the cl_khr_subgroups extension.
 *
 * - CL_HPP_USE_DX_INTEROP
 *
 *   Enable the cl_khr_d3d10_sharing extension.
 *
 * - CL_HPP_USE_IL_KHR
 *
 *   Enable the cl_khr_il_program extension.
 *
 *
 * \section example Example
 *
 * The following example shows a general use case for the C++
 * bindings, including support for the optional exception feature and
 * also the supplied vector and string classes, see following sections for
 * decriptions of these features.
 *
 * Note: the C++ bindings use std::call_once and therefore may need to be
 * compiled using special command-line options (such as "-pthread") on some
 * platforms!
 *
 * \code
    #define CL_HPP_ENABLE_EXCEPTIONS
    #define CL_HPP_TARGET_OPENCL_VERSION 200

    #include <CL/opencl.hpp>
    #include <algorithm>
    #include <iostream>
    #include <memory>
    #include <vector>

    const int numElements = 32;

    int main(void)
    {
        // Filter for a 2.0 or newer platform and set it as the default
        std::vector<cl::Platform> platforms;
        cl::Platform::get(&platforms);
        cl::Platform plat;
        for (auto &p : platforms) {
            std::string platver = p.getInfo<CL_PLATFORM_VERSION>();
            if (platver.find("OpenCL 2.") != std::string::npos ||
                platver.find("OpenCL 3.") != std::string::npos) {
                // Note: an OpenCL 3.x platform may not support all required features!
                plat = p;
            }
        }
        if (plat() == 0) {
            std::cout << "No OpenCL 2.0 or newer platform found.\n";
            return -1;
        }

        cl::Platform newP = cl::Platform::setDefault(plat);
        if (newP != plat) {
            std::cout << "Error setting default platform.\n";
            return -1;
        }

        // C++11 raw string literal for the first kernel
        std::string kernel1{R"CLC(
            global int globalA;
            kernel void updateGlobal()
            {
              globalA = 75;
            }
        )CLC"};

        // Raw string literal for the second kernel
        std::string kernel2{R"CLC(
            typedef struct { global int *bar; } Foo;
            kernel void vectorAdd(global const Foo* aNum, global const int *inputA, global const int *inputB,
                                  global int *output, int val, write_only pipe int outPipe, queue_t childQueue)
            {
              output[get_global_id(0)] = inputA[get_global_id(0)] + inputB[get_global_id(0)] + val + *(aNum->bar);
              write_pipe(outPipe, &val);
              queue_t default_queue = get_default_queue();
              ndrange_t ndrange = ndrange_1D(get_global_size(0)/2, get_global_size(0)/2);

              // Have a child kernel write into third quarter of output
              enqueue_kernel(default_queue, CLK_ENQUEUE_FLAGS_WAIT_KERNEL, ndrange,
                ^{
                    output[get_global_size(0)*2 + get_global_id(0)] =
                      inputA[get_global_size(0)*2 + get_global_id(0)] + inputB[get_global_size(0)*2 + get_global_id(0)]
 + globalA;
                });

              // Have a child kernel write into last quarter of output
              enqueue_kernel(childQueue, CLK_ENQUEUE_FLAGS_WAIT_KERNEL, ndrange,
                ^{
                    output[get_global_size(0)*3 + get_global_id(0)] =
                      inputA[get_global_size(0)*3 + get_global_id(0)] + inputB[get_global_size(0)*3 + get_global_id(0)]
 + globalA + 2;
                });
            }
        )CLC"};

        std::vector<std::string> programStrings;
        programStrings.push_back(kernel1);
        programStrings.push_back(kernel2);

        cl::Program vectorAddProgram(programStrings);
        try {
            vectorAddProgram.build("-cl-std=CL2.0");
        }
        catch (...) {
            // Print build info for all devices
            cl_int buildErr = CL_SUCCESS;
            auto buildInfo = vectorAddProgram.getBuildInfo<CL_PROGRAM_BUILD_LOG>(&buildErr);
            for (auto &pair : buildInfo) {
                std::cerr << pair.second << std::endl << std::endl;
            }

            return 1;
        }

        typedef struct { int *bar; } Foo;

        // Get and run kernel that initializes the program-scope global
        // A test for kernels that take no arguments
        auto program2Kernel =
            cl::KernelFunctor<>(vectorAddProgram, "updateGlobal");
        program2Kernel(
            cl::EnqueueArgs(
            cl::NDRange(1)));

        //////////////////
        // SVM allocations

        auto anSVMInt = cl::allocate_svm<int, cl::SVMTraitCoarse<>>();
        *anSVMInt = 5;
        cl::SVMAllocator<Foo, cl::SVMTraitCoarse<cl::SVMTraitReadOnly<>>> svmAllocReadOnly;
        auto fooPointer = cl::allocate_pointer<Foo>(svmAllocReadOnly);
        fooPointer->bar = anSVMInt.get();
        cl::SVMAllocator<int, cl::SVMTraitCoarse<>> svmAlloc;
        std::vector<int, cl::SVMAllocator<int, cl::SVMTraitCoarse<>>> inputA(numElements, 1, svmAlloc);
        cl::coarse_svm_vector<int> inputB(numElements, 2, svmAlloc);

        //////////////
        // Traditional cl_mem allocations

        std::vector<int> output(numElements, 0xdeadbeef);
        cl::Buffer outputBuffer(begin(output), end(output), false);
        cl::Pipe aPipe(sizeof(cl_int), numElements / 2);

        // Default command queue, also passed in as a parameter
        cl::DeviceCommandQueue defaultDeviceQueue = cl::DeviceCommandQueue::makeDefault(
            cl::Context::getDefault(), cl::Device::getDefault());

        auto vectorAddKernel =
            cl::KernelFunctor<
                decltype(fooPointer)&,
                int*,
                cl::coarse_svm_vector<int>&,
                cl::Buffer,
                int,
                cl::Pipe&,
                cl::DeviceCommandQueue
                >(vectorAddProgram, "vectorAdd");

        // Ensure that the additional SVM pointer is available to the kernel
        // This one was not passed as a parameter
        vectorAddKernel.setSVMPointers(anSVMInt);

        cl_int error;
        vectorAddKernel(
            cl::EnqueueArgs(
                cl::NDRange(numElements/2),
                cl::NDRange(numElements/2)),
            fooPointer,
            inputA.data(),
            inputB,
            outputBuffer,
            3,
            aPipe,
            defaultDeviceQueue,
            error
            );

        cl::copy(outputBuffer, begin(output), end(output));

        cl::Device d = cl::Device::getDefault();

        std::cout << "Output:\n";
        for (int i = 1; i < numElements; ++i) {
            std::cout << "\t" << output[i] << "\n";
        }
        std::cout << "\n\n";

        return 0;
    }
 *
 * \endcode
 *
 */
#ifndef CL_HPP_
#define CL_HPP_

/* Handle deprecated preprocessor definitions. In each case, we only check for
 * the old name if the new name is not defined, so that user code can define
 * both and hence work with either version of the bindings.
 */
#if !defined(CL_HPP_USE_DX_INTEROP) && defined(USE_DX_INTEROP)
#pragma message("opencl.hpp: USE_DX_INTEROP is deprecated. Define CL_HPP_USE_DX_INTEROP instead")
#define CL_HPP_USE_DX_INTEROP
#endif
#if !defined(CL_HPP_USE_CL_DEVICE_FISSION) && defined(USE_CL_DEVICE_FISSION)
#pragma message("opencl.hpp: USE_CL_DEVICE_FISSION is deprecated. Define CL_HPP_USE_CL_DEVICE_FISSION instead")
#define CL_HPP_USE_CL_DEVICE_FISSION
#endif
#if !defined(CL_HPP_ENABLE_EXCEPTIONS) && defined(__CL_ENABLE_EXCEPTIONS)
#pragma message("opencl.hpp: __CL_ENABLE_EXCEPTIONS is deprecated. Define CL_HPP_ENABLE_EXCEPTIONS instead")
#define CL_HPP_ENABLE_EXCEPTIONS
#endif
#if !defined(CL_HPP_NO_STD_VECTOR) && defined(__NO_STD_VECTOR)
#pragma message("opencl.hpp: __NO_STD_VECTOR is deprecated. Define CL_HPP_NO_STD_VECTOR instead")
#define CL_HPP_NO_STD_VECTOR
#endif
#if !defined(CL_HPP_NO_STD_STRING) && defined(__NO_STD_STRING)
#pragma message("opencl.hpp: __NO_STD_STRING is deprecated. Define CL_HPP_NO_STD_STRING instead")
#define CL_HPP_NO_STD_STRING
#endif
#if defined(VECTOR_CLASS)
#pragma message("opencl.hpp: VECTOR_CLASS is deprecated. Alias cl::vector instead")
#endif
#if defined(STRING_CLASS)
#pragma message("opencl.hpp: STRING_CLASS is deprecated. Alias cl::string instead.")
#endif
#if !defined(CL_HPP_USER_OVERRIDE_ERROR_STRINGS) && defined(__CL_USER_OVERRIDE_ERROR_STRINGS)
#pragma message( \
  "opencl.hpp: __CL_USER_OVERRIDE_ERROR_STRINGS is deprecated. Define CL_HPP_USER_OVERRIDE_ERROR_STRINGS instead")
#define CL_HPP_USER_OVERRIDE_ERROR_STRINGS
#endif

/* Warn about features that are no longer supported
 */
#if defined(__USE_DEV_VECTOR)
#pragma message("opencl.hpp: __USE_DEV_VECTOR is no longer supported. Expect compilation errors")
#endif
#if defined(__USE_DEV_STRING)
#pragma message("opencl.hpp: __USE_DEV_STRING is no longer supported. Expect compilation errors")
#endif

/* Detect which version to target */
#if !defined(CL_HPP_TARGET_OPENCL_VERSION)
#pragma message("opencl.hpp: CL_HPP_TARGET_OPENCL_VERSION is not defined. It will default to 300 (OpenCL 3.0)")
#define CL_HPP_TARGET_OPENCL_VERSION 300
#endif
#if CL_HPP_TARGET_OPENCL_VERSION != 100 && CL_HPP_TARGET_OPENCL_VERSION != 110 &&                                      \
  CL_HPP_TARGET_OPENCL_VERSION != 120 && CL_HPP_TARGET_OPENCL_VERSION != 200 && CL_HPP_TARGET_OPENCL_VERSION != 210 && \
  CL_HPP_TARGET_OPENCL_VERSION != 220 && CL_HPP_TARGET_OPENCL_VERSION != 300
#pragma message( \
  "opencl.hpp: CL_HPP_TARGET_OPENCL_VERSION is not a valid value (100, 110, 120, 200, 210, 220 or 300). It will be set to 300 (OpenCL 3.0).")
#undef CL_HPP_TARGET_OPENCL_VERSION
#define CL_HPP_TARGET_OPENCL_VERSION 300
#endif

/* Forward target OpenCL version to C headers if necessary */
#if defined(CL_TARGET_OPENCL_VERSION)
/* Warn if prior definition of CL_TARGET_OPENCL_VERSION is lower than
 * requested C++ bindings version */
#if CL_TARGET_OPENCL_VERSION < CL_HPP_TARGET_OPENCL_VERSION
#pragma message("CL_TARGET_OPENCL_VERSION is already defined as is lower than CL_HPP_TARGET_OPENCL_VERSION")
#endif
#else
#define CL_TARGET_OPENCL_VERSION CL_HPP_TARGET_OPENCL_VERSION
#endif

#if !defined(CL_HPP_MINIMUM_OPENCL_VERSION)
#define CL_HPP_MINIMUM_OPENCL_VERSION 200
#endif
#if CL_HPP_MINIMUM_OPENCL_VERSION != 100 && CL_HPP_MINIMUM_OPENCL_VERSION != 110 && \
  CL_HPP_MINIMUM_OPENCL_VERSION != 120 && CL_HPP_MINIMUM_OPENCL_VERSION != 200 &&   \
  CL_HPP_MINIMUM_OPENCL_VERSION != 210 && CL_HPP_MINIMUM_OPENCL_VERSION != 220 && CL_HPP_MINIMUM_OPENCL_VERSION != 300
#pragma message( \
  "opencl.hpp: CL_HPP_MINIMUM_OPENCL_VERSION is not a valid value (100, 110, 120, 200, 210, 220 or 300). It will be set to 100")
#undef CL_HPP_MINIMUM_OPENCL_VERSION
#define CL_HPP_MINIMUM_OPENCL_VERSION 100
#endif
#if CL_HPP_MINIMUM_OPENCL_VERSION > CL_HPP_TARGET_OPENCL_VERSION
#error "CL_HPP_MINIMUM_OPENCL_VERSION must not be greater than CL_HPP_TARGET_OPENCL_VERSION"
#endif

#if CL_HPP_MINIMUM_OPENCL_VERSION <= 100 && !defined(CL_USE_DEPRECATED_OPENCL_1_0_APIS)
#define CL_USE_DEPRECATED_OPENCL_1_0_APIS
#endif
#if CL_HPP_MINIMUM_OPENCL_VERSION <= 110 && !defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS)
#define CL_USE_DEPRECATED_OPENCL_1_1_APIS
#endif
#if CL_HPP_MINIMUM_OPENCL_VERSION <= 120 && !defined(CL_USE_DEPRECATED_OPENCL_1_2_APIS)
#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
#endif
#if CL_HPP_MINIMUM_OPENCL_VERSION <= 200 && !defined(CL_USE_DEPRECATED_OPENCL_2_0_APIS)
#define CL_USE_DEPRECATED_OPENCL_2_0_APIS
#endif
#if CL_HPP_MINIMUM_OPENCL_VERSION <= 210 && !defined(CL_USE_DEPRECATED_OPENCL_2_1_APIS)
#define CL_USE_DEPRECATED_OPENCL_2_1_APIS
#endif
#if CL_HPP_MINIMUM_OPENCL_VERSION <= 220 && !defined(CL_USE_DEPRECATED_OPENCL_2_2_APIS)
#define CL_USE_DEPRECATED_OPENCL_2_2_APIS
#endif

#ifdef _WIN32

#include <malloc.h>

#if defined(CL_HPP_USE_DX_INTEROP)
#include <CL/cl_d3d10.h>
#include <CL/cl_dx9_media_sharing.h>
#endif
#endif  // _WIN32

#if defined(_MSC_VER)
#include <intrin.h>
#endif  // _MSC_VER

// Check for a valid C++ version

// Need to do both tests here because for some reason __cplusplus is not
// updated in visual studio
#if (!defined(_MSC_VER) && __cplusplus < 201103L) || (defined(_MSC_VER) && _MSC_VER < 1700)
#error Visual studio 2013 or another C++11-supporting compiler required
#endif

//
#if defined(CL_HPP_USE_CL_DEVICE_FISSION) || defined(CL_HPP_USE_CL_SUB_GROUPS_KHR)
#include <CL/cl_ext.h>
#endif

#if defined(__APPLE__) || defined(__MACOSX)
#include <OpenCL/opencl.h>
#else
#include <CL/opencl.h>
#endif  // !__APPLE__

#if (__cplusplus >= 201103L || _MSVC_LANG >= 201103L)
#define CL_HPP_NOEXCEPT_ noexcept
#else
#define CL_HPP_NOEXCEPT_
#endif

#if __cplusplus >= 201703L
#define CL_HPP_DEFINE_STATIC_MEMBER_ inline
#elif defined(_MSC_VER)
#define CL_HPP_DEFINE_STATIC_MEMBER_ __declspec(selectany)
#elif defined(__MINGW32__)
#define CL_HPP_DEFINE_STATIC_MEMBER_ __attribute__((selectany))
#else
#define CL_HPP_DEFINE_STATIC_MEMBER_ __attribute__((weak))
#endif  // !_MSC_VER

// Define deprecated prefixes and suffixes to ensure compilation
// in case they are not pre-defined
#if !defined(CL_API_PREFIX__VERSION_1_1_DEPRECATED)
#define CL_API_PREFIX__VERSION_1_1_DEPRECATED
#endif  // #if !defined(CL_API_PREFIX__VERSION_1_1_DEPRECATED)
#if !defined(CL_API_SUFFIX__VERSION_1_1_DEPRECATED)
#define CL_API_SUFFIX__VERSION_1_1_DEPRECATED
#endif  // #if !defined(CL_API_SUFFIX__VERSION_1_1_DEPRECATED)

#if !defined(CL_API_PREFIX__VERSION_1_2_DEPRECATED)
#define CL_API_PREFIX__VERSION_1_2_DEPRECATED
#endif  // #if !defined(CL_API_PREFIX__VERSION_1_2_DEPRECATED)
#if !defined(CL_API_SUFFIX__VERSION_1_2_DEPRECATED)
#define CL_API_SUFFIX__VERSION_1_2_DEPRECATED
#endif  // #if !defined(CL_API_SUFFIX__VERSION_1_2_DEPRECATED)

#if !defined(CL_API_PREFIX__VERSION_2_2_DEPRECATED)
#define CL_API_PREFIX__VERSION_2_2_DEPRECATED
#endif  // #if !defined(CL_API_PREFIX__VERSION_2_2_DEPRECATED)
#if !defined(CL_API_SUFFIX__VERSION_2_2_DEPRECATED)
#define CL_API_SUFFIX__VERSION_2_2_DEPRECATED
#endif  // #if !defined(CL_API_SUFFIX__VERSION_2_2_DEPRECATED)

#if !defined(CL_CALLBACK)
#define CL_CALLBACK
#endif  // CL_CALLBACK

#include <cstring>
#include <functional>
#include <iterator>
#include <limits>
#include <mutex>
#include <utility>


// Define a size_type to represent a correctly resolved size_t
#if defined(CL_HPP_ENABLE_SIZE_T_COMPATIBILITY)
namespace cl
{
using size_type = ::size_t;
}  // namespace cl
#else   // #if defined(CL_HPP_ENABLE_SIZE_T_COMPATIBILITY)
namespace cl
{
using size_type = size_t;
}  // namespace cl
#endif  // #if defined(CL_HPP_ENABLE_SIZE_T_COMPATIBILITY)


#if defined(CL_HPP_ENABLE_EXCEPTIONS)
#include <exception>
#endif  // #if defined(CL_HPP_ENABLE_EXCEPTIONS)

#if !defined(CL_HPP_NO_STD_VECTOR)
#include <vector>
namespace cl
{
template <class T, class Alloc = std::allocator<T>>
using vector = std::vector<T, Alloc>;
}  // namespace cl
#endif  // #if !defined(CL_HPP_NO_STD_VECTOR)

#if !defined(CL_HPP_NO_STD_STRING)
#include <string>
namespace cl
{
using string = std::string;
}  // namespace cl
#endif  // #if !defined(CL_HPP_NO_STD_STRING)

#if CL_HPP_TARGET_OPENCL_VERSION >= 200

#if !defined(CL_HPP_NO_STD_UNIQUE_PTR)
#include <memory>
namespace cl
{
// Replace unique_ptr and allocate_pointer for internal use
// to allow user to replace them
template <class T, class D>
using pointer = std::unique_ptr<T, D>;
}  // namespace cl
#endif
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200
#if !defined(CL_HPP_NO_STD_ARRAY)
#include <array>
namespace cl
{
template <class T, size_type N>
using array = std::array<T, N>;
}  // namespace cl
#endif  // #if !defined(CL_HPP_NO_STD_ARRAY)

// Define size_type appropriately to allow backward-compatibility
// use of the old size_t interface class
#if defined(CL_HPP_ENABLE_SIZE_T_COMPATIBILITY)
namespace cl
{
namespace compatibility
{
/*! \brief class used to interface between C++ and
 *  OpenCL C calls that require arrays of size_t values, whose
 *  size is known statically.
 */
template <int N>
class size_t
{
private:
  size_type data_[N];

public:
  //! \brief Initialize size_t to all 0s
  size_t()
  {
    for (int i = 0; i < N; ++i)
    {
      data_[i] = 0;
    }
  }

  size_t(const array<size_type, N> &rhs)
  {
    for (int i = 0; i < N; ++i)
    {
      data_[i] = rhs[i];
    }
  }

  size_type &operator[](int index) { return data_[index]; }

  const size_type &operator[](int index) const { return data_[index]; }

  //! \brief Conversion operator to T*.
  operator size_type *() { return data_; }

  //! \brief Conversion operator to const T*.
  operator const size_type *() const { return data_; }

  operator array<size_type, N>() const
  {
    array<size_type, N> ret;

    for (int i = 0; i < N; ++i)
    {
      ret[i] = data_[i];
    }
    return ret;
  }
};
}  // namespace compatibility

template <int N>
using size_t = compatibility::size_t<N>;
}  // namespace cl
#endif  // #if defined(CL_HPP_ENABLE_SIZE_T_COMPATIBILITY)

// Helper alias to avoid confusing the macros
namespace cl
{
namespace detail
{
using size_t_array = array<size_type, 3>;
}  // namespace detail
}  // namespace cl


/*! \namespace cl
 *
 * \brief The OpenCL C++ bindings are defined within this namespace.
 *
 */
namespace cl
{
class Memory;

#define CL_HPP_INIT_CL_EXT_FCN_PTR_(name)                          \
  if (!pfn_##name)                                                 \
  {                                                                \
    pfn_##name = (PFN_##name)clGetExtensionFunctionAddress(#name); \
    if (!pfn_##name)                                               \
    {                                                              \
    }                                                              \
  }

#define CL_HPP_INIT_CL_EXT_FCN_PTR_PLATFORM_(platform, name)                            \
  if (!pfn_##name)                                                                      \
  {                                                                                     \
    pfn_##name = (PFN_##name)clGetExtensionFunctionAddressForPlatform(platform, #name); \
    if (!pfn_##name)                                                                    \
    {                                                                                   \
    }                                                                                   \
  }

class Program;
class Device;
class Context;
class CommandQueue;
class DeviceCommandQueue;
class Memory;
class Buffer;
class Pipe;

#if defined(CL_HPP_ENABLE_EXCEPTIONS)
/*! \brief Exception class
 *
 *  This may be thrown by API functions when CL_HPP_ENABLE_EXCEPTIONS is defined.
 */
class Error : public std::exception
{
private:
  cl_int err_;
  const char *errStr_;

public:
  /*! \brief Create a new CL error exception for a given error code
   *  and corresponding message.
   *
   *  \param err error code value.
   *
   *  \param errStr a descriptive string that must remain in scope until
   *                handling of the exception has concluded.  If set, it
   *                will be returned by what().
   */
  Error(cl_int err, const char *errStr = NULL)
    : err_(err)
    , errStr_(errStr)
  {}

  ~Error() throw() {}

  /*! \brief Get error string associated with exception
   *
   * \return A memory pointer to the error message string.
   */
  virtual const char *what() const throw()
  {
    if (errStr_ == NULL)
    {
      return "empty";
    }
    else
    {
      return errStr_;
    }
  }

  /*! \brief Get error code associated with exception
   *
   *  \return The error code.
   */
  cl_int err(void) const { return err_; }
};
#define CL_HPP_ERR_STR_(x) #x
#else
#define CL_HPP_ERR_STR_(x) NULL
#endif  // CL_HPP_ENABLE_EXCEPTIONS


namespace detail
{
#if defined(CL_HPP_ENABLE_EXCEPTIONS)
static inline cl_int errHandler(cl_int err, const char *errStr = NULL)
{
  if (err != CL_SUCCESS)
  {
    throw Error(err, errStr);
  }
  return err;
}
#else
static inline cl_int errHandler(cl_int err, const char *errStr = NULL)
{
  (void)errStr;  // suppress unused variable warning
  return err;
}
#endif  // CL_HPP_ENABLE_EXCEPTIONS
}  // namespace detail


//! \cond DOXYGEN_DETAIL
#if !defined(CL_HPP_USER_OVERRIDE_ERROR_STRINGS)
#define __GET_DEVICE_INFO_ERR        CL_HPP_ERR_STR_(clGetDeviceInfo)
#define __GET_PLATFORM_INFO_ERR      CL_HPP_ERR_STR_(clGetPlatformInfo)
#define __GET_DEVICE_IDS_ERR         CL_HPP_ERR_STR_(clGetDeviceIDs)
#define __GET_PLATFORM_IDS_ERR       CL_HPP_ERR_STR_(clGetPlatformIDs)
#define __GET_CONTEXT_INFO_ERR       CL_HPP_ERR_STR_(clGetContextInfo)
#define __GET_EVENT_INFO_ERR         CL_HPP_ERR_STR_(clGetEventInfo)
#define __GET_EVENT_PROFILE_INFO_ERR CL_HPP_ERR_STR_(clGetEventProfileInfo)
#define __GET_MEM_OBJECT_INFO_ERR    CL_HPP_ERR_STR_(clGetMemObjectInfo)
#define __GET_IMAGE_INFO_ERR         CL_HPP_ERR_STR_(clGetImageInfo)
#define __GET_SAMPLER_INFO_ERR       CL_HPP_ERR_STR_(clGetSamplerInfo)
#define __GET_KERNEL_INFO_ERR        CL_HPP_ERR_STR_(clGetKernelInfo)
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
#define __GET_KERNEL_ARG_INFO_ERR CL_HPP_ERR_STR_(clGetKernelArgInfo)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120
#if CL_HPP_TARGET_OPENCL_VERSION >= 200
#define __GET_KERNEL_SUB_GROUP_INFO_ERR CL_HPP_ERR_STR_(clGetKernelSubGroupInfo)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200
#define __GET_KERNEL_WORK_GROUP_INFO_ERR CL_HPP_ERR_STR_(clGetKernelWorkGroupInfo)
#define __GET_PROGRAM_INFO_ERR           CL_HPP_ERR_STR_(clGetProgramInfo)
#define __GET_PROGRAM_BUILD_INFO_ERR     CL_HPP_ERR_STR_(clGetProgramBuildInfo)
#define __GET_COMMAND_QUEUE_INFO_ERR     CL_HPP_ERR_STR_(clGetCommandQueueInfo)

#define __CREATE_CONTEXT_ERR              CL_HPP_ERR_STR_(clCreateContext)
#define __CREATE_CONTEXT_FROM_TYPE_ERR    CL_HPP_ERR_STR_(clCreateContextFromType)
#define __GET_SUPPORTED_IMAGE_FORMATS_ERR CL_HPP_ERR_STR_(clGetSupportedImageFormats)

#define __CREATE_BUFFER_ERR           CL_HPP_ERR_STR_(clCreateBuffer)
#define __COPY_ERR                    CL_HPP_ERR_STR_(cl::copy)
#define __CREATE_SUBBUFFER_ERR        CL_HPP_ERR_STR_(clCreateSubBuffer)
#define __CREATE_GL_BUFFER_ERR        CL_HPP_ERR_STR_(clCreateFromGLBuffer)
#define __CREATE_GL_RENDER_BUFFER_ERR CL_HPP_ERR_STR_(clCreateFromGLBuffer)
#define __GET_GL_OBJECT_INFO_ERR      CL_HPP_ERR_STR_(clGetGLObjectInfo)
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
#define __CREATE_IMAGE_ERR      CL_HPP_ERR_STR_(clCreateImage)
#define __CREATE_GL_TEXTURE_ERR CL_HPP_ERR_STR_(clCreateFromGLTexture)
#define __IMAGE_DIMENSION_ERR   CL_HPP_ERR_STR_(Incorrect image dimensions)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120
#define __SET_MEM_OBJECT_DESTRUCTOR_CALLBACK_ERR CL_HPP_ERR_STR_(clSetMemObjectDestructorCallback)

#define __CREATE_USER_EVENT_ERR     CL_HPP_ERR_STR_(clCreateUserEvent)
#define __SET_USER_EVENT_STATUS_ERR CL_HPP_ERR_STR_(clSetUserEventStatus)
#define __SET_EVENT_CALLBACK_ERR    CL_HPP_ERR_STR_(clSetEventCallback)
#define __WAIT_FOR_EVENTS_ERR       CL_HPP_ERR_STR_(clWaitForEvents)

#define __CREATE_KERNEL_ERR              CL_HPP_ERR_STR_(clCreateKernel)
#define __SET_KERNEL_ARGS_ERR            CL_HPP_ERR_STR_(clSetKernelArg)
#define __CREATE_PROGRAM_WITH_SOURCE_ERR CL_HPP_ERR_STR_(clCreateProgramWithSource)
#if CL_HPP_TARGET_OPENCL_VERSION >= 200
#define __CREATE_PROGRAM_WITH_IL_ERR CL_HPP_ERR_STR_(clCreateProgramWithIL)
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200
#define __CREATE_PROGRAM_WITH_BINARY_ERR CL_HPP_ERR_STR_(clCreateProgramWithBinary)
#if CL_HPP_TARGET_OPENCL_VERSION >= 210
#define __CREATE_PROGRAM_WITH_IL_ERR CL_HPP_ERR_STR_(clCreateProgramWithIL)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 210
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
#define __CREATE_PROGRAM_WITH_BUILT_IN_KERNELS_ERR CL_HPP_ERR_STR_(clCreateProgramWithBuiltInKernels)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120
#define __BUILD_PROGRAM_ERR CL_HPP_ERR_STR_(clBuildProgram)
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
#define __COMPILE_PROGRAM_ERR CL_HPP_ERR_STR_(clCompileProgram)
#define __LINK_PROGRAM_ERR    CL_HPP_ERR_STR_(clLinkProgram)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120
#define __CREATE_KERNELS_IN_PROGRAM_ERR CL_HPP_ERR_STR_(clCreateKernelsInProgram)

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
#define __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR CL_HPP_ERR_STR_(clCreateCommandQueueWithProperties)
#define __CREATE_SAMPLER_WITH_PROPERTIES_ERR       CL_HPP_ERR_STR_(clCreateSamplerWithProperties)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200
#define __SET_COMMAND_QUEUE_PROPERTY_ERR   CL_HPP_ERR_STR_(clSetCommandQueueProperty)
#define __ENQUEUE_READ_BUFFER_ERR          CL_HPP_ERR_STR_(clEnqueueReadBuffer)
#define __ENQUEUE_READ_BUFFER_RECT_ERR     CL_HPP_ERR_STR_(clEnqueueReadBufferRect)
#define __ENQUEUE_WRITE_BUFFER_ERR         CL_HPP_ERR_STR_(clEnqueueWriteBuffer)
#define __ENQUEUE_WRITE_BUFFER_RECT_ERR    CL_HPP_ERR_STR_(clEnqueueWriteBufferRect)
#define __ENQEUE_COPY_BUFFER_ERR           CL_HPP_ERR_STR_(clEnqueueCopyBuffer)
#define __ENQEUE_COPY_BUFFER_RECT_ERR      CL_HPP_ERR_STR_(clEnqueueCopyBufferRect)
#define __ENQUEUE_FILL_BUFFER_ERR          CL_HPP_ERR_STR_(clEnqueueFillBuffer)
#define __ENQUEUE_READ_IMAGE_ERR           CL_HPP_ERR_STR_(clEnqueueReadImage)
#define __ENQUEUE_WRITE_IMAGE_ERR          CL_HPP_ERR_STR_(clEnqueueWriteImage)
#define __ENQUEUE_COPY_IMAGE_ERR           CL_HPP_ERR_STR_(clEnqueueCopyImage)
#define __ENQUEUE_FILL_IMAGE_ERR           CL_HPP_ERR_STR_(clEnqueueFillImage)
#define __ENQUEUE_COPY_IMAGE_TO_BUFFER_ERR CL_HPP_ERR_STR_(clEnqueueCopyImageToBuffer)
#define __ENQUEUE_COPY_BUFFER_TO_IMAGE_ERR CL_HPP_ERR_STR_(clEnqueueCopyBufferToImage)
#define __ENQUEUE_MAP_BUFFER_ERR           CL_HPP_ERR_STR_(clEnqueueMapBuffer)
#define __ENQUEUE_MAP_IMAGE_ERR            CL_HPP_ERR_STR_(clEnqueueMapImage)
#define __ENQUEUE_UNMAP_MEM_OBJECT_ERR     CL_HPP_ERR_STR_(clEnqueueUnMapMemObject)
#define __ENQUEUE_NDRANGE_KERNEL_ERR       CL_HPP_ERR_STR_(clEnqueueNDRangeKernel)
#define __ENQUEUE_NATIVE_KERNEL            CL_HPP_ERR_STR_(clEnqueueNativeKernel)
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
#define __ENQUEUE_MIGRATE_MEM_OBJECTS_ERR CL_HPP_ERR_STR_(clEnqueueMigrateMemObjects)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120
#if CL_HPP_TARGET_OPENCL_VERSION >= 210
#define __ENQUEUE_MIGRATE_SVM_ERR              CL_HPP_ERR_STR_(clEnqueueSVMMigrateMem)
#define __SET_DEFAULT_DEVICE_COMMAND_QUEUE_ERR CL_HPP_ERR_STR_(clSetDefaultDeviceCommandQueue)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 210


#define __ENQUEUE_ACQUIRE_GL_ERR CL_HPP_ERR_STR_(clEnqueueAcquireGLObjects)
#define __ENQUEUE_RELEASE_GL_ERR CL_HPP_ERR_STR_(clEnqueueReleaseGLObjects)

#define __CREATE_PIPE_ERR   CL_HPP_ERR_STR_(clCreatePipe)
#define __GET_PIPE_INFO_ERR CL_HPP_ERR_STR_(clGetPipeInfo)


#define __RETAIN_ERR          CL_HPP_ERR_STR_(Retain Object)
#define __RELEASE_ERR         CL_HPP_ERR_STR_(Release Object)
#define __FLUSH_ERR           CL_HPP_ERR_STR_(clFlush)
#define __FINISH_ERR          CL_HPP_ERR_STR_(clFinish)
#define __VECTOR_CAPACITY_ERR CL_HPP_ERR_STR_(Vector capacity error)

#if CL_HPP_TARGET_OPENCL_VERSION >= 210
#define __GET_HOST_TIMER_ERR            CL_HPP_ERR_STR_(clGetHostTimer)
#define __GET_DEVICE_AND_HOST_TIMER_ERR CL_HPP_ERR_STR_(clGetDeviceAndHostTimer)
#endif
#if CL_HPP_TARGET_OPENCL_VERSION >= 220
#define __SET_PROGRAM_RELEASE_CALLBACK_ERR        CL_HPP_ERR_STR_(clSetProgramReleaseCallback)
#define __SET_PROGRAM_SPECIALIZATION_CONSTANT_ERR CL_HPP_ERR_STR_(clSetProgramSpecializationConstant)
#endif


/**
 * CL 1.2 version that uses device fission.
 */
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
#define __CREATE_SUB_DEVICES_ERR CL_HPP_ERR_STR_(clCreateSubDevices)
#else
#define __CREATE_SUB_DEVICES_ERR CL_HPP_ERR_STR_(clCreateSubDevicesEXT)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120

/**
 * Deprecated APIs for 1.2
 */
#if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS)
#define __ENQUEUE_MARKER_ERR          CL_HPP_ERR_STR_(clEnqueueMarker)
#define __ENQUEUE_WAIT_FOR_EVENTS_ERR CL_HPP_ERR_STR_(clEnqueueWaitForEvents)
#define __ENQUEUE_BARRIER_ERR         CL_HPP_ERR_STR_(clEnqueueBarrier)
#define __UNLOAD_COMPILER_ERR         CL_HPP_ERR_STR_(clUnloadCompiler)
#define __CREATE_GL_TEXTURE_2D_ERR    CL_HPP_ERR_STR_(clCreateFromGLTexture2D)
#define __CREATE_GL_TEXTURE_3D_ERR    CL_HPP_ERR_STR_(clCreateFromGLTexture3D)
#define __CREATE_IMAGE2D_ERR          CL_HPP_ERR_STR_(clCreateImage2D)
#define __CREATE_IMAGE3D_ERR          CL_HPP_ERR_STR_(clCreateImage3D)
#endif  // #if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS)

/**
 * Deprecated APIs for 2.0
 */
#if defined(CL_USE_DEPRECATED_OPENCL_1_2_APIS)
#define __CREATE_COMMAND_QUEUE_ERR CL_HPP_ERR_STR_(clCreateCommandQueue)
#define __ENQUEUE_TASK_ERR         CL_HPP_ERR_STR_(clEnqueueTask)
#define __CREATE_SAMPLER_ERR       CL_HPP_ERR_STR_(clCreateSampler)
#endif  // #if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS)

/**
 * CL 1.2 marker and barrier commands
 */
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
#define __ENQUEUE_MARKER_WAIT_LIST_ERR  CL_HPP_ERR_STR_(clEnqueueMarkerWithWaitList)
#define __ENQUEUE_BARRIER_WAIT_LIST_ERR CL_HPP_ERR_STR_(clEnqueueBarrierWithWaitList)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120

#if CL_HPP_TARGET_OPENCL_VERSION >= 210
#define __CLONE_KERNEL_ERR CL_HPP_ERR_STR_(clCloneKernel)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 210

#endif  // CL_HPP_USER_OVERRIDE_ERROR_STRINGS
//! \endcond


namespace detail
{
// Generic getInfoHelper. The final parameter is used to guide overload
// resolution: the actual parameter passed is an int, which makes this
// a worse conversion sequence than a specialization that declares the
// parameter as an int.
template <typename Functor, typename T>
inline cl_int getInfoHelper(Functor f, cl_uint name, T *param, long)
{
  return f(name, sizeof(T), param, NULL);
}

// Specialized for getInfo<CL_PROGRAM_BINARIES>
// Assumes that the output vector was correctly resized on the way in
template <typename Func>
inline cl_int getInfoHelper(Func f, cl_uint name, vector<vector<unsigned char>> *param, int)
{
  if (name != CL_PROGRAM_BINARIES)
  {
    return CL_INVALID_VALUE;
  }
  if (param)
  {
    // Create array of pointers, calculate total size and pass pointer array in
    size_type numBinaries = param->size();
    vector<unsigned char *> binariesPointers(numBinaries);

    for (size_type i = 0; i < numBinaries; ++i)
    {
      binariesPointers[i] = (*param)[i].data();
    }

    cl_int err = f(name, numBinaries * sizeof(unsigned char *), binariesPointers.data(), NULL);

    if (err != CL_SUCCESS)
    {
      return err;
    }
  }


  return CL_SUCCESS;
}

// Specialized getInfoHelper for vector params
template <typename Func, typename T>
inline cl_int getInfoHelper(Func f, cl_uint name, vector<T> *param, long)
{
  size_type required;
  cl_int err = f(name, 0, NULL, &required);
  if (err != CL_SUCCESS)
  {
    return err;
  }
  const size_type elements = required / sizeof(T);

  // Temporary to avoid changing param on an error
  vector<T> localData(elements);
  err = f(name, required, localData.data(), NULL);
  if (err != CL_SUCCESS)
  {
    return err;
  }
  if (param)
  {
    *param = std::move(localData);
  }

  return CL_SUCCESS;
}

/* Specialization for reference-counted types. This depends on the
 * existence of Wrapper<T>::cl_type, and none of the other types having the
 * cl_type member. Note that simplify specifying the parameter as Wrapper<T>
 * does not work, because when using a derived type (e.g. Context) the generic
 * template will provide a better match.
 */
template <typename Func, typename T>
inline cl_int getInfoHelper(Func f, cl_uint name, vector<T> *param, int, typename T::cl_type = 0)
{
  size_type required;
  cl_int err = f(name, 0, NULL, &required);
  if (err != CL_SUCCESS)
  {
    return err;
  }

  const size_type elements = required / sizeof(typename T::cl_type);

  vector<typename T::cl_type> value(elements);
  err = f(name, required, value.data(), NULL);
  if (err != CL_SUCCESS)
  {
    return err;
  }

  if (param)
  {
    // Assign to convert CL type to T for each element
    param->resize(elements);

    // Assign to param, constructing with retain behaviour
    // to correctly capture each underlying CL object
    for (size_type i = 0; i < elements; i++)
    {
      (*param)[i] = T(value[i], true);
    }
  }
  return CL_SUCCESS;
}

// Specialized GetInfoHelper for string params
template <typename Func>
inline cl_int getInfoHelper(Func f, cl_uint name, string *param, long)
{
  size_type required;
  cl_int err = f(name, 0, NULL, &required);
  if (err != CL_SUCCESS)
  {
    return err;
  }

  // std::string has a constant data member
  // a char vector does not
  if (required > 0)
  {
    vector<char> value(required);
    err = f(name, required, value.data(), NULL);
    if (err != CL_SUCCESS)
    {
      return err;
    }
    if (param)
    {
      param->assign(begin(value), prev(end(value)));
    }
  }
  else if (param)
  {
    param->assign("");
  }
  return CL_SUCCESS;
}

// Specialized GetInfoHelper for clsize_t params
template <typename Func, size_type N>
inline cl_int getInfoHelper(Func f, cl_uint name, array<size_type, N> *param, long)
{
  size_type required;
  cl_int err = f(name, 0, NULL, &required);
  if (err != CL_SUCCESS)
  {
    return err;
  }

  size_type elements = required / sizeof(size_type);
  vector<size_type> value(elements, 0);

  err = f(name, required, value.data(), NULL);
  if (err != CL_SUCCESS)
  {
    return err;
  }

  // Bound the copy with N to prevent overruns
  // if passed N > than the amount copied
  if (elements > N)
  {
    elements = N;
  }
  for (size_type i = 0; i < elements; ++i)
  {
    (*param)[i] = value[i];
  }

  return CL_SUCCESS;
}

template <typename T>
struct ReferenceHandler;

/* Specialization for reference-counted types. This depends on the
 * existence of Wrapper<T>::cl_type, and none of the other types having the
 * cl_type member. Note that simplify specifying the parameter as Wrapper<T>
 * does not work, because when using a derived type (e.g. Context) the generic
 * template will provide a better match.
 */
template <typename Func, typename T>
inline cl_int getInfoHelper(Func f, cl_uint name, T *param, int, typename T::cl_type = 0)
{
  typename T::cl_type value;
  cl_int err = f(name, sizeof(value), &value, NULL);
  if (err != CL_SUCCESS)
  {
    return err;
  }
  *param = value;
  if (value != NULL)
  {
    err = param->retain();
    if (err != CL_SUCCESS)
    {
      return err;
    }
  }
  return CL_SUCCESS;
}

#define CL_HPP_PARAM_NAME_INFO_1_0_(F)                                                      \
  F(cl_platform_info, CL_PLATFORM_PROFILE, string)                                          \
  F(cl_platform_info, CL_PLATFORM_VERSION, string)                                          \
  F(cl_platform_info, CL_PLATFORM_NAME, string)                                             \
  F(cl_platform_info, CL_PLATFORM_VENDOR, string)                                           \
  F(cl_platform_info, CL_PLATFORM_EXTENSIONS, string)                                       \
                                                                                            \
  F(cl_device_info, CL_DEVICE_TYPE, cl_device_type)                                         \
  F(cl_device_info, CL_DEVICE_VENDOR_ID, cl_uint)                                           \
  F(cl_device_info, CL_DEVICE_MAX_COMPUTE_UNITS, cl_uint)                                   \
  F(cl_device_info, CL_DEVICE_MAX_WORK_ITEM_DIMENSIONS, cl_uint)                            \
  F(cl_device_info, CL_DEVICE_MAX_WORK_GROUP_SIZE, size_type)                               \
  F(cl_device_info, CL_DEVICE_MAX_WORK_ITEM_SIZES, cl::vector<size_type>)                   \
  F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_CHAR, cl_uint)                         \
  F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_SHORT, cl_uint)                        \
  F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_INT, cl_uint)                          \
  F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_LONG, cl_uint)                         \
  F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_FLOAT, cl_uint)                        \
  F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_DOUBLE, cl_uint)                       \
  F(cl_device_info, CL_DEVICE_MAX_CLOCK_FREQUENCY, cl_uint)                                 \
  F(cl_device_info, CL_DEVICE_ADDRESS_BITS, cl_uint)                                        \
  F(cl_device_info, CL_DEVICE_MAX_READ_IMAGE_ARGS, cl_uint)                                 \
  F(cl_device_info, CL_DEVICE_MAX_WRITE_IMAGE_ARGS, cl_uint)                                \
  F(cl_device_info, CL_DEVICE_MAX_MEM_ALLOC_SIZE, cl_ulong)                                 \
  F(cl_device_info, CL_DEVICE_IMAGE2D_MAX_WIDTH, size_type)                                 \
  F(cl_device_info, CL_DEVICE_IMAGE2D_MAX_HEIGHT, size_type)                                \
  F(cl_device_info, CL_DEVICE_IMAGE3D_MAX_WIDTH, size_type)                                 \
  F(cl_device_info, CL_DEVICE_IMAGE3D_MAX_HEIGHT, size_type)                                \
  F(cl_device_info, CL_DEVICE_IMAGE3D_MAX_DEPTH, size_type)                                 \
  F(cl_device_info, CL_DEVICE_IMAGE_SUPPORT, cl_bool)                                       \
  F(cl_device_info, CL_DEVICE_MAX_PARAMETER_SIZE, size_type)                                \
  F(cl_device_info, CL_DEVICE_MAX_SAMPLERS, cl_uint)                                        \
  F(cl_device_info, CL_DEVICE_MEM_BASE_ADDR_ALIGN, cl_uint)                                 \
  F(cl_device_info, CL_DEVICE_MIN_DATA_TYPE_ALIGN_SIZE, cl_uint)                            \
  F(cl_device_info, CL_DEVICE_SINGLE_FP_CONFIG, cl_device_fp_config)                        \
  F(cl_device_info, CL_DEVICE_DOUBLE_FP_CONFIG, cl_device_fp_config)                        \
  F(cl_device_info, CL_DEVICE_HALF_FP_CONFIG, cl_device_fp_config)                          \
  F(cl_device_info, CL_DEVICE_GLOBAL_MEM_CACHE_TYPE, cl_device_mem_cache_type)              \
  F(cl_device_info, CL_DEVICE_GLOBAL_MEM_CACHELINE_SIZE, cl_uint)                           \
  F(cl_device_info, CL_DEVICE_GLOBAL_MEM_CACHE_SIZE, cl_ulong)                              \
  F(cl_device_info, CL_DEVICE_GLOBAL_MEM_SIZE, cl_ulong)                                    \
  F(cl_device_info, CL_DEVICE_MAX_CONSTANT_BUFFER_SIZE, cl_ulong)                           \
  F(cl_device_info, CL_DEVICE_MAX_CONSTANT_ARGS, cl_uint)                                   \
  F(cl_device_info, CL_DEVICE_LOCAL_MEM_TYPE, cl_device_local_mem_type)                     \
  F(cl_device_info, CL_DEVICE_LOCAL_MEM_SIZE, cl_ulong)                                     \
  F(cl_device_info, CL_DEVICE_ERROR_CORRECTION_SUPPORT, cl_bool)                            \
  F(cl_device_info, CL_DEVICE_PROFILING_TIMER_RESOLUTION, size_type)                        \
  F(cl_device_info, CL_DEVICE_ENDIAN_LITTLE, cl_bool)                                       \
  F(cl_device_info, CL_DEVICE_AVAILABLE, cl_bool)                                           \
  F(cl_device_info, CL_DEVICE_COMPILER_AVAILABLE, cl_bool)                                  \
  F(cl_device_info, CL_DEVICE_EXECUTION_CAPABILITIES, cl_device_exec_capabilities)          \
  F(cl_device_info, CL_DEVICE_PLATFORM, cl_platform_id)                                     \
  F(cl_device_info, CL_DEVICE_NAME, string)                                                 \
  F(cl_device_info, CL_DEVICE_VENDOR, string)                                               \
  F(cl_device_info, CL_DRIVER_VERSION, string)                                              \
  F(cl_device_info, CL_DEVICE_PROFILE, string)                                              \
  F(cl_device_info, CL_DEVICE_VERSION, string)                                              \
  F(cl_device_info, CL_DEVICE_EXTENSIONS, string)                                           \
                                                                                            \
  F(cl_context_info, CL_CONTEXT_REFERENCE_COUNT, cl_uint)                                   \
  F(cl_context_info, CL_CONTEXT_DEVICES, cl::vector<Device>)                                \
  F(cl_context_info, CL_CONTEXT_PROPERTIES, cl::vector<cl_context_properties>)              \
                                                                                            \
  F(cl_event_info, CL_EVENT_COMMAND_QUEUE, cl::CommandQueue)                                \
  F(cl_event_info, CL_EVENT_COMMAND_TYPE, cl_command_type)                                  \
  F(cl_event_info, CL_EVENT_REFERENCE_COUNT, cl_uint)                                       \
  F(cl_event_info, CL_EVENT_COMMAND_EXECUTION_STATUS, cl_int)                               \
                                                                                            \
  F(cl_profiling_info, CL_PROFILING_COMMAND_QUEUED, cl_ulong)                               \
  F(cl_profiling_info, CL_PROFILING_COMMAND_SUBMIT, cl_ulong)                               \
  F(cl_profiling_info, CL_PROFILING_COMMAND_START, cl_ulong)                                \
  F(cl_profiling_info, CL_PROFILING_COMMAND_END, cl_ulong)                                  \
                                                                                            \
  F(cl_mem_info, CL_MEM_TYPE, cl_mem_object_type)                                           \
  F(cl_mem_info, CL_MEM_FLAGS, cl_mem_flags)                                                \
  F(cl_mem_info, CL_MEM_SIZE, size_type)                                                    \
  F(cl_mem_info, CL_MEM_HOST_PTR, void *)                                                   \
  F(cl_mem_info, CL_MEM_MAP_COUNT, cl_uint)                                                 \
  F(cl_mem_info, CL_MEM_REFERENCE_COUNT, cl_uint)                                           \
  F(cl_mem_info, CL_MEM_CONTEXT, cl::Context)                                               \
                                                                                            \
  F(cl_image_info, CL_IMAGE_FORMAT, cl_image_format)                                        \
  F(cl_image_info, CL_IMAGE_ELEMENT_SIZE, size_type)                                        \
  F(cl_image_info, CL_IMAGE_ROW_PITCH, size_type)                                           \
  F(cl_image_info, CL_IMAGE_SLICE_PITCH, size_type)                                         \
  F(cl_image_info, CL_IMAGE_WIDTH, size_type)                                               \
  F(cl_image_info, CL_IMAGE_HEIGHT, size_type)                                              \
  F(cl_image_info, CL_IMAGE_DEPTH, size_type)                                               \
                                                                                            \
  F(cl_sampler_info, CL_SAMPLER_REFERENCE_COUNT, cl_uint)                                   \
  F(cl_sampler_info, CL_SAMPLER_CONTEXT, cl::Context)                                       \
  F(cl_sampler_info, CL_SAMPLER_NORMALIZED_COORDS, cl_bool)                                 \
  F(cl_sampler_info, CL_SAMPLER_ADDRESSING_MODE, cl_addressing_mode)                        \
  F(cl_sampler_info, CL_SAMPLER_FILTER_MODE, cl_filter_mode)                                \
                                                                                            \
  F(cl_program_info, CL_PROGRAM_REFERENCE_COUNT, cl_uint)                                   \
  F(cl_program_info, CL_PROGRAM_CONTEXT, cl::Context)                                       \
  F(cl_program_info, CL_PROGRAM_NUM_DEVICES, cl_uint)                                       \
  F(cl_program_info, CL_PROGRAM_DEVICES, cl::vector<Device>)                                \
  F(cl_program_info, CL_PROGRAM_SOURCE, string)                                             \
  F(cl_program_info, CL_PROGRAM_BINARY_SIZES, cl::vector<size_type>)                        \
  F(cl_program_info, CL_PROGRAM_BINARIES, cl::vector<cl::vector<unsigned char>>)            \
                                                                                            \
  F(cl_program_build_info, CL_PROGRAM_BUILD_STATUS, cl_build_status)                        \
  F(cl_program_build_info, CL_PROGRAM_BUILD_OPTIONS, string)                                \
  F(cl_program_build_info, CL_PROGRAM_BUILD_LOG, string)                                    \
                                                                                            \
  F(cl_kernel_info, CL_KERNEL_FUNCTION_NAME, string)                                        \
  F(cl_kernel_info, CL_KERNEL_NUM_ARGS, cl_uint)                                            \
  F(cl_kernel_info, CL_KERNEL_REFERENCE_COUNT, cl_uint)                                     \
  F(cl_kernel_info, CL_KERNEL_CONTEXT, cl::Context)                                         \
  F(cl_kernel_info, CL_KERNEL_PROGRAM, cl::Program)                                         \
                                                                                            \
  F(cl_kernel_work_group_info, CL_KERNEL_WORK_GROUP_SIZE, size_type)                        \
  F(cl_kernel_work_group_info, CL_KERNEL_COMPILE_WORK_GROUP_SIZE, cl::detail::size_t_array) \
  F(cl_kernel_work_group_info, CL_KERNEL_LOCAL_MEM_SIZE, cl_ulong)                          \
                                                                                            \
  F(cl_command_queue_info, CL_QUEUE_CONTEXT, cl::Context)                                   \
  F(cl_command_queue_info, CL_QUEUE_DEVICE, cl::Device)                                     \
  F(cl_command_queue_info, CL_QUEUE_REFERENCE_COUNT, cl_uint)                               \
  F(cl_command_queue_info, CL_QUEUE_PROPERTIES, cl_command_queue_properties)


#define CL_HPP_PARAM_NAME_INFO_1_1_(F)                                                  \
  F(cl_context_info, CL_CONTEXT_NUM_DEVICES, cl_uint)                                   \
  F(cl_device_info, CL_DEVICE_PREFERRED_VECTOR_WIDTH_HALF, cl_uint)                     \
  F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_CHAR, cl_uint)                        \
  F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_SHORT, cl_uint)                       \
  F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_INT, cl_uint)                         \
  F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_LONG, cl_uint)                        \
  F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_FLOAT, cl_uint)                       \
  F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_DOUBLE, cl_uint)                      \
  F(cl_device_info, CL_DEVICE_NATIVE_VECTOR_WIDTH_HALF, cl_uint)                        \
  F(cl_device_info, CL_DEVICE_OPENCL_C_VERSION, string)                                 \
                                                                                        \
  F(cl_mem_info, CL_MEM_ASSOCIATED_MEMOBJECT, cl::Memory)                               \
  F(cl_mem_info, CL_MEM_OFFSET, size_type)                                              \
                                                                                        \
  F(cl_kernel_work_group_info, CL_KERNEL_PREFERRED_WORK_GROUP_SIZE_MULTIPLE, size_type) \
  F(cl_kernel_work_group_info, CL_KERNEL_PRIVATE_MEM_SIZE, cl_ulong)                    \
                                                                                        \
  F(cl_event_info, CL_EVENT_CONTEXT, cl::Context)

#define CL_HPP_PARAM_NAME_INFO_1_2_(F)                                                        \
  F(cl_program_info, CL_PROGRAM_NUM_KERNELS, size_type)                                       \
  F(cl_program_info, CL_PROGRAM_KERNEL_NAMES, string)                                         \
                                                                                              \
  F(cl_program_build_info, CL_PROGRAM_BINARY_TYPE, cl_program_binary_type)                    \
                                                                                              \
  F(cl_kernel_info, CL_KERNEL_ATTRIBUTES, string)                                             \
                                                                                              \
  F(cl_kernel_arg_info, CL_KERNEL_ARG_ADDRESS_QUALIFIER, cl_kernel_arg_address_qualifier)     \
  F(cl_kernel_arg_info, CL_KERNEL_ARG_ACCESS_QUALIFIER, cl_kernel_arg_access_qualifier)       \
  F(cl_kernel_arg_info, CL_KERNEL_ARG_TYPE_NAME, string)                                      \
  F(cl_kernel_arg_info, CL_KERNEL_ARG_NAME, string)                                           \
  F(cl_kernel_arg_info, CL_KERNEL_ARG_TYPE_QUALIFIER, cl_kernel_arg_type_qualifier)           \
                                                                                              \
  F(cl_kernel_work_group_info, CL_KERNEL_GLOBAL_WORK_SIZE, cl::detail::size_t_array)          \
                                                                                              \
  F(cl_device_info, CL_DEVICE_LINKER_AVAILABLE, cl_bool)                                      \
  F(cl_device_info, CL_DEVICE_IMAGE_MAX_BUFFER_SIZE, size_type)                               \
  F(cl_device_info, CL_DEVICE_IMAGE_MAX_ARRAY_SIZE, size_type)                                \
  F(cl_device_info, CL_DEVICE_PARENT_DEVICE, cl::Device)                                      \
  F(cl_device_info, CL_DEVICE_PARTITION_MAX_SUB_DEVICES, cl_uint)                             \
  F(cl_device_info, CL_DEVICE_PARTITION_PROPERTIES, cl::vector<cl_device_partition_property>) \
  F(cl_device_info, CL_DEVICE_PARTITION_TYPE, cl::vector<cl_device_partition_property>)       \
  F(cl_device_info, CL_DEVICE_REFERENCE_COUNT, cl_uint)                                       \
  F(cl_device_info, CL_DEVICE_PREFERRED_INTEROP_USER_SYNC, cl_bool)                           \
  F(cl_device_info, CL_DEVICE_PARTITION_AFFINITY_DOMAIN, cl_device_affinity_domain)           \
  F(cl_device_info, CL_DEVICE_BUILT_IN_KERNELS, string)                                       \
  F(cl_device_info, CL_DEVICE_PRINTF_BUFFER_SIZE, size_type)                                  \
                                                                                              \
  F(cl_image_info, CL_IMAGE_ARRAY_SIZE, size_type)                                            \
  F(cl_image_info, CL_IMAGE_NUM_MIP_LEVELS, cl_uint)                                          \
  F(cl_image_info, CL_IMAGE_NUM_SAMPLES, cl_uint)

#define CL_HPP_PARAM_NAME_INFO_2_0_(F)                                                 \
  F(cl_device_info, CL_DEVICE_QUEUE_ON_HOST_PROPERTIES, cl_command_queue_properties)   \
  F(cl_device_info, CL_DEVICE_QUEUE_ON_DEVICE_PROPERTIES, cl_command_queue_properties) \
  F(cl_device_info, CL_DEVICE_QUEUE_ON_DEVICE_PREFERRED_SIZE, cl_uint)                 \
  F(cl_device_info, CL_DEVICE_QUEUE_ON_DEVICE_MAX_SIZE, cl_uint)                       \
  F(cl_device_info, CL_DEVICE_MAX_ON_DEVICE_QUEUES, cl_uint)                           \
  F(cl_device_info, CL_DEVICE_MAX_ON_DEVICE_EVENTS, cl_uint)                           \
  F(cl_device_info, CL_DEVICE_MAX_PIPE_ARGS, cl_uint)                                  \
  F(cl_device_info, CL_DEVICE_PIPE_MAX_ACTIVE_RESERVATIONS, cl_uint)                   \
  F(cl_device_info, CL_DEVICE_PIPE_MAX_PACKET_SIZE, cl_uint)                           \
  F(cl_device_info, CL_DEVICE_SVM_CAPABILITIES, cl_device_svm_capabilities)            \
  F(cl_device_info, CL_DEVICE_PREFERRED_PLATFORM_ATOMIC_ALIGNMENT, cl_uint)            \
  F(cl_device_info, CL_DEVICE_PREFERRED_GLOBAL_ATOMIC_ALIGNMENT, cl_uint)              \
  F(cl_device_info, CL_DEVICE_PREFERRED_LOCAL_ATOMIC_ALIGNMENT, cl_uint)               \
  F(cl_device_info, CL_DEVICE_IMAGE_PITCH_ALIGNMENT, cl_uint)                          \
  F(cl_device_info, CL_DEVICE_IMAGE_BASE_ADDRESS_ALIGNMENT, cl_uint)                   \
  F(cl_device_info, CL_DEVICE_MAX_READ_WRITE_IMAGE_ARGS, cl_uint)                      \
  F(cl_device_info, CL_DEVICE_MAX_GLOBAL_VARIABLE_SIZE, size_type)                     \
  F(cl_device_info, CL_DEVICE_GLOBAL_VARIABLE_PREFERRED_TOTAL_SIZE, size_type)         \
  F(cl_profiling_info, CL_PROFILING_COMMAND_COMPLETE, cl_ulong)                        \
  F(cl_kernel_exec_info, CL_KERNEL_EXEC_INFO_SVM_FINE_GRAIN_SYSTEM, cl_bool)           \
  F(cl_kernel_exec_info, CL_KERNEL_EXEC_INFO_SVM_PTRS, void **)                        \
  F(cl_command_queue_info, CL_QUEUE_SIZE, cl_uint)                                     \
  F(cl_mem_info, CL_MEM_USES_SVM_POINTER, cl_bool)                                     \
  F(cl_program_build_info, CL_PROGRAM_BUILD_GLOBAL_VARIABLE_TOTAL_SIZE, size_type)     \
  F(cl_pipe_info, CL_PIPE_PACKET_SIZE, cl_uint)                                        \
  F(cl_pipe_info, CL_PIPE_MAX_PACKETS, cl_uint)

#define CL_HPP_PARAM_NAME_INFO_SUBGROUP_KHR_(F)                                        \
  F(cl_kernel_sub_group_info, CL_KERNEL_MAX_SUB_GROUP_SIZE_FOR_NDRANGE_KHR, size_type) \
  F(cl_kernel_sub_group_info, CL_KERNEL_SUB_GROUP_COUNT_FOR_NDRANGE_KHR, size_type)

#define CL_HPP_PARAM_NAME_INFO_IL_KHR_(F)             \
  F(cl_device_info, CL_DEVICE_IL_VERSION_KHR, string) \
  F(cl_program_info, CL_PROGRAM_IL_KHR, cl::vector<unsigned char>)

#define CL_HPP_PARAM_NAME_INFO_2_1_(F)                                                            \
  F(cl_platform_info, CL_PLATFORM_HOST_TIMER_RESOLUTION, cl_ulong)                                \
  F(cl_program_info, CL_PROGRAM_IL, cl::vector<unsigned char>)                                    \
  F(cl_device_info, CL_DEVICE_MAX_NUM_SUB_GROUPS, cl_uint)                                        \
  F(cl_device_info, CL_DEVICE_IL_VERSION, string)                                                 \
  F(cl_device_info, CL_DEVICE_SUB_GROUP_INDEPENDENT_FORWARD_PROGRESS, cl_bool)                    \
  F(cl_command_queue_info, CL_QUEUE_DEVICE_DEFAULT, cl::DeviceCommandQueue)                       \
  F(cl_kernel_sub_group_info, CL_KERNEL_MAX_SUB_GROUP_SIZE_FOR_NDRANGE, size_type)                \
  F(cl_kernel_sub_group_info, CL_KERNEL_SUB_GROUP_COUNT_FOR_NDRANGE, size_type)                   \
  F(cl_kernel_sub_group_info, CL_KERNEL_LOCAL_SIZE_FOR_SUB_GROUP_COUNT, cl::detail::size_t_array) \
  F(cl_kernel_sub_group_info, CL_KERNEL_MAX_NUM_SUB_GROUPS, size_type)                            \
  F(cl_kernel_sub_group_info, CL_KERNEL_COMPILE_NUM_SUB_GROUPS, size_type)

#define CL_HPP_PARAM_NAME_INFO_2_2_(F)                               \
  F(cl_program_info, CL_PROGRAM_SCOPE_GLOBAL_CTORS_PRESENT, cl_bool) \
  F(cl_program_info, CL_PROGRAM_SCOPE_GLOBAL_DTORS_PRESENT, cl_bool)

#define CL_HPP_PARAM_NAME_DEVICE_FISSION_(F)                                                      \
  F(cl_device_info, CL_DEVICE_PARENT_DEVICE_EXT, cl_device_id)                                    \
  F(cl_device_info, CL_DEVICE_PARTITION_TYPES_EXT, cl::vector<cl_device_partition_property_ext>)  \
  F(cl_device_info, CL_DEVICE_AFFINITY_DOMAINS_EXT, cl::vector<cl_device_partition_property_ext>) \
  F(cl_device_info, CL_DEVICE_REFERENCE_COUNT_EXT, cl_uint)                                       \
  F(cl_device_info, CL_DEVICE_PARTITION_STYLE_EXT, cl::vector<cl_device_partition_property_ext>)

#define CL_HPP_PARAM_NAME_CL_KHR_EXTENDED_VERSIONING_CL3_SHARED_(F)                             \
  F(cl_platform_info, CL_PLATFORM_NUMERIC_VERSION_KHR, cl_version_khr)                          \
  F(cl_platform_info, CL_PLATFORM_EXTENSIONS_WITH_VERSION_KHR, cl::vector<cl_name_version_khr>) \
                                                                                                \
  F(cl_device_info, CL_DEVICE_NUMERIC_VERSION_KHR, cl_version_khr)                              \
  F(cl_device_info, CL_DEVICE_EXTENSIONS_WITH_VERSION_KHR, cl::vector<cl_name_version_khr>)     \
  F(cl_device_info, CL_DEVICE_ILS_WITH_VERSION_KHR, cl::vector<cl_name_version_khr>)            \
  F(cl_device_info, CL_DEVICE_BUILT_IN_KERNELS_WITH_VERSION_KHR, cl::vector<cl_name_version_khr>)

#define CL_HPP_PARAM_NAME_CL_KHR_EXTENDED_VERSIONING_KHRONLY_(F) \
  F(cl_device_info, CL_DEVICE_OPENCL_C_NUMERIC_VERSION_KHR, cl_version_khr)

#define CL_HPP_PARAM_NAME_INFO_3_0_(F)                                                            \
  F(cl_platform_info, CL_PLATFORM_NUMERIC_VERSION, cl_version)                                    \
  F(cl_platform_info, CL_PLATFORM_EXTENSIONS_WITH_VERSION, cl::vector<cl_name_version>)           \
                                                                                                  \
  F(cl_device_info, CL_DEVICE_NUMERIC_VERSION, cl_version)                                        \
  F(cl_device_info, CL_DEVICE_EXTENSIONS_WITH_VERSION, cl::vector<cl_name_version>)               \
  F(cl_device_info, CL_DEVICE_ILS_WITH_VERSION, cl::vector<cl_name_version>)                      \
  F(cl_device_info, CL_DEVICE_BUILT_IN_KERNELS_WITH_VERSION, cl::vector<cl_name_version>)         \
  F(cl_device_info, CL_DEVICE_ATOMIC_MEMORY_CAPABILITIES, cl_device_atomic_capabilities)          \
  F(cl_device_info, CL_DEVICE_ATOMIC_FENCE_CAPABILITIES, cl_device_atomic_capabilities)           \
  F(cl_device_info, CL_DEVICE_NON_UNIFORM_WORK_GROUP_SUPPORT, cl_bool)                            \
  F(cl_device_info, CL_DEVICE_OPENCL_C_ALL_VERSIONS, cl::vector<cl_name_version>)                 \
  F(cl_device_info, CL_DEVICE_PREFERRED_WORK_GROUP_SIZE_MULTIPLE, size_type)                      \
  F(cl_device_info, CL_DEVICE_WORK_GROUP_COLLECTIVE_FUNCTIONS_SUPPORT, cl_bool)                   \
  F(cl_device_info, CL_DEVICE_GENERIC_ADDRESS_SPACE_SUPPORT, cl_bool)                             \
  F(cl_device_info, CL_DEVICE_OPENCL_C_FEATURES, cl::vector<cl_name_version>)                     \
  F(cl_device_info, CL_DEVICE_DEVICE_ENQUEUE_CAPABILITIES, cl_device_device_enqueue_capabilities) \
  F(cl_device_info, CL_DEVICE_PIPE_SUPPORT, cl_bool)                                              \
  F(cl_device_info, CL_DEVICE_LATEST_CONFORMANCE_VERSION_PASSED, string)                          \
                                                                                                  \
  F(cl_command_queue_info, CL_QUEUE_PROPERTIES_ARRAY, cl::vector<cl_queue_properties>)            \
  F(cl_mem_info, CL_MEM_PROPERTIES, cl::vector<cl_mem_properties>)                                \
  F(cl_pipe_info, CL_PIPE_PROPERTIES, cl::vector<cl_pipe_properties>)                             \
  F(cl_sampler_info, CL_SAMPLER_PROPERTIES, cl::vector<cl_sampler_properties>)

template <typename enum_type, cl_int Name>
struct param_traits
{
};

#define CL_HPP_DECLARE_PARAM_TRAITS_(token, param_name, T) \
  struct token;                                            \
  template <>                                              \
  struct param_traits<detail::token, param_name>           \
  {                                                        \
    enum                                                   \
    {                                                      \
      value = param_name                                   \
    };                                                     \
    typedef T param_type;                                  \
  };

CL_HPP_PARAM_NAME_INFO_1_0_(CL_HPP_DECLARE_PARAM_TRAITS_)
#if CL_HPP_TARGET_OPENCL_VERSION >= 110
CL_HPP_PARAM_NAME_INFO_1_1_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 110
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
CL_HPP_PARAM_NAME_INFO_1_2_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120
#if CL_HPP_TARGET_OPENCL_VERSION >= 200
CL_HPP_PARAM_NAME_INFO_2_0_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200
#if CL_HPP_TARGET_OPENCL_VERSION >= 210
CL_HPP_PARAM_NAME_INFO_2_1_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 210
#if CL_HPP_TARGET_OPENCL_VERSION >= 220
CL_HPP_PARAM_NAME_INFO_2_2_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 220
#if CL_HPP_TARGET_OPENCL_VERSION >= 300
CL_HPP_PARAM_NAME_INFO_3_0_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 300

#if defined(CL_HPP_USE_CL_SUB_GROUPS_KHR) && CL_HPP_TARGET_OPENCL_VERSION < 210
CL_HPP_PARAM_NAME_INFO_SUBGROUP_KHR_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // #if defined(CL_HPP_USE_CL_SUB_GROUPS_KHR) && CL_HPP_TARGET_OPENCL_VERSION < 210

#if defined(CL_HPP_USE_IL_KHR) && CL_HPP_TARGET_OPENCL_VERSION < 210
CL_HPP_PARAM_NAME_INFO_IL_KHR_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // #if defined(CL_HPP_USE_IL_KHR)


// Flags deprecated in OpenCL 2.0
#define CL_HPP_PARAM_NAME_INFO_1_0_DEPRECATED_IN_2_0_(F) \
  F(cl_device_info, CL_DEVICE_QUEUE_PROPERTIES, cl_command_queue_properties)

#define CL_HPP_PARAM_NAME_INFO_1_1_DEPRECATED_IN_2_0_(F) F(cl_device_info, CL_DEVICE_HOST_UNIFIED_MEMORY, cl_bool)

#define CL_HPP_PARAM_NAME_INFO_1_2_DEPRECATED_IN_2_0_(F) F(cl_image_info, CL_IMAGE_BUFFER, cl::Buffer)

// Include deprecated query flags based on versions
// Only include deprecated 1.0 flags if 2.0 not active as there is an enum clash
#if CL_HPP_TARGET_OPENCL_VERSION > 100 && CL_HPP_MINIMUM_OPENCL_VERSION < 200 && CL_HPP_TARGET_OPENCL_VERSION < 200
CL_HPP_PARAM_NAME_INFO_1_0_DEPRECATED_IN_2_0_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 110
#if CL_HPP_TARGET_OPENCL_VERSION > 110 && CL_HPP_MINIMUM_OPENCL_VERSION < 200
CL_HPP_PARAM_NAME_INFO_1_1_DEPRECATED_IN_2_0_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 120
#if CL_HPP_TARGET_OPENCL_VERSION > 120 && CL_HPP_MINIMUM_OPENCL_VERSION < 200
CL_HPP_PARAM_NAME_INFO_1_2_DEPRECATED_IN_2_0_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 200

#if defined(CL_HPP_USE_CL_DEVICE_FISSION)
CL_HPP_PARAM_NAME_DEVICE_FISSION_(CL_HPP_DECLARE_PARAM_TRAITS_);
#endif  // CL_HPP_USE_CL_DEVICE_FISSION

#if defined(cl_khr_extended_versioning)
#if CL_HPP_TARGET_OPENCL_VERSION < 300
CL_HPP_PARAM_NAME_CL_KHR_EXTENDED_VERSIONING_CL3_SHARED_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // CL_HPP_TARGET_OPENCL_VERSION < 300
CL_HPP_PARAM_NAME_CL_KHR_EXTENDED_VERSIONING_KHRONLY_(CL_HPP_DECLARE_PARAM_TRAITS_)
#endif  // cl_khr_extended_versioning

#if defined(cl_khr_device_uuid)
using uuid_array = array<cl_uchar, CL_UUID_SIZE_KHR>;
using luid_array = array<cl_uchar, CL_LUID_SIZE_KHR>;
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_UUID_KHR, uuid_array)
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DRIVER_UUID_KHR, uuid_array)
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_LUID_VALID_KHR, cl_bool)
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_LUID_KHR, luid_array)
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_NODE_MASK_KHR, cl_uint)
#endif

#if defined(cl_khr_pci_bus_info)
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_PCI_BUS_INFO_KHR, cl_device_pci_bus_info_khr)
#endif

#if defined(cl_khr_integer_dot_product)
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_INTEGER_DOT_PRODUCT_CAPABILITIES_KHR,
                             cl_device_integer_dot_product_capabilities_khr)
#if defined(CL_DEVICE_INTEGER_DOT_PRODUCT_ACCELERATION_PROPERTIES_8BIT_KHR)
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_INTEGER_DOT_PRODUCT_ACCELERATION_PROPERTIES_8BIT_KHR,
                             cl_device_integer_dot_product_acceleration_properties_khr)
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_INTEGER_DOT_PRODUCT_ACCELERATION_PROPERTIES_4x8BIT_PACKED_KHR,
                             cl_device_integer_dot_product_acceleration_properties_khr)
#endif  // defined(CL_DEVICE_INTEGER_DOT_PRODUCT_ACCELERATION_PROPERTIES_8BIT_KHR)
#endif  // defined(cl_khr_integer_dot_product)

#ifdef CL_PLATFORM_ICD_SUFFIX_KHR
CL_HPP_DECLARE_PARAM_TRAITS_(cl_platform_info, CL_PLATFORM_ICD_SUFFIX_KHR, string)
#endif

#ifdef CL_DEVICE_PROFILING_TIMER_OFFSET_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_PROFILING_TIMER_OFFSET_AMD, cl_ulong)
#endif
#ifdef CL_DEVICE_GLOBAL_FREE_MEMORY_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_GLOBAL_FREE_MEMORY_AMD, vector<size_type>)
#endif
#ifdef CL_DEVICE_SIMD_PER_COMPUTE_UNIT_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_SIMD_PER_COMPUTE_UNIT_AMD, cl_uint)
#endif
#ifdef CL_DEVICE_SIMD_WIDTH_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_SIMD_WIDTH_AMD, cl_uint)
#endif
#ifdef CL_DEVICE_SIMD_INSTRUCTION_WIDTH_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_SIMD_INSTRUCTION_WIDTH_AMD, cl_uint)
#endif
#ifdef CL_DEVICE_WAVEFRONT_WIDTH_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_WAVEFRONT_WIDTH_AMD, cl_uint)
#endif
#ifdef CL_DEVICE_GLOBAL_MEM_CHANNELS_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_GLOBAL_MEM_CHANNELS_AMD, cl_uint)
#endif
#ifdef CL_DEVICE_GLOBAL_MEM_CHANNEL_BANKS_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_GLOBAL_MEM_CHANNEL_BANKS_AMD, cl_uint)
#endif
#ifdef CL_DEVICE_GLOBAL_MEM_CHANNEL_BANK_WIDTH_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_GLOBAL_MEM_CHANNEL_BANK_WIDTH_AMD, cl_uint)
#endif
#ifdef CL_DEVICE_LOCAL_MEM_SIZE_PER_COMPUTE_UNIT_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_LOCAL_MEM_SIZE_PER_COMPUTE_UNIT_AMD, cl_uint)
#endif
#ifdef CL_DEVICE_LOCAL_MEM_BANKS_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_LOCAL_MEM_BANKS_AMD, cl_uint)
#endif
#ifdef CL_DEVICE_BOARD_NAME_AMD
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_BOARD_NAME_AMD, string)
#endif

#ifdef CL_DEVICE_COMPUTE_UNITS_BITFIELD_ARM
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_COMPUTE_UNITS_BITFIELD_ARM, cl_ulong)
#endif
#ifdef CL_DEVICE_JOB_SLOTS_ARM
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_JOB_SLOTS_ARM, cl_uint)
#endif
#ifdef CL_DEVICE_SCHEDULING_CONTROLS_CAPABILITIES_ARM
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_SCHEDULING_CONTROLS_CAPABILITIES_ARM, cl_bitfield)
#endif
#ifdef CL_DEVICE_SUPPORTED_REGISTER_ALLOCATIONS_ARM
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_SUPPORTED_REGISTER_ALLOCATIONS_ARM, vector<cl_uint>)
#endif
#ifdef CL_KERNEL_EXEC_INFO_WORKGROUP_BATCH_SIZE_ARM
CL_HPP_DECLARE_PARAM_TRAITS_(cl_kernel_exec_info, CL_KERNEL_EXEC_INFO_WORKGROUP_BATCH_SIZE_ARM, cl_uint)
#endif
#ifdef CL_KERNEL_EXEC_INFO_WORKGROUP_BATCH_SIZE_MODIFIER_ARM
CL_HPP_DECLARE_PARAM_TRAITS_(cl_kernel_exec_info, CL_KERNEL_EXEC_INFO_WORKGROUP_BATCH_SIZE_MODIFIER_ARM, cl_int)
#endif

#ifdef CL_DEVICE_COMPUTE_CAPABILITY_MAJOR_NV
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_COMPUTE_CAPABILITY_MAJOR_NV, cl_uint)
#endif
#ifdef CL_DEVICE_COMPUTE_CAPABILITY_MINOR_NV
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_COMPUTE_CAPABILITY_MINOR_NV, cl_uint)
#endif
#ifdef CL_DEVICE_REGISTERS_PER_BLOCK_NV
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_REGISTERS_PER_BLOCK_NV, cl_uint)
#endif
#ifdef CL_DEVICE_WARP_SIZE_NV
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_WARP_SIZE_NV, cl_uint)
#endif
#ifdef CL_DEVICE_GPU_OVERLAP_NV
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_GPU_OVERLAP_NV, cl_bool)
#endif
#ifdef CL_DEVICE_KERNEL_EXEC_TIMEOUT_NV
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_KERNEL_EXEC_TIMEOUT_NV, cl_bool)
#endif
#ifdef CL_DEVICE_INTEGRATED_MEMORY_NV
CL_HPP_DECLARE_PARAM_TRAITS_(cl_device_info, CL_DEVICE_INTEGRATED_MEMORY_NV, cl_bool)
#endif

// Convenience functions

template <typename Func, typename T>
inline cl_int getInfo(Func f, cl_uint name, T *param)
{
  return getInfoHelper(f, name, param, 0);
}

template <typename Func, typename Arg0>
struct GetInfoFunctor0
{
  Func f_;
  const Arg0 &arg0_;
  cl_int operator()(cl_uint param, size_type size, void *value, size_type *size_ret)
  {
    return f_(arg0_, param, size, value, size_ret);
  }
};

template <typename Func, typename Arg0, typename Arg1>
struct GetInfoFunctor1
{
  Func f_;
  const Arg0 &arg0_;
  const Arg1 &arg1_;
  cl_int operator()(cl_uint param, size_type size, void *value, size_type *size_ret)
  {
    return f_(arg0_, arg1_, param, size, value, size_ret);
  }
};

template <typename Func, typename Arg0, typename T>
inline cl_int getInfo(Func f, const Arg0 &arg0, cl_uint name, T *param)
{
  GetInfoFunctor0<Func, Arg0> f0 = { f, arg0 };
  return getInfoHelper(f0, name, param, 0);
}

template <typename Func, typename Arg0, typename Arg1, typename T>
inline cl_int getInfo(Func f, const Arg0 &arg0, const Arg1 &arg1, cl_uint name, T *param)
{
  GetInfoFunctor1<Func, Arg0, Arg1> f0 = { f, arg0, arg1 };
  return getInfoHelper(f0, name, param, 0);
}


template <typename T>
struct ReferenceHandler
{
};

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
/**
 * OpenCL 1.2 devices do have retain/release.
 */
template <>
struct ReferenceHandler<cl_device_id>
{
  /**
   * Retain the device.
   * \param device A valid device created using createSubDevices
   * \return
   *   CL_SUCCESS if the function executed successfully.
   *   CL_INVALID_DEVICE if device was not a valid subdevice
   *   CL_OUT_OF_RESOURCES
   *   CL_OUT_OF_HOST_MEMORY
   */
  static cl_int retain(cl_device_id device) { return ::clRetainDevice(device); }
  /**
   * Retain the device.
   * \param device A valid device created using createSubDevices
   * \return
   *   CL_SUCCESS if the function executed successfully.
   *   CL_INVALID_DEVICE if device was not a valid subdevice
   *   CL_OUT_OF_RESOURCES
   *   CL_OUT_OF_HOST_MEMORY
   */
  static cl_int release(cl_device_id device) { return ::clReleaseDevice(device); }
};
#else   // CL_HPP_TARGET_OPENCL_VERSION >= 120
/**
 * OpenCL 1.1 devices do not have retain/release.
 */
template <>
struct ReferenceHandler<cl_device_id>
{
  // cl_device_id does not have retain().
  static cl_int retain(cl_device_id) { return CL_SUCCESS; }
  // cl_device_id does not have release().
  static cl_int release(cl_device_id) { return CL_SUCCESS; }
};
#endif  // ! (CL_HPP_TARGET_OPENCL_VERSION >= 120)

template <>
struct ReferenceHandler<cl_platform_id>
{
  // cl_platform_id does not have retain().
  static cl_int retain(cl_platform_id) { return CL_SUCCESS; }
  // cl_platform_id does not have release().
  static cl_int release(cl_platform_id) { return CL_SUCCESS; }
};

template <>
struct ReferenceHandler<cl_context>
{
  static cl_int retain(cl_context context) { return ::clRetainContext(context); }
  static cl_int release(cl_context context) { return ::clReleaseContext(context); }
};

template <>
struct ReferenceHandler<cl_command_queue>
{
  static cl_int retain(cl_command_queue queue) { return ::clRetainCommandQueue(queue); }
  static cl_int release(cl_command_queue queue) { return ::clReleaseCommandQueue(queue); }
};

template <>
struct ReferenceHandler<cl_mem>
{
  static cl_int retain(cl_mem memory) { return ::clRetainMemObject(memory); }
  static cl_int release(cl_mem memory) { return ::clReleaseMemObject(memory); }
};

template <>
struct ReferenceHandler<cl_sampler>
{
  static cl_int retain(cl_sampler sampler) { return ::clRetainSampler(sampler); }
  static cl_int release(cl_sampler sampler) { return ::clReleaseSampler(sampler); }
};

template <>
struct ReferenceHandler<cl_program>
{
  static cl_int retain(cl_program program) { return ::clRetainProgram(program); }
  static cl_int release(cl_program program) { return ::clReleaseProgram(program); }
};

template <>
struct ReferenceHandler<cl_kernel>
{
  static cl_int retain(cl_kernel kernel) { return ::clRetainKernel(kernel); }
  static cl_int release(cl_kernel kernel) { return ::clReleaseKernel(kernel); }
};

template <>
struct ReferenceHandler<cl_event>
{
  static cl_int retain(cl_event event) { return ::clRetainEvent(event); }
  static cl_int release(cl_event event) { return ::clReleaseEvent(event); }
};


#if CL_HPP_TARGET_OPENCL_VERSION >= 120 && CL_HPP_MINIMUM_OPENCL_VERSION < 200
// Extracts version number with major in the upper 16 bits, minor in the lower 16
static cl_uint getVersion(const vector<char> &versionInfo)
{
  int highVersion = 0;
  int lowVersion = 0;
  int index = 7;
  while (versionInfo[index] != '.')
  {
    highVersion *= 10;
    highVersion += versionInfo[index] - '0';
    ++index;
  }
  ++index;
  while (versionInfo[index] != ' ' && versionInfo[index] != '\0')
  {
    lowVersion *= 10;
    lowVersion += versionInfo[index] - '0';
    ++index;
  }
  return (highVersion << 16) | lowVersion;
}

static cl_uint getPlatformVersion(cl_platform_id platform)
{
  size_type size = 0;
  clGetPlatformInfo(platform, CL_PLATFORM_VERSION, 0, NULL, &size);

  vector<char> versionInfo(size);
  clGetPlatformInfo(platform, CL_PLATFORM_VERSION, size, versionInfo.data(), &size);
  return getVersion(versionInfo);
}

static cl_uint getDevicePlatformVersion(cl_device_id device)
{
  cl_platform_id platform;
  clGetDeviceInfo(device, CL_DEVICE_PLATFORM, sizeof(platform), &platform, NULL);
  return getPlatformVersion(platform);
}

static cl_uint getContextPlatformVersion(cl_context context)
{
  // The platform cannot be queried directly, so we first have to grab a
  // device and obtain its context
  size_type size = 0;
  clGetContextInfo(context, CL_CONTEXT_DEVICES, 0, NULL, &size);
  if (size == 0)
    return 0;
  vector<cl_device_id> devices(size / sizeof(cl_device_id));
  clGetContextInfo(context, CL_CONTEXT_DEVICES, size, devices.data(), NULL);
  return getDevicePlatformVersion(devices[0]);
}
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120 && CL_HPP_MINIMUM_OPENCL_VERSION < 200

template <typename T>
class Wrapper
{
public:
  typedef T cl_type;

protected:
  cl_type object_;

public:
  Wrapper()
    : object_(NULL)
  {}

  Wrapper(const cl_type &obj, bool retainObject)
    : object_(obj)
  {
    if (retainObject)
    {
      detail::errHandler(retain(), __RETAIN_ERR);
    }
  }

  ~Wrapper()
  {
    if (object_ != NULL)
    {
      release();
    }
  }

  Wrapper(const Wrapper<cl_type> &rhs)
  {
    object_ = rhs.object_;
    detail::errHandler(retain(), __RETAIN_ERR);
  }

  Wrapper(Wrapper<cl_type> &&rhs) CL_HPP_NOEXCEPT_
  {
    object_ = rhs.object_;
    rhs.object_ = NULL;
  }

  Wrapper<cl_type> &operator=(const Wrapper<cl_type> &rhs)
  {
    if (this != &rhs)
    {
      detail::errHandler(release(), __RELEASE_ERR);
      object_ = rhs.object_;
      detail::errHandler(retain(), __RETAIN_ERR);
    }
    return *this;
  }

  Wrapper<cl_type> &operator=(Wrapper<cl_type> &&rhs)
  {
    if (this != &rhs)
    {
      detail::errHandler(release(), __RELEASE_ERR);
      object_ = rhs.object_;
      rhs.object_ = NULL;
    }
    return *this;
  }

  Wrapper<cl_type> &operator=(const cl_type &rhs)
  {
    detail::errHandler(release(), __RELEASE_ERR);
    object_ = rhs;
    return *this;
  }

  const cl_type &operator()() const { return object_; }

  cl_type &operator()() { return object_; }

  cl_type get() const { return object_; }

protected:
  template <typename Func, typename U>
  friend inline cl_int getInfoHelper(Func, cl_uint, U *, int, typename U::cl_type);

  cl_int retain() const
  {
    if (object_ != nullptr)
    {
      return ReferenceHandler<cl_type>::retain(object_);
    }
    else
    {
      return CL_SUCCESS;
    }
  }

  cl_int release() const
  {
    if (object_ != nullptr)
    {
      return ReferenceHandler<cl_type>::release(object_);
    }
    else
    {
      return CL_SUCCESS;
    }
  }
};

template <>
class Wrapper<cl_device_id>
{
public:
  typedef cl_device_id cl_type;

protected:
  cl_type object_;
  bool referenceCountable_;

  static bool isReferenceCountable(cl_device_id device)
  {
    bool retVal = false;
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
#if CL_HPP_MINIMUM_OPENCL_VERSION < 120
    if (device != NULL)
    {
      int version = getDevicePlatformVersion(device);
      if (version > ((1 << 16) + 1))
      {
        retVal = true;
      }
    }
#else   // CL_HPP_MINIMUM_OPENCL_VERSION < 120
    retVal = true;
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 120
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120
    return retVal;
  }

public:
  Wrapper()
    : object_(NULL)
    , referenceCountable_(false)
  {}

  Wrapper(const cl_type &obj, bool retainObject)
    : object_(obj)
    , referenceCountable_(false)
  {
    referenceCountable_ = isReferenceCountable(obj);

    if (retainObject)
    {
      detail::errHandler(retain(), __RETAIN_ERR);
    }
  }

  ~Wrapper() { release(); }

  Wrapper(const Wrapper<cl_type> &rhs)
  {
    object_ = rhs.object_;
    referenceCountable_ = isReferenceCountable(object_);
    detail::errHandler(retain(), __RETAIN_ERR);
  }

  Wrapper(Wrapper<cl_type> &&rhs) CL_HPP_NOEXCEPT_
  {
    object_ = rhs.object_;
    referenceCountable_ = rhs.referenceCountable_;
    rhs.object_ = NULL;
    rhs.referenceCountable_ = false;
  }

  Wrapper<cl_type> &operator=(const Wrapper<cl_type> &rhs)
  {
    if (this != &rhs)
    {
      detail::errHandler(release(), __RELEASE_ERR);
      object_ = rhs.object_;
      referenceCountable_ = rhs.referenceCountable_;
      detail::errHandler(retain(), __RETAIN_ERR);
    }
    return *this;
  }

  Wrapper<cl_type> &operator=(Wrapper<cl_type> &&rhs)
  {
    if (this != &rhs)
    {
      detail::errHandler(release(), __RELEASE_ERR);
      object_ = rhs.object_;
      referenceCountable_ = rhs.referenceCountable_;
      rhs.object_ = NULL;
      rhs.referenceCountable_ = false;
    }
    return *this;
  }

  Wrapper<cl_type> &operator=(const cl_type &rhs)
  {
    detail::errHandler(release(), __RELEASE_ERR);
    object_ = rhs;
    referenceCountable_ = isReferenceCountable(object_);
    return *this;
  }

  const cl_type &operator()() const { return object_; }

  cl_type &operator()() { return object_; }

  cl_type get() const { return object_; }

protected:
  template <typename Func, typename U>
  friend inline cl_int getInfoHelper(Func, cl_uint, U *, int, typename U::cl_type);

  template <typename Func, typename U>
  friend inline cl_int getInfoHelper(Func, cl_uint, vector<U> *, int, typename U::cl_type);

  cl_int retain() const
  {
    if (object_ != nullptr && referenceCountable_)
    {
      return ReferenceHandler<cl_type>::retain(object_);
    }
    else
    {
      return CL_SUCCESS;
    }
  }

  cl_int release() const
  {
    if (object_ != nullptr && referenceCountable_)
    {
      return ReferenceHandler<cl_type>::release(object_);
    }
    else
    {
      return CL_SUCCESS;
    }
  }
};

template <typename T>
inline bool operator==(const Wrapper<T> &lhs, const Wrapper<T> &rhs)
{
  return lhs() == rhs();
}

template <typename T>
inline bool operator!=(const Wrapper<T> &lhs, const Wrapper<T> &rhs)
{
  return !operator==(lhs, rhs);
}

}  // namespace detail
//! \endcond


using BuildLogType =
  vector<std::pair<cl::Device,
                   typename detail::param_traits<detail::cl_program_build_info, CL_PROGRAM_BUILD_LOG>::param_type>>;
#if defined(CL_HPP_ENABLE_EXCEPTIONS)
/**
 * Exception class for build errors to carry build info
 */
class BuildError : public Error
{
private:
  BuildLogType buildLogs;

public:
  BuildError(cl_int err, const char *errStr, const BuildLogType &vec)
    : Error(err, errStr)
    , buildLogs(vec)
  {}

  BuildLogType getBuildLog() const { return buildLogs; }
};
namespace detail
{
static inline cl_int buildErrHandler(cl_int err, const char *errStr, const BuildLogType &buildLogs)
{
  if (err != CL_SUCCESS)
  {
    throw BuildError(err, errStr, buildLogs);
  }
  return err;
}
}  // namespace detail

#else
namespace detail
{
static inline cl_int buildErrHandler(cl_int err, const char *errStr, const BuildLogType &buildLogs)
{
  (void)buildLogs;  // suppress unused variable warning
  (void)errStr;
  return err;
}
}  // namespace detail
#endif  // #if defined(CL_HPP_ENABLE_EXCEPTIONS)


/*! \stuct ImageFormat
 *  \brief Adds constructors and member functions for cl_image_format.
 *
 *  \see cl_image_format
 */
struct ImageFormat : public cl_image_format
{
  //! \brief Default constructor - performs no initialization.
  ImageFormat() {}

  //! \brief Initializing constructor.
  ImageFormat(cl_channel_order order, cl_channel_type type)
  {
    image_channel_order = order;
    image_channel_data_type = type;
  }

  //! \brief Copy constructor.
  ImageFormat(const ImageFormat &other) { *this = other; }

  //! \brief Assignment operator.
  ImageFormat &operator=(const ImageFormat &rhs)
  {
    if (this != &rhs)
    {
      this->image_channel_data_type = rhs.image_channel_data_type;
      this->image_channel_order = rhs.image_channel_order;
    }
    return *this;
  }
};

/*! \brief Class interface for cl_device_id.
 *
 *  \note Copies of these objects are inexpensive, since they don't 'own'
 *        any underlying resources or data structures.
 *
 *  \see cl_device_id
 */
class Device : public detail::Wrapper<cl_device_id>
{
private:
  static std::once_flag default_initialized_;
  static Device default_;
  static cl_int default_error_;

  /*! \brief Create the default context.
   *
   * This sets @c default_ and @c default_error_. It does not throw
   * @c cl::Error.
   */
  static void makeDefault();

  /*! \brief Create the default platform from a provided platform.
   *
   * This sets @c default_. It does not throw
   * @c cl::Error.
   */
  static void makeDefaultProvided(const Device &p) { default_ = p; }

public:
#ifdef CL_HPP_UNIT_TEST_ENABLE
  /*! \brief Reset the default.
   *
   * This sets @c default_ to an empty value to support cleanup in
   * the unit test framework.
   * This function is not thread safe.
   */
  static void unitTestClearDefault() { default_ = Device(); }
#endif  // #ifdef CL_HPP_UNIT_TEST_ENABLE

  //! \brief Default constructor - initializes to NULL.
  Device()
    : detail::Wrapper<cl_type>()
  {}

  /*! \brief Constructor from cl_device_id.
   *
   *  This simply copies the device ID value, which is an inexpensive operation.
   */
  explicit Device(const cl_device_id &device, bool retainObject = false)
    : detail::Wrapper<cl_type>(device, retainObject)
  {}

  /*! \brief Returns the first device on the default context.
   *
   *  \see Context::getDefault()
   */
  static Device getDefault(cl_int *errResult = NULL)
  {
    std::call_once(default_initialized_, makeDefault);
    detail::errHandler(default_error_);
    if (errResult != NULL)
    {
      *errResult = default_error_;
    }
    return default_;
  }

  /**
   * Modify the default device to be used by
   * subsequent operations.
   * Will only set the default if no default was previously created.
   * @return updated default device.
   *         Should be compared to the passed value to ensure that it was updated.
   */
  static Device setDefault(const Device &default_device)
  {
    std::call_once(default_initialized_, makeDefaultProvided, std::cref(default_device));
    detail::errHandler(default_error_);
    return default_;
  }

  /*! \brief Assignment operator from cl_device_id.
   *
   *  This simply copies the device ID value, which is an inexpensive operation.
   */
  Device &operator=(const cl_device_id &rhs)
  {
    detail::Wrapper<cl_type>::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Device(const Device &dev)
    : detail::Wrapper<cl_type>(dev)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Device &operator=(const Device &dev)
  {
    detail::Wrapper<cl_type>::operator=(dev);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Device(Device &&dev) CL_HPP_NOEXCEPT_ : detail::Wrapper<cl_type>(std::move(dev)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Device &operator=(Device &&dev)
  {
    detail::Wrapper<cl_type>::operator=(std::move(dev));
    return *this;
  }

  //! \brief Wrapper for clGetDeviceInfo().
  template <typename T>
  cl_int getInfo(cl_device_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetDeviceInfo, object_, name, param), __GET_DEVICE_INFO_ERR);
  }

  //! \brief Wrapper for clGetDeviceInfo() that returns by value.
  template <cl_device_info name>
  typename detail::param_traits<detail::cl_device_info, name>::param_type getInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_device_info, name>::param_type param;
    cl_int result = getInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }


#if CL_HPP_TARGET_OPENCL_VERSION >= 210
  /**
   * Return the current value of the host clock as seen by the device.
   * The resolution of the device timer may be queried with the
   * CL_DEVICE_PROFILING_TIMER_RESOLUTION query.
   * @return The host timer value.
   */
  cl_ulong getHostTimer(cl_int *error = nullptr)
  {
    cl_ulong retVal = 0;
    cl_int err = clGetHostTimer(this->get(), &retVal);
    detail::errHandler(err, __GET_HOST_TIMER_ERR);
    if (error)
    {
      *error = err;
    }
    return retVal;
  }

  /**
   * Return a synchronized pair of host and device timestamps as seen by device.
   * Use to correlate the clocks and get the host timer only using getHostTimer
   * as a lower cost mechanism in between calls.
   * The resolution of the host timer may be queried with the
   * CL_PLATFORM_HOST_TIMER_RESOLUTION query.
   * The resolution of the device timer may be queried with the
   * CL_DEVICE_PROFILING_TIMER_RESOLUTION query.
   * @return A pair of (device timer, host timer) timer values.
   */
  std::pair<cl_ulong, cl_ulong> getDeviceAndHostTimer(cl_int *error = nullptr)
  {
    std::pair<cl_ulong, cl_ulong> retVal;
    cl_int err = clGetDeviceAndHostTimer(this->get(), &(retVal.first), &(retVal.second));
    detail::errHandler(err, __GET_DEVICE_AND_HOST_TIMER_ERR);
    if (error)
    {
      *error = err;
    }
    return retVal;
  }
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 210

  /**
   * CL 1.2 version
   */
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
  //! \brief Wrapper for clCreateSubDevices().
  cl_int createSubDevices(const cl_device_partition_property *properties, vector<Device> *devices)
  {
    cl_uint n = 0;
    cl_int err = clCreateSubDevices(object_, properties, 0, NULL, &n);
    if (err != CL_SUCCESS)
    {
      return detail::errHandler(err, __CREATE_SUB_DEVICES_ERR);
    }

    vector<cl_device_id> ids(n);
    err = clCreateSubDevices(object_, properties, n, ids.data(), NULL);
    if (err != CL_SUCCESS)
    {
      return detail::errHandler(err, __CREATE_SUB_DEVICES_ERR);
    }

    // Cannot trivially assign because we need to capture intermediates
    // with safe construction
    if (devices)
    {
      devices->resize(ids.size());

      // Assign to param, constructing with retain behaviour
      // to correctly capture each underlying CL object
      for (size_type i = 0; i < ids.size(); i++)
      {
        // We do not need to retain because this device is being created
        // by the runtime
        (*devices)[i] = Device(ids[i], false);
      }
    }

    return CL_SUCCESS;
  }
#elif defined(CL_HPP_USE_CL_DEVICE_FISSION)

  /**
   * CL 1.1 version that uses device fission extension.
   */
  cl_int createSubDevices(const cl_device_partition_property_ext *properties, vector<Device> *devices)
  {
    typedef CL_API_ENTRY cl_int(CL_API_CALL * PFN_clCreateSubDevicesEXT)(
      cl_device_id /*in_device*/, const cl_device_partition_property_ext * /* properties */, cl_uint /*num_entries*/,
      cl_device_id * /*out_devices*/, cl_uint * /*num_devices*/) CL_API_SUFFIX__VERSION_1_1;

    static PFN_clCreateSubDevicesEXT pfn_clCreateSubDevicesEXT = NULL;
    CL_HPP_INIT_CL_EXT_FCN_PTR_(clCreateSubDevicesEXT);

    cl_uint n = 0;
    cl_int err = pfn_clCreateSubDevicesEXT(object_, properties, 0, NULL, &n);
    if (err != CL_SUCCESS)
    {
      return detail::errHandler(err, __CREATE_SUB_DEVICES_ERR);
    }

    vector<cl_device_id> ids(n);
    err = pfn_clCreateSubDevicesEXT(object_, properties, n, ids.data(), NULL);
    if (err != CL_SUCCESS)
    {
      return detail::errHandler(err, __CREATE_SUB_DEVICES_ERR);
    }
    // Cannot trivially assign because we need to capture intermediates
    // with safe construction
    if (devices)
    {
      devices->resize(ids.size());

      // Assign to param, constructing with retain behaviour
      // to correctly capture each underlying CL object
      for (size_type i = 0; i < ids.size(); i++)
      {
        // We do not need to retain because this device is being created
        // by the runtime
        (*devices)[i] = Device(ids[i], false);
      }
    }
    return CL_SUCCESS;
  }
#endif  // defined(CL_HPP_USE_CL_DEVICE_FISSION)
};

CL_HPP_DEFINE_STATIC_MEMBER_ std::once_flag Device::default_initialized_;
CL_HPP_DEFINE_STATIC_MEMBER_ Device Device::default_;
CL_HPP_DEFINE_STATIC_MEMBER_ cl_int Device::default_error_ = CL_SUCCESS;

/*! \brief Class interface for cl_platform_id.
 *
 *  \note Copies of these objects are inexpensive, since they don't 'own'
 *        any underlying resources or data structures.
 *
 *  \see cl_platform_id
 */
class Platform : public detail::Wrapper<cl_platform_id>
{
private:
  static std::once_flag default_initialized_;
  static Platform default_;
  static cl_int default_error_;

  /*! \brief Create the default context.
   *
   * This sets @c default_ and @c default_error_. It does not throw
   * @c cl::Error.
   */
  static void makeDefault()
  {
    /* Throwing an exception from a call_once invocation does not do
     * what we wish, so we catch it and save the error.
     */
#if defined(CL_HPP_ENABLE_EXCEPTIONS)
    try
#endif
    {
      // If default wasn't passed ,generate one
      // Otherwise set it
      cl_uint n = 0;

      cl_int err = ::clGetPlatformIDs(0, NULL, &n);
      if (err != CL_SUCCESS)
      {
        default_error_ = err;
        return;
      }
      if (n == 0)
      {
        default_error_ = CL_INVALID_PLATFORM;
        return;
      }

      vector<cl_platform_id> ids(n);
      err = ::clGetPlatformIDs(n, ids.data(), NULL);
      if (err != CL_SUCCESS)
      {
        default_error_ = err;
        return;
      }

      default_ = Platform(ids[0]);
    }
#if defined(CL_HPP_ENABLE_EXCEPTIONS)
    catch (cl::Error &e)
    {
      default_error_ = e.err();
    }
#endif
  }

  /*! \brief Create the default platform from a provided platform.
   *
   * This sets @c default_. It does not throw
   * @c cl::Error.
   */
  static void makeDefaultProvided(const Platform &p) { default_ = p; }

public:
#ifdef CL_HPP_UNIT_TEST_ENABLE
  /*! \brief Reset the default.
   *
   * This sets @c default_ to an empty value to support cleanup in
   * the unit test framework.
   * This function is not thread safe.
   */
  static void unitTestClearDefault() { default_ = Platform(); }
#endif  // #ifdef CL_HPP_UNIT_TEST_ENABLE

  //! \brief Default constructor - initializes to NULL.
  Platform()
    : detail::Wrapper<cl_type>()
  {}

  /*! \brief Constructor from cl_platform_id.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  This simply copies the platform ID value, which is an inexpensive operation.
   */
  explicit Platform(const cl_platform_id &platform, bool retainObject = false)
    : detail::Wrapper<cl_type>(platform, retainObject)
  {}

  /*! \brief Assignment operator from cl_platform_id.
   *
   *  This simply copies the platform ID value, which is an inexpensive operation.
   */
  Platform &operator=(const cl_platform_id &rhs)
  {
    detail::Wrapper<cl_type>::operator=(rhs);
    return *this;
  }

  static Platform getDefault(cl_int *errResult = NULL)
  {
    std::call_once(default_initialized_, makeDefault);
    detail::errHandler(default_error_);
    if (errResult != NULL)
    {
      *errResult = default_error_;
    }
    return default_;
  }

  /**
   * Modify the default platform to be used by
   * subsequent operations.
   * Will only set the default if no default was previously created.
   * @return updated default platform.
   *         Should be compared to the passed value to ensure that it was updated.
   */
  static Platform setDefault(const Platform &default_platform)
  {
    std::call_once(default_initialized_, makeDefaultProvided, std::cref(default_platform));
    detail::errHandler(default_error_);
    return default_;
  }

  //! \brief Wrapper for clGetPlatformInfo().
  template <typename T>
  cl_int getInfo(cl_platform_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetPlatformInfo, object_, name, param), __GET_PLATFORM_INFO_ERR);
  }

  //! \brief Wrapper for clGetPlatformInfo() that returns by value.
  template <cl_platform_info name>
  typename detail::param_traits<detail::cl_platform_info, name>::param_type getInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_platform_info, name>::param_type param;
    cl_int result = getInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }

  /*! \brief Gets a list of devices for this platform.
   *
   *  Wraps clGetDeviceIDs().
   */
  cl_int getDevices(cl_device_type type, vector<Device> *devices) const
  {
    cl_uint n = 0;
    if (devices == NULL)
    {
      return detail::errHandler(CL_INVALID_ARG_VALUE, __GET_DEVICE_IDS_ERR);
    }
    cl_int err = ::clGetDeviceIDs(object_, type, 0, NULL, &n);
    if (err != CL_SUCCESS && err != CL_DEVICE_NOT_FOUND)
    {
      return detail::errHandler(err, __GET_DEVICE_IDS_ERR);
    }

    vector<cl_device_id> ids(n);
    if (n > 0)
    {
      err = ::clGetDeviceIDs(object_, type, n, ids.data(), NULL);
      if (err != CL_SUCCESS)
      {
        return detail::errHandler(err, __GET_DEVICE_IDS_ERR);
      }
    }

    // Cannot trivially assign because we need to capture intermediates
    // with safe construction
    // We must retain things we obtain from the API to avoid releasing
    // API-owned objects.
    if (devices)
    {
      devices->resize(ids.size());

      // Assign to param, constructing with retain behaviour
      // to correctly capture each underlying CL object
      for (size_type i = 0; i < ids.size(); i++)
      {
        (*devices)[i] = Device(ids[i], true);
      }
    }
    return CL_SUCCESS;
  }

#if defined(CL_HPP_USE_DX_INTEROP)
  /*! \brief Get the list of available D3D10 devices.
   *
   *  \param d3d_device_source.
   *
   *  \param d3d_object.
   *
   *  \param d3d_device_set.
   *
   *  \param devices returns a vector of OpenCL D3D10 devices found. The cl::Device
   *  values returned in devices can be used to identify a specific OpenCL
   *  device. If \a devices argument is NULL, this argument is ignored.
   *
   *  \return One of the following values:
   *    - CL_SUCCESS if the function is executed successfully.
   *
   *  The application can query specific capabilities of the OpenCL device(s)
   *  returned by cl::getDevices. This can be used by the application to
   *  determine which device(s) to use.
   *
   * \note In the case that exceptions are enabled and a return value
   * other than CL_SUCCESS is generated, then cl::Error exception is
   * generated.
   */
  cl_int getDevices(cl_d3d10_device_source_khr d3d_device_source, void *d3d_object,
                    cl_d3d10_device_set_khr d3d_device_set, vector<Device> *devices) const
  {
    typedef CL_API_ENTRY cl_int(CL_API_CALL * PFN_clGetDeviceIDsFromD3D10KHR)(
      cl_platform_id platform, cl_d3d10_device_source_khr d3d_device_source, void *d3d_object,
      cl_d3d10_device_set_khr d3d_device_set, cl_uint num_entries, cl_device_id *devices, cl_uint *num_devices);

    if (devices == NULL)
    {
      return detail::errHandler(CL_INVALID_ARG_VALUE, __GET_DEVICE_IDS_ERR);
    }

    static PFN_clGetDeviceIDsFromD3D10KHR pfn_clGetDeviceIDsFromD3D10KHR = NULL;
    CL_HPP_INIT_CL_EXT_FCN_PTR_PLATFORM_(object_, clGetDeviceIDsFromD3D10KHR);

    cl_uint n = 0;
    cl_int err = pfn_clGetDeviceIDsFromD3D10KHR(object_, d3d_device_source, d3d_object, d3d_device_set, 0, NULL, &n);
    if (err != CL_SUCCESS)
    {
      return detail::errHandler(err, __GET_DEVICE_IDS_ERR);
    }

    vector<cl_device_id> ids(n);
    err = pfn_clGetDeviceIDsFromD3D10KHR(object_, d3d_device_source, d3d_object, d3d_device_set, n, ids.data(), NULL);
    if (err != CL_SUCCESS)
    {
      return detail::errHandler(err, __GET_DEVICE_IDS_ERR);
    }

    // Cannot trivially assign because we need to capture intermediates
    // with safe construction
    // We must retain things we obtain from the API to avoid releasing
    // API-owned objects.
    if (devices)
    {
      devices->resize(ids.size());

      // Assign to param, constructing with retain behaviour
      // to correctly capture each underlying CL object
      for (size_type i = 0; i < ids.size(); i++)
      {
        (*devices)[i] = Device(ids[i], true);
      }
    }
    return CL_SUCCESS;
  }
#endif

  /*! \brief Gets a list of available platforms.
   *
   *  Wraps clGetPlatformIDs().
   */
  static cl_int get(vector<Platform> *platforms)
  {
    cl_uint n = 0;

    if (platforms == NULL)
    {
      return detail::errHandler(CL_INVALID_ARG_VALUE, __GET_PLATFORM_IDS_ERR);
    }

    cl_int err = ::clGetPlatformIDs(0, NULL, &n);
    if (err != CL_SUCCESS)
    {
      return detail::errHandler(err, __GET_PLATFORM_IDS_ERR);
    }

    vector<cl_platform_id> ids(n);
    err = ::clGetPlatformIDs(n, ids.data(), NULL);
    if (err != CL_SUCCESS)
    {
      return detail::errHandler(err, __GET_PLATFORM_IDS_ERR);
    }

    if (platforms)
    {
      platforms->resize(ids.size());

      // Platforms don't reference count
      for (size_type i = 0; i < ids.size(); i++)
      {
        (*platforms)[i] = Platform(ids[i]);
      }
    }
    return CL_SUCCESS;
  }

  /*! \brief Gets the first available platform.
   *
   *  Wraps clGetPlatformIDs(), returning the first result.
   */
  static cl_int get(Platform *platform)
  {
    cl_int err;
    Platform default_platform = Platform::getDefault(&err);
    if (platform)
    {
      *platform = default_platform;
    }
    return err;
  }

  /*! \brief Gets the first available platform, returning it by value.
   *
   * \return Returns a valid platform if one is available.
   *         If no platform is available will return a null platform.
   * Throws an exception if no platforms are available
   * or an error condition occurs.
   * Wraps clGetPlatformIDs(), returning the first result.
   */
  static Platform get(cl_int *errResult = NULL)
  {
    cl_int err;
    Platform default_platform = Platform::getDefault(&err);
    if (errResult)
    {
      *errResult = err;
    }
    return default_platform;
  }

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
  //! \brief Wrapper for clUnloadCompiler().
  cl_int unloadCompiler() { return ::clUnloadPlatformCompiler(object_); }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120
};      // class Platform

CL_HPP_DEFINE_STATIC_MEMBER_ std::once_flag Platform::default_initialized_;
CL_HPP_DEFINE_STATIC_MEMBER_ Platform Platform::default_;
CL_HPP_DEFINE_STATIC_MEMBER_ cl_int Platform::default_error_ = CL_SUCCESS;


/**
 * Deprecated APIs for 1.2
 */
#if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS)
/**
 * Unload the OpenCL compiler.
 * \note Deprecated for OpenCL 1.2. Use Platform::unloadCompiler instead.
 */
inline CL_API_PREFIX__VERSION_1_1_DEPRECATED cl_int UnloadCompiler() CL_API_SUFFIX__VERSION_1_1_DEPRECATED;
inline cl_int UnloadCompiler()
{
  return ::clUnloadCompiler();
}
#endif  // #if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS)

/*! \brief Class interface for cl_context.
 *
 *  \note Copies of these objects are shallow, meaning that the copy will refer
 *        to the same underlying cl_context as the original.  For details, see
 *        clRetainContext() and clReleaseContext().
 *
 *  \see cl_context
 */
class Context : public detail::Wrapper<cl_context>
{
private:
  static std::once_flag default_initialized_;
  static Context default_;
  static cl_int default_error_;

  /*! \brief Create the default context from the default device type in the default platform.
   *
   * This sets @c default_ and @c default_error_. It does not throw
   * @c cl::Error.
   */
  static void makeDefault()
  {
    /* Throwing an exception from a call_once invocation does not do
     * what we wish, so we catch it and save the error.
     */
#if defined(CL_HPP_ENABLE_EXCEPTIONS)
    try
#endif
    {
#if !defined(__APPLE__) && !defined(__MACOS)
      const Platform &p = Platform::getDefault();
      cl_platform_id defaultPlatform = p();
      cl_context_properties properties[3] = { CL_CONTEXT_PLATFORM, (cl_context_properties)defaultPlatform, 0 };
#else   // #if !defined(__APPLE__) && !defined(__MACOS)
      cl_context_properties *properties = nullptr;
#endif  // #if !defined(__APPLE__) && !defined(__MACOS)

      default_ = Context(CL_DEVICE_TYPE_DEFAULT, properties, NULL, NULL, &default_error_);
    }
#if defined(CL_HPP_ENABLE_EXCEPTIONS)
    catch (cl::Error &e)
    {
      default_error_ = e.err();
    }
#endif
  }


  /*! \brief Create the default context from a provided Context.
   *
   * This sets @c default_. It does not throw
   * @c cl::Error.
   */
  static void makeDefaultProvided(const Context &c) { default_ = c; }

public:
#ifdef CL_HPP_UNIT_TEST_ENABLE
  /*! \brief Reset the default.
   *
   * This sets @c default_ to an empty value to support cleanup in
   * the unit test framework.
   * This function is not thread safe.
   */
  static void unitTestClearDefault() { default_ = Context(); }
#endif  // #ifdef CL_HPP_UNIT_TEST_ENABLE

  /*! \brief Constructs a context including a list of specified devices.
   *
   *  Wraps clCreateContext().
   */
  Context(const vector<Device> &devices, const cl_context_properties *properties = NULL,
          void(CL_CALLBACK *notifyFptr)(const char *, const void *, size_type, void *) = NULL, void *data = NULL,
          cl_int *err = NULL)
  {
    cl_int error;

    size_type numDevices = devices.size();
    vector<cl_device_id> deviceIDs(numDevices);

    for (size_type deviceIndex = 0; deviceIndex < numDevices; ++deviceIndex)
    {
      deviceIDs[deviceIndex] = (devices[deviceIndex])();
    }

    object_ = ::clCreateContext(properties, (cl_uint)numDevices, deviceIDs.data(), notifyFptr, data, &error);

    detail::errHandler(error, __CREATE_CONTEXT_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  /*! \brief Constructs a context including a specific device.
   *
   *  Wraps clCreateContext().
   */
  Context(const Device &device, const cl_context_properties *properties = NULL,
          void(CL_CALLBACK *notifyFptr)(const char *, const void *, size_type, void *) = NULL, void *data = NULL,
          cl_int *err = NULL)
  {
    cl_int error;

    cl_device_id deviceID = device();

    object_ = ::clCreateContext(properties, 1, &deviceID, notifyFptr, data, &error);

    detail::errHandler(error, __CREATE_CONTEXT_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  /*! \brief Constructs a context including all or a subset of devices of a specified type.
   *
   *  Wraps clCreateContextFromType().
   */
  Context(cl_device_type type, const cl_context_properties *properties = NULL,
          void(CL_CALLBACK *notifyFptr)(const char *, const void *, size_type, void *) = NULL, void *data = NULL,
          cl_int *err = NULL)
  {
    cl_int error;

#if !defined(__APPLE__) && !defined(__MACOS)
    cl_context_properties prop[4] = { CL_CONTEXT_PLATFORM, 0, 0, 0 };

    if (properties == NULL)
    {
      // Get a valid platform ID as we cannot send in a blank one
      vector<Platform> platforms;
      error = Platform::get(&platforms);
      if (error != CL_SUCCESS)
      {
        detail::errHandler(error, __CREATE_CONTEXT_FROM_TYPE_ERR);
        if (err != NULL)
        {
          *err = error;
        }
        return;
      }

      // Check the platforms we found for a device of our specified type
      cl_context_properties platform_id = 0;
      for (unsigned int i = 0; i < platforms.size(); i++)
      {
        vector<Device> devices;

#if defined(CL_HPP_ENABLE_EXCEPTIONS)
        try
        {
#endif

          error = platforms[i].getDevices(type, &devices);

#if defined(CL_HPP_ENABLE_EXCEPTIONS)
        }
        catch (cl::Error &e)
        {
          error = e.err();
        }
        // Catch if exceptions are enabled as we don't want to exit if first platform has no devices of type
        // We do error checking next anyway, and can throw there if needed
#endif

        // Only squash CL_SUCCESS and CL_DEVICE_NOT_FOUND
        if (error != CL_SUCCESS && error != CL_DEVICE_NOT_FOUND)
        {
          detail::errHandler(error, __CREATE_CONTEXT_FROM_TYPE_ERR);
          if (err != NULL)
          {
            *err = error;
          }
        }

        if (devices.size() > 0)
        {
          platform_id = (cl_context_properties)platforms[i]();
          break;
        }
      }

      if (platform_id == 0)
      {
        detail::errHandler(CL_DEVICE_NOT_FOUND, __CREATE_CONTEXT_FROM_TYPE_ERR);
        if (err != NULL)
        {
          *err = CL_DEVICE_NOT_FOUND;
        }
        return;
      }

      prop[1] = platform_id;
      properties = &prop[0];
    }
#endif
    object_ = ::clCreateContextFromType(properties, type, notifyFptr, data, &error);

    detail::errHandler(error, __CREATE_CONTEXT_FROM_TYPE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Context(const Context &ctx)
    : detail::Wrapper<cl_type>(ctx)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Context &operator=(const Context &ctx)
  {
    detail::Wrapper<cl_type>::operator=(ctx);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Context(Context &&ctx) CL_HPP_NOEXCEPT_ : detail::Wrapper<cl_type>(std::move(ctx)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Context &operator=(Context &&ctx)
  {
    detail::Wrapper<cl_type>::operator=(std::move(ctx));
    return *this;
  }


  /*! \brief Returns a singleton context including all devices of CL_DEVICE_TYPE_DEFAULT.
   *
   *  \note All calls to this function return the same cl_context as the first.
   */
  static Context getDefault(cl_int *err = NULL)
  {
    std::call_once(default_initialized_, makeDefault);
    detail::errHandler(default_error_);
    if (err != NULL)
    {
      *err = default_error_;
    }
    return default_;
  }

  /**
   * Modify the default context to be used by
   * subsequent operations.
   * Will only set the default if no default was previously created.
   * @return updated default context.
   *         Should be compared to the passed value to ensure that it was updated.
   */
  static Context setDefault(const Context &default_context)
  {
    std::call_once(default_initialized_, makeDefaultProvided, std::cref(default_context));
    detail::errHandler(default_error_);
    return default_;
  }

  //! \brief Default constructor - initializes to NULL.
  Context()
    : detail::Wrapper<cl_type>()
  {}

  /*! \brief Constructor from cl_context - takes ownership.
   *
   *  This effectively transfers ownership of a refcount on the cl_context
   *  into the new Context object.
   */
  explicit Context(const cl_context &context, bool retainObject = false)
    : detail::Wrapper<cl_type>(context, retainObject)
  {}

  /*! \brief Assignment operator from cl_context - takes ownership.
   *
   *  This effectively transfers ownership of a refcount on the rhs and calls
   *  clReleaseContext() on the value previously held by this instance.
   */
  Context &operator=(const cl_context &rhs)
  {
    detail::Wrapper<cl_type>::operator=(rhs);
    return *this;
  }

  //! \brief Wrapper for clGetContextInfo().
  template <typename T>
  cl_int getInfo(cl_context_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetContextInfo, object_, name, param), __GET_CONTEXT_INFO_ERR);
  }

  //! \brief Wrapper for clGetContextInfo() that returns by value.
  template <cl_context_info name>
  typename detail::param_traits<detail::cl_context_info, name>::param_type getInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_context_info, name>::param_type param;
    cl_int result = getInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }

  /*! \brief Gets a list of supported image formats.
   *
   *  Wraps clGetSupportedImageFormats().
   */
  cl_int getSupportedImageFormats(cl_mem_flags flags, cl_mem_object_type type, vector<ImageFormat> *formats) const
  {
    cl_uint numEntries;

    if (!formats)
    {
      return CL_SUCCESS;
    }

    cl_int err = ::clGetSupportedImageFormats(object_, flags, type, 0, NULL, &numEntries);
    if (err != CL_SUCCESS)
    {
      return detail::errHandler(err, __GET_SUPPORTED_IMAGE_FORMATS_ERR);
    }

    if (numEntries > 0)
    {
      vector<ImageFormat> value(numEntries);
      err = ::clGetSupportedImageFormats(object_, flags, type, numEntries, (cl_image_format *)value.data(), NULL);
      if (err != CL_SUCCESS)
      {
        return detail::errHandler(err, __GET_SUPPORTED_IMAGE_FORMATS_ERR);
      }

      formats->assign(begin(value), end(value));
    }
    else
    {
      // If no values are being returned, ensure an empty vector comes back
      formats->clear();
    }

    return CL_SUCCESS;
  }
};

inline void Device::makeDefault()
{
  /* Throwing an exception from a call_once invocation does not do
   * what we wish, so we catch it and save the error.
   */
#if defined(CL_HPP_ENABLE_EXCEPTIONS)
  try
#endif
  {
    cl_int error = 0;

    Context context = Context::getDefault(&error);
    detail::errHandler(error, __CREATE_CONTEXT_ERR);

    if (error != CL_SUCCESS)
    {
      default_error_ = error;
    }
    else
    {
      default_ = context.getInfo<CL_CONTEXT_DEVICES>()[0];
      default_error_ = CL_SUCCESS;
    }
  }
#if defined(CL_HPP_ENABLE_EXCEPTIONS)
  catch (cl::Error &e)
  {
    default_error_ = e.err();
  }
#endif
}

CL_HPP_DEFINE_STATIC_MEMBER_ std::once_flag Context::default_initialized_;
CL_HPP_DEFINE_STATIC_MEMBER_ Context Context::default_;
CL_HPP_DEFINE_STATIC_MEMBER_ cl_int Context::default_error_ = CL_SUCCESS;

/*! \brief Class interface for cl_event.
 *
 *  \note Copies of these objects are shallow, meaning that the copy will refer
 *        to the same underlying cl_event as the original.  For details, see
 *        clRetainEvent() and clReleaseEvent().
 *
 *  \see cl_event
 */
class Event : public detail::Wrapper<cl_event>
{
public:
  //! \brief Default constructor - initializes to NULL.
  Event()
    : detail::Wrapper<cl_type>()
  {}

  /*! \brief Constructor from cl_event - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  This effectively transfers ownership of a refcount on the cl_event
   *  into the new Event object.
   */
  explicit Event(const cl_event &event, bool retainObject = false)
    : detail::Wrapper<cl_type>(event, retainObject)
  {}

  /*! \brief Assignment operator from cl_event - takes ownership.
   *
   *  This effectively transfers ownership of a refcount on the rhs and calls
   *  clReleaseEvent() on the value previously held by this instance.
   */
  Event &operator=(const cl_event &rhs)
  {
    detail::Wrapper<cl_type>::operator=(rhs);
    return *this;
  }

  //! \brief Wrapper for clGetEventInfo().
  template <typename T>
  cl_int getInfo(cl_event_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetEventInfo, object_, name, param), __GET_EVENT_INFO_ERR);
  }

  //! \brief Wrapper for clGetEventInfo() that returns by value.
  template <cl_event_info name>
  typename detail::param_traits<detail::cl_event_info, name>::param_type getInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_event_info, name>::param_type param;
    cl_int result = getInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }

  //! \brief Wrapper for clGetEventProfilingInfo().
  template <typename T>
  cl_int getProfilingInfo(cl_profiling_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetEventProfilingInfo, object_, name, param),
                              __GET_EVENT_PROFILE_INFO_ERR);
  }

  //! \brief Wrapper for clGetEventProfilingInfo() that returns by value.
  template <cl_profiling_info name>
  typename detail::param_traits<detail::cl_profiling_info, name>::param_type getProfilingInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_profiling_info, name>::param_type param;
    cl_int result = getProfilingInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }

  /*! \brief Blocks the calling thread until this event completes.
   *
   *  Wraps clWaitForEvents().
   */
  cl_int wait() const { return detail::errHandler(::clWaitForEvents(1, &object_), __WAIT_FOR_EVENTS_ERR); }

#if CL_HPP_TARGET_OPENCL_VERSION >= 110
  /*! \brief Registers a user callback function for a specific command execution status.
   *
   *  Wraps clSetEventCallback().
   */
  cl_int setCallback(cl_int type, void(CL_CALLBACK *pfn_notify)(cl_event, cl_int, void *), void *user_data = NULL)
  {
    return detail::errHandler(::clSetEventCallback(object_, type, pfn_notify, user_data), __SET_EVENT_CALLBACK_ERR);
  }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 110

  /*! \brief Blocks the calling thread until every event specified is complete.
   *
   *  Wraps clWaitForEvents().
   */
  static cl_int waitForEvents(const vector<Event> &events)
  {
    return detail::errHandler(
      ::clWaitForEvents((cl_uint)events.size(), (events.size() > 0) ? (cl_event *)&events.front() : NULL),
      __WAIT_FOR_EVENTS_ERR);
  }
};

#if CL_HPP_TARGET_OPENCL_VERSION >= 110
/*! \brief Class interface for user events (a subset of cl_event's).
 *
 *  See Event for details about copy semantics, etc.
 */
class UserEvent : public Event
{
public:
  /*! \brief Constructs a user event on a given context.
   *
   *  Wraps clCreateUserEvent().
   */
  UserEvent(const Context &context, cl_int *err = NULL)
  {
    cl_int error;
    object_ = ::clCreateUserEvent(context(), &error);

    detail::errHandler(error, __CREATE_USER_EVENT_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  //! \brief Default constructor - initializes to NULL.
  UserEvent()
    : Event()
  {}

  /*! \brief Sets the execution status of a user event object.
   *
   *  Wraps clSetUserEventStatus().
   */
  cl_int setStatus(cl_int status)
  {
    return detail::errHandler(::clSetUserEventStatus(object_, status), __SET_USER_EVENT_STATUS_ERR);
  }
};
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 110

/*! \brief Blocks the calling thread until every event specified is complete.
 *
 *  Wraps clWaitForEvents().
 */
inline static cl_int WaitForEvents(const vector<Event> &events)
{
  return detail::errHandler(
    ::clWaitForEvents((cl_uint)events.size(), (events.size() > 0) ? (cl_event *)&events.front() : NULL),
    __WAIT_FOR_EVENTS_ERR);
}

/*! \brief Class interface for cl_mem.
 *
 *  \note Copies of these objects are shallow, meaning that the copy will refer
 *        to the same underlying cl_mem as the original.  For details, see
 *        clRetainMemObject() and clReleaseMemObject().
 *
 *  \see cl_mem
 */
class Memory : public detail::Wrapper<cl_mem>
{
public:
  //! \brief Default constructor - initializes to NULL.
  Memory()
    : detail::Wrapper<cl_type>()
  {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   *  Optionally transfer ownership of a refcount on the cl_mem
   *  into the new Memory object.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *
   *  See Memory for further details.
   */
  explicit Memory(const cl_mem &memory, bool retainObject)
    : detail::Wrapper<cl_type>(memory, retainObject)
  {}

  /*! \brief Assignment operator from cl_mem - takes ownership.
   *
   *  This effectively transfers ownership of a refcount on the rhs and calls
   *  clReleaseMemObject() on the value previously held by this instance.
   */
  Memory &operator=(const cl_mem &rhs)
  {
    detail::Wrapper<cl_type>::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Memory(const Memory &mem)
    : detail::Wrapper<cl_type>(mem)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Memory &operator=(const Memory &mem)
  {
    detail::Wrapper<cl_type>::operator=(mem);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Memory(Memory &&mem) CL_HPP_NOEXCEPT_ : detail::Wrapper<cl_type>(std::move(mem)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Memory &operator=(Memory &&mem)
  {
    detail::Wrapper<cl_type>::operator=(std::move(mem));
    return *this;
  }


  //! \brief Wrapper for clGetMemObjectInfo().
  template <typename T>
  cl_int getInfo(cl_mem_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetMemObjectInfo, object_, name, param), __GET_MEM_OBJECT_INFO_ERR);
  }

  //! \brief Wrapper for clGetMemObjectInfo() that returns by value.
  template <cl_mem_info name>
  typename detail::param_traits<detail::cl_mem_info, name>::param_type getInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_mem_info, name>::param_type param;
    cl_int result = getInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }

#if CL_HPP_TARGET_OPENCL_VERSION >= 110
  /*! \brief Registers a callback function to be called when the memory object
   *         is no longer needed.
   *
   *  Wraps clSetMemObjectDestructorCallback().
   *
   *  Repeated calls to this function, for a given cl_mem value, will append
   *  to the list of functions called (in reverse order) when memory object's
   *  resources are freed and the memory object is deleted.
   *
   *  \note
   *  The registered callbacks are associated with the underlying cl_mem
   *  value - not the Memory class instance.
   */
  cl_int setDestructorCallback(void(CL_CALLBACK *pfn_notify)(cl_mem, void *), void *user_data = NULL)
  {
    return detail::errHandler(::clSetMemObjectDestructorCallback(object_, pfn_notify, user_data),
                              __SET_MEM_OBJECT_DESTRUCTOR_CALLBACK_ERR);
  }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 110
};

// Pre-declare copy functions
class Buffer;
template <typename IteratorType>
cl_int copy(IteratorType startIterator, IteratorType endIterator, cl::Buffer &buffer);
template <typename IteratorType>
cl_int copy(const cl::Buffer &buffer, IteratorType startIterator, IteratorType endIterator);
template <typename IteratorType>
cl_int copy(const CommandQueue &queue, IteratorType startIterator, IteratorType endIterator, cl::Buffer &buffer);
template <typename IteratorType>
cl_int copy(const CommandQueue &queue, const cl::Buffer &buffer, IteratorType startIterator, IteratorType endIterator);


#if CL_HPP_TARGET_OPENCL_VERSION >= 200
namespace detail
{
class SVMTraitNull
{
public:
  static cl_svm_mem_flags getSVMMemFlags() { return 0; }
};
}  // namespace detail

template <class Trait = detail::SVMTraitNull>
class SVMTraitReadWrite
{
public:
  static cl_svm_mem_flags getSVMMemFlags() { return CL_MEM_READ_WRITE | Trait::getSVMMemFlags(); }
};

template <class Trait = detail::SVMTraitNull>
class SVMTraitReadOnly
{
public:
  static cl_svm_mem_flags getSVMMemFlags() { return CL_MEM_READ_ONLY | Trait::getSVMMemFlags(); }
};

template <class Trait = detail::SVMTraitNull>
class SVMTraitWriteOnly
{
public:
  static cl_svm_mem_flags getSVMMemFlags() { return CL_MEM_WRITE_ONLY | Trait::getSVMMemFlags(); }
};

template <class Trait = SVMTraitReadWrite<>>
class SVMTraitCoarse
{
public:
  static cl_svm_mem_flags getSVMMemFlags() { return Trait::getSVMMemFlags(); }
};

template <class Trait = SVMTraitReadWrite<>>
class SVMTraitFine
{
public:
  static cl_svm_mem_flags getSVMMemFlags() { return CL_MEM_SVM_FINE_GRAIN_BUFFER | Trait::getSVMMemFlags(); }
};

template <class Trait = SVMTraitReadWrite<>>
class SVMTraitAtomic
{
public:
  static cl_svm_mem_flags getSVMMemFlags()
  {
    return CL_MEM_SVM_FINE_GRAIN_BUFFER | CL_MEM_SVM_ATOMICS | Trait::getSVMMemFlags();
  }
};

// Pre-declare SVM map function
template <typename T>
inline cl_int enqueueMapSVM(T *ptr, cl_bool blocking, cl_map_flags flags, size_type size,
                            const vector<Event> *events = NULL, Event *event = NULL);

/**
 * STL-like allocator class for managing SVM objects provided for convenience.
 *
 * Note that while this behaves like an allocator for the purposes of constructing vectors and similar objects,
 * care must be taken when using with smart pointers.
 * The allocator should not be used to construct a unique_ptr if we are using coarse-grained SVM mode because
 * the coarse-grained management behaviour would behave incorrectly with respect to reference counting.
 *
 * Instead the allocator embeds a Deleter which may be used with unique_ptr and is used
 * with the allocate_shared and allocate_ptr supplied operations.
 */
template <typename T, class SVMTrait>
class SVMAllocator
{
private:
  Context context_;

public:
  typedef T value_type;
  typedef value_type *pointer;
  typedef const value_type *const_pointer;
  typedef value_type &reference;
  typedef const value_type &const_reference;
  typedef std::size_t size_type;
  typedef std::ptrdiff_t difference_type;

  template <typename U>
  struct rebind
  {
    typedef SVMAllocator<U, SVMTrait> other;
  };

  template <typename U, typename V>
  friend class SVMAllocator;

  SVMAllocator()
    : context_(Context::getDefault())
  {}

  explicit SVMAllocator(cl::Context context)
    : context_(context)
  {}


  SVMAllocator(const SVMAllocator &other)
    : context_(other.context_)
  {}

  template <typename U>
  SVMAllocator(const SVMAllocator<U, SVMTrait> &other)
    : context_(other.context_)
  {}

  ~SVMAllocator() {}

  pointer address(reference r) CL_HPP_NOEXCEPT_ { return std::addressof(r); }

  const_pointer address(const_reference r) CL_HPP_NOEXCEPT_ { return std::addressof(r); }

  /**
   * Allocate an SVM pointer.
   *
   * If the allocator is coarse-grained, this will take ownership to allow
   * containers to correctly construct data in place.
   */
  pointer allocate(size_type size, typename cl::SVMAllocator<void, SVMTrait>::const_pointer = 0)
  {
    // Allocate memory with default alignment matching the size of the type
    void *voidPointer = clSVMAlloc(context_(), SVMTrait::getSVMMemFlags(), size * sizeof(T), 0);
    pointer retValue = reinterpret_cast<pointer>(voidPointer);
#if defined(CL_HPP_ENABLE_EXCEPTIONS)
    if (!retValue)
    {
      std::bad_alloc excep;
      throw excep;
    }
#endif  // #if defined(CL_HPP_ENABLE_EXCEPTIONS)

    // If allocation was coarse-grained then map it
    if (!(SVMTrait::getSVMMemFlags() & CL_MEM_SVM_FINE_GRAIN_BUFFER))
    {
      cl_int err = enqueueMapSVM(retValue, CL_TRUE, CL_MAP_READ | CL_MAP_WRITE, size * sizeof(T));
      if (err != CL_SUCCESS)
      {
        std::bad_alloc excep;
        throw excep;
      }
    }

    // If exceptions disabled, return null pointer from allocator
    return retValue;
  }

  void deallocate(pointer p, size_type) { clSVMFree(context_(), p); }

  /**
   * Return the maximum possible allocation size.
   * This is the minimum of the maximum sizes of all devices in the context.
   */
  size_type max_size() const CL_HPP_NOEXCEPT_
  {
    size_type maxSize = std::numeric_limits<size_type>::max() / sizeof(T);

    for (const Device &d : context_.getInfo<CL_CONTEXT_DEVICES>())
    {
      maxSize = std::min(maxSize, static_cast<size_type>(d.getInfo<CL_DEVICE_MAX_MEM_ALLOC_SIZE>()));
    }

    return maxSize;
  }

  template <class U, class... Args>
  void construct(U *p, Args &&...args)
  {
    new (p) T(args...);
  }

  template <class U>
  void destroy(U *p)
  {
    p->~U();
  }

  /**
   * Returns true if the contexts match.
   */
  inline bool operator==(SVMAllocator const &rhs) { return (context_ == rhs.context_); }

  inline bool operator!=(SVMAllocator const &a) { return !operator==(a); }
};  // class SVMAllocator        return cl::pointer<T>(tmp, detail::Deleter<T, Alloc>{alloc, copies});


template <class SVMTrait>
class SVMAllocator<void, SVMTrait>
{
public:
  typedef void value_type;
  typedef value_type *pointer;
  typedef const value_type *const_pointer;

  template <typename U>
  struct rebind
  {
    typedef SVMAllocator<U, SVMTrait> other;
  };

  template <typename U, typename V>
  friend class SVMAllocator;
};

#if !defined(CL_HPP_NO_STD_UNIQUE_PTR)
namespace detail
{
template <class Alloc>
class Deleter
{
private:
  Alloc alloc_;
  size_type copies_;

public:
  typedef typename std::allocator_traits<Alloc>::pointer pointer;

  Deleter(const Alloc &alloc, size_type copies)
    : alloc_{ alloc }
    , copies_{ copies }
  {}

  void operator()(pointer ptr) const
  {
    Alloc tmpAlloc{ alloc_ };
    std::allocator_traits<Alloc>::destroy(tmpAlloc, std::addressof(*ptr));
    std::allocator_traits<Alloc>::deallocate(tmpAlloc, ptr, copies_);
  }
};
}  // namespace detail

/**
 * Allocation operation compatible with std::allocate_ptr.
 * Creates a unique_ptr<T> by default.
 * This requirement is to ensure that the control block is not
 * allocated in memory inaccessible to the host.
 */
template <class T, class Alloc, class... Args>
cl::pointer<T, detail::Deleter<Alloc>> allocate_pointer(const Alloc &alloc_, Args &&...args)
{
  Alloc alloc(alloc_);
  static const size_type copies = 1;

  // Ensure that creation of the management block and the
  // object are dealt with separately such that we only provide a deleter

  T *tmp = std::allocator_traits<Alloc>::allocate(alloc, copies);
  if (!tmp)
  {
    std::bad_alloc excep;
    throw excep;
  }
  try
  {
    std::allocator_traits<Alloc>::construct(alloc, std::addressof(*tmp), std::forward<Args>(args)...);

    return cl::pointer<T, detail::Deleter<Alloc>>(tmp, detail::Deleter<Alloc>{ alloc, copies });
  }
  catch (std::bad_alloc &b)
  {
    std::allocator_traits<Alloc>::deallocate(alloc, tmp, copies);
    throw;
  }
}

template <class T, class SVMTrait, class... Args>
cl::pointer<T, detail::Deleter<SVMAllocator<T, SVMTrait>>> allocate_svm(Args... args)
{
  SVMAllocator<T, SVMTrait> alloc;
  return cl::allocate_pointer<T>(alloc, args...);
}

template <class T, class SVMTrait, class... Args>
cl::pointer<T, detail::Deleter<SVMAllocator<T, SVMTrait>>> allocate_svm(const cl::Context &c, Args... args)
{
  SVMAllocator<T, SVMTrait> alloc(c);
  return cl::allocate_pointer<T>(alloc, args...);
}
#endif  // #if !defined(CL_HPP_NO_STD_UNIQUE_PTR)

/*! \brief Vector alias to simplify contruction of coarse-grained SVM containers.
 *
 */
template <class T>
using coarse_svm_vector = vector<T, cl::SVMAllocator<int, cl::SVMTraitCoarse<>>>;

/*! \brief Vector alias to simplify contruction of fine-grained SVM containers.
 *
 */
template <class T>
using fine_svm_vector = vector<T, cl::SVMAllocator<int, cl::SVMTraitFine<>>>;

/*! \brief Vector alias to simplify contruction of fine-grained SVM containers that support platform atomics.
 *
 */
template <class T>
using atomic_svm_vector = vector<T, cl::SVMAllocator<int, cl::SVMTraitAtomic<>>>;

#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200


/*! \brief Class interface for Buffer Memory Objects.
 *
 *  See Memory for details about copy semantics, etc.
 *
 *  \see Memory
 */
class Buffer : public Memory
{
public:
  /*! \brief Constructs a Buffer in a specified context.
   *
   *  Wraps clCreateBuffer().
   *
   *  \param host_ptr Storage to be used if the CL_MEM_USE_HOST_PTR flag was
   *                  specified.  Note alignment & exclusivity requirements.
   */
  Buffer(const Context &context, cl_mem_flags flags, size_type size, void *host_ptr = NULL, cl_int *err = NULL)
  {
    cl_int error;
    object_ = ::clCreateBuffer(context(), flags, size, host_ptr, &error);

    detail::errHandler(error, __CREATE_BUFFER_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  /*! \brief Constructs a Buffer in the default context.
   *
   *  Wraps clCreateBuffer().
   *
   *  \param host_ptr Storage to be used if the CL_MEM_USE_HOST_PTR flag was
   *                  specified.  Note alignment & exclusivity requirements.
   *
   *  \see Context::getDefault()
   */
  Buffer(cl_mem_flags flags, size_type size, void *host_ptr = NULL, cl_int *err = NULL)
  {
    cl_int error;

    Context context = Context::getDefault(err);

    object_ = ::clCreateBuffer(context(), flags, size, host_ptr, &error);

    detail::errHandler(error, __CREATE_BUFFER_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  /*!
   * \brief Construct a Buffer from a host container via iterators.
   * IteratorType must be random access.
   * If useHostPtr is specified iterators must represent contiguous data.
   */
  template <typename IteratorType>
  Buffer(IteratorType startIterator, IteratorType endIterator, bool readOnly, bool useHostPtr = false,
         cl_int *err = NULL)
  {
    typedef typename std::iterator_traits<IteratorType>::value_type DataType;
    cl_int error;

    cl_mem_flags flags = 0;
    if (readOnly)
    {
      flags |= CL_MEM_READ_ONLY;
    }
    else
    {
      flags |= CL_MEM_READ_WRITE;
    }
    if (useHostPtr)
    {
      flags |= CL_MEM_USE_HOST_PTR;
    }

    size_type size = sizeof(DataType) * (endIterator - startIterator);

    Context context = Context::getDefault(err);

    if (useHostPtr)
    {
      object_ = ::clCreateBuffer(context(), flags, size, static_cast<DataType *>(&*startIterator), &error);
    }
    else
    {
      object_ = ::clCreateBuffer(context(), flags, size, 0, &error);
    }

    detail::errHandler(error, __CREATE_BUFFER_ERR);
    if (err != NULL)
    {
      *err = error;
    }

    if (!useHostPtr)
    {
      error = cl::copy(startIterator, endIterator, *this);
      detail::errHandler(error, __CREATE_BUFFER_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
  }

  /*!
   * \brief Construct a Buffer from a host container via iterators using a specified context.
   * IteratorType must be random access.
   * If useHostPtr is specified iterators must represent contiguous data.
   */
  template <typename IteratorType>
  Buffer(const Context &context, IteratorType startIterator, IteratorType endIterator, bool readOnly,
         bool useHostPtr = false, cl_int *err = NULL);

  /*!
   * \brief Construct a Buffer from a host container via iterators using a specified queue.
   * If useHostPtr is specified iterators must be random access.
   */
  template <typename IteratorType>
  Buffer(const CommandQueue &queue, IteratorType startIterator, IteratorType endIterator, bool readOnly,
         bool useHostPtr = false, cl_int *err = NULL);

  //! \brief Default constructor - initializes to NULL.
  Buffer()
    : Memory()
  {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with earlier versions.
   *
   *  See Memory for further details.
   */
  explicit Buffer(const cl_mem &buffer, bool retainObject = false)
    : Memory(buffer, retainObject)
  {}

  /*! \brief Assignment from cl_mem - performs shallow copy.
   *
   *  See Memory for further details.
   */
  Buffer &operator=(const cl_mem &rhs)
  {
    Memory::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Buffer(const Buffer &buf)
    : Memory(buf)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Buffer &operator=(const Buffer &buf)
  {
    Memory::operator=(buf);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Buffer(Buffer &&buf) CL_HPP_NOEXCEPT_ : Memory(std::move(buf)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Buffer &operator=(Buffer &&buf)
  {
    Memory::operator=(std::move(buf));
    return *this;
  }

#if CL_HPP_TARGET_OPENCL_VERSION >= 110
  /*! \brief Creates a new buffer object from this.
   *
   *  Wraps clCreateSubBuffer().
   */
  Buffer createSubBuffer(cl_mem_flags flags, cl_buffer_create_type buffer_create_type, const void *buffer_create_info,
                         cl_int *err = NULL)
  {
    Buffer result;
    cl_int error;
    result.object_ = ::clCreateSubBuffer(object_, flags, buffer_create_type, buffer_create_info, &error);

    detail::errHandler(error, __CREATE_SUBBUFFER_ERR);
    if (err != NULL)
    {
      *err = error;
    }

    return result;
  }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 110
};

#if defined(CL_HPP_USE_DX_INTEROP)
/*! \brief Class interface for creating OpenCL buffers from ID3D10Buffer's.
 *
 *  This is provided to facilitate interoperability with Direct3D.
 *
 *  See Memory for details about copy semantics, etc.
 *
 *  \see Memory
 */
class BufferD3D10 : public Buffer
{
public:
  /*! \brief Constructs a BufferD3D10, in a specified context, from a
   *         given ID3D10Buffer.
   *
   *  Wraps clCreateFromD3D10BufferKHR().
   */
  BufferD3D10(const Context &context, cl_mem_flags flags, ID3D10Buffer *bufobj, cl_int *err = NULL)
    : pfn_clCreateFromD3D10BufferKHR(nullptr)
  {
    typedef CL_API_ENTRY cl_mem(CL_API_CALL * PFN_clCreateFromD3D10BufferKHR)(
      cl_context context, cl_mem_flags flags, ID3D10Buffer * buffer, cl_int * errcode_ret);
    PFN_clCreateFromD3D10BufferKHR pfn_clCreateFromD3D10BufferKHR;
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
    vector<cl_context_properties> props = context.getInfo<CL_CONTEXT_PROPERTIES>();
    cl_platform platform = -1;
    for (int i = 0; i < props.size(); ++i)
    {
      if (props[i] == CL_CONTEXT_PLATFORM)
      {
        platform = props[i + 1];
      }
    }
    CL_HPP_INIT_CL_EXT_FCN_PTR_PLATFORM_(platform, clCreateFromD3D10BufferKHR);
#elif CL_HPP_TARGET_OPENCL_VERSION >= 110
    CL_HPP_INIT_CL_EXT_FCN_PTR_(clCreateFromD3D10BufferKHR);
#endif

    cl_int error;
    object_ = pfn_clCreateFromD3D10BufferKHR(context(), flags, bufobj, &error);

    detail::errHandler(error, __CREATE_GL_BUFFER_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  //! \brief Default constructor - initializes to NULL.
  BufferD3D10()
    : Buffer()
  {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit BufferD3D10(const cl_mem &buffer, bool retainObject = false)
    : Buffer(buffer, retainObject)
  {}

  /*! \brief Assignment from cl_mem - performs shallow copy.
   *
   *  See Memory for further details.
   */
  BufferD3D10 &operator=(const cl_mem &rhs)
  {
    Buffer::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  BufferD3D10(const BufferD3D10 &buf)
    : Buffer(buf)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  BufferD3D10 &operator=(const BufferD3D10 &buf)
  {
    Buffer::operator=(buf);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  BufferD3D10(BufferD3D10 &&buf) CL_HPP_NOEXCEPT_ : Buffer(std::move(buf)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  BufferD3D10 &operator=(BufferD3D10 &&buf)
  {
    Buffer::operator=(std::move(buf));
    return *this;
  }
};
#endif

/*! \brief Class interface for GL Buffer Memory Objects.
 *
 *  This is provided to facilitate interoperability with OpenGL.
 *
 *  See Memory for details about copy semantics, etc.
 *
 *  \see Memory
 */
class BufferGL : public Buffer
{
public:
  /*! \brief Constructs a BufferGL in a specified context, from a given
   *         GL buffer.
   *
   *  Wraps clCreateFromGLBuffer().
   */
  BufferGL(const Context &context, cl_mem_flags flags, cl_GLuint bufobj, cl_int *err = NULL)
  {
    cl_int error;
    object_ = ::clCreateFromGLBuffer(context(), flags, bufobj, &error);

    detail::errHandler(error, __CREATE_GL_BUFFER_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  //! \brief Default constructor - initializes to NULL.
  BufferGL()
    : Buffer()
  {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit BufferGL(const cl_mem &buffer, bool retainObject = false)
    : Buffer(buffer, retainObject)
  {}

  /*! \brief Assignment from cl_mem - performs shallow copy.
   *
   *  See Memory for further details.
   */
  BufferGL &operator=(const cl_mem &rhs)
  {
    Buffer::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  BufferGL(const BufferGL &buf)
    : Buffer(buf)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  BufferGL &operator=(const BufferGL &buf)
  {
    Buffer::operator=(buf);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  BufferGL(BufferGL &&buf) CL_HPP_NOEXCEPT_ : Buffer(std::move(buf)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  BufferGL &operator=(BufferGL &&buf)
  {
    Buffer::operator=(std::move(buf));
    return *this;
  }

  //! \brief Wrapper for clGetGLObjectInfo().
  cl_int getObjectInfo(cl_gl_object_type *type, cl_GLuint *gl_object_name)
  {
    return detail::errHandler(::clGetGLObjectInfo(object_, type, gl_object_name), __GET_GL_OBJECT_INFO_ERR);
  }
};

/*! \brief Class interface for GL Render Buffer Memory Objects.
 *
 *  This is provided to facilitate interoperability with OpenGL.
 *
 *  See Memory for details about copy semantics, etc.
 *
 *  \see Memory
 */
class BufferRenderGL : public Buffer
{
public:
  /*! \brief Constructs a BufferRenderGL in a specified context, from a given
   *         GL Renderbuffer.
   *
   *  Wraps clCreateFromGLRenderbuffer().
   */
  BufferRenderGL(const Context &context, cl_mem_flags flags, cl_GLuint bufobj, cl_int *err = NULL)
  {
    cl_int error;
    object_ = ::clCreateFromGLRenderbuffer(context(), flags, bufobj, &error);

    detail::errHandler(error, __CREATE_GL_RENDER_BUFFER_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  //! \brief Default constructor - initializes to NULL.
  BufferRenderGL()
    : Buffer()
  {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit BufferRenderGL(const cl_mem &buffer, bool retainObject = false)
    : Buffer(buffer, retainObject)
  {}

  /*! \brief Assignment from cl_mem - performs shallow copy.
   *
   *  See Memory for further details.
   */
  BufferRenderGL &operator=(const cl_mem &rhs)
  {
    Buffer::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  BufferRenderGL(const BufferRenderGL &buf)
    : Buffer(buf)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  BufferRenderGL &operator=(const BufferRenderGL &buf)
  {
    Buffer::operator=(buf);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  BufferRenderGL(BufferRenderGL &&buf) CL_HPP_NOEXCEPT_ : Buffer(std::move(buf)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  BufferRenderGL &operator=(BufferRenderGL &&buf)
  {
    Buffer::operator=(std::move(buf));
    return *this;
  }

  //! \brief Wrapper for clGetGLObjectInfo().
  cl_int getObjectInfo(cl_gl_object_type *type, cl_GLuint *gl_object_name)
  {
    return detail::errHandler(::clGetGLObjectInfo(object_, type, gl_object_name), __GET_GL_OBJECT_INFO_ERR);
  }
};

/*! \brief C++ base class for Image Memory objects.
 *
 *  See Memory for details about copy semantics, etc.
 *
 *  \see Memory
 */
class Image : public Memory
{
protected:
  //! \brief Default constructor - initializes to NULL.
  Image()
    : Memory()
  {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit Image(const cl_mem &image, bool retainObject = false)
    : Memory(image, retainObject)
  {}

  /*! \brief Assignment from cl_mem - performs shallow copy.
   *
   *  See Memory for further details.
   */
  Image &operator=(const cl_mem &rhs)
  {
    Memory::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image(const Image &img)
    : Memory(img)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image &operator=(const Image &img)
  {
    Memory::operator=(img);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image(Image &&img) CL_HPP_NOEXCEPT_ : Memory(std::move(img)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image &operator=(Image &&img)
  {
    Memory::operator=(std::move(img));
    return *this;
  }


public:
  //! \brief Wrapper for clGetImageInfo().
  template <typename T>
  cl_int getImageInfo(cl_image_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetImageInfo, object_, name, param), __GET_IMAGE_INFO_ERR);
  }

  //! \brief Wrapper for clGetImageInfo() that returns by value.
  template <cl_image_info name>
  typename detail::param_traits<detail::cl_image_info, name>::param_type getImageInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_image_info, name>::param_type param;
    cl_int result = getImageInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }
};

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
/*! \brief Class interface for 1D Image Memory objects.
 *
 *  See Memory for details about copy semantics, etc.
 *
 *  \see Memory
 */
class Image1D : public Image
{
public:
  /*! \brief Constructs a 1D Image in a specified context.
   *
   *  Wraps clCreateImage().
   */
  Image1D(const Context &context, cl_mem_flags flags, ImageFormat format, size_type width, void *host_ptr = NULL,
          cl_int *err = NULL)
  {
    cl_int error;

    cl_image_desc desc = { 0 };
    desc.image_type = CL_MEM_OBJECT_IMAGE1D;
    desc.image_width = width;

    object_ = ::clCreateImage(context(), flags, &format, &desc, host_ptr, &error);

    detail::errHandler(error, __CREATE_IMAGE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  //! \brief Default constructor - initializes to NULL.
  Image1D() {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit Image1D(const cl_mem &image1D, bool retainObject = false)
    : Image(image1D, retainObject)
  {}

  /*! \brief Assignment from cl_mem - performs shallow copy.
   *
   *  See Memory for further details.
   */
  Image1D &operator=(const cl_mem &rhs)
  {
    Image::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image1D(const Image1D &img)
    : Image(img)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image1D &operator=(const Image1D &img)
  {
    Image::operator=(img);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image1D(Image1D &&img) CL_HPP_NOEXCEPT_ : Image(std::move(img)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image1D &operator=(Image1D &&img)
  {
    Image::operator=(std::move(img));
    return *this;
  }
};

/*! \class Image1DBuffer
 * \brief Image interface for 1D buffer images.
 */
class Image1DBuffer : public Image
{
public:
  Image1DBuffer(const Context &context, cl_mem_flags flags, ImageFormat format, size_type width, const Buffer &buffer,
                cl_int *err = NULL)
  {
    cl_int error;

    cl_image_desc desc = { 0 };
    desc.image_type = CL_MEM_OBJECT_IMAGE1D_BUFFER;
    desc.image_width = width;
    desc.buffer = buffer();

    object_ = ::clCreateImage(context(), flags, &format, &desc, NULL, &error);

    detail::errHandler(error, __CREATE_IMAGE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  Image1DBuffer() {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit Image1DBuffer(const cl_mem &image1D, bool retainObject = false)
    : Image(image1D, retainObject)
  {}

  Image1DBuffer &operator=(const cl_mem &rhs)
  {
    Image::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image1DBuffer(const Image1DBuffer &img)
    : Image(img)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image1DBuffer &operator=(const Image1DBuffer &img)
  {
    Image::operator=(img);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image1DBuffer(Image1DBuffer &&img) CL_HPP_NOEXCEPT_ : Image(std::move(img)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image1DBuffer &operator=(Image1DBuffer &&img)
  {
    Image::operator=(std::move(img));
    return *this;
  }
};

/*! \class Image1DArray
 * \brief Image interface for arrays of 1D images.
 */
class Image1DArray : public Image
{
public:
  Image1DArray(const Context &context, cl_mem_flags flags, ImageFormat format, size_type arraySize, size_type width,
               size_type rowPitch, void *host_ptr = NULL, cl_int *err = NULL)
  {
    cl_int error;

    cl_image_desc desc = { 0 };
    desc.image_type = CL_MEM_OBJECT_IMAGE1D_ARRAY;
    desc.image_width = width;
    desc.image_array_size = arraySize;
    desc.image_row_pitch = rowPitch;

    object_ = ::clCreateImage(context(), flags, &format, &desc, host_ptr, &error);

    detail::errHandler(error, __CREATE_IMAGE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  Image1DArray() {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit Image1DArray(const cl_mem &imageArray, bool retainObject = false)
    : Image(imageArray, retainObject)
  {}


  Image1DArray &operator=(const cl_mem &rhs)
  {
    Image::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image1DArray(const Image1DArray &img)
    : Image(img)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image1DArray &operator=(const Image1DArray &img)
  {
    Image::operator=(img);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image1DArray(Image1DArray &&img) CL_HPP_NOEXCEPT_ : Image(std::move(img)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image1DArray &operator=(Image1DArray &&img)
  {
    Image::operator=(std::move(img));
    return *this;
  }
};
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 120


/*! \brief Class interface for 2D Image Memory objects.
 *
 *  See Memory for details about copy semantics, etc.
 *
 *  \see Memory
 */
class Image2D : public Image
{
public:
  /*! \brief Constructs a 2D Image in a specified context.
   *
   *  Wraps clCreateImage().
   */
  Image2D(const Context &context, cl_mem_flags flags, ImageFormat format, size_type width, size_type height,
          size_type row_pitch = 0, void *host_ptr = NULL, cl_int *err = NULL)
  {
    cl_int error;
    bool useCreateImage;

#if CL_HPP_TARGET_OPENCL_VERSION >= 120 && CL_HPP_MINIMUM_OPENCL_VERSION < 120
    // Run-time decision based on the actual platform
    {
      cl_uint version = detail::getContextPlatformVersion(context());
      useCreateImage = (version >= 0x10002);  // OpenCL 1.2 or above
    }
#elif CL_HPP_TARGET_OPENCL_VERSION >= 120
    useCreateImage = true;
#else
    useCreateImage = false;
#endif

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
    if (useCreateImage)
    {
      cl_image_desc desc = { 0 };
      desc.image_type = CL_MEM_OBJECT_IMAGE2D;
      desc.image_width = width;
      desc.image_height = height;
      desc.image_row_pitch = row_pitch;

      object_ = ::clCreateImage(context(), flags, &format, &desc, host_ptr, &error);

      detail::errHandler(error, __CREATE_IMAGE_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120
#if CL_HPP_MINIMUM_OPENCL_VERSION < 120
    if (!useCreateImage)
    {
      object_ = ::clCreateImage2D(context(), flags, &format, width, height, row_pitch, host_ptr, &error);

      detail::errHandler(error, __CREATE_IMAGE2D_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 120
  }

#if CL_HPP_TARGET_OPENCL_VERSION >= 200 || defined(CL_HPP_USE_CL_IMAGE2D_FROM_BUFFER_KHR)
  /*! \brief Constructs a 2D Image from a buffer.
   * \note This will share storage with the underlying buffer.
   *
   *  Wraps clCreateImage().
   */
  Image2D(const Context &context, ImageFormat format, const Buffer &sourceBuffer, size_type width, size_type height,
          size_type row_pitch = 0, cl_int *err = nullptr)
  {
    cl_int error;

    cl_image_desc desc = { 0 };
    desc.image_type = CL_MEM_OBJECT_IMAGE2D;
    desc.image_width = width;
    desc.image_height = height;
    desc.image_row_pitch = row_pitch;
    desc.buffer = sourceBuffer();

    object_ = ::clCreateImage(context(),
                              0,  // flags inherited from buffer
                              &format, &desc, nullptr, &error);

    detail::errHandler(error, __CREATE_IMAGE_ERR);
    if (err != nullptr)
    {
      *err = error;
    }
  }
#endif  //#if CL_HPP_TARGET_OPENCL_VERSION >= 200 || defined(CL_HPP_USE_CL_IMAGE2D_FROM_BUFFER_KHR)

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
  /*! \brief Constructs a 2D Image from an image.
   * \note This will share storage with the underlying image but may
   *       reinterpret the channel order and type.
   *
   * The image will be created matching with a descriptor matching the source.
   *
   * \param order is the channel order to reinterpret the image data as.
   *              The channel order may differ as described in the OpenCL
   *              2.0 API specification.
   *
   * Wraps clCreateImage().
   */
  Image2D(const Context &context, cl_channel_order order, const Image &sourceImage, cl_int *err = nullptr)
  {
    cl_int error;

    // Descriptor fields have to match source image
    size_type sourceWidth = sourceImage.getImageInfo<CL_IMAGE_WIDTH>();
    size_type sourceHeight = sourceImage.getImageInfo<CL_IMAGE_HEIGHT>();
    size_type sourceRowPitch = sourceImage.getImageInfo<CL_IMAGE_ROW_PITCH>();
    cl_uint sourceNumMIPLevels = sourceImage.getImageInfo<CL_IMAGE_NUM_MIP_LEVELS>();
    cl_uint sourceNumSamples = sourceImage.getImageInfo<CL_IMAGE_NUM_SAMPLES>();
    cl_image_format sourceFormat = sourceImage.getImageInfo<CL_IMAGE_FORMAT>();

    // Update only the channel order.
    // Channel format inherited from source.
    sourceFormat.image_channel_order = order;

    cl_image_desc desc = { 0 };
    desc.image_type = CL_MEM_OBJECT_IMAGE2D;
    desc.image_width = sourceWidth;
    desc.image_height = sourceHeight;
    desc.image_row_pitch = sourceRowPitch;
    desc.num_mip_levels = sourceNumMIPLevels;
    desc.num_samples = sourceNumSamples;
    desc.buffer = sourceImage();

    object_ = ::clCreateImage(context(),
                              0,  // flags should be inherited from mem_object
                              &sourceFormat, &desc, nullptr, &error);

    detail::errHandler(error, __CREATE_IMAGE_ERR);
    if (err != nullptr)
    {
      *err = error;
    }
  }
#endif  //#if CL_HPP_TARGET_OPENCL_VERSION >= 200

  //! \brief Default constructor - initializes to NULL.
  Image2D() {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit Image2D(const cl_mem &image2D, bool retainObject = false)
    : Image(image2D, retainObject)
  {}

  /*! \brief Assignment from cl_mem - performs shallow copy.
   *
   *  See Memory for further details.
   */
  Image2D &operator=(const cl_mem &rhs)
  {
    Image::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image2D(const Image2D &img)
    : Image(img)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image2D &operator=(const Image2D &img)
  {
    Image::operator=(img);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image2D(Image2D &&img) CL_HPP_NOEXCEPT_ : Image(std::move(img)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image2D &operator=(Image2D &&img)
  {
    Image::operator=(std::move(img));
    return *this;
  }
};


#if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS)
/*! \brief Class interface for GL 2D Image Memory objects.
 *
 *  This is provided to facilitate interoperability with OpenGL.
 *
 *  See Memory for details about copy semantics, etc.
 *
 *  \see Memory
 *  \note Deprecated for OpenCL 1.2. Please use ImageGL instead.
 */
class CL_API_PREFIX__VERSION_1_1_DEPRECATED Image2DGL : public Image2D
{
public:
  /*! \brief Constructs an Image2DGL in a specified context, from a given
   *         GL Texture.
   *
   *  Wraps clCreateFromGLTexture2D().
   */
  Image2DGL(const Context &context, cl_mem_flags flags, cl_GLenum target, cl_GLint miplevel, cl_GLuint texobj,
            cl_int *err = NULL)
  {
    cl_int error;
    object_ = ::clCreateFromGLTexture2D(context(), flags, target, miplevel, texobj, &error);

    detail::errHandler(error, __CREATE_GL_TEXTURE_2D_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  //! \brief Default constructor - initializes to NULL.
  Image2DGL()
    : Image2D()
  {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit Image2DGL(const cl_mem &image, bool retainObject = false)
    : Image2D(image, retainObject)
  {}

  /*! \brief Assignment from cl_mem - performs shallow copy.
   *c
   *  See Memory for further details.
   */
  Image2DGL &operator=(const cl_mem &rhs)
  {
    Image2D::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image2DGL(const Image2DGL &img)
    : Image2D(img)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image2DGL &operator=(const Image2DGL &img)
  {
    Image2D::operator=(img);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image2DGL(Image2DGL &&img) CL_HPP_NOEXCEPT_ : Image2D(std::move(img)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image2DGL &operator=(Image2DGL &&img)
  {
    Image2D::operator=(std::move(img));
    return *this;
  }

} CL_API_SUFFIX__VERSION_1_1_DEPRECATED;
#endif  // CL_USE_DEPRECATED_OPENCL_1_1_APIS

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
/*! \class Image2DArray
 * \brief Image interface for arrays of 2D images.
 */
class Image2DArray : public Image
{
public:
  Image2DArray(const Context &context, cl_mem_flags flags, ImageFormat format, size_type arraySize, size_type width,
               size_type height, size_type rowPitch, size_type slicePitch, void *host_ptr = NULL, cl_int *err = NULL)
  {
    cl_int error;

    cl_image_desc desc = { 0 };
    desc.image_type = CL_MEM_OBJECT_IMAGE2D_ARRAY;
    desc.image_width = width;
    desc.image_height = height;
    desc.image_array_size = arraySize;
    desc.image_row_pitch = rowPitch;
    desc.image_slice_pitch = slicePitch;

    object_ = ::clCreateImage(context(), flags, &format, &desc, host_ptr, &error);

    detail::errHandler(error, __CREATE_IMAGE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  Image2DArray() {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit Image2DArray(const cl_mem &imageArray, bool retainObject = false)
    : Image(imageArray, retainObject)
  {}

  Image2DArray &operator=(const cl_mem &rhs)
  {
    Image::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image2DArray(const Image2DArray &img)
    : Image(img)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image2DArray &operator=(const Image2DArray &img)
  {
    Image::operator=(img);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image2DArray(Image2DArray &&img) CL_HPP_NOEXCEPT_ : Image(std::move(img)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image2DArray &operator=(Image2DArray &&img)
  {
    Image::operator=(std::move(img));
    return *this;
  }
};
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 120

/*! \brief Class interface for 3D Image Memory objects.
 *
 *  See Memory for details about copy semantics, etc.
 *
 *  \see Memory
 */
class Image3D : public Image
{
public:
  /*! \brief Constructs a 3D Image in a specified context.
   *
   *  Wraps clCreateImage().
   */
  Image3D(const Context &context, cl_mem_flags flags, ImageFormat format, size_type width, size_type height,
          size_type depth, size_type row_pitch = 0, size_type slice_pitch = 0, void *host_ptr = NULL,
          cl_int *err = NULL)
  {
    cl_int error;
    bool useCreateImage;

#if CL_HPP_TARGET_OPENCL_VERSION >= 120 && CL_HPP_MINIMUM_OPENCL_VERSION < 120
    // Run-time decision based on the actual platform
    {
      cl_uint version = detail::getContextPlatformVersion(context());
      useCreateImage = (version >= 0x10002);  // OpenCL 1.2 or above
    }
#elif CL_HPP_TARGET_OPENCL_VERSION >= 120
    useCreateImage = true;
#else
    useCreateImage = false;
#endif

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
    if (useCreateImage)
    {
      cl_image_desc desc = { 0 };
      desc.image_type = CL_MEM_OBJECT_IMAGE3D;
      desc.image_width = width;
      desc.image_height = height;
      desc.image_depth = depth;
      desc.image_row_pitch = row_pitch;
      desc.image_slice_pitch = slice_pitch;

      object_ = ::clCreateImage(context(), flags, &format, &desc, host_ptr, &error);

      detail::errHandler(error, __CREATE_IMAGE_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120
#if CL_HPP_MINIMUM_OPENCL_VERSION < 120
    if (!useCreateImage)
    {
      object_ =
        ::clCreateImage3D(context(), flags, &format, width, height, depth, row_pitch, slice_pitch, host_ptr, &error);

      detail::errHandler(error, __CREATE_IMAGE3D_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 120
  }

  //! \brief Default constructor - initializes to NULL.
  Image3D()
    : Image()
  {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit Image3D(const cl_mem &image3D, bool retainObject = false)
    : Image(image3D, retainObject)
  {}

  /*! \brief Assignment from cl_mem - performs shallow copy.
   *
   *  See Memory for further details.
   */
  Image3D &operator=(const cl_mem &rhs)
  {
    Image::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image3D(const Image3D &img)
    : Image(img)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image3D &operator=(const Image3D &img)
  {
    Image::operator=(img);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image3D(Image3D &&img) CL_HPP_NOEXCEPT_ : Image(std::move(img)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image3D &operator=(Image3D &&img)
  {
    Image::operator=(std::move(img));
    return *this;
  }
};

#if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS)
/*! \brief Class interface for GL 3D Image Memory objects.
 *
 *  This is provided to facilitate interoperability with OpenGL.
 *
 *  See Memory for details about copy semantics, etc.
 *
 *  \see Memory
 */
class Image3DGL : public Image3D
{
public:
  /*! \brief Constructs an Image3DGL in a specified context, from a given
   *         GL Texture.
   *
   *  Wraps clCreateFromGLTexture3D().
   */
  Image3DGL(const Context &context, cl_mem_flags flags, cl_GLenum target, cl_GLint miplevel, cl_GLuint texobj,
            cl_int *err = NULL)
  {
    cl_int error;
    object_ = ::clCreateFromGLTexture3D(context(), flags, target, miplevel, texobj, &error);

    detail::errHandler(error, __CREATE_GL_TEXTURE_3D_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  //! \brief Default constructor - initializes to NULL.
  Image3DGL()
    : Image3D()
  {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit Image3DGL(const cl_mem &image, bool retainObject = false)
    : Image3D(image, retainObject)
  {}

  /*! \brief Assignment from cl_mem - performs shallow copy.
   *
   *  See Memory for further details.
   */
  Image3DGL &operator=(const cl_mem &rhs)
  {
    Image3D::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image3DGL(const Image3DGL &img)
    : Image3D(img)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Image3DGL &operator=(const Image3DGL &img)
  {
    Image3D::operator=(img);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image3DGL(Image3DGL &&img) CL_HPP_NOEXCEPT_ : Image3D(std::move(img)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Image3DGL &operator=(Image3DGL &&img)
  {
    Image3D::operator=(std::move(img));
    return *this;
  }
};
#endif  // CL_USE_DEPRECATED_OPENCL_1_1_APIS

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
/*! \class ImageGL
 * \brief general image interface for GL interop.
 * We abstract the 2D and 3D GL images into a single instance here
 * that wraps all GL sourced images on the grounds that setup information
 * was performed by OpenCL anyway.
 */
class ImageGL : public Image
{
public:
  ImageGL(const Context &context, cl_mem_flags flags, cl_GLenum target, cl_GLint miplevel, cl_GLuint texobj,
          cl_int *err = NULL)
  {
    cl_int error;
    object_ = ::clCreateFromGLTexture(context(), flags, target, miplevel, texobj, &error);

    detail::errHandler(error, __CREATE_GL_TEXTURE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  ImageGL()
    : Image()
  {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  See Memory for further details.
   */
  explicit ImageGL(const cl_mem &image, bool retainObject = false)
    : Image(image, retainObject)
  {}

  ImageGL &operator=(const cl_mem &rhs)
  {
    Image::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  ImageGL(const ImageGL &img)
    : Image(img)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  ImageGL &operator=(const ImageGL &img)
  {
    Image::operator=(img);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  ImageGL(ImageGL &&img) CL_HPP_NOEXCEPT_ : Image(std::move(img)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  ImageGL &operator=(ImageGL &&img)
  {
    Image::operator=(std::move(img));
    return *this;
  }
};
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120


#if CL_HPP_TARGET_OPENCL_VERSION >= 200
/*! \brief Class interface for Pipe Memory Objects.
 *
 *  See Memory for details about copy semantics, etc.
 *
 *  \see Memory
 */
class Pipe : public Memory
{
public:
  /*! \brief Constructs a Pipe in a specified context.
   *
   * Wraps clCreatePipe().
   * @param context Context in which to create the pipe.
   * @param flags Bitfield. Only CL_MEM_READ_WRITE and CL_MEM_HOST_NO_ACCESS are valid.
   * @param packet_size Size in bytes of a single packet of the pipe.
   * @param max_packets Number of packets that may be stored in the pipe.
   *
   */
  Pipe(const Context &context, cl_uint packet_size, cl_uint max_packets, cl_int *err = NULL)
  {
    cl_int error;

    cl_mem_flags flags = CL_MEM_READ_WRITE | CL_MEM_HOST_NO_ACCESS;
    object_ = ::clCreatePipe(context(), flags, packet_size, max_packets, nullptr, &error);

    detail::errHandler(error, __CREATE_PIPE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  /*! \brief Constructs a Pipe in a the default context.
   *
   * Wraps clCreatePipe().
   * @param flags Bitfield. Only CL_MEM_READ_WRITE and CL_MEM_HOST_NO_ACCESS are valid.
   * @param packet_size Size in bytes of a single packet of the pipe.
   * @param max_packets Number of packets that may be stored in the pipe.
   *
   */
  Pipe(cl_uint packet_size, cl_uint max_packets, cl_int *err = NULL)
  {
    cl_int error;

    Context context = Context::getDefault(err);

    cl_mem_flags flags = CL_MEM_READ_WRITE | CL_MEM_HOST_NO_ACCESS;
    object_ = ::clCreatePipe(context(), flags, packet_size, max_packets, nullptr, &error);

    detail::errHandler(error, __CREATE_PIPE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  //! \brief Default constructor - initializes to NULL.
  Pipe()
    : Memory()
  {}

  /*! \brief Constructor from cl_mem - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with earlier versions.
   *
   *  See Memory for further details.
   */
  explicit Pipe(const cl_mem &pipe, bool retainObject = false)
    : Memory(pipe, retainObject)
  {}

  /*! \brief Assignment from cl_mem - performs shallow copy.
   *
   *  See Memory for further details.
   */
  Pipe &operator=(const cl_mem &rhs)
  {
    Memory::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Pipe(const Pipe &pipe)
    : Memory(pipe)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Pipe &operator=(const Pipe &pipe)
  {
    Memory::operator=(pipe);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Pipe(Pipe &&pipe) CL_HPP_NOEXCEPT_ : Memory(std::move(pipe)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Pipe &operator=(Pipe &&pipe)
  {
    Memory::operator=(std::move(pipe));
    return *this;
  }

  //! \brief Wrapper for clGetMemObjectInfo().
  template <typename T>
  cl_int getInfo(cl_pipe_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetPipeInfo, object_, name, param), __GET_PIPE_INFO_ERR);
  }

  //! \brief Wrapper for clGetMemObjectInfo() that returns by value.
  template <cl_pipe_info name>
  typename detail::param_traits<detail::cl_pipe_info, name>::param_type getInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_pipe_info, name>::param_type param;
    cl_int result = getInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }
};      // class Pipe
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200


/*! \brief Class interface for cl_sampler.
 *
 *  \note Copies of these objects are shallow, meaning that the copy will refer
 *        to the same underlying cl_sampler as the original.  For details, see
 *        clRetainSampler() and clReleaseSampler().
 *
 *  \see cl_sampler
 */
class Sampler : public detail::Wrapper<cl_sampler>
{
public:
  //! \brief Default constructor - initializes to NULL.
  Sampler() {}

  /*! \brief Constructs a Sampler in a specified context.
   *
   *  Wraps clCreateSampler().
   */
  Sampler(const Context &context, cl_bool normalized_coords, cl_addressing_mode addressing_mode,
          cl_filter_mode filter_mode, cl_int *err = NULL)
  {
    cl_int error;

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
    cl_sampler_properties sampler_properties[] = { CL_SAMPLER_NORMALIZED_COORDS,
                                                   normalized_coords,
                                                   CL_SAMPLER_ADDRESSING_MODE,
                                                   addressing_mode,
                                                   CL_SAMPLER_FILTER_MODE,
                                                   filter_mode,
                                                   0 };
    object_ = ::clCreateSamplerWithProperties(context(), sampler_properties, &error);

    detail::errHandler(error, __CREATE_SAMPLER_WITH_PROPERTIES_ERR);
    if (err != NULL)
    {
      *err = error;
    }
#else
    object_ = ::clCreateSampler(context(), normalized_coords, addressing_mode, filter_mode, &error);

    detail::errHandler(error, __CREATE_SAMPLER_ERR);
    if (err != NULL)
    {
      *err = error;
    }
#endif
  }

  /*! \brief Constructor from cl_sampler - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  This effectively transfers ownership of a refcount on the cl_sampler
   *  into the new Sampler object.
   */
  explicit Sampler(const cl_sampler &sampler, bool retainObject = false)
    : detail::Wrapper<cl_type>(sampler, retainObject)
  {}

  /*! \brief Assignment operator from cl_sampler - takes ownership.
   *
   *  This effectively transfers ownership of a refcount on the rhs and calls
   *  clReleaseSampler() on the value previously held by this instance.
   */
  Sampler &operator=(const cl_sampler &rhs)
  {
    detail::Wrapper<cl_type>::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Sampler(const Sampler &sam)
    : detail::Wrapper<cl_type>(sam)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Sampler &operator=(const Sampler &sam)
  {
    detail::Wrapper<cl_type>::operator=(sam);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Sampler(Sampler &&sam) CL_HPP_NOEXCEPT_ : detail::Wrapper<cl_type>(std::move(sam)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Sampler &operator=(Sampler &&sam)
  {
    detail::Wrapper<cl_type>::operator=(std::move(sam));
    return *this;
  }

  //! \brief Wrapper for clGetSamplerInfo().
  template <typename T>
  cl_int getInfo(cl_sampler_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetSamplerInfo, object_, name, param), __GET_SAMPLER_INFO_ERR);
  }

  //! \brief Wrapper for clGetSamplerInfo() that returns by value.
  template <cl_sampler_info name>
  typename detail::param_traits<detail::cl_sampler_info, name>::param_type getInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_sampler_info, name>::param_type param;
    cl_int result = getInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }
};

class Program;
class CommandQueue;
class DeviceCommandQueue;
class Kernel;

//! \brief Class interface for specifying NDRange values.
class NDRange
{
private:
  size_type sizes_[3];
  cl_uint dimensions_;

public:
  //! \brief Default constructor - resulting range has zero dimensions.
  NDRange()
    : dimensions_(0)
  {
    sizes_[0] = 0;
    sizes_[1] = 0;
    sizes_[2] = 0;
  }

  //! \brief Constructs one-dimensional range.
  NDRange(size_type size0)
    : dimensions_(1)
  {
    sizes_[0] = size0;
    sizes_[1] = 1;
    sizes_[2] = 1;
  }

  //! \brief Constructs two-dimensional range.
  NDRange(size_type size0, size_type size1)
    : dimensions_(2)
  {
    sizes_[0] = size0;
    sizes_[1] = size1;
    sizes_[2] = 1;
  }

  //! \brief Constructs three-dimensional range.
  NDRange(size_type size0, size_type size1, size_type size2)
    : dimensions_(3)
  {
    sizes_[0] = size0;
    sizes_[1] = size1;
    sizes_[2] = size2;
  }

  /*! \brief Conversion operator to const size_type *.
   *
   *  \returns a pointer to the size of the first dimension.
   */
  operator const size_type *() const { return sizes_; }

  //! \brief Queries the number of dimensions in the range.
  size_type dimensions() const { return dimensions_; }

  //! \brief Returns the size of the object in bytes based on the
  // runtime number of dimensions
  size_type size() const { return dimensions_ * sizeof(size_type); }

  size_type *get() { return sizes_; }

  const size_type *get() const { return sizes_; }
};

//! \brief A zero-dimensional range.
static const NDRange NullRange;

//! \brief Local address wrapper for use with Kernel::setArg
struct LocalSpaceArg
{
  size_type size_;
};

namespace detail
{
template <typename T, class Enable = void>
struct KernelArgumentHandler;

// Enable for objects that are not subclasses of memory
// Pointers, constants etc
template <typename T>
struct KernelArgumentHandler<T, typename std::enable_if<!std::is_base_of<cl::Memory, T>::value>::type>
{
  static size_type size(const T &) { return sizeof(T); }
  static const T *ptr(const T &value) { return &value; }
};

// Enable for subclasses of memory where we want to get a reference to the cl_mem out
// and pass that in for safety
template <typename T>
struct KernelArgumentHandler<T, typename std::enable_if<std::is_base_of<cl::Memory, T>::value>::type>
{
  static size_type size(const T &) { return sizeof(cl_mem); }
  static const cl_mem *ptr(const T &value) { return &(value()); }
};

// Specialization for DeviceCommandQueue defined later

template <>
struct KernelArgumentHandler<LocalSpaceArg, void>
{
  static size_type size(const LocalSpaceArg &value) { return value.size_; }
  static const void *ptr(const LocalSpaceArg &) { return NULL; }
};

}  // namespace detail
//! \endcond

/*! Local
 * \brief Helper function for generating LocalSpaceArg objects.
 */
inline LocalSpaceArg Local(size_type size)
{
  LocalSpaceArg ret = { size };
  return ret;
}

/*! \brief Class interface for cl_kernel.
 *
 *  \note Copies of these objects are shallow, meaning that the copy will refer
 *        to the same underlying cl_kernel as the original.  For details, see
 *        clRetainKernel() and clReleaseKernel().
 *
 *  \see cl_kernel
 */
class Kernel : public detail::Wrapper<cl_kernel>
{
public:
  inline Kernel(const Program &program, const char *name, cl_int *err = NULL);

  //! \brief Default constructor - initializes to NULL.
  Kernel() {}

  /*! \brief Constructor from cl_kernel - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   *  This effectively transfers ownership of a refcount on the cl_kernel
   *  into the new Kernel object.
   */
  explicit Kernel(const cl_kernel &kernel, bool retainObject = false)
    : detail::Wrapper<cl_type>(kernel, retainObject)
  {}

  /*! \brief Assignment operator from cl_kernel - takes ownership.
   *
   *  This effectively transfers ownership of a refcount on the rhs and calls
   *  clReleaseKernel() on the value previously held by this instance.
   */
  Kernel &operator=(const cl_kernel &rhs)
  {
    detail::Wrapper<cl_type>::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Kernel(const Kernel &kernel)
    : detail::Wrapper<cl_type>(kernel)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Kernel &operator=(const Kernel &kernel)
  {
    detail::Wrapper<cl_type>::operator=(kernel);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Kernel(Kernel &&kernel) CL_HPP_NOEXCEPT_ : detail::Wrapper<cl_type>(std::move(kernel)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Kernel &operator=(Kernel &&kernel)
  {
    detail::Wrapper<cl_type>::operator=(std::move(kernel));
    return *this;
  }

  template <typename T>
  cl_int getInfo(cl_kernel_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetKernelInfo, object_, name, param), __GET_KERNEL_INFO_ERR);
  }

  template <cl_kernel_info name>
  typename detail::param_traits<detail::cl_kernel_info, name>::param_type getInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_kernel_info, name>::param_type param;
    cl_int result = getInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
  template <typename T>
  cl_int getArgInfo(cl_uint argIndex, cl_kernel_arg_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetKernelArgInfo, object_, argIndex, name, param),
                              __GET_KERNEL_ARG_INFO_ERR);
  }

  template <cl_kernel_arg_info name>
  typename detail::param_traits<detail::cl_kernel_arg_info, name>::param_type getArgInfo(cl_uint argIndex,
                                                                                         cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_kernel_arg_info, name>::param_type param;
    cl_int result = getArgInfo(argIndex, name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120

  template <typename T>
  cl_int getWorkGroupInfo(const Device &device, cl_kernel_work_group_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetKernelWorkGroupInfo, object_, device(), name, param),
                              __GET_KERNEL_WORK_GROUP_INFO_ERR);
  }

  template <cl_kernel_work_group_info name>
  typename detail::param_traits<detail::cl_kernel_work_group_info, name>::param_type getWorkGroupInfo(
    const Device &device, cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_kernel_work_group_info, name>::param_type param;
    cl_int result = getWorkGroupInfo(device, name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }

#if (CL_HPP_TARGET_OPENCL_VERSION >= 200 && defined(CL_HPP_USE_CL_SUB_GROUPS_KHR)) || \
  CL_HPP_TARGET_OPENCL_VERSION >= 210
  cl_int getSubGroupInfo(const cl::Device &dev, cl_kernel_sub_group_info name, const cl::NDRange &range,
                         size_type *param) const
  {
#if CL_HPP_TARGET_OPENCL_VERSION >= 210

    return detail::errHandler(
      clGetKernelSubGroupInfo(object_, dev(), name, range.size(), range.get(), sizeof(size_type), param, nullptr),
      __GET_KERNEL_SUB_GROUP_INFO_ERR);

#else  // #if CL_HPP_TARGET_OPENCL_VERSION >= 210

    typedef clGetKernelSubGroupInfoKHR_fn PFN_clGetKernelSubGroupInfoKHR;
    static PFN_clGetKernelSubGroupInfoKHR pfn_clGetKernelSubGroupInfoKHR = NULL;
    CL_HPP_INIT_CL_EXT_FCN_PTR_(clGetKernelSubGroupInfoKHR);

    return detail::errHandler(pfn_clGetKernelSubGroupInfoKHR(object_, dev(), name, range.size(), range.get(),
                                                             sizeof(size_type), param, nullptr),
                              __GET_KERNEL_SUB_GROUP_INFO_ERR);

#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 210
  }

  template <cl_kernel_sub_group_info name>
  size_type getSubGroupInfo(const cl::Device &dev, const cl::NDRange &range, cl_int *err = NULL) const
  {
    size_type param;
    cl_int result = getSubGroupInfo(dev, name, range, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
  /*! \brief setArg overload taking a shared_ptr type
   */
  template <typename T, class D>
  cl_int setArg(cl_uint index, const cl::pointer<T, D> &argPtr)
  {
    return detail::errHandler(::clSetKernelArgSVMPointer(object_, index, argPtr.get()), __SET_KERNEL_ARGS_ERR);
  }

  /*! \brief setArg overload taking a vector type.
   */
  template <typename T, class Alloc>
  cl_int setArg(cl_uint index, const cl::vector<T, Alloc> &argPtr)
  {
    return detail::errHandler(::clSetKernelArgSVMPointer(object_, index, argPtr.data()), __SET_KERNEL_ARGS_ERR);
  }

  /*! \brief setArg overload taking a pointer type
   */
  template <typename T>
  typename std::enable_if<std::is_pointer<T>::value, cl_int>::type setArg(cl_uint index, const T argPtr)
  {
    return detail::errHandler(::clSetKernelArgSVMPointer(object_, index, argPtr), __SET_KERNEL_ARGS_ERR);
  }
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200

  /*! \brief setArg overload taking a POD type
   */
  template <typename T>
  typename std::enable_if<!std::is_pointer<T>::value, cl_int>::type setArg(cl_uint index, const T &value)
  {
    return detail::errHandler(::clSetKernelArg(object_, index, detail::KernelArgumentHandler<T>::size(value),
                                               detail::KernelArgumentHandler<T>::ptr(value)),
                              __SET_KERNEL_ARGS_ERR);
  }

  cl_int setArg(cl_uint index, size_type size, const void *argPtr)
  {
    return detail::errHandler(::clSetKernelArg(object_, index, size, argPtr), __SET_KERNEL_ARGS_ERR);
  }

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
  /*!
   * Specify a vector of SVM pointers that the kernel may access in
   * addition to its arguments.
   */
  cl_int setSVMPointers(const vector<void *> &pointerList)
  {
    return detail::errHandler(::clSetKernelExecInfo(object_, CL_KERNEL_EXEC_INFO_SVM_PTRS,
                                                    sizeof(void *) * pointerList.size(), pointerList.data()));
  }

  /*!
   * Specify a std::array of SVM pointers that the kernel may access in
   * addition to its arguments.
   */
  template <int ArrayLength>
  cl_int setSVMPointers(const std::array<void *, ArrayLength> &pointerList)
  {
    return detail::errHandler(::clSetKernelExecInfo(object_, CL_KERNEL_EXEC_INFO_SVM_PTRS,
                                                    sizeof(void *) * pointerList.size(), pointerList.data()));
  }

  /*! \brief Enable fine-grained system SVM.
   *
   * \note It is only possible to enable fine-grained system SVM if all devices
   *       in the context associated with kernel support it.
   *
   * \param svmEnabled True if fine-grained system SVM is requested. False otherwise.
   * \return CL_SUCCESS if the function was executed succesfully. CL_INVALID_OPERATION
   *         if no devices in the context support fine-grained system SVM.
   *
   * \see clSetKernelExecInfo
   */
  cl_int enableFineGrainedSystemSVM(bool svmEnabled)
  {
    cl_bool svmEnabled_ = svmEnabled ? CL_TRUE : CL_FALSE;
    return detail::errHandler(
      ::clSetKernelExecInfo(object_, CL_KERNEL_EXEC_INFO_SVM_FINE_GRAIN_SYSTEM, sizeof(cl_bool), &svmEnabled_));
  }

  template <int index, int ArrayLength, class D, typename T0, typename T1, typename... Ts>
  void setSVMPointersHelper(std::array<void *, ArrayLength> &pointerList, const pointer<T0, D> &t0,
                            const pointer<T1, D> &t1, Ts &...ts)
  {
    pointerList[index] = static_cast<void *>(t0.get());
    setSVMPointersHelper<index + 1, ArrayLength>(pointerList, t1, ts...);
  }

  template <int index, int ArrayLength, typename T0, typename T1, typename... Ts>
  typename std::enable_if<std::is_pointer<T0>::value, void>::type setSVMPointersHelper(
    std::array<void *, ArrayLength> &pointerList, T0 t0, T1 t1, Ts... ts)
  {
    pointerList[index] = static_cast<void *>(t0);
    setSVMPointersHelper<index + 1, ArrayLength>(pointerList, t1, ts...);
  }

  template <int index, int ArrayLength, typename T0, class D>
  void setSVMPointersHelper(std::array<void *, ArrayLength> &pointerList, const pointer<T0, D> &t0)
  {
    pointerList[index] = static_cast<void *>(t0.get());
  }


  template <int index, int ArrayLength, typename T0>
  typename std::enable_if<std::is_pointer<T0>::value, void>::type setSVMPointersHelper(
    std::array<void *, ArrayLength> &pointerList, T0 t0)
  {
    pointerList[index] = static_cast<void *>(t0);
  }

  template <typename T0, typename... Ts>
  cl_int setSVMPointers(const T0 &t0, Ts &...ts)
  {
    std::array<void *, 1 + sizeof...(Ts)> pointerList;

    setSVMPointersHelper<0, 1 + sizeof...(Ts)>(pointerList, t0, ts...);
    return detail::errHandler(::clSetKernelExecInfo(object_, CL_KERNEL_EXEC_INFO_SVM_PTRS,
                                                    sizeof(void *) * (1 + sizeof...(Ts)), pointerList.data()));
  }

  template <typename T>
  cl_int setExecInfo(cl_kernel_exec_info param_name, const T &val)
  {
    return detail::errHandler(::clSetKernelExecInfo(object_, param_name, sizeof(T), &val));
  }

  template <cl_kernel_exec_info name>
  cl_int setExecInfo(typename detail::param_traits<detail::cl_kernel_exec_info, name>::param_type &val)
  {
    return setExecInfo(name, val);
  }
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200

#if CL_HPP_TARGET_OPENCL_VERSION >= 210
  /**
   * Make a deep copy of the kernel object including its arguments.
   * @return A new kernel object with internal state entirely separate from that
   *         of the original but with any arguments set on the original intact.
   */
  Kernel clone()
  {
    cl_int error;
    Kernel retValue(clCloneKernel(this->get(), &error));

    detail::errHandler(error, __CLONE_KERNEL_ERR);
    return retValue;
  }
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 210
};

/*! \class Program
 * \brief Program interface that implements cl_program.
 */
class Program : public detail::Wrapper<cl_program>
{
public:
#if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)
  typedef vector<vector<unsigned char>> Binaries;
  typedef vector<string> Sources;
#else   // #if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)
  typedef vector<std::pair<const void *, size_type>> Binaries;
  typedef vector<std::pair<const char *, size_type>> Sources;
#endif  // #if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)

  Program(const string &source, bool build = false, cl_int *err = NULL)
  {
    cl_int error;

    const char *strings = source.c_str();
    const size_type length = source.size();

    Context context = Context::getDefault(err);

    object_ = ::clCreateProgramWithSource(context(), (cl_uint)1, &strings, &length, &error);

    detail::errHandler(error, __CREATE_PROGRAM_WITH_SOURCE_ERR);

    if (error == CL_SUCCESS && build)
    {
      error = ::clBuildProgram(object_, 0, NULL,
#if !defined(CL_HPP_CL_1_2_DEFAULT_BUILD)
                               "-cl-std=CL2.0",
#else
                               "",
#endif  // #if !defined(CL_HPP_CL_1_2_DEFAULT_BUILD)
                               NULL, NULL);

      detail::buildErrHandler(error, __BUILD_PROGRAM_ERR, getBuildInfo<CL_PROGRAM_BUILD_LOG>());
    }

    if (err != NULL)
    {
      *err = error;
    }
  }

  Program(const Context &context, const string &source, bool build = false, cl_int *err = NULL)
  {
    cl_int error;

    const char *strings = source.c_str();
    const size_type length = source.size();

    object_ = ::clCreateProgramWithSource(context(), (cl_uint)1, &strings, &length, &error);

    detail::errHandler(error, __CREATE_PROGRAM_WITH_SOURCE_ERR);

    if (error == CL_SUCCESS && build)
    {
      error = ::clBuildProgram(object_, 0, NULL,
#if !defined(CL_HPP_CL_1_2_DEFAULT_BUILD)
                               "-cl-std=CL2.0",
#else
                               "",
#endif  // #if !defined(CL_HPP_CL_1_2_DEFAULT_BUILD)
                               NULL, NULL);

      detail::buildErrHandler(error, __BUILD_PROGRAM_ERR, getBuildInfo<CL_PROGRAM_BUILD_LOG>());
    }

    if (err != NULL)
    {
      *err = error;
    }
  }

  /**
   * Create a program from a vector of source strings and the default context.
   * Does not compile or link the program.
   */
  Program(const Sources &sources, cl_int *err = NULL)
  {
    cl_int error;
    Context context = Context::getDefault(err);

    const size_type n = (size_type)sources.size();

    vector<size_type> lengths(n);
    vector<const char *> strings(n);

    for (size_type i = 0; i < n; ++i)
    {
#if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)
      strings[i] = sources[(int)i].data();
      lengths[i] = sources[(int)i].length();
#else   // #if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)
      strings[i] = sources[(int)i].first;
      lengths[i] = sources[(int)i].second;
#endif  // #if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)
    }

    object_ = ::clCreateProgramWithSource(context(), (cl_uint)n, strings.data(), lengths.data(), &error);

    detail::errHandler(error, __CREATE_PROGRAM_WITH_SOURCE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  /**
   * Create a program from a vector of source strings and a provided context.
   * Does not compile or link the program.
   */
  Program(const Context &context, const Sources &sources, cl_int *err = NULL)
  {
    cl_int error;

    const size_type n = (size_type)sources.size();

    vector<size_type> lengths(n);
    vector<const char *> strings(n);

    for (size_type i = 0; i < n; ++i)
    {
#if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)
      strings[i] = sources[(int)i].data();
      lengths[i] = sources[(int)i].length();
#else   // #if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)
      strings[i] = sources[(int)i].first;
      lengths[i] = sources[(int)i].second;
#endif  // #if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)
    }

    object_ = ::clCreateProgramWithSource(context(), (cl_uint)n, strings.data(), lengths.data(), &error);

    detail::errHandler(error, __CREATE_PROGRAM_WITH_SOURCE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }


#if CL_HPP_TARGET_OPENCL_VERSION >= 210 || (CL_HPP_TARGET_OPENCL_VERSION == 200 && defined(CL_HPP_USE_IL_KHR))
  /**
   * Program constructor to allow construction of program from SPIR-V or another IL.
   * Valid for either OpenCL >= 2.1 or when CL_HPP_USE_IL_KHR is defined.
   */
  Program(const vector<char> &IL, bool build = false, cl_int *err = NULL)
  {
    cl_int error;

    Context context = Context::getDefault(err);

#if CL_HPP_TARGET_OPENCL_VERSION >= 210

    object_ = ::clCreateProgramWithIL(context(), static_cast<const void *>(IL.data()), IL.size(), &error);

#else  // #if CL_HPP_TARGET_OPENCL_VERSION >= 210

    typedef clCreateProgramWithILKHR_fn PFN_clCreateProgramWithILKHR;
    static PFN_clCreateProgramWithILKHR pfn_clCreateProgramWithILKHR = NULL;
    CL_HPP_INIT_CL_EXT_FCN_PTR_(clCreateProgramWithILKHR);

    object_ = pfn_clCreateProgramWithILKHR(context(), static_cast<const void *>(IL.data()), IL.size(), &error);

#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 210

    detail::errHandler(error, __CREATE_PROGRAM_WITH_IL_ERR);

    if (error == CL_SUCCESS && build)
    {
      error = ::clBuildProgram(object_, 0, NULL,
#if !defined(CL_HPP_CL_1_2_DEFAULT_BUILD)
                               "-cl-std=CL2.0",
#else
                               "",
#endif  // #if !defined(CL_HPP_CL_1_2_DEFAULT_BUILD)
                               NULL, NULL);

      detail::buildErrHandler(error, __BUILD_PROGRAM_ERR, getBuildInfo<CL_PROGRAM_BUILD_LOG>());
    }

    if (err != NULL)
    {
      *err = error;
    }
  }

  /**
   * Program constructor to allow construction of program from SPIR-V or another IL
   * for a specific context.
   * Valid for either OpenCL >= 2.1 or when CL_HPP_USE_IL_KHR is defined.
   */
  Program(const Context &context, const vector<char> &IL, bool build = false, cl_int *err = NULL)
  {
    cl_int error;

#if CL_HPP_TARGET_OPENCL_VERSION >= 210

    object_ = ::clCreateProgramWithIL(context(), static_cast<const void *>(IL.data()), IL.size(), &error);

#else  // #if CL_HPP_TARGET_OPENCL_VERSION >= 210

    typedef clCreateProgramWithILKHR_fn PFN_clCreateProgramWithILKHR;
    static PFN_clCreateProgramWithILKHR pfn_clCreateProgramWithILKHR = NULL;
    CL_HPP_INIT_CL_EXT_FCN_PTR_(clCreateProgramWithILKHR);

    object_ = pfn_clCreateProgramWithILKHR(context(), static_cast<const void *>(IL.data()), IL.size(), &error);

#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 210

    detail::errHandler(error, __CREATE_PROGRAM_WITH_IL_ERR);

    if (error == CL_SUCCESS && build)
    {
      error = ::clBuildProgram(object_, 0, NULL,
#if !defined(CL_HPP_CL_1_2_DEFAULT_BUILD)
                               "-cl-std=CL2.0",
#else
                               "",
#endif  // #if !defined(CL_HPP_CL_1_2_DEFAULT_BUILD)
                               NULL, NULL);

      detail::buildErrHandler(error, __BUILD_PROGRAM_ERR, getBuildInfo<CL_PROGRAM_BUILD_LOG>());
    }

    if (err != NULL)
    {
      *err = error;
    }
  }
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 210

  /**
   * Construct a program object from a list of devices and a per-device list of binaries.
   * \param context A valid OpenCL context in which to construct the program.
   * \param devices A vector of OpenCL device objects for which the program will be created.
   * \param binaries A vector of pairs of a pointer to a binary object and its length.
   * \param binaryStatus An optional vector that on completion will be resized to
   *   match the size of binaries and filled with values to specify if each binary
   *   was successfully loaded.
   *   Set to CL_SUCCESS if the binary was successfully loaded.
   *   Set to CL_INVALID_VALUE if the length is 0 or the binary pointer is NULL.
   *   Set to CL_INVALID_BINARY if the binary provided is not valid for the matching device.
   * \param err if non-NULL will be set to CL_SUCCESS on successful operation or one of the following errors:
   *   CL_INVALID_CONTEXT if context is not a valid context.
   *   CL_INVALID_VALUE if the length of devices is zero; or if the length of binaries does not match the length of
   * devices; or if any entry in binaries is NULL or has length 0. CL_INVALID_DEVICE if OpenCL devices listed in devices
   * are not in the list of devices associated with context. CL_INVALID_BINARY if an invalid program binary was
   * encountered for any device. binaryStatus will return specific status for each device. CL_OUT_OF_HOST_MEMORY if
   * there is a failure to allocate resources required by the OpenCL implementation on the host.
   */
  Program(const Context &context, const vector<Device> &devices, const Binaries &binaries,
          vector<cl_int> *binaryStatus = NULL, cl_int *err = NULL)
  {
    cl_int error;

    const size_type numDevices = devices.size();

    // Catch size mismatch early and return
    if (binaries.size() != numDevices)
    {
      error = CL_INVALID_VALUE;
      detail::errHandler(error, __CREATE_PROGRAM_WITH_BINARY_ERR);
      if (err != NULL)
      {
        *err = error;
      }
      return;
    }


    vector<size_type> lengths(numDevices);
    vector<const unsigned char *> images(numDevices);
#if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)
    for (size_type i = 0; i < numDevices; ++i)
    {
      images[i] = binaries[i].data();
      lengths[i] = binaries[(int)i].size();
    }
#else   // #if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)
    for (size_type i = 0; i < numDevices; ++i)
    {
      images[i] = (const unsigned char *)binaries[i].first;
      lengths[i] = binaries[(int)i].second;
    }
#endif  // #if !defined(CL_HPP_ENABLE_PROGRAM_CONSTRUCTION_FROM_ARRAY_COMPATIBILITY)

    vector<cl_device_id> deviceIDs(numDevices);
    for (size_type deviceIndex = 0; deviceIndex < numDevices; ++deviceIndex)
    {
      deviceIDs[deviceIndex] = (devices[deviceIndex])();
    }

    if (binaryStatus)
    {
      binaryStatus->resize(numDevices);
    }

    object_ =
      ::clCreateProgramWithBinary(context(), (cl_uint)devices.size(), deviceIDs.data(), lengths.data(), images.data(),
                                  (binaryStatus != NULL && numDevices > 0) ? &binaryStatus->front() : NULL, &error);

    detail::errHandler(error, __CREATE_PROGRAM_WITH_BINARY_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }


#if CL_HPP_TARGET_OPENCL_VERSION >= 120
  /**
   * Create program using builtin kernels.
   * \param kernelNames Semi-colon separated list of builtin kernel names
   */
  Program(const Context &context, const vector<Device> &devices, const string &kernelNames, cl_int *err = NULL)
  {
    cl_int error;


    size_type numDevices = devices.size();
    vector<cl_device_id> deviceIDs(numDevices);
    for (size_type deviceIndex = 0; deviceIndex < numDevices; ++deviceIndex)
    {
      deviceIDs[deviceIndex] = (devices[deviceIndex])();
    }

    object_ = ::clCreateProgramWithBuiltInKernels(context(), (cl_uint)devices.size(), deviceIDs.data(),
                                                  kernelNames.c_str(), &error);

    detail::errHandler(error, __CREATE_PROGRAM_WITH_BUILT_IN_KERNELS_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120

  Program() {}


  /*! \brief Constructor from cl_program - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   */
  explicit Program(const cl_program &program, bool retainObject = false)
    : detail::Wrapper<cl_type>(program, retainObject)
  {}

  Program &operator=(const cl_program &rhs)
  {
    detail::Wrapper<cl_type>::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Program(const Program &program)
    : detail::Wrapper<cl_type>(program)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  Program &operator=(const Program &program)
  {
    detail::Wrapper<cl_type>::operator=(program);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Program(Program &&program) CL_HPP_NOEXCEPT_ : detail::Wrapper<cl_type>(std::move(program)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  Program &operator=(Program &&program)
  {
    detail::Wrapper<cl_type>::operator=(std::move(program));
    return *this;
  }

  cl_int build(const vector<Device> &devices, const char *options = NULL,
               void(CL_CALLBACK *notifyFptr)(cl_program, void *) = NULL, void *data = NULL) const
  {
    size_type numDevices = devices.size();
    vector<cl_device_id> deviceIDs(numDevices);

    for (size_type deviceIndex = 0; deviceIndex < numDevices; ++deviceIndex)
    {
      deviceIDs[deviceIndex] = (devices[deviceIndex])();
    }

    cl_int buildError = ::clBuildProgram(object_, (cl_uint)devices.size(), deviceIDs.data(), options, notifyFptr, data);

    return detail::buildErrHandler(buildError, __BUILD_PROGRAM_ERR, getBuildInfo<CL_PROGRAM_BUILD_LOG>());
  }

  cl_int build(const Device &device, const char *options = NULL,
               void(CL_CALLBACK *notifyFptr)(cl_program, void *) = NULL, void *data = NULL) const
  {
    cl_device_id deviceID = device();

    cl_int buildError = ::clBuildProgram(object_, 1, &deviceID, options, notifyFptr, data);

    BuildLogType buildLog(1);
    buildLog.push_back(std::make_pair(device, getBuildInfo<CL_PROGRAM_BUILD_LOG>(device)));
    return detail::buildErrHandler(buildError, __BUILD_PROGRAM_ERR, buildLog);
  }

  cl_int build(const char *options = NULL, void(CL_CALLBACK *notifyFptr)(cl_program, void *) = NULL,
               void *data = NULL) const
  {
    cl_int buildError = ::clBuildProgram(object_, 0, NULL, options, notifyFptr, data);

    return detail::buildErrHandler(buildError, __BUILD_PROGRAM_ERR, getBuildInfo<CL_PROGRAM_BUILD_LOG>());
  }

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
  cl_int compile(const char *options = NULL, void(CL_CALLBACK *notifyFptr)(cl_program, void *) = NULL,
                 void *data = NULL) const
  {
    cl_int error = ::clCompileProgram(object_, 0, NULL, options, 0, NULL, NULL, notifyFptr, data);
    return detail::buildErrHandler(error, __COMPILE_PROGRAM_ERR, getBuildInfo<CL_PROGRAM_BUILD_LOG>());
  }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120

  template <typename T>
  cl_int getInfo(cl_program_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetProgramInfo, object_, name, param), __GET_PROGRAM_INFO_ERR);
  }

  template <cl_program_info name>
  typename detail::param_traits<detail::cl_program_info, name>::param_type getInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_program_info, name>::param_type param;
    cl_int result = getInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }

  template <typename T>
  cl_int getBuildInfo(const Device &device, cl_program_build_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetProgramBuildInfo, object_, device(), name, param),
                              __GET_PROGRAM_BUILD_INFO_ERR);
  }

  template <cl_program_build_info name>
  typename detail::param_traits<detail::cl_program_build_info, name>::param_type getBuildInfo(const Device &device,
                                                                                              cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_program_build_info, name>::param_type param;
    cl_int result = getBuildInfo(device, name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }

  /**
   * Build info function that returns a vector of device/info pairs for the specified
   * info type and for all devices in the program.
   * On an error reading the info for any device, an empty vector of info will be returned.
   */
  template <cl_program_build_info name>
  vector<std::pair<cl::Device, typename detail::param_traits<detail::cl_program_build_info, name>::param_type>>
    getBuildInfo(cl_int *err = NULL) const
  {
    cl_int result = CL_SUCCESS;

    auto devs = getInfo<CL_PROGRAM_DEVICES>(&result);
    vector<std::pair<cl::Device, typename detail::param_traits<detail::cl_program_build_info, name>::param_type>>
      devInfo;

    // If there was an initial error from getInfo return the error
    if (result != CL_SUCCESS)
    {
      if (err != NULL)
      {
        *err = result;
      }
      return devInfo;
    }

    for (const cl::Device &d : devs)
    {
      typename detail::param_traits<detail::cl_program_build_info, name>::param_type param;
      result = getBuildInfo(d, name, &param);
      devInfo.push_back(
        std::pair<cl::Device, typename detail::param_traits<detail::cl_program_build_info, name>::param_type>(d,
                                                                                                              param));
      if (result != CL_SUCCESS)
      {
        // On error, leave the loop and return the error code
        break;
      }
    }
    if (err != NULL)
    {
      *err = result;
    }
    if (result != CL_SUCCESS)
    {
      devInfo.clear();
    }
    return devInfo;
  }

  cl_int createKernels(vector<Kernel> *kernels)
  {
    cl_uint numKernels;
    cl_int err = ::clCreateKernelsInProgram(object_, 0, NULL, &numKernels);
    if (err != CL_SUCCESS)
    {
      return detail::errHandler(err, __CREATE_KERNELS_IN_PROGRAM_ERR);
    }

    vector<cl_kernel> value(numKernels);

    err = ::clCreateKernelsInProgram(object_, numKernels, value.data(), NULL);
    if (err != CL_SUCCESS)
    {
      return detail::errHandler(err, __CREATE_KERNELS_IN_PROGRAM_ERR);
    }

    if (kernels)
    {
      kernels->resize(value.size());

      // Assign to param, constructing with retain behaviour
      // to correctly capture each underlying CL object
      for (size_type i = 0; i < value.size(); i++)
      {
        // We do not need to retain because this kernel is being created
        // by the runtime
        (*kernels)[i] = Kernel(value[i], false);
      }
    }
    return CL_SUCCESS;
  }

#if CL_HPP_TARGET_OPENCL_VERSION >= 220
#if defined(CL_USE_DEPRECATED_OPENCL_2_2_APIS)
  /*! \brief Registers a callback function to be called when destructors for
   *         program scope global variables are complete and before the
   *         program is released.
   *
   *  Wraps clSetProgramReleaseCallback().
   *
   *  Each call to this function registers the specified user callback function
   *  on a callback stack associated with program. The registered user callback
   *  functions are called in the reverse order in which they were registered.
   */
  CL_API_PREFIX__VERSION_2_2_DEPRECATED cl_int
    setReleaseCallback(void(CL_CALLBACK *pfn_notify)(cl_program program, void *user_data),
                       void *user_data = NULL) CL_API_SUFFIX__VERSION_2_2_DEPRECATED
  {
    return detail::errHandler(::clSetProgramReleaseCallback(object_, pfn_notify, user_data),
                              __SET_PROGRAM_RELEASE_CALLBACK_ERR);
  }
#endif  // #if defined(CL_USE_DEPRECATED_OPENCL_2_2_APIS)

  /*! \brief Sets a SPIR-V specialization constant.
   *
   *  Wraps clSetProgramSpecializationConstant().
   */
  template <typename T>
  typename std::enable_if<!std::is_pointer<T>::value, cl_int>::type setSpecializationConstant(cl_uint index,
                                                                                              const T &value)
  {
    return detail::errHandler(::clSetProgramSpecializationConstant(object_, index, sizeof(value), &value),
                              __SET_PROGRAM_SPECIALIZATION_CONSTANT_ERR);
  }

  /*! \brief Sets a SPIR-V specialization constant.
   *
   *  Wraps clSetProgramSpecializationConstant().
   */
  cl_int setSpecializationConstant(cl_uint index, size_type size, const void *value)
  {
    return detail::errHandler(::clSetProgramSpecializationConstant(object_, index, size, value),
                              __SET_PROGRAM_SPECIALIZATION_CONSTANT_ERR);
  }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 220
};

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
inline Program linkProgram(Program input1, Program input2, const char *options = NULL,
                           void(CL_CALLBACK *notifyFptr)(cl_program, void *) = NULL, void *data = NULL,
                           cl_int *err = NULL)
{
  cl_int error_local = CL_SUCCESS;

  cl_program programs[2] = { input1(), input2() };

  Context ctx = input1.getInfo<CL_PROGRAM_CONTEXT>(&error_local);
  if (error_local != CL_SUCCESS)
  {
    detail::errHandler(error_local, __LINK_PROGRAM_ERR);
  }

  cl_program prog = ::clLinkProgram(ctx(), 0, NULL, options, 2, programs, notifyFptr, data, &error_local);

  detail::errHandler(error_local, __COMPILE_PROGRAM_ERR);
  if (err != NULL)
  {
    *err = error_local;
  }

  return Program(prog);
}

inline Program linkProgram(vector<Program> inputPrograms, const char *options = NULL,
                           void(CL_CALLBACK *notifyFptr)(cl_program, void *) = NULL, void *data = NULL,
                           cl_int *err = NULL)
{
  cl_int error_local = CL_SUCCESS;

  vector<cl_program> programs(inputPrograms.size());

  for (unsigned int i = 0; i < inputPrograms.size(); i++)
  {
    programs[i] = inputPrograms[i]();
  }

  Context ctx;
  if (inputPrograms.size() > 0)
  {
    ctx = inputPrograms[0].getInfo<CL_PROGRAM_CONTEXT>(&error_local);
    if (error_local != CL_SUCCESS)
    {
      detail::errHandler(error_local, __LINK_PROGRAM_ERR);
    }
  }
  cl_program prog = ::clLinkProgram(ctx(), 0, NULL, options, (cl_uint)inputPrograms.size(), programs.data(), notifyFptr,
                                    data, &error_local);

  detail::errHandler(error_local, __COMPILE_PROGRAM_ERR);
  if (err != NULL)
  {
    *err = error_local;
  }

  return Program(prog, false);
}
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120

// Template specialization for CL_PROGRAM_BINARIES
template <>
inline cl_int cl::Program::getInfo(cl_program_info name, vector<vector<unsigned char>> *param) const
{
  if (name != CL_PROGRAM_BINARIES)
  {
    return CL_INVALID_VALUE;
  }
  if (param)
  {
    // Resize the parameter array appropriately for each allocation
    // and pass down to the helper

    vector<size_type> sizes = getInfo<CL_PROGRAM_BINARY_SIZES>();
    size_type numBinaries = sizes.size();

    // Resize the parameter array and constituent arrays
    param->resize(numBinaries);
    for (size_type i = 0; i < numBinaries; ++i)
    {
      (*param)[i].resize(sizes[i]);
    }

    return detail::errHandler(detail::getInfo(&::clGetProgramInfo, object_, name, param), __GET_PROGRAM_INFO_ERR);
  }

  return CL_SUCCESS;
}

template <>
inline vector<vector<unsigned char>> cl::Program::getInfo<CL_PROGRAM_BINARIES>(cl_int *err) const
{
  vector<vector<unsigned char>> binariesVectors;

  cl_int result = getInfo(CL_PROGRAM_BINARIES, &binariesVectors);
  if (err != NULL)
  {
    *err = result;
  }
  return binariesVectors;
}

#if CL_HPP_TARGET_OPENCL_VERSION >= 220
// Template specialization for clSetProgramSpecializationConstant
template <>
inline cl_int cl::Program::setSpecializationConstant(cl_uint index, const bool &value)
{
  cl_uchar ucValue = value ? CL_UCHAR_MAX : 0;
  return detail::errHandler(::clSetProgramSpecializationConstant(object_, index, sizeof(ucValue), &ucValue),
                            __SET_PROGRAM_SPECIALIZATION_CONSTANT_ERR);
}
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 220

inline Kernel::Kernel(const Program &program, const char *name, cl_int *err)
{
  cl_int error;

  object_ = ::clCreateKernel(program(), name, &error);
  detail::errHandler(error, __CREATE_KERNEL_ERR);

  if (err != NULL)
  {
    *err = error;
  }
}

enum class QueueProperties : cl_command_queue_properties
{
  None = 0,
  Profiling = CL_QUEUE_PROFILING_ENABLE,
  OutOfOrder = CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE,
};

inline QueueProperties operator|(QueueProperties lhs, QueueProperties rhs)
{
  return static_cast<QueueProperties>(static_cast<cl_command_queue_properties>(lhs) |
                                      static_cast<cl_command_queue_properties>(rhs));
}

/*! \class CommandQueue
 * \brief CommandQueue interface for cl_command_queue.
 */
class CommandQueue : public detail::Wrapper<cl_command_queue>
{
private:
  static std::once_flag default_initialized_;
  static CommandQueue default_;
  static cl_int default_error_;

  /*! \brief Create the default command queue returned by @ref getDefault.
   *
   * It sets default_error_ to indicate success or failure. It does not throw
   * @c cl::Error.
   */
  static void makeDefault()
  {
    /* We don't want to throw an error from this function, so we have to
     * catch and set the error flag.
     */
#if defined(CL_HPP_ENABLE_EXCEPTIONS)
    try
#endif
    {
      int error;
      Context context = Context::getDefault(&error);

      if (error != CL_SUCCESS)
      {
        default_error_ = error;
      }
      else
      {
        Device device = Device::getDefault();
        default_ = CommandQueue(context, device, 0, &default_error_);
      }
    }
#if defined(CL_HPP_ENABLE_EXCEPTIONS)
    catch (cl::Error &e)
    {
      default_error_ = e.err();
    }
#endif
  }

  /*! \brief Create the default command queue.
   *
   * This sets @c default_. It does not throw
   * @c cl::Error.
   */
  static void makeDefaultProvided(const CommandQueue &c) { default_ = c; }

public:
#ifdef CL_HPP_UNIT_TEST_ENABLE
  /*! \brief Reset the default.
   *
   * This sets @c default_ to an empty value to support cleanup in
   * the unit test framework.
   * This function is not thread safe.
   */
  static void unitTestClearDefault() { default_ = CommandQueue(); }
#endif  // #ifdef CL_HPP_UNIT_TEST_ENABLE


  /*!
   * \brief Constructs a CommandQueue based on passed properties.
   * Will return an CL_INVALID_QUEUE_PROPERTIES error if CL_QUEUE_ON_DEVICE is specified.
   */
  CommandQueue(cl_command_queue_properties properties, cl_int *err = NULL)
  {
    cl_int error;

    Context context = Context::getDefault(&error);
    detail::errHandler(error, __CREATE_CONTEXT_ERR);

    if (error != CL_SUCCESS)
    {
      if (err != NULL)
      {
        *err = error;
      }
    }
    else
    {
      Device device = context.getInfo<CL_CONTEXT_DEVICES>()[0];
      bool useWithProperties;

#if CL_HPP_TARGET_OPENCL_VERSION >= 200 && CL_HPP_MINIMUM_OPENCL_VERSION < 200
      // Run-time decision based on the actual platform
      {
        cl_uint version = detail::getContextPlatformVersion(context());
        useWithProperties = (version >= 0x20000);  // OpenCL 2.0 or above
      }
#elif CL_HPP_TARGET_OPENCL_VERSION >= 200
      useWithProperties = true;
#else
      useWithProperties = false;
#endif

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
      if (useWithProperties)
      {
        cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, properties, 0 };
        if ((properties & CL_QUEUE_ON_DEVICE) == 0)
        {
          object_ = ::clCreateCommandQueueWithProperties(context(), device(), queue_properties, &error);
        }
        else
        {
          error = CL_INVALID_QUEUE_PROPERTIES;
        }

        detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
        if (err != NULL)
        {
          *err = error;
        }
      }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200
#if CL_HPP_MINIMUM_OPENCL_VERSION < 200
      if (!useWithProperties)
      {
        object_ = ::clCreateCommandQueue(context(), device(), properties, &error);

        detail::errHandler(error, __CREATE_COMMAND_QUEUE_ERR);
        if (err != NULL)
        {
          *err = error;
        }
      }
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 200
    }
  }

  /*!
   * \brief Constructs a CommandQueue based on passed properties.
   * Will return an CL_INVALID_QUEUE_PROPERTIES error if CL_QUEUE_ON_DEVICE is specified.
   */
  CommandQueue(QueueProperties properties, cl_int *err = NULL)
  {
    cl_int error;

    Context context = Context::getDefault(&error);
    detail::errHandler(error, __CREATE_CONTEXT_ERR);

    if (error != CL_SUCCESS)
    {
      if (err != NULL)
      {
        *err = error;
      }
    }
    else
    {
      Device device = context.getInfo<CL_CONTEXT_DEVICES>()[0];
      bool useWithProperties;

#if CL_HPP_TARGET_OPENCL_VERSION >= 200 && CL_HPP_MINIMUM_OPENCL_VERSION < 200
      // Run-time decision based on the actual platform
      {
        cl_uint version = detail::getContextPlatformVersion(context());
        useWithProperties = (version >= 0x20000);  // OpenCL 2.0 or above
      }
#elif CL_HPP_TARGET_OPENCL_VERSION >= 200
      useWithProperties = true;
#else
      useWithProperties = false;
#endif

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
      if (useWithProperties)
      {
        cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, static_cast<cl_queue_properties>(properties),
                                                   0 };

        object_ = ::clCreateCommandQueueWithProperties(context(), device(), queue_properties, &error);

        detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
        if (err != NULL)
        {
          *err = error;
        }
      }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200
#if CL_HPP_MINIMUM_OPENCL_VERSION < 200
      if (!useWithProperties)
      {
        object_ =
          ::clCreateCommandQueue(context(), device(), static_cast<cl_command_queue_properties>(properties), &error);

        detail::errHandler(error, __CREATE_COMMAND_QUEUE_ERR);
        if (err != NULL)
        {
          *err = error;
        }
      }
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 200
    }
  }

  /*!
   * \brief Constructs a CommandQueue for an implementation defined device in the given context
   * Will return an CL_INVALID_QUEUE_PROPERTIES error if CL_QUEUE_ON_DEVICE is specified.
   */
  explicit CommandQueue(const Context &context, cl_command_queue_properties properties = 0, cl_int *err = NULL)
  {
    cl_int error;
    bool useWithProperties;
    vector<cl::Device> devices;
    error = context.getInfo(CL_CONTEXT_DEVICES, &devices);

    detail::errHandler(error, __CREATE_CONTEXT_ERR);

    if (error != CL_SUCCESS)
    {
      if (err != NULL)
      {
        *err = error;
      }
      return;
    }

#if CL_HPP_TARGET_OPENCL_VERSION >= 200 && CL_HPP_MINIMUM_OPENCL_VERSION < 200
    // Run-time decision based on the actual platform
    {
      cl_uint version = detail::getContextPlatformVersion(context());
      useWithProperties = (version >= 0x20000);  // OpenCL 2.0 or above
    }
#elif CL_HPP_TARGET_OPENCL_VERSION >= 200
    useWithProperties = true;
#else
    useWithProperties = false;
#endif

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
    if (useWithProperties)
    {
      cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, properties, 0 };
      if ((properties & CL_QUEUE_ON_DEVICE) == 0)
      {
        object_ = ::clCreateCommandQueueWithProperties(context(), devices[0](), queue_properties, &error);
      }
      else
      {
        error = CL_INVALID_QUEUE_PROPERTIES;
      }

      detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200
#if CL_HPP_MINIMUM_OPENCL_VERSION < 200
    if (!useWithProperties)
    {
      object_ = ::clCreateCommandQueue(context(), devices[0](), properties, &error);

      detail::errHandler(error, __CREATE_COMMAND_QUEUE_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 200
  }

  /*!
   * \brief Constructs a CommandQueue for an implementation defined device in the given context
   * Will return an CL_INVALID_QUEUE_PROPERTIES error if CL_QUEUE_ON_DEVICE is specified.
   */
  explicit CommandQueue(const Context &context, QueueProperties properties, cl_int *err = NULL)
  {
    cl_int error;
    bool useWithProperties;
    vector<cl::Device> devices;
    error = context.getInfo(CL_CONTEXT_DEVICES, &devices);

    detail::errHandler(error, __CREATE_CONTEXT_ERR);

    if (error != CL_SUCCESS)
    {
      if (err != NULL)
      {
        *err = error;
      }
      return;
    }

#if CL_HPP_TARGET_OPENCL_VERSION >= 200 && CL_HPP_MINIMUM_OPENCL_VERSION < 200
    // Run-time decision based on the actual platform
    {
      cl_uint version = detail::getContextPlatformVersion(context());
      useWithProperties = (version >= 0x20000);  // OpenCL 2.0 or above
    }
#elif CL_HPP_TARGET_OPENCL_VERSION >= 200
    useWithProperties = true;
#else
    useWithProperties = false;
#endif

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
    if (useWithProperties)
    {
      cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, static_cast<cl_queue_properties>(properties), 0 };
      object_ = ::clCreateCommandQueueWithProperties(context(), devices[0](), queue_properties, &error);

      detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200
#if CL_HPP_MINIMUM_OPENCL_VERSION < 200
    if (!useWithProperties)
    {
      object_ =
        ::clCreateCommandQueue(context(), devices[0](), static_cast<cl_command_queue_properties>(properties), &error);

      detail::errHandler(error, __CREATE_COMMAND_QUEUE_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 200
  }

  /*!
   * \brief Constructs a CommandQueue for a passed device and context
   * Will return an CL_INVALID_QUEUE_PROPERTIES error if CL_QUEUE_ON_DEVICE is specified.
   */
  CommandQueue(const Context &context, const Device &device, cl_command_queue_properties properties = 0,
               cl_int *err = NULL)
  {
    cl_int error;
    bool useWithProperties;

#if CL_HPP_TARGET_OPENCL_VERSION >= 200 && CL_HPP_MINIMUM_OPENCL_VERSION < 200
    // Run-time decision based on the actual platform
    {
      cl_uint version = detail::getContextPlatformVersion(context());
      useWithProperties = (version >= 0x20000);  // OpenCL 2.0 or above
    }
#elif CL_HPP_TARGET_OPENCL_VERSION >= 200
    useWithProperties = true;
#else
    useWithProperties = false;
#endif

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
    if (useWithProperties)
    {
      cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, properties, 0 };
      object_ = ::clCreateCommandQueueWithProperties(context(), device(), queue_properties, &error);

      detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200
#if CL_HPP_MINIMUM_OPENCL_VERSION < 200
    if (!useWithProperties)
    {
      object_ = ::clCreateCommandQueue(context(), device(), properties, &error);

      detail::errHandler(error, __CREATE_COMMAND_QUEUE_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 200
  }

  /*!
   * \brief Constructs a CommandQueue for a passed device and context
   * Will return an CL_INVALID_QUEUE_PROPERTIES error if CL_QUEUE_ON_DEVICE is specified.
   */
  CommandQueue(const Context &context, const Device &device, QueueProperties properties, cl_int *err = NULL)
  {
    cl_int error;
    bool useWithProperties;

#if CL_HPP_TARGET_OPENCL_VERSION >= 200 && CL_HPP_MINIMUM_OPENCL_VERSION < 200
    // Run-time decision based on the actual platform
    {
      cl_uint version = detail::getContextPlatformVersion(context());
      useWithProperties = (version >= 0x20000);  // OpenCL 2.0 or above
    }
#elif CL_HPP_TARGET_OPENCL_VERSION >= 200
    useWithProperties = true;
#else
    useWithProperties = false;
#endif

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
    if (useWithProperties)
    {
      cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, static_cast<cl_queue_properties>(properties), 0 };
      object_ = ::clCreateCommandQueueWithProperties(context(), device(), queue_properties, &error);

      detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200
#if CL_HPP_MINIMUM_OPENCL_VERSION < 200
    if (!useWithProperties)
    {
      object_ =
        ::clCreateCommandQueue(context(), device(), static_cast<cl_command_queue_properties>(properties), &error);

      detail::errHandler(error, __CREATE_COMMAND_QUEUE_ERR);
      if (err != NULL)
      {
        *err = error;
      }
    }
#endif  // CL_HPP_MINIMUM_OPENCL_VERSION < 200
  }

  static CommandQueue getDefault(cl_int *err = NULL)
  {
    std::call_once(default_initialized_, makeDefault);
#if CL_HPP_TARGET_OPENCL_VERSION >= 200
    detail::errHandler(default_error_, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
#else   // CL_HPP_TARGET_OPENCL_VERSION >= 200
    detail::errHandler(default_error_, __CREATE_COMMAND_QUEUE_ERR);
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200
    if (err != NULL)
    {
      *err = default_error_;
    }
    return default_;
  }

  /**
   * Modify the default command queue to be used by
   * subsequent operations.
   * Will only set the default if no default was previously created.
   * @return updated default command queue.
   *         Should be compared to the passed value to ensure that it was updated.
   */
  static CommandQueue setDefault(const CommandQueue &default_queue)
  {
    std::call_once(default_initialized_, makeDefaultProvided, std::cref(default_queue));
    detail::errHandler(default_error_);
    return default_;
  }

  CommandQueue() {}


  /*! \brief Constructor from cl_command_queue - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   */
  explicit CommandQueue(const cl_command_queue &commandQueue, bool retainObject = false)
    : detail::Wrapper<cl_type>(commandQueue, retainObject)
  {}

  CommandQueue &operator=(const cl_command_queue &rhs)
  {
    detail::Wrapper<cl_type>::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  CommandQueue(const CommandQueue &queue)
    : detail::Wrapper<cl_type>(queue)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  CommandQueue &operator=(const CommandQueue &queue)
  {
    detail::Wrapper<cl_type>::operator=(queue);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  CommandQueue(CommandQueue &&queue) CL_HPP_NOEXCEPT_ : detail::Wrapper<cl_type>(std::move(queue)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  CommandQueue &operator=(CommandQueue &&queue)
  {
    detail::Wrapper<cl_type>::operator=(std::move(queue));
    return *this;
  }

  template <typename T>
  cl_int getInfo(cl_command_queue_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetCommandQueueInfo, object_, name, param),
                              __GET_COMMAND_QUEUE_INFO_ERR);
  }

  template <cl_command_queue_info name>
  typename detail::param_traits<detail::cl_command_queue_info, name>::param_type getInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_command_queue_info, name>::param_type param;
    cl_int result = getInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }

  cl_int enqueueReadBuffer(const Buffer &buffer, cl_bool blocking, size_type offset, size_type size, void *ptr,
                           const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueReadBuffer(
        object_, buffer(), blocking, offset, size, ptr, (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_READ_BUFFER_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  cl_int enqueueWriteBuffer(const Buffer &buffer, cl_bool blocking, size_type offset, size_type size, const void *ptr,
                            const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueWriteBuffer(
        object_, buffer(), blocking, offset, size, ptr, (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_WRITE_BUFFER_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  cl_int enqueueCopyBuffer(const Buffer &src, const Buffer &dst, size_type src_offset, size_type dst_offset,
                           size_type size, const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueCopyBuffer(
        object_, src(), dst(), src_offset, dst_offset, size, (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQEUE_COPY_BUFFER_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }
#if CL_HPP_TARGET_OPENCL_VERSION >= 110
  cl_int enqueueReadBufferRect(const Buffer &buffer, cl_bool blocking, const array<size_type, 3> &buffer_offset,
                               const array<size_type, 3> &host_offset, const array<size_type, 3> &region,
                               size_type buffer_row_pitch, size_type buffer_slice_pitch, size_type host_row_pitch,
                               size_type host_slice_pitch, void *ptr, const vector<Event> *events = NULL,
                               Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueReadBufferRect(
        object_, buffer(), blocking, buffer_offset.data(), host_offset.data(), region.data(), buffer_row_pitch,
        buffer_slice_pitch, host_row_pitch, host_slice_pitch, ptr, (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_READ_BUFFER_RECT_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  cl_int enqueueWriteBufferRect(const Buffer &buffer, cl_bool blocking, const array<size_type, 3> &buffer_offset,
                                const array<size_type, 3> &host_offset, const array<size_type, 3> &region,
                                size_type buffer_row_pitch, size_type buffer_slice_pitch, size_type host_row_pitch,
                                size_type host_slice_pitch, const void *ptr, const vector<Event> *events = NULL,
                                Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueWriteBufferRect(
        object_, buffer(), blocking, buffer_offset.data(), host_offset.data(), region.data(), buffer_row_pitch,
        buffer_slice_pitch, host_row_pitch, host_slice_pitch, ptr, (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_WRITE_BUFFER_RECT_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  cl_int enqueueCopyBufferRect(const Buffer &src, const Buffer &dst, const array<size_type, 3> &src_origin,
                               const array<size_type, 3> &dst_origin, const array<size_type, 3> &region,
                               size_type src_row_pitch, size_type src_slice_pitch, size_type dst_row_pitch,
                               size_type dst_slice_pitch, const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueCopyBufferRect(
        object_, src(), dst(), src_origin.data(), dst_origin.data(), region.data(), src_row_pitch, src_slice_pitch,
        dst_row_pitch, dst_slice_pitch, (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQEUE_COPY_BUFFER_RECT_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 110
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
  /**
   * Enqueue a command to fill a buffer object with a pattern
   * of a given size. The pattern is specified as a vector type.
   * \tparam PatternType The datatype of the pattern field.
   *     The pattern type must be an accepted OpenCL data type.
   * \tparam offset Is the offset in bytes into the buffer at
   *     which to start filling. This must be a multiple of
   *     the pattern size.
   * \tparam size Is the size in bytes of the region to fill.
   *     This must be a multiple of the pattern size.
   */
  template <typename PatternType>
  cl_int enqueueFillBuffer(const Buffer &buffer, PatternType pattern, size_type offset, size_type size,
                           const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueFillBuffer(object_, buffer(), static_cast<void *>(&pattern), sizeof(PatternType), offset, size,
                            (events != NULL) ? (cl_uint)events->size() : 0,
                            (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                            (event != NULL) ? &tmp : NULL),
      __ENQUEUE_FILL_BUFFER_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120

  cl_int enqueueReadImage(const Image &image, cl_bool blocking, const array<size_type, 3> &origin,
                          const array<size_type, 3> &region, size_type row_pitch, size_type slice_pitch, void *ptr,
                          const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueReadImage(object_, image(), blocking, origin.data(), region.data(), row_pitch, slice_pitch, ptr,
                           (events != NULL) ? (cl_uint)events->size() : 0,
                           (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                           (event != NULL) ? &tmp : NULL),
      __ENQUEUE_READ_IMAGE_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  cl_int enqueueWriteImage(const Image &image, cl_bool blocking, const array<size_type, 3> &origin,
                           const array<size_type, 3> &region, size_type row_pitch, size_type slice_pitch,
                           const void *ptr, const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueWriteImage(object_, image(), blocking, origin.data(), region.data(), row_pitch, slice_pitch, ptr,
                            (events != NULL) ? (cl_uint)events->size() : 0,
                            (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                            (event != NULL) ? &tmp : NULL),
      __ENQUEUE_WRITE_IMAGE_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  cl_int enqueueCopyImage(const Image &src, const Image &dst, const array<size_type, 3> &src_origin,
                          const array<size_type, 3> &dst_origin, const array<size_type, 3> &region,
                          const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueCopyImage(object_, src(), dst(), src_origin.data(), dst_origin.data(), region.data(),
                           (events != NULL) ? (cl_uint)events->size() : 0,
                           (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                           (event != NULL) ? &tmp : NULL),
      __ENQUEUE_COPY_IMAGE_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
  /**
   * Enqueue a command to fill an image object with a specified color.
   * \param fillColor is the color to use to fill the image.
   *     This is a four component RGBA floating-point color value if
   *     the image channel data type is not an unnormalized signed or
   *     unsigned data type.
   */
  cl_int enqueueFillImage(const Image &image, cl_float4 fillColor, const array<size_type, 3> &origin,
                          const array<size_type, 3> &region, const vector<Event> *events = NULL,
                          Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueFillImage(object_, image(), static_cast<void *>(&fillColor), origin.data(), region.data(),
                           (events != NULL) ? (cl_uint)events->size() : 0,
                           (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                           (event != NULL) ? &tmp : NULL),
      __ENQUEUE_FILL_IMAGE_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  /**
   * Enqueue a command to fill an image object with a specified color.
   * \param fillColor is the color to use to fill the image.
   *     This is a four component RGBA signed integer color value if
   *     the image channel data type is an unnormalized signed integer
   *     type.
   */
  cl_int enqueueFillImage(const Image &image, cl_int4 fillColor, const array<size_type, 3> &origin,
                          const array<size_type, 3> &region, const vector<Event> *events = NULL,
                          Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueFillImage(object_, image(), static_cast<void *>(&fillColor), origin.data(), region.data(),
                           (events != NULL) ? (cl_uint)events->size() : 0,
                           (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                           (event != NULL) ? &tmp : NULL),
      __ENQUEUE_FILL_IMAGE_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  /**
   * Enqueue a command to fill an image object with a specified color.
   * \param fillColor is the color to use to fill the image.
   *     This is a four component RGBA unsigned integer color value if
   *     the image channel data type is an unnormalized unsigned integer
   *     type.
   */
  cl_int enqueueFillImage(const Image &image, cl_uint4 fillColor, const array<size_type, 3> &origin,
                          const array<size_type, 3> &region, const vector<Event> *events = NULL,
                          Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueFillImage(object_, image(), static_cast<void *>(&fillColor), origin.data(), region.data(),
                           (events != NULL) ? (cl_uint)events->size() : 0,
                           (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                           (event != NULL) ? &tmp : NULL),
      __ENQUEUE_FILL_IMAGE_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120

  cl_int enqueueCopyImageToBuffer(const Image &src, const Buffer &dst, const array<size_type, 3> &src_origin,
                                  const array<size_type, 3> &region, size_type dst_offset,
                                  const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueCopyImageToBuffer(object_, src(), dst(), src_origin.data(), region.data(), dst_offset,
                                   (events != NULL) ? (cl_uint)events->size() : 0,
                                   (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                                   (event != NULL) ? &tmp : NULL),
      __ENQUEUE_COPY_IMAGE_TO_BUFFER_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  cl_int enqueueCopyBufferToImage(const Buffer &src, const Image &dst, size_type src_offset,
                                  const array<size_type, 3> &dst_origin, const array<size_type, 3> &region,
                                  const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueCopyBufferToImage(object_, src(), dst(), src_offset, dst_origin.data(), region.data(),
                                   (events != NULL) ? (cl_uint)events->size() : 0,
                                   (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                                   (event != NULL) ? &tmp : NULL),
      __ENQUEUE_COPY_BUFFER_TO_IMAGE_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  void *enqueueMapBuffer(const Buffer &buffer, cl_bool blocking, cl_map_flags flags, size_type offset, size_type size,
                         const vector<Event> *events = NULL, Event *event = NULL, cl_int *err = NULL) const
  {
    cl_event tmp;
    cl_int error;
    void *result = ::clEnqueueMapBuffer(object_, buffer(), blocking, flags, offset, size,
                                        (events != NULL) ? (cl_uint)events->size() : 0,
                                        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                                        (event != NULL) ? &tmp : NULL, &error);

    detail::errHandler(error, __ENQUEUE_MAP_BUFFER_ERR);
    if (err != NULL)
    {
      *err = error;
    }
    if (event != NULL && error == CL_SUCCESS)
      *event = tmp;

    return result;
  }

  void *enqueueMapImage(const Image &buffer, cl_bool blocking, cl_map_flags flags, const array<size_type, 3> &origin,
                        const array<size_type, 3> &region, size_type *row_pitch, size_type *slice_pitch,
                        const vector<Event> *events = NULL, Event *event = NULL, cl_int *err = NULL) const
  {
    cl_event tmp;
    cl_int error;
    void *result = ::clEnqueueMapImage(object_, buffer(), blocking, flags, origin.data(), region.data(), row_pitch,
                                       slice_pitch, (events != NULL) ? (cl_uint)events->size() : 0,
                                       (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                                       (event != NULL) ? &tmp : NULL, &error);

    detail::errHandler(error, __ENQUEUE_MAP_IMAGE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
    if (event != NULL && error == CL_SUCCESS)
      *event = tmp;
    return result;
  }

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
  /**
   * Enqueues a command that will allow the host to update a region of a coarse-grained SVM buffer.
   * This variant takes a raw SVM pointer.
   */
  template <typename T>
  cl_int enqueueMapSVM(T *ptr, cl_bool blocking, cl_map_flags flags, size_type size, const vector<Event> *events = NULL,
                       Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueSVMMap(
        object_, blocking, flags, static_cast<void *>(ptr), size, (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_MAP_BUFFER_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }


  /**
   * Enqueues a command that will allow the host to update a region of a coarse-grained SVM buffer.
   * This variant takes a cl::pointer instance.
   */
  template <typename T, class D>
  cl_int enqueueMapSVM(cl::pointer<T, D> &ptr, cl_bool blocking, cl_map_flags flags, size_type size,
                       const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueSVMMap(
        object_, blocking, flags, static_cast<void *>(ptr.get()), size, (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_MAP_BUFFER_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  /**
   * Enqueues a command that will allow the host to update a region of a coarse-grained SVM buffer.
   * This variant takes a cl::vector instance.
   */
  template <typename T, class Alloc>
  cl_int enqueueMapSVM(cl::vector<T, Alloc> &container, cl_bool blocking, cl_map_flags flags,
                       const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err =
      detail::errHandler(::clEnqueueSVMMap(object_, blocking, flags, static_cast<void *>(container.data()),
                                           container.size() * sizeof(T), (events != NULL) ? (cl_uint)events->size() : 0,
                                           (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                                           (event != NULL) ? &tmp : NULL),
                         __ENQUEUE_MAP_BUFFER_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200

  cl_int enqueueUnmapMemObject(const Memory &memory, void *mapped_ptr, const vector<Event> *events = NULL,
                               Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueUnmapMemObject(object_, memory(), mapped_ptr, (events != NULL) ? (cl_uint)events->size() : 0,
                                (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                                (event != NULL) ? &tmp : NULL),
      __ENQUEUE_UNMAP_MEM_OBJECT_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }


#if CL_HPP_TARGET_OPENCL_VERSION >= 200
  /**
   * Enqueues a command that will release a coarse-grained SVM buffer back to the OpenCL runtime.
   * This variant takes a raw SVM pointer.
   */
  template <typename T>
  cl_int enqueueUnmapSVM(T *ptr, const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueSVMUnmap(object_, static_cast<void *>(ptr), (events != NULL) ? (cl_uint)events->size() : 0,
                          (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                          (event != NULL) ? &tmp : NULL),
      __ENQUEUE_UNMAP_MEM_OBJECT_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  /**
   * Enqueues a command that will release a coarse-grained SVM buffer back to the OpenCL runtime.
   * This variant takes a cl::pointer instance.
   */
  template <typename T, class D>
  cl_int enqueueUnmapSVM(cl::pointer<T, D> &ptr, const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueSVMUnmap(object_, static_cast<void *>(ptr.get()), (events != NULL) ? (cl_uint)events->size() : 0,
                          (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                          (event != NULL) ? &tmp : NULL),
      __ENQUEUE_UNMAP_MEM_OBJECT_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  /**
   * Enqueues a command that will release a coarse-grained SVM buffer back to the OpenCL runtime.
   * This variant takes a cl::vector instance.
   */
  template <typename T, class Alloc>
  cl_int enqueueUnmapSVM(cl::vector<T, Alloc> &container, const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueSVMUnmap(
        object_, static_cast<void *>(container.data()), (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_UNMAP_MEM_OBJECT_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200

#if CL_HPP_TARGET_OPENCL_VERSION >= 120
  /**
   * Enqueues a marker command which waits for either a list of events to complete,
   * or all previously enqueued commands to complete.
   *
   * Enqueues a marker command which waits for either a list of events to complete,
   * or if the list is empty it waits for all commands previously enqueued in command_queue
   * to complete before it completes. This command returns an event which can be waited on,
   * i.e. this event can be waited on to insure that all events either in the event_wait_list
   * or all previously enqueued commands, queued before this command to command_queue,
   * have completed.
   */
  cl_int enqueueMarkerWithWaitList(const vector<Event> *events = 0, Event *event = 0) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueMarkerWithWaitList(object_, (events != NULL) ? (cl_uint)events->size() : 0,
                                    (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                                    (event != NULL) ? &tmp : NULL),
      __ENQUEUE_MARKER_WAIT_LIST_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  /**
   * A synchronization point that enqueues a barrier operation.
   *
   * Enqueues a barrier command which waits for either a list of events to complete,
   * or if the list is empty it waits for all commands previously enqueued in command_queue
   * to complete before it completes. This command blocks command execution, that is, any
   * following commands enqueued after it do not execute until it completes. This command
   * returns an event which can be waited on, i.e. this event can be waited on to insure that
   * all events either in the event_wait_list or all previously enqueued commands, queued
   * before this command to command_queue, have completed.
   */
  cl_int enqueueBarrierWithWaitList(const vector<Event> *events = 0, Event *event = 0) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueBarrierWithWaitList(object_, (events != NULL) ? (cl_uint)events->size() : 0,
                                     (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                                     (event != NULL) ? &tmp : NULL),
      __ENQUEUE_BARRIER_WAIT_LIST_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  /**
   * Enqueues a command to indicate with which device a set of memory objects
   * should be associated.
   */
  cl_int enqueueMigrateMemObjects(const vector<Memory> &memObjects, cl_mem_migration_flags flags,
                                  const vector<Event> *events = NULL, Event *event = NULL) const
  {
    cl_event tmp;

    vector<cl_mem> localMemObjects(memObjects.size());

    for (int i = 0; i < (int)memObjects.size(); ++i)
    {
      localMemObjects[i] = memObjects[i]();
    }

    cl_int err = detail::errHandler(
      ::clEnqueueMigrateMemObjects(object_, (cl_uint)memObjects.size(), localMemObjects.data(), flags,
                                   (events != NULL) ? (cl_uint)events->size() : 0,
                                   (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                                   (event != NULL) ? &tmp : NULL),
      __ENQUEUE_UNMAP_MEM_OBJECT_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120


#if CL_HPP_TARGET_OPENCL_VERSION >= 210
  /**
   * Enqueues a command that will allow the host associate ranges within a set of
   * SVM allocations with a device.
   * @param sizes - The length from each pointer to migrate.
   */
  template <typename T>
  cl_int enqueueMigrateSVM(const cl::vector<T *> &svmRawPointers, const cl::vector<size_type> &sizes,
                           cl_mem_migration_flags flags = 0, const vector<Event> *events = NULL,
                           Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueSVMMigrateMem(object_, svmRawPointers.size(), static_cast<void **>(svmRawPointers.data()),
                               sizes.data(),  // array of sizes not passed
                               flags, (events != NULL) ? (cl_uint)events->size() : 0,
                               (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                               (event != NULL) ? &tmp : NULL),
      __ENQUEUE_MIGRATE_SVM_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  /**
   * Enqueues a command that will allow the host associate a set of SVM allocations with
   * a device.
   */
  template <typename T>
  cl_int enqueueMigrateSVM(const cl::vector<T *> &svmRawPointers, cl_mem_migration_flags flags = 0,
                           const vector<Event> *events = NULL, Event *event = NULL) const
  {
    return enqueueMigrateSVM(svmRawPointers, cl::vector<size_type>(svmRawPointers.size()), flags, events, event);
  }


  /**
   * Enqueues a command that will allow the host associate ranges within a set of
   * SVM allocations with a device.
   * @param sizes - The length from each pointer to migrate.
   */
  template <typename T, class D>
  cl_int enqueueMigrateSVM(const cl::vector<cl::pointer<T, D>> &svmPointers, const cl::vector<size_type> &sizes,
                           cl_mem_migration_flags flags = 0, const vector<Event> *events = NULL,
                           Event *event = NULL) const
  {
    cl::vector<void *> svmRawPointers;
    svmRawPointers.reserve(svmPointers.size());
    for (auto p : svmPointers)
    {
      svmRawPointers.push_back(static_cast<void *>(p.get()));
    }

    return enqueueMigrateSVM(svmRawPointers, sizes, flags, events, event);
  }


  /**
   * Enqueues a command that will allow the host associate a set of SVM allocations with
   * a device.
   */
  template <typename T, class D>
  cl_int enqueueMigrateSVM(const cl::vector<cl::pointer<T, D>> &svmPointers, cl_mem_migration_flags flags = 0,
                           const vector<Event> *events = NULL, Event *event = NULL) const
  {
    return enqueueMigrateSVM(svmPointers, cl::vector<size_type>(svmPointers.size()), flags, events, event);
  }

  /**
   * Enqueues a command that will allow the host associate ranges within a set of
   * SVM allocations with a device.
   * @param sizes - The length from the beginning of each container to migrate.
   */
  template <typename T, class Alloc>
  cl_int enqueueMigrateSVM(const cl::vector<cl::vector<T, Alloc>> &svmContainers, const cl::vector<size_type> &sizes,
                           cl_mem_migration_flags flags = 0, const vector<Event> *events = NULL,
                           Event *event = NULL) const
  {
    cl::vector<void *> svmRawPointers;
    svmRawPointers.reserve(svmContainers.size());
    for (auto p : svmContainers)
    {
      svmRawPointers.push_back(static_cast<void *>(p.data()));
    }

    return enqueueMigrateSVM(svmRawPointers, sizes, flags, events, event);
  }

  /**
   * Enqueues a command that will allow the host associate a set of SVM allocations with
   * a device.
   */
  template <typename T, class Alloc>
  cl_int enqueueMigrateSVM(const cl::vector<cl::vector<T, Alloc>> &svmContainers, cl_mem_migration_flags flags = 0,
                           const vector<Event> *events = NULL, Event *event = NULL) const
  {
    return enqueueMigrateSVM(svmContainers, cl::vector<size_type>(svmContainers.size()), flags, events, event);
  }

#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 210

  cl_int enqueueNDRangeKernel(const Kernel &kernel, const NDRange &offset, const NDRange &global,
                              const NDRange &local = NullRange, const vector<Event> *events = NULL,
                              Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueNDRangeKernel(
        object_, kernel(), (cl_uint)global.dimensions(), offset.dimensions() != 0 ? (const size_type *)offset : NULL,
        (const size_type *)global, local.dimensions() != 0 ? (const size_type *)local : NULL,
        (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_NDRANGE_KERNEL_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

#if defined(CL_USE_DEPRECATED_OPENCL_1_2_APIS)
  CL_API_PREFIX__VERSION_1_2_DEPRECATED cl_int
    enqueueTask(const Kernel &kernel, const vector<Event> *events = NULL,
                Event *event = NULL) const CL_API_SUFFIX__VERSION_1_2_DEPRECATED
  {
    cl_event tmp;
    cl_int err =
      detail::errHandler(::clEnqueueTask(object_, kernel(), (events != NULL) ? (cl_uint)events->size() : 0,
                                         (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                                         (event != NULL) ? &tmp : NULL),
                         __ENQUEUE_TASK_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }
#endif  // #if defined(CL_USE_DEPRECATED_OPENCL_1_2_APIS)

  cl_int enqueueNativeKernel(void(CL_CALLBACK *userFptr)(void *), std::pair<void *, size_type> args,
                             const vector<Memory> *mem_objects = NULL, const vector<const void *> *mem_locs = NULL,
                             const vector<Event> *events = NULL, Event *event = NULL) const
  {
    size_type elements = 0;
    if (mem_objects != NULL)
    {
      elements = mem_objects->size();
    }
    vector<cl_mem> mems(elements);
    for (unsigned int i = 0; i < elements; i++)
    {
      mems[i] = ((*mem_objects)[i])();
    }

    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueNativeKernel(
        object_, userFptr, args.first, args.second, (mem_objects != NULL) ? (cl_uint)mem_objects->size() : 0,
        mems.data(), (mem_locs != NULL && mem_locs->size() > 0) ? (const void **)&mem_locs->front() : NULL,
        (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_NATIVE_KERNEL);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

/**
 * Deprecated APIs for 1.2
 */
#if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS)
  CL_API_PREFIX__VERSION_1_1_DEPRECATED
  cl_int enqueueMarker(Event *event = NULL) const CL_API_SUFFIX__VERSION_1_1_DEPRECATED
  {
    cl_event tmp;
    cl_int err = detail::errHandler(::clEnqueueMarker(object_, (event != NULL) ? &tmp : NULL), __ENQUEUE_MARKER_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  CL_API_PREFIX__VERSION_1_1_DEPRECATED
  cl_int enqueueWaitForEvents(const vector<Event> &events) const CL_API_SUFFIX__VERSION_1_1_DEPRECATED
  {
    return detail::errHandler(::clEnqueueWaitForEvents(object_, (cl_uint)events.size(),
                                                       events.size() > 0 ? (const cl_event *)&events.front() : NULL),
                              __ENQUEUE_WAIT_FOR_EVENTS_ERR);
  }
#endif  // defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS)

  cl_int enqueueAcquireGLObjects(const vector<Memory> *mem_objects = NULL, const vector<Event> *events = NULL,
                                 Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueAcquireGLObjects(
        object_, (mem_objects != NULL) ? (cl_uint)mem_objects->size() : 0,
        (mem_objects != NULL && mem_objects->size() > 0) ? (const cl_mem *)&mem_objects->front() : NULL,
        (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_ACQUIRE_GL_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  cl_int enqueueReleaseGLObjects(const vector<Memory> *mem_objects = NULL, const vector<Event> *events = NULL,
                                 Event *event = NULL) const
  {
    cl_event tmp;
    cl_int err = detail::errHandler(
      ::clEnqueueReleaseGLObjects(
        object_, (mem_objects != NULL) ? (cl_uint)mem_objects->size() : 0,
        (mem_objects != NULL && mem_objects->size() > 0) ? (const cl_mem *)&mem_objects->front() : NULL,
        (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_RELEASE_GL_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

#if defined(CL_HPP_USE_DX_INTEROP)
  typedef CL_API_ENTRY cl_int(CL_API_CALL *PFN_clEnqueueAcquireD3D10ObjectsKHR)(
    cl_command_queue command_queue, cl_uint num_objects, const cl_mem *mem_objects, cl_uint num_events_in_wait_list,
    const cl_event *event_wait_list, cl_event *event);
  typedef CL_API_ENTRY cl_int(CL_API_CALL *PFN_clEnqueueReleaseD3D10ObjectsKHR)(
    cl_command_queue command_queue, cl_uint num_objects, const cl_mem *mem_objects, cl_uint num_events_in_wait_list,
    const cl_event *event_wait_list, cl_event *event);

  cl_int enqueueAcquireD3D10Objects(const vector<Memory> *mem_objects = NULL, const vector<Event> *events = NULL,
                                    Event *event = NULL) const
  {
    static PFN_clEnqueueAcquireD3D10ObjectsKHR pfn_clEnqueueAcquireD3D10ObjectsKHR = NULL;
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
    cl_context context = getInfo<CL_QUEUE_CONTEXT>();
    cl::Device device(getInfo<CL_QUEUE_DEVICE>());
    cl_platform_id platform = device.getInfo<CL_DEVICE_PLATFORM>();
    CL_HPP_INIT_CL_EXT_FCN_PTR_PLATFORM_(platform, clEnqueueAcquireD3D10ObjectsKHR);
#endif
#if CL_HPP_TARGET_OPENCL_VERSION >= 110
    CL_HPP_INIT_CL_EXT_FCN_PTR_(clEnqueueAcquireD3D10ObjectsKHR);
#endif

    cl_event tmp;
    cl_int err = detail::errHandler(
      pfn_clEnqueueAcquireD3D10ObjectsKHR(
        object_, (mem_objects != NULL) ? (cl_uint)mem_objects->size() : 0,
        (mem_objects != NULL && mem_objects->size() > 0) ? (const cl_mem *)&mem_objects->front() : NULL,
        (events != NULL) ? (cl_uint)events->size() : 0, (events != NULL) ? (cl_event *)&events->front() : NULL,
        (event != NULL) ? &tmp : NULL),
      __ENQUEUE_ACQUIRE_GL_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }

  cl_int enqueueReleaseD3D10Objects(const vector<Memory> *mem_objects = NULL, const vector<Event> *events = NULL,
                                    Event *event = NULL) const
  {
    static PFN_clEnqueueReleaseD3D10ObjectsKHR pfn_clEnqueueReleaseD3D10ObjectsKHR = NULL;
#if CL_HPP_TARGET_OPENCL_VERSION >= 120
    cl_context context = getInfo<CL_QUEUE_CONTEXT>();
    cl::Device device(getInfo<CL_QUEUE_DEVICE>());
    cl_platform_id platform = device.getInfo<CL_DEVICE_PLATFORM>();
    CL_HPP_INIT_CL_EXT_FCN_PTR_PLATFORM_(platform, clEnqueueReleaseD3D10ObjectsKHR);
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 120
#if CL_HPP_TARGET_OPENCL_VERSION >= 110
    CL_HPP_INIT_CL_EXT_FCN_PTR_(clEnqueueReleaseD3D10ObjectsKHR);
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 110

    cl_event tmp;
    cl_int err = detail::errHandler(
      pfn_clEnqueueReleaseD3D10ObjectsKHR(
        object_, (mem_objects != NULL) ? (cl_uint)mem_objects->size() : 0,
        (mem_objects != NULL && mem_objects->size() > 0) ? (const cl_mem *)&mem_objects->front() : NULL,
        (events != NULL) ? (cl_uint)events->size() : 0,
        (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (event != NULL) ? &tmp : NULL),
      __ENQUEUE_RELEASE_GL_ERR);

    if (event != NULL && err == CL_SUCCESS)
      *event = tmp;

    return err;
  }
#endif

/**
 * Deprecated APIs for 1.2
 */
#if defined(CL_USE_DEPRECATED_OPENCL_1_1_APIS)
  CL_API_PREFIX__VERSION_1_1_DEPRECATED
  cl_int enqueueBarrier() const CL_API_SUFFIX__VERSION_1_1_DEPRECATED
  {
    return detail::errHandler(::clEnqueueBarrier(object_), __ENQUEUE_BARRIER_ERR);
  }
#endif  // CL_USE_DEPRECATED_OPENCL_1_1_APIS

  cl_int flush() const { return detail::errHandler(::clFlush(object_), __FLUSH_ERR); }

  cl_int finish() const { return detail::errHandler(::clFinish(object_), __FINISH_ERR); }
};  // CommandQueue

CL_HPP_DEFINE_STATIC_MEMBER_ std::once_flag CommandQueue::default_initialized_;
CL_HPP_DEFINE_STATIC_MEMBER_ CommandQueue CommandQueue::default_;
CL_HPP_DEFINE_STATIC_MEMBER_ cl_int CommandQueue::default_error_ = CL_SUCCESS;


#if CL_HPP_TARGET_OPENCL_VERSION >= 200
enum class DeviceQueueProperties : cl_command_queue_properties
{
  None = 0,
  Profiling = CL_QUEUE_PROFILING_ENABLE,
};

inline DeviceQueueProperties operator|(DeviceQueueProperties lhs, DeviceQueueProperties rhs)
{
  return static_cast<DeviceQueueProperties>(static_cast<cl_command_queue_properties>(lhs) |
                                            static_cast<cl_command_queue_properties>(rhs));
}

/*! \class DeviceCommandQueue
 * \brief DeviceCommandQueue interface for device cl_command_queues.
 */
class DeviceCommandQueue : public detail::Wrapper<cl_command_queue>
{
public:
  /*!
   * Trivial empty constructor to create a null queue.
   */
  DeviceCommandQueue() {}

  /*!
   * Default construct device command queue on default context and device
   */
  DeviceCommandQueue(DeviceQueueProperties properties, cl_int *err = NULL)
  {
    cl_int error;
    cl::Context context = cl::Context::getDefault();
    cl::Device device = cl::Device::getDefault();

    cl_command_queue_properties mergedProperties = CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE | CL_QUEUE_ON_DEVICE |
                                                   static_cast<cl_command_queue_properties>(properties);

    cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, mergedProperties, 0 };
    object_ = ::clCreateCommandQueueWithProperties(context(), device(), queue_properties, &error);

    detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  /*!
   * Create a device command queue for a specified device in the passed context.
   */
  DeviceCommandQueue(const Context &context, const Device &device,
                     DeviceQueueProperties properties = DeviceQueueProperties::None, cl_int *err = NULL)
  {
    cl_int error;

    cl_command_queue_properties mergedProperties = CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE | CL_QUEUE_ON_DEVICE |
                                                   static_cast<cl_command_queue_properties>(properties);
    cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, mergedProperties, 0 };
    object_ = ::clCreateCommandQueueWithProperties(context(), device(), queue_properties, &error);

    detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  /*!
   * Create a device command queue for a specified device in the passed context.
   */
  DeviceCommandQueue(const Context &context, const Device &device, cl_uint queueSize,
                     DeviceQueueProperties properties = DeviceQueueProperties::None, cl_int *err = NULL)
  {
    cl_int error;

    cl_command_queue_properties mergedProperties = CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE | CL_QUEUE_ON_DEVICE |
                                                   static_cast<cl_command_queue_properties>(properties);
    cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, mergedProperties, CL_QUEUE_SIZE, queueSize, 0 };
    object_ = ::clCreateCommandQueueWithProperties(context(), device(), queue_properties, &error);

    detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }

  /*! \brief Constructor from cl_command_queue - takes ownership.
   *
   * \param retainObject will cause the constructor to retain its cl object.
   *                     Defaults to false to maintain compatibility with
   *                     earlier versions.
   */
  explicit DeviceCommandQueue(const cl_command_queue &commandQueue, bool retainObject = false)
    : detail::Wrapper<cl_type>(commandQueue, retainObject)
  {}

  DeviceCommandQueue &operator=(const cl_command_queue &rhs)
  {
    detail::Wrapper<cl_type>::operator=(rhs);
    return *this;
  }

  /*! \brief Copy constructor to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  DeviceCommandQueue(const DeviceCommandQueue &queue)
    : detail::Wrapper<cl_type>(queue)
  {}

  /*! \brief Copy assignment to forward copy to the superclass correctly.
   * Required for MSVC.
   */
  DeviceCommandQueue &operator=(const DeviceCommandQueue &queue)
  {
    detail::Wrapper<cl_type>::operator=(queue);
    return *this;
  }

  /*! \brief Move constructor to forward move to the superclass correctly.
   * Required for MSVC.
   */
  DeviceCommandQueue(DeviceCommandQueue &&queue) CL_HPP_NOEXCEPT_ : detail::Wrapper<cl_type>(std::move(queue)) {}

  /*! \brief Move assignment to forward move to the superclass correctly.
   * Required for MSVC.
   */
  DeviceCommandQueue &operator=(DeviceCommandQueue &&queue)
  {
    detail::Wrapper<cl_type>::operator=(std::move(queue));
    return *this;
  }

  template <typename T>
  cl_int getInfo(cl_command_queue_info name, T *param) const
  {
    return detail::errHandler(detail::getInfo(&::clGetCommandQueueInfo, object_, name, param),
                              __GET_COMMAND_QUEUE_INFO_ERR);
  }

  template <cl_command_queue_info name>
  typename detail::param_traits<detail::cl_command_queue_info, name>::param_type getInfo(cl_int *err = NULL) const
  {
    typename detail::param_traits<detail::cl_command_queue_info, name>::param_type param;
    cl_int result = getInfo(name, &param);
    if (err != NULL)
    {
      *err = result;
    }
    return param;
  }

  /*!
   * Create a new default device command queue for the default device,
   * in the default context and of the default size.
   * If there is already a default queue for the specified device this
   * function will return the pre-existing queue.
   */
  static DeviceCommandQueue makeDefault(cl_int *err = nullptr)
  {
    cl_int error;
    cl::Context context = cl::Context::getDefault();
    cl::Device device = cl::Device::getDefault();

    cl_command_queue_properties properties =
      CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE | CL_QUEUE_ON_DEVICE | CL_QUEUE_ON_DEVICE_DEFAULT;
    cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, properties, 0 };
    DeviceCommandQueue deviceQueue(::clCreateCommandQueueWithProperties(context(), device(), queue_properties, &error));

    detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
    if (err != NULL)
    {
      *err = error;
    }

    return deviceQueue;
  }

  /*!
   * Create a new default device command queue for the specified device
   * and of the default size.
   * If there is already a default queue for the specified device this
   * function will return the pre-existing queue.
   */
  static DeviceCommandQueue makeDefault(const Context &context, const Device &device, cl_int *err = nullptr)
  {
    cl_int error;

    cl_command_queue_properties properties =
      CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE | CL_QUEUE_ON_DEVICE | CL_QUEUE_ON_DEVICE_DEFAULT;
    cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, properties, 0 };
    DeviceCommandQueue deviceQueue(::clCreateCommandQueueWithProperties(context(), device(), queue_properties, &error));

    detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
    if (err != NULL)
    {
      *err = error;
    }

    return deviceQueue;
  }

  /*!
   * Create a new default device command queue for the specified device
   * and of the requested size in bytes.
   * If there is already a default queue for the specified device this
   * function will return the pre-existing queue.
   */
  static DeviceCommandQueue makeDefault(const Context &context, const Device &device, cl_uint queueSize,
                                        cl_int *err = nullptr)
  {
    cl_int error;

    cl_command_queue_properties properties =
      CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE | CL_QUEUE_ON_DEVICE | CL_QUEUE_ON_DEVICE_DEFAULT;
    cl_queue_properties queue_properties[] = { CL_QUEUE_PROPERTIES, properties, CL_QUEUE_SIZE, queueSize, 0 };
    DeviceCommandQueue deviceQueue(::clCreateCommandQueueWithProperties(context(), device(), queue_properties, &error));

    detail::errHandler(error, __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR);
    if (err != NULL)
    {
      *err = error;
    }

    return deviceQueue;
  }


#if CL_HPP_TARGET_OPENCL_VERSION >= 210
  /*!
   * Modify the default device command queue to be used for subsequent kernels.
   * This can update the default command queue for a device repeatedly to account
   * for kernels that rely on the default.
   * @return updated default device command queue.
   */
  static DeviceCommandQueue updateDefault(const Context &context, const Device &device,
                                          const DeviceCommandQueue &default_queue, cl_int *err = nullptr)
  {
    cl_int error;
    error = clSetDefaultDeviceCommandQueue(context.get(), device.get(), default_queue.get());

    detail::errHandler(error, __SET_DEFAULT_DEVICE_COMMAND_QUEUE_ERR);
    if (err != NULL)
    {
      *err = error;
    }
    return default_queue;
  }

  /*!
   * Return the current default command queue for the specified command queue
   */
  static DeviceCommandQueue getDefault(const CommandQueue &queue, cl_int *err = NULL)
  {
    return queue.getInfo<CL_QUEUE_DEVICE_DEFAULT>(err);
  }

#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 210
};      // DeviceCommandQueue

namespace detail
{
// Specialization for device command queue
template <>
struct KernelArgumentHandler<cl::DeviceCommandQueue, void>
{
  static size_type size(const cl::DeviceCommandQueue &) { return sizeof(cl_command_queue); }
  static const cl_command_queue *ptr(const cl::DeviceCommandQueue &value) { return &(value()); }
};
}  // namespace detail

#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200


template <typename IteratorType>
Buffer::Buffer(const Context &context, IteratorType startIterator, IteratorType endIterator, bool readOnly,
               bool useHostPtr, cl_int *err)
{
  typedef typename std::iterator_traits<IteratorType>::value_type DataType;
  cl_int error;

  cl_mem_flags flags = 0;
  if (readOnly)
  {
    flags |= CL_MEM_READ_ONLY;
  }
  else
  {
    flags |= CL_MEM_READ_WRITE;
  }
  if (useHostPtr)
  {
    flags |= CL_MEM_USE_HOST_PTR;
  }

  size_type size = sizeof(DataType) * (endIterator - startIterator);

  if (useHostPtr)
  {
    object_ = ::clCreateBuffer(context(), flags, size, static_cast<DataType *>(&*startIterator), &error);
  }
  else
  {
    object_ = ::clCreateBuffer(context(), flags, size, 0, &error);
  }

  detail::errHandler(error, __CREATE_BUFFER_ERR);
  if (err != NULL)
  {
    *err = error;
  }

  if (!useHostPtr)
  {
    CommandQueue queue(context, 0, &error);
    detail::errHandler(error, __CREATE_BUFFER_ERR);
    if (err != NULL)
    {
      *err = error;
    }

    error = cl::copy(queue, startIterator, endIterator, *this);
    detail::errHandler(error, __CREATE_BUFFER_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }
}

template <typename IteratorType>
Buffer::Buffer(const CommandQueue &queue, IteratorType startIterator, IteratorType endIterator, bool readOnly,
               bool useHostPtr, cl_int *err)
{
  typedef typename std::iterator_traits<IteratorType>::value_type DataType;
  cl_int error;

  cl_mem_flags flags = 0;
  if (readOnly)
  {
    flags |= CL_MEM_READ_ONLY;
  }
  else
  {
    flags |= CL_MEM_READ_WRITE;
  }
  if (useHostPtr)
  {
    flags |= CL_MEM_USE_HOST_PTR;
  }

  size_type size = sizeof(DataType) * (endIterator - startIterator);

  Context context = queue.getInfo<CL_QUEUE_CONTEXT>();

  if (useHostPtr)
  {
    object_ = ::clCreateBuffer(context(), flags, size, static_cast<DataType *>(&*startIterator), &error);
  }
  else
  {
    object_ = ::clCreateBuffer(context(), flags, size, 0, &error);
  }

  detail::errHandler(error, __CREATE_BUFFER_ERR);
  if (err != NULL)
  {
    *err = error;
  }

  if (!useHostPtr)
  {
    error = cl::copy(queue, startIterator, endIterator, *this);
    detail::errHandler(error, __CREATE_BUFFER_ERR);
    if (err != NULL)
    {
      *err = error;
    }
  }
}

inline cl_int enqueueReadBuffer(const Buffer &buffer, cl_bool blocking, size_type offset, size_type size, void *ptr,
                                const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.enqueueReadBuffer(buffer, blocking, offset, size, ptr, events, event);
}

inline cl_int enqueueWriteBuffer(const Buffer &buffer, cl_bool blocking, size_type offset, size_type size,
                                 const void *ptr, const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.enqueueWriteBuffer(buffer, blocking, offset, size, ptr, events, event);
}

inline void *enqueueMapBuffer(const Buffer &buffer, cl_bool blocking, cl_map_flags flags, size_type offset,
                              size_type size, const vector<Event> *events = NULL, Event *event = NULL,
                              cl_int *err = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);
  detail::errHandler(error, __ENQUEUE_MAP_BUFFER_ERR);
  if (err != NULL)
  {
    *err = error;
  }

  void *result = ::clEnqueueMapBuffer(
    queue(), buffer(), blocking, flags, offset, size, (events != NULL) ? (cl_uint)events->size() : 0,
    (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL, (cl_event *)event, &error);

  detail::errHandler(error, __ENQUEUE_MAP_BUFFER_ERR);
  if (err != NULL)
  {
    *err = error;
  }
  return result;
}


#if CL_HPP_TARGET_OPENCL_VERSION >= 200
/**
 * Enqueues to the default queue a command that will allow the host to
 * update a region of a coarse-grained SVM buffer.
 * This variant takes a raw SVM pointer.
 */
template <typename T>
inline cl_int enqueueMapSVM(T *ptr, cl_bool blocking, cl_map_flags flags, size_type size, const vector<Event> *events,
                            Event *event)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);
  if (error != CL_SUCCESS)
  {
    return detail::errHandler(error, __ENQUEUE_MAP_BUFFER_ERR);
  }

  return queue.enqueueMapSVM(ptr, blocking, flags, size, events, event);
}

/**
 * Enqueues to the default queue a command that will allow the host to
 * update a region of a coarse-grained SVM buffer.
 * This variant takes a cl::pointer instance.
 */
template <typename T, class D>
inline cl_int enqueueMapSVM(cl::pointer<T, D> &ptr, cl_bool blocking, cl_map_flags flags, size_type size,
                            const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);
  if (error != CL_SUCCESS)
  {
    return detail::errHandler(error, __ENQUEUE_MAP_BUFFER_ERR);
  }

  return queue.enqueueMapSVM(ptr, blocking, flags, size, events, event);
}

/**
 * Enqueues to the default queue a command that will allow the host to
 * update a region of a coarse-grained SVM buffer.
 * This variant takes a cl::vector instance.
 */
template <typename T, class Alloc>
inline cl_int enqueueMapSVM(cl::vector<T, Alloc> &container, cl_bool blocking, cl_map_flags flags,
                            const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);
  if (error != CL_SUCCESS)
  {
    return detail::errHandler(error, __ENQUEUE_MAP_BUFFER_ERR);
  }

  return queue.enqueueMapSVM(container, blocking, flags, events, event);
}

#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200

inline cl_int enqueueUnmapMemObject(const Memory &memory, void *mapped_ptr, const vector<Event> *events = NULL,
                                    Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);
  detail::errHandler(error, __ENQUEUE_MAP_BUFFER_ERR);
  if (error != CL_SUCCESS)
  {
    return error;
  }

  cl_event tmp;
  cl_int err = detail::errHandler(
    ::clEnqueueUnmapMemObject(queue(), memory(), mapped_ptr, (events != NULL) ? (cl_uint)events->size() : 0,
                              (events != NULL && events->size() > 0) ? (cl_event *)&events->front() : NULL,
                              (event != NULL) ? &tmp : NULL),
    __ENQUEUE_UNMAP_MEM_OBJECT_ERR);

  if (event != NULL && err == CL_SUCCESS)
    *event = tmp;

  return err;
}

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
/**
 * Enqueues to the default queue a command that will release a coarse-grained
 * SVM buffer back to the OpenCL runtime.
 * This variant takes a raw SVM pointer.
 */
template <typename T>
inline cl_int enqueueUnmapSVM(T *ptr, const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);
  if (error != CL_SUCCESS)
  {
    return detail::errHandler(error, __ENQUEUE_UNMAP_MEM_OBJECT_ERR);
  }

  return detail::errHandler(queue.enqueueUnmapSVM(ptr, events, event), __ENQUEUE_UNMAP_MEM_OBJECT_ERR);
}

/**
 * Enqueues to the default queue a command that will release a coarse-grained
 * SVM buffer back to the OpenCL runtime.
 * This variant takes a cl::pointer instance.
 */
template <typename T, class D>
inline cl_int enqueueUnmapSVM(cl::pointer<T, D> &ptr, const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);
  if (error != CL_SUCCESS)
  {
    return detail::errHandler(error, __ENQUEUE_UNMAP_MEM_OBJECT_ERR);
  }

  return detail::errHandler(queue.enqueueUnmapSVM(ptr, events, event), __ENQUEUE_UNMAP_MEM_OBJECT_ERR);
}

/**
 * Enqueues to the default queue a command that will release a coarse-grained
 * SVM buffer back to the OpenCL runtime.
 * This variant takes a cl::vector instance.
 */
template <typename T, class Alloc>
inline cl_int enqueueUnmapSVM(cl::vector<T, Alloc> &container, const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);
  if (error != CL_SUCCESS)
  {
    return detail::errHandler(error, __ENQUEUE_UNMAP_MEM_OBJECT_ERR);
  }

  return detail::errHandler(queue.enqueueUnmapSVM(container, events, event), __ENQUEUE_UNMAP_MEM_OBJECT_ERR);
}

#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200

inline cl_int enqueueCopyBuffer(const Buffer &src, const Buffer &dst, size_type src_offset, size_type dst_offset,
                                size_type size, const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.enqueueCopyBuffer(src, dst, src_offset, dst_offset, size, events, event);
}

/**
 * Blocking copy operation between iterators and a buffer.
 * Host to Device.
 * Uses default command queue.
 */
template <typename IteratorType>
inline cl_int copy(IteratorType startIterator, IteratorType endIterator, cl::Buffer &buffer)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);
  if (error != CL_SUCCESS)
    return error;

  return cl::copy(queue, startIterator, endIterator, buffer);
}

/**
 * Blocking copy operation between iterators and a buffer.
 * Device to Host.
 * Uses default command queue.
 */
template <typename IteratorType>
inline cl_int copy(const cl::Buffer &buffer, IteratorType startIterator, IteratorType endIterator)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);
  if (error != CL_SUCCESS)
    return error;

  return cl::copy(queue, buffer, startIterator, endIterator);
}

/**
 * Blocking copy operation between iterators and a buffer.
 * Host to Device.
 * Uses specified queue.
 */
template <typename IteratorType>
inline cl_int copy(const CommandQueue &queue, IteratorType startIterator, IteratorType endIterator, cl::Buffer &buffer)
{
  typedef typename std::iterator_traits<IteratorType>::value_type DataType;
  cl_int error;

  size_type length = endIterator - startIterator;
  size_type byteLength = length * sizeof(DataType);

  DataType *pointer =
    static_cast<DataType *>(queue.enqueueMapBuffer(buffer, CL_TRUE, CL_MAP_WRITE, 0, byteLength, 0, 0, &error));
  // if exceptions enabled, enqueueMapBuffer will throw
  if (error != CL_SUCCESS)
  {
    return error;
  }
#if defined(_MSC_VER)
  std::copy(startIterator, endIterator, stdext::checked_array_iterator<DataType *>(pointer, length));
#else
  std::copy(startIterator, endIterator, pointer);
#endif
  Event endEvent;
  error = queue.enqueueUnmapMemObject(buffer, pointer, 0, &endEvent);
  // if exceptions enabled, enqueueUnmapMemObject will throw
  if (error != CL_SUCCESS)
  {
    return error;
  }
  endEvent.wait();
  return CL_SUCCESS;
}

/**
 * Blocking copy operation between iterators and a buffer.
 * Device to Host.
 * Uses specified queue.
 */
template <typename IteratorType>
inline cl_int copy(const CommandQueue &queue, const cl::Buffer &buffer, IteratorType startIterator,
                   IteratorType endIterator)
{
  typedef typename std::iterator_traits<IteratorType>::value_type DataType;
  cl_int error;

  size_type length = endIterator - startIterator;
  size_type byteLength = length * sizeof(DataType);

  DataType *pointer =
    static_cast<DataType *>(queue.enqueueMapBuffer(buffer, CL_TRUE, CL_MAP_READ, 0, byteLength, 0, 0, &error));
  // if exceptions enabled, enqueueMapBuffer will throw
  if (error != CL_SUCCESS)
  {
    return error;
  }
  std::copy(pointer, pointer + length, startIterator);
  Event endEvent;
  error = queue.enqueueUnmapMemObject(buffer, pointer, 0, &endEvent);
  // if exceptions enabled, enqueueUnmapMemObject will throw
  if (error != CL_SUCCESS)
  {
    return error;
  }
  endEvent.wait();
  return CL_SUCCESS;
}


#if CL_HPP_TARGET_OPENCL_VERSION >= 200
/**
 * Blocking SVM map operation - performs a blocking map underneath.
 */
template <typename T, class Alloc>
inline cl_int mapSVM(cl::vector<T, Alloc> &container)
{
  return enqueueMapSVM(container, CL_TRUE, CL_MAP_READ | CL_MAP_WRITE);
}

/**
 * Blocking SVM map operation - performs a blocking map underneath.
 */
template <typename T, class Alloc>
inline cl_int unmapSVM(cl::vector<T, Alloc> &container)
{
  return enqueueUnmapSVM(container);
}

#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200

#if CL_HPP_TARGET_OPENCL_VERSION >= 110
inline cl_int enqueueReadBufferRect(const Buffer &buffer, cl_bool blocking, const array<size_type, 3> &buffer_offset,
                                    const array<size_type, 3> &host_offset, const array<size_type, 3> &region,
                                    size_type buffer_row_pitch, size_type buffer_slice_pitch, size_type host_row_pitch,
                                    size_type host_slice_pitch, void *ptr, const vector<Event> *events = NULL,
                                    Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.enqueueReadBufferRect(buffer, blocking, buffer_offset, host_offset, region, buffer_row_pitch,
                                     buffer_slice_pitch, host_row_pitch, host_slice_pitch, ptr, events, event);
}

inline cl_int enqueueWriteBufferRect(const Buffer &buffer, cl_bool blocking, const array<size_type, 3> &buffer_offset,
                                     const array<size_type, 3> &host_offset, const array<size_type, 3> &region,
                                     size_type buffer_row_pitch, size_type buffer_slice_pitch, size_type host_row_pitch,
                                     size_type host_slice_pitch, const void *ptr, const vector<Event> *events = NULL,
                                     Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.enqueueWriteBufferRect(buffer, blocking, buffer_offset, host_offset, region, buffer_row_pitch,
                                      buffer_slice_pitch, host_row_pitch, host_slice_pitch, ptr, events, event);
}

inline cl_int enqueueCopyBufferRect(const Buffer &src, const Buffer &dst, const array<size_type, 3> &src_origin,
                                    const array<size_type, 3> &dst_origin, const array<size_type, 3> &region,
                                    size_type src_row_pitch, size_type src_slice_pitch, size_type dst_row_pitch,
                                    size_type dst_slice_pitch, const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.enqueueCopyBufferRect(src, dst, src_origin, dst_origin, region, src_row_pitch, src_slice_pitch,
                                     dst_row_pitch, dst_slice_pitch, events, event);
}
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 110

inline cl_int enqueueReadImage(const Image &image, cl_bool blocking, const array<size_type, 3> &origin,
                               const array<size_type, 3> &region, size_type row_pitch, size_type slice_pitch, void *ptr,
                               const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.enqueueReadImage(image, blocking, origin, region, row_pitch, slice_pitch, ptr, events, event);
}

inline cl_int enqueueWriteImage(const Image &image, cl_bool blocking, const array<size_type, 3> &origin,
                                const array<size_type, 3> &region, size_type row_pitch, size_type slice_pitch,
                                const void *ptr, const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.enqueueWriteImage(image, blocking, origin, region, row_pitch, slice_pitch, ptr, events, event);
}

inline cl_int enqueueCopyImage(const Image &src, const Image &dst, const array<size_type, 3> &src_origin,
                               const array<size_type, 3> &dst_origin, const array<size_type, 3> &region,
                               const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.enqueueCopyImage(src, dst, src_origin, dst_origin, region, events, event);
}

inline cl_int enqueueCopyImageToBuffer(const Image &src, const Buffer &dst, const array<size_type, 3> &src_origin,
                                       const array<size_type, 3> &region, size_type dst_offset,
                                       const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.enqueueCopyImageToBuffer(src, dst, src_origin, region, dst_offset, events, event);
}

inline cl_int enqueueCopyBufferToImage(const Buffer &src, const Image &dst, size_type src_offset,
                                       const array<size_type, 3> &dst_origin, const array<size_type, 3> &region,
                                       const vector<Event> *events = NULL, Event *event = NULL)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.enqueueCopyBufferToImage(src, dst, src_offset, dst_origin, region, events, event);
}


inline cl_int flush(void)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }

  return queue.flush();
}

inline cl_int finish(void)
{
  cl_int error;
  CommandQueue queue = CommandQueue::getDefault(&error);

  if (error != CL_SUCCESS)
  {
    return error;
  }


  return queue.finish();
}

class EnqueueArgs
{
private:
  CommandQueue queue_;
  const NDRange offset_;
  const NDRange global_;
  const NDRange local_;
  vector<Event> events_;

  template <typename... Ts>
  friend class KernelFunctor;

public:
  EnqueueArgs(NDRange global)
    : queue_(CommandQueue::getDefault())
    , offset_(NullRange)
    , global_(global)
    , local_(NullRange)
  {}

  EnqueueArgs(NDRange global, NDRange local)
    : queue_(CommandQueue::getDefault())
    , offset_(NullRange)
    , global_(global)
    , local_(local)
  {}

  EnqueueArgs(NDRange offset, NDRange global, NDRange local)
    : queue_(CommandQueue::getDefault())
    , offset_(offset)
    , global_(global)
    , local_(local)
  {}

  EnqueueArgs(Event e, NDRange global)
    : queue_(CommandQueue::getDefault())
    , offset_(NullRange)
    , global_(global)
    , local_(NullRange)
  {
    events_.push_back(e);
  }

  EnqueueArgs(Event e, NDRange global, NDRange local)
    : queue_(CommandQueue::getDefault())
    , offset_(NullRange)
    , global_(global)
    , local_(local)
  {
    events_.push_back(e);
  }

  EnqueueArgs(Event e, NDRange offset, NDRange global, NDRange local)
    : queue_(CommandQueue::getDefault())
    , offset_(offset)
    , global_(global)
    , local_(local)
  {
    events_.push_back(e);
  }

  EnqueueArgs(const vector<Event> &events, NDRange global)
    : queue_(CommandQueue::getDefault())
    , offset_(NullRange)
    , global_(global)
    , local_(NullRange)
    , events_(events)
  {}

  EnqueueArgs(const vector<Event> &events, NDRange global, NDRange local)
    : queue_(CommandQueue::getDefault())
    , offset_(NullRange)
    , global_(global)
    , local_(local)
    , events_(events)
  {}

  EnqueueArgs(const vector<Event> &events, NDRange offset, NDRange global, NDRange local)
    : queue_(CommandQueue::getDefault())
    , offset_(offset)
    , global_(global)
    , local_(local)
    , events_(events)
  {}

  EnqueueArgs(CommandQueue &queue, NDRange global)
    : queue_(queue)
    , offset_(NullRange)
    , global_(global)
    , local_(NullRange)
  {}

  EnqueueArgs(CommandQueue &queue, NDRange global, NDRange local)
    : queue_(queue)
    , offset_(NullRange)
    , global_(global)
    , local_(local)
  {}

  EnqueueArgs(CommandQueue &queue, NDRange offset, NDRange global, NDRange local)
    : queue_(queue)
    , offset_(offset)
    , global_(global)
    , local_(local)
  {}

  EnqueueArgs(CommandQueue &queue, Event e, NDRange global)
    : queue_(queue)
    , offset_(NullRange)
    , global_(global)
    , local_(NullRange)
  {
    events_.push_back(e);
  }

  EnqueueArgs(CommandQueue &queue, Event e, NDRange global, NDRange local)
    : queue_(queue)
    , offset_(NullRange)
    , global_(global)
    , local_(local)
  {
    events_.push_back(e);
  }

  EnqueueArgs(CommandQueue &queue, Event e, NDRange offset, NDRange global, NDRange local)
    : queue_(queue)
    , offset_(offset)
    , global_(global)
    , local_(local)
  {
    events_.push_back(e);
  }

  EnqueueArgs(CommandQueue &queue, const vector<Event> &events, NDRange global)
    : queue_(queue)
    , offset_(NullRange)
    , global_(global)
    , local_(NullRange)
    , events_(events)
  {}

  EnqueueArgs(CommandQueue &queue, const vector<Event> &events, NDRange global, NDRange local)
    : queue_(queue)
    , offset_(NullRange)
    , global_(global)
    , local_(local)
    , events_(events)
  {}

  EnqueueArgs(CommandQueue &queue, const vector<Event> &events, NDRange offset, NDRange global, NDRange local)
    : queue_(queue)
    , offset_(offset)
    , global_(global)
    , local_(local)
    , events_(events)
  {}
};


//----------------------------------------------------------------------------------------------


/**
 * Type safe kernel functor.
 *
 */
template <typename... Ts>
class KernelFunctor
{
private:
  Kernel kernel_;

  template <int index, typename T0, typename... T1s>
  void setArgs(T0 &&t0, T1s &&...t1s)
  {
    kernel_.setArg(index, t0);
    setArgs<index + 1, T1s...>(std::forward<T1s>(t1s)...);
  }

  template <int index, typename T0>
  void setArgs(T0 &&t0)
  {
    kernel_.setArg(index, t0);
  }

  template <int index>
  void setArgs()
  {}


public:
  KernelFunctor(Kernel kernel)
    : kernel_(kernel)
  {}

  KernelFunctor(const Program &program, const string name, cl_int *err = NULL)
    : kernel_(program, name.c_str(), err)
  {}

  //! \brief Return type of the functor
  typedef Event result_type;

  /**
   * Enqueue kernel.
   * @param args Launch parameters of the kernel.
   * @param t0... List of kernel arguments based on the template type of the functor.
   */
  Event operator()(const EnqueueArgs &args, Ts... ts)
  {
    Event event;
    setArgs<0>(std::forward<Ts>(ts)...);

    args.queue_.enqueueNDRangeKernel(kernel_, args.offset_, args.global_, args.local_, &args.events_, &event);

    return event;
  }

  /**
   * Enqueue kernel with support for error code.
   * @param args Launch parameters of the kernel.
   * @param t0... List of kernel arguments based on the template type of the functor.
   * @param error Out parameter returning the error code from the execution.
   */
  Event operator()(const EnqueueArgs &args, Ts... ts, cl_int &error)
  {
    Event event;
    setArgs<0>(std::forward<Ts>(ts)...);

    error = args.queue_.enqueueNDRangeKernel(kernel_, args.offset_, args.global_, args.local_, &args.events_, &event);

    return event;
  }

#if CL_HPP_TARGET_OPENCL_VERSION >= 200
  cl_int setSVMPointers(const vector<void *> &pointerList) { return kernel_.setSVMPointers(pointerList); }

  template <typename T0, typename... T1s>
  cl_int setSVMPointers(const T0 &t0, T1s &...ts)
  {
    return kernel_.setSVMPointers(t0, ts...);
  }
#endif  // #if CL_HPP_TARGET_OPENCL_VERSION >= 200

  Kernel getKernel() { return kernel_; }
};

namespace compatibility
{
/**
 * Backward compatibility class to ensure that cl.hpp code works with opencl.hpp.
 * Please use KernelFunctor directly.
 */
template <typename... Ts>
struct make_kernel
{
  typedef KernelFunctor<Ts...> FunctorType;

  FunctorType functor_;

  make_kernel(const Program &program, const string name, cl_int *err = NULL)
    : functor_(FunctorType(program, name, err))
  {}

  make_kernel(const Kernel kernel)
    : functor_(FunctorType(kernel))
  {}

  //! \brief Return type of the functor
  typedef Event result_type;

  //! \brief Function signature of kernel functor with no event dependency.
  typedef Event type_(const EnqueueArgs &, Ts...);

  Event operator()(const EnqueueArgs &enqueueArgs, Ts... args) { return functor_(enqueueArgs, args...); }
};
}  // namespace compatibility


//----------------------------------------------------------------------------------------------------------------------

#undef CL_HPP_ERR_STR_
#if !defined(CL_HPP_USER_OVERRIDE_ERROR_STRINGS)
#undef __GET_DEVICE_INFO_ERR
#undef __GET_PLATFORM_INFO_ERR
#undef __GET_DEVICE_IDS_ERR
#undef __GET_PLATFORM_IDS_ERR
#undef __GET_CONTEXT_INFO_ERR
#undef __GET_EVENT_INFO_ERR
#undef __GET_EVENT_PROFILE_INFO_ERR
#undef __GET_MEM_OBJECT_INFO_ERR
#undef __GET_IMAGE_INFO_ERR
#undef __GET_SAMPLER_INFO_ERR
#undef __GET_KERNEL_INFO_ERR
#undef __GET_KERNEL_ARG_INFO_ERR
#undef __GET_KERNEL_SUB_GROUP_INFO_ERR
#undef __GET_KERNEL_WORK_GROUP_INFO_ERR
#undef __GET_PROGRAM_INFO_ERR
#undef __GET_PROGRAM_BUILD_INFO_ERR
#undef __GET_COMMAND_QUEUE_INFO_ERR
#undef __CREATE_CONTEXT_ERR
#undef __CREATE_CONTEXT_FROM_TYPE_ERR
#undef __GET_SUPPORTED_IMAGE_FORMATS_ERR
#undef __CREATE_BUFFER_ERR
#undef __COPY_ERR
#undef __CREATE_SUBBUFFER_ERR
#undef __CREATE_GL_BUFFER_ERR
#undef __CREATE_GL_RENDER_BUFFER_ERR
#undef __GET_GL_OBJECT_INFO_ERR
#undef __CREATE_IMAGE_ERR
#undef __CREATE_GL_TEXTURE_ERR
#undef __IMAGE_DIMENSION_ERR
#undef __SET_MEM_OBJECT_DESTRUCTOR_CALLBACK_ERR
#undef __CREATE_USER_EVENT_ERR
#undef __SET_USER_EVENT_STATUS_ERR
#undef __SET_EVENT_CALLBACK_ERR
#undef __WAIT_FOR_EVENTS_ERR
#undef __CREATE_KERNEL_ERR
#undef __SET_KERNEL_ARGS_ERR
#undef __CREATE_PROGRAM_WITH_SOURCE_ERR
#undef __CREATE_PROGRAM_WITH_IL_ERR
#undef __CREATE_PROGRAM_WITH_BINARY_ERR
#undef __CREATE_PROGRAM_WITH_IL_ERR
#undef __CREATE_PROGRAM_WITH_BUILT_IN_KERNELS_ERR
#undef __BUILD_PROGRAM_ERR
#undef __COMPILE_PROGRAM_ERR
#undef __LINK_PROGRAM_ERR
#undef __CREATE_KERNELS_IN_PROGRAM_ERR
#undef __CREATE_COMMAND_QUEUE_WITH_PROPERTIES_ERR
#undef __CREATE_SAMPLER_WITH_PROPERTIES_ERR
#undef __SET_COMMAND_QUEUE_PROPERTY_ERR
#undef __ENQUEUE_READ_BUFFER_ERR
#undef __ENQUEUE_READ_BUFFER_RECT_ERR
#undef __ENQUEUE_WRITE_BUFFER_ERR
#undef __ENQUEUE_WRITE_BUFFER_RECT_ERR
#undef __ENQEUE_COPY_BUFFER_ERR
#undef __ENQEUE_COPY_BUFFER_RECT_ERR
#undef __ENQUEUE_FILL_BUFFER_ERR
#undef __ENQUEUE_READ_IMAGE_ERR
#undef __ENQUEUE_WRITE_IMAGE_ERR
#undef __ENQUEUE_COPY_IMAGE_ERR
#undef __ENQUEUE_FILL_IMAGE_ERR
#undef __ENQUEUE_COPY_IMAGE_TO_BUFFER_ERR
#undef __ENQUEUE_COPY_BUFFER_TO_IMAGE_ERR
#undef __ENQUEUE_MAP_BUFFER_ERR
#undef __ENQUEUE_MAP_IMAGE_ERR
#undef __ENQUEUE_UNMAP_MEM_OBJECT_ERR
#undef __ENQUEUE_NDRANGE_KERNEL_ERR
#undef __ENQUEUE_NATIVE_KERNEL
#undef __ENQUEUE_MIGRATE_MEM_OBJECTS_ERR
#undef __ENQUEUE_MIGRATE_SVM_ERR
#undef __ENQUEUE_ACQUIRE_GL_ERR
#undef __ENQUEUE_RELEASE_GL_ERR
#undef __CREATE_PIPE_ERR
#undef __GET_PIPE_INFO_ERR
#undef __RETAIN_ERR
#undef __RELEASE_ERR
#undef __FLUSH_ERR
#undef __FINISH_ERR
#undef __VECTOR_CAPACITY_ERR
#undef __CREATE_SUB_DEVICES_ERR
#undef __CREATE_SUB_DEVICES_ERR
#undef __ENQUEUE_MARKER_ERR
#undef __ENQUEUE_WAIT_FOR_EVENTS_ERR
#undef __ENQUEUE_BARRIER_ERR
#undef __UNLOAD_COMPILER_ERR
#undef __CREATE_GL_TEXTURE_2D_ERR
#undef __CREATE_GL_TEXTURE_3D_ERR
#undef __CREATE_IMAGE2D_ERR
#undef __CREATE_IMAGE3D_ERR
#undef __CREATE_COMMAND_QUEUE_ERR
#undef __ENQUEUE_TASK_ERR
#undef __CREATE_SAMPLER_ERR
#undef __ENQUEUE_MARKER_WAIT_LIST_ERR
#undef __ENQUEUE_BARRIER_WAIT_LIST_ERR
#undef __CLONE_KERNEL_ERR
#undef __GET_HOST_TIMER_ERR
#undef __GET_DEVICE_AND_HOST_TIMER_ERR

#endif  // CL_HPP_USER_OVERRIDE_ERROR_STRINGS

// Extensions
#undef CL_HPP_INIT_CL_EXT_FCN_PTR_
#undef CL_HPP_INIT_CL_EXT_FCN_PTR_PLATFORM_

#if defined(CL_HPP_USE_CL_DEVICE_FISSION)
#undef CL_HPP_PARAM_NAME_DEVICE_FISSION_
#endif  // CL_HPP_USE_CL_DEVICE_FISSION

#undef CL_HPP_NOEXCEPT_
#undef CL_HPP_DEFINE_STATIC_MEMBER_

}  // namespace cl

#endif  // CL_HPP_


================================================
FILE: clu/CMakeLists.txt
================================================


# Convert version string to a list of versions.
string(REPLACE "." ";" OPENCL_VERSION_LIST "${OHM_OPENCL_SDK_VER}")

set(OPENCL_VERSION_MINOR 0)
set(OPENCL_VERSION_PATCH 0)
list(LENGTH OPENCL_VERSION_LIST VERSION_COMPONENTS)
list(GET OPENCL_VERSION_LIST 0 OPENCL_VERSION_MAJOR)
if(VERSION_COMPONENTS GREATER 1)
  list(GET OPENCL_VERSION_LIST 1 OPENCL_VERSION_MINOR)
  if(VERSION_COMPONENTS GREATER 2)
    list(GET OPENCL_VERSION_LIST 2 OPENCL_VERSION_PATCH)
  endif(VERSION_COMPONENTS GREATER 2)
endif(VERSION_COMPONENTS GREATER 1)

# Convert version into an integer for use in code. Each version component is a digit.
set(CLU_TARGET_OPENCL_VERSION ${OPENCL_VERSION_MAJOR}${OPENCL_VERSION_MINOR}${OPENCL_VERSION_PATCH})

configure_file(cluConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/clu/cluConfig.h")

set(SOURCES
  clu.cpp
  clu.h
  cluBuffer.h
  cluConstraint.cpp
  cluConstraint.h
  cluKernel.cpp
  cluKernel.h
  cluProgram.cpp
  cluProgram.h
)

set(PUBLIC_HEADERS
  clu.h
  cluBuffer.h
  cluConstraint.h
  cluKernel.h
  cluProgram.h
  "${CMAKE_CURRENT_BINARY_DIR}/clu/cluConfig.h"
)

set(3P_HEADERS
  3rdparty/CL/opencl.hpp
)

add_library(clu STATIC ${SOURCES} ${PUBLIC_HEADERS})
clang_tidy_target(clu)

target_include_directories(clu
  PUBLIC
    $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/clu>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
)

target_include_directories(clu SYSTEM
  PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}/3rdparty>
)
# set_property(TARGET clu PROPERTY DEBUG_POSTFIX "d")

target_link_libraries(clu PUBLIC OpenCL::OpenCL)
if(OHM_BUILD_SHARED)
  # The OpenCL library depends on Threads::Threads.
  # We also need libdl on Linux.
  # Neither needs to be explicilty linked when building only static ohm libraries.
  # Not sure why.
  if(TARGET Thread::Threads)
    target_link_libraries(clu PUBLIC Threads::Threads)
  endif(TARGET Thread::Threads)
  if(CMAKE_DL_LIBS)
    target_link_libraries(clu PUBLIC ${CMAKE_DL_LIBS})
  endif(CMAKE_DL_LIBS)
endif(OHM_BUILD_SHARED)

install(TARGETS clu EXPORT ${CMAKE_PROJECT_NAME}-config-targets
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/clu
)

install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/clu)
install(FILES ${3P_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/clu)

source_group("source" REGULAR_EXPRESSION ".*$")


================================================
FILE: clu/clu.cpp
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) CSIRO 2016
//
#include "clu.h"

#include <mutex>
#include <sstream>

#include "cluConstraint.h"

#include <array>
#include <iostream>
#include <memory>

namespace
{
struct PrimaryContex
{
  cl::Context context;
  cl::Device device;
};

// Note: clang-tidy warns of potentially uncaught excepsions when the cl objects are static. Use a struct pointer to
// avoid the issue.
std::unique_ptr<PrimaryContex> g_context;
std::mutex g_lock;

struct InfoItem
{
  int id;
  std::string label;
};


void split(const std::string &str, char delim, std::vector<std::string> &tokens)
{
  size_t prev = 0;
  size_t pos = 0;
  do
  {
    pos = str.find(delim, prev);

    if (pos != std::string::npos)
    {
      tokens.push_back(str.substr(prev, pos - prev));
      prev = pos + 1;
    }
    else
    {
      tokens.push_back(str.substr(prev, str.length()));
    }
  } while (pos != std::string::npos);
}


std::string stripQuotes(const std::string &str)
{
  if (str.length() < 2)
  {
    return str;
  }

  if (str[0] == str[str.length() - 1])
  {
    if (str[0] == '"' || str[0] == '\'')  // fix syntax hilighting in VSCode :"
    {
      return str.substr(1, str.length() - 2);
    }
  }

  return str;
}
}  // namespace

namespace clu
{
enum ArgParse
{
  kApOk,
  kApMissingValue,
  kApParseFailure
};

cl::Platform createPlatform(cl_device_type type, const PlatformConstraint *constraints, unsigned constraint_count)
{
  std::vector<cl::Platform> platforms;
  std::vector<cl::Device> devices;
  cl::Platform::get(&platforms);

  for (cl::Platform &platform : platforms)
  {
    // Check constraints.
    bool constraints_ok = true;
    for (unsigned i = 0; i < constraint_count; ++i)
    {
      if (!(*constraints)(platform))
      {
        constraints_ok = false;
        break;
      }
    }

    if (!constraints_ok)
    {
      // A constraint has failed.
      continue;
    }

    if (type != CL_DEVICE_TYPE_ALL)
    {
      // Check device types.
      devices.clear();
      platform.getDevices(type, &devices);
      if (!devices.empty())
      {
        // Found a suitable platform.
        return platform;
      }
    }
    else
    {
      return platform;
    }
  }

  return cl::Platform();
}


cl::Platform createPlatform(cl_device_type type, const std::vector<PlatformConstraint> &constraints)
{
  return createPlatform(type, constraints.data(), unsigned(constraints.size()));
}


bool filterPlatforms(std::vector<cl::Platform> &platforms, cl_device_type /*type*/,
                     const PlatformConstraint *constraints, unsigned constraint_count)
{
  for (auto iter = platforms.begin(); iter != platforms.end();)
  {
    bool constraints_ok = true;
    for (unsigned i = 0; i < constraint_count; ++i)
    {
      if (!(*constraints)(*iter))
      {
        constraints_ok = false;
        break;
      }
    }

    if (constraints_ok)
    {
      ++iter;
    }
    else
    {
      iter = platforms.erase(iter);
    }
  }

  return !platforms.empty();
}


bool filterDevices(const cl::Platform &platform, std::vector<cl::Device> &devices, const DeviceConstraint *constraints,
                   unsigned constraint_count)
{
  for (auto iter = devices.begin(); iter != devices.end();)
  {
    bool constraints_ok = true;
    for (unsigned i = 0; i < constraint_count; ++i)
    {
      if (!(*constraints)(platform, *iter))
      {
        constraints_ok = false;
        break;
      }
    }

    if (constraints_ok)
    {
      ++iter;
    }
    else
    {
      iter = devices.erase(iter);
    }
  }

  return !devices.empty();
}


unsigned listDevices(std::vector<cl::Device> &devices, const cl::Context &context)
{
  cl_int device_count = 0;
  devices.clear();
  cl_int clerr = clGetContextInfo(context(), CL_CONTEXT_NUM_DEVICES, sizeof(device_count), &device_count, nullptr);

  if (device_count == 0 || clerr != CL_SUCCESS)
  {
    return 0;
  }

  auto *device_ids = static_cast<cl_device_id *>(alloca(sizeof(cl_device_id) * device_count));
  clerr = clGetContextInfo(context(), CL_CONTEXT_DEVICES, sizeof(cl_device_id) * device_count, device_ids, nullptr);
  if (clerr != CL_SUCCESS)
  {
    return 0;
  }

  devices.resize(device_count);
  for (cl_int i = 0; i < device_count; ++i)
  {
    devices[i] = cl::Device(device_ids[i]);
  }

  return device_count;
}


cl_device_id getFirstDevice(const cl::Context &context, cl_int *err)
{
  cl_int device_count = 0;
  cl_int clerr = clGetContextInfo(context(), CL_CONTEXT_NUM_DEVICES, sizeof(device_count), &device_count, nullptr);

  if (device_count == 0 || clerr != CL_SUCCESS)
  {
    if (err)
    {
      *err = clerr;
    }
    return nullptr;
  }

  auto *device_ids = static_cast<cl_device_id *>(alloca(sizeof(cl_device_id) * device_count));
  clerr = clGetContextInfo(context(), CL_CONTEXT_DEVICES, sizeof(cl_device_id) * device_count, device_ids, nullptr);
  if (err)
  {
    *err = clerr;
  }

  if (clerr != CL_SUCCESS)
  {
    return nullptr;
  }

  return device_ids[0];
}

#if 0
  cl::Context createContext(const cl::Platform &platform, cl_device_type type, const DeviceConstraint *constraints, unsigned constraintCount)
  {
    std::vector<cl::Device> devices;
    platform.getDevices(type, &devices);

    if (!filterDevices(platform, devices, constraints, constraintCount))
    {
      return cl::Context();
    }

    // Use the preferred platform and create a context
    cl_context_properties cprops[] = { CL_CONTEXT_PLATFORM, (cl_context_properties)(platform)(), 0 };
    cl::Context context = cl::Context(devices, cprops);
    return context;
  }


  cl::Context createContext(const cl::Platform &platform, cl_device_type type, const std::vector<DeviceConstraint> &constraints)
  {
    return createContext(platform, type, constraints.data(), unsigned(constraints.size()));
  }
#endif  // #

cl::Context createContext(cl::Device *device_out, cl_device_type type, const PlatformConstraint *platform_constraint,
                          unsigned platform_constraint_count, const DeviceConstraint *device_constraints,
                          unsigned device_constraint_count)
{
  std::vector<cl::Platform> platforms;
  std::vector<cl::Device> devices;
  cl::Platform::get(&platforms);
  filterPlatforms(platforms, type, platform_constraint, platform_constraint_count);

  for (cl::Platform &platform : platforms)
  {
    devices.clear();

    // // Version check: 1.2 is the minimum supported or we can segfault.
    // cl_uint version_major = 0u;
    // cl_uint version_minor = 0u;
    // platformVersion(platform(), &version_major, &version_minor);

    // const cl_uint min_version_major = 1;
    // const cl_uint min_version_minor = 2;
    // if (version_major < min_version_major || version_major == min_version_major && version_minor <
    // min_version_minor)
    // {
    //   continue;
    // }

    platform.getDevices(type, &devices);
    filterDevices(platform, devices, device_constraints, device_constraint_count);
    if (!devices.empty())
    {
      // Select a single device.
      std::array<cl_context_properties, 3> cprops = { CL_CONTEXT_PLATFORM, cl_context_properties((platform)()), 0 };
      cl::Context context = cl::Context(devices[0], cprops.data());

      if (context())
      {
        if (device_out)
        {
          *device_out = devices[0];
        }
        return context;
      }
    }
  }

  return cl::Context();
}


bool setPrimaryContext(const cl::Context &context, const cl::Device &device)
{
  if (!context())
  {
    return false;
  }

  std::unique_lock<std::mutex> guard(g_lock);
  if (!g_context)
  {
    g_context = std::make_unique<PrimaryContex>();
  }
  g_context->context = context;
  g_context->device = device;
  return true;
}


void clearPrimaryContext()
{
  std::unique_lock<std::mutex> guard(g_lock);
  if (g_context)
  {
    g_context = std::make_unique<PrimaryContex>();
    g_context->context = cl::Context();
    g_context->device = cl::Device();
  }
}


bool initPrimaryContext(cl_device_type type, const PlatformConstraint *platform_constraints,
                        unsigned platform_constraint_count, const DeviceConstraint *device_constraints,
                        unsigned device_constraint_count)
{
  cl::Device default_device;
  cl::Context context = createContext(&default_device, type, platform_constraints, platform_constraint_count,
                                      device_constraints, device_constraint_count);
  return setPrimaryContext(context, default_device);
}


bool initPrimaryContext(cl_device_type type, const std::vector<PlatformConstraint> &platform_constraint,
                        const std::vector<DeviceConstraint> &device_constraints)
{
  return initPrimaryContext(type, platform_constraint.data(), unsigned(platform_constraint.size()),
                            device_constraints.data(), unsigned(device_constraints.size()));
}


bool getPrimaryContext(cl::Context &context, cl::Device &device)
{
  std::unique_lock<std::mutex> guard(g_lock);
  if (g_context)
  {
    context = g_context->context;
    device = g_context->device;
  }
  else
  {
    context = cl::Context();
    device = cl::Device();
  }

  return context() != nullptr;
}


ArgParse argValue(std::string &val, const std::string &arg, std::list<std::string>::const_iterator &iter,
                  const std::list<std::string>::const_iterator &end)
{
  std::string::size_type eqpos = arg.find('=');
  auto next = iter;

  ++next;
  // Check for form
  if (eqpos == std::string::npos)
  {
    // Not of the form "--arg=value". Read next argument.
    if (next != end)
    {
      val = *next;
      if (val.find("--") != 0)
      {
        // Good value. Consume the next argument.
        iter = next;
        return kApOk;
      }

      // Argument missing value.
      return kApMissingValue;
    }
    // Not enough additional arguments.
    return kApMissingValue;
  }

  // Of the form "--arg=value"
  val = arg.substr(eqpos + 1);
  return (!val.empty()) ? kApOk : kApMissingValue;
}


void constraintsFromCommandLine(int argc, const char **argv, cl_device_type &type,
                                std::vector<PlatformConstraint> &platform_constraints,
                                std::vector<DeviceConstraint> &device_constraints, const char *arg_prefix)
{
  std::list<std::string> args;
  for (int i = 0; i < argc; ++i)
  {
    args.emplace_back(argv[i]);
  }

  return constraintsFromArgs(args, type, platform_constraints, device_constraints, arg_prefix);
}


void constraintsFromArgs(const std::list<std::string> &args, cl_device_type &type,
                         std::vector<PlatformConstraint> &platform_constraints,
                         std::vector<DeviceConstraint> &device_constraints, const char *arg_prefix)
{
  std::string arg;
  std::string val;
  std::string prefix = (arg_prefix) ? std::string("--") + std::string(arg_prefix) : "--";
  std::vector<std::string> tokens;
  ArgParse parse_result = kApOk;

  if (!type)
  {
    // NOLINTNEXTLINE(hicpp-signed-bitwise)
    type = CL_DEVICE_TYPE_GPU | CL_DEVICE_TYPE_ACCELERATOR;
  }

  for (auto iter = args.begin(); iter != args.end(); ++iter)
  {
    arg = *iter;
    if (arg.find(prefix) == 0)
    {
      arg.erase(0, prefix.length());
      if (arg.find("accel") == 0)
      {
        parse_result = argValue(val, arg, iter, args.end());
        if (parse_result == kApOk)
        {
          tokens.clear();
          split(val, ',', tokens);
          type = 0;
          for (const std::string &type_str : tokens)
          {
            if (type_str == "any")
            {
              type |= CL_DEVICE_TYPE_ALL;  // NOLINT(hicpp-signed-bitwise)
            }
            else if (type_str == "accel")
            {
              type |= CL_DEVICE_TYPE_ACCELERATOR;  // NOLINT(hicpp-signed-bitwise)
            }
            else if (type_str == "cpu")
            {
              type |= CL_DEVICE_TYPE_CPU;  // NOLINT(hicpp-signed-bitwise)
            }
            else if (type_str == "gpu")
            {
              type |= CL_DEVICE_TYPE_GPU;  // NOLINT(hicpp-signed-bitwise)
            }
            else
            {
              parse_result = kApParseFailure;
            }
          }
        }
      }
      else if (arg.find("clver") == 0)
      {
        parse_result = argValue(val, arg, iter, args.end());
        if (parse_result == kApOk)
        {
          int major = 0;
          int minor = 0;
          // Expecting: major[.minor]
          tokens.clear();
          split(val, '.', tokens);
          if (!tokens.empty() && tokens.size() <= 2)
          {
            std::istringstream str(tokens[0]);
            str >> major;
            if (str.fail())
            {
              parse_result = kApParseFailure;
            }
            if (tokens.size() > 1)
            {
              str = std::istringstream(tokens[1]);
              str >> minor;
              if (str.fail())
              {
                parse_result = kApParseFailure;
              }
            }

            if (parse_result == kApOk)
            {
              // Add a device version constraint.
              device_constraints.push_back(deviceVersionMin(major, minor));
            }
          }
          else
          {
            parse_result = kApParseFailure;
          }
        }
      }
      else if (arg.find("device") == 0)
      {
        parse_result = argValue(val, arg, iter, args.end());
        if (parse_result == kApOk)
        {
          val = stripQuotes(val);
          // Add a device constraint.
          device_constraints.push_back(deviceNameLike(val.c_str(), true));
        }
      }
      else if (arg.find("platform") == 0)
      {
        parse_result = argValue(val, arg, iter, args.end());
        if (parse_result == kApOk)
        {
          val = stripQuotes(val);
          // Add a device constraint.
          platform_constraints.push_back(platformNameLike(val.c_str(), true));
        }
      }
      else if (arg.find("vendor") == 0)
      {
        parse_result = argValue(val, arg, iter, args.end());
        if (parse_result == kApOk)
        {
          val = stripQuotes(val);
          // Add a device constraint.
          // platformConstraints.push_back(platformVendorLike(val.c_str(), true));
          device_constraints.push_back(deviceVendorLike(val.c_str(), true));
        }
      }

      if (parse_result != kApOk)
      {
        switch (parse_result)
        {
        case kApParseFailure:
          std::cerr << "Failed parsing argument '" << arg << "' value '" << val << "'" << std::endl;
          break;
        case kApMissingValue:
          std::cerr << "Argument '" << arg << "' missing value" << std::endl;
          break;
        default:
          std::cerr << "Error processing argument '" << arg << "'" << std::endl;
          break;
        }
      }
    }
  }
}


bool parseVersion(const char *version_string, cl_uint *version_major, cl_uint *version_minor)
{
  cl_uint high_version = 0;
  cl_uint low_version = 0;
  int index = 0;

  while (version_string[index] && !isdigit(version_string[index]))
  {
    ++index;
  }

  if (!version_string[index])
  {
    return false;
  }

  bool have_major = false;
  bool have_minor = false;
  const cl_uint base_shift = 10u;
  while (version_string[index] && isdigit(version_string[index]))
  {
    have_major = true;
    high_version *= base_shift;
    high_version += version_string[index] - '0';
    ++index;
  }

  // Verify and skip the '.'
  if (!version_string[index] || !version_string[++index])
  {
    return false;
  }

  while (version_string[index] && isdigit(version_string[index]))
  {
    have_minor = true;
    low_version *= base_shift;
    low_version += version_string[index] - '0';
    ++index;
  }

  if (version_major)
  {
    *version_major = high_version;
  }

  if (version_minor)
  {
    *version_minor = low_version;
  }

  return have_major && have_minor;
}


void platformVersion(cl_platform_id platform, cl_uint *version_major, cl_uint *version_minor)
{
  ::size_t size = 0;
  clGetPlatformInfo(platform, CL_PLATFORM_VERSION, 0, nullptr, &size);
  cl::string version_info;
  version_info.resize(size + 1);
  clGetPlatformInfo(platform, CL_PLATFORM_VERSION, size, &version_info[0], &size);
  parseVersion(version_info.c_str(), version_major, version_minor);
}


const char *errorCodeString(cl_int error)
{
#define CL_ERR_CASE(CODE) \
  case CODE:              \
    return #CODE;
#define CL_ERR_CASE2(CODE, msg) \
  case CODE:                    \
    return msg;

  switch (error)
  {
    CL_ERR_CASE(CL_SUCCESS)
    CL_ERR_CASE(CL_DEVICE_NOT_FOUND)
    CL_ERR_CASE(CL_DEVICE_NOT_AVAILABLE)
    CL_ERR_CASE(CL_COMPILER_NOT_AVAILABLE)
    CL_ERR_CASE(CL_MEM_OBJECT_ALLOCATION_FAILURE)
    CL_ERR_CASE(CL_OUT_OF_RESOURCES)
    CL_ERR_CASE(CL_OUT_OF_HOST_MEMORY)
    CL_ERR_CASE(CL_PROFILING_INFO_NOT_AVAILABLE)
    CL_ERR_CASE(CL_MEM_COPY_OVERLAP)
    CL_ERR_CASE(CL_IMAGE_FORMAT_MISMATCH)
    CL_ERR_CASE(CL_IMAGE_FORMAT_NOT_SUPPORTED)
    CL_ERR_CASE(CL_BUILD_PROGRAM_FAILURE)
    CL_ERR_CASE(CL_MAP_FAILURE)
    CL_ERR_CASE(CL_MISALIGNED_SUB_BUFFER_OFFSET)
    CL_ERR_CASE(CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST)
    CL_ERR_CASE(CL_COMPILE_PROGRAM_FAILURE)
    CL_ERR_CASE(CL_LINKER_NOT_AVAILABLE)
    CL_ERR_CASE(CL_LINK_PROGRAM_FAILURE)
    CL_ERR_CASE(CL_DEVICE_PARTITION_FAILED)
    CL_ERR_CASE(CL_KERNEL_ARG_INFO_NOT_AVAILABLE)
    CL_ERR_CASE(CL_INVALID_VALUE)
    CL_ERR_CASE(CL_INVALID_DEVICE_TYPE)
    CL_ERR_CASE(CL_INVALID_PLATFORM)
    CL_ERR_CASE(CL_INVALID_DEVICE)
    CL_ERR_CASE(CL_INVALID_CONTEXT)
    CL_ERR_CASE(CL_INVALID_QUEUE_PROPERTIES)
    CL_ERR_CASE(CL_INVALID_COMMAND_QUEUE)
    CL_ERR_CASE(CL_INVALID_HOST_PTR)
    CL_ERR_CASE(CL_INVALID_MEM_OBJECT)
    CL_ERR_CASE(CL_INVALID_IMAGE_FORMAT_DESCRIPTOR)
    CL_ERR_CASE(CL_INVALID_IMAGE_SIZE)
    CL_ERR_CASE(CL_INVALID_SAMPLER)
    CL_ERR_CASE(CL_INVALID_BINARY)
    CL_ERR_CASE(CL_INVALID_BUILD_OPTIONS)
    CL_ERR_CASE(CL_INVALID_PROGRAM)
    CL_ERR_CASE(CL_INVALID_PROGRAM_EXECUTABLE)
    CL_ERR_CASE(CL_INVALID_KERNEL_NAME)
    CL_ERR_CASE(CL_INVALID_KERNEL_DEFINITION)
    CL_ERR_CASE(CL_INVALID_KERNEL)
    CL_ERR_CASE(CL_INVALID_ARG_INDEX)
    CL_ERR_CASE(CL_INVALID_ARG_VALUE)
    CL_ERR_CASE(CL_INVALID_ARG_SIZE)
    CL_ERR_CASE(CL_INVALID_KERNEL_ARGS)
    CL_ERR_CASE(CL_INVALID_WORK_DIMENSION)
    CL_ERR_CASE(CL_INVALID_WORK_GROUP_SIZE)
    CL_ERR_CASE(CL_INVALID_WORK_ITEM_SIZE)
    CL_ERR_CASE(CL_INVALID_GLOBAL_OFFSET)
    CL_ERR_CASE(CL_INVALID_EVENT_WAIT_LIST)
    CL_ERR_CASE(CL_INVALID_EVENT)
    CL_ERR_CASE(CL_INVALID_OPERATION)
    CL_ERR_CASE(CL_INVALID_GL_OBJECT)
    CL_ERR_CASE(CL_INVALID_BUFFER_SIZE)
    CL_ERR_CASE(CL_INVALID_MIP_LEVEL)
    CL_ERR_CASE(CL_INVALID_GLOBAL_WORK_SIZE)
    CL_ERR_CASE(CL_INVALID_PROPERTY)
    CL_ERR_CASE(CL_INVALID_IMAGE_DESCRIPTOR)
    CL_ERR_CASE(CL_INVALID_COMPILER_OPTIONS)
    CL_ERR_CASE(CL_INVALID_LINKER_OPTIONS)
    CL_ERR_CASE(CL_INVALID_DEVICE_PARTITION_COUNT)

#if CL_VERSION_2_0
    CL_ERR_CASE(CL_INVALID_PIPE_SIZE)
    CL_ERR_CASE(CL_INVALID_DEVICE_QUEUE)
#endif  // CL_VERSION_2_0

    // extension errors
#ifdef CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR
    CL_ERR_CASE(CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR)
#endif  // CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR

#ifdef CL_PLATFORM_NOT_FOUND_KHR
    CL_ERR_CASE(CL_PLATFORM_NOT_FOUND_KHR)
#endif  // CL_PLATFORM_NOT_FOUND_KHR

#ifdef CL_INVALID_D3D10_DEVICE_KHR
    CL_ERR_CASE(CL_INVALID_D3D10_DEVICE_KHR)
#endif  // CL_INVALID_D3D10_DEVICE_KHR

#ifdef CL_INVALID_D3D10_RESOURCE_KHR
    CL_ERR_CASE(CL_INVALID_D3D10_RESOURCE_KHR)
#endif  // CL_INVALID_D3D10_RESOURCE_KHR

#ifdef CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR
    CL_ERR_CASE(CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR)
#endif  // CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR

#ifdef CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR
    CL_ERR_CASE(CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR)
#endif  // CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR

  default:
    return "Unknown OpenCL error";
  }

#undef CL_ERR_CASE
#undef CL_ERR_CASE2
}


void printPlatformInfo(std::ostream &out, const cl::Platform &platform, const char *prefix, const char *endl)
{
  // NOLINTNEXTLINE(cppcoreguidelines-avoid-c-arrays)
  static const std::array<InfoItem, 3> items =  //
    {
      InfoItem{ CL_PLATFORM_NAME, "Name" },        //
      InfoItem{ CL_PLATFORM_VERSION, "Version" },  //
      InfoItem{ CL_PLATFORM_VENDOR, "Vendor" }     //
      //{ CL_PLATFORM_PROFILE, "Profile" },
      //{ CL_PLATFORM_EXTENSIONS, "Extensions" },
    };
  std::string info_str;
  std::string info_str2;
  if (platform())
  {
    bool first = true;
    for (const InfoItem &item : items)
    {
      platform.getInfo(cl_platform_info(item.id), &info_str);
      if (!first)
      {
        out << endl;
      }
      // Use c_str() otherwise we get extra '\0' characters.
      out << prefix << item.label << ": " << info_str;
      first = false;
    }
  }
}


void printDeviceInfo(std::ostream &out, const cl::Device &device, const char *prefix, const char *endl)
{
  static const std::array<InfoItem, 2> items =  //
    {
      InfoItem{ CL_DEVICE_NAME, "Name" },       //
      InfoItem{ CL_DEVICE_VERSION, "Version" }  //
      //{ CL_PLATFORM_PROFILE, "Profile" },
      //{ CL_PLATFORM_EXTENSIONS, "Extensions" },
    };
  std::string info_str;
  if (device())
  {
    bool first = true;
    for (const InfoItem &item : items)
    {
      device.getInfo(cl_device_info(item.id), &info_str);
      if (!first)
      {
        out << endl;
      }
      // Use c_str() otherwise we get extra '\0' characters.
      out << prefix << item.label << ": " << info_str;
      first = false;
    }
  }
}
}  // namespace clu


================================================
FILE: clu/clu.h
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) CSIRO 2016
//
#ifndef CLU_H
#define CLU_H

#include "cluConfig.h"

#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-function"
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wignored-qualifiers"
#pragma GCC diagnostic ignored "-Wsign-compare"
#pragma GCC diagnostic ignored "-Wignored-attributes"
#endif  // __GNUC__
#include "CL/opencl.hpp"
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif  // __GNUC__

#include <functional>
#include <iostream>
#include <list>
#include <vector>

namespace clu
{
/// Constraint function prototype used in platform selection.
using PlatformConstraint = std::function<bool(const cl::Platform &)>;
/// Constraint function prototype used in device selection.
using DeviceConstraint = std::function<bool(const cl::Platform &, const cl::Device &)>;

/// Create the OpenCL platform optionally applying the given constraints.
///
/// The selected device must pass all the @p constraints given, each returning true, in order
/// to be selected. If no platform passes all tests, then an invalid platform is returned.
/// The first valid platform is always returned including when there are no constraints.
///
/// See @c clucontraint.h for some constraint examples.
///
/// @param type The required OpenCL device type.
/// @param constraints The array of constraints to pass or null for no constraints.
/// @param constraint_count The number of @p constraints.
/// @return The first valid platform found, passing all constraints.
cl::Platform createPlatform(cl_device_type type, const PlatformConstraint *constraints = nullptr,
                            unsigned constraint_count = 0);
/// Create the OpenCL platform optionally applying the given constraints.
///
/// The selected device must pass all the @p constraints given, each returning true, in order
/// to be selected. If no platform passes all tests, then an invalid platform is returned.
///
/// @param type The required OpenCL device type.
/// @param constraints The array of constraints to pass or empty for no constraints.
/// @return The first valid platform found, passing all constraints.
cl::Platform createPlatform(cl_device_type type, const std::vector<PlatformConstraint> &constraints);

/// Filter the given list of @p platforms, removing all which do not match @p type or pass
/// @p constraints.
///
/// See @c clucontraint.h for some constraint examples.
///
/// @param[in,out] platforms The Platforms to filter.
/// @param type The required device type(s).
/// @param constraints The constraints to filter by.
/// @param constraint_count The number of @p constraints.
/// @return True if there is at least one valid platform left in @p platforms.
bool filterPlatforms(std::vector<cl::Platform> &platforms, cl_device_type type, const PlatformConstraint *constraints,
                     unsigned constraint_count);

/// Filter the given list of @p devices, removing all which do not match @p type or pass
/// @p constraints.
///
/// See @c clucontraint.h for some constraint examples.
///
/// @param platform The platform to which the @p devices belong.
/// @param[in,out] devices The Platforms to filter.
/// @param constraints The device constraints to filter by.
/// @param constraint_count The number of @p constraints.
/// @return True if there is at least one valid device left in @p devices.
bool filterDevices(const cl::Platform &platform, std::vector<cl::Device> &devices, const DeviceConstraint *constraints,
                   unsigned constraint_count);

/// List all the available devices in @p context.
/// @param devices Populated with the devices from @p context.
/// @param context The OpenCL context of interest.
/// @return The number of items in @p devices.
unsigned listDevices(std::vector<cl::Device> &devices, const cl::Context &context);

/// Retrieve the first available device from @p context.
/// @param context The OpenCL context of interest.
/// @param err On failure, set to any error value occurring, or @c CL_SUCCESS on success.
///     Ignored when null.
/// @return The first device of @p context, or null on failure.
cl_device_id getFirstDevice(const cl::Context &context, cl_int *err = nullptr);

#if 0
  /// Create the OpenCL context optionally applying the given device constraints.
  ///
  /// The selected context must have device(s) passing all the @p constraints given, each
  /// returning true, in order to be selected. If no context has devices passes all tests,
  /// then an invalid platform is returned. The first valid context is always returned
  /// including when there are no constraints.
  ///
  /// See @c clucontraint.h for some constraint examples.
  ///
  /// @param platform The OpenCL platform to create the context for.
  /// @param type The required OpenCL device type.
  /// @param constraints The array of device constraints to pass or null for no constraints.
  /// @param contraintCount The number of @p constraints.
  /// @return The first valid context found, passing all constraints.
  cl::Context createContext(const cl::Platform &platform, cl_device_type type,
                            const DeviceConstraint *constraints = nullptr,
                            unsigned constraintCount = 0);

  /// Create the OpenCL context optionally applying the given device constraints.
  ///
  /// The selected context must have device(s) passing all the @p constraints given, each
  /// returning true, in order to be selected. If no context has devices passes all tests,
  /// then an invalid platform is returned. The first valid context is always returned
  /// including when there are no constraints.
  ///
  /// See @c clucontraint.h for some constraint examples.
  ///
  /// @param platform The OpenCL platform to create the context for.
  /// @param type The required OpenCL device type.
  /// @param constraints The array of device constraints to pass or null for no constraints.
  /// @return The first valid context found, passing all constraints.
  cl::Context createContext(const cl::Platform &platform, cl_device_type type,
                            const std::vector<DeviceConstraint> &constraints);
#endif  // #

/// Create the OpenCL platform and context optionally applying the given platform and
/// device constraints.
///
/// The selected platform must pass all @p platformConstraints while the selected
/// context must pass all @p deviceContraints.
///
/// See @c clucontraint.h for some constraint examples.
///
/// @param device_out Set to the selected OpenCL device.
/// @param type The required OpenCL device type.
/// @param platform_constraint The array of platform constraints to pass or null for no constraints.
/// @param platform_constraint_count The number of @p platformConstraint.
/// @param device_constraints The array of device constraints to pass or null for no constraints.
/// @param device_constraint_count The number of @p deviceConstraints.
/// @return The first valid context found, passing all constraints.
cl::Context createContext(cl::Device *device_out, cl_device_type type, const PlatformConstraint *platform_constraint,
                          unsigned platform_constraint_count, const DeviceConstraint *device_constraints,
                          unsigned device_constraint_count);

/// Initialise the primary OpenCL context. Retrieved via @c getPrimaryContext().
/// @param device The device to set.
/// @param context The context to set.
bool setPrimaryContext(const cl::Context &context, const cl::Device &device);
/// Releases and clears the primary context available from @c getPrimaryContext().
void clearPrimaryContext();
/// Initialise the primary context (@c getPrimaryContext()) according to the given constraints.
/// @param type The required OpenCL device type.
/// @param platform_constraint The array of platform constraints to pass or null for no constraints.
/// @param platform_constraint_count The number of @p platformConstraint.
/// @param device_constraints The array of device constraints to pass or null for no constraints.
/// @param device_constraint_count The number of @p deviceConstraints.
/// @return True if all constraints are met and the context initialised.
bool initPrimaryContext(cl_device_type type, const PlatformConstraint *platform_constraint,
                        unsigned platform_constraint_count, const DeviceConstraint *device_constraints,
                        unsigned device_constraint_count);
/// Initialise the primary context (@c getPrimaryContext()) according to the given constraints.
/// @param type The required OpenCL device type.
/// @param platform_constraint The array of platform constraints to pass or null for no constraints.
/// @param device_constraints The array of device constraints to pass or null for no constraints.
/// @return True if all constraints are met and the context initialised.
bool initPrimaryContext(cl_device_type type, const std::vector<PlatformConstraint> &platform_constraint,
                        const std::vector<DeviceConstraint> &device_constraints);
/// Get the stored primary context.
/// @param[out] context Set to the primary context.
/// @param device The OpenCL device to operate on.
/// @return True on success, false if not context has been initialised.
bool getPrimaryContext(cl::Context &context, cl::Device &device);

/// Read and initialise constraints from command line arguments.
///
/// All arguments described below must be provided in the form:
/// @code{.unparsed}
/// --<prefix><name>=<value>
///   or
/// --<prefix><name> <value>
/// @endcode
/// Where &lt;name&gt; matches an argument in the table below. The &lt;prefix&gt; must match
/// @p argPrefix. It is nothing by default.
///
/// Supports the following arguments:
/// Argument | Values               | Description
/// -------- | -------------------- | ---------------------------------------------------------
/// accel    | any, accel, cpu, gpu | The accelerator type. The default is all.
/// clver    | 1.0, 1.2, 2.0, ...   | The minimum required OpenCL version; @c deviceVersionMin()
/// device   | &lt;string&gt;       | Uses @c deviceNameLike() to match the device name.
/// platform | &lt;string&gt;       | Uses @c platformNameLike() to match the platform name.
/// vendor   | &lt;string&gt;       | Uses @c platformVendorLike() to match the platform vendor.
///
/// @param argc Number of arguments in @p argv.
/// @param argv Command line arguments.
/// @param[out] type Set to the requested @c accel type or @c CL_DEVICE_TYPE_ALL.
/// @param platform_constraints Populated with requested platform constraints.
/// @param device_constraints Populated with requested device constraints.
/// @param arg_prefix Optional prefix to required an all arguments described above.
void constraintsFromCommandLine(int argc, const char **argv, cl_device_type &type,
                                std::vector<PlatformConstraint> &platform_constraints,
                                std::vector<DeviceConstraint> &device_constraints, const char *arg_prefix = nullptr);

/// @overload
inline void constraintsFromCommandLine(int argc, char **argv, cl_device_type &type,
                                       std::vector<PlatformConstraint> &platform_constraints,
                                       std::vector<DeviceConstraint> &device_constraints,
                                       const char *arg_prefix = nullptr)
{
  return constraintsFromCommandLine(argc,
                                    const_cast<const char **>(argv),  // NOLINT(cppcoreguidelines-pro-type-const-cast)
                                    type, platform_constraints, device_constraints, arg_prefix);
}

/// @overload
void constraintsFromArgs(const std::list<std::string> &args, cl_device_type &type,
                         std::vector<PlatformConstraint> &platform_constraints,
                         std::vector<DeviceConstraint> &device_constraints, const char *arg_prefix = nullptr);

/// Parse an OpenCL version string. This supports both device and platform version strings of the form:
/// "OpenCL <major>.<minor>".
/// @param version_string The version string to parse.
/// @param[out] version_major The parsed major version.
/// @param[out] version_minor The parsed minor version.
/// @param True on success.
bool parseVersion(const char *version_string, cl_uint *version_major, cl_uint *version_minor);

/// Resolve the platform version.
/// @param platform The platform to get the version for.
/// @param[out] version_major The parsed major version.
/// @param[out] version_minor The parsed minor version.
void platformVersion(cl_platform_id platform, cl_uint *version_major, cl_uint *version_minor);

/// Convert a known OpenCL error code to an English string.
/// @param error The error code.
/// @return An English representation of @p error.
const char *errorCodeString(cl_int error);

/// Dump platform information about @p platform to @p out.
///
/// Lists:
/// - @c CL_PLATFORM_NAME
/// - @c CL_PLATFORM_VERSION
/// - @c CL_PLATFORM_VENDOR
/// @param out Stream to write to.
/// @param platform The platform to dump information about.
/// @param prefix Optional prefix to add to each line to @p out.
/// @param endl End of line character used.
void printPlatformInfo(std::ostream &out, const cl::Platform &platform, const char *prefix = "",
                       const char *endl = "\n");

/// Dump device information about @p device to @p out.
///
/// Lists:
/// - @c CL_DEVICE_NAME
/// - @c CL_DEVICE_VERSION
/// @param out Stream to write to.
/// @param device The device to dump information about.
/// @param prefix Optional prefix to add to each line to @p out.
/// @param endl End of line character used.
void printDeviceInfo(std::ostream &out, const cl::Device &device, const char *prefix, const char *endl = "\n");

/// A helper error logging function.
///
/// Logs an error message to @p out if @p clerr is not CL_SUCCESS.
/// The message is formatted:
/// @code{.unparsed}
///   out << "Error " << context [ << index << ] " : " clu::errorCodeString(clerr) << std::endl;
/// @endcode
/// Where the 'index' is conditionally inserted when it is not negative.
///
/// @param out The stream to log to.
/// @param clerr The error value.
/// @param context A context for the error string (user defined).
/// @param index An index associated with @p context - e.g., context = "arg" index = 3.
/// @return True if @p clerr is @c CL_SUCCESS, false otherwise.
inline bool checkError(std::ostream &out, cl_int clerr, const char *context, cl_uint index = ~cl_uint(0u))
{
  if (clerr != CL_SUCCESS)
  {
    out << "Error " << context;
    if (index != ~cl_uint(0u))
    {
      out << index;
    }
    out << " : " << clu::errorCodeString(clerr) << std::endl;
    return false;
  }
  return true;
}
}  // namespace clu

#endif  // CLU_H


================================================
FILE: clu/cluBuffer.h
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) CSIRO 2016
//
#ifndef CLUBUFFER_H
#define CLUBUFFER_H

#include "clu.h"

#include <cstring>

namespace clu
{
/// Calculate the best allocation size for @p requestedAllocationSize.
///
/// Allocations should be cache line aligned for optimal efficiency.
/// See https://software.intel.com/en-us/node/558501.
/// The allocation is padded to either @p byteSizeAlignment or to the cache line size when
/// @p byteSizeAlignment is zero.
///
/// @param context The device context to allocate for.
/// @param requested_allocation_size The requested allocation in bytes.
/// @param byte_size_alignment The requested data alignment. Zero to use the cache line size.
/// @return The padded/aligned allocation size.
inline size_t bestAllocationSize(const cl::Context &context, size_t requested_allocation_size,
                                 size_t byte_size_alignment = 0)
{
  size_t allocation_size = requested_allocation_size;

  if (byte_size_alignment == 0)
  {
    // Alignment padding not specified. Use the cache line size for optimal efficiency.
    // See https://software.intel.com/en-us/node/558501
    cl_device_id device_id = getFirstDevice(context);
    if (device_id)
    {
      clGetDeviceInfo(device_id, CL_DEVICE_GLOBAL_MEM_CACHELINE_SIZE, sizeof(byte_size_alignment), &byte_size_alignment,
                      nullptr);
    }
  }

  // Pad allocations as requested.
  if (byte_size_alignment)
  {
    const size_t size_mod = allocation_size % byte_size_alignment;
    if (size_mod)
    {
      allocation_size += (byte_size_alignment - allocation_size % byte_size_alignment);
    }
  }

  return allocation_size;
}

/// Ensure the given @p buffer is of a size sufficient to hold @p elementCount elements of @p T.
///
/// This ensures a minimum allocation of <tt>sizeof(T) * elementCount</tt> rounded up to the
/// given @p byteSizeAlignment.
///
/// @param buffer The buffer to operate on.
/// @param flags OpenCL memory flags for the buffer.
/// @param context The OpenCL context to allocate in. For existing allocated @p buffer objects
///   this must match the context the buffer has already been allocated within.
/// @param element_count The number of elements required.
/// @param error Error code return value.
/// @param byte_size_alignment The byte padding alignment for the allocation size.
///       Zero to ensure padding to the cache line size in the default device of
///       @p context.
/// @return True if the @p buffer has been allocated for the first time or reallocated.
/// False if the buffer has been left as is. Errors are reported via @p error.
template <typename T>
bool ensureBufferSize(cl::Buffer &buffer, cl_mem_flags flags, const cl::Context &context, size_t element_count,
                      cl_int *error = nullptr, cl_uint byte_size_alignment = 0)
{
  if (buffer())
  {
    size_t buffer_size = 0;
    clGetMemObjectInfo(buffer(), CL_MEM_SIZE, sizeof(buffer_size), &buffer_size, nullptr);
    if (buffer_size >= element_count * sizeof(T))
    {
      if (error)
      {
        *error = CL_SUCCESS;
      }
      return false;
    }

    //      if (bufferSize)
    //      {
    //        std::cout << "Free " << bufferSize << std::endl;
    //      }

    // Current buffer too small. Release existing buffer first (if any).
    buffer = cl::Buffer();
  }

  // Allocate the new buffer.
  cl_int clerr = 0;
  const size_t allocation_size = bestAllocationSize(context, sizeof(T) * element_count, byte_size_alignment);
  buffer = cl::Buffer(context, flags, allocation_size, nullptr, &clerr);
  //    std::cout << "Allocate " << allocationSize << std::endl;
  if (error)
  {
    *error = clerr;
  }
  return clerr == CL_SUCCESS;
}


/// Upload data to the GPU, creating the GPU buffer if required.
///
/// The GPU buffer is created, or resized if it does not support @p elementCount
/// elements. However, there may be a size mismatch, with the @p gpuBuffer elements
/// being larger than the @p cpuBuffer elements. This most notably comes about when
/// using 12 byte float3 data type and uploading to cl_float3 which is generally 16
/// bytes. As such, the @p GPUT must be at least as large as @p CPUT. The upload is
/// fastest when the sizes match exactly.
///
/// @param queue The command queue used to upload with.
/// @param context OpenCL context.
/// @param gpu_buffer GPU buffer object. Size may be increased as required, but not decreased.
/// @param cpu_buffer GPU buffer to upload from.
/// @param element_count The number of elements in @p cpuBuffer to copy into @p gpuBuffer.
/// @param allow_write When true, @p gpuBuffer is created with @c CL_MEM_READ_WRITE, otherwise
///     it is created with @c CL_MEM_READ_ONLY. Ignored if @p gpuBuffer is already initialised.
/// @param reference Reference message passed to @p clu::checkError() on error.
/// @param log The stream to log errors to.
/// @tparam GPUT The GPU buffer type to upload to.
/// @tparam CPUT The CPU buffer type to upload to.
template <typename GPUT, typename CPUT>
bool upload(cl::CommandQueue &queue, cl::Context &context, cl::Buffer &gpu_buffer, const CPUT *cpu_buffer,
            size_t element_count, bool allow_write, const char *reference, std::ostream &log = std::cerr)
{
  static_assert(sizeof(GPUT) >= sizeof(CPUT), "GPU type smaller than CPU type.");
  const CPUT *cpu = cpu_buffer;

  cl_int clerr = 0;
  const cl_mem_flags mem_flags = (allow_write) ? CL_MEM_READ_ONLY : CL_MEM_READ_WRITE;  // NOLINT(hicpp-signed-bitwise)
  clu::ensureBufferSize<GPUT>(gpu_buffer, mem_flags, context, element_count, &clerr);
  if (!clu::checkError(log, clerr, reference))
  {
    return false;
  }

  // Map target buffer.
  // NOLINTNEXTLINE(hicpp-signed-bitwise)
  GPUT *gpu_mem = static_cast<GPUT *>(clEnqueueMapBuffer(queue(), gpu_buffer(), CL_TRUE, CL_MAP_WRITE, 0,
                                                         sizeof(GPUT) * element_count, 0, nullptr, nullptr, &clerr));

  if (!clu::checkError(log, clerr, reference))
  {
    return false;
  }

  if (sizeof(CPUT) == sizeof(GPUT))
  {
    memcpy(gpu_mem, cpu, sizeof(CPUT) * element_count);
  }
  else
  {
    GPUT *gpu = gpu_mem;
    for (size_t i = 0; i < element_count; ++i, ++gpu, ++cpu)
    {
      memcpy(gpu, cpu, sizeof(CPUT));
    }
  }

  // Unmap buffer.
  clerr = clEnqueueUnmapMemObject(queue(), gpu_buffer(), gpu_mem, 0, nullptr, nullptr);
  return clu::checkError(log, clerr, reference);
}


/// Download data from an existing GPU buffer to a CPU buffer.
///
/// The size restrictions on @p GPUT and @p CPUT are the same as for @c upload().
/// Both buffers must support at least @p elementCount elements.
///
/// @param queue The command queue used to upload with.
/// @param cpu_buffer GPU buffer to upload from.
/// @param gpu_buffer GPU buffer object. Size may be increased as required, but not decreased.
/// @param element_count The number of elements in @p cpuBuffer to copy into @p gpuBuffer.
/// @param reference Reference message passed to @p clu::checkError() on error.
/// @param log The stream to log errors to.
/// @tparam GPUT The GPU buffer type to upload to.
/// @tparam CPUT The CPU buffer type to upload to.
template <typename GPUT, typename CPUT>
bool download(cl::CommandQueue &queue, CPUT *cpu_buffer, cl::Buffer &gpu_buffer, size_t element_count,
              const char *reference, std::ostream &log = std::cerr)
{
  static_assert(sizeof(GPUT) >= sizeof(CPUT), "GPU type smaller than CPU type.");
  CPUT *cpu = cpu_buffer;

  cl_int clerr = 0;
  // Map target buffer.
  GPUT *gpu_mem =
    static_cast<GPUT *>(clEnqueueMapBuffer(queue(), gpu_buffer(), CL_TRUE, CL_MAP_READ,  // NOLINT(hicpp-signed-bitwise)
                                           0, sizeof(GPUT) * element_count, 0, nullptr, nullptr, &clerr));

  if (!clu::checkError(log, clerr, reference))
  {
    return false;
  }

  if (sizeof(GPUT) == sizeof(CPUT))
  {
    memcpy(cpu, gpu_mem, sizeof(CPUT) * element_count);
  }
  else
  {
    GPUT *gpu = gpu_mem;
    for (size_t i = 0; i < element_count; ++i, ++gpu, ++cpu)
    {
      memcpy(cpu, gpu, sizeof(*cpu));
    }
  }

  // Unmap buffer.
  clerr = clEnqueueUnmapMemObject(queue(), gpu_buffer(), gpu_mem, 0, nullptr, nullptr);
  return clu::checkError(log, clerr, reference);
}
}  // namespace clu

#endif  // CLUBUFFER_H


================================================
FILE: clu/cluConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef CLUCONFIG_H
#define CLUCONFIG_H

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

// clang-format off
#define CL_HPP_MINIMUM_OPENCL_VERSION 120
#define CL_HPP_TARGET_OPENCL_VERSION @CLU_TARGET_OPENCL_VERSION@
// clang-format on

#endif  // CLUCONFIG_H


================================================
FILE: clu/cluConstraint.cpp
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) CSIRO 2016
//
#include "cluConstraint.h"

#include <algorithm>
#include <string>

namespace clu
{
PlatformConstraint platformNameLike(const char *name, bool ignore_case)
{
  std::string like_name(name);
  if (ignore_case)
  {
    std::transform(like_name.begin(), like_name.end(), like_name.begin(), ::tolower);
  }

  PlatformConstraint constraint = [like_name, ignore_case](const cl::Platform &platform) -> bool {
    std::string platform_name;
    platform.getInfo(CL_PLATFORM_NAME, &platform_name);
    if (ignore_case)
    {
      std::transform(platform_name.begin(), platform_name.end(), platform_name.begin(), ::tolower);
    }
    return platform_name.find(like_name) != std::string::npos;
  };

  return constraint;
}


PlatformConstraint platformVendorLike(const char *name, bool ignore_case)
{
  std::string like_name(name);
  if (ignore_case)
  {
    std::transform(like_name.begin(), like_name.end(), like_name.begin(), ::tolower);
  }

  PlatformConstraint constraint = [like_name, ignore_case](const cl::Platform &platform) -> bool {
    std::string vendor;
#ifndef NDEBUG
    std::string name;
    platform.getInfo(CL_PLATFORM_NAME, &name);
#endif  // NDEBUG
    platform.getInfo(CL_PLATFORM_VENDOR, &vendor);
    if (ignore_case)
    {
      std::transform(vendor.begin(), vendor.end(), vendor.begin(), ::tolower);
    }
    return vendor.find(like_name) != std::string::npos;
  };

  return constraint;
}


PlatformConstraint platformVersionMin(unsigned major, unsigned minor)
{
  PlatformConstraint constraint = [major, minor](const cl::Platform &platform) -> bool {
    cl_uint major_version = 0;
    cl_uint minor_version = 0;
    platformVersion(platform(), &major_version, &minor_version);
    return major_version > major || major_version == major && minor_version >= minor;
  };

  return constraint;
}


DeviceConstraint deviceVersionMin(unsigned major, unsigned minor)
{
  DeviceConstraint constraint = [major, minor](const cl::Platform &platform, const cl::Device &device) -> bool {
    (void)platform;  // unused
    ::size_t size = 0;
    clGetDeviceInfo(device(), CL_DEVICE_VERSION, 0, nullptr, &size);
    cl::string version_info;
    version_info.resize(size + 1);
    clGetDeviceInfo(device(), CL_DEVICE_VERSION, size, &version_info[0], &size);

    cl_uint major_version = 0;
    cl_uint minor_version = 0;
    if (!parseVersion(version_info.c_str(), &major_version, &minor_version))
    {
      return false;
    }
    return major_version > major || major_version == major && minor_version >= minor;
  };

  return constraint;
}


DeviceConstraint deviceNameLike(const char *name, bool ignore_case)
{
  std::string like_name(name);
  if (ignore_case)
  {
    std::transform(like_name.begin(), like_name.end(), like_name.begin(), ::tolower);
  }

  DeviceConstraint constraint = [like_name, ignore_case](const cl::Platform &platform,
                                                         const cl::Device &device) -> bool {
    (void)platform;  // unused
    std::string device_name;
    device.getInfo(CL_DEVICE_NAME, &device_name);
    if (ignore_case)
    {
      std::transform(device_name.begin(), device_name.end(), device_name.begin(), ::tolower);
    }
    return device_name.find(like_name) != std::string::npos;
  };

  return constraint;
}


DeviceConstraint deviceVendorLike(const char *name, bool ignore_case)
{
  std::string like_name(name);
  if (ignore_case)
  {
    std::transform(like_name.begin(), like_name.end(), like_name.begin(), ::tolower);
  }

  DeviceConstraint constraint = [like_name, ignore_case](const cl::Platform &platform,
                                                         const cl::Device &device) -> bool {
    std::string platform_vendor;
    std::string device_vendor;
    platform.getInfo(CL_PLATFORM_VENDOR, &platform_vendor);
    device.getInfo(CL_DEVICE_VENDOR, &device_vendor);
    if (ignore_case)
    {
      std::transform(platform_vendor.begin(), platform_vendor.end(), platform_vendor.begin(), ::tolower);
      std::transform(device_vendor.begin(), device_vendor.end(), device_vendor.begin(), ::tolower);
    }
    return device_vendor.find(like_name) != std::string::npos || platform_vendor.find(like_name) != std::string::npos;
  };

  return constraint;
}
}  // namespace clu


================================================
FILE: clu/cluConstraint.h
================================================
#ifndef CLUCONSTRAINT_H
#define CLUCONSTRAINT_H

#include "clu.h"

namespace clu
{
/// Create a platform constraint requiring the @c CL_PLATFORM_NAME is line @p name.
///
/// To pass the @c CL_PLATFORM_NAME must contain the @p name string optionally ignoring
/// case.
/// @param name The partial name match required.
/// @param ignore_case True for case insensitive checks.
/// @return The platform constraint function object.
PlatformConstraint platformNameLike(const char *name, bool ignore_case);

/// Create a platform constraint requiring the @c CL_PLATFORM_VENDOR is line @p name.
///
/// To pass the @c CL_PLATFORM_VENDOR must contain the @p name string optionally ignoring
/// case.
/// @param name The partial name match required.
/// @param ignore_case True for case insensitive checks.
/// @return The platform constraint function object.
PlatformConstraint platformVendorLike(const char *name, bool ignore_case);

/// Create a platform version constraint.
/// The platform must have an OpenCL version at least as high as @p major, @p minor version
/// The version may be higher.
///
/// For example, @c platformVersionMin(1, 2) creates a constraint requiring OpenCL 1.2 or
/// higher. Note: a platform constraint to at least OpenCL 1.2 should always be added as
/// this is the minimum supported API version.
///
/// @param major The minimum required major version.
/// @param minor The minimum required minor version.
/// @return The platform constraint function object.
PlatformConstraint platformVersionMin(unsigned major, unsigned minor);

/// Create a device version constraint.
/// The device must have an OpenCL version at least as high as @p major, @p minor version
/// The version may be higher.
///
/// For example, @c deviceVersionMin(2, 0) creates a constraint requiring OpenCL 2.0 or
/// higher.
///
/// @param major The minimum required major version.
/// @param minor The minimum required minor version.
/// @return The device constraint function object.
DeviceConstraint deviceVersionMin(unsigned major, unsigned minor);

/// Create a device constraint requiring the @c CL_DEVICE_NAME is line @p name.
///
/// To pass the @c CL_DEVICE_NAME must contain the @p name string optionally ignoring
/// case.
/// @param name The partial name match required.
/// @param ignore_case True for case insensitive checks.
/// @return The device constraint function object.
DeviceConstraint deviceNameLike(const char *name, bool ignore_case);

/// Create a device constraint requiring the @c CL_PLATFORM_VENDOR is line @p name.
///
/// To pass the @c CL_DEVICE_VENDOR must contain the @p name string optionally ignoring
/// case.
/// @param name The partial name match required.
/// @param ignore_case True for case insensitive checks.
/// @return The device constraint function object.
DeviceConstraint deviceVendorLike(const char *name, bool ignore_case);
}  // namespace clu

#endif


================================================
FILE: clu/cluKernel.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "cluKernel.h"

#include "cluProgram.h"

namespace clu
{
KernelSize KernelGrid::adjustedGlobal() const
{
  if (!isValid())
  {
    return KernelSize();
  }

  KernelSize adjusted;
  for (int i = 0; i < int(global_size.dimensions()); ++i)
  {
    adjusted[i] = work_group_size[i] * ((global_size[i] + work_group_size[i] - 1) / work_group_size[i]);
  }

  return adjusted;
}

Kernel::Kernel() = default;


Kernel::Kernel(cl::Program &program, const char *entry_point, std::ostream *log)
{
  setEntry(program, entry_point, log);
}


Kernel::Kernel(cl::Kernel &cl_kernel)
  : kernel_(cl_kernel)
{}


bool Kernel::isValid() const
{
  return kernel_() != nullptr;
}


cl_int Kernel::setEntry(cl::Program &program, const char *entry_point, std::ostream *log)
{
  std::ostream &out = (log) ? *log : std::cerr;
  cl_int clerr = CL_SUCCESS;
  kernel_ = cl::Kernel(program, entry_point, &clerr);

  if (clerr != CL_SUCCESS)
  {
    out << "Failed to resolve kernel " << entry_point << "() : " << clu::errorCodeString(clerr) << '\n';
    return clerr;
  }

  return clerr;
}


cl_int Kernel::addLocal(const LocalMemArgSizeFunc &arg_func)
{
  if (local_mem_arg_count_ < kMaxLocalMemArgs)
  {
    local_mem_args_[local_mem_arg_count_++] = arg_func;  // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index)

    return local_mem_arg_count_;
  }

  return -1;
}


size_t Kernel::calculateOptimalWorkGroupSize()
{
  cl_int clerr = CL_SUCCESS;

  optimal_work_group_size_ = 1;

  if (local_mem_arg_count_)
  {
    clu::LocalMemCalcFunc local_mem_func = [this](size_t work_group_size) -> size_t {
      size_t mem_size = 0;
      for (int i = 0; i < local_mem_arg_count_; ++i)
      {
        // Lint: already bounds checked.
        // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
        mem_size += local_mem_args_[i](work_group_size);
      }

      return mem_size;
    };

    optimal_work_group_size_ = clu::maxWorkgroupSize(kernel_, local_mem_func, &clerr);
  }
  else
  {
    optimal_work_group_size_ = clu::maxWorkgroupSize(kernel_, &clerr);
  }

  return optimal_work_group_size_;
}


cl_int Kernel::setLocalMemArgs(int arg_count)
{
  int arg_index = (!local_mem_first_) ? arg_count : 0;
  cl_int clerr = CL_SUCCESS;

  for (int i = 0; i < local_mem_arg_count_; ++i)
  {
    if (local_mem_args_[i])  // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index)
    {
      // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
      clerr = kernel_.setArg(arg_index++, cl::Local(local_mem_args_[i](optimal_work_group_size_)));
    }
    else
    {
      clerr = kernel_.setArg(arg_index++, cl::Local(0));
    }

    if (clerr != CL_SUCCESS)
    {
      return clerr;
    }
  }

  return clerr;
}


cl_int Kernel::invoke(cl::CommandQueue &queue, const KernelGrid &grid, const EventList &events)
{
  // Copy wait on event into a local stack list.
  cl_event *wait_on_events = nullptr;
  if (events.event_count)
  {
    wait_on_events = static_cast<cl_event *>(alloca(sizeof(cl_event) * events.event_count));
    for (unsigned i = 0; i < events.event_count; ++i)
    {
      wait_on_events[i] = events.wait_on_events[i]();
    }
  }

  cl_event local_event{};

  // Invoke the kernel.
  cl_int clerr =
    clEnqueueNDRangeKernel(queue(), kernel_(),
                           grid.global_size.dimensions(),                                      // Dimensions
                           !grid.global_offset.isNull() ? grid.global_offset.arg() : nullptr,  // Global offset
                           grid.adjustedGlobal().arg(),                                        // Global size
                           grid.work_group_size.arg(),                                         // Work group size
                           events.event_count, wait_on_events, events.completion ? &local_event : nullptr);
  if (clerr == CL_SUCCESS && events.completion)
  {
    *events.completion = local_event;
  }

  return clerr;
}
}  // namespace clu


================================================
FILE: clu/cluKernel.h
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) CSIRO 2017
//
#ifndef CLUKERNEL_H
#define CLUKERNEL_H

#include "clu.h"

#include <array>
#include <functional>
#include <iostream>

namespace clu
{
/// Helper class for setting OpenCL kernel arguments based on their type
///
/// The default implementation is to call @c cl::Kernel::setArg() with the given value.
template <typename T>
struct KernelArgHandler
{
  /// Set a kernel argument.
  /// @param kernel The OpenCL kernel to set an argument for.
  /// @param arg_index Index of the argument to set.
  /// @param arg The argument value.
  static cl_int set(cl::Kernel &kernel, int arg_index, const T &arg) { return kernel.setArg(arg_index, arg); }
};

/// Explicitly handle @c cl_mem type. Changing to the C++ <tt>cl2.hpp</tt> API changed how @c cl::Kernel handled
/// pointer arguments. In practice it expects all buffer arguments to be of C++ @c cl::Buffer, @c cl::Image,
/// @c cl::Pipe, etc types. Any pointer invokes @c clSetKernelArgSVMPointer() instead of @c clSetKernelArg() which
/// also captures @c cl_mem type as it is a pointer typedef. This results in incorrect behaviour and we correct
/// for this by calling the C function @c clSetKernelArg explicitly here.
template <>
struct KernelArgHandler<cl_mem>
{
  /// Set a kernel argument.
  /// @param kernel The OpenCL kernel to set an argument for.
  /// @param arg_index Index of the argument to set.
  /// @param arg The argument value.
  static cl_int set(cl::Kernel &kernel,

                    int arg_index, cl_mem arg)
  {
    // Disable lint warning - we do mean to take the size of a pointer.
    // NOLINTNEXTLINE(bugprone-sizeof-expression)
    return ::clSetKernelArg(kernel(), arg_index, sizeof(arg), &arg);
  }
};


template <typename T>
cl_int setKernelArgs2(cl::Kernel &kernel, int &arg_index, const T &arg)
{
  const cl_int clerr = KernelArgHandler<T>::set(kernel, arg_index, arg);
  ++arg_index;
  clu::checkError(std::cerr, clerr, "Arg", arg_index);
  return clerr;
}


template <typename T, typename... ARGS>
cl_int setKernelArgs2(cl::Kernel &kernel, int &arg_index, const T &arg, ARGS... args)
{
  const cl_int clerr = KernelArgHandler<T>::set(kernel, arg_index, arg);
  ++arg_index;
  clu::checkError(std::cerr, clerr, "Arg", arg_index);
  const cl_int clerr_other = setKernelArgs2(kernel, arg_index, args...);
  if (clerr == CL_SUCCESS)
  {
    return clerr_other;
  }
  return clerr;
}


/// Sets the arguments for @p kernel to the passed arguments, in order.
/// Each of the @p args is passed to @c cl::Kernel::setArg() in turn, modifying the indexing as
/// required. On any argument failure, the error is reported to @c std::cerr. Each argument is
/// set regardless of early failures, so each failure will be logged.
///
/// The function then returns either @c CL_SUCCESS or the first failure error value.
///
/// @param kernel The kernel to set arguments for.
/// @param args The arguments to pass. Captured by const reference.
/// @return @c CL_SUCCESS on success or the first failure error code on failure.
template <typename... ARGS>
cl_int setKernelArgs(cl::Kernel &kernel, ARGS... args)
{
  int arg_index = 0;
  return setKernelArgs2(kernel, arg_index, args...);
}

/// Class which defines the size and dimensions of a kernel local or global size.
class KernelSize
{
public:
  /// Size dimensionality.
  enum Dimension : unsigned
  {
    kDNull,  ///< Not set
    kD1D,    ///< One dimensional size.
    kD2D,    ///< Two dimensional size.
    kD3D     ///< Three dimensional size.
  };

  /// Intiantite a zero size, null dimensinoed kernel.
  inline KernelSize() = default;

  /// Instantiate a 1D kernel size.
  /// @param a Size of the first dimension.
  inline KernelSize(size_t a)  // NOLINT(google-explicit-constructor)
  {
    sizes_[0] = a;
    sizes_[1] = sizes_[2] = 0;
  }

  /// Instantiate a 2D kernel size.
  /// @param a Size of the first dimension.
  /// @param b Size of the second dimension.
  inline KernelSize(size_t a, size_t b)
  {
    sizes_[0] = a;
    sizes_[1] = b;
    sizes_[2] = 0;
  }

  /// Instantiate a 3D kernel size.
  /// @param a Size of the first dimension.
  /// @param b Size of the second dimension.
  /// @param c Size of the third dimension.
  inline KernelSize(size_t a, size_t b, size_t c)
  {
    sizes_[0] = a;
    sizes_[1] = b;
    sizes_[2] = c;
  }

  /// Copy constructor.
  /// @param other Object to copy.
  inline KernelSize(const KernelSize &other) { *this = other; }

  inline ~KernelSize() = default;

  /// Query the dimensionality of the size specification.
  ///
  /// The dimensionality is determined by the first zero value in the size specification. A zero size in the first
  /// dimension implies @c kDNull , a zero size in the second implies @c kD1D , etc.
  /// @return The dimensionality of this size specification.
  inline Dimension dimensions() const
  {
    return (sizes_[2]) ? kD3D : ((sizes_[1]) ? kD2D : ((sizes_[0]) ? kD1D : kDNull));
  }

  /// Query the size of the selected dimension.
  /// @param dim The dimension to query in the range `[0, 2]`.
  /// @return The size of the queried dimension.
  inline size_t size(int dim = 0) const
  {
    // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
    return sizes_[dim];
  }

  /// Query the size of the indexed dimension.
  /// @param dim The dimension to query in the range `[0, 2]`.
  /// @return The size of the queried dimension.
  inline size_t operator[](int dim) const
  {
    // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
    return sizes_[dim];
  }
  /// Get a reference to the size of the indexed dimension.
  /// @param dim The dimension to query in the range `[0, 2]`.
  /// @return The size of the queried dimension.
  inline size_t &operator[](int dim)
  {
    // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
    return sizes_[dim];
  }

  /// Convert the @c KernelSize for use in kernel invocation.
  /// @return A pointer to the size array.
  inline const size_t *arg() const { return sizes_.data(); }

  /// Query if the size specification is null.
  /// @return True if the dimensionality is @c kDNull .
  inline bool isNull() const { return dimensions() == kDNull; }
  /// Query if the size specification is valid.
  /// @return True if the dimensionality is not @c kDNull .
  inline bool isValid() const { return !isNull(); }

  /// Calculate the product of the sizes to calculate a kernel "volume".
  /// @return The product of the dimension values.
  inline size_t volume() const
  {
    size_t total = sizes_[0];
    total *= dimensions() > kD1D ? sizes_[1] : 1;
    total *= dimensions() > kD2D ? sizes_[2] : 1;
    return total;
  }

  /// Assignment operator.
  /// @param other Object to assign
  /// @return `*this`
  inline KernelSize &operator=(const KernelSize &other)  // NOLINT(bugprone-unhandled-self-assignment)
  {
    // Note: self assignment will be fine.
    sizes_[0] = other.sizes_[0];
    sizes_[1] = other.sizes_[1];
    sizes_[2] = other.sizes_[2];
    return *this;
  }

  /// @c kD1 or @c kDNull assignment operator. Assigning a non zero value sets the size specification to a @c k1D
  /// specification of @p size . Setting a zero value makes this a @c kDNull specification.
  /// @param size The size value to assign.
  /// @return `*this`
  inline KernelSize &operator=(size_t size)
  {
    sizes_[0] = size;
    sizes_[1] = sizes_[2] = 0;
    return *this;
  }

private:
  std::array<size_t, 3> sizes_ = { 0, 0, 0 };
};


/// A helper structure for setting the execution size of a kernel.
///
/// Specifies a @c global_offset , applied to the global kernel size queries in OpenCL, a @c global_size controlling
/// the total number of GPU threads and a @c work_group_size setting the number of threads in each work group.
/// The dimensionality of all must match excepting that @c global_offset may be null implying a zero offset.
///
/// The @c global_size is used as a guide, but will be adjusted up by @c adjustGlobal() to be a multiple of the
/// @c work_group_size .
struct KernelGrid
{
  KernelSize global_offset;    ///< Global kernel size offset.
  KernelSize global_size;      ///< Global kernel size
  KernelSize work_group_size;  ///< Work group size.

  /// Default constructor. All size values are null.
  inline KernelGrid() = default;

  /// Create a kernel grid speficiation.
  /// @param global_offset The kernel global offset value. May be null
  /// @param global_size The global thread size.
  /// @param work_group_size The workgroup thread size.
  inline KernelGrid(const KernelSize &global_offset, const KernelSize &global_size, const KernelSize &work_group_size)
    : global_offset(global_offset)
    , global_size(global_size)
    , work_group_size(work_group_size)
  {}

  /// Create a kernel grid specification with implied zero offset.
  /// @param global_size The global thread size.
  /// @param work_group_size The workgroup thread size.
  inline KernelGrid(const KernelSize &global_size, const KernelSize &work_group_size)
    : KernelGrid(KernelSize(), global_size, work_group_size)
  {}

  /// Calculates an adjusted global size such that it is a multiple of the work group size.
  /// @return A global size such that it is a multiple of the work group size in each valid dimension.
  KernelSize adjustedGlobal() const;

  /// Validate the grid specification.
  ///
  /// Requires that the dimensionality of the global and workgroup sizes match. The global offset must also match
  /// if specified, but may be null.
  /// @return True if the specification is valid.
  inline bool isValid() const
  {
    return global_size.isValid() && work_group_size.isValid() &&
           global_size.dimensions() == work_group_size.dimensions() &&
           (global_size.dimensions() == global_offset.dimensions() || global_offset.isNull());
  }
};

/// Encapsulates events to complete before a kernel can execute as well as exposing an event to mark
/// kernel completion.
///
/// Used as an argument to @c Kernel::operator() to aid in overloading event setup.
struct EventList
{
  /// The even object to be used to mark completion of the queued operation (optional).
  cl::Event *completion = nullptr;
  cl::Event *wait_on_events = nullptr;  ///< Events to wait on before continuing the execution queue (optional).
  unsigned event_count = 0;             ///< Number of items in @c wait_on_events.

  /// Create an empty event list.
  inline EventList() = default;

  /// Create an event list with only a completion event.
  /// @param completion_event Event object to use to mark queue completion for the requested operation.
  explicit inline EventList(cl::Event *completion_event)
    : completion(completion_event)
  {}

  /// Create an event list to wait on with optional completion.
  /// @param wait_on Events to wait on before continuing queue execution.
  /// @param event_count Number of items in @p wait_on .
  /// @param completion_event Optional event object to use to mark queue completion for the requested operation.
  inline EventList(cl::Event *wait_on, unsigned event_count, cl::Event *completion_event = nullptr)
    : completion(completion_event)
    , wait_on_events(wait_on)
    , event_count(event_count)
  {}
};


/// A utility class for referencing, setting up and invoking OpenCL kernels.
///
/// @par Local Memory Arguments
/// The @c Kernel class supports a fixed number of local memory arguments appearing either before
/// all other arguments, or after all other arguments. The kernel may have at most @c MAX_LOCAL_MEM_ARGS
/// such arguments. Each local memory argument has an associated @c LocalMemArgSizeFunc function
/// object, which is used to determine the size of the local memory argument (bytes).
/// Each size function is invoked passing the local work group size allowing the function to determine
/// the required size on a per thread basis.
///
/// All local memory arguments must be registed sequentially using @p addLocal() before calling
/// @c calculateOptimalWorkGroupSize(). The @c calculateOptimalWorkGroupSize() function also uses
/// the function object to determine the best local size for the kernel maximising occupancy,
/// partly based on local memory requirements.
///
/// By default all local memory arguments are assumed to come after other arguments passed to the
/// @c operator(), but this may be changed by calling @c setLocalMemFirst().
class Kernel
{
public:
  /// Maximum supported number of local memory arguments which may be added with @c addLocal() .
  static const int kMaxLocalMemArgs = 8;

  /// Typedef for functional objects used to calculate the size of a local memory argument.
  /// The argument passed is the work group size while the return value specifies the
  /// required local memory for the corresponding work group size (bytes).
  using LocalMemArgSizeFunc = std::function<size_t(size_t)>;

  /// Default constructor.
  Kernel();
  /// Create a kernel from @p program with the given @p entry_point (kernel function) name.
  /// @param program Compiled program object to find the kernel entry point in.
  /// @param entry_point Kernel function name to resolve in @p program .
  /// @param log Optional error logging stream.
  Kernel(cl::Program &program, const char *entry_point, std::ostream *log = nullptr);
  /// Constructor to wrap an existing OpenCL C++ API kernel object.
  /// @param cl_kernel The kernel object to wrap.
  explicit Kernel(cl::Kernel &cl_kernel);

  /// Is this a valid kernel?
  bool isValid() const;

  /// Set the kernel entry point.
  /// @param program The program to find the kernel in.
  /// @param entry_point The kernel entry point/function name.
  /// @param log Optional error logging stream.
  /// @return @c CL_SUCCESS on success, an OpenCL error code otherwise.
  cl_int setEntry(cl::Program &program, const char *entry_point, std::ostream *log = nullptr);

  /// Set local memory arguments ordering. See class notes.
  /// @param first True to have local memory arguments before other arguments.
  inline void setLocalMemFirst(bool first) { local_mem_first_ = first; }

  /// Do local memory arguments come before other arguments?
  /// @return True if local memory arguments appear before other arguments.
  inline bool localMemFirst() const { return local_mem_first_; }

  /// Adds a local memory argument whose is calculated by invoking @p argFunc.
  ///
  /// See class notes on local memory arguments.
  ///
  /// Note: no more than 8 local arguments arg supported.
  ///
  /// @param arg_func The function to invoke when calculating the local memory size (bytes).
  /// @return The index of the local argument. This is not the same as the final argument index,
  ///   just the index into the set of local arguments, zero based. -1 on failure.
  cl_int addLocal(const LocalMemArgSizeFunc &arg_func);

  /// Calculates the optimal work group size for the kernel based on the compiled code and local memory usage.
  ///
  /// This calculates the work group size which achieves maximum occupancy. This is affected by
  /// the device characteristics and the local memory requirements calculated using the functions registed
  /// calling @c addLocal().
  ///
  /// The resulting size is one dimensional, so it is up to user code to determine the best subdivisions
  /// when using multiple dimensions.
  ///
  /// The result is returned, but also stored and accessible by calling @c optimalWorkGroupSize().
  ///
  /// @return The calculated, optimal size.
  size_t calculateOptimalWorkGroupSize();

  /// Queries the optimal work group size calculated by @c calculateOptimalWorkGroupSize().
  /// Zero if not calculated yet.
  /// @return The optimal work group size for maximum occupancy.
  inline size_t optimalWorkGroupSize() const { return optimal_work_group_size_; }

  /// Invocation operator. This invoked the kernel passing the given template arguments.
  ///
  /// The operator requires three standard arguments to specify the @c cl::CommandQueue,
  /// the execution offset and sizes (@c KernelGrid), the events preceeding execution and
  /// the completion event. Both preceeding and completion events are wrapped up in the
  /// @c events argument. Arguments following these standard arguments are passed to the
  /// kernel itself.
  ///
  /// Local memory arguments may also be set up using the function objects previously passed
  /// to @c addLocal() calls. See class notes.
  ///
  /// The @c EventList object is expected to be transient and may be:
  /// - @c EventList() empty for no preceeding events and no completion event.
  /// - @c EventList(cl::Event *) a single event pointer used to resolve the event to mark completion
  /// - @c EventList(cl::Event *, size_t, cl::Event *) to specify a list of events to wait on
  ///   (first argument), the number of items in the list and the completion event.
  ///
  /// Note that this is a non-blocking function and the kernel will not have completed on return.
  ///
  /// @param queue The command queue to execute on.
  /// @param grid The execution sizes.
  /// @param events Event list to wait on and completion events.
  /// @param args Arguments to pass to the kernel.
  /// @return @c CL_SUCCESS on success, or an error code on failure.
  template <typename... ARGS>
  cl_int operator()(cl::CommandQueue &queue, const KernelGrid &grid, const EventList &events, ARGS... args)
  {
    cl_int clerr = preInvoke(args...);
    if (clerr != CL_SUCCESS)
    {
      return clerr;
    }
    return invoke(queue, grid, events);
  }

  /// Access the internal OpenCL kernel object.
  /// @return The kernel object.
  inline cl::Kernel &kernel() { return kernel_; }
  /// Access the internal OpenCL kernel object.
  /// @return The kernel object.
  inline const cl::Kernel &kernel() const { return kernel_; }

private:
  /// Preinvocation setup.
  /// @param args Arguments to pass to the kernel.
  template <typename... ARGS>
  inline cl_int preInvoke(ARGS... args)
  {
    int arg_count = (!local_mem_first_) ? 0 : local_mem_arg_count_;
    cl_int clerr = setKernelArgs2(kernel_, arg_count, args...);
    if (clerr != CL_SUCCESS)
    {
      return clerr;
    }
    clerr = setLocalMemArgs(arg_count);
    if (clerr != CL_SUCCESS)
    {
      return clerr;
    }
    return clerr;
  }

  /// Setup local memory arguments.
  /// @param arg_count Number of non-local mem args when @c _localMemFirst is false, otherwise it's the total args.
  /// @return @c CL_SUCCESS on success, an OpenCL error code on failure.
  cl_int setLocalMemArgs(int arg_count);

  cl_int invoke(cl::CommandQueue &queue, const KernelGrid &grid, const EventList &events);

  cl::Kernel kernel_;
  size_t optimal_work_group_size_ = 0;
  std::array<LocalMemArgSizeFunc, kMaxLocalMemArgs> local_mem_args_;
  int local_mem_arg_count_ = 0;
  bool local_mem_first_ = false;
};
}  // namespace clu

#endif  // CLUKERNEL_H


================================================
FILE: clu/cluProgram.cpp
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) CSIRO 2016
//
#include "cluProgram.h"

#include "clu.h"

#include <array>
#include <fstream>
#include <ostream>
#include <sstream>

#ifdef WIN32
#define WIN32_LEAN_AND_MEAN
#include <Windows.h>
#else  // WIN32
#include <unistd.h>
#endif  // WIN32

#ifdef __APPLE__
#include <mach-o/dyld.h>
#endif  // __APPLE__

namespace
{
const size_t kMaxPath = 2048u;
}  // namespace

namespace clu
{
void printBuildLog(std::ostream &out, const cl::Program &program, const cl::Device &device, int filter_status)
{
  size_t log_size = 0;
  std::string log;
  std::string device_name;
  cl_build_status status{};

  if (filter_status <= 0)
  {
    clGetProgramBuildInfo(program(), device(), CL_PROGRAM_BUILD_STATUS, sizeof(status), &status, nullptr);
    if (status != filter_status)
    {
      return;
    }
  }

  clGetProgramBuildInfo(program(), device(), CL_PROGRAM_BUILD_LOG, 0, nullptr, &log_size);
  if (log.size() < log_size + 1)
  {
    log.resize(log_size + 1);
  }
  clGetProgramBuildInfo(program(), device(), CL_PROGRAM_BUILD_LOG, sizeof(*log.data()) * log.size(), &log.at(0),
                        nullptr);
  device.getInfo(CL_DEVICE_NAME, &device_name);
  out << device_name << '\n';
  out << log << std::endl;
}


void printBuildLogs(std::ostream &out, const cl::Program &program, const cl::Context &context, int filter_status)
{
  size_t log_size = 0;
  std::string log;
  std::string device_name;
  std::vector<cl::Device> devices;
  cl_build_status status{};

  listDevices(devices, context);

  for (cl::Device &device : devices)
  {
    if (filter_status <= 0)
    {
      clGetProgramBuildInfo(program(), device(), CL_PROGRAM_BUILD_STATUS, sizeof(status), &status, nullptr);
      if (status != filter_status)
      {
        continue;
      }
    }

    clGetProgramBuildInfo(program(), device(), CL_PROGRAM_BUILD_LOG, 0, nullptr, &log_size);
    if (log.size() < log_size + 1)
    {
      log.resize(log_size + 1);
    }
    clGetProgramBuildInfo(program(), device(), CL_PROGRAM_BUILD_LOG, sizeof(*log.data()) * log.size(), &log.at(0),
                          nullptr);
    device.getInfo(CL_DEVICE_NAME, &device_name);
    out << device_name << '\n';
    out << log << std::endl;
  }
}

unsigned listDevices(std::vector<cl::Device> &devices, const cl::Program &program)
{
  cl_uint device_count = 0;
  devices.clear();
  cl_int clerr = clGetProgramInfo(program(), CL_PROGRAM_NUM_DEVICES, sizeof(device_count), &device_count, nullptr);

  if (device_count == 0 || clerr != CL_SUCCESS)
  {
    return 0;
  }

  auto *device_ids = static_cast<cl_device_id *>(alloca(sizeof(cl_device_id) * device_count));
  clerr = clGetProgramInfo(program(), CL_PROGRAM_DEVICES, sizeof(cl_device_id) * device_count, device_ids, nullptr);
  if (clerr != CL_SUCCESS)
  {
    return 0;
  }

  devices.resize(device_count);
  for (cl_uint i = 0; i < device_count; ++i)
  {
    devices[i] = cl::Device(device_ids[i]);
  }

  return device_count;
}

size_t maxWorkgroupSize(const cl::Kernel &kernel, const LocalMemCalcFunc &local_mem_func, cl_int *err)
{
  cl_program program = nullptr;
  cl_int clerr = clGetKernelInfo(kernel(), CL_KERNEL_PROGRAM, sizeof(cl_program), &program, nullptr);

  if (clerr != CL_SUCCESS)
  {
    if (err)
    {
      *err = clerr;
    }
    return 0;
  }

  cl_uint device_count = 0;
  clerr = clGetProgramInfo(program, CL_PROGRAM_NUM_DEVICES, sizeof(device_count), &device_count, nullptr);

  if (device_count == 0 || clerr != CL_SUCCESS)
  {
    return 0;
  }

  auto *device_ids = static_cast<cl_device_id *>(alloca(sizeof(cl_device_id) * device_count));
  clerr = clGetProgramInfo(program, CL_PROGRAM_DEVICES, sizeof(cl_device_id) * device_count, device_ids, nullptr);
  if (clerr != CL_SUCCESS)
  {
    if (err)
    {
      *err = clerr;
    }
    return 0;
  }

  size_t max_items = 0;

  // Iterate looking for a compatible device. I've had the experience that the program is
  // associated with multiple devices, but just taking the first device results in a
  // CL_INVALID_DEVICE result from clGetKernelWorkGroupInfo(). This strategy
  // works around the issue, ensuring success.
  for (cl_uint i = 0; max_items == 0 && i < device_count; ++i)
  {
    cl_device_id device = device_ids[i];

    // size_t maxItems1 = 0;
    cl_kernel kernel_obj = kernel();
    // Interesting note: using Apple OpenCL 1.2 and the CPU driver can give what seems to be
    // odd results here for CL_KERNEL_WORK_GROUP_SIZE. Specifically I noted that it kept being
    // reported as 1. Playing with the OpenCL code, I found that it dropped from 'reasonable values'
    // (say 128) to 1 as soon as the OpenCL call included a call to barrier(), either local or global.
    // I guess that the thread synchronisation for this implementation is not very good, so it simply
    // drops the resolution back to avoid having any synchronisation code.
    clerr =
      clGetKernelWorkGroupInfo(kernel_obj, device, CL_KERNEL_WORK_GROUP_SIZE, sizeof(max_items), &max_items, nullptr);
    if (clerr != CL_SUCCESS)
    {
      if (clerr == CL_INVALID_DEVICE)
      {
        // Try the next device.
        continue;
      }

      if (err)
      {
        *err = clerr;
      }
      return 0;
    }

    if (local_mem_func)
    {
      const size_t upper_limit = max_items;
      size_t last_limit = 0;
      size_t local_mem_used = 0;
      cl_ulong kernel_local_memory = 0;
      cl_ulong kernel_private_memory = 0;
      cl_ulong available_local_memory = 0;

      // Get the base memory used by the kernel.
      clerr = clGetKernelWorkGroupInfo(kernel_obj, device, CL_KERNEL_LOCAL_MEM_SIZE, sizeof(kernel_local_memory),
                                       &kernel_local_memory, nullptr);
      if (clerr != CL_SUCCESS)
      {
        if (err)
        {
          *err = clerr;
        }
        return 0;
      }
      clerr = clGetKernelWorkGroupInfo(kernel_obj, device, CL_KERNEL_PRIVATE_MEM_SIZE, sizeof(kernel_private_memory),
                                       &kernel_private_memory, nullptr);
      if (clerr != CL_SUCCESS)
      {
        if (err)
        {
          *err = clerr;
        }
        return 0;
      }

      // Get the max local memory.
      clerr = clGetDeviceInfo(device, CL_DEVICE_LOCAL_MEM_SIZE, sizeof(available_local_memory), &available_local_memory,
                              nullptr);
      if (clerr != CL_SUCCESS)
      {
        if (err)
        {
          *err = clerr;
        }
        return 0;
      }

      if (available_local_memory < kernel_local_memory + kernel_private_memory)
      {
        // Failed.
        return 0;
      }

      // Subtract the kernel overhead.
      available_local_memory -= kernel_local_memory;
      available_local_memory -= kernel_private_memory;

      // Ensure some overhead.
      if (kernel_local_memory == 0 && kernel_private_memory == 0)
      {
        available_local_memory -= 1;
      }

      // Loop to find a reasonable value.
      const int loop_limit = 8;
      int loops = 0;

      // Prime the loop.
      max_items = max_items << 1u;
      do
      {
        last_limit = max_items;
        max_items = max_items >> 1u;
        local_mem_used = local_mem_func(max_items);
      } while (local_mem_used > available_local_memory && loops++ < loop_limit);

      if (local_mem_used > available_local_memory)
      {
        // Failed.
        return 0;
      }

      if (max_items < upper_limit)
      {
        // We have a viable value. Now try increase it to increase occupancy.
        loops = 0;
        size_t test_items = max_items;
        size_t diff = 0;
        do
        {
          diff = last_limit - test_items;
          test_items += diff / 2;
          local_mem_used = local_mem_func(test_items);
          if (local_mem_used <= available_local_memory)
          {
            max_items = test_items;
          }
          last_limit = test_items;
        } while (diff > 0 && loops++ < loop_limit);
      }
    }
  }

  // clGetDeviceInfo(device_, CL_DEVICE_MAX_WORK_GROUP_SIZE, sizeof(maxItems1), &maxItems1, nullptr);
  return max_items;
}


char *upDir(char *path)
{
  const char path_sep = char(pathSeparator());
  size_t last_dir_pos = std::char_traits<char>::length(path);

  while (last_dir_pos > 0)
  {
    if (path[last_dir_pos] == path_sep)
    {
      break;
    }

    --last_dir_pos;
  }

  path[last_dir_pos] = '\0';
  return path;
}


bool executablePath(char *buffer, size_t buffer_size)
{
  if (buffer && buffer_size)
  {
#if defined(WIN32)
    GetModuleFileNameA(nullptr, buffer, static_cast<DWORD>(buffer_size));
    buffer[buffer_size - 1] = '\0';
    return true;
#elif defined(__APPLE__)
    uint32_t size = uint32_t(buffer_size);
    if (_NSGetExecutablePath(buffer, &size) == 0)
    {
      buffer[buffer_size - 1] = '\0';
      return true;
    }
#else
    int read = ::readlink("/proc/self/exe", buffer, buffer_size - 1);
    buffer[buffer_size - 1] = '\0';
    if (read != -1)
    {
      return true;
    }
#endif  // WIN32
  }
  return false;
}


size_t applicationDirectory(char *buffer, size_t buffer_size)
{
  std::array<char, kMaxPath> path{};
  size_t path_length = path.size();
  if (executablePath(path.data(), path_length))
  {
    path[path_length - 1] = '\0';  // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index)
    upDir(path.data());

    path_length = std::char_traits<char>::length(path.data());
    if (buffer && buffer_size)
    {
      if (buffer_size <= path_length)
      {
        // Truncate
        path[buffer_size - 1] = '\0';  // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index)
      }

      std::char_traits<char>::copy(buffer, path.data(), path_length + 1);
    }

    return path_length;
  }

  return 0;
}


char *currentWorkingDirectory(char *cwd, size_t buffer_length)
{
#ifdef WIN32
  GetCurrentDirectoryA(static_cast<DWORD>(buffer_length), cwd);
#else   // WIN32
  // const char *ignore =
  cwd = getcwd(cwd, buffer_length);
#endif  // WIN32
  // Guarantee null termination.
  cwd[buffer_length - 1] = '\0';
  return cwd;
}


int pathSeparator()
{
#ifdef WIN32
  return '\\';
#else   // WIN32
  return '/';
#endif  // WIN32
}

char *findProgramPath(char *file_name, size_t buffer_length, const char *search_paths)
{
  // Resolve the current working directory first.
  std::array<char, kMaxPath> cwd{};
  currentWorkingDirectory(cwd.data(), cwd.size());
  cwd[cwd.size() - 1] = '\0';
  std::ostringstream str;
  str << cwd.data() << char(pathSeparator()) << file_name;

  std::ifstream infile;
  infile.open(str.str());
  if (infile.is_open())
  {
    infile.close();
    // File is in current working directory.
#ifdef _MSC_VER
    strcpy_s(file_name, buffer_length, str.str().c_str());
#else   // _MSC_VER
    strncpy(file_name, str.str().c_str(), buffer_length);
#endif  // _MSC_VER
    return file_name;
  }

  // Try the application directory.
  applicationDirectory(cwd.data(), cwd.size());
  str.str(std::string());
  str << cwd.data() << char(pathSeparator()) << file_name;
  infile.open(str.str());
  if (infile.is_open())
  {
    infile.close();
    // File is in application directory.
#ifdef _MSC_VER
    strncpy_s(file_name, buffer_length, str.str().c_str(), buffer_length);
#else   // _MSC_VER
    strncpy(file_name, str.str().c_str(), buffer_length);
#endif  // _MSC_VER
    file_name[buffer_length - 1] = '\0';
    return file_name;
  }

  // Try search paths.
  if (search_paths && search_paths[0])
  {
    const char *path = search_paths;
    const char *ch = path;
    size_t path_len = 0;
    do
    {
      if (*ch == ',' || *ch == '\0')
      {
        path_len = size_t(ch - path);
        if (path_len >= cwd.size())
        {
          path_len = cwd.size() - 1;
        }

        if (path_len)
        {
#ifdef _MSC_VER
          strncpy_s(cwd.data(), cwd.size(), path, path_len);
#else   // _MSC_VER
          strncpy(cwd.data(), path, path_len);
#endif  // _MSC_VER

          cwd[path_len] = '\0';  // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index)
          // Try the path.
          str.str(std::string());
          str << cwd.data();
          if (cwd[path_len - 1] != pathSeparator())  // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index)
          {
            str << char(pathSeparator());
          }
          str << file_name;
          infile.open(str.str());
          if (infile.is_open())
          {
            // Found it.
            infile.close();
            // File is on a search path.
#ifdef _MSC_VER
            strncpy_s(file_name, buffer_length, str.str().c_str(), buffer_length);
#else   // _MSC_VER
            strncpy(file_name, str.str().c_str(), buffer_length);
#endif  // _MSC_VER
            file_name[buffer_length - 1] = '\0';
            return file_name;
          }

          // Adjust start of next path pointer.
          path = ch;
          do
          {
            ++path;
          } while (*path == ',');
        }
      }
      ++ch;
    } while (ch[-1]);
  }
  // Not found.
  return nullptr;
}


char *findProgramDir(char *file_name, size_t buffer_length, const char *search_paths)
{
  // Resolve the current working directory first.
  std::array<char, kMaxPath> cwd{};
  currentWorkingDirectory(cwd.data(), cwd.size());
  std::ostringstream str;
  str << cwd.data() << char(pathSeparator()) << file_name;
  // std::cout << "First try: " << str.str() << std::endl;

  std::ifstream infile;
  infile.open(str.str());
  if (infile.is_open())
  {
    infile.close();
    // File is in current working directory.
#ifdef _MSC_VER
    strcpy_s(file_name, buffer_length, cwd.data());
#else   // _MSC_VER
    strncpy(file_name, cwd.data(), buffer_length);
#endif  // _MSC_VER
    return file_name;
  }

  // Try the application directory.
  applicationDirectory(cwd.data(), cwd.size());
  str.str(std::string());
  str << cwd.data() << char(pathSeparator()) << file_name;
  // std::cout << "try: " << str.str() << std::endl;
  infile.open(str.str());
  if (infile.is_open())
  {
    infile.close();
    // File is in application directory.
#ifdef _MSC_VER
    strncpy_s(file_name, buffer_length, cwd.data(), buffer_length);
#else   // _MSC_VER
    strncpy(file_name, cwd.data(), buffer_length);
#endif  // _MSC_VER
    file_name[buffer_length - 1] = '\0';
    return file_name;
  }

  // Try search paths.
  if (search_paths && search_paths[0])
  {
    const char *path = search_paths;
    const char *ch = path;
    size_t path_len = 0;
    do
    {
      if (*ch == ',' || *ch == '\0')
      {
        path_len = size_t(ch - path);
        if (path_len >= cwd.size())
        {
          path_len = cwd.size() - 1;
        }

        if (path_len)
        {
#ifdef _MSC_VER
          strncpy_s(cwd.data(), cwd.size(), path, path_len);
#else   // _MSC_VER
          strncpy(cwd.data(), path, path_len);
#endif  // _MSC_VER

          cwd[path_len] = '\0';  // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index)
          // Try the path.
          str.str(std::string());
          str << cwd.data();
          if (cwd[path_len - 1] != pathSeparator())  // NOLINT(cppcoreguidelines-pro-bounds-constant-array-index)
          {
            str << char(pathSeparator());
          }
          str << file_name;
          infile.open(str.str());
          if (infile.is_open())
          {
            // Found it.
            infile.close();
            // File is on a search path.
#ifdef _MSC_VER
            strncpy_s(file_name, buffer_length, cwd.data(), buffer_length);
#else   // _MSC_VER
            strncpy(file_name, cwd.data(), buffer_length);
#endif  // _MSC_VER
            file_name[buffer_length - 1] = '\0';
            return file_name;
          }

          // Adjust start of next path pointer.
          path = ch;
          do
          {
            ++path;
          } while (*path == ',');
        }
      }
      ++ch;
    } while (ch[-1]);
  }

  // Not found.
  return nullptr;
}

cl_int buildProgramFromFile(cl::Program &program, cl::Context &ocl, std::string &source_file_name, std::ostream &log,
                            const char *args, const char *debug_option, const char *source_file_opt,
                            const char *search_paths)
{
  if (program())
  {
    // Already initialised.
    return CL_SUCCESS;
  }

  // Compile and initialise.
  std::array<char, kMaxPath> source_dir{};  // MAX_PATH length.
  std::ostringstream source_file;
#ifdef _MSC_VER
  strcpy_s(source_dir.data(), source_dir.size(), source_file_name.c_str());
#else   // !_MSC_VER
  strncpy(source_dir.data(), source_file_name.c_str(), kMaxPath - 1);
#endif  // _MSC_VER
  if (!clu::findProgramDir(source_dir.data(), source_dir.size(), search_paths))
  {
    // Not found.
    log << "Failed to find CL source file: " << source_file_name << std::endl;
    return 1;
  }
  // log << "Found in " << source_dir.data() << std::endl;
  source_file << source_dir.data() << char(pathSeparator()) << source_file_name;
  // log << "load from " << sourceFile.str() << std::endl;
  std::ifstream file(source_file.str());
  if (!file.is_open())
  {
    // Not found.
    log << "Failed to open CL source file: " << source_file_name << std::endl;
    return 1;
  }
  source_file_name = source_file.str();

  // Setup additional build options such as include dirs.
  std::ostringstream build_opt;

  build_opt << "-I " << source_dir.data();

  if (debug_option && debug_option[0])
  {
    build_opt << ' ' << debug_option;
  }
  if (args && args[0])
  {
    build_opt << ' ' << args;
  }

  if (source_file_opt && source_file_opt[0])
  {
    build_opt << ' ' << source_file_opt << "\"" << source_file_name << "\"";
  }

  std::string prog(std::istreambuf_iterator<char>(file), (std::istreambuf_iterator<char>()));
  std::string build_opt_str = build_opt.str();
  std::string source_file_str = source_file.str();
  // No need to pass debugOpt. Already in buildOptStr.
  return buildProgramFromString(program, ocl, prog.c_str(), prog.length(), log, source_file_str.c_str(),
                                build_opt_str.c_str(), nullptr);
}

cl_int buildProgramFromString(cl::Program &program, cl::Context &ocl, const char *source_code, size_t source_length,
                              std::ostream &log, const char *reference_name, const char *build_args,
                              const char *debug_option)
{
  if (source_length == 0)
  {
    source_length = strlen(source_code);
  }

  cl::Program::Sources source(1, cl::string(source_code, source_length));
  cl::Program local_program(ocl, source);

  // Setup additional build options such as include dirs.
  std::ostringstream build_opt;
  bool first_opt = true;

  auto prefix_opt = [&first_opt](std::ostream &out) -> std::ostream & {
    if (!first_opt)
    {
      out << ' ';
    }
    first_opt = false;
    return out;
  };

  if (build_args && build_args[0])
  {
    prefix_opt(build_opt) << build_args;
  }

  if (debug_option && debug_option[0])
  {
    prefix_opt(build_opt) << debug_option;
  }

  cl_int clerr = local_program.build(build_opt.str().c_str());

  if (clerr == CL_SUCCESS)
  {
    program = local_program;
  }
  else
  {
    log << "Failed to build " << reference_name << ": " << clu::errorCodeString(clerr) << '\n';
    clu::printBuildLogs(log, local_program, ocl, CL_BUILD_ERROR);
  }

  return clerr;
}
}  // namespace clu


================================================
FILE: clu/cluProgram.h
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) CSIRO 2016
//
#ifndef CLUPROGRAM_H
#define CLUPROGRAM_H

#include "clu.h"

#include <functional>
#include <string>
#include <vector>

namespace clu
{
using LocalMemCalcFunc = std::function<size_t(size_t)>;

/// Output the build log to @p out.
///
/// Logging is filtered based on the build status when @p filterStatus is a valid
/// @c cl_build_status value. Otherwise any status passes.
void printBuildLog(std::ostream &out, const cl::Program &program, const cl::Device &device, int filter_status = 1);

/// Output the build logs to @p out.
///
/// Outputs the log for all devices associated with @p context.
///
/// Logging is filtered based on the build status when @p filterStatus is a valid
/// @c cl_build_status value. Otherwise any status passes.
void printBuildLogs(std::ostream &out, const cl::Program &program, const cl::Context &context, int filter_status = 1);

/// List the devices associated with the given program.
/// @param devices Populated with the devices from @p context.
/// @param program The program to list devices for.
/// @return The number of items in @p devices.
unsigned listDevices(std::vector<cl::Device> &devices, const cl::Program &program);

/// Calculate the maximum work group size a @p kernel.
///
/// If @p localMemFunc is provide, then the calculated size considers local memory constraints
/// as well. The function must accept a desired work group size and return the amount
/// of local memory required by the kernel given that work group size. The return value
/// is in bytes.
///
/// The function iterates the devices available for the program associated with kernel to find
/// a "valid" result. This behaviour was instigated after simply taking the first device
/// would generate a @c CL_INVALID_DEVICE error code from @c clGetKernelWorkGroupInfo().
/// Iterating the devices avoided the issue.
///
/// There may also be some perculiarities with some CPU devices. It has been noted that the Apple
/// CPU implementation (on Intel) will start to report a @c CL_KERNEL_WORK_GROUP_SIZE from
/// @c clGetKernelWorkGroupInfo() as soon as the kernel includes a call to @c barrier(), either
/// local or global. This is suggestive of lacking any work item synchronisation, forcing all
/// work groups to have one item under these conditions.
///
/// @param kernel The kernel of interest.
/// @param local_mem_func Optional function to calculate Local memory required based on
///     work groups size.
/// @param err Error code return.
/// @return The maximum work group size.
size_t maxWorkgroupSize(const cl::Kernel &kernel, const LocalMemCalcFunc &local_mem_func, cl_int *err);

/// @overload
inline size_t maxWorkgroupSize(const cl::Kernel &kernel, cl_int *err = nullptr)
{
  return maxWorkgroupSize(kernel, LocalMemCalcFunc(), err);
}

/// Resolves the directory in which the application executable lies.
/// This is not the same as the current working directory.
///
/// The directory is written to @p buffer and the number of characters required
/// for the path returned. The @p buffer may be null, in which case the return
/// value can still be used to determine the buffer size (add one for null terminator).
/// @param buffer Populated with the application directory. May be null to get the required size.
/// @param buffer_size Size of @p buffer in bytes.
/// @return The string length of the application directory, excluding the null terminator.
size_t applicationDirectory(char *buffer, size_t buffer_size);

/// A helper function for getting the current working directory.
/// @param cwd The buffer to output to.
/// @param buffer_length The capacity of @p cwd in characters.
/// @return @c cwd
char *currentWorkingDirectory(char *cwd, size_t buffer_length);

/// Get the path separator character for the current platform.
/// @return Either '/' or '\\' (Windows only).
int pathSeparator();

/// Find the directory containing the OpenCL program @p fileName either int he current
/// working directory (preferred) or the application executable directory.
///
/// The @c fileName buffer is modified to the directory containing the original name.
///
/// @param file_name The target file name.
/// @param buffer_length The buffer capacity of @p fileName (in characters).
/// @param search_paths Additional search paths in a comma separated list.
/// @return A pointer to @c fileName on success, null on failure.
char *findProgramDir(char *file_name, size_t buffer_length, const char *search_paths = nullptr);

/// Similar to @c fineProgramDir(), except that the @c fileName is preserved as part of
/// the modified path.
///
/// @param file_name The target file name.
/// @param buffer_length The buffer capacity of @p fileName (in characters).
/// @param search_paths Additional search paths in a comma separated list.
/// @return A pointer to @c fileName on success, or null on failure.
char *findProgramPath(char *file_name, size_t buffer_length, const char *search_paths = nullptr);

/// Build the program from @p sourceFileName found either in the current working
/// directory (preferred), or in the application executable directory.
///
/// This attempts to build the source file @p sourceFileName into @p program using the
/// given @p ocl context. Any @p args are appended to the build options. On success,
/// the @p program is initialised for use and @p sourceFileName is modified to refer
/// to the full path from which the source was loaded. The latter is actually set as
/// soon as the location is resolved even should building fail.
///
/// The @p sourceFileName is search for in the following locations:
/// -# The current working directory
/// -# The current application directory.
/// -# The comma separated paths listed in @p serachPaths.
///
/// The @p sourceOption is intended to identify the build option used to specify the
/// full source file path to the compiler. For instance, this may be used in debugging
/// Intel OpenCL by passing "-g -s", in which case "-g -s &lt;sourceFileName&gt;" is
/// included as the first build enabling debugging and setting the source file reference.
///
/// Any errors are logged to @p log.
///
/// The function simply returns @c CL_SUCCESS if the program is already loaded.
///
/// This function is inline to avoid ABI issues using std library objects.
///
/// @param program The program option to build into.
/// @param ocl The full OpenCL context to build using.
/// @param source_file_name Source file to build from.
/// @param args Additional arguments string to pass to @c cl::Program::build().
/// @param log All errors are logged here including compile errors and warnings.
/// @param debug_option Debug option to be added and postfixed with the full source file name.
/// @param source_file_opt Compiler option used to specify the path to the source file.
///     Source file path is not passed if this is null. Warning: no spaces are inserted
///     between the argument and the source file name. The caller must do so if required.
/// @param search_paths Additional search paths for the @p source_file_name. Use comma as a path separator.
cl_int buildProgramFromFile(cl::Program &program, cl::Context &ocl, std::string &source_file_name, std::ostream &log,
                            const char *args = nullptr, const char *debug_option = nullptr,
                            const char *source_file_opt = nullptr, const char *search_paths = nullptr);

cl_int buildProgramFromString(cl::Program &program, cl::Context &ocl, const char *source_code, size_t source_length,
                              std::ostream &log, const char *reference_name, const char *build_args = nullptr,
                              const char *debug_option = nullptr);
}  // namespace clu

#endif  // CLUPROGRAM_H


================================================
FILE: cmake/ClangTidy.cmake
================================================
# Copyright (c) 2018
# Commonwealth Scientific and Industrial Research Organisation (CSIRO)
# ABN 41 687 119 230
#
# Author: Kazys Stepanas

# Add clang-tidy support at a per target level.
#
# This script allows clang-tidy support to be added to individual targets which can then be built collectively.
# Additionally, this supports having multiple clang-tidy configurations to be selected and used dynamically.
#
# Available configurations are in clang-tidy yaml format (see 'clang-tidy -dump-config') using the file naming
# 'clang-tidy-<level>.yaml' where <level> is a meaningful work identifying the level of checing. These files are
# searched for in the 'tidy/' subdirectory directory alongside this script; specifically
# '${CMAKE_CURRENT_LIST_DIR}/tidy'.
#
# The configuration to actually use is selected by the CMake cache variable: CLANG_TIDY_LEVEL. This appears as a drop
# down list in visual CMake configuration tools such as ccmake and cmake-gui.
#
# Targets must be explicitly added for clang-tidy linting using the CMake function 'clang_tidy_target(<target>)'. This
# creates a target name '<target>-clang-tidy-<level>' where <level> matches the CLANG_TIDY_LEVEL. A target may also
# be create which builds all registered clang-tidy targets by invoking 'clang_tidy_global()'. This creates a target
# named 'clang-tidy-<level>' which processes all the registered targets.
#
# As a special case the CLANG_TIDY_LEVEL may be set to 'all'. This causes clang-tidy targets to be created for all
# available 'clang-tidy-<level>.yaml files.
#
# Note: we do not use CMAKE_CXX_CLANG_TIDY for several reasons. Clang-tidy adds significant compilation time and as
# such should only be selectively enabled. Conversely, this setup supports different clang profiles and selective
# compilation with clang-tidy.

if(CLANG_TIDY_CMAKE_INCLUDED)
  return()
endif(CLANG_TIDY_CMAKE_INCLUDED)

set(CLANG_TIDY_CMAKE_INCLUDED YES)

set(CLANG_TIDY_OK TRUE)
# We required python 3 to run clang-tidy
set(CLANG_TIDY_PYTHON)
if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.12)
  # CMake 3.12 introduced find_package(Python3) distict from finding python 2
  # Older versions cannot support both python2 and 3.
  find_package(Python3 REQUIRED QUIET)
  set(CLANG_TIDY_PYTHON "${Python3_EXECUTABLE}" CACHE PATH "Python3 executable for use with clang-tidy")
else(CMAKE_VERSION VERSION_GREATER_EQUAL 3.12)
  find_package(PythonInterp REQUIRED QUIET)
  set(CLANG_TIDY_PYTHON "${PYTHON_EXECUTABLE}" CACHE PATH "Python executable for use with clang-tidy")
endif(CMAKE_VERSION VERSION_GREATER_EQUAL 3.12)
if(NOT CLANG_TIDY_PYTHON)
  message("python interpreter not found")
  set(CLANG_TIDY_OK FALSE)
endif(NOT CLANG_TIDY_PYTHON)
# Warn once that Visual Studio is not compatible with Clang-Tidy support
set(CLANG_TIDY_VS_WARN_ONCE ON CACHE INTERNAL "" FORCE)
set(CLANG_TIDY_NAMES "clang-tidy")
# run-clang-tidy is a script which ships with clang-tidy and supports multi-threaded invocation of clang-tidy
set(RUN_CLANG_TIDY_NAMES "run-clang-tidy")
foreach(CLANG_TIDY_VER_NUM RANGE 12 6 -1)
  list(APPEND CLANG_TIDY_NAMES "clang-tidy-${CLANG_TIDY_VER_NUM}")
  list(APPEND CLANG_TIDY_NAMES "clang-tidy-${CLANG_TIDY_VER_NUM}.0")
  list(APPEND RUN_CLANG_TIDY_NAMES "run-clang-tidy-${CLANG_TIDY_VER_NUM}")
  list(APPEND RUN_CLANG_TIDY_NAMES "run-clang-tidy-${CLANG_TIDY_VER_NUM}.0")
endforeach(CLANG_TIDY_VER_NUM)

# Look for clang-tidy
find_program(CLANG_TIDY_EXE NAMES ${CLANG_TIDY_NAMES} DOC "Path to clang-tidy executable")
find_program(RUN_CLANG_TIDY_EXE NAMES ${RUN_CLANG_TIDY_NAMES} DOC "Path to run-clang-tidy executable")

if(NOT CLANG_TIDY_EXE)
  message("clang-tidy executable not found")
  set(CLANG_TIDY_OK FALSE)
endif(NOT CLANG_TIDY_EXE)
if(NOT CLANG_TIDY_OK)
  message("clang-tidy support components not found: clang-tidy directives will be ignored")
  function(clang_tidy_target)
  endfunction(clang_tidy_target)
  function(clang_tidy_global)
  endfunction(clang_tidy_global)
  return()
endif(NOT CLANG_TIDY_OK)

#-------------------------------------------------------------------------------
# Enable generation of compile_commands.json for Makefiles and Ninja.
#-------------------------------------------------------------------------------
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

option(CLANG_TIDY_FORCE_ASSERTS
  "Force visibility of assert() statements to clang-tidy? This undefine NDEBUG and can suppress false positives, but may also have side effects."
  ON)

function(clang_tidy_target)
endfunction(clang_tidy_target)
function(clang_tidy_global)
endfunction(clang_tidy_global)
include(CMakeParseArguments)


# Set the path where clang tidy config files are found
set(CLANG_TIDY_CONFIG_PATH "${CMAKE_CURRENT_LIST_DIR}/tidy")

# Resolve the names of the available configurations.
file(GLOB CLANG_TIDY_CONFIG_FILES RELATIVE "${CLANG_TIDY_CONFIG_PATH}" "${CLANG_TIDY_CONFIG_PATH}/*clang-tidy-*")

set(CLANG_TIDY_CONFIGS "")
foreach(CLANG_TIDY_CONFIG_FILE ${CLANG_TIDY_CONFIG_FILES})
  string(REGEX REPLACE ".*clang-tidy-([^\.]*)(\.yaml)?" "\\1" CLANG_TIDY_CONFIG "${CLANG_TIDY_CONFIG_FILE}")
  if(CLANG_TIDY_CONFIG)
    list(APPEND CLANG_TIDY_CONFIGS ${CLANG_TIDY_CONFIG})
  endif(CLANG_TIDY_CONFIG)
endforeach(CLANG_TIDY_CONFIG_FILE)

if(NOT CLANG_TIDY_CONFIGS)
  message(FATAL_ERROR "No clang-tidy-<name>.yaml files found in ${CLANG_TIDY_CONFIG_PATH}")
endif(NOT CLANG_TIDY_CONFIGS)
set(CLANG_TIDY_CONFIGS ${CLANG_TIDY_CONFIGS} CACHE INTERNAL "Available clang-tidy configurations")

# Add 'file' config before we clear the cached target list.
list(APPEND CLANG_TIDY_CONFIGS file)

foreach(CONFIG ${CLANG_TIDY_CONFIGS})
  # Clear cache variable listing all -clang-tidy targets
  set(CLANG_TIDY_TARGETS_${CONFIG} "" CACHE INTERNAL "" FORCE)
  set(CLANG_TIDY_TARGETS_${CONFIG}-fix "" CACHE INTERNAL "" FORCE)
endforeach(CONFIG)

# Add the 'all' config after everything else.
list(APPEND CLANG_TIDY_CONFIGS all)

# Add 'file' as a config implying using the standard .clang-tidy file finding approach.
# set(CTT_DEBUG ON)

# Enable clang-tidy if it was found.
set(CLANG_TIDY_LEVEL "all" CACHE STRING "Defines the level of clang tidy checks to invoke.")
set_property(CACHE CLANG_TIDY_LEVEL PROPERTY STRINGS ${CLANG_TIDY_CONFIGS})

#-------------------------------------------------------------------------------
# Utility functions.
#-------------------------------------------------------------------------------

# _ctt_debug(message)
# Print a message if CTT_DEBUG is true.
function(_ctt_debug)
  if(CTT_DEBUG)
    message(${ARGN})
  endif(CTT_DEBUG)
endfunction(_ctt_debug)

function(__ctt_setup_target TARGET WORKING_DIRECTORY)
  cmake_parse_arguments(CTT "FIX" "CONFIG_LEVEL;BUILD_PATH" "" ${ARGN})
  if(CTT_CONFIG_LEVEL)
    if(NOT CTT_CONFIG_LEVEL STREQUAL "file")
      set(CONFIG_ARG "-config-file=${CLANG_TIDY_CONFIG_PATH}/clang-tidy-${CTT_CONFIG_LEVEL}.yaml")
    endif(NOT CTT_CONFIG_LEVEL STREQUAL "file")
  endif(CTT_CONFIG_LEVEL)

  if(CTT_BUILD_PATH)
    set(CTT_BUILD_PATH "-p=${CTT_BUILD_PATH}")
  endif(CTT_BUILD_PATH)

  # Convert CTT_FIX to either be empty or '-fix' when the argument is present. Also set CTT_SUFFIX to '-fix' to the in
  # the latter case.
  if(CTT_FIX)
    set(CTT_FIX "-fix")
    set(CTT_SUFFIX "-fix")
    set(CTT_)
  else(CTT_FIX)
    set(CTT_FIX)
    set(CTT_SUFFIX)
  endif(CTT_FIX)

  set(CTT_TARGET_NAME ${TARGET}-clang-tidy)
  if(NOT CTT_CONFIG_LEVEL STREQUAL "file")
    set(CTT_TARGET_NAME ${CTT_TARGET_NAME}-${CTT_CONFIG_LEVEL})
  endif(NOT CTT_CONFIG_LEVEL STREQUAL "file")
  set(CTT_TARGET_NAME ${CTT_TARGET_NAME}${CTT_SUFFIX})

  set(ADD_CLANG_TIDY_ARGS)

  if(RUN_CLANG_TIDY_EXE)
    list(APPEND ADD_CLANG_TIDY_ARGS "-runner-py=${RUN_CLANG_TIDY_EXE}")
  endif(RUN_CLANG_TIDY_EXE)

  if(CLANG_TIDY_FORCE_ASSERTS)
    list(APPEND ADD_CLANG_TIDY_ARGS "-extra-arg=-UNDEBUG")
  endif(CLANG_TIDY_FORCE_ASSERTS)

  add_custom_target(${CTT_TARGET_NAME}
    WORKING_DIRECTORY "${CMAKE_CURRENT_LIST_DIR}"
    COMMAND
      "${CLANG_TIDY_PYTHON}" "${CLANG_TIDY_CONFIG_PATH}/wrap-clang-tidy.py"
      "-clang-tidy-binary=${CLANG_TIDY_EXE}"
      "-relative-to=${CMAKE_SOURCE_DIR}"
      ${ADD_CLANG_TIDY_ARGS}
      ${CONFIG_ARG} ${CTT_BUILD_PATH} ${CTT_UNPARSED_ARGUMENTS} ${CTT_FIX}
  )
  # message ("${CLANG_TIDY_PYTHON} ${CLANG_TIDY_CONFIG_PATH}/wrap-clang-tidy.py ${CLANG_TIDY_EXE} ${CONFIG_ARG} ${CTT_BUILD_PATH} ${CTT_UNPARSED_ARGUMENTS} ${CTT_FIX}")
  # message(FATAL_ERROR stop)
  set_target_properties(${CTT_TARGET_NAME} PROPERTIES FOLDER clang-tidy)

  # Experiment: add a target per source file in order to allow multi-thread compilation across the board.
  # Requires the CTT_TARGET_NAME target have no command.
  # Using run-clang-tidy.py allows a single project to use multiple threads, but this has two problems:
  # 1. When the build system (ninja or make) also runs multiple threads per target, then we have multiple threads
  #   spawning multiple threads which takes longer over all.
  # 2. Output is some times missed (bug)
  #
  # Early experiments shows the build system threading is faster than run-clang-tidy: using ohm analysis as an example
  # run-clang-tidy is ~34s, using the approach below is ~18s.
  # foreach(SOURCE_FILE ${CTT_UNPARSED_ARGUMENTS})
  #   get_filename_component(SOURCE_FILE_RELATIVE "${SOURCE_FILE}" REALPATH BASE_DIR "${CMAKE_CURRENT_LIST_DIR}")
  #   string(REGEX REPLACE "[/ \t]" "_" SOURCE_FILE_RELATIVE "${SOURCE_FILE_RELATIVE}")
  #   # set(SOURCE_TARGET_NAME ${SOURCE_FILE_RELATIVE}-${CTT_CONFIG_LEVEL}${CTT_SUFFIX})
  #   set(SOURCE_TARGET_NAME ${CTT_TARGET_NAME}-${SOURCE_FILE_RELATIVE})
  #   # message("SOURCE_TARGET_NAME: ${SOURCE_TARGET_NAME}")
  #   add_custom_target(${SOURCE_TARGET_NAME}
  #     WORKING_DIRECTORY "${WORKING_DIRECTORY}"
  #     COMMAND
  #       "${CLANG_TIDY_PYTHON}" "${CLANG_TIDY_CONFIG_PATH}/wrap-clang-tidy.py"
  #       "-clang-tidy-binary=${CLANG_TIDY_EXE}"
  #       ${CONFIG_ARG} ${CTT_BUILD_PATH} "${SOURCE_FILE}" ${CTT_FIX}
  #   )
  #   add_dependencies(${CTT_TARGET_NAME} ${SOURCE_TARGET_NAME})
  # endforeach(SOURCE_FILE)

  # Track the added clang-tidy target in the cache.
  set(CTT_TARGETS ${CLANG_TIDY_TARGETS_${CTT_CONFIG_LEVEL}${CTT_SUFFIX}})
  list(APPEND CTT_TARGETS)
  list(APPEND CTT_TARGETS ${CTT_TARGET_NAME})
  set(CLANG_TIDY_TARGETS_${CTT_CONFIG_LEVEL}${CTT_SUFFIX} "${CTT_TARGETS}" CACHE INTERNAL "" FORCE)
endfunction(__ctt_setup_target TARGET WORKING_DIRECTORY)

# clang_tidy_target(
#   target
#   [EXCLUDE file1 file2 ...]
#   [EXCLUDE_MATCHES regex1 regex2 ...]
#   [INCLUDE_HEADERS]
# )
#
# Add clang tidy support for a target.
#
# This function has two ways to apply clang-tidy support:
# - Using CXX_CLANG_TIDY property on the target
# - Using a custom, post build step.
#
# CXX_CLANG_TIDY provides better integration, but is only supported for Unix Makefiles and Ninja based CMake generators.
# For these generators, a compile_commands.json file is generated and provided to clang-tidy, ensuring clang-tidy runs
# with the same build flags. In this mode the optional EXCLUDE, EXCLUDE_MATCHES and INCLUDE_HEADERS arguments are
# ignored.
#
# Other generators use a custom build step and the build flags may differ. For increased compatibility, the build step
# implementation extracts the SOURCES and INCLUDE_DIRECTORIES properties to derive the target files and include paths.
#
# The SOURCES are filtered first filtered to remove all header files, unless INCLUDE_HEADERS was passed, then to remove
# any file specified after EXCLUDES (exact match), while EXCLUDE_MATCHES removes items using regular expressions.
# Header files are matched using the regular expression ".*\.(h|hpp|hxx|inl)".
#
# Note by default EXCLUDE_MATCHES is setup to exclude header files and files of the forms "file.in.extention" and
# "file.extention.in". The '.in' files are assumed to be used with configure_file().
#
# The include directories are filtered for explict directories, $<BUILD_INTERFACE:xxx> and
# $<TARGET_PROPERTY:target,INCLUDE_DIRECTORIES> generator expressions, adding both to the resulting include directory
# list.
#
# Finally, this mode of operation also considered the C++ standard using the CXX_STANDARD target property.
function(clang_tidy_target TARGET)
  if(CMAKE_GENERATOR MATCHES "Visual Studio")
    if(CLANG_TIDY_VS_WARN_ONCE)
      set(CLANG_TIDY_VS_WARN_ONCE OFF CACHE INTERNAL "" FORCE)
      message(STATUS "Clang tidy setup is not compatible with full Visual Studio. Recommend using Visual Studio Code with Ninja build.")
    endif(CLANG_TIDY_VS_WARN_ONCE)
    return()
  endif(CMAKE_GENERATOR MATCHES "Visual Studio")


  # Configure as a post build step
  if(NOT TARGET ${TARGET})
    message(error "${TARGET} is not defined as a target. Cannot configure clang-tidy for ${TARGET}")
    return()
  endif(NOT TARGET ${TARGET})

  cmake_parse_arguments(CTT
    "INCLUDE_HEADERS;DEBUG"
    ""
    "EXCLUDE;EXCLUDE_MATCHES"
    ${ARGN})

  # Get target sources and include directories.
  get_target_property(TARGET_SOURCES ${TARGET} SOURCES)

  if(NOT CTT_INCLUDE_HEADERS)
    set(HEADER_REX ".*\\.(h|hpp|hxx)")
    if(CTT_EXCLUDE_MATCHES)
      list(APPEND CTT_EXCLUDE_MATCHES "${HEADER_REX}")
    else(CTT_EXCLUDE_MATCHES)
      set(CTT_EXCLUDE_MATCHES ".*\\.in($|\\..*)" "${HEADER_REX}")
    endif(CTT_EXCLUDE_MATCHES)
  endif(NOT CTT_INCLUDE_HEADERS)

  # Filter target sources by EXCLUDE and EXCLUDE_MATCHES arguments
  _ctt_debug("TARGET_SOURCES: ${TARGET_SOURCES}")
  set(SOURCES)
  foreach(SRC ${TARGET_SOURCES})
    set(ADD_SRC TRUE)

    # Check exclude patterns.
    if(CTT_EXCLUDE)
      foreach(EXCLUDE ${CTT_EXCLUDE})
        if(SRC STREQUAL EXCLUDE)
          set(ADD_SRC FALSE)
          _ctt_debug("EXCLUDE ${SRC}")
          continue()
        endif(SRC STREQUAL EXCLUDE)
      endforeach(EXCLUDE)
    endif(CTT_EXCLUDE)

    # Check exclude expressions.
    if(CTT_EXCLUDE_MATCHES)
      foreach(EXCLUDE ${CTT_EXCLUDE_MATCHES})
        if(SRC MATCHES "${EXCLUDE}")
          set(ADD_SRC FALSE)
          _ctt_debug("EXCLUDE_MATCHES ${SRC}")
          continue()
        endif(SRC MATCHES "${EXCLUDE}")
      endforeach(EXCLUDE)
    endif(CTT_EXCLUDE_MATCHES)

    if(ADD_SRC)
      _ctt_debug("Add ${SRC}")
      get_filename_component(SRC "${SRC}" ABSOLUTE BASE_DIR ${CMAKE_CURRENT_LIST_DIR})
      list(APPEND SOURCES "${SRC}")
    endif(ADD_SRC)
  endforeach(SRC)

  _ctt_debug("SOURCES: ${SOURCES}")

  if(NOT SOURCES)
    message(SEND_ERROR "Target ${TARGET} has no post filter source files for clang-tidy")
    return()
  endif(NOT SOURCES)

  # We have a source list. We can get the compile flags from compile_commands.json if it exists.
  if("${CLANG_TIDY_LEVEL}" STREQUAL "all")
    foreach(LEVEL ${CLANG_TIDY_CONFIGS})
      if(NOT "${LEVEL}" STREQUAL "all")
        __ctt_setup_target(${TARGET} "${CMAKE_CURRENT_LIST_DIR}" CONFIG_LEVEL "${LEVEL}"
          BUILD_PATH "${CMAKE_BINARY_DIR}" ${SOURCES}
        )
        if(CLANG_APPLY_REPLACEMENTS_EXE)
          __ctt_setup_target(${TARGET} "${CMAKE_CURRENT_LIST_DIR}" FIX CONFIG_LEVEL "${LEVEL}"
            BUILD_PATH "${CMAKE_BINARY_DIR}" ${SOURCES}
          )
        endif(CLANG_APPLY_REPLACEMENTS_EXE)
      endif(NOT "${LEVEL}" STREQUAL "all")
    endforeach(LEVEL)
  else("${CLANG_TIDY_LEVEL}" STREQUAL "all")
    __ctt_setup_target(${TARGET} "${CMAKE_CURRENT_LIST_DIR}" CONFIG_LEVEL "${CLANG_TIDY_LEVEL}"
      BUILD_PATH "${CMAKE_BINARY_DIR}" ${SOURCES}
    )
    if(CLANG_APPLY_REPLACEMENTS_EXE)
      __ctt_setup_target(${TARGET} "${CMAKE_CURRENT_LIST_DIR}" FIX CONFIG_LEVEL "${CLANG_TIDY_LEVEL}"
        BUILD_PATH "${CMAKE_BINARY_DIR}" ${SOURCES}
      )
    endif(CLANG_APPLY_REPLACEMENTS_EXE)
  endif("${CLANG_TIDY_LEVEL}" STREQUAL "all")
endfunction(clang_tidy_target)

# Setup a single target which makes all the other clang-tidy targets.
function(__ctt_add_clang_tidy_global CONFIG_NAME CONFIG_LEVEL)
  if(NOT CLANG_TIDY_TARGETS_${CONFIG_LEVEL})
    message("No clang-tidy targets have been added for ${CONFIG_LEVEL}")
    return()
  endif(NOT CLANG_TIDY_TARGETS_${CONFIG_LEVEL})

  set(TARGET_NAME clang-tidy)
  set(DEPENDENCY_SUFFIX)
  if(NOT CONFIG_LEVEL STREQUAL "file")
    set(TARGET_NAME "${TARGET_NAME}-${CONFIG_LEVEL}")
    set(DEPENDENCY_SUFFIX "-${CONFIG_NAME}")
  endif(NOT CONFIG_LEVEL STREQUAL "file")

  add_custom_target(${TARGET_NAME})
  add_dependencies(clang-tidy${DEPENDENCY_SUFFIX} ${CLANG_TIDY_TARGETS_${CONFIG_LEVEL}})
endfunction(__ctt_add_clang_tidy_global)

# Setup a single target which makes all the other clang-tidy targets.
function(clang_tidy_global)
  if(CMAKE_GENERATOR MATCHES "Visual Studio")
    return()
  endif(CMAKE_GENERATOR MATCHES "Visual Studio")

  if("${CLANG_TIDY_LEVEL}" STREQUAL "all")
    set(ADDED_SOMETHING FALSE)
    foreach(LEVEL ${CLANG_TIDY_CONFIGS})
      if(NOT "${LEVEL}" STREQUAL "all")
        __ctt_add_clang_tidy_global(${LEVEL} ${LEVEL})
        if(CLANG_APPLY_REPLACEMENTS_EXE)
          __ctt_add_clang_tidy_global(${LEVEL}-fix ${LEVEL}-fix)
        endif(CLANG_APPLY_REPLACEMENTS_EXE)
        set(ADDED_SOMETHING TRUE)
      endif(NOT "${LEVEL}" STREQUAL "all")
    endforeach(LEVEL)

    if(NOT ADDED_SOMETHING)
    message("No clang-tidy targets have been added")
  endif(NOT ADDED_SOMETHING)

  else("${CLANG_TIDY_LEVEL}" STREQUAL "all")
    __ctt_add_clang_tidy_global(${CLANG_TIDY_LEVEL} ${CLANG_TIDY_LEVEL})
    if(CLANG_APPLY_REPLACEMENTS_EXE)
      __ctt_add_clang_tidy_global(${CLANG_TIDY_LEVEL}-fix ${CLANG_TIDY_LEVEL}-fix)
    endif(CLANG_APPLY_REPLACEMENTS_EXE)
  endif("${CLANG_TIDY_LEVEL}" STREQUAL "all")
endfunction(clang_tidy_global)


================================================
FILE: cmake/FindLASZIP.cmake
================================================
# This module searches LASzip and defines
# LASZIP_LIBRARIES - link libraries
# LASZIP_FOUND, if false, do not try to link
# LASZIP_INCLUDE_DIR, where to find the headers
#
# $LASZIP_DIR is an environment variable that would
# correspond to the ./configure --prefix=$LASZIP_DIR

find_path(LASZIP_INCLUDE_DIR laszip.hpp HINTS ENV LASZIP_DIR PATH_SUFFIXES include/laszip)

find_library(LASZIP_LIBRARY_DEBUG NAMES laszipd HINTS ENV LASZIP_DIR PATH_SUFFIXES lib)
find_library(LASZIP_LIBRARY_RELEASE NAMES laszip HINTS ENV LASZIP_DIR PATH_SUFFIXES lib)

find_file(LASZIP_RUNTIME_DEBUG NAMES laszipd${CMAKE_SHARED_LIBRARY_SUFFIX} HINTS ENV LASZIP_DIR PATH_SUFFIXES lib bin)
find_file(LASZIP_RUNTIME_RELEASE NAMES laszip${CMAKE_SHARED_LIBRARY_SUFFIX} HINTS ENV LASZIP_DIR PATH_SUFFIXES lib bin)

set(LASZIP_RUNTIME_DIRS)
if(LASZIP_RUNTIME_DEBUG)
  get_filename_component(RUNTIME_DIR "${LASZIP_RUNTIME_DEBUG}" DIRECTORY)
  list(APPEND LASZIP_RUNTIME_DIRS "${RUNTIME_DIR}")
endif(LASZIP_RUNTIME_DEBUG)
if(LASZIP_RUNTIME_RELEASE)
  get_filename_component(RUNTIME_DIR "${LASZIP_RUNTIME_RELEASE}" DIRECTORY)
  list(APPEND LASZIP_RUNTIME_DIRS "${RUNTIME_DIR}")
endif(LASZIP_RUNTIME_RELEASE)
if(LASZIP_RUNTIME_DIRS)
  list(REMOVE_DUPLICATES LASZIP_RUNTIME_DIRS)
endif(LASZIP_RUNTIME_DIRS)
set(LASZIP_RUNTIME_DIRS "${LASZIP_RUNTIME_DIRS}" CACHE PATH "LASZIP runtime directories")

if(LASZIP_LIBRARY_DEBUG)
  list(APPEND LASZIP_LIBRARIES debug ${LASZIP_LIBRARY_DEBUG})
  if(LASZIP_LIBRARY_RELEASE)
    list(APPEND LASZIP_LIBRARIES optimized ${LASZIP_LIBRARY_RELEASE})
  endif(LASZIP_LIBRARY_RELEASE)
else(LASZIP_LIBRARY_DEBUG)
  list(APPEND LASZIP_LIBRARIES ${LASZIP_LIBRARY_RELEASE})
endif(LASZIP_LIBRARY_DEBUG)

# handle the QUIETLY and REQUIRED arguments and set LASZIP_FOUND to TRUE if
# all listed variables are TRUE
include(FindPackageHandleStandardArgs)
FIND_PACKAGE_HANDLE_STANDARD_ARGS(LASZIP REQUIRED_VARS LASZIP_LIBRARIES LASZIP_INCLUDE_DIR)

if(LASZIP_FOUND)
  mark_as_advanced(LASZIP_INCLUDE_DIR LASZIP_LIBRARIES LASZIP_RUNTIME_DIRS LASZIP_RUNTIME_DEBUG LASZIP_RUNTIME_RELEASE)
endif(LASZIP_FOUND)


================================================
FILE: cmake/FindLIBLAS.cmake
================================================
# This module searches liblas and defines
# LIBLAS_LIBRARIES - link libraries
# LIBLAS_RUNTIME_LIBRARIES - runtime binaries (DLLs)
# LIBLAS_FOUND, if false, do not try to link
# LIBLAS_INCLUDE_DIR, where to find the headers
#
# $LIBLAS_ROOT is an environment variable that would

set(LL_HEADER liblas/liblas.hpp)
set(LL_HEADER_SUFFIX include)
set(LL_LIB liblas las)
set(LL_LIB_DEBUG)
set(LL_SHARED)
set(LL_SHARED_DEBUG)

foreach(LLIB ${LL_LIB})
  list(APPEND LL_LIB_DEBUG ${LLIB}d)
endforeach(LLIB)

foreach(LLIB ${LL_LIB})
  list(APPEND LL_SHARED ${LLIB}${CMAKE_SHARED_LIBRARY_SUFFIX})
endforeach(LLIB)

foreach(LLIB ${LL_LIB_DEBUG})
  list(APPEND LL_SHARED_DEBUG ${LLIB}${CMAKE_SHARED_LIBRARY_SUFFIX})
endforeach(LLIB)

# Target the C API if COMPONENTS specified as "capi"
if(LIBLAS_FIND_COMPONENTS STREQUAL "capi")
  set(LL_HEADER liblas/capi/liblas.h)
  set(LL_HEADER_SUFFIX include)
  set(LL_LIB liblas_c las_c)
else(LIBLAS_FIND_COMPONENTS STREQUAL "capi")
endif(LIBLAS_FIND_COMPONENTS STREQUAL "capi")

find_path(LIBLAS_INCLUDE_DIR ${LL_HEADER} HINTS ENV LIBLAS_ROOT PATH_SUFFIXES ${LL_HEADER_SUFFIX})

find_library(LIBLAS_LIBRARY_DEBUG NAMES ${LL_LIB_DEBUG} HINTS ENV LIBLAS_ROOT PATH_SUFFIXES lib)
find_library(LIBLAS_LIBRARY_RELEASE NAMES ${LL_LIB} HINTS ENV LIBLAS_ROOT PATH_SUFFIXES lib)

find_file(LIBLAS_RUNTIME_DEBUG NAMES ${LL_SHARED_DEBUG} HINTS ENV LIBLAS_ROOT PATH_SUFFIXES bin)
find_file(LIBLAS_RUNTIME_RELEASE NAMES ${LL_SHARED} HINTS ENV LIBLAS_ROOT PATH_SUFFIXES bin)

if(LIBLAS_LIBRARY_DEBUG)
  list(APPEND LIBLAS_LIBRARIES debug ${LIBLAS_LIBRARY_DEBUG})
  if(LIBLAS_LIBRARY_RELEASE)
    list(APPEND LIBLAS_LIBRARIES optimized ${LIBLAS_LIBRARY_RELEASE})
  endif(LIBLAS_LIBRARY_RELEASE)
else(LIBLAS_LIBRARY_DEBUG)
  list(APPEND LIBLAS_LIBRARIES ${LIBLAS_LIBRARY_RELEASE})
endif(LIBLAS_LIBRARY_DEBUG)

if(LIBLAS_RUNTIME_DEBUG)
  list(APPEND LIBLAS_RUNTIME_LIBRARIES debug ${LIBLAS_RUNTIME_DEBUG})
  if(LIBLAS_RUNTIME_RELEASE)
    list(APPEND LIBLAS_RUNTIME_LIBRARIES optimized ${LIBLAS_RUNTIME_RELEASE})
  endif(LIBLAS_RUNTIME_RELEASE)
else(LIBLAS_RUNTIME_DEBUG)
  list(APPEND LIBLAS_RUNTIME_LIBRARIES ${LIBLAS_RUNTIME_RELEASE})
endif(LIBLAS_RUNTIME_DEBUG)

if(LIBLAS_RUNTIME_DEBUG)
  get_filename_component(RUNTIME_DIR "${LIBLAS_RUNTIME_DEBUG}" DIRECTORY)
  list(APPEND LIBLAS_RUNTIME_DIRS "${RUNTIME_DIR}")
endif(LIBLAS_RUNTIME_DEBUG)
if(LIBLAS_RUNTIME_RELEASE)
  get_filename_component(RUNTIME_DIR "${LIBLAS_RUNTIME_RELEASE}" DIRECTORY)
  list(APPEND LIBLAS_RUNTIME_DIRS "${RUNTIME_DIR}")
endif(LIBLAS_RUNTIME_RELEASE)
if (LIBLAS_RUNTIME_DIRS)
  list(REMOVE_DUPLICATES LIBLAS_RUNTIME_DIRS)
endif(LIBLAS_RUNTIME_DIRS)
set(LIBLAS_RUNTIME_DIRS "${LIBLAS_RUNTIME_DIRS}" CACHE PATH "LIBLAS runtime directories")


# handle the QUIETLY and REQUIRED arguments and set LIBLAS_FOUND to TRUE if
# all listed variables are TRUE
include(FindPackageHandleStandardArgs)
FIND_PACKAGE_HANDLE_STANDARD_ARGS(LIBLAS REQUIRED_VARS LIBLAS_LIBRARIES LIBLAS_INCLUDE_DIR)
if(LIBLAS_RUNTIME_LIBRARIES)
  set(LIBLAS_RUNTIME_LIBRARIES ${LIBLAS_RUNTIME_LIBRARIES} CACHE PATH "LIBBLAS runtime libraries")
endif(LIBLAS_RUNTIME_LIBRARIES)

mark_as_advanced(LIBLAS_INCLUDE_DIR LIBLAS_LIBRARIES LIBLAS_RUNTIME_LIBRARIES)


================================================
FILE: cmake/FindOctomap.cmake
================================================
# This module searches liboctoma and defines
# OCTOMAP_LIBRARIES - link libraries
# OCTOMAP_FOUND, if false, do not try to link
# OCTOMAP_INCLUDE_DIRS, where to find the headers
# OCTOMAP_VERSION, reports either version 4 or 3 (for everything before version 4)
#
# $OCTOMAP_DIR is an environment variable that would
# correspond to the ./configure --prefix=$OCTOMAP_DIR

find_path(OCTOMAP_INCLUDE_DIRS octomap/octomap.h HINTS ENV OCTOMAP_DIR PATH_SUFFIXES include)

find_library(OCTOMAP_LIBRARY_DEBUG NAMES octomapd HINTS ENV OCTOMAP_DIR PATH_SUFFIXES lib)
find_library(OCTOMAP_LIBRARY_RELEASE NAMES octomap HINTS ENV OCTOMAP_DIR PATH_SUFFIXES lib)

find_library(OCTOMATH_LIBRARY_DEBUG NAMES octomathd HINTS ENV OCTOMAP_DIR PATH_SUFFIXES lib)
find_library(OCTOMATH_LIBRARY_RELEASE NAMES octomath HINTS ENV OCTOMAP_DIR PATH_SUFFIXES lib)

find_library(DYNAMICEDT3D_LIBRARY_DEBUG NAMES dynamicedt3dd HINTS ENV OCTOMAP_DIR PATH_SUFFIXES lib)
find_library(DYNAMICEDT3D_LIBRARY_RELEASE NAMES dynamicedt3d HINTS ENV OCTOMAP_DIR PATH_SUFFIXES lib)

if(OCTOMAP_LIBRARY_DEBUG)
  list(APPEND OCTOMAP_LIBRARIES debug ${OCTOMAP_LIBRARY_DEBUG})
  list(APPEND OCTOMAP_DEBUG_LIBRARIES ${OCTOMAP_LIBRARY_DEBUG})
  if(OCTOMAP_LIBRARY_RELEASE)
    list(APPEND OCTOMAP_LIBRARIES optimized ${OCTOMAP_LIBRARY_RELEASE})
    list(APPEND OCTOMAP_RELEASE_LIBRARIES ${OCTOMAP_LIBRARY_RELEASE})
  endif(OCTOMAP_LIBRARY_RELEASE)
else(OCTOMAP_LIBRARY_DEBUG)
  list(APPEND OCTOMAP_LIBRARIES ${OCTOMAP_LIBRARY_RELEASE})
endif(OCTOMAP_LIBRARY_DEBUG)

if(OCTOMATH_LIBRARY_DEBUG)
  list(APPEND OCTOMAP_LIBRARIES debug ${OCTOMATH_LIBRARY_DEBUG})
  list(APPEND OCTOMAP_DEBUG_LIBRARIES ${OCTOMATH_LIBRARY_DEBUG})
  if(OCTOMATH_LIBRARY_RELEASE)
    list(APPEND OCTOMAP_LIBRARIES optimized ${OCTOMATH_LIBRARY_RELEASE})
    list(APPEND OCTOMAP_RELEASE_LIBRARIES ${OCTOMATH_LIBRARY_RELEASE})
  endif(OCTOMATH_LIBRARY_RELEASE)
else(OCTOMATH_LIBRARY_DEBUG)
  list(APPEND OCTOMAP_LIBRARIES ${OCTOMATH_LIBRARY_RELEASE})
endif(OCTOMATH_LIBRARY_DEBUG)

if(DYNAMICEDT3D_LIBRARY_DEBUG)
  list(APPEND OCTOMAP_LIBRARIES debug ${DYNAMICEDT3D_LIBRARY_DEBUG})
  list(APPEND OCTOMAP_DEBUG_LIBRARIES ${DYNAMICEDT3D_LIBRARY_DEBUG})
  if(DYNAMICEDT3D_LIBRARY_RELEASE)
    list(APPEND OCTOMAP_LIBRARIES optimized ${DYNAMICEDT3D_LIBRARY_RELEASE})
    list(APPEND OCTOMAP_RELEASE_LIBRARIES ${DYNAMICEDT3D_LIBRARY_RELEASE})
  endif(DYNAMICEDT3D_LIBRARY_RELEASE)
else(DYNAMICEDT3D_LIBRARY_DEBUG)
  list(APPEND OCTOMAP_LIBRARIES ${DYNAMICEDT3D_LIBRARY_RELEASE})
endif(DYNAMICEDT3D_LIBRARY_DEBUG)

# handle the QUIETLY and REQUIRED arguments and set OCTOMAP_FOUND to TRUE if
# all listed variables are TRUE
include(FindPackageHandleStandardArgs)
FIND_PACKAGE_HANDLE_STANDARD_ARGS(OCTOMAP REQUIRED_VARS OCTOMAP_LIBRARIES OCTOMAP_INCLUDE_DIRS)

mark_as_advanced(OCTOMAP_INCLUDE_DIRS OCTOMAP_LIBRARIES)


================================================
FILE: cmake/FindTBB.cmake
================================================
# Locate Intel Threading Building Blocks include paths and libraries
# FindTBB.cmake can be found at https://code.google.com/p/findtbb/
# Written by Hannes Hofmann <hannes.hofmann _at_ informatik.uni-erlangen.de>
# Improvements by Gino van den Bergen <gino _at_ dtecta.com>,
#   Florian Uhlig <F.Uhlig _at_ gsi.de>,
#   Jiri Marsik <jiri.marsik89 _at_ gmail.com>

# The MIT License
#
# Copyright (c) 2011 Hannes Hofmann
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.

# GvdB: This module uses the environment variable TBB_ARCH_PLATFORM which defines architecture and compiler.
#   e.g. "ia32/vc8" or "em64t/cc4.1.0_libc2.4_kernel2.6.16.21"
#   TBB_ARCH_PLATFORM is set by the build script tbbvars[.bat|.sh|.csh], which can be found
#   in the TBB installation directory (TBB_INSTALL_DIR).
#
# GvdB: Mac OS X distribution places libraries directly in lib directory.
#
# For backwards compatibility, you may explicitely set the CMake variables TBB_ARCHITECTURE and TBB_COMPILER.
# TBB_ARCHITECTURE [ ia32 | em64t | itanium ]
#   which architecture to use
# TBB_COMPILER e.g. vc9 or cc3.2.3_libc2.3.2_kernel2.4.21 or cc4.0.1_os10.4.9
#   which compiler to use (detected automatically on Windows)

# This module respects
# TBB_INSTALL_DIR or $ENV{TBB21_INSTALL_DIR} or $ENV{TBB_INSTALL_DIR}

# This module defines
# TBB_INCLUDE_DIRS, where to find task_scheduler_init.h, etc.
# TBB_LIBRARY_DIRS, where to find libtbb, libtbbmalloc
# TBB_DEBUG_LIBRARY_DIRS, where to find libtbb_debug, libtbbmalloc_debug
# TBB_INSTALL_DIR, the base TBB install directory
# TBB_LIBRARIES, the libraries to link against to use TBB.
# TBB_DEBUG_LIBRARIES, the libraries to link against to use TBB with debug symbols.
# TBB_FOUND, If false, don't try to use TBB.
# TBB_INTERFACE_VERSION, as defined in tbb/tbb_stddef.h

# Try config file first. This particular file is really only for supporting older TBB such as found on Ubuntu 18.04.
# Newer TBB with config.cmake files is stringly recommended.
find_package(TBB QUIET CONFIG)
if(TBB_FOUND)
    message(STATUS "Found TBB config file.")
    # Lots of legacy permutations here. In a way the most likely is via vcpkg which is TBB::tbb even though it's static.
    if(NOT TBB_LIBRARIES)
        if(TBB_IMPORTED_TARGETS)
            set(TBB_LIBRARIES "${TBB_IMPORTED_TARGETS}")
        elseif(TARGET TBB::tbb_static)
            set(TBB_LIBRARIES TBB::tbb_static TBB::tbbmalloc_static)
        elseif(TARGET TBB::tbb)
            set(TBB_LIBRARIES TBB::tbb TBB::tbbmalloc)
        elseif(TARGET tbb::tbb_static)
            set(TBB_LIBRARIES tbb::tbb_static tbb::tbbmalloc_static)
        elseif(TARGET tbb::tbb)
            set(TBB_LIBRARIES tbb::tbb tbb::tbbmalloc)
        else()
            message(FATAL_ERROR "Could not resolve tbb libraries")
        endif()
    endif(NOT TBB_LIBRARIES)
return()
endif(TBB_FOUND)
message(STATUS "Searching for TBB libraries...")


if (WIN32)
    # has em64t/vc8 em64t/vc9
    # has ia32/vc7.1 ia32/vc8 ia32/vc9
    set(_TBB_DEFAULT_INSTALL_DIR "C:/Program Files/Intel/TBB" "C:/Program Files (x86)/Intel/TBB")
    set(_TBB_LIB_NAME "tbb")
    set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
    set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
    set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
    if (MSVC71)
        set (_TBB_COMPILER "vc7.1")
    endif(MSVC71)
    if (MSVC80)
        set(_TBB_COMPILER "vc8")
    endif(MSVC80)
    if (MSVC90)
        set(_TBB_COMPILER "vc9")
    endif(MSVC90)
    if(MSVC10)
        set(_TBB_COMPILER "vc10")
    endif(MSVC10)
    if(MSVC11)
        set(_TBB_COMPILER "vc11")
    endif(MSVC11)
    if(MSVC12)
        set(_TBB_COMPILER "vc12")
    endif(MSVC12)
    if(MSVC14)
        set(_TBB_COMPILER "vc14")
    endif(MSVC14)
    if(MSVC15)
        set(_TBB_COMPILER "vc15")
    endif(MSVC15)
    # Todo: add other Windows compilers such as ICL.
    set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})

    if(NOT DEFINED _TBB_ARCHITECTURE)
        # Architecture has not been explicitly selected. Determine 32 or 64 bit compilation
        # mode and select the appropriate, known architecture.
        if(CMAKE_SIZEOF_VOID_P EQUAL 8)
            set(_TBB_ARCHITECTURE "intel64")
        else(CMAKE_SIZEOF_VOID_P EQUAL 8)
            set(_TBB_ARCHITECTURE "ia32")
        endif(CMAKE_SIZEOF_VOID_P EQUAL 8)
    endif(NOT DEFINED _TBB_ARCHITECTURE)
endif (WIN32)

if (UNIX)
    if (APPLE)
        # MAC
        set(_TBB_DEFAULT_INSTALL_DIR "/Library/Frameworks/Intel_TBB.framework/Versions")
        # libs: libtbb.dylib, libtbbmalloc.dylib, *_debug
        set(_TBB_LIB_NAME "tbb")
        set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
        set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
        set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
        # default flavor on apple: ia32/cc4.0.1_os10.4.9
        # Jiri: There is no reason to presume there is only one flavor and
        #       that user's setting of variables should be ignored.
        if(NOT TBB_COMPILER)
            set(_TBB_COMPILER "cc4.0.1_os10.4.9")
        elseif (NOT TBB_COMPILER)
            set(_TBB_COMPILER ${TBB_COMPILER})
        endif(NOT TBB_COMPILER)
        if(NOT TBB_ARCHITECTURE)
            set(_TBB_ARCHITECTURE "ia32")
        elseif(NOT TBB_ARCHITECTURE)
            set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
        endif(NOT TBB_ARCHITECTURE)
    else (APPLE)
        # LINUX
        set(_TBB_DEFAULT_INSTALL_DIR "/opt/intel/tbb" "/usr/local/include" "/usr/include")
        set(_TBB_LIB_NAME "tbb")
        set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
        set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
        set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
        # has em64t/cc3.2.3_libc2.3.2_kernel2.4.21 em64t/cc3.3.3_libc2.3.3_kernel2.6.5 em64t/cc3.4.3_libc2.3.4_kernel2.6.9 em64t/cc4.1.0_libc2.4_kernel2.6.16.21
        # has ia32/*
        # has itanium/*
        set(_TBB_COMPILER ${TBB_COMPILER})
        set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
    endif (APPLE)
endif (UNIX)

if (CMAKE_SYSTEM MATCHES "SunOS.*")
# SUN
# not yet supported
# has em64t/cc3.4.3_kernel5.10
# has ia32/*
endif (CMAKE_SYSTEM MATCHES "SunOS.*")

#-- Clear the public variables
set (TBB_FOUND "NO")

#-- Find TBB install dir and set ${_TBB_INSTALL_DIR} and cached ${TBB_INSTALL_DIR}
# first: use CMake variable TBB_INSTALL_DIR
if (TBB_INSTALL_DIR)
    set (_TBB_INSTALL_DIR ${TBB_INSTALL_DIR})
endif (TBB_INSTALL_DIR)
# second: use environment variable
if (NOT _TBB_INSTALL_DIR)
    if (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "")
        file(TO_CMAKE_PATH "$ENV{TBB_INSTALL_DIR}" _TBB_INSTALL_DIR)
    endif (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "")
    # Intel recommends setting TBB21_INSTALL_DIR
    if (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "")
        file(TO_CMAKE_PATH "$ENV{TBB21_INSTALL_DIR}" _TBB_INSTALL_DIR)
    endif (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "")
    if (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "")
        file(TO_CMAKE_PATH "$ENV{TBB22_INSTALL_DIR}" _TBB_INSTALL_DIR)
    endif (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "")
    if (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "")
        file(TO_CMAKE_PATH "$ENV{TBB30_INSTALL_DIR}" _TBB_INSTALL_DIR)
    endif (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "")
endif (NOT _TBB_INSTALL_DIR)
# third: try to find path automatically
if (NOT _TBB_INSTALL_DIR)
    if (_TBB_DEFAULT_INSTALL_DIR)
        set (_TBB_INSTALL_DIR ${_TBB_DEFAULT_INSTALL_DIR})
    endif (_TBB_DEFAULT_INSTALL_DIR)
endif (NOT _TBB_INSTALL_DIR)
# sanity check
if (NOT _TBB_INSTALL_DIR)
    message ("ERROR: Unable to find Intel TBB install directory. ${_TBB_INSTALL_DIR}")
else (NOT _TBB_INSTALL_DIR)
# finally: set the cached CMake variable TBB_INSTALL_DIR
if (NOT TBB_INSTALL_DIR)
    set (TBB_INSTALL_DIR ${_TBB_INSTALL_DIR} CACHE PATH "Intel TBB install directory")
    mark_as_advanced(TBB_INSTALL_DIR)
endif (NOT TBB_INSTALL_DIR)

#-- A macro to rewrite the paths of the library. This is necessary, because
#   find_library() always found the em64t/vc9 version of the TBB libs
macro(TBB_CORRECT_LIB_DIR var_name)
#    if (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t")
        string(REPLACE em64t "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}})
#    endif (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t")
    string(REPLACE ia32 "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}})
    string(REPLACE vc7.1 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
    string(REPLACE vc8 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
    string(REPLACE vc9 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
    string(REPLACE vc10 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
    string(REPLACE vc11 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
    string(REPLACE vc12 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
    string(REPLACE vc14 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
    string(REPLACE vc15 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
endmacro(TBB_CORRECT_LIB_DIR var_content)

#-- A macro which adds a path suffix to a list of directories.
#   Most useful for expanding _TBB_DEFAULT_INSTALL_DIR under windows where
#   there are two possible install locations: "C:/Program Files" or
#   "C:/Program Files (x86)"
macro(TBB_APPEND_PATH var_name base_paths path_suffix)
    # path_platform may have backslash paths on Windows. Ensure they are replaced.
    foreach(_TBB_PATH ${base_paths})
        # Paths must be quoted to handle spaces.
        list(APPEND ${var_name} "${_TBB_PATH}/${path_suffix}")
    endforeach(_TBB_PATH)
endmacro(TBB_APPEND_PATH)

#-- Look for include directory and set ${TBB_INCLUDE_DIR}
TBB_APPEND_PATH(TBB_INC_SEARCH_DIR "${_TBB_INSTALL_DIR}" include)
# Jiri: tbbvars now sets the CPATH environment variable to the directory
#       containing the headers.
find_path(TBB_INCLUDE_DIR
    tbb/task_scheduler_init.h
    PATHS ${TBB_INC_SEARCH_DIR} ENV CPATH
)
mark_as_advanced(TBB_INCLUDE_DIR)

#-- Look for libraries
# GvdB: $ENV{TBB_ARCH_PLATFORM} is set by the build script tbbvars[.bat|.sh|.csh]
if (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "")
    # Environment variables may be partial paths and contain backslashes under windows.
    file(TO_CMAKE_PATH "$ENV{TBB_ARCH_PLATFORM}" TBB_ARCH_PLATFORM)
    TBB_APPEND_PATH(_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}" /lib/${TBB_ARCH_PLATFORM})
    TBB_APPEND_PATH(_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}" /${TBB_ARCH_PLATFORM}/lib)
    # set (_TBB_LIBRARY_DIR
    #      ${_TBB_INSTALL_DIR}/lib/$ENV{TBB_ARCH_PLATFORM}
    #      ${_TBB_INSTALL_DIR}/$ENV{TBB_ARCH_PLATFORM}/lib
    #     )
endif (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "")
# Jiri: This block isn't mutually exclusive with the previous one
#       (hence no else), instead I test if the user really specified
#       the variables in question.
if ((NOT ${_TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${_TBB_COMPILER} STREQUAL ""))
    # Jiri: It doesn't hurt to look in more places, so I store the hints from
    #       ENV{TBB_ARCH_PLATFORM} and the TBB_ARCHITECTURE and TBB_COMPILER
    #       variables and search them both.
    # set (_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}/lib" ${_TBB_LIBRARY_DIR})
    # Search the new paths first, (thus the use of a temporary variable)
    set(__TBB_LIBRARY_DIR)
    TBB_APPEND_PATH(__TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}" "lib/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}")
    list(INSERT _TBB_LIBRARY_DIR 0 "${__TBB_LIBRARY_DIR}")
endif ((NOT ${_TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${_TBB_COMPILER} STREQUAL ""))

# GvdB: Mac OS X distribution places libraries directly in lib directory.
TBB_APPEND_PATH(_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}" lib)

# Jiri: No reason not to check the default paths. From recent versions,
#       tbbvars has started exporting the LIBRARY_PATH and LD_LIBRARY_PATH
#       variables, which now point to the directories of the lib files.
#       It all makes more sense to use the ${_TBB_LIBRARY_DIR} as a HINTS
#       argument instead of the implicit PATHS as it isn't hard-coded
#       but computed by system introspection. Searching the LIBRARY_PATH
#       and LD_LIBRARY_PATH environment variables is now even more important
#       that tbbvars doesn't export TBB_ARCH_PLATFORM and it facilitates
#       the use of TBB built from sources.
find_library(TBB_LIBRARY ${_TBB_LIB_NAME} HINTS ${_TBB_LIBRARY_DIR}
        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
find_library(TBB_MALLOC_LIBRARY ${_TBB_LIB_MALLOC_NAME} HINTS ${_TBB_LIBRARY_DIR}
        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)

#Extract path from TBB_LIBRARY name
get_filename_component(TBB_LIBRARY_DIR ${TBB_LIBRARY} PATH)

#TBB_CORRECT_LIB_DIR(TBB_LIBRARY)
#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY)
mark_as_advanced(TBB_LIBRARY TBB_MALLOC_LIBRARY)

#-- Look for debug libraries
# Jiri: Changed the same way as for the release libraries.
find_library(TBB_LIBRARY_DEBUG ${_TBB_LIB_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR}
        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
find_library(TBB_MALLOC_LIBRARY_DEBUG ${_TBB_LIB_MALLOC_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR}
        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)

# Jiri: Self-built TBB stores the debug libraries in a separate directory.
#       Extract path from TBB_LIBRARY_DEBUG name
get_filename_component(TBB_LIBRARY_DEBUG_DIR ${TBB_LIBRARY_DEBUG} PATH)

#TBB_CORRECT_LIB_DIR(TBB_LIBRARY_DEBUG)
#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY_DEBUG)
mark_as_advanced(TBB_LIBRARY_DEBUG TBB_MALLOC_LIBRARY_DEBUG)


if (TBB_INCLUDE_DIR)
    if (TBB_LIBRARY)
        set (TBB_FOUND "YES")
        if (TBB_LIBRARY_DEBUG)
            set (TBB_LIBRARIES optimized ${TBB_LIBRARY} optimized ${TBB_MALLOC_LIBRARY} debug ${TBB_LIBRARY_DEBUG} debug ${TBB_MALLOC_LIBRARY_DEBUG})
        else (TBB_LIBRARY_DEBUG)
            set (TBB_LIBRARIES ${TBB_LIBRARY} ${TBB_MALLOC_LIBRARY} ${TBB_LIBRARIES})
        endif (TBB_LIBRARY_DEBUG)
        set (TBB_DEBUG_LIBRARIES ${TBB_LIBRARY_DEBUG} ${TBB_MALLOC_LIBRARY_DEBUG} ${TBB_DEBUG_LIBRARIES})
        set (TBB_INCLUDE_DIRS ${TBB_INCLUDE_DIR} CACHE PATH "TBB include directory" FORCE)
        set (TBB_LIBRARY_DIRS ${TBB_LIBRARY_DIR} CACHE PATH "TBB library directory" FORCE)
        # Jiri: Self-built TBB stores the debug libraries in a separate directory.
        set (TBB_DEBUG_LIBRARY_DIRS ${TBB_LIBRARY_DEBUG_DIR} CACHE PATH "TBB debug library directory" FORCE)
        mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARY_DIRS TBB_DEBUG_LIBRARY_DIRS TBB_LIBRARIES TBB_DEBUG_LIBRARIES)
        message(STATUS "Found Intel TBB")
    endif (TBB_LIBRARY)
endif (TBB_INCLUDE_DIR)

if (NOT TBB_FOUND)
    message("ERROR: Intel TBB NOT found!")
    message(STATUS "Looked for Threading Building Blocks in ${_TBB_INSTALL_DIR}")
    # do only throw fatal, if this pkg is REQUIRED
    if (TBB_FIND_REQUIRED)
        message(FATAL_ERROR "Could NOT find TBB library.")
    endif (TBB_FIND_REQUIRED)
endif (NOT TBB_FOUND)

endif (NOT _TBB_INSTALL_DIR)

if (TBB_FOUND)
    set(TBB_INTERFACE_VERSION 0)
    FILE(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _TBB_VERSION_CONTENTS)
    STRING(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" TBB_INTERFACE_VERSION "${_TBB_VERSION_CONTENTS}")
    set(TBB_INTERFACE_VERSION "${TBB_INTERFACE_VERSION}")

    find_package(Threads QUIET)

    # Define TBB import libraries from what we've found: TBB::tbb
    # This is a partial definition, enough to make it work with OHM.
    add_library(TBB::tbb UNKNOWN IMPORTED)

    set_target_properties(TBB::tbb PROPERTIES 
        INTERFACE_INCLUDE_DIRECTORIES "${TBB_INCLUDE_DIRS}"
        IMPORTED_LOCATION_RELEASE "${TBB_LIBRARY}"
        IMPORTED_LOCATION_RELWITHDEBINFO "${TBB_LIBRARY}"
        IMPORTED_LOCATION_MINSIZEREL "${TBB_LIBRARY}"
    )
    if(TBB_LIBRARY_DEBUG)
        set_target_properties(TBB::tbb PROPERTIES 
            IMPORTED_LOCATION_DEBUG "${TBB_LIBRARY_DEBUG}"
        )
    endif(TBB_LIBRARY_DEBUG)
    if(TARGET Threads::Threads)
        set_target_properties(TBB::tbb PROPERTIES 
            INTERFACE_LINK_LIBRARIES Threads::Threads
        )
    endif(TARGET Threads::Threads)
endif (TBB_FOUND)


================================================
FILE: cmake/Findglm.cmake
================================================

# FindGLM (OpenGL Mathematics library)
# --------
#
# try to find include files.
#
# IMPORTED Targets
# ^^^^^^^^^^^^^^^^
#
# This module defines the :prop_tgt:`IMPORTED` targets:
#
# ``glm::glm``
#  Defined if the system has OpenGL Mathematics headers.
#
# Result Variables
# ^^^^^^^^^^^^^^^^
#
# This module sets the following variables:
#
# ::
#
#   GLM_INCLUDE_DIRS, where to find glm/glm.hpp, etc.
#   glm_FOUND, If false, do not try to use GLM.
#   glm::glm, import target

# Try config file first. This particular file is really only for supporting older GLM such as found on Ubuntu 18.04.
# Newer GLM with config.cmake files is strongly recommended.
find_package(glm QUIET CONFIG)
if(NOT glm_FOUND)
  find_path(GLM_INCLUDE_DIRS NAMES glm/glm.hpp
            PATHS $ENV{GLM_ROOT_PATH}/include ${GLM_ROOT_PATH}/include
            /usr/local/include
            )

  mark_as_advanced(
      GLM_INCLUDE_DIRS
    )
endif()

# Note: glmConfig.cmake on Ubuntu 18.04 does not provide a target glm::glm.
# 20.04 does.
if(NOT TARGET glm::glm)
  add_library(glm::glm INTERFACE IMPORTED)
  set_target_properties(glm::glm PROPERTIES
    INTERFACE_INCLUDE_DIRECTORIES "${GLM_INCLUDE_DIRS}"
  )
else()
  get_target_property(GLM_INCLUDE_DIRS glm::glm INTERFACE_INCLUDE_DIRECTORIES)
endif()

include(FindPackageHandleStandardArgs)
FIND_PACKAGE_HANDLE_STANDARD_ARGS(glm REQUIRED_VARS GLM_INCLUDE_DIRS)


================================================
FILE: cmake/LeakTrack.cmake
================================================
# Copyright (c) 2019
# Commonwealth Scientific and Industrial Research Organisation (CSIRO)
# ABN 41 687 119 230
#
# Author: Kazys Stepanas

# This script adds memory leak tracking to any exectuable target built using GCC or Clang via Address Sanitizer.
#
# Oustanding memory allocations are reported at the end of the run to stderr.
#
# Expected usage is as follows, written as a CMake script (with comments);
#
# # Find the package containing this script
# find_package(ras_cmake)
# # Import this script
# include(LeakTrack)
# # Define a CMake option() to toggle leak tracking. Make sure you enable this as required.
# option(MY_PROJECT_LEAK_TRACK "Enable memory leak tracking?" OFF)
# # Define your exectuable target. Also works with cuda_add_executable()
# add_executable(myprogram)
# # Conditionally enable leak tracking when MY_PROJECT_LEAK_TRACK is true
# leak_track_target_enable(myprogram CONDITION MY_PROJECT_LEAK_TRACK)
# # For a CUDA based program, ensure you disable protect_shadow_gap checking:
# leak_track_default_options(myprogram CONDITION MY_PROJECT_LEAK_TRACK "protect_shadow_gap=0")
#
#
# This enables all leak reporting for myprogram. Optionally, you can add additional ASAN options by passing additional
# arguments to leak_track_default_options(). See Overview resource below.
#
# There can also be spurrious memory leaks reported, or leaks in 3rd party libraries which cannot be dealt with. These
# can be suppressed from the leak report by adding:
# leak_track_suppress(myprogram CONDITION MY_PROJECT_LEAK_TRACK
#     symbol1
#     symbol2
#     symbol3
# )
# Where 'symbol#' matches part of the reported callstack symbols.
#
# If leak tracking is supported using AddressSanitizer, then the CMake cache variable LEAK_TRACK_WITH_ASAN will be set
# ON.
#
# Memory leak tracking may also be forcibly enabled vai the CMake option LEAK_TRACK_FORCE. This is intended as a global,
# consistent option in multi-project environments.
#
# Note that these directives are ignored when not building with GCC or Clang.
#
# The following links provide some much needed information about using libasan, althought the documentation is scarce:
# - Overview: https://github.com/google/sanitizers/wiki/AddressSanitizer
# - Weak symbols for in code overrides: https://chromium.googlesource.com/chromium/src/build/+/master/sanitizers/sanitizer_options.cc

include(CMakeParseArguments)

set(LEAK_TRACK_WITH_ASAN OFF CACHE STRING "Using libasan for leak tracking" FORCE)
mark_as_advanced(LEAK_TRACK_WITH_ASAN)

option(LEAK_TRACK_FORCE "Force memory leak tracking to be enabled regardless of condition checks?" OFF)

# Setup: find libasan
if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU" OR "${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang")
  find_library(ASAN_LIBRARAY NAMES asan libasan.so libasan.so.4)
  if(ASAN_LIBRARAY)
    set(LEAK_TRACK_WITH_ASAN ON CACHE STRING "Using libasan for leak tracking" FORCE)
  else(ASAN_LIBRARY)
    message("Unable to find libasan. Leak tracking unavailable.")
    set(TARGET_ENABLE_ASAN_ONCE TRUE)
  endif(ASAN_LIBRARAY)
endif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU" OR "${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang")

# Utility function to extend a property on a target by adding a list of items.
#
# _extend_target_property(<target> <property> [FIRST] value1 [value2 ...])
#
# The FIRST keyword forces the new values to appear first in the property value list.
function(_leak_track_extend_target_property TARGET PROPERTY)
  cmake_parse_arguments(ETP "FIRST" "" "" ${ARGN})
  get_target_property(INITIAL_VALUE ${TARGET} ${PROPERTY})

  if(ETP_FIRST)
    set(MODIFIED_VALUE ${ETP_UNPARSED_ARGUMENTS})
    if(INITIAL_VALUE)
      list(APPEND MODIFIED_VALUE ${INITIAL_VALUE})
    endif(INITIAL_VALUE)
  else(ETP_FIRST)
    if(INITIAL_VALUE)
      set(MODIFIED_VALUE ${INITIAL_VALUE})
      list(APPEND MODIFIED_VALUE ${ETP_UNPARSED_ARGUMENTS})
    else(INITIAL_VALUE)
      set(MODIFIED_VALUE ${ETP_UNPARSED_ARGUMENTS})
    endif(INITIAL_VALUE)
  endif(ETP_FIRST)

  set_target_properties(${TARGET} PROPERTIES ${PROPERTY} "${MODIFIED_VALUE}")
endfunction(_leak_track_extend_target_property TARGET PROPERTY CONTENT)

# Add inline source to be built as part of a target.
#
# _leak_track_add_source(<target> <filename> <source_code>)
function(_leak_track_add_source TARGET FILENAME SOURCE)
  # Create the source in a dummy file initially, then marshal to the actual target location if different.
  # This prevents redundant compilation.

  # Define the output directory.
  set(SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/${TARGET}_leak")

  # Write to an intermediate file
  file(WRITE "${SOURCE_DIR}/${FILENAME}.in" "${SOURCE}")

  # See if the intermediate file differs from the intended target file.
  execute_process(COMMAND
    ${CMAKE_COMMAND} -E compare_files
      "${SOURCE_DIR}/${FILENAME}.in"
      "${SOURCE_DIR}/${FILENAME}"
      OUTPUT_VARIABLE STDOUT_LOG
      ERROR_VARIABLE STDERR_LOG
      RESULT_VARIABLE NEEDS_UPDATE)
  if(NEEDS_UPDATE)
    execute_process(COMMAND
      ${CMAKE_COMMAND} -E copy
        "${SOURCE_DIR}/${FILENAME}.in"
        "${SOURCE_DIR}/${FILENAME}")
  endif(NEEDS_UPDATE)

  target_sources(${TARGET} PRIVATE "${SOURCE_DIR}/${FILENAME}")
endfunction(_leak_track_add_source)

# Enable libasan for a target. The target must be an exectuable or this command is ignored.
#
# leak_track_target_enable_asan(<target> [todo-options] [todo-suppress])
#
# This adds the required address santisiation parameters. This process also supports adding Address Sanitizer options
# and supressions.
function(leak_track_target_enable_asan TARGET)
  get_target_property(TARGET_TYPE ${TARGET} TYPE)

  if(TARGET_TYPE STREQUAL "EXECUTABLE")
    # Extend the library linking properties directly to ensure that libasan ends up first. This is a requirement for asan.
    # Publically link the asan library and add the compiler options, while privately linking the asan_config library to
    # configure asan properties.
    _leak_track_extend_target_property(${TARGET} LINK_LIBRARIES FIRST ${ASAN_LIBRARAY})# asan_config)
    _leak_track_extend_target_property(${TARGET} COMPILE_OPTIONS -fsanitize=address -fno-omit-frame-pointer)
  else(TARGET_TYPE STREQUAL "EXECUTABLE")
    message("Cannot enable memory leak tracking for ${TARGET}: only executables can have leak tracking enabled")
  endif(TARGET_TYPE STREQUAL "EXECUTABLE")
endfunction(leak_track_target_enable_asan TARGET)

# Check for a CONDITION keywork argument and evaluate it.
#
# _leak_track_check_condition(<variable> <unparsed_args> ${ARGN})
#
# Typical usage is:
#   _leak_track_check_condition(CONDITION_OK _ARGN ${ARGN})
#   if(NOT CONDITION_OK)
#     return()
#   endif(NOT CONDITION_OK)
#
# This extracts the CONDITION keyword argument and sets the variable (CONDITION_OK) if that argument expression
# evaluates to true. The variable is also set to true if there is no CONDITION argument.
#
# Unused values from ${ARGN} are written to ${UNPARSED_VARIABLE}
function(_leak_track_check_condition VARIABLE UNPARSED_VARIABLE)
  cmake_parse_arguments(LTC "" "CONDITION" "" ${ARGN})
  set(${VARIABLE} ON PARENT_SCOPE)
  if(LTC_CONDITION AND NOT LEAK_TRACK_FORCE)
    # Have a condition to check
    if(${LTC_CONDITION})
      set(${VARIABLE} ON PARENT_SCOPE)
    else(${LTC_CONDITION})
    set(${VARIABLE} OFF PARENT_SCOPE)
    endif(${LTC_CONDITION})
  endif(LTC_CONDITION AND NOT LEAK_TRACK_FORCE)
  set(${UNPARSED_VARIABLE} ${LTC_UNPARSED_ARGUMENTS} PARENT_SCOPE)
endfunction(_leak_track_check_condition)

# Suppress a list of memory leaks for a target.
#
# leak_track_suppress(<target> suppress1 [suppress2...])
#
# To suppress memory leaks, the function is provided a list of symbols from which to ignore memory leaks. These symbols
# can be matched anywhere in the stack to be ignored.
function(leak_track_suppress TARGET)
  if(NOT LEAK_TRACK_WITH_ASAN)
    return()
  endif(NOT LEAK_TRACK_WITH_ASAN)

  _leak_track_check_condition(CONDITION_OK _ARGN ${ARGN})
  if(NOT CONDITION_OK)
    return()
  endif(NOT CONDITION_OK)

  set(SUPPRESS_LIST)

  foreach(ARG ${_ARGN})
    set(SUPPRESS_LIST "${SUPPRESS_LIST}leak:${ARG}\\n")
  endforeach(ARG)

  set(SUPPRESS_SOURCE "
#ifdef __cplusplus
extern \"C\"
{
#endif // __cplusplus
const char *__lsan_default_suppressions()
{
  static const char *kDefaultSuppressions = \"${SUPPRESS_LIST}\";
  return kDefaultSuppressions;
}
#ifdef __cplusplus
}
#endif // __cplusplus
")
  _leak_track_add_source(${TARGET} asanSuppress.cpp "${SUPPRESS_SOURCE}")
endfunction(leak_track_suppress TARGET)

# Suppress a list of memory leaks for a target.
#
# leak_track_suppress(<target> [CONDITION condition] suppress1 [suppress2...])
#
# To suppress memory leaks, the function is provided a list of symbols from which to ignore memory leaks. These symbols
# can be matched anywhere in the stack to be ignored.
function(leak_track_default_options TARGET)
  if(NOT LEAK_TRACK_WITH_ASAN)
    return()
  endif(NOT LEAK_TRACK_WITH_ASAN)

  _leak_track_check_condition(CONDITION_OK _ARGN ${ARGN})
  if(NOT CONDITION_OK)
    return()
  endif(NOT CONDITION_OK)

  set(OPTION_LIST)

  foreach(ARG ${_ARGN})
    set(OPTION_LIST "${OPTION_LIST}${ARG} ")
  endforeach(ARG)

  set(OPTIONS_SOURCE "
#ifdef __cplusplus
extern \"C\"
{
#endif // __cplusplus
const char *__asan_default_options()
{
  static const char *kDefaultOptions = \"${OPTION_LIST}\";
  return kDefaultOptions;
}
#ifdef __cplusplus
}
#endif // __cplusplus
")
  _leak_track_add_source(${TARGET} asanOptions.cpp "${OPTIONS_SOURCE}")
endfunction(leak_track_default_options TARGET)

# Enable memory leak checking for a target.
#
# leak_track_target_enable(<target> [CONDITION condition])
#
# Optionally specify a CONDITION to check, only enabling leak tracking if this passes. Typically this would be the name
# of a CMake option() variable.
function(leak_track_target_enable TARGET)
  _leak_track_check_condition(CONDITION_OK _ARGN ${ARGN})
  if(NOT CONDITION_OK)
    return()
  endif(NOT CONDITION_OK)

  if(LEAK_TRACK_WITH_ASAN)
    leak_track_target_enable_asan(${TARGET})
  endif(LEAK_TRACK_WITH_ASAN)
endfunction(leak_track_target_enable)


================================================
FILE: cmake/OhmCuda.cmake
================================================
# OHM_FEATURE_CUDA_DEFAULT is used to initialise the OHM_FEATURE_CUDA option. We have to work out if CUDA is present first.
# How we do so depends on the CMake version.
set(OHM_FEATURE_CUDA_DEFAULT OFF) # Initialisation value for OHM_FEATURE_CUDA

# CMake 3.10 introduces a native way of configuring CUDA. However, this lacked certain features such as CUDA
# architecture selection, which made it useful only for local builds (not for distribution). 3.18 adds CUDA architecture
# selection as well has having other CUDA features stabilised, making 3.18 the minimum preferred version.
# We prefer the deprecated CUDA setup for versions before 3.18.
#
# Note: there are issues runing nvcc on Windows from within VSCode using the Ninja generator. When configuring or
# building in VSCode+Ninja, nvcc fails reporting that the command line is too long as it tries to run the Visual Studio
# vcvars64.bat file to configure the environment. Nvcc will simply not succeed. The same CMake command lines will work
# fine from a shell prompt (even from the terminal inside VSCode).
#
# For now, the work arounds seem to be:
# 1. Set OHM_USE_DEPRECATED_CMAKE_CUDA
# 2. Use the Visual Studio generator (slower builds)
set(OHM_USE_DEPRECATED_CMAKE_CUDA_DEFAULT ON) # Initialisation value for OHM_USE_DEPRECATED_CMAKE_CUDA
if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.18)
  set(OHM_USE_DEPRECATED_CMAKE_CUDA_DEFAULT OFF)
endif(CMAKE_VERSION VERSION_GREATER_EQUAL 3.18)

# Configure an option for forcibly using the deprecated CUDA project configuration.
option(OHM_USE_DEPRECATED_CMAKE_CUDA
  "Use deprecated CUDA project setup? Disabling requires CMake 3.10+ but the CUDA architecture cannot be set until 3.18."
  ${OHM_USE_DEPRECATED_CMAKE_CUDA_DEFAULT})

# Start by allowing VCPKG to enable the "cuda" feature if it can, so we can find it.
# Skip on Windows; cuda is not available via vcpkg.
if(NOT WIN32)
  if(OHM_VCPKG)
    # Using vcpkg
    if(OHM_FEATURE_CUDA)
      # Already enabled: from cache or command line. Ensure VCPKG feature is present.
      list(APPEND VCPKG_MANIFEST_FEATURES cuda)
    else()
      list(FIND VCPKG_MANIFEST_FEATURES cuda _find_at)
      # Present in the VCPKG feature list. Default to enable cuda cmake option()
      if(_find_at GREATER_EQUAL 0)
        set(OHM_FEATURE_CUDA_DEFAULT ON)
      endif(_find_at GREATER_EQUAL 0)
    endif(OHM_FEATURE_CUDA)
  endif(OHM_VCPKG)
endif(NOT WIN32)

# Find if CUDA is available.
if(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)
  # Newer way of finding CUDA.
  include(CheckLanguage)
  # Check if CUDA is available.
  # If CMake does not find CUDA on the first attempt, then it will set CMAKE_CUDA_COMPILER to "NOTFOUND" which CMake
  # considers a TRUE result when tested - `if(CMAKE_CUDA_COMPILER)`. As such it will not try to find it again. Thus we
  # explicitly unset CMAKE_CUDA_COMPILER when it is NOTFOUND and we keep trying when the CUDA feature is requested.
  if(OHM_FEATURE_CUDA AND CMAKE_CUDA_COMPILER STREQUAL "NOTFOUND")
    unset(CMAKE_CUDA_COMPILER CACHE)
  endif(OHM_FEATURE_CUDA AND CMAKE_CUDA_COMPILER STREQUAL "NOTFOUND")
  check_language(CUDA)
  # Note: CMake 3.11 enabled CMAKE_CUDA_COMPILER on check_language(CUDA). Otherwise it's not propertly set.
  # Another argument for why we perfer a later version for new CUDA project configuration.
  if(CMAKE_VERSION VERSION_LESS 3.11)
    message("WARNING: Cannot automatically check for CUDA using CMake 3.10 unless OHM_USE_DEPRECATED_CMAKE_CUDA is ON.")
  endif(CMAKE_VERSION VERSION_LESS 3.11)
  if(CMAKE_CUDA_COMPILER)
    # CUDA compiler is available. Set OHM_FEATURE_CUDA_DEFAULT so that we initialse OHM_FEATURE_CUDA to ON by default.
    set(OHM_FEATURE_CUDA_DEFAULT ON)
  endif(CMAKE_CUDA_COMPILER)

else(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)
  # Ensure we use the dynamic/shared runtime libraries when building shared libaries. See message below.
  if(BUILD_SHARED_LIBS)
    if(NOT DEFINED CUDA_USE_STATIC_CUDA_RUNTIME OR CUDA_USE_STATIC_CUDA_RUNTIME)
      if(CUDA_USE_STATIC_CUDA_RUNTIME)
        # Wrong value for CUDA_USE_STATIC_CUDA_RUNTIME : warn user.
        message("Warning: BUILD_SHARED_LIBS and CUDA_USE_STATIC_CUDA_RUNTIME are both set. This can result in runtime "
                "failures when invoking CUDA kernels. Forcing CUDA_USE_STATIC_CUDA_RUNTIME to OFF.\n"
                "Use cmake-gui, ccmake or manually edit the CMakeCache.txt file for this project to update these values.")
      endif(CUDA_USE_STATIC_CUDA_RUNTIME)
      set(CUDA_USE_STATIC_CUDA_RUNTIME Off CACHE BOOL
          "Use the static version of the CUDA runtime library if available"
          FORCE)
    endif(NOT DEFINED CUDA_USE_STATIC_CUDA_RUNTIME OR CUDA_USE_STATIC_CUDA_RUNTIME)
  endif(BUILD_SHARED_LIBS)
  find_package(CUDA)
  if(CUDA_FOUND)
    set(OHM_CUDA_VERSION "${CUDA_VERSION}")
    set(OHM_FEATURE_CUDA_DEFAULT ON)
  endif(CUDA_FOUND)
endif(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)

# Set the option for building ohm with CUDA based on whether or not we found it.
option(OHM_FEATURE_CUDA "Build ohm library and utlities for CUDA?" ${OHM_FEATURE_CUDA_DEFAULT})

# Additional options
if(OHM_FEATURE_CUDA)
  set(OHM_CUDA_DEBUG "none" CACHE STRING
    "Controls the CUDA debug level, regardless of the current build type. lineinfo adds debug symbols, full also disabled optimisation."
  )
  set_property(CACHE OHM_CUDA_DEBUG PROPERTY STRINGS full lineinfo none)
endif(OHM_FEATURE_CUDA)

macro(_cuda_get_version VAR)
  if(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)
    # CMAKE_CUDA_COMPILER_VERSION only becomes available after enabling the cuda language.
    enable_language(CUDA)
    set(${VAR} "${CMAKE_CUDA_COMPILER_VERSION}")
  else()
    set(${VAR} "${CUDA_VERSION}")
  endif()
endmacro()

#==============================================================================
# Configure various CUDA building options.
#==============================================================================
macro(cuda_setup)
  if(OHM_FEATURE_CUDA)
    _cuda_get_version(OHM_CUDA_VERSION)
    # Architecture reference:
    # https://arnon.dk/matching-sm-architectures-arch-and-gencode-for-various-nvidia-cards/
    #
    # Require version 9 implied minimum.
    set(CUDA_ARCH_INIT "50;52;53;60;61;62;70;72")
    if(OHM_CUDA_VERSION VERSION_GREATER 11)
      # Last version to support 5.x
      set(CUDA_ARCH_INIT "50;52;53;60;61;62;70;72;75;80;86")
    elseif(OHM_CUDA_VERSION VERSION_GREATER 10)
      # Adds 7.0, drops 3.0, 3.5, 3.7
      set(CUDA_ARCH_INIT "50;52;53;60;61;62;70;72;75")
    endif()
    # Set the default architectures to all those vailable for the current CUDA verison.
    set(OHM_CUDA_ARCHITECTURES "${CUDA_ARCH_INIT}" CACHE STRING "Specifies the list of CUDA compute architectures to compile for.")

    if(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)
      _cuda_setup_build_options()
    else(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)
      _cuda_setup_deprecated_build_options()
    endif(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)
  endif(OHM_FEATURE_CUDA)
endmacro(cuda_setup)

#==============================================================================
#==============================================================================
macro(_cuda_setup_build_options)
  # Setup architectures
  set(CMAKE_CUDA_ARCHITECTURES "${OHM_CUDA_ARCHITECTURES}")
  # Match CUDA standard to the C++ standard for the project.
  set(CMAKE_CUDA_STANDARD ${CMAKE_CXX_STANDARD})
  set(CMAKE_CUDA_STANDARD_REQUIRED ${CMAKE_CXX_STANDARD_REQUIRED})
  # Note; the CMake setup (3.18) adds the compiler flag '-std=c++##'. This is correct for NVCC, GCC and Clang, but is
  # not correct for MSVC where it should be '/std:c++##' (note the use of ':' rather than '='). This generates a build
  # warning that the host compiler will ignore '-std=c++##'. This is harmless, but would be nice to avoid.
  # However, we should add the right flag for MSVC, so we do it below.
  if(MSVC)
    add_compile_options("-std:c++${CMAKE_CXX_STANDARD}")
  endif( MSVC)
  if(OHM_CUDA_DEBUG STREQUAL "full")
    add_compile_options(
      "$<$<COMPILE_LANGUAGE:CUDA>:-g>"
      "$<$<COMPILE_LANGUAGE:CUDA>:-G>"
    )
  elseif(OHM_CUDA_DEBUG STREQUAL "lineinfo")
    add_compile_options(
      "$<$<COMPILE_LANGUAGE:CUDA>:-g>"
      "$<$<COMPILE_LANGUAGE:CUDA>:-lineinfo>"
    )
  endif(OHM_CUDA_DEBUG STREQUAL "full")
  if(BUILD_SHARED_LIBS)
    set(OHM_CUDA_LIBRARY CUDA::cudart)
  else(BUILD_SHARED_LIBS)
    set(OHM_CUDA_LIBRARY CUDA::cudart_static)
  endif(BUILD_SHARED_LIBS)
endmacro(_cuda_setup_build_options)

#==============================================================================
# Setup CUDA build options for the older, deprecated project configuration.
#==============================================================================
macro(_cuda_setup_deprecated_build_options)
  set(CUDA_LINK_LIBRARIES_KEYWORD PRIVATE)

    # Configure CUDA NVCC falgs for each selected CUDA architecture.
  if (OHM_CUDA_ARCHITECTURES)
    foreach(ARCH ${OHM_CUDA_ARCHITECTURES})
      _nvcc_flags_append(CUDA_NVCC_FLAGS "-gencode arch=compute_${ARCH},code=sm_${ARCH}")
    endforeach(ARCH)
  endif(OHM_CUDA_ARCHITECTURES)

  # Add debugging flags.
  if(OHM_CUDA_DEBUG STREQUAL "full")
    _nvcc_flags_append(CUDA_NVCC_FLAGS "-g -G")
  elseif(OHM_CUDA_DEBUG STREQUAL "lineinfo")
    _nvcc_flags_append(CUDA_NVCC_FLAGS "-g -lineinfo")
  endif(OHM_CUDA_DEBUG STREQUAL "full")

  # CUDA compiler doesn't seem to respect the CMAKE_POSITION_INDEPENDENT_CODE variable. Explicitly add it for
  # GCC comilation. May be needed for Clang as well (untested).
  if(CMAKE_POSITION_INDEPENDENT_CODE)
    if(CMAKE_CXX_COMPILER_ID MATCHES "GNU")# OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
      _nvcc_flags_append(CUDA_NVCC_FLAGS "-Xcompiler -fPIC")
    endif(CMAKE_CXX_COMPILER_ID MATCHES "GNU")# OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  endif(CMAKE_POSITION_INDEPENDENT_CODE)

  # Migrate the flags to the calling scope.
  set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS}")
endmacro(_cuda_setup_deprecated_build_options)

#==============================================================================
# This function deals with a peculiar issue with the generation of the
# generated CUDA compilation script.
#
# The CUDA compilation script can end up with bad values in nvcc_flags
# as extracted from the directory property COMPILE_DEFINITIONS. In particular,
# VTK leaves definitions in two poor forms:
# - vtkRenderingContext2D_AUTOINIT=1(vtkRenderingContextOpenGL)
# - vtkRenderingCore_INCLUDE="this/is/a/path"
#
# The first has an issue with brackets, the second the issue is the quotes not
# being escaped. Likely, neither would be an issue if the nvcc_flags value was
# properly quoted when generated.
#==============================================================================
function(_nvcc_fix_definitions)
  get_directory_property(CUDA_NVCC_DEFINITIONS COMPILE_DEFINITIONS)
  # VTK compile definitions interfere with the CUDA definitions. For now take the easy options and remove problem options.
  set(CUDA_NVCC_DEFINITIONS_FIXED)
  if(CUDA_NVCC_DEFINITIONS)
    foreach(_definition ${CUDA_NVCC_DEFINITIONS})
      set(USEDEF "${_definition}")
      if(_definition MATCHES ".*=.*\\(.*\\)")
        # Handle VKT options in the form: vtkRenderingContext2D_AUTOINIT=1(vtkRenderingContextOpenGL)
        string(REGEX REPLACE "\\(.*\\)" "" _definitionFixed "${_definition}")
        #message("Problem: ${_definition} -> ${_definitionFixed}")
        set(_definition "${_definitionFixed}")
      elseif (_definition MATCHES ".*_INCLUDE=\\\".*\\\"")
        # vtkRenderingCore_INCLUDE="path\path"
        # The issue is the quotes not getting escaped later.
        string(REGEX REPLACE "\\\"" "\\\\\"" _definitionFixed "${_definition}")
        #message("Problem: ${_definition} -> ${_definitionFixed}")
        set(_definition "${_definitionFixed}")
      endif()
      #message("${_definition}")
      _nvcc_flags_append(CUDA_NVCC_DEFINITIONS_FIXED "${_definition}")
    endforeach()
    #message("CUDA_NVCC_DEFINITIONS_FIXED: ${CUDA_NVCC_DEFINITIONS_FIXED}")
    set_property(DIRECTORY . PROPERTY COMPILE_DEFINITIONS "${CUDA_NVCC_DEFINITIONS_FIXED}")
    get_directory_property(CUDA_NVCC_DEFINITIONS COMPILE_DEFINITIONS)
    #message("CUDA_NVCC_DEFINITIONS: ${CUDA_NVCC_DEFINITIONS}")
  endif()
endfunction(_nvcc_fix_definitions)


#==============================================================================
# Extend a compiler flags variable FLAGS_VAR with additional FLAGS string.
#==============================================================================
macro(_nvcc_flags_append FLAGS_VAR FLAGS)
  if(${FLAGS_VAR})
    set(${FLAGS_VAR} "${${FLAGS_VAR}} ${FLAGS}")
  else(FLAGS_VAR)
    # Nothing set. Add new options.
    set(${FLAGS_VAR} "${FLAGS}")
  endif(${FLAGS_VAR})
endmacro(_nvcc_flags_append)


================================================
FILE: cmake/OhmGTest.cmake
================================================
option(OHM_SYSTEM_GTEST "Have ohm use a system available version of Googletest via find_package()?" ${OHM_VCPKG})

if(OHM_SYSTEM_GTEST)
  # Enable VCPKG tests feature. This will only have an effect when using vcpkg.
  find_package(GTest REQUIRED)
  get_target_property(GTEST_INCLUDE_DIRS GTest::gtest INTERFACE_INCLUDE_DIRECTORIES)
  set(GTEST_LIBRARIES GTest::gtest)
  set(GTEST_MAIN_LIBRARIES GTest::gtest_main)
  return()
endif(OHM_SYSTEM_GTEST)

include(ExternalProject)

find_package(Threads)

# State setup
get_property(_GENERATOR_IS_MULTI_CONFIG GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG)

set(_GTEST_CONFIG_ARGS "-Dgtest_force_shared_crt=On")

if(NOT _GENERATOR_IS_MULTI_CONFIG)
  list(APPEND _GTEST_CONFIG_ARGS "-DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE}")
endif(NOT _GENERATOR_IS_MULTI_CONFIG)

# Setup download and build of Googletest
ExternalProject_Add(
    googletest
    URL https://github.com/google/googletest/archive/release-1.10.0.zip
    PREFIX ${CMAKE_BINARY_DIR}/3rd-party/googletest
    URL_MD5 82358affdd7ab94854c8ee73a180fc53
    INSTALL_COMMAND ""
    CMAKE_ARGS ${_GTEST_CONFIG_ARGS}
)

# Resolve directories used by gtest
ExternalProject_Get_Property(googletest source_dir)
ExternalProject_Get_Property(googletest binary_dir)

# Helper function for resolving a gtest library name of a build config
function(_gtest_library_path OUTVAR LIBNAME BUILD_CONFIG binary_dir)
  # Handle post fix; gtest is configured to add postifx the debug library names with 'd'
  unset(_POSTFIX)
  if(BUILD_CONFIG STREQUAL "Debug")
    set(_POSTFIX "_d")
  endif(BUILD_CONFIG STREQUAL "Debug")
  set(_PATH_DIR "${binary_dir}/lib")
  # Handle path prefix
  if(_GENERATOR_IS_MULTI_CONFIG)
    set(_PATH_DIR "${_PATH_DIR}/${BUILD_CONFIG}")
  endif(_GENERATOR_IS_MULTI_CONFIG)
  set(${OUTVAR}
    ${_PATH_DIR}/${CMAKE_STATIC_LIBRARY_PREFIX}${LIBNAME}${_POSTFIX}${CMAKE_STATIC_LIBRARY_SUFFIX}
    PARENT_SCOPE)
endfunction(_gtest_library_path)

# Resolve build configuration(s)
set(_BUILD_CONFIGS)
if(_GENERATOR_IS_MULTI_CONFIG)
  set(_BUILD_CONFIGS ${CMAKE_CONFIGURATION_TYPES})
else(_GENERATOR_IS_MULTI_CONFIG)
  if(CMAKE_BUILD_TYPE)
    set(_BUILD_CONFIGS ${CMAKE_BUILD_TYPE})
  else(CMAKE_BUILD_TYPE)
    set(_BUILD_CONFIGS Release)
  endif(CMAKE_BUILD_TYPE)
endif(_GENERATOR_IS_MULTI_CONFIG)

# Setup import targets
add_library(gtest UNKNOWN IMPORTED)
add_dependencies(gtest googletest)

add_library(gtest_main UNKNOWN IMPORTED)
add_dependencies(gtest_main gtest googletest)

# Configure include directory now. Used in loop below.
set(GTEST_INCLUDE_DIR "${source_dir}/googletest/include")

# Resolve library paths.
foreach(_CONFIG ${_BUILD_CONFIGS})
  string(TOUPPER ${_CONFIG} _CONFIG_UPPER)
  _gtest_library_path(GTEST_LIBRARY_PATH gtest "${_CONFIG}" "${binary_dir}")
  _gtest_library_path(GTEST_MAIN_LIBRARY_PATH gtest_main "${_CONFIG}" "${binary_dir}")
  if(_GENERATOR_IS_MULTI_CONFIG)
    set_property(TARGET gtest PROPERTY IMPORTED_LOCATION_${_CONFIG_UPPER} "${GTEST_LIBRARY_PATH}")
    set_property(TARGET gtest_main PROPERTY IMPORTED_LOCATION_${_CONFIG_UPPER} "${GTEST_MAIN_LIBRARY_PATH}")
  else(_GENERATOR_IS_MULTI_CONFIG)
    set_property(TARGET gtest PROPERTY IMPORTED_LOCATION "${GTEST_LIBRARY_PATH}")
    set_property(TARGET gtest_main PROPERTY IMPORTED_LOCATION "${GTEST_MAIN_LIBRARY_PATH}")
  endif(_GENERATOR_IS_MULTI_CONFIG)
  set_property(TARGET gtest PROPERTY INCLUDE_DIRECTORIES "${GTEST_INCLUDE_DIR}")
  set_property(TARGET gtest_main PROPERTY INCLUDE_DIRECTORIES "${GTEST_INCLUDE_DIR}")

  # Make Ninja builds work
  # Ninja has a different way of scanning to Make and needs the gtest library artefact files to exist in order to start
  # the build at all. We "touch" those files here.
  if(CMAKE_GENERATOR MATCHES "^Ninja")
    if(NOT EXISTS ${GTEST_LIBRARY_PATH})
    file(WRITE ${GTEST_LIBRARY_PATH} "")
      endif(NOT EXISTS ${GTEST_LIBRARY_PATH})
    if(NOT EXISTS ${GTEST_MAIN_LIBRARY_PATH})
      file(WRITE ${GTEST_MAIN_LIBRARY_PATH} "")
    endif(NOT EXISTS ${GTEST_MAIN_LIBRARY_PATH})
  endif(CMAKE_GENERATOR MATCHES "^Ninja")
endforeach(_CONFIG)

# Setup GTest variables, mimicing FindGTest.cmake
set(GTEST_INCLUDE_DIRS "${GTEST_INCLUDE_DIR}")
set(GTEST_LIBRARIES gtest Threads::Threads)
set(GTEST_MAIN_LIBRARIES gtest_main)


================================================
FILE: cmake/TextFileResource.cmake
================================================
include(CMakeParseArguments)

set(_TEXT_FILE_RESOURCE_PY "${CMAKE_CURRENT_LIST_DIR}/TextFileResource.py")

# text_file_resource(
#     textFile resourceName
#     [FILELIST varname]
#     [TYPE filetype]
#     [INCLUDE_PATTERN pattern]
#     [COMMENT_PATTERN pattern]
#     [NO_IMPORT]
#     [MINIFY]
#     [BINARY]
#     [ECHO]
#     [PATHS path1 path2 ...]
# )
#
# Take a text file and generate a C++ header and source file containing a string variable with the file content.
# Can optionally expand include/import statements either inferred by file type or by using a regular expression match.
# This essentially will paste "included" file contents at the include location
#
# The resource files are generated in the CMAKE_CURRENT_BINARY_DIR as follows:
# - Generates a .h and a .cpp file for the TEXT_FILE
# - Generated file names are based on TEXT_FILE:
#   - Remove directory and extension from TEXT_FILE.
#   - Append 'Resource'
# - Generated files define a string variable named RESOURCE_NAME.
#
# The FILELIST argument may be used to track the generated files. The two generated files are appended to the specified
# variable.
#
# For example:
#   text_file_resource(resources/text.txt EmbeddedText FILELIST RESOURCE_FILES)
# This generates the following:
# - textResource.h and textResource.cpp in CMAKE_CURRENT_BINARY_DIR.
# - Define the variable EmbeddedText containing the content of resource/text.txt.
# - RESOURCE_FILES has the following appended to it: ${CMAKE_CURRENT_BINARY_DIR}/textResource.h and
#   ${CMAKE_CURRENT_BINARY_DIR}/textResource.cpp
#
# Include/import statement expansion is supported when the IMPORT argument is given. The include pattern is normally
# inferred by file type; for example, the include pattern for a C/C++ file is:
#   #include <file_path>
# for which we use the following regular expression:
#     ^[ \t]*#include[ \t]+[<"](.*)[>"].*$
#
# The include pattern may be overridden by specifying INCLUDE_PATTERN pattern where pattern is a a regular expression.
# This expression must have a capture (1) which isolates the included file path.
#
# Such expressions are located and replaced by the contents for <file_path>. We search for the
# file_path first in the same directy as the origin file, but also on the given PATHS. This behaviour may be disabled
# by adding the NO_IMPORT argument.
#
# Note that string literals are limited to less than 2^16 characters. Longer content may be handled by the following
# options:
# - MINIFY : strips leading and trailing whitespace and removes 'comment' strings (see COMMENT_PATTERN).
# - BINARY : inserts the text as an array of bytes instead of as a string.
#
# Note that MINIFY will yield shorter strings, but they may still be too larger. BINARY mode is always invoked when
# the string to be written is too long.
#
# MINIFY supports comment stripping, normally identified by the file type. The comment regular expression may be
# overridden by specifying COMMENT_PATTERN pattern.
#
# The resource type is normally inferred by file extension, but may also be explicitly selected. The following
# table identifies the supported types and the extentions.
#
# Type    | Language  | Extensions
# ------- | --------- | ----------
# c       | C         | c, h
# c#      | C#        | cs
# cmake   | CMake     | cmake, CMakeLists.txt
# cpp     | C++       | cpp, cxx, h, hpp, hxx
# cuda    | CUDA      | cu
# java    | Java      | java
# opencl  | OpenCL    | cl
# python  | Python    | py
#
# Include/import example:
#   text_file_resource(gpucode.cl GpuCode
#       PATHS ${CMAKE_CURRENT_LIST_DIR} anotherPath
#   )
# The INCLUDE_PATTERN matches patterns such as '#include "somefile.h"' with the capture extracting 'somefile.h'.
# This will be replaced with the contents of 'somefile.h' resolved from one of the PATHS.
#
# Finally, the command line executed may be shown by adding ECHO.
#
# FIXME: strings are limited to 2^16 - 1 characters. Provide a workaround.
function(text_file_resource TEXT_FILE RESOURCE_NAME)
  # Requires python to do the conversion.
  find_package(PythonInterp REQUIRED)

  cmake_parse_arguments(TFR "BINARY;MINIFY;NO_IMPORT;ECHO" "COMMENT_PATTERN;INCLUDE_PATTERN;FILELIST;TYPE" "PATHS" ${ARGN})

  # Prepare command line arguments.
  set(_args)
  if(TFR_BINARY)
    list(APPEND _args "--mode=binary")
  endif()
  if(TFR_MINIFY)
    list(APPEND _args "--minify=on")
  endif()
  if(TFR_NO_IMPORT)
    list(APPEND _args "--no-import")
  endif()
  if(TFR_INCLUDE_PATTERN)
    list(APPEND _args "--include-regex=\"${TFR_INCLUDE_PATTERN}\"")
  endif()
  if(TFR_COMMENT_PATTERN)
    list(APPEND _args "--comment-regex=\"${TFR_COMMENT_PATTERN}\"")
  endif()
  if(TFR_TYPE)
    list(APPEND _args "--style=${TFR_TYPE}")
  endif()

  foreach(PATH ${TFR_PATHS})
    list(APPEND _args "-I\"${PATH}\"")
  endforeach(PATH)

  get_filename_component(BASE_FILE "${TEXT_FILE}" NAME_WE)
  string(APPEND BASE_FILE Resource)

  if(TFR_FILELIST)
    list(APPEND ${TFR_FILELIST}
      "${CMAKE_CURRENT_BINARY_DIR}/${BASE_FILE}.cpp"
      "${CMAKE_CURRENT_BINARY_DIR}/${BASE_FILE}.h")
    set(${TFR_FILELIST} ${${TFR_FILELIST}} PARENT_SCOPE)
    # message("...${TFR_FILELIST}: ${CMAKE_CURRENT_BINARY_DIR}/${RESOURCE_NAME}.cpp ${CMAKE_CURRENT_BINARY_DIR}/${RESOURCE_NAME}.h")
  endif(TFR_FILELIST)

  if(TFR_ECHO)
    message("${PYTHON_EXECUTABLE} ${_TEXT_FILE_RESOURCE_PY} ${TEXT_FILE} ${CMAKE_CURRENT_BINARY_DIR}/${BASE_FILE} --name=${RESOURCE_NAME} ${_args}")
  endif(TFR_ECHO)

  execute_process(
    COMMAND "${PYTHON_EXECUTABLE}" "${_TEXT_FILE_RESOURCE_PY}" "${TEXT_FILE}" "${CMAKE_CURRENT_BINARY_DIR}/${BASE_FILE}" --name=${RESOURCE_NAME} ${_args}
    OUTPUT_VARIABLE PROCESS_OUTPUT ERROR_VARIABLE PROCESS_OUTPUT
    RESULT_VARIABLE EXIT_CODE
    WORKING_DIRECTORY "${CMAKE_CURRENT_LIST_DIR}")

  if(NOT EXIT_CODE EQUAL 0)
    message(SEND_ERROR "text_file_resource(${TEXT_FILE} ${RESOURCE_NAME}) failed (${ERROR_CODE})")
    message("${PROCESS_OUTPUT}")
  elseif(PROCESS_OUTPUT)
    message("text_file_resource(${TEXT_FILE} ${RESOURCE_NAME}):")
    message("${PROCESS_OUTPUT}")
  endif()
endfunction(text_file_resource)


================================================
FILE: cmake/TextFileResource.py
================================================
"""Convert a text file into a symbol into a symbol contained in a source file.

This can be used to embed a text file into a compiled binary.
"""
import argparse
import copy
import os.path
import re
import sys

if sys.version_info[0] == 2:
    import __future__


#-------------------------------------------------------------------------------
# Argument parsing setup
#-------------------------------------------------------------------------------
class ArgParser(argparse.ArgumentParser):

    def error(self, message):
        if len(sys.argv) > 1:
            sys.stderr.write('error: %s\n' % message)
            self.print_usage()
            sys.exit(1)
        else:
            self.print_help()
        sys.exit(0)


class StyleInfo:

    def __init__(self,
                 file_matches=(),
                 comment_regex=None,
                 include_regex=None,
                 priority=0):
        self.extensions = file_matches
        self.comment_regex = comment_regex
        self.include_regex = include_regex
        self.priority = priority


def buildStyles():
    # Base C-Style comment
    c_style_comment = re.compile(
        r'(^[ \t\r]*\n)|(^[ \t]*//.*\n)|(//.*$)|(/\*.*\*/)', re.M)
    # C/C++ include regular expression
    c_style_include = re.compile(r'^[ \t]*#include[ \t]+[<"](.*)[>"].*$', re.M)
    styles = {
        'none':
        StyleInfo(),
        'text':
        StyleInfo(),
        'c':
        StyleInfo(['c', 'h'], c_style_comment, c_style_include),
        'c#':
        StyleInfo(['cs'], c_style_comment),
        'cmake':
        StyleInfo(['cmake', 'CMakeLists.txt'], re.compile(r'#.*$', re.M)),
        'cpp':
        StyleInfo(['cpp', 'cxx', 'h', 'hpp', 'hxx'],
                  c_style_comment,
                  c_style_include,
                  priority=1),
        'cuda':
        StyleInfo(['cu'], c_style_comment, c_style_include),
        'java':
        StyleInfo(['java'], c_style_comment),
        'opencl':
        StyleInfo(['cl'], c_style_comment, c_style_include),
        'python':
        StyleInfo(['py'], c_style_comment)
    }
    return styles


def str2bool(val):
    if val.lower() in ('yes', 'true', 'on', 'y', '1'):
        return True
    elif val.lower() in ('no', 'false', 'off', 'n', '0'):
        return False
    else:
        raise argparse.ArgumentTypeError('Invalid boolean string: %s' % val)


parser = ArgParser(description = \
"""Convert a text file into a symbol into a symbol contained in a source file.

This can be used to embed a text file into a compiled binary.
""")

# Build styles
text_file_styles = buildStyles()

parser.add_argument('input',
                    metavar='input',
                    type=str,
                    help='Text file to convert into a C++ compilation unit.')
parser.add_argument('output',
                    metavar='output',
                    type=str,
                    help='Base name/path for the output without extension.')

parser.add_argument('--src-ext',
                    type=str,
                    default='cpp',
                    help='File extention for the source file. Default is cpp.')
parser.add_argument('--h-ext',
                    type=str,
                    default='h',
                    help='File extention for the header file.')
parser.add_argument(
    '--mode',
    type=str,
    default='text',
    choices=('binary', 'text'),
    help=
    'Output mode control; text mode writes as readable text, while bytes write as an array of '
    + 'bytes. Bytes are also written if the string content is too long.')
parser.add_argument(
    '--style',
    type=str,
    default='none',
    choices=text_file_styles.keys(),
    help=
    'Explicitly specify the file type. Affects comment and include directive handling. '
    + 'Matched by extension when set to none. Use "text" for no style.')
parser.add_argument(
    '--comment-regex',
    type=str,
    help=
    'Override for the regular expression used to match comments for minification.'
    + 'Applied as a multi-line regular expression.')
parser.add_argument(
    "--name",
    type=str,
    help=
    "Specifies the output variable name. The default is '<filename>Resource' without the path."
)
parser.add_argument(
    '--minify',
    type=str,
    default='off',
    choices=('off', 'on', 'comments', 'whitespace'),
    help='Minification mode.\n' + '\t\toff : leave as is.\n' +
    '\t\ton  : full minification.\n' +
    '\t\tcomments : strip comment strings. See also --comment-style.\n' +
    '\t\twhitespace : strip leading and trailing whitespace.')
parser.add_argument('--no-import',
                    type=str2bool,
                    const=True,
                    nargs='?',
                    help='Do not expand include/import statements?')
parser.add_argument(
    '--include-regex',
    type=str,
    help=
    'Override for the regular expression used to "include" other files. Expression is '
    +
    'replaced by the file identified in capture 1. This file is searched on the -I paths'
)
parser.add_argument(
    '-I',
    type=str,
    dest='include_dirs',
    action='append',
    help=
    'Specifies a path used to resolve include statements. May be specified multiple times.'
)

args = parser.parse_args()


#-------------------------------------------------------------------------------
# Other functions
#-------------------------------------------------------------------------------
def resolveIncludeFilePath(include_file, include_dirs):
    for dir in include_dirs:
        path = os.path.join(dir, include_file)
        # path = path.replace("/", "\\")
        if os.path.exists(path) and os.path.isfile(path):
            return path
    return None


def resolveIncludeDirectives(regex, content, include_dirs):
    directive = regex.search(content)
    haveShownPaths = False
    included_file_paths = []
    while directive:
        include_file_path = resolveIncludeFilePath(directive.group(1),
                                                   include_dirs)
        next_search_offset = directive.start()
        if include_file_path:
            # Load replacement text if we haven't already
            include_content = ''
            # if not include_file_path in included_file_paths:
            with open(include_file_path, 'r') as include_file:
                include_content = include_file.read()
            included_file_paths.append(include_file_path)
            content = content[:directive.start(
            )] + include_content + content[directive.end():]
        else:
            # Failed to resolve.
            if not haveShownPaths:
                haveShownPaths = True
                print("Using include paths:", include_dirs)
            print("Failed to resolve include", directive.group(0), ":",
                  directive.group(1))
            next_search_offset = directive.end()
        directive = regex.search(content, next_search_offset)
    return content


def selectStyle(styles, args):
    if args.style == 'none':
        input_file = os.path.split(args.input)[1]
        input_ext = os.path.splitext(input_file)[1]
        # Strip leading '.' in extention.
        if len(input_ext):
            input_ext = input_ext[1:]
        # Find the appropriate style by extention.
        selected_style = None
        for style_name in styles.keys():
            style = styles[style_name]
            for extention in style.extensions:
                if input_ext == extention or input_file == extention:
                    if selected_style == None or style.priority > selected_style.priority:
                        selected_style = style
                        break
        if not selected_style:
            # Default to text style.
            selected_style = styles['text']
    else:
        selected_style = styles[args.style]

    # Handle overriding comment and include regular expressions.
    if args.comment_regex or args.include_regex:
        selected_style = copy.copy(selected_style)
        if args.comment_regex:
            selected_style.comment_regex = re.compile(args.comment_regex, re.M)
        if args.include_:
            selected_style.include_regex = re.compile(args.include_regex, re.M)

    return selected_style


#-------------------------------------------------------------------------------
# Processing script.
#-------------------------------------------------------------------------------
# Setup output file names
if args.src_ext[0] != '.':
    args.src_ext = '.' + args.src_ext
if args.h_ext[0] != '.':
    args.h_ext = '.' + args.h_ext

args.output_source = os.path.splitext(args.output)[0] + args.src_ext
args.output_header = os.path.splitext(args.output)[0] + args.h_ext

# Add in the input file directory to the search paths.
if not args.include_dirs:
    args.include_dirs = []
args.include_dirs.append(os.path.split(os.path.abspath(args.input))[0])

# Strip quotes from include_dirs in case they are passed with quotes.
strip_quotes_rex = re.compile(r'(^")|("$)')
for i in range(len(args.include_dirs)):
    args.include_dirs[i] = strip_quotes_rex.sub('', args.include_dirs[i])

# Setup comment regular expression.
style = selectStyle(text_file_styles, args)

# Define the header guard for the output file.
header_guard = os.path.splitext(os.path.split(args.output)[1])[0]
# Replace invalid characters.
resource_name = re.sub(r'[-\+\*\@\!\.]', '_', header_guard)
header_guard = resource_name.upper() + '_H'

if args.name:
    resource_name = args.name

# Write header file
with open(args.output_header, 'w') as header_file:
    header_file.write("""// Resource file generated from {0}
#ifndef {1}
#define {1}

extern const unsigned {2}_length; // NOLINT
extern const char * const {2}; // NOLINT

#endif // {1}
""".format(args.input, header_guard, resource_name))

# Read the file content.
with open(args.input, 'r') as input_file:
    input_file_content = input_file.read()

# Find include directives
if style.include_regex and not args.no_import:
    input_file_content = resolveIncludeDirectives(style.include_regex,
                                                  input_file_content,
                                                  args.include_dirs)

# Minify if required
minify_whitepace = args.minify == 'whitespace'
minify_comments = args.minify == 'comments'
if args.minify == 'on':
    minify_whitepace = True
    minify_comments = True

# Strip comments
if minify_comments and style.comment_regex != None:
    input_file_content = style.comment_regex.sub('', input_file_content)
if minify_whitepace:
    # Remove trailing whitespace
    input_file_content = re.sub(r'[ \t]+$', '', input_file_content, flags=re.M)
    # Replace leading whitespace
    input_file_content = re.sub(r'(^[ \t]+)',
                                '',
                                input_file_content,
                                flags=re.M)

input_file_content_length = len(input_file_content)

# Verify the content length: C++ string declarations are limited to less than 2^16 characters.
# If ok, we append quotes.
if input_file_content_length < 1 << 16 and args.mode == 'text':
    # Build the content string to write
    # Do content replacements:
    #   \           : \\
    #   <newline>   : \n\"<newline>\"
    #   <tab>       : \t
    # Replace quotes.
    input_file_content_string = input_file_content.replace('"', '\\"')
    # Replace backslashes excluding the quoted we just escaped.
    input_file_content_string = re.sub(r'\\(?!")', r'\\\\',
                                       input_file_content_string)
    input_file_content_string = re.sub(r'\r?\n', r'\\n"\n"',
                                       input_file_content_string)
    input_file_content_string = re.sub(r'\t', '\\t', input_file_content_string)
    input_file_content_string = '"' + input_file_content_string + '"'
else:
    # String too long. Convert the original content to byte array.
    input_file_content_bytes = []
    input_file_content_bytes = input_file_content.encode('utf8')
    input_file_content_length = len(input_file_content_bytes)
    input_file_content_string = ''
    content_buffer = ['{\n']
    buffered_count = 0
    for byte in input_file_content_bytes:
        if sys.version_info < (3, 0):
            byte = ord(byte)
        content_buffer.append(str(byte))
        content_buffer.append(',')
        buffered_count += 2
        if buffered_count % 110 == 0:
            content_buffer.append('\n')
    content_buffer.append('\n0\n}')
    input_file_content_string = ''.join(content_buffer)

# Start writing output
with open(args.output_source, 'w') as source_file:
    source_file.write("""// Resource file generated from {0}
#include "{1}"

const unsigned {2}_length = {3};  // NOLINT
static const char {2}_[] =  // NOLINT\n""".format(
        args.input, os.path.basename(args.output_header), resource_name,
        input_file_content_length))

    source_file.write(input_file_content_string)
    source_file.write('; // NOLINT\n')
    source_file.write(
        "const char * const {0} = {0}_; // NOLINT\n".format(resource_name))
    source_file.write('\n')


================================================
FILE: cmake/compilerSetup.cmake
================================================
# Copyright (c) 2017
# Commonwealth Scientific and Industrial Research Organisation (CSIRO)
# ABN 41 687 119 230
#
# Author: Kazys Stepanas

# Configure compiler warnings depending on the current compiler.

# Configure warnings for gcc.
macro(warnings_gcc)
  add_compile_options(
    # "-pedantic"
    "-Wall"
    "-Wextra"
    "$<$<COMPILE_LANGUAGE:CXX>:-Werror=return-type>"
    # "-Wconversion"
    # "$<$<COMPILE_LANGUAGE:CXX>:-Werror=pedantic>"
    "$<$<COMPILE_LANGUAGE:CXX>:-Werror=vla>"
    "-Wno-unused-function"
    "-Wno-missing-braces"
    "-Wno-unknown-pragmas"
    "-Wno-parentheses"
    "-Wno-ignored-attributes"
  )
endmacro(warnings_gcc)

# Configure for apple clang.
macro(setup_apple_clang)
  warnings_gcc()
  # Disable precedence warning of && and || in if statements:
  #   && === *
  #   || === +
  add_compile_options(
    "-Wno-logical-op-parentheses"
  )
endmacro(setup_apple_clang)

# Configure for GCC
macro(setup_gcc)
  warnings_gcc()
endmacro(setup_gcc)

# Configure for MSVC
macro(setup_msvc)
  # For Visual Studio, force PDB based debug information even in release builds.
  # This has a small impact on the size of the of the DLLs, but provides debug information for release mode crashes.
  # The PDBs can be kept for debugging specific releases, but do not need to be shipped as part of the runtime, unless
  # shipping an SDK. The last point is to address MSVC linked warnings which are impossible to suppress without providing
  # PDB files.
  # We also add /FS (Force Synchronous PDB Writes) to support CUDA compilation which will try to use the same PDB file
  # for various .cu files (from the same project).
  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /FS")
  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /FS")
  set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /FS")
  set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} /Zi /FS")
  set(CMAKE_MODULE_LINKER_FLAGS_RELEASE "/debug ${CMAKE_MODULE_LINKER_FLAGS_RELEASE}")
  set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "/debug ${CMAKE_SHARED_LINKER_FLAGS_RELEASE}")
  set(CMAKE_EXE_LINKER_FLAGS_RELEASE "/debug ${CMAKE_EXE_LINKER_FLAGS_RELEASE}")

  # Level 3 warnings by default is fine. May go to level 4.
  # Enable multi-processor project compilation.
  option(COMPILER_MULTI_PROCESSOR "Use multiple processor compilation (MSVC)?" OFF)
  if(COMPILER_MULTI_PROCESSOR)
    add_compile_options("/MP")
  endif(COMPILER_MULTI_PROCESSOR)
  if(OHM_BUILD_SHARED_SUPPRESS_WARNINGS)
    # Disable the following MSC compiler warnings.
    # 4251 Warning that data members of a DLL export definition do not themselves have DLL export directives.
    # 4275 Warning that a base class of a DLL export definition do not themselves have DLL export directives.
    # These are valid and important warning which indicate potential ABI incompatibilities. However, ohm is not
    # engineered to be 100% robust to ABI changes and these warnings generate many difficult to avoid errors.
    #
    # For example, using std::vector as a member of a class which is exported from a DLL raises warning C4251. The issue
    # is that this is vulnerable to ABI incompatibilies if a different C++ runtime library is used - such as building
    # ohm and building client code using difference Visual Studio versions.
    #
    # Similarly, C4275 is raised by actions such as deriving std::runtime_error.
    #
    # Both these cases are essentially valid code, but the warnings are also correct to point out the ABI vulnerability.
    # The side effect is that upgrading version will often require a rebuild of client code against the ohm libraries
    # rather than a drop in replacement. Linux and MacOS are slightly innocculated against this because the C++ runtime
    # libraries tend to stay the same for an OS version. The warnings would be much more relevant when writing things
    # like COM interfaces.
    #
    # Note: we use the generator to only add these for C++ language compilation to avoid pushing these out to
    # the NVCC compiler for CUDA compilation.
    add_compile_options("$<$<COMPILE_LANGUAGE:CXX>:/wd4251>" "$<$<COMPILE_LANGUAGE:CXX>:/wd4275>")
  endif(OHM_BUILD_SHARED_SUPPRESS_WARNINGS)
endmacro(setup_msvc)

# Select the correct warnings configuration.
if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang")
  # using Apple Clang (MacOS)
  setup_apple_clang()
elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
  # using GCC
  setup_gcc()
# elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Intel")
#   # using Intel C++
elseif (MSVC)
  # using Visual Studio C++
  setup_msvc()
else()
  message("Unknown compiler ${CMAKE_CXX_COMPILER_ID}")
endif()


================================================
FILE: cmake/doxyfile.in
================================================
# Doxyfile 1.9.2

# This file describes the settings to be used by the documentation system
# doxygen (www.doxygen.org) for a project.
#
# All text after a double hash (##) is considered a comment and is placed in
# front of the TAG it is preceding.
#
# All text after a single hash (#) is considered a comment and will be ignored.
# The format is:
# TAG = value [value, ...]
# For lists, items can also be appended using:
# TAG += value [value, ...]
# Values that contain spaces should be placed between quotes (\" \").

#---------------------------------------------------------------------------
# Project related configuration options
#---------------------------------------------------------------------------

# This tag specifies the encoding used for all characters in the configuration
# file that follow. The default is UTF-8 which is also the encoding used for all
# text before the first occurrence of this tag. Doxygen uses libiconv (or the
# iconv built into libc) for the transcoding. See
# https://www.gnu.org/software/libiconv/ for the list of possible encodings.
# The default value is: UTF-8.

DOXYFILE_ENCODING      = UTF-8

# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
# double-quotes, unless you are using Doxywizard) that should identify the
# project for which the documentation is generated. This name is used in the
# title of most generated pages and in a few other places.
# The default value is: My Project.

PROJECT_NAME           = @DOXYGEN_PROJECT_NAME@

# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
# could be handy for archiving the generated documentation or if some version
# control system is used.

PROJECT_NUMBER         = @DOXYGEN_PROJECT_VERSION@

# Using the PROJECT_BRIEF tag one can provide an optional one line description
# for a project that appears at the top of each page and should give viewer a
# quick idea about the purpose of the project. Keep the description short.

PROJECT_BRIEF          =

# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
# in the documentation. The maximum height of the logo should not exceed 55
# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
# the logo to the output directory.

PROJECT_LOGO           =

# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
# into which the generated documentation will be written. If a relative path is
# entered, it will be relative to the location where doxygen was started. If
# left blank the current directory will be used.

OUTPUT_DIRECTORY       = .

# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
# directories (in 2 levels) under the output directory of each output format and
# will distribute the generated files over these directories. Enabling this
# option can be useful when feeding doxygen a huge amount of source files, where
# putting all generated files in the same directory would otherwise causes
# performance problems for the file system.
# The default value is: NO.

CREATE_SUBDIRS         = NO

# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
# characters to appear in the names of generated files. If set to NO, non-ASCII
# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
# U+3044.
# The default value is: NO.

ALLOW_UNICODE_NAMES    = NO

# The OUTPUT_LANGUAGE tag is used to specify the language in which all
# documentation generated by doxygen is written. Doxygen will use this
# information to generate all constant output in the proper language.
# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
# Ukrainian and Vietnamese.
# The default value is: English.

OUTPUT_LANGUAGE        = English

# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
# descriptions after the members that are listed in the file and class
# documentation (similar to Javadoc). Set to NO to disable this.
# The default value is: YES.

BRIEF_MEMBER_DESC      = YES

# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
# description of a member or function before the detailed description
#
# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
# brief descriptions will be completely suppressed.
# The default value is: YES.

REPEAT_BRIEF           = YES

# This tag implements a quasi-intelligent brief description abbreviator that is
# used to form the text in various listings. Each string in this list, if found
# as the leading text of the brief description, will be stripped from the text
# and the result, after processing the whole list, is used as the annotated
# text. Otherwise, the brief description is used as-is. If left blank, the
# following values are used ($name is automatically replaced with the name of
# the entity):The $name class, The $name widget, The $name file, is, provides,
# specifies, contains, represents, a, an and the.

ABBREVIATE_BRIEF       =

# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
# doxygen will generate a detailed section even if there is only a brief
# description.
# The default value is: NO.

ALWAYS_DETAILED_SEC    = NO

# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
# inherited members of a class in the documentation of that class as if those
# members were ordinary class members. Constructors, destructors and assignment
# operators of the base classes will not be shown.
# The default value is: NO.

INLINE_INHERITED_MEMB  = NO

# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
# before files name in the file list and in the header files. If set to NO the
# shortest path that makes the file name unique will be used
# The default value is: YES.

FULL_PATH_NAMES        = NO

# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
# Stripping is only done if one of the specified strings matches the left-hand
# part of the path. The tag can be used to show relative paths in the file list.
# If left blank the directory from which doxygen is run is used as the path to
# strip.
#
# Note that you can specify absolute paths here, but also relative paths, which
# will be relative from the directory where doxygen is started.
# This tag requires that the tag FULL_PATH_NAMES is set to YES.

STRIP_FROM_PATH        =

# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
# path mentioned in the documentation of a class, which tells the reader which
# header file to include in order to use a class. If left blank only the name of
# the header file containing the class definition is used. Otherwise one should
# specify the list of include paths that are normally passed to the compiler
# using the -I flag.

STRIP_FROM_INC_PATH    =

# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
# less readable) file names. This can be useful is your file systems doesn't
# support long names like on DOS, Mac, or CD-ROM.
# The default value is: NO.

SHORT_NAMES            = NO

# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
# first line (until the first dot) of a Javadoc-style comment as the brief
# description. If set to NO, the Javadoc-style will behave just like regular Qt-
# style comments (thus requiring an explicit @brief command for a brief
# description.)
# The default value is: NO.

JAVADOC_AUTOBRIEF      = YES

# If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line
# such as
# /***************
# as being the beginning of a Javadoc-style comment "banner". If set to NO, the
# Javadoc-style will behave just like regular comments and it will not be
# interpreted by doxygen.
# The default value is: NO.

JAVADOC_BANNER         = NO

# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
# line (until the first dot) of a Qt-style comment as the brief description. If
# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
# requiring an explicit \brief command for a brief description.)
# The default value is: NO.

QT_AUTOBRIEF           = NO

# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
# a brief description. This used to be the default behavior. The new default is
# to treat a multi-line C++ comment block as a detailed description. Set this
# tag to YES if you prefer the old behavior instead.
#
# Note that setting this tag to YES also means that rational rose comments are
# not recognized any more.
# The default value is: NO.

MULTILINE_CPP_IS_BRIEF = NO

# By default Python docstrings are displayed as preformatted text and doxygen's
# special commands cannot be used. By setting PYTHON_DOCSTRING to NO the
# doxygen's special commands can be used and the contents of the docstring
# documentation blocks is shown as doxygen documentation.
# The default value is: YES.

PYTHON_DOCSTRING       = YES

# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
# documentation from any documented member that it re-implements.
# The default value is: YES.

INHERIT_DOCS           = YES

# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
# page for each member. If set to NO, the documentation of a member will be part
# of the file/class/namespace that contains it.
# The default value is: NO.

SEPARATE_MEMBER_PAGES  = NO

# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
# uses this value to replace tabs by spaces in code fragments.
# Minimum value: 1, maximum value: 16, default value: 4.

TAB_SIZE               = 2

# This tag can be used to specify a number of aliases that act as commands in
# the documentation. An alias has the form:
# name=value
# For example adding
# "sideeffect=@par Side Effects:^^"
# will allow you to put the command \sideeffect (or @sideeffect) in the
# documentation, which will result in a user-defined paragraph with heading
# "Side Effects:". Note that you cannot put \n's in the value part of an alias
# to insert newlines (in the resulting output). You can put ^^ in the value part
# of an alias to insert a newline as if a physical newline was in the original
# file. When you need a literal { or } or , in the value part of an alias you
# have to escape them by means of a backslash (\), this can lead to conflicts
# with the commands \{ and \} for these it is advised to use the version @{ and
# @} or use a double escape (\\{ and \\})

ALIASES                =

# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
# only. Doxygen will then generate output that is more tailored for C. For
# instance, some of the names that are used will be different. The list of all
# members will be omitted, etc.
# The default value is: NO.

OPTIMIZE_OUTPUT_FOR_C  = NO

# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
# Python sources only. Doxygen will then generate output that is more tailored
# for that language. For instance, namespaces will be presented as packages,
# qualified scopes will look different, etc.
# The default value is: NO.

OPTIMIZE_OUTPUT_JAVA   = NO

# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
# sources. Doxygen will then generate output that is tailored for Fortran.
# The default value is: NO.

OPTIMIZE_FOR_FORTRAN   = NO

# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
# sources. Doxygen will then generate output that is tailored for VHDL.
# The default value is: NO.

OPTIMIZE_OUTPUT_VHDL   = NO

# Set the OPTIMIZE_OUTPUT_SLICE tag to YES if your project consists of Slice
# sources only. Doxygen will then generate output that is more tailored for that
# language. For instance, namespaces will be presented as modules, types will be
# separated into more groups, etc.
# The default value is: NO.

OPTIMIZE_OUTPUT_SLICE  = NO

# Doxygen selects the parser to use depending on the extension of the files it
# parses. With this tag you can assign which parser to use for a given
# extension. Doxygen has a built-in mapping, but you can override or extend it
# using this tag. The format is ext=language, where ext is a file extension, and
# language is one of the parsers supported by doxygen: IDL, Java, JavaScript,
# Csharp (C#), C, C++, Lex, D, PHP, md (Markdown), Objective-C, Python, Slice,
# VHDL, Fortran (fixed format Fortran: FortranFixed, free formatted Fortran:
# FortranFree, unknown formatted Fortran: Fortran. In the later case the parser
# tries to guess whether the code is fixed or free formatted code, this is the
# default for Fortran type files). For instance to make doxygen treat .inc files
# as Fortran files (default is PHP), and .f files as C (default is Fortran),
# use: inc=Fortran f=C.
#
# Note: For files without extension you can use no_extension as a placeholder.
#
# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
# the files are not read by doxygen. When specifying no_extension you should add
# * to the FILE_PATTERNS.
#
# Note see also the list of default file extension mappings.

EXTENSION_MAPPING      =

# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
# according to the Markdown format, which allows for more readable
# documentation. See https://daringfireball.net/projects/markdown/ for details.
# The output of markdown processing is further processed by doxygen, so you can
# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
# case of backward compatibilities issues.
# The default value is: YES.

MARKDOWN_SUPPORT       = YES

# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up
# to that level are automatically included in the table of contents, even if
# they do not have an id attribute.
# Note: This feature currently applies only to Markdown headings.
# Minimum value: 0, maximum value: 99, default value: 5.
# This tag requires that the tag MARKDOWN_SUPPORT is set to YES.

TOC_INCLUDE_HEADINGS   = 5

# When enabled doxygen tries to link words that correspond to documented
# classes, or namespaces to their corresponding documentation. Such a link can
# be prevented in individual cases by putting a % sign in front of the word or
# globally by setting AUTOLINK_SUPPORT to NO.
# The default value is: YES.

AUTOLINK_SUPPORT       = YES

# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
# to include (a tag file for) the STL sources as input, then you should set this
# tag to YES in order to let doxygen match functions declarations and
# definitions whose arguments contain STL classes (e.g. func(std::string);
# versus func(std::string) {}). This also make the inheritance and collaboration
# diagrams that involve STL classes more complete and accurate.
# The default value is: NO.

BUILTIN_STL_SUPPORT    = NO

# If you use Microsoft's C++/CLI language, you should set this option to YES to
# enable parsing support.
# The default value is: NO.

CPP_CLI_SUPPORT        = NO

# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
# https://www.riverbankcomputing.com/software/sip/intro) sources only. Doxygen
# will parse them like normal C++ but will assume all classes use public instead
# of private inheritance when no explicit protection keyword is present.
# The default value is: NO.

SIP_SUPPORT            = NO

# For Microsoft's IDL there are propget and propput attributes to indicate
# getter and setter methods for a property. Setting this option to YES will make
# doxygen to replace the get and set methods by a property in the documentation.
# This will only work if the methods are indeed getting or setting a simple
# type. If this is not the case, or you want to show the methods anyway, you
# should set this option to NO.
# The default value is: YES.

IDL_PROPERTY_SUPPORT   = YES

# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
# tag is set to YES then doxygen will reuse the documentation of the first
# member in the group (if any) for the other members of the group. By default
# all members of a group must be documented explicitly.
# The default value is: NO.

DISTRIBUTE_GROUP_DOC   = NO

# If one adds a struct or class to a group and this option is enabled, then also
# any nested class or struct is added to the same group. By default this option
# is disabled and one has to add nested compounds explicitly via \ingroup.
# The default value is: NO.

GROUP_NESTED_COMPOUNDS = NO

# Set the SUBGROUPING tag to YES to allow class member groups of the same type
# (for instance a group of public functions) to be put as a subgroup of that
# type (e.g. under the Public Functions section). Set it to NO to prevent
# subgrouping. Alternatively, this can be done per class using the
# \nosubgrouping command.
# The default value is: YES.

SUBGROUPING            = YES

# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
# are shown inside the group in which they are included (e.g. using \ingroup)
# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
# and RTF).
#
# Note that this feature does not work in combination with
# SEPARATE_MEMBER_PAGES.
# The default value is: NO.

INLINE_GROUPED_CLASSES = NO

# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
# with only public data fields or simple typedef fields will be shown inline in
# the documentation of the scope in which they are defined (i.e. file,
# namespace, or group documentation), provided this scope is documented. If set
# to NO, structs, classes, and unions are shown on a separate page (for HTML and
# Man pages) or section (for LaTeX and RTF).
# The default value is: NO.

INLINE_SIMPLE_STRUCTS  = NO

# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
# enum is documented as struct, union, or enum with the name of the typedef. So
# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
# with name TypeT. When disabled the typedef will appear as a member of a file,
# namespace, or class. And the struct will be named TypeS. This can typically be
# useful for C code in case the coding convention dictates that all compound
# types are typedef'ed and only the typedef is referenced, never the tag name.
# The default value is: NO.

TYPEDEF_HIDES_STRUCT   = NO

# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
# cache is used to resolve symbols given their name and scope. Since this can be
# an expensive process and often the same symbol appears multiple times in the
# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
# doxygen will become slower. If the cache is too large, memory is wasted. The
# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
# symbols. At the end of a run doxygen will report the cache usage and suggest
# the optimal cache size from a speed point of view.
# Minimum value: 0, maximum value: 9, default value: 0.

LOOKUP_CACHE_SIZE      = 0

# The NUM_PROC_THREADS specifies the number threads doxygen is allowed to use
# during processing. When set to 0 doxygen will based this on the number of
# cores available in the system. You can set it explicitly to a value larger
# than 0 to get more control over the balance between CPU load and processing
# speed. At this moment only the input processing can be done using multiple
# threads. Since this is still an experimental feature the default is set to 1,
# which effectively disables parallel processing. Please report any issues you
# encounter. Generating dot graphs in parallel is controlled by the
# DOT_NUM_THREADS setting.
# Minimum value: 0, maximum value: 32, default value: 1.

NUM_PROC_THREADS       = 1

#---------------------------------------------------------------------------
# Build related configuration options
#---------------------------------------------------------------------------

# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
# documentation are documented, even if no documentation was available. Private
# class members and static file members will be hidden unless the
# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
# Note: This will also disable the warnings about undocumented members that are
# normally produced when WARNINGS is set to YES.
# The default value is: NO.

EXTRACT_ALL            = NO

# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
# be included in the documentation.
# The default value is: NO.

EXTRACT_PRIVATE        = NO

# If the EXTRACT_PRIV_VIRTUAL tag is set to YES, documented private virtual
# methods of a class will be included in the documentation.
# The default value is: NO.

EXTRACT_PRIV_VIRTUAL   = NO

# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
# scope will be included in the documentation.
# The default value is: NO.

EXTRACT_PACKAGE        = NO

# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
# included in the documentation.
# The default value is: NO.

EXTRACT_STATIC         = NO

# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
# locally in source files will be included in the documentation. If set to NO,
# only classes defined in header files are included. Does not have any effect
# for Java sources.
# The default value is: YES.

EXTRACT_LOCAL_CLASSES  = NO

# This flag is only useful for Objective-C code. If set to YES, local methods,
# which are defined in the implementation section but not in the interface are
# included in the documentation. If set to NO, only methods in the interface are
# included.
# The default value is: NO.

EXTRACT_LOCAL_METHODS  = NO

# If this flag is set to YES, the members of anonymous namespaces will be
# extracted and appear in the documentation as a namespace called
# 'anonymous_namespace{file}', where file will be replaced with the base name of
# the file that contains the anonymous namespace. By default anonymous namespace
# are hidden.
# The default value is: NO.

EXTRACT_ANON_NSPACES   = NO

# If this flag is set to YES, the name of an unnamed parameter in a declaration
# will be determined by the corresponding definition. By default unnamed
# parameters remain unnamed in the output.
# The default value is: YES.

RESOLVE_UNNAMED_PARAMS = YES

# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
# undocumented members inside documented classes or files. If set to NO these
# members will be included in the various overviews, but no documentation
# section is generated. This option has no effect if EXTRACT_ALL is enabled.
# The default value is: NO.

HIDE_UNDOC_MEMBERS     = NO

# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
# undocumented classes that are normally visible in the class hierarchy. If set
# to NO, these classes will be included in the various overviews. This option
# has no effect if EXTRACT_ALL is enabled.
# The default value is: NO.

HIDE_UNDOC_CLASSES     = NO

# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
# declarations. If set to NO, these declarations will be included in the
# documentation.
# The default value is: NO.

HIDE_FRIEND_COMPOUNDS  = NO

# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
# documentation blocks found inside the body of a function. If set to NO, these
# blocks will be appended to the function's detailed documentation block.
# The default value is: NO.

HIDE_IN_BODY_DOCS      = NO

# The INTERNAL_DOCS tag determines if documentation that is typed after a
# \internal command is included. If the tag is set to NO then the documentation
# will be excluded. Set it to YES to include the internal documentation.
# The default value is: NO.

INTERNAL_DOCS          = NO

# With the correct setting of option CASE_SENSE_NAMES doxygen will better be
# able to match the capabilities of the underlying filesystem. In case the
# filesystem is case sensitive (i.e. it supports files in the same directory
# whose names only differ in casing), the option must be set to YES to properly
# deal with such files in case they appear in the input. For filesystems that
# are not case sensitive the option should be be set to NO to properly deal with
# output files written for symbols that only differ in casing, such as for two
# classes, one named CLASS and the other named Class, and to also support
# references to files without having to specify the exact matching casing. On
# Windows (including Cygwin) and MacOS, users should typically set this option
# to NO, whereas on Linux or other Unix flavors it should typically be set to
# YES.
# The default value is: system dependent.

CASE_SENSE_NAMES       = YES

# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
# their full class and namespace scopes in the documentation. If set to YES, the
# scope will be hidden.
# The default value is: NO.

HIDE_SCOPE_NAMES       = NO

# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
# append additional text to a page's title, such as Class Reference. If set to
# YES the compound reference will be hidden.
# The default value is: NO.

HIDE_COMPOUND_REFERENCE= NO

# If the SHOW_HEADERFILE tag is set to YES then the documentation for a class
# will show which file needs to be included to use the class.
# The default value is: YES.

SHOW_HEADERFILE        = YES

# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
# the files that are included by a file in the documentation of that file.
# The default value is: YES.

SHOW_INCLUDE_FILES     = YES

# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
# grouped member an include statement to the documentation, telling the reader
# which file to include in order to use the member.
# The default value is: NO.

SHOW_GROUPED_MEMB_INC  = NO

# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
# files with double quotes in the documentation rather than with sharp brackets.
# The default value is: NO.

FORCE_LOCAL_INCLUDES   = NO

# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
# documentation for inline members.
# The default value is: YES.

INLINE_INFO            = YES

# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
# (detailed) documentation of file and class members alphabetically by member
# name. If set to NO, the members will appear in declaration order.
# The default value is: YES.

SORT_MEMBER_DOCS       = YES

# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
# descriptions of file, namespace and class members alphabetically by member
# name. If set to NO, the members will appear in declaration order. Note that
# this will also influence the order of the classes in the class list.
# The default value is: NO.

SORT_BRIEF_DOCS        = NO

# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
# (brief and detailed) documentation of class members so that constructors and
# destructors are listed first. If set to NO the constructors will appear in the
# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
# member documentation.
# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
# detailed member documentation.
# The default value is: NO.

SORT_MEMBERS_CTORS_1ST = NO

# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
# of group names into alphabetical order. If set to NO the group names will
# appear in their defined order.
# The default value is: NO.

SORT_GROUP_NAMES       = NO

# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
# fully-qualified names, including namespaces. If set to NO, the class list will
# be sorted only by class name, not including the namespace part.
# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
# Note: This option applies only to the class list, not to the alphabetical
# list.
# The default value is: NO.

SORT_BY_SCOPE_NAME     = NO

# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
# type resolution of all parameters of a function it will reject a match between
# the prototype and the implementation of a member function even if there is
# only one candidate or it is obvious which candidate to choose by doing a
# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
# accept a match between prototype and implementation in such cases.
# The default value is: NO.

STRICT_PROTO_MATCHING  = NO

# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
# list. This list is created by putting \todo commands in the documentation.
# The default value is: YES.

GENERATE_TODOLIST      = YES

# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
# list. This list is created by putting \test commands in the documentation.
# The default value is: YES.

GENERATE_TESTLIST      = YES

# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
# list. This list is created by putting \bug commands in the documentation.
# The default value is: YES.

GENERATE_BUGLIST       = YES

# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
# the deprecated list. This list is created by putting \deprecated commands in
# the documentation.
# The default value is: YES.

GENERATE_DEPRECATEDLIST= YES

# The ENABLED_SECTIONS tag can be used to enable conditional documentation
# sections, marked by \if <section_label> ... \endif and \cond <section_label>
# ... \endcond blocks.

ENABLED_SECTIONS       =

# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
# initial value of a variable or macro / define can have for it to appear in the
# documentation. If the initializer consists of more lines than specified here
# it will be hidden. Use a value of 0 to hide initializers completely. The
# appearance of the value of individual variables and macros / defines can be
# controlled using \showinitializer or \hideinitializer command in the
# documentation regardless of this setting.
# Minimum value: 0, maximum value: 10000, default value: 30.

MAX_INITIALIZER_LINES  = 30

# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
# the bottom of the documentation of classes and structs. If set to YES, the
# list will mention the files that were used to generate the documentation.
# The default value is: YES.

SHOW_USED_FILES        = YES

# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
# will remove the Files entry from the Quick Index and from the Folder Tree View
# (if specified).
# The default value is: YES.

SHOW_FILES             = NO

# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
# page. This will remove the Namespaces entry from the Quick Index and from the
# Folder Tree View (if specified).
# The default value is: YES.

SHOW_NAMESPACES        = YES

# The FILE_VERSION_FILTER tag can be used to specify a program or script that
# doxygen should invoke to get the current version for each file (typically from
# the version control system). Doxygen will invoke the program by executing (via
# popen()) the command command input-file, where command is the value of the
# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
# by doxygen. Whatever the program writes to standard output is used as the file
# version. For an example see the documentation.

FILE_VERSION_FILTER    =

# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
# by doxygen. The layout file controls the global structure of the generated
# output files in an output format independent way. To create the layout file
# that represents doxygen's defaults, run doxygen with the -l option. You can
# optionally specify a file name after the option, if omitted DoxygenLayout.xml
# will be used as the name of the layout file. See also section "Changing the
# layout of pages" for information.
#
# Note that if you run doxygen from a directory containing a file called
# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
# tag is left empty.

LAYOUT_FILE            =

# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
# the reference definitions. This must be a list of .bib files. The .bib
# extension is automatically appended if omitted. This requires the bibtex tool
# to be installed. See also https://en.wikipedia.org/wiki/BibTeX for more info.
# For LaTeX the style of the bibliography can be controlled using
# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
# search path. See also \cite for info how to create references.

CITE_BIB_FILES         =

#---------------------------------------------------------------------------
# Configuration options related to warning and progress messages
#---------------------------------------------------------------------------

# The QUIET tag can be used to turn on/off the messages that are generated to
# standard output by doxygen. If QUIET is set to YES this implies that the
# messages are off.
# The default value is: NO.

QUIET                  = YES

# The WARNINGS tag can be used to turn on/off the warning messages that are
# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
# this implies that the warnings are on.
#
# Tip: Turn warnings on while writing the documentation.
# The default value is: YES.

WARNINGS               = YES

# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
# will automatically be disabled.
# The default value is: YES.

WARN_IF_UNDOCUMENTED   = YES

# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
# potential errors in the documentation, such as documenting some parameters in
# a documented function twice, or documenting parameters that don't exist or
# using markup commands wrongly.
# The default value is: YES.

WARN_IF_DOC_ERROR      = YES

# If WARN_IF_INCOMPLETE_DOC is set to YES, doxygen will warn about incomplete
# function parameter documentation. If set to NO, doxygen will accept that some
# parameters have no documentation without warning.
# The default value is: YES.

WARN_IF_INCOMPLETE_DOC = YES

# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
# are documented, but have no documentation for their parameters or return
# value. If set to NO, doxygen will only warn about wrong parameter
# documentation, but not about the absence of documentation. If EXTRACT_ALL is
# set to YES then this flag will automatically be disabled. See also
# WARN_IF_INCOMPLETE_DOC
# The default value is: NO.

WARN_NO_PARAMDOC       = NO

# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
# a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS
# then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but
# at the end of the doxygen process doxygen will return with a non-zero status.
# Possible values are: NO, YES and FAIL_ON_WARNINGS.
# The default value is: NO.

WARN_AS_ERROR          = NO

# The WARN_FORMAT tag determines the format of the warning messages that doxygen
# can produce. The string should contain the $file, $line, and $text tags, which
# will be replaced by the file and line number from which the warning originated
# and the warning text. Optionally the format may contain $version, which will
# be replaced by the version of the file (if it could be obtained via
# FILE_VERSION_FILTER)
# The default value is: $file:$line: $text.

WARN_FORMAT            = "$file:$line: $text"

# The WARN_LOGFILE tag can be used to specify a file to which warning and error
# messages should be written. If left blank the output is written to standard
# error (stderr).

WARN_LOGFILE           =

#---------------------------------------------------------------------------
# Configuration options related to the input files
#---------------------------------------------------------------------------

# The INPUT tag is used to specify the files and/or directories that contain
# documented source files. You may enter file names like myfile.cpp or
# directories like /usr/src/myproject. Separate the files or directories with
# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
# Note: If this tag is empty the current directory is searched.

INPUT                  = @DOXYGEN_INPUT_DIRS@

# This tag can be used to specify the character encoding of the source files
# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
# documentation (see:
# https://www.gnu.org/software/libiconv/) for the list of possible encodings.
# The default value is: UTF-8.

INPUT_ENCODING         = UTF-8

# If the value of the INPUT tag contains directories, you can use the
# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
# *.h) to filter out the source-files in the directories.
#
# Note that for custom extensions or not directly supported extensions you also
# need to set EXTENSION_MAPPING for the extension otherwise the files are not
# read by doxygen.
#
# Note the list of default checked file patterns might differ from the list of
# default file extension mappings.
#
# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
# *.hh, *.hxx, *.hpp, *.h++, *.l, *.cs, *.d, *.php, *.php4, *.php5, *.phtml,
# *.inc, *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C
# comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f18, *.f, *.for, *.vhd,
# *.vhdl, *.ucf, *.qsf and *.ice.

FILE_PATTERNS          =

# The RECURSIVE tag can be used to specify whether or not subdirectories should
# be searched for input files as well.
# The default value is: NO.

RECURSIVE              = YES

# The EXCLUDE tag can be used to specify files and/or directories that should be
# excluded from the INPUT source files. This way you can easily exclude a
# subdirectory from a directory tree whose root is specified with the INPUT tag.
#
# Note that relative paths are relative to the directory from which doxygen is
# run.

EXCLUDE                = @DOXYGEN_EXCLUDE_DIRS@

# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
# directories that are symbolic links (a Unix file system feature) are excluded
# from the input.
# The default value is: NO.

EXCLUDE_SYMLINKS       = NO

# If the value of the INPUT tag contains directories, you can use the
# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
# certain files from those directories.
#
# Note that the wildcards are matched against the file with absolute path, so to
# exclude all test directories for example use the pattern */test/*

EXCLUDE_PATTERNS       =  *.cpp \
                          ui_*.h \
                          moc_*.cpp \
                          qrc_*.cpp

# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
# (namespaces, classes, functions, etc.) that should be excluded from the
# output. The symbol name can be a fully qualified name, a word, or if the
# wildcard * is used, a substring. Examples: ANamespace, AClass,
# AClass::ANamespace, ANamespace::*Test
#
# Note that the wildcards are matched against the file with absolute path, so to
# exclude all test directories use the pattern */test/*

EXCLUDE_SYMBOLS        =

# The EXAMPLE_PATH tag can be used to specify one or more files or directories
# that contain example code fragments that are included (see the \include
# command).

EXAMPLE_PATH           = @DOXYGEN_EXAMPLE_PATH@

# If the value of the EXAMPLE_PATH tag contains directories, you can use the
# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
# *.h) to filter out the source-files in the directories. If left blank all
# files are included.

EXAMPLE_PATTERNS       =

# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
# searched for input files to be used with the \include or \dontinclude commands
# irrespective of the value of the RECURSIVE tag.
# The default value is: NO.

EXAMPLE_RECURSIVE      = NO

# The IMAGE_PATH tag can be used to specify one or more files or directories
# that contain images that are to be included in the documentation (see the
# \image command).

IMAGE_PATH             = @DOXYGEN_IMAGE_PATH@

# The INPUT_FILTER tag can be used to specify a program that doxygen should
# invoke to filter for each input file. Doxygen will invoke the filter program
# by executing (via popen()) the command:
#
# <filter> <input-file>
#
# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
# name of an input file. Doxygen will then use the output that the filter
# program writes to standard output. If FILTER_PATTERNS is specified, this tag
# will be ignored.
#
# Note that the filter must not add or remove lines; it is applied before the
# code is scanned, but not when the output code is generated. If lines are added
# or removed, the anchors will not be placed correctly.
#
# Note that for custom extensions or not directly supported extensions you also
# need to set EXTENSION_MAPPING for the extension otherwise the files are not
# properly processed by doxygen.

INPUT_FILTER           =

# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
# basis. Doxygen will compare the file name with each pattern and apply the
# filter if there is a match. The filters are a list of the form: pattern=filter
# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
# patterns match the file name, INPUT_FILTER is applied.
#
# Note that for custom extensions or not directly supported extensions you also
# need to set EXTENSION_MAPPING for the extension otherwise the files are not
# properly processed by doxygen.

FILTER_PATTERNS        =

# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
# INPUT_FILTER) will also be used to filter the input files that are used for
# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
# The default value is: NO.

FILTER_SOURCE_FILES    = NO

# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
# it is also possible to disable source filtering for a specific pattern using
# *.ext= (so without naming a filter).
# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.

FILTER_SOURCE_PATTERNS =

# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
# is part of the input, its contents will be placed on the main page
# (index.html). This can be useful if you have a project on for instance GitHub
# and want to reuse the introduction page also for the doxygen output.

USE_MDFILE_AS_MAINPAGE =

#---------------------------------------------------------------------------
# Configuration options related to source browsing
#---------------------------------------------------------------------------

# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
# generated. Documented entities will be cross-referenced with these sources.
#
# Note: To get rid of all source code in the generated output, make sure that
# also VERBATIM_HEADERS is set to NO.
# The default value is: NO.

SOURCE_BROWSER         = NO

# Setting the INLINE_SOURCES tag to YES will include the body of functions,
# classes and enums directly into the documentation.
# The default value is: NO.

INLINE_SOURCES         = NO

# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
# special comment blocks from generated source code fragments. Normal C, C++ and
# Fortran comments will always remain visible.
# The default value is: YES.

STRIP_CODE_COMMENTS    = YES

# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
# entity all documented functions referencing it will be listed.
# The default value is: NO.

REFERENCED_BY_RELATION = YES

# If the REFERENCES_RELATION tag is set to YES then for each documented function
# all documented entities called/used by that function will be listed.
# The default value is: NO.

REFERENCES_RELATION    = YES

# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
# to YES then the hyperlinks from functions in REFERENCES_RELATION and
# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
# link to the documentation.
# The default value is: YES.

REFERENCES_LINK_SOURCE = YES

# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
# source code will show a tooltip with additional information such as prototype,
# brief description and links to the definition and documentation. Since this
# will make the HTML file larger and loading of large files a bit slower, you
# can opt to disable this feature.
# The default value is: YES.
# This tag requires that the tag SOURCE_BROWSER is set to YES.

SOURCE_TOOLTIPS        = YES

# If the USE_HTAGS tag is set to YES then the references to source code will
# point to the HTML generated by the htags(1) tool instead of doxygen built-in
# source browser. The htags tool is part of GNU's global source tagging system
# (see https://www.gnu.org/software/global/global.html). You will need version
# 4.8.6 or higher.
#
# To use it do the following:
# - Install the latest version of global
# - Enable SOURCE_BROWSER and USE_HTAGS in the configuration file
# - Make sure the INPUT points to the root of the source tree
# - Run doxygen as normal
#
# Doxygen will invoke htags (and that will in turn invoke gtags), so these
# tools must be available from the command line (i.e. in the search path).
#
# The result: instead of the source browser generated by doxygen, the links to
# source code will now point to the output of htags.
# The default value is: NO.
# This tag requires that the tag SOURCE_BROWSER is set to YES.

USE_HTAGS              = NO

# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
# verbatim copy of the header file for each class for which an include is
# specified. Set to NO to disable this.
# See also: Section \class.
# The default value is: YES.

VERBATIM_HEADERS       = YES

# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
# clang parser (see:
# http://clang.llvm.org/) for more accurate parsing at the cost of reduced
# performance. This can be particularly helpful with template rich C++ code for
# which doxygen's built-in parser lacks the necessary type information.
# Note: The availability of this option depends on whether or not doxygen was
# generated with the -Duse_libclang=ON option for CMake.
# The default value is: NO.

CLANG_ASSISTED_PARSING = NO

# If the CLANG_ASSISTED_PARSING tag is set to YES and the CLANG_ADD_INC_PATHS
# tag is set to YES then doxygen will add the directory of each input to the
# include path.
# The default value is: YES.
# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.

CLANG_ADD_INC_PATHS    = YES

# If clang assisted parsing is enabled you can provide the compiler with command
# line options that you would normally use when invoking the compiler. Note that
# the include paths will already be set by doxygen for the files and directories
# specified with INPUT and INCLUDE_PATH.
# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.

CLANG_OPTIONS          =

# If clang assisted parsing is enabled you can provide the clang parser with the
# path to the directory containing a file called compile_commands.json. This
# file is the compilation database (see:
# http://clang.llvm.org/docs/HowToSetupToolingForLLVM.html) containing the
# options used when the source files were built. This is equivalent to
# specifying the -p option to a clang tool, such as clang-check. These options
# will then be passed to the parser. Any options specified with CLANG_OPTIONS
# will be added as well.
# Note: The availability of this option depends on whether or not doxygen was
# generated with the -Duse_libclang=ON option for CMake.

CLANG_DATABASE_PATH    =

#---------------------------------------------------------------------------
# Configuration options related to the alphabetical class index
#---------------------------------------------------------------------------

# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
# compounds will be generated. Enable this if the project contains a lot of
# classes, structs, unions or interfaces.
# The default value is: YES.

ALPHABETICAL_INDEX     = NO

# In case all classes in a project start with a common prefix, all classes will
# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
# can be used to specify a prefix (or a list of prefixes) that should be ignored
# while generating the index headers.
# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.

IGNORE_PREFIX          =

#---------------------------------------------------------------------------
# Configuration options related to the HTML output
#---------------------------------------------------------------------------

# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
# The default value is: YES.

GENERATE_HTML          = YES

# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
# it.
# The default directory is: html.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_OUTPUT            = @DOXYGEN_HTML_OUTPUT@

# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
# generated HTML page (for example: .htm, .php, .asp).
# The default value is: .html.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_FILE_EXTENSION    = .html

# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
# each generated HTML page. If the tag is left blank doxygen will generate a
# standard header.
#
# To get valid HTML the header file that includes any scripts and style sheets
# that doxygen needs, which is dependent on the configuration options used (e.g.
# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
# default header using
# doxygen -w html new_header.html new_footer.html new_stylesheet.css
# YourConfigFile
# and then modify the file new_header.html. See also section "Doxygen usage"
# for information on how to generate the default header that doxygen normally
# uses.
# Note: The header is subject to change so you typically have to regenerate the
# default header when upgrading to a newer version of doxygen. For a description
# of the possible markers and block names see the documentation.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_HEADER            =

# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
# generated HTML page. If the tag is left blank doxygen will generate a standard
# footer. See HTML_HEADER for more information on how to generate a default
# footer and what special commands can be used inside the footer. See also
# section "Doxygen usage" for information on how to generate the default footer
# that doxygen normally uses.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_FOOTER            =

# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
# sheet that is used by each HTML page. It can be used to fine-tune the look of
# the HTML output. If left blank doxygen will generate a default style sheet.
# See also section "Doxygen usage" for information on how to generate the style
# sheet that doxygen normally uses.
# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
# it is more robust and this tag (HTML_STYLESHEET) will in the future become
# obsolete.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_STYLESHEET        = @DOXYGEN_CSS@

# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
# cascading style sheets that are included after the standard style sheets
# created by doxygen. Using this option one can overrule certain style aspects.
# This is preferred over using HTML_STYLESHEET since it does not replace the
# standard style sheet and is therefore more robust against future updates.
# Doxygen will copy the style sheet files to the output directory.
# Note: The order of the extra style sheet files is of importance (e.g. the last
# style sheet in the list overrules the setting of the previous ones in the
# list). For an example see the documentation.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_EXTRA_STYLESHEET  =

# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
# other source files which should be copied to the HTML output directory. Note
# that these files will be copied to the base HTML output directory. Use the
# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
# files will be copied as-is; there are no commands or markers available.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_EXTRA_FILES       =

# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
# will adjust the colors in the style sheet and background images according to
# this color. Hue is specified as an angle on a color-wheel, see
# https://en.wikipedia.org/wiki/Hue for more information. For instance the value
# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
# purple, and 360 is red again.
# Minimum value: 0, maximum value: 359, default value: 220.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_COLORSTYLE_HUE    = 220

# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
# in the HTML output. For a value of 0 the output will use gray-scales only. A
# value of 255 will produce the most vivid colors.
# Minimum value: 0, maximum value: 255, default value: 100.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_COLORSTYLE_SAT    = 100

# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
# luminance component of the colors in the HTML output. Values below 100
# gradually make the output lighter, whereas values above 100 make the output
# darker. The value divided by 100 is the actual gamma applied, so 80 represents
# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
# change the gamma.
# Minimum value: 40, maximum value: 240, default value: 80.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_COLORSTYLE_GAMMA  = 80

# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
# page will contain the date and time when the page was generated. Setting this
# to YES can help to show when doxygen was last run and thus if the
# documentation is up to date.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_TIMESTAMP         = NO

# If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML
# documentation will contain a main index with vertical navigation menus that
# are dynamically created via JavaScript. If disabled, the navigation index will
# consists of multiple levels of tabs that are statically embedded in every HTML
# page. Disable this option to support browsers that do not have JavaScript,
# like the Qt help browser.
# The default value is: YES.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_DYNAMIC_MENUS     = YES

# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
# documentation will contain sections that can be hidden and shown after the
# page has loaded.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_DYNAMIC_SECTIONS  = NO

# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
# shown in the various tree structured indices initially; the user can expand
# and collapse entries dynamically later on. Doxygen will expand the tree to
# such a level that at most the specified number of entries are visible (unless
# a fully collapsed tree already exceeds this amount). So setting the number of
# entries 1 will produce a full collapsed tree by default. 0 is a special value
# representing an infinite number of entries and will result in a full expanded
# tree by default.
# Minimum value: 0, maximum value: 9999, default value: 100.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_INDEX_NUM_ENTRIES = 100

# If the GENERATE_DOCSET tag is set to YES, additional index files will be
# generated that can be used as input for Apple's Xcode 3 integrated development
# environment (see:
# https://developer.apple.com/xcode/), introduced with OSX 10.5 (Leopard). To
# create a documentation set, doxygen will generate a Makefile in the HTML
# output directory. Running make will produce the docset in that directory and
# running make install will install the docset in
# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
# startup. See https://developer.apple.com/library/archive/featuredarticles/Doxy
# genXcode/_index.html for more information.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.

GENERATE_DOCSET        = NO

# This tag determines the name of the docset feed. A documentation feed provides
# an umbrella under which multiple documentation sets from a single provider
# (such as a company or product suite) can be grouped.
# The default value is: Doxygen generated docs.
# This tag requires that the tag GENERATE_DOCSET is set to YES.

DOCSET_FEEDNAME        = "Doxygen generated docs"

# This tag specifies a string that should uniquely identify the documentation
# set bundle. This should be a reverse domain-name style string, e.g.
# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
# The default value is: org.doxygen.Project.
# This tag requires that the tag GENERATE_DOCSET is set to YES.

DOCSET_BUNDLE_ID       = @DOXYGEN_PROJECT_ID@

# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
# the documentation publisher. This should be a reverse domain-name style
# string, e.g. com.mycompany.MyDocSet.documentation.
# The default value is: org.doxygen.Publisher.
# This tag requires that the tag GENERATE_DOCSET is set to YES.

DOCSET_PUBLISHER_ID    = @DOXYGEN_PUBLISHER@

# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
# The default value is: Publisher.
# This tag requires that the tag GENERATE_DOCSET is set to YES.

DOCSET_PUBLISHER_NAME  = @DOXYGEN_PUBLISHER@

# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
# on Windows. In the beginning of 2021 Microsoft took the original page, with
# a.o. the download links, offline the HTML help workshop was already many years
# in maintenance mode). You can download the HTML help workshop from the web
# archives at Installation executable (see:
# http://web.archive.org/web/20160201063255/http://download.microsoft.com/downlo
# ad/0/A/9/0A939EF6-E31C-430F-A3DF-DFAE7960D564/htmlhelp.exe).
#
# The HTML Help Workshop contains a compiler that can convert all HTML output
# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
# files are now used as the Windows 98 help format, and will replace the old
# Windows help format (.hlp) on all Windows platforms in the future. Compressed
# HTML files also contain an index, a table of contents, and you can search for
# words in the documentation. The HTML workshop also contains a viewer for
# compressed HTML files.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.

GENERATE_HTMLHELP      = NO

# The CHM_FILE tag can be used to specify the file name of the resulting .chm
# file. You can add a path in front of the file if the result should not be
# written to the html output directory.
# This tag requires that the tag GENERATE_HTMLHELP is set to YES.

CHM_FILE               = YES

# The HHC_LOCATION tag can be used to specify the location (absolute path
# including file name) of the HTML help compiler (hhc.exe). If non-empty,
# doxygen will try to run the HTML help compiler on the generated index.hhp.
# The file has to be specified with full path.
# This tag requires that the tag GENERATE_HTMLHELP is set to YES.

HHC_LOCATION           =

# The GENERATE_CHI flag controls if a separate .chi index file is generated
# (YES) or that it should be included in the main .chm file (NO).
# The default value is: NO.
# This tag requires that the tag GENERATE_HTMLHELP is set to YES.

GENERATE_CHI           = NO

# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
# and project file content.
# This tag requires that the tag GENERATE_HTMLHELP is set to YES.

CHM_INDEX_ENCODING     =

# The BINARY_TOC flag controls whether a binary table of contents is generated
# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
# enables the Previous and Next buttons.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTMLHELP is set to YES.

BINARY_TOC             = YES

# The TOC_EXPAND flag can be set to YES to add extra items for group members to
# the table of contents of the HTML help documentation and to the tree view.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTMLHELP is set to YES.

TOC_EXPAND             = YES

# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
# (.qch) of the generated HTML documentation.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.

GENERATE_QHP           = NO

# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
# the file name of the resulting .qch file. The path specified is relative to
# the HTML output folder.
# This tag requires that the tag GENERATE_QHP is set to YES.

QCH_FILE               =

# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
# Project output. For more information please see Qt Help Project / Namespace
# (see:
# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace).
# The default value is: org.doxygen.Project.
# This tag requires that the tag GENERATE_QHP is set to YES.

QHP_NAMESPACE          = @DOXYGEN_PROJECT_ID@

# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
# Help Project output. For more information please see Qt Help Project / Virtual
# Folders (see:
# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual-folders).
# The default value is: doc.
# This tag requires that the tag GENERATE_QHP is set to YES.

QHP_VIRTUAL_FOLDER     = doc

# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
# filter to add. For more information please see Qt Help Project / Custom
# Filters (see:
# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters).
# This tag requires that the tag GENERATE_QHP is set to YES.

QHP_CUST_FILTER_NAME   =

# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
# custom filter to add. For more information please see Qt Help Project / Custom
# Filters (see:
# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters).
# This tag requires that the tag GENERATE_QHP is set to YES.

QHP_CUST_FILTER_ATTRS  =

# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
# project's filter section matches. Qt Help Project / Filter Attributes (see:
# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#filter-attributes).
# This tag requires that the tag GENERATE_QHP is set to YES.

QHP_SECT_FILTER_ATTRS  =

# The QHG_LOCATION tag can be used to specify the location (absolute path
# including file name) of Qt's qhelpgenerator. If non-empty doxygen will try to
# run qhelpgenerator on the generated .qhp file.
# This tag requires that the tag GENERATE_QHP is set to YES.

QHG_LOCATION           =

# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
# generated, together with the HTML files, they form an Eclipse help plugin. To
# install this plugin and make it available under the help contents menu in
# Eclipse, the contents of the directory containing the HTML and XML files needs
# to be copied into the plugins directory of eclipse. The name of the directory
# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
# After copying Eclipse needs to be restarted before the help appears.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.

GENERATE_ECLIPSEHELP   = NO

# A unique identifier for the Eclipse help plugin. When installing the plugin
# the directory name containing the HTML and XML files should also have this
# name. Each documentation set should have its own identifier.
# The default value is: org.doxygen.Project.
# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.

ECLIPSE_DOC_ID         = @DOXYGEN_PROJECT_ID@

# If you want full control over the layout of the generated HTML pages it might
# be necessary to disable the index and replace it with your own. The
# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
# of each HTML page. A value of NO enables the index and the value YES disables
# it. Since the tabs in the index contain the same information as the navigation
# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.

DISABLE_INDEX          = NO

# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
# structure should be generated to display hierarchical information. If the tag
# value is set to YES, a side panel will be generated containing a tree-like
# index structure (just like the one that is generated for HTML Help). For this
# to work a browser that supports JavaScript, DHTML, CSS and frames is required
# (i.e. any modern browser). Windows users are probably better off using the
# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
# further fine tune the look of the index (see "Fine-tuning the output"). As an
# example, the default style sheet generated by doxygen has an example that
# shows how to put an image at the root of the tree instead of the PROJECT_NAME.
# Since the tree basically has the same information as the tab index, you could
# consider setting DISABLE_INDEX to YES when enabling this option.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.

GENERATE_TREEVIEW      = YES

# When both GENERATE_TREEVIEW and DISABLE_INDEX are set to YES, then the
# FULL_SIDEBAR option determines if the side bar is limited to only the treeview
# area (value NO) or if it should extend to the full height of the window (value
# YES). Setting this to YES gives a layout similar to
# https://docs.readthedocs.io with more room for contents, but less room for the
# project logo, title, and description. If either GENERATOR_TREEVIEW or
# DISABLE_INDEX is set to NO, this option has no effect.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.

FULL_SIDEBAR           = NO

# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
# doxygen will group on one line in the generated HTML documentation.
#
# Note that a value of 0 will completely suppress the enum values from appearing
# in the overview section.
# Minimum value: 0, maximum value: 20, default value: 4.
# This tag requires that the tag GENERATE_HTML is set to YES.

ENUM_VALUES_PER_LINE   = 4

# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
# to set the initial width (in pixels) of the frame in which the tree is shown.
# Minimum value: 0, maximum value: 1500, default value: 250.
# This tag requires that the tag GENERATE_HTML is set to YES.

TREEVIEW_WIDTH         = 250

# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
# external symbols imported via tag files in a separate window.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.

EXT_LINKS_IN_WINDOW    = NO

# If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg
# tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see
# https://inkscape.org) to generate formulas as SVG images instead of PNGs for
# the HTML output. These images will generally look nicer at scaled resolutions.
# Possible values are: png (the default) and svg (looks nicer but requires the
# pdf2svg or inkscape tool).
# The default value is: png.
# This tag requires that the tag GENERATE_HTML is set to YES.

HTML_FORMULA_FORMAT    = png

# Use this tag to change the font size of LaTeX formulas included as images in
# the HTML documentation. When you change the font size after a successful
# doxygen run you need to manually remove any form_*.png images from the HTML
# output directory to force them to be regenerated.
# Minimum value: 8, maximum value: 50, default value: 10.
# This tag requires that the tag GENERATE_HTML is set to YES.

FORMULA_FONTSIZE       = 10

# Use the FORMULA_TRANSPARENT tag to determine whether or not the images
# generated for formulas are transparent PNGs. Transparent PNGs are not
# supported properly for IE 6.0, but are supported on all modern browsers.
#
# Note that when changing this option you need to delete any form_*.png files in
# the HTML output directory before the changes have effect.
# The default value is: YES.
# This tag requires that the tag GENERATE_HTML is set to YES.

FORMULA_TRANSPARENT    = YES

# The FORMULA_MACROFILE can contain LaTeX \newcommand and \renewcommand commands
# to create new LaTeX commands to be used in formulas as building blocks. See
# the section "Including formulas" for details.

FORMULA_MACROFILE      =

# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
# https://www.mathjax.org) which uses client side JavaScript for the rendering
# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
# installed or if you want to formulas look prettier in the HTML output. When
# enabled you may also need to install MathJax separately and configure the path
# to it using the MATHJAX_RELPATH option.
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.

USE_MATHJAX            = NO

# With MATHJAX_VERSION it is possible to specify the MathJax version to be used.
# Note that the different versions of MathJax have different requirements with
# regards to the different settings, so it is possible that also other MathJax
# settings have to be changed when switching between the different MathJax
# versions.
# Possible values are: MathJax_2 and MathJax_3.
# The default value is: MathJax_2.
# This tag requires that the tag USE_MATHJAX is set to YES.

MATHJAX_VERSION        = MathJax_2

# When MathJax is enabled you can set the default output format to be used for
# the MathJax output. For more details about the output format see MathJax
# version 2 (see:
# http://docs.mathjax.org/en/v2.7-latest/output.html) and MathJax version 3
# (see:
# http://docs.mathjax.org/en/latest/web/components/output.html).
# Possible values are: HTML-CSS (which is slower, but has the best
# compatibility. This is the name for Mathjax version 2, for MathJax version 3
# this will be translated into chtml), NativeMML (i.e. MathML. Only supported
# for NathJax 2. For MathJax version 3 chtml will be used instead.), chtml (This
# is the name for Mathjax version 3, for MathJax version 2 this will be
# translated into HTML-CSS) and SVG.
# The default value is: HTML-CSS.
# This tag requires that the tag USE_MATHJAX is set to YES.

MATHJAX_FORMAT         = HTML-CSS

# When MathJax is enabled you need to specify the location relative to the HTML
# output directory using the MATHJAX_RELPATH option. The destination directory
# should contain the MathJax.js script. For instance, if the mathjax directory
# is located at the same level as the HTML output directory, then
# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
# Content Delivery Network so you can quickly see the result without installing
# MathJax. However, it is strongly recommended to install a local copy of
# MathJax from https://www.mathjax.org before deployment. The default value is:
# - in case of MathJax version 2: https://cdn.jsdelivr.net/npm/mathjax@2
# - in case of MathJax version 3: https://cdn.jsdelivr.net/npm/mathjax@3
# This tag requires that the tag USE_MATHJAX is set to YES.

MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest

# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
# extension names that should be enabled during MathJax rendering. For example
# for MathJax version 2 (see
# https://docs.mathjax.org/en/v2.7-latest/tex.html#tex-and-latex-extensions):
# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
# For example for MathJax version 3 (see
# http://docs.mathjax.org/en/latest/input/tex/extensions/index.html):
# MATHJAX_EXTENSIONS = ams
# This tag requires that the tag USE_MATHJAX is set to YES.

MATHJAX_EXTENSIONS     =

# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
# of code that will be used on startup of the MathJax code. See the MathJax site
# (see:
# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. For an
# example see the documentation.
# This tag requires that the tag USE_MATHJAX is set to YES.

MATHJAX_CODEFILE       =

# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
# the HTML output. The underlying search engine uses javascript and DHTML and
# should work on any modern browser. Note that when using HTML help
# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
# there is already a search function so this one should typically be disabled.
# For large projects the javascript based search engine can be slow, then
# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
# search using the keyboard; to jump to the search box use <access key> + S
# (what the <access key> is depends on the OS and browser, but it is typically
# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
# key> to jump into the search results window, the results can be navigated
# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
# the search. The filter options can be selected when the cursor is inside the
# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
# to select a filter and <Enter> or <escape> to activate or cancel the filter
# option.
# The default value is: YES.
# This tag requires that the tag GENERATE_HTML is set to YES.

SEARCHENGINE           = NO

# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
# implemented using a web server instead of a web client using JavaScript. There
# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
# setting. When disabled, doxygen will generate a PHP script for searching and
# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
# and searching needs to be provided by external tools. See the section
# "External Indexing and Searching" for details.
# The default value is: NO.
# This tag requires that the tag SEARCHENGINE is set to YES.

SERVER_BASED_SEARCH    = NO

# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
# script for searching. Instead the search results are written to an XML file
# which needs to be processed by an external indexer. Doxygen will invoke an
# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
# search results.
#
# Doxygen ships with an example indexer (doxyindexer) and search engine
# (doxysearch.cgi) which are based on the open source search engine library
# Xapian (see:
# https://xapian.org/).
#
# See the section "External Indexing and Searching" for details.
# The default value is: NO.
# This tag requires that the tag SEARCHENGINE is set to YES.

EXTERNAL_SEARCH        = NO

# The SEARCHENGINE_URL should point to a search engine hosted by a web server
# which will return the search results when EXTERNAL_SEARCH is enabled.
#
# Doxygen ships with an example indexer (doxyindexer) and search engine
# (doxysearch.cgi) which are based on the open source search engine library
# Xapian (see:
# https://xapian.org/). See the section "External Indexing and Searching" for
# details.
# This tag requires that the tag SEARCHENGINE is set to YES.

SEARCHENGINE_URL       =

# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
# search data is written to a file for indexing by an external tool. With the
# SEARCHDATA_FILE tag the name of this file can be specified.
# The default file is: searchdata.xml.
# This tag requires that the tag SEARCHENGINE is set to YES.

SEARCHDATA_FILE        = searchdata.xml

# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
# projects and redirect the results back to the right project.
# This tag requires that the tag SEARCHENGINE is set to YES.

EXTERNAL_SEARCH_ID     =

# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
# projects other than the one defined by this configuration file, but that are
# all added to the same external search index. Each project needs to have a
# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
# to a relative location where the documentation can be found. The format is:
# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
# This tag requires that the tag SEARCHENGINE is set to YES.

EXTRA_SEARCH_MAPPINGS  =

#---------------------------------------------------------------------------
# Configuration options related to the LaTeX output
#---------------------------------------------------------------------------

# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
# The default value is: YES.

GENERATE_LATEX         = NO

# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
# it.
# The default directory is: latex.
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_OUTPUT           = latex

# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
# invoked.
#
# Note that when not enabling USE_PDFLATEX the default is latex when enabling
# USE_PDFLATEX the default is pdflatex and when in the later case latex is
# chosen this is overwritten by pdflatex. For specific output languages the
# default can have been set differently, this depends on the implementation of
# the output language.
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_CMD_NAME         = latex

# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
# index for LaTeX.
# Note: This tag is used in the Makefile / make.bat.
# See also: LATEX_MAKEINDEX_CMD for the part in the generated output file
# (.tex).
# The default file is: makeindex.
# This tag requires that the tag GENERATE_LATEX is set to YES.

MAKEINDEX_CMD_NAME     = makeindex

# The LATEX_MAKEINDEX_CMD tag can be used to specify the command name to
# generate index for LaTeX. In case there is no backslash (\) as first character
# it will be automatically added in the LaTeX code.
# Note: This tag is used in the generated output file (.tex).
# See also: MAKEINDEX_CMD_NAME for the part in the Makefile / make.bat.
# The default value is: makeindex.
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_MAKEINDEX_CMD    = makeindex

# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
# documents. This may be useful for small projects and may help to save some
# trees in general.
# The default value is: NO.
# This tag requires that the tag GENERATE_LATEX is set to YES.

COMPACT_LATEX          = NO

# The PAPER_TYPE tag can be used to set the paper type that is used by the
# printer.
# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
# 14 inches) and executive (7.25 x 10.5 inches).
# The default value is: a4.
# This tag requires that the tag GENERATE_LATEX is set to YES.

PAPER_TYPE             = a4

# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
# that should be included in the LaTeX output. The package can be specified just
# by its name or with the correct syntax as to be used with the LaTeX
# \usepackage command. To get the times font for instance you can specify :
# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
# To use the option intlimits with the amsmath package you can specify:
# EXTRA_PACKAGES=[intlimits]{amsmath}
# If left blank no extra packages will be included.
# This tag requires that the tag GENERATE_LATEX is set to YES.

EXTRA_PACKAGES         =

# The LATEX_HEADER tag can be used to specify a user-defined LaTeX header for
# the generated LaTeX document. The header should contain everything until the
# first chapter. If it is left blank doxygen will generate a standard header. It
# is highly recommended to start with a default header using
# doxygen -w latex new_header.tex new_footer.tex new_stylesheet.sty
# and then modify the file new_header.tex. See also section "Doxygen usage" for
# information on how to generate the default header that doxygen normally uses.
#
# Note: Only use a user-defined header if you know what you are doing!
# Note: The header is subject to change so you typically have to regenerate the
# default header when upgrading to a newer version of doxygen. The following
# commands have a special meaning inside the header (and footer): For a
# description of the possible markers and block names see the documentation.
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_HEADER           =

# The LATEX_FOOTER tag can be used to specify a user-defined LaTeX footer for
# the generated LaTeX document. The footer should contain everything after the
# last chapter. If it is left blank doxygen will generate a standard footer. See
# LATEX_HEADER for more information on how to generate a default footer and what
# special commands can be used inside the footer. See also section "Doxygen
# usage" for information on how to generate the default footer that doxygen
# normally uses. Note: Only use a user-defined footer if you know what you are
# doing!
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_FOOTER           =

# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
# LaTeX style sheets that are included after the standard style sheets created
# by doxygen. Using this option one can overrule certain style aspects. Doxygen
# will copy the style sheet files to the output directory.
# Note: The order of the extra style sheet files is of importance (e.g. the last
# style sheet in the list overrules the setting of the previous ones in the
# list).
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_EXTRA_STYLESHEET =

# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
# other source files which should be copied to the LATEX_OUTPUT output
# directory. Note that the files will be copied as-is; there are no commands or
# markers available.
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_EXTRA_FILES      =

# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
# contain links (just like the HTML output) instead of page references. This
# makes the output suitable for online browsing using a PDF viewer.
# The default value is: YES.
# This tag requires that the tag GENERATE_LATEX is set to YES.

PDF_HYPERLINKS         = NO

# If the USE_PDFLATEX tag is set to YES, doxygen will use the engine as
# specified with LATEX_CMD_NAME to generate the PDF file directly from the LaTeX
# files. Set this option to YES, to get a higher quality PDF documentation.
#
# See also section LATEX_CMD_NAME for selecting the engine.
# The default value is: YES.
# This tag requires that the tag GENERATE_LATEX is set to YES.

USE_PDFLATEX           = NO

# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
# command to the generated LaTeX files. This will instruct LaTeX to keep running
# if errors occur, instead of asking the user for help.
# The default value is: NO.
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_BATCHMODE        = NO

# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
# index chapters (such as File Index, Compound Index, etc.) in the output.
# The default value is: NO.
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_HIDE_INDICES     = NO

# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
# bibliography, e.g. plainnat, or ieeetr. See
# https://en.wikipedia.org/wiki/BibTeX and \cite for more info.
# The default value is: plain.
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_BIB_STYLE        = plain

# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
# page will contain the date and time when the page was generated. Setting this
# to NO can help when comparing the output of multiple runs.
# The default value is: NO.
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_TIMESTAMP        = NO

# The LATEX_EMOJI_DIRECTORY tag is used to specify the (relative or absolute)
# path from which the emoji images will be read. If a relative path is entered,
# it will be relative to the LATEX_OUTPUT directory. If left blank the
# LATEX_OUTPUT directory will be used.
# This tag requires that the tag GENERATE_LATEX is set to YES.

LATEX_EMOJI_DIRECTORY  =

#---------------------------------------------------------------------------
# Configuration options related to the RTF output
#---------------------------------------------------------------------------

# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
# RTF output is optimized for Word 97 and may not look too pretty with other RTF
# readers/editors.
# The default value is: NO.

GENERATE_RTF           = NO

# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
# it.
# The default directory is: rtf.
# This tag requires that the tag GENERATE_RTF is set to YES.

RTF_OUTPUT             = rtf

# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
# documents. This may be useful for small projects and may help to save some
# trees in general.
# The default value is: NO.
# This tag requires that the tag GENERATE_RTF is set to YES.

COMPACT_RTF            = NO

# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
# contain hyperlink fields. The RTF file will contain links (just like the HTML
# output) instead of page references. This makes the output suitable for online
# browsing using Word or some other Word compatible readers that support those
# fields.
#
# Note: WordPad (write) and others do not support links.
# The default value is: NO.
# This tag requires that the tag GENERATE_RTF is set to YES.

RTF_HYPERLINKS         = NO

# Load stylesheet definitions from file. Syntax is similar to doxygen's
# configuration file, i.e. a series of assignments. You only have to provide
# replacements, missing definitions are set to their default value.
#
# See also section "Doxygen usage" for information on how to generate the
# default style sheet that doxygen normally uses.
# This tag requires that the tag GENERATE_RTF is set to YES.

RTF_STYLESHEET_FILE    =

# Set optional variables used in the generation of an RTF document. Syntax is
# similar to doxygen's configuration file. A template extensions file can be
# generated using doxygen -e rtf extensionFile.
# This tag requires that the tag GENERATE_RTF is set to YES.

RTF_EXTENSIONS_FILE    =

#---------------------------------------------------------------------------
# Configuration options related to the man page output
#---------------------------------------------------------------------------

# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
# classes and files.
# The default value is: NO.

GENERATE_MAN           = NO

# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
# it. A directory man3 will be created inside the directory specified by
# MAN_OUTPUT.
# The default directory is: man.
# This tag requires that the tag GENERATE_MAN is set to YES.

MAN_OUTPUT             = man

# The MAN_EXTENSION tag determines the extension that is added to the generated
# man pages. In case the manual section does not start with a number, the number
# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
# optional.
# The default value is: .3.
# This tag requires that the tag GENERATE_MAN is set to YES.

MAN_EXTENSION          = .3

# The MAN_SUBDIR tag determines the name of the directory created within
# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
# MAN_EXTENSION with the initial . removed.
# This tag requires that the tag GENERATE_MAN is set to YES.

MAN_SUBDIR             =

# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
# will generate one additional man file for each entity documented in the real
# man page(s). These additional files only source the real man page, but without
# them the man command would be unable to find the correct page.
# The default value is: NO.
# This tag requires that the tag GENERATE_MAN is set to YES.

MAN_LINKS              = NO

#---------------------------------------------------------------------------
# Configuration options related to the XML output
#---------------------------------------------------------------------------

# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
# captures the structure of the code including all documentation.
# The default value is: NO.

GENERATE_XML           = NO

# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
# it.
# The default directory is: xml.
# This tag requires that the tag GENERATE_XML is set to YES.

XML_OUTPUT             = xml

# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
# listings (including syntax highlighting and cross-referencing information) to
# the XML output. Note that enabling this will significantly increase the size
# of the XML output.
# The default value is: YES.
# This tag requires that the tag GENERATE_XML is set to YES.

XML_PROGRAMLISTING     = YES

# If the XML_NS_MEMB_FILE_SCOPE tag is set to YES, doxygen will include
# namespace members in file scope as well, matching the HTML output.
# The default value is: NO.
# This tag requires that the tag GENERATE_XML is set to YES.

XML_NS_MEMB_FILE_SCOPE = NO

#---------------------------------------------------------------------------
# Configuration options related to the DOCBOOK output
#---------------------------------------------------------------------------

# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
# that can be used to generate PDF.
# The default value is: NO.

GENERATE_DOCBOOK       = NO

# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
# front of it.
# The default directory is: docbook.
# This tag requires that the tag GENERATE_DOCBOOK is set to YES.

DOCBOOK_OUTPUT         = docbook

#---------------------------------------------------------------------------
# Configuration options for the AutoGen Definitions output
#---------------------------------------------------------------------------

# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
# AutoGen Definitions (see http://autogen.sourceforge.net/) file that captures
# the structure of the code including all documentation. Note that this feature
# is still experimental and incomplete at the moment.
# The default value is: NO.

GENERATE_AUTOGEN_DEF   = NO

#---------------------------------------------------------------------------
# Configuration options related to Sqlite3 output
#---------------------------------------------------------------------------

#---------------------------------------------------------------------------
# Configuration options related to the Perl module output
#---------------------------------------------------------------------------

# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
# file that captures the structure of the code including all documentation.
#
# Note that this feature is still experimental and incomplete at the moment.
# The default value is: NO.

GENERATE_PERLMOD       = NO

# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
# output from the Perl module output.
# The default value is: NO.
# This tag requires that the tag GENERATE_PERLMOD is set to YES.

PERLMOD_LATEX          = NO

# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
# formatted so it can be parsed by a human reader. This is useful if you want to
# understand what is going on. On the other hand, if this tag is set to NO, the
# size of the Perl module output will be much smaller and Perl will parse it
# just the same.
# The default value is: YES.
# This tag requires that the tag GENERATE_PERLMOD is set to YES.

PERLMOD_PRETTY         = YES

# The names of the make variables in the generated doxyrules.make file are
# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
# so different doxyrules.make files included by the same Makefile don't
# overwrite each other's variables.
# This tag requires that the tag GENERATE_PERLMOD is set to YES.

PERLMOD_MAKEVAR_PREFIX =

#---------------------------------------------------------------------------
# Configuration options related to the preprocessor
#---------------------------------------------------------------------------

# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
# C-preprocessor directives found in the sources and include files.
# The default value is: YES.

ENABLE_PREPROCESSING   = YES

# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
# in the source code. If set to NO, only conditional compilation will be
# performed. Macro expansion can be done in a controlled way by setting
# EXPAND_ONLY_PREDEF to YES.
# The default value is: NO.
# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.

MACRO_EXPANSION        = NO

# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
# the macro expansion is limited to the macros specified with the PREDEFINED and
# EXPAND_AS_DEFINED tags.
# The default value is: NO.
# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.

EXPAND_ONLY_PREDEF     = NO

# If the SEARCH_INCLUDES tag is set to YES, the include files in the
# INCLUDE_PATH will be searched if a #include is found.
# The default value is: YES.
# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.

SEARCH_INCLUDES        = NO

# The INCLUDE_PATH tag can be used to specify one or more directories that
# contain include files that are not input files but should be processed by the
# preprocessor.
# This tag requires that the tag SEARCH_INCLUDES is set to YES.

INCLUDE_PATH           =

# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
# patterns (like *.h and *.hpp) to filter out the header-files in the
# directories. If left blank, the patterns specified with FILE_PATTERNS will be
# used.
# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.

INCLUDE_FILE_PATTERNS  =

# The PREDEFINED tag can be used to specify one or more macro names that are
# defined before the preprocessor is started (similar to the -D option of e.g.
# gcc). The argument of the tag is a list of macros of the form: name or
# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
# is assumed. To prevent a macro definition from being undefined via #undef or
# recursively expanded use the := operator instead of the = operator.
# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.

PREDEFINED             = DOXYGEN_SHOULD_SKIP_THIS

# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
# tag can be used to specify a list of macro names that should be expanded. The
# macro definition that is found in the sources will be used. Use the PREDEFINED
# tag if you want to use a different macro definition that overrules the
# definition found in the source code.
# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.

EXPAND_AS_DEFINED      =

# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
# remove all references to function-like macros that are alone on a line, have
# an all uppercase name, and do not end with a semicolon. Such function macros
# are typically used for boiler-plate code, and will confuse the parser if not
# removed.
# The default value is: YES.
# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.

SKIP_FUNCTION_MACROS   = YES

#---------------------------------------------------------------------------
# Configuration options related to external references
#---------------------------------------------------------------------------

# The TAGFILES tag can be used to specify one or more tag files. For each tag
# file the location of the external documentation should be added. The format of
# a tag file without this location is as follows:
# TAGFILES = file1 file2 ...
# Adding location for the tag files is done as follows:
# TAGFILES = file1=loc1 "file2 = loc2" ...
# where loc1 and loc2 can be relative or absolute paths or URLs. See the
# section "Linking to external documentation" for more information about the use
# of tag files.
# Note: Each tag file must have a unique name (where the name does NOT include
# the path). If a tag file is not located in the directory in which doxygen is
# run, you must also specify the path to the tagfile here.

TAGFILES               =

# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
# tag file that is based on the input files it reads. See section "Linking to
# external documentation" for more information about the usage of tag files.

GENERATE_TAGFILE       =

# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
# the class index. If set to NO, only the inherited external classes will be
# listed.
# The default value is: NO.

ALLEXTERNALS           = NO

# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
# in the modules index. If set to NO, only the current project's groups will be
# listed.
# The default value is: YES.

EXTERNAL_GROUPS        = YES

# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
# the related pages index. If set to NO, only the current project's pages will
# be listed.
# The default value is: YES.

EXTERNAL_PAGES         = YES

#---------------------------------------------------------------------------
# Configuration options related to the dot tool
#---------------------------------------------------------------------------

# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
# NO turns the diagrams off. Note that this option also works with HAVE_DOT
# disabled, but it is recommended to install and use dot, since it yields more
# powerful graphs.
# The default value is: YES.

CLASS_DIAGRAMS         = YES

# You can include diagrams made with dia in doxygen documentation. Doxygen will
# then run dia to produce the diagram and insert it in the documentation. The
# DIA_PATH tag allows you to specify the directory where the dia binary resides.
# If left empty dia is assumed to be found in the default search path.

DIA_PATH               =

# If set to YES the inheritance and collaboration graphs will hide inheritance
# and usage relations if the target is undocumented or is not a class.
# The default value is: YES.

HIDE_UNDOC_RELATIONS   = YES

# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
# available from the path. This tool is part of Graphviz (see:
# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
# Bell Labs. The other options in this section have no effect if this option is
# set to NO
# The default value is: NO.

HAVE_DOT               = NO

# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
# to run in parallel. When set to 0 doxygen will base this on the number of
# processors available in the system. You can set it explicitly to a value
# larger than 0 to get control over the balance between CPU load and processing
# speed.
# Minimum value: 0, maximum value: 32, default value: 0.
# This tag requires that the tag HAVE_DOT is set to YES.

DOT_NUM_THREADS        = 0

# When you want a differently looking font in the dot files that doxygen
# generates you can specify the font name using DOT_FONTNAME. You need to make
# sure dot is able to find the font, which can be done by putting it in a
# standard location or by setting the DOTFONTPATH environment variable or by
# setting DOT_FONTPATH to the directory containing the font.
# The default value is: Helvetica.
# This tag requires that the tag HAVE_DOT is set to YES.

DOT_FONTNAME           = Helvetica

# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
# dot graphs.
# Minimum value: 4, maximum value: 24, default value: 10.
# This tag requires that the tag HAVE_DOT is set to YES.

DOT_FONTSIZE           = 10

# By default doxygen will tell dot to use the default font as specified with
# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
# the path where dot can find it using this tag.
# This tag requires that the tag HAVE_DOT is set to YES.

DOT_FONTPATH           =

# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
# each documented class showing the direct and indirect inheritance relations.
# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
# The default value is: YES.
# This tag requires that the tag HAVE_DOT is set to YES.

CLASS_GRAPH            = YES

# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
# graph for each documented class showing the direct and indirect implementation
# dependencies (inheritance, containment, and class references variables) of the
# class with other documented classes.
# The default value is: YES.
# This tag requires that the tag HAVE_DOT is set to YES.

COLLABORATION_GRAPH    = YES

# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
# groups, showing the direct groups dependencies.
# The default value is: YES.
# This tag requires that the tag HAVE_DOT is set to YES.

GROUP_GRAPHS           = YES

# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
# collaboration diagrams in a style similar to the OMG's Unified Modeling
# Language.
# The default value is: NO.
# This tag requires that the tag HAVE_DOT is set to YES.

UML_LOOK               = NO

# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
# class node. If there are many fields or methods and many nodes the graph may
# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
# number of items for each type to make the size more manageable. Set this to 0
# for no limit. Note that the threshold may be exceeded by 50% before the limit
# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
# but if the number exceeds 15, the total amount of fields shown is limited to
# 10.
# Minimum value: 0, maximum value: 100, default value: 10.
# This tag requires that the tag UML_LOOK is set to YES.

UML_LIMIT_NUM_FIELDS   = 10

# If the DOT_UML_DETAILS tag is set to NO, doxygen will show attributes and
# methods without types and arguments in the UML graphs. If the DOT_UML_DETAILS
# tag is set to YES, doxygen will add type and arguments for attributes and
# methods in the UML graphs. If the DOT_UML_DETAILS tag is set to NONE, doxygen
# will not generate fields with class member information in the UML graphs. The
# class diagrams will look similar to the default class diagrams but using UML
# notation for the relationships.
# Possible values are: NO, YES and NONE.
# The default value is: NO.
# This tag requires that the tag UML_LOOK is set to YES.

DOT_UML_DETAILS        = NO

# The DOT_WRAP_THRESHOLD tag can be used to set the maximum number of characters
# to display on a single line. If the actual line length exceeds this threshold
# significantly it will wrapped across multiple lines. Some heuristics are apply
# to avoid ugly line breaks.
# Minimum value: 0, maximum value: 1000, default value: 17.
# This tag requires that the tag HAVE_DOT is set to YES.

DOT_WRAP_THRESHOLD     = 17

# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
# collaboration graphs will show the relations between templates and their
# instances.
# The default value is: NO.
# This tag requires that the tag HAVE_DOT is set to YES.

TEMPLATE_RELATIONS     = YES

# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
# YES then doxygen will generate a graph for each documented file showing the
# direct and indirect include dependencies of the file with other documented
# files.
# The default value is: YES.
# This tag requires that the tag HAVE_DOT is set to YES.

INCLUDE_GRAPH          = YES

# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
# set to YES then doxygen will generate a graph for each documented file showing
# the direct and indirect include dependencies of the file with other documented
# files.
# The default value is: YES.
# This tag requires that the tag HAVE_DOT is set to YES.

INCLUDED_BY_GRAPH      = YES

# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
# dependency graph for every global function or class method.
#
# Note that enabling this option will significantly increase the time of a run.
# So in most cases it will be better to enable call graphs for selected
# functions only using the \callgraph command. Disabling a call graph can be
# accomplished by means of the command \hidecallgraph.
# The default value is: NO.
# This tag requires that the tag HAVE_DOT is set to YES.

CALL_GRAPH             = NO

# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
# dependency graph for every global function or class method.
#
# Note that enabling this option will significantly increase the time of a run.
# So in most cases it will be better to enable caller graphs for selected
# functions only using the \callergraph command. Disabling a caller graph can be
# accomplished by means of the command \hidecallergraph.
# The default value is: NO.
# This tag requires that the tag HAVE_DOT is set to YES.

CALLER_GRAPH           = NO

# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
# hierarchy of all classes instead of a textual one.
# The default value is: YES.
# This tag requires that the tag HAVE_DOT is set to YES.

GRAPHICAL_HIERARCHY    = YES

# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
# dependencies a directory has on other directories in a graphical way. The
# dependency relations are determined by the #include relations between the
# files in the directories.
# The default value is: YES.
# This tag requires that the tag HAVE_DOT is set to YES.

DIRECTORY_GRAPH        = YES

# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
# generated by dot. For an explanation of the image formats see the section
# output formats in the documentation of the dot tool (Graphviz (see:
# http://www.graphviz.org/)).
# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
# to make the SVG files visible in IE 9+ (other browsers do not have this
# requirement).
# Possible values are: png, jpg, gif, svg, png:gd, png:gd:gd, png:cairo,
# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
# png:gdiplus:gdiplus.
# The default value is: png.
# This tag requires that the tag HAVE_DOT is set to YES.

DOT_IMAGE_FORMAT       = png

# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
# enable generation of interactive SVG images that allow zooming and panning.
#
# Note that this requires a modern browser other than Internet Explorer. Tested
# and working are Firefox, Chrome, Safari, and Opera.
# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
# the SVG files visible. Older versions of IE do not have SVG support.
# The default value is: NO.
# This tag requires that the tag HAVE_DOT is set to YES.

INTERACTIVE_SVG        = NO

# The DOT_PATH tag can be used to specify the path where the dot tool can be
# found. If left blank, it is assumed the dot tool can be found in the path.
# This tag requires that the tag HAVE_DOT is set to YES.

DOT_PATH               =

# The DOTFILE_DIRS tag can be used to specify one or more directories that
# contain dot files that are included in the documentation (see the \dotfile
# command).
# This tag requires that the tag HAVE_DOT is set to YES.

DOTFILE_DIRS           =

# The MSCFILE_DIRS tag can be used to specify one or more directories that
# contain msc files that are included in the documentation (see the \mscfile
# command).

MSCFILE_DIRS           =

# The DIAFILE_DIRS tag can be used to specify one or more directories that
# contain dia files that are included in the documentation (see the \diafile
# command).

DIAFILE_DIRS           =

# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
# path where java can find the plantuml.jar file. If left blank, it is assumed
# PlantUML is not used or called during a preprocessing step. Doxygen will
# generate a warning when it encounters a \startuml command in this case and
# will not generate output for the diagram.

PLANTUML_JAR_PATH      =

# When using plantuml, the PLANTUML_CFG_FILE tag can be used to specify a
# configuration file for plantuml.

PLANTUML_CFG_FILE      =

# When using plantuml, the specified paths are searched for files specified by
# the !include statement in a plantuml block.

PLANTUML_INCLUDE_PATH  =

# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
# that will be shown in the graph. If the number of nodes in a graph becomes
# larger than this value, doxygen will truncate the graph, which is visualized
# by representing a node as a red box. Note that doxygen if the number of direct
# children of the root node in a graph is already larger than
# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
# Minimum value: 0, maximum value: 10000, default value: 50.
# This tag requires that the tag HAVE_DOT is set to YES.

DOT_GRAPH_MAX_NODES    = 50

# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
# generated by dot. A depth value of 3 means that only nodes reachable from the
# root by following a path via at most 3 edges will be shown. Nodes that lay
# further from the root node will be omitted. Note that setting this option to 1
# or 2 may greatly reduce the computation time needed for large code bases. Also
# note that the size of a graph can be further restricted by
# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
# Minimum value: 0, maximum value: 1000, default value: 0.
# This tag requires that the tag HAVE_DOT is set to YES.

MAX_DOT_GRAPH_DEPTH    = 0

# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
# background. This is disabled by default, because dot on Windows does not seem
# to support this out of the box.
#
# Warning: Depending on the platform used, enabling this option may lead to
# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
# read).
# The default value is: NO.
# This tag requires that the tag HAVE_DOT is set to YES.

DOT_TRANSPARENT        = NO

# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
# files in one run (i.e. multiple -o and -T options on the command line). This
# makes dot run faster, but since only newer versions of dot (>1.8.10) support
# this, this feature is disabled by default.
# The default value is: NO.
# This tag requires that the tag HAVE_DOT is set to YES.

DOT_MULTI_TARGETS      = NO

# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
# explaining the meaning of the various boxes and arrows in the dot generated
# graphs.
# The default value is: YES.
# This tag requires that the tag HAVE_DOT is set to YES.

GENERATE_LEGEND        = YES

# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate
# files that are used to generate the various graphs.
#
# Note: This setting is not only used for dot files but also for msc temporary
# files.
# The default value is: YES.

DOT_CLEANUP            = YES


================================================
FILE: cmake/doxygen.cmake
================================================
# Copyright (c) 2017
# Commonwealth Scientific and Industrial Research Organisation (CSIRO)
# ABN 41 687 119 230
#
# Author: Kazys Stepanas

include(CMakeParseArguments)

find_package(Doxygen)

# doxygen_create(
#   [DOXYFILE doxyfile.in]
#   [PROJECT name]
#   [VERSION version]
#   [OUTPUT_DIR dir]
#   [CSS style.css]
#   [PUBLISHER name]
#   [PUBLISHER_ID id.publisher]
#   [PROJECT_ID id.publisher.project]
#   [EXAMPLE_PATHS path1 path2 ...]
#   [IMAGE_PATHS path1 path2 ...]
#   [EXCLUDE_PATHS path1 path2 ...]
#   PATHS path1 path2 ...
# )
#
# Create a custom target which builds the doxygen documentation.
#
# The function configures the Doxyfile, optionally specified by DOXYFILE, by converting various arguments to this
# function into the equivalent Doxyfile variables. By default the DOXYFILE is cmake/doxyfile.in.
# Note a full description of the Doxyfile variables and their function is beyond the scope of this documentation. See
# doxyfile documentation or comments in a Doxyfile generated by Doxywizard for more information.
#
# The function arguments are mapped as follows:
#
# Argument      | Doxygen Variable      | Description
# ------------- | --------------------- | ---------------------------------------------------------------------------------
# DOXYFILE      | N/A                   | The input doxyfile to configure using configure_file.
# PROJECT       | PROJECT_NAME          | Name of the project.
# VERSION       | PROJECT_NUMBER        | Project version number, in the form <major>.<minor>[.<patch>]
# OUTPUT_DIR    | HTML_OUPTUT           | HTML output directory under the build tree. Default is 'html'.
# CSS           | HTML_STYLESHEET       | Style sheet to style the documentation pages with.
# PUBLISHER     | DOCSET_PUBLISHER_NAME | Publisher name.
# PUBLISHER_ID  | DOCSET_PUBLISHER_ID   | Publisher ID in the form org.publisher
# PROJECT_ID    | DOCSET_BUNDLE_ID      | Project ID in the form org.publisher.project. Also maps to QHP_NAMESPACE, ECLIPSE_DOC_ID
# EXAMPLE_PATHS | EXAMPLE_PATH          | Where to find source files for @example tags.
# IMAGE_PATHS   | IMAGE_PATH            | Where to file images for @image tags.
# EXCLUDE_PATHS | EXCLUDE               | Exclude these directories from Doxygen parsing.
# PATHS         | INPUT                 | Directories to parse source for Doxygen comments. RECURSIVE is assumed on.
function(doxygen_create)
  cmake_parse_arguments(DGEN
    ""
    "PROJECT;VERSION;OUTPUT_DIR;CSS;PUBLISHER;PUBLISHER_ID;PROJECT_ID;DOXYFILE"
    "EXAMPLE_PATHS;IMAGE_PATHS;EXCLUDE_PATHS;PATHS"
    ${ARGN})

  # Doxygen configuration.
  # Short project name.
  if(DGEN_PROJECT)
    set(DOXYGEN_PROJECT_NAME "${DGEN_PROJECT}")
  else(DGEN_PROJECT)
    set(DOXYGEN_PROJECT_NAME "${CMAKE_PROJECT_NAME}")
  endif(DGEN_PROJECT)
  # Project version
  set(DOXYGEN_PROJECT_VERSION ${DGEN_VERSION})
  # Paths to example sources used with the @example tag.
  if(DGEN_EXAMPLE_PATHS)
    set(DOXYGEN_EXAMPLE_PATH "${DGEN_EXAMPLE_PATHS}")
  endif(DGEN_EXAMPLE_PATHS)
  # Paths to images included with the @image tag.
  if(DGEN_IMAGE_PATHS)
    set(DOXYGEN_IMAGE_PATH "${DGEN_IMAGE_PATHS}")
  endif(DGEN_IMAGE_PATHS)
  # Output subdirectory for generated documentation.
  if(DGEN_OUTPUT_DIR)
    set(DOXYGEN_HTML_OUTPUT ${DGEN_OUTPUT_DIR})
  else(DGEN_OUTPUT_DIR)
    set(DOXYGEN_HTML_OUTPUT html)
  endif(DGEN_OUTPUT_DIR)
  # CSS file used to style the documentation pages. Setting nothing here used the default Doxygen style.
  if(DGEN_CSS)
    set(DOXYGEN_CSS ${DGEN_CSS})
  endif(DGEN_CSS)
  # Publisher and project details.
  if(DGEN_PUBLISHER_ID)
    set(DOXYGEN_PUBLISHER_ID ${DOXYGEN_PUBLISHER_ID})
  endif(DGEN_PUBLISHER_ID)
  if(DGEN_PUBLISHER)
    set(DOXYGEN_PUBLISHER "${DOXYGEN_PUBLISHER}")
  endif(DGEN_PUBLISHER)

  if(DGEN_PROJECT_ID)
    set(DOXYGEN_PROJECT_ID ${DGEN_PROJECT_ID})
  endif(DGEN_PROJECT_ID)

  # Input directories:
  set(DOXYGEN_INPUT_DIRS)
  foreach(dir ${DGEN_PATHS})
    get_filename_component(dir ${dir} ABSOLUTE)
    set(DOXYGEN_INPUT_DIRS "${DOXYGEN_INPUT_DIRS} \\\n                         ${dir}")
  endforeach(dir)

  # Exclude directories
  set(DOXYGEN_EXCLUDE_DIRS)
  foreach(dir ${DGEN_EXCLUDE_PATHS})
    get_filename_component(dir ${dir} ABSOLUTE)
    set(DOXYGEN_EXCLUDE_DIRS "${DOXYGEN_EXCLUDE_DIRS} \\\n                         ${dir}")
  endforeach(dir)

  # Configure the doxyfile with the variables set above.
  if(NOT DGEN_DOXYFILE)
    set(DGEN_DOXYFILE "cmake/doxyfile.in")
  endif(NOT DGEN_DOXYFILE)
  configure_file("${DGEN_DOXYFILE}" "${CMAKE_CURRENT_BINARY_DIR}/doxyfile")
  get_filename_component(DOXYFILE_PATH "cmake/doxyfile.in" ABSOLUTE)

  # Setup the Doxygen target.
  add_custom_target(${DGEN_PROJECT}-doc
    "${DOXYGEN_EXECUTABLE}" "${CMAKE_CURRENT_BINARY_DIR}/doxyfile"
    DEPENDS
      "${DOXYFILE_PATH}"
      "${CMAKE_CURRENT_BINARY_DIR}/doxyfile"
    )
  add_dependencies(${DGEN_PROJECT} ${DGEN_PROJECT}-doc)
endfunction(doxygen_create)

# doxygen_install(source destination)
#
# Setup an install command to install generated Doxygen documentation. The source specifies the directory under the
# build tree where to find the Doxygen pages. This relates to the OUTPUT_DIR option given to doxygen_create(). The
# destination specifies the subdirectory for the documentation pages and should generatelly match the project name.
function(doxygen_install dir dest)
  install(DIRECTORY ${dir} DESTINATION share/${dest} COMPONENT Devel)
endfunction(doxygen_install)


================================================
FILE: cmake/ohm-config.cmake
================================================
# Copyright (c) 2017
# Commonwealth Scientific and Industrial Research Organisation (CSIRO)
# ABN 41 687 119 230
#
# Author: Kazys Stepanas

# CMake package documentation (https://cmake.org/cmake/help/v3.5/manual/cmake-packages.7.html) shows how to generate
# cmake package config files. While a <package>-config.cmake file can be generated and used directly, the documentation
# recommends providing a wrapper such as this file. The purpose is to ensure other dependencies are found first and
# bound as required. That is, this is a good place to find external dependencies for the project libraries.
# https://cmake.org/cmake/help/v3.5/manual/cmake-packages.7.html#creating-a-package-configuration-file

# Supports selection between OpenCL and CUDA implementations via find_package(ohm COMPONENTS [OpenCL,CUDA]). The
# selected library is defined in the variable OHM_GPU_LIBRARY with the supporting gputil library defined in
# OHM_GPUTIL_LIBRARY

set(OHM_VERSION @ohm_VERSION@)

set(OHM_FEATURE_CUDA @OHM_FEATURE_CUDA@)
set(OHM_FEATURE_HEIGHTMAP_IMAGE @OHM_FEATURE_HEIGHTMAP_IMAGE@)
set(OHM_FEATURE_OPENCL @OHM_FEATURE_OPENCL@)
set(OHM_FEATURE_PDAL @OHM_FEATURE_PDAL@)
set(OHM_FEATURE_THREADS @OHM_FEATURE_THREADS@)

set(OHM_USE_DEPRECATED_CMAKE_CUDA @OHM_USE_DEPRECATED_CMAKE_CUDA@)

set(OHM_TES_DEBUG @OHM_TES_DEBUG@)
set(OHM_BUILD_SHARED @OHM_BUILD_SHARED@)

# Configuration
include("${CMAKE_CURRENT_LIST_DIR}/ohm-packages.cmake")

# Include the generated configuration file.
include("${CMAKE_CURRENT_LIST_DIR}/ohm-config-targets.cmake")

function(register_target TARGET INCLUDES_VAR LIBRARIES_VAR)
  if(TARGET ${TARGET})
    # Resolve include directories
    get_target_property(TARGET_INCLUDE_DIRS ${TARGET} INTERFACE_INCLUDE_DIRECTORIES)
    set(TARGET_INCLUDE_DIRS "${TARGET_INCLUDE_DIRS}" CACHE PATH "${TARGET} include directories")

    # Append to existing include directories
    list(APPEND ${INCLUDES_VAR} ${TARGET_INCLUDE_DIRS})
    list(REMOVE_DUPLICATES ${INCLUDES_VAR})
    # Propagate to caller
    set(${INCLUDES_VAR} ${${INCLUDES_VAR}} PARENT_SCOPE)

    # Resolve release and debug libraries.
    get_target_property(TARGET_LIBRARY_DEBUG ${TARGET} IMPORTED_LOCATION_DEBUG)
    get_target_property(TARGET_LIBRARY_RELEASE ${TARGET} IMPORTED_LOCATION_RELEASE)

    # For non-Windows, substitute RelWithDebInfo for release library. Can't for Windows as it
    # links the debug runtime so is more optimised debug than release with debug info.
    if(NOT WIN32 AND NOT TARGET_LIBRARY_RELEASE)
      get_target_property(TARGET_LIBRARY_RELEASE ${TARGET} IMPORTED_LOCATION_REL_WITH_DEB_INFO)
    endif(NOT WIN32 AND NOT TARGET_LIBRARY_RELEASE)

    # Try with no config specified.
    if(NOT TARGET_LIBRARY_RELEASE)
      get_target_property(TARGET_LIBRARY_RELEASE ${TARGET} IMPORTED_LOCATION)

      if(NOT TARGET_LIBRARY_RELEASE)
        get_target_property(TARGET_LIBRARY_RELEASE ${TARGET} IMPORTED_LOCATION_NOCONFIG)
      endif(NOT TARGET_LIBRARY_RELEASE)
    endif(NOT TARGET_LIBRARY_RELEASE)

    # Get any supporting libraries for the target.
    get_target_property(TARGET_SUPPORT_LIBRARIES ${TARGET} INTERFACE_LINK_LIBRARIES)

    # Extent the library list.
    set(TARGET_LIBRARIES)
    if(TARGET_LIBRARY_DEBUG)
      list(APPEND TARGET_LIBRARIES debug "${TARGET_LIBRARY_DEBUG}" optimized "${TARGET_LIBRARY_RELEASE}")
    else(TARGET_LIBRARY_DEBUG)

      set(TARGET_LIBRARIES "${TARGET_LIBRARY_RELEASE}")
    endif(TARGET_LIBRARY_DEBUG)

    list(APPEND ${LIBRARIES_VAR} ${TARGET_LIBRARIES})
    if(TARGET_SUPPORT_LIBRARIES)
      list(APPEND ${LIBRARIES_VAR} ${TARGET_SUPPORT_LIBRARIES})
    endif(TARGET_SUPPORT_LIBRARIES)

    # Export to caller.
    set(${LIBRARIES_VAR} ${${LIBRARIES_VAR}} PARENT_SCOPE)

  else(TARGET ${TARGET})
    message(SEND_ERROR "${TARGET} not found")
  endif(TARGET ${TARGET})
endfunction(register_target)

# Define the OHM library according to the selected component
if(ohm_FIND_COMPONENTS)
  foreach(_comp ${ohm_FIND_COMPONENTS})
    if(_comp EQUAL "OpenCL")
      set(OHM_GPUTIL_LIBRARY ohm::gputilocl)
      set(OHM_GPU_LIBRARY ohm::ohmocl)
    elseif(_comp EQUAL "CUDA")
      set(OHM_GPUTIL_LIBRARY ohm::gputilcuda)
      set(OHM_GPU_LIBRARY ohm::ohmcuda)
    endif()
  endforeach(_comp)
endif(ohm_FIND_COMPONENTS)

register_target(ohm::ohmtools OHM_INCLUDE_DIRS OHM_LIBRARIES)
register_target(ohm::ohmutil OHM_INCLUDE_DIRS OHM_LIBRARIES)
register_target(ohm::ohm OHM_INCLUDE_DIRS OHM_LIBRARIES)
register_target(ohm::logutil OHM_INCLUDE_DIRS OHM_LIBRARIES)
register_target(ohm::slamio OHM_INCLUDE_DIRS OHM_LIBRARIES)

if(OHM_FEATURE_CUDA)
  register_target(ohm::gputilcuda OHM_INCLUDE_DIRS OHM_LIBRARIES)
  register_target(ohm::ohmcuda OHM_INCLUDE_DIRS OHM_LIBRARIES)
  if(NOT DEFINED OHM_GPU_LIBRARY)
      set(OHM_GPUTIL_LIBRARY ohm::gputilcuda)
    set(OHM_GPU_LIBRARY ohm::ohmcuda)
  endif(NOT DEFINED OHM_GPU_LIBRARY)
  if(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)
    # Using new CUDA configuration. Need to have CUDA::cudart[_static] defined by finding the toolkit.
    find_package(CUDAToolkit)
  endif(NOT OHM_USE_DEPRECATED_CMAKE_CUDA)
endif(OHM_FEATURE_CUDA)

if(OHM_FEATURE_OPENCL)
  register_target(ohm::clu OHM_INCLUDE_DIRS OHM_LIBRARIES)
  register_target(ohm::gputilocl OHM_INCLUDE_DIRS OHM_LIBRARIES)
  register_target(ohm::ohmocl OHM_INCLUDE_DIRS OHM_LIBRARIES)
  if(NOT DEFINED OHM_GPU_LIBRARY)
   set(OHM_GPUTIL_LIBRARY ohm::gputilocl)
    set(OHM_GPU_LIBRARY ohm::ohmocl)
  endif(NOT DEFINED OHM_GPU_LIBRARY)
endif(OHM_FEATURE_OPENCL)

if(OHM_FEATURE_HEIGHTMAP_IMAGE)
  register_target(ohm::ohmheightmap OHM_INCLUDE_DIRS OHM_LIBRARIES)
endif(OHM_FEATURE_HEIGHTMAP_IMAGE)

# Packages required for ohmheightmapimage
if(OHM_FEATURE_HEIGHTMAP_IMAGE)
  find_package(OpenGL REQUIRED)
  find_package(GLEW REQUIRED)
  find_package(glfw3 REQUIRED)
  register_target(ohm::ohmheightmapimage OHM_INCLUDE_DIRS OHM_LIBRARIES)
endif(OHM_FEATURE_HEIGHTMAP_IMAGE)


================================================
FILE: cmake/ohm-packages.cmake
================================================
# Copyright (c) 2017
# Commonwealth Scientific and Industrial Research Organisation (CSIRO)
# ABN 41 687 119 230
#
# Author: Kazys Stepanas

# Configure variables and include packages.

# Find packages
find_package(Threads)
find_package(glm REQUIRED)

if(OHM_FEATURE_THREADS)
  find_package(TBB CONFIG)
endif(OHM_FEATURE_THREADS)

if(NOT OHM_BUILD_SHARED)
  # We only need to propagate these packages for static ohm builds.
  if(OHM_TES_DEBUG)
    find_package(3es)
  endif(OHM_TES_DEBUG)

  if(OHM_FEATURE_PDAL)
    find_package(PDAL REQUIRED)
  endif(OHM_FEATURE_PDAL)

  find_package(ZLIB)
endif(NOT OHM_BUILD_SHARED)


================================================
FILE: cmake/tidy/clang-tidy-names.yaml
================================================
# Run clang tidy variable naming checks only.
---
Checks: >
  -*,
  readability-identifier-naming
WarningsAsErrors: ''
HeaderFilterRegex: '.*'
AnalyzeTemporaryDtors: false
FormatStyle:     none
User: csiro
CheckOptions:
  # ---------------------------------------------------------------------------
  # Classes and structs
  # Class names
  - key:  readability-identifier-naming.ClassCase
    value: CamelCase
  # Structs
  - key:  readability-identifier-naming.StructCase
    value: CamelCase
  # static const class members
  - key:  readability-identifier-naming.ClassConstantCase
    value: CamelCase
  - key:  readability-identifier-naming.ClassConstantPrefix
    value: 'k'
  # static class members (non-const)
  - key:  readability-identifier-naming.ClassMemberCase
    value: lower_case
  # Class members which are const, but not static - omitted to control by access scope (public, private, etc)
  # - key:  readability-identifier-naming.ConstantMemberCase
  #   value: lower_case
  # Class member functions - any access modifier (public, private, etc)
  - key:  readability-identifier-naming.ClassMethodCase
    value: camelBack
  # Class member variable catchall
  - key:  readability-identifier-naming.MemberCase
    value: lower_case
  # Private class member variables
  - key:  readability-identifier-naming.PrivateMemberCase
    value: lower_case
  - key:  readability-identifier-naming.PrivateMemberSuffix
    value: '_'
  # Protected member variables
  - key:  readability-identifier-naming.ProtectedMemberCase
    value: lower_case
  - key:  readability-identifier-naming.ProtectedMemberSuffix
    value: '_'

  # ---------------------------------------------------------------------------
  # Enum declaration name case
  - key:  readability-identifier-naming.EnumCase
    value: CamelCase
  # Enum value declarations (the stuff inside the enum)
  - key:  readability-identifier-naming.EnumConstantCase
    value: CamelCase
  - key:  readability-identifier-naming.EnumConstantPrefix
    value: 'k'

  # ---------------------------------------------------------------------------
  # Templates
  - key:  readability-identifier-naming.TemplateParameterCase
    value: CamelCase
  # - key:  readability-identifier-naming.TemplateTemplateParameterCase
  #   value: CamelCase
  # - key:  readability-identifier-naming.TypeTemplateParameterCase
  #   value: CamelCase
  # - key:  readability-identifier-naming.ValueTemplateParameterCase
  #   value: CamelCase


  # ---------------------------------------------------------------------------
  # General and global
  # constexpr variable assignments
  - key:  readability-identifier-naming.ConstexprVariableCase
    value: CamelCase
  - key:  readability-identifier-naming.ConstexprVariablePrefix
    value: 'k'
  # Namespaces
  - key:  readability-identifier-naming.NamespaceCase
    value: lower_case
  # General function parameter names
  - key:  readability-identifier-naming.ParameterCase
    value: lower_case
  # Union names
  - key:  readability-identifier-naming.UnionCase
    value: CamelCase
  # General variable declarations
  - key:  readability-identifier-naming.VariableCase
    value: lower_case
  # Typedef names
  - key:  readability-identifier-naming.TypedefCase
    value: CamelCase
  # Names for type aliases: using Name = OtherThing;
  # Includes aliases declared in classes.
  - key:  readability-identifier-naming.TypeAliasCase
    value: CamelCase
  # Free function case
  - key:  readability-identifier-naming.FunctionCase
    value: camelBack
  # Global/free constant variable case. Includes anynomous namespaces.
  - key:  readability-identifier-naming.GlobalConstantCase
    value: CamelCase
  - key:  readability-identifier-naming.GlobalConstantPrefix
    value: 'k'
  # Variables in the global scope.
  - key:  readability-identifier-naming.GlobalVariableCase
    value: lower_case
  - key:  readability-identifier-naming.GlobalVariablePrefix
    value: 'g_'
  # Constants declared within local function scopes. Same as normal variables.
  - key:  readability-identifier-naming.LocalConstantCase
    value: lower_case
  - key:  readability-identifier-naming.LocalConstantPrefix
    value: ''
  # Local variable
  - key:  readability-identifier-naming.LocalVariableCase
    value: lower_case
...


================================================
FILE: cmake/tidy/wrap-clang-tidy.py
================================================
#!/usr/bin/python3
"""This script wraps up running clang tidy to acheive the goals listed below.

1. Support specifying a yaml configuration file on the command line (-config-file)
2. Optionally run the `run-clang-tidy` python wrapper, which supports job threads
3. Filter redundant output from `run-clang-tidy`
4. Mutate paths to deal with several path reporting issues, which come from how compile_commands.json is configured
5. Optionally report project relative paths.

These goals are described in more detail below.

1. Command line config file

Clang-tidy frustratingly only supports a specific .clang-tidy file which is recursively searched, or command line
cofiguration. Configuration can be very long, so file configuration is preferable, but we want to support different
analysis modes. So we add support to load a .clang-tidy yaml file as specified by `-config-file` and pass this to
clang tidy via the command line.

2. Use `run-clang-tidy`

clang-tidy ships with a companion python script `run-clang-tidy`. This script is currently the only way to run
clang-tidy and use multiple threads. The script essentially splits the files listed and sets up separate processes
for each file.

3. Fix redundant output from `run-clang-tidy`

When using `run-clang-tidy` it outputs the entire configuration used to stdout. This makes for a not of redundant
output especially when processing many files. We filter and suppress this output.

4. Mutate paths

An interesting quirk was discovered in path reporting when using ninja to generate the compile_commands.json. Ninja can
add relative include paths to the json file. This chains on to highlight an undefined behaviour bug in clang-tidy.
Essentially, depending on which checks actually report issues or not, the output can inlcude a mix of absolute and
relative paths - relative to the compile_commands.json file. Specifically, `google-readability-casting` would report
relative paths, unless `readability-identifier-naming` also had issues to report (this is the only known case).

TODO(KS): insert bug report reference

We detect these relative paths, assume they are relative to compile_commands.json and make them absolute.

5. Project relative paths

Project relative paths can be reported by specifying the argument `-relative-to`. Reported paths are made relative to
this path. The intension is to report portable paths which can, for example, be generated on a CI server, but handled
on another computer.
"""
from __future__ import print_function

import argparse
import os
import platform
import re
import subprocess
import sys
import threading

if sys.version_info[0] >= 3:
    from queue import Queue
else:
    from Queue import Queue

# Command line parser setup.


def setup_args():
    """Setup and return the command line argument parser"""
    parser = argparse.ArgumentParser(description='')
    # parser.add_argument('csv', type=str, help='CSV file to load')
    parser.add_argument(
        '-clang-tidy-binary', help='Path to the clang-tidy executable.',  metavar='PATH', required=True)
    parser.add_argument('-clang-apply-replacements-binary',
                        help='Path to the clang-apply-replacements binary. Required when using -fix and -runner-py' +
                        ' arguments.')
    parser.add_argument(
        '-runner-py', help='Python script wrapping clang-tidy with support for multiple jobs. run-clang-tidy.py ships' +
        ' with clang-tidy. Without this clang-tidy is run directly.', metavar='PATH')
    parser.add_argument('-fix', action='store_true',
                        help='Apply automatic fixes. Passes -fix to clang-tidy. When using -runner-py' +
                        ' (run-clang-tidy.py), the argument -clang-apply-replacements-binary must also be set to the' +
                        ' clang-apply-fixes binary.')
    parser.add_argument(
        '-config-file', help='clang-tidy configuration file. Extracted and passed as the -config argument to' +
                             ' clang-tidy.')
    parser.add_argument(
        '-p', help='clang-tidy build path (path to compile_commands.json). Extracted and passed as the -p argument to' +
                   ' clang-tidy.', required=False)
    parser.add_argument(
        '-j', help='Number of parallel jobs to run. Only supported when using the -runner-py script. Ignored ' +
        'otherwise.', required=False)
    parser.add_argument(
        '-relative-to', help='Modify clang-tidy message paths to be relative to this directory. Intended for CI' +
        ' builds to report portable paths.', required=False)
    return parser


def resolve_build_path(args):
    """Resolve the compile_commands.json directory if specified. None when not present."""
    # Use argparser to resolve the -p option so we support '-p <path>" (space)
    # and "-p=<path>" (equals)
    build_path = None
    if args.p is not None:
        # Strip 'comile_commands.json' if present
        build_path = args.p
        if build_path.endswith('compile_commands.json'):
            build_path = build_path[:len(
                build_path) - len('compile_commands.json')]
    return build_path


def escape_path(path):
    # Need to escape back slashes in args for Windows.
    if platform.system() == 'Windows':
        return path.replace('\\', '\\\\')
    return path


class ProcessMessagePump:
    """A helper class for handling subprocess output and attempting to maintain output order.

    Starts a thread each for stdout and stderr then collates on the main thread.

    Usage:
    - Create a subprocess with both stdout and stderr set to subprocess.PIPE
    - Create a ProcessMessagePump around the process
    - Call ProcessMessagePump.pump() with an appropriate logging function.
    """

    def __init__(self, process):
        """Create a piper around process"""
        self.process = process
        self.log_queue = Queue()
        self.pipes_running = 0

    def pump(self, log):
        """Start logging using the log function until the process is done.

        The log function signature must be log(process, pipe, line)
        """
        threading.Thread(target=ProcessMessagePump._pump, args=[
                         self, self.process.stdout]).start()
        threading.Thread(target=ProcessMessagePump._pump, args=[
                         self, self.process.stderr]).start()
        self.pipes_running += 2
        while self.pipes_running > 0 or not self.log_queue.empty():
            pipe_source, line = self.log_queue.get()
            if pipe_source is None or line is None:
                continue
            log(self.process, pipe_source, line)

    def _pump(self, pipe):
        """Thread pump function"""
        try:
            # Keep going until the process ends
            while self.process.poll() is None:
                # Read a line each loop and add to the queue
                line = pipe.readline()
                if line:
                    self.log_queue.put((pipe, line))
            # Final flush
            try:
                for line in iter(pipe.readline, ''):
                    self.log_queue.put((pipe, line))
            except:
                # Ok to have an I/O operation failure on this call. The pipe may have been closed
                pass
        finally:
            # Ensure we note completion of this thread in the queue and class.
            self.log_queue.put((pipe, None))
            self.pipes_running -= 1


if __name__ == '__main__':
    # ---------------------------------------------------------------------------
    # Parse arguments.
    arg_parse = setup_args()
    args = arg_parse.parse_known_args(sys.argv[1:])

    # ---------------------------------------------------------------------------
    # Start building process arguments in tidy_args
    tidy_args = []
    using_runner = False

    # Handle using run-clang-tidy or clang-tidy directly
    if args[0].runner_py:
        using_runner = True
        if platform.system() == 'Windows':
            # The runner will be an executable on platforms *other* than Windows. For Windows, run via python.
            tidy_args.append(sys.executable)
        tidy_args.append(args[0].runner_py)
        if args[0].clang_tidy_binary:
            tidy_args.append('-clang-tidy-binary=' +
                             escape_path(args[0].clang_tidy_binary))
        if args[0].clang_apply_replacements_binary:
            tidy_args.append('-clang-apply-replacements-binary=' +
                             escape_path(args[0].clang_apply_replacements_binary))
        if args[0].j:
            tidy_args.append('-j')
            tidy_args.append(args[0].j)
        else:
            # We explicitly specify the number of jobs to run. The parallel run script fully loads the CPUs when running
            # parallel, so we limit it to keep any UI and OS tasks responsive.
            try:
                import psutil
                job_threads = psutil.cpu_count() - 2
                if job_threads < 2:
                    job_threads = 2
                tidy_args.append('-j')
                tidy_args.append(str(job_threads))
            except ImportError:
                pass

    else:
        tidy_args.append(escape_path(args[0].clang_tidy_binary))

    # Resolve the compile_commands.json path. Note this must be the path to this file, but exclude the file name.
    # This is perculiar to using run-clang-tidy, and clang-tidy itself is ok with the file name.
    build_path = None
    if args[0].p is not None:
        tidy_args.append('-p={}'.format(args[0].p))
        build_path = resolve_build_path(args[0])

    # Apply fixes?
    if args[0].fix:
        tidy_args.append('-fix')

    # Use command line specified .clang-tidy yaml file and extract content to the command line
    config_lines = []
    if args[0].config_file:
        # Read the config file to use.
        with open(args[0].config_file) as config_file:
            config = config_file.read()
            # # Replace line endings with the character sequence '\' 'n' (2 characters) in a way which deals with
            # # any line ending setup.
            # # Replace microsoft line endings
            # config = config.replace('\r\n', '\\n')
            # # Replace MacOS line endings
            # config = config.replace('\r', '\\n')
            # # Replace Unix line endings
            # config = config.replace('\n', '\\n')
        tidy_args.append('-config={}'.format(config))
        # Build the filter for goal "Fix redundant output from `run-clang-tidy`"
        config_lines = config.splitlines() if using_runner else config_lines

    # Add -quiet to suppress a message about which checks are being used (run-clang-tidy)
    tidy_args.append('-quiet')

    # Add other arguments - like the file list
    tidy_args.extend(args[1])

    # ---------------------------------------------------------------------------
    # Setup running the process.

    # Build a regular expression for path fix parsing.
    # Groups:
    # 0: file path (to normalise)
    # 1: line number if column also present
    # 2: column number if 1/2 present, otherwise line number
    # 3: the rest (message)
    error_msg_re = re.compile(r'^(.*?):(.*)')

    def fix_path(line):
        """Fix certain aspects of paths.

        Firstly clang-tidy with ninja generated compile_commands.json can report relative
        paths (to the compile_commands.json path). We ensure these paths are made absolute.

        Secondly we normalise paths.

        Lastly we modify paths to be relative to args.relative_to if specified.
        """
        match = error_msg_re.match(line)
        if match:
            path = match.groups()[0]
            if build_path is not None:
                if not os.path.isabs(path):
                    # Relative path reported. Make relative to compile_commands.json
                    path = os.path.join(build_path, path)
            # Normalise the path.
            path = os.path.abspath(path)
            if args[0].relative_to:
                path = os.path.relpath(path, args[0].relative_to)
            return '{0}:{1}'.format(path, match.groups()[1])
        return line

    def read_output(process, pipe, line):
        """Process output pump and passthrough"""
        # The run-clang-tidy script (which adds threads) does two annoying things - everything is on stdout and
        # it repeats the config in the log.
        # We filter this here, but only if using the runner. Just checking if the line is in the config setup
        # seems to work well.
        out = sys.stderr
        if pipe == process.stdout:
            out = sys.stdout
            if line.strip('\r\n') in config_lines:
                line = None
        if line:
            out.write(fix_path(line))

    # ---------------------------------------------------------------------------
    # Run the process
    error_code = -1
    tidy = subprocess.Popen(tidy_args, stdout=subprocess.PIPE,
                            stderr=subprocess.PIPE, universal_newlines=True)
    # Setup message pump
    piper = ProcessMessagePump(tidy)
    piper.pump(read_output)
    error_code = tidy.returncode
    sys.exit(error_code)


================================================
FILE: cmake/utils.cmake
================================================
# Copyright (c) 2018
# Commonwealth Scientific and Industrial Research Organisation (CSIRO)
# ABN 41 687 119 230
#
# Author: Kazys Stepanas

# A collection of useful utility functions.

include(CMakeParseArguments)

#-------------------------------------------------------------------------------
# ohm_feature(<name> <help_string>
#   [FIND <package1> [<package2>...]]
#   [CONDITION <condition>]
# )
#
# Setup an OHM feature which can be enabled via either VCPKG or CMake with VCPKG_MANIFEST_FEATURES being authoritative.
#
# The feature NAME is mapped to a cmake option as 'OHM_FEATURE_$<NAME>' and to an equivalent VCPKG feature - an entry
# in the VCPKG_MANIFEST_FEATURES list - where the vcpkg feature name is the lower case NAME with underscores replaced by
# hyphens '-'. The CMake option defaults to on when the feature is present in VCPKG_MANIFEST_FEATURES.
#
# Additionally, the default state of the CMake option can be affected by the results of find_package() commands. When
# FIND is specified and all packages listed are located, then the CMake OHM_FEATURE_<NAME> option is enabled. Note that
# this does not guarantee that the library is located from the VCPKG packages, thus when using vcpkg, it's best to
# toggle features using VCPKG_MANIFEST_FEATURES exclusively.
#
# FIND - list of packages which, if found, will enable this feature implicitly.
#
# DEFAULT - a variable which determines the default state of the feature option if not explicitly enabled.
function(ohm_feature NAME HELP)
  cmake_parse_arguments(ARG "" "DEFAULT" "FIND" ${ARGN})
  set(_require_from_vcpkg No)   # Required from VCPKG feature set?
  set(_require_from_option No)  # Required from OHM_FEATURE_ option?
  set(_imply_from_package No)   # Package is available if needed, potentially implying the feature?

  string(TOLOWER "${NAME}" _vcpkg_name)
  # _ not allowed in vcpkg feature
  string(REPLACE "_" "-" _vcpkg_name "${_vcpkg_name}")

  # Check for VCPKG specified feature.
  list(FIND VCPKG_MANIFEST_FEATURES ${_vcpkg_name} _find_at)
  if(_find_at GREATER_EQUAL 0)
    # VCPKG feature explicitly enabled.
    set(_require_from_vcpkg Yes)
    set(_feature_source "vcpkg")
  endif(_find_at GREATER_EQUAL 0)

  # Check for build option enabled (from command line or cache).
  if(OHM_FEATURE_${NAME})
    set(_require_from_option Yes)
    set(_feature_source "option")
  endif(OHM_FEATURE_${NAME})

  # Determine default state.
  if(_require_from_vcpkg OR _require_from_option)

    # If explicitly required then enable by default.
    set(_default On)

  else(_require_from_vcpkg OR _require_from_option)

    # If not explicitly required then check the DEFAULT and FIND args.
    set(_default On)

    if(DEFINED ARG_DEFAULT)
      if(NOT ${ARG_DEFAULT})
        set(_default Off)
      endif(NOT ${ARG_DEFAULT})
      if(_default)
        set(_feature_source "default")
      endif(_default)
    endif(DEFINED ARG_DEFAULT)

    if(DEFINED ARG_FIND)
      # If enable by default, try to find packages.
      if(_default)
        foreach(pkg ${ARG_FIND}) # Check each package
          find_package(${pkg} QUIET)
          if(NOT ${pkg}_FOUND)
            # Missing package. Do not enable by default.
            set(_default Off)
            break()
          endif(NOT ${pkg}_FOUND)
        endforeach(pkg)
      endif(_default)
      if(_default)
        set(_feature_source "find_package")
      endif(_default)
    endif(DEFINED ARG_FIND)

  endif(_require_from_vcpkg OR _require_from_option)

  option(OHM_FEATURE_${NAME} "${HELP}" "${_default}")
  if(OHM_FEATURE_${NAME})
    message(STATUS "Enable feature ${NAME} from ${_feature_source}")
  endif(OHM_FEATURE_${NAME})
endfunction(ohm_feature)

#-------------------------------------------------------------------------------
# option_expr
# Add an option with the given expression determining the default option state.
function(option_cond VAR HELP EXPR)
  set(_DEFAULT OFF)
  if(${EXPR})
    set(_DEFAULT ON)
  endif(${EXPR})
  option(${VAR} "${HELP}" ${_DEFAULT})
endfunction(option_cond)

#-------------------------------------------------------------------------------
# show_properties(<GLOBAL           |
#                  DIRECTORY [dir]  |
#                  TARGET <target>  |
#                  SOURCE <source>  |
#                  INSTALL <file>   |
#                  TEST <test>      |
#                  CACHE <entry>    |
#                  VARIABLE>)
# Print all the properties and their values of the selected type. For example, target properties are listed by
# specifying show_properties(TARGET <target>), while global properties are listed using show_properties(GLOBAL)
function(show_properties TYPE)
  # Validate TYPE values requiring an argument.
  set(TARGETED_ITEMS CACHE INSTALL SOURCE TARGET TEST)
  list(FIND TARGETED_ITEMS ${TYPE} TYPE_INDEX)
  if(TYPE_INDEX GREATER -1)
    if("${ARGV1}" STREQUAL "")
      message(SEND_ERROR "show_properties(${TYPE} <item>) missing <item> argument.")
      return()
    endif("${ARGV1}" STREQUAL "")
  endif(TYPE_INDEX GREATER -1)

  execute_process(COMMAND ${CMAKE_COMMAND} --help-property-list OUTPUT_VARIABLE CMAKE_PROPERTY_LIST)
  # Convert command output into a CMake list
  string(REGEX REPLACE ";" "\\\\;" CMAKE_PROPERTY_LIST "${CMAKE_PROPERTY_LIST}")
  string(REGEX REPLACE "\n" ";" CMAKE_PROPERTY_LIST "${CMAKE_PROPERTY_LIST}")

  # Filter the property list.
  set(ITEM_PROPERTY_LIST)
  foreach(PROPERTY ${CMAKE_PROPERTY_LIST})
    # message("${PROPERTY}...")
    if(PROPERTY MATCHES ".*<CONFIG>.*")
      # Replace <CONFIG> with each of the build configs and add those to the list.
      foreach(CONFIG Debug;MinSizeRel;Release;RelWithDebInfo)
        string(REGEX REPLACE "(.*)<CONFIG>(.*)" "\\1${CONFIG}\\2" CONFIG_PROPERTY "${PROPERTY}")
        list(APPEND ITEM_PROPERTY_LIST "${CONFIG_PROPERTY}")
        # message("...${CONFIG_PROPERTY}")
      endforeach(CONFIG)
    else(PROPERTY MATCHES "$.*<CONFIG>^")
      list(APPEND ITEM_PROPERTY_LIST "${PROPERTY}")
      # message("...${PROPERTY}")
    endif(PROPERTY MATCHES ".*<CONFIG>.*")
  endforeach(PROPERTY)

  if("${ARGV1}" STREQUAL "")
    message("${TYPE} properties:")
  else("${ARGV1}" STREQUAL "")
    set(ITEM "${ARGV1}")
    message("${ITEM} properties:")
  endif("${ARGV1}" STREQUAL "")
  foreach(PROPERTY ${ITEM_PROPERTY_LIST})
    # message ("Checking ${prop}")
    get_property(PROPERTY_VALUE ${TYPE} ${ITEM} PROPERTY ${PROPERTY} SET)
    if(PROPERTY_VALUE)
      get_property(PROPERTY_VALUE ${TYPE} ${ITEM} PROPERTY ${PROPERTY})
      message ("  ${PROPERTY}: ${PROPERTY_VALUE}")
    endif(PROPERTY_VALUE)
  endforeach(PROPERTY)
endfunction(show_properties)

#-------------------------------------------------------------------------------
# show_target_properties(<target>)
# Print all the properties and their values for <target>. Only lists the properties which have been set for <target>.
function(show_target_properties TARGET)
  # https://stackoverflow.com/questions/32183975/how-to-print-all-the-properties-of-a-target-in-cmake
  if(NOT TARGET ${TARGET})
    message("There is no target named '${TARGET}'")
    return()
  endif(NOT TARGET ${TARGET})

  show_properties(TARGET ${TARGET})
endfunction(show_target_properties)

#-------------------------------------------------------------------------------
# show_variables()
# List all variables and their values.
function(show_variables)
  get_cmake_property(VAR_NAMES VARIABLES)
  foreach (VAR ${VAR_NAMES})
      message("${VAR}: ${${VAR}}")
  endforeach(VAR)
endfunction(show_variables)


================================================
FILE: docs/docglossary.md
================================================
<!--
Copyright (c) 2022
Commonwealth Scientific and Industrial Research Organisation (CSIRO)
ABN 41 687 119 230

Author: Kazys Stepanas

-->

<!-- Use details section to partly hide doxygen specific details of the page. -->
<details><summary></summary>
@page docglossary OHM Glossary
</details>

# OHM Glossary

| Term      | Description                                                                     |
| --------- | ------------------------------------------------------------------------------- |
| atomic    | Generally, atomic CPU/GPU instructions, executed as a single operation.         |
| CAS       | Atomic Compare and Swap operation                                               |
| Chunk     | Related to region, normally focusing on the memory layout of the region.        |
| GP GPU    | General Purpose GPU programming                                                 |
| NDT       | Normal distribution transform, normally implying NDT use in occupancy maps.     |
| Occupancy | Probabilistic determination of whether a voxel is occupied, free or unobserved. |
| Region    | A region of space containing a fixed number of voxels.                          |
| TSDF      | Truncated signed distance fields.                                               |
| Voxel     | A 3D cube (normally) division of space.                                         |


================================================
FILE: docs/docmain.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230

// Author: Kazys Stepanas
#ifndef DOCMAIN_H
#define DOCMAIN_H

/*!
@mainpage Occupancy Homogeneous Map documentation

The ohm library is a probabilistic voxel occupancy map supporting fast GPU based population and operations and normal
distribution transform semantics. The ohm library defines an occupancy map consisting of regions or chunks of
homogeneous voxels, arranged in contiguous memory blocks. This homogeneous voxel layout, rather than an octree layout,
supports fast GPU based map population using OpenCL and CUDA.

- @subpage docglossary
- @subpage docusage
- @subpage docutils
- @subpage docgpualgorithm
- @subpage docgpudetail
- @subpage docvoxellayers

*/

#endif  // DOCMAIN_H


================================================
FILE: docs/docusage.md
================================================
<!--
Copyright (c) 2020
Commonwealth Scientific and Industrial Research Organisation (CSIRO)
ABN 41 687 119 230

Author: Kazys Stepanas
-->

<!-- Use details section to partly hide doxygen specific details of the page. -->
<details><summary></summary>
@page docusage Occupancy map usage
</details>

# Occupancy map usage

This section details general usage of an `OccupancyMap` including map generation and data access. The page is
split into sections for CPU map access and GPU map generation.

# CPU map usage

The `OccupancyMap` class is used to contain the occupancy map data. This class is always used at some level
regardless of how the map is generated or populated. For example, the `GpuMap` wraps an `OccupancyMap` to
support populating the map in GPU, but the underlying map object is the same.

The data within the map are stored in "chunks" (`MapChunk`) which represent contiguous blocks of voxel memory
corresponding to fixed 3D regions of the map. Chunks are created as needed. This dense memory layout is what makes GPU
update of the map possible. The additional overhead of using a dense memory layout is mitigated by using
background compression (`VoxelBlockCompressionQueue`) of voxel data when not in use.

The map object stores the map data as well as the parametrisation of the map - for example the
occupancy threshold (`OccupancyMap::occupancyThresholdProbability()`) and
resolution (`OccupancyMap::resolution()`). The map object is also responsible for mapping between global
coordinates and voxel keys (`Key`) via the `OccupancyMap::voxelKey()` function.

A map is created with a fixed resolution and fixed chunk size by specifying the voxel extents on each axis for a chunk.
The following code creates an empty occupancy map at a resolution of 0.25 units per voxel and 32x32x32 voxels in each
chunk.

```cpp
ohm::OccupancyMap map(0.25, glm::u8vec3(32));
```

Data can be added to the map by using a `RayMapper`. Note that map object does not feature any methods for adding
data to the map. The simplest mapper implementation is the `RayMapperOccupancy`. Using this mapper, origin/sample
pairs (rays) are added to the map with the sample voxel occupancy increased and the occupancy for voxels along the ray
decreased. This is shown in the code snipped below.

```cpp
// A virtual utility class used in the examples to provide data for an occupancy map.
class DataProvider
{
public:
  virtual ~DataProvider() = default;
  // Get the next batch of rays to add to the map.
  // @param rays Populated with the next batch of origin/sample pairs (rays) to add to the map.
  // @return True while there are more rays to add.
  virtual bool nextBatch(std::vector<glm::dvec3> &rays) = 0;
};

void populateMap(DataProvider &provider)
{
  ohm::OccupancyMap map(0.25);          // Create a map
  ohm::RayMapperOccupancy mapper(&map); // Create a mapper for the map.
  std::vector<glm::dvec3> rays;         // Ray set storage.

  // While data are available.
  while (provider.nextBatch(rays))
  {
    // Add data to the map.
    mapper.integrateRays(rays.data(), rays.size());
  }
}
```

Data can then be queried about individual voxels using a combination of voxel keys and `Voxel` objects. The code below
queries whether the voxel containing a given spatial position is occupied.

```cpp
bool isVoxelAtPositionOccupied(const ohm::OccupancyMap &map, const glm::dvec3 &position)
{
  // First create a Voxel object which will reference the voxel occupancy data.
  // - We declare the template type as const float specifying that we expect voxel data to be a float per voxel.
  // - We use the map layout to resolve the map layer which contains occupancy data (more on this below).
  Voxel<const float> voxel_occupancy(&map, map.layout().occupancyLayer());

  // Validate that the Voxel is referencing a valid layer.
  if (!voxel_occupancy.isLayerValid())
  {
    // Layer is invalid - there is no occupancy layer.
    return false;
  }

  // Next resolve a key for the voxel containing position.
  const ohm::Key key = map.voxelKey(position);

  // Set the key for the Voxel object to reference.
  voxel_occupancy.setKey(key);

  // Ensure the key reference is valid within the map. An invalid voxel indicates the voxel has not been observed.
  if (!voxel_occupancy.isValid())
  {
    return false;
  }

  // Get the voxel occupancy value.
  float occupancy_value;
  // We use a read function to address C++ memory access standards.
  voxel_occupancy.read(&occupancy_value);

  // Finally check if the value is above the occupancy threshold.
  return occupancy_value >= map.occupancyThresholdValue();
}
```

The example above introduces several concepts including the map layout (`MapLayout`). The example demonstrates how
to validate and access the data for a particular voxel layer (`MapLayer`), in this case the occupancy layer. Note that
there are convenience functions for this available in the `voxeloccupancy` section.

## The MapLayout

An `OccupancyMap` has an associated `MapLayout` which specifies the data available in the map and how that data
are laid out. The `MapLayout` specifies a set of `MapLayer` objects and each layer identifies some data associated
with each voxel. That is, each voxel may have multiple data types associated with it and for each data type the
`MapChunk` stores a contiguous memory allocation for that data.

By default, an `OccupancyMap` contains a `float` layer which stores the occupancy value for each voxel. Maps
constructed with the `kVoxelMean` flag also contain a layer which tracks a subvoxel position which
represents an approximate mean value of all samples which have contributed to that voxel. This layer adds a
`VoxelMean` structure for each voxel. The `NdtMap` also adds a covariance layer (`CovarianceVoxel`). Additional
user data may also be added using the `MapLayout` and `MapLayer` API.

Voxel data should generally be accessed using the `Voxel` template class. This class is designed to
handle referencing a particular voxel layer and to validate the data size against the voxel layer size. The `Voxel`
template type is used to specify both the data type and read only vs read/write access.

For example, the code below shows how to access the `VoxelMean` data including some invalid access patterns.

```cpp
void meanExample(ohm::OccupancyMap &map)
{
  // Manually resolve the voxel layer index. This is also available in ohm::MapLayout::meanLayer() .
  // Resolve by name. This name is also available as ohm::default_layer::meanLayerName() .
  const MapLayer *layer = map.layout().layer("mean");

  // Validate layer.
  if (!layer)
  {
    return;
  }

  // Create a Voxel object for read/write access to the layer.
  Voxel<VoxelMean> mean_rw(&map, layer->layerIndex());
  // Create a Voxel object for read only access. Note the template type is const
  Voxel<const VoxelMean> mean_read(&map, layer->layerIndex());

  // Query the voxel at the origin. This cannot create the voxel chunk.
  mean_read.setKey(map.voxelKey(glm::dvec3(0)));
  if (mean_read.isValid())
  {
    // read and report the number of points contributing to the mean.
    VoxelMean mean;
    mean_read.read(&mean);
    std::cout << "The voxel at the origin contains " << mean.count << " samples" << std::endl;
  }
  else
  {
    std::cout << "The voxel at the origin has not been created" << std::endl;
  }

  // We copy the mean_rw key from the mean_read object. This can be more efficient in tight loops
  // as some data lookups can be skipped.
  // This call can create the MapChunk for the key, whereas mean_read.setKey() could not.
  mean_rw.setKey(mean_read);
  // The validity check can only fail if mean_rw.isLayerValid() is false. Conversely, it will always be valid so long as
  // the map and layer references are valid.
  if (mean_rw.isValid())
  {
    // Read and report the number of points contributing to the mean.
    VoxelMean mean;
    mean_rw.read(&mean);
    std::cout << "The voxel at the origin contains " << mean.count << " samples" << std::endl;
    // Reset the number of samples to zero.
    mean.count = 0;
    mean_rw.write(mean);
  }
}
```

# NDT map

The `NdtMap` is an extension of the `OccupancyMap` which adds normal distribution transforms
semantics. This adds a covariance representation to each voxel which can be used to represent a "surfel" within the
voxel. See `CovarianceVoxel` for more details. The `NdtMap` should always be populated (in CPU) using the
`RayMapperNdt`.

# Iterating a map

Once a map has been populated, it is possible to iterate the voxels in the map using either a using range based for
loop over the map or using a `OccupancyMap::iterator`. In either case, this will iterate the chunks (`MapChunk`) in the
map in an undefined order and iterate each voxel within the chunk. Iteration of voxels within a chunk starts
from the chunk's `MapChunk::firstValidKey()` which is maintained as the first voxel in the chunk memory which has
been touched.

Iterating with a range based for loop or dereferencing the `OccupancyMap::iterator` yields a `Key` for the
current voxel. The data associated with the voxel must be resolved using the `Voxel` template class. The
`OccupancyMap::iterator` has additional, non-standard iterator functions which provide access to the target
`MapChunk` and `OccupancyMap`. Below is an example of iterating a map.

```cpp
// A structure detailing some map statistics
struct MapStats
{
  // Smallest point count for an occupied voxel.
  unsigned min_point_count{0};
  // Largest point count for an occupied voxel.
  unsigned max_point_count{0};
  // Total number of samples contributing to the map.
  uint_64 total_sample_count{0};
  // Average point count for an occupied voxel.
  unsigned average_point_count{0};
  // Number of occupied voxels.
  unsigned occupied_voxel_count{0};
  // Number of free voxels.
  unsigned free_voxel_count{0};
};

MapStats collectMapStats(const ohm::OccupancyMap &map)
{
  MapStats stats;

  // Setup voxel data.
  ohm::Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  ohm::Voxel<ohm::VoxelMean> mean(&map, map.layout().meanLayer());

  ohm::VoxelMean mean_value;

  for (auto iter = map.begin(); iter != map.end(); ++iter)
  {
    // Set the key for all voxel data accessors in one call.
    // Note: passing the iterator to setVoxelKey() instead of the key is more efficient as the MapChunk can be copied
    // from the iterator. A range based for loop will miss this minor efficiency gain.
    ohm::setVoxelKey(iter, occupancy, mean);
    if (occupancy.isValid()) // Should always be true - we are only iterating known voxels.
    {
      if (ohm::isOccupied(occupancy))
      {
        // Increment occupied voxels.
        ++stats.occupied_voxel_count;
        mean.read(&mean_value);

        stats.total_sample_count += mean_value.count;
        stats.min_point_count =
          (stats.occupied_voxel_count > 1) ? std::min(stats.min_point_count, mean_value.count) : mean_value.count;
        stats.max_point_count = std::max(stats.min_point_count, mean_value.count);
      }
      else if (ohm::isFree(voxel))
      {
        ++stats.free_voxel_count;
      }
    }
  }

  // Finalise average.
  if (stats.occupied_voxel_count)
  {
    stats.average_point_count = unsigned(stats.total_sample_count / stats.occupied_voxel_count);
  }

  return stats;
}
```

# GPU map

GPU support is implemented in the `ohmgpucuda` and `ohmgpuocl` libraries, using CUDA and OpenCL respectively. While
these are technically optional libraries, they are the focus of the ohm innovation. These libraries have the same
API backed by the associated GPU SDK. The SDK selection is forced at compile time and cannot be switched at runtime.

The ohm GPU API introduces the `GpuMap` class, which is both a wrapper for an `OccupancyMap` and
the `RayMapper` implementation which should be used to update the map. The code below shows how to use the `GpuMap`
to populate an `OccupancyMap`.

```cpp
void populateGpuMap(DataProvider &provider)
{
  ohm::OccupancyMap map(0.1);   // Create a map
  ohm::GpuMap gpu_map(&map);    // Create a GPU map
  std::vector<glm::dvec3> rays; // Ray set storage.

  // While data are available.
  while (provider.nextBatch(rays))
  {
    // Add data to the map.
    gpu_map.integrateRays(rays.data(), rays.size());
  }

  // Synchronise GPU data back to the CPU map memory.
  gpu_map.syncVoxels();
}
```

Note the call to `gpu_map.syncVoxels()`. The `GpuMap` does not automatically sync GPU changes back to CPU memory.
This call ensures data synchronisation and allows read access to the map on CPU following this call.

The `GpuMap` relies on a `GpuCache` which keeps a copy of relevant `MapChunk` data in GPU memory. This cache
keeps recently accessed chunks in GPU memory and will move data back to CPU memory once the cache is full and new
chunks need to be modified. Changes in CPU will mark the chunk as dirty and the `GpuCache` will upload the updated
data from CPU, however there is no resolution mechanism for merging simultaneous changes on CPU and GPU.

For optimal performance the number of rays given to each `GpuMap::integrateRays()` call may
need to be tuned to the current platform. Batch sizes of 2048 or 4096 are recommended. Small batch sizes will be
slower than performing the same update in CPU.

Note that the `GpuMap` implementation will update the `VoxelMean` for maps which have a `VoxelMean` layer.

## GPU NDT Map

The `GpuNdtMap` extends the `GpuMap` adding the NDT semantics to the GPU update. This `GpuNdtMap` must be used in place
of the `GpuMap` object if NDT semantics are required. The NDT update is notably
more expensive than the base GPU update.

```cpp
void populateGpuNdtMap(DataProvider &provider)
{
  ohm::OccupancyMap map(0.1);   // Create a map
  ohm::GpuNdtMap ndt_map(&map); // Create GPU map with NDT support
  std::vector<glm::dvec3> rays; // Ray set storage.

  // While data are available.
  while (provider.nextBatch(rays))
  {
    // Add data to the map.
    gpu_map.integrateRays(rays.data(), rays.size());
  }

  // Synchronise GPU data back to the CPU map memory.
  gpu_map.syncVoxels();
}
```


================================================
FILE: docs/docutils.md
================================================
<!--
Copyright (c) 2021
Commonwealth Scientific and Industrial Research Organisation (CSIRO)
ABN 41 687 119 230

Author: Kazys Stepanas
-->

<!-- Use details section to partly hide doxygen specific details of the page. -->
<details><summary></summary>
@page docutils OHM utilities
</details>

# OHM utilities

OHM includes a number of command line utilities supporting generating and manipulating ohm map files. This page provides
high level information about the available utilities, their primary purpose and general usage.

## ohmpop\[cpu,cuda,ocl\]

The `ohmpop` - "ohm populate" - utilities are used to generate ohm map files from a point cloud and corresponding
trajectory file. There are three versions of this utility which mostly share the same command line options. The GPU
enabled versions support some additional options to control GPU related settings.

- `ohmpopcpu` the CPU implementation of ohm map generation
- `ohmpopcuda` a CUDA enabled version of the algorithm. Only present when building with CUDA libraries.
- `ohmpopocl` an OpenCL enabled version of the algorithm. Only present when building with OpenCL libraries.

The input point cloud is expected to be the results of a SLAM scan after global optimisation and loop closure and
requires each point to be correctly timestamped in order to correlate the point against the trajectory file. The
trajectory file is used to identify the scanner position corresponding to each sample.

Supported inputs are as follows:

- Point cloud:
  - Supports LAS/LAZ when ohm is built against `libLAS` and `lasZIP`.
  - Supports many point cloud formats when built against PDAL. Precise formats depend on the PDAL support.
- Trajectory file:
  - Supports a text file format (see below).
  - Supports the same point cloud formats as the input cloud.

The text file trajectory format supports an optional "headings" line as the first line of the file after which each line
must be of the following form:

| Field       | Description                                                                |
| ----------- | -------------------------------------------------------------------------- |
| Timestamp   | Floating point timestamp value in the same time base as the point cloud    |
| X           | X coordinate for the sensor position, in the same frame as the point cloud |
| Y           | Y coordinate for the sensor position, in the same frame as the point cloud |
| Z           | X coordinate for the sensor position, in the same frame as the point cloud |
| q0          | Quaternion W component for the sensor rotation (unused)                    |
| q1          | Quaternion X component for the sensor rotation (unused)                    |
| q2          | Quaternion Y component for the sensor rotation (unused)                    |
| q3          | Quaternion Z component for the sensor rotation (unused)                    |
| user fields | Additional fields (optional, ignored)                                      |

For each sample point, `ohmpop` tries to match a corresponding trajectory interval in the trajectory file and
interpolates the scanner pose between the nearest trajectory samples.

`ohmpop` generates a `.ohm` file for the map. By default it generates only the occupancy map layer, but command line
options may be used to generate the voxel mean layer and covariance (normal distribution transforms or ndt) layer. A ply
representation of the ohm map is also generated by default which contains a single point per occupied voxel.

## ohminfo

A basic utility for displaying the content of a `.ohm` map file. Most ohm algorithms add meta data about how the map was
generated which are included in the `ohminfo` display.

## ohm2ply

A conversion utility which extracts data from an ohm map into a PLY point cloud file. `ohm2ply` supports different
colourisation modes - e.g., colour by height - as well as different data extraction modes.

| ohm2ply mode     | Description                                                                                              |
| ---------------- | -------------------------------------------------------------------------------------------------------- |
| occupancy        | (default) Extract a single point per occupied voxel preferring voxel mean over voxel centre if available |
| occupancy-centre | Same as `occupancy`, but always uses the voxel centre to position points                                 |
| covariance       | Uses the covariance/ndt layer to export polygonal ellipsoids from an ndt based map                       |
| clearance        | Extract the `clearance` layer colouring points by proximity to an occupied voxel (experimental)          |
| density          | Use the voxel density mode as generated by `ohmpop[X] --traversal`                                       |
| heightmap        | Extract points from a heightmap map file (see `ohmheightmap`). Uses heights from the heightmap layer     |
| heightmap-mesh   | Meshes a single layer heightmap into a polygonal PLY file (experimental)                                 |

Additionally the `--voxel-mode` modifies how voxels are exported. This has no effect when using `--mode=covariance`.


| ohm2ply voxel mode | Description                      |
| ------------------ | -------------------------------- |
| points             | (default) extract a point cloud. |
| voxel              | Extract a cube per voxel.        |

## ohmheightmap

Generates a heightmap from an ohm map file. The output is another ohm map file with a heightmap data layer. Supports a
number of heightmap generation methods including generating a multi-layer heightmap.

| ohmheightmap mode | Description                                                                                              |
| ----------------- | -------------------------------------------------------------------------------------------------------- |
| planar            | Simple planar walk, searching for a ground voxel within a band across the map. Generates a single layer. |
| fill              | A flood fill based extension of the planar technique. Generates a single layer.                          |
| layered           | Generates a multi-layer heightmap. Columns are sorted in ascending height order.                         |
| layered-unordered | Generates a multi-layer heightmap. Columns are unsorted.                                                 |

## ohmfilter

Filters an input point cloud against an ohm map rejecting points which do not fall within occupied voxels in the
occupancy map.

## ohmprob

A simple utility which converts a floating point value between an occupancy probability and an occupancy value (log
space). This uses the same algorithm used in ohm map occupancy calculations.

## ohmhm2img (experimental)

Converts an ohm heightmap file into a png image. This is an experimental utility and does not support layered
heightmaps.

## ohmquery\[cuda,ocl\] (experimental)

Supports running experimental ohm query algorithms - `OhmLineQuery`, `OhmNearestNeighbours`.


================================================
FILE: docs/docvoxellayers.md
================================================
<!--
Copyright (c) 2020
Commonwealth Scientific and Industrial Research Organisation (CSIRO)
ABN 41 687 119 230

Author: Kazys Stepanas
-->

<!-- Use details section to partly hide doxygen specific details of the page. -->
<details><summary></summary>
@page docvoxellayers Built in voxel layers
</details>

# Built in voxel layers

There are a number of "built in" or "default" supported OHM voxels layers. These layers are known within the OHM library and in some cases are in algorithms unrelated to the voxel calculations which generated them. For example, occupancy and TSDF layers may both be used to identify the `MapChunk::first_valid_index`, which marks the first observed voxel in a `MapChunk`. The default OHM layers are listed below.

| Layer               | Internal structure      | Byte size | Description                                                                       |
| ------------------- | ----------------------- | --------- | --------------------------------------------------------------------------------- |
| `occupancy`         | `float`                 | 4         | Log odds, probabilistic occupancy value.                                          |
| `mean`              | `VoxelMean`             | 8         | Sub voxel mean positioning.                                                       |
| `traversal`         | `float`                 | 4         | Voxel distance traversal used to support the density model.                       |
| `covariance`        | `CovarianceVoxel`       | 24        | Triangular square root matrix, voxel covariance storage.                          |
| `intensity`         | `IntensityMeanCov`      | 8         | Intensity mean and covariance for traversability mode (NDT-TM)                    |
| `hit_miss_count`    | `HitMissCount`          | 8         | Number of hits and misses on a voxel for NDT-TM                                   |
| `touch_time`        | `uint32_t`              | 4         | Approximate, quantised last update time (ms) of a voxel (not supported for TSDF). |
| `incident_normal`   | `uint32_t`              | 4         | Quantised, packed approximate normal of the average ray incident for a voxel.     |
| `tsdf`              | `VoxelTsdf`             | 8         | Truncated signed distance fields voxel (experimental).                            |
| `clearance`         | `float`                 | 4         | Distance to nearest occupied voxel (experimental).                                |
| `secondary_samples` | `VoxelSecondarySamples` | 8         | Stats on secondary samples (lidar dual returns) falling in a voxel.               |

## Example memory requirements

Some algorithms use layers independently - e.g., pure occupancy - while others algorithms introduce layer codependencies - e.g., NDT-OM requires occupancy, mean and covariance. This can impose significant memory burdens in large maps. For offline processing, the `VoxelBlockCompressionQueue` can be enabled to compress least recently used voxel blocks, based on memory watermark levels.

The expectation for realtime processing is that only a local map is maintained, and may need to be sized such that all layers can fit in the `GpuCache`. Below are some examples of approximate memory requirements based on the local region of interest, voxel resolution and voxel algorithm. Values are given in mibibytes (MiB) or gibibytes (GiB).

| Dimensions (m) | Resolution |    OCC |    NDT |   TSDF |
| -------------- | ---------: | -----: | -----: | -----: |
| 20x20x10       |       0.2m | 1.9MiB |  17MiB | 3.8MiB |
| 20x20x20       |       0.2m | 3.8MiB |  34MiB | 7.6MiB |
| 20x20x10       |       0.1m |  15MiB | 137MiB |  31MiB |
| 20x20x20       |       0.1m |  31MiB | 275MiB |  61MiB |
| 40x40x20       |       0.1m | 122MiB | 1.1GiB | 244MiB |
| 40x40x40       |       0.1m | 244MiB | 2.1GiB | 488MiB |


# Layer population

This section describes how to populate each of the default layers. Using a layer first requires the layer is present in a map.

## Creating layers

Layers can be created:

1. using `MapFlags` on construction,
2. using `OccupancyMap` functions which add known layers, or
3. updating the layout manually.

`MapFlags` contains a value for all of the default layers which can be composed as a constructor argument:

```c++
// Construct a map with occupancy, mean, traversal and TSDF layers.
ohm::OccupancyMap map(0.1, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal | ohm::MapFlag::Tsdf);
```

Note the occupancy map layer is implied, except with TSDF is the *only* layer requested. That is, the occupancy layer is created when;

- No `MapFlag` layer values are specified.
- An `MapFlag` layer other than `MapFlag::Tsdf` is specified regardless of whether `MapFlag::Tsdf` is specified.

However, the occupancy layer is not created when only `MapFlag::Tsdf` is specified.

The following functions of `OccupancyMap` may be used to add layers after a map has been created:

- `OccupancyMap::addVoxelMeanLayer()`
- `OccupancyMap::addTraversalLayer()`
- `OccupancyMap::addTouchTimeLayer()`
- `OccupancyMap::addIncidentNormalLayer()`
- `OccupancyMap::addLayer()` - generic helper

These functions perform similar code which is equivalent to updating the layout manually as shown below.

```c++
ohm::OccupancyMap map(0.1);

// Add tsdf layer.
if (map.layout().layerIndex(ohm::default_layer::tsdfName()) == -1)
{
  // Layer not already present.
  // Copy the layout.
  ohm::MapLayout layout = map.layout();
  // Add the Tsdf layer.
  ohm::addTsdf(layout);
  // Apply the updated layout back to the map.
  map.updateLayout(layout);
}
```

## Populating layers

Most layers are populated using the either `RayMapperOccupancy` and `RayMapperNdt`, or the equivalent GPU implementations - `GpuMap` and `GpuNdtMap` respectively. The table below shows which layers are populated by which `RayMapper` implementations. Some layers are optional and are populate if present, other layers are required. Using a `RayMapper` without the required layers present leads to undefined behaviour. Optional layers are shown in square brackets `[]` Note that `GpuMap` is a derivation of `RayMapper`.

| `RayMapper`                         | Affected layers                                                                               |
| ----------------------------------- | --------------------------------------------------------------------------------------------- |
| `RayMapperOccupancy`                | `occupancy`, `[mean]`, `[traversal]`, `[touch_time]`, `[incident_normal]`                     |
| `GpuMap`                            | Equivalent to `RayMapperOccupancy`                                                            |
| `RayMapperNdt` occupancy model      | `occupancy`, `mean`, `covariance,`, `[traversal]`, `[touch_time]`, `[incident_normal]`        |
| `RayMapperNdt` traversability model | As occupancy model, plus: `intensity`, `hit_miss_count`. Requires rays with intensity values. |
| `GpuNdtMap`                         | Equivalent as `RayMapperNdt`                                                                  |
| `RayMapperTsdf`                     | `tsdf`                                                                                        |
| `GpuTsdfMap`                        | Equivalent to `RayMapperTsdf`                                                                 |
| `RayMapperSecondarySample`          | `secondary_samples` Has no GPU equivalent.                                                    |

Note there is no `RayMapper` which affects the `clearance` layer. Instead, use `ClearanceProcess` in `ohmgpu`. This is an experimental feature and is not considered sufficiently performance for general use.


================================================
FILE: docs/gpu/docgpualgorithm.md
================================================
<!--
Copyright (c) 2022
Commonwealth Scientific and Industrial Research Organisation (CSIRO)
ABN 41 687 119 230

Author: Kazys Stepanas

-->

<!-- Use details section to partly hide doxygen specific details of the page. -->
<details><summary></summary>
@page docgpualgorithm GPU algorithm overview
</details>

# GPU algorithm overview

There are two parts of interest to highlight in the high level GPU occupancy algorithm;

- GPU thread algorithm
- CPU cache management

# GPU thread algorithm

OHM GPU threads are used such that there is one thread per input ray. Each GPU thread traces its voxels using a 3D
adaptation of <em>"A Fast Voxel Traversal Algorithm for Ray Tracing", John Amanatides and Andrew Woo</em>. This
algorithm steps a single voxel on each iteration selecting the step direction based on the largest remaining distance
to cover on each axis. Using this algorithm on GPU introduces contention when updating voxels, which is resolved using
atomic Compare and Swap (CAS) semantics.

Below is a high level pseudo code listing of the OHM GPU ray algorithm.

```cpp
struct float3
{
  float x, y, z;
};

struct GpuKey
{
  short region_key[3];
  unsigned char voxel_key[4]; // Index 3 is used as a marker bit (not show below).
};

// voxel_occupancies : Voxels for the regions intersecting the input rays
// rays : Start/end point pairs for the sample rays to process
// occupied_occupancy_adjustment : occupancy adjustment to make for the end voxel.
// free_occupancy_adjustment : occupancy adjustment to make for voxels other than the end voxel.
void traceRay(float *voxel_occupancies, float3 *rays,
              float occupied_occupancy_adjustment, float free_occupancy_adjustment)
{
  // Initialise the ray
  float3 ray_start = rays[thread_index * 2];
  float3 ray_end = rays[thread_index * 2 + 1];
  GpuKey end_voxel = getVoxel(ray_end);
  GpuKey start_voxel = getVoxel(ray_start);
  GpuKey current_voxel = start_voxel;

  // Walk the ray
  bool done = false;
  while (!done)
  {
    // Calculate the occupancy adjustment.
    float occupancy_adjustment = 0;
    if (current_voxel == end_voxel)
    {
      occupancy_adjustment = occupied_occupancy_adjustment;
    }
    else
    {
      occupancy_adjustment = free_occupancy_adjustment;
    }

    // Update the value using CAS, looping to avoid contention.
    // Note: the implementation imposes a hard limit on the maximum number of attempts to be made.
    bool updated = false;
    while (!updated)
    {
      // Note: the implementation of voxelIndex() is quite intricate (see below)
      int voxel_index = voxelIndex(current_voxel, voxel_occupancies)
      float expected_value = voxel_occupancies[voxel_index];
      float new_value = expected_value + occupancy_adjustment;
      updated = atomicCAS(&voxel_occupancies[voxel_index], expected_value, new_value);
    }

    // Walk the next voxel.
    done = current_voxel == end_voxel;
    current_voxel = walkNextVoxel(current_voxel, start_voxel, end_voxel);
}
```

The actual implementation differs significantly in its details - for example OHM supports reverse ray tracing while
the listing shows forward ray tracing and more variables are required to track the ray tracing. Another hidden detail is
that `voxelIndex()` appears to be a trivial operation in the listing above, but its actual implementation is fairly
complex. The CPU uploads voxel regions required for the update into `voxel_occupancies`. This is a single, flat array of
voxels, but contains data for multiple regions, though each region's voxels are in a contiguous block. To resolve
`current_voxel` key to an index, we must first calculate the offset of `current_voxel->region_key` into
`voxel_occupancies` using a lookup table, which is traversed using a linear search, then the `current_voxel->voxel_key`
can be simply converted from a 3D index into a 1D index. In practice, we cache the last accessed `region_key` and
corresponding offset to reduce the number of linear searches performed as rays will generally access multiple voxels
from the same region in sequence.

NDT uses a more involved calculation for the `free_occupancy_adjustment` and defers the `occupied_occupancy_adjustment`
to a second phase. The second, occupancy phase is required to avoid contention when updating a voxel's covariance
matrix. While the occupancy value is independent and can be updated using CAS, the covariance matrix consist of
multiple, codependent values which must be updated together.

The NDT occupancy phase launches one thread per voxel ray as well, however, rays are pre-sorted (on CPU) such that all
rays ending at the same sample voxel are collated. Thus multiple NDT occupancy threads may be launched for a single end
voxel (one per sample), however, only the first thread is allowed to update the voxel occupancy, covariance matrix, mean
or other values. The remaining threads targetting the same voxel are essentially wasted.

While this is inefficient, this has proven effective in updating NDT occupancy in GPU without needing to involve the
CPU for this phase.

Note that the earliest implementation of OHM occupancy launched one thread per voxel in a region with each voxel
checking which rays intersect it. This proved very slow as most GPU threads performed no work. While the ray solution
introduces contention and prevents the use of local memory caching, it is significantly faster than the per voxel thread
algorithm and significantly faster than the single threaded CPU algorithm.

# CPU cache management

Before updating voxels in GPU, the CPU must ensure that the required voxel regions are uploaded to the GPU. The regions
to be uploaded are identified by performing the same line walking algorithm for each ray in CPU, however, we do so at
the much coarser region resolution. Most rays only touch a handful of regions. The intersected regions are added to the
appropriate `GpuLayerCache`.


================================================
FILE: docs/gpu/docgpudetail.md
================================================
<!--
Copyright (c) 2022
Commonwealth Scientific and Industrial Research Organisation (CSIRO)
ABN 41 687 119 230

Author: Kazys Stepanas
-->

<!-- Use details section to partly hide doxygen specific details of the page. -->
<details><summary></summary>
@page docgpudetail GPU technical details and performance tips
</details>

# GPU technical details and performance tips

This page outlines some more technical details how the OHM library uses the GPU.

# GPU vs CPU

The OHM library has been specifically authored to support GPU execution of voxel occupancy type algorithms using GPU
threads rather than focusing on multi-threaded CPU algorithms to update such voxel maps. While GPUs excel at solving
some "embarrassingly parallel" problems, the occupancy map update does not fall into this category and the GPU
adaptation of the occupancy algorithms does not represent an ideal GPU problem. However, it has been demonstrated that
these GPU algorithms do perform significantly better than single threaded CPU algorithms.

There are several things to be aware of with respect to how the GPU algorithms operate and how the results may differ
from the single threaded CPU implementations.

- GPU memory contention
- Non determinism
- Single precision operations
- Syncing between GPU and CPU memory

## GPU memory contention

The OHM GPU algorithm is specifically written to allow memory contention when updating voxels. This contention is
resolved by using atomic operations; primarily compare and swap operations (CAS) and atomic increment. The
general design pattern for using CAS is to attempt to update voxel memory and note whether the update was successful or
not. Unsuccessful update attempts are retried until they succeed, up to a hard coded limit to prevent excessive waiting.

This is a less than ideal uses of GPU code. There is in fact an inherent increase in the likelihood of CAS failures in
GPU because because all threads in a GPU warp (CUDA terminology) or work group (OpenCL terminology) will execute the
same instruction at the same time. Thus CAS failures are practically guaranteed in cases where multiple threads target
the same voxels.

One technique used to mitigate contention is to trace rays in reverse, from sample point back to sensor (this is enabled
by default). Consider a case where multiple rays of varying lengths originate from the same sensor location. The dense
ray packing near the sensor, coupled with the discrete nature of voxels will see increased contention when forward
tracing from the sensor location. Reverse tracing mitigates this as the ray length variation will see those same voxels
reach by different threads at different times.

It is also worth noting that this update using CAS requires we write directly to GPU global memory, thus we cannot gain
the benefits of faster GPU local memory.

## Non determinism

Once a work load has been has been queued, it is up to the GPU driver to manage execution. The driver may submit work
items/warps as it sees fit. This coupled with the non deterministic nature of CAS means that there is no way to generate
a deterministic execution order while still maintaining parallelism. Thus GPU map results will vary and may be different
to maps generated by single threaded CPU algorithms.

Additionally, the use of single precision calculations on GPU vs double precision in CPU, will result in different
rounding errors when tracing rays near the edges of voxels.

However, the nature of occupancy maps is that they are statistical, probabilistic approximations, thus the results
should be comparable.

## Single precision operations

Double precision maths has been supported in CUDA for some time. However, double precision is an extension in OpenCL. We
choose to support a wider range of GPU accelerators by using single precision maths. In order to achieve this, rays are
converted to single precision before uploading to GPU memory. Specifically, each ray is converted to single precision
ray in a coordinate frame centred on the voxel containing the ray sample, or end point. This ensures overall precision
in a large maps should not be lost.

# CUDA vs OpenCL

OHM uses a GPU abstraction API to wrap either CUDA or OpenCL. The runtime code is then written in OpenCL style, with
support functions added to convert to CUDA. This allows the same code to compile and run with either API.

There are some differences between the two APIs and performance differences should be expected between CUDA and OpenCL
running on the same hardware. CUDA should generally perform better than OpenCL on the same GPU, however, NVIDIA's recent
addition of OpenCL 3.0 has reduced the difference.

OpenCL support is designed to cover a number of OpenCL standards; 1.2, 2.0, 3.0. However, OpenCL support across GPU
hardware can vary. Code which compiles on one card may fail to compile for another hardware manufacturer. Alternatively,
some algorithms may not be supported. For example, the TSDF implementation requires 64-bit atomic operations
(specifically CAS). It is recommended that OHM be tested extensively on the target hardware.

# Performance

As noted above, CUDA performance is generally better than OpenCL. There are also a number of data factors which may
affect performance and algorithm options which can be used to improve performance.

## Ray length

Long rays will always take longer to process in both CPU and GPU. However, in GPU there is an added complication of
redundant threads. Consider a warp which contains a number of very short rays and one very long ray. The time to execute
the warp is determined by the longest rays with other threads effectively idle for the additional voxels touched by the
long ray.

Two options can be used to reduce the impact of inconsistent ray lengths. Below we describe the API function to set the
option as well as the equivalent command line option for `ohmpopcuda` or `ohmpopocl`.

- `GpuMap::setRaySegmentLength()` (`--gpu-ray-segment-length`) sets a threshold above which the CPU will segment a
  long ray into multiple smaller GPU work items.
- `GpuMap::setRayFilter()` may be used to install a filter function which can modify rays before they are uploaded to
  the GPU. A number of common filter functions are available in `RayFilter.h` such as `clipRayFilter()`
  (`--ray-length-max`).

## GPU Cache

The GPU maintains a fixed size cache for voxel data. Voxel regions are uploaded from CPU to GPU as required. Once the
cache is full, voxel regions are returned back to the CPU to make space. This incurs an IO cost. A small cache can
result in cache thrashing between batches of rays and severely impact performance.

The cache size may be set when constructing a `GpuMap` via the `gpu_mem_size` argument. This is exposed to the `ohmpop`
command line as `--gpu-cache-size`.

Consider the following when choosing a cache size:

- The use of a local or global map? A local map requires less memory as regions can be dropped outside some local
  spatial region (such as when running on a sensing payload).
- The voxel layers in use and the region size. The `MapLayout` defines the `MapLayer` definitions and the number of
  bytes per region is calculated by `MapLayer::layerByteSize()`.


================================================
FILE: gputil/CMakeLists.txt
================================================
include(GenerateExportHeader)

if(NOT OHM_FEATURE_CUDA AND NOT OHM_FEATURE_OPENCL)
  message(FATAL_ERROR "Neither OHM_FEATURE_CUDA nor OHM_FEATURE_OPENCL selected for build. A GPU SDK is required to build gputil.")
endif(NOT OHM_FEATURE_CUDA AND NOT OHM_FEATURE_OPENCL)

if(OHM_FEATURE_CUDA AND OHM_USE_DEPRECATED_CMAKE_CUDA)
  # Need to find CUDA for deprecated project configuration
  find_package(CUDA)
endif(OHM_FEATURE_CUDA AND OHM_USE_DEPRECATED_CMAKE_CUDA)

set(GPUTIL_TYPE_OPENCL 1)
set(GPUTIL_TYPE_CUDA 2)

set(PUBLIC_HEADERS
  gpu_ext.h
  gpuApiException.h
  gpuBuffer.h
  gpuConfig.in.h
  gpuDevice.h
  gpuDeviceInfo.h
  gpuEvent.h
  gpuEventList.h
  gpuException.h
  gpuKernel.h
  gpuPinMode.h
  gpuPinnedBuffer.h
  gpuPlatform.h
  gpuProgram.h
  gpuQueue.h
  gpuThrow.h
  gpuVersion.h
  "${CMAKE_CURRENT_BINARY_DIR}/gputil/gputilExport.h"
  "${CMAKE_CURRENT_BINARY_DIR}/gputil/gpuConfig.h"
)

set(SOURCES
  gpuApiException.cpp
  gpuConfig.in.h
  gpuEventList.cpp
  gpuException.cpp
  gpuThrow.cpp
)

list(APPEND SOURCES ${PUBLIC_HEADERS})

function(_gputil_setup_library TARGET_NAME GPUTIL_TYPE)
  target_include_directories(${TARGET_NAME}
    PUBLIC
      $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}>
      $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}/gputil>
      $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/gputil>
      $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
      $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}>
  )

  if(GPU_INCLUDE_DIRS)
    target_include_directories(${TARGET_NAME} SYSTEM PRIVATE "${GPU_INCLUDE_DIRS}")
  endif(GPU_INCLUDE_DIRS)

  # Select GPUTIL_TYPE
  target_compile_definitions(${TARGET_NAME} PUBLIC "-DGPUTIL_TYPE=${GPUTIL_TYPE}")
  # Because of the way we compile our GPU library twice with different names, we must explicitly define the export
  # macro. Curiously, there's a way to overide all the macros except the one used to control whether to export the
  # symbols or not. This puts us in a position where it could either be ohmcuda_EXPORTS or ohmocl_EXPORTS depending
  # on which targets are enabled. We build both the same way though, so define both symbols for all builds.
  target_compile_definitions(${TARGET_NAME} PRIVATE "-Dgputilcuda_EXPORTS" "-Dgputilocl_EXPORTS")

  # set_property(TARGET gputil PROPERTY DEBUG_POSTFIX "d")

  generate_export_header(${TARGET_NAME}
    EXPORT_MACRO_NAME gputilAPI
    DEPRECATED_MACRO_NAME gputil_DEPRECATED
    NO_EXPORT_MACRO_NAME gputil_NO_EXPORT
    NO_DEPRECATED_MACRO_NAME gputil_NO_DEPRECATED
    EXPORT_FILE_NAME gputil/gputilExport.h
    STATIC_DEFINE GPUTIL_STATIC)

  install(TARGETS ${TARGET_NAME} EXPORT ${CMAKE_PROJECT_NAME}-config-targets
    LIBRARY DESTINATION lib
    ARCHIVE DESTINATION lib
    RUNTIME DESTINATION bin
    INCLUDES DESTINATION include
  )
endfunction(_gputil_setup_library)

configure_file(gpuConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/gputil/gpuConfig.h")

if(OHM_FEATURE_OPENCL)
  set(SOURCES_OPENCL
    cl/gpuApiExceptionCode.cpp
    cl/gpuBuffer.cpp
    cl/gpuBufferDetail.h
    cl/gpuDevice.cpp
    cl/gpuDeviceDetail.h
    cl/gpuEvent.cpp
    cl/gpuEventDetail.h
    cl/gpuKernel.cpp
    cl/gpuKernel2.h
    cl/gpuKernelDetail.h
    cl/gpuPinnedBuffer.cpp
    cl/gpuPlatform2.h
    cl/gpuProgram.cpp
    cl/gpuProgramDetail.h
    cl/gpuQueue.cpp
    cl/gpuQueueDetail.h
    )

  set(PUBLIC_HEADERS_OPENCL
    cl/gpuBufferDetail.h
    cl/gpuDeviceDetail.h
    cl/gpuKernel2.h
    cl/gpuKernelDetail.h
    cl/gpuPlatform2.h
    cl/gpuProgramDetail.h
    cl/gpuQueueDetail.h
  )

  add_library(gputilocl ${SOURCES} ${SOURCES_OPENCL})
  clang_tidy_target(gputilocl EXCLUDE_MATCHES ".*\.cl")
  target_include_directories(gputilocl PRIVATE
      $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}/cl>
  )

  target_link_libraries(gputilocl PUBLIC clu)
  _gputil_setup_library(gputilocl ${GPUTIL_TYPE_OPENCL})
endif(OHM_FEATURE_OPENCL)

if(OHM_FEATURE_CUDA)
  set(SOURCES_CUDA
    cuda/cutil_atomic.h
    cuda/cutil_decl.h
    cuda/cutil_importcl.h
    cuda/cutil_math.h
    cuda/gpuApiExceptionCode.cpp
    cuda/gpuBuffer.cpp
    cuda/gpuBufferDetail.h
    cuda/gpuDevice.cpp
    cuda/gpuDeviceDetail.h
    cuda/gpuEvent.cpp
    cuda/gpuEventDetail.h
    cuda/gpuKernel.cpp
    cuda/gpuKernel2.h
    cuda/gpuKernelDetail.h
    cuda/gpuMemRegion.cpp
    cuda/gpuMemRegion.h
    cuda/gpuPinnedBuffer.cpp
    cuda/gpuPlatform2.h
    cuda/gpuProgram.cpp
    cuda/gpuProgramDetail.h
    cuda/gpuQueue.cpp
    cuda/gpuQueueDetail.h
    cuda/ref.h
    )

  set(PUBLIC_HEADERS_CUDA
    cuda/cutil_atomic.h
    cuda/cutil_decl.h
    cuda/cutil_importcl.h
    cuda/cutil_math.h
    cuda/gpuBufferDetail.h
    cuda/gpuDeviceDetail.h
    cuda/gpuEventDetail.h
    cuda/gpuKernel2.h
    cuda/gpuKernelDetail.h
    cuda/gpuMemRegion.h
    cuda/gpuPlatform2.h
    cuda/gpuProgramDetail.h
    cuda/gpuQueueDetail.h
    cuda/ref.h
  )

  if(OHM_USE_DEPRECATED_CMAKE_CUDA)
    cuda_add_library(gputilcuda ${SOURCES} ${SOURCES_CUDA})
    target_include_directories(gputilcuda SYSTEM PUBLIC "${CUDA_INCLUDE_DIRS}")
  else(OHM_USE_DEPRECATED_CMAKE_CUDA)
    add_library(gputilcuda ${SOURCES} ${SOURCES_CUDA})
    target_link_libraries(gputilcuda PUBLIC "${OHM_CUDA_LIBRARY}")
  endif(OHM_USE_DEPRECATED_CMAKE_CUDA)
  target_link_libraries(gputilcuda PUBLIC logutil)

  clang_tidy_target(gputilcuda EXCLUDE_MATCHES ".*\.cl")
  target_include_directories(gputilcuda SYSTEM
    PRIVATE
      $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}/cuda>
  )
  _gputil_setup_library(gputilcuda ${GPUTIL_TYPE_CUDA})
endif(OHM_FEATURE_CUDA)

install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/gputil)
if(PUBLIC_HEADERS_CUDA)
  install(FILES ${PUBLIC_HEADERS_CUDA} DESTINATION ${OHM_PREFIX_INCLUDE}/gputil/cuda)
endif(PUBLIC_HEADERS_CUDA)
if(PUBLIC_HEADERS_OPENCL)
  install(FILES ${PUBLIC_HEADERS_OPENCL} DESTINATION ${OHM_PREFIX_INCLUDE}/gputil/cl)
endif(PUBLIC_HEADERS_OPENCL)

source_group("source" REGULAR_EXPRESSION ".*$")
source_group("source\\cl" REGULAR_EXPRESSION "/cl/.*$")
source_group("source\\cuda" REGULAR_EXPRESSION "/cuda/.*$")


================================================
FILE: gputil/cl/gpuApiExceptionCode.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuApiException.h"

#include <clu/clu.h>

namespace gputil
{
const char *ApiException::errorCodeString(int error_code)
{
  return clu::errorCodeString(error_code);
}
}  // namespace gputil

================================================
FILE: gputil/cl/gpuBuffer.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "gputil/gpuBuffer.h"

#include "gpuApiException.h"
#include "gpuBufferDetail.h"
#include "gpuDeviceDetail.h"
#include "gpuQueueDetail.h"

#include "cl/gpuEventDetail.h"

#include <clu/clu.h>
#include <clu/cluBuffer.h>

#include <algorithm>
#include <cinttypes>
#include <cstring>

#define USE_PINNED 1

namespace
{
/// Helper function to select either the command queue from @p explicitQueue or the default command queue for @p
/// device.
/// @param device The device holding the default command queue.
/// @param explicit_queue The preferred command queue. May be null.
inline cl_command_queue selectQueue(gputil::Device &device, gputil::Queue *explicit_queue)
{
  if (explicit_queue)
  {
    return explicit_queue->internal()->queue();
  }
  return device.detail()->default_queue.internal()->queue();
}


size_t resizeBuffer(gputil::Buffer &buffer, gputil::BufferDetail &imp, size_t new_size, bool force)
{
  const size_t initial_size = buffer.actualSize();
  size_t best_size = clu::bestAllocationSize(imp.device.detail()->context, new_size);
  if ((!force && initial_size >= best_size) || (force && initial_size == best_size))
  {
    imp.requested_size = new_size;
    return best_size;
  }

  // Needs resize.
  imp.buffer = cl::Buffer();

  cl_mem_flags cl_flags = 0;

  if (imp.flags & gputil::kBfRead)
  {
    if (imp.flags & gputil::kBfWrite)
    {
      cl_flags = CL_MEM_READ_WRITE;  // NOLINT(hicpp-signed-bitwise)
    }
    else
    {
      cl_flags = CL_MEM_READ_ONLY;  // NOLINT(hicpp-signed-bitwise)
    }
  }
  else if (imp.flags & gputil::kBfWrite)
  {
    cl_flags = CL_MEM_WRITE_ONLY;  // NOLINT(hicpp-signed-bitwise)
  }

  if (imp.request_flags & gputil::kBfHostAccess)
  {
    cl_flags = CL_MEM_ALLOC_HOST_PTR;  // NOLINT(hicpp-signed-bitwise)
  }

  cl_int clerr = 0;
  clu::ensureBufferSize<uint8_t>(imp.buffer, cl_flags, imp.device.detail()->context, new_size, &clerr);
  GPUAPICHECK(clerr, CL_SUCCESS, 0);

  // Validate the CL_MEM_ALLOC_HOST_PTR flag worked.
  cl_mem_flags actual_flags = 0;
  clGetMemObjectInfo(imp.buffer(), CL_MEM_FLAGS, sizeof(actual_flags), &actual_flags, nullptr);

  imp.flags = imp.request_flags;
  // NOLINTNEXTLINE(hicpp-signed-bitwise)
  if ((imp.request_flags & gputil::kBfHostAccess) && !(actual_flags & CL_MEM_ALLOC_HOST_PTR))
  {
    // Failed to allocate in host memory.
    imp.flags &= ~gputil::kBfHostAccess;
    // std::cout << "Failed host access " << std::endl;
  }

  imp.requested_size = new_size;
  const size_t actual_size = buffer.actualSize();

  return actual_size;
}


size_t actualSize(const gputil::BufferDetail &imp)
{
  size_t buffer_size = 0;
  if (imp.buffer())
  {
    clGetMemObjectInfo(imp.buffer(), CL_MEM_SIZE, sizeof(buffer_size), &buffer_size, nullptr);
  }
  return buffer_size;
}
}  // namespace


namespace gputil
{
uint8_t *pin(BufferDetail &imp, PinMode mode)
{
  cl_command_queue queue_cl = selectQueue(imp.device, nullptr);
  cl_map_flags map_flags = 0;

  switch (mode)
  {
  default:
  case kPinNone:
    break;
  case kPinRead:
    map_flags = CL_MAP_READ;  // NOLINT(hicpp-signed-bitwise)
    break;
  case kPinWrite:
    map_flags = CL_MAP_WRITE;  // NOLINT(hicpp-signed-bitwise)
    break;
  case kPinReadWrite:
    map_flags = CL_MAP_READ | CL_MAP_WRITE;  // NOLINT(hicpp-signed-bitwise)
    break;
  }

  cl_int clerr = CL_SUCCESS;
  auto *pinned_ptr = static_cast<uint8_t *>(
    clEnqueueMapBuffer(queue_cl, imp.buffer(), CL_TRUE, map_flags, 0, actualSize(imp), 0, nullptr, nullptr, &clerr));
  //  &event, &clerr);
  GPUAPICHECK(clerr, CL_SUCCESS, nullptr);

  return pinned_ptr;
}


void unpin(BufferDetail &imp, void *pinned_ptr, Queue *explicit_queue, Event *block_on, Event *completion)
{
  if (completion)
  {
    completion->release();
  }

  if (pinned_ptr)
  {
    cl_command_queue queue_cl = selectQueue(imp.device, explicit_queue);
    cl_int clerr = CL_SUCCESS;

    cl_event event{};
    cl_event *event_ptr = (!explicit_queue || completion) ? &event : nullptr;
    int block_on_count = (block_on && block_on->isValid()) ? 1 : 0;
    cl_event block_on_ocl = (block_on_count) ? block_on->detail()->event : nullptr;

    clerr = clEnqueueUnmapMemObject(queue_cl, imp.buffer(), pinned_ptr, block_on_count,
                                    (block_on_count) ? &block_on_ocl : nullptr, event_ptr);

    GPUAPICHECK2(clerr, CL_SUCCESS);

    if (completion)
    {
      completion->detail()->event = event;
    }
    else if (event_ptr)
    {
      // Wait for DMA to complete.
      clerr = clWaitForEvents(1, event_ptr);
      GPUAPICHECK2(clerr, CL_SUCCESS);
      if (clerr != CL_SUCCESS)
      {
        GPUAPICHECK2(clerr, CL_SUCCESS);
      }
    }
  }
}

Buffer::Buffer()
  : imp_(new BufferDetail)
{}


Buffer::Buffer(const Device &device, size_t byte_size, unsigned flags)
  : imp_(new BufferDetail)
{
  create(device, byte_size, flags);
}


Buffer::Buffer(Buffer &&other) noexcept
  : imp_(other.imp_)
{
  other.imp_ = nullptr;
}


Buffer::~Buffer()
{
  delete imp_;
}


Buffer &Buffer::operator=(Buffer &&other) noexcept
{
  delete imp_;
  imp_ = other.imp_;
  other.imp_ = nullptr;
  return *this;
}


void Buffer::create(const Device &device, size_t byte_size, unsigned flags)
{
  if (isValid())
  {
    release();
  }
  imp_->device = device;
  imp_->request_flags = imp_->flags = flags;
  resize(byte_size);
}


void Buffer::release()
{
  imp_->device = Device();
  imp_->buffer = cl::Buffer();
  imp_->request_flags = imp_->flags = 0;
}


void Buffer::swap(Buffer &other) noexcept
{
  std::swap(imp_, other.imp_);
}


bool Buffer::isValid() const
{
  // Can't check _imp->buffer here as it's valid to have a zero sized buffer.
  return imp_ && imp_->device.isValid();
}


unsigned Buffer::flags() const
{
  return imp_->flags;
}


size_t Buffer::size() const
{
  return imp_->requested_size;
}

size_t Buffer::actualSize() const
{
  return ::actualSize(*imp_);
}


size_t Buffer::resize(size_t new_size)
{
  return resizeBuffer(*this, *imp_, new_size, false);
}


size_t Buffer::forceResize(size_t new_size)
{
  return resizeBuffer(*this, *imp_, new_size, true);
}


void Buffer::fill(const void *pattern, size_t pattern_size, Queue *queue, Event *block_on, Event *completion)
{
  if (completion)
  {
    completion->release();
  }

  if (isValid())
  {
    cl_int clerr = CL_SUCCESS;
    cl_command_queue ocl_queue = selectQueue(imp_->device, queue);

    // Note: clEnqueueFillBuffer() appears to be faster than memory mapping. That is, it doesn't
    // suffer the same performance issues as clEnqueueReadBuffer()/clEnqueueWriteBuffer().
    GPUAPICHECK2(clerr, CL_SUCCESS);

    const int block_on_count = (block_on && block_on->isValid()) ? 1 : 0;
    cl_event block_on_ocl = (block_on_count) ? block_on->detail()->event : nullptr;
    clerr = clEnqueueFillBuffer(ocl_queue, imp_->buffer(), pattern, pattern_size, 0, actualSize(), block_on_count,
                                (block_on_count) ? &block_on_ocl : nullptr,
                                (queue && completion) ? &completion->detail()->event : nullptr);
    GPUAPICHECK2(clerr, CL_SUCCESS);

    if (!queue || queue->internal()->force_synchronous)
    {
      // Blocking operation.
      clerr = clFinish(ocl_queue);
      GPUAPICHECK2(clerr, CL_SUCCESS);
    }
  }
}


void Buffer::fillPartial(const void *pattern, size_t pattern_size, size_t fill_bytes, size_t offset, Queue *queue)
{
  // Contrain the byte counts.
  const size_t this_buffer_size = size();
  if (offset >= this_buffer_size)
  {
    return;
  }

  fill_bytes = std::min(fill_bytes, this_buffer_size - offset);

#ifdef USE_PINNED
  if (imp_->flags & kBfHostAccess)
  {
    // Using pinned memory. Make multiple memcpy calls.
    if (uint8_t *dst_mem = gputil::pin(*imp_, kPinWrite))
    {
      size_t filled = 0u;
      for (; filled < fill_bytes; filled += pattern_size)
      {
        memcpy(dst_mem + offset + filled, pattern, pattern_size);
      }

      if (filled < fill_bytes)
      {
        // Finish the last (partial pattern) write.
        const size_t last_write_size = fill_bytes - filled;
        memcpy(dst_mem + offset + filled, pattern, last_write_size);
      }

      gputil::unpin(*imp_, dst_mem, queue);
    }
    return;
  }
#endif  // USE_PINNED

  // Not pinned memory. Make multiple writes.
  size_t filled = 0u;
  for (; filled < fill_bytes; filled += pattern_size)
  {
    write(pattern, pattern_size, offset + filled, queue);
  }

  if (filled < fill_bytes)
  {
    // Finish the last (partial pattern) write.
    size_t last_write_size = fill_bytes - filled;
    write(pattern, last_write_size, offset + filled, queue);
  }
}


size_t Buffer::read(void *dst, size_t read_byte_count, size_t src_offset, Queue *queue, Event *block_on,
                    Event *completion)
{
  // Constrain the byte counts.
  const size_t this_buffer_size = size();
  if (completion)
  {
    completion->release();
  }

  if (src_offset >= this_buffer_size)
  {
    return 0;
  }

  cl_int clerr = CL_SUCCESS;
  const size_t copy_bytes = std::min(read_byte_count, this_buffer_size - src_offset);

#if USE_PINNED
  // Can only use pinned memory for non-asynchronous transfer.
  if ((imp_->flags & kBfHostAccess) && !queue)
  {
    if (block_on && block_on->isValid())
    {
      block_on->wait();
    }

    if (uint8_t *src_mem = gputil::pin(*imp_, kPinRead))
    {
      memcpy(dst, src_mem + src_offset, copy_bytes);
      gputil::unpin(*imp_, src_mem, queue);
      return copy_bytes;
    }
  }
#endif  // USE_PINNED

  cl_command_queue ocl_queue = selectQueue(imp_->device, queue);
  int block_on_count = (block_on && block_on->isValid()) ? 1 : 0;
  cl_event block_on_ocl = (block_on_count) ? block_on->detail()->event : nullptr;

  // Non-blocking when an explicit queue has been provided.
  const cl_bool synchronous = (!queue || queue->internal()->force_synchronous) ? CL_TRUE : CL_FALSE;
  clerr = clEnqueueReadBuffer(ocl_queue, imp_->buffer(), synchronous, src_offset, copy_bytes, dst, block_on_count,
                              (block_on_count) ? &block_on_ocl : nullptr,
                              (queue && completion) ? &completion->detail()->event : nullptr);

  GPUAPICHECK(clerr, CL_SUCCESS, 0u);

  return copy_bytes;
}


size_t Buffer::write(const void *src, size_t byte_count, size_t dst_offset, Queue *queue, Event *block_on,
                     Event *completion)

{
  // Contrain the byte counts.
  const size_t this_buffer_size = size();
  if (completion)
  {
    completion->release();
  }

  if (dst_offset >= this_buffer_size)
  {
    return 0;
  }

  const size_t copy_bytes = std::min(byte_count, this_buffer_size - dst_offset);
  cl_int clerr = CL_SUCCESS;

#ifdef USE_PINNED
  if (imp_->flags & kBfHostAccess)
  {
    if (uint8_t *dst_mem = gputil::pin(*imp_, kPinWrite))
    {
      memcpy(dst_mem + dst_offset, src, copy_bytes);
      gputil::unpin(*imp_, dst_mem, queue, block_on, completion);
    }
    return copy_bytes;
  }
#endif  // USE_PINNED

  cl_command_queue queue_cl = selectQueue(imp_->device, queue);
  const int block_on_count = (block_on && block_on->isValid()) ? 1 : 0;
  cl_event block_on_ocl = (block_on_count) ? block_on->detail()->event : nullptr;

  // Non-blocking when an explicit queue has been provided.
  const cl_bool synchronous = (!queue || queue->internal()->force_synchronous) ? CL_TRUE : CL_FALSE;
  clerr = clEnqueueWriteBuffer(queue_cl, imp_->buffer(), synchronous, dst_offset, copy_bytes, src, block_on_count,
                               (block_on_count) ? &block_on_ocl : nullptr,
                               (queue && completion) ? &completion->detail()->event : nullptr);

  GPUAPICHECK(clerr, CL_SUCCESS, 0u);

  return copy_bytes;
}


size_t Buffer::readElements(void *dst, size_t element_size, size_t element_count, size_t offset_elements,
                            size_t buffer_element_size, Queue *queue, Event *block_on, Event *completion)
{
  if (completion)
  {
    completion->release();
  }

  if (!isValid() && !dst)
  {
    return 0u;
  }

  if (!buffer_element_size || element_size == buffer_element_size)
  {
    // Matching element size. Use normal write.
    return read(dst, element_size * element_count, element_size * offset_elements, queue, block_on, completion);
  }

  // Calculate the read offset based on the passed bufferElementSize.
  const size_t byte_read_offset = offset_elements * buffer_element_size;
  if (byte_read_offset >= size())
  {
    return 0u;
  }

  // Element size mismatch. Use piecewise write.
  const size_t copy_size = std::min(element_size, buffer_element_size);
  const size_t copy_element_count = std::min(element_count, (size() - byte_read_offset) / buffer_element_size);
  auto *dst_mem = static_cast<uint8_t *>(dst);

  if (copy_element_count)
  {
#if USE_PINNED
    // Mapped memory only when not using an explicit queue as queue implies a non-blocking call.
    if (!queue && (imp_->flags & kBfHostAccess))
    {
      if (block_on && block_on->isValid())
      {
        block_on->wait();
      }

      if (uint8_t *pinned_ptr = gputil::pin(*imp_, kPinRead))
      {
        uint8_t *src_mem = pinned_ptr;
        src_mem += byte_read_offset;
        for (size_t i = 0; i < copy_element_count; ++i)
        {
          memcpy(dst_mem, src_mem, copy_size);
          dst_mem += element_size;
          src_mem += buffer_element_size;
        }

        gputil::unpin(*imp_, pinned_ptr, queue);
        return copy_element_count;
      }
    }
#endif  // USE_PINNED

    size_t buffer_offset = byte_read_offset;
    cl_int clerr = CL_SUCCESS;
    cl_int clerr2 = CL_SUCCESS;
    cl_command_queue queue_cl = selectQueue(imp_->device, queue);
    const int block_on_count = (block_on && block_on->isValid()) ? 1 : 0;
    cl_event block_on_ocl = (block_on_count) ? block_on->detail()->event : nullptr;

    // Unfortunately I can't find whether the copy order is guaranteed for clEnqueueReadBuffer(). To support the
    // completeion event, we'll set barrier after the copy instructions and wait on that.
    const cl_bool synchronous = (!queue || queue->internal()->force_synchronous) ? CL_TRUE : CL_FALSE;
    for (size_t i = 0; i < copy_element_count; ++i)
    {
      clerr2 = clEnqueueReadBuffer(queue_cl, imp_->buffer(), synchronous, buffer_offset, copy_size, dst_mem,
                                   block_on_count, (block_on_count) ? &block_on_ocl : nullptr, nullptr);
      dst_mem += element_size;
      buffer_offset += buffer_element_size;

      clerr = (clerr) ? clerr : clerr2;
    }

    if (queue && completion)
    {
      // Set a barrier to ensure the copy commands complete and wait on that event.
      clEnqueueBarrierWithWaitList(queue_cl, 0, nullptr, &completion->detail()->event);
    }

    // Blocking operation when no explicit queue has been provided.
    if (!queue)
    {
      clerr2 = clFinish(queue_cl);
      clerr = (clerr) ? clerr : clerr2;
    }

    GPUAPICHECK(clerr, CL_SUCCESS, 0);
  }

  return copy_element_count;
}


size_t Buffer::writeElements(const void *src, size_t element_size, size_t element_count, size_t offset_elements,
                             size_t buffer_element_size, Queue *queue, Event *block_on, Event *completion)
{
  if (completion)
  {
    completion->release();
  }

  if (!isValid() && !src)
  {
    return 0u;
  }

  if (!buffer_element_size || element_size == buffer_element_size)
  {
    // Matching element size. Use normal write.
    return write(src, element_size * element_count, offset_elements * element_size, queue, block_on, completion);
  }

  const size_t byte_write_offset = offset_elements * buffer_element_size;
  if (byte_write_offset >= size())
  {
    return 0u;
  }

  // Element size mismatch. Use piecewise write.
  const size_t copy_size = std::min(element_size, buffer_element_size);
  const size_t copy_element_count = std::min(element_count, (size() - byte_write_offset) / buffer_element_size);
  const size_t clear_byte_count = (buffer_element_size > element_size) ? buffer_element_size - element_size : 0u;
  // const_cast because OpenCL API does not support const.
  const auto *src_mem = static_cast<const uint8_t *>(src);

  if (copy_element_count)
  {
#if USE_PINNED
    // Mapped memory only when not using an explicit queue as queue implies a non-blocking call.
    if (!queue && (imp_->flags & kBfHostAccess))
    {
      if (block_on && block_on->isValid())
      {
        block_on->wait();
      }

      if (uint8_t *pinned_ptr = gputil::pin(*imp_, kPinWrite))
      {
        uint8_t *dst_mem = pinned_ptr;
        dst_mem += byte_write_offset;
        for (size_t i = 0; i < copy_element_count; ++i)
        {
          memcpy(dst_mem, src_mem, copy_size);
          // Clear any extra bytes.
          if (clear_byte_count)
          {
            memset(dst_mem + element_size, 0, clear_byte_count);
          }
          dst_mem += buffer_element_size;
          src_mem += element_size;
        }

        gputil::unpin(*imp_, pinned_ptr, queue);
        return copy_element_count;
      }
    }
#endif  // USE_PINNED

    size_t buffer_offset = byte_write_offset;
    cl_int clerr = CL_SUCCESS;
    cl_int clerr2 = CL_SUCCESS;
    cl_command_queue queue_cl = selectQueue(imp_->device, queue);
    const int block_on_count = (block_on && block_on->isValid()) ? 1 : 0;
    cl_event block_on_ocl = (block_on_count) ? block_on->detail()->event : nullptr;

    // Unfortunately I can't find whether the copy order is guaranteed for clEnqueueWriteBuffer(). To support the
    // completeion event, we'll set barrier after the copy instructions and wait on that.
    const cl_bool synchronous = (!queue || queue->internal()->force_synchronous) ? CL_TRUE : CL_FALSE;
    for (size_t i = 0; i < copy_element_count; ++i)
    {
      clerr2 = clEnqueueWriteBuffer(queue_cl, imp_->buffer(), synchronous, buffer_offset, copy_size, src_mem,
                                    block_on_count, (block_on_count) ? &block_on_ocl : nullptr, nullptr);
      src_mem += element_size;
      buffer_offset += buffer_element_size;

      clerr = (clerr) ? clerr : clerr2;
    }

    if (queue && completion)
    {
      // Set a barrier to ensure the copy commands complete and wait on that event.
      clEnqueueBarrierWithWaitList(queue_cl, 0, nullptr, &completion->detail()->event);
    }

    // Blocking operation when no explicit queue has been provided.
    if (!queue)
    {
      clerr2 = clFinish(queue_cl);
      clerr = (clerr) ? clerr : clerr2;
    }

    GPUAPICHECK(clerr, CL_SUCCESS, 0);
  }

  return copy_element_count;
}


void *Buffer::argPtr() const
{
  if (imp_)
  {
    return imp_->buffer();
  }

  return nullptr;
}


void *Buffer::address() const
{
  return argPtr();
}


size_t copyBuffer(Buffer &dst, const Buffer &src, Queue *queue, Event *block_on, Event *completion)
{
  return copyBuffer(dst, 0, src, 0, src.size(), queue, block_on, completion);
}


size_t copyBuffer(Buffer &dst, const Buffer &src, size_t byte_count, Queue *queue, Event *block_on, Event *completion)
{
  return copyBuffer(dst, 0, src, 0, byte_count, queue, block_on, completion);
}


size_t copyBuffer(Buffer &dst, size_t dst_offset, const Buffer &src, size_t src_offset, size_t byte_count, Queue *queue,
                  Event *block_on, Event *completion)
{
  const size_t dst_size = dst.size();
  const size_t src_size = src.size();

  if (completion)
  {
    completion->release();
  }

  // Check offsets.
  if (dst_size < dst_offset)
  {
    return 0;
  }

  if (src_size < src_offset)
  {
    return 0;
  }

  // Check sizes after offset.
  byte_count = std::min(byte_count, dst_size - dst_offset);
  byte_count = std::min(byte_count, src_size - src_offset);

  // Disable linting for auto on the following - it requires `auto *` because cl_mem is a pointer typedef, but this
  // is somewhat misleading because cl_mem doesn't look like a pointer type.
  cl_mem src_mem_cl = src.arg<cl_mem>();  // NOLINT(modernize-use-auto)
  cl_mem dst_mem_cl = dst.arg<cl_mem>();  // NOLINT(modernize-use-auto)

  cl_int clerr = CL_SUCCESS;
  cl_command_queue queue_cl = selectQueue(src.detail()->device, queue);
  const int block_on_count = (block_on && block_on->isValid()) ? 1 : 0;
  cl_event block_on_ocl = (block_on_count) ? block_on->detail()->event : nullptr;

  clerr = clEnqueueCopyBuffer(queue_cl, src_mem_cl, dst_mem_cl, src_offset, dst_offset, byte_count, block_on_count,
                              (block_on_count) ? &block_on_ocl : nullptr,
                              (queue && completion) ? &completion->detail()->event : nullptr);

  GPUAPICHECK(clerr, CL_SUCCESS, 0);

  // Block if no explicit queue provided.
  if (!queue || queue->internal()->force_synchronous)
  {
    clerr = clFinish(queue_cl);
    GPUAPICHECK(clerr, CL_SUCCESS, 0u);
  }

  return byte_count;
}
}  // namespace gputil


================================================
FILE: gputil/cl/gpuBufferDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef GPUBUFFERDETAIL_H
#define GPUBUFFERDETAIL_H

#include "gpuConfig.h"

#include <clu/clu.h>

#include "gputil/gpuDevice.h"

#include <string>

namespace gputil
{
class Event;
class Queue;

struct BufferDetail
{
  Device device;
  cl::Buffer buffer;
  size_t requested_size = 0;
  unsigned flags = 0;
  unsigned request_flags = 0;
};

uint8_t *pin(BufferDetail &imp, PinMode mode);
/// Unpin memory. Supports asynchronous unpinning by providing a queue, in which case
/// a @p completion event is also recommended.
void unpin(BufferDetail &imp, void *pinned_ptr, Queue *queue = nullptr, Event *block_on = nullptr,
           Event *completion = nullptr);
}  // namespace gputil

#endif  // GPUBUFFERDETAIL_H


================================================
FILE: gputil/cl/gpuDevice.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "gpuDevice.h"

#include "gpuApiException.h"
#include "gpuDeviceDetail.h"

#include <clu/clu.h>
#include <clu/cluConstraint.h>

#include <sstream>
#include <vector>

namespace gputil
{
namespace
{
void finaliseDetail(DeviceDetail &detail, const DeviceInfo *info)
{
  if (info)
  {
    detail.info = *info;
  }
  else
  {
    detail.device.getInfo(CL_DEVICE_NAME, &detail.info.name);
    // cl::Platform platform = detail.device.pla
    detail.device.getInfo(CL_DEVICE_NAME, &detail.info.platform);

    std::string ver_string;
    detail.device.getInfo(CL_DEVICE_VERSION, &ver_string);

    // Parse the version string.
    cl_uint ver_major = 0;
    cl_uint ver_minor = 0;
    clu::parseVersion(ver_string.c_str(), &ver_major, &ver_minor);
    detail.info.version.major = ver_major;
    detail.info.version.minor = ver_minor;

    cl_platform_id platform_id{};
    detail.device.getInfo(CL_DEVICE_PLATFORM, &platform_id);

    cl_device_type device_type{};
    detail.device.getInfo(CL_DEVICE_TYPE, &device_type);

    detail.info.type = kDeviceNull;
    if (device_type & CL_DEVICE_TYPE_GPU)  // NOLINT(hicpp-signed-bitwise)
    {
      detail.info.type = kDeviceGpu;
    }
    else if (device_type & CL_DEVICE_TYPE_CPU)  // NOLINT(hicpp-signed-bitwise)
    {
      detail.info.type = kDeviceCpu;
    }
    else if (device_type & CL_DEVICE_TYPE_ACCELERATOR)  // NOLINT(hicpp-signed-bitwise)
    {
      detail.info.type = kDeviceOther;
    }

    size_t required_len = 0;
    clGetPlatformInfo(platform_id, CL_PLATFORM_NAME, 0, nullptr, &required_len);
    if (required_len)
    {
      std::vector<char> info_buffer(required_len + 1);
      info_buffer[0] = '\0';
      clGetPlatformInfo(platform_id, CL_PLATFORM_NAME, required_len + 1, info_buffer.data(), nullptr);
      detail.info.platform = std::string(info_buffer.data());
    }

    detail.device.getInfo(CL_DEVICE_EXTENSIONS, &detail.extensions);

    std::ostringstream str;
    clu::printDeviceInfo(str, detail.device, "");
    detail.description = str.str();
  }
}


unsigned enumerateDevices(std::vector<cl::Device> &cl_devices, std::vector<DeviceInfo> &device_infos)
{
  std::vector<cl::Platform> platforms;
  std::vector<cl::Device> devices;
  std::string ver_string;
  cl::Platform::get(&platforms);
  unsigned added = 0;

  // FIXME(Kazys): set the minimum version by the version of the SDK we've compiled against.
  // API minimum version is 1.2.
  const auto platform_version_constraint = clu::platformVersionMin(1, 2);

  for (cl::Platform &platform : platforms)
  {
    DeviceInfo info;
    platform.getInfo(CL_PLATFORM_NAME, &info.platform);

    if (!platform_version_constraint(platform))
    {
      continue;
    }

    devices.clear();
    platform.getDevices(CL_DEVICE_TYPE_ALL, &devices);

    for (cl::Device &device : devices)
    {
      device.getInfo(CL_DEVICE_NAME, &info.name);
      device.getInfo(CL_DEVICE_VERSION, &ver_string);

      // Parse the version string.
      cl_uint ver_major = 0;
      cl_uint ver_minor = 0;
      clu::parseVersion(ver_string.c_str(), &ver_major, &ver_minor);
      info.version.major = ver_major;
      info.version.minor = ver_minor;

      cl_device_type device_type{};
      device.getInfo(CL_DEVICE_TYPE, &device_type);

      info.type = kDeviceNull;
      if (device_type & CL_DEVICE_TYPE_GPU)  // NOLINT(hicpp-signed-bitwise)
      {
        info.type = kDeviceGpu;
      }
      else if (device_type & CL_DEVICE_TYPE_CPU)  // NOLINT(hicpp-signed-bitwise)
      {
        info.type = kDeviceCpu;
      }
      else if (device_type & CL_DEVICE_TYPE_ACCELERATOR)  // NOLINT(hicpp-signed-bitwise)
      {
        info.type = kDeviceOther;
      }

      cl_devices.push_back(device);
      device_infos.push_back(info);
      ++added;
    }
  }

  return added;
}
}  // namespace

Device::Device(bool default_device)
  : imp_(std::make_unique<DeviceDetail>())
{
  if (default_device)
  {
    if (!clu::getPrimaryContext(imp_->context, imp_->device))
    {
      // Needs initialisation.
      // Empty constraints.
      const cl_device_type device_type =
        CL_DEVICE_TYPE_GPU | CL_DEVICE_TYPE_ACCELERATOR;  // NOLINT(hicpp-signed-bitwise)
      const std::vector<clu::PlatformConstraint> platform_constraints;
      const std::vector<clu::DeviceConstraint> device_constraints;
      clu::initPrimaryContext(device_type, platform_constraints, device_constraints);
      clu::getPrimaryContext(imp_->context, imp_->device);
      imp_->default_queue = createQueue();
    }

    if (isValid())
    {
      std::vector<cl::Device> devices;
      clu::listDevices(devices, imp_->context);
      imp_->device = devices[0];
      imp_->default_queue = createQueue();
      finaliseDetail(*imp_, nullptr);
    }
  }
}


Device::Device(const DeviceInfo &device_info)
  : imp_(std::make_unique<DeviceDetail>())
{
  select(device_info);
}


Device::Device(int argc, const char **argv, const char *default_device, unsigned device_type_flags)
  : imp_(std::make_unique<DeviceDetail>())
{
  select(argc, argv, default_device, device_type_flags);
}


Device::Device(const Device &other)
  : imp_(std::make_unique<DeviceDetail>())
{
  *imp_ = *other.imp_;
}


Device::Device(Device &&other) noexcept
  : imp_(std::move(other.imp_))
{}


Device::~Device() = default;


unsigned Device::enumerateDevices(std::vector<DeviceInfo> &enumerated_devices)
{
  std::vector<cl::Device> devices;
  return gputil::enumerateDevices(devices, enumerated_devices);
}


const char *Device::name() const
{
  return imp_->info.name.c_str();
}


const char *Device::description() const
{
  return imp_->description.c_str();
}


const DeviceInfo &Device::info() const
{
  return imp_->info;
}


Queue Device::defaultQueue() const
{
  return imp_->default_queue;
}


Queue Device::createQueue(unsigned flags) const
{
  cl_int clerr = CL_SUCCESS;
#if CL_HPP_TARGET_OPENCL_VERSION >= 200
  // FIXME: The following should only be used with OpenCL 2 compatible devices,
  // even if the SDK is version 2.0 compatible.
  // We need a device.version() function to address this.
  cl_command_queue_properties queue_props[] = { 0, 0, 0 };
  if (flags & Queue::kProfile)
  {
    queue_props[0] = CL_QUEUE_PROPERTIES;
    queue_props[1] |= CL_QUEUE_PROFILING_ENABLE;
  }
  cl_command_queue queue = clCreateCommandQueueWithProperties(imp_->context(), imp_->device(), queue_props, &clerr);
#else   // CL_HPP_TARGET_OPENCL_VERSION >= 200
  cl_command_queue_properties queue_props = 0;
  if (flags & Queue::kProfile)
  {
    queue_props |= CL_QUEUE_PROFILING_ENABLE;  // NOLINT(hicpp-signed-bitwise)
  }
  cl_command_queue queue = clCreateCommandQueue(imp_->context(), imp_->device(), queue_props, &clerr);
#endif  // CL_HPP_TARGET_OPENCL_VERSION >= 200
  GPUAPICHECK(clerr, CL_SUCCESS, Queue());

  // This constructor will not add a reference.
  return Queue(queue);
}


bool Device::select(int argc, const char **argv, const char *default_device, unsigned device_type_flags)
{
  cl_device_type device_type = 0;
  std::vector<clu::PlatformConstraint> platform_constraints;
  std::vector<clu::DeviceConstraint> device_constraints;

  if (device_type_flags)
  {
    if (device_type_flags & kCpu)
    {
      device_type |= CL_DEVICE_TYPE_CPU;  // NOLINT(hicpp-signed-bitwise)
    }
    if (device_type_flags & kGpu)
    {
      device_type |= CL_DEVICE_TYPE_GPU;  // NOLINT(hicpp-signed-bitwise)
    }
    if (device_type_flags & kAccelerator)
    {
      device_type |= CL_DEVICE_TYPE_ACCELERATOR;  // NOLINT(hicpp-signed-bitwise)
    }
  }

  clu::constraintsFromCommandLine(argc, argv, device_type, platform_constraints, device_constraints);

  // Add a platform constraint to at least 1.2 for the minimum supported API.
  platform_constraints.push_back(clu::platformVersionMin(1, 2));

  if (device_constraints.empty() && default_device && default_device[0])
  {
    device_constraints.push_back(clu::deviceNameLike(default_device, true));
  }

  imp_->context =
    clu::createContext(&imp_->device, device_type, platform_constraints.data(), unsigned(platform_constraints.size()),
                       device_constraints.data(), unsigned(device_constraints.size()));

  if (imp_->context())
  {
    imp_->default_queue = createQueue();
    finaliseDetail(*imp_, nullptr);
  }
  else
  {
    imp_->context = cl::Context();
    imp_->device = cl::Device();
    imp_->default_queue = Queue();
  }

  if (isValid())
  {
    cl::Context context;
    cl::Device device;
    if (!clu::getPrimaryContext(context, device))
    {
      clu::setPrimaryContext(imp_->context, imp_->device);
    }

    // Add debug flag.
    for (int i = 0; i < argc; ++i)
    {
      if (strstr(argv[i], "--gpu-debug") == argv[i])
      {
        // Set default level.
        setDebugGpu(kDlLow);
        // Look for =<number> for more specific value.
        const char *arg_val = strstr(argv[i], "=");
        if (arg_val && arg_val[1])
        {
          ++arg_val;
          std::istringstream str(arg_val);
          int level = kDlLow;
          str >> level;
          if (kDlOff <= level && level <= kDlFull)
          {
            setDebugGpu(DebugLevel(level));
          }
        }
        break;
      }
    }

    return true;
  }

  return false;
}


bool Device::select(const DeviceInfo &device_info)
{
  std::vector<cl::Device> devices;
  std::vector<DeviceInfo> infos;
  gputil::enumerateDevices(devices, infos);

  for (size_t i = 0; i < infos.size(); ++i)
  {
    if (infos[i] == device_info)
    {
      // Found the device.
      if (i > devices.size())
      {
        return false;
      }

      imp_->device = devices[i];
      imp_->context = cl::Context(imp_->device);
      imp_->default_queue = createQueue();
      finaliseDetail(*imp_, &device_info);
      if (isValid())
      {
        cl::Context context;
        cl::Device device;
        if (!clu::getPrimaryContext(context, device))
        {
          clu::setPrimaryContext(imp_->context, imp_->device);
        }
      }
      return true;
    }
  }

  return false;
}


void Device::setDebugGpu(DebugLevel debug_level)
{
  if (imp_)
  {
    imp_->debug = debug_level;
  }
}


Device::DebugLevel Device::debugGpu() const
{
  return (imp_) ? DebugLevel(imp_->debug) : kDlOff;
}


bool Device::supportsFeature(const char *feature_id) const
{
  return imp_->extensions.find(feature_id) != std::string::npos;
}


void Device::addSearchPath(const char *path)
{
  if (imp_)
  {
    if (!imp_->search_paths.empty())
    {
      imp_->search_paths += ",";
      imp_->search_paths += path;
    }
    else
    {
      imp_->search_paths = path;
    }
  }
}


const char *Device::searchPaths() const
{
  return (imp_) ? imp_->search_paths.c_str() : "";
}


bool Device::isValid() const
{
  return imp_ && imp_->context();
}


uint64_t Device::deviceMemory() const
{
  if (!isValid())
  {
    return 0;
  }

  cl_ulong mem = 0;
  clGetDeviceInfo(imp_->device(), CL_DEVICE_GLOBAL_MEM_SIZE, sizeof(mem), &mem, nullptr);

  return static_cast<uint64_t>(mem);
}


uint64_t Device::maxAllocationSize() const
{
  if (!isValid())
  {
    return 0;
  }

  cl_ulong mem = 0;
  clGetDeviceInfo(imp_->device(), CL_DEVICE_MAX_MEM_ALLOC_SIZE, sizeof(mem), &mem, nullptr);

  return static_cast<uint64_t>(mem);
}


bool Device::unifiedMemory() const
{
  if (!isValid())
  {
    return false;
  }

  cl_bool unified = false;
  clGetDeviceInfo(imp_->device(), CL_DEVICE_HOST_UNIFIED_MEMORY, sizeof(unified), &unified, nullptr);
  return unified;
}


Device &Device::operator=(const Device &other)
{
  if (this != &other)
  {
    *imp_ = *other.imp_;
  }
  return *this;
}


Device &Device::operator=(Device &&other) noexcept
{
  imp_ = std::move(other.imp_);
  return *this;
}
}  // namespace gputil


================================================
FILE: gputil/cl/gpuDeviceDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef GPUDEVICEDETAIL_H
#define GPUDEVICEDETAIL_H

#include "gpuConfig.h"

#include "gputil/gpuDeviceInfo.h"
#include "gputil/gpuQueue.h"

#include <clu/clu.h>

namespace gputil
{
struct DeviceDetail
{
  cl::Context context;
  cl::Device device;
  /// The default queue object. We need a Queue object rather than just a cl::CommandQueue because we add data members.
  Queue default_queue;
  DeviceInfo info;
  std::string description;
  std::string search_paths;
  std::string extensions;  ///< OpenCL supported extension string.
  unsigned debug = 0;
};
}  // namespace gputil

#endif  // GPUDEVICEDETAIL_H


================================================
FILE: gputil/cl/gpuEvent.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuEvent.h"

#include "cl/gpuEventDetail.h"

#include "gpuApiException.h"
#include "gpuThrow.h"

namespace gputil
{
Event::Event() = default;

Event::Event(const Event &other)
{
  if (other.imp_)
  {
    imp_ = new EventDetail;  // NOLINT(cppcoreguidelines-owning-memory)
    imp_->event = other.imp_->event;
    clRetainEvent(imp_->event);
  }
}


Event::Event(Event &&other) noexcept
  : imp_(other.imp_)
{
  other.imp_ = nullptr;
}


Event::~Event()
{
  release();
  delete imp_;
}


bool Event::isValid() const
{
  cl_uint ref_count = 0;
  return imp_ && imp_->event &&
         clGetEventInfo(imp_->event, CL_EVENT_REFERENCE_COUNT, sizeof(ref_count), &ref_count, nullptr) !=
           CL_INVALID_EVENT;
}


void Event::release()
{
  if (imp_)
  {
    if (imp_->event)
    {
      clReleaseEvent(imp_->event);
      imp_->event = nullptr;
    }
  }
}


bool Event::isComplete() const
{
  if (!isValid())
  {
    return true;
  }

  cl_int status = 0;
  cl_int clerr = clGetEventInfo(imp_->event, CL_EVENT_COMMAND_EXECUTION_STATUS, sizeof(status), &status, nullptr);

  GPUAPICHECK(clerr, CL_SUCCESS, false);

  return status == CL_COMPLETE;
}


void Event::wait() const
{
  if (imp_ && imp_->event)
  {
    cl_int clerr = clWaitForEvents(1, &imp_->event);
    GPUAPICHECK2(clerr, CL_SUCCESS);
  }
}


void Event::wait(const Event *events, size_t event_count)
{
  if (event_count)
  {
    auto *events_ocl = static_cast<cl_event *>(alloca(sizeof(cl_event) * event_count));
    cl_uint actual_count = 0;
    for (size_t i = 0; i < event_count; ++i)
    {
      if (events[i].imp_ && events[i].imp_->event)
      {
        events_ocl[actual_count++] = events[i].imp_->event;
      }
    }

    if (actual_count)
    {
      clWaitForEvents(cl_uint(event_count), events_ocl);
    }
  }
}


void Event::wait(const Event **events, size_t event_count)
{
  if (event_count)
  {
    auto *events_ocl = static_cast<cl_event *>(alloca(sizeof(cl_event) * event_count));
    cl_uint actual_count = 0;
    for (size_t i = 0; i < event_count; ++i)
    {
      if (events[i]->imp_ && events[i]->imp_->event)
      {
        events_ocl[actual_count++] = events[i]->imp_->event;
      }
    }

    if (actual_count)
    {
      clWaitForEvents(actual_count, events_ocl);
    }
  }
}


Event &Event::operator=(const Event &other)
{
  if (this != &other)
  {
    release();
    if (other.imp_)
    {
      // Call detail() to ensure _imp is allocated.
      detail()->event = other.imp_->event;
      if (imp_->event)
      {
        clRetainEvent(imp_->event);
      }
    }
  }

  return *this;
}


Event &Event::operator=(Event &&other) noexcept
{
  release();
  if (other.imp_)
  {
    delete imp_;
    imp_ = other.imp_;
    other.imp_ = nullptr;
  }
  return *this;
}


EventDetail *Event::detail()
{
  // Detail requested. Allocate if required.
  if (!imp_)
  {
    imp_ = new EventDetail;  // NOLINT(cppcoreguidelines-owning-memory)
    imp_->event = nullptr;
  }
  return imp_;
}


EventDetail *Event::detail() const
{
  return imp_;
}
}  // namespace gputil


================================================
FILE: gputil/cl/gpuEventDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUEVENTDETAIL_H
#define GPUEVENTDETAIL_H

#include "gpuConfig.h"

#include <clu/clu.h>

namespace gputil
{
struct EventDetail
{
  cl_event event;
};
}  // namespace gputil

#endif  // GPUEVENTDETAIL_H


================================================
FILE: gputil/cl/gpuKernel.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuKernel.h"

#include "gpuApiException.h"
#include "gpuDevice.h"
#include "gpuKernelDetail.h"
#include "gpuProgram.h"
#include "gpuProgramDetail.h"
#include "gpuThrow.h"

#include "gpuDeviceDetail.h"

#include <algorithm>

namespace gputil
{
Kernel::Kernel()
  : imp_(new KernelDetail)
{
  imp_->kernel.setLocalMemFirst(false);
}


Kernel::Kernel(Kernel &&other) noexcept
  : imp_(other.imp_)
{
  other.imp_ = nullptr;
}


Kernel::~Kernel()
{
  delete imp_;
}


bool Kernel::isValid() const
{
  return imp_ && imp_->kernel.isValid();
}


void Kernel::setErrorChecking(bool check)
{
  imp_->auto_error_checking = check;
}


bool Kernel::errorChecking() const
{
  return imp_->auto_error_checking;
}


void Kernel::release()
{
  delete imp_;
  imp_ = nullptr;
}

void Kernel::addLocal(const std::function<size_t(size_t)> &local_calc)
{
  imp_->local_mem_args.push_back(local_calc);
  imp_->kernel.addLocal(local_calc);
}


size_t Kernel::calculateOptimalWorkGroupSize()
{
  return imp_->kernel.calculateOptimalWorkGroupSize();
}


size_t Kernel::optimalWorkGroupSize() const
{
  return imp_->kernel.optimalWorkGroupSize();
}

void Kernel::calculateGrid(gputil::Dim3 *global_size, gputil::Dim3 *local_size, const gputil::Dim3 &total_work_items)
{
  if (!isValid())
  {
    *global_size = *local_size = Dim3(0, 0, 0);
    return;
  }

  const size_t calc_volume = total_work_items.x * total_work_items.y * total_work_items.z;
  const size_t target_group_size = std::min(calculateOptimalWorkGroupSize(), calc_volume);
  const Device &gpu = imp_->program.device();

  // Try to setup the workgroup as a cubic spatial division.
  // However we have to consider where the max work item dimensions don't allow this.
  cl_uint group_dim = 3;
  cl_int err = 0;
  err =
    clGetDeviceInfo(gpu.detail()->device(), CL_DEVICE_MAX_WORK_ITEM_DIMENSIONS, sizeof(group_dim), &group_dim, nullptr);
  if (err)
  {
    GPUTHROW2(ApiException(err, nullptr, __FILE__, __LINE__));
  }
  auto *max_work_size = static_cast<size_t *>(alloca(sizeof(size_t) * group_dim));
  err = clGetDeviceInfo(gpu.detail()->device(), CL_DEVICE_MAX_WORK_ITEM_SIZES, sizeof(*max_work_size) * group_dim,
                        max_work_size, nullptr);
  if (err)
  {
    GPUTHROW2(ApiException(err, nullptr, __FILE__, __LINE__));
  }
  max_work_size[0] = std::min<size_t>(max_work_size[0], total_work_items.x);
  max_work_size[1] = std::min<size_t>(max_work_size[1], total_work_items.y);
  max_work_size[2] = std::min<size_t>(max_work_size[2], total_work_items.z);

  const double cube_root = 1.0f / 3.0f;
  auto target_dimension_value = unsigned(std::floor(std::pow(double(target_group_size), cube_root)));
  if (target_dimension_value < 1)
  {
    target_dimension_value = 1;
  }

  // Set the target dimensions to the minimum of the target and the max work group size.
  const double sqr_root = 1.0f / 2.0f;
  local_size->z = std::min<size_t>(max_work_size[2], target_dimension_value);
  // Lint(KS): division should be OK to convert to double for square root, then to int.
  // NOLINTNEXTLINE(bugprone-integer-division)
  target_dimension_value = unsigned(std::floor(std::pow(double(target_group_size / local_size->z), sqr_root)));
  local_size->y = std::min<size_t>(max_work_size[1], target_dimension_value);
  target_dimension_value = unsigned(std::max<size_t>(target_group_size / (local_size->y * local_size->z), 1));
  local_size->x = std::min<size_t>(max_work_size[0], target_dimension_value);

  // Reduce size to <= targetGroupSize
  int i = 2;  // Start by reducing 3rd dimension.
  while (i >= 0 && local_size->volume() > target_group_size)
  {
    if ((*local_size)[i] > 1)
    {
      (*local_size)[i] = (*local_size)[i] - 1;
    }
    else
    {
      // Don't reduce to less than 1. Next dimension.
      --i;
    }
  }

  // max work item sizes
  global_size->x = local_size->x * (total_work_items.x + local_size->x - 1) / local_size->x;
  global_size->y = local_size->y * (total_work_items.y + local_size->y - 1) / local_size->y;
  global_size->z = local_size->z * (total_work_items.z + local_size->z - 1) / local_size->z;
}


Device Kernel::device()
{
  if (!isValid())
  {
    return Device();
  }

  return imp_->program.device();
}


Kernel &Kernel::operator=(Kernel &&other) noexcept
{
  delete imp_;
  imp_ = other.imp_;
  other.imp_ = nullptr;
  return *this;
}


bool Kernel::checkResult(int invocation_result, bool allow_exceptions)
{
  (void)allow_exceptions;  // Unused if GPU_EXCEPTIONS disabled
  if (invocation_result != CL_SUCCESS)
  {
    auto exception = ApiException(invocation_result, "Kernel invocation error", __FILE__, __LINE__);
#if GPU_EXCEPTIONS
    if (allow_exceptions)
    {
      throw exception;
    }
    else
#endif  // GPU_EXCEPTIONS
    {
      log(exception);
      return false;
    }
  }
  return true;
}


Kernel openCLKernel(Program &program, const char *kernel_name)
{
  Kernel kernel;

  cl_int err = 0;
  if (!program.isValid())
  {
    err = CL_INVALID_PROGRAM;
    GPUTHROW(ApiException(err, nullptr, __FILE__, __LINE__), Kernel());
  }

  err = kernel.detail()->kernel.setEntry(program.detail()->program, kernel_name);
  if (err)
  {
    GPUTHROW(ApiException(err, nullptr, __FILE__, __LINE__), Kernel());
  }
  kernel.detail()->program = program;
  return kernel;
}
}  // namespace gputil


================================================
FILE: gputil/cl/gpuKernel2.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUKERNEL2_H
#define GPUKERNEL2_H

#include "gpuConfig.h"

#include "gputil/gpuApiException.h"
#include "gputil/gpuBuffer.h"
#include "gputil/gpuEventList.h"
#include "gputil/gpuThrow.h"

#include "gpuBufferDetail.h"
#include "gpuEventDetail.h"
#include "gpuKernelDetail.h"
#include "gpuQueueDetail.h"

#include <clu/cluKernel.h>

#include <cstdlib>

#define GPUTIL_BUILD_FROM_FILE(program, file_name, build_args) (program).buildFromFile(file_name, build_args)
#define GPUTIL_BUILD_FROM_SOURCE(program, source, source_length, build_args) \
  (program).buildFromSource(source, source_length, build_args)
#define GPUTIL_MAKE_KERNEL(program, kernel_name) gputil::openCLKernel(program, #kernel_name)

namespace clu
{
/// Override kernel argument setting using @c gputil::Buffer to map to @c cl_mem.
template <>
struct KernelArgHandler<gputil::Buffer>
{
  static cl_int set(cl::Kernel &kernel, int arg_index, const gputil::Buffer &arg)
  {
    // Lint: explicitly need size of pointer.
    // NOLINTNEXTLINE(bugprone-sizeof-expression)
    return ::clSetKernelArg(kernel(), arg_index, sizeof(arg.detail()->buffer()), &arg.detail()->buffer());
  }
};

/// Override kernel argument setting using @c gputil::BufferArg to map to @c cl_mem.
template <typename T>
struct KernelArgHandler<gputil::BufferArg<T>>
{
  static cl_int set(cl::Kernel &kernel, int arg_index, const gputil::BufferArg<T> &arg)
  {
    if (arg.buffer)
    {
      cl::Buffer &buffer = arg.buffer->detail()->buffer;
      // Lint: explicitly need size of pointer.
      // NOLINTNEXTLINE(bugprone-sizeof-expression)
      return ::clSetKernelArg(kernel(), arg_index, sizeof(buffer()), &buffer());
    }
    cl_mem null_mem = nullptr;
    // Lint: explicitly need size of pointer.
    // NOLINTNEXTLINE(bugprone-sizeof-expression)
    return ::clSetKernelArg(kernel(), arg_index, sizeof(null_mem), null_mem);
  }
};
}  // namespace clu

namespace gputil
{
template <typename... ARGS>
int Kernel::operator()(const Dim3 &global_size, const Dim3 &local_size, Queue *queue, ARGS... args)
{
  clu::KernelGrid grid;
  grid.work_group_size = clu::KernelSize(local_size.x, local_size.y, local_size.z);
  grid.global_size = clu::KernelSize(global_size.x, global_size.y, global_size.z);
  cl::CommandQueue &queue_cl = (queue) ? queue->internal()->queue : device().defaultQueue().internal()->queue;
  return detail()->kernel(queue_cl, grid, clu::EventList(), args...);
}


template <typename... ARGS>
int Kernel::operator()(const Dim3 &global_size, const Dim3 &local_size, Event &completion_event, Queue *queue,
                       ARGS... args)
{
  clu::KernelGrid grid;
  clu::EventList events_clu;
  cl::Event completion_tracker;
  events_clu.completion = &completion_tracker;

  grid.work_group_size = clu::KernelSize(local_size.x, local_size.y, local_size.z);
  grid.global_size = clu::KernelSize(global_size.x, global_size.y, global_size.z);
  cl::CommandQueue &queue_cl = (queue) ? queue->internal()->queue : device().defaultQueue().internal()->queue;
  int err = detail()->kernel(queue_cl, grid, events_clu, args...);
  completion_event.release();
  completion_event.detail()->event = completion_tracker();
  clRetainEvent(completion_tracker());

  if (detail()->auto_error_checking)
  {
    GPUAPICHECK(err, CL_SUCCESS, err);
  }

  if (queue && queue->internal()->force_synchronous)
  {
    // Force kernel completion if in synchronous mode.
    queue->finish();
  }

  return err;
}


template <typename... ARGS>
int Kernel::operator()(const Dim3 &global_size, const Dim3 &local_size, const EventList &event_list, Queue *queue,
                       ARGS... args)
{
  clu::KernelGrid grid;
  clu::EventList events_clu;

  events_clu.event_count = unsigned(event_list.count());
  if (events_clu.event_count)
  {
    events_clu.wait_on_events = static_cast<cl::Event *>(alloca(sizeof(cl::Event) * events_clu.event_count));
    for (unsigned i = 0; i < events_clu.event_count; ++i)
    {
      // Placement new into stack allocation.
      ::new (&events_clu.wait_on_events[i])
        cl::Event(event_list.events()[i].detail() ? event_list.events()[i].detail()->event : nullptr, true);
    }
  }

  grid.work_group_size = clu::KernelSize(local_size.x, local_size.y, local_size.z);
  grid.global_size = clu::KernelSize(global_size.x, global_size.y, global_size.z);
  cl::CommandQueue &queue_cl = (queue) ? queue->internal()->queue : device().defaultQueue().internal()->queue;
  int err = detail()->kernel(queue_cl, grid, events_clu, args...);

  // Cleanup stack objects.
  // TODO(KS): RAIA for this while avoiding a head allocation.
  for (unsigned i = 0; i < events_clu.event_count; ++i)
  {
    // Lint: the explicit desctructor call doesn't read well when we need to explicitly identify the namespace and
    // class. Just disable lint warning - very local.
    // NOLINTNEXTLINE(google-build-using-namespace)
    using namespace cl;
    // Call destructor in stack allocation.
    events_clu.wait_on_events[i].~Event();
  }

  if (detail()->auto_error_checking)
  {
    GPUAPICHECK(err, CL_SUCCESS, err);
  }

  if (queue && queue->internal()->force_synchronous)
  {
    // Force kernel completion if in synchronous mode.
    queue->finish();
  }

  return err;
}


template <typename... ARGS>
int Kernel::operator()(const Dim3 &global_size, const Dim3 &local_size, const EventList &event_list,
                       Event &completion_event, Queue *queue, ARGS... args)
{
  clu::KernelGrid grid;
  clu::EventList events_clu;

  events_clu.event_count = unsigned(event_list.count());
  if (events_clu.event_count)
  {
    events_clu.wait_on_events = static_cast<cl::Event *>(alloca(sizeof(cl::Event) * events_clu.event_count));
    for (unsigned i = 0; i < events_clu.event_count; ++i)
    {
      // Placement new into stack allocation.
      ::new (&events_clu.wait_on_events[i]) cl::Event(event_list.events()[i].detail()->event, true);
    }
  }
  cl::Event completion_tracker;
  events_clu.completion = &completion_tracker;

  grid.work_group_size = clu::KernelSize(local_size.x, local_size.y, local_size.z);
  grid.global_size = clu::KernelSize(global_size.x, global_size.y, global_size.z);
  cl::CommandQueue &queue_cl = (queue) ? queue->internal()->queue : device().defaultQueue().internal()->queue;
  int err = detail()->kernel(queue_cl, grid, events_clu, args...);

  // Cleanup stack objects.
  // TODO(KS): RAIA for this while avoiding a head allocation.
  for (unsigned i = 0; i < events_clu.event_count; ++i)
  {
    // Lint: the explicit desctructor call doesn't read well when we need to explicitly idnetify the namespace and
    // class. Just diable lint warning - very local.
    using namespace cl;  // NOLINT(google-build-using-namespace)
    // Call destructor in stack allocation.
    events_clu.wait_on_events[i].~Event();
  }

  completion_event.release();
  completion_event.detail()->event = completion_tracker();
  clRetainEvent(completion_tracker());

  if (detail()->auto_error_checking)
  {
    GPUAPICHECK(err, CL_SUCCESS, err);
  }

  if (queue && queue->internal()->force_synchronous)
  {
    // Force kernel completion if in synchronous mode.
    queue->finish();
  }

  return err;
}


class Program;
Kernel gputilAPI openCLKernel(Program &program, const char *kernel_name);
}  // namespace gputil

#endif  // GPUKERNEL2_H


================================================
FILE: gputil/cl/gpuKernelDetail.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUKERNELDETAIL_H
#define GPUKERNELDETAIL_H

#include "gpuConfig.h"

#include "../gpuDevice.h"
#include "../gpuProgram.h"

#include <clu/cluKernel.h>

#include <functional>

namespace gputil
{
struct KernelDetail
{
  clu::Kernel kernel;
  Program program;
  std::vector<std::function<size_t(size_t)>> local_mem_args;
  bool auto_error_checking = true;
};
}  // namespace gputil

#endif  // GPUKERNELDETAIL_H


================================================
FILE: gputil/cl/gpuPinnedBuffer.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gputil/gpuPinnedBuffer.h"

#include "gputil/cl/gpuBufferDetail.h"
#include "gputil/gpuBuffer.h"

#include <algorithm>
#include <cstring>

namespace gputil
{
PinnedBuffer::PinnedBuffer() = default;


PinnedBuffer::PinnedBuffer(Buffer &buffer, PinMode mode)
  : buffer_(&buffer)
  , mode_(mode)
{
  pin();
}


PinnedBuffer::PinnedBuffer(PinnedBuffer &&other) noexcept
  : buffer_(std::exchange(other.buffer_, nullptr))
  , pinned_(std::exchange(other.pinned_, nullptr))
  , mode_(std::exchange(other.mode_, kPinNone))
{}


PinnedBuffer::~PinnedBuffer()
{
  unpin();
}


bool PinnedBuffer::isPinned() const
{
  return pinned_ != nullptr;
}


void PinnedBuffer::pin()
{
  if (!pinned_ && buffer_)
  {
    pinned_ = gputil::pin(*buffer_->detail(), mode_);
  }
}


void PinnedBuffer::unpin(Queue *queue, Event *block_on, Event *completion)
{
  if (pinned_ && buffer_)
  {
    gputil::unpin(*buffer_->detail(), pinned_, queue, block_on, completion);
    pinned_ = nullptr;
  }
}


size_t PinnedBuffer::read(void *dst, size_t byte_count, size_t src_offset) const
{
  if (buffer_)
  {
    if (pinned_)
    {
      const auto *src_mem = static_cast<const uint8_t *>(pinned_);
      auto *dst_mem = static_cast<uint8_t *>(dst);

      byte_count = std::min(byte_count, buffer_->size() - src_offset);
      src_mem += src_offset;
      memcpy(dst_mem, src_mem, byte_count);
      return byte_count;
    }

    return buffer_->read(dst, byte_count, src_offset);
  }

  return 0u;
}


size_t PinnedBuffer::write(const void *src, size_t byte_count, size_t dst_offset)
{
  if (buffer_)
  {
    if (pinned_)
    {
      const auto *src_mem = static_cast<const uint8_t *>(src);
      auto *dst_mem = static_cast<uint8_t *>(pinned_);

      byte_count = std::min(byte_count, buffer_->size() - dst_offset);
      dst_mem += dst_offset;
      memcpy(dst_mem, src_mem, byte_count);
      return byte_count;
    }

    return buffer_->write(src, byte_count, dst_offset);
  }

  return 0u;
}


size_t PinnedBuffer::readElements(void *dst, size_t element_size, size_t element_count, size_t offset_elements,
                                  size_t buffer_element_size)
{
  if (!pinned_)
  {
    return buffer_->readElements(dst, element_size, element_count, offset_elements, buffer_element_size);
  }

  const auto *src_mem = static_cast<const uint8_t *>(pinned_);
  auto *dst_mem = static_cast<uint8_t *>(dst);
  if (element_size == buffer_element_size || buffer_element_size == 0)
  {
    const size_t src_offset = offset_elements * element_size;

    if (src_offset + element_size >= buffer_->size())
    {
      // Can't even copy one element from the requested offset.
      return 0;
    }

    const size_t byte_count = std::min(element_count * element_size, buffer_->size() - src_offset);
    memcpy(dst_mem, src_mem + src_offset, byte_count);
    return byte_count / element_count;
  }

  // Size-mismatch. Iterative copy.
  const uint8_t *src_end = src_mem + buffer_->size();
  const size_t element_copy_size = std::min(element_size, buffer_element_size);
  src_mem += offset_elements * buffer_element_size;
  size_t copy_count = 0;
  for (size_t i = 0; i < element_count && src_mem < src_end; ++i)
  {
    memcpy(dst_mem, src_mem, element_copy_size);
    src_mem += buffer_element_size;
    dst_mem += element_size;
    ++copy_count;
  }

  return copy_count;
}

size_t PinnedBuffer::writeElements(const void *src, size_t element_size, size_t element_count, size_t offset_elements,
                                   size_t buffer_element_size)
{
  if (!pinned_)
  {
    return buffer_->writeElements(src, element_size, element_count, offset_elements, buffer_element_size);
  }

  auto *dst_mem = static_cast<uint8_t *>(pinned_);
  const auto *src_mem = static_cast<const uint8_t *>(src);
  if (element_size == buffer_element_size || buffer_element_size == 0)
  {
    const size_t dst_offset = offset_elements * element_size;

    if (dst_offset + element_size >= buffer_->size())
    {
      // Can't even copy one element from to requested offset.
      return 0;
    }

    const size_t byte_count = std::min(element_count * element_size, buffer_->size() - dst_offset);
    memcpy(dst_mem + dst_offset, src_mem, byte_count);
    return byte_count / element_count;
  }

  // Size-mismatch. Iterative copy.
  const uint8_t *dst_end = dst_mem + buffer_->size();
  const size_t element_copy_size = std::min(element_size, buffer_element_size);
  dst_mem += offset_elements * buffer_element_size;
  size_t copy_count = 0;
  for (size_t i = 0; i < element_count && dst_mem < dst_end; ++i)
  {
    memcpy(dst_mem, src_mem, element_copy_size);
    dst_mem += buffer_element_size;
    src_mem += element_size;
    ++copy_count;
  }

  return copy_count;
}


PinnedBuffer &PinnedBuffer::operator=(PinnedBuffer &&other) noexcept
{
  unpin();
  buffer_ = other.buffer_;
  pinned_ = other.pinned_;
  mode_ = other.mode_;
  other.buffer_ = nullptr;
  other.pinned_ = nullptr;
  return *this;
}
}  // namespace gputil


================================================
FILE: gputil/cl/gpuPlatform2.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUPLATFORM2_H
#define GPUPLATFORM2_H

#if defined(__APPLE__) || defined(__MACOSX)
#include <OpenCL/opencl.h>
#else
#include <CL/opencl.h>
#endif  // !__APPLE__

namespace gputil
{
typedef cl_char char1;       // NOLINT
typedef cl_char2 char2;      // NOLINT
typedef cl_char3 char3;      // NOLINT
typedef cl_char4 char4;      // NOLINT
typedef cl_uchar uchar;      // NOLINT
typedef cl_uchar uchar1;     // NOLINT
typedef cl_uchar2 uchar2;    // NOLINT
typedef cl_uchar3 uchar3;    // NOLINT
typedef cl_uchar4 uchar4;    // NOLINT
typedef short short1;        // NOLINT
typedef cl_short2 short2;    // NOLINT
typedef cl_short3 short3;    // NOLINT
typedef cl_short4 short4;    // NOLINT
typedef cl_ushort ushort;    // NOLINT
typedef cl_ushort ushort1;   // NOLINT
typedef cl_ushort2 ushort2;  // NOLINT
typedef cl_ushort3 ushort3;  // NOLINT
typedef cl_ushort4 ushort4;  // NOLINT
typedef cl_int int1;         // NOLINT
typedef cl_int2 int2;        // NOLINT
typedef cl_int3 int3;        // NOLINT
typedef cl_int4 int4;        // NOLINT
typedef cl_uint uint;        // NOLINT
typedef cl_uint uint1;       // NOLINT
typedef cl_uint2 uint2;      // NOLINT
typedef cl_uint3 uint3;      // NOLINT
typedef cl_uint4 uint4;      // NOLINT
typedef cl_long long1;       // NOLINT
typedef cl_long2 long2;      // NOLINT
typedef cl_long3 long3;      // NOLINT
typedef cl_long4 long4;      // NOLINT
typedef cl_ulong ulong;      // NOLINT
typedef cl_ulong ulong1;     // NOLINT
typedef cl_ulong2 ulong2;    // NOLINT
typedef cl_ulong3 ulong3;    // NOLINT
typedef cl_ulong4 ulong4;    // NOLINT
typedef cl_float float1;     // NOLINT
typedef cl_float2 float2;    // NOLINT
typedef cl_float3 float3;    // NOLINT
typedef cl_float4 float4;    // NOLINT
typedef cl_double double1;   // NOLINT
typedef cl_double2 double2;  // NOLINT
}  // namespace gputil

#endif  // GPUPLATFORM2_H


================================================
FILE: gputil/cl/gpuProgram.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuProgram.h"

#include "gpuDeviceDetail.h"
#include "gpuProgramDetail.h"

#include <clu/cluProgram.h>

#include <algorithm>
#include <sstream>

namespace gputil
{
namespace
{
void prepareDebugBuildArgs(const gputil::Device &gpu, const BuildArgs &build_args, std::ostream &debug_opt,
                           std::ostream &build_opt, std::string &source_file_opt)
{
  // Compile and initialise.
  source_file_opt = std::string();

  std::string platform_name;
  cl_platform_id platform_id{};

  gputil::DeviceDetail &ocl = *gpu.detail();
  ocl.device.getInfo(CL_DEVICE_PLATFORM, &platform_id);
  cl::Platform platform(platform_id);
  platform.getInfo(CL_PLATFORM_VENDOR, &platform_name);
  std::transform(platform_name.begin(), platform_name.end(), platform_name.begin(), ::tolower);

  int debug_level = std::max<int>(gpu.debugGpu(), build_args.debug_level);

  // FIXME: work out how to resolve additional debug arguments, such as the source file argument
  // for Intel, but not the beignet drivers.
  // For Intel platforms, add debug compilation and source file option as we may debug
  // using the Intel SDK.
  if (platform_name.find("intel") != std::string::npos)
  {
#ifdef WIN32
    source_file_opt = "-s";
#endif  // WIN32
    switch (gpu.debugGpu())
    {
    case 2:
      debug_opt << "-cl-opt-disable ";
      // Don't break. Cascade to enable the next option.
      /* fall through */
    case 1:  // NOLINT(bugprone-branch-clone)
#ifdef WIN32
      debug_opt << "-g ";
#endif  // WIN32
      break;

    default:
      break;
    }
  }

  if (debug_level)
  {
    debug_opt << "-D DEBUG";
  }

  bool first_arg = true;
  if (build_args.version_major > 0 && build_args.version_minor >= 0)
  {
    build_opt << "-cl-std=CL";
    build_opt << build_args.version_major << "." << build_args.version_minor;
    first_arg = false;
  }

  if (build_args.args)
  {
    for (auto &&arg : *build_args.args)
    {
      if (!first_arg)
      {
        build_opt << ' ';
      }
      build_opt << arg;
      first_arg = false;
    }
  }
}
}  // namespace


Program::Program() = default;


Program::Program(Device &device, const char *program_name)
  : imp_(std::make_unique<ProgramDetail>())
{
  imp_->device = device;
  imp_->program_name = program_name;
}


Program::Program(Program &&other) noexcept
  : imp_(std::move(other.imp_))
{}


Program::Program(const Program &other)
{
  if (other.imp_)
  {
    imp_ = std::make_unique<ProgramDetail>();
    *imp_ = *other.imp_;
  }
}


Program::~Program() = default;


bool Program::isValid() const
{
  return imp_ && imp_->program();
}


const char *Program::programName() const
{
  return imp_->program_name.c_str();
}


Device Program::device()
{
  if (isValid())
  {
    return imp_->device;
  }

  return Device();
}


int Program::buildFromFile(const char *file_name, const BuildArgs &build_args)
{
  std::ostringstream debug_opt;
  std::ostringstream build_opt;
  std::string source_file_opt;
  cl::Context &ocl_context = imp_->device.detail()->context;
  prepareDebugBuildArgs(imp_->device, build_args, debug_opt, build_opt, source_file_opt);

  std::string source_file_name(file_name);
  cl_int clerr =
    clu::buildProgramFromFile(imp_->program, ocl_context, source_file_name, std::cerr, build_opt.str().c_str(),
                              debug_opt.str().c_str(), source_file_opt.c_str(), imp_->device.searchPaths());

  if (clerr != CL_SUCCESS)
  {
    imp_->program = cl::Program();
    return clerr;
  }

  return clerr;
}


int Program::buildFromSource(const char *source, size_t source_length, const BuildArgs &build_args)
{
  std::ostringstream debug_opt;
  std::ostringstream build_opt;
  std::string source_file_opt;
  cl::Context &ocl_context = imp_->device.detail()->context;
  prepareDebugBuildArgs(imp_->device, build_args, debug_opt, build_opt, source_file_opt);

  cl_int clerr = clu::buildProgramFromString(imp_->program, ocl_context, source, source_length, std::cerr,
                                             programName(), build_opt.str().c_str(), debug_opt.str().c_str());

  if (clerr != CL_SUCCESS)
  {
    imp_->program = cl::Program();

    return clerr;
  }

  return clerr;
}


Program &Program::operator=(const Program &other)
{
  if (this != &other)
  {
    if (other.imp_)
    {
      if (!imp_)
      {
        imp_ = std::make_unique<ProgramDetail>();
      }
      *imp_ = *other.imp_;
    }
    else
    {
      imp_.reset();
    }
  }
  return *this;
}


Program &Program::operator=(Program &&other) noexcept
{
  imp_ = std::move(other.imp_);
  return *this;
}
}  // namespace gputil


================================================
FILE: gputil/cl/gpuProgramDetail.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUPROGRAMDETAIL_H
#define GPUPROGRAMDETAIL_H

#include "gpuConfig.h"

#include "gpuDevice.h"

#include <clu/clu.h>

#include <string>

namespace gputil
{
struct ProgramDetail
{
  cl::Program program;
  Device device;
  std::string program_name;
};
}  // namespace gputil

#endif  // GPUPROGRAMDETAIL_H


================================================
FILE: gputil/cl/gpuQueue.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuQueue.h"

#include "gpuApiException.h"

#include "cl/gpuEventDetail.h"
#include "cl/gpuQueueDetail.h"

#include <clu/clu.h>

namespace gputil
{
namespace
{
struct CallbackWrapper
{
  std::function<void(void)> callback;

  explicit inline CallbackWrapper(std::function<void(void)> callback)
    : callback(std::move(callback))
  {}
};

// inline cl_command_queue q(void *p) { return static_cast<cl_command_queue>(p); }

void eventCallback(cl_event /*event*/, cl_int /*status*/, void *user_data)
{
  auto *wrapper = static_cast<CallbackWrapper *>(user_data);
  wrapper->callback();
  // Lint(KS): No RAII option available for this
  delete wrapper;  // NOLINT(cppcoreguidelines-owning-memory)
}
}  // namespace

Queue::Queue()
  : queue_(new QueueDetail())
{}


Queue::Queue(Queue &&other) noexcept
  : queue_(std::exchange(other.queue_, nullptr))
{}


Queue::Queue(const Queue &other)
  : queue_(other.queue_)
{}


Queue::Queue(void *platform_queue)
  : queue_(new QueueDetail)
{
  // Note: This code path takes ownership of an existing reference on the
  // given cl_command_queue object. It does not retain an additional reference.
  // Lint: linting suggest using `auto` for the type below, but cl_command_queue is a pointer typedef so it would
  // need to be auto *. I find this misleading becuase cl_command_queue is not a pointer type.
  cl_command_queue queue = static_cast<cl_command_queue>(platform_queue);  // NOLINT(modernize-use-auto)
  queue_->queue = queue;
}


Queue::~Queue() = default;


bool Queue::isValid() const
{
  return queue_ != nullptr && queue_->queue() != nullptr;
}


void Queue::insertBarrier()
{
  clEnqueueBarrierWithWaitList(queue_->queue(), 0, nullptr, nullptr);
}


Event Queue::mark()
{
  Event event;
  clEnqueueBarrierWithWaitList(queue_->queue(), 0, nullptr, &event.detail()->event);
  return event;
}


void Queue::setSynchronous(bool synchronous)
{
  queue_->force_synchronous = synchronous;
}


bool Queue::synchronous() const
{
  return queue_->force_synchronous;
}


void Queue::flush()
{
  cl_int err = clFlush(queue_->queue());
  GPUAPICHECK2(err, CL_SUCCESS);
}


void Queue::finish()
{
  cl_int err = clFinish(queue_->queue());
  GPUAPICHECK2(err, CL_SUCCESS);
}


void Queue::queueCallback(const std::function<void(void)> &callback)
{
  CallbackWrapper *wrapper = nullptr;
  cl_event barrier_event{};
  cl_int clerr = 0;

  clerr = clEnqueueBarrierWithWaitList(queue_->queue(), 0, nullptr, &barrier_event);
  GPUAPICHECK2(clerr, CL_SUCCESS);
  // Lint(KS): No nice RAII alternative for this
  wrapper = new CallbackWrapper(callback);  // NOLINT(cppcoreguidelines-owning-memory)
  clerr = clSetEventCallback(barrier_event, CL_COMPLETE, &eventCallback, wrapper);
  if (clerr)
  {
    // Lint(KS): No nice RAII alternative for this
    delete wrapper;  // NOLINT(cppcoreguidelines-owning-memory)
    GPUTHROW2(ApiException(clerr, nullptr, __FILE__, __LINE__));
  }
  clerr = clReleaseEvent(barrier_event);
  GPUAPICHECK2(clerr, CL_SUCCESS);
}


QueueDetail *Queue::internal() const
{
  return queue_.get();
}


Queue &Queue::operator=(const Queue &other)
{
  if (this != &other)
  {
    queue_ = other.queue_;
  }
  return *this;
}


Queue &Queue::operator=(Queue &&other) noexcept
{
  queue_ = std::move(other.queue_);
  return *this;
}
}  // namespace gputil


================================================
FILE: gputil/cl/gpuQueueDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUQUEUEDETAIL_H
#define GPUQUEUEDETAIL_H

#include "gpuConfig.h"

#include <clu/clu.h>

namespace gputil
{
struct QueueDetail
{
  cl::CommandQueue queue;
  bool force_synchronous = false;
};
}  // namespace gputil

#endif  // GPUQUEUEDETAIL_H


================================================
FILE: gputil/cuda/cutil_atomic.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef CUTIL_ATOMIC_H
#define CUTIL_ATOMIC_H

#include <cstddef>

//----------------------------------------------------------------------------------------------------------------------
// Alias CUDA atomics to match C/OpenCL 2.0 atomics
//----------------------------------------------------------------------------------------------------------------------
#ifdef __CUDACC__
using atomic_int = int;
using atomic_uint = uint;
using atomic_long = long long;
using atomic_ulong = unsigned long long;
using atomic_float = float;
using atomic_double = double;
using atomic_intptr_t = size_t;
using atomic_uintptr_t = size_t;
using atomic_size_t = size_t;
using atomic_ptrdiff_t = size_t;

inline __device__ void gputilAtomicInitI32(atomic_int *obj, int val)
{
  *obj = val;
}
inline __device__ void gputilAtomicStoreI32(atomic_int *obj, int val)
{
  *obj = val;
}
inline __device__ int gputilAtomicLoadI32(atomic_int *obj)
{
  return *obj;
}
inline __device__ int gputilAtomicExchangeI32(atomic_int *obj, int desired)
{
  return atomicExch(obj, desired);
}
inline __device__ bool gputilAtomicCasI32(atomic_int *obj, int expected, int desired)
{
  return atomicCAS(obj, expected, desired) == expected;
}

#define gputilAtomicInitI32L gputilAtomicInitI32
#define gputilAtomicStoreI32L gputilAtomicStoreI32
#define gputilAtomicLoadI32L gputilAtomicLoadI32
#define gputilAtomicExchangeI32L gputilAtomicExchangeI32
#define gputilAtomicCasI32L gputilAtomicCasI32

inline __device__ void gputilAtomicInitU32(atomic_uint *obj, uint val)
{
  *obj = val;
}
inline __device__ void gputilAtomicStoreU32(atomic_uint *obj, uint val)
{
  *obj = val;
}
inline __device__ uint gputilAtomicLoadU32(atomic_uint *obj)
{
  return *obj;
}
inline __device__ uint gputilAtomicExchangeU32(atomic_uint *obj, uint desired)
{
  return atomicExch(obj, desired);
}
inline __device__ bool gputilAtomicCasU32(atomic_uint *obj, uint expected, uint desired)
{
  return atomicCAS(obj, expected, desired) == expected;
}

#define gputilAtomicInitU32L gputilAtomicInitU32
#define gputilAtomicStoreU32L gputilAtomicStoreU32
#define gputilAtomicLoadU32L gputilAtomicLoadU32
#define gputilAtomicExchangeU32L gputilAtomicExchangeU32
#define gputilAtomicCasU32L gputilAtomicCasU32

inline __device__ void gputilAtomicInitU64(atomic_ulong *obj, unsigned long long val)
{
  *obj = val;
}
inline __device__ void gputilAtomicStoreU64(atomic_ulong *obj, unsigned long long val)
{
  *obj = val;
}
inline __device__ unsigned long long gputilAtomicLoadU64(atomic_ulong *obj)
{
  return *obj;
}
inline __device__ unsigned long long gputilAtomicExchangeU64(atomic_ulong *obj, unsigned long long desired)
{
  return atomicExch(obj, desired);
}
inline __device__ bool gputilAtomicCasU64(atomic_ulong *obj, unsigned long long expected, unsigned long long desired)
{
  return atomicCAS(obj, expected, desired) == expected;
}

#define gputilAtomicInitU64L gputilAtomicInitU64
#define gputilAtomicStoreU64L gputilAtomicStoreU64
#define gputilAtomicLoadU64L gputilAtomicLoadU64
#define gputilAtomicExchangeU64L gputilAtomicExchangeU64
#define gputilAtomicCasU64L gputilAtomicCasU64


inline __device__ void gputilAtomicInitF32(atomic_float *obj, float val)
{
  *obj = val;
}
inline __device__ void gputilAtomicStoreF32(atomic_float *obj, float val)
{
  *obj = val;
}
inline __device__ float gputilAtomicLoadF32(atomic_float *obj)
{
  return *obj;
}
inline __device__ float gputilAtomicExchangeF32(atomic_float *obj, float desired)
{
  return atomicExch(obj, desired);
}
inline __device__ bool gputilAtomicCasF32(atomic_float *obj, float expected, float desired)
{
  return __int_as_float(atomicCAS((atomic_int *)obj, __float_as_int(expected), __float_as_int(desired))) == expected;
}


#define gputilAtomicInitF32L gputilAtomicInitF32
#define gputilAtomicStoreF32L gputilAtomicStoreF32
#define gputilAtomicLoadF32L gputilAtomicLoadF32
#define gputilAtomicExchangeF32L gputilAtomicExchangeF32
#define gputilAtomicCasF32L gputilAtomicCasF32

// Note: CUDA semantics for atomicInc/Dec differ from OpenCL atomic_inc/dec. We use the OpenCL semantics, where there
// is no validation value and always increments/decrements.
inline __device__ int gputilAtomicAdd(atomic_int *p, int val)
{
  return atomicAdd(p, val);
}
inline __device__ int gputilAtomicSub(atomic_int *p, int val)
{
  return atomicSub(p, val);
}
inline __device__ int gputilAtomicInc(atomic_int *p)
{
  return atomicAdd(p, 1);
}
inline __device__ int gputilAtomicDec(atomic_int *p)
{
  return atomicSub(p, 1);
}
inline __device__ int gputilAtomicMin(atomic_int *p, int val)
{
  return atomicMin(p, val);
}
inline __device__ int gputilAtomicMax(atomic_int *p, int val)
{
  return atomicMax(p, val);
}
inline __device__ int gputilAtomicAnd(atomic_int *p, int val)
{
  return atomicAnd(p, val);
}
inline __device__ int gputilAtomicOr(atomic_int *p, int val)
{
  return atomicOr(p, val);
}
inline __device__ int gputilAtomicXor(atomic_int *p, int val)
{
  return atomicXor(p, val);
}

inline __device__ uint gputilAtomicAdd(atomic_uint *p, uint val)
{
  return atomicAdd(p, val);
}
inline __device__ uint gputilAtomicSub(atomic_uint *p, uint val)
{
  return atomicSub(p, val);
}
inline __device__ uint gputilAtomicInc(atomic_uint *p)
{
  return atomicAdd(p, 1);
}
inline __device__ uint gputilAtomicDec(atomic_uint *p)
{
  return atomicSub(p, 1);
}
inline __device__ uint gputilAtomicMin(atomic_uint *p, uint val)
{
  return atomicMin(p, val);
}
inline __device__ uint gputilAtomicMax(atomic_uint *p, uint val)
{
  return atomicMax(p, val);
}
inline __device__ uint gputilAtomicAnd(atomic_uint *p, uint val)
{
  return atomicAnd(p, val);
}
inline __device__ uint gputilAtomicOr(atomic_uint *p, uint val)
{
  return atomicOr(p, val);
}
inline __device__ uint gputilAtomicXor(atomic_uint *p, uint val)
{
  return atomicXor(p, val);
}

#endif  // __CUDACC__

#endif  // CUTIL_ATOMIC_H


================================================
FILE: gputil/cuda/cutil_decl.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef CUTIL_DECL_H
#define CUTIL_DECL_H

// Whilst the functions below are __host__, the declaration of std::function apparent is __host__ __device__.
// This can yield a situation where a __host__ function is calling a __host__ __device__ function which is not
// allowed. We force the issue by excluding this code when building device code.
#ifndef __CUDA_ARCH__

#include <functional>

// Helper macros for exposing CUDA kernel functions.

namespace gputil
{
using SharedMemCalculation = std::function<size_t(size_t)>;
using OptimalGroupSizeCalculation = std::function<int(size_t *, const SharedMemCalculation &)>;
}  // namespace gputil

#define _GPUTIL_DECLARE_KERNEL(kernel_name) const void *kernel_name##Ptr()
#define _GPUTIL_DECLARE_GROUP_CALC(kernel_name) \
  gputil::OptimalGroupSizeCalculation kernel_name##OptimalGroupSizeCalculator()

/// Delcaration of the a function which exposes a CUDA kernel for use with gputil.
/// Returns a pointer to the CUDA kernel.
#define GPUTIL_CUDA_DECLARE_KERNEL(kernel_name) \
  _GPUTIL_DECLARE_KERNEL(kernel_name);          \
  _GPUTIL_DECLARE_GROUP_CALC(kernel_name)


/// Definition of the a function which exposes a CUDA kernel for use with gputil.
#define GPUTIL_CUDA_DEFINE_KERNEL(kernel_name)                                                                 \
  __host__ _GPUTIL_DECLARE_KERNEL(kernel_name) { return (const void *)&(kernel_name); }                        \
  __host__ _GPUTIL_DECLARE_GROUP_CALC(kernel_name)                                                             \
  {                                                                                                            \
    return [](size_t *optimal_size, const gputil::SharedMemCalculation &shared_mem_calc) {                     \
      int min_grid_size = 0, optimal_block_size = 0;                                                           \
      cudaError_t err = cudaSuccess;                                                                           \
      err = ::cudaOccupancyMaxPotentialBlockSizeVariableSMem(&min_grid_size, &optimal_block_size, kernel_name, \
                                                             shared_mem_calc);                                 \
      *optimal_size = size_t(optimal_block_size);                                                              \
      return err;                                                                                              \
    };                                                                                                         \
  }

#else  // !__CUDA_ARCH__

#define GPUTIL_CUDA_DEFINE_KERNEL(kernel_name)

#endif  // !__CUDA_ARCH__

#endif  // CUTIL_DECL_H


================================================
FILE: gputil/cuda/cutil_importcl.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef CUDA_IMPORTCL_H
#define CUDA_IMPORTCL_H

#include <cfloat>
#include <cstddef>
#include <cstdio>

#include "cutil_decl.h"

#define LOCAL_ARG(TYPE, VAR)
#define LOCAL_MEM_ENABLE()       \
  size_t shared_mem_offset_ = 0; \
  extern __shared__ char shared_mem_[]
#define LOCAL_VAR(TYPE, VAR, SIZE)                   \
  TYPE VAR = (TYPE)&shared_mem_[shared_mem_offset_]; \
  shared_mem_offset_ += (SIZE)
#define LM_PER_THREAD(per_thread_size) ((per_thread_size)*blockDim.x * blockDim.y * blockDim.z)

// Kernel
#define __kernel __global__

// Memory
// For CUDA compatibility, we use __local on arguments, (removed in CUDA) and local for local delcarations (convert to
// __shared__)
#define __constant __constant__
#define __global
#define __local
#define local __shared__

// CUDA devices are so far little endian
#define __ENDIAN_LITTLE__ 1

// Useful information:
// https://www.sharcnet.ca/help/index.php/Porting_CUDA_to_OpenCL

/*

Hardware Terminology
CUDA                          OpenCL
SM (Stream Multiprocessor)    CU (Compute Unit)
Thread                        Work-item
Block                         Work-group
Global memory                 Global memory
Constant memory               Constant memory
Shared memory                 Local memory
Local memory                  Private memory


Qualifiers for Kernel Functions
CUDA                                OpenCL
__constant__ variable declaration   __constant variable declaration
__device__ function                 No annotation necessary
__device__ variable declaration     __global variable declaration
__global__ function                 __kernel function
__shared__ variable declaration     local variable declaration
argument declaration declaration    __local variable declaration


Kernels Indexing
CUDA                              OpenCL
gridDim                           get_num_groups()
blockDim                          get_local_size()
blockIdx                          get_group_id()
threadIdx                         get_local_id()
blockIdx * blockDim + threadIdx   get_global_id()
gridDim * blockDim                get_global_size()
CUDA is using threadIdx.x to get the id for the first dimension while OpenCL is using get_local_id(0).


Kernels Synchronization
CUDA                    OpenCL
__syncthreads()         barrier(CLK_LOCAL_MEM_FENCE)
__threadfence()         No direct equivalent
__threadfence_block()   mem_fence()
No direct equivalent    read_mem_fence()
No direct equivalent    write_mem_fence()

*/

// Synchronisation.
#define barrier(...) __syncthreads()
#define mem_fence    __threadfence_block

typedef unsigned char uchar;
typedef unsigned int uint;
typedef long long longlong;
typedef unsigned long long ulonglong;

inline __device__ unsigned get_num_groups(int i)
{
  switch (i)
  {
  default:
  case 0:
    return gridDim.x;
  case 1:
    return gridDim.y;
  case 2:
    return gridDim.z;
  }
}

inline __device__ unsigned get_local_size(int i)
{
  switch (i)
  {
  default:
  case 0:
    return blockDim.x;
  case 1:
    return blockDim.y;
  case 2:
    return blockDim.z;
  }
}

inline __device__ unsigned get_group_id(int i)
{
  switch (i)
  {
  default:
  case 0:
    return blockIdx.x;
  case 1:
    return blockIdx.y;
  case 2:
    return blockIdx.z;
  }
}

inline __device__ unsigned get_local_id(int i)
{
  switch (i)
  {
  default:
  case 0:
    return threadIdx.x;
  case 1:
    return threadIdx.y;
  case 2:
    return threadIdx.z;
  }
}

inline __device__ unsigned get_global_size(int i)
{
  switch (i)
  {
  default:
  case 0:
    return gridDim.x * blockDim.x;
  case 1:
    return gridDim.y * blockDim.y;
  case 2:
    return gridDim.z * blockDim.z;
  }
}

inline __device__ unsigned get_global_id(int i)
{
  switch (i)
  {
  default:
  case 0:
    return blockIdx.x * blockDim.x + threadIdx.x;
  case 1:
    return blockIdx.y * blockDim.y + threadIdx.y;
  case 2:
    return blockIdx.z * blockDim.z + threadIdx.z;
  }
}

inline __device__ float3 xyz(float4 v)
{
  return make_float3(v.x, v.y, v.z);
}

// // Atomic operation mapping : OpenCL to CUDA
// #define atomic_add atomicAdd
// #define atomic_sub atomicSub
// #define atomic_xchg atomicExch
// #define atomic_min atomicMin
// #define atomic_max atomicMax
// #define atomic_inc(addr) atomicAdd(addr, 1)
// #define atomic_dec(addr) atomicSub(addr, 1)
// #define atomic_cmpxchg atomicCAS
// #define atomic_and atomicAnd
// #define atomic_or atomicOr
// #define atomic_xor atomicXor

#include <math_constants.h>

// #define M_PI CUDART_PI

#include "cutil_atomic.h"
#include "cutil_math.h"

#endif  // CUDA_IMPORTCL_H


================================================
FILE: gputil/cuda/cutil_math.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef CUTIL_MATH_H
#define CUTIL_MATH_H

#include <cuda_runtime.h>

#include <cmath>

using uchar = unsigned char;
using ushort = unsigned short;
using uint = unsigned int;
using ulonglong = unsigned long long int;

//----------------------------------------------------------------------------------------------------------------------
// Basic vector operations.
//----------------------------------------------------------------------------------------------------------------------
#define _VECTOR_UOPS(type)                                                                                       \
  inline __host__ __device__ bool operator==(const type##2 & a, const type##2 & b)                               \
  {                                                                                                              \
    return a.x == b.x && a.y == b.y;                                                                             \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ bool operator!=(const type##2 & a, const type##2 & b) { return !operator==(a, b); } \
                                                                                                                 \
  inline __host__ __device__ type##2 operator+(const type##2 & a, const type##2 & b)                             \
  {                                                                                                              \
    return make_##type##2(a.x + b.x, a.y + b.y);                                                                 \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##2 operator+(const type##2 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##2(a.x + scalar, a.y + scalar);                                                           \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##2 operator-(const type##2 & a, const type##2 & b)                             \
  {                                                                                                              \
    return make_##type##2(a.x - b.x, a.y - b.y);                                                                 \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##2 operator-(const type##2 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##2(a.x - scalar, a.y - scalar);                                                           \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##2 &operator+=(type##2 & a, const type##2 & b)                                 \
  {                                                                                                              \
    a.x += b.x;                                                                                                  \
    a.y += b.y;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##2 &operator+=(type##2 & a, const S &scalar)                                   \
  {                                                                                                              \
    a.x += scalar;                                                                                               \
    a.y += scalar;                                                                                               \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##2 &operator-=(type##2 & a, const type##2 & b)                                 \
  {                                                                                                              \
    a.x -= b.x;                                                                                                  \
    a.y -= b.y;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##2 &operator-=(type##2 & a, const S &scalar)                                   \
  {                                                                                                              \
    a.x -= scalar;                                                                                               \
    a.y -= scalar;                                                                                               \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##2 operator*(const type##2 & a, const type##2 & b)                             \
  {                                                                                                              \
    return make_##type##2(a.x * b.x, a.y * b.y);                                                                 \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##2 &operator*=(type##2 & a, const type##2 & b)                                 \
  {                                                                                                              \
    a.x *= b.x;                                                                                                  \
    a.y *= b.y;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##2 operator*(const type##2 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##2(a.x * scalar, a.y * scalar);                                                           \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##2 operator*(const S &scalar, const type##2 & a)                               \
  {                                                                                                              \
    return make_##type##2(a.x * scalar, a.y * scalar);                                                           \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##2 &operator*=(type##2 & a, const S &scalar)                                   \
  {                                                                                                              \
    a.x *= scalar;                                                                                               \
    a.y *= scalar;                                                                                               \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##2 operator/(const type##2 & a, const type##2 & b)                             \
  {                                                                                                              \
    return make_##type##2(a.x / b.x, a.y / b.y);                                                                 \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##2 &operator/=(type##2 & a, const type##2 & b)                                 \
  {                                                                                                              \
    a.x /= b.x;                                                                                                  \
    a.y /= b.y;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##2 operator/(const type##2 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##2(a.x / scalar, a.y / scalar);                                                           \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##2 &operator/=(type##2 & a, const S &scalar)                                   \
  {                                                                                                              \
    a.x /= scalar;                                                                                               \
    a.y /= scalar;                                                                                               \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ bool operator==(const type##3 & a, const type##3 & b)                               \
  {                                                                                                              \
    return a.x == b.x && a.y == b.y && a.z == b.z;                                                               \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ bool operator!=(const type##3 & a, const type##3 & b) { return !operator==(a, b); } \
                                                                                                                 \
  inline __host__ __device__ type##3 operator+(const type##3 & a, const type##3 & b)                             \
  {                                                                                                              \
    return make_##type##3(a.x + b.x, a.y + b.y, a.z + b.z);                                                      \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##3 operator-(const type##3 & a, const type##3 & b)                             \
  {                                                                                                              \
    return make_##type##3(a.x - b.x, a.y - b.y, a.z - b.z);                                                      \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##3 operator+(const type##3 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##3(a.x + scalar, a.y + scalar, a.z + scalar);                                             \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##3 operator-(const type##3 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##3(a.x - scalar, a.y - scalar, a.z - scalar);                                             \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##3 &operator+=(type##3 & a, const type##3 & b)                                 \
  {                                                                                                              \
    a.x += b.x;                                                                                                  \
    a.y += b.y;                                                                                                  \
    a.z += b.z;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##3 &operator-=(type##3 & a, const type##3 & b)                                 \
  {                                                                                                              \
    a.x -= b.x;                                                                                                  \
    a.y -= b.y;                                                                                                  \
    a.z -= b.z;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##3 &operator+=(type##3 & a, const S &scalar)                                   \
  {                                                                                                              \
    a.x += scalar;                                                                                               \
    a.y += scalar;                                                                                               \
    a.z += scalar;                                                                                               \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##3 &operator-=(type##3 & a, const S &scalar)                                   \
  {                                                                                                              \
    a.x -= scalar;                                                                                               \
    a.y -= scalar;                                                                                               \
    a.z -= scalar;                                                                                               \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##3 operator*(const type##3 & a, const type##3 & b)                             \
  {                                                                                                              \
    return make_##type##3(a.x * b.x, a.y * b.y, a.z * b.z);                                                      \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##3 &operator*=(type##3 & a, const type##3 & b)                                 \
  {                                                                                                              \
    a.x *= b.x;                                                                                                  \
    a.y *= b.y;                                                                                                  \
    a.z *= b.z;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##3 operator*(const type##3 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##3(a.x * scalar, a.y * scalar, a.z * scalar);                                             \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##3 operator*(const S &scalar, const type##3 & a)                               \
  {                                                                                                              \
    return make_##type##3(a.x * scalar, a.y * scalar, a.z * scalar);                                             \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##3 &operator*=(type##3 & a, const S &scalar)                                   \
  {                                                                                                              \
    a.x *= scalar;                                                                                               \
    a.y *= scalar;                                                                                               \
    a.z *= scalar;                                                                                               \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##3 operator/(const type##3 & a, const type##3 & b)                             \
  {                                                                                                              \
    return make_##type##3(a.x / b.x, a.y / b.y, a.z / b.z);                                                      \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##3 &operator/=(type##3 & a, const type##3 & b)                                 \
  {                                                                                                              \
    a.x /= b.x;                                                                                                  \
    a.y /= b.y;                                                                                                  \
    a.z /= b.z;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##3 operator/(const type##3 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##3(a.x / scalar, a.y / scalar, a.z / scalar);                                             \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##3 &operator/=(type##3 & a, const S &scalar)                                   \
  {                                                                                                              \
    a.x /= scalar;                                                                                               \
    a.y /= scalar;                                                                                               \
    a.z /= scalar;                                                                                               \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ bool operator==(const type##4 & a, const type##4 & b)                               \
  {                                                                                                              \
    return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w;                                                 \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ bool operator!=(const type##4 & a, const type##4 & b) { return !operator==(a, b); } \
                                                                                                                 \
  inline __host__ __device__ type##4 operator+(const type##4 & a, const type##4 & b)                             \
  {                                                                                                              \
    return make_##type##4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);                                           \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##4 operator-(const type##4 & a, const type##4 & b)                             \
  {                                                                                                              \
    return make_##type##4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);                                           \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##4 operator+(const type##4 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##4(a.x + scalar, a.y + scalar, a.z + scalar, a.w + scalar);                               \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##4 operator-(const type##4 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##4(a.x - scalar, a.y - scalar, a.z - scalar, a.w - scalar);                               \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##4 &operator+=(type##4 & a, const type##4 & b)                                 \
  {                                                                                                              \
    a.x += b.x;                                                                                                  \
    a.y += b.y;                                                                                                  \
    a.z += b.z;                                                                                                  \
    a.w += b.w;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##4 &operator-=(type##4 & a, const type##4 & b)                                 \
  {                                                                                                              \
    a.x -= b.x;                                                                                                  \
    a.y -= b.y;                                                                                                  \
    a.z -= b.z;                                                                                                  \
    a.w -= b.w;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##4 operator*(const type##4 & a, const type##4 & b)                             \
  {                                                                                                              \
    return make_##type##4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);                                           \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##4 &operator*=(type##4 & a, const type##4 & b)                                 \
  {                                                                                                              \
    a.x *= b.x;                                                                                                  \
    a.y *= b.y;                                                                                                  \
    a.z *= b.z;                                                                                                  \
    a.w *= b.w;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##4 operator*(const type##4 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##4(a.x * scalar, a.y * scalar, a.z * scalar, a.w * scalar);                               \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##4 operator*(const S &scalar, const type##4 & a)                               \
  {                                                                                                              \
    return make_##type##4(a.x * scalar, a.y * scalar, a.z * scalar, a.w * scalar);                               \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##4 &operator*=(type##4 & a, const S &scalar)                                   \
  {                                                                                                              \
    a.x *= scalar;                                                                                               \
    a.y *= scalar;                                                                                               \
    a.z *= scalar;                                                                                               \
    a.w *= scalar;                                                                                               \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##4 operator/(const type##4 & a, const type##4 & b)                             \
  {                                                                                                              \
    return make_##type##4(a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w);                                           \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ type##4 &operator/=(type##4 & a, const type##4 & b)                                 \
  {                                                                                                              \
    a.x /= b.x;                                                                                                  \
    a.y /= b.y;                                                                                                  \
    a.z /= b.z;                                                                                                  \
    a.w /= b.w;                                                                                                  \
    return a;                                                                                                    \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##4 operator/(const type##4 & a, const S &scalar)                               \
  {                                                                                                              \
    return make_##type##4(a.x / scalar, a.y / scalar, a.z / scalar, a.w / scalar);                               \
  }                                                                                                              \
                                                                                                                 \
  template <typename S>                                                                                          \
  inline __host__ __device__ type##4 &operator/=(type##4 & a, const S &scalar)                                   \
  {                                                                                                              \
    a.x /= scalar;                                                                                               \
    a.y /= scalar;                                                                                               \
    a.z /= scalar;                                                                                               \
    a.w /= scalar;                                                                                               \
    return a;                                                                                                    \
  }

#define _VECTOR_OPS(type)                                                                                      \
  _VECTOR_UOPS(type)                                                                                           \
  inline __host__ __device__ type##2 operator-(const type##2 & a) { return make_##type##2(-a.x, -a.y); }       \
  inline __host__ __device__ type##3 operator-(const type##3 & a) { return make_##type##3(-a.x, -a.y, -a.z); } \
  inline __host__ __device__ type##4 operator-(const type##4 & a) { return make_##type##4(-a.x, -a.y, -a.z, -a.w); }


_VECTOR_OPS(char);
_VECTOR_UOPS(uchar);
_VECTOR_OPS(short);
_VECTOR_UOPS(ushort);
_VECTOR_OPS(int);
_VECTOR_UOPS(uint);
_VECTOR_OPS(long);
_VECTOR_UOPS(ulong);
_VECTOR_OPS(float);
_VECTOR_OPS(double);

//----------------------------------------------------------------------------------------------------------------------
// OpenCL equivalent extensions
//----------------------------------------------------------------------------------------------------------------------
#define _VECTOR_LOGIC(type, itype)                                                                           \
  inline __device__ __host__ itype##2 isequal(const type##2 & a, const type##2 & b)                          \
  {                                                                                                          \
    return make_##itype##2((a.x == b.x) ? -1 : 0, (a.y == b.y) ? -1 : 0);                                    \
  }                                                                                                          \
                                                                                                             \
  inline __device__ __host__ itype##3 isequal(const type##3 & a, const type##3 & b)                          \
  {                                                                                                          \
    return make_##itype##3((a.x == b.x) ? -1 : 0, (a.y == b.y) ? -1 : 0, (a.z == b.z) ? -1 : 0);             \
  }                                                                                                          \
                                                                                                             \
  inline __device__ __host__ itype##4 isequal(const type##4 & a, const type##4 & b)                          \
  {                                                                                                          \
    return make_##itype##4((a.x == b.x) ? -1 : 0, (a.y == b.y) ? -1 : 0, (a.z == b.z) ? -1 : 0,              \
                           (a.w == b.w) ? -1 : 0);                                                           \
  }                                                                                                          \
                                                                                                             \
  inline __device__ __host__ int any(const type##2 & x) { return (x.x != 0 || x.y != 0) ? 1 : 0; }           \
                                                                                                             \
  inline __device__ __host__ int any(const type##3 x) { return (x.x != 0 || x.y != 0 || x.z != 0) ? 1 : 0; } \
                                                                                                             \
  inline __device__ __host__ int any(const type##4 & x)                                                      \
  {                                                                                                          \
    return (x.x != 0 || x.y != 0 || x.z != 0 || x.w != 0) ? 1 : 0;                                           \
  }                                                                                                          \
                                                                                                             \
  inline __device__ __host__ int all(type##2 x) { return (x.x != 0 && x.y != 0) ? 1 : 0; }                   \
                                                                                                             \
  inline __device__ __host__ int all(type##3 x) { return (x.x != 0 && x.y != 0 && x.z != 0) ? 1 : 0; }       \
                                                                                                             \
  inline __device__ __host__ int all(type##4 x) { return (x.x != 0 && x.y != 0 && x.z != 0 && x.w != 0) ? 1 : 0; }

_VECTOR_LOGIC(char, int);
_VECTOR_LOGIC(uchar, int);
_VECTOR_LOGIC(short, int);
_VECTOR_LOGIC(ushort, int);
_VECTOR_LOGIC(int, int);
_VECTOR_LOGIC(uint, int);
_VECTOR_LOGIC(long, long);
_VECTOR_LOGIC(ulong, long);
_VECTOR_LOGIC(float, int);
_VECTOR_LOGIC(double, long);

//----------------------------------------------------------------------------------------------------------------------
// Vector product
//----------------------------------------------------------------------------------------------------------------------

#define _VECTOR_GEOM(type)                                                                                      \
  inline __host__ __device__ type dot(const type##2 & a, const type##2 & b) { return a.x * b.x + a.y * b.y; }   \
                                                                                                                \
  inline __host__ __device__ type dot(const type##3 & a, const type##3 & b)                                     \
  {                                                                                                             \
    return a.x * b.x + a.y * b.y + a.z * b.z;                                                                   \
  }                                                                                                             \
                                                                                                                \
  inline __host__ __device__ type dot(const type##4 & a, const type##4 & b)                                     \
  {                                                                                                             \
    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;                                                       \
  }                                                                                                             \
                                                                                                                \
  inline __host__ __device__ type length(const type##2 & a) { return std::sqrt(dot(a, a)); }                    \
                                                                                                                \
  inline __host__ __device__ type length(const type##3 & a) { return std::sqrt(dot(a, a)); }                    \
                                                                                                                \
  inline __host__ __device__ type length(const type##4 & a) { return std::sqrt(dot(a, a)); }                    \
                                                                                                                \
  inline __host__ __device__ type##2 normalize(const type##2 & a) { return a * (1.0f / std::sqrt(dot(a, a))); } \
                                                                                                                \
  inline __host__ __device__ type##3 normalize(const type##3 & a) { return a * (1.0f / std::sqrt(dot(a, a))); } \
                                                                                                                \
  inline __host__ __device__ type##4 normalize(const type##4 & a) { return a * (1.0f / std::sqrt(dot(a, a))); } \
                                                                                                                \
  inline __host__ __device__ type##3 cross(type##3 a, type##3 b)                                                \
  {                                                                                                             \
    return make_##type##3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);                 \
  }

_VECTOR_GEOM(float)
_VECTOR_GEOM(double)

//----------------------------------------------------------------------------------------------------------------------
// Conversion
//----------------------------------------------------------------------------------------------------------------------
#define _VECTOR_CONVERT(to, from)                                                                                \
  inline __host__ __device__ to##2 convert_##to##2(const from##2 & a) { return make_##to##2(to(a.x), to(a.y)); } \
                                                                                                                 \
  inline __host__ __device__ to##3 convert_##to##3(const from##3 & a)                                            \
  {                                                                                                              \
    return make_##to##3(to(a.x), to(a.y), to(a.z));                                                              \
  }                                                                                                              \
                                                                                                                 \
  inline __host__ __device__ to##4 convert_##to##4(const from##4 & a)                                            \
  {                                                                                                              \
    return make_##to##4(to(a.x), to(a.y), to(a.z), to(a.w));                                                     \
  }

#define _VECTOR_CONVERT_SET(to) \
  _VECTOR_CONVERT(to, char)     \
  _VECTOR_CONVERT(to, uchar)    \
  _VECTOR_CONVERT(to, short)    \
  _VECTOR_CONVERT(to, ushort)   \
  _VECTOR_CONVERT(to, int)      \
  _VECTOR_CONVERT(to, uint)     \
  _VECTOR_CONVERT(to, float)    \
  _VECTOR_CONVERT(to, double)

_VECTOR_CONVERT_SET(char)
_VECTOR_CONVERT_SET(uchar)
_VECTOR_CONVERT_SET(short)
_VECTOR_CONVERT_SET(ushort)
_VECTOR_CONVERT_SET(int)
_VECTOR_CONVERT_SET(uint)
_VECTOR_CONVERT_SET(float)
_VECTOR_CONVERT_SET(double)

//----------------------------------------------------------------------------------------------------------------------
// Utility
//----------------------------------------------------------------------------------------------------------------------
template <typename T>
inline __host__ __device__ T clamp(const T &val, const T &min_val, const T &max_val)
{
  return (val < min_val) ? min_val : (val > max_val) ? max_val : val;
}


#endif  // CUTIL_MATH_H


================================================
FILE: gputil/cuda/gpuApiExceptionCode.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "gputil/gpuApiException.h"

#include <cuda_runtime.h>

namespace gputil
{
const char *ApiException::errorCodeString(int error_code)
{
  return cudaGetErrorString(static_cast<cudaError_t>(error_code));
}
}  // namespace gputil


================================================
FILE: gputil/cuda/gpuBuffer.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "gpuBuffer.h"

#include "gputil/cuda/gpuBufferDetail.h"
#include "gputil/cuda/gpuEventDetail.h"
#include "gputil/cuda/gpuQueueDetail.h"

#include "gputil/gpuApiException.h"
#include "gputil/gpuQueue.h"
#include "gputil/gpuThrow.h"

#include <logutil/LogUtil.h>
#include <logutil/Logger.h>

#include <cuda.h>
#include <cuda_runtime.h>

#include <algorithm>
#include <cinttypes>
#include <iostream>

namespace gputil
{
inline cudaStream_t selectStream(Queue *queue)
{
  return (queue) ? queue->internal()->queue : nullptr;
}

/// Internal copy command. Manages synchronous vs. asynchronous code paths.
///
/// See @c Buffer class comments for details on how @p queue, @p block_on, and @c completion behave.
///
/// @param dst The destination address.
/// @param src The source address.
/// @param byte_count Number of bytes to copy.
/// @param kind The type of memory copy to execute.
/// @param queue Event queue to resolve the cuda stream from. Null for default.
/// @param block_on Event to wait for before copying.
/// @param completion Event to set to mark completion.
/// @return @c true when the synchronous, blocking code path was taken, @c false for asynchronous copy.
bool bufferCopy(void *dst, const void *src, size_t byte_count, cudaMemcpyKind kind, Queue *queue, Event *block_on,
                Event *completion)
{
  cudaError_t err = cudaSuccess;
  // Manage asynchronous.
  if (queue && !queue->internal()->force_synchronous)
  {
    // Async copy.
    // Resolve the stream.
    cudaStream_t stream = selectStream(queue);
    if (block_on && block_on->isValid())
    {
      err = cudaStreamWaitEvent(stream, block_on->detail()->obj(), 0);
      GPUAPICHECK(err, cudaSuccess, true);
    }

    err = cudaMemcpyAsync(dst, src, byte_count, kind, stream);
    GPUAPICHECK(err, cudaSuccess, true);

    if (completion)
    {
      err = cudaEventRecord(completion->detail()->obj(), stream);
      GPUAPICHECK(err, cudaSuccess, true);
    }
    return false;
  }

  if (block_on)
  {
    block_on->wait();
  }

  err = cudaMemcpy(dst, src, byte_count, kind);
  GPUAPICHECK(err, cudaSuccess, true);

  if (completion)
  {
    // Release completion event. Should not have been provided without queue argument.
    completion->release();
  }

  return true;
}


uint8_t *pin(BufferDetail &buf, PinMode mode)
{
  if (buf.pinned_status)
  {
    // Already pinned.
    return nullptr;
  }

  if (buf.mapped_mem)
  {
    if (mode == kPinRead || mode == kPinReadWrite)
    {
      // Copy from host.
      // Currently only support synchronous mem copy.
      cudaError_t err = cudaMemcpy(buf.mapped_mem, buf.device_mem, buf.alloc_size, cudaMemcpyDeviceToHost);
      GPUAPICHECK(err, cudaSuccess, nullptr);
      buf.pinned_status |= kPinnedForRead;
    }

    if (mode == kPinWrite || mode == kPinReadWrite)
    {
      buf.pinned_status |= kPinnedForWrite;
    }
  }

  return static_cast<uint8_t *>(buf.mapped_mem);
}


void unpin(BufferDetail &imp, void *pinned_ptr, PinMode mode, Queue *queue, Event *block_on, Event *completion)
{
  if (completion)
  {
    completion->release();
  }

  unsigned expected_pin_flags = 0;
  if (mode == kPinRead || mode == kPinReadWrite)
  {
    expected_pin_flags |= kPinnedForRead;
  }
  if (mode == kPinWrite || mode == kPinReadWrite)
  {
    expected_pin_flags |= kPinnedForWrite;
  }

  // Validate pin mode.
  if (!pinned_ptr || pinned_ptr != imp.mapped_mem || expected_pin_flags != imp.pinned_status)
  {
    return;
  }

  cudaError_t err = cudaSuccess;
  if (expected_pin_flags & kPinnedForWrite)
  {
    // Process the dirty list, copying regions back to the device.
    MemRegion::mergeRegionList(imp.dirty_write);

    // Process the merged dirty list.
    bool async = false;
    for (const MemRegion &region : imp.dirty_write)
    {
      if (region.byte_count)
      {
        if (!bufferCopy(static_cast<uint8_t *>(imp.device_mem) + region.offset,
                        static_cast<uint8_t *>(imp.mapped_mem) + region.offset, region.byte_count,
                        cudaMemcpyHostToDevice, queue, block_on, nullptr))
        {
          async = true;
        }
      }
    }

    if (async)
    {
      // Async copy. Setup completion event after the last queued copy.
      if (completion && queue)
      {
        cudaStream_t stream = queue->internal()->queue;
        err = cudaEventRecord(completion->detail()->obj(), stream);
        GPUAPICHECK2(err, cudaSuccess);
      }
    }

    imp.dirty_write.clear();
  }

  imp.pinned_status = 0;
}

/// Internal memset command for device memory. Manages synchronous vs. asynchronous code paths.
///
/// See @c Buffer class comments for details on how @p queue, @p block_on, and @c completion behave.
///
/// @param mem The destination address.
/// @param pattern Clear pattern.
/// @param count Number of bytes to set.
/// @param kind The type of memory copy to execute.
/// @param queue Event queue to resolve the cuda stream from. Null for default.
/// @param block_on Event to wait for before copying.
/// @param completion Event to set to mark completion.
/// @return @c true when the synchronous, blocking code path was taken, @c false for asynchronous copy.
bool bufferSet(void *mem, int value, size_t count, Queue *queue, Event *block_on, Event *completion)
{
  cudaError_t err = cudaSuccess;
  if (queue && !queue->internal()->force_synchronous)
  {
    cudaStream_t stream = selectStream(queue);

    if (block_on && block_on->isValid())
    {
      err = cudaStreamWaitEvent(stream, block_on->detail()->obj(), 0);
      GPUAPICHECK(err, cudaSuccess, true);
    }

    err = cudaMemsetAsync(mem, value, count, stream);
    GPUAPICHECK(err, cudaSuccess, true);


    if (completion)
    {
      err = cudaEventRecord(completion->detail()->obj(), stream);
      GPUAPICHECK(err, cudaSuccess, true);
    }
    return false;
  }

  if (block_on)
  {
    block_on->wait();
  }

  err = cudaMemset(mem, value, count);
  GPUAPICHECK(err, cudaSuccess, true);

  if (completion)
  {
    // Release completion event. Should not have been provided without queue argument.
    completion->release();
  }
  return true;
}


cudaError_t bufferAlloc(BufferDetail *buf, size_t alloc_size)
{
  cudaError_t err = cudaSuccess;

  logutil::trace("gputil::gpuBuffer: allocating ", logutil::Bytes(alloc_size, logutil::ByteMagnitude::kByte));
  err = cudaMalloc(&buf->device_mem, alloc_size);

  if (err == cudaSuccess)
  {
    buf->alloc_size = alloc_size;

    if (buf->flags & kBfHostAccess)
    {
      // Allocate mapped memory buffer too.
      err = cudaHostAlloc(&buf->mapped_mem, alloc_size, 0);
      if (err == cudaErrorMemoryAllocation)
      {
        // Ignore error. Ok to run out of mapped memory.
        err = cudaSuccess;
      }
      else
      {
        return err;
      }
    }
  }
  else
  {
    buf->alloc_size = 0;
  }
  return err;
}

void bufferFree(BufferDetail *buf)
{
  if (buf && buf->device_mem)
  {
    logutil::trace("gputil::gpuBuffer freeing ", logutil::Bytes(buf->alloc_size, logutil::ByteMagnitude::kByte));
    cudaFree(buf->device_mem);
    if (buf->mapped_mem)
    {
      cudaFreeHost(buf->mapped_mem);
    }
    buf->device_mem = nullptr;
    buf->mapped_mem = nullptr;
    buf->alloc_size = 0;
    buf->pinned_status = 0;
    buf->dirty_write.clear();
  }
}

size_t resizeBuffer(BufferDetail *imp, size_t new_size, bool force)
{
  if (new_size == imp->alloc_size || !force && new_size < imp->alloc_size)
  {
    return imp->alloc_size;
  }

  bufferFree(imp);
  cudaError_t err = bufferAlloc(imp, new_size);
  if (err != cudaSuccess)
  {
    imp->device_mem = nullptr;
    imp->alloc_size = 0;
    GPUTHROW(ApiException(err, nullptr, __FILE__, __LINE__), 0);
  }
  imp->alloc_size = new_size;

  return imp->alloc_size;
}

Buffer::Buffer()
  : imp_(new BufferDetail)
{
  imp_->flags = 0;
}


Buffer::Buffer(const Device &device, size_t byte_size, unsigned flags)
  : imp_(new BufferDetail)
{
  create(device, byte_size, flags);
}


Buffer::Buffer(Buffer &&other) noexcept
  : imp_(other.imp_)
{
  other.imp_ = nullptr;
}


Buffer::~Buffer()
{
  release();
  delete imp_;
}


Buffer &Buffer::operator=(Buffer &&other) noexcept
{
  bufferFree(imp_);
  delete imp_;
  imp_ = other.imp_;
  other.imp_ = nullptr;
  return *this;
}


void Buffer::create(const Device &device, size_t byte_size, unsigned flags)
{
  release();
  imp_->device = device;
  imp_->flags = flags;

  resize(byte_size);
}


void Buffer::release()
{
  bufferFree(imp_);
}


void Buffer::swap(Buffer &other) noexcept
{
  std::swap(imp_, other.imp_);
}


bool Buffer::isValid() const
{
  return imp_->device_mem != nullptr;
}


unsigned Buffer::flags() const
{
  return imp_->flags;
}


size_t Buffer::size() const
{
  return imp_->alloc_size;
}


size_t Buffer::actualSize() const
{
  return imp_->alloc_size;
}


size_t Buffer::resize(size_t new_size)
{
  return resizeBuffer(imp_, new_size, false);
}


size_t Buffer::forceResize(size_t new_size)
{
  resizeBuffer(imp_, new_size, true);
  return actualSize();
}


void Buffer::fill(const void *pattern, size_t pattern_size, Queue *queue, Event *block_on, Event *completion)
{
  if (isValid())
  {
    if (pattern_size == sizeof(int))
    {
      bufferSet(imp_->device_mem, *static_cast<const int *>(pattern), imp_->alloc_size, queue, block_on, completion);
    }
    else
    {
      size_t wrote = 0;
      auto *dst_mem = static_cast<uint8_t *>(imp_->device_mem);
      while (wrote + pattern_size < imp_->alloc_size)
      {
        bufferCopy(dst_mem + wrote, pattern, pattern_size, cudaMemcpyHostToDevice, queue, block_on, nullptr);
        wrote += pattern_size;
      }

      // Last copy.
      if (wrote < imp_->alloc_size)
      {
        bufferCopy(dst_mem + wrote, pattern, imp_->alloc_size - wrote, cudaMemcpyHostToDevice, queue, block_on,
                   completion);
        // wrote += imp_->alloc_size - wrote;
      }
    }
  }
}


void Buffer::fillPartial(const void *pattern, size_t pattern_size, size_t fill_bytes, size_t offset, Queue *queue)
{
  if (isValid())
  {
    if (offset > imp_->alloc_size)
    {
      return;
    }

    if (offset + fill_bytes > imp_->alloc_size)
    {
      fill_bytes = imp_->alloc_size - offset;
    }

    uint8_t *dst_mem = static_cast<uint8_t *>(imp_->device_mem) + offset;
    if (pattern_size == sizeof(int))
    {
      bufferSet(dst_mem, *static_cast<const int *>(pattern), fill_bytes, queue, nullptr, nullptr);
    }
    else
    {
      size_t wrote = 0;
      while (wrote + pattern_size < fill_bytes)
      {
        bufferCopy(dst_mem + wrote, pattern, pattern_size, cudaMemcpyHostToDevice, queue, nullptr, nullptr);
        wrote += pattern_size;
      }

      // Last copy.
      if (wrote < fill_bytes)
      {
        bufferCopy(dst_mem + wrote + offset, pattern, fill_bytes - wrote, cudaMemcpyHostToDevice, queue, nullptr,
                   nullptr);
        // wrote += fill_bytes - wrote;
      }
    }
  }
}


size_t Buffer::read(void *dst, size_t read_byte_count, size_t src_offset, Queue *queue, Event *block_on,
                    Event *completion)
{
  size_t copy_bytes = 0;
  if (imp_ && imp_->device_mem)
  {
    if (src_offset >= imp_->alloc_size)
    {
      return 0;
    }

    copy_bytes = read_byte_count;
    if (copy_bytes > imp_->alloc_size - src_offset)
    {
      copy_bytes = imp_->alloc_size - src_offset;
    }

    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    bufferCopy(dst, reinterpret_cast<const uint8_t *>(imp_->device_mem) + src_offset, copy_bytes,
               cudaMemcpyDeviceToHost, queue, block_on, completion);
  }
  return copy_bytes;
}


size_t Buffer::write(const void *src, size_t write_byte_count, size_t dst_offset, Queue *queue, Event *block_on,
                     Event *completion)
{
  size_t copy_bytes = 0;
  if (imp_ && imp_->device_mem)
  {
    if (dst_offset >= imp_->alloc_size)
    {
      return 0;
    }

    copy_bytes = write_byte_count;
    if (write_byte_count > imp_->alloc_size - dst_offset)
    {
      copy_bytes = imp_->alloc_size - dst_offset;
    }

    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    bufferCopy(reinterpret_cast<uint8_t *>(imp_->device_mem) + dst_offset, src, copy_bytes, cudaMemcpyHostToDevice,
               queue, block_on, completion);
  }
  return copy_bytes;
}


size_t Buffer::readElements(void *dst, size_t element_size, size_t element_count, size_t offset_elements,
                            size_t buffer_element_size, Queue *queue, Event *block_on, Event *completion)
{
  if (element_size == buffer_element_size || buffer_element_size == 0)
  {
    return read(dst, element_size * element_count, offset_elements * element_size, queue, block_on, completion);
  }

  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  const auto *src = reinterpret_cast<const uint8_t *>(imp_->device_mem);
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  auto *dst2 = reinterpret_cast<uint8_t *>(dst);
  size_t copy_size = std::min(element_size, buffer_element_size);
  size_t src_offset = 0;

  src += offset_elements * buffer_element_size;
  for (size_t i = 0; i < element_count && src_offset <= imp_->alloc_size; ++i)
  {
    const bool final_copy = i + 1 == element_count || src_offset + buffer_element_size > imp_->alloc_size;
    bufferCopy(dst2, src + src_offset, copy_size, cudaMemcpyDeviceToHost, queue, block_on,
               (!final_copy) ? nullptr : completion);

    dst2 += element_size;
    src_offset += buffer_element_size;
  }

  return src_offset;
}


size_t Buffer::writeElements(const void *src, size_t element_size, size_t element_count, size_t offset_elements,
                             size_t buffer_element_size, Queue *queue, Event *block_on, Event *completion)
{
  if (element_size == buffer_element_size || buffer_element_size == 0)
  {
    return write(src, element_size * element_count, offset_elements * element_size, queue, block_on, completion);
  }

  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  auto *dst = reinterpret_cast<uint8_t *>(imp_->device_mem);
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  const auto *src2 = reinterpret_cast<const uint8_t *>(src);
  size_t copy_size = std::min(element_size, buffer_element_size);
  size_t dst_offset = 0;

  dst += offset_elements * buffer_element_size;
  for (size_t i = 0; i < element_count && dst_offset <= imp_->alloc_size; ++i)
  {
    const bool final_copy = i + 1 == element_count || dst_offset + buffer_element_size > imp_->alloc_size;
    bufferCopy(dst + dst_offset, src2, copy_size, cudaMemcpyDeviceToHost, queue, block_on,
               (!final_copy) ? nullptr : completion);

    dst += element_size;
    dst_offset += buffer_element_size;
  }

  return dst_offset;
}


void *Buffer::argPtr() const
{
  return &imp_->device_mem;
}


void *Buffer::address() const
{
  return imp_->device_mem;
}


size_t copyBuffer(Buffer &dst, const Buffer &src, Queue *queue, Event *block_on, Event *completion)
{
  return copyBuffer(dst, 0, src, 0, src.size(), queue, block_on, completion);
}


size_t copyBuffer(Buffer &dst, const Buffer &src, size_t byte_count, Queue *queue, Event *block_on, Event *completion)
{
  return copyBuffer(dst, 0, src, 0, byte_count, queue, block_on, completion);
}


size_t copyBuffer(Buffer &dst, size_t dst_offset, const Buffer &src, size_t src_offset, size_t byte_count, Queue *queue,
                  Event *block_on, Event *completion)
{
  BufferDetail *dst_detail = dst.detail();
  BufferDetail *src_detail = src.detail();
  if (!src_detail || !dst_detail)
  {
    return 0;
  }

  // Check offsets.
  if (dst_detail->alloc_size < dst_offset)
  {
    return 0;
  }

  if (src_detail->alloc_size < src_offset)
  {
    return 0;
  }

  // Check sizes after offset.
  byte_count = std::min(byte_count, dst_detail->alloc_size - dst_offset);
  byte_count = std::min(byte_count, src_detail->alloc_size - src_offset);

  void *dst_address = static_cast<uint8_t *>(dst_detail->device_mem) + dst_offset;
  const void *src_address = static_cast<const uint8_t *>(src_detail->device_mem) + src_offset;
  bufferCopy(dst_address, src_address, byte_count, cudaMemcpyDeviceToDevice, queue, block_on, completion);

  return byte_count;
}
}  // namespace gputil


================================================
FILE: gputil/cuda/gpuBufferDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef GPUBUFFERDETAIL_H
#define GPUBUFFERDETAIL_H

#include "gputil/gpuDevice.h"

#include "gputil/cuda/gpuMemRegion.h"

#include <cuda_runtime.h>

namespace gputil
{
enum PinStatus : unsigned
{
  kPinnedForRead = (1u << 0u),
  kPinnedForWrite = (1u << 1u)
};

struct BufferDetail
{
  void *device_mem = nullptr;
  /// Host mapped memory for pinning. Maintained by gpuPinnedBuffer code.
  void *mapped_mem = nullptr;
  size_t alloc_size = 0;
  unsigned flags = 0;
  unsigned pinned_status = 0;
  Device device;
  /// List of dirty regions which need to be copied from mapped_mep to device_mem.
  std::vector<MemRegion> dirty_write;
};

bool bufferCopy(void *dst, const void *src, size_t byte_count, cudaMemcpyKind kind, Queue *queue, Event *block_on,
                Event *completion);
uint8_t *pin(BufferDetail &buf, PinMode mode);
void unpin(BufferDetail &imp, void *pinned_ptr, PinMode mode, Queue *queue, Event *block_on, Event *completion);
}  // namespace gputil

#endif  // GPUBUFFERDETAIL_H


================================================
FILE: gputil/cuda/gpuDevice.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "gputil/gpuDevice.h"

#include "gputil/cuda/gpuDeviceDetail.h"

#include "gputil/gpuApiException.h"
#include "gputil/gpuThrow.h"

#include <cuda.h>
#include <cuda_runtime.h>

#include <algorithm>
#include <cstring>
#include <sstream>

namespace gputil
{
namespace
{
void initDeviceInfo(DeviceInfo &gputil_info, const cudaDeviceProp &cuda_info)
{
  gputil_info.name = cuda_info.name;
  gputil_info.platform = "CUDA";  // cuda_info.name;
  gputil_info.version.major = cuda_info.major;
  gputil_info.version.minor = cuda_info.minor;
  gputil_info.version.patch = 0;
  gputil_info.type = kDeviceGpu;
}

bool selectDevice(DeviceDetail &detail, int device_id = -1)
{
  cudaError_t err = cudaSuccess;

  if (device_id < 0)
  {
    // Select default device.
    err = cudaGetDevice(&device_id);
  }

  if (err == cudaSuccess)
  {
    cudaDeviceProp cuda_info{};
    memset(&cuda_info, 0, sizeof(cuda_info));
    if (cudaGetDeviceProperties(&cuda_info, device_id) == cudaSuccess)
    {
      detail.device = device_id;
      initDeviceInfo(detail.info, cuda_info);
      return true;
    }
  }

  detail.device = -1;
  detail.info = DeviceInfo();
  detail.default_queue = Queue(nullptr);

  return false;
}
}  // namespace

Device::Device(bool default_device)
  : imp_(std::make_unique<DeviceDetail>())
{
  imp_->device = -1;
  if (default_device)
  {
    selectDevice(*imp_);
  }
}


Device::Device(const DeviceInfo &device_info)
  : imp_(std::make_unique<DeviceDetail>())
{
  select(device_info);
}


Device::Device(int argc, const char **argv, const char *default_device, unsigned device_type_flags)
  : imp_(std::make_unique<DeviceDetail>())
{
  select(argc, argv, default_device, device_type_flags);
}


Device::Device(const Device &other)
  : imp_(std::make_unique<DeviceDetail>())
{
  *imp_ = *other.imp_;
}


Device::Device(Device &&other) noexcept
  : imp_(std::move(other.imp_))
{}


Device::~Device() = default;


unsigned Device::enumerateDevices(std::vector<DeviceInfo> &devices)
{
  int device_count = 0;
  cudaGetDeviceCount(&device_count);

  for (int i = 0; i < device_count; ++i)
  {
    cudaDeviceProp cuda_info{};
    DeviceInfo gputil_info;
    memset(&cuda_info, 0, sizeof(cuda_info));
    if (cudaGetDeviceProperties(&cuda_info, i) == cudaSuccess)
    {
      initDeviceInfo(gputil_info, cuda_info);
      devices.push_back(gputil_info);
    }
  }

  return static_cast<unsigned>(device_count);
}


const char *Device::name() const
{
  return imp_->name.c_str();
}


const char *Device::description() const
{
  return imp_->name.c_str();
}


const DeviceInfo &Device::info() const
{
  return imp_->info;
}


Queue Device::defaultQueue() const
{
  return imp_->default_queue;
}


Queue Device::createQueue(unsigned /*flags*/) const
{
  cudaStream_t stream = nullptr;
  cudaError_t err;
  err = cudaSetDevice(imp_->device);
  GPUAPICHECK(err, cudaSuccess, Queue());
  err = cudaStreamCreate(&stream);
  GPUAPICHECK(err, cudaSuccess, Queue());
  return Queue(stream);
}


bool Device::select(int argc, const char **argv, const char *default_device, unsigned device_type_flags)
{
  if ((device_type_flags & kGpu) == 0)
  {
    // Only GPU supported for CUDA.
    return false;
  }

  // Resolve constraints:
  // - (Partial) device name match
  // - Compute capability version number.

  std::string name_constraint = default_device ? default_device : "";
  Version version_min;

  const std::string device_arg = "--device";
  const std::string clver_arg = "--clver";
  for (int i = 0; i < argc; ++i)
  {
    if (strncmp(device_arg.c_str(), argv[i], device_arg.size()) == 0)
    {
      // Read value from "--device="
      if (argv[i][device_arg.size()] == '=')
      {
        // Clip '='
        name_constraint = argv[i] + device_arg.size() + 1;
        // Strip quotes
        if (!name_constraint.empty())
        {
          if (name_constraint.front() == '"')
          {
            name_constraint.erase(name_constraint.begin());
            if (!name_constraint.empty())
            {
              if (name_constraint.back() == '"')
              {
                name_constraint.pop_back();
              }
            }
          }
        }
      }
    }
    else if (strncmp(clver_arg.c_str(), argv[i], clver_arg.size()) == 0)
    {
      // Read version from "--clver="
      if (argv[i][clver_arg.size()] == '=')
      {
        char dot;
        std::string ver_str = argv[i] + clver_arg.size() + 1;
        std::istringstream in(ver_str);
        in >> version_min.major;
        in >> dot;
        in >> version_min.minor;
      }
    }
  }

  std::vector<DeviceInfo> devices;
  enumerateDevices(devices);

  std::transform(name_constraint.begin(), name_constraint.end(), name_constraint.begin(), ::tolower);

  const auto name_check = [](const std::string &device_name, const std::string &constraint) -> bool  //
  {
    if (constraint.empty())
    {
      return true;
    }

    std::string device_lower = device_name;
    std::transform(device_lower.begin(), device_lower.end(), device_lower.begin(), ::tolower);
    return device_lower.find(constraint) != std::string::npos;
  };

  const auto ver_check = [](const Version &device_version, const Version &constraint) -> bool  //
  {
    if (device_version.major > constraint.major)
    {
      return true;
    }
    if (device_version.major == constraint.major)
    {
      if (device_version.minor > constraint.minor)
      {
        return true;
      }
      if (device_version.minor == constraint.minor)
      {
        if (device_version.patch > constraint.patch)
        {
          return true;
        }
        if (device_version.patch == constraint.patch)
        {
          return true;
        }
      }
    }

    return false;
  };

  for (auto &&device : devices)
  {
    if (name_check(device.name, name_constraint) && ver_check(device.version, version_min))
    {
      return select(device);
    }
  }

  return false;
}


bool Device::select(const DeviceInfo &device_info)
{
  int device_count = 0;
  cudaGetDeviceCount(&device_count);

  for (int i = 0; i < device_count; ++i)
  {
    cudaDeviceProp cuda_info{};
    DeviceInfo gputil_info;
    memset(&cuda_info, 0, sizeof(cuda_info));
    if (cudaGetDeviceProperties(&cuda_info, i) == cudaSuccess)
    {
      initDeviceInfo(gputil_info, cuda_info);
      if (gputil_info == device_info)
      {
        return selectDevice(*imp_, i);
      }
    }
  }

  return false;
}


// Lint(KS): required for API compatibility
// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
void Device::setDebugGpu(DebugLevel /*level*/)
{
  // Ignored for CUDA.
}


// Lint(KS): required for API compatibility
// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
Device::DebugLevel Device::debugGpu() const
{
  return kDlOff;
}


// Lint(KS): required for API compatibility
// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
bool Device::supportsFeature(const char * /*feature_id*/) const
{
  // TODO(KS): find CUDA feature checking or use compute capabilities.
  return false;
}


// Lint(KS): required for API compatibility
// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
void Device::addSearchPath(const char * /*path*/)
{
  // Ignored for CUDA.
}


// Lint(KS): required for API compatibility
// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
const char *Device::searchPaths() const
{
  return "";
}


bool Device::isValid() const
{
  return imp_ && imp_->device >= 0;
}


uint64_t Device::deviceMemory() const
{
  cudaError_t err = cudaSuccess;
  cudaDeviceProp cuda_info{};
  memset(&cuda_info, 0, sizeof(cuda_info));
  err = cudaGetDeviceProperties(&cuda_info, imp_->device);
  GPUAPICHECK(err, cudaSuccess, 0u);
  return cuda_info.totalGlobalMem;
}


uint64_t Device::maxAllocationSize() const
{
  // Is this right?
  return deviceMemory();
}


bool Device::unifiedMemory() const
{
  cudaDeviceProp cuda_info{};
  memset(&cuda_info, 0, sizeof(cuda_info));
  cudaError_t err = cudaGetDeviceProperties(&cuda_info, imp_->device);
  GPUAPICHECK(err, cudaSuccess, false);
  // This is a bit of an assumption.
  return cuda_info.integrated != 0;
}


Device &Device::operator=(const Device &other)
{
  if (this != &other)
  {
    *imp_ = *other.imp_;
  }
  return *this;
}


Device &Device::operator=(Device &&other) noexcept
{
  imp_ = std::move(other.imp_);
  return *this;
}
}  // namespace gputil

================================================
FILE: gputil/cuda/gpuDeviceDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef GPUDEVICEDETAIL_H
#define GPUDEVICEDETAIL_H

#include "gpuConfig.h"

#include "gputil/gpuDeviceInfo.h"
#include "gputil/gpuQueue.h"

#include <cuda_runtime.h>

#include <string>

namespace gputil
{
struct DeviceDetail
{
  int device = -1;
  std::string name;
  DeviceInfo info;
  Queue default_queue;
};
}  // namespace gputil

#endif  // GPUDEVICEDETAIL_H


================================================
FILE: gputil/cuda/gpuDirtyRegion
================================================


================================================
FILE: gputil/cuda/gpuEvent.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gputil/gpuEvent.h"

#include "gputil/cuda/gpuEventDetail.h"

#include "gputil/gpuApiException.h"
#include "gputil/gpuThrow.h"

#include <cuda.h>
#include <cuda_runtime.h>

namespace gputil
{
void destroyEvent(cudaEvent_t event)
{
  if (event)
  {
    cudaError_t err = cudaEventDestroy(event);
    event = nullptr;
    GPUAPICHECK2(err, cudaSuccess);
  }
}

Event::Event() = default;

Event::Event(const Event &other)
{
  if (other.imp_)
  {
    imp_ = other.imp_;
    imp_->reference();
  }
}


Event::Event(Event &&other) noexcept
  : imp_(other.imp_)
{
  other.imp_ = nullptr;
}


Event::~Event()
{
  release();
}


bool Event::isValid() const
{
  return imp_ && imp_->obj();
}


void Event::release()
{
  if (imp_)
  {
    imp_->release();
    imp_ = nullptr;
  }
}


bool Event::isComplete() const
{
  if (!isValid())
  {
    return true;
  }

  cudaError_t status = cudaEventQuery(imp_->obj());
  switch (status)
  {
  case cudaSuccess:
  case cudaErrorNotReady:
    break;
  default:
    GPUAPICHECK(status, cudaSuccess, true);
    break;
  }

  return status == cudaSuccess;
}


void Event::wait() const
{
  if (imp_ && imp_->obj())
  {
    cudaError_t err = cudaEventSynchronize(imp_->obj());
    GPUAPICHECK2(err, cudaSuccess);
  }
}


void Event::wait(const Event *events, size_t event_count)
{
  for (size_t i = 0; i < event_count; ++i)
  {
    events[i].wait();
  }
}


void Event::wait(const Event **events, size_t event_count)
{
  for (size_t i = 0; i < event_count; ++i)
  {
    events[i]->wait();
  }
}


Event &Event::operator=(const Event &other)
{
  if (this != &other)
  {
    release();
    if (other.imp_)
    {
      imp_ = other.imp_;
      imp_->reference();
    }
  }

  return *this;
}


Event &Event::operator=(Event &&other) noexcept
{
  release();
  imp_ = other.imp_;
  other.imp_ = nullptr;
  return *this;
}


EventDetail *Event::detail()
{
  // Detail requested. Allocate if required.
  if (!imp_)
  {
    cudaEvent_t event = nullptr;
    cudaError_t err = cudaEventCreateWithFlags(&event, cudaEventBlockingSync);
    GPUAPICHECK(err, cudaSuccess, nullptr);
    imp_ = new EventDetail(event, 1, &destroyEvent);  // NOLINT(cppcoreguidelines-owning-memory)
  }
  return imp_;
}


EventDetail *Event::detail() const
{
  return imp_;
}
}  // namespace gputil


================================================
FILE: gputil/cuda/gpuEventDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUEVENTDETAIL_H
#define GPUEVENTDETAIL_H

#include "gpuConfig.h"

#include "gputil/cuda/ref.h"

#include <cuda_runtime.h>

#include <utility>

namespace gputil
{
void destroyEvent(cudaEvent_t event);

struct EventDetail : public Ref<cudaEvent_t>
{
  inline EventDetail(cudaEvent_t obj, unsigned initial_ref_count, const ReleaseFunc &release)
    : Ref<cudaEvent_t>(obj, initial_ref_count, release)
  {}

  explicit inline EventDetail(Ref &&other) noexcept
    : Ref<cudaEvent_t>(std::move(other))
  {}

  inline EventDetail(const Ref &other) = delete;

protected:
  inline ~EventDetail() = default;
};
}  // namespace gputil

#endif  // GPUEVENTDETAIL_H


================================================
FILE: gputil/cuda/gpuKernel.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuKernel.h"

#include "gputil/gpuApiException.h"
#include "gputil/gpuBuffer.h"
#include "gputil/gpuDevice.h"
#include "gputil/gpuEventList.h"
#include "gputil/gpuThrow.h"

#include "gputil/cuda/gpuBufferDetail.h"
#include "gputil/cuda/gpuDeviceDetail.h"
#include "gputil/cuda/gpuEventDetail.h"
#include "gputil/cuda/gpuKernelDetail.h"
#include "gputil/cuda/gpuQueueDetail.h"

#include <cuda_runtime.h>

#include <algorithm>
#include <cstring>

namespace gputil
{
namespace cuda
{
int preInvokeKernel(const Device &device)
{
  // Select device.
  cudaError_t err = cudaSuccess;
  err = cudaSetDevice(device.detail()->device);
  GPUAPICHECK(err, cudaSuccess, err);
  // cudaDeviceProp info;
  // memset(&info, 0, sizeof(info));
  // err = cudaGetDeviceProperties(&info, device.detail()->device);
  // GPUAPICHECK(err, cudaSuccess, err);
  return err;
}


size_t calcSharedMemSize(const KernelDetail &imp, size_t block_threads)
{
  // To calculate the total size, sum the desired sizes with 8 byte alignment in between. This may be larger than
  // required.
  size_t shared_mem_size = 0u;
  const size_t alignment = 8u;
  size_t alignment_remainder = 0;
  for (auto &&local_mem_func : imp.local_mem_args)
  {
    shared_mem_size += local_mem_func(block_threads);
    // Align to 8 bytes.
    alignment_remainder = shared_mem_size % alignment;
    shared_mem_size += !!(alignment_remainder) * (alignment - alignment_remainder);
  }
  return shared_mem_size;
}


int invokeKernel(const KernelDetail &imp, const Dim3 &global_size, const Dim3 &local_size, const EventList *event_list,
                 Event *completion_event, Queue *queue, void **args, size_t /*arg_count*/)
{
  cudaError_t err = cudaSuccess;

  // TODO(KS): Enable once kernel definition arguments are counted.
  // if (arg_count != imp.arg_count)
  // {
  //   err = cudaErrorLaunchFailure;
  //   const char *msg = (arg_count > imp.arg_count) ? "too many arguments" : "too few arguments";
  //   GPUTHROW(ApiException(err, msg, __FILE__, __LINE__), err);
  //   return err;
  // }

  // Resolve shared memory size
  const size_t shared_mem_size = calcSharedMemSize(imp, local_size.volume());

  // Resolve cuda stream.
  cudaStream_t cuda_stream = (queue) ? queue->internal()->queue : nullptr;

  // Make the stream wait on the event list.
  if (event_list)
  {
    const Event *event = event_list->events();
    for (size_t i = 0; i < event_list->count(); ++i, ++event)
    {
      if (event && event->detail() && event->detail()->obj())
      {
        cudaEvent_t cuda_event = event->detail()->obj();
        const unsigned flags = 0u;  // Must be zero at the time of implementation.
        err = cudaStreamWaitEvent(cuda_stream, cuda_event, flags);
        GPUAPICHECK(err, cudaSuccess, err);
      }
    }
  }

  // int n = 0;
  // std::vector<void *> dummy_args(5);
  // for (int i = 0; i < 5; ++i)
  // {
  //   dummy_args[i] = nullptr;
  // }
  // dummy_args[3] = &n;

  // args = dummy_args.data();

  // Launch kernel.
  dim3 grid_dim;
  grid_dim.x = (local_size.x) ? unsigned((global_size.x + local_size.x - 1) / local_size.x) : 1u;
  grid_dim.y = (local_size.y) ? unsigned((global_size.y + local_size.y - 1) / local_size.y) : 1u;
  grid_dim.z = (local_size.z) ? unsigned((global_size.z + local_size.z - 1) / local_size.z) : 1u;
  err = cudaLaunchKernel(imp.cuda_kernel_function, grid_dim,
                         dim3(unsigned(local_size.x), unsigned(local_size.y), unsigned(local_size.z)), args,
                         shared_mem_size, cuda_stream);
  GPUAPICHECK(err, cudaSuccess, err);

  // Hook up completion event.
  if (completion_event)
  {
    // Create new event.
    completion_event->release();
    err = cudaEventRecord(completion_event->detail()->obj(), cuda_stream);
    GPUAPICHECK(err, cudaSuccess, err);
  }

  if (imp.auto_error_checking)
  {
    GPUAPICHECK(err, cudaSuccess, err);
  }

  if (queue && queue->internal()->force_synchronous)
  {
    // Force kernel completion if in synchronous mode.
    queue->finish();
  }

  return err;
}
}  // namespace cuda

Kernel::Kernel()
  : imp_(new KernelDetail)
{}


Kernel::Kernel(Kernel &&other) noexcept
  : imp_(other.imp_)
{
  other.imp_ = nullptr;
}


Kernel::~Kernel()
{
  delete imp_;
}


bool Kernel::isValid() const
{
  return imp_ && imp_->cuda_kernel_function;
}


void Kernel::setErrorChecking(bool check)
{
  imp_->auto_error_checking = check;
}


bool Kernel::errorChecking() const
{
  return imp_->auto_error_checking;
}


void Kernel::release()
{
  delete imp_;
  imp_ = nullptr;
}

void Kernel::addLocal(const std::function<size_t(size_t)> &local_calc)
{
  imp_->local_mem_args.push_back(local_calc);
}


size_t Kernel::calculateOptimalWorkGroupSize()
{
  const size_t default_workgroup_size = 8;
  if (!imp_->maximum_potential_workgroup_size)
  {
    int err = cuda::preInvokeKernel(device());
    GPUAPICHECK(err, cudaSuccess, 0u);
    const auto calc_shared_mem_size = [this](size_t block_size)  //
    {                                                            //
      return cuda::calcSharedMemSize(*imp_, block_size);
    };
    err = imp_->optimal_group_size_calc(&imp_->maximum_potential_workgroup_size, calc_shared_mem_size);
    if (!imp_->maximum_potential_workgroup_size)
    {
      imp_->maximum_potential_workgroup_size = default_workgroup_size;
    }
    GPUAPICHECK(err, cudaSuccess, 0);
  }

  return imp_->maximum_potential_workgroup_size;
}


size_t Kernel::optimalWorkGroupSize() const
{
  return imp_->maximum_potential_workgroup_size;
}

void Kernel::calculateGrid(gputil::Dim3 *global_size, gputil::Dim3 *local_size, const gputil::Dim3 &total_work_items)
{
  if (!isValid())
  {
    *global_size = *local_size = Dim3(0, 0, 0);
    return;
  }

  const size_t calc_volume = total_work_items.x * total_work_items.y * total_work_items.z;
  const size_t target_group_size = std::min(calculateOptimalWorkGroupSize(), calc_volume);
  const Device &gpu = imp_->program.device();

  cudaError_t err = cudaSuccess;
  cudaDeviceProp cuda_info{};
  memset(&cuda_info, 0, sizeof(cuda_info));
  err = cudaGetDeviceProperties(&cuda_info, gpu.detail()->device);
  GPUAPICHECK2(err, cudaSuccess);

  // Try to setup the workgroup as a cubic spatial division.
  const double cube_root = 1.0f / 3.0f;
  auto target_dimension_value = unsigned(std::floor(std::pow(double(target_group_size), cube_root)));
  if (target_dimension_value < 1)
  {
    target_dimension_value = 1;
  }

  // Set the target dimensions to the minimum of the target and the max work group size.
  const double sqr_root = 1.0f / 2.0f;
  local_size->z = std::min<size_t>(cuda_info.maxThreadsDim[2], target_dimension_value);
  // Lint(KS): division should be OK to convert to double for square root, then to int.
  // NOLINTNEXTLINE(bugprone-integer-division)
  target_dimension_value = unsigned(std::floor(std::pow(double(target_group_size / local_size->z), sqr_root)));
  local_size->y = std::min<size_t>(cuda_info.maxThreadsDim[1], target_dimension_value);
  target_dimension_value = unsigned(std::max<size_t>(target_group_size / (local_size->y * local_size->z), 1));
  local_size->x = std::min<size_t>(cuda_info.maxThreadsDim[0], target_dimension_value);

  // Reduce size to <= targetGroupSize
  int i = 2;  // Start by reducing 3rd dimension.
  while (i >= 0 && local_size->volume() > target_group_size)
  {
    if ((*local_size)[i] > 1)
    {
      (*local_size)[i] = (*local_size)[i] - 1;
    }
    else
    {
      // Don't reduce to less than 1. Next dimension.
      --i;
    }
  }

  // max work item sizes
  global_size->x = local_size->x * (total_work_items.x + local_size->x - 1) / local_size->x;
  global_size->y = local_size->y * (total_work_items.y + local_size->y - 1) / local_size->y;
  global_size->z = local_size->z * (total_work_items.z + local_size->z - 1) / local_size->z;
}


Device Kernel::device()
{
  if (!isValid())
  {
    return Device();
  }

  return imp_->program.device();
}


Kernel &Kernel::operator=(Kernel &&other) noexcept
{
  delete imp_;
  imp_ = other.imp_;
  other.imp_ = nullptr;
  return *this;
}


bool Kernel::checkResult(int invocation_result, bool allow_exceptions)
{
  (void)allow_exceptions;  // Unused if GPU_EXCEPTIONS disabled
  if (invocation_result != cudaSuccess)
  {
    auto exception = ApiException(invocation_result, "Kernel invocation error", __FILE__, __LINE__);
#if GPU_EXCEPTIONS
    if (allow_exceptions)
    {
      throw exception;
    }
    else
#endif  // GPU_EXCEPTIONS
    {
      log(exception);
      return false;
    }
  }
  return true;
}


Kernel cudaKernel(Program &program, const void *kernel_function_ptr,
                  const gputil::OptimalGroupSizeCalculation &group_calc)
{
  Kernel kernel;
  kernel.detail()->cuda_kernel_function = kernel_function_ptr;
  kernel.detail()->optimal_group_size_calc = group_calc;
  kernel.detail()->program = program;
  // TODO(KS): count arguments
  return kernel;
}
}  // namespace gputil


================================================
FILE: gputil/cuda/gpuKernel2.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUKERNEL2_H
#define GPUKERNEL2_H

#include "gpuConfig.h"

#include "gputil/gpuApiException.h"
#include "gputil/gpuBuffer.h"
#include "gputil/gpuEventList.h"
#include "gputil/gpuThrow.h"

#include "gputil/cuda/gpuBufferDetail.h"
#include "gputil/cuda/gpuEventDetail.h"
#include "gputil/cuda/gpuKernelDetail.h"
#include "gputil/cuda/gpuQueueDetail.h"

#include "gputil/cuda/cutil_decl.h"

#include <cuda_runtime.h>

#include <cstdlib>

#define GPUTIL_BUILD_FROM_FILE(program, file_name, build_args)               0
#define GPUTIL_BUILD_FROM_SOURCE(program, source, source_length, build_args) 0
#define GPUTIL_MAKE_KERNEL(program, kernel_name) \
  gputil::cudaKernel(program, kernel_name##Ptr(), kernel_name##OptimalGroupSizeCalculator())

namespace gputil
{
namespace cuda
{
inline size_t countArgs()
{
  return 0u;
}

template <typename ARG, typename... ARGS>
inline size_t countArgs(const ARG &, ARGS... args)  // NOLINT(readability-named-parameter)
{
  return 1u + countArgs(args...);
}

template <typename ARG>
inline void *collateArgPtr(ARG *arg)
{
  // Necessary evils
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-const-cast, cppcoreguidelines-pro-type-reinterpret-cast)
  return const_cast<void *>(reinterpret_cast<const void *>(arg));
}

template <typename T>
inline void *collateArgPtr(BufferArg<T> *arg)
{
  // Note(KS): this is a static variable in an inline function. The address may vary.
  static T *null_arg = nullptr;
  return arg->buffer ? arg->buffer->argPtr() : &null_arg;
}

template <typename T>
inline void *collateArgPtr(const BufferArg<T> *arg)
{
  // Note(KS): this is a static variable in an inline function. The address may vary.
  static T *null_arg = nullptr;
  return arg->buffer ? arg->buffer->argPtr() : &null_arg;
}

inline void collateArgs(unsigned /*index*/, void ** /*collated_args*/)
{
  // NOOP
}

template <typename ARG, typename... ARGS>
inline void collateArgs(unsigned index, void **collated_args, const ARG &arg, ARGS... args)
{
  collated_args[index] = collateArgPtr(arg);
  collateArgs(index + 1, collated_args, args...);
}

int gputilAPI preInvokeKernel(const Device &device);
int gputilAPI invokeKernel(const KernelDetail &imp, const Dim3 &global_size, const Dim3 &local_size,
                           const EventList *event_list, Event *completion_event, Queue *queue, void **args,
                           size_t arg_count);
}  // namespace cuda

template <typename... ARGS>
int Kernel::operator()(const Dim3 &global_size, const Dim3 &local_size, Queue *queue, ARGS... args)
{
  // Prime device
  int err = 0;
  err = cuda::preInvokeKernel(device());
  if (err)
  {
    return err;
  }

  // Collate arguments into void **
  size_t arg_count = cuda::countArgs(args...);
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  void **collated_args = (arg_count) ? reinterpret_cast<void **>(alloca(arg_count * sizeof(void *))) : nullptr;
  // Capture args by pointer as we will be packing that address into collated_args and it must stay valid.
  cuda::collateArgs(0, collated_args, &args...);

  // Invoke
  err = cuda::invokeKernel(*detail(), global_size, local_size, nullptr, nullptr, queue, collated_args, arg_count);
  return err;
}


template <typename... ARGS>
int Kernel::operator()(const Dim3 &global_size, const Dim3 &local_size, Event &completion_event, Queue *queue,
                       ARGS... args)
{
  // Prime device
  int err = 0;
  err = cuda::preInvokeKernel(device());
  if (err)
  {
    return err;
  }

  // Collate arguments into void **
  size_t arg_count = cuda::countArgs(args...);
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  void **collated_args = (arg_count) ? reinterpret_cast<void **>(alloca(arg_count * sizeof(void *))) : nullptr;
  // Capture args by pointer as we will be packing that address into collated_args and it must stay valid.
  cuda::collateArgs(0, collated_args, &args...);

  // Invoke
  err =
    cuda::invokeKernel(*detail(), global_size, local_size, nullptr, &completion_event, queue, collated_args, arg_count);
  return err;
}


template <typename... ARGS>
int Kernel::operator()(const Dim3 &global_size, const Dim3 &local_size, const EventList &event_list, Queue *queue,
                       ARGS... args)
{
  // Prime device
  int err = 0;
  err = cuda::preInvokeKernel(device());
  if (err)
  {
    return err;
  }

  // Collate arguments into void **
  size_t arg_count = cuda::countArgs(args...);
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  void **collated_args = (arg_count) ? reinterpret_cast<void **>(alloca(arg_count * sizeof(void *))) : nullptr;
  // Capture args by pointer as we will be packing that address into collated_args and it must stay valid.
  cuda::collateArgs(0, collated_args, &args...);

  // Invoke
  err = cuda::invokeKernel(*detail(), global_size, local_size, &event_list, nullptr, queue, collated_args, arg_count);
  return err;
}


template <typename... ARGS>
int Kernel::operator()(const Dim3 &global_size, const Dim3 &local_size, const EventList &event_list,
                       Event &completion_event, Queue *queue, ARGS... args)
{
  // Prime device
  int err = 0;
  err = cuda::preInvokeKernel(device());
  if (err)
  {
    return err;
  }

  // Collate arguments into void **
  size_t arg_count = cuda::countArgs(args...);
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  void **collated_args = (arg_count) ? reinterpret_cast<void **>(alloca(arg_count * sizeof(void *))) : nullptr;
  // Capture args by pointer as we will be packing that address into collated_args and it must stay valid.
  cuda::collateArgs(0, collated_args, &args...);

  // Invoke
  err = cuda::invokeKernel(*detail(), global_size, local_size, &event_list, &completion_event, queue, collated_args,
                           arg_count);
  return err;
}


Kernel gputilAPI cudaKernel(Program &program, const void *kernel_function_ptr,
                            const gputil::OptimalGroupSizeCalculation &group_calc);
}  // namespace gputil

#endif  // GPUKERNEL2_H


================================================
FILE: gputil/cuda/gpuKernelDetail.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUKERNELDETAIL_H
#define GPUKERNELDETAIL_H

#include "gpuConfig.h"

#include "gputil/gpuDevice.h"
#include "gputil/gpuProgram.h"

#include <gputil/cuda/cutil_decl.h>

#include <functional>

namespace gputil
{
struct KernelDetail
{
  const void *cuda_kernel_function = nullptr;
  OptimalGroupSizeCalculation optimal_group_size_calc;
  size_t arg_count = 0u;
  std::vector<std::function<size_t(size_t)>> local_mem_args;
  size_t maximum_potential_workgroup_size = 0;
  Program program;
  bool auto_error_checking = true;
};
}  // namespace gputil

#endif  // GPUKERNELDETAIL_H


================================================
FILE: gputil/cuda/gpuMemRegion.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuMemRegion.h"

namespace gputil
{
void MemRegion::mergeRegionList(std::vector<MemRegion> &regions)
{
  if (regions.empty())
  {
    return;
  }

  // Process the dirty list, copying regions back to the device.
  // First sort and merge the dirty list.
  std::sort(regions.begin(), regions.end());

  // Merge regions in the dirty list.
  auto merge_iter = regions.begin();
  for (auto next_iter = merge_iter + 1; merge_iter != regions.end() && next_iter != regions.end();)
  {
    if (!merge_iter->overlaps(*next_iter))
    {
      // Move on to the next block.
      merge_iter = next_iter;
      ++next_iter;
    }
    else
    {
      // Merge the two items.
      merge_iter->merge(*next_iter);
      // Zero out the merged item.
      next_iter->byte_count = 0;
      // Next item
      ++next_iter;
    }
  }
}
}  // namespace gputil


================================================
FILE: gputil/cuda/gpuMemRegion.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef MEM_REGION_H
#define MEM_REGION_H

#include "gpuConfig.h"

#include <algorithm>
#include <cstddef>
#include <vector>

namespace gputil
{
class MemRegion
{
public:
  size_t offset;
  size_t byte_count;

  inline bool operator<(const MemRegion &other) const
  {
    return offset < other.offset || offset == other.offset && byte_count < other.byte_count;
  }


  inline bool overlaps(const MemRegion &other) const
  {
    const size_t start_a = offset;
    const size_t end_a = offset + byte_count;
    const size_t start_b = other.offset;
    const size_t end_b = other.offset + other.byte_count - 1;

    // Overlap if any end point falls within the other region.
    // Only test non-zero regions.
    return byte_count && other.byte_count && (start_a <= start_b && start_b <= end_a) ||
           (start_b <= start_a && start_a <= end_b) || (start_a <= end_b && end_b <= end_a) ||
           (start_b <= end_a && end_a <= end_b);
  }

  inline MemRegion &merge(const MemRegion &other)
  {
    const size_t end_a = offset + byte_count;
    const size_t end_b = other.offset + other.byte_count;
    offset = std::min(offset, other.offset);
    byte_count = std::max(end_a, end_b) - offset;
    return *this;
  }

  /// Sorts and merged the list @p regions. The merged regions are reduced to zero size, but left in the list.
  /// @param regions The list to sort and merge.
  static void mergeRegionList(std::vector<MemRegion> &regions);
};
}  // namespace gputil

#endif  // MEM_REGION_H


================================================
FILE: gputil/cuda/gpuPinnedBuffer.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuPinnedBuffer.h"

#include "gputil/cuda/gpuBufferDetail.h"
#include "gputil/cuda/gpuQueueDetail.h"
#include "gputil/gpuApiException.h"
#include "gputil/gpuBuffer.h"
#include "gputil/gpuThrow.h"

#include <algorithm>
#include <cstring>
#include <mutex>

namespace gputil
{
namespace
{
void addDirty(std::vector<MemRegion> &dirty_list, const MemRegion &region)
{
  // Merge into the last region if possible. Better for contiguous writes.
  if (!dirty_list.empty() && dirty_list.back().overlaps(region))
  {
    dirty_list.back().merge(region);
  }
  else
  {
    dirty_list.push_back(region);
  }
}
}  // namespace

PinnedBuffer::PinnedBuffer() = default;


PinnedBuffer::PinnedBuffer(Buffer &buffer, PinMode mode)
  : buffer_(&buffer)
  , mode_(mode)
{
  pin();
}


PinnedBuffer::PinnedBuffer(PinnedBuffer &&other) noexcept
  : buffer_(std::exchange(other.buffer_, nullptr))
  , pinned_(std::exchange(other.pinned_, nullptr))
  , mode_(std::exchange(other.mode_, kPinNone))
{}


PinnedBuffer::~PinnedBuffer()
{
  unpin();
}


bool PinnedBuffer::isPinned() const
{
  return pinned_ != nullptr;
}


void PinnedBuffer::pin()
{
  if (buffer_ && !pinned_)
  {
    BufferDetail *imp = buffer_->detail();
    pinned_ = gputil::pin(*imp, mode_);
  }
}


void PinnedBuffer::unpin(Queue *queue, Event *block_on, Event *completion)
{
  if (buffer_ && pinned_)
  {
    gputil::unpin(*buffer_->detail(), pinned_, mode_, queue, block_on, completion);
    pinned_ = nullptr;
  }
}


size_t PinnedBuffer::read(void *dst, size_t byte_count, size_t src_offset) const
{
  if (buffer_)
  {
    if (pinned_)
    {
      const auto *src_mem = static_cast<const uint8_t *>(pinned_);
      auto *dst_mem = static_cast<uint8_t *>(dst);

      byte_count = std::min(byte_count, buffer_->size() - src_offset);
      src_mem += src_offset;
      // Pinned pointers are host accessible. Don't need to use the CUDA API.
      memcpy(dst_mem, src_mem, byte_count);
      return byte_count;
    }

    return buffer_->read(dst, byte_count, src_offset);
  }

  return 0u;
}


size_t PinnedBuffer::write(const void *src, size_t byte_count, size_t dst_offset)
{
  if (buffer_)
  {
    if (pinned_)
    {
      const auto *src_mem = static_cast<const uint8_t *>(src);
      auto *dst_mem = static_cast<uint8_t *>(pinned_);

      byte_count = std::min(byte_count, buffer_->size() - dst_offset);
      dst_mem += dst_offset;
      // Pinned pointers are host accessible. Don't need to use the CUDA API.
      memcpy(dst_mem, src_mem, byte_count);
      addDirty(buffer_->detail()->dirty_write, MemRegion{ dst_offset, byte_count });

      return byte_count;
    }

    return buffer_->write(src, byte_count, dst_offset);
  }

  return 0u;
}


size_t PinnedBuffer::readElements(void *dst, size_t element_size, size_t element_count, size_t offset_elements,
                                  size_t buffer_element_size)
{
  if (!pinned_)
  {
    return buffer_->readElements(dst, element_size, element_count, offset_elements, buffer_element_size);
  }

  const auto *src_mem = static_cast<const uint8_t *>(pinned_);
  auto *dst_mem = static_cast<uint8_t *>(dst);
  cudaError_t err = cudaSuccess;
  if (element_size == buffer_element_size || buffer_element_size == 0)
  {
    const size_t src_offset = offset_elements * element_size;

    if (src_offset + element_size >= buffer_->size())
    {
      // Can't even copy one element from the requested offset.
      return 0;
    }

    const size_t byte_count = std::min(element_count * element_size, buffer_->size() - src_offset);
    err = cudaMemcpy(dst_mem, src_mem + src_offset, byte_count, cudaMemcpyHostToHost);
    GPUAPICHECK(err, cudaSuccess, 0u);
    return byte_count / element_count;
  }

  // Size-mismatch. Iterative copy.
  const uint8_t *src_end = src_mem + buffer_->size();
  const size_t element_copy_size = std::min(element_size, buffer_element_size);
  src_mem += offset_elements * buffer_element_size;
  size_t copy_count = 0;
  for (size_t i = 0; i < element_count && src_mem < src_end; ++i)
  {
    err = cudaMemcpy(dst_mem, src_mem, element_copy_size, cudaMemcpyHostToHost);
    GPUAPICHECK(err, cudaSuccess, 0u);
    src_mem += buffer_element_size;
    dst_mem += element_size;
    ++copy_count;
  }

  return copy_count;
}

size_t PinnedBuffer::writeElements(const void *src, size_t element_size, size_t element_count, size_t offset_elements,
                                   size_t buffer_element_size)
{
  if (!pinned_)
  {
    return buffer_->writeElements(src, element_size, element_count, offset_elements, buffer_element_size);
  }

  auto *dst_mem = static_cast<uint8_t *>(pinned_);
  const auto *src_mem = static_cast<const uint8_t *>(src);
  cudaError_t err = cudaSuccess;
  if (element_size == buffer_element_size || buffer_element_size == 0)
  {
    const size_t dst_offset = offset_elements * element_size;

    if (dst_offset + element_size >= buffer_->size())
    {
      // Can't even copy one element from to requested offset.
      return 0;
    }

    const size_t byte_count = std::min(element_count * element_size, buffer_->size() - dst_offset);
    err = cudaMemcpy(dst_mem + dst_offset, src_mem, byte_count, cudaMemcpyHostToHost);
    GPUAPICHECK(err, cudaSuccess, 0u);
    addDirty(buffer_->detail()->dirty_write, MemRegion{ dst_offset, byte_count });
    return byte_count / element_count;
  }

  // Size-mismatch. Iterative copy.
  const uint8_t *dst_end = dst_mem + buffer_->size();
  const size_t element_copy_size = std::min(element_size, buffer_element_size);
  dst_mem += offset_elements * buffer_element_size;
  size_t copy_count = 0;
  for (size_t i = 0; i < element_count && dst_mem < dst_end; ++i)
  {
    err = cudaMemcpy(dst_mem, src_mem, element_copy_size, cudaMemcpyHostToHost);
    GPUAPICHECK(err, cudaSuccess, 0u);
    addDirty(buffer_->detail()->dirty_write,
             MemRegion{ size_t(dst_mem - static_cast<uint8_t *>(pinned_)), element_copy_size });
    dst_mem += buffer_element_size;
    src_mem += element_size;
    ++copy_count;
  }

  return copy_count;
}


PinnedBuffer &PinnedBuffer::operator=(PinnedBuffer &&other) noexcept
{
  unpin();
  buffer_ = other.buffer_;
  pinned_ = other.pinned_;
  mode_ = other.mode_;
  other.buffer_ = nullptr;
  other.pinned_ = nullptr;
  return *this;
}
}  // namespace gputil


================================================
FILE: gputil/cuda/gpuPlatform2.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUPLATFORM2_H
#define GPUPLATFORM2_H

#include <cuda_runtime.h>

namespace gputil
{
using char1 = ::char1;          // NOLINT
using char2 = ::char2;          // NOLINT
using char3 = ::char3;          // NOLINT
using char4 = ::char4;          // NOLINT
using uchar = unsigned char;    // NOLINT
using uchar1 = ::uchar1;        // NOLINT
using uchar2 = ::uchar2;        // NOLINT
using uchar3 = ::uchar3;        // NOLINT
using uchar4 = ::uchar4;        // NOLINT
using short1 = ::short1;        // NOLINT
using short2 = ::short2;        // NOLINT
using short3 = ::short3;        // NOLINT
using short4 = ::short4;        // NOLINT
using ushort = unsigned short;  // NOLINT
using ushort1 = ::ushort1;      // NOLINT
using ushort2 = ::ushort2;      // NOLINT
using ushort3 = ::ushort3;      // NOLINT
using ushort4 = ::ushort4;      // NOLINT
using int1 = ::int1;            // NOLINT
using int2 = ::int2;            // NOLINT
using int3 = ::int3;            // NOLINT
using int4 = ::int4;            // NOLINT
using uint = unsigned int;      // NOLINT
using uint1 = ::uint1;          // NOLINT
using uint2 = ::uint2;          // NOLINT
using uint3 = ::uint3;          // NOLINT
using uint4 = ::uint4;          // NOLINT
using long1 = ::longlong1;      // NOLINT
using long2 = ::longlong2;      // NOLINT
using long3 = ::longlong3;      // NOLINT
using long4 = ::longlong4;      // NOLINT
using ulong1 = ::ulonglong1;    // NOLINT
using ulong2 = ::ulonglong2;    // NOLINT
using ulong3 = ::ulonglong3;    // NOLINT
using ulong4 = ::ulonglong4;    // NOLINT
using float1 = ::float1;        // NOLINT
using float2 = ::float2;        // NOLINT
using float3 = ::float3;        // NOLINT
using float4 = ::float4;        // NOLINT
using double1 = ::double1;      // NOLINT
using double2 = ::double2;      // NOLINT
}  // namespace gputil

#endif  // GPUPLATFORM2_H


================================================
FILE: gputil/cuda/gpuProgram.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuProgram.h"

#include "gputil/cuda/gpuProgramDetail.h"

namespace gputil
{
Program::Program() = default;


Program::Program(Device &device, const char *program_name)
  : imp_(std::make_unique<ProgramDetail>())
{
  imp_->device = device;
  imp_->program_name = program_name;
}


Program::Program(Program &&other) noexcept
  : imp_(std::move(other.imp_))
{}


Program::Program(const Program &other)
{
  *this = other;
}


Program::~Program() = default;


bool Program::isValid() const
{
  return imp_ != nullptr && imp_->device.isValid();
}

const char *Program::programName() const
{
  return imp_->program_name.c_str();
}

Device Program::device()
{
  return (imp_) ? imp_->device : Device();
}

// Lint(KS): API implementation compatibility requirement. Cannot make static
// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
int Program::buildFromFile(const char * /*file_name*/, const BuildArgs & /*build_args*/)
{
  return 0;
}

// Lint(KS): API implementation compatibility requirement. Cannot make static
// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
int Program::buildFromSource(const char * /*source*/, size_t /*source_length*/, const BuildArgs & /*build_args*/)
{
  return 0;
}

Program &Program::operator=(const Program &other)
{
  if (this != &other)
  {
    if (other.imp_)
    {
      if (!imp_)
      {
        imp_ = std::make_unique<ProgramDetail>();
      }
      imp_->device = other.imp_->device;
      imp_->program_name = other.imp_->program_name;
    }
    else
    {
      imp_.reset();
    }
  }
  return *this;
}

Program &Program::operator=(Program &&other) noexcept
{
  imp_ = std::move(other.imp_);
  return *this;
}
}  // namespace gputil


================================================
FILE: gputil/cuda/gpuProgramDetail.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUPROGRAMDETAIL_H
#define GPUPROGRAMDETAIL_H

#include "gpuConfig.h"

#include "gputil/gpuDevice.h"

#include <string>

namespace gputil
{
struct ProgramDetail
{
  Device device;
  std::string program_name;
};
}  // namespace gputil

#endif  // GPUPROGRAMDETAIL_H


================================================
FILE: gputil/cuda/gpuQueue.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuQueue.h"

#include "gputil/cuda/gpuEventDetail.h"
#include "gputil/cuda/gpuQueueDetail.h"

#include "gputil/gpuApiException.h"
#include "gputil/gpuThrow.h"

#include <cuda_runtime.h>

namespace gputil
{
void destroyStream(cudaStream_t &stream)
{
  if (stream)
  {
    cudaError_t err = cudaStreamDestroy(stream);
    stream = nullptr;
    GPUAPICHECK2(err, cudaSuccess);
  }
}

struct CallbackWrapper
{
  std::function<void(void)> callback;

  explicit inline CallbackWrapper(std::function<void(void)> callback)
    : callback(std::move(callback))
  {}
};

void streamCallback(cudaStream_t /*event*/, cudaError_t /*status*/, void *user_data)
{
  auto *wrapper = static_cast<CallbackWrapper *>(user_data);
  wrapper->callback();
  // Lint(KS): No RAII option available for this
  delete wrapper;  // NOLINT(cppcoreguidelines-owning-memory)
}


Queue::Queue()
  : queue_(new QueueDetail())
{}


Queue::Queue(Queue &&other) noexcept
  : queue_(std::exchange(other.queue_, nullptr))
{}


Queue::Queue(const Queue &other)
  : queue_(other.queue_)
{}


Queue::Queue(void *platform_queue)
  : queue_(new QueueDetail())
{
  // Note: the platform_queue will be null for the default stream.
  queue_->queue = static_cast<cudaStream_t>(platform_queue);
}


Queue::~Queue() = default;


bool Queue::isValid() const
{
  return queue_ != nullptr;
}


void Queue::insertBarrier()
{
  // Nothing to do for CUDA. A single stream is implicitly sequential.
}


Event Queue::mark()
{
  Event event;
  cudaError_t err = cudaSuccess;
  err = cudaEventRecord(event.detail()->obj(), queue_->queue);
  GPUAPICHECK(err, cudaSuccess, Event());
  return event;
}


void Queue::setSynchronous(bool synchronous)
{
  queue_->force_synchronous = synchronous;
}


bool Queue::synchronous() const
{
  return queue_->force_synchronous;
}


void Queue::flush()
{
  // Not needed.
}


void Queue::finish()
{
  cudaError_t err = cudaStreamSynchronize(queue_->queue);
  GPUAPICHECK2(err, cudaSuccess);
}


void Queue::queueCallback(const std::function<void(void)> &callback)
{
  CallbackWrapper *wrapper = nullptr;

  cudaError_t err = cudaSuccess;
  // Lint(KS): No nice RAII alternative for this
  wrapper = new CallbackWrapper(callback);  // NOLINT(cppcoreguidelines-owning-memory)

  err = cudaStreamAddCallback(queue_->queue, streamCallback, wrapper, 0);

  if (err)
  {
    // Lint(KS): No nice RAII alternative for this
    delete wrapper;  // NOLINT(cppcoreguidelines-owning-memory)
    GPUTHROW2(ApiException(err, nullptr, __FILE__, __LINE__));
  }
}


QueueDetail *Queue::internal() const
{
  return queue_.get();
}


Queue &Queue::operator=(const Queue &other)
{
  if (this != &other)
  {
    queue_ = other.queue_;
  }
  return *this;
}


Queue &Queue::operator=(Queue &&other) noexcept
{
  queue_ = std::move(other.queue_);
  return *this;
}
}  // namespace gputil


================================================
FILE: gputil/cuda/gpuQueueDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUQUEUEDETAIL_H
#define GPUQUEUEDETAIL_H

#include "gpuConfig.h"

#include "gputil/gpuApiException.h"
#include "gputil/gpuThrow.h"

#include <cuda_runtime.h>

#include <utility>

namespace gputil
{
struct QueueDetail
{
  cudaStream_t queue = nullptr;
  bool force_synchronous = false;

  inline ~QueueDetail()
  {
    if (queue)
    {
      cudaError_t err = cudaStreamDestroy(queue);
      if (err != cudaSuccess)
      {
        // Dangerous to throw from destructor, so just log on exceptions.
        gputil::log(gputil::ApiException(err, nullptr, __FILE__, __LINE__));
      }
    }
  }
};
}  // namespace gputil

#endif  // GPUQUEUEDETAIL_H


================================================
FILE: gputil/cuda/ref.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef REF_H
#define REF_H

#include "gpuConfig.h"

#include <functional>
#include <mutex>

namespace gputil
{
template <typename T>
class Ref
{
public:
  using ReleaseFunc = std::function<void(T &)>;

  Ref(T obj, unsigned initial_ref_count, ReleaseFunc release);
  Ref(Ref &&other) noexcept;
  Ref(const Ref &other) = delete;

protected:
  ~Ref();

public:
  unsigned reference();
  unsigned release();

  void set(T obj, unsigned ref_count);

  inline T obj() { return obj_; }
  inline const T obj() const { return obj_; }  // NOLINT(readability-const-return-type)

  inline T operator()() { return obj_; }
  inline const T operator()() const { return obj_; }  // NOLINT(readability-const-return-type)

  inline unsigned referenceCount() const { return reference_count_; }

  Ref &operator=(Ref &&other) noexcept;
  Ref &operator=(const Ref &other) = delete;

private:
  T obj_;
  unsigned reference_count_;
  ReleaseFunc release_func_;
  std::mutex lock_;
};

template <typename T>
inline Ref<T>::Ref(T obj, unsigned initial_ref_count, ReleaseFunc release)
  : obj_(obj)
  , reference_count_(initial_ref_count)
  , release_func_(std::move(release))
{}


template <typename T>
inline Ref<T>::Ref(Ref &&other) noexcept
  : obj_(other.obj_)
  , reference_count_(other.reference_count_)
  , release_func_(std::move(other.release_func_))
{
  other.reference_count_ = 0;
}


template <typename T>
inline Ref<T>::~Ref() = default;


template <typename T>
inline unsigned Ref<T>::reference()
{
  std::unique_lock<std::mutex> guard(lock_);
  if (reference_count_)
  {
    ++reference_count_;
  }
  return reference_count_;
}


template <typename T>
inline unsigned Ref<T>::release()
{
  std::unique_lock<std::mutex> guard(lock_);
  unsigned released_count = 0;
  if (reference_count_)
  {
    --reference_count_;
    released_count = reference_count_;
    if (reference_count_ == 0)
    {
      release_func_(obj_);
      guard.unlock();
      delete this;
    }
  }

  return released_count;
}


template <typename T>
inline void Ref<T>::set(T obj, unsigned ref_count)
{
  std::unique_lock<std::mutex> guard(lock_);
  if (reference_count_)
  {
    // Should be an error when reference_count_ > 1.
    release_func_(obj_);
  }

  obj_ = obj;
  reference_count_ = ref_count;
}


template <typename T>
inline Ref<T> &Ref<T>::operator=(Ref &&other) noexcept
{
  std::unique_lock<std::mutex> guard(lock_);
  if (reference_count_)
  {
    // Should be an error when reference_count_ > 1.
    release_func_(obj_);
  }
  reference_count_ = other.reference_count_;
  obj_ = other.obj_;
  release_func_ = other.release_func_;
  other.reference_count_ = 0;
  return *this;
}
}  // namespace gputil

#endif  // REF_H


================================================
FILE: gputil/gpuApiException.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuApiException.h"

#include <sstream>
#include <utility>

namespace gputil
{
ApiException::ApiException(const int error_code, const char *const msg, const char *const filename,
                           const int line_number)
  : error_code_(error_code)
{
  if (msg)
  {
    setMessage(msg, (filename == nullptr) ? "" : filename, line_number);
  }
  else
  {
    std::ostringstream str;
    str << "API error (" << error_code << ") " << errorCodeString(error_code);
    setMessage(str.str(), (filename == nullptr) ? "" : filename, line_number);
  }
}


ApiException::ApiException(ApiException &&other) noexcept
  : Exception(std::move(other))
  , error_code_(other.error_code_)
{}


ApiException::ApiException(const ApiException &other) noexcept
  : Exception(other)
  , error_code_(other.error_code_)
{}
}  // namespace gputil


================================================
FILE: gputil/gpuApiException.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef GPUGPUAPICHECK_H
#define GPUGPUAPICHECK_H

#include "gpuConfig.h"

#include "gpuException.h"

namespace gputil
{
/// Raised due to an exception in the underlying GPU SDK.
class gputilAPI ApiException : public Exception  // NOLINT(cppcoreguidelines-special-member-functions)
{
public:
  /// Construct an API exception.
  /// @param error_code The underlying SDK error code - e.g., a @c cudaError_t .
  /// @param msg Optional test for the api error. When null, the @c errorCodeString() for @p error_code is used.
  /// @param filename Optional name of file the exception is thrown from.
  /// @param line_number Optional line number where the exception is thrown.
  ApiException(int error_code, const char *msg, const char *filename = nullptr, int line_number = 0);

  /// @overload
  explicit inline ApiException(int error_code)
    : ApiException(error_code, nullptr)
  {}

  /// Move constructor.
  /// @param other Object to move.
  ApiException(ApiException &&other) noexcept;

  /// Copy constructor.
  /// @param other Object to copy.
  ApiException(const ApiException &other) noexcept;

  /// A helper function which converts the @p error_code into a string message.
  /// @param error_code The GPU API error code to retrieve a message for - e.g., a @c cudaError_t .
  /// @return A string which identifies the @p error_code . Some codes may be unknown.
  static const char *errorCodeString(int error_code);

  /// Query the error code value for the exception.
  /// @return The underlying SDK error code - e.g., a @c cudaError_t .
  inline int errorCode() const { return error_code_; }

private:
  int error_code_;
};
}  // namespace gputil

#endif  // GPUGPUAPICHECK_H


================================================
FILE: gputil/gpuBuffer.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef GPUBUFFER_H
#define GPUBUFFER_H

#include "gpuConfig.h"

#include "gpuPinMode.h"

#include <cstddef>

namespace gputil
{
struct BufferDetail;
class Device;
class Event;
class Queue;

/// Flags used to control @c Buffer creation.
enum BufferFlag : unsigned
{
  /// Buffer memory can be read on the device.
  kBfRead = (1u << 0u),
  /// Buffer memory can be written on the device.
  kBfWrite = (1u << 1u),
  /// Buffer is in host accessible memory on the device.
  /// Required for buffer pinning.
  kBfHostAccess = (1u << 2u),

  /// Alias for combining read/write flags.
  kBfReadWrite = kBfRead | kBfWrite,

  /// Alias for host accessible read.
  kBfReadHost = kBfRead | kBfHostAccess,

  /// Alias for host accessible write.
  kBfWriteHost = kBfWrite | kBfHostAccess,

  /// Alias for host accessible read/write.
  kBfReadWriteHost = kBfRead | kBfWrite | kBfHostAccess
};

// clang-format off
/// @class Buffer
/// Represents a GPU based buffer. Implementation depends on the
/// GPU SDK.
///
/// @par Synchronous vs Asynchronous transfer
/// Memory transfer function typically accept three optional parameters: a @c Queue and two @c Event pointers,
/// typically labelled @c blockOn and @c completion. The behaviour of memory transfer functions may differ depending
/// on whether a @c Queue is provide and whether @c blockOn and/or @c completion events are provide. Firstly,
/// providing a non-null @c Queue invokes an asynchronous data transfer. As such the host memory buffer must remain
/// valid until the transfer completes. The @p completion event supports the asynchronous transfer providing a means
/// of monitoring completion of the memory transfer via either @c Event::waitFor() or @c Event::isComplete(). Finally,
/// the @c blockOn argument specifies an existing event which must complete before @em starting the memory transfer.
/// @c blockOn may be used with a null @p queue, in which case this call remains a blocking call. This behaviour is
/// summarised below.
///
/// @p queue  | @p blockOn  | @p completion | Behaviour
/// --------- | ----------- | ------------- | ---------
/// nullptr   | nullptr     | ignored       | Blocking call using host accessible memory if possible.
/// nullptr   | non-null    | ignored       | Blocking call, waiting on @p blockOn first. Providing @p completion is redundant.
/// non-null  | nullptr     | nullptr       | Asynchronous call, no event information available.
/// non-null  | non-null    | -             | Asynchronous call. Memory transfer occurs after @p blockOn completes.
/// non-null  | any         | non-null      | Asynchronous call. @p completion tracks the completion of the transfers.
///
/// Note that using asychronous transfer may invoke a less optimal memory tranfer path (in OpenCL).
// clang-format on
class gputilAPI Buffer
{
public:
  /// Create an invalid buffer object.
  Buffer();

  /// Create a buffer for the given @p device and @p byteSize.
  /// @param device The GPU device to create the buffer on/for.
  /// @param byte_size Number of bytes to allocate. Actual allocation may be larger.
  /// @param flags See @c BufferFlag.
  Buffer(const Device &device, size_t byte_size, unsigned flags = kBfReadWrite);

  /// R-Value constructor.
  Buffer(Buffer &&other) noexcept;

  /// Deleted.
  Buffer(const Buffer &other) = delete;

  /// Destructor.
  ~Buffer();

  /// Deleted because we can't have two buffers referencing the same address.
  /// Use @c swap().
  Buffer &operator=(const Buffer &) = delete;

  /// R-value assignment.
  /// @return *this
  Buffer &operator=(Buffer &&other) noexcept;

  /// Create and initialise the @c Buffer object.
  ///
  /// The @c Buffer is created for the given @p device and initialised to the
  /// given @p byteSize. Read/write accessibility are set by @p flags.
  /// If the buffer has already been created, then the existing memory is release.
  /// This may change the @c Device associated with the @c Buffer.
  ///
  /// @param device The GPU device to create the buffer on/for.
  /// @param byte_size Number of bytes to allocate. Actual allocation may be larger.
  /// @param flags See @c BufferFlag.
  void create(const Device &device, size_t byte_size, unsigned flags = kBfRead);

  /// Release any existing memory and disassociate this @c Buffer from a @c Device.
  /// The @c Buffer object becomes invalid.
  void release();

  /// Swap this buffer with @p other. This swaps the underlying addresses and allocations.
  /// @param other The buffer to swap with.
  void swap(Buffer &other) noexcept;

  /// Checks if the buffer is valid for use.
  /// @return true if valid.
  bool isValid() const;

  /// Access the @c BufferFlag values used to create the buffer.
  unsigned flags() const;

  /// Returns the requested buffer allocation in bytes.
  /// @return Buffer size in bytes.
  size_t size() const;

  /// Returns the actual size of the buffer in bytes. May be larger than the requested size
  /// as the buffer is padded for better alignment.
  /// @return Actual buffer size in bytes.
  size_t actualSize() const;

  /// Check the number of @p T typed elements the buffer can hold.
  /// @tparam T The data type defining the element size.
  /// @return The number of elements the buffer has the capacity for.
  template <typename T>
  inline size_t elementCount() const
  {
    return size() / sizeof(T);
  }

  /// Check the number of data elements of size @p bufferElementSize the buffer can hold.
  /// @param buffer_element_size The size or stride of a single element in the buffer.
  /// @return The number of elements the buffer has the capacity for.
  inline size_t elementCount(size_t buffer_element_size) const { return size() / buffer_element_size; }

  /// Resize the buffer to the @p newSize if necessary.
  ///
  /// The buffer is only reallocated if @c actualSize() is insufficient to meet
  /// the needs of @p newSize.
  ///
  /// Note: existing data in the buffer may be lost.
  ///
  /// @param new_size The requested size in bytes.
  /// @return The actual allocation size (bytes).
  size_t resize(size_t new_size);

  /// Resizes the buffer even if it already has the capacity for @p newSize.
  /// This method behaves like @c resize(), except that it will re-allocate
  /// when the best actual size for @p newSize is smaller than the current actual size.
  /// @param new_size The requested size in bytes.
  /// @return The actual allocation size (bytes).
  size_t forceResize(size_t new_size);

  /// Resize the buffer to support @p element_count elements of type @c T.
  ///
  /// Note: existing data in the buffer may be lost.
  ///
  /// @param element_count The number of elements required.
  /// @tparam T The data type defining the element size.
  /// @return The actual buffer size in elements of @p T.
  template <typename T>
  inline size_t elementsResize(size_t element_count)
  {
    return resize(sizeof(T) * element_count) / sizeof(T);
  }

  /// Clear the buffer contents to @p value.
  /// Behaves a bit like @c memset(). Uses @c fill() underneath.
  /// @param value The value to clear to.
  /// @param queue Optional queue to perform the operation on. Providing a non null queue
  ///     changes the operation to occur asynchronously. See @c fill().
  /// @param block_on Block the clear opteration on the completion of this event.
  /// @param completion When provided, makes changes to an asynchronous call and the event is set to mark completion.
  inline void clear(int value, Queue *queue = nullptr, Event *block_on = nullptr, Event *completion = nullptr)
  {
    fill(&value, sizeof(value), queue, block_on, completion);
  }

  /// @overload.
  inline void clear(unsigned value, Queue *queue = nullptr, Event *block_on = nullptr, Event *completion = nullptr)
  {
    fill(&value, sizeof(value), queue, block_on, completion);
  }

  /// Fill the buffer with the given @p pattern.
  ///
  /// The @c fill() operation may optionally be performed asynchronously by passing
  /// a pointer to an empty @c event object. Note that asynchronous operation may not be
  /// faster as synchronous operation can use pinned pointers.
  ///
  /// @param pattern The memory pattern to fill with.
  /// @param pattern_size The size of the data at @p pattern, in bytes.
  /// @param queue Optional queue to perform the operation on. Providing a non null queue
  ///     changes the operation to occur asynchronously.
  /// @param block_on Optional event to wait for be performing the fill.
  /// @param completion Optional event to setup to mark completion of the fill operation.
  void fill(const void *pattern, size_t pattern_size, Queue *queue = nullptr, Event *block_on = nullptr,
            Event *completion = nullptr);

  /// Fill part of the buffer with @p pattern.
  ///
  /// This is much less efficient than filling the entire buffer using @c fill(), but may be
  /// more efficient than making multiple @c write() calls.
  ///
  /// The @c fill() operation may optionally be performed asynchronously by passing
  /// a pointer to an empty @c event object. Note that asynchronous operation may not be
  /// faster as synchronous operation can use pinned pointers.
  ///
  /// @param pattern The memory pattern to fill with.
  /// @param pattern_size The size of the data at @p pattern, in bytes.
  /// @param offset Offset into the buffer to write at (bytes).
  /// @param fill_bytes Number of total bytes to fill in the buffer. Best to be a multiple of @p patternSize, but need
  /// not be.
  /// @param queue The command queue in which to perform the operation.
  void fillPartial(
    const void *pattern, size_t pattern_size, size_t fill_bytes, size_t offset,
    Queue *queue = nullptr);  // too tricky for now: , Event *blockOn = nullptr, Event *completion = nullptr);

  /// Read data from the buffer.
  ///
  /// Address ranges are constrained by @p readByteCount, @p srcOffset and
  /// @c size() to ensure valid address ranges. This may limit the number of
  /// bytes actually read.
  ///
  /// @param dst The address to read data into.
  /// @param read_byte_count The number of bytes to read. @p dst must support this byte count.
  /// @param src_offset srcOffset The offset into this @c Buffer to start reading from (bytes).
  /// @param queue Optional queue to perform the operation on. Providing a non null queue
  ///     changes the operation to occur asynchronously.
  /// @param block_on Optional event to wait for be performing the read.
  /// @param completion Optional event to setup to mark completion of the read operation.
  /// @return The number of bytes read.
  size_t read(void *dst, size_t read_byte_count, size_t src_offset = 0, Queue *queue = nullptr,
              Event *block_on = nullptr, Event *completion = nullptr);

  /// Write data to the buffer.
  ///
  /// Address ranges are constrained by @p byteCount, @p dstOffset
  /// and @c size() to ensure valid address ranges. This may limit the number of
  /// bytes actually written.
  ///
  /// The behaviour of the write function is modified by the @p queue, @c blockOn and @c completion parameters
  /// just like the @c read() function. See that function for details.
  ///
  /// @param src The address holding the data to write into this @c Buffer.
  /// @param byte_count The number of bytes from @p src to write to this @c Buffer.
  /// @param dst_offset The offset into this @c Buffer to start writing at.
  /// @param queue Optional queue to perform the operation on. Providing a non null queue
  ///     changes the operation to occur asynchronously.
  /// @param block_on Optional event to wait for be performing the write.
  /// @param completion Optional event to setup to mark completion of the write operation.
  /// @return The number of bytes written.
  size_t write(const void *src, size_t byte_count, size_t dst_offset = 0, Queue *queue = nullptr,
               Event *block_on = nullptr, Event *completion = nullptr);

  /// Read an array of data elements from the buffer.
  ///
  /// This method supports a size discrepancy between elements in the buffer and elements in @c dst.
  /// This is primarily to cater for the size difference between a 3-element float vector and
  /// @c cl_float3, which is a 4-element vector, with a redundant @c w component.
  ///
  /// Where elements in @p dst and @c this buffer do not match, specify the @p element_size as the
  /// element stride in @p dst and @p bufferElementSize as the stride in @c this buffer.
  ///
  /// The method will copy as many elements as are available in @c this buffer. The @p dst memory
  /// must be sized as follows <tt>element_count * element_size</tt>.
  ///
  /// @param dst The buffer to write into. Must be sized to suit.
  /// @param element_size The element stride in @p dst. The same stride is used for @c this buffer when
  ///     @p bufferElementSize is zero.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements Number of elements to offset the read start. That is skip this number of elements before
  ///     reading.
  /// @param buffer_element_size Optional element stride within @c this buffer when different from
  ///     @p element_size.
  /// @param queue Optional queue to perform the operation on. Providing a non null queue
  ///     changes the operation to occur asynchronously.
  /// @param block_on Optional event to wait for be performing the read.
  /// @param completion Optional event to setup to mark completion of the read operation.
  /// @return The number of elements read. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  size_t readElements(void *dst, size_t element_size, size_t element_count, size_t offset_elements,
                      size_t buffer_element_size, Queue *queue = nullptr, Event *block_on = nullptr,
                      Event *completion = nullptr);

  /// An overload of @c readElements() using the template sizes to determine the sizes.
  /// @tparam T The data type to read into and assumed to exactly match that stored in the GPU buffer.
  /// @param dst The buffer to write into. Must be sized to suit.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements Number of elements to offset the read start. That is skip this number of elements before
  ///     reading.
  /// @param queue Optional queue to perform the operation on. Providing a non null queue
  ///     changes the operation to occur asynchronously.
  /// @param block_on Optional event to wait for be performing the read.
  /// @param completion Optional event to setup to mark completion of the read operation.
  /// @return The number of elements read. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  template <typename T>
  inline size_t readElements(T *dst, size_t element_count, size_t offset_elements = 0, Queue *queue = nullptr,
                             Event *block_on = nullptr, Event *completion = nullptr)
  {
    return readElements(dst, sizeof(T), element_count, offset_elements, 0u, queue, block_on, completion);
  }

  /// An overload of @c readElements() using the template sizes to determine the sizes.
  /// @tparam BufferType The data type stored in the GPU buffer. Only required when its size differs from that of
  ///   @p T
  /// @tparam T The data type to read into.
  /// @param dst The buffer to write into. Must be sized to suit.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements Number of elements to offset the read start. That is skip this number of elements before
  ///     reading.
  /// @param queue Optional queue to perform the operation on. Providing a non null queue
  ///     changes the operation to occur asynchronously.
  /// @param block_on Optional event to wait for be performing the read.
  /// @param completion Optional event to setup to mark completion of the read operation.
  /// @return The number of elements read. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  template <typename BufferType, typename T>
  inline size_t readElements(T *dst, size_t element_count, size_t offset_elements = 0, Queue *queue = nullptr,
                             Event *block_on = nullptr, Event *completion = nullptr)
  {
    return readElements(dst, sizeof(T), element_count, offset_elements, sizeof(BufferType), queue, block_on,
                        completion);
  }

  /// Write an array of data elements to the buffer.
  ///
  /// This method supports the same size discrepancy as @c readElements().
  ///
  /// The method will copy as many elements as are available in @c this buffer. The @p dst memory
  /// must be sized as follows <tt>element_count * element_size</tt>.
  ///
  /// @param src The buffer to read from.
  /// @param element_size The element stride in @p src. The same stride is used for @c this buffer when
  ///     @p bufferElementSize is zero.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements The offset to start writing at.
  /// @param buffer_element_size Optional element stride within @c this buffer when different from
  ///     @p element_size. Use zero when the size matches the @p element_size.
  /// @param queue Optional queue to perform the operation on. Providing a non null queue
  ///     changes the operation to occur asynchronously.
  /// @param block_on Optional event to wait for be performing the write.
  /// @param completion Optional event to setup to mark completion of the write operation.
  /// @return The number of elements written. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  size_t writeElements(const void *src, size_t element_size, size_t element_count, size_t offset_elements,
                       size_t buffer_element_size, Queue *queue = nullptr, Event *block_on = nullptr,
                       Event *completion = nullptr);

  /// An overload of @c writeElements() using the template sizes to determine the sizes.
  /// @tparam T The data type to read into and assumed to exactly match that stored in the GPU buffer.
  /// @param src The buffer to read from into. Must be sized to suit.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements Number of elements to offset the write start. That is skip this number of elements
  ///     before writing.
  /// @param queue Optional queue to perform the operation on. Providing a non null queue
  ///     changes the operation to occur asynchronously.
  /// @param block_on Optional event to wait for be performing the write.
  /// @param completion Optional event to setup to mark completion of the write operation.
  /// @return The number of elements written. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  template <typename T>
  inline size_t writeElements(const T *src, size_t element_count, size_t offset_elements = 0, Queue *queue = nullptr,
                              Event *block_on = nullptr, Event *completion = nullptr)
  {
    return writeElements(src, sizeof(T), element_count, offset_elements, 0u, queue, block_on, completion);
  }

  /// An overload of @c writeElements() using the template sizes to determine the sizes.
  /// @tparam BufferType The data type stored in the GPU buffer. Only required when its size differs from that of
  ///   @p T
  /// @tparam T The data type to read into and assumed to exactly match that stored in the GPU buffer.
  /// @param src The buffer to read from into. Must be sized to suit.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements Number of elements to offset the write start. That is skip this number of elements
  ///     before writing.
  /// @param queue Optional queue to perform the operation on. Providing a non null queue
  ///     changes the operation to occur asynchronously.
  /// @param block_on Optional event to wait for be performing the write.
  /// @param completion Optional event to setup to mark completion of the write operation.
  /// @return The number of elements written. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  template <typename BufferType, typename T>
  inline size_t writeElements(const T *src, size_t element_count, size_t offset_elements = 0, Queue *queue = nullptr,
                              Event *block_on = nullptr, Event *completion = nullptr)
  {
    return writeElements(src, sizeof(T), element_count, offset_elements, sizeof(BufferType), queue, block_on,
                         completion);
  }

  /// Internal pointer for argument passing to the device function/kernel.
  ///
  /// For CUDA this is a pointer to a pointer at which which the device memory is made.
  ///
  /// For OpenCL the type is @c cl_mem, which may be set as kernel argument.
  void *argPtr() const;

  /// Internal pointer for argument passing to the device function/kernel.
  ///
  /// For CUDA this the address of the allocation.
  ///
  /// For OpenCL the same as @c argPtr().
  void *address() const;

  /// Return the internal pointer for argument passing as the given pointer type.
  ///
  /// CUDA looks as follows:
  /// @code
  ///   gputil::Buffer buffer;
  ///   //...
  ///   // Pass buffer as a CUDA float3 *
  ///   deviceFunction<<<...>>>(buffer.arg<float3 *>());
  /// @endcode
  ///
  /// OpenCL usage looks as follows (using OpenCL C++ wrapper):
  /// @code
  ///   gputil::Buffer buffer;
  ///   //...
  ///   cl::Kernel kernel;
  ///   //...
  ///   kernel.setArg(0, buffer.arg<cl_mem>());
  /// @endcode
  template <typename T>
  inline T arg()
  {
    return static_cast<T>(address());
  }

  /// @c overload
  template <typename T>
  inline T arg() const
  {
    return static_cast<T>(address());
  }

  // void *pin(PinMode mode);
  // void unpin(void *ptr, Queue *queue = nullptr, Event *block_on = nullptr, Event *completion = nullptr);

  /// Get the internal buffer representation.
  /// @return The internal buffer detail.
  inline BufferDetail *detail() const { return imp_; }

private:
  BufferDetail *imp_;
};

/// Copy data from @p src to @p dst. All data from @p src are copied and @p dst must be at least as large as @p src.
///
/// See @c Buffer class comments for details on asynchronous transfer.
///
/// @param dst The destination buffer.
/// @param src The target buffer.
/// @param queue Optional queue to perform the operation on. Providing a non null queue
///     changes the operation to occur asynchronously.
/// @param block_on Optional event to wait for be performing the copy.
/// @param completion Optional event to setup to mark completion of the copy operation.
/// @return The number of bytes copied
size_t gputilAPI copyBuffer(Buffer &dst, const Buffer &src, Queue *queue = nullptr, Event *block_on = nullptr,
                            Event *completion = nullptr);

/// Copy data from @p src to @p dst copying only @p byteCount bytes.
/// Both buffers must support @p byteCount bytes.
///
/// See @c Buffer class comments for details on asynchronous transfer.
///
/// @param dst The destination buffer.
/// @param src The target buffer.
/// @param byte_count The number of bytes to copy.
/// @param queue Optional queue to perform the operation on. Providing a non null queue
///     changes the operation to occur asynchronously.
/// @param block_on Optional event to wait for be performing the copy.
/// @param completion Optional event to setup to mark completion of the copy operation.
/// @return The number of bytes copied
size_t gputilAPI copyBuffer(Buffer &dst, const Buffer &src, size_t byte_count, Queue *queue = nullptr,
                            Event *block_on = nullptr, Event *completion = nullptr);

/// Copy data from @p src to @p dst with offsets, copying only @p byteCount bytes.
/// Both buffers must support @p byteCount bytes and the given offsets.
///
/// See @c Buffer class comments for details on asynchronous transfer.
///
/// @param dst The destination buffer.
/// @param dst_offset Offset into the destination buffer to copy into (bytes).
/// @param src The target buffer.
/// @param src_offset Offset into the source buffer to copy from (bytes).
/// @param byte_count The number of bytes to copy.
/// @param queue Optional queue to perform the operation on. Providing a non null queue
///     changes the operation to occur asynchronously.
/// @param block_on Optional event to wait for be performing the copy.
/// @param completion Optional event to setup to mark completion of the copy operation.
/// @return The number of bytes copied
size_t gputilAPI copyBuffer(Buffer &dst, size_t dst_offset, const Buffer &src, size_t src_offset, size_t byte_count,
                            Queue *queue = nullptr, Event *block_on = nullptr, Event *completion = nullptr);
}  // namespace gputil

#endif  // GPUBUFFER_H


================================================
FILE: gputil/gpuConfig.in.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef GPUCONFIG_H
#define GPUCONFIG_H

#include "gputilExport.h"

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX
#include <cmath>

// clang-format off
#define GPUTIL_NONE 0
#define GPUTIL_OPENCL 1
#define GPUTIL_CUDA 2
// clang-format on

#endif  // GPUCONFIG_H


================================================
FILE: gputil/gpuDevice.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef GPUDEVICE_H
#define GPUDEVICE_H

#include "gpuConfig.h"

#include "gpuDeviceInfo.h"
#include "gpuQueue.h"

#include <memory>
#include <vector>

namespace gputil
{
struct DeviceDetail;

/// Represents a GPU device and context.
class gputilAPI Device
{
public:
  /// Specifies the level of debugging applied to the GPU.
  enum DebugLevel
  {
    /// No debugging.
    kDlOff,
    /// Minimal debugging. Optimisations are still active.
    kDlLow,
    /// Full debugging, optimisations disabled.
    kDlFull
  };

  /// Accelerator type. The available types are driven by the OpenCL API.
  enum Type : unsigned
  {
    /// CPU acceleration device.
    kCpu = (1u << 0u),
    /// GPU device (preferred)
    kGpu = (1u << 1u),
    /// Non-GPU accelerator device.
    kAccelerator = (1u << 2u)
  };

  /// Construct to access default device or as an invalid device.
  /// @param default_device True to construct access the default device.
  explicit Device(bool default_device = false);

  /// Create a device from the selected @c DeviceInfo.
  /// @param device_info Info structure about the desired device.
  explicit Device(const DeviceInfo &device_info);

  /// Construct using @c select().
  /// @param argc Number of values in @c argv.
  /// @param argv Argument string to parse.
  /// @param default_device Partial device name to match as the default selection.
  /// @param device_type_flags Preferred device @c Type flags.
  Device(int argc, const char **argv, const char *default_device = nullptr, unsigned device_type_flags = kGpu);
  /// @overload
  inline Device(int argc, char **argv, const char *default_device = nullptr, unsigned device_type_flags = kGpu)
    : Device(argc, const_cast<const char **>(argv),  // NOLINT(cppcoreguidelines-pro-type-const-cast)
             default_device, device_type_flags)
  {}

  /// Copy constructor.
  /// @param other Object to copy.
  Device(const Device &other);

  /// Move constructor.
  /// @param other Object to move.
  Device(Device &&other) noexcept;

  /// Destructor.
  ~Device();

  /// Enumerate all devices attached to the current host.
  static unsigned enumerateDevices(std::vector<DeviceInfo> &devices);

  /// Display name for the device.
  const char *name() const;

  /// Device description.
  const char *description() const;

  /// General device info.
  const DeviceInfo &info() const;

  /// Get the default queue associated with this device.
  /// @return The default device queue.
  Queue defaultQueue() const;

  /// Create a new queue for this device.
  /// @param flags Creation flags for the queue. See @c Queue::QueueFlag.
  /// @return The new queue.
  Queue createQueue(unsigned flags = 0) const;

  /// Select an active device based on the given command line (style) arguments.
  ///
  /// TODO: More documentation here.
  ///
  /// For OpenCL you can do the following:
  /// - "--accel=[any,accel,gpu,cpu]" to select the accelerator type.
  /// - "--clver=#.#" to select a minimum required OpenCL version.
  /// - "--device=&lt;like-name&gt;" for a device approximately matching the given name.
  /// - "--platform=&lt;like-name&gt;" for a plaform approximately matching the given name.
  /// - "--vendor=&lt;like-name&gt;" for a vendor approximately matching the given name.
  /// - "--gpu-debug[=&lt;level&gt;]" set the @c debugGpu() level. Uses @c DL_Low (1) if no level is specified.
  ///
  /// @param argc Number of values in @c argv.
  /// @param argv Argument string to parse.
  /// @param default_device Default device constraint to select if non specified.
  /// @param device_type_flags Preferred device @c Type flags.
  /// @return True on success.
  bool select(int argc, const char **argv, const char *default_device = nullptr, unsigned device_type_flags = kGpu);

  /// @overload
  inline bool select(int argc, char **argv, const char *default_device = nullptr, unsigned device_type_flags = kGpu)
  {
    return select(argc, const_cast<const char **>(argv),  // NOLINT(cppcoreguidelines-pro-type-const-cast)
                  default_device, device_type_flags);
  }

  /// Select a device from the given @c DeviceInfo.
  /// @param device_info The selected device details.
  /// @return True on success, false on failure to match @p device_info.
  bool select(const DeviceInfo &device_info);

  /// Set the value for @c debugGpu().
  /// @param debug_level The @c DebugLevel to set.
  void setDebugGpu(DebugLevel debug_level);

  /// Compile GPU code for debugging?
  /// This only affects OpenCL late compilation (not CUDA).
  /// @return The @c DebugLevel to execute with.
  DebugLevel debugGpu() const;

  /// Check if the device supports the specified feature.
  ///
  /// Feature depend on the underlying platform. For example, "cl_khr_global_int32_base_atomics".
  ///
  /// @param feature_id Platform specific feature name.
  /// @return True if the feature is valid and supported.
  bool supportsFeature(const char *feature_id) const;

  /// Add a path to the GPU source @c searchPaths().
  /// @param path The path to add.
  void addSearchPath(const char *path);

  /// Retrieve the search paths used for finding GPU source files (OpenCL sources).
  /// The search paths are comma separated.
  /// @return The search paths to be used to find GPU sources.
  const char *searchPaths() const;

  /// Is the device valid?
  /// @return True if valid.
  bool isValid() const;

  /// Query the memory for the device.
  /// @return The device memory in bytes.
  uint64_t deviceMemory() const;

  /// Query the maximum device allocation size for a single allocation.
  /// @return The maximum, single allocation size in bytes.
  uint64_t maxAllocationSize() const;

  /// Does the device have unified memory with the host?
  /// @return True for unified host/device memory.
  bool unifiedMemory() const;

  /// Copy assignment.
  /// @param other Object to copy.
  /// @return *this
  Device &operator=(const Device &other);

  /// Move assignment.
  /// @param other Object to move.
  /// @return *this
  Device &operator=(Device &&other) noexcept;

  /// Get the internal device representation.
  /// @return The internal device detail.
  DeviceDetail *detail() const { return imp_.get(); }

private:
  std::unique_ptr<DeviceDetail> imp_;
};
}  // namespace gputil

#endif  // GPUDEVICE_H


================================================
FILE: gputil/gpuDeviceInfo.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUDEVICEINFO_H
#define GPUDEVICEINFO_H

#include "gpuConfig.h"

#include "gpuVersion.h"

#include <string>

namespace gputil
{
/// Enumeration of accelerator types.
enum DeviceType : uint16_t
{
  kDeviceNull,  ///< Invalid
  kDeviceGpu,   ///< A GPU accelerator device.
  kDeviceCpu,   ///< A CPU accelerator device
  kDeviceOther  ///< Some other accelerator type.
};

/// Structure detailing information about a GPU or accelerator device. Available details may vary depending on the
/// GPU SDK.
struct DeviceInfo
{
  /// The device name - e.g., the name of the GPU card.
  std::string name;
  /// Details of the accelerator platform - e.g., OpenCL &lt;version&gt; or CUDA.
  std::string platform;
  /// Accelerator API version - e.g., OpenCL 1.2 vs 2.0
  Version version;
  /// The accelerator type of the default.
  DeviceType type = DeviceType::kDeviceNull;

  /// Equality operator.
  /// @param other The structure to compare against.
  /// @return True if @c this exactly matches @p other .
  inline bool operator==(const DeviceInfo &other) const
  {
    return version == other.version && type == other.type && name == other.name && platform == other.platform;
  }
};
}  // namespace gputil

#endif  // GPUDEVICEINFO_H


================================================
FILE: gputil/gpuEvent.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUTILEVENT_H
#define GPUTILEVENT_H

#include "gpuConfig.h"

#include <cstddef>

namespace gputil
{
struct EventDetail;

/// Marks a point in the execution stream on which we can block and await completion.
///
/// Note: this is analogous to an OpenCL event. CUDA does not have as precise a parallel, supporting only an
/// event concept which blocks an entire stream awaiting event completion. However, CUDA streams are inherently
/// more serial, awaiting completion of each command, whereas OpenCL queues start execution of each item without
/// waiting for the previous item unless an events are used.
class gputilAPI Event
{
public:
  /// Construct a null/invalid event.
  Event();
  /// Copy constructor. Refers to the same GPU API event as @p other .
  /// @param other Even to copy.
  Event(const Event &other);
  /// Move constructor.
  /// @param other Even to move.
  Event(Event &&other) noexcept;

  /// Destructor - releases GPU API event.
  ~Event();

  /// Is this a valid event?
  bool isValid() const;

  /// Release the underlying event, invalidating this object.
  void release();

  /// Has this event completed. Note that invalid events always return @c true.
  /// @return True if this event is invalid or has completed execution.
  bool isComplete() const;

  /// Block until this event to completes.
  void wait() const;

  /// Block the CPU on multiple events before continuing.
  ///
  /// An overload accepts an array of pointers.
  ///
  /// @param events Array of events to wait on.
  /// @param event_count The number of elements in @p events.
  static void wait(const Event *events, size_t event_count);

  /// @overload
  static void wait(const Event **events, size_t event_count);

  /// Copy assignment. Refers to the same GPU API event as @p other .
  /// @param other Even to copy.
  /// @return `*this`
  Event &operator=(const Event &other);
  /// Move assignment.
  /// @param other Even to move.
  /// @return `*this`
  Event &operator=(Event &&other) noexcept;

  /// Get the internal event representation - constructed if null.
  /// @return The internal event detail.
  EventDetail *detail();
  /// Get the internal event representation.
  /// @return The internal event detail - may be null.
  EventDetail *detail() const;

private:
  // Not a unique pointer due to internal management requirements.
  EventDetail *imp_ = nullptr;
};
}  // namespace gputil

#endif  // GPUTILEVENT_H


================================================
FILE: gputil/gpuEventList.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpuEventList.h"

#include <algorithm>

namespace gputil
{
EventList::EventList() = default;


EventList::EventList(const Event &event)
  : EventList()
{
  events_[0] = event;
  count_ = 1;
}


EventList::EventList(const Event *event)
  : EventList(*event)
{}


EventList::EventList(const Event *events, size_t event_count)
  : EventList()
{
  for (size_t i = 0; i < event_count; ++i)
  {
    add(events[i]);
  }
}


EventList::EventList(std::initializer_list<Event> events)
  : EventList()
{
  add(events);
}


EventList::EventList(std::initializer_list<const Event *> events)
  : EventList()
{
  add(events);
}


EventList::~EventList()
{
  clear();
  delete[] extended_;
  extended_ = nullptr;
}


void EventList::add(const Event &event)
{
  if (count_ + 1 >= capacity())
  {
    reserve(std::max(capacity() * 2, capacity() + 1));
  }

  Event *target_array = (extended_) ? extended_ : events_.data();
  target_array[count_++] = event;
}


void EventList::add(std::initializer_list<Event> events)
{
  const size_t new_event_count = events.size();
  if (count_ + new_event_count >= capacity())
  {
    reserve(std::max(capacity() * 2, capacity() + new_event_count));
  }

  Event *target_array = (extended_) ? extended_ : events_.data();
  for (const Event &e : events)
  {
    target_array[count_++] = e;
  }
}


void EventList::add(std::initializer_list<const Event *> events)
{
  const size_t new_event_count = events.size();
  if (count_ + new_event_count >= capacity())
  {
    reserve(std::max(capacity() * 2, capacity() + new_event_count));
  }

  Event *target_array = (extended_) ? extended_ : events_.data();
  for (const Event *e : events)
  {
    target_array[count_++] = *e;
  }
}


void EventList::clear()
{
  Event *target_array = (extended_) ? extended_ : events_.data();
  for (size_t i = 0; i < count_; ++i)
  {
    target_array[i].release();
  }
  count_ = 0;
}


void EventList::reserve(size_t new_size)
{
  if (new_size < capacity_ || !extended_ && new_size < kShortCount)
  {
    // Already large enough.
    return;
  }

  // Lint(KS): only way I can think of to manage this setup.
  auto *new_array = new Event[new_size];  // NOLINT(cppcoreguidelines-owning-memory)
  Event *old_array = (extended_) ? extended_ : events_.data();
  for (size_t i = 0; i < count_; ++i)
  {
    new_array[i] = std::move(old_array[i]);
  }

  delete[] extended_;
  extended_ = new_array;
  capacity_ = new_size;
}
}  // namespace gputil

================================================
FILE: gputil/gpuEventList.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUEVENTLIST_H
#define GPUEVENTLIST_H

#include "gpuConfig.h"

#include "gpuEvent.h"

#include <array>
#include <initializer_list>

namespace gputil
{
/// Represents a short term object identifying a set of @c Event objects. Generally used to specify a set of events to
/// wait on before a queued operation may begin. @c EventList objects are intended to be short lived objects, normally
/// used only as arguments functions enqueuing gpu operations.
///
/// The event list maintains its own reference counting for the added events.
class gputilAPI EventList
{
public:
  /// Number of items the short list. No additional allocation is required until more events are aded.
  static const unsigned kShortCount = 8;

  /// Construct an empty event list.
  EventList();
  /// Construct an event list containing just the given event.
  /// @param event The event to add to the list.
  explicit EventList(const Event &event);
  /// Construct an evet list containing just the given event.
  /// @param event The event to add to the list.
  explicit EventList(const Event *event);
  /// Construct an event list with multiple events.
  /// @param events A pointer to the array of events to add.
  /// @param event_count The number of elements to add from @p events .
  EventList(const Event *events, size_t event_count);
  /// Construct an event list with multiple events.
  /// @param events The list of events to add.
  EventList(std::initializer_list<Event> events);
  /// Construct an event list with multiple events.
  /// @param events The list of events to add.
  EventList(std::initializer_list<const Event *> events);

  EventList(const EventList &other) = delete;
  EventList &operator=(const EventList &other) = delete;

  /// Destructor - releases any reference counts for events in the list.
  ~EventList();

  /// Request a pointer to the event array. There are @c count() elements.
  /// @return The event array.
  const Event *events() const { return (!extended_) ? events_.data() : extended_; }

  /// Query the number of events in the list.
  /// @return The number of events in the list.
  inline size_t count() const { return count_; }
  /// An alias of @c count() .
  /// @return The number of events in the list.
  inline size_t size() const { return count_; }
  /// Query the capacity of the event list. Capacity will be extended as required. The minimum capacity is
  /// @c kShortCount .
  /// @return The available capacity.
  inline size_t capacity() const { return (!extended_) ? kShortCount : capacity_; }

  /// Add an event to the list.
  /// @param event The event to add.
  void add(const Event &event);

  /// An alias for @c add() .
  /// @param event The event to add.
  inline void push_back(const Event &event) { add(event); }  // NOLINT

  /// Add multiple items to the list.
  /// @param events The events to add.
  void add(std::initializer_list<Event> events);
  /// Add multiple items to the list.
  /// @param events The events to add.
  void add(std::initializer_list<const Event *> events);

  /// Clear the event list, releasing all event references.
  void clear();

  /// Ensure the @c capacity() is at least @c new_size .
  /// @param new_size The event capacity to allow for.
  void reserve(size_t new_size);

private:
  /// Event short list. Only valid when _count <= ShortCount
  std::array<Event, kShortCount> events_;
  /// Number of events in the list.
  size_t count_ = 0;
  /// Capacity of _extended. Only used when _extended is valid.
  size_t capacity_ = 0;
  /// Extended list. Allocated when the number of events is ShortCount or greater.
  Event *extended_ = nullptr;
};
}  // namespace gputil

#endif  // GPUEVENTLIST_H


================================================
FILE: gputil/gpuException.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "gpuException.h"
#include <cstdio>
#include <cstring>
#include <sstream>
#include <string>

namespace gputil
{
#ifdef WIN32
#define strncpy(dst, msg, len) strncpy_s(dst, len + 1, msg, len)
#endif  // WIN32

Exception::Exception(const std::string &msg, const std::string &filename, int line_number)
{
  setMessage(msg, filename, line_number);
}


Exception::Exception(Exception &&other) noexcept
  : message_(std::move(other.message_))
{}


Exception::Exception(const Exception &other) noexcept
  : message_(other.message_)
{}


Exception::~Exception() = default;


const char *Exception::what() const noexcept
{
  return message_.c_str();
}


void Exception::setMessage(const std::string &message, const std::string &filename, int line_number)
{
  std::string str;
  if (!message.empty() || !filename.empty())
  {
    std::ostringstream sstr;
    if (!filename.empty())
    {
      sstr << filename;

      if (line_number > 0)
      {
        sstr << '(' << line_number << "):";
      }
      sstr << ' ';
    }
    if (!message.empty())
    {
      sstr << message;
    }
    str = sstr.str();
  }

  message_ = std::move(str);
}
}  // namespace gputil


================================================
FILE: gputil/gpuException.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef GPUEXCEPTION_H
#define GPUEXCEPTION_H

#include "gpuConfig.h"
#include "gpuThrow.h"

#include <exception>
#include <string>

namespace gputil
{
/// Base class for exceptions thrown from this library.
class gputilAPI Exception : public std::exception  // NOLINT(cppcoreguidelines-special-member-functions)
{
public:
  /// Construct an exception.
  /// @param msg Optional exception message.
  /// @param filename Optional filename where the exception was thrown.
  /// @param line_number Optional line number where the exception was thrown.
  Exception(const std::string &msg = std::string(),  // NOLINT(google-explicit-constructor)
            const std::string &filename = std::string(), int line_number = 0);

  /// Move constructor.
  /// @param other The object to move.
  Exception(Exception &&other) noexcept;

  /// Copy constructor.
  /// @param other The object to copy.
  Exception(const Exception &other) noexcept;

  /// Destructor.
  ~Exception() override;

  /// Query the exception message. This is a combination of the constructed message, filename and line number.
  /// @return The exception message.
  const char *what() const noexcept override;

protected:
  /// Build an exception message from the given base @p message , @p filename and @p line_number .
  ///
  /// Formatted as `<filename>(<line_number>): <message>` where possible. Optional items may be omitted.
  ///
  /// @param message The base exception message.
  /// @param filename Optional filename where the exception was thrown.
  /// @param line_number Optional line number where the exception was thrown.
  void setMessage(const std::string &message, const std::string &filename = std::string(), int line_number = 0);

private:
  std::string message_;
};
}  // namespace gputil

#endif  // GPUEXCEPTION_H


================================================
FILE: gputil/gpuKernel.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUKERNEL_H
#define GPUKERNEL_H

#include "gpuConfig.h"

#include <functional>

namespace gputil
{
struct KernelDetail;
class Buffer;
class Device;
class EventList;
class Event;
class Queue;

/// A helper structure for defining the number of global work items or the size of a local work group.
///
/// Unused dimensions must have a value of 1 to ensure the correct @c volume().
struct gputilAPI Dim3
{
  /// Dimension X/first value.
  size_t x = 0;
  /// Dimension Y/second value.
  size_t y = 1;
  /// Dimension Z/third value.
  size_t z = 1;

  /// Default constructor: @c volume() is 1.
  Dim3() = default;

  /// Constructor.
  /// @param x The x item count.
  /// @param y The y item count.
  /// @param z The z item count.
  Dim3(size_t x, size_t y = 1u, size_t z = 1u)  // NOLINT(google-explicit-constructor)
    : x(x)
    , y(y)
    , z(z)
  {}

  /// Defines the total item size by multiplying the XYZ dimensions.
  inline size_t volume() const { return x * y * z; }

  /// Index operator.
  /// @param i The element index [0, 2].
  /// @return The value of the corresponding element.
  inline size_t operator[](int i) const
  {
    switch (i)
    {
    case 0:
      return x;
    case 1:
      return y;
    case 2:
      return z;
    default:
      break;  // Fallout
    }
    return 0;
  }

  /// @overload
  inline size_t &operator[](int i)
  {
    switch (i)
    {
    case 0:
      return x;
    case 1:
      return y;
    case 2:
      return z;
    default:
      break;  // Fall out to invalid.
    }
    static size_t invalid = 0u;
    return invalid;
  }

  /// @overload
  inline size_t operator[](unsigned i) const { return operator[](int(i)); }
  /// @overload
  inline size_t &operator[](unsigned i) { return operator[](int(i)); }

  /// @overload
  inline size_t operator[](size_t i) const { return operator[](int(i)); }
  /// @overload
  inline size_t &operator[](size_t i) { return operator[](int(i)); }
};

/// A helper class for passing buffer arguments to a kernel.
///
/// Given a kernel with the signature: @c myKernel(__global float3 *buffer), the argument may be passed as follows:
/// @code
/// void bufferArgExample(gputil::Kernel &kernel, gputil::Buffer &buffer)
/// {
///   kernel(nullptr, gputil::BufferArg<gputil::float3>(buffer));
/// }
/// @endcode
///
/// Note how the template type is the target type without any decoration (such as pointer).
template <typename T>
struct BufferArg
{
  /// The type to cast to in the GPU kernel.
  using ArgType = T;

  /// Constructor.
  /// @param buffer The buffer to wrap.
  explicit inline BufferArg(Buffer &buffer)
    : buffer(&buffer)
  {}

  /// Alternative constructor supporting a null buffer argument. Passing NULL ensures the kernel argument on device
  /// is also null.
  /// @param buffer A pointer to the buffer to wrap or null for a null argument on device.
  explicit inline BufferArg(Buffer *buffer = nullptr)
    : buffer(buffer)
  {}

  /// A reference to the wrapped buffer.
  Buffer *buffer;
};

/// Local memory calculation function.
/// @param work_group_size The work group total size.
/// @return The number of bytes required for a group this size.
using LocalMemFunc = std::function<size_t(size_t)>;

/// Defines a callable kernel object.
///
/// For OpenCL, this wraps the OpenCL kernel object and is initialised using <tt>gputil::openCLKernel()</tt>
/// using a @c Program and the entry point name.
///
/// For CUDA, this wraps a function pointer which calls the CUDA kernel and is created using
/// <tt>gputil::cudaKernel()</tt>.
///
/// There is no implementation indendent way of creating a @c Kernel.
///
/// Invoking the kernel requires at least a global and local size (threads and blocks size). OpenCL global offset
/// is not supported. A @c Queue pointer must be passed, though may be null, as it marks the beginning of device
/// arguments. An @c Event object to track completion and an @p EventList to wait on before executing may also be
/// optionally given any any combination. @c Buffer objects must be wrapped in a @c BufferArg in order to define
/// (pointer) type on the device.
///
/// A kernel invocation then takes this form:
/// @code{.unparsed}
///   kernel(global_size, local_size[, wait_on_events][, completion_event], queue, ...args);
/// @endcode
///
/// Local memory is sized by using @c addLocal() which defines a functional object to define requires local memory
/// size based on the total local memory size.
class gputilAPI Kernel
{
public:
  /// Construct an empty kernel.
  Kernel();
  /// Move constructor
  /// @param other The object to move.
  Kernel(Kernel &&other) noexcept;

  Kernel(const Kernel &other) = delete;

  Kernel &operator=(const Kernel &other) = delete;

  /// Destuctor - ensures @c release() is called.
  ~Kernel();

  /// Query if the kernel has been correctly setup.
  /// @return True if the kernel has been setup with error and can be invoked.
  bool isValid() const;

  /// Enable automatic error checking of the results of kernel invocations @c operator() .
  ///
  /// This is turned on by default and will automatically check the return value of kernel invocation. This will either
  /// throw an exception on failure - @c GPU_EXCEPTIONS on - or log an error message. Otherwise, the caller must check
  /// the return value manually, normally using @c checkResult()
  ///
  /// @param check True to enable checking, false to disable.
  void setErrorChecking(bool check);

  /// Is automatic kernel invocation error checking enabled (on by default). See @c setErrorChecking() .
  /// @return True if invocation automatic error checking is enabled.
  bool errorChecking() const;

  /// Release the GPU kernel.
  void release();

  /// Add local memory calculation.
  ///
  /// Local memory is calculated by invoking the given function, passing the single dimensional work group size
  /// in order to calculate the required work group local memory size in bytes. This function is invoked just
  /// prior to invoking the kernel and when calculating the optimal work group size.
  ///
  /// Under CUDA, local memory requirements are tallied and passed to the kernel function hook given the total local
  /// memory required.
  ///
  /// Under OpenCL, each @c addLocal() call adds a local memory argument to the end of the argument list.
  ///
  /// @param local_calc The functional object used to calculate local memory size requirements.
  void addLocal(const LocalMemFunc &local_calc);

  /// Calculate the optimal size (or volume) of a local work group. This attempts to gain maximum occupancy while
  /// considering the required local memory usage.
  /// @return The optimal work group size.
  size_t calculateOptimalWorkGroupSize();

  /// Fetch the previously calculated optimal work group size (see @c calculateOptimalWorkGroupSize()).
  /// @return The optimal work group size.
  size_t optimalWorkGroupSize() const;

  /// Calculate the appropriate global and work group sizes for executing this @c Kernel to process
  /// @p total_work_items items. The aim is to gain maximum local thread occupancy.
  ///
  /// The @p total_work_items defines a volume of items to process. The global size is set appropriately to cover
  /// the @p total_work_items with the @p local_size set to cover these in a grid pattern with consideration to the
  /// device capabilities and maximum occupancy. This includes maximum work group sizes and local memory constraints.
  ///
  /// @param[out] global_size Set to the grid global size. This may be larger than @p total_work_group_items to ensure
  ///   an exact multiple of the @p local_size.
  /// @param[out] local_size Set to the local work group size required to cover the @p global_size/@p
  /// total_work_items.
  /// @param total_work_items The total volume of items to process.
  void calculateGrid(gputil::Dim3 *global_size, gputil::Dim3 *local_size, const gputil::Dim3 &total_work_items);

  /// Queue an invocation of the kernel with the given global and local sizes and the specified queue - arguments
  /// follow.
  /// @param global_size The global dimensions of the of GPU thread pool to run the the kernel with.
  /// @param local_size The local thread division of the @p global_size .
  /// @param queue The GPU queue to invoke the kernel on. May be null to used the default queue
  /// @param args Arguments to pass to the kernel.
  /// @return Zero on success or an SDK error code on falure - e.g., @c cudaSuccess .
  template <typename... ARGS>
  int operator()(const Dim3 &global_size, const Dim3 &local_size, Queue *queue, ARGS... args);

  /// Queue an invocation of the kernel with the given global and local sizes and the specified queue - arguments
  /// follow. The @p completion_event is set to mark the completion of the invocation and can be used for
  /// synchronisation. For example waiting on @p completion_event on CPU blocks the CPU until the kernel completes.
  /// @param global_size The global dimensions of the of GPU thread pool to run the the kernel with.
  /// @param local_size The local thread division of the @p global_size .
  /// @param completion_event Event object modified to mark the completion of the kernel execution.
  /// @param queue The GPU queue to invoke the kernel on. May be null to used the default queue
  /// @param args Arguments to pass to the kernel.
  /// @return Zero on success or an SDK error code on falure - e.g., @c cudaSuccess .
  template <typename... ARGS>
  int operator()(const Dim3 &global_size, const Dim3 &local_size, Event &completion_event, Queue *queue, ARGS... args);

  /// Queue an invocation of the kernel to start after all @p event_list items complete.
  /// @param global_size The global dimensions of the of GPU thread pool to run the the kernel with.
  /// @param local_size The local thread division of the @p global_size .
  /// @param event_list Events which must complete before kernel execution can start.
  /// @param queue The GPU queue to invoke the kernel on. May be null to used the default queue
  /// @param args Arguments to pass to the kernel.
  /// @return Zero on success or an SDK error code on falure - e.g., @c cudaSuccess .
  template <typename... ARGS>
  int operator()(const Dim3 &global_size, const Dim3 &local_size, const EventList &event_list, Queue *queue,
                 ARGS... args);


  /// Queue an invocation of the kernel to start after all @p event_list items complete. The @p completion_event is
  /// set to mark the completion of the invocation and can be used for synchronisation. For example waiting on @p
  /// completion_event on CPU blocks the CPU until the kernel completes.
  /// @param global_size The global dimensions of the of GPU thread pool to run the the kernel with.
  /// @param local_size The local thread division of the @p global_size .
  /// @param event_list Events which must complete before kernel execution can start.
  /// @param completion_event Event object modified to mark the completion of the kernel execution.
  /// @param queue The GPU queue to invoke the kernel on. May be null to used the default queue
  /// @param args Arguments to pass to the kernel.
  /// @return Zero on success or an SDK error code on falure - e.g., @c cudaSuccess .
  template <typename... ARGS>
  int operator()(const Dim3 &global_size, const Dim3 &local_size, const EventList &event_list, Event &completion_event,
                 Queue *queue, ARGS... args);

  /// Internal kernel representation.
  /// @return The internal representation.
  KernelDetail *detail() const { return imp_; }

  /// Query the @c Device which the kernel has been associated with and on which the kernel will run when invoked.
  /// @return The kernel's @c Device .
  Device device();

  /// Move assignment.
  /// @param other The object to move.
  /// @return `*this`
  Kernel &operator=(Kernel &&other) noexcept;

  /// A helper function for checking the results of a kernel invocation ( @c operator() ). On an error result, this will
  /// either throw a @c gputil::ApiException or log an error and return false.
  ///
  /// @note Kernel invocations are asynchronous so errors may be delayed until the next GPU API call. These can be
  /// trapped synchronously by setting @c Queue::setSynchronous(true) on the queue used as part of the invocation.
  ///
  /// @param invocation_result The return value for @c operator()
  /// @param allow_throw True to allow throwing of exceptions on failure even if exceptions are enabled.
  /// @return True if there is no error, false if there is an error and gputil exceptions are disabled -
  /// @c `GPU_EXCEPTIONS 0` or @p allow_throw is false.
  static bool checkResult(int invocation_result, bool allow_throw = true);

private:
  KernelDetail *imp_;
};
}  // namespace gputil

#if GPUTIL_TYPE == GPUTIL_OPENCL
#include "cl/gpuKernel2.h"
#elif GPUTIL_TYPE == GPUTIL_CUDA
#include "cuda/gpuKernel2.h"
#else  // GPUTIL_TYPE == ???
#error Unknown GPU base API
#endif  // GPUTIL_TYPE

#endif  // GPUKERNEL_H


================================================
FILE: gputil/gpuPinMode.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUPINMODE_H
#define GPUPINMODE_H

#include "gpuConfig.h"

namespace gputil
{
// Pinning functions.
enum PinMode
{
  /// Null pinning mode - invalid.
  kPinNone = 0,
  /// Pin for reading on CPU.
  kPinRead,
  /// Pin for writing from CPU.
  kPinWrite,
  /// Pin for read/write.
  kPinReadWrite
};
}  // namespace gputil

#endif  // GPUPINMODE_H


================================================
FILE: gputil/gpuPinnedBuffer.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUPINNEDBUFFER_H
#define GPUPINNEDBUFFER_H

#include "gpuConfig.h"

#include "gpuPinMode.h"

#include <cstddef>

namespace gputil
{
class Buffer;
class Event;
class Queue;

/// A utility class for direct memory access to pinned, host accessible memory.
///
/// Pinned memory allows for far faster transfers.
///
/// Falls back to unpinned memory transfers when required.
///
/// To be used as transient objects only.
class gputilAPI PinnedBuffer
{
public:
  /// Construct an invalid pinned buffer. May not be used.
  PinnedBuffer();

  /// Try pin @p buffer in the given @p mode.
  /// @param buffer Buffer to pin.
  /// @param mode The mode to pin in.
  PinnedBuffer(Buffer &buffer, PinMode mode);

  /// RValue constructor.
  /// @param other Temporary object to copy from.
  PinnedBuffer(PinnedBuffer &&other) noexcept;

  PinnedBuffer(const PinnedBuffer &other) = delete;

  /// Destructor. Ensures unpinning in blocking mode. Call @c unpin() explicitly to support non-blocking writes.
  ~PinnedBuffer();

  /// Is the memory actually pinned?
  /// @return True if actually pinned, false when using fallbacks.
  bool isPinned() const;

  /// Repin the buffer if it has been unpinned.
  void pin();

  /// Unpin or release the pinned memory, invalidating this object.
  ///
  /// The unpin call blocks until the memory transfer completes when @p blocking is @c true. Otherwise this method
  /// may return before the memory transfer is complete. When calling asynchronously, it may be necessary to
  /// use Queue::insertBarrier() before performing any operations which read the target memory.
  ///
  /// Note that the destructor calls this method in blocking mode when this method has not been explicitly called.
  ///
  /// See @c Buffer class notes on asynchronous memory transfer for details on how the parameters modify the transfer,
  /// noting that asynchronous unpinning is only relevant when writting to the buffer.
  ///
  /// @param queue Optional queue to use for the memory transfer. Recommended only for non-blocking use.
  /// @param block_on Option event to wait on before unpinning.
  /// @param completion Event to track completion of the unpinning DMA.
  void unpin(Queue *queue = nullptr, Event *block_on = nullptr, Event *completion = nullptr);

  /// Access the pinned buffer.
  /// @return The pinned buffer.
  inline Buffer *buffer() const { return buffer_; }

  /// Query the pinning mode.
  inline PinMode mode() const { return mode_; }

  /// Read from the buffer into @p dst.
  /// @param dst The memory to write to.
  /// @param byte_count Number of bytes to read.
  /// @param src_offset Byte offset into the buffer to read from.
  /// @return The number of bytes read.
  size_t read(void *dst, size_t byte_count, size_t src_offset = 0) const;

  /// Write data from @p src to the buffer.
  /// @param src Data to write into the buffer.
  /// @param byte_count Number of bytes to write.
  /// @param dst_offset Byte offset into the buffer to write at.
  /// @return The number of bytes written.
  size_t write(const void *src, size_t byte_count, size_t dst_offset = 0);

  /// Read an array of data elements from the buffer.
  ///
  /// This method supports a size discrepancy between elements in the buffer and elements in @c dst.
  /// This is primarily to cater for the size difference between a 3-element float vector and
  /// @c cl_float3, which is a 4-element vector, with a redundant @c w component.
  ///
  /// Where elements in @p dst and @c this buffer do not match, specify the @p elementSize as the
  /// element stride in @p dst and @p bufferElementSize as the stride in @c this buffer.
  ///
  /// The method will copy as many elements as are available in @c this buffer. The @p dst memory
  /// must be sized as follows <tt>element_count * elementSize</tt>.
  ///
  /// @param dst The buffer to write into. Must be sized to suit.
  /// @param element_size The element stride in @p dst. The same stride is used for @c this buffer when
  ///     @p bufferElementSize is zero.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements Number of elements to offset the read start. That is skip this number of elements before
  ///     reading.
  /// @param buffer_element_size Optional element stride within @c this buffer when different from
  ///     @p elementSize.
  /// @return The number of elements read. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  size_t readElements(void *dst, size_t element_size, size_t element_count, size_t offset_elements,
                      size_t buffer_element_size);

  /// An overload of @c readElements() using the template sizes to determine the sizes.
  /// @tparam T The data type to read into and assumed to exactly match that stored in the GPU buffer.
  /// @param dst The buffer to write into. Must be sized to suit.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements Number of elements to offset the read start. That is skip this number of elements before
  ///     reading.
  /// @return The number of elements read. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  template <typename T>
  inline size_t readElements(T *dst, size_t element_count, size_t offset_elements = 0)
  {
    return readElements(dst, sizeof(T), element_count, offset_elements, 0u);
  }

  /// An overload of @c readElements() using the template sizes to determine the sizes.
  /// @tparam BufferType The data type stored in the GPU buffer. Only required when its size differs from that of
  ///   @p T
  /// @tparam T The data type to read into.
  /// @param dst The buffer to write into. Must be sized to suit.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements Number of elements to offset the read start. That is skip this number of elements before
  ///     reading.
  /// @return The number of elements read. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  template <typename BufferType, typename T>
  inline size_t readElements(T *dst, size_t element_count, size_t offset_elements = 0)
  {
    return readElements(dst, sizeof(T), element_count, offset_elements, sizeof(BufferType));
  }

  /// Write an array of data elements to the buffer.
  ///
  /// This method supports the same size discrepancy as @c readElements().
  ///
  /// The method will copy as many elements as are available in @c this buffer. The @p dst memory
  /// must be sized as follows <tt>element_count * element_size</tt>.
  ///
  /// @param src The buffer to read from.
  /// @param element_size The element stride in @p src. The same stride is used for @c this buffer when
  ///     @p bufferElementSize is zero.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements The offset to start writing at.
  /// @param buffer_element_size Optional element stride within @c this buffer when different from
  ///     @p element_size. Use zero when the size matches the @p element_size.
  /// @return The number of elements written. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  size_t writeElements(const void *src, size_t element_size, size_t element_count, size_t offset_elements,
                       size_t buffer_element_size);

  /// An overload of @c writeElements() using the template sizes to determine the sizes.
  /// @tparam T The data type to read into and assumed to exactly match that stored in the GPU buffer.
  /// @param src The buffer to read from into. Must be sized to suit.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements Number of elements to offset the write start. That is skip this number of elements
  ///     before writing.
  /// @return The number of elements written. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  template <typename T>
  inline size_t writeElements(const T *src, size_t element_count, size_t offset_elements = 0)
  {
    return writeElements(src, sizeof(T), element_count, offset_elements, 0u);
  }

  /// An overload of @c writeElements() using the template sizes to determine the sizes.
  /// @tparam BufferType The data type stored in the GPU buffer. Only required when its size differs from that of
  ///   @p T
  /// @tparam T The data type to read into and assumed to exactly match that stored in the GPU buffer.
  /// @param src The buffer to read from into. Must be sized to suit.
  /// @param element_count The number of elements to copy.
  /// @param offset_elements Number of elements to offset the write start. That is skip this number of elements
  ///     before writing.
  /// @return The number of elements written. May be less than @c element_count if this buffer is not
  ///     large enough to hold @c element_count.
  template <typename BufferType, typename T>
  inline size_t writeElements(const T *src, size_t element_count, size_t offset_elements = 0)
  {
    return writeElements(src, sizeof(T), element_count, offset_elements, sizeof(BufferType));
  }

  /// RValue assignment operator.
  /// @param other Temporary object to copy from.
  PinnedBuffer &operator=(PinnedBuffer &&other) noexcept;
  PinnedBuffer &operator=(const PinnedBuffer &other) = delete;

private:
  Buffer *buffer_ = nullptr;
  void *pinned_ = nullptr;
  PinMode mode_ = kPinNone;
};
}  // namespace gputil

#endif  // GPUPINNEDBUFFER_H


================================================
FILE: gputil/gpuPlatform.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUPLATFORM_H
#define GPUPLATFORM_H

#include "gpuConfig.h"

#if GPUTIL_TYPE == GPUTIL_OPENCL
#include "cl/gpuPlatform2.h"
#elif GPUTIL_TYPE == GPUTIL_CUDA
#include "cuda/gpuPlatform2.h"
#else  // GPUTIL_TYPE == ???
#error Unknown GPU base API
#endif  // GPUTIL_TYPE

#endif  // GPUPLATFORM_H


================================================
FILE: gputil/gpuProgram.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUPROGRAM_H
#define GPUPROGRAM_H

#include "gpuConfig.h"

#include <memory>
#include <string>
#include <vector>

namespace gputil
{
class Device;
struct ProgramDetail;

/// Additional arguments specifying how to build a GPU program.
struct gputilAPI BuildArgs
{
  /// Target (major) API version to build for - e.g., the `1` in OpenCL 1.2. Optional.
  int version_major = -1;
  /// Target (minor) API version to build for - e.g., the `2` in OpenCL 1.2. Optional.
  int version_minor = -1;
  /// Specifies the level of debugging to enable. Increasing values increase the debug information, which varies
  /// depending on the underlying API and vendor. For CUDA, this is ignored as the debug level is set at C++
  /// compilation time. For OpenCL the levels are:
  /// - 0 : Disabled
  /// - 1 : Define `DEBUG` in the OpenCL code. For Intel, add '-g' option.
  /// - 2 : For Intel, add `-cl-opt-disable`
  ///
  /// Note that the @c Program uses the maximum of this value and the @c Device::debugGpu() value.
  int debug_level = 0;
  /// Additional arguments to pass to the GPU compiler.
  std::vector<std::string> *args = nullptr;
};

/// Defines a compiled GPU program.
///
/// This object has an empty CUDA implementation.
class gputilAPI Program
{
public:
  /// Create an empty/invalid program.
  Program();
  /// Create a program to run on @p device and refered to as @c program_name .
  /// @param device The @c Device to bind the program to.
  /// @param program_name The reference name for the program.
  Program(Device &device, const char *program_name);
  /// Move constructor.
  /// @param other The object to move.
  Program(Program &&other) noexcept;
  /// Copy constructor.
  /// @param other The object to copy.
  Program(const Program &other);

  /// Destuctor - releases the GPU program.
  ~Program();

  /// Query the validity of this object.
  /// @return True if the program is valid.
  bool isValid() const;

  /// Query the program name.
  /// @return The program reference name.
  const char *programName() const;

  /// Query the @c Device the program is bound to.
  /// @return The program device.
  Device device();

  /// Build the program from the given source @p file_name .
  /// @param file_name Path to the source file - resolved on the given file system and working directory.
  /// @param build_args See @c BuildArgs .
  /// @return Zero on success or an SDK error code on failure.
  int buildFromFile(const char *file_name, const BuildArgs &build_args);
  /// Build the program from the given source string @p source .
  /// @param source The string containing the GPU source code.
  /// @param source_length The number of characters in @p source excluding the null terminator.
  /// @param build_args See @c BuildArgs .
  /// @return Zero on success or an SDK error code on failure.
  int buildFromSource(const char *source, size_t source_length, const BuildArgs &build_args);

  /// Internal program details.
  /// @return Internal details.
  inline ProgramDetail *detail() { return imp_.get(); }
  /// Internal program details.
  /// @return Internal details.
  inline const ProgramDetail *detail() const { return imp_.get(); }

  /// Copy assignment. Increments the program reference count.
  /// @param other The program to copy.
  /// @return `*this`
  Program &operator=(const Program &other);
  /// Move assignment.
  /// @param other The program to move.
  /// @return `*this`
  Program &operator=(Program &&other) noexcept;

private:
  std::unique_ptr<ProgramDetail> imp_;
};
}  // namespace gputil

#endif  // GPUPROGRAM_H


================================================
FILE: gputil/gpuQueue.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUQUEUE_H
#define GPUQUEUE_H

#include "gpuConfig.h"

#include "gpuEvent.h"

#include <functional>
#include <memory>

namespace gputil
{
struct QueueDetail;

/// Represents a command queue or stream on the GPU.
///
/// Using multiple queues allow commands to be queue and managed in parallel. A queue may be
/// synchronised using @c finish() or @c insertBarrier(). The former blocks until all queued operations
/// have completed, while the latter creates a synchronisation point within the queue.
///
/// Some functions, particularly those relating to @c Buffer, support an optional @c Queue argument.
/// providing an explicit @c Queue argument changes the behaviour of such methods to be non-blocking
/// operations. That is, the operation is inserted into the queue, without waiting for execution to
/// complete. The @c Queue synchronisation methods, noted above, may be used to explicitly synchronise.
///
/// In OpenCL terms this is an OpenCL queue. In CUDA terms it is a stream. A @c Queue may be used
/// to invoke asynchronous GPU operations and control waiting for the results.
///
/// Note that the synchronisation functionality provided here, namely @c finish() and @c insertBarrier(),
/// are modelled more closely on CUDA streams than on the OpenCL event model. This is for simplicity.
/// An equivalent to OpenCL style events is not really supported in CUDA.
///
/// @todo Consider adding CUDA event/OpenCL marker supporting wait for methods.
class gputilAPI Queue
{
public:
  /// Queue construction flags.
  enum QueueFlag : unsigned
  {
    kProfile = (1u << 0u)  ///< Enable profiling of the queue object.
  };

  /// Empty constructor.
  Queue();
  /// Copy constructor - references the same queue as @p other .
  /// @param other The queue to copy.
  Queue(const Queue &other);
  /// Move constructor.
  /// @param other The object to move.
  Queue(Queue &&other) noexcept;
  /// Construct from the underlying SDK queue implemntation.
  explicit Queue(void *platform_queue);

  /// Destructor - release a reference to the unerlying queue object.
  ~Queue();

  /// Is this a valid @c Queue object?
  /// @return True if valid.
  bool isValid() const;

  /// Insert a barrier which ensures all operations before the barrier complete before executing
  /// operations queued after the barrier.
  void insertBarrier();

  /// Insert an event into the queue which marks the end of currently queued operations.
  /// This event may be used to block on completion of all currently queued operations.
  /// @return An event marking the current queue location.
  Event mark();

  /// Enable or disable synchronous execution mode. Normally providing a @c Queue object to various gputil API functions
  /// implies asynchronous operation. Setting the @p synchronous flag will disable asynchronous operations, reverting
  /// to synchronous usage of the underlying GPU API. This is intended for debugging API exceptions as in asynchronous
  /// mode, reporting of API errors may be reported on later GPU API function calls rather than on the call which caused
  /// the error.
  ///
  /// All memory operations memory operations become synchronous and kernel function invocations are checked for
  /// completion before the @c Kernel::operator() invocation function returns.
  ///
  /// @param synchronous True to enable forcining synchronous API operations.
  void setSynchronous(bool synchronous);

  /// Check if the queue is in synchronous mode. See @c setSynchronous()
  /// @return True if asynchronous GPU API function calls are disabled for this queue.
  bool synchronous() const;

  /// Ensure that all queued commands have been submitted to the devices. A return does
  /// not mean that they have completed. For that, use @c finish().
  void flush();

  /// Wait for all outstanding operations in the queue to complete.
  void finish();

  /// Add a callback to the queue. The callback is invoked on CPU when queue execution reaches the current location
  /// in the queue. Note this callback may be called from a thread other than the main thread.
  /// @param callback The function to invoke.
  void queueCallback(const std::function<void(void)> &callback);

  /// Internal data access for private code.
  /// @return Internal queue details.
  QueueDetail *internal() const;

  /// Copy assignment.
  /// @param other The object to copy.
  Queue &operator=(const Queue &other);
  /// Move assignment.
  /// @param other The object to move.
  Queue &operator=(Queue &&other) noexcept;

private:
  // Cannot use unique pointer due to CUDA implementation details.
  std::shared_ptr<QueueDetail> queue_;
};
}  // namespace gputil

#endif  // GPUQUEUE_H


================================================
FILE: gputil/gpuThrow.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "gpuThrow.h"

#include "gpuException.h"

#include <iostream>

namespace gputil
{
void log(const Exception &e)
{
  std::cerr << "GPU Exception: " << e.what() << std::endl;
}


void log(const Exception &e, const char *file, int line)
{
  std::cerr << file << "(" << line << "): GPU exception: " << e.what() << std::endl;
}
}  // namespace gputil


================================================
FILE: gputil/gpuThrow.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef GPUTHROW_H
#define GPUTHROW_H

#include "gpuConfig.h"

#define GPU_EXCEPTIONS 1

#if GPU_EXCEPTIONS
// Return statement present to prevent compilation errors when switching GPU_EXCEPTIONS.
#define GPUTHROW(e, r) \
  {                    \
    throw(e);          \
    return r;          \
  }
#define GPUTHROW2(e) throw e
#else  // GPU_EXCEPTIONS
#define GPUTHROW(e, r) \
  {                    \
    gputil::log(e);    \
    return r;          \
  }
#define GPUTHROW2(e) \
  {                  \
    gputil::log(e);  \
    return;          \
  }
#endif  // GPU_EXCEPTIONS

#define GPUAPICHECK(err, expect, r)                                      \
  if ((err) != (expect))                                                 \
  {                                                                      \
    GPUTHROW(gputil::ApiException(err, nullptr, __FILE__, __LINE__), r); \
  }
#define GPUAPICHECK2(err, expect)                                      \
  if ((err) != (expect))                                               \
  {                                                                    \
    GPUTHROW2(gputil::ApiException(err, nullptr, __FILE__, __LINE__)); \
  }

namespace gputil
{
class Exception;

void gputilAPI log(const Exception &e);
void gputilAPI log(const Exception &e, const char *file, int line);
}  // namespace gputil

#endif  // GPUTHROW_H


================================================
FILE: gputil/gpuVersion.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUVERSION_H
#define GPUVERSION_H

#include "gpuConfig.h"

#include <cinttypes>

namespace gputil
{
/// Version number structure for the GPU device/API split as `<major>.<minor>.<patch>`.
struct Version
{
  uint16_t major = 0;  ///< Major version part.
  uint16_t minor = 0;  ///< Minor version part.
  uint16_t patch = 0;  ///< Patch version part.

  /// Equality operator.
  /// @param other The object to compare against.
  /// @return True if the two versions exactly match.
  inline bool operator==(const Version &other) const
  {
    return major == other.major && minor == other.minor && patch == other.patch;
  }
};
}  // namespace gputil

#endif  // GPUVERSION_H


================================================
FILE: gputil/gpu_ext.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

// This header file can be included into an OpenCL to be compiled for either OpenCL or CUDA.
// It is used to help provide a mapping from functions available on one platform to the other.
// For example, we define make_typeN() macros on OpenCL to match CUDA style initialisation of
// vector types.
//
// The header also helps resolve some missing defines on some OpenCL platforms.
//
// This is a work in progress and will be extended as needed.
//
// Other usage notes:
// - Use __local as a prefix to local memory arguments in OpenCL. Removed in CUDA
// - Use local for local memory delcarations. Converted to __shared__ in CUDA.
//   This is then removed in CUDA compilation as it is not needed. __local already has meaning
//   in CUDA so we couldn't just use that.

#ifndef GPU_EXT_H_
#define GPU_EXT_H_

#define GPUTIL_NULL   0
#define GPUTIL_OPENCL 1
#define GPUTIL_CUDA   2

// Setup GPUTIL_DEVICE. This has a non-zero value only when building device code.
#if defined(__OPENCL_C_VERSION__)
#define GPUTIL_DEVICE GPUTIL_OPENCL
#elif defined(__CUDACC__)
#define GPUTIL_DEVICE GPUTIL_CUDA
#else
#define GPUTIL_DEVICE GPUTIL_NULL
#endif

#if GPUTIL_DEVICE == GPUTIL_OPENCL
#endif  // GPUTIL_DEVICE == GPUTIL_OPENCL

#if GPUTIL_DEVICE == GPUTIL_OPENCL
#ifndef NULL
#define NULL 0
#endif  // !NULL

#define LOCAL_ARG(TYPE, VAR) , __local TYPE VAR
#define LOCAL_VAR(TYPE, VAR, SIZE)
#define LOCAL_MEM_ENABLE()
#define LM_PER_THREAD(PER_THREAD_SIZE)

//-----------------------------------------------------------------------------
// OpenCL defines
//-----------------------------------------------------------------------------
#define __device__
#define __host__

#ifndef make_char2
#define make_char2   (char2)
#define make_char3   (char3)
#define make_char4   (char4)
#define make_uchar2  (uchar2)
#define make_uchar3  (uchar3)
#define make_uchar4  (uchar4)
#define make_short2  (short2)
#define make_short3  (short3)
#define make_short4  (short4)
#define make_short16 (short16)
#define make_ushort2 (ushort2)
#define make_ushort3 (ushort3)
#define make_ushort4 (ushort4)
#define make_int2    (int2)
#define make_int3    (int3)
#define make_int4    (int4)
#define make_uint2   (uint2)
#define make_uint3   (uint3)
#define make_uint4   (uint4)
#define make_long2   (long2)
#define make_long3   (long3)
#define make_long4   (long4)
#define make_ulong2  (ulong2)
#define make_ulong3  (ulong3)
#define make_ulong4  (ulong4)
#define make_float2  (float2)
#define make_float3  (float3)
#define make_float4  (float4)
#define make_double2 (double2)
#endif  // make_char2

#ifndef xyz
#define xyz(V) (V).xyz
#endif  // xyz

// Missing defines on some OpenCL devices.
// TODO: add more.
#ifndef M_PI
#define M_PI 3.14159265358979323846264338327950288f
#endif  // M_PI

#define SQR(X) ((X) * (X))

// long/ulong may be defined for CUDA linux as a 32-bit types.
// TODO(KS): Find a better way of seting with this by forcing CUDA to use long/ulong as a 64-bit types.
typedef long longlong;
typedef ulong ulonglong;

#if defined(__OPENCL_C_VERSION__) && GPUTIL_ATOMICS_64
#pragma OPENCL EXTENSION cl_khr_int64_base_atomics : enable
#pragma OPENCL EXTENSION cl_khr_int64_extended_atomics : enable
#endif  // defined(__OPENCL_C_VERSION__) && GPUTIL_ATOMICS_64

//----------------------------------------------------------------------------------------------------------------------
// Atomic definitions. Make consistent atomic function interface across all GPU code versions.
//
// Only 32-bit types are supported.
//
// Note: The selection of which underlying OpenCL functions to use is currently based on OpenCL standard used with
// Intel GPU. This may not be portable enough. We may need to define this API based on capabilities instead.
//----------------------------------------------------------------------------------------------------------------------
#if __OPENCL_C_VERSION__ >= 200

inline void gputilAtomicInitI32(__global atomic_int *obj, int val)
{
  atomic_init(obj, val);
}
inline void gputilAtomicInitI32L(__local atomic_int *obj, int val)
{
  atomic_init(obj, val);
}
inline void gputilAtomicStoreI32(__global atomic_int *obj, int val)
{
  atomic_store(obj, val);
}
inline void gputilAtomicStoreI32L(__local atomic_int *obj, int val)
{
  atomic_store(obj, val);
}
inline int gputilAtomicLoadI32(__global atomic_int *obj)
{
  return atomic_load(obj);
}
inline int gputilAtomicLoadI32L(__local atomic_int *obj)
{
  return atomic_load(obj);
}
inline int gputilAtomicExchangeI32(__global atomic_int *obj, int desired)
{
  return atomic_exchange(obj, desired);
}
inline int gputilAtomicExchangeI32L(__local atomic_int *obj, int desired)
{
  return atomic_exchange(obj, desired);
}
inline bool gputilAtomicCasI32(__global atomic_int *obj, int expected, int desired)
{
  return atomic_compare_exchange_weak(obj, &expected, desired);
}
inline bool gputilAtomicCasI32L(__local atomic_int *obj, int expected, int desired)
{
  return atomic_compare_exchange_weak(obj, &expected, desired);
}

inline void gputilAtomicInitU32(__global atomic_uint *obj, uint val)
{
  atomic_init(obj, val);
}
inline void gputilAtomicInitU32L(__local atomic_uint *obj, uint val)
{
  atomic_init(obj, val);
}
inline void gputilAtomicStoreU32(__global atomic_uint *obj, uint val)
{
  atomic_store(obj, val);
}
inline void gputilAtomicStoreU32L(__local atomic_uint *obj, uint val)
{
  atomic_store(obj, val);
}
inline uint gputilAtomicLoadU32(__global atomic_uint *obj)
{
  return atomic_load(obj);
}
inline uint gputilAtomicLoadU32L(__local atomic_uint *obj)
{
  return atomic_load(obj);
}
inline uint gputilAtomicExchangeU32(__global atomic_uint *obj, uint desired)
{
  return atomic_exchange(obj, desired);
}
inline uint gputilAtomicExchangeU32L(__local atomic_uint *obj, uint desired)
{
  return atomic_exchange(obj, desired);
}
inline bool gputilAtomicCasU32(__global atomic_uint *obj, uint expected, uint desired)
{
  return atomic_compare_exchange_weak(obj, &expected, desired);
}
inline bool gputilAtomicCasU32L(__local atomic_uint *obj, uint expected, uint desired)
{
  return atomic_compare_exchange_weak(obj, &expected, desired);
}

#if GPUTIL_ATOMICS_64
inline void gputilAtomicInitU64(__global atomic_ulong *obj, ulonglong val)
{
  atomic_init(obj, val);
}
inline void gputilAtomicInitU64L(__local atomic_ulong *obj, ulonglong val)
{
  atomic_init(obj, val);
}
inline void gputilAtomicStoreU64(__global atomic_ulong *obj, ulonglong val)
{
  atomic_store(obj, val);
}
inline void gputilAtomicStoreU64L(__local atomic_ulong *obj, ulonglong val)
{
  atomic_store(obj, val);
}
inline ulonglong gputilAtomicLoadU64(__global atomic_ulong *obj)
{
  return atomic_load(obj);
}
inline ulonglong gputilAtomicLoadU64L(__local atomic_ulong *obj)
{
  return atomic_load(obj);
}
inline ulonglong gputilAtomicExchangeU64(__global atomic_ulong *obj, ulonglong desired)
{
  return atomic_exchange(obj, desired);
}
inline ulonglong gputilAtomicExchangeU64L(__local atomic_ulong *obj, ulonglong desired)
{
  return atomic_exchange(obj, desired);
}
inline bool gputilAtomicCasU64(__global atomic_ulong *obj, ulonglong expected, ulonglong desired)
{
  return atomic_compare_exchange_weak(obj, &expected, desired);
}
inline bool gputilAtomicCasU64L(__local atomic_ulong *obj, ulonglong expected, ulonglong desired)
{
  return atomic_compare_exchange_weak(obj, &expected, desired);
}
#endif  // GPUTIL_ATOMICS_64

#if __OPENCL_C_VERSION__ < 210
inline void gputilAtomicInitF32(__global atomic_float *obj, float val)
{
  gputilAtomicInitI32((__global atomic_int *)obj, *(int *)&val);
}
inline void gputilAtomicInitF32L(__local atomic_float *obj, float val)
{
  gputilAtomicInitI32L((__local atomic_int *)obj, *(int *)&val);
}
inline void gputilAtomicStoreF32(__global atomic_float *obj, float val)
{
  gputilAtomicStoreI32((__global atomic_int *)obj, *(int *)&val);
}
inline void gputilAtomicStoreF32L(__local atomic_float *obj, float val)
{
  gputilAtomicStoreI32L((__local atomic_int *)obj, *(int *)&val);
}
inline float gputilAtomicLoadF32(__global atomic_float *obj)
{
  int r = gputilAtomicLoadI32((__global atomic_int *)obj);
  return *(float *)&r;
}
inline float gputilAtomicLoadF32L(__local atomic_float *obj)
{
  int r = gputilAtomicLoadI32L((__local atomic_int *)obj);
  return *(float *)&r;
}
inline float gputilAtomicExchangeF32(__global atomic_float *obj, float desired)
{
  int r = gputilAtomicExchangeI32((__global atomic_int *)obj, *(int *)&desired);
  return *(float *)&r;
}
inline float gputilAtomicExchangeF32L(__local atomic_float *obj, float desired)
{
  int r = gputilAtomicExchangeI32L((__local atomic_int *)obj, *(int *)&desired);
  return *(float *)&r;
}
inline bool gputilAtomicCasF32(__global atomic_float *obj, float expected, float desired)
{
  return gputilAtomicCasI32((__global atomic_int *)obj, *(int *)&expected, *(int *)&desired);
}
inline bool gputilAtomicCasF32L(__local atomic_float *obj, float expected, float desired)
{
  return gputilAtomicCasI32L((__local atomic_int *)obj, *(int *)&expected, *(int *)&desired);
}
#else   // __OPENCL_C_VERSION__ < 210
inline void gputilAtomicInitF32(__global atomic_float *obj, float val)
{
  atomic_init(obj, val);
}
inline void gputilAtomicInitF32L(__local atomic_float *obj, float val)
{
  atomic_init(obj, val);
}
inline void gputilAtomicStoreF32(__global atomic_float *obj, float val)
{
  atomic_store(obj, val);
}
inline void gputilAtomicStoreF32L(__local atomic_float *obj, float val)
{
  atomic_store(obj, val);
}
inline float gputilAtomicLoadF32(__global atomic_float *obj)
{
  return atomic_load(obj);
}
inline float gputilAtomicLoadF32L(__local atomic_float *obj)
{
  return atomic_load(obj);
}
inline float gputilAtomicExchangeF32(__global atomic_float *obj, float desired)
{
  return atomic_exchange(obj, desired);
}
inline float gputilAtomicExchangeF32L(__local atomic_float *obj, float desired)
{
  return atomic_exchange(obj, desired);
}
inline bool gputilAtomicCasF32(__global atomic_float *obj, float expected, float desired)
{
  // FIXME(KS):
  // Seems intel OpenCL does not support atomic_compare_exchange_weak() for atomic_float - undefined references
  // were observed on an Intel 11th Gen i9 UHD graphics. I can't find any indication of what feature needs to be
  // supported for this so I'm going with a hack in order to support the current requirement.
  return atomic_compare_exchange_weak((__global atomic_int *)obj, (int *)&expected, *(int *)&desired);
  //return atomic_compare_exchange_weak(obj, &expected, desired);
}
inline bool gputilAtomicCasF32L(__local atomic_float *obj, float expected, float desired)
{
  // See comment in gputilAtomicCasF32()
  return atomic_compare_exchange_weak((__local atomic_int *)obj, (int *)&expected, *(int *)&desired);
}
#endif  // __OPENCL_C_VERSION__ < 210

#define gputilAtomicAdd(p, val) atomic_fetch_add(p, val)
#define gputilAtomicSub(p, val) atomic_fetch_sub(p, val)
#define gputilAtomicInc(p)      atomic_fetch_add(p, 1)
#define gputilAtomicDec(p)      atomic_fetch_sub(p, 1)
#define gputilAtomicMin(p, val) atomic_min(p, val)
#define gputilAtomicMax(p, val) atomic_max(p, val)
#define gputilAtomicAnd(p, val) atomic_and(p, val)
#define gputilAtomicOr(p, val)  atomic_or(p, val)
#define gputilAtomicXor(p, val) atomic_xor(p, val)

#else  // __OPENCL_C_VERSION__ < 200
typedef volatile int atomic_int;
typedef volatile uint atomic_uint;
typedef volatile float atomic_float;
typedef volatile ulong atomic_ulong;

#define gputilAtomicInitI32(obj, val)               *(obj) = val
#define gputilAtomicInitI32L(obj, val)              *(obj) = val
#define gputilAtomicStoreI32(obj, val)              *(obj) = val
#define gputilAtomicStoreI32L(obj, val)             *(obj) = val
#define gputilAtomicLoadI32(obj)                    *(obj)
#define gputilAtomicLoadI32L(obj)                   *(obj)
#define gputilAtomicExchangeI32(obj, desired)       atomic_xchg(obj, desired)
#define gputilAtomicExchangeI32L(obj, desired)      atomic_xchg(obj, desired)
#define gputilAtomicCasI32(obj, expected, desired)  (atomic_cmpxchg(obj, expected, desired) == (expected))
#define gputilAtomicCasI32L(obj, expected, desired) (atomic_cmpxchg(obj, expected, desired) == (expected))

#define gputilAtomicInitU32(obj, val)               *(obj) = val
#define gputilAtomicInitU32L(obj, val)              *(obj) = val
#define gputilAtomicStoreU32(obj, val)              *(obj) = val
#define gputilAtomicStoreU32L(obj, val)             *(obj) = val
#define gputilAtomicLoadU32(obj)                    *(obj)
#define gputilAtomicLoadU32L(obj)                   *(obj)
#define gputilAtomicExchangeU32(obj, desired)       atomic_xchg(obj, desired)
#define gputilAtomicExchangeU32L(obj, desired)      atomic_xchg(obj, desired)
#define gputilAtomicCasU32(obj, expected, desired)  (atomic_cmpxchg(obj, expected, desired) == (expected))
#define gputilAtomicCasU32L(obj, expected, desired) (atomic_cmpxchg(obj, expected, desired) == (expected))

#if GPUTIL_ATOMICS_64
#error 64-bit atomics not supported before OpenCL 2.0
#endif  // GPUTIL_ATOMICS_64

#define gputilAtomicInitF32(obj, val)   *(obj) = val
#define gputilAtomicInitF32L(obj, val)  *(obj) = val
#define gputilAtomicStoreF32(obj, val)  *(obj) = val
#define gputilAtomicStoreF32L(obj, val) *(obj) = val
#define gputilAtomicLoadF32(obj)        *(obj)
#define gputilAtomicLoadF32L(obj)       *(obj)
inline float gputilAtomicExchangeF32(__global atomic_float *obj, float desired)
{
  int r = atomic_xchg((__global atomic_int *)obj, *(int *)&desired);
  return *(float *)&r;
}
inline float gputilAtomicExchangeF32L(__local atomic_float *obj, float desired)
{
  int r = atomic_xchg((__local atomic_int *)obj, *(int *)&desired);
  return *(float *)&r;
}

#define gputilAtomicCasF32(obj, expected, desired) \
  (atomic_cmpxchg((__global atomic_int *)obj, *(int *)&expected, *(int *)&desired) == *(int *)&expected)
#define gputilAtomicCasF32L(obj, expected, desired) \
  (atomic_cmpxchg((__local atomic_int *)obj, *(int *)&expected, *(int *)&desired) == *(int *)&expected)


#define gputilAtomicAdd(p, val) atomic_add(p, val)
#define gputilAtomicSub(p, val) atomic_sub(p, val)
#define gputilAtomicInc(p)      atomic_inc(p)
#define gputilAtomicDec(p)      atomic_dec(p)
#define gputilAtomicMin(p, val) atomic_min(p, val)
#define gputilAtomicMax(p, val) atomic_max(p, val)
#define gputilAtomicAnd(p, val) atomic_and(p, val)
#define gputilAtomicOr(p, val)  atomic_or(p, val)
#define gputilAtomicXor(p, val) atomic_xor(p, val)

#endif  // __OPENCL_C_VERSION__

#endif  // GPUTIL_DEVICE == GPUTIL_OPENCL

//----------------------------------------------------------------------------------------------------------------------
// Utility/helper functions.
//----------------------------------------------------------------------------------------------------------------------
inline __device__ bool isGlobalThread(size_t x, size_t y, size_t z)
{
  return x == get_global_id(0) && y == get_global_id(1) && z == get_global_id(2);
}

inline __device__ bool isLocalThread(size_t x, size_t y, size_t z)
{
  return x == get_local_id(0) && y == get_local_id(1) && z == get_local_id(2);
}

inline __device__ bool isInGroup(size_t x, size_t y, size_t z)
{
  return x == get_group_id(0) && y == get_group_id(1) && z == get_group_id(2);
}

#endif  // GPU_EXT_H_


================================================
FILE: logutil/CMakeLists.txt
================================================

include(GenerateExportHeader)
include(TextFileResource)

configure_file(LogUtilConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/logutil/LogUtilConfig.h")

set(SOURCES
  Logger.cpp
  Logger.h
  LoggerDetail.h
  LogUtil.cpp
  LogUtil.h
)

set(PUBLIC_HEADERS
  "${CMAKE_CURRENT_BINARY_DIR}/logutil/LogUtilConfig.h"
  "${CMAKE_CURRENT_BINARY_DIR}/logutil/LogUtilExport.h"
  Logger.h
  LoggerDetail.h
  LogUtil.h
)

add_library(logutil ${SOURCES})
clang_tidy_target(logutil)

target_include_directories(logutil
  PUBLIC
    $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/logutil>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
  PRIVATE
    $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}>
)

generate_export_header(logutil
  EXPORT_MACRO_NAME logutil_API
  EXPORT_FILE_NAME logutil/LogUtilExport.h
  STATIC_DEFINE LOGUTIL_STATIC)

install(TARGETS logutil EXPORT ${CMAKE_PROJECT_NAME}-config-targets
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/logutil
)

install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/logutil)

source_group("source" REGULAR_EXPRESSION ".*$")


================================================
FILE: logutil/LogUtil.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "LogUtil.h"

#include <algorithm>
#include <cctype>
#include <cmath>

namespace logutil
{
const std::array<size_t, 9> Bytes::ByteScale =  //
  {
    1ull,
    1024ull,
    1024ull * 1024ull,
    1024ull * 1024ull * 1024ull,
    1024ull * 1024ull * 1024ull * 1024ull,
    1024ull * 1024ull * 1024ull * 1024ull * 1024ull,
    1024ull * 1024ull * 1024ull * 1024ull * 1024ull * 1024ull,
    1024ull * 1024ull * 1024ull * 1024ull * 1024ull * 1024ull * 1024ull,
    1024ull * 1024ull * 1024ull * 1024ull * 1024ull * 1024ull * 1024ull * 1024ull,
  };

const std::array<const char *const, 9> Bytes::MagnitudeSuffix =  //
  {
    "B",    //
    "KiB",  //
    "MiB",  //
    "GiB",  //
    "TiB",  //
    "PiB",  //
    "EiB",  //
    "ZiB",  //
    "YiB",  //
  };

bool parseBytes(std::istream &in, Bytes &bytes, bool read_suffix)
{
  double numeric_value = 0;
  std::string suffix;

  // Read values.
  in >> numeric_value;

  // Validate it's a +ve value.
  if (numeric_value < 0)
  {
    in.setstate(std::ios::failbit);
    bytes = Bytes(0);
    return false;
  }

  if (in.good() && read_suffix)
  {
    // Need suffix.
    in >> suffix;
  }

  // Process result.
  double integer_part = 0;
  double fraction_part = std::modf(numeric_value, &integer_part);
  ByteMagnitude byte_magnitude = ByteMagnitude::kByte;
  if (suffix.empty())
  {
    // No suffix. Assume bytes. A floating point value is invalid for this.
    if (fraction_part != 0.0)
    {
      // Has a factional part. Fail.
      in.setstate(std::ios::failbit);
      bytes = Bytes(0);
      return false;
    }

    byte_magnitude = ByteMagnitude::kByte;
  }
  else
  {
    std::transform(suffix.begin(), suffix.end(), suffix.begin(), ::tolower);
    // Try match the suffix.
    bool parsed_suffix = false;
    for (size_t i = 0; i < Bytes::MagnitudeSuffix.size(); ++i)
    {
      std::string sfx = Bytes::MagnitudeSuffix[i];
      std::transform(sfx.begin(), sfx.end(), sfx.begin(), ::tolower);
      if (suffix == sfx)
      {
        byte_magnitude = ByteMagnitude(i);
        parsed_suffix = true;
        break;
      }
    }

    if (!parsed_suffix)
    {
      // Unparsed suffix.
      in.setstate(std::ios::failbit);
      bytes = Bytes(0);
      return false;
    }
  }

  // Slide the magnitude if we have a fractional part.
  while (fraction_part > 0 && byte_magnitude != ByteMagnitude::kByte &&
         unsigned(byte_magnitude) < Bytes::MagnitudeSuffix.size())
  {
    byte_magnitude = ByteMagnitude(int(byte_magnitude) + 1);
    fraction_part = std::modf(numeric_value * 1024.0, &integer_part);
  }

  // Finalise.
  bytes = Bytes(uint64_t(integer_part), byte_magnitude);
  return true;
}
}  // namespace logutil


================================================
FILE: logutil/LogUtil.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef LOGUTIL_LOGUTIL_H
#define LOGUTIL_LOGUTIL_H

#include "LogUtilConfig.h"

#include <array>
#include <chrono>
#include <cstdint>
#include <iomanip>
#include <sstream>
#include <string>

namespace logutil
{
///
const size_t kKibiSize = 1024u;

/// Defines the set of byte units starting from bytes, kibibytes, mibibytes, etc.
enum class ByteMagnitude : int
{
  kByte,
  kKiB,
  kMiB,
  kGiB,
  kTiB,
  kPiB,
  kEiB,
  kZiB,
  kYiB
};

/// A helper structure for displaying byte values. Can be used as an io stream manipulator to write raw byte values into
/// a text stream for display.
class logutil_API Bytes
{
public:
  /// Byte scale conversion values from any @c ByteMagnitude to @c ByteMagnitude::kByte.
  static const std::array<size_t, 9> ByteScale;

  /// Display string suffixes for @c ByteMagnitude values.
  static const std::array<const char *const, 9> MagnitudeSuffix;

  /// Construct a byte value optionally specifying the byte magnitude.
  /// @param byte_value The byte value to store.
  /// @param magnitude The magnitude of the byte value.
  explicit inline Bytes(size_t byte_value, ByteMagnitude magnitude = ByteMagnitude::kByte)
    : value_(byte_value)
    , magnitude_(magnitude)
  {}

  /// Default constructor. Zero byte size.
  inline Bytes()
    : Bytes(0u)
  {}

  /// Copy constructor.
  /// @param other Object to copy
  inline Bytes(const Bytes &other) = default;
  /// Assignment operator.
  /// @param other Object to copy
  /// @return @c *this
  inline Bytes &operator=(const Bytes &other) = default;

  /// Static conversion from any @c ByteMagnitude into @c ByteMagnitude::kByte.
  /// @param value The byte value
  /// @param magnitude The magnitude of @c value
  /// @return The @c value expressed in bytes.
  inline static size_t byteSize(size_t value, ByteMagnitude magnitude) { return value * ByteScale[int(magnitude)]; }

  /// Convert this value into bytes regardless of stored @c ByteMagnitude.
  /// @return The @c value() expressed in bytes.
  inline size_t byteSize() const { return byteSize(value_, magnitude_); }

  /// Query the current @c value().
  /// @return The current byte value.
  inline size_t value() const { return value_; }
  /// Set a new @c value. The @c magnitude() is unaffected.
  /// @param value The new value.
  inline void setValue(size_t value) { value_ = value; }
  /// Query the current stored magnitude.
  /// @return The stored @c ByteMagnitude.
  inline ByteMagnitude magnitude() const { return magnitude_; }
  /// Set the @c magnitude. @c value() is unaffected.
  /// @param magnitude The new @c ByteMagnitude to set.
  inline void setMagnitude(ByteMagnitude magnitude) { magnitude_ = magnitude; }

  /// Convert this byte value to a new magnitude. Precision may be lost.
  /// @param magnitude The new magnitude to convert to.
  /// @return A @c Byte object at the desired @c ByteMagnitude expressing this object's byte value (approx).
  inline Bytes convertTo(ByteMagnitude magnitude) const
  {
    const size_t raw_byte_size = byteSize();
    const size_t inverse_scale = ByteScale[int(magnitude)];
    return Bytes(raw_byte_size / inverse_scale, magnitude);
  }

  /// Convert the byte value to the most compact string representation expressed to three decimal places and appropriate
  /// @c MagnitudeSuffix.
  /// @return A string representation of this object.
  std::string toString() const;

private:
  size_t value_ = 0;
  ByteMagnitude magnitude_ = ByteMagnitude::kByte;
};

/// Log a @c std::chrono::clock::duration to an output stream.
///
/// The resulting string displays in the smallest possible unit to show three three
/// decimal places with display units ranging from seconds to nanoseconds. The table below
/// shows some example times.
///
/// Time(s)     | Display
/// ----------- | --------
/// 0.000000018 | 18ns
/// 0.000029318 | 29.318us
/// 0.0295939   | 29.593ms
/// 0.93        | 930ms
/// 15.023      | 15.023s
/// 15.000025   | 15.000s
///
/// Note that times are truncated, not rounded.
///
/// @tparam D The duration type of the form @c std::chrono::clock::duration.
/// @param out The output stream to log to.
/// @param duration The duration to convert to string.
template <typename D>
inline void logDuration(std::ostream &out, const D &duration)
{
  const size_t kThousand = 1000u;
  const bool negative = std::chrono::duration_cast<std::chrono::nanoseconds>(duration).count() < 0;
  const char *sign = (!negative) ? "" : "-";
  D abs_duration = (!negative) ? duration : duration * -1;
  auto s = std::chrono::duration_cast<std::chrono::seconds>(abs_duration).count();
  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(abs_duration).count();
  ms = ms % kThousand;

  if (s)
  {
    out << sign << s << "." << std::setw(3) << std::setfill('0') << ms << "s";
  }
  else
  {
    auto us = std::chrono::duration_cast<std::chrono::microseconds>(abs_duration).count();
    us = us % kThousand;

    if (ms)
    {
      out << sign << ms << "." << std::setw(3) << std::setfill('0') << us << "ms";
    }
    else
    {
      auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(abs_duration).count();
      ns = ns % kThousand;

      if (us)
      {
        out << sign << us << "." << std::setw(3) << std::setfill('0') << ns << "us";
      }
      else
      {
        out << sign << ns << "ns";
      }
    }
  }
}

/// Convert a @c std::chrono::clock::duration type to a display string using the same
/// logic as @c logTime().
/// @tparam D The duration type of the form @c std::chrono::clock::duration.
/// @param time_str The string to populate with the result.
/// @param duration The duration to convert to string.
template <typename D>
inline void timeString(std::string &time_str, const D &duration)
{
  std::ostringstream out;
  logDuration(out, duration);
  out.flush();
  time_str = out.str();
}

/// Parse a @c Byte value from @p in.
///
/// The expected format must be a number followed by a valid @c Bytes::MagnitudeSuffix : `<number>[B,KiB,GiB,...]`
///
/// The number specifies the byte value and the suffix identifies the magnitude.
///
/// The number must be zero or positive. It may also be a floating point value in which case it is converted into a the
/// magnitude below. Such a non-integer value is not valid for the 'B' (bytes) suffix.
///
/// The suffix is case insensitive.
///
/// The suffix may be omitted when @p read_suffix is false, in which case the `<number>` must be an integer byte value.
///
/// On failure, the stream fail bit is set.
///
/// @param in The stream to read from.
/// @param bytes The byte structure to parse into.
/// @return True on success.
bool logutil_API parseBytes(std::istream &in, Bytes &bytes, bool read_suffix = true);

/// Convert a byte value to a memory usage display string converting to the largest appropriate byte unit.
/// For example, this displays 2048 bytes as 2KiB, rather than a byte value. String display supports up to exbibyte.
///
/// Note: unlike hard-drive manufacturers, this function uses base 1024 units not base 1000.
/// @param[out] str The result is written here.
/// @param bytes The byte value to convert.
inline std::ostream &operator<<(std::ostream &out, const Bytes &bytes)
{
  unsigned magnitude_index = 0;
  uint64_t prev_bytes = 0;
  std::array<char, 3> decimal_places = { 0, 0, 0 };
  bool need_fractional = false;
  size_t bytes_value = bytes.byteSize();
  while (bytes_value >= kKibiSize && magnitude_index < Bytes::MagnitudeSuffix.size())
  {
    prev_bytes = bytes_value;
    bytes_value /= kKibiSize;
    ++magnitude_index;
  }

  if (magnitude_index)
  {
    // Use prevBytes for fractional display, but only to 3 decimal places.
    prev_bytes = prev_bytes % kKibiSize;
    // Convert to fixed point thousands.
    prev_bytes *= kKibiSize;
    prev_bytes /= kKibiSize;
    for (int i = 0; i < 3 && prev_bytes; ++i)
    {
      need_fractional = true;
      decimal_places[2 - i] = char(prev_bytes % 10);             // NOLINT(readability-magic-numbers)
      prev_bytes = (prev_bytes) ? prev_bytes / 10 : prev_bytes;  // NOLINT(readability-magic-numbers)
    }
  }

  out << bytes_value;
  if (need_fractional)
  {
    out << '.';
    for (int i = 0; i < 3 && decimal_places[i] > 0; ++i)
    {
      out << int(decimal_places[i]);
    }
  }
  out << Bytes::MagnitudeSuffix[magnitude_index];
  return out;
}

inline std::istream &operator>>(std::istream &in, Bytes &bytes)
{
  parseBytes(in, bytes);
  return in;
}

inline std::string Bytes::toString() const
{
  std::ostringstream s;
  s << *this;
  return s.str();
}
}  // namespace logutil

namespace std
{
namespace chrono
{
template <typename T, typename R>
inline ostream &operator<<(ostream &out, const duration<T, R> &d)
{
  logutil::logDuration(out, d);
  return out;
}
}  // namespace chrono
}  // namespace std

#endif  // LOGUTIL_LOGUTIL_H


================================================
FILE: logutil/LogUtilConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef LOGUTILCONFIG_H
#define LOGUTILCONFIG_H

#include "LogUtilExport.h"

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

#endif  // LOGUTILCONFIG_H


================================================
FILE: logutil/Logger.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "Logger.h"

#include <iostream>

namespace
{
logutil::LogOStream g_default_logger;
logutil::LogInterface *g_current_logger = &g_default_logger;
}  // namespace

namespace logutil
{
LogInterface::LogInterface(LogLevel level) noexcept
  : level_(level)
{}


LogInterface::~LogInterface() = default;


LogOStream::LogOStream(LogLevel level) noexcept
  : LogInterface(level)
{}


void LogOStream::message(LogLevel level, const char *msg)
{
  if (int(level) <= int(this->level()))
  {
    switch (level)
    {
    default:
    case LogLevel::kTrace:
      std::clog << msg << std::flush;
      break;
    case LogLevel::kInfo:
      std::cout << msg << std::flush;
      break;
    case LogLevel::kWarn:
      std::cout << msg << std::flush;
      break;
    case LogLevel::kError:
    case LogLevel::kFatal:
      std::cerr << msg << std::flush;
      break;
    }
  }
}


LogInterface *logger()
{
  return g_current_logger;
}


LogInterface *setLogger(LogInterface *logger)
{
  auto *previous = g_current_logger;
  g_current_logger = logger;
  return previous;
}


LogInterface *defaultLogger()
{
  return &g_default_logger;
}
}  // namespace logutil


================================================
FILE: logutil/Logger.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef LOGUTIL_LOGGER_H
#define LOGUTIL_LOGGER_H

#include "LogUtilConfig.h"

#include "LoggerDetail.h"

#include <stdexcept>

namespace logutil
{
/// Logging levels
enum class LogLevel : int
{
  /// Fatal error - an exception will be thrown.
  kFatal,
  /// Error message - will attempt to cleanup or continue, but may not succeed.
  kError,
  /// Warning
  kWarn,
  /// Information message
  kInfo,
  /// Debug/trace message.
  kTrace
};

/// Abstract logging interface.
class logutil_API LogInterface
{
public:
  /// Default constructor.
  LogInterface(LogLevel level = LogLevel::kInfo) noexcept;
  /// Virtual destructor.
  virtual ~LogInterface();

  /// Log a message at the given level. Newline characters are not automatically added.
  ///
  /// @note Logging helper functions are allowed to check the log level before calling this function and skip the
  /// call when the current log level is below the @p level argument.
  ///
  /// @param level The severity of the message.
  /// @param msg The message to log. Must include newline characters as desired by the caller.
  virtual void message(LogLevel level, const char *msg) = 0;

  /// Query the current logging level.
  /// @return The current logging level.
  inline LogLevel level() const { return level_; }
  /// Set the target logging level.
  /// @param level The logging  level.
  inline void setLevel(LogLevel level) { level_ = level; }

private:
  LogLevel level_ = LogLevel::kInfo;  ///< Curent logging level.
};

/// Default logging interface, using  @c std::clog , @c std::cout  and @c std::cerr .
///
/// The output stream is selected based on a message @c LogLevel
///
/// | @c LogLevel         | Stream       |
/// | ------------------- | ------------ |
/// | @c LogLevel::kTrace | @c std::clog |
/// | @c LogLevel::kInfo  | @c std::cout |
/// | @c LogLevel::kWarn  | @c std::cout |
/// | @c LogLevel::kError | @c std::cerr |
/// | @c LogLevel::kFatal | @c std::cerr |
class logutil_API LogOStream : public LogInterface
{
public:
  LogOStream(LogLevel level = LogLevel::kInfo) noexcept;

  /// Log a message at the given level. Output stream will be flushed.
  ///
  /// @param level The severity of the message.
  /// @param msg The message to log. Must include newline characters as desired by the caller.
  void message(LogLevel level, const char *msg) override;
};

/// Get the current, global logging output object.
///
/// A @c LogOStream object is installed by default.
///
/// @return The current @c LogInterface .
LogInterface logutil_API *logger();

/// Set the global logging object. Ownership is retained by the caller and a null logger should be set before the
/// @p logger object expires.
///
/// Not threadsafe.
///
/// @param logger A pointer to the logger.
/// @return The previously set logger interface.
LogInterface logutil_API *setLogger(LogInterface *logger);

/// Get the default logger object installed on startup. May be used to restore the @c logger() after a custom logger
/// has been used.
/// @return The default logging object.
LogInterface logutil_API *defaultLogger();

/// A helper for assembling a logger message at the given severity @p level .
///
/// The @p args are converted into a string using @c std::ostringstream however this may change as better string
/// formatting becomes available in C++20.
///
/// @note Argument conversions are affected by some stream modifiers such as:
/// - @c std::boolalpha
///
/// @note No work is done if the @c logger() is null or the @c logger() level is below the selected message severity.
///
/// @param level The message severity.
/// @param args Arguments to convert to string and concatenate into a log message.
template <typename... Args>
inline void message(LogInterface *log_interface, LogLevel level, Args... args)
{
  if (log_interface)
  {
    if (int(log_interface->level()) >= int(level))
    {
      std::ostringstream out;
      logger_detail::prepareStream(out);
      logger_detail::message(out, args...);
      const std::string msg = out.str();
      log_interface->message(level, msg.c_str());
    }
  }
}


/// Log a @c LogLevel::kTrace message using the @c message() function.
/// @param log_iterface The object to logger to.
/// @param args Arguments to convert to string and concatenate into a logger message.
template <typename... Args>
inline void trace(LogInterface *log_interface, Args... args)
{
  message(log_interface, LogLevel::kTrace, args...);
}


/// Log a @c LogLevel::kTrace message using the @c message() function and default @c logger() .
/// @param args Arguments to convert to string and concatenate into a logger message.
template <typename... Args>
inline void trace(Args... args)
{
  message(logger(), LogLevel::kTrace, args...);
}


/// Log a @c LogLevel::kInfo message using the @c message() function.
/// @param log_iterface The object to logger to.
/// @param args Arguments to convert to string and concatenate into a logger message.
template <typename... Args>
inline void info(LogInterface *log_interface, Args... args)
{
  message(log_interface, LogLevel::kInfo, args...);
}


/// Log a @c LogLevel::kInfo message using the @c logger() function and default @c logger() .
/// @param args Arguments to convert to string and concatenate into a logger message.
template <typename... Args>
inline void info(Args... args)
{
  message(logger(), LogLevel::kInfo, args...);
}


/// Log a @c LogLevel::kWarn message using the @c message() function.
/// @param log_iterface The object to logger to.
/// @param args Arguments to convert to string and concatenate into a logger message.
template <typename... Args>
inline void warn(LogInterface *log_interface, Args... args)
{
  message(log_interface, LogLevel::kWarn, args...);
}


/// Log a @c LogLevel::kWarn message using the @c message() function and default @c logger() .
/// @param args Arguments to convert to string and concatenate into a logger message.
template <typename... Args>
inline void warn(Args... args)
{
  message(logger(), LogLevel::kWarn, args...);
}


/// Log a @c LogLevel::kError message using the @c message() function.
/// @param log_iterface The object to logger to.
/// @param args Arguments to convert to string and concatenate into a logger message.
template <typename... Args>
inline void error(LogInterface *log_interface, Args... args)
{
  message(log_interface, LogLevel::kError, args...);
}


/// Log a @c LogLevel::kError message using the @c message() function and default @c logger() .
/// @param args Arguments to convert to string and concatenate into a logger message.
template <typename... Args>
inline void error(Args... args)
{
  message(logger(), LogLevel::kError, args...);
}


/// Log a @c LogLevel::kFatal message using the @c message() function.
///
/// This functions throws a @c std::runtime_error with the assembled message.
///
/// @param log_iterface The object to logger to.
/// @param args Arguments to convert to string and concatenate into a logger message.
template <typename... Args>
inline void fatal(LogInterface *log_interface, Args... args)
{
  std::ostringstream out;
  logger_detail::prepareStream(out);
  logger_detail::message(out, args...);
  const std::string msg = out.str();
  if (log_interface)
  {
    log_interface->message(LogLevel::kFatal, msg.c_str());
  }
  throw std::runtime_error(msg);
}

/// Log a @c LogLevel::kFatal message using the @c message() function and default @c logger() .
///
/// This functions throws a @c std::runtime_error with the assembled message.
///
/// @param args Arguments to convert to string and concatenate into a logger message.
template <typename... Args>
inline void fatal(Args... args)
{
  fatal(logger(), args...);
}
}  // namespace logutil

#endif  // LOGUTIL_LOGGER_H


================================================
FILE: logutil/LoggerDetail.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef LOGUTIL_LOGGERDETAIL_H
#define LOGUTIL_LOGGERDETAIL_H

#include "LogUtilConfig.h"

#include <iomanip>
#include <locale>
#include <sstream>
#include <string>

namespace logutil
{
namespace logger_detail
{
/// Prepare the @p out stream for logging, imbuing the locale.
///
/// Other effects:
/// - Set @c std::boolalpha
///
/// @param out The stream to prepare.
inline void prepareStream(std::ostream &out)
{
  out.imbue(std::locale(""));
  out << std::boolalpha;
}

template <typename T, typename... Args>
inline void message(std::ostream &stream, const T &value)
{
  stream << value;
}

template <typename T, typename... Args>
inline void message(std::ostream &stream, const T &value, Args... args)
{
  stream << value;
  message(stream, args...);
}
}  // namespace logger_detail
}  // namespace logutil

#endif  // LOGUTIL_LOGGERDETAIL_H


================================================
FILE: ohm/Aabb.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_AABB_H
#define OHM_AABB_H

#include "OhmConfig.h"

#include <glm/geometric.hpp>
#include <glm/vec2.hpp>
#include <glm/vec3.hpp>

#include <algorithm>
#include <array>
#include <ostream>
#include <sstream>

namespace ohm
{
/// An axis aligned box
class ohm_API Aabb
{
public:
  /// Empty constructor: uninitialised.
  Aabb();

  /// Create an initialised AABB using the given @p seed scalar.
  ///
  /// The resulting box has all channels set to @p seed. This box will not pass @c isValid(), but is initialised.
  /// Typically, use this constructor to ensure an initialised, but unpopulated box calling @c Aabb(0.0).
  ///
  /// @param seed The seed value for all channels.
  explicit Aabb(double seed);

  /// Copy constructor.
  /// @param other Value to copy.
  Aabb(const Aabb &other);
  /// Initialise with the given min/max extents.
  /// @param min_ext Minimum extents. Must be <= @p max_ext.
  /// @param max_ext Maximum extents.
  Aabb(const glm::dvec3 &min_ext, const glm::dvec3 &max_ext);

  inline ~Aabb() = default;

  /// Initialise from @p centre and @p half_extents.
  ///
  /// Box is calculated as:
  /// @code
  ///   min_ext = centre - half_extents;
  ///   max_ext = centre + half_extents;
  /// @endcode
  ///
  /// @param centre The box centre.
  /// @param half_extents The half extents for the box. All elements must be >= 0.
  static Aabb fromCentreHalfExtents(const glm::dvec3 &centre, const glm::dvec3 &half_extents);

  /// Initialise from @p centre and @p full_extents.
  ///
  /// Box is calculated as:
  /// @code
  ///   min_ext = centre - 0.5 * full_extents;
  ///   max_ext = centre + 0.5 * full_extents;
  /// @endcode
  ///
  /// @param centre The box centre.
  /// @param full_extents The extents (diagonal) for the box. All elements must be >= 0.
  static Aabb fromCentreFullExtents(const glm::dvec3 &centre, const glm::dvec3 &full_extents);

  /// Set the box lower extents corner.
  /// @param ext New coordinate for the lower box extents. No validation (for being <= @c maxExtents).
  inline void setMinExtents(const glm::dvec3 &ext) { corners_[0] = ext; }

  /// Set the box upper extents corner.
  /// @param ext New coordinate for the upper box extents. No validation (for being >= @c minExtents).
  inline void setMaxExtents(const glm::dvec3 &ext) { corners_[1] = ext; }

  /// Query the lower box extents.
  /// @return The lower bounds of the box.
  inline glm::dvec3 minExtents() const { return corners_[0]; }

  /// Access a mutable reference to the minimum extents. Incorrect usage may result in undefined behaviour.
  /// @return A reference to the lower bounds.
  inline glm::dvec3 &minExtentsMutable() { return corners_[0]; }

  /// Query the upper box extents.
  /// @return The upper bounds of the box.
  inline glm::dvec3 maxExtents() const { return corners_[1]; }

  /// Access a mutable reference to the maximum extents. Incorrect usage may result in undefined behaviour.
  /// @return A reference to the upper bounds.
  inline glm::dvec3 &maxExtentsMutable() { return corners_[1]; }

  /// Query the box centre.
  /// @return The box centre.
  inline glm::dvec3 centre() const
  {
    return 0.5 * (corners_[0] + corners_[1]);  // NOLINT(readability-magic-numbers)
  }

  /// Query the box half extents.
  /// @return The box half extents (half the diagonal).
  inline glm::dvec3 halfExtents() const
  {
    return 0.5 * (corners_[1] - corners_[0]);  // NOLINT(readability-magic-numbers)
  }

  /// Query the full box extents.
  /// @return The full extents (the diagonal).
  inline glm::dvec3 diagonal() const { return corners_[1] - corners_[0]; }

  /// Corner indexing enum. See @c corner()
  /// Corners are defined by a three letter acronym, each letter referencing a axis in XYZ order.
  /// Each letter may be either L/l for lower bound (minimum extent) or U/u for upper bound (maximum extent).
  enum CornerIndex : int
  {
    kCornerLll,
    kCornerUll,
    kCornerLul,
    kCornerUul,
    kCornerLlu,
    kCornerUlu,
    kCornerLuu,
    kCornerUuu,
    kCornerCount
  };

  /// Return a corner point of the box.
  ///
  /// The corner indexing is defined by @c CornerIndex.
  /// @param corner_index The index of the corner of interest [0, 8).
  /// @return The coordinates of the requested corner.
  glm::dvec3 corner(int corner_index) const;

  /// Expand the extents to include @p pt. Does not handle the first point of a point set.
  /// @param pt The point to expand the extents to include.
  void expand(const glm::dvec3 &pt);

  /// Test whether this box and @p other overlap. Touching is considered an overlap.
  ///
  /// The box is optionally extended by @p epsilon.
  ///
  /// @param other The box to test.
  /// @param epsilon Error tolerance value.
  /// @return True if the boxes overlap.
  bool overlaps(const Aabb &other, double epsilon = 0) const;

  /// Test whether this box contains the point @p point. Touching is containing.
  ///
  /// The box is optionally extended by @p epsilon.
  ///
  /// @param point The point to test.
  /// @param epsilon Error tolerance value.
  /// @return True @p point is in this box.
  bool contains(const glm::dvec3 &point, double epsilon = 0) const;

  /// Test for equality between this and @p other with tolerance @p epsilon.
  /// @param other The box to test against.
  /// @param epsilon Error tolerance value.
  /// @return True if this and @p other are within @p epsilon of equality (per axis error).
  bool isEqual(const Aabb &other, double epsilon) const;

  /// Is this a valid AABB where the minimum corner is less than the maximum.
  /// @return True if this is a valid, non-zero sized box.
  bool isValid() const;

  /// Flags for clipping results.
  enum ClipResult : unsigned
  {
    kClippedStart = (1u << 0u),
    kClippedEnd = (1u << 1u)
  };

  /// Test for ray/AABB intersection.
  /// @param ray_origin The ray start point.
  /// @param ray_dir The ray direction.
  /// @param ray_dir_inv Must be a per channel inversion of ray_dir : `(1.0/ray_dir.x, 1.0/ray_dir.y, 1.0/ray_dir.z)`.
  ///   Use the overload to have this function perform the calculation.
  /// @param[out] hit_times The hit times. Element 0 is the entry time and element 1 is the exit time. Either may be < 0
  ///   indicating we do not enter/exit.
  /// @return True when the ray segment intersects the box.
  bool rayIntersect(const glm::dvec3 &ray_origin, const glm::dvec3 &ray_dir, const glm::dvec3 &ray_dir_inv,
                    std::array<double, 2> *hit_times = nullptr) const;
  /// @overload
  bool rayIntersect(const glm::dvec3 &ray_origin, const glm::dvec3 &ray_dir,
                    std::array<double, 2> *hit_times = nullptr) const;

  /// Clip the line from @p start to @p end such that both points lie in or on the box.
  /// @param[in,out] start The line start point.
  /// @param[in,out] end The line end point.
  /// @param[out] clip_flags Clipping flags indicating how the line has been clipped. See : @c ClipResult.
  /// @param[in] allow_clamp Allow clamping any clipped point to the edges of the box? This ensures a clipped point
  ///   is exactly on the boundary of the box without the need for an epsilon. However, it may change the ray direction.
  /// @return True when the line segment intersects the box and has been clipped. Essentially, false when @p start
  ///     and @p end are unmodified.
  bool clipLine(glm::dvec3 &start, glm::dvec3 &end, unsigned *clip_flags = nullptr, bool allow_clamp = false) const;

  /// Test for precise quality between this and @p other.
  /// @param other The box to test against.
  /// @return True on exact equality.
  bool operator==(const Aabb &other) const;

  /// Test whether and @p other are not presicely equal.
  /// @param other The box to test against.
  /// @return True if the boxes are not exactly equality.
  bool operator!=(const Aabb &other) const;

  /// Assignment operator.
  /// @param other The value to assign.
  /// @return *this
  Aabb &operator=(const Aabb &other);

  /// Translate the box by @p offset.
  /// @param offset Translation to apply.
  /// @return *this
  Aabb &operator+=(const glm::dvec3 &offset);

  /// Translate the box by @p -offset.
  /// @param offset Inverse of the translation to apply.
  /// @return *this
  Aabb &operator-=(const glm::dvec3 &offset);

  /// Scale box by @p scalar.
  /// @param scalar Scaling factor.
  /// @return *this
  Aabb &operator*=(double scalar);

  /// Scale box by the inverse of @p scalar.
  /// @param scalar Inverse scaling factor. Must not be zero.
  /// @return *this
  Aabb &operator/=(double scalar);

private:
  static inline double sign(double val) { return val >= 0 ? 1.0f : -1.0f; }
  static double calcTimeVal(double limit, double origin, double direction);
  static bool calcIntervalOverlap(const glm::dvec2 &a, const glm::dvec2 &b, glm::dvec2 *overlap);

  // NOLINTNEXTLINE(cppcoreguidelines-avoid-c-arrays, modernize-avoid-c-arrays)
  glm::dvec3 corners_[2];  // = { glm::dvec3(0), glm::dvec3(0) };
};


inline Aabb::Aabb() = default;  // NOLINT(cppcoreguidelines-pro-type-member-init)


inline Aabb::Aabb(double seed)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  corners_[0] = corners_[1] = glm::dvec3(seed);
}


inline Aabb::Aabb(const Aabb &other)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  corners_[0] = other.corners_[0];
  corners_[1] = other.corners_[1];
}


inline Aabb::Aabb(const glm::dvec3 &min_ext, const glm::dvec3 &max_ext)  // NOLINT
{
  corners_[0] = min_ext;
  corners_[1] = max_ext;
}


inline Aabb Aabb::fromCentreHalfExtents(const glm::dvec3 &centre, const glm::dvec3 &half_extents)
{
  return Aabb(centre - half_extents, centre + half_extents);
}


inline Aabb Aabb::fromCentreFullExtents(const glm::dvec3 &centre, const glm::dvec3 &full_extents)
{
  // NOLINTNEXTLINE(readability-magic-numbers)
  return Aabb(centre - 0.5 * full_extents, centre + 0.5 * full_extents);
}


inline glm::dvec3 Aabb::corner(int corner_index) const
{
  glm::dvec3 c;
  c[0] = (corner_index & 1) == 0 ? corners_[0][0] : corners_[1][0];  // NOLINT(hicpp-signed-bitwise)
  c[1] = (corner_index & 2) == 0 ? corners_[0][1] : corners_[1][1];  // NOLINT(hicpp-signed-bitwise)
  c[2] = (corner_index & 4) == 0 ? corners_[0][2] : corners_[1][2];  // NOLINT(hicpp-signed-bitwise)
  return c;
}


inline void Aabb::expand(const glm::dvec3 &pt)
{
  corners_[0].x = std::min(corners_[0].x, pt.x);
  corners_[0].y = std::min(corners_[0].y, pt.y);
  corners_[0].z = std::min(corners_[0].z, pt.z);
  corners_[1].x = std::max(corners_[1].x, pt.x);
  corners_[1].y = std::max(corners_[1].y, pt.y);
  corners_[1].z = std::max(corners_[1].z, pt.z);
}


inline bool Aabb::overlaps(const Aabb &other, double epsilon) const
{
  const glm::bvec3 max_less_min = glm::lessThan(corners_[1] + glm::dvec3(epsilon), other.corners_[0]);
  const glm::bvec3 min_greater_max = glm::greaterThan(corners_[0] - glm::dvec3(epsilon), other.corners_[1]);
  return !glm::any(max_less_min) && !glm::any(min_greater_max);
}


inline bool Aabb::contains(const glm::dvec3 &point, double epsilon) const
{
  return overlaps(Aabb(point - glm::dvec3(epsilon), point + glm::dvec3(epsilon)));
}


inline bool Aabb::isEqual(const Aabb &other, double epsilon) const
{
  const glm::dvec3 min_diff = glm::abs(corners_[0] - other.corners_[0]);
  const glm::dvec3 max_diff = glm::abs(corners_[1] - other.corners_[1]);
  const glm::dvec3 epsilon_v(epsilon);
  return glm::all(glm::lessThanEqual(min_diff, epsilon_v)) && glm::all(glm::lessThanEqual(max_diff, epsilon_v));
}


inline bool Aabb::isValid() const
{
  return glm::all(glm::lessThan(corners_[0], corners_[1]));
}


inline bool Aabb::rayIntersect(const glm::dvec3 &ray_origin, const glm::dvec3 &ray_dir, const glm::dvec3 &ray_dir_inv,
                               std::array<double, 2> *hit_times_out) const
{
  // From
  // https://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-box-intersection
  double tmin;
  double tmax;
  std::array<int, 3> sign;
  std::array<double, 2> hit_times_internal;
  std::array<double, 2> &hit_times = (hit_times_out) ? *hit_times_out : hit_times_internal;

  sign[0] = ray_dir.x < 0.0;
  sign[1] = ray_dir.y < 0.0;
  sign[2] = ray_dir.z < 0.0;

  hit_times[0] = (corners_[sign[0]].x - ray_origin.x) * ray_dir_inv.x;
  hit_times[1] = (corners_[1 - sign[0]].x - ray_origin.x) * ray_dir_inv.x;
  tmin = (corners_[sign[1]].y - ray_origin.y) * ray_dir_inv.y;
  tmax = (corners_[1 - sign[1]].y - ray_origin.y) * ray_dir_inv.y;

  // Branchless:
  bool miss = !!(hit_times[0] > tmax) + !!(tmin > hit_times[1]);

  // Set the best entry time to the best value so far.
  // I tried a branchless version of this:
  //    !!(tmin > hit_times[0]) * tmin + !!(tmin <= hit_times[0]) * hit_times[0]
  // This failed when tmin was infinite (parallel line) so the result was NaN (0 * inf => NaN).
  // Hopefully the compiler can work out an appropriate branchless solution.
  hit_times[0] = (tmin > hit_times[0] || std::isnan(hit_times[0])) ? tmin : hit_times[0];
  // Set the best exit time to the best value so far.
  hit_times[1] = (tmax < hit_times[1] || std::isnan(hit_times[1])) ? tmax : hit_times[1];

  // Calculate Z axis results into tmin/tmax[1], now that the best values are in [0]
  tmin = (corners_[sign[2]].z - ray_origin.z) * ray_dir_inv.z;
  tmax = (corners_[1 - sign[2]].z - ray_origin.z) * ray_dir_inv.z;

  // Update miss check for the new axes.
  miss = !!(!!(hit_times[0] > tmax) + !!(tmin > hit_times[1]) + !!miss);

  // Set the best entry time to the best value.
  hit_times[0] = (tmin > hit_times[0] || std::isnan(hit_times[0])) ? tmin : hit_times[0];
  // Set the best exit time to the best value.
  hit_times[1] = (tmax < hit_times[1] || std::isnan(hit_times[1])) ? tmax : hit_times[1];

  return !miss;
}


inline bool Aabb::rayIntersect(const glm::dvec3 &ray_origin, const glm::dvec3 &ray_dir,
                               std::array<double, 2> *hit_times) const
{
  const glm::dvec3 ray_dir_inv(1.0 / ray_dir.x, 1.0 / ray_dir.y, 1.0 / ray_dir.z);
  return rayIntersect(ray_origin, ray_dir, ray_dir_inv, hit_times);
}


inline bool Aabb::clipLine(glm::dvec3 &start, glm::dvec3 &end, unsigned *clip_flags, bool allow_clamp) const
{
  const glm::dvec3 ray_origin = start;
  glm::dvec3 ray_dir = end - start;
  glm::dvec3 ray_dir_inv{};

  if (clip_flags)
  {
    *clip_flags = 0;
  }

  if (glm::dot(ray_dir, ray_dir) < 1e-9)
  {
    // Degenerate ray.
    return false;
  }
  const double length = glm::length(ray_dir);
  ray_dir /= length;

  std::array<double, 2> hit_times = { 0 };
  if (!rayIntersect(ray_origin, ray_dir, &hit_times))
  {
    return false;
  }

  // Ray intersects, but we have to validate the times. Increment the hit count only as we confirm the hits.
  int hit_count = 0;
  if (hit_times[0] > 0 && hit_times[0] < length)
  {
    start = ray_origin + ray_dir * hit_times[0];
    if (allow_clamp)
    {
      start[0] = std::max(corners_[0][0], std::min(start[0], corners_[1][0]));
      start[1] = std::max(corners_[0][1], std::min(start[1], corners_[1][1]));
      start[2] = std::max(corners_[0][2], std::min(start[2], corners_[1][2]));
    }
    if (clip_flags)
    {
      *clip_flags |= kClippedStart;
      // Clamp to the box to cater for floating point error.
    }
    ++hit_count;
  }

  if (hit_times[1] > 0 && hit_times[1] < length)
  {
    end = ray_origin + ray_dir * hit_times[1];
    if (allow_clamp)
    {
      end[0] = std::max(corners_[0][0], std::min(end[0], corners_[1][0]));
      end[1] = std::max(corners_[0][1], std::min(end[1], corners_[1][1]));
      end[2] = std::max(corners_[0][2], std::min(end[2], corners_[1][2]));
    }
    if (clip_flags)
    {
      *clip_flags |= kClippedEnd;
    }
    ++hit_count;
  }

  return hit_count > 0;
}


inline bool Aabb::operator==(const Aabb &other) const
{
  return glm::all(glm::equal(corners_[0], other.corners_[0])) && glm::all(glm::equal(corners_[1], other.corners_[1]));
}


inline bool Aabb::operator!=(const Aabb &other) const
{
  return !(*this == other);
}


// Lint(KS): self assignment is fine
inline Aabb &Aabb::operator=(const Aabb &other)  // NOLINT(bugprone-unhandled-self-assignment)
{
  corners_[0] = other.corners_[0];
  corners_[1] = other.corners_[1];
  return *this;
}


inline Aabb operator+(const Aabb &box, const glm::dvec3 &offset)
{
  return Aabb(box.minExtents() + offset, box.maxExtents() + offset);
}


inline Aabb operator+(const glm::dvec3 &offset, const Aabb &box)
{
  return box + offset;
}


inline Aabb operator-(const Aabb &box, const glm::dvec3 &offset)
{
  return Aabb(box.minExtents() - offset, box.maxExtents() - offset);
}


inline Aabb operator-(const glm::dvec3 &offset, const Aabb &box)
{
  return box - offset;
}


inline Aabb &Aabb::operator+=(const glm::dvec3 &offset)
{
  corners_[0] += offset;
  corners_[1] += offset;
  return *this;
}


inline Aabb &Aabb::operator-=(const glm::dvec3 &offset)
{
  corners_[0] -= offset;
  corners_[1] -= offset;
  return *this;
}


inline Aabb operator*(const Aabb &box, double scalar)
{
  return Aabb(box.minExtents() * scalar, box.maxExtents() * scalar);
}


inline Aabb operator*(double scalar, const Aabb &box)
{
  return box * scalar;
}


inline Aabb operator/(const Aabb &box, double scalar)
{
  return box * (1.0 / scalar);
}


inline Aabb &Aabb::operator*=(double scalar)
{
  corners_[0] *= scalar;
  corners_[1] *= scalar;
  return *this;
}


inline Aabb &Aabb::operator/=(double scalar)
{
  const double scalar_inv = 1.0 / scalar;
  corners_[0] *= scalar_inv;
  corners_[1] *= scalar_inv;
  return *this;
}


inline double Aabb::calcTimeVal(double limit, double origin, double direction)
{
  // Always performing the line on the return value nearly works, but occasionally
  // returns NaN instead of infinite results.
  if (direction == std::numeric_limits<double>::infinity())
  {
    return sign(limit - origin) * std::numeric_limits<double>::infinity();
  }
  if (direction == -std::numeric_limits<double>::infinity())
  {
    return sign(limit - origin) * -std::numeric_limits<double>::infinity();
  }
  return (limit - origin) * direction;
}


inline bool Aabb::calcIntervalOverlap(const glm::dvec2 &a, const glm::dvec2 &b, glm::dvec2 *overlap)
{
  if (a[0] > b[1])
  {
    return false;
  }
  if (b[0] > a[1])
  {
    return false;
  }

  (*overlap)[0] = std::max(a[0], b[0]);
  (*overlap)[1] = std::min(a[1], b[1]);
  return true;
}

inline std::istream &operator>>(std::istream &in, ohm::Aabb &box)
{
  std::string box_str;
  in >> box_str;
  std::istringstream in2(box_str);
  char comma;
  glm::dvec3 min_ext;
  glm::dvec3 max_ext;
  in2 >> min_ext.x;
  in2 >> comma;
  in2 >> min_ext.y;
  in2 >> comma;
  in2 >> min_ext.z;
  in2 >> comma;
  in2 >> max_ext.x;
  in2 >> comma;
  in2 >> max_ext.y;
  in2 >> comma;
  in2 >> max_ext.z;
  box = ohm::Aabb(min_ext, max_ext);
  return in;
}

inline std::ostream &operator<<(std::ostream &out, const ohm::Aabb &box)
{
  out << box.minExtents().x << ',' << box.minExtents().y << ',' << box.minExtents().z << ',';
  out << box.maxExtents().x << ',' << box.maxExtents().y << ',' << box.maxExtents().z;
  return out;
}
}  // namespace ohm

#endif  // OHM_AABB_H


================================================
FILE: ohm/CMakeLists.txt
================================================

include(GenerateExportHeader)
include(TextFileResource)


find_package(ZLIB)
if(OHM_FEATURE_THREADS)
  find_package(TBB)
endif(OHM_FEATURE_THREADS)

set(TES_ENABLE ${OHM_TES_DEBUG})
configure_file(OhmConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/ohm/OhmConfig.h")

set(SOURCES
  private/ClearingPatternDetail.h
  private/LineQueryDetail.h
  private/MapLayerDetail.h
  private/MapLayoutDetail.h
  private/NdtMapDetail.h
  private/NearestNeighboursDetail.h
  private/OccupancyMapDetail.cpp
  private/OccupancyMapDetail.h
  private/QueryDetail.h
  private/RaysQueryDetail.h
  private/SerialiseUtil.h
  private/VoxelAlgorithms.cpp
  private/VoxelAlgorithms.h
  private/VoxelBlockCompressionQueueDetail.h
  private/VoxelLayoutDetail.h
  serialise/MapSerialiseV0.1.cpp
  serialise/MapSerialiseV0.1.h
  serialise/MapSerialiseV0.2.cpp
  serialise/MapSerialiseV0.2.h
  # Note: there is no ongong support for 0.3.x. That added "sub-voxel" support, but in a way which mutated the occupancy
  # layer and did not generate good results. Version 0.4.0 introduced an explicit VoxelMean layer for much better
  # results and obviated the need for map header changes.
  serialise/MapSerialiseV0.4.cpp
  serialise/MapSerialiseV0.4.h
  serialise/MapSerialiseV0.cpp
  serialise/MapSerialiseV0.h
  Aabb.h
  CopyUtil.cpp
  CopyUtil.h
  CalculateSegmentKeys.cpp
  CalculateSegmentKeys.h
  ClearingPattern.cpp
  ClearingPattern.h
  CompareMaps.cpp
  CompareMaps.h
  CovarianceVoxel.cpp
  CovarianceVoxel.h
  CovarianceVoxelCompute.h
  DataType.cpp
  DataType.h
  DebugDraw.cpp
  DebugDraw.h
  Density.cpp
  Density.h
  DefaultLayer.cpp
  DefaultLayer.h
  Key.cpp
  Key.h
  KeyStream.h
  KeyHash.h
  KeyList.cpp
  KeyList.h
  KeyRange.cpp
  KeyRange.h
  LineKeysQuery.cpp
  LineKeysQuery.h
  LineQuery.cpp
  LineQuery.h
  LineWalk.h
  LineWalkCompute.h
  MapChunk.cpp
  MapChunk.h
  MapCoord.h
  MapFlag.cpp
  MapFlag.h
  MapInfo.cpp
  MapInfo.h
  MapLayer.cpp
  MapLayer.h
  MapLayout.cpp
  MapLayout.h
  Mapper.cpp
  Mapper.h
  MappingProcess.cpp
  MappingProcess.h
  MapProbability.h
  MapRegion.cpp
  MapRegion.h
  MapRegionCache.cpp
  MapRegionCache.h
  MapSerialise.cpp
  MapSerialise.h
  Mutex.cpp
  Mutex.h
  NdtMap.cpp
  NdtMap.h
  NdtMode.cpp
  NdtMode.h
  NearestNeighbours.cpp
  NearestNeighbours.h
  OccupancyMap.cpp
  OccupancyMap.h
  OccupancyType.cpp
  OccupancyType.h
  Query.cpp
  Query.h
  QueryFlag.h
  RayFilter.cpp
  RayFilter.h
  RayFlag.h
  RayMapper.cpp
  RayMapper.h
  RayMapperNdt.cpp
  RayMapperNdt.h
  RayMapperOccupancy.cpp
  RayMapperOccupancy.h
  RayMapperSecondarySample.cpp
  RayMapperSecondarySample.h
  RayMapperTrace.cpp
  RayMapperTrace.h
  RayMapperTsdf.cpp
  RayMapperTsdf.h
  RayPattern.cpp
  RayPattern.h
  RayPatternConical.cpp
  RayPatternConical.h
  RaysQuery.cpp
  RaysQuery.h
  Stream.cpp
  Stream.h
  Trace.cpp
  Trace.h
  Voxel.cpp
  Voxel.h
  VoxelBlock.cpp
  VoxelBlock.h
  VoxelBlockCompressionQueue.cpp
  VoxelBlockCompressionQueue.h
  VoxelBuffer.cpp
  VoxelBuffer.h
  VoxelData.h
  VoxelIncident.h
  VoxelIncidentCompute.h
  VoxelLayout.cpp
  VoxelLayout.h
  VoxelMean.h
  VoxelMeanCompute.h
  VoxelOccupancy.h
  VoxelOccupancyCompute.h
  VoxelSecondarySample.h
  VoxelTouchTime.h
  VoxelTouchTimeCompute.h
  VoxelTsdf.cpp
  VoxelTsdf.h
  VoxelTsdfCompute.h
)

if(TES_ENABLE)
  # Only required to define the 3es server symbol.
  list(APPEND SOURCES OccupancyUtil.cpp)
endif(TES_ENABLE)

set(PUBLIC_HEADERS
  Aabb.h
  CalculateSegmentKeys.h
  ClearingPattern.h
  CompareMaps.h
  CopyUtil.h
  CovarianceVoxel.h
  CovarianceVoxelCompute.h
  DataType.h
  Density.h
  DefaultLayer.h
  Key.h
  KeyStream.h
  KeyHash.h
  KeyList.h
  KeyRange.h
  LineKeysQuery.h
  LineQuery.h
  LineWalk.h
  LineWalkCompute.h
  MapChunkFlag.h
  MapChunk.h
  MapCoord.h
  MapFlag.h
  MapInfo.h
  MapLayer.h
  MapLayout.h
  MapLayoutMatch.h
  Mapper.h
  MappingProcess.h
  MapProbability.h
  MapRegionCache.h
  MapRegion.h
  MapSerialise.h
  Mutex.h
  NdtMap.h
  NdtMode.h
  NearestNeighbours.h
  OccupancyMap.h
  OccupancyType.h
  OccupancyUtil.h
  QueryFlag.h
  Query.h
  RayFilter.h
  RayFlag.h
  RayMapper.h
  RayMapperNdt.h
  RayMapperOccupancy.h
  RayMapperSecondarySample.h
  RayMapperTrace.h
  RayPatternConical.h
  RayPattern.h
  RaysQuery.h
  Stream.h
  Trace.h
  Voxel.h
  VoxelBlock.h
  VoxelBlockCompressionQueue.h
  VoxelBuffer.h
  VoxelData.h
  VoxelIncident.h
  VoxelLayout.h
  VoxelMean.h
  VoxelMeanCompute.h
  VoxelOccupancy.h
  VoxelOccupancyCompute.h
  VoxelSecondarySample.h
  VoxelTouchTime.h
  VoxelTouchTimeCompute.h
  VoxelTsdf.h
  VoxelTsdfCompute.h
  "${CMAKE_CURRENT_BINARY_DIR}/ohm/OhmConfig.h"
  "${CMAKE_CURRENT_BINARY_DIR}/ohm/OhmExport.h"
  )

add_library(ohm ${SOURCES})
clang_tidy_target(ohm)

target_link_libraries(ohm
  PUBLIC
    logutil
    ohmutil
    glm::glm
    $<$<BOOL:${OHM_FEATURE_THREADS}>:TBB::tbb>
  PRIVATE
    # Link Eigen as a private depedency. This will only apply additional include dirs as eigen is header only.
    # Usage is entirely private.
    $<BUILD_INTERFACE:$<$<BOOL:${OHM_FEATURE_EIGEN}>:Eigen3::Eigen>>
)
if(BUILD_SHARED)
  # Link additional dependencies when building shared ohm.
  # Because ohm is shared and these are private, static depenencies, we do not need to propagate
  # the dependencies and limit this with $<BUILD_INTERFACE:>
  target_link_libraries(ohm
    PRIVATE
      # Link 3es if enabled.
      $<BUILD_INTERFACE:$<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>>
      $<BUILD_INTERFACE:ZLIB::ZLIB>
  )
else(BUILD_SHARED)
  # With ohm static, we link depdencencies in such as way that the will propagate and need to be
  # found when ohm is linked.
  target_link_libraries(ohm
    PRIVATE
      # Link 3es if enabled.
      $<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>
      ZLIB::ZLIB
  )
endif(BUILD_SHARED)

if(TARGET Threads::Threads)
  target_link_libraries(ohm PUBLIC Threads::Threads)
endif(TARGET Threads::Threads)

target_include_directories(ohm
  PUBLIC
    $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/ohm>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
  PRIVATE
    $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}>
)

target_include_directories(ohm
  SYSTEM PRIVATE
    "${CMAKE_CURRENT_LIST_DIR}/3rdparty"
    # When OHM_TES_DEBUG is disabled, we don't link the 3es library, which would resolve our include dirs.
    # However, we need to include a dummy 3esservermacros.h file, which is in this project and will be
    # found under ${3ES_INCLUDE_DIRS}.
    $<$<NOT:$<BOOL:${OHM_TES_DEBUG}>>:${3ES_INCLUDE_DIRS}>
)

generate_export_header(ohm
  EXPORT_MACRO_NAME ohm_API
  EXPORT_FILE_NAME ohm/OhmExport.h
  STATIC_DEFINE OHM_STATIC)

install(TARGETS ohm EXPORT ${CMAKE_PROJECT_NAME}-config-targets
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/ohm
)

install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/ohm)

source_group("source" REGULAR_EXPRESSION ".*$")
source_group("source\\private" REGULAR_EXPRESSION "/private/.*$")
source_group("source\\serialise" REGULAR_EXPRESSION "/serialise/.*$")


================================================
FILE: ohm/CalculateSegmentKeys.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "CalculateSegmentKeys.h"

#include "KeyList.h"
#include "LineWalk.h"

namespace ohm
{
size_t calculateSegmentKeys(KeyList &keys, const OccupancyMap &map, const glm::dvec3 &start_point,
                            const glm::dvec3 &end_point, bool include_end_point)
{
  keys.clear();
  return walkSegmentKeys(LineWalkContext(map,
                                         [&keys](const Key &key, double enter_range, double exit_range) {
                                           (void)enter_range;  // Unused
                                           (void)exit_range;   // Unused

                                           keys.add(key);
                                           return true;
                                         }),
                         start_point, end_point, (include_end_point) ? 0u : kExcludeEndVoxel);
}
}  // namespace ohm


================================================
FILE: ohm/CalculateSegmentKeys.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef CALCULATESEGMENTKEYS_H
#define CALCULATESEGMENTKEYS_H

#include "OhmConfig.h"

#include <cstddef>

#include <glm/fwd.hpp>

namespace ohm
{
class KeyList;
class OccupancyMap;

/// This populates a @c KeyList with the voxel @c Key values intersected by a line segment.
///
/// This utility function leverages @c walkSegmentKeys() in order to calculate the set of voxel @c Key values
/// intersected by the line segment @p start_point to @p end_point .
///
/// @param[out] keys Populates with the keys intersected by the specified line segment.
/// @param map The occupancy map to calculate key segments for.
/// @param start_point The coordinate of the line segment start or sensor position. Global map frame.
/// @param end_point The coordinate of the line segment end or sample position. Global map frame.
/// @param include_end_point True to include the voxel containing the @p end_point , false to omit this voxel,
///     even when the @p start_point is in the same voxel (this case would generate an empty list).
size_t ohm_API calculateSegmentKeys(KeyList &keys, const OccupancyMap &map, const glm::dvec3 &start_point,
                                    const glm::dvec3 &end_point, bool include_end_point = true);
}  // namespace ohm

#endif  // CALCULATESEGMENTKEYS_H


================================================
FILE: ohm/ClearingPattern.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "ClearingPattern.h"
#include "private/ClearingPatternDetail.h"

#include "OccupancyMap.h"
#include "RayPattern.h"

namespace ohm
{
ClearingPattern::ClearingPattern(const RayPattern *pattern, bool take_ownership)
  : imp_(new ClearingPatternDetail)
{
  imp_->pattern = pattern;
  imp_->has_pattern_ownership = take_ownership;
  imp_->ray_flags = kDefaultRayFlags;
}

ClearingPattern::~ClearingPattern()
{
  if (imp_->has_pattern_ownership)
  {
    delete imp_->pattern;
  }
}

const RayPattern *ClearingPattern::pattern() const
{
  return imp_->pattern;
}

bool ClearingPattern::hasPatternOwnership() const
{
  return imp_->has_pattern_ownership;
}

unsigned ClearingPattern::rayFlags() const
{
  // Must not use kRfReverseWalk for clearing patterns and ray queries or we cannot stop on the first obstruction.
  return imp_->ray_flags & ~kRfReverseWalk;
}

void ClearingPattern::setRayFlags(unsigned ray_flags)
{
  imp_->ray_flags = ray_flags;
}

const glm::dvec3 *ClearingPattern::lastRaySet(size_t *element_count) const
{
  *element_count = imp_->ray_set.size();
  return imp_->ray_set.data();
}


const glm::dvec3 *ClearingPattern::buildRaySet(size_t *element_count, const glm::dvec3 &position,
                                               const glm::dquat &rotation)
{
  imp_->pattern->buildRays(&imp_->ray_set, position, rotation);
  *element_count = imp_->ray_set.size();
  return imp_->ray_set.data();
}


const glm::dvec3 *ClearingPattern::buildRaySet(size_t *element_count, const glm::dmat4 &pattern_transform)
{
  imp_->pattern->buildRays(&imp_->ray_set, pattern_transform);
  *element_count = imp_->ray_set.size();
  return imp_->ray_set.data();
}
}  // namespace ohm


================================================
FILE: ohm/ClearingPattern.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef CLEARINGPATTERN_H
#define CLEARINGPATTERN_H

#include "OhmConfig.h"

#include "ohm/RayFlag.h"
#include "ohm/RayMapper.h"
#include "ohm/RayPattern.h"

#include <glm/glm.hpp>

#include <vector>

namespace ohm
{
struct ClearingPatternDetail;
class OccupancyMap;

/// A helper class for applying a @c RayPattern as a clearing pattern to an @c OccupancyMap. The @p apply() method is
/// templated so that it may also be used to apply a clearing pattern to a @c GpuMap.
///
/// The class is constructed with a @c RayPattern, optionally taking ownership of the pointer. The @c apply() method
/// is called to generate transformed rays from the @p RayPattern and integrate them into the map as clearing rays.
/// The rays are applied using the configure @c rayFlag() bit set, which defaults to @c kDefaultRayFlags. The flags
/// are designed to effect rays which only degrade the first occupied voxel struck, then halt traversal. Intervening
/// voxels are left unchanged.
///
/// Technically the @c rayFlags() can be modified to generate different behaviour even non-clearing behaviour.
///
/// @todo Rename this class to @c RayPattern and configure a selection of default flags including @c kClearingFlags
/// for the current default behaviour.
class ohm_API ClearingPattern
{
public:
  /// Default flags used for a clearing pattern. The flags are set to ensure rays only affect occupied voxels
  /// (`kRfExcludeFree | kRfExcludeUnobserved`), reducing their occupancy and we stop on the first occupied voxel
  /// (`kRfStopOnFirstOccupied`) and the voxel at the end of the ray is treated as a clear voxel rather than a sample
  /// voxel (`kRfEndPointAsFree`).
  static const unsigned kDefaultRayFlags =
    kRfEndPointAsFree | kRfStopOnFirstOccupied | kRfExcludeFree | kRfExcludeUnobserved;

  /// Create a clearing pattern.
  /// @param pattern The ray pattern to use.
  /// @param take_ownership Should the @c ClearingPattern own the @p pattern and delete the pointer when done?
  ClearingPattern(const RayPattern *pattern, bool take_ownership);

  /// Destructor: deletes @c pattern() if it @c hasPatternOwnership().
  ~ClearingPattern();

  /// Retrieve the actual pattern.
  /// @return The @c RayPattern.
  const RayPattern *pattern() const;

  /// Query ownership of @p pattern().
  /// @return True if this class owns the @p pattern() memory.
  bool hasPatternOwnership() const;

  /// Query the combination of @c RayFlag bits to be used when applying this pattern. Defaults to @c kDefaultRayFlags .
  ///
  /// @note @c kRfReverseWalk is never set for a clearing pattern. We must trace forwards so that we can stop on the
  /// first obstruction.
  ///
  /// @return The configured @c RayFlag bit set.
  unsigned rayFlags() const;

  /// Set the combination of @c RayFlag bits to be used when applying this pattern. Note that patterns which deviate
  /// from the @c kDefaultRayFlags can result in significantly different semantics which invalidate the name
  /// "ClearingPattern" as it is used in this context. For example, clearing the flag @c kRfEndPointAsFree can have
  /// the "ClearingPattern" generate occupied voxels. As such, this function should be used with care.
  /// @param ray_flags Modified @c RayFlag bit set to use when applying this pattern.
  void setRayFlags(unsigned ray_flags);

  /// Apply the clearing @c pattern() to @p map. This supports both APIs for both @c OccupancyMap and the @p GpuMap
  /// extension.
  ///
  /// This @c pattern() rays are transformed by the given @p position and @p rotation before being integrated
  /// into the @p map. Each ray reduced the probabilty of the first occupied voxel it encounters but no further.
  ///
  /// @param map The map to integrate the clearing pattern into.
  /// @param position Used to the origin of the rays in this clearing pattern.
  /// @param rotation Rotates the ray end points in this clearing pattern.
  /// @param probability_scaling Scaling factor applied to probability values.
  template <typename MAP>
  void apply(MAP *map, const glm::dvec3 &position, const glm::dquat &rotation, float probability_scaling = 1.0);

  /// @overload
  template <typename MAP>
  void apply(MAP *map, const glm::dmat4 &pattern_transform, float probability_scaling = 1.0);

  /// Query the last ray set used in @c apply . This is the pattern transformed to match the position and rotation
  /// last supplied to @c apply() . Contains start/end pairs.
  /// @param[out] element_count Set to the number of elements in the returned value. The number of rays is half this.
  /// @return A pointer to the ray set.
  const glm::dvec3 *lastRaySet(size_t *element_count) const;

private:
  const glm::dvec3 *buildRaySet(size_t *element_count, const glm::dvec3 &position, const glm::dquat &rotation);

  const glm::dvec3 *buildRaySet(size_t *element_count, const glm::dmat4 &pattern_transform);

  std::unique_ptr<ClearingPatternDetail> imp_;
};

template <typename MAP>
void ClearingPattern::apply(MAP *map, const glm::dvec3 &position, const glm::dquat &rotation, float probability_scaling)
{
  size_t ray_element_count = 0u;
  const glm::dvec3 *ray_set = buildRaySet(&ray_element_count, position, rotation);
  const float initial_miss_value = map->missValue();
  map->setMissValue(initial_miss_value * probability_scaling);
  map->integrateRays(ray_set, unsigned(ray_element_count), nullptr, nullptr, rayFlags());
  map->setMissValue(initial_miss_value);
}

template <typename MAP>
void ClearingPattern::apply(MAP *map, const glm::dmat4 &pattern_transform, float probability_scaling)
{
  // Reserve memory for the ray set.
  size_t ray_element_count = 0u;
  const glm::dvec3 *ray_set = buildRaySet(&ray_element_count, pattern_transform);
  const float initial_miss_value = map->missValue();
  map->setMissValue(initial_miss_value * probability_scaling);
  map->integrateRays(ray_set, unsigned(ray_element_count), nullptr, nullptr, rayFlags());
  map->setMissValue(initial_miss_value);
}
}  // namespace ohm

#endif  // CLEARINGPATTERN_H


================================================
FILE: ohm/CompareMaps.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "CompareMaps.h"

#include "Key.h"
#include "MapLayer.h"
#include "MapLayout.h"
#include "OccupancyMap.h"
#include "VoxelBuffer.h"

#include <algorithm>
#include <memory>
#include <sstream>
#include <vector>

namespace ohm
{
namespace compare
{
namespace
{
template <typename T>
void buildLogMessage(std::ostream &out, T val)
{
  out << val;
}

template <typename T, typename... Args>
void buildLogMessage(std::ostream &out, T val, const Args &...args)
{
  out << val;
  buildLogMessage(out, args...);
}

template <typename... Args>
void logMessage(Log log, Severity severity, const Args &...args)
{
  std::ostringstream msg;
  buildLogMessage(msg, args...);
  log(severity, msg.str());
}

template <typename T, typename... Args>
bool compareItem(const T &val, const T &ref, Log log, Severity severity, const Args &...args)
{
  if (val != ref)
  {
    logMessage(log, severity, args...);
    return false;
  }

  return true;
}

template <typename T>
bool compareDatum(const void *val_ptr, const void *ref_ptr, uint64_t tolerance)
{
  T val{};
  T ref{};
  T epsilon{};
  memcpy(&val, val_ptr, sizeof(T));
  memcpy(&ref, ref_ptr, sizeof(T));
  memcpy(&epsilon, &tolerance, sizeof(T));

  // We may have unsigned quantities, so ensure the we subtract from the larger
  if (ref > val)
  {
    std::swap(ref, val);
  }
  // We do an equality comparison as well as a diff because we may be comparing floating point inf.
  return val == ref || (val - ref) <= epsilon;
}
}  // namespace

bool compareLayoutLayer(const OccupancyMap &eval_map, const OccupancyMap &ref_map, const std::string &layer_name,
                        unsigned flags, Log log)
{
  (void)flags;
  const MapLayout &eval_layout = eval_map.layout();
  const MapLayout &ref_layout = ref_map.layout();

  int ref_layer_index = ref_layout.layerIndex(layer_name.c_str());
  int eval_layer_index = eval_layout.layerIndex(layer_name.c_str());

  // Check layer presence and naming.
  bool ok = true;
  if (ref_layer_index == -1)
  {
    logMessage(log, Severity::kError, "Reference map does not have layer ", layer_name);
    ok = false;
  }

  if (eval_layer_index == -1)
  {
    logMessage(log, Severity::kError, "Test map does not have layer ", layer_name);
    ok = false;
  }

  if (!ok)
  {
    // Missing layer(s). Can't continue.
    return false;
  }

  const MapLayer &eval_layer = eval_layout.layer(eval_layer_index);
  const MapLayer &ref_layer = ref_layout.layer(ref_layer_index);

  // Check voxel content.
  auto eval_voxels = eval_layer.voxelLayout();
  auto ref_voxels = ref_layer.voxelLayout();

  ok = compareItem(eval_voxels.voxelByteSize(), ref_voxels.voxelByteSize(), log, Severity::kError,  //
                   "Voxel size mismatch: ", eval_voxels.voxelByteSize(), " expect ", ref_voxels.voxelByteSize()) &&
       ok;

  ok = compareItem(eval_voxels.memberCount(), ref_voxels.memberCount(), log, Severity::kError,  //
                   "Voxel member count mismatch: ", eval_voxels.memberCount(), " expect ", ref_voxels.memberCount()) &&
       ok;

  const size_t member_count = std::min(eval_voxels.memberCount(), ref_voxels.memberCount());

  for (size_t i = 0; i < member_count; ++i)
  {
    ok = compareItem<std::string>(eval_voxels.memberName(i), ref_voxels.memberName(i), log, Severity::kError,  //
                                  "Voxel member name mismatch:(", i, ") ", eval_voxels.memberName(i), " expect ",
                                  ref_voxels.memberName(i)) &&
         ok;

    ok = compareItem(eval_voxels.memberType(i), ref_voxels.memberType(i), log, Severity::kError,  //
                     "Voxel member type mismatch:(", i, ") ", eval_voxels.memberType(i), " expect ",
                     ref_voxels.memberType(i)) &&
         ok;

    ok = compareItem(eval_voxels.memberSize(i), ref_voxels.memberSize(i), log, Severity::kError,  //
                     "Voxel member size mismatch:(", i, ") ", eval_voxels.memberSize(i), " expect ",
                     ref_voxels.memberSize(i)) &&
         ok;
  }

  return ok;
}


template <typename T>
std::string memberValueErrorString(const void *val_ptr, const void *ref_ptr)
{
  T val{};
  T ref{};
  memcpy(&val, val_ptr, sizeof(T));
  memcpy(&ref, ref_ptr, sizeof(T));

  std::ostringstream str;
  str << "have " << val << " expect " << ref;
  return str.str();
}


std::string memberValueErrorString(DataType::Type data_type, const void *val_ptr, const void *ref_ptr)
{
  switch (data_type)
  {
  case DataType::kInt8:
    return memberValueErrorString<int8_t>(val_ptr, ref_ptr);
  case DataType::kUInt8:
    return memberValueErrorString<uint8_t>(val_ptr, ref_ptr);
  case DataType::kInt16:
    return memberValueErrorString<int16_t>(val_ptr, ref_ptr);
  case DataType::kUInt16:
    return memberValueErrorString<uint16_t>(val_ptr, ref_ptr);
  case DataType::kInt32:
    return memberValueErrorString<int32_t>(val_ptr, ref_ptr);
  case DataType::kUInt32:
    return memberValueErrorString<uint32_t>(val_ptr, ref_ptr);
  case DataType::kInt64:
    return memberValueErrorString<int64_t>(val_ptr, ref_ptr);
  case DataType::kUInt64:
    return memberValueErrorString<uint64_t>(val_ptr, ref_ptr);
  case DataType::kFloat:
    return memberValueErrorString<float>(val_ptr, ref_ptr);
  case DataType::kDouble:
    return memberValueErrorString<double>(val_ptr, ref_ptr);
  default:
    break;
  }

  return "<value-error>";
}


bool compareVoxel(const Key &key, VoxelBuffer<const VoxelBlock> &eval_buffer, VoxelLayoutConst &eval_voxel_layout,
                  VoxelBuffer<const VoxelBlock> &ref_buffer, VoxelLayoutConst &ref_voxel_layout,
                  const MapLayer *tolerance, Log log)
{
  if (!eval_buffer.isValid())
  {
    return false;
  }

  if (!ref_buffer.isValid())
  {
    return false;
  }

  bool ok = true;
  // Iterate reference members and extract from eval. Note we are not validating mismatches here, only validating the
  // data where we can.
  for (size_t i = 0; i < ref_voxel_layout.memberCount(); ++i)
  {
    std::string ref_name = ref_voxel_layout.memberName(i);

    size_t eval_member_index{};

    // Only search if the member at i is the not the same.
    if (i < eval_voxel_layout.memberCount() && ref_name == eval_voxel_layout.memberName(i))
    {
      eval_member_index = i;
    }
    else
    {
      bool found_member = false;
      // Search by name.
      for (size_t j = 0; j < eval_voxel_layout.memberCount(); ++j)
      {
        if (ref_name == eval_voxel_layout.memberName(j))
        {
          eval_member_index = j;
          found_member = true;
          break;
        }
      }

      if (!found_member)
      {
        return false;
      }
    }

    // Check data type match.
    if (eval_voxel_layout.memberType(eval_member_index) != ref_voxel_layout.memberType(i))
    {
      return false;
    }

    // Check value.
    int tolerance_index = (tolerance) ? tolerance->voxelLayout().indexOf(ref_name.c_str()) : -1;

    bool value_match = true;
    if (tolerance_index != -1)
    {
      // Comparison with tolerance.
      switch (ref_voxel_layout.memberType(i))
      {
      case DataType::kInt8:
        value_match = compareDatum<int8_t>(eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()),
                                           ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()),
                                           tolerance->voxelLayout().memberClearValue(tolerance_index));
        break;
      case DataType::kUInt8:
        value_match = compareDatum<uint8_t>(eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()),
                                            ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()),
                                            tolerance->voxelLayout().memberClearValue(tolerance_index));
        break;
      case DataType::kInt16:
        value_match = compareDatum<int16_t>(eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()),
                                            ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()),
                                            tolerance->voxelLayout().memberClearValue(tolerance_index));
        break;
      case DataType::kUInt16:
        value_match = compareDatum<uint16_t>(eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()),
                                             ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()),
                                             tolerance->voxelLayout().memberClearValue(tolerance_index));
        break;
      case DataType::kInt32:
        value_match = compareDatum<int32_t>(eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()),
                                            ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()),
                                            tolerance->voxelLayout().memberClearValue(tolerance_index));
        break;
      case DataType::kUInt32:
        value_match = compareDatum<uint32_t>(eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()),
                                             ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()),
                                             tolerance->voxelLayout().memberClearValue(tolerance_index));
        break;
      case DataType::kInt64:
        value_match = compareDatum<int64_t>(eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()),
                                            ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()),
                                            tolerance->voxelLayout().memberClearValue(tolerance_index));
        break;
      case DataType::kUInt64:
        value_match = compareDatum<uint64_t>(eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()),
                                             ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()),
                                             tolerance->voxelLayout().memberClearValue(tolerance_index));
        break;
      case DataType::kFloat:
        value_match = compareDatum<float>(eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()),
                                          ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()),
                                          tolerance->voxelLayout().memberClearValue(tolerance_index));
        break;
      case DataType::kDouble:
        value_match = compareDatum<double>(eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()),
                                           ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()),
                                           tolerance->voxelLayout().memberClearValue(tolerance_index));
        break;
      default:
        log(Severity::kError, "Unsupported data tolerance");
        break;
      }
    }
    else
    {
      // No tolerance. Raw byte comparison.
      const uint8_t *ref_mem = static_cast<const uint8_t *>(ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()));
      const uint8_t *eval_mem =
        static_cast<const uint8_t *>(eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()));

      value_match = std::memcmp(ref_mem, eval_mem, ref_voxel_layout.memberSize(i)) == 0;
    }

    if (!value_match)
    {
      ok = false;
      std::string error_str = memberValueErrorString(
        ref_voxel_layout.memberType(i), eval_voxel_layout.memberPtr(eval_member_index, eval_buffer.voxelMemory()),
        ref_voxel_layout.memberPtr(i, ref_buffer.voxelMemory()));
      logMessage(log, Severity::kError, "Voxel ", key, " value mismatch on member ", ref_name, ": ", error_str);
    }
  }

  return ok;
}

VoxelsResult compareVoxels(const OccupancyMap &eval_map, const OccupancyMap &ref_map, const std::string &layer_name,
                           const MapLayer *tolerance, unsigned flags, Log log)
{
  VoxelsResult result{};

  result.layout_match = compareLayoutLayer(eval_map, ref_map, layer_name, flags, log);
  if (!result.layout_match)
  {
    // Cannot continue on this failure.
    return result;
  }

  int ref_layer_index = ref_map.layout().layerIndex(layer_name.c_str());
  int eval_layer_index = eval_map.layout().layerIndex(layer_name.c_str());

  // We've compared layers so we know the indices are valid.
  VoxelLayoutConst ref_voxel_layout = ref_map.layout().layer(ref_layer_index).voxelLayout();
  VoxelLayoutConst eval_voxel_layout = eval_map.layout().layer(eval_layer_index).voxelLayout();

  VoxelBuffer<const VoxelBlock> ref_buffer;
  VoxelBuffer<const VoxelBlock> eval_buffer;

  Key last_key = Key(nullptr);
  const MapChunk *ref_chunk = nullptr;
  const MapChunk *eval_chunk = nullptr;
  for (auto key : ref_map)
  {
    if (last_key.regionKey() != key.regionKey())
    {
      // Changed regions. Need to update the voxel buffers.
      ref_chunk = ref_map.region(key.regionKey());
      eval_chunk = eval_map.region(key.regionKey());

      ref_buffer = (ref_chunk) ? VoxelBuffer<const VoxelBlock>(ref_chunk->voxel_blocks[ref_layer_index]) :
                                 VoxelBuffer<const VoxelBlock>();
      eval_buffer = (eval_chunk) ? VoxelBuffer<const VoxelBlock>(eval_chunk->voxel_blocks[eval_layer_index]) :
                                   VoxelBuffer<const VoxelBlock>();
    }

    if (compareVoxel(key, eval_buffer, eval_voxel_layout, ref_buffer, ref_voxel_layout, tolerance, log))
    {
      ++result.voxels_passed;
    }
    else
    {
      ++result.voxels_failed;
      OHM_CMP_FAIL(flags, result);
    }
    last_key = key;
  }

  return result;
}


void configureTolerance(ohm::MapLayer &layer, const char *member_name, DataType::Type data_type, uint64_t epsilon)
{
  auto voxel_layout = layer.voxelLayout();
  voxel_layout.addMember(member_name, data_type, epsilon);
}


void configureTolerance(ohm::MapLayer &layer, const char *member_name, int8_t epsilon)
{
  const uint64_t e = std::abs(epsilon);
  configureTolerance(layer, member_name, DataType::kInt8, e);
}


void configureTolerance(ohm::MapLayer &layer, const char *member_name, uint8_t epsilon)
{
  const uint64_t e = epsilon;
  configureTolerance(layer, member_name, DataType::kUInt8, e);
}


void configureTolerance(ohm::MapLayer &layer, const char *member_name, int16_t epsilon)
{
  const uint64_t e = std::abs(epsilon);
  configureTolerance(layer, member_name, DataType::kInt16, e);
}


void configureTolerance(ohm::MapLayer &layer, const char *member_name, uint16_t epsilon)
{
  const uint64_t e = epsilon;
  configureTolerance(layer, member_name, DataType::kUInt16, e);
}


void configureTolerance(ohm::MapLayer &layer, const char *member_name, int32_t epsilon)
{
  const uint64_t e = std::abs(epsilon);
  configureTolerance(layer, member_name, DataType::kInt32, e);
}


void configureTolerance(ohm::MapLayer &layer, const char *member_name, uint32_t epsilon)
{
  const uint64_t e = epsilon;
  configureTolerance(layer, member_name, DataType::kUInt32, e);
}


void configureTolerance(ohm::MapLayer &layer, const char *member_name, int64_t epsilon)
{
  const uint64_t e = std::abs(epsilon);
  configureTolerance(layer, member_name, DataType::kInt64, e);
}


void configureTolerance(ohm::MapLayer &layer, const char *member_name, uint64_t epsilon)
{
  const uint64_t e = epsilon;
  configureTolerance(layer, member_name, DataType::kUInt64, e);
}


void configureTolerance(ohm::MapLayer &layer, const char *member_name, float epsilon)
{
  epsilon = std::abs(epsilon);
  uint64_t e = 0;
  static_assert(sizeof(epsilon) <= sizeof(e), "epsilon data type size too large");
  memcpy(&e, &epsilon, sizeof(epsilon));
  configureTolerance(layer, member_name, DataType::kFloat, e);
}


void configureTolerance(ohm::MapLayer &layer, const char *member_name, double epsilon)
{
  epsilon = std::abs(epsilon);
  uint64_t e = 0;
  static_assert(sizeof(epsilon) <= sizeof(e), "epsilon data type size too large");
  memcpy(&e, &epsilon, sizeof(epsilon));
  configureTolerance(layer, member_name, DataType::kDouble, e);
}
}  // namespace compare
}  // namespace ohm


================================================
FILE: ohm/CompareMaps.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_COMPARE_H
#define OHM_COMPARE_H

#include "OhmConfig.h"

#include "VoxelBlock.h"
#include "VoxelBuffer.h"
#include "VoxelLayout.h"

#include <functional>
#include <string>

#define OHM_CMP_FAIL(flags, ret)              \
  if (((flags)&ohm::compare::kContinue) == 0) \
  {                                           \
    return (ret);                             \
  }

namespace ohm
{
class Key;
class OccupancyMap;

/// An experimental set of functions for comparing maps.
namespace compare
{
/// Severity of a comparison message.
enum class Severity
{
  kInfo,
  kWarning,
  kError
};

/// Comparison flag values.
enum Flag : unsigned
{
  kZero = 0u,             ///< Zero value.
  kContinue = (1u << 0u)  ///< Continue on error.
};

/// Results on comparing voxels
struct ohm_API VoxelsResult
{
  size_t voxels_passed = 0;   ///< Number of voxels passed.
  size_t voxels_failed = 0;   ///< Number of voxels failed.
  bool layout_match = false;  ///< Results of layout check. Must pass to compare content.

  /// Conversion to bool value.
  explicit inline operator bool() const { return voxels_failed == 0 && layout_match; }
};

/// Logging function type.
using Log = std::function<void(Severity, const std::string &)>;

/// Empty/dummy logging function.
inline void ohm_API emptyLog(Severity /*severity*/, const std::string & /*msg*/){};

/// Compare the layout of a layer in two maps.
///
/// This first checks that the layer exists in both maps, then gradually delves down to validating the content.
/// Compares:
/// - Voxel byte size
/// - Number of voxel members.
/// - Member alignment.
/// - Member type.
/// - Member size.
///
/// @param eval_map The map to evaluate.
/// @param ref_map The reference map.
/// @param layer_name The name fo the layer to compare layouts.
/// @param flags See @c Flag values.
/// @param log Logging function.
/// @return False if any validation step fails.
bool ohm_API compareLayoutLayer(const OccupancyMap &eval_map, const OccupancyMap &ref_map,
                                const std::string &layer_name, unsigned flags = 0, Log log = emptyLog);


/// Compare the voxel identified by @p key in two voxel buffers. The presumptions are that the same key is referencing
/// equivalent voxels from two different maps.
///
/// This checks the content member by member. A tolerance in value differences is supported via the @p toleranace
/// object. Whenever a member is matched for value comparison, we looking the @c tolerance for a member of the same
/// name (and type). When found, the @c VoxelLayoutT<T>::memberClearValue() is read and used as the allowed absolute
/// error difference between the eval map and the reference map.
///
/// For example, the following code configures an allowed deviation in occupancy value of:
///
/// @code
/// // Instantiate the tolerance object.
/// ohm::MapLayer occupancy_tolerance("occupancy");
/// auto voxel_layout = occupancy_tolerance.voxelLayout();
/// // Add an 'occupancy' member.
/// float tolerance = 1e-2f;
/// voxel_layout.addMember("occupancy", ohm::DataType::kFloat, *(uint64_t*)&tolerance);
/// @endcode
///
/// A convenience function, @c configureTolerance(), is provided to assist in configuring tolerances.
/// This changes the example above to:
///
/// @code
/// // Instantiate the tolerance object.
/// ohm::MapLayer occupancy_tolerance("occupancy");
/// ohm::compare::configureTolerance(occupancy_tolerance, "occupancy", 1e-2f);
/// @endcode
///
/// Hopefully this usage of @c MapLayer doesn't prove "too clever".
///
/// @param key Identifies the voxel of interest. Must be valid in both buffers.
/// @param eval_buffer The voxel buffer containing the data to evaluate. May be null, otherwise the @p key be a valid
/// key into this buffer. A null buffer is a failure condition.
/// @param eval_voxel_layout The voxel layer associated with the @p eval_buffer.
/// @param ref_buffer The voxel buffer containing the reference data. May be null, otherwise the @p key be a valid
/// key into this buffer. A null buffer is a failure condition.
/// @param ref_voxel_layout The voxel layer associated with the @p ref_buffer.
/// @param log Logging function.
/// @param tolerance A dummy @c MapLayer object which wraps the allowed tolerances for differences in voxel member
/// values.
/// @return False if any validation step fails.
bool ohm_API compareVoxel(const Key &key, VoxelBuffer<const VoxelBlock> &eval_buffer,
                          VoxelLayoutConst &eval_voxel_layout, VoxelBuffer<const VoxelBlock> &ref_buffer,
                          VoxelLayoutConst &ref_voxel_layout, const MapLayer *tolerance = nullptr, Log log = emptyLog);

/// Compare the layer content for all voxels in @p ref_map ensuring they exit in, and match in @c eval_map.
///
/// @param eval_map The map to evaluate.
/// @param ref_map The reference map.
/// @param flags See @c Flag values.
/// @param layer_name The name fo the layer to compare voxel content.
/// @param tolerance A dummy @c MapLayer object which wraps the allowed tolerances for differences in voxel member
/// values. See @c compareVoxel().
/// @param log Logging function.
/// @return False if any validation step fails.
VoxelsResult ohm_API compareVoxels(const OccupancyMap &eval_map, const OccupancyMap &ref_map,
                                   const std::string &layer_name, const MapLayer *tolerance = nullptr,
                                   unsigned flags = 0, Log log = emptyLog);


/// Configure a data tolerance value for @c member_name. The allowed absolute error tolerance is @p epsilon.
///
/// See @c compareVoxel() for intended usage.
/// @param layer The tolerance object to configure.
/// @param member_name The data member name to configure tolerance for.
/// @param epsilon The allowed error limit (absolute value is used).
void ohm_API configureTolerance(ohm::MapLayer &layer, const char *member_name, int8_t epsilon);
/// @overload
void ohm_API configureTolerance(ohm::MapLayer &layer, const char *member_name, uint8_t epsilon);
/// @overload
void ohm_API configureTolerance(ohm::MapLayer &layer, const char *member_name, int16_t epsilon);
/// @overload
void ohm_API configureTolerance(ohm::MapLayer &layer, const char *member_name, uint16_t epsilon);
/// @overload
void ohm_API configureTolerance(ohm::MapLayer &layer, const char *member_name, int32_t epsilon);
/// @overload
void ohm_API configureTolerance(ohm::MapLayer &layer, const char *member_name, uint32_t epsilon);
/// @overload
void ohm_API configureTolerance(ohm::MapLayer &layer, const char *member_name, int64_t epsilon);
/// @overload
void ohm_API configureTolerance(ohm::MapLayer &layer, const char *member_name, uint64_t epsilon);
/// @overload
void ohm_API configureTolerance(ohm::MapLayer &layer, const char *member_name, float epsilon);
/// @overload
void ohm_API configureTolerance(ohm::MapLayer &layer, const char *member_name, double epsilon);
}  // namespace compare
}  // namespace ohm

#endif  // OHM_COMPARE_H


================================================
FILE: ohm/CopyUtil.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "CopyUtil.h"

#include "private/OccupancyMapDetail.h"

#include "DefaultLayer.h"
#include "MapChunk.h"
#include "MapLayer.h"
#include "MapRegionCache.h"
#include "OccupancyMap.h"
#include "VoxelBlock.h"
#include "VoxelBuffer.h"

#include <glm/glm.hpp>

#include <cstring>

namespace
{
void copyChunkLayerUnsafe(ohm::MapChunk &dst_chunk, unsigned dst_layer, const ohm::MapChunk &src_chunk,
                          unsigned src_layer)
{
  ohm::VoxelBuffer<ohm::VoxelBlock> dst_buffer(dst_chunk.voxel_blocks[dst_layer]);
  ohm::VoxelBuffer<const ohm::VoxelBlock> src_buffer(src_chunk.voxel_blocks[src_layer]);

  memcpy(dst_buffer.voxelMemory(), src_buffer.voxelMemory(), src_buffer.voxelMemorySize());
}
}  // namespace


namespace ohm
{
CopyChunkFilter copyFilterExtents(const glm::dvec3 &min_ext, const glm::dvec3 &max_ext)
{
  return [min_ext, max_ext](const MapChunk &chunk) {
    const glm::dvec3 region_half_ext = 0.5 * chunk.map->region_spatial_dimensions;
    const glm::dvec3 region_min = chunk.region.centre - region_half_ext;
    const glm::dvec3 region_max = chunk.region.centre + region_half_ext;
    return !glm::any(glm::lessThan(region_max, min_ext)) && !glm::any(glm::greaterThan(region_min, max_ext));
  };
}

CopyChunkFilter copyFilterStamp(uint64_t after_stamp)
{
  return [after_stamp](const MapChunk &chunk) { return chunk.dirty_stamp > after_stamp; };
}

bool canCopy(const OccupancyMap &dst, const OccupancyMap &src)
{
  return &src != &dst && src.resolution() == dst.resolution() &&
         src.regionVoxelDimensions() == dst.regionVoxelDimensions() && src.origin() == dst.origin();
}

bool copyMap(OccupancyMap &dst, const OccupancyMap &src, const CopyChunkFilter &copy_chunk_filter,
             const CopyLayerFilter &copy_layer_filter)
{
  if (!canCopy(dst, src))
  {
    return false;
  }

  OccupancyMapDetail &dst_detail = *dst.detail();
  const MapLayout &dst_layout = dst_detail.layout;
  const OccupancyMapDetail &src_detail = *src.detail();

  // Lock required mutexes.
  // We only lock the source map and assume we are the only writers to the dst map. The @c region() call will lock dst
  // map mutex.
  std::unique_lock<decltype(src_detail.mutex)> src_guard(src_detail.mutex);

  // First resolve the overlapping layer set. Holds src, dst map layer index pairs.
  std::vector<std::pair<unsigned, unsigned>> layer_overlap;
  std::vector<MapRegionCache *> layer_caches;  // Cache pointers matching layer_overlap[n].first .
  if (src_detail.layout.calculateOverlappingLayerSet(layer_overlap, dst_detail.layout) == 0)
  {
    // No common layers to copy.
    return false;
  }

  // We have layer_overlap which identifies source to destination layer indices. Now we remove any source layer entries
  // which fail copy_layer_filter.
  if (copy_layer_filter)
  {
    for (auto iter = layer_overlap.begin(); iter != layer_overlap.end();)
    {
      const std::string source_layer_name = src_detail.layout.layer(iter->first).name();
      if (!copy_layer_filter(source_layer_name))
      {
        iter = layer_overlap.erase(iter);
      }
      else
      {
        ++iter;
      }
    }
  }

  // Find layer caches for each layer_overlap entry.
  if (src_detail.gpu_cache)
  {
    for (const auto &overlap : layer_overlap)
    {
      layer_caches.emplace_back(src_detail.gpu_cache->findLayerCache(overlap.first));
    }
  }
  else if (!layer_overlap.empty())
  {
    layer_caches.resize(layer_overlap.size());
    std::fill(layer_caches.begin(), layer_caches.end(), nullptr);
  }

  const int tsdf_layer_index = dst_layout.layerIndex(default_layer::tsdfLayerName());
  for (const auto &src_iter : src_detail.chunks)
  {
    if (!src_iter.second || (copy_chunk_filter && !copy_chunk_filter(*src_iter.second)))
    {
      // Excluded chunk.
      continue;
    }

    const MapChunk &src_chunk = *src_iter.second;
    MapChunk &dst_chunk = *dst.region(src_iter.first, true);
    assert(&dst_chunk);

    // Included chunk.
    // First try copy via the GPU cache.
    for (size_t i = 0; i < layer_overlap.size(); ++i)
    {
      const auto &layer_pair = layer_overlap[i];
      auto *layer_cache = (i < layer_caches.size()) ? layer_caches[i] : nullptr;
      const bool update_first_valid =
        (dst_layout.occupancyLayer() >= 0 && layer_pair.second == unsigned(dst_layout.occupancyLayer())) ||
        (tsdf_layer_index >= 0 && layer_pair.second == unsigned(tsdf_layer_index));
      // First try letting the layer cache handle the copy/sync.
      if (layer_cache && layer_cache->syncLayerTo(dst_chunk, layer_pair.second, src_chunk, layer_pair.first))
      {
        // Special case: if we are dealing with the occupancy or TSDF layer, then we need to update
        // MapRegion::first_valid_index in the target map for correct map iteration. However, when the layer cache
        // handles the copy we can't guarantee the source map value is up to date, so we can't just copy from the source
        // chunk value.
        if (update_first_valid)
        {
          dst_chunk.searchAndUpdateFirstValid(dst_detail.region_voxel_dimensions);
        }
      }
      else
      {
        // Layer cache not present or it didn't handle the copy. Use the fallback function.
        copyChunkLayerUnsafe(dst_chunk, layer_pair.second, src_chunk, layer_pair.first);
        // Special case: as in the branch above, but this time we can just copy the first_valid_index from the source
        // chunk as there's no layer cache and we can assume the MapChunk is fully up to date.
        if (update_first_valid)
        {
          dst_chunk.updateFirstValid(src_chunk.first_valid_index);
        }
      }
    }
  }

  return true;
}  // namespace ohm

}  // namespace ohm


================================================
FILE: ohm/CopyUtil.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_COPYUTIL_H
#define OHM_COPYUTIL_H

#include "OhmConfig.h"

#include <glm/fwd.hpp>

#include <algorithm>
#include <functional>
#include <string>
#include <vector>

namespace ohm
{
class OccupancyMap;
struct MapChunk;

/// Filter used to determine if a chunk should be copied. Note that an empty filter always indicates an implied pass.
/// This is relevant to logical operations applied to filter functions.
using CopyChunkFilter = std::function<bool(const MapChunk &chunk)>;

/// Filter used to determine if a layer should be included when copying a map.
using CopyLayerFilter = std::function<bool(const std::string &layer_name)>;

/// Create a copy filter function which passes any @c MapChunk which overlaps the given axis aligned extents.
/// @param min_ext The minimum extents value (not validated).
/// @param max_ext The maximum extents value (not validated).
/// @return The filtering function object.
CopyChunkFilter ohm_API copyFilterExtents(const glm::dvec3 &min_ext, const glm::dvec3 &max_ext);

/// Create a copy filter function which passes any @c MapChunk with a @c MapChunk::dirty_stamp greater than
/// @p after_stamp .
/// @param after_stamp The stamp value at/after which to pass chunks.
/// @return The filtering function object.
CopyChunkFilter ohm_API copyFilterStamp(uint64_t after_stamp);

/// Returns a copy filter function which passes if both @c filter_a and @c filter_b pass.
///
/// Note that if either @c filter_a or @c filter_b are empty, then the other object is required. It emerges that an
/// empty filter object is returned when both are empty.
///
/// @param filter_a The first filter function object.
/// @param filter_a The second filter function object.
/// @return A filter function which passes if both given filters pass. May be empty if both arguments are empty.
inline CopyChunkFilter ohm_API copyFilterAnd(const CopyChunkFilter &filter_a, const CopyChunkFilter &filter_b)
{
  if (!filter_a || !filter_b)
  {
    return (filter_a) ? filter_a : filter_b;
  }
  return [filter_a, filter_b](const MapChunk &chunk) { return filter_a(chunk) && filter_b(chunk); };
}

/// Returns a copy filter function which passes if either @c filter_a or @c filter_b pass.
///
/// Note that if either @c filter_a or @c filter_b are empty, then an empty filter object is returned which implies
/// always passing.
///
/// @param filter_a The first filter function object.
/// @param filter_a The second filter function object.
/// @return A filter function which passes if either given filters pass. May be empty if both either argument is empty.
inline CopyChunkFilter ohm_API copyFilterOr(const CopyChunkFilter &filter_a, const CopyChunkFilter &filter_b)
{
  if (!filter_a || !filter_b)
  {
    // In the case one filter is empty, that's an implied pass. We simply return an empty filter.
    return {};
  }
  return [filter_a, filter_b](const MapChunk &chunk) { return filter_a(chunk) || filter_b(chunk); };
}

/// Returns a copy filter function which negates the results of @p filter . Remember that an empty filter is an implied
/// pass, so the negation of an empty filter always returns @c false .
/// @param filter The filter to negate the results of.
/// @return A filter function which negates the results of @p filter .
inline CopyChunkFilter ohm_API copyFilterNegate(const CopyChunkFilter &filter)
{
  if (filter)
  {
    return [filter](const MapChunk &chunk) { return !filter(chunk); };
  }
  return [](const MapChunk &chunk) {
    (void)chunk;
    return false;
  };
}

/// A layer filter function for @c copyMap() which passes the selected @p layers .
/// @param layers The set of allowed layers.
/// @return A filter function for the given layers.
inline CopyLayerFilter ohm_API copyLayersFilter(const std::vector<std::string> &layers)
{
  return [layers](const std::string &layer_name) {
    return std::find(layers.begin(), layers.end(), layer_name) != layers.end();
  };
}

/// Returns a copy filter function which negates the results of @p filter . Remember that an empty filter is an implied
/// pass, so the negation of an empty filter always returns @c false .
/// @param filter The filter to negate the results of.
/// @return A filter function which negates the results of @p filter .
inline CopyLayerFilter ohm_API copyFilterNegate(const CopyLayerFilter &filter)
{
  if (filter)
  {
    return [filter](const std::string &layer_name) { return !filter(layer_name); };
  }
  return [](const std::string &layer_name) {
    (void)layer_name;
    return false;
  };
}

/// Validate that it is possible to copy data from this map to @p other .
///
/// The requirements are:
///
/// - Cannot copy to self.
/// - Both maps must:
///   - have the same resolution
///   - have the same region size
///   - have the same origin
/// - The @p dst map is not being modifies by another thread (not enforced).
///
/// @param src The map to copy from.
/// @param dst The map to copy do.
bool ohm_API canCopy(const OccupancyMap &dst, const OccupancyMap &src);

/// Copy data from this map to @p other, copying only the regions in the range `[min_ext, max_ext]`. Only layers
/// which are present in both maps are copied.
///
/// There are some specific conditions which must be met in order for this to work:
/// - @c canCopy() must pass.
/// - The maps must have common map layers matched by name and voxel layout.
///
/// @note This is not currently threadsafe.
///
/// @param dst The map to copy into.
/// @param src The map to copy from.
/// @param copy_chunk_filter Optional @c MapChunk filter to apply restricting what is copied.
/// @param copy_layer_filter Optional filter function for determining which layers are copied from the source map,
/// provided they exist in the destination map. Only source layers which pass the filter function are copied.
bool ohm_API copyMap(OccupancyMap &dst, const OccupancyMap &src, const CopyChunkFilter &copy_chunk_filter = {},
                     const CopyLayerFilter &copy_layer_filter = {});
}  // namespace ohm

#endif  // OHM_COPYUTIL_H


================================================
FILE: ohm/CovarianceVoxel.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmConfig.h"

// #define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

// Must come after glm includes due to usage on GPU.
#include "CovarianceVoxel.h"

#include "NdtMap.h"
#include "OccupancyMap.h"
#include "Voxel.h"
#include "VoxelMean.h"
#include "VoxelOccupancy.h"

#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/matrix_factorisation.hpp>

#ifdef OHM_FEATURE_EIGEN
#include <Eigen/Dense>
#endif  // OHM_FEATURE_EIGEN

#include <3esservermacros.h>
#ifdef TES_ENABLE
#include <3esserver.h>
#include <shapes/3esshapes.h>
#endif  // TES_ENABLE

#include <array>

namespace ohm
{
#if OHM_COV_DEBUG
namespace
{
unsigned max_iterations = 0;
double max_error = 0;
}  // namespace
#endif  // OHM_COV_DEBUG

namespace
{
#ifdef OHM_FEATURE_EIGEN
void covarianceEigenDecompositionEigen(const CovarianceVoxel *cov, glm::dmat3 *eigenvectors, glm::dvec3 *eigenvalues)
{
  // This has been noted to be ~3x faster than the GLM iterative version.
  const glm::dmat3 cov_mat = covarianceMatrix(cov);
  // Both GLM and Eigen are column major. Direct mapping is fine.
  const auto cov_eigen = Eigen::Matrix3d::ConstMapType(glm::value_ptr(cov_mat), 3, 3);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov_eigen);
  Eigen::Vector3d evals = Eigen::Vector3d::Ones();
  Eigen::Matrix3d evecs = Eigen::Matrix3d::Identity();

  if (eigensolver.info() == Eigen::Success)
  {
    evals = eigensolver.eigenvalues();
    evecs = eigensolver.eigenvectors();
  }
  const double det = evecs.determinant();
  if (det < 0.0)
  {
    evecs.col(0) = -evecs.col(0);  // must be valid rotation matrix (determinant=+1)
  }
  else if (det == 0.0)
  {
    evecs = Eigen::Matrix3d::Identity();
  }

  *eigenvalues = glm::dvec3(evals[0], evals[1], evals[2]);

  for (int r = 0; r < 3; ++r)
  {
    for (int c = 0; c < 3; ++c)
    {
      (*eigenvectors)[c][r] = evecs(r, c);
    }
  }
}
#else  // OHM_FEATURE_EIGEN
void covarianceEigenDecompositionGlm(const CovarianceVoxel *cov, glm::dmat3 *eigenvectors, glm::dvec3 *eigenvalues)
{
  // This has been noted to be ~3x slower than the Eigen solver.
  glm::dmat3 mat = covarianceMatrix(cov);

  *eigenvectors = glm::dmat3(1.0);  // Identity initialisation

  // Use QR decomposition to implement the QR algorithm.
  // For this algorithm, we iterate as follows:
  //
  // P = cov * cov^T  // cov by cov transpose
  // for i = 1 to N
  //   Q, R = qr_decomposition(P)
  //   P = R * Q
  // end
  //
  // The eigenvalues converge in the diagonal of R.
  // Meanwhile the eigenvectors are found by the product all the Q matrices
  //
  // We set a hard iteration limit, but we also check for convergence with last iteration and may early out.
  const unsigned iteration_limit = 20;
  glm::dmat3 q, r;
  glm::dvec3 eigenvalues_last, eigenvalues_current(0);
  const glm::dvec3 delta_threshold(1e-9);
  for (unsigned i = 0; i < iteration_limit; ++i)
  {
    eigenvalues_last = eigenvalues_current;
#if OHM_COV_DEBUG
    max_iterations = std::max(max_iterations, i + 1);
#endif  // OHM_COV_DEBUG

    glm::qr_decompose(mat, q, r);
    // Progressively refine the eigenvectors.
    *eigenvectors = *eigenvectors * q;
    // Update eigenvalues and check for convergence
    eigenvalues_current[0] = r[0][0];
    eigenvalues_current[1] = r[1][1];
    eigenvalues_current[2] = r[2][2];

    mat = r * q;

    const glm::dvec3 eval_delta = glm::abs(eigenvalues_current - eigenvalues_last);
    if (glm::all(glm::lessThanEqual(eval_delta, delta_threshold)))
    {
      break;
    }
  }

#if OHM_COV_DEBUG
  const glm::dvec3 eval_delta = glm::abs(eigenvalues_current - eigenvalues_last);
  max_error = std::max(eval_delta.x, max_error);
  max_error = std::max(eval_delta.y, max_error);
  max_error = std::max(eval_delta.z, max_error);
#endif  // OHM_COV_DEBUG

  *eigenvalues = eigenvalues_current;
}
#endif  // OHM_FEATURE_EIGEN
}  // namespace

void covarianceEigenDecomposition(const CovarianceVoxel *cov, glm::dmat3 *eigenvectors, glm::dvec3 *eigenvalues)
{
#ifdef OHM_FEATURE_EIGEN
  covarianceEigenDecompositionEigen(cov, eigenvectors, eigenvalues);
#else   // OHM_FEATURE_EIGEN
  covarianceEigenDecompositionGlm(cov, eigenvectors, eigenvalues);
#endif  // OHM_FEATURE_EIGEN
}


void covarianceEstimatePrimaryNormal(const CovarianceVoxel *cov, glm::dvec3 *normal, int preferred_axis)
{
  glm::dmat3 eigenvectors;
  glm::dvec3 eigenvalues;

  covarianceEigenDecomposition(cov, &eigenvectors, &eigenvalues);

  int smallest_eval_index = preferred_axis;
  for (int i = 0; i < 3; ++i)
  {
    if (eigenvalues[i] < eigenvalues[smallest_eval_index])
    {
      smallest_eval_index = i;
    }
  }

  const double length2 = glm::length2(eigenvectors[smallest_eval_index]);
  // const double confidence = 1.0 - length2;
  *normal = (length2 > 0) ? eigenvectors[smallest_eval_index] / glm::sqrt(length2) : eigenvectors[smallest_eval_index];
  // return confidence;
}


bool covarianceUnitSphereTransformation(const CovarianceVoxel *cov, glm::dquat *rotation, glm::dvec3 *scale)
{
  glm::dmat3 eigenvectors;
  glm::dvec3 eigenvalues;

  covarianceEigenDecomposition(cov, &eigenvectors, &eigenvalues);

  const double det = glm::determinant(eigenvectors);
  if (det < 0.0)
  {
    eigenvectors[0] = -eigenvectors[0];  // must be valid rotation matrix (determinant=+1)
  }
  else if (det == 0.0)
  {
    eigenvectors = glm::dmat3(1.0);
  }

  *rotation = glm::dquat(eigenvectors);
  for (int i = 0; i < 3; ++i)
  {
    const double eval = std::abs(eigenvalues[i]);  // abs just in case.
    const double epsilon = 1e-9;
    (*scale)[i] = (eval > epsilon) ? std::sqrt(eval) : eval;
  }

  return true;
}


#if OHM_COV_DEBUG
#include <logutil/Logger.h>
void covDebugStats()
{
  logutil::trace("QR algorithm max iterations: ", max_iterations, '\n');
  logutil::trace("QR algorithm max error: ", max_error, '\n');
}
#endif  // OHM_COV_DEBUG

void integrateNdtHit(NdtMap &map, const Key &key, const glm::dvec3 &sensor, const glm::dvec3 &sample, bool ndt_tm,
                     const float sample_intensity)
{
  OccupancyMap &occupancy_map = map.map();
  Voxel<float> occupancy_voxel(&occupancy_map, occupancy_map.layout().occupancyLayer(), key);
  Voxel<VoxelMean> mean_voxel(occupancy_voxel, occupancy_map.layout().meanLayer());
  Voxel<CovarianceVoxel> cov_voxel(occupancy_voxel, occupancy_map.layout().covarianceLayer());
  const glm::dvec3 voxel_centre = occupancy_map.voxelCentreGlobal(key);

  assert(occupancy_voxel.isValid());
  assert(mean_voxel.isValid());
  assert(cov_voxel.isValid());

  // Keep clang analysis happy
#ifdef __clang_analyzer__
  if (!occupancy_voxel.voxelMemory() || !mean_voxel.voxelMemory() || !cov_voxel.voxelMemory())
  {
    return;
  }
#endif  // __clang_analyzer__

  // NDT-OM
  CovarianceVoxel cov;
  VoxelMean mean;
  float occupancy;
  occupancy_voxel.read(&occupancy);
  mean_voxel.read(&mean);
  cov_voxel.read(&cov);

  float updated_value = occupancy;
  const glm::dvec3 voxel_pos = position(mean, voxel_centre, occupancy_map.resolution());

  // NDT-TM
  if (ndt_tm)
  {
    Voxel<IntensityMeanCov> intensity_voxel(&occupancy_map, occupancy_map.layout().intensityLayer(), key);
    Voxel<HitMissCount> hit_miss_count_voxel(&occupancy_map, occupancy_map.layout().hitMissCountLayer(), key);

    if (intensity_voxel.isLayerValid() && hit_miss_count_voxel.isLayerValid())
    {
      IntensityMeanCov intensity;
      HitMissCount hit_miss_count;
      intensity_voxel.read(&intensity);
      hit_miss_count_voxel.read(&hit_miss_count);

      const bool reinitialise_permeability_with_covariance = true;  // TODO: make a parameter of map
      calculateHitMissUpdateOnHit(&cov, updated_value, &hit_miss_count, sensor, sample, voxel_pos, mean.count,
                                  unobservedOccupancyValue(), reinitialise_permeability_with_covariance,
                                  map.adaptationRate(), map.sensorNoise(), map.reinitialiseCovarianceThreshold(),
                                  map.reinitialiseCovariancePointCount(), map.ndtSampleThreshold());

      hit_miss_count_voxel.write(hit_miss_count);

      calculateIntensityUpdateOnHit(&intensity, updated_value, sample_intensity, map.initialIntensityCovariance(),
                                    mean.count, map.reinitialiseCovarianceThreshold(),
                                    map.reinitialiseCovariancePointCount());

      intensity_voxel.write(intensity);
    }
  }

  if (calculateHitWithCovariance(&cov, &updated_value, sample, voxel_pos, mean.count, occupancy_map.hitValue(),
                                 unobservedOccupancyValue(), float(occupancy_map.resolution()),
                                 map.reinitialiseCovarianceThreshold(), map.reinitialiseCovariancePointCount()))
  {
    // Covariance matrix has reset. Reset the point count to clear the mean value.
    mean.count = 0;
  }

  // Ensure we update the occupancy within the configured map limits.
  occupancyAdjustUp(
    &occupancy, occupancy, updated_value, unobservedOccupancyValue(), occupancy_map.maxVoxelValue(),
    occupancy_map.saturateAtMinValue() ? occupancy_map.minVoxelValue() : std::numeric_limits<float>::lowest(),
    occupancy_map.saturateAtMaxValue() ? occupancy_map.maxVoxelValue() : std::numeric_limits<float>::max(), false);
  occupancy_voxel.write(occupancy);

  // Update the voxel mean.
  updatePosition(&mean, sample, voxel_centre, occupancy_map.resolution());
  mean_voxel.write(mean);
  cov_voxel.write(cov);
}


void integrateNdtMiss(NdtMap &map, const Key &key, const glm::dvec3 &sensor, const glm::dvec3 &sample, bool ndt_tm)
{
  OccupancyMap &occupancy_map = map.map();
  Voxel<float> occupancy_voxel(&occupancy_map, occupancy_map.layout().occupancyLayer(), key);
  Voxel<const VoxelMean> mean_voxel(occupancy_voxel, occupancy_map.layout().meanLayer());
  Voxel<const CovarianceVoxel> cov_voxel(occupancy_voxel, occupancy_map.layout().covarianceLayer());
  const glm::dvec3 voxel_centre = occupancy_map.voxelCentreGlobal(key);

  assert(occupancy_voxel.isValid());
  assert(mean_voxel.isValid());
  assert(cov_voxel.isValid());

  // Keep clang analysis happy
#ifdef __clang_analyzer__
  if (!occupancy_voxel.voxelMemory() || !mean_voxel.voxelMemory() || !cov_voxel.voxelMemory())
  {
    return;
  }
#endif  // __clang_analyzer__

  // NDT-OM
  CovarianceVoxel cov;
  VoxelMean mean;
  float occupancy;
  occupancy_voxel.read(&occupancy);
  mean_voxel.read(&mean);
  cov_voxel.read(&cov);

  float updated_value = occupancy;
#ifdef TES_ENABLE
  const float initial_value = occupancy;
  glm::dvec3 voxel_maximum_likelihood;
#endif  // TES_ENABLE
  const glm::dvec3 voxel_mean = position(mean, voxel_centre, occupancy_map.resolution());
  bool confirm_miss = false;
  calculateMissNdt(&cov, &updated_value, &confirm_miss, sensor, sample, voxel_mean, mean.count,
                   unobservedOccupancyValue(), occupancy_map.missValue(), map.adaptationRate(), map.sensorNoise(),
                   map.ndtSampleThreshold());

  if (ndt_tm && confirm_miss)
  {
    Voxel<HitMissCount> hit_miss_count_voxel(&occupancy_map, occupancy_map.layout().hitMissCountLayer(), key);
    if (hit_miss_count_voxel.isLayerValid())
    {
      HitMissCount hit_miss_count;
      hit_miss_count_voxel.read(&hit_miss_count);
      ++hit_miss_count.miss_count;
      hit_miss_count_voxel.write(hit_miss_count);
    }
  }

  occupancyAdjustDown(
    &occupancy, occupancy, updated_value, unobservedOccupancyValue(), occupancy_map.minVoxelValue(),
    occupancy_map.saturateAtMinValue() ? occupancy_map.minVoxelValue() : std::numeric_limits<float>::lowest(),
    occupancy_map.saturateAtMaxValue() ? occupancy_map.maxVoxelValue() : std::numeric_limits<float>::max(), false);
  occupancy_voxel.write(occupancy);

#ifdef TES_ENABLE
  if (map.trace())
  {
    const glm::dvec3 voxel_centre = occupancy_map.voxelCentreGlobal(key);
    TES_BOX_W(g_tes, TES_COLOUR(OrangeRed), tes::Id(cov_voxel.voxelMemory()),
              tes::Transform(glm::value_ptr(voxel_centre), glm::value_ptr(glm::dvec3(occupancy_map.resolution()))));
    TES_SERVER_UPDATE(g_tes, 0.0f);

    bool drew_surfel = false;
    glm::dquat rot;
    glm::dvec3 scale;
    if (covarianceUnitSphereTransformation(&cov, &rot, &scale))
    {
      TES_SPHERE(g_tes, TES_COLOUR(SeaGreen), tes::Id(cov_voxel.voxelMemory()),
                 tes::Transform(tes::Vector3d(glm::value_ptr(voxel_mean)), tes::Quaterniond(rot.x, rot.y, rot.z, rot.w),
                                tes::Vector3d(glm::value_ptr(scale))));
      drew_surfel = true;
    }

    // Trace the voxel mean, maximum likelihood point and the ellipsoid.
    // Mean
    const float mean_pos_radius = 0.05f;
    const float likely_pos_radius = 0.1f;
    TES_SPHERE(g_tes, TES_COLOUR(OrangeRed), tes::Id(&voxel_mean),
               tes::Spherical(glm::value_ptr(voxel_mean), mean_pos_radius));
    // Maximum likelihood
    TES_SPHERE_W(g_tes, TES_COLOUR(PowderBlue), tes::Id(&voxel_maximum_likelihood),
                 tes::Spherical(glm::value_ptr(voxel_maximum_likelihood), likely_pos_radius));

    std::array<char, 64> text;  // NOLINT(readability-magic-numbers)
    text[0] = '\0';
    sprintf(text.data(), "P %.3f", valueToProbability(occupancy - initial_value));
    TES_TEXT2D_WORLD(g_tes, TES_COLOUR(White), text.data(), tes::Id(),
                     tes::Spherical(tes::Vector3d(glm::value_ptr(voxel_centre))));

    TES_SERVER_UPDATE(g_tes, 0.0f);
    TES_BOX_END(g_tes, tes::Id(cov_voxel.voxelMemory()));
    TES_SPHERE_END(g_tes, tes::Id(&voxel_mean));
    TES_SPHERE_END(g_tes, tes::Id(&voxel_maximum_likelihood));
    if (drew_surfel)
    {
      TES_SPHERE_END(g_tes, tes::Id(cov_voxel.voxelMemory()));
    }
  }
#endif  // TES_ENABLE
}
}  // namespace ohm


================================================
FILE: ohm/CovarianceVoxel.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef COVARIANCEVOXEL_H
#define COVARIANCEVOXEL_H

#include "OhmConfig.h"

#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

/// Allows additional debug information on @c covarianceEigenDecomposition()
#define OHM_COV_DEBUG 0


/// @defgroup voxelcovariance Voxel Covariance Functions
///
/// These functions support updating @c VoxelCovariance in CPU and GPU. This update implements a normal distributions
/// transform logic for occupancy voxels.
///
/// For reference, see:
///
/// Saarinen, Jari & Andreasson, Henrik & Stoyanov, Todor & Lilienthal, Achim. (2013). 3D Normal Distributions Transform
/// Occupancy Maps: An Efficient Representation for Mapping in Dynamic Environments. The International Journal of
/// Robotics Research. 32. 1627-1644. 10.1177/0278364913499415.

namespace ohm
{
#include "CovarianceVoxelCompute.h"

class Key;
class NdtMap;

/// @ingroup voxelcovariance
/// Perform an eigen decomposition on the covariance data in @p cov .
///
/// This currently uses the QR algorithm. This is an iterative solution, which is not recommended. Therefore this
/// function is not recommended for high performance code.
///
/// @param cov The covariance voxel to operate on.
/// @param[out] eigenvectors Set to the eigen vectors extracted from @p cov .
/// @param[out] eigenvalues Set to the eigen values extracted from @p cov .
void ohm_API covarianceEigenDecomposition(const CovarianceVoxel *cov, glm::dmat3 *eigenvectors,
                                          glm::dvec3 *eigenvalues);

/// @ingroup voxelcovariance
/// Estimate a primary normal from the given covariance. This selects the eigenvector with the smallest eigenvalue.
/// This may be ambiguous.
/// @param cov The covariance data for the voxel.
/// @param normal The normal value is written here.
/// @param preferred_axis Optionally specify which axis to prefer when the covariance is ambiguous. E.g., for voxels
/// with insufficient data.
void ohm_API covarianceEstimatePrimaryNormal(const CovarianceVoxel *cov, glm::dvec3 *normal, int preferred_axis = 0);

/// @ingroup voxelcovariance
/// Convert @p cov into a rotation and scale factors to deform a unit sphere to approximate the covariance cluster.
///
/// This calls @c covarianceEigenDecomposition() and suffers the same performance constraints.
///
/// @param cov The covariance voxel to operate on.
/// @param[out] rotation The quaternion rotation to apply to the unit sphere after applying @p scale .
/// @param[out] scale The scaling to apply to the unit sphere before @p rotation .
bool ohm_API covarianceUnitSphereTransformation(const CovarianceVoxel *cov, glm::dquat *rotation, glm::dvec3 *scale);

/// @ingroup voxelcovariance
/// Unpack @c cov.trianglar_covariance into a 3x3 covariance matrix.
inline glm::dmat3 covarianceSqrtMatrix(const CovarianceVoxel *cov)
{
  glm::dmat3 cov_sqrt_mat;

  glm::dvec3 *col = &cov_sqrt_mat[0];
  (*col)[0] = cov->trianglar_covariance[0];
  (*col)[1] = cov->trianglar_covariance[1];
  (*col)[2] = cov->trianglar_covariance[3];

  col = &cov_sqrt_mat[1];
  (*col)[0] = 0;
  (*col)[1] = cov->trianglar_covariance[2];
  (*col)[2] = cov->trianglar_covariance[4];

  col = &cov_sqrt_mat[2];
  (*col)[0] = 0;
  (*col)[1] = 0;
  (*col)[2] = cov->trianglar_covariance[5];  // NOLINT(readability-magic-numbers)

  return cov_sqrt_mat;
}

/// @ingroup voxelcovariance
/// Unpack @c cov.trianglar_covariance into a 3x3 covariance matrix.
inline glm::dmat3 covarianceMatrix(const CovarianceVoxel *cov)
{
  const glm::dmat3 cov_sqrt_mat = covarianceSqrtMatrix(cov);
  return cov_sqrt_mat * glm::transpose(cov_sqrt_mat);
}

#if OHM_COV_DEBUG
void covDebugStats();
#endif  // OHM_COV_DEBUG

/// Integrate a hit result for a single voxel of @p map with NDT or NDT-TM support. The NDT-TM is used when
/// @p ndt_tm is true and and the layers @c default_layer::intensityLayerName() and
/// @c default_layer::hitMissCountLayerName() layers are available to update @c IntensityMeanCov and @c HitMissCount
/// respectively.
///
/// @param map The @c NdtMap to integrate the hit for.
/// @param key The key for the voxel to modify. This is the voxel containing @p sample
/// @param sensor The sensor location from which the @p sample was attained.
/// @param sample The global sample coordinate.
/// @param ndt_tm True to use NDT-TM logic.
/// @param sample_intensity The intensity value of the lidar sample required for NDT-TM.
void ohm_API integrateNdtHit(NdtMap &map, const Key &key, const glm::dvec3 &sensor, const glm::dvec3 &sample,
                             bool ndt_tm = false, float sample_intensity = 0.0f);

/// Integrate a miss result for a single voxel of @p map with NDT or NDT-TM support. The NDT-TM is used when
/// @p ndt_tm is @c true and the map has a @c default_layer::hitMissCountLayerName() layer for @c HitMissCount data.
///
/// @param map The @c NdtMap to integrate the hit for.
/// @param key The key for the voxel to modify. This is a voxel along the line segment @p sensor to @p sample , but
/// not the voxel containing @p sample .
/// @param sensor The sensor location from which the @p sample was attained.
/// @param sample The global sample coordinate.
/// @param ndt_tm True to use NDT-TM logic.
void ohm_API integrateNdtMiss(NdtMap &map, const Key &key, const glm::dvec3 &sensor, const glm::dvec3 &sample,
                              bool ndt_tm = false);
}  // namespace ohm

#endif  // COVARIANCEVOXEL_H


================================================
FILE: ohm/CovarianceVoxelCompute.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef COVARIANCEVOXELCOMPUTE_H
#define COVARIANCEVOXELCOMPUTE_H

// Note: this header is included in GPU code.

#if GPUTIL_DEVICE
// Define GPU type aliases
typedef float3 CovVec3;
typedef float CovReal;
typedef uint uint32_t;

// Vector support functions
inline __device__ CovReal covdot(const CovVec3 a, const CovVec3 b)
{
  return dot(a, b);
}
inline __device__ CovReal covlength2(const CovVec3 v)
{
  return dot(v, v);
}
inline __device__ CovVec3 covnormalize(const CovVec3 v)
{
  return normalize(v);
}

#else  // GPUTIL_DEVICE
// Define CPU type aliases
using CovVec3 = glm::dvec3;
using CovReal = double;


inline CovReal covdot(const CovVec3 &a, const CovVec3 &b)
{
  return glm::dot(a, b);
}
inline CovReal covlength2(const CovVec3 &v)
{
  return glm::length2(v);
}
inline CovVec3 covnormalize(const CovVec3 &v)
{
  return glm::normalize(v);
}
#ifndef __device__
#define __device__
#endif  //  __device__
#endif  // GPUTIL_DEVICE

/// @ingroup voxelcovariance/// Defines the covariance voxel structure. This is stored as a triangular square root
/// covariance matrix to reduce memory storage.
typedef struct CovarianceVoxel_t  // NOLINT(readability-identifier-naming, modernize-use-using)
{
  /// Trianglar square root covariance matrix. Represents a covariance matrix via the triangular
  /// square root matrix, P = S * S^T.
  /// | cov[0]  |      .  |      .  |
  /// | cov[1]  | cov[2]  |      .  |
  /// | cov[3]  | cov[4]  | cov[5]  |
  float trianglar_covariance[6];  // NOLINT(readability-magic-numbers, modernize-avoid-c-arrays)
} CovarianceVoxel;

/// Data stucture tracking intensity information for a voxel. This is used for NDT-TM.
typedef struct IntensityMeanCov_t  // NOLINT(readability-identifier-naming, modernize-use-using)
{
  /// Current intensity mean (approximate)
  float intensity_mean;
  /// Current intensity covariance (approximate)
  float intensity_cov;
} IntensityMeanCov;

/// Data structure tacking the number of hits and misses on a voxel.
typedef struct HitMissCount_t  // NOLINT(readability-identifier-naming, modernize-use-using)
{
  /// Number of hits recoreded
  uint32_t hit_count;
  /// Number of misses recoreded
  uint32_t miss_count;
} HitMissCount;

/// @ingroup voxelcovariance
/// Initialise the packed square root covariance matrix in @p cov
/// The covariance value is initialised to an identity matrix, scaled by the @p voxel_resolution .
/// This is to ensure we do not start with a zero matrix, which causes all sorts of mathematical problems.
/// @param[out] cov The @c CovarianceVoxel to initialse.
/// @param voxel_resolution The voxel resolution, which is used to seed the covariance.
inline __device__ void initialiseCovariance(CovarianceVoxel *cov, float voxel_resolution)
{
  const float covariance_scale_factor = 0.1f;
  // Initialise the square root covariance matrix to a scaled identity matrix.
  // NOLINTNEXTLINE(readability-magic-numbers)
  cov->trianglar_covariance[0] = cov->trianglar_covariance[2] = cov->trianglar_covariance[5] =
    covariance_scale_factor * voxel_resolution;
  cov->trianglar_covariance[1] = cov->trianglar_covariance[3] = cov->trianglar_covariance[4] = 0;
}

/// @ingroup voxelcovariance
/// dot product of j-th and k-th columns of A
/// A is (4,3), assumed to be packed as follows, where z is non-represented zero
/// 0 1 3
/// z 2 4
/// z z 5
/// 6 7 8
inline __device__ CovReal packedDot(const CovReal A[9],  // NOLINT
                                    const int j, const int k)
{
  const int col_first_el[] = { 0, 1, 3 };  // NOLINT(modernize-avoid-c-arrays)
  const int indj = col_first_el[j];
  const int indk = col_first_el[k];
  const int m = (j <= k) ? j : k;
  CovReal d = A[6 + k] * A[6 + j];  // // NOLINT(readability-magic-numbers)
  for (int i = 0; i <= m; ++i)
  {
    d += A[indj + i] * A[indk + i];
  }
  return d;
}


/// @ingroup voxelcovariance
/// Unpack the covariance matrix storage.
///
/// Unpacks covariance matrix square root and mean into the following sparse 3,4 form:
///
/// |         |         |         |
/// | ------- | ------- | ------- |
/// | cov[0]  | cov[1]  | cov[3]  |
/// | .       | cov[2]  | cov[4]  |
/// | .       | .       | cov[5]  |
/// | mean[0] | mean[1] | mean[2] |
///
/// With corresponding layout in the output @p matrix.
///
/// |   |   |   |
/// | - | - | - |
/// | 0 | 1 | 3 |
/// | . | 2 | 4 |
/// | . | . | 5 |
/// | 6 | 7 | 8 |
///
/// Items marked `cov[n]` are extracted from the @c cov->trianglar_covariance, while `mean[n]` items are derived from
/// @p sample_to_mean . Items marked '.' are not represented in the martix and are treated as zero.
/// Note that the extracted values also have a co-efficient applied based on the @p point_count .
///
/// @param cov Covariance details of the voxel in question.
/// @param point_count Number of samples which have been used to generate the @c cov and voxel mean.
/// @param sample_to_mean The difference between the new sample point and the voxel mean.
/// @param[out] matrix The matrix to unpack to. This is an array of 9 elements.
inline __device__ void unpackCovariance(const CovarianceVoxel *cov, unsigned point_count, const CovVec3 sample_to_mean,
                                        CovReal *matrix)
{
  // NOLINTNEXTLINE(google-readability-casting)
  const CovReal one_on_num_pt_plus_one = (CovReal)1 / (point_count + (CovReal)1);
  // NOLINTNEXTLINE(google-readability-casting)
  const CovReal sc_1 = point_count ? sqrt(point_count * one_on_num_pt_plus_one) : (CovReal)1;
  // NOLINTNEXTLINE(google-readability-casting)
  const CovReal sc_2 = one_on_num_pt_plus_one * sqrt((CovReal)point_count);

  for (int i = 0; i < 6; ++i)  // NOLINT(readability-magic-numbers)
  {
    matrix[i] = sc_1 * cov->trianglar_covariance[i];
  }

  matrix[0 + 6] = sc_2 * sample_to_mean.x;  // NOLINT(readability-magic-numbers)
  matrix[1 + 6] = sc_2 * sample_to_mean.y;  // NOLINT(readability-magic-numbers)
  matrix[2 + 6] = sc_2 * sample_to_mean.z;  // NOLINT(readability-magic-numbers)
}

/// @ingroup voxelcovariance
/// Find x for Mx = y, given lower triangular M where M is @c trianglar_covariance
/// Storage order for M:
///
/// @code{.unparsed}
/// 0 z z
/// 1 2 z
/// 3 4 5
/// @endcode
/// @param cov Covariance details of the voxel in question.
/// @param y The vector to solve for.
inline __device__ CovVec3 solveTriangular(const CovarianceVoxel *cov, const CovVec3 y)
{
  // Note: if we generate the voxel with point on a perfect plane, say (0, 0, 1, 0), then do this operation,
  // we get a divide by zero. We avoid this by seeding the covariance matrix with an identity matrix scaled
  // by the sensor noise (see initialiseCovariance()).
  CovVec3 x;
  CovReal d;

  d = y.x;
  x.x = d / cov->trianglar_covariance[0];

  d = y.y;
  d -= cov->trianglar_covariance[1 + 0] * x.x;
  x.y = d / cov->trianglar_covariance[1 + 1];

  d = y.z;
  d -= cov->trianglar_covariance[3 + 0] * x.x;
  d -= cov->trianglar_covariance[3 + 1] * x.y;
  x.z = d / cov->trianglar_covariance[3 + 2];

  return x;
}

/// @ingroup voxelcovariance
/// Calculate the two likelihoods required for Normalised Distribution Transform (NDT) update logic.
///
/// This algorithm is based on the following paper:
/// > 3D normal distributions transform occupancy maps: An efficient representation for mapping in dynamic
/// > environments
/// > Jari P. Saarinen, Henrik Andreasson, Todor Stoyanov and Achim J. Lilienthal
///
/// This improves the probably adjustment for a voxel using the voxel covariance (if present). This only takes effect
/// when there have been samples collected for the voxel and `point_count &gt; 0`. The standard occupancy adjustment
/// is used whenever the `point_count &lt; sample_threshold`, with @p miss_value added to @p voxel_value or
/// @p voxel_value set to @p miss_value when @p voxel_value equals @p uninitialised_value .
///
/// @param cov_voxel The packed covariance for the voxel.
/// @param sensor The location of the sensor from where the sample was taken.
/// @param sample The sample position.
/// @param voxel_mean The current accumulated mean position within the voxel. Only valid if @p point_count is &gt; 0.
/// @param sensor_noise The sensor range noise error (standard deviation). Must be greater than zero.
/// @param[out] p_x_ml_given_voxel is the likelihood p(x_ML|N(u,P))
/// @param[out] p_x_ml_given_sample is the likelihood p(x_ML|z_i)
/// @return The point where the ray has the highest likelihood in the voxel Gaussian, x_ML
inline __device__ CovVec3 calculateSampleLikelihoods(const CovarianceVoxel *cov_voxel, CovVec3 sensor, CovVec3 sample,
                                                     CovVec3 voxel_mean, float sensor_noise,
                                                     CovReal *p_x_ml_given_voxel, CovReal *p_x_ml_given_sample)
{
#if GPUTIL_DEVICE
  // Must explicitly use single precision values for OpenCL 2.0+ or it will require double precision compile options.
  const CovReal kHalf = 0.5f;
#else   // GPUTIL_DEVICE
  const CovReal kHalf = 0.5;
#endif  // GPUTIL_DEVICE
  // Bracketed numbers below reference equation numbers in the paper mentioned in the function comments.
  const CovVec3 sensor_to_sample = sample - sensor;
  const CovVec3 sensor_ray = covnormalize(sensor_to_sample);  // Verified
  const CovVec3 mean_to_sensor = sensor - voxel_mean;

  // Packed data solutions:
  const CovVec3 a = solveTriangular(cov_voxel, sensor_ray);
  const CovVec3 b_norm = solveTriangular(cov_voxel, mean_to_sensor);

  // const CovVec3 a = covariance_inv * sensor_ray;  // Verified (unpacked version)
  // (28)
  // const CovReal t = covdot(a, mean_to_sensor) / covdot(a, sensor_ray); // Verified (unpacked version)
  const CovReal t = -covdot(a, b_norm) / covdot(a, a);  // Verified

  // (25)
  // Note: maximum_likelihood is abbreviated to ml in associated variable names.
  const CovVec3 voxel_maximum_likelihood = sensor_ray * t + sensor;  // Verified

  // (22)
  // const CovReal p_x_ml_given_voxel = std::exp(
  //   -0.5 * covdot(voxel_maximum_likelihood - voxel_mean, covariance_inv * (voxel_maximum_likelihood -
  //   voxel_mean)));
  // Corrected:
  *p_x_ml_given_voxel = exp(-kHalf * covlength2(solveTriangular(cov_voxel, voxel_maximum_likelihood - voxel_mean)));

  // (23)
  const CovReal sensor_noise_variance = sensor_noise * sensor_noise;
  *p_x_ml_given_sample = exp(-kHalf * covlength2(voxel_maximum_likelihood - sample) / sensor_noise_variance);

  return voxel_maximum_likelihood;
}


/// @ingroup voxelcovariance
/// Calculate a voxel hit with packed covariance. This supports Normalised Distribution Transform (NDT) logic in
/// @c calculateHitNdt() .
///
/// The square root covariance in @p cov_voxel and occupancy in @p voxel_value are both updated, but the voxel mean
/// calculation is not performed here. However, it is expected that the voxel mean will be updated after this call and
/// the @c point_count incremented, otherwise future behaviour is undefined.
///
/// The @p cov_voxel may be zero and is fully initialised when the @p point_count is zero, implying this is the first
/// hit. It will also be reinitialised whenever the @p voxel_value is below the the @p reinitialise_threshold and the
/// @p point_count is above @p reinitialise_sample_count .
///
/// This reinitialisation is to handle situations where a voxel may have been occupied by a transient object, become
/// free, then becomes occupied once more. In this case, the new occupancy covariance may differ and should disregard
/// the previous covariance and mean. The @c reinitialise_threshold is used as the primary trigger to indicate previous
/// data may be invalid while the @c reinitialise_sample_count is intended to prevent repeated reintialisation as the
/// probablity value may oscillate around the threshold.
///
/// @param[in,out] cov_voxel The packed covariance to update for the voxel being updated.
/// @param[in,out] voxel_value The occupancy value for the voxel being updated.
/// @param sample The sample which falls in this voxel.
/// @param voxel_mean The current accumulated mean position within the voxel. Only valid if @p point_count is &gt; 0.
/// @param point_count The number of samples which have been used to generate the @p voxel_mean and @p cov_voxel.
/// @param hit_value The log probability value increase for occupancy on a hit. This must be greater than zero to
/// increase the voxel occupancy probability.
/// @param uninitialised_value The @p voxel_value for an uncertain voxel - one which has yet to be observed.
/// @param voxel_resolution The voxel size along each cubic edge.
/// @param reinitialise_threshold @p voxel_value threshold below which the covariance and mean should reset.
/// @param reinitialise_sample_count The @p point_count required to allow @c reinitialise_threshold to be triggered.
/// @return True if the covariance value is re-initialised. This should be used as a signal to diregard the current
///     @p voxel_mean and @c point_count and restart accumulating those values.
inline __device__ bool calculateHitWithCovariance(CovarianceVoxel *cov_voxel, float *voxel_value, CovVec3 sample,
                                                  CovVec3 voxel_mean, unsigned point_count, float hit_value,
                                                  float uninitialised_value, float voxel_resolution,
                                                  float reinitialise_threshold, unsigned reinitialise_sample_count)
{
  const float initial_value = *voxel_value;
  const bool was_uncertain = initial_value == uninitialised_value;
  bool initialised_covariance = false;

  if (point_count == 0 || (initial_value < reinitialise_threshold && point_count >= reinitialise_sample_count))
  {
    initialiseCovariance(cov_voxel, voxel_resolution);
    initialised_covariance = true;
    // Reinitialising co-variance. We need to ensure the point count is zero when we reset the covariance to
    // correctly calculate the covariance.
    point_count = 0;
  }

  // Initialise the cov_voxel data if this transitions the voxel to an occupied state (the voxel may have already had
  // misses so we could have was_uncertain==false && point_count==0).
  *voxel_value = (!was_uncertain) ? hit_value + initial_value : hit_value;

  // Code represents covariance via square root matrix, i.e., covariance P = C * C^T
  // Let old covariance be P, new covariance Pnew, old mean mu, new point z
  // The required update for the covariance is
  //   Pnew = num_pt/(num_pt + 1)*P + num_pt/(num_pt+1)^2 * (z-mu)(z-mu)^T
  // This code implements that update directly via a matrix square root by forming the matrix A
  // such that A^T A = Pnew. A is not square, so a modified Gram-Schmidt decomposition is utilised
  // to find the triangular square root matrix Cnew such that Pnew = Cnew Cnew^T
  // Reference: Maybeck 1978 Stochastic Models, Estimation and Control, vol 1, p381
  // https://www.sciencedirect.com/bookseries/mathematics-in-science-and-engineering/vol/141/part/P1

  // sample_to_mean should be zero when we (re)initialise the covariance.
  // We use sample - sample rather than a hard zero due to API differences between CPU and GPU code.
  const CovVec3 sample_to_mean = (!initialised_covariance) ? sample - voxel_mean :
  // clang-format off
#if !defined(GPUTIL_DEVICE) || GPUTIL_DEVICE == 0
                                                             CovVec3(0, 0, 0)
#else   // !defined(GPUTIL_DEVICE) || GPUTIL_DEVICE == 0
                                                             make_float3(0, 0, 0)
#endif  // !defined(GPUTIL_DEVICE) || GPUTIL_DEVICE == 0
  ;
  // clang-format on
  CovReal unpacked_covariance[9];  // NOLINT(readability-magic-numbers, modernize-avoid-c-arrays)
  unpackCovariance(cov_voxel, point_count, sample_to_mean, unpacked_covariance);

  // Update covariance.
  for (int k = 0; k < 3; ++k)
  {
    // NOLINTNEXTLINE(hicpp-signed-bitwise)
    const int ind1 = (k * (k + 3)) >> 1;  // packed index of (k,k) term
    const int indk = ind1 - k;            // packed index of (1,k)
    const CovReal ak = sqrt(packedDot(unpacked_covariance, k, k));
    cov_voxel->trianglar_covariance[ind1] = (float)ak;  // NOLINT(google-readability-casting)
    if (ak > 0)
    {
      const CovReal aki = (CovReal)1 / ak;  // NOLINT(google-readability-casting)
      for (int j = k + 1; j < 3; ++j)
      {
        const int indj = (j * (j + 1)) >> 1;  // NOLINT(hicpp-signed-bitwise)
        const int indkj = indj + k;
        CovReal c = packedDot(unpacked_covariance, j, k) * aki;
        cov_voxel->trianglar_covariance[indkj] = (float)c;  // NOLINT(google-readability-casting)
        c *= aki;
        unpacked_covariance[j + 6] -= c * unpacked_covariance[k + 6];  // NOLINT(readability-magic-numbers)
        for (int l = 0; l <= k; ++l)
        {
          unpacked_covariance[indj + l] -= c * unpacked_covariance[indk + l];
        }
      }
    }
  }

  return initialised_covariance;
}

/// @ingroup voxelcovariance
/// Calculate the update to a voxel's intensity distribution using a new hit. This supports Normalised Distribution
/// Transform-Traversability Map (NDT-TM) logic in @c calculateHitNdt() .
///
/// @param[in,out] intensity The intensity mean and covariance for the voxel being updated.
/// @param voxel_value The occupancy value for the voxel being updated.
/// @param intensity_sample The intensity measurement of the sample that falls in this voxel.
/// @param initial_intensity_covariance The covariance to utilise in voxel when initialising with a new sample.
/// @param point_count The number of samples which have been used to generate the @p intensity_mean_voxel and @p
/// intensity_cov_voxel.
/// @param reinitialise_threshold @p voxel_value threshold below which the covariance and mean should reset.
/// @param reinitialise_sample_count The @p point_count required to allow @c reinitialise_threshold to be triggered.
/// @return True if the covariance value is re-initialised. This should be used as a signal to diregard the current
///     @p voxel_mean and @c point_count and restart accumulating those values.
inline __device__ void calculateIntensityUpdateOnHit(IntensityMeanCov *intensity, float voxel_value,
                                                     float intensity_sample, float initial_intensity_covariance,
                                                     unsigned point_count, float reinitialise_threshold,
                                                     unsigned reinitialise_sample_count)
{
  // Branchless for better GPU execution.
  const float initial_value = voxel_value;
  const bool needs_reset =
    point_count == 0 || (initial_value < reinitialise_threshold && point_count >= reinitialise_sample_count);
  const float delta = intensity->intensity_mean - intensity_sample;
  const float point_count_float = (float)point_count;  // NOLINT
  float inv_point_count_plus_one = 1.0f / (point_count_float + 1.0f);
  intensity->intensity_mean =
    (!needs_reset) ?  //
      inv_point_count_plus_one * (point_count_float * intensity->intensity_mean + intensity_sample) :
      intensity_sample;
  intensity->intensity_cov = (!needs_reset) ?  //
                               inv_point_count_plus_one * (point_count_float * intensity->intensity_cov +
                                                           inv_point_count_plus_one * delta * delta) :
                               initial_intensity_covariance;
}


/// @ingroup voxelcovariance
/// Update voxel hit/miss count upon hit. This supports Normalised Distribution Transform-Traversability Map (NDT-TM)
/// logic in @c calculateMissNdt() . The update is nontrivial as it selectively updates hit or miss depending on whether
/// the return is consistent with the Gaussian already in the voxel. Assumes  @c calculateHitWithCovariance() has just
/// been called.
///
/// The @p cov_voxel may be zero and is fully initialised when the @p point_count is zero, implying this is the first
/// hit. It will also be reinitialised whenever the @p voxel_value is below the the @p reinitialise_threshold and the
/// @p point_count is above @p reinitialise_sample_count .
///
/// This reinitialisation is to handle situations where a voxel may have been occupied by a transient object, become
/// free, then becomes occupied once more. In this case, the new occupancy covariance may differ and should disregard
/// the previous covariance and mean. The @c reinitialise_threshold is used as the primary trigger to indicate previous
/// data may be invalid while the @c reinitialise_sample_count is intended to prevent repeated reinitialisation as the
/// probability value may oscillate around the threshold.
///
/// @param[in,out] cov_voxel The packed covariance to update for the voxel being updated.
/// @param voxel_value The occupancy value for the voxel being updated.
/// @param[in,out] hit_miss_count Tracks the number of hits and misses in the current voxel.
/// @param sensor The position of the sensor or ray origin from which the sample was measured.
/// @param sample The sample which falls in this voxel.
/// @param voxel_mean The current accumulated mean position within the voxel. Only valid if @p point_count is &gt; 0.
/// @param point_count The number of samples which have been used to generate the @p voxel_mean and @p cov_voxel.
/// @param uninitialised_value The @p voxel_value for an uncertain voxel - one which has yet to be observed.
/// @param reinitialise_permeability_with_covariance If true the voxel permeability is reinitialised any time the
/// covariance is reinitialised. Otherwise, it is only initialised on first observation.
/// @param adaptation_rate The adaptation rate for ellipsoid intersections. This value is given [0, 1], but it's
/// usage sees it scaled [0, 0.5].
/// @param sensor_noise The sensor range noise error (standard deviation). Must be greater than zero.
/// @param reinitialise_threshold @p voxel_value threshold below which the covariance and mean should reset.
/// @param reinitialise_sample_count The @p point_count required to allow @c reinitialise_threshold to be triggered.
/// @param sample_threshold The @p point_count required before using NDT logic, i.e., before the covariance value is
/// usable.
inline __device__ void calculateHitMissUpdateOnHit(CovarianceVoxel *cov_voxel, float voxel_value,
                                                   HitMissCount *hit_miss_count, CovVec3 sensor, CovVec3 sample,
                                                   CovVec3 voxel_mean, unsigned point_count, float uninitialised_value,
                                                   bool reinitialise_permeability_with_covariance,
                                                   float adaptation_rate, float sensor_noise,
                                                   float reinitialise_threshold, unsigned reinitialise_sample_count,
                                                   unsigned sample_threshold)
{
#if GPUTIL_DEVICE
  // Must explicitly use single precision values for OpenCL 2.0+ or it will require double precision compile options.
  const CovReal kHalf = 0.5f;
#else   // GPUTIL_DEVICE
  const CovReal kHalf = 0.5;
#endif  // GPUTIL_DEVICE
  // Branchless for better GPU execution
  const bool needs_reset =
    voxel_value == uninitialised_value ||
    (reinitialise_permeability_with_covariance &&
     (point_count == 0 || (voxel_value < reinitialise_threshold && point_count >= reinitialise_sample_count)));

  const unsigned initial_hit = (!needs_reset) ? hit_miss_count->hit_count : 0;
  const unsigned initial_miss = (!needs_reset) ? hit_miss_count->miss_count : 0;

  CovReal p_x_ml_given_voxel;
  CovReal p_x_ml_given_sample;
  // FIXME(KS): This is calculated both here and in calculateMissNdt(). We need only calculate once and share the
  // results
  calculateSampleLikelihoods(cov_voxel, sensor, sample, voxel_mean, sensor_noise, &p_x_ml_given_voxel,
                             &p_x_ml_given_sample);
  const CovReal prod = p_x_ml_given_voxel * p_x_ml_given_sample;
  const CovReal eta = kHalf * adaptation_rate;  // NOLINT

  const bool inc_hit =
    needs_reset || point_count < sample_threshold || (point_count >= sample_threshold && prod >= eta);
  const bool inc_miss = !needs_reset && point_count >= sample_threshold && prod < eta && p_x_ml_given_voxel >= eta;

  // Logically we should yield the following results:
  // 1. needs_reset is true:
  //    - initial_hit = 0
  //    - initial_miss = 0
  //    => hit_count = 1
  //    => miss_count = 0
  // 2. needs_reset is false and point_count < sample_threshold
  //    - initial_hit = hit_miss_count->hit_count
  //    - initial_miss = hit_miss_count->miss_count
  //    - hit_count increment_condition is true
  //    - miss_count increment_condition is false
  //    -> ++hit_count
  // 3. needs_reset is false and point_count >= sample_threshold
  //    - initial_hit = hit_miss_count->hit_count
  //    - initial_miss = hit_miss_count->miss_count
  //    - hit_count increment_condition true if (prod >= eta)
  //    - miss_count increment_condition true if (prod < eta && p_x_ml_given_voxel >= eta)
  //    -> hit_count may increment
  //    -> miss_count may increment

  hit_miss_count->hit_count = initial_hit + (inc_hit ? 1 : 0);
  hit_miss_count->miss_count = initial_miss + (inc_miss ? 1 : 0);
}


/// @ingroup voxelcovariance
/// Calculate a voxel miss (ray passthrough) using Normalised Distribution Transform (NDT) logic.
///
/// This algorithm is based on the following paper:
/// > 3D normal distributions transform occupancy maps: An efficient representation for mapping in dynamic
/// > environments
/// > Jari P. Saarinen, Henrik Andreasson, Todor Stoyanov and Achim J. Lilienthal
///
/// This improves the probably adjustment for a voxel using the voxel covariance (if present). This only takes effect
/// when there have been samples collected for the voxel and `point_count &gt; 0`. The standard occupancy adjustment
/// is used whenever the `point_count &lt; sample_threshold`, with @p miss_value added to @p voxel_value or
/// @p voxel_value set to @p miss_value when @p voxel_value equals @p uninitialised_value .
///
/// Note the @p adaptation_rate affects the strength of the NDT probability adjustment on a miss. This should generally
/// be configured to be in proportion with the probability corresponding to the @p miss_value else the NDT erosion
/// effects can be more significant than those of using pure occupancy. For suggested value initialisation see
/// @c NdtMap::ndtAdaptationRateFromMissProbability() .
///
/// @param cov_voxel The packed covariance for the voxel. Only used if the `point_count &gt; sample_threshold`.
/// @param[in,out] voxel_value The current voxel log probably value.
/// @param[out] is_miss Set to true if the ray misses the estimated voxel shape (or there is insufficient data yet).
/// @param sensor The location of the sensor from where the sample was taken.
/// @param sample The sample position.
/// @param voxel_mean The current accumulated mean position within the voxel. Only valid if @p point_count is &gt; 0.
/// @param point_count The number of samples which have been used to generate the @p voxel_mean and @p cov_voxel.
/// @param uninitialised_value The @p voxel_value for an uncertain voxel - one which has yet to be observed.
/// @param miss_value The @p voxel_value adjustment to apply from a miss. This must be less than zero to decrease the
/// occupancy probability. The NDT scaling factor is also derived from this value.
/// @param adaptation_rate The adaptation rate for ellipsoid intersections. This value is given [0, 1], but it's
/// usage sees it scaled [0, 0.5] as values greater than 0.5 would yield negative probabilities.
/// @param sensor_noise The sensor range noise error (standard deviation). Must be greater than zero.
/// @param sample_threshold The @p point_count required before using NDT logic, i.e., before the covariance value is
/// usable.
/// @return The point where the ray has the highest likelihood in the voxel Gaussian, x_ML
inline __device__ CovVec3 calculateMissNdt(const CovarianceVoxel *cov_voxel, float *voxel_value, bool *is_miss,
                                           CovVec3 sensor, CovVec3 sample, CovVec3 voxel_mean, unsigned point_count,
                                           float uninitialised_value, float miss_value, float adaptation_rate,
                                           float sensor_noise, unsigned sample_threshold)
{
#if GPUTIL_DEVICE
  // Must explicitly use single precision values for OpenCL 2.0+ or it will require double precision compile options.
  const CovReal kOne = 1.0f;
  const CovReal kHalf = 0.5f;
#else   // GPUTIL_DEVICE
  const CovReal kOne = 1.0;
  const CovReal kHalf = 0.5;
#endif  // GPUTIL_DEVICE
  // Bracketed numbers below reference equation numbers in the paper mentioned in the function comments.
  if (*voxel_value == uninitialised_value)
  {
    // First touch of the voxel. Apply the miss value as is.
    // Same behaviour as OccupancyMap.
    *voxel_value = miss_value;
    *is_miss = true;
    return voxel_mean;
  }

  // Direct value adjustment if not occupied or insufficient samples.
  if (point_count < sample_threshold)
  {
    // Re-enforcement of free voxel or too few points to resolve a Gaussian. Use standard value update.
    // Add miss value, same behaviour as OccupancyMap.
    *voxel_value += miss_value;
    // ++hit_miss_count->miss_count;
    *is_miss = true;
    return voxel_mean;
  }

  // Update of an occupied voxel. We have to unpack the covariance and apply NDT logic.

  // Notes:
  // - Equation references are in relation to the paper on which this is based (see class comments).
  // - Variable subscripts are denoted by '_<subscript>'; e.g., "z subscript i" is written "z_i".
  // - A transpose is denoted by [T]
  // - Ordinals are denoted by [#]; e.g.,
  //    - [-1] -> inverse
  //    - [2] -> square
  // - The paper used capital Sigma for the covariance matrix. We use P.
  //
  // Goal is to calculate equation (24)
  // p(m_k = 1|z_i) = 0.5 - eta p(x_ML|N(u,P)) (1 - p(x_ML|z_i))  (24)
  // We have already established we have sufficient points for a Gaussian.

  // p(x_ML|N(u,P)) ~ exp( -0.5(x_ML - u)[T] P[-1](x_ML - u))     (22)
  // Where know values are:
  //  - u existing mean voxel position (voxel mean position)
  //  - P is the covariance matrix.
  //  - z_i is the sample
  // To be calculated:
  // - x_ML

  // p(x_ML|z_i) ~ exp( -0.5 || x_ML - z_i ||[2] / s_s[2] )       (23)
  // Where:
  // - s_s is the sensor noise

  // x_ML = l.t + l_0                                             (25)
  // Know:
  // - l : sensor ray = (sample - sensor) / ||sample - sensor||
  // - l_0 : sensor position

  // t =  a_x b_x + a_y b_y + a_z b_z /                           (28)
  //      a_x l_x + a_y l_y + a_z l-z
  //
  // a = P[-1] l
  // b = (l_0 - u)

  CovReal p_x_ml_given_voxel;
  CovReal p_x_ml_given_sample;
  const CovVec3 voxel_maximum_likelihood = calculateSampleLikelihoods(
    cov_voxel, sensor, sample, voxel_mean, sensor_noise, &p_x_ml_given_voxel, &p_x_ml_given_sample);

  const CovReal scaling_factor = kHalf * adaptation_rate;
  const CovReal prod = p_x_ml_given_voxel * (kOne - p_x_ml_given_sample);
  const CovReal probability_update = kHalf - scaling_factor * prod;

  // NDT-TM update of miss count
  *is_miss = prod < scaling_factor;

  // Check for NaN
  // This should no longer be occurring.
  if (probability_update == probability_update)
  {
    // Convert the probability to a log value.
    *voxel_value += (float)log(probability_update / (kOne - probability_update));  // NOLINT(google-readability-casting)
  }

  return voxel_maximum_likelihood;
}

#endif  // COVARIANCEVOXEL_H


================================================
FILE: ohm/DataType.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "DataType.h"

#include <array>

namespace ohm
{
const char *DataType::name(unsigned type)
{
  static const std::array<const char *, 12> names =  //
    {
      "<unknown>",  //
      "int8_t",     //
      "uint8_t",    //
      "int16_t",    //
      "uint16_t",   //
      "int32_t",    //
      "uint32_t",   //
      "int64_t",    //
      "uint64_t",   //
      "float",      //
      "double",     //
      "struct"      //
    };

  if (type < kUser)
  {
    if (type < names.size())
    {
      return names[type];
    }

    return "<invalid>";
  }

  return "user";
}


size_t DataType::size(unsigned type)
{
  static const std::array<size_t, 12> sizes =  //
    {
      0u,  // unknown
      1u,  // int8
      1u,  // uint8
      2u,  // int16
      2u,  // uin16
      4u,  // int32
      4u,  // uint32
      8u,  // int64
      8u,  // uint64
      4u,  // float
      8u,  // double
      0u   // struct
    };

  if (type < kUser)
  {
    if (type < sizes.size())
    {
      return sizes[type];
    }

    return 0u;
  }

  return 0u;
}
}  // namespace ohm


================================================
FILE: ohm/DataType.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_DATATYPE_H
#define OHM_DATATYPE_H

#include "OhmConfig.h"

namespace ohm
{
/// Data type enumeration, particularly for @c VoxelLayout data.
struct ohm_API DataType
{
  /// Enumeration of the supported data types.
  enum Type
  {
    kUnknown,
    kInt8,
    kUInt8,
    kInt16,
    kUInt16,
    kInt32,
    kUInt32,
    kInt64,
    kUInt64,
    kFloat,
    kDouble,
    // NYI: May implement in future to support more interesting voxel structures.
    kStruct,

    kUser = 256
  };

  /// Returns a string representation of @p type.
  /// @param type The data type value to convert to string.
  /// @return A string name for @p type or "&lt;invalid&gt;" if @p type is invalid. Any user type is "user".
  static const char *name(unsigned type);

  /// Returns the byte size of @p type if known.
  /// @param type The data type value to return a byte size for.
  /// @return The byte size of @p type or zero if the size is unknown, variable or @p type is invalid.
  static size_t size(unsigned type);
};
}  // namespace ohm

#endif  // OHM_DATATYPE_H


================================================
FILE: ohm/DebugDraw.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "DebugDraw.h"

#include "OccupancyMap.h"
#include "Trace.h"
#include "VoxelData.h"

#include <3esservermacros.h>

#include <glm/gtc/type_ptr.hpp>

#ifdef TES_ENABLE
namespace
{
template <typename ShapeType>
void sendShapes(std::vector<ShapeType> &shapes, std::vector<tes::Shape *> &shape_ptrs)
{
  if (!shapes.empty() && ohm::g_tes)
  {
    shape_ptrs.clear();
    for (auto &ellipsoid : shapes)
    {
      shape_ptrs.emplace_back(&ellipsoid);
    }

    ohm::g_tes->create(tes::MultiShape(shape_ptrs.data(), shape_ptrs.size()));
    shapes.clear();
  }
}
}  // namespace
#endif  // TES_ENABLE

namespace ohm
{
bool debugDraw(const ohm::OccupancyMap &map)
{
#ifdef TES_ENABLE
  if (map.layout().intensityLayer() >= 0 && map.layout().hitMissCountLayer() >= 0)
  {
    return debugDrawNdtTm(map);
  }
  if (map.layout().covarianceLayer() >= 0)
  {
    return debugDrawNdtOm(map);
  }
  // Occupancy only
  return debugDrawOccupancy(map);
#else   // TES_ENABLE
  (void)map;
  return false;
#endif  // TES_ENABLE
}


bool debugDrawNdtTm(const ohm::OccupancyMap &map)
{
  if (!trace::available())
  {
    return false;
  }
#ifdef TES_ENABLE
  if (!g_tes || g_tes->connectionCount() == 0)
  {
    // No connections. Nothing to send data to, so do no work.
    return true;
  }

  // NDT-TM
  static float min_intensity = 0.0f, max_intensity = 255.0f;
  auto next_id = static_cast<uint32_t>(size_t(&map));
  Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  Voxel<const CovarianceVoxel> cov(&map, map.layout().covarianceLayer());
  Voxel<const VoxelMean> mean(&map, map.layout().meanLayer());
  Voxel<const IntensityMeanCov> intensity_voxel(&map, map.layout().intensityLayer());
  Voxel<const HitMissCount> hit_miss_voxel(&map, map.layout().hitMissCountLayer());

  if (!occupancy.isLayerValid() || !cov.isLayerValid() || !mean.isLayerValid() || !intensity_voxel.isLayerValid() ||
      !hit_miss_voxel.isLayerValid())
  {
    return false;
  }

  std::vector<tes::Sphere> ellipsoids;
  std::vector<tes::Shape *> shape_ptrs;
  for (auto iter = map.begin(); iter != map.end(); ++iter)
  {
    occupancy.setKey(*iter);
    if (isOccupied(occupancy))
    {
      cov.setKey(occupancy);

      glm::dquat rot;
      glm::dvec3 scale;
      CovarianceVoxel cv;
      cov.read(&cv);
      if (!covarianceUnitSphereTransformation(&cv, &rot, &scale))
      {
        continue;
      }

      mean.setKey(occupancy);
      intensity_voxel.setKey(occupancy);
      hit_miss_voxel.setKey(occupancy);

      const glm::dvec3 voxel_mean = positionUnsafe(mean);

      IntensityMeanCov intensity_mean_cov;
      intensity_voxel.read(&intensity_mean_cov);
      min_intensity = std::fmin(min_intensity, intensity_mean_cov.intensity_mean);
      max_intensity = std::fmax(max_intensity, intensity_mean_cov.intensity_mean);
      const float scaled_intensity =
        float(M_PI) * (-0.75f + 1.5f * std::log(intensity_mean_cov.intensity_mean - min_intensity + 1.0f) /
                                  std::fmax(1.0f, std::log(max_intensity - min_intensity + 1.0f)));
      const float sin_sc = std::sin(scaled_intensity), cos_sc = std::cos(scaled_intensity);
      HitMissCount hit_miss_count;
      hit_miss_voxel.read(&hit_miss_count);

      tes::Sphere ellipsoid(next_id, tes::Spherical(tes::Vector3d(glm::value_ptr(voxel_mean))));
      ellipsoid.setRotation(tes::Quaterniond(rot.x, rot.y, rot.z, rot.w));
      ellipsoid.setScale(2.0 * tes::Vector3d(scale.x, scale.y, scale.z));
      ellipsoid.setColour(
        tes::Colour(0.5f * (1.0f + sin_sc), 0.5f * (1.0f + cos_sc), 0.5f * (1.0f - sin_sc),
                    0.1f + 0.89f * float(hit_miss_count.hit_count) /
                             std::fmax(1.0f, float(hit_miss_count.hit_count + hit_miss_count.miss_count))));
      ellipsoid.setTransparent(true);

      // const float alpha = 0.9f * float(hit_miss_count.hit_count) /
      //                    std::fmax(1.0f, float(hit_miss_count.hit_count + hit_miss_count.miss_count));
      // ellipsoid.setColour(tes::Colour(0.1f + alpha * 0.5f * (1.0f + sin_sc), 0.1f + alpha * 0.5f * (1.0f + cos_sc),
      //                                0.1f + alpha * 0.5f * (1.0f - sin_sc), 1.0f));

      ellipsoids.emplace_back(ellipsoid);

      if (ellipsoids.size() >= tes::MultiShape::ShapeCountLimit)
      {
        sendShapes(ellipsoids, shape_ptrs);
        ++next_id;
      }
    }
  }
  sendShapes(ellipsoids, shape_ptrs);

  TES_SERVER_UPDATE(ohm::g_tes, 0.0f);
  return true;
#else   // TES_ENABLE
  (void)map;
  return false;
#endif  // TES_ENABLE
}


bool debugDrawNdtOm(const ohm::OccupancyMap &map)
{
  if (!trace::available())
  {
    return false;
  }
#ifdef TES_ENABLE
  if (!g_tes || g_tes->connectionCount() == 0)
  {
    // No connections. Nothing to send data to, so do no work.
    return true;
  }

  // NDT-OM
  std::vector<tes::Sphere> ellipsoids;
  std::vector<tes::Shape *> shape_ptrs;
  const tes::Colour c = tes::Colour::Colours[tes::Colour::SeaGreen];
  auto next_id = static_cast<uint32_t>(size_t(&map));
  Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  Voxel<const CovarianceVoxel> cov(&map, map.layout().covarianceLayer());
  Voxel<const VoxelMean> mean(&map, map.layout().meanLayer());

  if (!occupancy.isLayerValid() || !cov.isLayerValid() || !mean.isLayerValid())
  {
    return false;
  }

  for (auto iter = map.begin(); iter != map.end(); ++iter)
  {
    occupancy.setKey(*iter);
    if (isOccupied(occupancy))
    {
      cov.setKey(occupancy);

      glm::dquat rot;
      glm::dvec3 scale;
      CovarianceVoxel cv;
      if (!cov.isValid())
      {
        // Should be impossible, but helps clang-tidy.
        continue;
      }
      cov.read(&cv);
      if (!covarianceUnitSphereTransformation(&cv, &rot, &scale))
      {
        continue;
      }

      mean.setKey(occupancy);

      const glm::dvec3 voxel_mean = positionUnsafe(mean);

      tes::Sphere ellipsoid(next_id, tes::Spherical(tes::Vector3d(glm::value_ptr(voxel_mean))));
      ellipsoid.setRotation(tes::Quaterniond(rot.x, rot.y, rot.z, rot.w));
      ellipsoid.setScale(2.0 * tes::Vector3d(scale.x, scale.y, scale.z));
      ellipsoid.setColour(c);
      ellipsoids.emplace_back(ellipsoid);

      if (ellipsoids.size() >= tes::MultiShape::ShapeCountLimit)
      {
        sendShapes(ellipsoids, shape_ptrs);
        ++next_id;
      }
    }
  }
  sendShapes(ellipsoids, shape_ptrs);

  TES_SERVER_UPDATE(ohm::g_tes, 0.0f);
  return true;
#else   // TES_ENABLE
  (void)map;
  return false;
#endif  // TES_ENABLE
}


bool debugDrawOccupancy(const ohm::OccupancyMap &map)
{
  if (!trace::available())
  {
    return false;
  }
#ifdef TES_ENABLE
  if (!g_tes || g_tes->connectionCount() == 0)
  {
    // No connections. Nothing to send data to, so do no work.
    return true;
  }

  // occupancy
  std::vector<tes::Box> boxes;
  std::vector<tes::Shape *> shape_ptrs;
  auto next_id = static_cast<uint32_t>(size_t(&map));
  Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  Voxel<const VoxelMean> mean(&map, map.layout().meanLayer());
  float occupancy_value = 0;
  const tes::Vector3d scale(map.resolution());

  // Ok for mean layer to be invalid. We check that below.
  if (!occupancy.isLayerValid())
  {
    return false;
  }

  for (auto iter = map.begin(); iter != map.end(); ++iter)
  {
    occupancy.setKey(*iter);
    if (isOccupied(occupancy))
    {
      mean.setKey(occupancy);
      const glm::dvec3 voxel_mean = positionSafe(mean);

      occupancy_value = occupancy.data();
      occupancy_value = valueToProbability(occupancy_value);
      const auto intensity =
        float((occupancy_value - map.occupancyThresholdProbability()) / (1.0 - map.occupancyThresholdProbability()));
      const int c = int(255 * intensity);

      tes::Box box(next_id, tes::Transform(tes::Vector3d(glm::value_ptr(voxel_mean)), scale));
      box.setColour(tes::Colour(c, c, c));
      boxes.emplace_back(box);

      if (boxes.size() >= tes::MultiShape::ShapeCountLimit)
      {
        sendShapes(boxes, shape_ptrs);
        ++next_id;
      }
    }
  }
  sendShapes(boxes, shape_ptrs);

  TES_SERVER_UPDATE(ohm::g_tes, 0.0f);
  return true;
#else   // TES_ENABLE
  (void)map;
  return false;
#endif  // TES_ENABLE
}
}  // namespace ohm


================================================
FILE: ohm/DebugDraw.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmConfig.h"

namespace ohm
{
class OccupancyMap;

/// Debug draw the @p map using 3rd Eye Scene (if available).
///
/// Requires @c ohm::trace::available()
/// @param map The map to draw.
/// @return True if a 3es is available and a connection is present.
bool ohm_API debugDraw(const ohm::OccupancyMap &map);

/// Debug draw @p map via 3es with Ndt TM data. Fails if required layers are not available.
/// @param map The map to draw.
/// @return True if a 3es is available and a connection is present.
bool ohm_API debugDrawNdtTm(const ohm::OccupancyMap &map);

/// Debug draw @p map via 3es with Ndt occupancy data. Fails if required layers are not available.
/// @param map The map to draw.
/// @return True if a 3es is available and a connection is present.
bool ohm_API debugDrawNdtOm(const ohm::OccupancyMap &map);

/// Debug draw @p map via 3es using only occupancy and voxel mean data. Fails if occupancy layer is not available.
/// @param map The map to draw.
/// @return True if a 3es is available and a connection is present.
bool ohm_API debugDrawOccupancy(const ohm::OccupancyMap &map);
}  // namespace ohm


================================================
FILE: ohm/DebugIDs.h
================================================
//
// author Kazys Stepanas
//
#ifndef DEBUG_IDS_H_
#define DEBUG_IDS_H_

namespace ohm
{
enum class Category
{
  kDefault,
  kMap,
  // Parent category for below.
  kPopulate,
  kRays,
  kFreeCells,
  kOccupiedCells,
  kInfo
};

enum Resource
{
  kMap = 1,
  kMapMesh = 1,  // OK to be the same as kMap. One is object, the other is resource.
};
}  // namespace ohm

#endif  // DEBUG_IDS_H_

================================================
FILE: ohm/DefaultLayer.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "DefaultLayer.h"

#include "MapLayer.h"
#include "MapLayout.h"
#include "VoxelMean.h"
#include "VoxelSecondarySample.h"
#include "VoxelTsdf.h"

#include <algorithm>

#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

#include "CovarianceVoxel.h"

#include <stdexcept>

namespace ohm
{
namespace default_layer
{
const char *occupancyLayerName()
{
  return "occupancy";
}
const char *meanLayerName()
{
  return "mean";
}
const char *traversalLayerName()
{
  return "traversal";
}
const char *covarianceLayerName()
{
  return "covariance";
}
const char *clearanceLayerName()
{
  return "clearance";
}
const char *intensityLayerName()
{
  return "intensity";
}
const char *hitMissCountLayerName()
{
  return "hit_miss_count";
}
const char *touchTimeLayerName()
{
  return "touch_time";
}
const char *incidentNormalLayerName()
{
  return "incident_normal";
}
const char *tsdfLayerName()
{
  return "tsdf";
}
const char *secondarySamplesLayerName()
{
  return "secondary_samples";
}
}  // namespace default_layer


MapLayer *addOccupancy(MapLayout &layout)
{
  int layer_index = layout.occupancyLayer();
  if (layer_index != -1)
  {
    // Already present.
    return layout.layerPtr(layer_index);
  }

  MapLayer *layer = layout.addLayer(default_layer::occupancyLayerName(), 0);

  const float invalid_marker_value = unobservedOccupancyValue();
  size_t clear_value = 0;
  memcpy(&clear_value, &invalid_marker_value, sizeof(invalid_marker_value));

  layer->voxelLayout().addMember(default_layer::occupancyLayerName(), DataType::kFloat, clear_value);

  return layer;
}


MapLayer *addVoxelMean(MapLayout &layout)
{
  int layer_index = layout.meanLayer();
  if (layer_index != -1)
  {
    // Already present.
    return layout.layerPtr(layer_index);
  }

  // Add the mean layer.
  MapLayer *layer = layout.addLayer(default_layer::meanLayerName());

  const size_t clear_value = 0u;
  layer->voxelLayout().addMember("coord", DataType::kUInt32, clear_value);
  layer->voxelLayout().addMember("count", DataType::kUInt32, clear_value);

  if (layer->voxelByteSize() != sizeof(VoxelMean))
  {
    throw std::runtime_error("VoxelMean layer size mismatch");
  }

  return layer;
}


MapLayer *addTraversal(MapLayout &layout)
{
  int layer_index = layout.traversalLayer();
  if (layer_index != -1)
  {
    // Already present.
    return layout.layerPtr(layer_index);
  }

  // Add the mean layer.
  MapLayer *layer = layout.addLayer(default_layer::traversalLayerName());

  const size_t clear_value = 0u;
  layer->voxelLayout().addMember("traversal", DataType::kFloat, clear_value);

  if (layer->voxelByteSize() != sizeof(float))
  {
    throw std::runtime_error("Traversal layer size mismatch");
  }

  return layer;
}


MapLayer *addCovariance(MapLayout &layout)
{
  if (const MapLayer *layer = layout.layer(default_layer::covarianceLayerName()))
  {
    // Already present.
    // Oddities below as we can only retrieve const layers by name.
    return layout.layerPtr(layer->layerIndex());
  }

  MapLayer *layer = layout.addLayer(default_layer::covarianceLayerName());
  VoxelLayout voxel = layer->voxelLayout();
  // Add members to represent an upper triangular covariance matrix.
  voxel.addMember("P00", DataType::kFloat, 0);
  voxel.addMember("P01", DataType::kFloat, 0);
  voxel.addMember("P11", DataType::kFloat, 0);
  voxel.addMember("P02", DataType::kFloat, 0);
  voxel.addMember("P12", DataType::kFloat, 0);
  voxel.addMember("P22", DataType::kFloat, 0);

  if (layer->voxelByteSize() != sizeof(CovarianceVoxel))
  {
    throw std::runtime_error("CovarianceVoxel layer size mismatch");
  }

  return layer;
}


MapLayer *addClearance(MapLayout &layout)
{
  int layer_index = layout.clearanceLayer();
  if (layer_index != -1)
  {
    // Already present.
    return layout.layerPtr(layer_index);
  }

  const float default_clearance = -1.0f;
  size_t clear_value = 0;
  memcpy(&clear_value, &default_clearance, std::min(sizeof(default_clearance), sizeof(clear_value)));
  MapLayer *layer = layout.addLayer(default_layer::clearanceLayerName(), 0);
  VoxelLayout voxel = layer->voxelLayout();
  voxel.addMember(default_layer::clearanceLayerName(), DataType::kFloat, clear_value);

  if (layer->voxelByteSize() != sizeof(float))
  {
    throw std::runtime_error("Clearance layer size mismatch");
  }

  return layer;
}

MapLayer *addIntensity(MapLayout &layout)
{
  if (const MapLayer *layer = layout.layer(default_layer::intensityLayerName()))
  {
    // Already present.
    // Oddities below as we can only retrieve const layers by name.
    return layout.layerPtr(layer->layerIndex());
  }

  MapLayer *layer = layout.addLayer(default_layer::intensityLayerName());
  VoxelLayout voxel = layer->voxelLayout();
  // Add members to represent mean and covariance of points in voxel
  voxel.addMember("mean", DataType::kFloat, 0);
  voxel.addMember("cov", DataType::kFloat, 0);

  if (layer->voxelByteSize() != sizeof(IntensityMeanCov))
  {
    throw std::runtime_error("Intensity layer size mismatch");
  }

  return layer;
}

MapLayer *addHitMissCount(MapLayout &layout)
{
  if (const MapLayer *layer = layout.layer(default_layer::hitMissCountLayerName()))
  {
    // Already present.
    // Oddities below as we can only retrieve const layers by name.
    return layout.layerPtr(layer->layerIndex());
  }

  MapLayer *layer = layout.addLayer(default_layer::hitMissCountLayerName());
  VoxelLayout voxel = layer->voxelLayout();
  // Add members to represent hit and miss count in voxel, according to NDT-TM
  voxel.addMember("hit_count", DataType::kUInt32, 0);
  voxel.addMember("miss_count", DataType::kUInt32, 0);

  if (layer->voxelByteSize() != sizeof(HitMissCount))
  {
    throw std::runtime_error("HitMissCount layer size mismatch");
  }

  return layer;
}

MapLayer *addTouchTime(MapLayout &layout)
{
  if (const MapLayer *layer = layout.layer(default_layer::touchTimeLayerName()))
  {
    // Already present.
    // Oddities below as we can only retrieve const layers by name.
    return layout.layerPtr(layer->layerIndex());
  }

  MapLayer *layer = layout.addLayer(default_layer::touchTimeLayerName());
  VoxelLayout voxel = layer->voxelLayout();
  // Add members to represent hit and miss count in voxel, according to NDT-TM
  voxel.addMember("touch", DataType::kUInt32, 0);

  if (layer->voxelByteSize() != sizeof(uint32_t))
  {
    throw std::runtime_error("Touch time layer size mismatch");
  }

  return layer;
}

MapLayer *addIncidentNormal(MapLayout &layout)
{
  if (const MapLayer *layer = layout.layer(default_layer::incidentNormalLayerName()))
  {
    // Already present.
    // Oddities below as we can only retrieve const layers by name.
    return layout.layerPtr(layer->layerIndex());
  }

  MapLayer *layer = layout.addLayer(default_layer::incidentNormalLayerName());
  VoxelLayout voxel = layer->voxelLayout();
  // Add members to represent hit and miss count in voxel, according to NDT-TM
  voxel.addMember("packed_normal", DataType::kUInt32, 0);

  if (layer->voxelByteSize() != sizeof(uint32_t))
  {
    throw std::runtime_error("Incident normal layer size mismatch");
  }

  return layer;
}


MapLayer *addTsdf(MapLayout &layout)
{
  int layer_index = layout.layerIndex(default_layer::tsdfLayerName());
  if (layer_index != -1)
  {
    // Already present.
    return layout.layerPtr(layer_index);
  }

  // Add the mean layer.
  MapLayer *layer = layout.addLayer(default_layer::tsdfLayerName());

  const size_t clear_value = 0u;
  layer->voxelLayout().addMember("weight", DataType::kFloat, clear_value);
  layer->voxelLayout().addMember("distance", DataType::kFloat, clear_value);

  if (layer->voxelByteSize() != sizeof(VoxelTsdf))
  {
    throw std::runtime_error("VoxelTsdf layer size mismatch");
  }

  return layer;
}

MapLayer ohm_API *addSecondarySamples(MapLayout &layout)
{
  int layer_index = layout.layerIndex(default_layer::secondarySamplesLayerName());
  if (layer_index != -1)
  {
    // Already present.
    return layout.layerPtr(layer_index);
  }

  // Add the mean layer.
  MapLayer *layer = layout.addLayer(default_layer::secondarySamplesLayerName());

  const size_t clear_value = 0u;
  layer->voxelLayout().addMember("m2", DataType::kFloat, clear_value);
  layer->voxelLayout().addMember("range_mean", DataType::kUInt16, clear_value);
  layer->voxelLayout().addMember("count", DataType::kUInt16, clear_value);

  if (layer->voxelByteSize() != sizeof(VoxelSecondarySample))
  {
    throw std::runtime_error("VoxelSecondarySample layer size mismatch");
  }

  return layer;
}
}  // namespace ohm


================================================
FILE: ohm/DefaultLayer.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMDEFAULTLAYER_H
#define OHMDEFAULTLAYER_H

#include "OhmConfig.h"

namespace ohm
{
namespace default_layer
{
/// Name of the occupancy layer.
/// @return "occupancy"
const char ohm_API *occupancyLayerName();
/// Name of the @c VoxelMean layer containing mean voxel coordinates.
/// @return "mean"
const char ohm_API *meanLayerName();
/// Name of the traversal layer.
/// @return "traversal"
const char ohm_API *traversalLayerName();
/// Name of the @c CovarianceVoxel layer containing packed voxel covariances.
/// @return "covariance"
const char ohm_API *covarianceLayerName();
/// Name of the voxel clearance layer.
/// @return "clearance"
const char ohm_API *clearanceLayerName();
/// Name of the voxel intensity layer.
/// @return "intensity"
const char ohm_API *intensityLayerName();
/// Name of the voxel hit miss count layer.
/// @return "hit_miss_count"
const char ohm_API *hitMissCountLayerName();
/// Name of the voxel touch time layer.
/// @return "touch_time"
const char ohm_API *touchTimeLayerName();
/// Name of the voxel incident layer.
/// @return "incident_normal"
const char ohm_API *incidentNormalLayerName();
/// Name of the TSDF voxel layer.
/// @return "tsdf"
const char ohm_API *tsdfLayerName();
/// Name of the layer relating to information on secondary samples (lidar dual returns) falling in a voxel.
/// @return "secondary_samples"
const char ohm_API *secondarySamplesLayerName();
}  // namespace default_layer

class MapLayout;
class MapLayer;

/// Add the occupancy layer to @p layout.
///
/// This ensures @p layout has a layer with a name matching @p occupancyLayerName() setup to hold @c float occupancy
/// data.
///
/// The function makes no changes if @p layout already has a layer named according to @c occupancyLayerName() , but no
/// validation is performed to ensure that the data contained in that layer matches @c float occupancy data .
///
/// @param layout The @p MapLayout to modify.
/// @return The map layer added or the pre-existing layer named according to @c occupancyLayerName() .
MapLayer ohm_API *addOccupancy(MapLayout &layout);

/// Add the @c VoxelMean layer to @p layout.
///
/// This ensures @p layout has a layer with a name matching @p meanLayerName() setup to hold @c VoxelMean data.
///
/// The function makes no changes if @p layout already has a layer named according to @c meanLayerName() , but no
/// validation is performed to ensure that the data contained in that layer matches @c VoxelMean .
///
/// @param layout The @p MapLayout to modify.
/// @return The map layer added or the pre-existing layer named according to @c meanLayerName() .
/// @see @c voxelmean
MapLayer ohm_API *addVoxelMean(MapLayout &layout);

/// Add the traversal layer to @p layout.
///
/// This ensures @p layout has a layer with a name matching @p traversalLayerName() setup to hold a single @c float per
/// voxel. The layer accumulates the total distance travelled all rays through each voxel.
///
/// The function makes no changes if @p layout already has a layer named according to @c traversalLayerName() , but no
/// validation is performed to ensure that the data contained in that layer matches the expected layout.
///
/// @param layout The @p MapLayout to modify.
/// @return The map layer added or the pre-existing layer named according to @c traversalLayerName() .
MapLayer ohm_API *addTraversal(MapLayout &layout);

/// Add the @c CovarianceVoxel layer to @p layout.
///
/// Similar to @c addVoxelMean() , this function adds @c CovarianceVoxel support using the @c covarianceLayerName() .
///
/// @param layout The @p MapLayout to modify.
/// @return The map layer added or the pre-existing layer named according to @c covarianceLayerName() .
MapLayer ohm_API *addCovariance(MapLayout &layout);

/// Add the voxel clearance layer to @p layout.
///
/// Similar to @c addVoxelMean() , this function adds voxel clearance using the @c clearanceLayerName() .
///
/// @param layout The @p MapLayout to modify.
/// @return The map layer added or the pre-existing layer named according to @c clearanceLayerName() .
MapLayer ohm_API *addClearance(MapLayout &layout);

/// Add the voxel intensity (mean and covariance) layer to @p layout.
///
/// Similar to @c addVoxelMean(), this function adds voxel intensity (mean and covariance) using the
/// @c intensityLayerName() .
///
/// @param layout The @p MapLayout to modify.
/// @return The map layer added or the pre-existing layer named according to @c intensityLayerName() .
MapLayer ohm_API *addIntensity(MapLayout &layout);

/// Add the voxel hit and miss counts layer to @p layout.
///
/// Similar to @c addVoxelMean(), this function adds voxel hit count and miss count using the
/// @c hitMissCountLayerName() . The hit count is slightly different to the count in the voxel mean, as it follows the
/// NDT-TM conventions of when to count hits.
///
/// @param layout The @p MapLayout to modify.
/// @return The map layer added or the pre-existing layer named according to @c hitMissCountLayerName() .
MapLayer ohm_API *addHitMissCount(MapLayout &layout);

/// Add the voxel touch time layer to @p layout.
///
/// Similar to @c addVoxelMean(), this function adds touch timestamp using the @c touchTimeLayerName() .
/// The touch time marks the most recent timestamp on which a voxel has been updated. The timestamp stored as a 32-bit
/// unsigned value. The value is in milliseconds since the time base provided during the occupancy update.
///
/// Not supported for TSDF.
///
/// @param layout The @p MapLayout to modify.
/// @return The map layer added or the pre-existing layer named according to @c touchTimeLayerName() .
MapLayer ohm_API *addTouchTime(MapLayout &layout);

/// Add the voxel incident normal layer to @p layout.
///
/// Similar to @c addVoxelMean(), this function adds voxel incident normals using the @c incidentNormalLayerName() .
/// An incident normal approximates the average incident ray from which sample updates occur. That is, the incident
/// averages all the incoming sample rays for the voxels in which the samples falls - i.e., it only the sample voxel is
/// updated. The normal is an approximate, compressed normal and uses a progressive average.
///
/// @note The incident normal points from the sample voxel back to the ray origin.
///
/// @param layout The @p MapLayout to modify.
/// @return The map layer added or the pre-existing layer named according to @c incidentNormalLayerName() .
MapLayer ohm_API *addIncidentNormal(MapLayout &layout);

/// Add the truncated signed distance fields voxel layer to @p layout.
///
/// Similar to @c addVoxelMean(), this function adds a @c VoxelTsdf layer using the @c tsdfNormalLayerName() .
///
/// @param layout The @p MapLayout to modify.
/// @return The map layer added or the pre-existing layer named according to @c tsdfNormalLayerName() .
MapLayer ohm_API *addTsdf(MapLayout &layout);

/// Add the secondary samples voxel layer to @p layout.
///
/// Similar to @c addVoxelMean(), this function adds a @c VoxelSecondarySamples layer using the
/// @c secondarySamplesLayerName() . This layer can be used to collect information about lidar dual returns.
///
/// @param layout The @p MapLayout to modify.
/// @return The map layer added or the pre-existing layer named according to @c secondarySamplesLayerName() .
MapLayer ohm_API *addSecondarySamples(MapLayout &layout);
}  // namespace ohm

#endif  // OHMDEFAULTLAYER_H


================================================
FILE: ohm/Density.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "Density.h"

#include "Key.h"
#include "MapLayout.h"
#include "OccupancyMap.h"

#include <limits>

namespace ohm
{
float voxelDensity(const OccupancyMap &map, const Key &key)
{
  if (!key.isNull())
  {
    Voxel<const float> traversal_voxel(&map, map.layout().traversalLayer());
    Voxel<const VoxelMean> mean_voxel(&map, map.layout().meanLayer());
    if (traversal_voxel.isLayerValid() && mean_voxel.isLayerValid())
    {
      setVoxelKey(key, traversal_voxel, mean_voxel);
      return voxelDensity(traversal_voxel, mean_voxel);
    }
  }
  return 0.0f;
}
}  // namespace ohm


================================================
FILE: ohm/Density.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_DENSITY_H
#define OHM_DENSITY_H

#include "OhmConfig.h"

#include "Voxel.h"
#include "VoxelMean.h"

namespace ohm
{
class OccupancyMap;
class Key;

/// Query the density for the voxel at @p key . The density is calculated as the number of hits (samples) in a
/// voxel divided by the traversal (see @c default_layer::addTraversalLayer() ). It is left to the user to interpret or
/// threshold this value.
///
/// This function requires the @p map have a traversal layer and a voxel mean layer. Otherwise the result will be zero.
/// The result is also zero if the @p key is invalid.
///
/// @todo Should the traversal value be scaled by the voxel resolution? That would make the @c density() value
/// independent of voxel size.
///
/// @param map The map object to query.
/// @param key The key identifying the voxel of interest.
/// @return The density or zero on failure.
float ohm_API voxelDensity(const OccupancyMap &map, const Key &key);

/// Calculate the density given a traversal layer voxel and the corresponding @c VoxelMean . This function assumes
/// that both are referencing the same voxel key.
///
/// The density is calculated as the @c `VoxelMean::count / traversal`. The result is zero if the mean count is zero
/// or either voxel is invalid. The result is infinite if the count is non zero, but the traversal is zero.
///
/// @param traversal_voxel Details of the voxel traversal accumulation. See @c default_layer::addTraversalLayer()
/// @param mean_voxel The voxel mean and hit count.
/// @return The density or zero on failure.
inline float ohm_API voxelDensity(const Voxel<const float> &traversal_voxel, const Voxel<const VoxelMean> &mean_voxel)
{
  if (traversal_voxel.isValid() && mean_voxel.isValid())
  {
    const float count = float(mean_voxel.data().count);
    const float traversal = traversal_voxel.data();
    if (count > 0)
    {
      return (traversal > 0) ? count / traversal : std::numeric_limits<float>::infinity();
    }
  }
  return 0.0f;
}
}  // namespace ohm

#endif  // OHM_DENSITY_H

================================================
FILE: ohm/Key.cpp
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2017
//
#include "Key.h"

#include <ohmutil/VectorHash.h>

#define INVALID_VALUE std::numeric_limits<decltype(Key::kInvalidValue)>::lowest()

namespace ohm
{
const int16_t Key::kInvalidValue = INVALID_VALUE;
const Key Key::kNull(glm::ivec3(INVALID_VALUE), 0, 0, 0);  // NOLINT

size_t Key::Hash::operator()(const Key &key) const
{
  const unsigned one_byte_shift = 8u;
  const unsigned two_byte_shift = 16u;
  glm::u32vec3 hash;
  hash.x = key.region_key_.x | key.region_key_.z << two_byte_shift;  // NOLINT(hicpp-signed-bitwise)
  hash.y = key.region_key_.y;
  hash.z =
    key.local_[0] | key.local_[1] << one_byte_shift | key.local_[2] << two_byte_shift;  // NOLINT(hicpp-signed-bitwise)
  return vhash::hashBits(hash.x, hash.y, hash.z);
}

unsigned Key::regionHash() const
{
  const glm::u32vec3 rk = region_key_;
  return vhash::hashBits(rk.x, rk.y, rk.z);
}


void Key::clampToAxis(int axis, const Key &min_bounds, const Key &max_bounds)
{
  if (region_key_[axis] < min_bounds.region_key_[axis])
  {
    // Below lower bounds region. Clamp region and local.
    region_key_[axis] = min_bounds.region_key_[axis];
    local_[axis] = min_bounds.local_[axis];
  }
  else if (region_key_[axis] == min_bounds.region_key_[axis] && local_[axis] < min_bounds.local_[axis])
  {
    // Below lower bounds local. Clamp local.
    local_[axis] = min_bounds.local_[axis];
  }

  if (region_key_[axis] > max_bounds.region_key_[axis])
  {
    // Above upper bounds region. Clamp region and local.
    region_key_[axis] = max_bounds.region_key_[axis];
    local_[axis] = max_bounds.local_[axis];
  }
  else if (region_key_[axis] == max_bounds.region_key_[axis] && local_[axis] > max_bounds.local_[axis])
  {
    // Above upper bounds local. Clamp local.
    local_[axis] = max_bounds.local_[axis];
  }
}
}  // namespace ohm


================================================
FILE: ohm/Key.h
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2017
//
#ifndef OHM_KEY_H
#define OHM_KEY_H

#include "OhmConfig.h"

#include <glm/glm.hpp>

#include <cinttypes>
#include <ostream>

namespace ohm
{
/// A key into the occupancy map.
///
/// The key has two indexing parts: a @c regionKey() and a @c localKey(). The region key
/// represents the high level region or chunk indexed by the key (see @c OccupancyMap for
/// details), while the local key is a three dimensional index into the chunk itself.
///
/// Note: The default constructor creates a garbage key. Use @c Key::null
/// or @c Key(nullptr) to initialise a null key.
class ohm_API Key
{
public:
  /// Initialiser value for @c kNull.
  static const int16_t kInvalidValue;
  /// A static instance of a null key (equivalent to @c Key(nullptr) ).
  static const Key kNull;

  /// Hashing structure for the key. To use with hash based containers.
  /// The resulting hash is a 32-bit unsigned integer.
  struct ohm_API Hash
  {
    /// Calculate the 32-bit hash for @c key.
    /// @param key The key to hash.
    /// @return The 32-bit hash value.
    size_t operator()(const Key &key) const;
  };
  friend Hash;  ///< Friend specifier for @c Hash .

  /// Construct a garbage key. Does not initialise member variables for performance reasons.
  inline Key() = default;  // NOLINT(cppcoreguidelines-pro-type-member-init)

  /// Copy constructor
  /// @param other Key to copy.
  Key(const Key &other);

  /// Construct a @c null key, but only if passed a @c nullptr. Passing a non-null pointer
  /// has undefined results.
  /// @param ptr Must be @c nullptr for defined results.
  explicit Key(void *ptr);

  /// Construct a key for a region.
  /// @param rx The x coordinate for the region key.
  /// @param ry The y coordinate for the region key.
  /// @param rz The z coordinate for the region key.
  /// @param x The x coordinate for the local key. Must be in range for the region.
  /// @param y The y coordinate for the local key. Must be in range for the region.
  /// @param z The z coordinate for the local key. Must be in range for the region.
  Key(int16_t rx, int16_t ry, int16_t rz, uint8_t x, uint8_t y, uint8_t z);

  /// Construct a key for a region.
  /// @param region_key Initialises the region key part of the key.
  /// @param x The x coordinate for the local key. Must be in range for the region.
  /// @param y The y coordinate for the local key. Must be in range for the region.
  /// @param z The z coordinate for the local key. Must be in range for the region.
  Key(const glm::i16vec3 &region_key, uint8_t x, uint8_t y, uint8_t z);

  /// Construct a key for a region.
  /// @param region_key Initialises the region key part of the key.
  /// @param local_key Initialises the local key part of the key.
  Key(const glm::i16vec3 &region_key, const glm::u8vec3 &local_key);

  ~Key() = default;

  /// Calculates the hash value of the @c regionKey(), used to lookup a @c MapChunk.
  /// @return The hash of the @c regionKey() part.
  unsigned regionHash() const;

  /// Set the region and local key coordinates along @p axis to match that of @p other.
  ///
  /// @param axis Identifies the axis to set [0, 2] matching XYZ.
  /// @param other The key to copy from.
  void setAxisFrom(int axis, const Key &other);

  /// Set one of the region key axis values.
  /// @param axis The axis to set with 0, 1, 2 cooresponding to x, y, z.
  /// @param val The value to set for the axis.
  void setRegionAxis(int axis, int16_t val);

  /// Access the region part of this key. See class comments.
  /// @return The region part of the key.
  inline const glm::i16vec3 &regionKey() const { return region_key_; }

  /// Set the region part for this key. See class comments.
  /// @param key The new @c regionKey() to set.
  inline void setRegionKey(const glm::i16vec3 &key) { region_key_ = key; }

  /// Set one of the local key axis values.
  /// @param axis The axis to set with 0, 1, 2 cooresponding to x, y, z.
  /// @param val The value to set for the axis. Must be valid for the associated @c MapChunk.
  void setLocalAxis(int axis, uint8_t val);

  /// Access the local part for this key. See class comments.
  /// @return The local part of the key.
  inline const glm::u8vec3 &localKey() const { return local_; }

  /// Set the local part for this key. See class comments.
  /// @param local The new @c localKey() to set.
  inline void setLocalKey(const glm::u8vec3 &local) { local_ = local; }

  /// Check if this key and @p other have the same coordinate on the specified @p axis.
  /// @param other Key to compare against.
  /// @param axis The axis to compare. Must be in range [0, 2], mapping to XYZ respectively.
  /// @return True if the keys have the same coordinate on the specified axis.
  inline bool equalOnAxis(const Key &other, int axis) const
  {
    return other.region_key_[axis] == region_key_[axis] && other.local_[axis] == local_[axis];
  }

  /// Test if this key lies within the extents of @p min_bounds and @p max_bounds along the X axis.
  ///
  /// The bounds parameters specify a closed bounding interval, meaning that a key is bounded if it equals
  /// either of the bounding parameters.
  ///
  /// @param min_bounds The minimum bounding range.
  /// @param max_bounds The maximum bounding range.
  /// @return True if this key lies within the X range of [min_bounds, max_bounds].
  inline bool isBoundedX(const Key &min_bounds, const Key &max_bounds) const
  {
    return isBounded(0, min_bounds, max_bounds);
  }

  /// Test if this key lies within the extents of @p min_bounds and @p max_bounds along the Y axis.
  ///
  /// The bounds parameters specify a closed bounding interval, meaning that a key is bounded if it equals
  /// either of the bounding parameters.
  ///
  /// @param min_bounds The minimum bounding range.
  /// @param max_bounds The maximum bounding range.
  /// @return True if this key lies within the Y range of [min_bounds, max_bounds].
  inline bool isBoundedY(const Key &min_bounds, const Key &max_bounds) const
  {
    return isBounded(1, min_bounds, max_bounds);
  }

  /// Test if this key lies within the extents of @p min_bounds and @p max_bounds along the Z axis.
  ///
  /// The bounds parameters specify a closed bounding interval, meaning that a key is bounded if it equals
  /// either of the bounding parameters.
  ///
  /// @param min_bounds The minimum bounding range.
  /// @param max_bounds The maximum bounding range.
  /// @return True if this key lies within the Z range of [min_bounds, max_bounds].
  inline bool isBoundedZ(const Key &min_bounds, const Key &max_bounds) const
  {
    return isBounded(2, min_bounds, max_bounds);
  }

  /// Test if this key lies within the 3D extents of @p min_bounds and @p max_bounds.
  ///
  /// The bounds parameters specify a closed bounding interval, meaning that a key is bounded if it equals
  /// either of the bounding parameters.
  ///
  /// @param min_bounds The minimum bounding extents.
  /// @param max_bounds The maximum bounding extents.
  /// @return True if this key lies within the box defined by [min_bounds, max_bounds].
  bool isBounded(const Key &min_bounds, const Key &max_bounds) const;

  /// Test if this key lies within the extents of @p min_bounds and @p max_bounds along the specified @p axis.
  ///
  /// The bounds parameters specify a closed bounding interval, meaning that a key is bounded if it equals
  /// either of the bounding parameters.
  ///
  /// @param axis The bounding axis to test. Must be [0, 2] identifying XYZ respectively.
  /// @param min_bounds The minimum bounding range.
  /// @param max_bounds The maximum bounding range.
  /// @return True if this key lies within the X range of [min_bounds, max_bounds].
  bool isBounded(int axis, const Key &min_bounds, const Key &max_bounds) const;

  /// Clamp this key to the given bounds region. Behaviour is undefined if @p min_bounds and @p max_bounds do not
  /// define a valid bounding volume.
  /// @param min_bounds The lower bounding key.
  /// @param max_bounds The upper bounding key.
  void clampTo(const Key &min_bounds, const Key &max_bounds);

  /// Clamp this key to the given bounds region on a single @p axis. Behaviour is undefined if @p min_bounds and @p
  /// max_bounds do not define a valid bounding volume.
  /// @param axis The index of the axis to clamp [0, 2].
  /// @param min_bounds The lower bounding key.
  /// @param max_bounds The upper bounding key.
  void clampToAxis(int axis, const Key &min_bounds, const Key &max_bounds);

  /// Do the region and local key coordinates along @p axis match @p other?
  /// @param axis The axis to test. Must be [0, 2] identifying XYZ respectively.
  /// @param other The key to test against.
  /// @return True on precise region and local key match in @p axis.
  bool axisMatches(int axis, const Key &other) const;

  /// Test whether this is a null key or not. Note, the default constructor creates
  /// invalid keys, not null keys.
  /// @return True if this is a null key.
  inline bool isNull() const { return region_key_ == glm::i16vec3(kInvalidValue); }

  /// Assignment operator.
  /// @param other Key to copy.
  /// @return @c *this
  Key &operator=(const Key &other);

  /// Key equality test.
  /// @param other The key to compare against.
  /// @return True when this key's region and local parts exactly match that of @p other.
  bool operator==(const Key &other) const;

  /// Key inequality test.
  /// @param other The key to compare against.
  /// @return True when this key's region and local parts do not exactly match that of @p other.
  bool operator!=(const Key &other) const;

  /// Less than operator used for sorting.
  bool operator<(const Key &other) const;

private:
  glm::i16vec3 region_key_;
  glm::u8vec3 local_;
};

inline Key::Key(int16_t rx, int16_t ry, int16_t rz, uint8_t x, uint8_t y, uint8_t z)
  : Key(glm::i16vec3(rx, ry, rz), glm::u8vec3(x, y, z))
{}


inline Key::Key(const glm::i16vec3 &region_key, uint8_t x, uint8_t y, uint8_t z)
  : Key(region_key, glm::u8vec3(x, y, z))
{}


inline Key::Key(const glm::i16vec3 &region_key, const glm::u8vec3 &local_key)
  : region_key_(region_key)
  , local_(local_key)
{}


// clang-tidy reports cppcoreguidelines-pro-type-member-init that the members are not initialised.
// However, the unrolled the constructor raises modernize-use-equals-default.
// Using the unrolled version and suppressing the warning.
inline Key::Key(const Key &other)  // NOLINT(modernize-use-equals-default)
  : region_key_(other.region_key_)
  , local_(other.local_)
{}


inline Key::Key(void *ptr)
  : region_key_(0)
  , local_(0)
{
  if (ptr == nullptr)
  {
    *this = Key::kNull;
  }
}


inline void Key::setAxisFrom(int axis, const Key &other)
{
  setRegionAxis(axis, other.region_key_[axis]);
  setLocalAxis(axis, other.local_[axis]);
}


inline void Key::setRegionAxis(int axis, int16_t val)
{
  region_key_[axis] = val;
}


inline void Key::setLocalAxis(int axis, uint8_t val)
{
  local_[axis] = val;
}


inline bool Key::isBounded(const Key &min_bounds, const Key &max_bounds) const
{
  return isBounded(0, min_bounds, max_bounds) && isBounded(1, min_bounds, max_bounds) &&
         isBounded(2, min_bounds, max_bounds);
}


inline bool Key::isBounded(int axis, const Key &min_bounds, const Key &max_bounds) const
{
  if (region_key_[axis] < min_bounds.region_key_[axis] || region_key_[axis] > max_bounds.region_key_[axis])
  {
    return false;
  }

  if (region_key_[axis] == min_bounds.region_key_[axis] && local_[axis] < min_bounds.local_[axis])
  {
    return false;
  }

  if (region_key_[axis] == max_bounds.region_key_[axis] && local_[axis] > max_bounds.local_[axis])
  {
    return false;
  }

  return true;
}


inline void Key::clampTo(const Key &min_bounds, const Key &max_bounds)
{
  clampToAxis(0, min_bounds, max_bounds);
  clampToAxis(1, min_bounds, max_bounds);
  clampToAxis(2, min_bounds, max_bounds);
}


inline bool Key::axisMatches(int axis, const Key &other) const
{
  return region_key_[axis] == other.region_key_[axis] && local_[axis] == other.local_[axis];
}


inline Key &Key::operator=(const Key &other) = default;

inline bool Key::operator==(const Key &other) const
{
  return region_key_ == other.region_key_ && local_ == other.local_;
}


inline bool Key::operator!=(const Key &other) const
{
  return region_key_ != other.region_key_ || local_ != other.local_;
}


inline bool Key::operator<(const Key &other) const
{
  // Linearise the region index.
  uint64_t region_a = uint64_t(unsigned(region_key_.x) + 0xffffu) |                 // NOLINT(readability-magic-numbers)
                      (uint64_t(unsigned(region_key_.y) + 0xffffu) << 16u) |        // NOLINT(readability-magic-numbers)
                      (uint64_t(unsigned(region_key_.z) + 0xffffu) << 32u);         // NOLINT(readability-magic-numbers)
  uint64_t region_b = uint64_t(unsigned(other.region_key_.x) + 0xffffu) |           // NOLINT(readability-magic-numbers)
                      (uint64_t(unsigned(other.region_key_.y) + 0xffffu) << 16u) |  // NOLINT(readability-magic-numbers)
                      (uint64_t(unsigned(other.region_key_.z) + 0xffffu) << 32u);   // NOLINT(readability-magic-numbers)
  // NOLINTNEXTLINE(readability-magic-numbers)
  uint32_t local_a = uint32_t(local_.x) | (uint32_t(local_.y) << 8u) | (uint32_t(local_.z) << 16u);
  // NOLINTNEXTLINE(readability-magic-numbers)
  uint32_t local_b = uint32_t(other.local_.x) | (uint32_t(other.local_.y) << 8u) | (uint32_t(other.local_.z) << 16u);
  return region_a < region_b || region_a == region_b && local_a < local_b;
}


inline std::ostream &operator<<(std::ostream &o, const ohm::Key &key)
{
  o << "[(" << key.regionKey().x << ',' << key.regionKey().y << ',' << key.regionKey().z << "):("
    << int(key.localKey().x) << ',' << int(key.localKey().y) << ',' << int(key.localKey().z) << ")]";
  return o;
}
}  // namespace ohm

namespace std
{
/// @c std::hash overload for @c ohm::Key .
template <>
struct ohm_API hash<ohm::Key> : public ohm::Key::Hash
{
};
}  // namespace std

#endif  // OHM_KEY_H


================================================
FILE: ohm/KeyHash.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
//
// Source file to make sure KeyHash symbols are DLL exported.
#include "KeyHash.h"


================================================
FILE: ohm/KeyHash.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef KEYHASH_H
#define KEYHASH_H

#include "OhmConfig.h"

#include "Key.h"

#include <ohmutil/VectorHash.h>

namespace ohm
{
/// Hashing function for @c Key objects.
struct ohm_API KeyHash
{
  /// Hash operator.
  /// @param key The key to hash.
  /// @return A 32-bit hash value for @p key.
  inline uint32_t operator()(const Key &key) const
  {
    const auto &region = key.regionKey();
    const auto &local = key.localKey();
    return vhash::hashBits(region.x | (local.x << 16), region.y | (local.y << 16), region.z | (local.z << 16));
  }
};
}  // namespace ohm

#endif  // KEYHASH_H


================================================
FILE: ohm/KeyList.cpp
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2017
//
#include "KeyList.h"

#include <cmath>
#include <cstring>

namespace ohm
{
KeyList::KeyList(size_t initial_count)
{
  if (initial_count)
  {
    keys_.resize(initial_count);
  }
}


KeyList::~KeyList() = default;


void KeyList::reserve(size_t capacity)
{
  keys_.reserve(capacity);
}


void KeyList::resize(size_t count)
{
  keys_.resize(count);
}


void KeyList::emplace_back(const Key &key)  // NOLINT
{
  keys_.emplace_back(key);
}


Key &KeyList::add()
{
  keys_.emplace_back(Key::kNull);
  return keys_.back();
}
}  // namespace ohm


================================================
FILE: ohm/KeyList.h
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2017
//
#ifndef OHM_KEYLIST_H
#define OHM_KEYLIST_H

#include "OhmConfig.h"

#include "Key.h"

#include <vector>

namespace ohm
{
/// A container class encapsulating a set of @c Key objects.
class ohm_API KeyList
{
public:
  /// Iterator for an @c KeyList.
  class ohm_API iterator  // NOLINT
  {
  public:
    /// Empty constructor: creates an undefined iterator object.
    inline iterator() = default;
    /// Copy constuctor.
    /// @param other Iterator to copy.
    inline iterator(const iterator &other) = default;
    /// Internal constructor used to iterator the given set of keys.
    /// @param key The first key in the set.
    inline explicit iterator(std::vector<Key>::iterator key)
      : key_(key)
    {}

    /// Dereference the @c iterator into an @c Key.
    /// Iterator must be valid to use or behaviour is undefined.
    /// @return A reference to the current @c Key.
    inline Key &operator*() const { return *key_; }

    /// Dereference the @c iterator into an @c Key.
    /// Iterator must be valid to use or behaviour is undefined.
    /// @return A reference to the current @c Key.
    inline Key *operator->() const { return &*key_; }

    /// Increment to the next key (prefix).
    /// @return A reference to @c this.
    inline iterator &operator++()
    {
      ++key_;
      return *this;
    }
    /// Increment to the next key (postfix).
    /// @return A reference to @c this.
    inline iterator operator++(int)
    {
      iterator i(key_);
      ++key_;
      return i;
    }

    /// Decrement to the previous key (prefix).
    /// @return A reference to @c this.
    inline iterator &operator--()
    {
      --key_;
      return *this;
    }
    /// Decrement to the previous key (postfix).
    /// @return A reference to @c this.
    inline iterator operator--(int)
    {
      iterator i(key_);
      --key_;
      return i;
    }

    /// Internal iterator access.
    /// @return The underlying iterator.
    inline const std::vector<Key>::iterator &internalIterator() const { return key_; }

    /// Compare this iterator to @p other for equality.
    /// @return True when both iterators reference the same key.
    inline bool operator==(const iterator &other) { return key_ == other.key_; }
    /// Compare this iterator to @p other for inequality.
    /// @return True when the iterators do not reference the same key.
    inline bool operator!=(const iterator &other) { return key_ != other.key_; }

  private:
    std::vector<Key>::iterator key_;
  };

  /// Iterator referencing an @c KeyList referencing a @c const @c Key.
  class ohm_API const_iterator  // NOLINT
  {
  public:
    /// Empty constructor: creates an undefined iterator object.
    inline const_iterator() = default;
    /// Copy constuctor.
    /// @param other Iterator to copy.
    inline const_iterator(const const_iterator &other) = default;
    /// Copy constuctor from a non-const @c iterator.
    /// @param other Iterator to copy.
    inline explicit const_iterator(const iterator &other) { key_ = other.internalIterator(); }
    /// Internal constructor used to iterator the given set of keys.
    /// @param key The first key in the set.
    inline explicit const_iterator(std::vector<Key>::const_iterator key)
      : key_(key)
    {}

    /// Dereference the @c const_iterator into a @c const @c Key.
    /// Iterator must be valid to use or behaviour is undefined.
    /// @return A reference to the current @c Key.
    inline const Key &operator*() const { return *key_; }
    /// Dereference the @c const_iterator into a @c const @c Key.
    /// Iterator must be valid to use or behaviour is undefined.
    /// @return A reference to the current @c Key.
    inline const Key *operator->() const { return &*key_; }

    /// Increment to the next key (prefix).
    /// @return A reference to @c this.
    inline const_iterator &operator++()
    {
      ++key_;
      return *this;
    }
    /// Increment to the next key (postfix).
    /// @return A reference to @c this.
    inline const_iterator operator++(int)
    {
      const_iterator i(key_);
      ++key_;
      return i;
    }

    /// Decrement to the previous key (prefix).
    /// @return A reference to @c this.
    inline const_iterator &operator--()
    {
      --key_;
      return *this;
    }
    /// Decrement to the previous key (postfix).
    /// @return A reference to @c this.
    inline const_iterator operator--(int)
    {
      const_iterator i(key_);
      --key_;
      return i;
    }

    /// Compare this iterator to @p other for equality.
    /// @return True when both iterators reference the same key.
    inline bool operator==(const const_iterator &other) { return key_ == other.key_; }
    /// Compare this iterator to @p other for inequality.
    /// @return True when the iterators do not reference the same key.
    inline bool operator!=(const const_iterator &other) { return key_ != other.key_; }

  private:
    std::vector<Key>::const_iterator key_;
  };

  /// Create a key list supporting sized to the given @p initialCount.
  /// @param initial_count The initial size for the key list (see @c resize()).
  explicit KeyList(size_t initial_count = 0);
  /// Destructor.
  ~KeyList();

  /// Create an @c interator to the first element in the key list.
  /// @return An iterator to the first key in the list.
  inline iterator begin() { return iterator(keys_.begin()); }
  /// Create an @c const_interator to the first element in the key list.
  /// @return An iterator to the first key in the list.
  inline const_iterator begin() const { return const_iterator(keys_.begin()); }
  /// Create the end @c iterator, marking the end of iteration. The iterator is not valid for deferencing.
  /// @return An end style iterator.
  inline iterator end() { return iterator(keys_.end()); }
  /// Create the end @c const_iterator, marking the end of iteration. The iterator is not valid for deferencing.
  /// @return An end style iterator.
  inline const_iterator end() const { return const_iterator(keys_.end()); }

  /// Reserve space to contain @p capacity keys in the set.
  /// @param capacity The desired capacity.
  void reserve(size_t capacity);
  /// Resize the key set to contain @p count keys.
  ///
  /// The capacity is increased if required, but will not be decreased.
  ///
  /// @param count The number of keys to hold.
  void resize(size_t count);

  /// Clear the key list to contain zero elements.
  /// This does not free resources.
  inline void clear() { keys_.clear(); }

  /// Check if the key list is empty, containing no elements.
  /// @return True if the key list is empty.
  inline bool empty() const { return keys_.empty(); }
  /// An alias for @c empty().
  /// @return True if the key list is empty.
  inline bool isEmpty() const { return keys_.empty(); }

  /// Query the capacity of the key list.
  /// @return The maximum number of keys the list can currently hold without re-allocation.
  inline size_t capacity() const { return keys_.capacity(); }
  /// Query the number of items currently in the list.
  /// @return The number of keys available in the list.
  inline size_t size() const { return keys_.size(); }
  /// Query the number of items currently in the list.
  /// @return The number of keys available in the list.
  inline size_t count() const { return keys_.size(); }

  /// Direct access to the underlying key list array. Use with care.
  /// @return A pointer to the memory used to store the keys.
  inline Key *data() { return keys_.data(); }
  /// @overload
  inline const Key *data() const { return keys_.data(); }

  /// Request the key at index @p i (unsafe). The request is not bounds checked and the user
  /// is responsible for ensuring <tt>i < count()</tt>.
  /// @param i The index of the requested key. Must be in the range <tt>[0, count())</tt>.
  /// @return The key at index @p i.
  inline Key &at(size_t i) { return keys_[i]; }
  /// @overload
  inline const Key &at(size_t i) const { return keys_[i]; }

  /// Array style access to the key at index @p i. Semantically equivalent to @c at().
  /// @param i The index of the requested key. Must be in the range <tt>[0, count())</tt>.
  /// @return The key at index @p i.
  inline Key &operator[](size_t i) { return keys_[i]; }
  /// @overload
  inline const Key &operator[](size_t i) const { return keys_[i]; }

  /// Add a key to the end of the list.
  /// The list will grow if required, reallocating the underlying memory.
  /// @param key The key to add.
  void emplace_back(const Key &key);  // NOLINT

  /// Add a key to the end of the list and return a reference to the new key.
  /// The list will grow if required, reallocating the underlying memory.
  ///
  /// The returned key reference is only valid so long as the key list memory remains the same.
  ///
  /// @return A reference to the added key.
  Key &add();

  /// Add a key to the end of the list.
  /// The list will grow if required, reallocating the underlying memory.
  /// @param key The key to add.
  inline void add(const Key &key) { return emplace_back(key); }

private:
  std::vector<Key> keys_;
};
}  // namespace ohm

#endif  // OHM_KEYLIST_H


================================================
FILE: ohm/KeyRange.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "KeyRange.h"

#include "OccupancyMap.h"
#include "private/OccupancyMapDetail.h"

namespace ohm
{
KeyRange::KeyRange(const Key &min_key, const Key &max_key, const ohm::OccupancyMap &map)
  : KeyRange(min_key, max_key, map.regionVoxelDimensions())
{}

void KeyRange::expand(const Key &key)
{
  // First handle null keys.
  if (key.isNull())
  {
    // New key is null. Cannot expand.
    return;
  }

  if (min_key_.isNull())
  {
    // Min is null. Set to key.
    min_key_ = key;
  }

  if (max_key_.isNull())
  {
    // Max is null. Set to key.
    max_key_ = key;
  }

  // Build ranges from the min and max respectively to the new key.
  const glm::ivec3 min_to_key = OccupancyMap::rangeBetween(min_key_, key, region_dimensions_);
  const glm::ivec3 max_to_key = OccupancyMap::rangeBetween(max_key_, key, region_dimensions_);
  // With min_to_key and max_to_key set, we can ascertain which axes need expanding.
  // Each axis of min_to_key which is negative indicates that the new key has a lower bound than min_key_.
  // Likewise, each axis of max_to_key which is positive indicates that the new key has a higher bound than max_key_.

  for (int i = 0; i < 3; ++i)
  {
    if (min_to_key[i] < 0)
    {
      min_key_.setRegionAxis(i, key.regionKey()[i]);
      min_key_.setLocalAxis(i, key.localKey()[i]);
    }
    if (max_to_key[i] > 0)
    {
      max_key_.setRegionAxis(i, key.regionKey()[i]);
      max_key_.setLocalAxis(i, key.localKey()[i]);
    }
  }
}

glm::ivec3 KeyRange::range() const
{
  // The range defines a closed interval, so we have to add 1 in each dimension for the volume to ensure the maximal
  // extents are considered.
  const glm::ivec3 volume_correction = glm::ivec3(1);
  return ohm::OccupancyMap::rangeBetween(min_key_, max_key_, region_dimensions_) + volume_correction;
}


size_t KeyRange::indexOf(const Key &key) const
{
  if (!isValid())
  {
    return 0;
  }

  const glm::ivec3 range = this->range();
  const glm::ivec3 range_to_key = ohm::OccupancyMap::rangeBetween(min_key_, key, region_dimensions_);
  return size_t(range_to_key.x) + size_t(range_to_key.y * range.x) + size_t(range_to_key.z * range.x * range.y);
}


Key KeyRange::fromIndex(size_t index) const
{
  if (!isValid())
  {
    return Key::kNull;
  }

  const glm::ivec3 range = this->range();
  glm::ivec3 offset(0);
  offset.z = int(index / (range.x * range.y));
  offset.y = index % (range.x * range.y);
  offset.x = offset.y % range.x;
  offset.y /= range.x;

  Key key = min_key_;
  OccupancyMapDetail::moveKeyAlongAxis(key, 0, offset.x, region_dimensions_);
  OccupancyMapDetail::moveKeyAlongAxis(key, 1, offset.y, region_dimensions_);
  OccupancyMapDetail::moveKeyAlongAxis(key, 2, offset.z, region_dimensions_);

  return key;
}


void KeyRange::walkNext(Key &key, const Key &min_key, const Key &max_key, const glm::ivec3 &region_dimensions)
{
  // Try walk X axis.
  if (!key.equalOnAxis(max_key, 0))
  {
    OccupancyMapDetail::moveKeyAlongAxis(key, 0, 1, region_dimensions);
    return;
  }

  // At X limit. Reset X axis to min and try walk Y.
  key.setRegionAxis(0, min_key.regionKey()[0]);
  key.setLocalAxis(0, min_key.localKey()[0]);

  if (!key.equalOnAxis(max_key, 1))
  {
    OccupancyMapDetail::moveKeyAlongAxis(key, 1, 1, region_dimensions);
    return;
  }

  // At Y limit. Reset Y axis to min and try walk Z.
  key.setRegionAxis(1, min_key.regionKey()[1]);
  key.setLocalAxis(1, min_key.localKey()[1]);

  OccupancyMapDetail::moveKeyAlongAxis(key, 2, 1, region_dimensions);
}


void KeyRange::updateEndKey()
{
  end_key_ = min_key_;
  // Last axis which gets walked. We need to set this to the max key, then walk that axis on 1 as this is the pattern
  // iteration takes.
  const unsigned last_axis = 2;
  end_key_.setRegionAxis(last_axis, max_key_.regionKey()[last_axis]);
  end_key_.setLocalAxis(last_axis, max_key_.localKey()[last_axis]);
  OccupancyMapDetail::moveKeyAlongAxis(end_key_, last_axis, 1, region_dimensions_);
}

}  // namespace ohm


================================================
FILE: ohm/KeyRange.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_KEYRANGE_H
#define OHM_KEYRANGE_H

#include "OhmConfig.h"

#include "Key.h"

#include <glm/vec3.hpp>

namespace ohm
{
class OccupancyMap;

class KeyRangeIterator;

/// A utility class which represents a key range in a map. The range supports iteration and volume operations.
///
/// Note that a @c KeyRange always behaves as a closed interval. That is, the range defines a rectangular prism from
/// the @c minKey() to the @c maxKey() including the maximal key. This is illustrated in 2D below.
///
/// @code{.unparsed}
///
///   +-----------------------------M
///   |                             |
///   |                             |
///   |                             |
///   |                             |
///   |                             |
///   |                             |
///   m-----------------------------+
///
/// - m is the minimum key in the range.
/// - M is the maximum key in the range.
/// - The far left and the top border are included when iterating the key range as well as the max key (M).
///
/// @endcode{.unparsed}
///
/// Also note, a key range is bound to the map for which it was created. This is because of the way that the region
/// voxel dimensions affect key iteration.
class ohm_API KeyRange
{
public:
  /// Iterator alias for C++ API compatibility.
  using iterator = KeyRangeIterator;  // NOLINT(readability-identifier-naming)

  /// Default constructor. The created range consists of null min/max keys and is not valid.
  KeyRange() = default;

  /// Create a range between the given keys. The resulting range is only valid if the given @p min_key and @p max_key
  /// values are correctly configured to define a zero or increasing range along each axis. Otherwise @c isValid() will
  /// be false.
  /// @param min_key The minimal bounds for the range to define.
  /// @param max_key The maximal bounds for the range to define.
  /// @param region_voxel_dim Defines the number of voxels in each region of the owning map.
  inline KeyRange(const Key &min_key, const Key &max_key, const glm::u8vec3 &region_voxel_dim)
    : min_key_(min_key)
    , max_key_(max_key)
    , region_dimensions_(region_voxel_dim)
  {
    updateEndKey();
  }

  /// Create a range between the given keys for the specified map. The resulting range is only valid if the given
  /// @p min_key and @p max_key values are correctly configured to define a zero or increasing range along each axis.
  /// Otherwise @c isValid() will be false.
  /// @param min_key The minimal bounds for the range to define.
  /// @param max_key The maximal bounds for the range to define.
  /// @param map Defines the map to which the range belongs. This sets the @c regionDimensions() but no reference or
  /// pointer to the map is retained.
  KeyRange(const Key &min_key, const Key &max_key, const ohm::OccupancyMap &map);

  /// Copy constructor
  /// @param other Range to copy.
  inline KeyRange(const KeyRange &other) = default;

  /// Assignment operator.
  /// @param other Range to copy.
  inline KeyRange &operator=(const KeyRange &other) = default;

  /// Create an iterator to start iterating the range.
  ///
  /// @note Iteration of an invalid range - where @c isValid() is false - creates undefined behaviour.
  ///
  /// @return An iterator object referencing the @c minKey() .
  iterator begin() const;

  /// Create an iterator which references the voxel one step after the @c maxKey() .
  ///
  /// @note Iteration of an invalid range - where @c isValid() is false - creates undefined behaviour.
  ///
  /// @return An iterator object referencing the key one step beyond @c maxKey() .
  iterator end() const;

  /// Query the minimum key for the range.
  /// @return The minimum key value.
  inline const Key &minKey() const { return min_key_; }
  /// Set the minimum key value for the range.
  /// @note No validation is performed by this call.
  /// @param key The new value for @c minKey().
  inline void setMinKey(const Key &key)
  {
    min_key_ = key;
    updateEndKey();
  }
  /// Query the maximum key for the range.
  /// @return The maximum key value.
  inline const Key &maxKey() const { return max_key_; }
  /// Set the maximum key value for the range.
  /// @note No validation is performed by this call.
  /// @note This may invalidate any current iterator objects referencing this range.
  /// @param key The new value for @c maxKey().
  inline void setMaxKey(const Key &key)
  {
    max_key_ = key;
    updateEndKey();
  }

  /// Expand the key range to include the given @p key .
  ///
  /// No change is made if @p key is null. For each of min/max keys which are null, they are set to match @p key .
  ///
  /// @param key The key to include in the range.
  void expand(const Key &key);

  /// Expand the key range to include the given @p range.
  ///
  /// No change is made by each null source key from @p range . For each of min/max keys in this range which are null,
  /// they are set to match the input range keys.
  ///
  /// @param range The other @c KeyRange to include in this one.
  inline void expand(const KeyRange &range)
  {
    expand(range.minKey());
    expand(range.maxKey());
  }

  /// Query the key value used to represent the end of iteration.
  ///
  /// The resulting key references a single step beyond the @c maxKey() value. The resulting key has it's X and Y axes
  /// equal to those of the @c minKey() while the Z axis is one step greater than the @c maxKey() range. This matches
  /// the way key ranges are iterated.
  ///
  /// @note The result is undefined for an invalid range.
  ///
  /// @return The key value representing the end of iteration.
  inline const Key &endKey() const { return end_key_; }

  /// Query the region dimensions which this range operates on. See @c OccupancyMap::regionVoxelDimensions().
  /// @return A vector defining the number of voxels along each axis for a @c MapRegion.
  inline const glm::u8vec3 &regionDimensions() const { return region_dimensions_; }
  /// Set the @c regionDimensions() value. For expert use.
  /// @param dim The new @c regionDimensions() value.
  inline void setRegionDimensions(const glm::u8vec3 &dim) { region_dimensions_ = dim; }

  /// Query if the selected min/max keys create a valid range.
  ///
  /// A range is invalid if:
  /// - @c minKey().isNull() is true
  /// - @c maxKey().isNull() is true
  /// - The range defined by @c minKey() to @c maxKey() is zero or negative.
  ///
  /// @return True if this object defines a valid range.
  inline bool isValid() const
  {
    const auto r = range();
    return !min_key_.isNull() && !max_key_.isNull() && r.x > 0 && r.y > 0 && r.z > 0;
  }

  /// Query the number of voxels along each axis in the range.
  ///
  /// A range defines a closed interval, so a range where @c minkey() is equal to @c maxKey() will have a @c range()
  /// value of `(1, 1, 1)`.
  ///
  /// @note Behaviour is undefined for an invalid @c KeyRange.
  ///
  /// @return The number of voxels along each axis which this range encloses.
  glm::ivec3 range() const;

  /// Query the number of voxels in the enclosed region.
  /// @note Behaviour is undefined for an invalid @c KeyRange.
  /// @return The number of voxels contained in the range.
  inline int volume() const
  {
    const auto r = range();
    return r.x * r.y * r.z;
  }

  /// Calculate a 1D index for the given @p key in this range.
  /// Indexing runs along each X coordinate first, then Y then Z.
  /// @param key The key to query the index of. Assumed to be in the range.
  /// @return The 1D index of @p key in this range or zero if this range is invalid.
  size_t indexOf(const Key &key) const;

  /// Performs the reverse operation of @c indexOf() , yielding a key from a 1D offset into this range.
  /// @param index The index of this range. Assumed to be in the range.
  /// @return The key coresponding to @p index or a null key if this range is invalid.
  Key fromIndex(size_t index) const;

  /// Walk the given @p key to the next key in this range. Note: this is not bounds checked along Z and will continue
  /// to iterate the XY region beyond the range in Z. This function is intended for internal use to support
  /// @c KeyRangeIterator .
  /// @param[in,out] key The key to walk to the next voxel.
  inline void walkNext(Key &key) const { return walkNext(key, min_key_, max_key_, region_dimensions_); }

  /// Static utility function to walk the given @p key to the next key in this range. Note: this is not bounds checked
  /// along Z and will continue to iterate the XY region beyond the range in Z. This function is intended for internal
  /// use to support @c KeyRangeIterator .
  /// @param[in,out] key The key to walk to the next voxel.
  /// @param min_key The minimal key of the range to walk.
  /// @param max_key The maximal (inclusive) key of the range to walk.
  /// @param region_dimensions Defines the extents of a region in the map to which the defined range belongs.
  static void walkNext(Key &key, const Key &min_key, const Key &max_key, const glm::ivec3 &region_dimensions);

private:
  /// Update the value of @c end_key_ . To be called whenever the min or max keys change.
  void updateEndKey();

  Key min_key_ = Key::kNull;       ///< The minimal/first key in the range.
  Key max_key_ = Key::kNull;       ///< The maximal/last key in the range.
  Key end_key_ = Key::kNull;       ///< A cached version of the end key - essentially the `max_key_ + 1`
  glm::u8vec3 region_dimensions_;  ///< Defines the number of voxels in each region for the map to which this belongs.
};

/// Defines an object used to iterate a @c KeyRange . It is assumed that the range does not change during iteration.
class ohm_API KeyRangeIterator
{
public:
  /// Default constructor. The resulting iterator is not valid.
  inline KeyRangeIterator() = default;

  /// Create an iterator object targeting the given @p range starting at @p initial_key .
  ///
  /// The @c initial_key is assumed to either be contained by the range, or to be the range object's @c endKey() value.
  ///
  /// @param range The range object to iterate.
  /// @param initial_key The key to start iterating at or the @c range.endKey() value.
  inline KeyRangeIterator(const KeyRange &range, const Key &initial_key)
    : key_(initial_key)
    , range_(range)
  {}

  /// Create an iterator object targeting the given @p range starting at the range object's @c minKey() value.
  /// @param range The range to begin iterating.
  explicit inline KeyRangeIterator(const KeyRange &range)
    : KeyRangeIterator(range, range.minKey())
  {}

  /// Copy constructor.
  /// @param other The iterator object to copy.
  inline KeyRangeIterator(const KeyRangeIterator &other) = default;
  /// Assignment operator.
  /// @param other The iterator object value to assign.
  inline KeyRangeIterator &operator=(const KeyRangeIterator &other) = default;

  /// Equality operator.
  /// @return True if the @c key() exactly matches @p other.key() . The associated ranges are not checked.
  inline bool operator==(const KeyRangeIterator &other) const { return other.key() == key(); }
  /// Inequality operator.
  /// @return True if the @c key() does not exactly match @p other.key() . The associated ranges are not checked.
  inline bool operator!=(const KeyRangeIterator &other) const { return other.key() != key(); }

  /// Query the current key.
  /// @return The current key value.
  inline Key key() const { return key_; }

  /// Dereference the iterator as a key.
  /// @return The current voxel key.
  inline const Key &operator*() const { return key_; }

  /// Dereference the iterator as a key.
  /// @return The current voxel key.
  inline const Key *operator->() const { return &key_; }

  /// Prefix increment for the iterator. Iterator becomes invalid when incrementing an iterator
  /// referencing the last voxel in the map. Safe to call on an invalid iterator (no change).
  /// @return This iterator after the increment.
  inline KeyRangeIterator &operator++()
  {
    range_.walkNext(key_);
    return *this;
  }

  /// Postfix increment for the iterator. Iterator becomes invalid when incrementing an iterator
  /// referencing the last voxel in the map. Safe to call on an invalid iterator (no change).
  /// @return This iterator before the increment.
  inline KeyRangeIterator operator++(int)
  {
    const auto iter = *this;
    range_.walkNext(key_);
    return iter;
  }

private:
  Key key_ = Key::kNull;
  KeyRange range_{};
};  // namespace ohm


inline KeyRange::iterator KeyRange::begin() const
{
  return KeyRange::iterator(*this);
}


inline KeyRange::iterator KeyRange::end() const
{
  return KeyRange::iterator(*this, endKey());
}
}  // namespace ohm

#endif  // OHM_KEYRANGE_H


================================================
FILE: ohm/KeyStream.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_KEYSTREAM_H_
#define OHM_KEYSTREAM_H_

#include "OhmConfig.h"

#include "Key.h"

// Deprecated. Stream operator now appears in Key.h

#endif  // OHM_KEYSTREAM_H_


================================================
FILE: ohm/LineKeysQuery.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "LineKeysQuery.h"

#include "private/LineKeysQueryDetail.h"

#include "CalculateSegmentKeys.h"
#include "KeyList.h"
#include "OccupancyMap.h"
#include "OccupancyUtil.h"

#include <glm/gtc/type_ptr.hpp>

#include <algorithm>
#include <chrono>
#include <cstring>
#include <thread>

namespace ohm
{
namespace
{
unsigned nextPow2(unsigned v)
{
  // compute the next highest power of 2 of 32-bit v
  v--;
  v |= v >> 1u;
  v |= v >> 2u;
  v |= v >> 4u;
  v |= v >> 8u;
  v |= v >> 16u;  // NOLINT(readability-magic-numbers)
  v++;
  return v;
}
}  // namespace

LineKeysQuery::LineKeysQuery(LineKeysQueryDetail *detail)
  : Query(detail)
{}


LineKeysQuery::LineKeysQuery(ohm::OccupancyMap &map, unsigned query_flags)
  : LineKeysQuery(query_flags)
{
  setMap(&map);
}


LineKeysQuery::LineKeysQuery(unsigned query_flags)
  : LineKeysQuery(new LineKeysQueryDetail)
{
  setQueryFlags(query_flags);
}


LineKeysQuery::~LineKeysQuery()
{
  auto *d = static_cast<LineKeysQueryDetail *>(imp_);
  delete d;
  imp_ = nullptr;
}


void LineKeysQuery::setRays(const glm::dvec3 *rays, size_t point_count)
{
  auto *d = static_cast<LineKeysQueryDetail *>(imp_);
  d->rays.resize(point_count);
  memcpy(d->rays.data(), rays, sizeof(*rays) * point_count);
}


const glm::dvec3 *LineKeysQuery::rays() const
{
  const auto *d = static_cast<LineKeysQueryDetail *>(imp_);
  return d->rays.data();
}


size_t LineKeysQuery::rayPointCount() const
{
  const auto *d = static_cast<LineKeysQueryDetail *>(imp_);
  return d->rays.size();
}


const size_t *LineKeysQuery::resultIndices() const
{
  auto *d = static_cast<LineKeysQueryDetail *>(imp_);
  return d->result_indices.data();
}


const size_t *LineKeysQuery::resultCounts() const
{
  auto *d = static_cast<LineKeysQueryDetail *>(imp_);
  return d->result_counts.data();
}


bool LineKeysQuery::onExecute()
{
  auto *d = static_cast<LineKeysQueryDetail *>(imp_);

  KeyList key_list;
  d->result_indices.resize(d->rays.size() / 2);
  d->result_counts.resize(d->rays.size() / 2);
  for (size_t i = 0; i < d->rays.size(); i += 2)
  {
    key_list.clear();
    calculateSegmentKeys(key_list, *d->map, d->rays[i + 0], d->rays[i + 1], true);
    d->result_indices[i / 2] = d->intersected_voxels.size();
    d->result_counts[i / 2] = key_list.size();
    for (auto &&key : key_list)
    {
      d->intersected_voxels.push_back(key);
    }
  }

  d->number_of_results = d->result_indices.size();

  return true;
}


bool LineKeysQuery::onExecuteAsync()
{
  auto *d = static_cast<LineKeysQueryDetail *>(imp_);

  if ((d->query_flags & kQfGpuEvaluate))
  {
    static bool once = false;
    if (!once)
    {
      once = true;
      std::cerr << "GPU unavailable for LineKeysQuery. Failing async call.\n" << std::flush;
    }
  }

  return false;
}


void LineKeysQuery::onReset(bool /*hard_reset*/)
{
  auto *d = static_cast<LineKeysQueryDetail *>(imp_);
  d->result_indices.clear();
  d->result_counts.clear();
}


LineKeysQueryDetail *LineKeysQuery::imp()
{
  return static_cast<LineKeysQueryDetail *>(imp_);
}


const LineKeysQueryDetail *LineKeysQuery::imp() const
{
  return static_cast<const LineKeysQueryDetail *>(imp_);
}
}  // namespace ohm


================================================
FILE: ohm/LineKeysQuery.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_LINEKEYSQUERY_H
#define OHM_LINEKEYSQUERY_H

#include "OhmConfig.h"

#include "Query.h"
#include "QueryFlag.h"

#include <glm/fwd.hpp>

namespace ohm
{
struct LineKeysQueryDetail;

/// This query calculates the voxels intersecting a batch of rays.
///
/// The results are similar to those of @c OccupancyMap::calculateSegmentKeys() (identical when using CPU),
/// but supports batched and GPU based calculation. The GPU calculation is generally only marginally faster
/// than CPU, but GPU supports @c executeAsync(), which is not supported for CPU.
///
/// In practice, this query isn't very useful as the GPU performance gains are minimal.
///
/// General usage is:
/// - Initialise the query object setting the map and the GPU flag if required.
/// - Call @c setRays() to define the ray start/end point pairs.
/// - Call @c execute() or @c executeAsync() followed by @c wait() (GPU only).
/// - Process results (see below).
///
/// The @c numberOfResults() will match the number of rays (@c pointCount given to @c setRays()) and for
/// each of these results, there is an entry in @c resultIndices() and @c resultCounts(). The indices indicate the
/// offsets into @c intersectedVoxels() for each ray in the same order they appear in @c setRays(). The
/// counts identify how many voxels are present for the current line counting from the associated index.
/// There should always be at least one voxel per ray for the start/end voxel. More generally, the first voxel
/// is the ray start voxel and the last voxel is the ray end voxel.
class ohm_API LineKeysQuery : public Query
{
protected:
  /// Constructor used for inherited objects. This supports deriving @p LineKeysQueryDetail into
  /// more specialised forms.
  /// @param detail pimple style data structure. When null, a @c LineKeysQueryDetail is allocated by
  /// this method.
  explicit LineKeysQuery(LineKeysQueryDetail *detail);

public:
  /// Construct a new query using the given parameters.
  /// @param map The map to operate on. Only the voxel resolution and region sizes are used.
  /// @param query_flags Flags controlling the query behaviour. See @c QueryFlag and @c LineKeysQuery::Flag.
  explicit LineKeysQuery(ohm::OccupancyMap &map, unsigned query_flags = 0u);

  /// Construct a new query using the given parameters.
  /// @param query_flags Flags controlling the query behaviour. See @c QueryFlag and @c LineKeysQuery::Flag.
  explicit LineKeysQuery(unsigned query_flags = 0);

  /// Destructor.
  ~LineKeysQuery() override;

  /// Set the ray point pairs to operate on. The @p rays elements must be in start/end point pairs.
  /// @param rays Ray start/end point pairs.
  /// @param point_count Number of elements in @p rays. Expected to be an even number; i.e., twice the number of rays.
  void setRays(const glm::dvec3 *rays, size_t point_count);

  /// Get the array of ray points set in the last call to @c setRays().
  /// @return The last ray set assigned in @c setRays(). The number of elements is @c rayPointCount().
  const glm::dvec3 *rays() const;

  /// Return the number of point elements in @p rays(). The number of rays is half this as ray points are in
  /// start/end point pairs.
  /// @return The number of elements in @p rays().
  size_t rayPointCount() const;

  /// Get the array of result index offsets into @c intersectsVoxels().
  ///
  /// This identifies the offsets for each ray into @c intersectedVoxels() where the results for that ray begin.
  /// The corresponding number of voxels for each ray are accessible via @c resultCounts(). The number of elements
  /// is set by the number of rays, accessible via @c numberOfResults().
  ///
  /// Only value once execution completes.
  ///
  /// @return Index offsets into @p intersectedVoxels() for each ray.
  const size_t *resultIndices() const;

  /// Get the array of result voxel counts in @c intersectedVoxels() for each ray.
  ///
  /// This identifies the number of voxels for each ray in @c intersectedVoxels(). The corresponding offset for
  /// each ray into @c intersectedVoxels() is available via @c resultIndices(). The number of elements
  /// is set by the number of rays, accessible via @c numberOfResults().
  ///
  /// Only value once execution completes.
  ///
  /// @return Number of voxels intersected for each ray.
  const size_t *resultCounts() const;

protected:
  bool onExecute() override;
  bool onExecuteAsync() override;
  void onReset(bool hard_reset) override;

  /// Access internal details.
  /// @return Internal details.
  LineKeysQueryDetail *imp();
  /// Access internal details.
  /// @return Internal details.
  const LineKeysQueryDetail *imp() const;
};
}  // namespace ohm


#endif  // OHM_LINEKEYSQUERY_H


================================================
FILE: ohm/LineQuery.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "LineQuery.h"

#include "CalculateSegmentKeys.h"
#include "Key.h"
#include "OccupancyMap.h"
#include "QueryFlag.h"
#include "private/LineQueryDetail.h"
#include "private/OccupancyMapDetail.h"
#include "private/OccupancyQueryAlg.h"
#include "private/VoxelAlgorithms.h"

#include <3esservermacros.h>

#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>

#ifdef OHM_FEATURE_THREADS
#include <tbb/blocked_range.h>
#include <tbb/parallel_for.h>
#endif  // OHM_FEATURE_THREADS

#include <algorithm>
#include <functional>
#include <iostream>
#include <limits>

namespace ohm
{
namespace
{
void calculateNearestNeighboursRange(LineQueryDetail &query, size_t start_index, size_t end_index,
                                     const OccupancyMap &map, const glm::ivec3 &voxel_search_half_extents)
{
  float range;

  for (size_t i = start_index; i < end_index; ++i)
  {
    const Key &key = query.segment_keys[i];
    range =
      calculateNearestNeighbour(key, map, voxel_search_half_extents, (query.query_flags & kQfUnknownAsOccupied) != 0,
                                false, query.search_radius, query.axis_scaling);
    // if (range < 0)
    // {
    //   range = query.default_range;
    // }
    query.intersected_voxels[i] = key;
    query.ranges[i] = range;
  }
}

unsigned occupancyLineQueryCpu(const OccupancyMap &map, LineQueryDetail &query, ClosestResult &closest)
{
  glm::ivec3 voxel_search_half_extents = calculateVoxelSearchHalfExtents(map, query.search_radius);
  calculateSegmentKeys(query.segment_keys, map, query.start_point, query.end_point);

  // Allocate results.
  query.intersected_voxels.resize(query.segment_keys.size());
  query.ranges.resize(query.segment_keys.size());

  // Perform query.
#ifdef OHM_FEATURE_THREADS
  const auto parallel_query_func = [&query, &map, voxel_search_half_extents](const tbb::blocked_range<size_t> &range) {
    calculateNearestNeighboursRange(query, range.begin(), range.end(), map, voxel_search_half_extents);
  };
  tbb::parallel_for(tbb::blocked_range<size_t>(0u, query.segment_keys.size()), parallel_query_func);

#else   // OHM_FEATURE_THREADS
  calculateNearestNeighboursRange(query, 0u, query.segment_keys.size(), map, voxel_search_half_extents);
#endif  // OHM_FEATURE_THREADS

  // Find closest result.
  for (size_t i = 0; i < query.ranges.size(); ++i)
  {
    double range = query.ranges[i];
    if (range * range < closest.range)
    {
      closest.range = range * range;
      closest.index = i;
    }
  }

  return unsigned(query.segment_keys.size());
}
}  // namespace


LineQuery::LineQuery(LineQueryDetail *detail)
  : Query(detail)
{}


LineQuery::LineQuery()
  : LineQuery(new LineQueryDetail)
{}


LineQuery::LineQuery(OccupancyMap &map, const glm::dvec3 &start_point, const glm::dvec3 &end_point, float search_radius,
                     unsigned query_flags)
  : LineQuery(new LineQueryDetail)
{
  setMap(&map);
  setStartPoint(start_point);
  setEndPoint(end_point);
  setSearchRadius(search_radius);
  setQueryFlags(query_flags);
}


LineQuery::~LineQuery()
{
  LineQueryDetail *d = imp();
  delete d;
  // Clear pointer for base class.
  imp_ = nullptr;
}


glm::dvec3 LineQuery::startPoint() const
{
  const LineQueryDetail *d = imp();
  return d->start_point;
}


void LineQuery::setStartPoint(const glm::dvec3 &point)
{
  LineQueryDetail *d = imp();
  d->start_point = point;
}


glm::dvec3 LineQuery::endPoint() const
{
  const LineQueryDetail *d = imp();
  return d->end_point;
}


void LineQuery::setEndPoint(const glm::dvec3 &point)
{
  LineQueryDetail *d = imp();
  d->end_point = point;
}


float LineQuery::searchRadius() const
{
  const LineQueryDetail *d = imp();
  return d->search_radius;
}


void LineQuery::setSearchRadius(float radius)
{
  LineQueryDetail *d = imp();
  d->search_radius = radius;
}


float LineQuery::defaultRangeValue() const
{
  const LineQueryDetail *d = imp();
  return d->default_range;
}


void LineQuery::setDefaultRangeValue(float range)
{
  LineQueryDetail *d = imp();
  d->default_range = range;
}


glm::vec3 LineQuery::axisScaling() const
{
  const LineQueryDetail *d = imp();
  return d->axis_scaling;
}


void LineQuery::setAxisScaling(const glm::vec3 &scaling)
{
  LineQueryDetail *d = imp();
  d->axis_scaling = scaling;
}


bool LineQuery::onExecute()
{
  LineQueryDetail *d = imp();

  if (!d->map)
  {
    return false;
  }

  ClosestResult closest;
  occupancyLineQueryCpu(*d->map, *d, closest);

  if ((d->query_flags & kQfNearestResult) && !d->intersected_voxels.empty())
  {
    d->intersected_voxels[0] = d->intersected_voxels[closest.index];
    d->intersected_voxels.resize(1);
    d->ranges[0] = d->ranges[closest.index];
    d->number_of_results = 1u;
    d->ranges.resize(1);
  }
  else
  {
    d->number_of_results = d->intersected_voxels.size();
  }

  return true;
}


bool LineQuery::onExecuteAsync()
{
  return false;
}


void LineQuery::onReset(bool /*hard_reset*/)
{
  // LineQueryDetail *d = imp();
}


LineQueryDetail *LineQuery::imp()
{
  return static_cast<LineQueryDetail *>(imp_);
}


const LineQueryDetail *LineQuery::imp() const
{
  return static_cast<const LineQueryDetail *>(imp_);
}
}  // namespace ohm


================================================
FILE: ohm/LineQuery.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_LINEQUERY_H
#define OHM_LINEQUERY_H

#include "OhmConfig.h"

#include "Query.h"
#include "QueryFlag.h"

#include <glm/fwd.hpp>

namespace ohm
{
class GpuMap;
struct LineQueryDetail;

/// A line segment intersection query for an @c OccupancyMap.
///
/// A line segment query determines the range of voxels for which intersect a given line segment and for each such
/// voxels, calculates the range to the nearest obstructed voxel. Obstructed voxels are defined as occupied voxels
/// and optionally unknown voxels when the query flag @c QF_UnknownAsOccupied is set.
///
/// The results are given in @c intersectedVoxels() and @c ranges(). The @c intersectedVoxels() identify the voxels
/// touched by the line segment, in order, from start to end. The corresponding @c ranges() yield the nearest
/// obstacle range for each voxel in @c intersectedVoxels(), or -1 if there is no voxel within the search range.
/// Setting the flag @c QF_NearestResult modifies the results to only report one voxel which has the shortest
/// range value (excluding unobstructed voxels). The result set may be empty in the case where @c QF_NearestResult is
/// set and all voxels along the line have no obstructions within the search range.
///
/// The CPU implementation is O(nmm) where n is the number of voxels in the line and m is defined by search radius
/// divided by the map resolution (i.e., the number of voxels required to reach the search radius). The GPU
/// implementation is closer to worst case O(m) although it incurs additional, initial overhead. The GPU
/// implementation supports caching to offset this overhead.
///
/// Using the GPU implementation, the @c LineQuery invokes the @c VoxelRanges query in order to cache the obstacle
/// ranges. On a soft @c reset(), the obstacle ranges are only recalculated for regions which have not yet
/// been calculated. A hard @c reset() should be invoked whenever the map changes to recalculate these ranges.
///
/// Note that a @c VoxelRanges query pointer may be given to the constructor to use an external @c VoxelRanges
/// query object. In this case this object does not take ownership of the external @c VoxelRanges query object.
///
/// @todo Add a flag which will early out of completing the query when the line enters a region which contains only
/// uncertain voxels (or does not exist). This is to help avoid creating those regions when using GPU queries
/// based on the assumption that we are not interested in continuing the query when it is wholly obstructed.
///
/// @bug Update the comment without reference to defunct VoxelRange, instead noting that ClearanceProcess is
/// required and precalculated clearance values may be assumed in GPU mode. searchRadius() is ingored in this
/// mode.
class ohm_API LineQuery : public Query
{
public:
  /// Default flags to execute this query with.
  static const unsigned kDefaultFlags = kQfNoCache;

protected:
  /// Constructor used for inherited objects. This supports deriving @p LineQueryDetail into
  /// more specialised forms.
  /// @param detail pimple style data structure. When null, a @c LineQueryDetail is allocated by
  /// this method.
  explicit LineQuery(LineQueryDetail *detail);

public:
  LineQuery();

  /// Construct a new query using the given parameters.
  /// @param map The map to perform the query on.
  /// @param start_point The global coordinate marking the start of the line segment.
  /// @param end_point The global coordinate marking the end of the line segment.
  /// @param search_radius Defines the "width" of the line. See @c searchRadius().
  /// @param query_flags Flags controlling the query behaviour. See @c QueryFlag and @c LineQuery::Flag.
  LineQuery(OccupancyMap &map, const glm::dvec3 &start_point, const glm::dvec3 &end_point, float search_radius,
            unsigned query_flags = kDefaultFlags);

  /// Destructor.
  ~LineQuery() override;

  /// Get the query's line segment start point.
  /// @return The start point in global coordinates.
  glm::dvec3 startPoint() const;
  /// Set the query's line segment start point.
  /// @param point The start point in global coordinates.
  void setStartPoint(const glm::dvec3 &point);

  /// Get the query's line segment end point.
  /// @return The end point in global coordinates.
  glm::dvec3 endPoint() const;
  /// Set the query's line segment end point.
  /// @param point The end point in global coordinates.
  void setEndPoint(const glm::dvec3 &point);

  /// Get the search radius around the line segment.
  /// @return The search radius around the line segment used by the query.
  float searchRadius() const;
  /// Set the search radius around the line segment. This effectively defines a width for the line segment.
  /// All reported results are within this distance of the line segment.
  /// @param radius The new search radius.
  void setSearchRadius(float radius);

  /// Get the range value used when there are no obstructions within range.
  /// @return The range value reported for voxels with no obstructions in range.
  /// @see setDefaultRangeValue()
  float defaultRangeValue() const;
  /// Set the range value used when there are no obstructions for a voxel within the @c searchRadius().
  /// The default is -1.
  ///
  /// Note that setting this value to be smaller than the @c searchRadius() is ambiguous.
  /// @param range The range to report for voxels with no obstructions within range.
  void setDefaultRangeValue(float range);

  /// Get the axis weightings applied when determining the nearest obstructing voxel.
  /// @return Current axis weighting.
  /// @see @c setAxisScaling()
  glm::vec3 axisScaling() const;

  /// Set the per axis scaling applied when determining the closest obstructing voxel.
  ///
  /// This vector effectively distorts the map using the following scaling matrix:
  /// @code{.unparsed}
  ///   axisScaling.x   0               0
  ///   0               axisScaling.y   0
  ///   0               0               axisScaling.z
  /// @endcode
  ///
  /// The default scaling of (1, 1, 1) has no overall impact, meaning that the closest obstructing voxel is
  /// selected. Scaling of (1, 1, 2) scales the Z axis by a factor of two, making the Z axis half as important as
  /// either the X or Y axes.
  ///
  /// Results are scaled by calculating the distance between two voxels as show in the following pseudo code:
  /// @code
  ///   separation = nearestObstacleCentre - voxelCentre;
  ///   separation.x *= 1.0f * axisScaling.x;
  ///   separation.y *= 1.0f * axisScaling.y;
  ///   separation.z *= 1.0f * axisScaling.z;
  ///   float range = length(separation)
  /// @endcode
  ///
  /// Note a zero value causes the scaling on that axis to be zero, making all voxels on that axis logically,
  /// equidistant. This may yield undefined resuts. Negative values are not recommended, but will effectively be
  /// equivalent to their absolute value.
  ///
  /// Modifying this value will require a cache reset (@c reset(true)) to recalculate ranges.
  ///
  /// @param scaling The new axis scaling to apply.
  void setAxisScaling(const glm::vec3 &scaling);

protected:
  bool onExecute() override;
  bool onExecuteAsync() override;
  void onReset(bool hard_reset) override;

  /// Access internal details.
  /// @return Internal details.
  LineQueryDetail *imp();
  /// Access internal details.
  /// @return Internal details.
  const LineQueryDetail *imp() const;
};
}  // namespace ohm

#endif  // OHM_LINEQUERY_H


================================================
FILE: ohm/LineWalk.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_LINEWALK_H
#define OHM_LINEWALK_H

#include "Key.h"
#include "OccupancyMap.h"

#include <glm/glm.hpp>

#include <array>
#include <cassert>
#include <functional>

namespace ohm
{
class Key;

/// Function signature for the visit function called from @c walkSegmentKeys().
/// @param key The key of the current voxel being visited.
/// @param enter_range Range at which the voxel is entered. detail required
/// @param exit_range Range at which the voxel is entered. detail required
/// @return True to keep walking the voxels along the ray, false to abort walking the ray.
using WalkVisitFunction = std::function<bool(const Key &, double, double)>;

/// A utility key adaptor around an @c OccupancyMap for use with @c walkSegmentKeys() .
struct ohm_API LineWalkContext
{
  /// Map reference.
  const OccupancyMap &map;
  /// Cached value of @c OccupancyMap::regionVoxelDimensions().
  const glm::ivec3 region_voxel_dimensions;
  /// Function to invoke to visit each voxel.
  const WalkVisitFunction visit;

  /// Create an adaptor for @p map .
  /// @param map The map to adapt.
  /// @param visit The function to call when visiting each voxel.
  inline LineWalkContext(const OccupancyMap &map, WalkVisitFunction visit)
    : map(map)
    , region_voxel_dimensions(map.regionVoxelDimensions())
    , visit(visit)
  {}
};

/// Flags for use with @c walkSegmentKeys() .
enum WalkKeyFlag : unsigned
{
  /// Skip reporting the voxel containing the start point/origin if different from the end point?
  kExcludeStartVoxel = (1u << 0u),
  /// Skip reporting the voxel containing the end point/sample if different from the start point.
  kExcludeEndVoxel = (1u << 1u),
};


namespace detail
{
using WalkContext = const LineWalkContext;
inline void walkKeyDiff(const LineWalkContext *context, int diff[3], const Key *key_a, const Key *key_b)
{
  (void)context;  // Unused
  const glm::ivec3 diff_iv3 = OccupancyMap::rangeBetween(*key_b, *key_a, context->region_voxel_dimensions);
  diff[0] = diff_iv3.x;
  diff[1] = diff_iv3.y;
  diff[2] = diff_iv3.z;
}


/// Adjust the value of @p key by stepping it along @p axis
/// @param key The key to modify.
/// @param axis The axis to modifier where 0, 1, 2 map to X, Y, Z respectively.
/// @param dir The direction to step: must be 1 or -1
inline void walkStepKey(const LineWalkContext *context, Key *key, int axis, int step_dir)
{
  OccupancyMap::stepKey(*key, axis, step_dir, context->region_voxel_dimensions);
}


inline bool walkVisitVoxel(const LineWalkContext *context, const Key *voxel_key, const Key *start_key,
                           const Key *end_key, unsigned voxel_marker, double enter_time, double exit_time)
{
  (void)start_key;     // Unused.
  (void)end_key;       // Unused.
  (void)voxel_marker;  // Unused.
  return context->visit(*voxel_key, enter_time, exit_time);
}

#include "LineWalkCompute.h"
}  // namespace detail


/// Implements the voxel tracing algorithm with the given @p context .
///
/// This traces the voxels intersected by the line segment from @p start_point to @p end_point invoking
/// @c context.visit for each voxel intersected by the line segment. Each such visit passes the @c Key of the visited
/// voxel and the distanaces from the @p start_point at which the voxel is entered and exited.
///
/// See @c walkLineVoxels() for further implementation details.
///
/// @param context Context within which we are tracing. Provides the @c OccupancyMap and a @c WalkVisitFunction to
///   invoke.
/// @param start_point The start of the line in 3D space.
/// @param end_point The end of the line in 3D space.
/// @param flags Flags from @c WalkKeyFlag. Should be @c true if @p walkFunc should be called for the voxel containing
///   @c endPoint, when it does not lie in the same voxel as @p startPoint.
/// @param length_epsilon The segment length below which a ray is considered degenerate and will only report the start
///   voxel.
/// @return The number of voxels traversed. This includes @p end_point when @p include_end_point is true.
inline unsigned walkSegmentKeys(const LineWalkContext &context, const glm::dvec3 &start_point,
                                const glm::dvec3 &end_point, unsigned flags = 0u,
                                double length_epsilon = 1e-6)  // NOLINT(readability-magic-numbers)
{
  const Key start_point_key = context.map.voxelKey(start_point);
  const Key end_point_key = context.map.voxelKey(end_point);

  if (start_point_key.isNull() || end_point_key.isNull())
  {
    return 0;
  }

  const glm::dvec3 start_voxel_centre = context.map.voxelCentreGlobal(start_point_key);
  const glm::dvec3 voxel_resolution(context.map.resolution());

  return detail::walkLineVoxels(&context, start_point, end_point, &start_point_key, &end_point_key, start_voxel_centre,
                                voxel_resolution, flags, length_epsilon);
}
}  // namespace ohm

#endif  // OHM_LINEWALK_H


================================================
FILE: ohm/LineWalkCompute.h
================================================
// Copyright (c) 2022
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_LINEWALKCOMPUTE_H
#define OHM_LINEWALKCOMPUTE_H

// Note: this header is included in GPU code.
// When included from CPU code, note that all functions are static and can be either wrapped in a class or private
// translation unit.

/// @defgroup linewalk Line Walking Usage
/// Shared CPU/GPU line walking code. Because of the shared nature of this code, there are many peculiarities.
/// There are several types and functions which must be defined before including this code as specified below.
/// There are also several aliased types which differ between CPU and GPU.
///
/// - @c WalkContext is a struct which must be defined for use with the functions below. This holds user data passed to
/// the callback functions listed below.
/// - @c WalkKey is defined as @c ohm::Key for CPU and @c GpuKey for GPU.
/// - @c WalkVec3 is defined as @c glm::dvec3 for CPU and @c float3 for GPU.
/// - @c WalkReal is defined as @c double for CPU and @c float for GPU.
/// - Two constant bit flag values: @c kExcludeStartVoxel and @c kExcludeEndVoxel . See @c WalkLinkVoxelFlags below for
///   an example definition.
///
/// The required function definitions are:
///
/// @code{.c}
/// /// Calculate the number of voxels along each axis separating two keys.
/// /// @param context The @c WalkContext pointer. May optionally be @c const qualified.
/// /// @param[out] diff Storage diff. Logically as `diff[i] = key_a[i] - key_b[i]` with @c i is `[0, 1, 2]`.
/// /// @param key_a The key to subtract from.
/// /// @param key_b The key to subtract.
/// void walkKeyDiff(WalkContext *context, int diff[3], WalkKey *key_a, WalkKey *key_b)
///
/// /// Step a key, one voxel along an axis in the specified direction.
/// /// @param context The @c WalkContext pointer. May optionally be @c const qualified.
/// /// @param[in,out] key The key to modify.
/// /// @param axis The axis to step: `[0, 1, 2]`.
/// /// @param step_dir The direction to step: `[-1, 1]`.
/// void walkStepKey(WalkContext *context, WalkKey *key, int axis, int step_dir)
///
/// /// Function called to visit each voxel traced along the line. Behaviour is user defined.
/// /// @param context The @c WalkContext pointer. May optionally be @c const qualified.
/// /// @param voxel_key The current voxel being visited.
/// /// @param walk_start_key The first voxel visited by the walk.
/// /// @param walk_end_key The last voxel to be visited by the walk.
/// /// @param voxel_marker Identifies the nature of @p voxel_key as one of
/// ///     `[kLineWalkMarkerSegment, kLineWalkMarkerStart, kLineWalkMarkerEnd]`
/// /// @param enter_range The distance which has been traces along the line at which we enter @c voxel_key .
/// /// @param exit_range The distance which has been traces along the line at which we exit @c voxel_key .
/// bool walkVisitVoxel(WalkContext *context, const WalkKey *voxel_key, const WalkKey *walk_start_key,
///                     const WalkKey *walk_end_key, unsigned voxel_marker, int voxel_marker,
///                      WalkReal enter_range, WalkReal exit_range)
///
/// /// Flags for use with @c walkLineVoxels() .
/// enum WalkLinkVoxelFlags
/// {
///   /// Skip reporting the voxel containing the start point if different from the end point?
///   kExcludeStartVoxel = (1u << 0u),
///   /// Skip reporting the voxel containing the end point if different from the start point.
///   kExcludeEndVoxel = (1u << 1u)
/// };
/// @endcode
///
/// Note that the shared CPU/GPU code may prove less efficient for GPU. During the conversion, it was notes that the
/// thread occupancy decreased for NDT and TSDF algorithms on an NVIDIA card (Intel OpenCL was unaffected due to small
/// maximum work group sizes). While overall performance seemed was not impacted, this may increase the overall GPU
/// usage and descrease the efficiency as the number of warps used increases. Should this prove an issue, it will
/// be necessary to revisit splitting the algorithm in order to decrease the amount of memory used by each GPU thread,
/// in particular focusing on reducing stack variable usage.

#if GPUTIL_DEVICE
#define WALK_FUNC __device__

// Define GPU type aliases
typedef GpuKey WalkKey;
typedef float3 WalkVec3;
typedef float WalkReal;

__device__ inline float walkInfinity()
{
  return INFINITY;
}

__device__ inline float walkHalf()
{
  return 0.5f;
}

__device__ inline bool walkEqualKeys(const WalkKey *key_a, const WalkKey *key_b)
{
  // From GpuKey.h
  return equalKeys(key_a, key_b);
}

#if GPUTIL_DEVICE == GPUTIL_OPENCL
#define walkCopyKey copyKey
#else   // GPUTIL_DEVICE == GPUTIL_OPENCL
__device__ inline void walkCopyKey(WalkKey *dst, const WalkKey *src)
{
  // From GpuKey.h
  copyKey(dst, src);
}
#endif  // GPUTIL_DEVICE == GPUTIL_OPENCL

#else  // GPUTIL_DEVICE
#define WALK_FUNC

// Define CPU types
using WalkKey = ohm::Key;
using WalkVec3 = glm::dvec3;
using WalkReal = double;

WALK_FUNC inline WalkReal walkInfinity()
{
  return std::numeric_limits<WalkReal>::infinity();
}


// Constants to address OpenCL floating point literal strictness.
WALK_FUNC inline WalkReal walkHalf()
{
  return 0.5;
}

WALK_FUNC inline bool walkEqualKeys(const WalkKey *key_a, const WalkKey *key_b)
{
  return *key_a == *key_b;
}

WALK_FUNC inline void walkCopyKey(WalkKey *dst, const WalkKey *src)
{
  *dst = *src;
}

#endif  // GPUTIL_DEVICE

/// Enumeration describing the type of voxel being reported on a line walk callback.
typedef enum LineWalkMarker
{
  /// Voxel inside the line segment.
  kLineWalkMarkerSegment = 0,
  /// First voxel in the line segment.
  kLineWalkMarkerStart = 1,
  /// Last voxel in the line segment.
  kLineWalkMarkerEnd = 2
} LineWalkMarker;

/// Temporary ray data used to initialise the line walk.
typedef struct LineWalkRay
{
  WalkVec3 origin;                ///< Start point of the line/ray.
  WalkVec3 direction;             ///< Direction traced by the ray: unit length.
  WalkVec3 direction_inverse;     ///< Inverse of @c direction.
  int sign[3];                    ///< Per axis tracing direction.
  WalkReal initial_exit_time[3];  ///< Time at which the start voxel is exited (per axis).
  WalkReal step_delta[3];         ///< Time step for each axis.
  WalkReal length;  ///< Total length of the line segment traces. End point is `origin + direction * length`.
} LineWalkRay;

WALK_FUNC inline int walkStepDir(int sign)
{
  // Derive the step direction from the ray->sign. The ray->sign values are 0 for positive step direction, 1 for
  // a negative step direction. Thus we can resolve the step direction as:
  // -2 * sign + 1
  //
  // When sign is 0:
  //  -2 * 0 + 1 = 1
  // When sign is 1:
  //  -2 * 1 + 1 = -1
  return -2 * sign + 1;
}

/// Calculate the times at which the ray will exit a voxel wall along each axis.
/// Note: @p exit_time must have space for three elements to be written back to it.
WALK_FUNC inline void walkCalculateVoxelWallExit(const LineWalkRay *ray, const WalkVec3 voxel_min,
                                                 const WalkVec3 voxel_max, WalkReal *exit_time)
{
  // Based on:
  // https://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-box-intersection
  const WalkVec3 *bounds[2] = { &voxel_min, &voxel_max };
  exit_time[0] = (bounds[1 - ray->sign[0]]->x - ray->origin.x) * ray->direction_inverse.x;
  exit_time[1] = (bounds[1 - ray->sign[1]]->y - ray->origin.y) * ray->direction_inverse.y;
  exit_time[2] = (bounds[1 - ray->sign[2]]->z - ray->origin.z) * ray->direction_inverse.z;
}

WALK_FUNC inline void walkInitRay(LineWalkRay *ray, const WalkVec3 start, const WalkVec3 end,
                                  const WalkVec3 start_voxel_centre, const WalkVec3 voxel_resolution,
                                  WalkReal length_epsilon)
{
  ray->origin = start;
  ray->direction = end - start;
  ray->length =
    ray->direction.x * ray->direction.x + ray->direction.y * ray->direction.y + ray->direction.z * ray->direction.z;
  ray->length = (ray->length > length_epsilon) ? sqrt(ray->length) : 0;

  // Resolve the direction before we potentially divide by zero as we can for very small rays.
  // This at leasts sets the ray direction correctly.
  ray->sign[0] = ray->direction.x < 0;
  ray->sign[1] = ray->direction.y < 0;
  ray->sign[2] = ray->direction.z < 0;

  ray->direction.x /= ray->length;
  ray->direction.y /= ray->length;
  ray->direction.z /= ray->length;

  ray->direction_inverse.x = (ray->length > 0) ? 1 / ray->direction.x : 0;
  ray->direction_inverse.y = (ray->length > 0) ? 1 / ray->direction.y : 0;
  ray->direction_inverse.z = (ray->length > 0) ? 1 / ray->direction.z : 0;

  // Calculate how much the exit time changes along an axis each time we leave an axis.
  // We start with the initial voxel to also calculate start_voxel_centre, but it could be arbitrary for the delta
  // calculation. Then we calculate the exit time for the next voxel along each axis.
  WalkVec3 voxel_min = start_voxel_centre;
  WalkVec3 voxel_max = start_voxel_centre;

  voxel_min -= walkHalf() * voxel_resolution;
  voxel_max += walkHalf() * voxel_resolution;

  walkCalculateVoxelWallExit(ray, voxel_min, voxel_max, ray->initial_exit_time);

  // Move the voxel along on each axis.
  WalkVec3 voxel_shift;
  voxel_shift.x = walkStepDir(ray->sign[0]) * voxel_resolution.x;
  voxel_shift.y = walkStepDir(ray->sign[1]) * voxel_resolution.y;
  voxel_shift.z = walkStepDir(ray->sign[2]) * voxel_resolution.z;

  voxel_min += voxel_shift;
  voxel_max += voxel_shift;

  // Calculate the time to exit the next voxel wall along each axis.
  walkCalculateVoxelWallExit(ray, voxel_min, voxel_max, ray->step_delta);

  // The difference between them is the step delta. This is invariant for this ray.
  if (ray->step_delta[0] != walkInfinity())
  {
    ray->step_delta[0] -= ray->initial_exit_time[0];
  }
  if (ray->step_delta[1] != walkInfinity())
  {
    ray->step_delta[1] -= ray->initial_exit_time[1];
  }
  if (ray->step_delta[2] != walkInfinity())
  {
    ray->step_delta[2] -= ray->initial_exit_time[2];
  }
}

/// Data structure used to to track line segment tracing progress.
typedef struct WalkSteps
{
  WalkReal time_next[3];      ///< Time to next voxel on each axis.
  WalkReal initial_delta[3];  ///< Initial offset for each axis accounting for start point offset from centre.
  WalkReal step_delta[3];     ///< Per axis time delta applied on each step.
  int sign[3];                ///< Step direction for each axis.
  WalkReal length;            ///< Total segment length to trace.
} WalkSteps;

WALK_FUNC inline void walkCalculateSteps(WalkSteps *walk_steps, const WalkVec3 start_point, const WalkVec3 end_point,
                                         const WalkVec3 start_voxel_centre, const WalkVec3 voxel_resolution,
                                         WalkReal length_epsilon)
{
  LineWalkRay ray;
  walkInitRay(&ray, start_point, end_point, start_voxel_centre, voxel_resolution, length_epsilon);

  walk_steps->initial_delta[0] = walk_steps->time_next[0] = ray.initial_exit_time[0];
  walk_steps->initial_delta[1] = walk_steps->time_next[1] = ray.initial_exit_time[1];
  walk_steps->initial_delta[2] = walk_steps->time_next[2] = ray.initial_exit_time[2];

  walk_steps->step_delta[0] = ray.step_delta[0];
  walk_steps->step_delta[1] = ray.step_delta[1];
  walk_steps->step_delta[2] = ray.step_delta[2];

  walk_steps->sign[0] = ray.sign[0];
  walk_steps->sign[1] = ray.sign[1];
  walk_steps->sign[2] = ray.sign[2];

  walk_steps->length = ray.length;
}

WALK_FUNC inline int walkSelectNextAxis(const WalkReal *time_next)
{
  // Select next axis based on the earliest next time. An expired axis will have infinity for next time.
  int axis = 0;
  axis = (time_next[axis] < time_next[1]) ? axis : 1;
  axis = (time_next[axis] < time_next[2]) ? axis : 2;
  return axis;
}

WALK_FUNC inline unsigned walkStepNext(WalkContext *context, WalkSteps *steps, WalkKey *current_key, unsigned *axis,
                                       int *steps_remaining, int *stepped)
{
  const int step_dir = walkStepDir(steps->sign[*axis]);
  walkStepKey(context, current_key, *axis, step_dir);
  steps_remaining[*axis] -= step_dir;
  stepped[*axis] += step_dir;
  steps->time_next[*axis] = (steps_remaining[*axis]) ?
                              steps->initial_delta[*axis] + steps->step_delta[*axis] * abs(stepped[*axis]) :
                              walkInfinity();
  const unsigned limit_flags_change = !!(steps_remaining[*axis] == 0) * (1u << *axis);

  // Choose the next step axis.
  *axis = walkSelectNextAxis(steps->time_next);

  return limit_flags_change;
}

/// Line walking function for tracing voxels between two points/voxels.
///
/// This function depends on some additional definitions before including this file. See @ref linewalk .
///
/// The original version of this algorithm was based on
///
/// > J. Amanatides and A. Woo, "A fast voxel traversal algorithm for raytracing," 1987.
///
/// However that algorithm was found to be inaccurate for tracing points which were not at voxel centres, though this
/// updated algorithm has many similarities. Conceptually, the algorithm performs ray/AABB intersection tests to see
/// which voxel wall will be exited fist. This wall is chosen for the next step.
///
/// In practice, we only need to perform a two ray/AABB tests on initialisation. The first is to calculate the times at
/// which each of the three axis planes of the first voxel will be left. The second is used to calculate the times for
/// leaving the next voxel along each axis. Because voxels are uniform sizes, we can use the difference between the two
/// values as a time delta between each voxel axis along each axis. Note this leverages the knowledge that the ray
/// continues in the same direction along each axis.
///
/// We walk the ray by choosing the smallest next exit time, sepping to  the next voxel on that axis. The next time
/// for that axis is incremented by the axis time delta.
///
/// For each voxel, we call @c walkVisitVoxel() as described in @ref linewalk.
///
/// @param context User data for the ray trace. Passed to the helper functions.
/// @param start_point The start of the line in 3D space.
/// @param end_point The end of the line in 3D space.
/// @param start_point_key The key for the voxel containing @c start_point .
/// @param end_point_key The key for the voxel containing @c end_point .
/// @param start_voxel_centre The coordinate for the centre of the voxel containing @c start_point .
/// @param flags Flags from @c WalkLinkVoxelFlags allowing the start/end point voxels to be skipped with no call to
///   @c walkVisitVoxel() .
/// @param include_end_point @c true to call @p walkVisitVoxel() the voxel containing @c endPoint, when it does not lie
///   in the same voxel as @p start_point.
/// @param length_epsilon Epsilon used to detect small rays. Such rays are considered to have no length.
/// @return The number of voxels traversed. This includes @p end_point when @p include_end_point is true and does not
///   otherwise.
WALK_FUNC inline unsigned walkLineVoxels(WalkContext *context, const WalkVec3 start_point, const WalkVec3 end_point,
                                         const WalkKey *start_point_key, const WalkKey *end_point_key,
                                         const WalkVec3 start_voxel_centre, const WalkVec3 voxel_resolution,
                                         unsigned flags,           // include_end_point,
                                         WalkReal length_epsilon)  // NOLINT(readability-magic-numbers)
{
  WalkSteps steps;
  walkCalculateSteps(&steps, start_point, end_point, start_voxel_centre, voxel_resolution, length_epsilon);

  int steps_remaining[3] = { 0, 0, 0 };
  int stepped[3] = { 0, 0, 0 };

  walkKeyDiff(context, steps_remaining, end_point_key, start_point_key);

  WalkKey current_key;
  walkCopyKey(&current_key, start_point_key);

  WalkReal last_time = 0;
  unsigned axis = 0;
  unsigned voxel_count = 0;
  unsigned limit_flags = 0;
  bool continue_traversal = true;

  // Initialise limit flags to mark which axes won't be stepped. Bits 0, 1, 2 map to axis X, Y, Z respectively.
  limit_flags |= !!(steps_remaining[0] == 0) * (1u << 0u);
  limit_flags |= !!(steps_remaining[1] == 0) * (1u << 1u);
  limit_flags |= !!(steps_remaining[2] == 0) * (1u << 2u);

  for (int i = 0; i < 3; ++i)
  {
    steps.time_next[i] = (steps_remaining[i]) ? steps.initial_delta[i] : walkInfinity();
  }

  // Select next axis based on the earliest next time.
  axis = walkSelectNextAxis(steps.time_next);

  int voxel_marker = kLineWalkMarkerStart;
  if (flags & kExcludeStartVoxel)
  {
    voxel_marker = kLineWalkMarkerSegment;
    last_time = steps.time_next[axis];
    ++voxel_count;

    // Step on from the current voxel.
    limit_flags |= walkStepNext(context, &steps, &current_key, &axis, steps_remaining, stepped);
  }

  while (continue_traversal && limit_flags < 7u && !walkEqualKeys(&current_key, end_point_key))
  {
    // Visit the current voxel.
    continue_traversal = walkVisitVoxel(context, &current_key, start_point_key, end_point_key, voxel_marker, last_time,
                                        steps.time_next[axis]);
    voxel_marker = kLineWalkMarkerSegment;
    last_time = steps.time_next[axis];
    ++voxel_count;

    // Step on from the current voxel.
    limit_flags |= walkStepNext(context, &steps, &current_key, &axis, steps_remaining, stepped);
  }

  // Touch the last voxel.
  if (continue_traversal && (flags & kExcludeEndVoxel) == 0u)
  {
    walkVisitVoxel(context, end_point_key, start_point_key, end_point_key, kLineWalkMarkerEnd, last_time, steps.length);
    ++voxel_count;
  }

  return voxel_count;
}

#endif  // OHM_LINEWALKCOMPUTE_H


================================================
FILE: ohm/MapChunk.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapChunk.h"

#include "DefaultLayer.h"
#include "MapLayer.h"
#include "MapLayout.h"
#include "VoxelBuffer.h"
#include "VoxelOccupancy.h"
#include "VoxelTsdf.h"

#include "private/MapLayoutDetail.h"
#include "private/OccupancyMapDetail.h"

#include <cassert>
#include <cstdio>

namespace ohm
{
MapChunk::MapChunk(const MapRegion &region, const OccupancyMapDetail &map)
{
  this->region = region;
  this->map = &map;

  const MapLayout &layout = this->layout();
  voxel_blocks.resize(layout.layerCount());
  touched_stamps = std::make_unique<std::atomic_uint64_t[]>(layout.layerCount());  // NOLINT(modernize-avoid-c-arrays)
  for (size_t i = 0; i < layout.layerCount(); ++i)
  {
    const MapLayer &layer = layout.layer(i);
    voxel_blocks[i].reset(new VoxelBlock(&map, layer));
    touched_stamps[i] = 0u;
  }
}


MapChunk::MapChunk(const OccupancyMapDetail &map)
  : MapChunk(MapRegion(), map)
{}


MapChunk::MapChunk(MapChunk &&other) noexcept
  : region(std::exchange(other.region, MapRegion()))
  , map(std::exchange(other.map, nullptr))
  , first_valid_index(std::exchange(other.first_valid_index, ~0u))
  , touched_time(std::exchange(other.touched_time, 0))
  , dirty_stamp(other.dirty_stamp.load())
  , touched_stamps(std::move(other.touched_stamps))
  , voxel_blocks(std::move(other.voxel_blocks))
  , flags(std::exchange(other.flags, 0))
{}


MapChunk::~MapChunk() = default;


const MapLayout &MapChunk::layout() const
{
  return map->layout;
}


Key MapChunk::keyForIndex(size_t voxel_index, const glm::ivec3 &region_voxel_dimensions,
                          const glm::i16vec3 &region_coord)
{
  Key key;

  if (voxel_index < unsigned(region_voxel_dimensions.x * region_voxel_dimensions.y * region_voxel_dimensions.z))
  {
    key.setRegionKey(region_coord);

    size_t local_coord = voxel_index % region_voxel_dimensions.x;
    key.setLocalAxis(0, uint8_t(local_coord));
    voxel_index /= region_voxel_dimensions.x;
    local_coord = voxel_index % region_voxel_dimensions.y;
    key.setLocalAxis(1, uint8_t(local_coord));
    voxel_index /= region_voxel_dimensions.y;
    local_coord = voxel_index;
    key.setLocalAxis(2, uint8_t(local_coord));
  }
  else
  {
    key = Key::kNull;
  }

  return key;
}


void MapChunk::updateLayout(const MapLayout *new_layout,
                            const std::vector<std::pair<const MapLayer *, const MapLayer *>> &preserve_layer_mapping)
{
  // Allocate voxel pointer array.
  std::vector<VoxelBlock::Ptr> new_voxel_blocks(new_layout->layerCount());
  std::unique_ptr<std::atomic_uint64_t[]> new_touched_stamps =           // NOLINT(modernize-avoid-c-arrays)
    std::make_unique<std::atomic_uint64_t[]>(new_layout->layerCount());  // NOLINT(modernize-avoid-c-arrays)

  for (const auto &mapping : preserve_layer_mapping)
  {
    new_voxel_blocks[mapping.second->layerIndex()].swap(voxel_blocks[mapping.first->layerIndex()]);
    new_touched_stamps[mapping.second->layerIndex()] = touched_stamps[mapping.first->layerIndex()].load();
    // Memory ownership moved: nullify to prevent release.
    voxel_blocks[mapping.first->layerIndex()] = nullptr;
  }

  // Now initialise any new or unmapped layers and release those not preserved.
  for (size_t i = 0; i < new_layout->layerCount(); ++i)
  {
    const MapLayer &layer = new_layout->layer(i);
    if (!new_voxel_blocks[i])
    {
      // Initilised layer.
      new_voxel_blocks[i].reset(new VoxelBlock(map, layer));
      new_touched_stamps[i] = 0u;
    }
    else
    {
      // Preserving layer. Ensure it's index is updated.
      new_voxel_blocks[i]->updateLayerIndex(layer.layerIndex());
    }
  }

  // Release redundant layers.
  const MapLayout &old_layout = layout();
  for (size_t i = 0; i < old_layout.layerCount(); ++i)
  {
    if (voxel_blocks[i])
    {
      // Unmigrated/redudant layer. Release.
      voxel_blocks[i]->destroy();
      voxel_blocks[i] = nullptr;
    }
  }

  // Update pointers
  std::swap(voxel_blocks, new_voxel_blocks);
  std::swap(touched_stamps, new_touched_stamps);
  // We do nothing to update the layout() to new_layout. This object is owned by the occupancy map which we assume is
  // about to change internally. It's address will remain unchanged.
}


void MapChunk::searchAndUpdateFirstValid(const glm::ivec3 &region_voxel_dimensions, const glm::u8vec3 &search_from)
{
  const MapLayout &layout = this->layout();
  // First mark as unknown
  first_valid_index = ~0u;
  // Try by occupancy
  if (layout.occupancyLayer() != -1)
  {
    VoxelBuffer<const VoxelBlock> voxel_buffer(voxel_blocks[layout.occupancyLayer()]);
    const size_t voxel_stride = layout.layer(layout.occupancyLayer()).voxelByteSize();
    const uint8_t *voxel_mem = voxel_buffer.voxelMemory();

    unsigned voxel_index = 0;
    float occupancy = 0;
    bool found = false;
    for (int z = search_from.z; !found && z < region_voxel_dimensions.z; ++z)
    {
      for (int y = search_from.y; !found && y < region_voxel_dimensions.y; ++y)
      {
        for (int x = search_from.x; x < region_voxel_dimensions.x; ++x)
        {
          voxel_index =
            unsigned(x) + y * region_voxel_dimensions.x + z * region_voxel_dimensions.y * region_voxel_dimensions.x;
          memcpy(&occupancy, voxel_mem + voxel_stride * voxel_index, sizeof(occupancy));
          if (occupancy != unobservedOccupancyValue())
          {
            first_valid_index = std::min(voxel_index, first_valid_index);
            found = true;
            break;
          }
        }
      }
    }
  }
  // Try by TSDF
  // This is a bit intimate here. Needs a fix for better resolving TSDF vs occupancy maps.
  if (layout.layerIndex(default_layer::tsdfLayerName()) != -1)
  {
    const int layer_index = layout.layerIndex(default_layer::tsdfLayerName());
    VoxelBuffer<const VoxelBlock> voxel_buffer(voxel_blocks[layer_index]);
    const size_t voxel_stride = layout.layer(layer_index).voxelByteSize();
    const uint8_t *voxel_mem = voxel_buffer.voxelMemory();

    unsigned voxel_index = 0;
    VoxelTsdf tsdf{};
    bool found = false;
    for (int z = search_from.z; !found && z < region_voxel_dimensions.z; ++z)
    {
      for (int y = search_from.y; !found && y < region_voxel_dimensions.y; ++y)
      {
        for (int x = search_from.x; x < region_voxel_dimensions.x; ++x)
        {
          voxel_index =
            unsigned(x) + y * region_voxel_dimensions.x + z * region_voxel_dimensions.y * region_voxel_dimensions.x;
          memcpy(&tsdf, voxel_mem + voxel_stride * voxel_index, sizeof(tsdf));
          if (isValidTsdf(&tsdf))
          {
            first_valid_index = std::min(voxel_index, first_valid_index);
            found = true;
            break;
          }
        }
      }
    }
  }
}


glm::u8vec3 MapChunk::firstValidKey() const
{
  return firstValidKey(map->region_voxel_dimensions);
}


bool MapChunk::validateFirstValid() const
{
  const MapLayout &layout = this->layout();
  VoxelBuffer<const VoxelBlock> voxel_buffer(voxel_blocks[layout.occupancyLayer()].get());
  const size_t voxel_stride = layout.layer(layout.occupancyLayer()).voxelByteSize();
  const uint8_t *voxel_mem = voxel_buffer.voxelMemory();

  unsigned voxel_index = 0;
  float occupancy;
  for (int z = 0; z < map->region_voxel_dimensions.z; ++z)
  {
    for (int y = 0; y < map->region_voxel_dimensions.y; ++y)
    {
      for (int x = 0; x < map->region_voxel_dimensions.x; ++x)
      {
        memcpy(&occupancy, voxel_mem + voxel_stride * voxel_index, sizeof(occupancy));
        if (occupancy != unobservedOccupancyValue())
        {
          if (first_valid_index != voxel_index)
          {
            fprintf(stderr, "First valid validation failure. Current: (%d) actual: (%d)\n", int(first_valid_index),
                    int(voxel_index));
            return false;
          }
          return true;
        }
        ++voxel_index;
      }
    }
  }

  // No valid voxels.
  if (first_valid_index != ~0u)
  {
    fprintf(stderr, "First valid validation failure. Current: (%d) actual: (%d)\n", int(first_valid_index),
            int(voxel_index));
    return false;
  }

  return true;
}


bool MapChunk::overlapsExtents(const glm::dvec3 &min_ext, const glm::dvec3 &max_ext) const
{
  glm::dvec3 region_min;
  glm::dvec3 region_max;
  extents(region_min, region_max);

  const bool min_fail = glm::any(glm::greaterThan(region_min, max_ext));
  const bool max_fail = glm::any(glm::greaterThan(min_ext, region_max));

  return !min_fail && !max_fail;
}

void MapChunk::extents(glm::dvec3 &min_ext, glm::dvec3 &max_ext) const
{
  min_ext = max_ext = region.centre;
  min_ext -= 0.5 * map->region_spatial_dimensions;
  max_ext += 0.5 * map->region_spatial_dimensions;
}
}  // namespace ohm


================================================
FILE: ohm/MapChunk.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_MAPCHUNK_H
#define OHM_MAPCHUNK_H

#include "OhmConfig.h"

#include "Key.h"
#include "MapRegion.h"
#include "VoxelBlock.h"

#include <algorithm>
#include <atomic>
#include <utility>
#include <vector>

namespace ohm
{
class MapLayer;
class MapLayout;
struct OccupancyMapDetail;

/// Convert a 3D index into a @c MapChunk into a linear index into the chunk's voxels.
/// @param x The index into the chunk along the X axis.
/// @param y The index into the chunk along the Y axis.
/// @param z The index into the chunk along the Z axis.
/// @param dimx The number of voxels in a @c MapChunk along the X axis.
/// @param dimy The number of voxels in a @c MapChunk along the Y axis.
/// @param dimz The number of voxels in a @c MapChunk along the Z axis.
inline unsigned voxelIndex(unsigned x, unsigned y, unsigned z, unsigned dimx, unsigned dimy, unsigned /*dimz*/)
{
  return x + y * dimx + z * dimx * dimy;
}


/// @overload
inline unsigned voxelIndex(const glm::u8vec3 &key, const glm::ivec3 &dim)
{
  return key.x + key.y * dim.x + key.z * dim.x * dim.y;
}


/// @overload
inline unsigned voxelIndex(const Key &key, const glm::ivec3 &dim)
{
  return key.localKey().x + key.localKey().y * dim.x + key.localKey().z * dim.x * dim.y;
}


inline glm::u8vec3 voxelLocalKey(unsigned index, const glm::ivec3 &dim)
{
  return glm::u8vec3(index % dim.x, (index % (dim.x * dim.y)) / dim.x, index / (dim.x * dim.y));
}


/// Move a region local key to the next coordinate in that region. The operation is constrained by the region
/// dimensions @p dim.
///
/// The key movement is along the full extents of X, then X/Y, then X/Y/Z.
///
/// @param local_key The local_key to adjust.
/// @param dim The region voxel dimensions.
/// @return False if the key is out of range or at the limit of the region.
inline bool nextLocalKey(glm::u8vec3 &local_key, const glm::ivec3 &dim)
{
  if (local_key.x + 1 < dim.x)
  {
    ++local_key.x;
    return true;
  }
  if (local_key.y + 1 < dim.y)
  {
    local_key.x = 0;
    ++local_key.y;
    return true;
  }
  if (local_key.z + 1 < dim.z)
  {
    local_key.x = local_key.y = 0;
    ++local_key.z;
    return true;
  }

  return false;
}


/// Move a key's local key to reference the next coordinate in that region. The operation is constrained by the
/// region dimensions @p dim.
///
/// The key movement is along the full extents of X, then X/Y, then X/Y/Z.
///
/// @param key The key to adjust the local coordinates of.
/// @param dim The region voxel dimensions.
/// @return False if the key is out of range or at the limit of the region.
inline bool nextLocalKey(Key &key, const glm::ivec3 &dim)
{
  glm::u8vec3 local_key = key.localKey();
  if (nextLocalKey(local_key, dim))
  {
    key.setLocalKey(local_key);
    return true;
  }
  return false;
}


/// Internal representation of a section of the map.
///
/// A covers a contiguous, voxel region within the map. This structure associated
/// a @c MapRegion with a set voxel occupancy and clearance values (one each per voxel).
/// A coarse clearance array is also maintained, which is essentially a set of lower resolution
/// voxel representation of voxel @c clearance values.
///
/// Check chunk contains an array of @c voxel_maps, containing at least one element. The first element
/// is always the occupancy probability map, forming a contiguous block of memory to densely represent the
/// voxels in the chunk. Other elements are user defined, but the common set is:
/// - occupancy
/// - clearance values
/// - coarse clearance
///
/// The clearance values are 1-1 with the occupancy, while the coarse clearance map is a downsampled version of the
/// clearance map. Details of the available maps is stored in a @c MapLayout.
///
/// @par Voxel access
/// Direct access to the voxel data in @c voxel_maps is by far the fastest option for inner loops (as opposed to the
/// @c Voxel interface). However, it is a much more manual process and requires more care and process. In general
/// update logic for writing voxel data should be as follows:
/// - Resolve the desired layer indices via @c MapLayout and cache these values.
/// - Validate @c MapLayer size vs the expected size.
/// - Cache the @c MapLayer::dimensions() for use with @c voxelIndex() calls (below)
/// - Ensure GPU memory is synched if required - @c GpuMap::syncVoxels()
/// - Touch the map and cache the value @c OccupancyMap::touch()
/// - Generate the key of interest
/// - Request the @c MapChunk using @c Key::regionKey() and @c OccupancyMap::region() - see note below
/// - Get the memory for the required layer(s) - @c MapChunk::voxel_maps[layer_index]
/// - Cast the voxel memory to the expected type - e.g., @c float for occupancy, @c VoxelMean for the voxel mean layer
/// - Resolve the @c Key::localKey() into a one dimensional index using @c voxelIndex()
/// - Read/write to the indexed voxel as required
/// - Update the @c MapChunk::dirty_stamp to the cached @c OccupancyMap::touch() value
/// - Update the @c MapChunk::touched_stamps for the affected layer(s) to the same touch value.
///     - Recommend using @c std::atomic_uint64_t::.store() with @c std::memory_order_relaxed if permitted
///
/// This logic avoids constantly querying and validating @c MapLayer details, while ensuring that the
/// @c OccupancyMap::stamp() , @c MapChunk::dirty_stamp and are @c MapChunk::touched_stamps are correctly managed.
///
/// An additional optimisation can be made by avoiding calls to @c OccupancyMap::region() . Iterative and line walk
/// style updates of a map have a strong spatial coherency from one voxel to update to the next. In this case, the
/// @c MapChunk will often be the same as the previous one. In this case, an easy performance gain comes by keeping
/// checking if the new @c Key::regionKey() is the same as the last one, and using the previous @c MapChunk when the
/// @c regionKey() is unchanged.
struct ohm_API MapChunk
{
  /// Defines the spatial region covered by the chunk.
  MapRegion region = MapRegion{};
  /// Details of the map to which this chunk belongs.
  const OccupancyMapDetail *map = nullptr;
  /// Index of the first voxel with valid data: occupied or free, but not unobserved. May be used to skip irrelevant
  /// voxels during map voxel iteration.
  unsigned first_valid_index = ~0u;
  /// Last timestamp the occupancy layer of this chunk was modified.
  double touched_time = 0;

  /// A monotonic stamp value occupancy layer, used to indicate when this chunk was last updated.
  /// The map maintains the most up to date stamp: @c OccupancyMap::stamp().
  std::atomic_uint64_t dirty_stamp{ 0 };

  /// A monotonic stamp value for each @c voxelMap, used to indicate when the layer was last updated.
  /// The map maintains the most up to date stamp: @c OccupancyMap::stamp().
  /// @note It is not possible to have a @c std::vector of atomic types. We use a unique pointer to an arrray
  /// instead.
  std::unique_ptr<std::atomic_uint64_t[]> touched_stamps;  // NOLINT(modernize-avoid-c-arrays)

  /// Array of voxel blocks. Layer semantics are defined in the owning @c OccupancyMap.
  /// Use @c layout to access specific maps.
  std::vector<VoxelBlock::Ptr> voxel_blocks;

  /// Chunk flags set from @c MapChunkFlag.
  unsigned flags = 0;

  /// Create an empty @c MapChunk object.
  MapChunk() = default;
  /// Create a @c MapChunk for the given @p map .
  /// @param map Implementation details of the owning map object.
  explicit MapChunk(const OccupancyMapDetail &map);
  /// Create a @c MapChunk for the given @p region within @p map .
  /// @param region Region details for the chunk.
  /// @param map Implementation details of the owning map object.
  MapChunk(const MapRegion &region, const OccupancyMapDetail &map);
  /// Copy constructor (deep copy).
  /// @param other Object to copy.
  MapChunk(const MapChunk &other) = delete;
  /// Move constructor.
  /// @param other Object to move.
  MapChunk(MapChunk &&other) noexcept;
  /// Destructor - releases all this chunk's memory.
  ~MapChunk();

  /// Access details of the voxel layers and layouts for this map.
  const MapLayout &layout() const;

  /// Given a @p voxelIndex into voxels, get the associated @c Key.
  /// @param voxel_index An index into voxels. Must be in range
  ///   <tt>[0, regionVoxelDimensions.x * regionVoxelDimensions.y * regionVoxelDimensions.z)</tt>
  ///   or a null key is returned.
  /// @param region_voxel_dimensions The dimensions of each chunk/region along each axis.
  /// @param region_coord The coordinate of the containing region.
  /// @return An @c Key to reference the requested voxel.
  static Key keyForIndex(size_t voxel_index, const glm::ivec3 &region_voxel_dimensions,
                         const glm::i16vec3 &region_coord);

  /// @overload
  inline Key keyForIndex(size_t voxel_index, const glm::ivec3 &region_voxel_dimensions) const
  {
    return keyForIndex(voxel_index, region_voxel_dimensions, region.coord);
  }

  /// Update the @p layout for the chunk, preserving current layers which have an equivalent in @p new_layout.
  ///
  /// Note: this does not change the @p layout pointer as it is assumed the object at that location is about to be
  /// updated with the information from @p new_layout.
  ///
  /// @param new_layout The new memory layout for voxel chunks.
  /// @param preserve_layer_mapping Indicates which layers from @p layout are mapped to new layers in @p new_layout.
  void updateLayout(const MapLayout *new_layout,
                    const std::vector<std::pair<const MapLayer *, const MapLayer *>> &preserve_layer_mapping);

  /// Returns true if the chunk contains any valid voxels. A valid voxel is one who's value has
  /// been set.
  ///
  /// This is a quick test based on the state of @c first_valid_index being valid and not
  /// (255, 255, 255). Thus the result is only corect insofar as @c first_valid_index is correctly
  /// maintained.
  ///
  /// @return True if this chunk contains at least one voxel with a valid value.
  bool hasValidNodes() const;

  /// Set the @c first_valid_index to the unknown/invalid value.
  inline void invalidateFirstValidIndex() { first_valid_index = ~0u; }

  /// Update the @c first_valid_index based on adding @p localIndex as a valid index. This should be called whenever
  /// a voxel becomes observed with @c local_index set to index the modified voxel. This allows the chunk to keep track
  /// of the first observed/valid voxel in its chunk and skip unobserved voxels during iteration.
  ///
  /// This simply chooses whichever occurs first from @c first_valid_index and
  /// @p localIndex.
  /// @param local_index The voxel index within this chunk. Equivalent to Key::localKey().
  /// @param region_voxel_dimensions The dimensions of each chunk/region along each axis.
  void updateFirstValid(const glm::u8vec3 &local_index, const glm::ivec3 &region_voxel_dimensions);

  /// Update the @c first_valid_index to be the minimum of the current value and the given @p index .
  /// @param index Index of the new first valid candidate.
  inline void updateFirstValid(unsigned index) { first_valid_index = std::min(index, first_valid_index); }

  /// Update the @c first_valid_index by brute force, searching for the first valid voxel.
  /// @param region_voxel_dimensions The dimensions of each chunk/region along each axis.
  /// @param search_from Start searching from this voxel index (must be a valid index).
  void searchAndUpdateFirstValid(const glm::ivec3 &region_voxel_dimensions,
                                 const glm::u8vec3 &search_from = glm::u8vec3(0, 0, 0));

  /// Request the @c Key::localKey() value for the first valid voxel in this this region.
  /// @param region_voxel_dimensions Specifies the voxel dimensions of (this) chunk.
  /// @return The @c Key::localKey() value indexing the first (potential) valid voxel in this region.
  inline glm::u8vec3 firstValidKey(const glm::ivec3 &region_voxel_dimensions) const
  {
    return voxelLocalKey(first_valid_index, region_voxel_dimensions);
  }

  /// Request the @c Key::localKey() value for the first valid voxel in this this region.
  /// This overload is marginally less efficient than the overload. This efficiency is only really valid in tight
  /// loops.
  /// @return The @c Key::localKey() value indexing the first (potential) valid voxel in this region.
  glm::u8vec3 firstValidKey() const;

  /// Recalculates what the @c first_valid_index should be (brute force) and validates against its current value.
  /// @return True when the @c first_valid_index value matches what it should be.
  bool validateFirstValid() const;

  /// Query if this @c MapChunk overlaps the axis aligned bounding box.
  /// @param min_ext The lower extents of the AABB.
  /// @param max_ext The upper extents of the AABB.
  /// @return True if the chunk overlaps or touches the given AABB.
  bool overlapsExtents(const glm::dvec3 &min_ext, const glm::dvec3 &max_ext) const;

  /// Query the spacial extents of this @c MapChunk as a min/max axis aligned bounding box.
  /// @param[out] min_ext Set to the lower extents of the AABB.
  /// @param[out] max_ext Set to the upper extents of the AABB.
  void extents(glm::dvec3 &min_ext, glm::dvec3 &max_ext) const;
};


inline bool MapChunk::hasValidNodes() const
{
  return first_valid_index != ~0u;
  // return first_valid_index.x != 255 && first_valid_index.y != 255 && first_valid_index.z != 255;
}


inline void MapChunk::updateFirstValid(const glm::u8vec3 &local_index, const glm::ivec3 &region_voxel_dimensions)
{
  first_valid_index = std::min(voxelIndex(local_index.x, local_index.y, local_index.z, region_voxel_dimensions.x,
                                          region_voxel_dimensions.y, region_voxel_dimensions.z),
                               first_valid_index);
  // const unsigned current_first =
  //   voxelIndex(first_valid_index.x, first_valid_index.y, first_valid_index.z, region_voxel_dimensions.x,
  //              region_voxel_dimensions.y, region_voxel_dimensions.z);
  // const unsigned new_first = voxelIndex(local_index.x, local_index.y, local_index.z, region_voxel_dimensions.x,
  //                                       region_voxel_dimensions.y, region_voxel_dimensions.z);
  // first_valid_index = (new_first < current_first) ? local_index : first_valid_index;
#ifdef OHM_VALIDATION
  if (test_first < current_first)
  {
    validateFirstValid(regionVoxelDimensions);
  }
#endif  // OHM_VALIDATION
}


}  // namespace ohm

#endif  // OHM_MAPCHUNK_H


================================================
FILE: ohm/MapChunkFlag.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef MAPCHUNKFLAG_H_
#define MAPCHUNKFLAG_H_

#include "OhmConfig.h"

namespace ohm
{
enum MapChunkFlag
{
  MainMemDirty = (1 << 0),
  GpuMemDirty = (1 << 1)
};
}

#endif  // MAPCHUNKFLAG_H_


================================================
FILE: ohm/MapCoord.h
================================================
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// Copyright (c) 2017
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_MAPCOORD_H
#define OHM_MAPCOORD_H

// Note: this header is included in GPU code.
// Because of this <cmath> cannot be included here and you may need to include that first.

#if !GPUTIL_DEVICE
#ifndef __device__
#define __device__
#endif  // __device__
#ifndef __host__
#define __host__
#endif  // __host__

namespace ohm
{
#endif  // !GPUTIL_DEVICE

// Define real coordinate for functions below. Normally double, float for GPU.
#if GPUTIL_DEVICE == 1 && !defined(COORD_REAL)
#define COORD_REAL
typedef float coord_real;
#endif  // GPUTIL_DEVICE && !defined(COORD_REAL)

/// Calculate the map local centre coordinate for a region along a single axis.
/// @param regionCoord The coordinate of the region in the map along the axis of interest.
/// @param regionDimension The global size of each region along the axis of interest.
/// @return The centre of the region along this axis local to the map centre.
#if GPUTIL_DEVICE != 1
template <typename coord_real>  // NOLINT(readability-identifier-naming)
#endif                          // GPUTIL_DEVICE != 1
inline __device__ __host__ coord_real regionCentreCoord(int region_coord, const coord_real region_dimesion)
{
  return region_coord * region_dimesion;
}

#if GPUTIL_DEVICE != 1
template <typename coord_real>  // NOLINT(readability-identifier-naming)
#endif                          // GPUTIL_DEVICE != 1
inline __device__ __host__ int pointToRegionVoxel(coord_real coord, coord_real voxel_resolution,
                                                  coord_real region_resolution)
{
  // Due to precision error, we can end up with coordinates just outside the region spatial boundary.
  // That is, either just below zero, or just above regionResolution. Since by now we should have determined
  // the correct region for coord, we need to handle this imprecision. To do so, we simply adjust such
  // values by a small epsilon.
  //
  // In testing, the largest double precision delta I've seen has been on the order of 1e-15, while
  // the largest single precision delta should be around 5e-7. We use an epsilon of 1e-6 to support
  // single precision. Anything larger is treated as an invalid input.
  const coord_real epsilon = (coord_real)1e-6f;  // NOLINT
  if (-epsilon <= coord && coord < 0)
  {
    // Note: TEST_EPSILON_MAGNITUDE is not explicitly defined anywhere. The code is for testing only.
#ifdef TEST_EPSILON_MAGNITUDE
    static coord_real largestDelta = 0;
    if (-coord > -largestDelta)
    {
      largestDelta = coord;
      fprintf(stderr, "W: pointToRegionVoxel(%lg, %lg) : negative coord\n", coord, voxelResolution);
    }
#endif  // TEST_EPSILON_MAGNITUDE
    coord = 0;
  }
  else if (coord >= region_resolution && coord - epsilon < region_resolution)
  {
    coord -= epsilon;
  }
#if defined(__CUDACC__)
  return (int)floorf((coord / voxel_resolution));
#else   // defined(__CUDACC__)
  // NOLINTNEXTLINE
  return (int)floor((coord / voxel_resolution));
#endif  // defined(__CUDACC__)
}

#if GPUTIL_DEVICE != 1
template <typename coord_real>  // NOLINT(readability-identifier-naming)
#endif                          // GPUTIL_DEVICE != 1
inline __device__ __host__ int pointToRegionCoord(coord_real coord, coord_real resolution)
{
#if defined(__CUDACC__)
  return (int)floorf(coord / resolution + (coord_real)0.5);
#else   // defined(__CUDACC__)
  // NOLINTNEXTLINE
  return (int)floor(coord / resolution + (coord_real)0.5f);
#endif  // defined(__CUDACC__)
}
#if !GPUTIL_DEVICE
}  // namespace ohm
#endif  // !GPUTIL_DEVICE

#endif  // OHM_MAPCOORD_H


================================================
FILE: ohm/MapFlag.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapFlag.h"

#include <array>
#include <string>

namespace
{
const std::array<const char *, 3> kMapFlagNames =  //
  { "VoxelMean", "Compressed", "Traversal" };
}  // namespace

namespace ohm
{
const char *mapFlagToString(MapFlag flag)
{
  unsigned bit = 1;
  for (unsigned i = 0; i < kMapFlagNames.size(); ++i, bit <<= 1u)
  {
    if (unsigned(flag) & bit)
    {
      return kMapFlagNames[i];
    }
  }

  return "None";
}


MapFlag mapFlagFromString(const char *str)
{
  std::string name(str);
  unsigned bit = 1;
  for (unsigned i = 0; i < kMapFlagNames.size(); ++i, bit <<= 1u)
  {
    if (name == kMapFlagNames[i])
    {
      return MapFlag(bit);
    }
  }

  return MapFlag::kNone;
}

}  // namespace ohm


================================================
FILE: ohm/MapFlag.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef MAPFLAG_H
#define MAPFLAG_H

#include "OhmConfig.h"

#include <type_traits>

namespace ohm
{
/// Flags used to augment initialisation of an @c OccupancyMap.
enum class MapFlag : unsigned
{
  /// No special features.
  kNone = 0u,
  /// Enable voxel mean position tracking.
  kVoxelMean = (1u << 0u),
  /// Maintain compressed voxels in memory. Compression is performed off thread.
  kCompressed = (1u << 1u),
  /// Maintain the traversal in addition to the occupancy layer. See @c default_layer::traversalLayerName() .
  /// The @c kVoxelMean layer should also be enabled to support traversal in order to track the voxel sample count.
  kTraversal = (1u << 2u),
  /// Maintain a (32-bit) touch time stamp for each normal.
  kTouchTime = (1u << 3u),
  /// Maintain an incident normal for each sample voxel.
  kIncidentNormal = (1u << 4u),
  /// Add a TSDF layer.
  kTsdf = (1u << 5u),
  /// Add the secondary samples layer.
  kSecondarySample = (1u << 6u),

  /// Default map creation flags.
  kDefault = kCompressed
};

const char ohm_API *mapFlagToString(MapFlag flag);
MapFlag ohm_API mapFlagFromString(const char *str);
}  // namespace ohm

inline ohm::MapFlag operator|(ohm::MapFlag left, ohm::MapFlag right)
{
  using T = std::underlying_type_t<ohm::MapFlag>;
  return static_cast<ohm::MapFlag>(static_cast<T>(left) | static_cast<T>(right));
}

inline ohm::MapFlag &operator|=(ohm::MapFlag &left, ohm::MapFlag right)
{
  left = left | right;
  return left;
}

inline ohm::MapFlag operator&(ohm::MapFlag left, ohm::MapFlag right)
{
  using T = std::underlying_type_t<ohm::MapFlag>;
  return static_cast<ohm::MapFlag>(static_cast<T>(left) & static_cast<T>(right));
}

inline ohm::MapFlag &operator&=(ohm::MapFlag &left, ohm::MapFlag right)
{
  left = left & right;
  return left;
}

inline ohm::MapFlag operator^(ohm::MapFlag left, ohm::MapFlag right)
{
  using T = std::underlying_type_t<ohm::MapFlag>;
  return static_cast<ohm::MapFlag>(static_cast<T>(left) ^ static_cast<T>(right));
}

inline ohm::MapFlag &operator^=(ohm::MapFlag &left, ohm::MapFlag right)
{
  left = left ^ right;
  return left;
}

inline ohm::MapFlag operator~(ohm::MapFlag value)
{
  using T = std::underlying_type_t<ohm::MapFlag>;
  return static_cast<ohm::MapFlag>(~static_cast<T>(value));
}

#endif  // MAPFLAG_H


================================================
FILE: ohm/MapInfo.cpp
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) 2015 CSIRO
//
#include "MapInfo.h"

#include <cstring>
#include <sstream>
#include <string>
#include <unordered_set>

#ifndef _MSC_VER
#include <strings.h>
#define _stricmp strcasecmp
#endif  // _MSC_VER

namespace std
{
template <>
struct hash<ohm::MapValue>
{
  size_t operator()(const ohm::MapValue &value) const
  {
    return std::hash<std::string>()(*static_cast<const std::string *>(value.namePtr()));
  }
};
}  // namespace std

namespace ohm
{
struct TreeSearchPredicate
{
  bool operator()(const MapValue &left, const MapValue &right) const { return left.namesEqual(right); }
};


struct MapInfoDetail
{
  std::unordered_set<MapValue, std::hash<MapValue>, TreeSearchPredicate> dictionary;
};

namespace
{
template <typename T>
class StringConvert
{
public:
  static T convert(const char *str)
  {
    if (!str)
    {
      return T(0);
    }

    std::istringstream s(str);
    T val = T(0);
    s >> val;
    return val;
  }
};


template <>
class StringConvert<const char *>
{
public:
  static const char *convert(const char *str) { return str; }
};


template <>
class StringConvert<bool>
{
public:
  static bool convert(const char *str)
  {
    if (!str)
    {
      return false;
    }

    if (_stricmp("true", str) == 0 || _stricmp("yes", str) == 0 || _stricmp("on", str) == 0)
    {
      return true;
    }
    if (_stricmp("false", str) == 0 || _stricmp("no", str) == 0 || _stricmp("off", str) == 0)
    {
      return true;
    }

    return StringConvert<float>::convert(str) != 0;
  }
};
}  // namespace

MapValue::MapValue()  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
}


MapValue::MapValue(const MapValue &other)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  *this = other;
}


MapValue::MapValue(MapValue &&other) noexcept  // NOLINT(cppcoreguidelines-pro-type-member-init)
  : name_(other.name_)
  , type_(other.type_)
{
  memcpy(&value_, &other.value_, sizeof(value_));
  other.type_ = kTypeNone;
  other.name_ = nullptr;
  memset(&other.value_, 0, sizeof(other.value_));
}


MapValue::MapValue(const char *name, int8_t val)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = val;
}


MapValue::MapValue(const char *name, uint8_t val)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = val;
}


MapValue::MapValue(const char *name, int16_t val)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = val;
}


MapValue::MapValue(const char *name, uint16_t val)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = val;
}


MapValue::MapValue(const char *name, int32_t val)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = val;
}


MapValue::MapValue(const char *name, uint32_t val)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = val;
}


MapValue::MapValue(const char *name, int64_t val)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = val;
}


MapValue::MapValue(const char *name, uint64_t val)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = val;
}


MapValue::MapValue(const char *name, float val)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = val;
}


MapValue::MapValue(const char *name, double val)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = val;
}


MapValue::MapValue(const char *name, bool val)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = val;
}


MapValue::MapValue(const char *name, const char *string)  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  memset(&value_, 0, sizeof(value_));
  setName(name);
  *this = string;
}


MapValue &MapValue::operator=(const MapValue &other)
{
  if (this != &other)
  {
    clearValue();
    type_ = other.type_;
    if (type_ != kString)
    {
      value_.u64 = other.value_.u64;
    }
    else
    {
      value_.str = nullptr;
      size_t len = other.value_.str ? strlen(other.value_.str) : 0;
      // Lint(KS): current API is not RAII compatible
      value_.str = new char[len + 1];  // NOLINT(cppcoreguidelines-owning-memory)
      value_.str[0] = '\0';
      if (other.value_.str)
      {
#ifdef _MSC_VER
        strncpy_s(value_.str, len + 1, other.value_.str, len);
#else   // !_MSC_VER
        strncpy(value_.str, other.value_.str, len);
#endif  // _MSC_VER
        value_.str[len] = '\0';
      }
    }
    setName(other.name());
  }

  return *this;
}


MapValue &MapValue::operator=(MapValue &&other) noexcept
{
  clear();
  name_ = other.name_;
  type_ = other.type_;
  memcpy(&value_, &other.value_, sizeof(value_));
  other.type_ = kTypeNone;
  other.name_ = nullptr;
  memset(&other.value_, 0, sizeof(other.value_));
  return *this;
}


MapValue &MapValue::operator=(int8_t val)
{
  clearValue();
  type_ = kInt8;
  value_.i8 = val;
  return *this;
}


MapValue &MapValue::operator=(uint8_t val)
{
  clearValue();
  type_ = kUInt8;
  value_.u8 = val;
  return *this;
}


MapValue &MapValue::operator=(int16_t val)
{
  clearValue();
  type_ = kInt16;
  value_.i16 = val;
  return *this;
}


MapValue &MapValue::operator=(uint16_t val)
{
  clearValue();
  type_ = kUInt16;
  value_.u16 = val;
  return *this;
}


MapValue &MapValue::operator=(int32_t val)
{
  clearValue();
  type_ = kInt32;
  value_.i32 = val;
  return *this;
}


MapValue &MapValue::operator=(uint32_t val)
{
  clearValue();
  type_ = kUInt32;
  value_.u32 = val;
  return *this;
}


MapValue &MapValue::operator=(int64_t val)
{
  clearValue();
  type_ = kInt64;
  value_.i64 = val;
  return *this;
}


MapValue &MapValue::operator=(uint64_t val)
{
  clearValue();
  type_ = kUInt64;
  value_.u64 = val;
  return *this;
}


MapValue &MapValue::operator=(float val)
{
  clearValue();
  type_ = kFloat32;
  value_.f32 = val;
  return *this;
}


MapValue &MapValue::operator=(double val)
{
  clearValue();
  type_ = kFloat64;
  value_.f64 = val;
  return *this;
}


MapValue &MapValue::operator=(bool val)
{
  clearValue();
  type_ = kBoolean;
  value_.b = val;
  return *this;
}


MapValue &MapValue::operator=(const char *string)
{
  clearValue();
  type_ = kString;
  value_.str = nullptr;
  const size_t len = string ? strlen(string) : 0;
  // Lint(KS): current API is not RAII compatible
  value_.str = new char[len + 1];  // NOLINT(cppcoreguidelines-owning-memory)
  value_.str[0] = '\0';
  if (string)
  {
#ifdef _MSC_VER
    strncpy_s(value_.str, len + 1, string, len);
#else   // _MSC_VER
    // Ignore -Wstringop-truncation for this strncpy as the null terminator is
    // manually copied immediately after
    #pragma GCC diagnostic push
    #pragma GCC diagnostic ignored "-Wstringop-truncation"
    strncpy(value_.str, string, len);
    #pragma GCC diagnostic pop
#endif  // _MSC_VER
    value_.str[len] = '\0';
  }
  return *this;
}


MapValue::~MapValue()
{
  clear();
  delete static_cast<std::string *>(name_);
}


void MapValue::clear()
{
  clearValue();
  if (name_)
  {
    *static_cast<std::string *>(name_) = std::string();
  }
}


void MapValue::clearValue()
{
  if (type_ == kString)
  {
    delete[] value_.str;
  }
  type_ = kTypeNone;
  value_.u64 = 0ull;
}


const char *MapValue::name() const
{
  if (name_)
  {
    return static_cast<std::string *>(name_)->c_str();
  }

  return "";
}


void MapValue::setName(const char *name)
{
  if (!name_)
  {
    name_ = new std::string();
  }
  *static_cast<std::string *>(name_) = name;
}


bool MapValue::namesEqual(const MapValue &other) const
{
  const auto *a = static_cast<const std::string *>(name_);
  const auto *b = static_cast<const std::string *>(other.name_);

  if (!a && (!b || b->empty()) || !b && (!a || a->empty()))
  {
    // Both names empty/null.
    return true;
  }

  if (a && b)
  {
    return *a == *b;
  }

  return false;
}


MapValue::operator int8_t() const
{
  return value<int8_t>();
}


MapValue::operator uint8_t() const
{
  return value<uint8_t>();
}


MapValue::operator int16_t() const
{
  return value<int16_t>();
}


MapValue::operator uint16_t() const
{
  return value<uint16_t>();
}


MapValue::operator int32_t() const
{
  return value<int32_t>();
}


MapValue::operator uint32_t() const
{
  return value<uint32_t>();
}


MapValue::operator int64_t() const
{
  return value<int64_t>();
}


MapValue::operator uint64_t() const
{
  return value<uint64_t>();
}


MapValue::operator float() const
{
  return value<float>();
}


MapValue::operator double() const
{
  return value<double>();
}


MapValue::operator bool() const
{
  return value<bool>();
}


MapValue::operator const char *() const
{
  if (type_ == kString)
  {
    return value_.str;
  }
  return "";
}


MapValue MapValue::toStringValue() const
{
  if (type_ == kString)
  {
    return *this;
  }

  std::stringstream str;

  switch (type_)
  {
  case kInt8:
    str << int(value_.i8);
    break;
  case kUInt8:
    str << int(value_.u8);
    break;
  case kInt16:
    str << value_.i16;
    break;
  case kUInt16:
    str << value_.u16;
    break;
  case kInt32:
    str << value_.i32;
    break;
  case kUInt32:
    str << value_.u32;
    break;
  case kInt64:
    str << value_.i64;
    break;
  case kUInt64:
    str << value_.u64;
    break;
  case kFloat32:
    str << value_.f32;
    break;
  case kFloat64:
    str << value_.f64;
    break;
  case kBoolean:
    str << (value_.b ? "true" : "false");
    break;
  default:
    break;
  }

  return MapValue(name(), str.str().c_str());
}


bool MapValue::operator==(const MapValue &other) const
{
  if (type_ != other.type_)
  {
    return false;
  }

  const auto *n1 = static_cast<const std::string *>(name_);
  const auto *n2 = static_cast<const std::string *>(other.name_);

  if (!(n1 == nullptr && n2 == nullptr || n1 == nullptr && n2->empty() || n1 && n1->empty() && n2 == nullptr ||
        n1 == n2))
  {
    return false;
  }

  switch (type_)
  {
  case kInt8:
    return value_.i8 == other.value_.i8;
  case kUInt8:
    return value_.u8 == other.value_.u8;
  case kInt16:
    return value_.i16 == other.value_.i16;
  case kUInt16:
    return value_.u16 == other.value_.u16;
  case kInt32:
    return value_.i32 == other.value_.i32;
  case kUInt32:
    return value_.u32 == other.value_.u32;
  case kInt64:
    return value_.i64 == other.value_.i64;
  case kUInt64:
    return value_.u64 == other.value_.u64;
  case kFloat32:
    return value_.f32 == other.value_.f32;
  case kFloat64:
    return value_.f64 == other.value_.f64;
  case kBoolean:
    return value_.b == other.value_.b;
  case kString:
    return value_.str == nullptr && other.value_.str == nullptr ||
           value_.str && other.value_.str && strcmp(value_.str, other.value_.str) == 0;

  default:
    break;
  }

  return false;
}


bool MapValue::operator!=(const MapValue &other) const
{
  return !(*this == other);
}


#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4800)
#endif  // _MSC_VER
template <typename T>
T MapValue::value() const
{
  switch (type_)
  {
  case kInt8:
    return T(value_.i8);
  case kUInt8:
    return T(value_.u8);
  case kInt16:
    return T(value_.i16);
  case kUInt16:
    return T(value_.u16);
  case kInt32:
    return T(value_.i32);
  case kUInt32:
    return T(value_.u32);
  case kInt64:
    return T(value_.i64);
  case kUInt64:
    return T(value_.u64);
  case kFloat32:
    return T(value_.f32);
  case kFloat64:
    return T(value_.f64);
  case kBoolean:
    return value_.b ? T(1) : T(0);
  case kString:
    return StringConvert<T>::convert(value_.str);
  default:
    break;
  }

  return T(0);
}
#ifdef _MSC_VER
#pragma warning(pop)
#endif  // _MSC_VER


bool MapValue::operator<(const MapValue &other) const
{
  return *static_cast<const std::string *>(name_) < *static_cast<const std::string *>(other.name_);
}


bool MapValue::sortCompare(const MapValue &a, const MapValue &b)
{
  return *static_cast<std::string *>(a.name_) < *static_cast<std::string *>(b.name_);
}


MapInfo::MapInfo()
  : imp_(std::make_unique<MapInfoDetail>())
{}


MapInfo::MapInfo(const MapInfo &other)
  : imp_(std::make_unique<MapInfoDetail>())
{
  for (const auto &item : other.imp_->dictionary)
  {
    imp_->dictionary.insert(item);
  }
}


MapInfo::MapInfo(MapInfo &&other) noexcept
  : imp_(std::move(other.imp_))
{}


MapInfo &MapInfo::operator=(const MapInfo &other)
{
  if (this != &other)
  {
    *imp_ = *other.imp_;
  }
  return *this;
}


MapInfo &MapInfo::operator=(MapInfo &&other) noexcept
{
  imp_ = std::move(other.imp_);
  return *this;
}

MapInfo::~MapInfo() = default;


void MapInfo::set(const MapValue &value)
{
  auto iter = imp_->dictionary.find(value);
  if (iter != imp_->dictionary.end())
  {
    imp_->dictionary.erase(iter);
  }
  imp_->dictionary.insert(value);
}


MapValue MapInfo::get(const char *name, MapValue default_value) const
{
  auto iter = imp_->dictionary.find(MapValue(name, int32_t(0)));
  if (iter != imp_->dictionary.end())
  {
    return *iter;
  }

  return default_value;
}


bool MapInfo::remove(const char *name)
{
  auto iter = imp_->dictionary.find(MapValue(name, int32_t(0)));
  if (iter != imp_->dictionary.end())
  {
    imp_->dictionary.erase(iter);
    return true;
  }
  return false;
}


void MapInfo::clear()
{
  if (imp_)
  {
    imp_->dictionary.clear();
  }
}


unsigned MapInfo::extract(MapValue *values, unsigned max_count, unsigned offset) const
{
  if (!values || !max_count)
  {
    return unsigned(imp_->dictionary.size());
  }

  unsigned extracted = 0;
  for (auto iter = imp_->dictionary.begin(); iter != imp_->dictionary.end() && extracted < max_count; ++iter)
  {
    if (!offset)
    {
      values[extracted++] = *iter;
    }
    else
    {
      --offset;
    }
  }

  return extracted;
}
}  // namespace ohm


================================================
FILE: ohm/MapInfo.h
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) 2015 CSIRO
//
#ifndef MAPINFO_H
#define MAPINFO_H

#include "OhmConfig.h"

#include <cinttypes>
#include <memory>

namespace ohm
{
/// A data value for use with @c MapInfo. Acts as a key/value pair, supporting
/// a variety of data types.
///
/// @par Conversion Operators
/// @parblock
/// The class includes cast operators for converting to any of the supported types.
/// Conversion succeeds even if the contained type does not match the target type,
/// including converting from strings to numeric types. However, numeric truncation
/// may occur and failure to convert is not detectable.
///
/// Conversion from string is implied, parsing the string to the appropriate type.
/// String conversion from bool includes converting the following strings (case insensitive):
/// { true, false, yes, no, on, off }.
///
/// Conversion to string is not automatic. The cast operator to string only works if
/// the type is @c kString. String conversion is better supported by @c toStringValue(),
/// which creates a new @c MapValue() of type @c kString.
/// @endparblock
class ohm_API MapValue
{
public:
  /// Supported data types for @c MapValue.
  enum Type
  {
    kTypeNone,
    kInt8,
    kUInt8,
    kInt16,
    kUInt16,
    kInt32,
    kUInt32,
    kInt64,
    kUInt64,
    kFloat32,
    kFloat64,
    kBoolean,
    kString
  };

  /// Create an invalid @c MapValue().
  MapValue();
  /// Constructor making a (deep) copy of @c other.
  /// @param other The value to copy.
  MapValue(const MapValue &other);
  /// R-value reference constructor.
  /// @param other The reference to steal data from.
  MapValue(MapValue &&other) noexcept;
  /// Create a @c kInt8 value.
  /// @param name The value key.
  /// @param val The value to assign.
  MapValue(const char *name, int8_t val);
  /// Create a @c kUInt8 value.
  /// @param name The value key.
  /// @param val The value to assign.
  MapValue(const char *name, uint8_t val);
  /// Create a @c kInt16 value.
  /// @param name The value key.
  /// @param val The value to assign.
  MapValue(const char *name, int16_t val);
  /// Create a @c kUInt16 value.
  /// @param name The value key.
  /// @param val The value to assign.
  MapValue(const char *name, uint16_t val);
  /// Create a @c kInt32 value.
  /// @param name The value key.
  /// @param val The value to assign.
  MapValue(const char *name, int32_t val);
  /// Create a @c kUInt32 value.
  /// @param name The value key.
  /// @param val The value to assign.
  MapValue(const char *name, uint32_t val);
  /// Create a @c kInt64 value.
  /// @param name The value key.
  /// @param val The value to assign.
  MapValue(const char *name, int64_t val);
  /// Create a @c kUInt64 value.
  /// @param name The value key.
  /// @param val The value to assign.
  MapValue(const char *name, uint64_t val);
  /// Create a @c kFloat32 value.
  /// @param name The value key.
  /// @param val The value to assign.
  MapValue(const char *name, float val);
  /// Create a @c kFloat64 value.
  /// @param name The value key.
  /// @param val The value to assign.
  MapValue(const char *name, double val);
  /// Create a @c kBoolean value.
  /// @param name The value key.
  /// @param val The value to assign.
  MapValue(const char *name, bool val);
  /// Create a @c kString value.
  /// @param name The value key.
  /// @param string The value to assign.
  MapValue(const char *name, const char *string);

  /// Copy assignment.
  /// @param other Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(const MapValue &other);
  /// Move assignment.
  /// @param other Object to move.
  /// @return A reference to this object.
  MapValue &operator=(MapValue &&other) noexcept;
  /// Value assignment. This changes the @c Type as required.
  /// @param val Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(int8_t val);
  /// Value assignment. This changes the @c Type as required.
  /// @param val Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(uint8_t val);
  /// Value assignment. This changes the @c Type as required.
  /// @param val Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(int16_t val);
  /// Value assignment. This changes the @c Type as required.
  /// @param val Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(uint16_t val);
  /// Value assignment. This changes the @c Type as required.
  /// @param val Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(int32_t val);
  /// Value assignment. This changes the @c Type as required.
  /// @param val Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(uint32_t val);
  /// Value assignment. This changes the @c Type as required.
  /// @param val Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(int64_t val);
  /// Value assignment. This changes the @c Type as required.
  /// @param val Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(uint64_t val);
  /// Value assignment. This changes the @c Type as required.
  /// @param val Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(float val);
  /// Value assignment. This changes the @c Type as required.
  /// @param val Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(double val);
  /// Value assignment. This changes the @c Type as required.
  /// @param val Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(bool val);
  /// Value assignment. This changes the @c Type as required.
  /// @param string Value to assign.
  /// @return A reference to this object.
  MapValue &operator=(const char *string);

  /// Destructor.
  ~MapValue();

  /// Clear the value and release the key name, making @c isValid() false and @c name() empty.
  void clear();

  /// Clear just the value, leaving the name unchanged. @c isValid() becomes false.
  void clearValue();

  /// Is the @c MapValue valid. This checks only the type. The entry may have a (dangling) name.
  /// @return True if the value is of a valid type.
  inline bool isValid() const { return type_ != kTypeNone; }

  /// Retrieve the value name/key.
  /// @return The value name.
  const char *name() const;

  /// Set the value name/key, replacing the existing name. Does not change the results of @c isValid().
  /// @param name The name to set.
  void setName(const char *name);

  /// Provide access to the internal name pointer. For internal use.
  /// @return The name key pointer.
  inline const void *namePtr() const { return name_; }

  /// Get the @c MapValue type.
  /// @return The value @c Type.
  inline Type type() const { return type_; }

  /// Compare the name of @p other against this @c name().
  /// @param other The @c MapValue to compare the name of.
  /// @return True if the names match or are othe empty/null or any combination of empty/null.
  bool namesEqual(const MapValue &other) const;

  /// Cast conversion. See casting notes in class description.
  /// @return The value as a signed byte.
  explicit operator int8_t() const;
  /// Cast conversion. See casting notes in class description.
  /// @return The value as an unsigned byte.
  explicit operator uint8_t() const;
  /// Cast conversion. See casting notes in class description.
  /// @return The value as a signed 16-bit integer.
  explicit operator int16_t() const;
  /// Cast conversion. See casting notes in class description.
  /// @return The value as an unsigned 16-bit integer.
  explicit operator uint16_t() const;
  /// Cast conversion. See casting notes in class description.
  /// @return The value as a signed 32-bit integer.
  explicit operator int32_t() const;
  /// Cast conversion. See casting notes in class description.
  /// @return The value as an unsigned 32-bit integer.
  explicit operator uint32_t() const;
  /// Cast conversion. See casting notes in class description.
  /// @return The value as a signed 64-bit integer.
  explicit operator int64_t() const;
  /// Cast conversion. See casting notes in class description.
  /// @return The value as an unsigned 64-bit integer.
  explicit operator uint64_t() const;
  /// Cast conversion. See casting notes in class description.
  /// @return The value as a single precision float.
  explicit operator float() const;
  /// Cast conversion. See casting notes in class description.
  /// @return The value as a double precision float.
  explicit operator double() const;
  /// Cast conversion. See casting notes in class description.
  /// @return The value as a boolean value.
  explicit operator bool() const;
  /// Cast conversion. See casting notes in class description.
  /// @return The value as a string.
  explicit operator const char *() const;

  /// Return a @c MapValue with type @c kString containing a string representation of
  /// this item's value.
  /// @return A string conversion of this @c MapValue.
  MapValue toStringValue() const;

  /// Equality comparison. Equality requires:
  /// - Same @c Type (including @c kTypeNone).
  /// - Same or empty/null names.
  /// - Equal values.
  /// @param other Item to compare.
  /// @return True if this equals @p other as described above.
  bool operator==(const MapValue &other) const;

  /// Inequality. Negation of ==.
  /// @param other Item to compare.
  /// @return False if this equals @p other as described in the == operator.
  bool operator!=(const MapValue &other) const;

  /// Less than operator for sorting. The result is entirely based on the names
  /// and the result of a less than comparison on the names.
  bool operator<(const MapValue &other) const;

  /// Comparison function for sorting list of @c MapValue objects.
  /// @param a The first value.
  /// @param b The second value.
  /// @return True when @p a is less than @p b.
  static bool sortCompare(const MapValue &a, const MapValue &b);

private:
  /// Internal type cast.
  /// @tparam T Target type.
  /// @return The converted value.
  template <typename T>
  T value() const;

  void *name_ = nullptr;  ///< Name member. Internal type is hidden to prevent exposure in the header.
  union
  {
    int8_t i8;
    uint8_t u8;
    int16_t i16;
    uint16_t u16;
    int32_t i32;
    uint32_t u32;
    int64_t i64;
    uint64_t u64;
    float f32;
    double f64;
    bool b;
    char *str;
  } value_{};              ///< Value member.
  Type type_ = kTypeNone;  ///< Type member.
};


struct MapInfoDetail;

/// A data store used to store meta data about an @c Octree. This includes
/// information about the tree depth. The structure can contain any data type
/// supported by @c MapValue. Some keys are reserved for use by the @c Octree
/// implementation, while others may be added by wrapping data structures such
/// as @c Region and @c OccupancyMap.
class ohm_API MapInfo
{
public:
  /// Create an empty @c MapInfo data store.
  MapInfo();

  /// Create @c MapInfo as an exact copy of @p other.
  /// @param other Data to copy.
  MapInfo(const MapInfo &other);

  /// R-value reference constructor, taking ownership of data in @c other and invalidating @c other.
  /// @param other Data to take ownership of.
  MapInfo(MapInfo &&other) noexcept;

  /// Copy constructor
  /// @param other Object to copy.
  MapInfo &operator=(const MapInfo &other);
  /// Assignment operator.
  /// @param other Object to copy.
  MapInfo &operator=(MapInfo &&other) noexcept;

  /// Destructor.
  ~MapInfo();

  /// Set a value in the data store. Replaces existing data with a matching key.
  /// @param value Data value to set.
  void set(const MapValue &value);

  /// Get a value from the data store.
  /// @param name The key of the data to retrieve.
  /// @param default_value The value to return if the key @p name is not found.
  /// @return The existing value or an empty/invalid @c MapValue when @p name is not matched.
  MapValue get(const char *name, MapValue default_value = MapValue()) const;

  /// Remove the data entry associated with @p name, removing it from the tree.
  /// Does nothing if there is no such key.
  /// @param name The key of the value to remove.
  /// @return True if there was an item matching @p name.
  bool remove(const char *name);

  /// Remove all data entries.
  void clear();

  /// Extracts values from the info structure, or fetches the value count.
  ///
  /// @param values Array to extract into or null to query number of values.
  /// @param max_count Element capacity of @p values, or zero to query number of values.
  /// @param offset Offset the extraction by this amount. That is, skip the first N entries
  ///     where N = @p offset.
  /// @return The number of items added to values. If @p values is null or @p max_count is
  ///   zero, returns the number of values available.
  unsigned extract(MapValue *values, unsigned max_count, unsigned offset = 0) const;

private:
  std::unique_ptr<MapInfoDetail> imp_;  ///< Data implementation.
};
}  // namespace ohm

#endif  // MAPINFO_H


================================================
FILE: ohm/MapLayer.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapLayer.h"

#include "MapChunk.h"
#include "OccupancyMap.h"
#include "VoxelLayout.h"

#include "private/MapLayerDetail.h"
#include "private/VoxelLayoutDetail.h"

#include <glm/glm.hpp>

#include <cstring>

namespace ohm
{
MapLayer::MapLayer(const char *name, unsigned layer_index, unsigned subsampling)
{
  name_ = name;
  voxel_layout_ = std::make_unique<VoxelLayoutDetail>();
  layer_index_ = layer_index;
  subsampling_ = subsampling;
  flags_ = 0;
}


MapLayer::~MapLayer() = default;


void MapLayer::clear()
{
  voxel_layout_->members.clear();
}


void MapLayer::copyVoxelLayout(const MapLayer &other)
{
  VoxelLayout voxels = voxelLayout();
  for (auto &&src_member : other.voxel_layout_->members)
  {
    voxels.addMember(src_member.name.data(), DataType::Type(src_member.type), src_member.clear_value);
  }
}


VoxelLayoutConst MapLayer::voxelLayout() const
{
  return VoxelLayoutConst(voxel_layout_.get());
}


VoxelLayout MapLayer::voxelLayout()
{
  return VoxelLayout(voxel_layout_.get());
}


MapLayoutMatch MapLayer::checkEquivalent(const MapLayer &other) const
{
  if (this == &other)
  {
    return MapLayoutMatch::kExact;
  }

  // Check the obvious first: number of layers and layer sizes.
  if (subsampling_ != other.subsampling_ || flags_ != other.flags_)
  {
    return MapLayoutMatch::kDifferent;
  }

  if (voxel_layout_->next_offset != other.voxel_layout_->next_offset ||
      voxel_layout_->voxel_byte_size != other.voxel_layout_->voxel_byte_size ||
      voxel_layout_->members.size() != other.voxel_layout_->members.size())
  {
    return MapLayoutMatch::kDifferent;
  }

  MapLayoutMatch match = MapLayoutMatch::kExact;

  if (name_ != other.name_)
  {
    // No name match. At best the layers are equivalent.
    match = MapLayoutMatch::kEquivalent;
  }

  for (size_t i = 0; i < voxel_layout_->members.size(); ++i)
  {
    const VoxelMember &a = voxel_layout_->members[i];
    const VoxelMember &b = other.voxel_layout_->members[i];
    if (a.clear_value != b.clear_value || a.type != b.type || a.offset != b.offset)
    {
      return MapLayoutMatch::kDifferent;
    }

    if (strncmp(a.name.data(), b.name.data(), a.name.size()) != 0)
    {
      match = MapLayoutMatch::kEquivalent;
    }
  }

  return match;
}

size_t MapLayer::voxelByteSize() const
{
  return voxel_layout_->voxel_byte_size;
}


size_t MapLayer::layerByteSize(const glm::u8vec3 &region_dim) const
{
  // Apply subsampling
  return volume(region_dim) * voxel_layout_->voxel_byte_size;
}


uint8_t *MapLayer::allocate(const glm::u8vec3 &region_dim) const
{
  return new uint8_t[layerByteSize(region_dim)];
}


void MapLayer::release(const uint8_t *voxels)
{
  delete[] voxels;
}


void MapLayer::clear(uint8_t *mem, const glm::u8vec3 &region_dim) const
{
  const glm::u8vec3 layer_dim = dimensions(region_dim);
  // Build a clear pattern. We progressively copy more and more memory.
  // - first build a single voxel at mem
  // - copy voxel pattern from mem to create layer_dim.x elements at mem (rows)
  // - copy first row into layer_dim.y - 1 rows to create layers
  // - copy first layer into layer_dim.z - 1 layers
  //
  // We also have a special case when the clear pattern is zero. In that case we use a single memset call.
  const size_t voxel_byte_size = voxel_layout_->voxel_byte_size;

  // Write one voxel into the destination location.
  uint8_t *dst = mem;
  bool zero_clear_pattern = true;
  for (size_t i = 0; i < voxel_layout_->members.size(); ++i)
  {
    // Grab the current member.
    VoxelMember &member = voxel_layout_->members[i];
    // Work out the member size by the difference in offets to the next member or the end of the voxel.
    size_t member_size =
      ((i + 1 < voxel_layout_->members.size()) ? voxel_layout_->members[i + 1].offset : voxel_byte_size) -
      member.offset;
    // Work out how may bytes to clear. Either the member size or the clear value size.
    const size_t clear_size = std::min(member_size, sizeof(member.clear_value));
    zero_clear_pattern = zero_clear_pattern && member.clear_value == 0;
    // Clear the bytes.
    memcpy(dst, &member.clear_value, clear_size);
    // Move to the next address.
    dst += member_size;
  }

  if (zero_clear_pattern)
  {
    // Voxel clear pattern is all zero. Just use a memset.
    memset(mem, 0, voxel_byte_size * layer_dim.x * layer_dim.y * layer_dim.z);
    return;
  }

  // Fill out a single voxel column (layer_dim.x)
  for (int x = 1; x < layer_dim.x; ++x, dst += voxel_byte_size)
  {
    memcpy(dst, mem, voxel_byte_size);
  }

  // Now repeat the column into the rows (layer_dim.y)
  for (int y = 1; y < layer_dim.y; ++y, dst += voxel_byte_size * layer_dim.x)
  {
    memcpy(dst, mem, voxel_byte_size * layer_dim.x);
  }

  // Now copy each 2D layer.
  for (int z = 1; z < layer_dim.z; ++z, dst += voxel_byte_size * layer_dim.x * layer_dim.y)
  {
    memcpy(dst, mem, voxel_byte_size * layer_dim.x * layer_dim.y);
  }

  assert(dst == mem + volume(region_dim) * voxel_byte_size);
}
}  // namespace ohm


================================================
FILE: ohm/MapLayer.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_MAPLAYER_H
#define OHM_MAPLAYER_H

#include "OhmConfig.h"

#include "MapChunk.h"
#include "MapLayoutMatch.h"
#include "VoxelLayout.h"

#include <glm/vec3.hpp>

#include <memory>
#include <string>

namespace ohm
{
struct MapChunk;
struct VoxelLayoutDetail;

/// Defines a layer in a @c MapLayout.
///
/// See @c MapLayout for details.
class ohm_API MapLayer
{
public:
  /// Configuration flags.
  enum Flag
  {
    /// Layer data is not serialised to disk.
    kSkipSerialise = (1u << 0u)
  };

  /// Construct a new layer.
  /// @param name The layer name.
  /// @param layer_index The layer index in @p MapLayout.
  /// @param subsampling Voxel downsampling factor.
  explicit MapLayer(const char *name, unsigned layer_index = 0, unsigned subsampling = 0);

  /// Destructor.
  ~MapLayer();

  /// Clears the layer details.
  void clear();

  /// Access the name.
  /// @return The layer name.
  inline const char *name() const { return name_.c_str(); }

  /// Access the layer index.
  /// @return The layer index in it's @c MapLayout.
  inline unsigned layerIndex() const { return layer_index_; }

  /// Access the layer subsampling factor.
  /// @return The layer subsampling factor.
  inline unsigned subsampling() const { return subsampling_; }

  /// Access the layer flags.
  /// @return The layer @c Flag values.
  inline unsigned flags() const { return flags_; }

  /// Set the layer flags.
  /// @param flags New flags to set.
  inline void setFlags(unsigned flags) { flags_ = flags; }

  /// Copy the @c VoxelLayout from @p other.
  /// @param other Layer to copy the voxel structure of.
  void copyVoxelLayout(const MapLayer &other);

  /// Read only access to the voxel layout of this layer.
  /// @return Voxel layout details.
  VoxelLayoutConst voxelLayout() const;

  /// Writable access to the voxel layout of this layer.
  /// @return Voxel layout configuration.
  VoxelLayout voxelLayout();

  /// Check if this @c MapLayer is equivalent to @p other.
  ///
  /// The layers are may be equivalent if voxel patterns are the same and the clearing patterns match. The names do
  /// not have to be the same.
  ///
  /// The layers match if the names also match.
  ///
  /// @param other The other layer to compare against.
  /// @return The equivalence @c MapLayoutMatch
  MapLayoutMatch checkEquivalent(const MapLayer &other) const;

  /// Gives the per chunk voxel dimensions of this layer given @p regionDim defines the maximum voxel dimensions.
  ///
  /// For each magnitude step of @c subsampling(), each element of @p regionDim is halved to a minimum of 1.
  ///
  /// @param region_dim The dimensions of each chunk the owning @c OccupancyMap.
  /// @return The voxel dimensions for this layer based on @p regionDim.
  inline glm::u8vec3 dimensions(const glm::u8vec3 &region_dim) const
  {
    const glm::u8vec3 dim = (subsampling_ == 0) ? region_dim : region_dim / uint8_t(1u << subsampling_);
    return glm::max(dim, glm::u8vec3(1));
  }

  /// Retrieve the volume of voxels in each chunk for this layer.
  ///
  /// This is the same as the volume of @c dimensions().
  ///
  /// @param region_dim The dimensions of each chunk the owning @c OccupancyMap.
  /// @return The voxel volume for this layer based on @p regionDim.
  inline size_t volume(const glm::u8vec3 &region_dim) const
  {
    const glm::u8vec3 dim = dimensions(region_dim);
    return size_t(dim.x) * size_t(dim.y) * size_t(dim.z);
  }

  /// Query the size of each voxel in this layer in bytes.
  ///
  /// Same as @c voxelLayout()->voxelByteSize().
  ///
  /// @return The size of each voxel in this layer.
  size_t voxelByteSize() const;

  /// Query the byte size this layer in each each @c MapChunk.
  /// @return The size of this layer in each @p MapChunk.
  size_t layerByteSize(const glm::u8vec3 &region_dim) const;

  /// Allocate memory for this layer given @p regionDim.
  /// @param region_dim The dimensions of each chunk the owning @c OccupancyMap.
  /// @return An uninitialised memory block of size @c layerByteSize(regionDim).
  uint8_t *allocate(const glm::u8vec3 &region_dim) const;

  /// Release memory previously allocated by @c allocate().
  /// @param voxels Memory previously allocated by @c allocate().
  static void release(const uint8_t *voxels);

  /// Clear the given layer memory to the default value.
  /// The default values are set by the @c VoxelLayout details.
  /// @param mem The memory to clear, previously allocated from @c allocate().
  /// @param region_dim The dimensions of each region with no subsampling. Subsampling is resolved as needed.
  void clear(uint8_t *mem, const glm::u8vec3 &region_dim) const;

  /// Set the layer index. Used in layer reordering. Must be maintained correctly.
  /// @param index The new layer index.
  inline void setLayerIndex(unsigned index) { layer_index_ = index; }

private:
  std::string name_;
  std::unique_ptr<VoxelLayoutDetail> voxel_layout_;
  uint16_t layer_index_ = 0;
  uint16_t subsampling_ = 0;
  unsigned flags_ = 0;
};
}  // namespace ohm

#endif  // OHM_MAPLAYER_H


================================================
FILE: ohm/MapLayout.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapLayout.h"

#include "DefaultLayer.h"
#include "MapChunk.h"

#include "private/MapLayoutDetail.h"

#include <glm/glm.hpp>

#include <algorithm>
#include <list>
#include <string>

namespace ohm
{
namespace
{
void filterLayers(MapLayoutDetail &imp, const std::vector<unsigned> &preserve_layers)
{
  if (imp.layers.empty())
  {
    return;
  }

  unsigned effective_index = 0;
  for (unsigned i = 0; i < unsigned(imp.layers.size()); ++effective_index)
  {
    MapLayer *layer = imp.layers[i];
    bool preserve = false;
    for (unsigned preserve_index : preserve_layers)
    {
      if (preserve_index == effective_index)
      {
        preserve = true;
        break;
      }
    }

    if (!preserve)
    {
      delete layer;
      imp.layers.erase(imp.layers.begin() + i);
    }
    else
    {
      layer->setLayerIndex(i);
      ++i;
    }
  }
}
}  // namespace


MapLayout::MapLayout()
  : imp_(new MapLayoutDetail)
{}


MapLayout::MapLayout(MapLayout &&other) noexcept
  : imp_(other.imp_)
{
  other.imp_ = nullptr;
}


MapLayout::MapLayout(const MapLayout &other)
  : MapLayout()
{
  *this = other;
}


MapLayout::~MapLayout()
{
  clear();
  delete imp_;
}


void MapLayout::clear()
{
  if (imp_)
  {
    imp_->clear();
  }
}


int MapLayout::occupancyLayer() const
{
  return imp_->occupancy_layer;
}


int MapLayout::meanLayer() const
{
  return imp_->mean_layer;
}


int MapLayout::traversalLayer() const
{
  return imp_->traversal_layer;
}


int MapLayout::covarianceLayer() const
{
  return imp_->covariance_layer;
}


int MapLayout::clearanceLayer() const
{
  return imp_->clearance_layer;
}


int MapLayout::intensityLayer() const
{
  return imp_->intensity_layer;
}


int MapLayout::hitMissCountLayer() const
{
  return imp_->hit_miss_count_layer;
}


MapLayoutMatch MapLayout::checkEquivalent(const MapLayout &other) const
{
  if (this == &other)
  {
    return MapLayoutMatch::kExact;
  }

  // Check the obvious first: number of layers and layer sizes.
  if (layerCount() != other.layerCount())
  {
    return MapLayoutMatch::kDifferent;
  }

  MapLayoutMatch match = MapLayoutMatch::kExact;
  for (size_t i = 0; i < layerCount(); ++i)
  {
    const MapLayoutMatch layer_match = layer(i).checkEquivalent(other.layer(i));
    match = std::min(match, layer_match);
    if (match == MapLayoutMatch::kDifferent)
    {
      return MapLayoutMatch::kDifferent;
    }
  }

  return match;
}


size_t MapLayout::calculateOverlappingLayerSet(std::vector<std::pair<unsigned, unsigned>> &overlap,
                                               const MapLayout &other) const
{
  size_t matched = 0;
  // Special case self overlap.
  if (this == &other)
  {
    for (const MapLayer *layer : imp_->layers)
    {
      assert(layer);
      overlap.emplace_back(std::make_pair<unsigned, unsigned>(layer->layerIndex(), layer->layerIndex()));
      ++matched;
    }
    return matched;
  }

  for (const MapLayer *layer : imp_->layers)
  {
    assert(layer);
    const int other_layer_index = other.layerIndex(layer->name());
    if (other_layer_index >= 0)
    {
      // Matched by name. Check layout equivalence.
      const MapLayoutMatch layer_match = layer->checkEquivalent(other.layer(other_layer_index));
      if (layer_match == MapLayoutMatch::kExact)
      {
        overlap.emplace_back(std::make_pair<unsigned, unsigned>(layer->layerIndex(), unsigned(other_layer_index)));
        ++matched;
      }
    }
  }
  return matched;
}


void MapLayout::filterLayers(const std::initializer_list<const char *> &preserve_layers)
{
  if (imp_->layers.empty())
  {
    return;
  }

  // Remove layers.
  // Convert preserve list to std::string
  std::vector<unsigned> preserve_indices;
  for (const char *layer_name : preserve_layers)
  {
    if (const MapLayer *layer = this->layer(layer_name))
    {
      preserve_indices.push_back(layer->layerIndex());
    }
  }

  ohm::filterLayers(*imp_, preserve_indices);
  // Rebind the layer index caches.
  cacheLayerIndices();
}


void MapLayout::filterLayers(const std::initializer_list<unsigned> &preserve_layers)
{
  ohm::filterLayers(*imp_, preserve_layers);
  // Rebind the layer index caches.
  cacheLayerIndices();
}


MapLayer *MapLayout::addLayer(const char *name, uint16_t subsampling)
{
  auto *new_layer = new MapLayer(name, static_cast<uint16_t>(imp_->layers.size()), subsampling);
  imp_->layers.push_back(new_layer);

  cacheLayerIndex(new_layer);
  return new_layer;
}


void MapLayout::cacheLayerIndex(const MapLayer *layer)
{
  if (layer)
  {
    std::string name_str(layer->name());
    // This form of caching layer indices is not scalable. Do no more.
    if (imp_->occupancy_layer == -1 && name_str == default_layer::occupancyLayerName())
    {
      imp_->occupancy_layer = int(layer->layerIndex());
    }
    else if (imp_->mean_layer == -1 && name_str == default_layer::meanLayerName())
    {
      imp_->mean_layer = int(layer->layerIndex());
    }
    else if (imp_->traversal_layer == -1 && name_str == default_layer::traversalLayerName())
    {
      imp_->traversal_layer = int(layer->layerIndex());
    }
    else if (imp_->covariance_layer == -1 && name_str == default_layer::covarianceLayerName())
    {
      imp_->covariance_layer = int(layer->layerIndex());
    }
    else if (imp_->clearance_layer == -1 && name_str == default_layer::clearanceLayerName())
    {
      imp_->clearance_layer = int(layer->layerIndex());
    }
    else if (imp_->intensity_layer == -1 && name_str == default_layer::intensityLayerName())
    {
      imp_->intensity_layer = layer->layerIndex();
    }
    else if (imp_->hit_miss_count_layer == -1 && name_str == default_layer::hitMissCountLayerName())
    {
      imp_->hit_miss_count_layer = layer->layerIndex();
    }
  }
}


void MapLayout::cacheLayerIndices()
{
  imp_->occupancy_layer = -1;
  imp_->mean_layer = -1;
  imp_->traversal_layer = -1;
  imp_->covariance_layer = -1;
  imp_->clearance_layer = -1;
  imp_->intensity_layer = -1;
  imp_->hit_miss_count_layer = -1;
  for (MapLayer *layer : imp_->layers)
  {
    cacheLayerIndex(layer);
  }
}


const MapLayer *MapLayout::layer(const char *layer_name) const
{
  const std::string name(layer_name);
  for (const MapLayer *layer : imp_->layers)
  {
    if (layer && name == layer->name())
    {
      return layer;
    }
  }

  return nullptr;
}


const MapLayer &MapLayout::layer(size_t index) const
{
  return *imp_->layers[index];
}


const MapLayer *MapLayout::layerPtr(size_t index) const
{
  return (imp_ && index < imp_->layers.size()) ? imp_->layers[index] : nullptr;
}


MapLayer *MapLayout::layerPtr(size_t index)
{
  return (imp_ && index < imp_->layers.size()) ? imp_->layers[index] : nullptr;
}


int MapLayout::layerIndex(const char *layer_name) const
{
  const std::string name(layer_name);
  for (const MapLayer *layer : imp_->layers)
  {
    if (layer && name == layer->name())
    {
      return int(layer->layerIndex());
    }
  }

  return -1;
}


size_t MapLayout::layerCount() const
{
  return imp_->layers.size();
}


MapLayout &MapLayout::operator=(MapLayout &&other) noexcept
{
  delete imp_;
  imp_ = other.imp_;
  other.imp_ = nullptr;
  return *this;
}


MapLayout &MapLayout::operator=(const MapLayout &other)
{
  if (this != &other)
  {
    clear();
    if (other.imp_)
    {
      for (auto &&layer : other.imp_->layers)
      {
        MapLayer *new_layer = addLayer(layer->name(), layer->subsampling());
        new_layer->copyVoxelLayout(*layer);
      }
    }
  }
  return *this;
}
}  // namespace ohm


================================================
FILE: ohm/MapLayout.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_MAPLAYOUT_H
#define OHM_MAPLAYOUT_H

#include "OhmConfig.h"

#include "MapLayoutMatch.h"

#include <initializer_list>
#include <utility>
#include <vector>

namespace ohm
{
struct MapLayoutDetail;
class MapLayer;
struct MapChunk;

/// A @c MapLayout defines the structure of the voxel in an @c OccupancyMap stored in a @c MapChunk. Each chunk has
/// @c MapChunk::voxel_maps which store voxel data in a series of independent layers. The @c MapLayout defines the
/// number of layers and identifies the data structure of each layer (via @c MapLayer).
///
/// Each @c MapLayer is named, indexes a specific array in @c MapChunk::voxel_maps and defines the data contained
/// in the layer via a @c VoxelLayout. The @c VoxelLayout may be used to define a pseudo data structure by adding
/// named "members" of the specified type and default value. The @c MapLayer also provided functions for accessing
/// voxels from the @c MapChunk. A @c MapLayer may optionally downsample the map's default voxel resolution.
///
/// The default @c MapLayout specifies the following layers:
/// - @c DL_Occupancy - An array of float voxels identifying the occupancy for each voxel.
/// - @c DL_Clearance - An array of float ranges identifying the distance to the nearest obstruction for each voxel.
/// - @c DL_CoarseClearance - A float array downsampling the @c DL_Clearance layer (*NYI*).
/// Additional layers are user defined.
///
/// The following example code shows firstly how the default layers are created and adds user defined layer to hold
/// a data structure.
///
/// @code
/// struct UserLayerStruct
/// {
///   double timestamp;
///   float x, y, z;
/// };
///
/// void createLayers(ohm::OccupancyMap &map)
/// {
///   ohm::MapLayer *layer;
///   ohm::VoxelLayout voxel;
///   size_t clearValue = 0;
///
///   ohm::MayLayout layout = map.layout();
///   layout.clear(); // Clear the existing layout. Must be done before adding to the map.
///
///   // Setup DL_Occupancy layer.
///   // Fetch the value we'll clear voxels with (default value).
///   const float invalidMarkerValue = unobservedOccupancyValue();
///   /// Write the invalidMarkerValue value into a size_t item which will be the clear value for the member.
///   memcpy(&clearValue, &invalidMarkerValue, std::min(sizeof(invalidMarkerValue), sizeof(clearValue)));
///   // Create the occupancy layer.
///   layer = layout.addLayer(default_layer::occupancyLayerName(), 0);
///   // Get it's VoxelLayout for modification.
///   voxel = layer->voxelLayout();
///   /// Add the occupancy value member as a float. Set the default value to -1.
///   voxel.addMember(default_layer::occupancyLayerName(), DataType::Float, clearValue);
///
///   // Setup DL_Clearance
///   const float defaultClearance = -1.0f; // Default value is -1.
///   memcpy(&clearValue, &defaultClearance, std::min(sizeof(defaultClearance), sizeof(clearValue)));
///   // Create the layer.
///   layer = layout.addLayer(default_layer::clearanceLayerName(), 0);
///   voxel = layer->voxelLayout();
///   // Add clearance value member.
///   voxel.addMember(default_layer::clearanceLayerName(), DataType::Float, clearValue);
///
///   // Add a layer supporting UserLayerStruct above. Always clear to zero for these members.
///   clearValue = 0;
///   // Add the layer.
///   layer = layout.addLayer("userLayer");
///   voxel = layer->voxelLayout();
///   // Add multiple members in order matching UserLayerStruct.
///   voxel.addMember("timestamp", DataType::Double, clearValue);
///   voxel.addMember("x", DataType::Float, clearValue);
///   voxel.addMember("y", DataType::Float, clearValue);
///   voxel.addMember("z", DataType::Float, clearValue);
///   // Assert the size matches the expected size.
///   // Note: in practice packing directives may be needed on UserLayerStruct to make this true.
///   assert(sizeof(UserLayerStruct) == voxel.voxelByteSize());
/// }
///
/// UserLayerStruct *getUserLayerStruct(const ohm::Key &voxelKey, ohm::OccupancyMap &map)
/// {
///   MapChunk *chunk = map.region(voxelKey.regionKey(), false);
///   MapLayer *layer = map.layout().layer("userLayer");
///   if (!chunk || !layer)
///   {
///     return nullptr;
///   }
///
///   UserLayerStruct *voxels = return layer->voxelsAs<UserLayerStruct>(*chunk);
///   // Resolve the layer dimensions via the layer API to account for any downsampling.
///   const glm::u8vec3 layerDimensions = layer->dimensions(map.regionVoxelDimensions());
///   // Convert to a linear index into voxels.
///   const unsigned voxelIndex = ohm::voxelIndex(voxelKey, layerDimensions); // From MapChunk.h
///   return voxels[voxelIndex];
/// }
/// @endcode
class ohm_API MapLayout
{
public:
  /// Create an empty layout.
  MapLayout();
  /// Move constructor.
  /// @param other The layout to move.
  MapLayout(MapLayout &&other) noexcept;
  /// Copy constructor.
  /// @param other The layout to deep copy.
  MapLayout(const MapLayout &other);

  /// Destructor.
  ~MapLayout();

  /// Drop all layout information, resulting in an empty layout.
  void clear();

  /// Cached index to the "occupancy" layer.
  /// @return The occupancy layer index or -1 if not present.
  int occupancyLayer() const;

  /// Cached index to the "mean" layer. This layer holds @c VoxelMean data.
  /// @return The voxel mean layer index or -1 if not present.
  int meanLayer() const;

  /// Cache index to the "traversal" layer. This layer holds a single @c float per voxel which tracks the accumulated
  /// distance travelled of all rays though that voxel.
  /// @return The voxel traversal layer index or -1 if not present.
  int traversalLayer() const;

  /// Cached index to the "covariance" layer. This layer holds the @c CovarianceVoxel data.
  /// @return The voxel covariance layer index or -1 if not present.
  int covarianceLayer() const;

  /// Cached index to the "clearance" layer.
  /// @return The clearance layer index or -1 if not present.
  int clearanceLayer() const;

  /// Cached index to the "intensity" layer.
  /// @return The intensity layer index or -1 if not present.
  int intensityLayer() const;

  /// Cached index to the "hit_miss_count" layer.
  /// @return The hit miss count layer index or -1 if not present.
  int hitMissCountLayer() const;

  /// Check if this @c MapLayout is equivalent to @p other.
  ///
  /// The layouts are may be equivalent if they share the same number of layers, the voxel patterns are the same
  /// for each layer and the clearing patterns match. The names do not have to be the same.
  ///
  /// The layouts match if all layers names and voxel layouts match.
  ///
  /// @param other The other layout to compare against.
  /// @return The equivalence @c MapLayoutMatch
  MapLayoutMatch checkEquivalent(const MapLayout &other) const;

  /// Calculate which layers from this object are also present in @p other . For any match we add an entry to @p overlap
  /// which identifies this object's layer index and the @p other object's layer index (in that order).
  ///
  /// Layers are matched first by name, then using @c MapLayer::checkEquivalent() looking for an exact match.
  ///
  /// @param[out] overlap The overlap set. Note: it is the caller's responsibility to ensure this object is empty before
  ///   calling this function.
  /// @param other The map to determine the overlap with.
  /// @return The number of layers matched between the @c MapLayout objects.
  size_t calculateOverlappingLayerSet(std::vector<std::pair<unsigned, unsigned>> &overlap,
                                      const MapLayout &other) const;

  /// Remove all layers except for the named layers. Removing interleaved layers may create gaps in the layer
  /// array. These gaps are removed with the array being repacked.
  /// @param preserve_layers Names of the layers to preserve. Exact match required.
  void filterLayers(const std::initializer_list<const char *> &preserve_layers);

  /// Remove all layers except for the identified layers. Removing interleaved layers may create gaps in the layer
  /// array. These gaps are removed with the array being repacked.
  /// @param preserve_layers Indices of the layers to preserve.
  void filterLayers(const std::initializer_list<unsigned> &preserve_layers);

  /// Add a layer to the map. The layer starts undefined and needs to have it's @c VoxelLayout populated.
  ///
  /// @param name The name of the new layer. Should be unique, but this is not checked.
  /// @param subsampling Voxel subsampling. Each increment combines 8 voxels into 1.
  /// @return The new layer. The @c MapLayer::layerIndex() serves as it's id for use with @c layer() calls.
  MapLayer *addLayer(const char *name, uint16_t subsampling = 0);

  /// Retrieve a layer by name (exact match). By iterative search.
  /// @param layer_name The name of the layer to search for.
  /// @return The first layer matching @p layerName or null if not found.
  const MapLayer *layer(const char *layer_name) const;

  /// Retrieve a layer by index.
  /// @param index The layer index in the range [0, @c layerCount()).
  /// @return The layer at the given @p index.
  const MapLayer &layer(size_t index) const;

  /// Retrieve a layer pointer by index. This allows @p index to be out of range.
  /// @param index The layer index.
  /// @return The layer at the given @p index or null if @p index is out of range [0, @c layerCount()).
  const MapLayer *layerPtr(size_t index) const;

  /// Retrieve a layer pointer by index. This allows @p index to be out of range.
  ///
  /// For internal use only. Changing a layer will invalidate a map leading to undefined behaviour.
  ///
  /// @param index The layer index.
  /// @return The layer at the given @p index or null if @p index is out of range [0, @c layerCount()).
  MapLayer *layerPtr(size_t index);

  /// Search for a layer matching @p layer_name and return it's index or -1 if not found.
  /// @param layer_name Name of the layer to search for.
  /// @return The index of the first layer matching @p layer_name or -1 if not found.
  int layerIndex(const char *layer_name) const;

  /// Retrieve the number of layers.
  /// @return The number of registered layers.
  size_t layerCount() const;

  /// Move assignment.
  /// @param other The layout to move.
  MapLayout &operator=(MapLayout &&other) noexcept;

  /// Copy assignment (deep copy).
  /// @param other The layout to copy.
  MapLayout &operator=(const MapLayout &other);

private:
  /// Cache layer index if @p layer is a known layer such as @c meanLayer() .
  /// @param layer The layer to check.
  void cacheLayerIndex(const MapLayer *layer);

  /// Cache all known layer indices such as @c meanLayer() .
  void cacheLayerIndices();

  MapLayoutDetail *imp_;
};
}  // namespace ohm

#endif  // OHM_MAPLAYOUT_H


================================================
FILE: ohm/MapLayoutMatch.h
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2020
//
#ifndef MAPLAYOUTMATCH_H
#define MAPLAYOUTMATCH_H

#include "OhmConfig.h"

namespace ohm
{
/// Return values for @c MapLayout, @p MapLayer and @p VoxelLayout checks.
enum class MapLayoutMatch : int
{
  /// Layers are different and do not match.
  kDifferent = 0,
  /// The layout matches, but some of the layer and voxel member names may differ.
  kEquivalent,
  /// The layer match exactly.
  kExact
};
}  // namespace ohm

#endif  // MAPLAYOUTMATCH_H


================================================
FILE: ohm/MapProbability.h
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) 2015 CSIRO
//
#ifndef OHM_PROBABILITY_H
#define OHM_PROBABILITY_H

#include "OhmConfig.h"

#include <cmath>
#include <limits>

namespace ohm
{
/// Calculate a probability from its log value.
/// @param value The log probability value.
/// @return A real probability value [0, 1].
template <typename real>  // NOLINT(readability-identifier-naming)
inline real valueToProbability(real value)
{
  // Ensure -infinity yields a zero probability.
  // Not all environments respect the sign of an infinity. Some will and yield 0 and 1
  // for -infinity and +infinity respectively, other's yield 1. We explicitly correct this.
  return (value == -std::numeric_limits<real>::infinity()) ? real{ 0 } :
                                                             real{ 1 } - (real{ 1 } / (real{ 1 } + std::exp(value)));
}

/// Convert a probability to a storable value. Inverse of @p valueToProbability().
/// @param probability The probability to convert
/// @return The value associated with @p probability."ohmconfig.h"
template <typename real>  // NOLINT(readability-identifier-naming)
inline real probabilityToValue(real probability)
{
  return std::log(probability / (real{ 1 } - probability));
}
}  // namespace ohm

#endif  // OHM_PROBABILITY_H


================================================
FILE: ohm/MapRegion.cpp
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2017
//
#include "MapRegion.h"

#include <ohmutil/VectorHash.h>

#include "Key.h"
#include "MapCoord.h"

#ifdef OHM_VALIDATION
#include <cstdio>
#endif  // OHM_VALIDATION

namespace ohm
{
unsigned MapRegion::Hash::calculate(const MapRegion &key)
{
  const glm::i32vec3 hash_coord(key.coord);
  return vhash::hashBits(hash_coord.x, hash_coord.y, hash_coord.z);
}


unsigned MapRegion::Hash::calculate(const glm::i16vec3 &region_coord)
{
  const glm::i32vec3 hash_coord(region_coord);
  return vhash::hashBits(hash_coord.x, hash_coord.y, hash_coord.z);
}


MapRegion::MapRegion(const glm::dvec3 &point, const glm::dvec3 &map_origin, const glm::dvec3 &region_dimensions)
{
  // Quantise.
  coord.x = pointToRegionCoord(double(point.x - map_origin.x), region_dimensions.x);
  coord.y = pointToRegionCoord(double(point.y - map_origin.y), region_dimensions.y);
  coord.z = pointToRegionCoord(double(point.z - map_origin.z), region_dimensions.z);

  // Centre
  centre.x = regionCentreCoord(coord.x, region_dimensions.x);
  centre.y = regionCentreCoord(coord.y, region_dimensions.y);
  centre.z = regionCentreCoord(coord.z, region_dimensions.z);
}


bool MapRegion::voxelKey(Key &key, const glm::dvec3 &point, const glm::dvec3 &map_origin,
                         const glm::dvec3 &region_dimensions, const glm::ivec3 &voxel_counts,
                         double voxel_resolution) const
{
  // Localise.
  const glm::dvec3 region_min = centre - 0.5 * region_dimensions;
  const glm::dvec3 p = point - map_origin - region_min;
  // Quantise.
  const glm::ivec3 q(pointToRegionVoxel(p.x, voxel_resolution, region_dimensions.x),
                     pointToRegionVoxel(p.y, voxel_resolution, region_dimensions.y),
                     pointToRegionVoxel(p.z, voxel_resolution, region_dimensions.z));

  if (0 <= q.x && q.x < voxel_counts.x && 0 <= q.y && q.y < voxel_counts.y && 0 <= q.z && q.z < voxel_counts.z)
  {
    key.setRegionKey(coord);
    key.setLocalKey(q);
    return true;
  }

  // Invalidate the key.
  key = Key::kNull;
  // Out of range.
  return false;
}
}  // namespace ohm


================================================
FILE: ohm/MapRegion.h
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2017
//
#ifndef OHM_MAPREGION_H
#define OHM_MAPREGION_H

#include "OhmConfig.h"

#include <glm/glm.hpp>

namespace ohm
{
class Key;

/// Represents a large scale spatial region within an @c OccupancyMap.
///
/// This is usually associated with a set of voxels contained in a @c MapChunk.
struct ohm_API MapRegion
{
  /// Centre of the map region local to the map origin.
  glm::dvec3 centre = glm::dvec3(0);
  /// Quantised integer indexing of the region within the map.
  glm::i16vec3 coord = glm::dvec3(0);

  /// Hashing function converting a region key or region coordinates into a 32-bit hash value.
  ///
  /// Supports various data sources.
  struct ohm_API Hash
  {
    /// Hash a @c MapRegion.
    /// @param key The region to hash.
    /// @return The 32-bit hash for @p key.
    inline unsigned operator()(const MapRegion &key) const { return calculate(key); }
    /// Hash a @c MapRegion.
    /// @param key The region to hash.
    /// @return The 32-bit hash for @p key.
    static unsigned calculate(const MapRegion &key);
    /// Hash quantised integer indexing coordinates for a region.
    /// @param region_coord The region coordinates to hash.
    /// @return The 32-bit hash for @p key.
    inline unsigned operator()(const glm::i16vec3 &region_coord) const { return calculate(region_coord); }
    /// Hash quantised integer indexing coordinates for a region.
    /// @param region_coord The region coordinates to hash.
    /// @return The 32-bit hash for @p key.
    static unsigned calculate(const glm::i16vec3 &region_coord);
  };
  friend struct Hash;

  /// Default constructor: member initialisation is undefined.
  MapRegion() = default;

  /// Initialise a region containing the given @p point, based on the given map details.
  /// @param point A spatial point contained by the region.
  /// @param map_origin The origin of the map containing the region. The @p point internally is made relative to this
  /// origin.
  /// @param region_dimensions Spatial expanse of a region within the map of interest.
  MapRegion(const glm::dvec3 &point, const glm::dvec3 &map_origin, const glm::dvec3 &region_dimensions);

  /// Attempt to generate an @c Key for a voxel containing @p point if it lies in this region.
  ///
  /// Fails if the @p point does not lie within this region.
  ///
  /// @param[out] key Set to the key for the voxel containing @p point. A null key on failure.
  /// @param point The spatial point for which to generate a voxel key.
  /// @param map_origin The origin of the map containing this region. The @p point is internally
  ///   made relative to this origin.
  /// @param region_dimensions Spatial expanse of a region within the map of interest.
  /// @param voxel_counts The number of voxels in each region along each axis.
  /// @param voxel_resolution The length of a voxel cube edge.
  /// @return True if the @p point lies within this region and @p key has been set to reference
  ///   the containing voxel. Otherwise @p key is a @c OccupancyMap::null key.
  bool voxelKey(Key &key, const glm::dvec3 &point, const glm::dvec3 &map_origin, const glm::dvec3 &region_dimensions,
                const glm::ivec3 &voxel_counts, double voxel_resolution) const;
};
}  // namespace ohm

#endif  // OHM_MAPREGION_H


================================================
FILE: ohm/MapRegionCache.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapRegionCache.h"

namespace ohm
{
struct MapRegionCacheDetail
{
  // empty
};

MapRegionCache::MapRegionCache() = default;

MapRegionCache::~MapRegionCache() = default;

}  // namespace ohm


================================================
FILE: ohm/MapRegionCache.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_MAPREGIONCACHE_H
#define OHM_MAPREGIONCACHE_H

#include "OhmConfig.h"

#include <glm/fwd.hpp>

namespace ohm
{
struct MapRegionCacheDetail;
struct MapChunk;

/// Base class for any memory cache responsible for ensuring map regions are available in the correct memory.
///
/// Initially this class is simply a placeholder base class for the GPU cache. In future it map be extended to
/// maintain an in-core, uncompressed region cache with compressed data attached to @c MapRegion objects.
class ohm_API MapRegionCache
{
public:
  /// Constructor.
  MapRegionCache();
  /// Virtual destructor.
  virtual ~MapRegionCache();

  /// Reinitialise the cache. Called after restructuring region layout.
  virtual void reinitialise() = 0;

  /// Flush any outstanding operations. For example, the GPU cache will ensure host memory is up to date.
  virtual void flush() = 0;

  /// Clear the cache. May block on in flight operations. Called before restructuring region layout.
  virtual void clear() = 0;

  /// Remove/flush the region matching @p region_coord from the cache.
  /// @param region_coord The region to flush from the cache.
  virtual void remove(const glm::i16vec3 &region_coord) = 0;

  /// Sync from @p src_chunk at @p src_layer to @p dst_chunk at @p dst_layer provided the source chunk and layer are
  /// currently cached.
  ///
  /// While the @p src_chunk and @p src_layer must be valid in the same map the @p MapRegionCache operates on, the
  /// @p dst_chunk and @p dst_layer may be in another map. For example, this function can be used to sync from GPU
  /// memory of one layer to CPU memory of another layer, even in a different map, without having to sync the original
  /// chunk back to CPU first.
  ///
  /// Implementations may make the following assumptions on the arguments given:
  ///
  /// - The source and destination layers are not the same.
  /// - The source chunk belongs to the same map as that which this cache object targets.
  /// - The layer indices are valid.
  ///
  /// The call should do return false if the @p src_chunk and @p src_layer pair are not currently cached by this object.
  /// In this case, no other operation should be performed.
  ///
  /// @param dst_chunk The chunk object to sync to.
  /// @param dst_layer The index to sync to in @c MapChunk::voxel_blocks .
  /// @param src_chunk The chunk to sync from.
  /// @param src_layer The layer to sync from.
  /// @return True if the source chunk/layer pairing are cached by this object and have been copied to the destination
  ///   chunk/layer pairing.
  virtual bool syncLayerTo(MapChunk &dst_chunk, unsigned dst_layer, const MapChunk &src_chunk, unsigned src_layer) = 0;

  /// Find the @c MapRegionCache which specifically targets the specified voxel @p layer . This supports nested caching
  /// as used by the @c GpuLayerCache . May return itself. May return null there is no cache for @p layer .
  /// @param layer The specific layer of interest.
  /// @return The subcache for @p layer - possibly @c this - on success, null on failure.
  virtual MapRegionCache *findLayerCache(unsigned layer) = 0;

private:
  std::unique_ptr<MapRegionCacheDetail> imp_;
};
}  // namespace ohm

#endif  // OHM_MAPREGIONCACHE_H


================================================
FILE: ohm/MapSerialise.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapSerialise.h"

#include "DefaultLayer.h"
#include "MapChunk.h"
#include "MapFlag.h"
#include "MapLayer.h"
#include "MapLayout.h"
#include "OccupancyMap.h"
#include "Stream.h"
#include "VoxelBlock.h"
#include "VoxelBuffer.h"
#include "VoxelLayout.h"

#include "private/OccupancyMapDetail.h"
#include "private/SerialiseUtil.h"

#include "serialise/MapSerialiseV0.1.h"
#include "serialise/MapSerialiseV0.2.h"
#include "serialise/MapSerialiseV0.4.h"
#include "serialise/MapSerialiseV0.5.h"
#include "serialise/MapSerialiseV0.h"

#include <glm/glm.hpp>

#include <array>
#include <cstdio>
#include <cstring>
#include <functional>
#include <map>
#include <mutex>
#include <string>
#include <type_traits>
#include <vector>

#include <zlib.h>

namespace
{
inline std::pair<int, std::string> makeErrorCode(ohm::SerialisationError code, const std::string &str)
{
  return std::make_pair(int(code), str);
}

std::mutex s_error_code_lock;
std::map<int, std::string> s_error_codes = { makeErrorCode(ohm::kSeOk, "ok"),
                                             makeErrorCode(ohm::kSeFileCreateFailure, "file create failure"),
                                             makeErrorCode(ohm::kSeFileOpenFailure, "file open failure"),
                                             makeErrorCode(ohm::kSeFileWriteFailure, "write failure"),
                                             makeErrorCode(ohm::kSeFileReadFailure, "read failure"),
                                             makeErrorCode(ohm::kSeValueOverflow, "value overflow"),
                                             makeErrorCode(ohm::kSeMemberOffsetError, "member offset error"),
                                             makeErrorCode(ohm::kSeInfoError, "info error"),
                                             makeErrorCode(ohm::kSeDataItemTooLarge, "data item too large"),
                                             makeErrorCode(ohm::kSeUnknownDataType, "unknown data type"),
                                             makeErrorCode(ohm::kSeUnsupportedVersion, "unsupported version"),
                                             makeErrorCode(ohm::kSeDeprecatedVersion, "deprecated version"),
                                             makeErrorCode(ohm::kSeExtensionCode, "unknown extension error") };
}  // namespace

namespace ohm
{
/// Contains the header marker and version information for an occupancy map.
/// The header should equal @c MapHeaderMarker, while the version may vary.
/// Version zero did not store any header marker or version number.
///
/// The version number is arranged as follows (in base 10) @c vvvMMMPPP where:
/// - @c vvv is the major version number (any number)
/// - @c MMM is a three digit specification of the current minor version.
/// - @c PPP is a three digit specification of the current patch version.
struct HeaderVersion
{
  /// Marker equal to @c MapHeaderMarker if valid.
  uint32_t marker = 0;
  /// Map format version number.
  MapVersion version = { 0, 0, 0 };
};

const uint32_t kMapHeaderMarker = 0x44330011u;

// Digits are arranged as follows:
//    vvvMMMPPP
// where:
// - vvv is the major version number (any number)
// - MMM is a three digit specification of the current minor version.
// - PPP is a three digit specification of the current patch version.
const MapVersion kSupportedVersionMin = { 0, 0, 0 };
const MapVersion kSupportedVersionMax = { 0, 5, 0 };
const MapVersion kCurrentVersion = { 0, 5, 0 };

// Note: version 0.3.x is not supported.

int saveItem(OutputStream &stream, const MapValue &value)
{
  //{
  //  MapValue strValue = value.toStringValue();
  //  fprintf(stderr, "w: %s = %s\n", value.name(), static_cast<const char *>(strValue));
  //}
  const char *name = value.name();
  size_t len = strlen(name);
  if (len > std::numeric_limits<uint16_t>::max())
  {
    return kSeDataItemTooLarge;
  }

  auto len16 = uint16_t(len);
  // if (endianSwap)
  // {
  //   endian::endianSwap(&len16);
  // }
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  stream.write(reinterpret_cast<char *>(&len16), sizeof(len16));

  if (len)
  {
    stream.write(name, unsigned(len));
  }

  uint8_t type = value.type();
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  stream.write(reinterpret_cast<char *>(&type), 1);

  switch (value.type())
  {
  case MapValue::kInt8:
  {
    auto val = static_cast<int8_t>(value);
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&val), 1);
    break;
  }
  case MapValue::kUInt8:
  {
    auto val = static_cast<uint8_t>(value);
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&val), 1);
    break;
  }
  case MapValue::kInt16:
  {
    auto val = static_cast<int16_t>(value);
    // if (endianSwap) { endian::endianSwap(&val); }
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&val), sizeof(val));
    break;
  }
  case MapValue::kUInt16:
  {
    auto val = static_cast<uint16_t>(value);
    // if (endianSwap) { endian::endianSwap(&val); }
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&val), sizeof(val));
    break;
  }
  case MapValue::kInt32:
  {
    auto val = static_cast<int32_t>(value);
    // if (endianSwap) { endian::endianSwap(&val); }
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&val), sizeof(val));
    break;
  }
  case MapValue::kUInt32:
  {
    auto val = static_cast<uint32_t>(value);
    // if (endianSwap) { endian::endianSwap(&val); }
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&val), sizeof(val));
    break;
  }
  case MapValue::kInt64:
  {
    auto val = static_cast<int64_t>(value);
    // if (endianSwap) { endian::endianSwap(&val); }
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&val), sizeof(val));
    break;
  }
  case MapValue::kUInt64:
  {
    auto val = static_cast<uint64_t>(value);
    // if (endianSwap) { endian::endianSwap(&val); }
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&val), sizeof(val));
    break;
  }
  case MapValue::kFloat32:
  {
    auto val = static_cast<float>(value);
    // if (endianSwap) { endian::endianSwap(&val); }
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&val), sizeof(val));
    break;
  }
  case MapValue::kFloat64:
  {
    auto val = static_cast<double>(value);
    // if (endianSwap) { endian::endianSwap(&val); }
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&val), sizeof(val));
    break;
  }
  case MapValue::kBoolean:
  {
    bool bval = static_cast<bool>(value);
    uint8_t val = (bval) ? 1 : 0;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&val), 1);
    break;
  }
  case MapValue::kString:
  {
    const char *str = static_cast<const char *>(value);
    len = strlen(str);
    if (len > std::numeric_limits<uint16_t>::max())
    {
      return kSeDataItemTooLarge;
    }

    len16 = uint16_t(len);
    // if (endianSwap)
    // {
    //   endian::endianSwap(&len16);
    // }
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    stream.write(reinterpret_cast<char *>(&len16), sizeof(len16));

    if (unsigned(len))
    {
      stream.write(str, unsigned(len));
    }
    break;
  }

  default:
    return kSeDataItemTooLarge;
  }

  return kSeOk;
}


int saveMapInfo(OutputStream &stream, const MapInfo &map_info)
{
  uint32_t item_count = map_info.extract(nullptr, 0);

  bool ok = true;
  ok = writeUncompressed<uint32_t>(stream, item_count) && ok;

  if (!ok)
  {
    return kSeInfoError;
  }

  if (item_count == 0)
  {
    return kSeOk;
  }

  std::vector<MapValue> values(item_count);
  values.resize(item_count);
  unsigned extracted = map_info.extract(values.data(), item_count);

  if (extracted != item_count)
  {
    return kSeInfoError;
  }

  int err = 0;
  for (unsigned i = 0; i < extracted; ++i)
  {
    err = saveItem(stream, values[i]);
    if (err != kSeOk)
    {
      return err;
    }
  }

  return kSeOk;
}


int saveHeader(OutputStream &stream, const OccupancyMapDetail &map)
{
  bool ok = true;
  // Header marker + version
  HeaderVersion version;
  version.marker = kMapHeaderMarker;
  version.version = kCurrentVersion;

  ok = writeUncompressed<uint32_t>(stream, version.marker) && ok;
  ok = writeUncompressed<uint32_t>(stream, version.version.major) && ok;
  ok = writeUncompressed<uint16_t>(stream, version.version.minor) && ok;
  ok = writeUncompressed<uint16_t>(stream, version.version.patch) && ok;

  ok = writeUncompressed<double>(stream, map.origin.x) && ok;
  ok = writeUncompressed<double>(stream, map.origin.y) && ok;
  ok = writeUncompressed<double>(stream, map.origin.z) && ok;
  ok = writeUncompressed<double>(stream, map.region_spatial_dimensions.x) && ok;
  ok = writeUncompressed<double>(stream, map.region_spatial_dimensions.y) && ok;
  ok = writeUncompressed<double>(stream, map.region_spatial_dimensions.z) && ok;
  ok = writeUncompressed<int32_t>(stream, map.region_voxel_dimensions.x) && ok;
  ok = writeUncompressed<int32_t>(stream, map.region_voxel_dimensions.y) && ok;
  ok = writeUncompressed<int32_t>(stream, map.region_voxel_dimensions.z) && ok;
  ok = writeUncompressed<double>(stream, map.resolution) && ok;
  ok = writeUncompressed<double>(stream, map.occupancy_threshold_value) && ok;
  ok = writeUncompressed<double>(stream, map.hit_value) && ok;
  ok = writeUncompressed<double>(stream, map.miss_value) && ok;
  ok = writeUncompressed<uint32_t>(stream, map.chunks.size()) && ok;

  // Added v0.5.0
  ok = writeUncompressed<double>(stream, map.first_ray_time) && ok;

  // Added v0.3.0
  // Saving the map stamp has become important to ensure MapChunk::touched_stamps are correctly maintained.
  ok = writeUncompressed<uint64_t>(stream, map.stamp) && ok;

  // Add v0.3.2
  ok = writeUncompressed<uint32_t>(stream, std::underlying_type_t<MapFlag>(map.flags)) && ok;

  return (ok) ? 0 : kSeFileWriteFailure;
}


int saveLayout(OutputStream &stream, const OccupancyMapDetail &map)
{
  // Save details about the map layers.
  const MapLayout &layout = map.layout;
  const auto layer_count = uint32_t(layout.layerCount());
  bool ok = true;

  ok = write<int32_t>(stream, layer_count) && ok;

  for (unsigned i = 0; i < layer_count; ++i)
  {
    const MapLayer &layer = layout.layer(i);
    // Write the layer name.
    auto val32 = uint32_t(strlen(layer.name()));
    ok = write<uint32_t>(stream, val32) && ok;
    ok = stream.write(layer.name(), val32) == val32 && ok;

    // Write flags.
    val32 = layer.flags();
    ok = write<uint32_t>(stream, val32) && ok;

    // Write the subsampling
    ok = write<uint16_t>(stream, uint16_t(layer.subsampling())) && ok;

    // Write voxel layout.
    VoxelLayoutConst voxel_layout = layer.voxelLayout();
    // Write voxel size.
    val32 = uint32_t(voxel_layout.voxelByteSize());
    ok = write<uint32_t>(stream, val32) && ok;
    // Write member count.
    val32 = uint32_t(voxel_layout.memberCount());
    ok = write<uint32_t>(stream, val32) && ok;
    for (size_t j = 0; j < voxel_layout.memberCount(); ++j)
    {
      // Write member name.
      val32 = uint32_t(strlen(voxel_layout.memberName(j)));
      ok = write<uint32_t>(stream, val32) && ok;
      ok = stream.write(voxel_layout.memberName(j), val32) == val32 && ok;
      // Member type.

      // Write member type and offset.
      uint16_t val16;
      val16 = uint16_t(voxel_layout.memberType(j));
      ok = write<uint16_t>(stream, val16) && ok;
      val16 = uint16_t(voxel_layout.memberOffset(j));
      ok = write<uint16_t>(stream, val16) && ok;

      // Write clear value.
      uint64_t clear_value = voxel_layout.memberClearValue(j);
      ok = write<uint64_t>(stream, clear_value) && ok;
    }
  }

  return (ok) ? 0 : kSeFileWriteFailure;
}


int saveChunk(OutputStream &stream, const MapChunk &chunk, const OccupancyMapDetail &detail)
{
  bool ok = true;

  // Write region details, then nodes. MapChunk members are derived.
  ok = write<int32_t>(stream, chunk.region.coord.x) && ok;
  ok = write<int32_t>(stream, chunk.region.coord.y) && ok;
  ok = write<int32_t>(stream, chunk.region.coord.z) && ok;
  ok = write<double>(stream, chunk.region.centre.x) && ok;
  ok = write<double>(stream, chunk.region.centre.y) && ok;
  ok = write<double>(stream, chunk.region.centre.z) && ok;
  ok = write<double>(stream, chunk.touched_time) && ok;

  // Save each map layer.
  const MapLayout &layout = chunk.layout();
  for (size_t i = 0; i < layout.layerCount(); ++i)
  {
    const MapLayer &layer = layout.layer(i);

    if (layer.flags() & MapLayer::kSkipSerialise)
    {
      // Not to be serialised.
      continue;
    }

    uint64_t layer_touched_stamp = chunk.touched_stamps[i];
    ok = write<uint64_t>(stream, layer_touched_stamp) && ok;

    // Get the layer memory.
    VoxelBuffer<const VoxelBlock> voxel_buffer(chunk.voxel_blocks[layer.layerIndex()]);
    const uint8_t *layer_mem = voxel_buffer.voxelMemory();
    const size_t node_count = layer.volume(detail.region_voxel_dimensions);
    const size_t node_byte_count = layer.voxelByteSize() * node_count;
    if (node_byte_count != unsigned(node_byte_count))
    {
      return kSeValueOverflow;
    }

    ok = stream.write(layer_mem, unsigned(node_byte_count)) == node_byte_count && ok;
  }

  return (ok) ? 0 : kSeFileWriteFailure;
}


int loadHeader(InputStream &stream, HeaderVersion &version, OccupancyMapDetail &map, size_t &region_count)
{
  bool ok = true;

  // Try for marker and version number. Was not present in the original code.
  static_assert(sizeof(version.marker) + sizeof(version.version.major) == sizeof(map.origin.x),
                "Assuming we can migrate to double if no marker present");
  ok = readRaw<uint32_t>(stream, version.marker) && ok;
  ok = readRaw<uint32_t>(stream, version.version.major) && ok;

  if (version.marker == kMapHeaderMarker)
  {
    ok = readRaw<uint16_t>(stream, version.version.minor) && ok;
    ok = readRaw<uint16_t>(stream, version.version.patch) && ok;

    if (!ok)
    {
      return kSeFileReadFailure;
    }

    // Have a header marker and version number. Check version.
    if (version.version < kSupportedVersionMin || version.version > kSupportedVersionMax)
    {
      return kSeUnsupportedVersion;
    }

    if (version.version.major == 0 && version.version.minor == 3)
    {
      // Version 0.3.x not supported. That introduced voxel mean positioning using a progressive weighting.
      // Support in 0.4.0 changed to separate VoxelMean layer using a progressive point count yielding much better
      // coordinates.
      return kSeDeprecatedVersion;
    }

    ok = readRaw<double>(stream, map.origin.x) && ok;
  }
  else
  {
    // No marker. Assume version zero and migrate to map.origin.x
    std::array<uint8_t, sizeof(double)> buffer;
    static_assert(sizeof(buffer) >= sizeof(version.marker) + sizeof(version.version.major),
                  "Read ahead buffer too small.");
    memcpy(buffer.data(), &version.marker, sizeof(version.marker));
    memcpy(buffer.data() + sizeof(version.marker), &version.version.major, sizeof(version.version.major));
    memcpy(&map.origin.x, buffer.data(), sizeof(map.origin.x));

    version.marker = 0;
    version.version = { 0, 0, 0 };
  }

  ok = readRaw<double>(stream, map.origin.y) && ok;
  ok = readRaw<double>(stream, map.origin.z) && ok;
  ok = readRaw<double>(stream, map.region_spatial_dimensions.x) && ok;
  ok = readRaw<double>(stream, map.region_spatial_dimensions.y) && ok;
  ok = readRaw<double>(stream, map.region_spatial_dimensions.z) && ok;
  ok = readRaw<int32_t>(stream, map.region_voxel_dimensions.x) && ok;
  ok = readRaw<int32_t>(stream, map.region_voxel_dimensions.y) && ok;
  ok = readRaw<int32_t>(stream, map.region_voxel_dimensions.z) && ok;
  ok = readRaw<double>(stream, map.resolution) && ok;
  ok = readRaw<double>(stream, map.occupancy_threshold_value) && ok;
  ok = readRaw<double>(stream, map.hit_value) && ok;
  ok = readRaw<double>(stream, map.miss_value) && ok;
  region_count = 0;
  ok = readRaw<uint32_t>(stream, region_count) && ok;
  map.loaded_region_count = region_count;

  if (version.version.major > 0 || version.version.major == 0 && version.version.minor >= 5)
  {
    // Read the map first ray time stamp.
    ok = readRaw<double>(stream, map.first_ray_time) && ok;
  }

  if (version.version.major > 0 || version.version.major == 0 && version.version.minor >= 3)
  {
    // Read the map stamp.
    ok = readRaw<uint64_t>(stream, map.stamp) && ok;
  }

  // v0.3.2 added serialisation of map flags
  if (version.version.major > 0 || version.version.minor > 3 ||
      version.version.minor == 3 && version.version.patch >= 2)
  {
    uint32_t flags = 0;
    ok = readRaw<std::underlying_type_t<MapFlag>>(stream, map.flags) && ok;
    map.flags = static_cast<MapFlag>(flags);
  }
  else
  {
    map.flags = MapFlag::kNone;
  }

  if (!ok)
  {
    return kSeFileReadFailure;
  }

  return kSeOk;
}


// Current version of chunk loading.
int loadChunk(InputStream &stream, MapChunk &chunk, const OccupancyMapDetail &detail)
{
  bool ok = true;

  // Write region details, then nodes. MapChunk members are derived.
  ok = read<int32_t>(stream, chunk.region.coord.x) && ok;
  ok = read<int32_t>(stream, chunk.region.coord.y) && ok;
  ok = read<int32_t>(stream, chunk.region.coord.z) && ok;
  ok = read<double>(stream, chunk.region.centre.x) && ok;
  ok = read<double>(stream, chunk.region.centre.y) && ok;
  ok = read<double>(stream, chunk.region.centre.z) && ok;
  ok = read<double>(stream, chunk.touched_time) && ok;

  if (ok)
  {
    const MapLayout &layout = detail.layout;
    for (size_t i = 0; i < layout.layerCount(); ++i)
    {
      const MapLayer &layer = layout.layer(i);
      VoxelBuffer<VoxelBlock> voxel_buffer(chunk.voxel_blocks[i]);
      uint8_t *layer_mem = voxel_buffer.voxelMemory();

      if (layer.flags() & MapLayer::kSkipSerialise)
      {
        // Not to be serialised. Clear instead.
        layer.clear(layer_mem, detail.region_voxel_dimensions);
        continue;
      }

      // Get the layer memory.
      const size_t node_count = layer.volume(detail.region_voxel_dimensions);
      const size_t node_byte_count = layer.voxelByteSize() * node_count;
      if (node_byte_count != unsigned(node_byte_count))
      {
        return kSeValueOverflow;
      }

      ok = stream.read(layer_mem, unsigned(node_byte_count)) == node_byte_count && ok;
    }
  }

  return (ok) ? 0 : kSeFileReadFailure;
}


const char *serialiseErrorCodeString(int err)
{
  std::unique_lock<std::mutex> guard(s_error_code_lock);
  const auto lookup = s_error_codes.find(err);
  if (lookup != s_error_codes.end())
  {
    return lookup->second.c_str();
  }
  return "<unknown>";
}


void registerSerialiseExtensionErrorCodeString(int err, const char *error_string)
{
  std::unique_lock<std::mutex> guard(s_error_code_lock);
  s_error_codes.insert(std::make_pair(err, std::string(error_string)));
}


int save(const std::string &filename, const OccupancyMap &map, SerialiseProgress *progress)
{
  OutputStream stream(filename, kSfCompress);
  const OccupancyMapDetail &detail = *map.detail();

  if (!stream.isOpen())
  {
    return kSeFileCreateFailure;
  }

  if (progress)
  {
    progress->setTargetProgress(unsigned(detail.chunks.size()));
  }

  // Header is written uncompressed.
  int err = saveHeader(stream, detail);

  if (err)
  {
    return err;
  }

  // Save the MapInfo
  err = saveMapInfo(stream, detail.info);

  if (err)
  {
    return err;
  }

  err = saveLayout(stream, detail);

  if (err)
  {
    return err;
  }

  for (auto region_iter = detail.chunks.begin(); region_iter != detail.chunks.end() && (!progress || !progress->quit());
       ++region_iter)
  {
    err = saveChunk(stream, *region_iter->second, detail);
    if (err)
    {
      return err;
    }

    if (progress)
    {
      progress->incrementProgress();
    }
  }

  return kSeOk;
}


int load(const std::string &filename, OccupancyMap &map, SerialiseProgress *progress, MapVersion *version_out)
{
  InputStream stream(filename, kSfCompress);
  OccupancyMapDetail &detail = *map.detail();

  if (!stream.isOpen())
  {
    return kSeFileOpenFailure;
  }

  map.clear();

  // Header is read uncompressed.
  size_t region_count = 0;
  HeaderVersion version;
  int err = loadHeader(stream, version, detail, region_count);
  if (version_out)
  {
    *version_out = version.version;
  }

  if (err)
  {
    return err;
  }

  err = kSeUnsupportedVersion;
  if (version.marker == 0 || version.version.major == 0 && version.version.minor == 0)
  {
    err = v0::load(stream, detail, progress, version.version, region_count);
  }
  else if (kSupportedVersionMin <= version.version && version.version <= kSupportedVersionMax)
  {
    if (version.version.major == 0 && version.version.minor == 1)
    {
      err = v0_1::load(stream, detail, progress, version.version, region_count);
    }
    else if (version.version.major == 0 && version.version.minor == 2)
    {
      err = v0_2::load(stream, detail, progress, version.version, region_count);
    }
    else if (version.version.major == 0 && version.version.minor == 4)
    {
      err = v0_4::load(stream, detail, progress, version.version, region_count);
    }
    else if (version.version.major == 0 && version.version.minor == 5)
    {
      err = v0_5::load(stream, detail, progress, version.version, region_count);
    }
  }

  return err;
}


int loadHeader(const std::string &filename, OccupancyMap &map, MapVersion *version_out, size_t *region_count)
{
  InputStream stream(filename, kSfCompress);
  OccupancyMapDetail &detail = *map.detail();

  if (!stream.isOpen())
  {
    return kSeFileOpenFailure;
  }

  map.clear();

  // Header is read uncompressed.
  size_t region_count_local = 0;
  HeaderVersion version;
  int err = loadHeader(stream, version, detail, region_count_local);
  if (version_out)
  {
    *version_out = version.version;
  }

  // From version 0.2 we have MapInfo.
  detail.info.clear();
  if (version.version.major > 0 || version.version.minor > 1)
  {
    if (!err)
    {
      err = v0_2::loadMapInfo(stream, detail.info);
    }
  }

  if (region_count)
  {
    *region_count = region_count_local;
  }

  if (!err)
  {
    if (version.version.major == 0 && version.version.minor == 0 && version.version.patch == 0)
    {
      // Version 0.0.0 had no layout.
      detail.setDefaultLayout(MapFlag::kNone);
    }
    else
    {
      err = v0_1::loadLayout(stream, detail);
    }
  }

  // Correct the voxel mean flags. The flags may not match the reality of the map layout, such as when we load an older
  // version.
  if (!err)
  {
    if (detail.layout.meanLayer() >= 0)
    {
      detail.flags |= MapFlag::kVoxelMean;
    }
    else
    {
      detail.flags &= ~MapFlag::kVoxelMean;
    }
  }

  return err;
}

}  // namespace ohm


================================================
FILE: ohm/MapSerialise.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_MAPSERIALISE_H
#define OHM_MAPSERIALISE_H

#include "OhmConfig.h"

#include <glm/fwd.hpp>

#include <cinttypes>
#include <string>

#ifdef major
#undef major
#endif  // major
#ifdef minor
#undef minor
#endif  // minor
#ifdef patch
#undef patch
#endif  // patch

namespace ohm
{
class OccupancyMap;

/// An enumeration of potential serialisation errors.
enum SerialisationError
{
  /// No error.
  kSeOk = 0,
  /// Failed to create a file.
  kSeFileCreateFailure,
  /// Failed to open a file for reading.
  kSeFileOpenFailure,

  /// Failed to write to a file.
  kSeFileWriteFailure,
  /// Failed to read a file.
  kSeFileReadFailure,

  kSeValueOverflow,

  kSeMemberOffsetError,

  /// Error serialising @c MapInfo.
  kSeInfoError,

  kSeDataItemTooLarge,
  kSeUnknownDataType,

  kSeUnsupportedVersion,

  /// A previously supported version, but one which does not support upgrading.
  kSeDeprecatedVersion,

  kSeExtensionCode = 0x1000
};

/// Structure holding information about a loaded map version number specified as `<major>.<minor>.<patch>`.
struct ohm_API MapVersion
{
  uint32_t major = 0;  ///< The major version number.
  uint16_t minor = 0;  ///< The minor version number.
  uint16_t patch = 0;  ///< The patch version number.

  /// Default constructor - generates a zero version number.
  inline MapVersion() = default;
  /// Construct with the given version number.
  /// @param major The major version number.
  /// @param minor The minor version number.
  /// @param patch The patch version number.
  inline MapVersion(uint32_t major, uint16_t minor, uint16_t patch = 0) noexcept
    : major(major)
    , minor(minor)
    , patch(patch)
  {}

  /// @overload
  inline explicit MapVersion(uint32_t major) noexcept
    : MapVersion(major, 0, 0)
  {}

  /// Copy by value assignment.
  /// @param other Object to copy.
  inline MapVersion &operator=(const MapVersion &other) = default;

  /// Equalty operator comparing two version for exact match.
  /// @param other Version to compare against.
  /// @return True if the version numbers exactly match.
  inline bool operator==(const MapVersion &other) const
  {
    return major == other.major && minor == other.minor && patch == other.patch;
  }

  /// Negated equality operator comparing two version.
  /// @param other Version to compare against.
  /// @return True if the version numbers do not exactly match.
  inline bool operator!=(const MapVersion &other) const { return !operator==(other); }

  /// Inequality operator comparing two versions.
  /// @param other Version to compare against.
  /// @return True if this version number is less than @p other .
  inline bool operator<(const MapVersion &other) const
  {
    return major < other.major && minor < other.minor && patch < other.patch;
  }

  /// Inequality operator comparing two versions.
  /// @param other Version to compare against.
  /// @return True if this version number is less than or equal to @p other .
  inline bool operator<=(const MapVersion &other) const
  {
    return major <= other.major && minor <= other.minor && patch <= other.patch;
  }

  /// Inequality operator comparing two versions.
  /// @param other Version to compare against.
  /// @return True if this version number is greater than @p other .
  inline bool operator>(const MapVersion &other) const
  {
    return major > other.major && minor > other.minor && patch > other.patch;
  }

  /// Inequality operator comparing two versions.
  /// @param other Version to compare against.
  /// @return True if this version number is greater than or equal to @p other .
  inline bool operator>=(const MapVersion &other) const
  {
    return major >= other.major && minor >= other.minor && patch >= other.patch;
  }
};

/// Header marker bytes for a serialised occupancy map.
extern const uint32_t ohm_API kMapHeaderMarker;
/// Minimum version number which can be loaded by this library.
extern const MapVersion ohm_API kSupportedVersionMin;
/// Maximum version number which can be loaded by this library.
extern const MapVersion ohm_API kSupportedVersionMax;
/// Current MapVersion version.
extern const MapVersion ohm_API kCurrentVersion;

/// Progress observer interface for serialisation.
///
/// This can be derived to track serialisation progress in @c save() and @c load().
/// When given to one of those methods, the following methods will be called during serialisation:
/// - @c setTargetProgress() to set the maximum progress value once known.
/// - @c incrementProgress() periodically to increment progress up to the target.
///
/// The @c quit() method may also be used to abort serialisation. It is called periodically to
/// check if serialisation should abort.
class ohm_API SerialiseProgress
{
public:
  /// Virtual destructor.
  virtual inline ~SerialiseProgress() = default;

  /// Should serialisation quit early?
  /// @return True to quit/abort.
  virtual bool quit() const = 0;
  /// Sets the target progress value. Current progress should be reset when this is called.
  /// @param target The maximum progress value.
  virtual void setTargetProgress(unsigned target) = 0;
  /// Increment progress towards the target value.
  /// @param inc The increment. Generally 1.
  virtual void incrementProgress(unsigned inc) = 0;
  /// @overload
  inline void incrementProgress() { incrementProgress(1); }
};

/// Translate @c err to an English error code string.
/// @param err The error code. Out of range values are handled.
/// @return The English error code string or "<unknown>".
const char ohm_API *serialiseErrorCodeString(int err);

/// Register a new error code string for @c serialiseErrorCodeString() based on an extended error code value.
/// @param err The error code to register.
/// @param error_string The display string for @p err .
void ohm_API registerSerialiseExtensionErrorCodeString(int err, const char *error_string);

/// Save @p map to @p filename.
///
/// This method saves an @c OccupancyMap to file. The progress may optionally be tracked by providing
/// a @c SerialiseProgress object via @p progress. That object may also be used to abort serialisation
/// should it's @c SerialiseProgress::quit() method report @c true.
///
/// @param filename The name of the file to save to.
/// @param map The map to save.
/// @param progress Optional progress tracking object.
/// @return @c SE_OK on success, or a non zero @c SerialisationError on failure.
int ohm_API save(const std::string &filename, const OccupancyMap &map, SerialiseProgress *progress = nullptr);

/// Load @p map from @p filename.
///
/// This method loads an @c OccupancyMap from file. The progress may optionally be tracked by providing
/// a @c SerialiseProgress object via @p progress. That object may also be used to abort serialisation
/// should it's @c SerialiseProgress::quit() method report @c true.
///
/// The current content of @p map is overwritten by the loaded data.
///
/// @param filename The name of the file to load from.
/// @param map The map object to load into.
/// @param progress Optional progress tracking object.
/// @param[out] version_out When present, set to the version number of the loaded map format.
/// @return @c SE_OK on success, or a non zero @c SerialisationError on failure.
int ohm_API load(const std::string &filename, OccupancyMap &map, SerialiseProgress *progress = nullptr,
                 MapVersion *version_out = nullptr);

/// Loads the header and layers of a map file without loading the chunks for voxel data.
///
/// The resulting @p map contains no chunks or voxel data, but does contain valid @c MapLayout data.
/// This can be used to inspect information about the map, but without the data content.
///
/// The current content of @p map is emptied and overwritten by the loaded data.
///
/// @param filename The name of the file to load from.
/// @param map The map object to load into.
/// @param[out] version_out When present, set to the version number of the loaded map format.
/// @param[out] region_count When present, set to the number of regions in the map. Regions are not loaded.
/// @return @c SE_OK on success, or a non zero @c SerialisationError on failure.
int ohm_API loadHeader(const std::string &filename, OccupancyMap &map, MapVersion *version_out = nullptr,
                       size_t *region_count = nullptr);
}  // namespace ohm

#endif  // OHM_MAPSERIALISE_H


================================================
FILE: ohm/Mapper.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "Mapper.h"

#include "MappingProcess.h"

#include "private/MapperDetail.h"

#include <algorithm>
#include <chrono>

namespace ohm
{
Mapper::Mapper(OccupancyMap *map)
  : imp_(std::make_unique<MapperDetail>())
{
  setMap(map);
}


Mapper::~Mapper()
{
  if (imp_)
  {
    for (MappingProcess *process : imp_->processes)
    {
      delete process;
    }
  }
}

void Mapper::setMap(OccupancyMap *map)
{
  imp_->map = map;
}


OccupancyMap *Mapper::map() const
{
  return imp_->map;
}


int Mapper::update(double time_slice_sec)
{
  using Clock = std::chrono::high_resolution_clock;
  const Clock::time_point start_time = Clock::now();
  Clock::time_point cur_time;

  OccupancyMap *map = this->map();

  int status = kMprUpToDate;
  if (map && !imp_->processes.empty())
  {
    int process_result;
    double elapsed_sec = 0;

    // Ensure first item is in range.
    imp_->next_process = imp_->next_process % unsigned(imp_->processes.size());
    const unsigned initial_index = imp_->next_process;
    bool first_iteration = true;

    // Update until we consume the timeSliceSec or everything is up to date.
    while ((time_slice_sec == 0 || elapsed_sec < time_slice_sec) &&
           (first_iteration || imp_->next_process != initial_index))
    {
      // Enforce range of imp_->next_process. This plus the increment ensures we run processes in a round robin,
      // but don't go out of range.
      MappingProcess *process = imp_->processes[imp_->next_process++];
      imp_->next_process = imp_->next_process % unsigned(imp_->processes.size());
      if (!process->paused())
      {
        process_result = process->update(*map, time_slice_sec);
        status = std::max(process_result, status);
      }
      cur_time = Clock::now();
      elapsed_sec = std::chrono::duration_cast<std::chrono::duration<double>>(cur_time - start_time).count();
      first_iteration = false;
    }
  }

  return status;
}


void Mapper::addProcess(MappingProcess *process)
{
  // ASSERT(!hasProcess(process));
  // ASSERT(process);
  imp_->processes.push_back(process);
}


bool Mapper::removeProcess(const MappingProcess *process)
{
  auto search = std::find(imp_->processes.begin(), imp_->processes.end(), process);
  if (search != imp_->processes.end())
  {
    imp_->processes.erase(search);
    return true;
  }

  return false;
}


bool Mapper::hasProcess(const MappingProcess *process) const
{
  return indexOfProcess(process) != npos;
}


unsigned Mapper::indexOfProcess(const MappingProcess *process) const
{
  if (imp_)
  {
    const auto iter = std::find(imp_->processes.begin(), imp_->processes.end(), process);
    if (iter != imp_->processes.end())
    {
      return static_cast<unsigned>(iter - imp_->processes.begin());
    }
  }

  return npos;
}


unsigned Mapper::processCount() const
{
  return unsigned(imp_->processes.size());
}


MappingProcess *Mapper::process(unsigned index)
{
  return imp_->processes[index];
}


const MappingProcess *Mapper::process(unsigned index) const
{
  return imp_->processes[index];
}
}  // namespace ohm


================================================
FILE: ohm/Mapper.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_MAPPER_H
#define OHM_MAPPER_H

#include "OhmConfig.h"

#include <memory>

namespace ohm
{
class OccupancyMap;
struct MapperDetail;
class MappingProcess;

/// The @c Mapper class manages a set of @c MappingProcess objects to execute as the map changes.
///
/// The @c Mapper update is invoked interspersed with occupancy map generation in order to make
/// additional calculations supporting the occupancy map. For example, a @c Mapper instance may
/// contain a @c MappingProcess which updates map layers derived from the current occupancy layer.
/// More concretely, the @c ClearanceProcess uses the occupancy layer to generate a clearance layer
/// which specifies the range to the nearest obstacle for each voxel.
///
/// @c Mapper execution is designed to be limited to a specified time slice and each @c MappingProcess
/// should only process dirty map regions (see @c MappingProcess for more details). Calling of
/// @c MappingProcess updates is set to run each process round robin so no single process is starved,
/// until the allowed time slice is exceeded.
///
/// Intended usage is as follows:
/// - Instantiate an @c OccupancyMap for population.
/// - Instantiate an associated @c Mapper.
/// - Register a set of @c MappingProcess objects to run round robin with occupancy map generation.
/// - While new occupancy rays come in:
///   - Integrate new occupancy rays into the map.
///   - Update the @p Mapper, optionally with a time limit.
///
/// The @c Mapper is designed to be strongly associated with one @c OccupancyMap which does not change.
class ohm_API Mapper
{
public:
  /// Invalid return value for @c indexOfProcess()
  const unsigned npos = ~0u;

  /// Instantiate a mapper optionally providing the target @p map on construction.
  /// If @p map is null here then @c setMap() must be called before @c update().
  explicit Mapper(OccupancyMap *map = nullptr);

  /// Destructor deleting all the registered @c MappingProcesses.
  ~Mapper();

  /// Sets the map to operate on. Must be set either here or on construction before @c update().
  /// @param map The map to operate on.s
  void setMap(OccupancyMap *map);

  /// Gets the target map for the @p Mapper.
  /// @return The target @c OccupancyMap.
  OccupancyMap *map() const;

  /// Updates the @c Mapper giving each @c MappingProcess an opportunity to update.
  ///
  /// Each @c MappingProcess is updated in turn until the @c timeSliceSec is exceeded. This process tracks the last
  /// @c MappingProcess updated, so the next call to this @c update() will target the next @c MappingProcess in
  /// the list. This ensures each process has the same opportunity to run.
  ///
  /// It is not recommended that the set of @c MappingProcess objects change between calls, though the method is
  /// robust to such changes.
  ///
  /// A full update may be forced by setting @p timeSliceSec to zero.
  ///
  /// @param time_slice_sec The allowed time slice for this update (seconds). Zero for full update.
  /// @return The overall status. See @c MappingProcessResult.
  int update(double time_slice_sec);

  /// Adds @p process to the update list. The @c Mapper takes ownership of the pointer. The specific @p process
  /// must not already be registered in this or any other @c Mapper.
  ///
  /// @param process The new process to add.
  void addProcess(MappingProcess *process);

  /// Remove @p process from the update list. The @c Mapper relinquishes ownership of the pointer.
  ///
  /// @param process The process to remove.
  /// @return True if @p process was registered and has been removed.
  bool removeProcess(const MappingProcess *process);

  /// Checks whether @p process is in the update list or not.
  /// @param process The process to search for.
  /// @return True if @p process is being managed by this @c Mapper.
  bool hasProcess(const MappingProcess *process) const;

  /// Returns the index of @p process in the process list.
  /// @return The index of @p process or @c npos on failure.
  unsigned indexOfProcess(const MappingProcess *process) const;

  /// Queries the number of registered @p MappingProcess objects.
  /// @return The number of registered processes.
  unsigned processCount() const;

  /// Gets the @c MappingProcess at @p index.
  /// @param index The index of the process of interest. Must be in the range [0, @c processCount()).
  /// @return The registered process at @p index.
  MappingProcess *process(unsigned index);
  /// @overload
  const MappingProcess *process(unsigned index) const;

  /// Internal data access.
  /// @return Internal data.
  inline MapperDetail *detail() { return imp_.get(); }
  /// @overload
  inline const MapperDetail *detail() const { return imp_.get(); }

private:
  std::unique_ptr<MapperDetail> imp_;
};
}  // namespace ohm

#endif  // OHM_MAPPER_H


================================================
FILE: ohm/MappingProcess.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MappingProcess.h"

namespace ohm
{
MappingProcess::MappingProcess() = default;

MappingProcess::~MappingProcess() = default;
}  // namespace ohm


================================================
FILE: ohm/MappingProcess.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_MAPPROCESS_H
#define OHM_MAPPROCESS_H

#include "OhmConfig.h"

namespace ohm
{
class OccupancyMap;

/// Return value for @c Mapper::update() and @c MappingProcess::update(). Values must be ordered
/// such that more significant results come later. This supports a simple amalgamation of results
/// in @c Mapper::update() by simply using the maximum value of the aggregate processes.
enum MappingProcessResult
{
  /// Everything up to date or nothing to do.
  kMprUpToDate,
  /// Some progress made. More to be done.
  kMprProgressing
};

/// Base class for processes to be added to the @p Mapper for processing during map update.
///
/// @note This class and all derivations are experimental.
///
/// @todo Features to consider:
/// - Update period and scheduling
/// - Update only on dirty?
class ohm_API MappingProcess
{
public:
  /// Constructor.
  MappingProcess();
  /// Virtual destructor.
  virtual ~MappingProcess();

  /// Request a pause or unpause the process.
  /// @param pause True to pause, false to unpause.
  inline void pause(bool pause = true) { paused_ = pause; }
  /// Query if the process is currently paused.
  /// @return True if paused.
  inline bool paused() const { return paused_; }

  /// Request a reset of the process. Must drop and reinitialise all data.
  virtual void reset() = 0;

  /// Called to update the process on the given map and limited processing time.
  /// @param map The map to target.
  /// @param time_slice Time processing limit for the update (seconds).
  virtual int update(OccupancyMap &map, double time_slice) = 0;

private:
  bool paused_ = false;
};
}  // namespace ohm

#endif  // OHM_MAPPROCESS_H


================================================
FILE: ohm/Mutex.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "Mutex.h"

// Nothing.


================================================
FILE: ohm/Mutex.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMMUTEX_H
#define OHMMUTEX_H

#include "OhmConfig.h"

#include <mutex>

#ifdef OHM_FEATURE_THREADS
#include <tbb/spin_mutex.h>
namespace ohm
{
// tbb::mutex has been deprecated in favour of std::mutex
using Mutex = std::mutex;
using SpinMutex = tbb::spin_mutex;
}  // namespace ohm
#else   // OHM_FEATURE_THREADS
namespace ohm
{
using Mutex = std::mutex;
using SpinMutex = std::mutex;
}  // namespace ohm
#endif  // OHM_FEATURE_THREADS

#endif  // OHMMUTEX_H


================================================
FILE: ohm/NdtMap.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas, Jason Williams
#include "NdtMap.h"

#include "private/NdtMapDetail.h"

#include "DefaultLayer.h"
#include "MapInfo.h"
#include "MapLayer.h"
#include "MapLayout.h"
#include "MapProbability.h"
#include "OccupancyMap.h"
#include "VoxelData.h"

#include <glm/gtc/type_ptr.hpp>
#include <glm/vec4.hpp>

#include <iostream>

#include <3esservermacros.h>
#ifdef TES_ENABLE
#include <3esserver.h>
#include <shapes/3esshapes.h>
#endif  // TES_ENABLE

#include <cassert>

namespace ohm
{
NdtMap::NdtMap(OccupancyMap *map, bool borrowed_map, NdtMode mode)
  : imp_(new NdtMapDetail)
{
  imp_->map = map;
  imp_->borrowed_map = borrowed_map;
  imp_->mode = mode;
  if (imp_->adaptation_rate <= 0)
  {
    imp_->adaptation_rate = ndtAdaptationRateFromMissProbability(map->missProbability());
  }
  enableNdt(map, mode);
  updateMapInfo();
}


NdtMap::~NdtMap()
{
  if (imp_)
  {
    if (!imp_->borrowed_map)
    {
      delete imp_->map;
    }
    delete imp_;
  }
}


OccupancyMap &NdtMap::map()
{
  return *imp_->map;
}


const OccupancyMap &NdtMap::map() const
{
  return *imp_->map;
}


NdtMode NdtMap::mode() const
{
  return imp_->mode;
}


bool NdtMap::borrowedMap() const
{
  return imp_->borrowed_map;
}


void NdtMap::setAdaptationRate(float rate)
{
  imp_->adaptation_rate = rate;
  updateMapInfo();
}


float NdtMap::adaptationRate() const
{
  return imp_->adaptation_rate;
}


void NdtMap::setSensorNoise(float noise_range)
{
  imp_->sensor_noise = noise_range;
  updateMapInfo();
}


float NdtMap::sensorNoise() const
{
  return imp_->sensor_noise;
}


void NdtMap::setNdtSampleThreshold(unsigned sample_count)
{
  imp_->sample_threshold = sample_count;
  updateMapInfo();
}


unsigned NdtMap::ndtSampleThreshold()
{
  return imp_->sample_threshold;
}


void NdtMap::setReinitialiseCovarianceThreshold(float threshold)
{
  imp_->reinitialise_covariance_threshold = threshold;
  updateMapInfo();
}


float NdtMap::reinitialiseCovarianceThreshold() const
{
  return imp_->reinitialise_covariance_threshold;
}


void NdtMap::setReinitialiseCovariancePointCount(unsigned count)
{
  imp_->reinitialise_covariance_point_count = count;
  updateMapInfo();
}


unsigned NdtMap::reinitialiseCovariancePointCount() const
{
  return imp_->reinitialise_covariance_point_count;
}


void NdtMap::setInitialIntensityCovariance(float initial_intensity_covariance)
{
  imp_->initial_intensity_covariance = initial_intensity_covariance;
  updateMapInfo();
}


float NdtMap::initialIntensityCovariance() const
{
  return imp_->initial_intensity_covariance;
}


void NdtMap::setTrace(bool trace)
{
  imp_->trace = trace;
}


bool NdtMap::trace() const
{
  return imp_->trace;
}


void NdtMap::updateMapInfo()
{
  if (!imp_->map)
  {
    return;
  }
  MapInfo &info = imp_->map->mapInfo();
  info.set(MapValue("Ndt mode", int(imp_->mode)));
  info.set(MapValue("Ndt mode name", ndtModeToString(imp_->mode).c_str()));
  info.set(MapValue("Ndt adaptation rate", imp_->adaptation_rate));
  info.set(MapValue("Ndt sensor noise", imp_->sensor_noise));
  info.set(MapValue("Ndt sample threshold", imp_->sample_threshold));
  info.set(MapValue("Ndt reinitialisation threshold", imp_->reinitialise_covariance_threshold));
  info.set(MapValue("Ndt reinitialisation threshold (probability)",
                    valueToProbability(imp_->reinitialise_covariance_threshold)));
  info.set(MapValue("Ndt reinitialisation point count", imp_->reinitialise_covariance_point_count));
}


int NdtMap::enableNdt(OccupancyMap *map, NdtMode ndt_mode)
{
  // Prepare layout for update.
  MapLayout new_layout = map->layout();

  addVoxelMean(new_layout);
  // Cache the layer index.
  int layer_index = int(addCovariance(new_layout)->layerIndex());

  if (ndt_mode == NdtMode::kTraversability)
  {
    addIntensity(new_layout);
    addHitMissCount(new_layout);
  }

  // Update the map.
  map->updateLayout(new_layout);

  return layer_index;
}
}  // namespace ohm


================================================
FILE: ohm/NdtMap.h
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2020
//
#ifndef NDTMAP_H
#define NDTMAP_H

#include "OhmConfig.h"

#include "NdtMode.h"

#include <glm/fwd.hpp>

namespace ohm
{
class OccupancyMap;
struct NdtMapDetail;

/// A variation on @c OccupancyMap which uses a 3D Normal Distribution Transform (NDT) to calculate occupancy
/// adjustments.
///
/// This technique adds a voxel layer which includes a packed covariance matrix (see @c CovarianceVoxel ) and ensures
/// voxel mean positioning is enabled.
///
/// This covariance matrix describes an ellipsoid within the voxel which approximates the collection of samples used
/// to populate that voxel. This is used to refine the probably adjustment of rays passing through the voxel.
/// Conceptually, we adjust the probably according to the likelihood that the ray passing through the voxel intersects
/// this ellipsoid. This improves standard occupancy map issues of erosion. This calculation can only be made where
/// sufficient samples have been collected, as determined by @c ndtSampleThreshold() , otherwise a fixed probably
/// adjustment is made as set by the @c OccupancyMap::missProbability() . This probably is also used as the learning
/// rate for the NDT model.
///
/// See paper:
/// > 3D normal distributions transform occupancy maps: An efficient representation for mapping in dynamic
/// > environments
/// > Jari P. Saarinen, Henrik Andreasson, Todor Stoyanov and Achim J. Lilienthal
class ohm_API NdtMap
{
public:
  /// Create an NDT map for the given @c OccupancyMap . The voxel layout is adjusted to include a layer for
  /// @c CovarianceVoxel data.
  ///
  /// The @p map ownership is passed to this object if @c borrowed_map is false, in which case the @c NdtMap takes
  /// ownership of the @c OccupancyMap .
  ///
  /// @param map The occupancy map to apply NDT updates to.
  /// @param borrowed_map True if @p map is a borrowed pointer. False if the NDT map takes ownership of the memory.
  /// @param mode The NDT mapping mode. Using @c NdtMode::kNone is invalid in the context and behaviour is undefined.
  NdtMap(OccupancyMap *map, bool borrowed_map, NdtMode mode = NdtMode::kOccupancy);

  /// Destructor: destroys the @c map() if @c borrowedMap() is false.
  ~NdtMap();

  /// Access the underlying occupancy map.
  OccupancyMap &map();
  /// Access the underlying occupancy map.
  const OccupancyMap &map() const;

  /// Specified the NDT mode the map is to use.
  NdtMode mode() const;

  /// True if @p map() is a borrowed pointer, false if the NDT map takes ownership.
  bool borrowedMap() const;

  /// Sets the adaptation rate for intersected NDT voxels. The value must be in the range [0, 1] with larger values
  /// yielding stronger effects.
  /// @param rate The adaptation rate to set [0, 1]
  void setAdaptationRate(float rate);
  /// Query the adaptation rate, which affects how quickly NDT logic removes intersected voxels.
  /// @return The current adaptation rate.
  float adaptationRate() const;

  /// Set the range sensor noise estimate. For example, the range noise for a lidar sensor.
  ///
  /// @param noise_range The sensor noise range. Must be greater than zero.
  void setSensorNoise(float noise_range);
  /// Read the range sensor noise estimate.
  float sensorNoise() const;

  /// Set the number of samples required in a voxel before using the NDT algorithm for @c integateMiss() adjustments.
  void setNdtSampleThreshold(unsigned sample_count);
  /// Get the number of samples required in a voxel before using the NDT algorithm for @c integateMiss() adjustments.
  unsigned ndtSampleThreshold();

  /// Set the occupancy threshold value at which the covariance matrix may be reinitialised.
  ///
  /// This maps to the @c reinitialise_threshold parameter of @c calculateHitWithCovariance() . See that function for
  /// details.
  /// @param threshold The probability threshold value. Must be < 0 or behaviour is undefined.
  void setReinitialiseCovarianceThreshold(float threshold);

  /// Get the occupancy threshold value at which the covariance matrix may be reinitialised.
  /// @return The reset probability threshold value.
  float reinitialiseCovarianceThreshold() const;

  /// Set the occupancy threshold value at which the covariance matrix may be reinitialised.
  ///
  /// This maps to the @c reinitialise_sample_count parameter of @c calculateHitWithCovariance() . See that function
  /// for details.
  /// @param count The requires point count.
  void setReinitialiseCovariancePointCount(unsigned count);

  /// Get the occupancy threshold value at which the covariance matrix may be reinitialised.
  /// @return The reset point count threshold.
  unsigned reinitialiseCovariancePointCount() const;

  /// Set the initial covariance of intensity.
  ///
  /// @param initial_intensity_covariance The covariance of intensity for a voxel, for initialisation on receipt of
  /// first point. Must be greater than zero. Note that it is covariance, not standard deviation.
  void setInitialIntensityCovariance(float initial_intensity_covariance);
  /// Read the intensity covariance upon initialisation.
  float initialIntensityCovariance() const;

  /// Enable detailed tracing via 3rd Eye Scene.
  /// @param trace True to enable tracing.
  void setTrace(bool trace);
  /// Is tracing enabled?
  bool trace() const;

  /// Calculate an NDT adaptation rate which matches the strength of a pure occupancy miss probability.
  ///
  /// The NDT adaptation rate is a scaling factor for adjusting the probability on a miss using the NDT algorithm.
  /// While it may be set independently to the standard occupancy miss adjustment, this can yield much stronger NDT
  /// clearing effects than standard occupancy. This function seeks to keep the adaptation rate in line with the
  /// strength of the standard miss probability. That is, to achieve the same clearing effect as pure occupancy when
  /// the NDT algorithm yields a intersection probability of 1.
  ///
  /// The @p scale may be used to increase or decrease the NDT clearing effect in proportion with the pure occupancy
  /// clearing effect. For example, setting the scale to 1 will make the NDT clearing effect as strong as pure
  /// occupancy on a direct intersection with an occupied voxel's ellipsoid as described by the covariance matrix. The
  /// default scaling is 2 as NDT clearing effect is already dynamic, with increased effect the more directlly a ray
  /// passes through the ellipsoid.
  ///
  /// The adaptation rate is calculated as follows:
  ///
  /// @code{.unparsed}
  ///   adaptation_rate = scale * (1 - 2 * miss_probability)
  /// @endcode
  ///
  /// The result is then clamped to the range [0, 1].
  ///
  /// @param miss_probability The probability adjustment made on a pure occupancy miss, ranging (0, 0.5)
  /// @param scale Optional scaling factor for the result.
  /// @return The calculated NDT adaptation rate/scale factor [0, 1].
  static inline constexpr float ndtAdaptationRateFromMissProbability(float miss_probability, float scale = 2.0f)
  {
    return std::max(0.0f, std::min(scale * (1.0f - 2.0f * miss_probability), 1.0f));
  }

private:
  /// Update members of the underlying @c OccupancyMap::mapInfo() to include NDT parameters.
  void updateMapInfo();

  /// Enable NDT for the given @p map. This enables voxel mean positioning and adds a voxel layer to store the
  /// voxel covariance matrix approximation.
  /// @param map The occupancy map to enable NDT for.
  /// @param ndt_mode Specifies the NDT mapping mode. Must not be @c NdtMode::kNone.
  /// @return The voxel layer index for the covariance matrix.
  static int enableNdt(OccupancyMap *map, NdtMode ndt_mode);

  NdtMapDetail *imp_;
};
}  // namespace ohm

#endif  // NDTMAP_H


================================================
FILE: ohm/NdtMode.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "NdtMode.h"

#include <array>

namespace ohm
{
const std::array<std::string, 3> &ndtModeNames()
{
  static const std::array<std::string, 3> mode_names = { "none", "occupancy", "traversability" };
  return mode_names;
}

std::string ndtModeToString(NdtMode mode)
{
  if (unsigned(mode) < ndtModeNames().size())
  {
    return ndtModeNames()[int(mode)];
  }

  return "<unknown>";
}


NdtMode ndtModeFromString(const std::string &str)
{
  for (size_t i = 0; i < ndtModeNames().size(); ++i)
  {
    if (str == ndtModeNames()[i])
    {
      return NdtMode(i);
    }
  }

  return NdtMode::kNone;
}
}  // namespace ohm


================================================
FILE: ohm/NdtMode.h
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2021
//
#ifndef OHM_NDTMODE_H
#define OHM_NDTMODE_H

#include "OhmConfig.h"

#include <string>
namespace ohm
{
/// NDT mapping mode
enum class NdtMode
{
  kNone,  ///< Ndt not in use.
  /// Ndt occupancy map mode. This uses the occupancy, mean and covariance layers.
  kOccupancy,
  /// Ndt traversability map. A super set of @c Mode::kOccupancy which adds intensity and covariance hit/miss count.
  kTraversability  ///< Ndt traversability mode.
};

/// Convert an @c NdtMode value to a display string. @c "<unknown>" on failure.
std::string ohm_API ndtModeToString(NdtMode mode);
/// Convert a string value to an @c NdtMode or @c NdtMode::kNone on failure. Inverse of @c ndtModeToString()
NdtMode ohm_API ndtModeFromString(const std::string &str);
}  // namespace ohm

#endif  // OHM_NDTMODE_H


================================================
FILE: ohm/NearestNeighbours.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "NearestNeighbours.h"

#include "private/MapLayoutDetail.h"
#include "private/NearestNeighboursDetail.h"
#include "private/OccupancyMapDetail.h"
#include "private/OccupancyQueryAlg.h"

#include "DefaultLayer.h"
#include "Key.h"
#include "MapChunk.h"
#include "OccupancyMap.h"
#include "QueryFlag.h"
#include "VoxelBlock.h"
#include "VoxelBuffer.h"
#include "VoxelOccupancy.h"

#include <3esservermacros.h>

#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>

#include <algorithm>
#include <functional>
#include <iostream>

namespace ohm
{
namespace
{
unsigned regionNearestNeighboursCpu(OccupancyMap &map, NearestNeighboursDetail &query, const glm::i16vec3 &region_key,
                                    ClosestResult &closest)
{
  const float invalid_occupancy_value = unobservedOccupancyValue();
  const OccupancyMapDetail &map_data = *map.detail();
  const auto chunk_search = map_data.chunks.find(region_key);
  glm::vec3 query_origin;
  glm::vec3 voxel_vector;
  Key voxel_key(nullptr);
  const MapChunk *chunk = nullptr;
  const uint8_t *occupancy_mem = nullptr;
  float range_squared = 0;
  unsigned added = 0;
  VoxelBuffer<VoxelBlock> voxel_buffer;

  std::function<bool(float, const OccupancyMapDetail &)> voxel_occupied_func;

  query_origin = glm::vec3(query.near_point - map.origin());

  if (chunk_search == map_data.chunks.end())
  {
    // The entire region is unknown space...
    if ((query.query_flags & ohm::kQfUnknownAsOccupied) == 0)
    {
      // ... and unknown space is considered free. No results to add.
      return 0;
    }

    // ... and we have to treat unknown space as occupied.
    chunk = nullptr;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    occupancy_mem = reinterpret_cast<const uint8_t *>(&invalid_occupancy_value);
    // Setup voxel occupancy test function to pass all voxels in this region.
    voxel_occupied_func = [](const float /*voxel*/, const OccupancyMapDetail & /*map_data*/) -> bool { return true; };
  }
  else
  {
    chunk = chunk_search->second;
    // FIXME: (KS) This is a bit of a mix of legacy direct voxel access and newer VoxelBlock access. Makes things a
    // bit unclear.
    voxel_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[chunk->layout().occupancyLayer()]);
    occupancy_mem = voxel_buffer.voxelMemory();
    // Setup the voxel test function to check the occupancy threshold and behaviour flags.
    voxel_occupied_func = [&query](const float voxel, const OccupancyMapDetail &map_data) -> bool {
      if (voxel == unobservedOccupancyValue())
      {
        return query.query_flags & ohm::kQfUnknownAsOccupied;
      }
      return (voxel >= map_data.occupancy_threshold_value ||
              (query.query_flags & ohm::kQfUnknownAsOccupied) && voxel == unobservedOccupancyValue());
    };
  }

  TES_STMT(std::vector<tes::Vector3d> includedOccupied);
  TES_STMT(std::vector<tes::Vector3d> excludedOccupied);
  TES_STMT(std::vector<tes::Vector3d> includedUncertain);
  TES_STMT(std::vector<tes::Vector3d> excludedUncertain);
  // TES_BOX_W(g_tes, TES_COLOUR(LightSeaGreen), 0u,
  //           glm::value_ptr(region_centre), glm::value_ptr(map.regionSpatialResolution()));

  float occupancy;
  for (int z = 0; z < map_data.region_voxel_dimensions.z; ++z)
  {
    for (int y = 0; y < map_data.region_voxel_dimensions.y; ++y)
    {
      for (int x = 0; x < map_data.region_voxel_dimensions.x; ++x)
      {
        memcpy(&occupancy, occupancy_mem, sizeof(float));
        if (voxel_occupied_func(occupancy, map_data))
        {
          // Occupied voxel, or invalid voxel to be treated as occupied.
          // Calculate range to centre.
          voxel_key = Key(region_key, x, y, z);
          voxel_vector = map.voxelCentreLocal(voxel_key);
          voxel_vector -= query_origin;
          range_squared = glm::dot(voxel_vector, voxel_vector);
          if (range_squared <= query.search_radius * query.search_radius)
          {
            query.intersected_voxels.push_back(voxel_key);
            query.ranges.push_back(std::sqrt(range_squared));

            if (range_squared < closest.range)
            {
              closest.index = query.intersected_voxels.size() - 1;
              closest.range = range_squared;
            }

            ++added;
#ifdef TES_ENABLE
            if (occupancy != unobservedOccupancyValue())
            {
              includedOccupied.emplace_back(tes::Vector3d(glm::value_ptr(map.voxelCentreGlobal(voxel_key))));
            }
            else
            {
              includedUncertain.emplace_back(tes::Vector3d(glm::value_ptr(map.voxelCentreGlobal(voxel_key))));
            }
#endif  // TES_ENABLE
          }
#ifdef TES_ENABLE
          else
          {
            if (occupancy != unobservedOccupancyValue())
            {
              excludedOccupied.emplace_back(tes::Vector3d(glm::value_ptr(map.voxelCentreGlobal(voxel_key))));
            }
            else
            {
              excludedUncertain.emplace_back(tes::Vector3d(glm::value_ptr(map.voxelCentreGlobal(voxel_key))));
            }
          }
#endif  // TES_ENABLE
        }

        // Next voxel. Leave pointer as is (pointing to invalid_occupancy_value) if the chunk is invalid.
        occupancy_mem += (chunk != nullptr) ? sizeof(occupancy) : 0;
      }
    }
  }

#ifdef TES_ENABLE
  // Visualise points.
  if (!excludedUncertain.empty())
  {
    TES_POINTS(g_tes, TES_COLOUR(Salmon), tes::Id(), tes::DataBuffer(excludedUncertain));
  }

  if (!includedUncertain.empty())
  {
    TES_POINTS(g_tes, TES_COLOUR(PowderBlue), tes::Id(), tes::DataBuffer(includedUncertain));
  }

  if (!excludedOccupied.empty())
  {
    TES_POINTS(g_tes, TES_COLOUR(Orange), tes::Id(), tes::DataBuffer(excludedOccupied));
  }

  if (!includedOccupied.empty())
  {
    TES_POINTS(g_tes, TES_COLOUR(LightSkyBlue), tes::Id(), tes::DataBuffer(includedOccupied));
  }
#endif  //  TES_ENABLE

  TES_SERVER_UPDATE(g_tes, 0.0f);

  return added;
}
}  // namespace


NearestNeighbours::NearestNeighbours(NearestNeighboursDetail *detail)
  : Query(detail)
{
  // Can't initalise GPU until onSetMap() (need a valid map).
}


NearestNeighbours::NearestNeighbours(OccupancyMap &map, const glm::dvec3 &near_point, float search_radius,
                                     unsigned query_flags)
  : Query(new NearestNeighboursDetail)
  , query_flags_(query_flags)
{
  setMap(&map);
  setNearPoint(near_point);
  setSearchRadius(search_radius);
  setQueryFlags(query_flags);
}


NearestNeighbours::~NearestNeighbours()
{
  NearestNeighboursDetail *d = imp();
  delete d;
  imp_ = nullptr;
}


glm::dvec3 NearestNeighbours::nearPoint() const
{
  const NearestNeighboursDetail *d = imp();
  return d->near_point;
}


void NearestNeighbours::setNearPoint(const glm::dvec3 &point)
{
  NearestNeighboursDetail *d = imp();
  d->near_point = point;
}


float NearestNeighbours::searchRadius() const
{
  const NearestNeighboursDetail *d = imp();
  return d->search_radius;
}


void NearestNeighbours::setSearchRadius(float range)
{
  NearestNeighboursDetail *d = imp();
  d->search_radius = range;
}


bool NearestNeighbours::onExecute()
{
  NearestNeighboursDetail *d = imp();

  if (!d->map)
  {
    return false;
  }

  ClosestResult closest;

  const glm::dvec3 min_extents = d->near_point - glm::dvec3(d->search_radius, d->search_radius, d->search_radius);
  const glm::dvec3 max_extents = d->near_point + glm::dvec3(d->search_radius, d->search_radius, d->search_radius);

  // Create debug visualisation objects. We use the map address to persist objects.
  // Search sphere.
  TES_SPHERE_W(g_tes, TES_COLOUR_A(GreenYellow, 128), tes::Id(d->map),
               tes::Spherical(tes::Vector3d(glm::value_ptr(d->near_point)), double(d->search_radius)));
  // Search bounds.
  TES_BOX_W(g_tes, TES_COLOUR(FireBrick), tes::Id(d->map),
            tes::Transform(tes::Vector3d(glm::value_ptr(0.5 * min_extents + max_extents)),
                           tes::Vector3d(glm::value_ptr(max_extents - min_extents))));

  ohm::occupancyQueryRegions(*d->map, *d, closest, min_extents, max_extents, &regionNearestNeighboursCpu);

  TES_SERVER_UPDATE(g_tes, 0.0f);
  // Delete debug objects.
  TES_SPHERE_END(g_tes, uint32_t((size_t)d->map));
  TES_BOX_END(g_tes, uint32_t((size_t)d->map));

  if ((d->query_flags & kQfNearestResult) && !d->intersected_voxels.empty())
  {
    d->intersected_voxels[0] = d->intersected_voxels[closest.index];
    d->intersected_voxels.resize(1);
    d->ranges[0] = d->ranges[closest.index];
    d->number_of_results = 1u;
    d->ranges.resize(1);
  }
  else
  {
    d->number_of_results = d->intersected_voxels.size();
  }

  return true;
}


bool NearestNeighbours::onExecuteAsync()
{
  return false;
}


void NearestNeighbours::onReset(bool /*hard_reset*/)
{
  // NearestNeighboursDetail *d = imp();
}


NearestNeighboursDetail *NearestNeighbours::imp()
{
  return static_cast<NearestNeighboursDetail *>(imp_);
}


const NearestNeighboursDetail *NearestNeighbours::imp() const
{
  return static_cast<const NearestNeighboursDetail *>(imp_);
}
}  // namespace ohm


================================================
FILE: ohm/NearestNeighbours.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMNEARESTNEIGHBOURS_H
#define OHMNEARESTNEIGHBOURS_H

#include "OhmConfig.h"

#include "Query.h"

#include <glm/fwd.hpp>

namespace ohm
{
struct NearestNeighboursDetail;

/// A nearest neighbours query for an @c OccupancyMap.
///
/// This finds obstructed voxels within a fixed radius of a known point; i.e., obstructed voxels intersecting the
/// query sphere. The reported @c intersectedVoxels() contains all obstructed voxels within the search sphere while
/// the corresponding @c ranges() identify the distance from the query centre to the centre of each relevant voxel.
/// The order of results is undefined and may change between calls.
///
/// Setting the flag @c QF_NearestResult modifies the results such than only one voxel is reported and this is the
/// voxel closest to the search centre. Again, which voxel this is may be change between calls when multiple voxels
/// report exactly the same range.
///
/// The query only tests for intersections between the query sphere and the centre of nearby voxels. This means that
/// specifying a small search radius (~ the voxel resolution) may consistently yield zero results.
///
/// A GPU implementation is supported for this query, however it is inferior to the CPU implementation in two ways:
/// - The CPU implementation is usually faster.
/// - The GPU implementation is too memory intensive and may result in a crash/SEGFAULT.
class ohm_API NearestNeighbours : public Query
{
protected:
  /// Constructor used for inherited objects. This supports deriving @p NearestNeighboursDetail into
  /// more specialised forms.
  /// @param detail pimple style data structure. When null, a @c NearestNeighboursDetail is allocated by
  /// this method.
  explicit NearestNeighbours(NearestNeighboursDetail *detail = nullptr);

public:
  /// Construct a new query using the given parameters.
  /// @param map The map to perform the query on.
  /// @param near_point The global coordinate to search around.
  /// @param search_radius Defines the search radius around @p nearPoint.
  /// @param query_flags Flags controlling the query behaviour. See @c QueryFlag and @c LineQuery::Flag.
  NearestNeighbours(OccupancyMap &map, const glm::dvec3 &near_point, float search_radius, unsigned query_flags);
  /// Destructor.
  ~NearestNeighbours() override;

  /// Get the global coordinate around which the search is centred.
  /// @return The centre of the search.
  glm::dvec3 nearPoint() const;
  /// Set the global coordinate around which to search.
  /// @param point The new search coordinate.
  void setNearPoint(const glm::dvec3 &point);

  /// Get the search radius around @p nearPoint().
  /// @return The search radius around the search centre.
  float searchRadius() const;
  /// Set the search radius around the search centre.
  /// @param range The new search radius.
  void setSearchRadius(float range);

protected:
  bool onExecute() override;
  bool onExecuteAsync() override;
  void onReset(bool hard_reset) override;

  /// Access internal details.
  /// @return Internal details.
  NearestNeighboursDetail *imp();
  /// Access internal details.
  /// @return Internal details.
  const NearestNeighboursDetail *imp() const;

  unsigned query_flags_ = 0;  ///< @c QueryFlag values.
};
}  // namespace ohm

#endif  // OHMNEARESTNEIGHBOURS_H


================================================
FILE: ohm/OccupancyMap.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OccupancyMap.h"

#include "Aabb.h"
#include "DefaultLayer.h"
#include "KeyList.h"
#include "KeyRange.h"
#include "MapChunk.h"
#include "MapCoord.h"
#include "MapLayer.h"
#include "MapProbability.h"
#include "MapRegionCache.h"
#include "RayMapperOccupancy.h"
#include "VoxelBlockCompressionQueue.h"
#include "VoxelBuffer.h"
#include "VoxelOccupancy.h"

#include "OccupancyUtil.h"

#include "private/OccupancyMapDetail.h"

#include <algorithm>
#include <cassert>
#ifdef OHM_VALIDATION
#include <cstdio>
#endif  // OHM_VALIDATION
#include <cstring>
#include <functional>
#include <limits>
#include <utility>

namespace ohm
{
namespace
{
inline Key firstKeyForChunk(const OccupancyMapDetail &map, const MapChunk &chunk)
{
#ifdef OHM_VALIDATION
  chunk.validateFirstValid(map.region_voxel_dimensions);
#endif  // OHM_VALIDATION

  // We use std::min() to ensure the first_valid_index is in range and we at least check
  // the last voxel. This primarily deals with iterating a chunk with contains no
  // valid voxels.
  const glm::u8vec3 first_valid_key = chunk.firstValidKey(map.region_voxel_dimensions);
  return Key(chunk.region.coord, std::min(first_valid_key.x, uint8_t(map.region_voxel_dimensions.x - 1)),
             std::min(first_valid_key.y, uint8_t(map.region_voxel_dimensions.y - 1)),
             std::min(first_valid_key.z, uint8_t(map.region_voxel_dimensions.z - 1)));
  // if (voxelIndex(key, map.region_voxel_dimensions) >= map.region_voxel_dimensions.x * map.region_voxel_dimensions.y
  // * map.region_voxel_dimensions.z)
  // {
  //   // First valid index is out of range. This implies it has not been set correctly.
  //   // Modify the key to address voxel [0, 0, 0].
  //   key.setLocalKey(glm::u8vec3(0, 0, 0));
  // }
  // return key;
}

bool nextChunk(OccupancyMapDetail &map, ChunkMap::iterator &chunk_iter, Key &key)
{
  ++chunk_iter;
  if (chunk_iter != map.chunks.end())
  {
    const MapChunk *chunk = chunk_iter->second;
    key = firstKeyForChunk(map, *chunk);
    return true;
  }

  return false;
}

ChunkMap::iterator &initChunkIter(uint8_t *mem)  // NOLINT(readability-non-const-parameter)
{
  // Placement new.
  return *(new (mem) ChunkMap::iterator());
}

// NOLINTNEXTLINE(readability-non-const-parameter)
ChunkMap::iterator &chunkIter(uint8_t *mem)
{
  return *reinterpret_cast<ChunkMap::iterator *>(mem);
}

const ChunkMap::iterator &chunkIter(const uint8_t *mem)
{
  return *reinterpret_cast<const ChunkMap::iterator *>(mem);
}

void releaseChunkIter(uint8_t *mem)
{
  using Iterator = ChunkMap::iterator;
  chunkIter(mem).~Iterator();
}
}  // namespace

OccupancyMap::base_iterator::base_iterator()  // NOLINT
  : key_(Key::kNull)
{
  static_assert(sizeof(ChunkMap::iterator) <= sizeof(OccupancyMap::base_iterator::chunk_mem_), "Insufficient space for "
                                                                                               "chunk iterator.");
  initChunkIter(chunk_mem_.data());
}

OccupancyMap::base_iterator::base_iterator(OccupancyMap *map, const Key &key)  // NOLINT
  : map_(map)
  , key_(key)
{
  ChunkMap::iterator &chunk_iter = initChunkIter(chunk_mem_.data());
  if (!key.isNull())
  {
    std::unique_lock<decltype(map->detail()->mutex)> guard(map->detail()->mutex);
    chunk_iter = map->detail()->chunks.find(key.regionKey());
  }
}

OccupancyMap::base_iterator::base_iterator(const base_iterator &other)  // NOLINT
  : map_(other.map_)
  , key_(other.key_)
{
  static_assert(sizeof(ChunkMap::iterator) <= sizeof(OccupancyMap::base_iterator::chunk_mem_),  //
                "Insufficient space for chunk iterator.");
  initChunkIter(chunk_mem_.data()) = chunkIter(other.chunk_mem_.data());
}

OccupancyMap::base_iterator::~base_iterator()
{
  // Explicit destructor invocation.
  releaseChunkIter(chunk_mem_.data());
}

OccupancyMap::base_iterator &OccupancyMap::base_iterator::operator=(const base_iterator &other)
{
  if (this != &other)
  {
    map_ = other.map_;
    key_ = other.key_;
    chunkIter(chunk_mem_.data()) = chunkIter(other.chunk_mem_.data());
  }
  return *this;
}

bool OccupancyMap::base_iterator::operator==(const base_iterator &other) const
{
  // Chunks only have to match when not the end/invalid iterator.
  return map_ == other.map_ && key_ == other.key_ &&
         (key_.isNull() || chunkIter(chunk_mem_.data()) == chunkIter(other.chunk_mem_.data()));
}

bool OccupancyMap::base_iterator::operator!=(const base_iterator &other) const
{
  return !(*this == other);
}

bool OccupancyMap::base_iterator::base_iterator::isValid() const
{
  return map_ && !key_.isNull();
}

void OccupancyMap::base_iterator::walkNext()
{
  if (!key_.isNull())
  {
    if (!nextLocalKey(key_, map_->detail()->region_voxel_dimensions))
    {
      // Need to move to the next chunk.
      ChunkMap::iterator &chunk = chunkIter(chunk_mem_.data());
      if (!nextChunk(*map_->detail(), chunk, key_))
      {
        // Invalidate.
        key_ = Key::kNull;
        releaseChunkIter(chunk_mem_.data());
        initChunkIter(chunk_mem_.data());
      }
    }
  }
}

const MapChunk *OccupancyMap::base_iterator::chunk() const
{
  return chunkIter(chunk_mem_.data())->second;
}

MapChunk *OccupancyMap::iterator::chunk() const
{
  return chunkIter(chunk_mem_.data())->second;
}

OccupancyMap::OccupancyMap(double resolution, const glm::u8vec3 &region_voxel_dimensions, MapFlag flags)
  : imp_(new OccupancyMapDetail)
{
  // Start the voxel map compression queue thread.
  VoxelBlockCompressionQueue::instance().retain();
  imp_->resolution = resolution;
  imp_->region_voxel_dimensions.x =
    (region_voxel_dimensions.x > 0) ? region_voxel_dimensions.x : OHM_DEFAULT_CHUNK_DIM_X;
  imp_->region_voxel_dimensions.y =
    (region_voxel_dimensions.y > 0) ? region_voxel_dimensions.y : OHM_DEFAULT_CHUNK_DIM_Y;
  imp_->region_voxel_dimensions.z =
    (region_voxel_dimensions.z > 0) ? region_voxel_dimensions.z : OHM_DEFAULT_CHUNK_DIM_Z;
  imp_->region_spatial_dimensions.x = imp_->region_voxel_dimensions.x * resolution;
  imp_->region_spatial_dimensions.y = imp_->region_voxel_dimensions.y * resolution;
  imp_->region_spatial_dimensions.z = imp_->region_voxel_dimensions.z * resolution;
  imp_->saturate_at_min_value = imp_->saturate_at_max_value = false;
  // Default min/max thresholds taken from octomap as a guide.
  imp_->min_voxel_value = -2.0f;   // NOLINT(readability-magic-numbers)
  imp_->max_voxel_value = 3.511f;  // NOLINT(readability-magic-numbers)
  setHitProbability(0.9f);         // NOLINT(readability-magic-numbers)
  setMissProbability(0.45f);       // NOLINT(readability-magic-numbers)
  setOccupancyThresholdProbability(0.5f);

  imp_->ray_filter = [](glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags) {
    const double large_long_ray_limit = 1e10;
    return ohm::goodRayFilter(start, end, filter_flags, large_long_ray_limit);
  };

  imp_->flags = flags;
  imp_->setDefaultLayout(flags);
}

OccupancyMap::OccupancyMap(double resolution, const glm::u8vec3 &region_voxel_dimensions, MapFlag flags,
                           const MapLayout &seed_layout)
  : OccupancyMap(resolution, region_voxel_dimensions, flags)
{
  imp_->layout = seed_layout;
  if ((flags & MapFlag::kVoxelMean) != MapFlag::kNone)
  {
    addVoxelMeanLayer();
  }
  if ((flags & MapFlag::kTraversal) != MapFlag::kNone)
  {
    addTraversalLayer();
  }
}

OccupancyMap::OccupancyMap(double resolution, MapFlag flags, const MapLayout &seed_layout)
  : OccupancyMap(resolution, glm::u8vec3(0, 0, 0), flags, seed_layout)
{}

OccupancyMap::OccupancyMap(double resolution, MapFlag flags)
  : OccupancyMap(resolution, glm::u8vec3(0, 0, 0), flags)
{}

OccupancyMap::~OccupancyMap()
{
  if (imp_)
  {
    clear();
    delete imp_;
  }
  // Release the voxel map compression queue thread.
  VoxelBlockCompressionQueue::instance().release();
}

OccupancyMap::iterator OccupancyMap::begin()
{
  return iterator(this, firstIterationKey());
}

OccupancyMap::const_iterator OccupancyMap::begin() const
{
  // TODO(KS): remove const cast by templating the base_iterator
  return const_iterator(const_cast<OccupancyMap *>(this), firstIterationKey());
}

OccupancyMap::iterator OccupancyMap::end()
{
  return iterator(this, Key::kNull);
}

OccupancyMap::const_iterator OccupancyMap::end() const
{
  // TODO(KS): remove const cast by templating the base_iterator
  return const_iterator(const_cast<OccupancyMap *>(this), Key::kNull);
}

size_t OccupancyMap::calculateApproximateMemory() const
{
  size_t byte_count = 0;
  byte_count += sizeof(*this);
  if (imp_)
  {
    std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);

    const size_t chunk_count = (!imp_->chunks.empty()) ? imp_->chunks.size() : imp_->loaded_region_count;
    byte_count += sizeof(OccupancyMapDetail);
    byte_count += chunk_count * sizeof(MapChunk);

    for (unsigned i = 0; i < imp_->layout.layerCount(); ++i)
    {
      const MapLayer &layer = imp_->layout.layer(i);
      byte_count += chunk_count * layer.layerByteSize(imp_->region_voxel_dimensions);
    }

    // Approximate hash map usage.
    const size_t map_bucked_count = imp_->chunks.bucket_count();
    const double map_load = imp_->chunks.max_load_factor();
    if (map_load > 1.0)
    {
      // Lint(KS): Size of pointer is correct
      // NOLINTNEXTLINE(bugprone-sizeof-expression)
      byte_count += size_t(map_bucked_count * map_load * sizeof(MapChunk *));
    }
    else
    {
      // Lint(KS): Size of pointer is correct
      // NOLINTNEXTLINE(bugprone-sizeof-expression)
      byte_count += map_bucked_count * sizeof(MapChunk *);
    }
  }

  return byte_count;
}

double OccupancyMap::resolution() const
{
  return imp_->resolution;
}

uint64_t OccupancyMap::stamp() const
{
  return imp_->stamp;
}

uint64_t OccupancyMap::touch()
{
  return ++imp_->stamp;
}

double OccupancyMap::firstRayTime() const
{
  return imp_->first_ray_time;
}

void OccupancyMap::setFirstRayTime(double time)
{
  imp_->first_ray_time = time;
}

double OccupancyMap::updateFirstRayTime(double time)
{
  imp_->first_ray_time = (imp_->first_ray_time < 0) ? time : imp_->first_ray_time;
  return imp_->first_ray_time;
}

glm::dvec3 OccupancyMap::regionSpatialResolution() const
{
  return imp_->region_spatial_dimensions;
}

glm::u8vec3 OccupancyMap::regionVoxelDimensions() const
{
  return imp_->region_voxel_dimensions;
}

size_t OccupancyMap::regionVoxelVolume() const
{
  size_t v = imp_->region_voxel_dimensions.x;
  v *= imp_->region_voxel_dimensions.y;
  v *= imp_->region_voxel_dimensions.z;
  return v;
}

glm::dvec3 OccupancyMap::regionSpatialMin(const glm::i16vec3 &region_key) const
{
  const glm::dvec3 spatial_min = regionSpatialCentre(region_key) - 0.5 * imp_->region_spatial_dimensions;
  return spatial_min;
}

glm::dvec3 OccupancyMap::regionSpatialMax(const glm::i16vec3 &region_key) const
{
  const glm::dvec3 spatial_max = regionSpatialCentre(region_key) + 0.5 * imp_->region_spatial_dimensions;
  return spatial_max;
}

glm::dvec3 OccupancyMap::regionSpatialCentre(const glm::i16vec3 &region_key) const
{
  const glm::dvec3 centre(regionCentreCoord(region_key.x, imp_->region_spatial_dimensions.x),
                          regionCentreCoord(region_key.y, imp_->region_spatial_dimensions.y),
                          regionCentreCoord(region_key.z, imp_->region_spatial_dimensions.z));
  return centre;
}

void OccupancyMap::setOrigin(const glm::dvec3 &origin)
{
  imp_->origin = origin;
}

const glm::dvec3 &OccupancyMap::origin() const
{
  return imp_->origin;
}

bool OccupancyMap::calculateExtents(glm::dvec3 *min_ext, glm::dvec3 *max_ext, KeyRange *key_range) const
{
  glm::dvec3 region_min;
  glm::dvec3 region_max;
  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  // Empty map if there are no chunks or the voxel dimensions are zero (latter just shouldn't happen).
  if (imp_->chunks.empty() || glm::any(glm::equal(imp_->region_voxel_dimensions, glm::u8vec3(0))))
  {
    // Empty map. Use the origin.
    if (min_ext)
    {
      *min_ext = imp_->origin;
    }
    if (max_ext)
    {
      *max_ext = imp_->origin;
    }

    if (key_range)
    {
      // Set an invalid range.
      *key_range = KeyRange();
    }
    return false;
  }

  glm::dvec3 min_spatial(std::numeric_limits<double>::max());
  glm::dvec3 max_spatial(-std::numeric_limits<double>::max());
  // We only need to track the min/max region keys. The min local voxel coordinate within a region is always (0, 0, 0),
  // while the maximum is always the region voxel dimensions - 1
  glm::i16vec3 min_region_key(std::numeric_limits<int16_t>::max());
  glm::i16vec3 max_region_key(std::numeric_limits<int16_t>::min());
  bool have_extents = false;

  for (auto &&chunk : imp_->chunks)
  {
    const MapRegion region = chunk.second->region;
    region_min = region_max = region.centre;
    region_min -= 0.5 * regionSpatialResolution();
    region_max += 0.5 * regionSpatialResolution();

    min_spatial.x = std::min(min_spatial.x, region_min.x);
    min_spatial.y = std::min(min_spatial.y, region_min.y);
    min_spatial.z = std::min(min_spatial.z, region_min.z);

    max_spatial.x = std::max(max_spatial.x, region_max.x);
    max_spatial.y = std::max(max_spatial.y, region_max.y);
    max_spatial.z = std::max(max_spatial.z, region_max.z);

    min_region_key.x = std::min(region.coord.x, min_region_key.x);
    min_region_key.y = std::min(region.coord.y, min_region_key.y);
    min_region_key.z = std::min(region.coord.z, min_region_key.z);
    max_region_key.x = std::max(region.coord.x, max_region_key.x);
    max_region_key.y = std::max(region.coord.y, max_region_key.y);
    max_region_key.z = std::max(region.coord.z, max_region_key.z);

    have_extents = true;
  }

  // Finalise the min/max voxel keys.
  const Key min_voxel(min_region_key, glm::u8vec3(0, 0, 0));
  const Key max_voxel(max_region_key, imp_->region_voxel_dimensions - glm::u8vec3(1, 1, 1));

  // Write output values.
  if (min_ext)
  {
    *min_ext = min_spatial;
  }

  if (max_ext)
  {
    *max_ext = max_spatial;
  }

  if (key_range)
  {
    key_range->setRegionDimensions(imp_->region_voxel_dimensions);
    key_range->setMinKey(min_voxel);
    key_range->setMaxKey(max_voxel);
  }

  return have_extents;
}

bool OccupancyMap::calculateExtents(glm::dvec3 *min_ext, glm::dvec3 *max_ext, Key *min_key, Key *max_key) const
{
  KeyRange range;
  bool valid = calculateExtents(min_ext, max_ext, &range);
  if (min_key)
  {
    *min_key = range.minKey();
  }
  if (max_key)
  {
    *max_key = range.maxKey();
  }
  return valid;
}

MapInfo &OccupancyMap::mapInfo()
{
  return imp_->info;
}

const MapInfo &OccupancyMap::mapInfo() const
{
  return imp_->info;
}

MapFlag OccupancyMap::flags() const
{
  return imp_->flags;
}

const MapLayout &OccupancyMap::layout() const
{
  return imp_->layout;
}

MapLayout &OccupancyMap::layout()
{
  return imp_->layout;
}

void OccupancyMap::addVoxelMeanLayer()
{
  if (imp_->layout.meanLayer() >= 0)
  {
    // Already present.
    return;
  }

  MapLayout layout = imp_->layout;
  addVoxelMean(layout);
  updateLayout(layout);
}


bool OccupancyMap::voxelMeanEnabled() const
{
  return imp_->layout.meanLayer() >= 0;
}


void OccupancyMap::addTraversalLayer()
{
  if (imp_->layout.traversalLayer() >= 0)
  {
    // Already present.
    return;
  }

  MapLayout layout = imp_->layout;
  addTraversal(layout);
  updateLayout(layout);
}


bool OccupancyMap::traversalEnabled() const
{
  return imp_->layout.traversalLayer() >= 0;
}


void OccupancyMap::addTouchTimeLayer()
{
  if (touchTimeEnabled())
  {
    // Already present.
    return;
  }

  MapLayout layout = imp_->layout;
  addTouchTime(layout);
  updateLayout(layout);
}


bool OccupancyMap::touchTimeEnabled() const
{
  return imp_->layout.layerIndex(default_layer::touchTimeLayerName()) >= 0;
}


void OccupancyMap::addIncidentNormalLayer()
{
  if (touchTimeEnabled())
  {
    // Already present.
    return;
  }

  MapLayout layout = imp_->layout;
  addIncidentNormal(layout);
  updateLayout(layout);
}


bool OccupancyMap::incidentNormalEnabled() const
{
  return imp_->layout.layerIndex(default_layer::incidentNormalLayerName()) >= 0;
}


void OccupancyMap::addLayer(const char *layer_name, const std::function<void(MapLayout &)> &add_layer_function)
{
  if (imp_->layout.layerIndex(layer_name) >= 0)
  {
    // Already present.
    return;
  }

  MapLayout layout = imp_->layout;
  add_layer_function(layout);
  updateLayout(layout);
}


void OccupancyMap::updateLayout(const MapLayout &new_layout, bool preserve_map)
{
  // First check if there is a difference between the @c MapLayout and the actual layout.
  // There's no work to do otherwise.

  const MapLayoutMatch match = imp_->layout.checkEquivalent(new_layout);
  if (match == MapLayoutMatch::kExact)
  {
    // Already matches the current layout. Nothing to do.
    return;
  }

  // Check for partial match. In this case we just have to update to the new layout values and don't need to adjust
  // the chunks.
  if (match == MapLayoutMatch::kEquivalent)
  {
    imp_->layout = new_layout;
    return;
  }

  // We have a memory change. A full update is required.

  // First we have to synchronise the GPU cache(s).
  if (imp_->gpu_cache)
  {
    imp_->gpu_cache->clear();
  }

  if (preserve_map)
  {
    /// Tracking of layer indices to preserve. First item is the layer index in the current layout, while the second is
    /// in the new layout.
    std::vector<std::pair<const MapLayer *, const MapLayer *>> layer_mapping;

    for (size_t i = 0; i < new_layout.layerCount(); ++i)
    {
      const MapLayer &new_layer = new_layout.layer(i);
      // Look for a matching layer in the current layout.
      if (const MapLayer *layer = imp_->layout.layer(new_layer.name()))
      {
        // Found a layer with matching name. Check for equivalent layout.
        if (layer->checkEquivalent(new_layer) != MapLayoutMatch::kDifferent)
        {
          // Layers are equivalent. Add a mapping.
          layer_mapping.emplace_back(std::make_pair(layer, &new_layer));
        }
      }
    }

    // Walk the chunks preserving which layers we can.
    for (auto &chunk : imp_->chunks)
    {
      chunk.second->updateLayout(&new_layout, layer_mapping);
    }
  }
  else
  {
    clear();
  }

  imp_->layout = new_layout;

  // Now reallocate any GPU cache which relies on the occupancy layer.
  if (imp_->gpu_cache)
  {
    imp_->gpu_cache->reinitialise();
  }
}


size_t OccupancyMap::regionCount() const
{
  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  return imp_->chunks.size();
}

unsigned OccupancyMap::expireRegions(double timestamp)
{
  const auto should_remove_chunk = [timestamp](const MapChunk &chunk) { return chunk.touched_time < timestamp; };

  return cullRegions(should_remove_chunk);
}

unsigned OccupancyMap::removeDistanceRegions(const glm::dvec3 &relative_to, float distance)
{
  const float dist_sqr = distance * distance;
  const auto should_remove_chunk = [relative_to, dist_sqr](const MapChunk &chunk) {
    glm::dvec3 separation;
    separation = chunk.region.centre - relative_to;
    const double region_distance_sqr = glm::dot(separation, separation);
    return region_distance_sqr >= dist_sqr;
  };

  return cullRegions(should_remove_chunk);
}

unsigned OccupancyMap::cullRegionsOutside(const glm::dvec3 &min_extents, const glm::dvec3 &max_extents)
{
  const glm::dvec3 region_extents = imp_->region_spatial_dimensions;
  const Aabb cull_box(min_extents, max_extents);
  const auto should_remove_chunk = [cull_box, region_extents](const MapChunk &chunk) {
    return !cull_box.overlaps(
      Aabb(chunk.region.centre - 0.5 * region_extents, chunk.region.centre + 0.5 * region_extents));
  };

  return cullRegions(should_remove_chunk);
}

void OccupancyMap::touchRegionTimestampByKey(const glm::i16vec3 &region_key, double timestamp, bool allow_create)
{
  MapChunk *chunk = region(region_key, allow_create);
  if (chunk)
  {
    chunk->touched_time = timestamp;
  }
}

glm::dvec3 OccupancyMap::regionCentreGlobal(const glm::i16vec3 &region_key) const
{
  return imp_->origin + regionCentreLocal(region_key);
}

glm::dvec3 OccupancyMap::regionCentreLocal(const glm::i16vec3 &region_key) const
{
  glm::dvec3 centre;
  centre.x = region_key.x * imp_->region_spatial_dimensions.x;
  centre.y = region_key.y * imp_->region_spatial_dimensions.y;
  centre.z = region_key.z * imp_->region_spatial_dimensions.z;
  return centre;
}

glm::i16vec3 OccupancyMap::regionKey(const glm::dvec3 &point) const
{
  MapRegion region(point, imp_->origin, imp_->region_spatial_dimensions);
  return region.coord;
}

float OccupancyMap::hitValue() const
{
  return imp_->hit_value;
}

float OccupancyMap::hitProbability() const
{
  return valueToProbability(imp_->hit_value);
}

void OccupancyMap::setHitProbability(float probability)
{
  imp_->hit_value = probabilityToValue(probability);
}

void OccupancyMap::setHitValue(float value)
{
  imp_->hit_value = value;
  ;
}

float OccupancyMap::missValue() const
{
  return imp_->miss_value;
}

float OccupancyMap::missProbability() const
{
  return valueToProbability(imp_->miss_value);
}

void OccupancyMap::setMissProbability(float probability)
{
  imp_->miss_value = probabilityToValue(probability);
}

void OccupancyMap::setMissValue(float value)
{
  imp_->miss_value = value;
  ;
}

float OccupancyMap::occupancyThresholdValue() const
{
  return imp_->occupancy_threshold_value;
}

float OccupancyMap::occupancyThresholdProbability() const
{
  return valueToProbability(imp_->occupancy_threshold_value);
}

void OccupancyMap::setOccupancyThresholdProbability(float probability)
{
  imp_->occupancy_threshold_value = probabilityToValue(probability);
}

float OccupancyMap::minVoxelValue() const
{
  return imp_->min_voxel_value;
}

void OccupancyMap::setMinVoxelValue(float value)
{
  imp_->min_voxel_value = value;
}

bool OccupancyMap::saturateAtMinValue() const
{
  return imp_->saturate_at_min_value;
}

void OccupancyMap::setSaturateAtMinValue(bool saturate)
{
  imp_->saturate_at_min_value = saturate;
}

float OccupancyMap::maxVoxelValue() const
{
  return imp_->max_voxel_value;
}

void OccupancyMap::setMaxVoxelValue(float value)
{
  imp_->max_voxel_value = value;
}

bool OccupancyMap::saturateAtMaxValue() const
{
  return imp_->saturate_at_max_value;
}

void OccupancyMap::setSaturateAtMaxValue(bool saturate)
{
  imp_->saturate_at_max_value = saturate;
}

glm::dvec3 OccupancyMap::voxelCentreLocal(const Key &key) const
{
  return ohm::OccupancyMap::voxelCentre(key, imp_->resolution, imp_->region_spatial_dimensions);
}

glm::dvec3 OccupancyMap::voxelCentreGlobal(const Key &key) const
{
  return ohm::OccupancyMap::voxelCentre(key, imp_->resolution, imp_->region_spatial_dimensions, imp_->origin);
}

Key OccupancyMap::voxelKey(const glm::dvec3 &point) const
{
  Key key;
  // const glm::dvec3 map_local_key = point - imp_->origin;
  MapRegion region(point, imp_->origin, imp_->region_spatial_dimensions);
  // VALIDATION code ensures the region we calculate to contain the point does.
  // Floating point error was causing issues where it nearly, but not quite would.
  const bool voxelKeyOk = region.voxelKey(key, point, imp_->origin, imp_->region_spatial_dimensions,
                                          imp_->region_voxel_dimensions, imp_->resolution);
  (void)voxelKeyOk;
#ifdef OHM_VALIDATION
  if (!voxelKeyOk)
  {
    fprintf(stderr, "E: Validation failure: Point (%lg %lg %lg) fell into a region which generated an invalid key.\n",
            point.x, point.y, point.z);
    fprintf(stderr, "  Point: %.20lf %.20lf %.20lf\n", point.x, point.y, point.z);
    fprintf(stderr, "  Map origin: %lf %lf %lf\n", imp_->origin.x, imp_->origin.y, imp_->origin.z);
    fprintf(stderr, "  Map resolution: %lf\n", imp_->resolution);
    fprintf(stderr, "  Region sizing: %lf %lf %lf\n", imp_->region_spatial_dimensions.x,
            imp_->region_spatial_dimensions.y, imp_->region_spatial_dimensions.z);
    fprintf(stderr, "  Region voxels: %d %d %d\n", imp_->region_voxel_dimensions.x, imp_->region_voxel_dimensions.y,
            imp_->region_voxel_dimensions.z);
    fprintf(stderr, "  Region coord: %d %d %d\n", region.coord.x, region.coord.y, region.coord.z);
    fprintf(stderr, "  Region centre: %lf %lf %lf\n", region.centre.x, region.centre.y, region.centre.z);
  }
#endif  // OHM_VALIDATION
  return key;
}

Key OccupancyMap::voxelKey(const glm::vec3 &point) const
{
  Key key;
  MapRegion region(point, imp_->origin, imp_->region_spatial_dimensions);
  region.voxelKey(key, point, imp_->origin, imp_->region_spatial_dimensions, imp_->region_voxel_dimensions,
                  imp_->resolution);
  return key;
}

Key OccupancyMap::voxelKeyLocal(const glm::vec3 &local_point) const
{
  Key key;
  const glm::dvec3 zero_origin(0, 0, 0);
  MapRegion region(local_point, zero_origin, imp_->region_spatial_dimensions);
  region.voxelKey(key, local_point, zero_origin, imp_->region_spatial_dimensions, imp_->region_voxel_dimensions,
                  imp_->resolution);
  return key;
}

void OccupancyMap::moveKeyAlongAxis(Key &key, int axis, int step) const
{
  imp_->moveKeyAlongAxis(key, axis, step);
}

void OccupancyMap::stepKey(Key &key, int axis, int dir) const
{
  stepKey(key, axis, dir, imp_->region_voxel_dimensions);
}

void OccupancyMap::moveKey(Key &key, int x, int y, int z) const
{
  moveKeyAlongAxis(key, 0, x);
  moveKeyAlongAxis(key, 1, y);
  moveKeyAlongAxis(key, 2, z);
}

glm::ivec3 OccupancyMap::rangeBetween(const Key &from, const Key &to) const
{
  return rangeBetween(from, to, imp_->region_voxel_dimensions);
}

void OccupancyMap::setRayFilter(const RayFilterFunction &ray_filter)
{
  imp_->ray_filter = ray_filter;
}

const RayFilterFunction &OccupancyMap::rayFilter() const
{
  return imp_->ray_filter;
}

void OccupancyMap::clearRayFilter()
{
  imp_->ray_filter = RayFilterFunction();
}

void OccupancyMap::integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities,
                                 const double *timestamps, unsigned ray_update_flags)
{
  // This function has been updated to leverage the new RayMapper interface and remove code duplication. It is
  // maintained for legacy reasons.
  // TODO(KS): remove this function and require the use of a RayMapper.
  RayMapperOccupancy(this).integrateRays(rays, element_count, intensities, timestamps, ray_update_flags);
}

OccupancyMap *OccupancyMap::clone() const
{
  return clone(-glm::dvec3(std::numeric_limits<double>::infinity()),
               glm::dvec3(std::numeric_limits<double>::infinity()));
}

OccupancyMap *OccupancyMap::clone(const glm::dvec3 &min_ext, const glm::dvec3 &max_ext) const
{
  auto *new_map = new OccupancyMap(imp_->resolution, imp_->region_voxel_dimensions);

  if (imp_->ray_filter)
  {
    new_map->setRayFilter(imp_->ray_filter);
  }

  // Copy general details.
  new_map->detail()->copyFrom(*imp_);

  glm::dvec3 region_min;
  glm::dvec3 region_max;
  const glm::dvec3 region_half_ext = 0.5 * imp_->region_spatial_dimensions;
  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  for (const auto &chunk_iter : imp_->chunks)
  {
    const MapChunk *src_chunk = chunk_iter.second;
    region_min = region_max = src_chunk->region.centre;
    region_min -= region_half_ext;
    region_max += region_half_ext;

    if (!glm::any(glm::lessThan(region_max, min_ext)) && !glm::any(glm::greaterThan(region_min, max_ext)))
    {
      MapChunk *dst_chunk = new_map->region(src_chunk->region.coord, true);
      dst_chunk->first_valid_index = src_chunk->first_valid_index;
      dst_chunk->touched_time = src_chunk->touched_time;
      dst_chunk->dirty_stamp = src_chunk->dirty_stamp.load();
      dst_chunk->flags = src_chunk->flags;

      for (unsigned i = 0; i < imp_->layout.layerCount(); ++i)
      {
        dst_chunk->touched_stamps[i] = static_cast<uint64_t>(src_chunk->touched_stamps[i]);
        if (src_chunk->voxel_blocks[i])
        {
          VoxelBuffer<const VoxelBlock> src_buffer(src_chunk->voxel_blocks[i]);
          VoxelBuffer<VoxelBlock> dst_buffer(dst_chunk->voxel_blocks[i]);
          memcpy(dst_buffer.voxelMemory(), src_buffer.voxelMemory(), dst_buffer.voxelMemorySize());
        }
      }
    }
  }

  return new_map;
}

void OccupancyMap::enumerateRegions(std::vector<const MapChunk *> &chunks) const
{
  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  for (auto &&chunk_iter : imp_->chunks)
  {
    chunks.push_back(chunk_iter.second);
  }
}

MapChunk *OccupancyMap::region(const glm::i16vec3 &region_key, bool allow_create)
{
  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  const auto region_search = imp_->chunks.find(region_key);
  if (region_search != imp_->chunks.end())
  {
    MapChunk *chunk = region_search->second;
#ifdef OHM_VALIDATION
    chunk->validateFirstValid(imp_->region_voxel_dimensions);
#endif  // OHM_VALIDATION
    return chunk;
  }

  if (allow_create)
  {
    // No such chunk. Create one.
    MapChunk *chunk = newChunk(Key(region_key, 0, 0, 0));
    imp_->chunks.insert(std::make_pair(chunk->region.coord, chunk));
    // No need to touch the map here. We haven't changed the semantics of the map.
    // That happens when the value of a voxel in the region changes.
    return chunk;
  }

  return nullptr;
}

const MapChunk *OccupancyMap::region(const glm::i16vec3 &region_key) const
{
  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  const auto region_search = imp_->chunks.find(region_key);
  if (region_search != imp_->chunks.end())
  {
    const MapChunk *chunk = region_search->second;
    return chunk;
  }

  return nullptr;
}

unsigned OccupancyMap::collectDirtyRegions(uint64_t from_stamp,
                                           std::vector<std::pair<uint64_t, glm::i16vec3>> &regions) const
{
  // Brute for for now.
  unsigned added_count = 0;
  bool added;
  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  for (auto &&chunk_ref : imp_->chunks)
  {
    if (chunk_ref.second->dirty_stamp > from_stamp)
    {
      added = false;

      // Insertion sorted on the chunk's dirty stamp. Least recently touched (oldtest) first.
      // TODO(KS): test efficiency of the sorted insertion on a vector.
      // Scope should be small so I expect little impact.
      auto item = std::make_pair(chunk_ref.second->dirty_stamp.load(), chunk_ref.second->region.coord);
      for (auto iter = regions.begin(); iter != regions.end(); ++iter)
      {
        if (item.first < iter->first)
        {
          regions.insert(iter, item);
          added = true;
          break;
        }
      }

      if (!added)
      {
        regions.push_back(item);
      }

      ++added_count;
    }
  }

  return added_count;
}

uint64_t OccupancyMap::calculateDirtyExtents(uint64_t from_stamp, glm::i16vec3 *min_ext, glm::i16vec3 *max_ext) const
{
  *min_ext = glm::i16vec3(std::numeric_limits<decltype(min_ext->x)>::max());
  *max_ext = glm::i16vec3(std::numeric_limits<decltype(min_ext->x)>::min());

  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  const uint64_t at_stamp = imp_->stamp;
  for (auto &&chunk_ref : imp_->chunks)
  {
    if (chunk_ref.second->dirty_stamp > from_stamp)
    {
      min_ext->x = std::min(chunk_ref.second->region.coord.x, min_ext->x);
      min_ext->y = std::min(chunk_ref.second->region.coord.y, min_ext->y);
      min_ext->z = std::min(chunk_ref.second->region.coord.z, min_ext->z);

      max_ext->x = std::max(chunk_ref.second->region.coord.x, max_ext->x);
      max_ext->y = std::max(chunk_ref.second->region.coord.y, max_ext->y);
      max_ext->z = std::max(chunk_ref.second->region.coord.z, max_ext->z);
    }
  }
  guard.unlock();

  if (min_ext->x > max_ext->x)
  {
    *min_ext = glm::i16vec3(1);
    *max_ext = glm::i16vec3(0);
  }
  return at_stamp;
}

void OccupancyMap::calculateDirtyClearanceExtents(glm::i16vec3 *min_ext, glm::i16vec3 *max_ext,
                                                  unsigned region_padding) const
{
  *min_ext = glm::i16vec3(std::numeric_limits<decltype(min_ext->x)>::max());
  *max_ext = glm::i16vec3(std::numeric_limits<decltype(min_ext->x)>::min());

  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  const int occupancy_layer = imp_->layout.occupancyLayer();
  const int clearance_layer = imp_->layout.clearanceLayer();

  if (occupancy_layer >= 0 && clearance_layer >= 0)
  {
    for (auto &&chunk_ref : imp_->chunks)
    {
      if (chunk_ref.second->touched_stamps[clearance_layer] < chunk_ref.second->touched_stamps[occupancy_layer])
      {
        min_ext->x = std::min<int>(chunk_ref.second->region.coord.x - region_padding, min_ext->x);
        min_ext->y = std::min<int>(chunk_ref.second->region.coord.y - region_padding, min_ext->y);
        min_ext->z = std::min<int>(chunk_ref.second->region.coord.z - region_padding, min_ext->z);

        max_ext->x = std::max<int>(chunk_ref.second->region.coord.x + region_padding, max_ext->x);
        max_ext->y = std::max<int>(chunk_ref.second->region.coord.y + region_padding, max_ext->y);
        max_ext->z = std::max<int>(chunk_ref.second->region.coord.z + region_padding, max_ext->z);
      }
    }
  }
  guard.unlock();

  if (min_ext->x > max_ext->x)
  {
    *min_ext = glm::i16vec3(1);
    *max_ext = glm::i16vec3(0);
  }
}

void OccupancyMap::clear()
{
  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  // Clear the GPU cache (if present).
  // Must occur before deleting the chunks as it will be referencing some.
  if (imp_->gpu_cache)
  {
    imp_->gpu_cache->clear();
  }

  for (auto &&chunk_ref : imp_->chunks)
  {
    releaseChunk(chunk_ref.second);
  }

  imp_->chunks.clear();
  imp_->loaded_region_count = 0;
}

Key OccupancyMap::firstIterationKey() const
{
  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  const auto first_chunk_iter = imp_->chunks.begin();
  if (first_chunk_iter != imp_->chunks.end())
  {
    MapChunk *chunk = first_chunk_iter->second;
    return firstKeyForChunk(*imp_, *chunk);
  }

  return Key::kNull;
}

MapChunk *OccupancyMap::newChunk(const Key &for_key)
{
  auto *chunk =
    new MapChunk(MapRegion(voxelCentreGlobal(for_key), imp_->origin, imp_->region_spatial_dimensions), *imp_);
  return chunk;
}

void OccupancyMap::releaseChunk(const MapChunk *chunk)
{
  delete chunk;
}

unsigned OccupancyMap::cullRegions(const RegionCullFunc &cull_func)
{
  unsigned removed_count = 0;
  std::unique_lock<decltype(imp_->mutex)> guard(imp_->mutex);
  auto region_iter = imp_->chunks.begin();
  const MapChunk *chunk = nullptr;
  while (region_iter != imp_->chunks.end())
  {
    chunk = region_iter->second;

    if (cull_func(*chunk))
    {
      // Remove from the GPU cache.
      if (imp_->gpu_cache)
      {
        imp_->gpu_cache->remove(chunk->region.coord);
      }

      // Culled region. Remove from the map.
      region_iter = imp_->chunks.erase(region_iter);
      releaseChunk(chunk);
      ++removed_count;
    }
    else
    {
      // Next.
      ++region_iter;
    }
  }

  return removed_count;
}
}  // namespace ohm


================================================
FILE: ohm/OccupancyMap.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OCCUPANCYMAP_H
#define OCCUPANCYMAP_H

#include "OhmConfig.h"

#include "Key.h"
#include "MapFlag.h"
#include "MapProbability.h"
#include "OccupancyType.h"
#include "RayFilter.h"
#include "RayFlag.h"

#include <glm/glm.hpp>

#include <array>
#include <functional>
#include <vector>

#define OHM_DEFAULT_CHUNK_DIM_X 32
#define OHM_DEFAULT_CHUNK_DIM_Y OHM_DEFAULT_CHUNK_DIM_X
#define OHM_DEFAULT_CHUNK_DIM_Z OHM_DEFAULT_CHUNK_DIM_X

namespace ohm
{
class Aabb;
class KeyList;
class KeyRange;
struct MapChunk;
class MapInfo;
class MapLayout;
struct OccupancyMapDetail;
class RayFilter;

/// A spatial container using a voxel representation of 3D space.
///
/// The map is divided into rectangular regions of voxels. The map uses an initial spatial hash to identify a larger
/// @c MapRegion which contains a contiguous array of voxels. This is used rather than an octree to support:
/// - Fast region allocation/de-allocation
/// - Constant lookup
/// - Dropping regions
/// - Eventually, out of core serialisation.
///
/// The size of the regions is determined by the @c regionVoxelDimensions argument given on construction. Larger
/// regions consume more memory each, but are more suited to GPU based operations. The default size of 32*32*32, or an
/// equivalent volume is recommended.
///
/// The contents of an @c OccupancyMap are determined by the @c MapLayout with voxel data split into separate layers.
/// An @c OccupancyMap is always expected to support a occupancy layer of @c float values. A map may optionally
/// support additional, standard layers and user defined layers. The standard layers are:
///
/// Layer Name    | Type          | Purpose
/// ------------- | ------------- | --------------
/// occupancy     | @c float      | Stores occupancy values
/// mean          | @c VoxelMean  | Quantised voxel mean coordinates and sample count
/// covariance    | @c CovarianceVoxel  | Tracks the covariance within the voxel for @c NdtMap
/// clearance     | @c float      | Distance to the nearest occupied voxel (experimental - see @c ClearanceProcess )
///
/// Voxels may be accessed directly from @c MapChunk entries accessed via @c region() calls. This provides direct
/// access to the voxel layer memory and requires manual decoding of voxel layer content and indexing. Direct access
/// may yield the best performance, but requires advanced use code. The @c Voxel<T> template class may be used to
/// access voxel data via a convenient abstraction and provides some validation of the voxel content against the
/// requested data type.
///
/// For example, the occupancy layer and the @c VoxelMean may be accessed with the following code:
///
/// @code
/// ohm::OccupancyMap map(0.1, ohm::MapFlag::kVoxelMean);
///
/// ohm::Voxel<float> occupancy(&map, map.layout().occupancyLayer());
/// ohm::Voxel<ohm::VoxelMean> mean(&map, map.layout().occupancyLayer());
///
/// const glm::dvec3 sample{1, 2, 3};
/// occupancy.setKey(map.voxelKey(sample);
/// mean.setKey(occupancy.key());
///
/// ohm::integrateHit(occupancy);
/// ohm::updatePosition(mean);
/// @endcode
///
/// The map is typically updated by collecting point samples with corresponding sensor origin coordinates via a
/// @c RayMapper class such as @c RayMapperOccupancy . More manual updates may also be made by passing each
/// origin/sample pair to @c calculateSegmentKeys() . This identifies the voxels intersected by the line segment
/// connecting the two points. These voxels, excluding the voxel containing the sample, should be updated in the map
/// by calling @c integrateMiss() reinforcing such voxels as free. For sample voxels, @c integrateHit() should be
/// called.
///
/// There are two forms of addressing for the @c OccupancyMap - region and local - encapsulated by the @c Key class.
/// The region key is a coarse indexer which identifies a @c MapRegion and its associated memory in @c MapChunk .
/// The local key is a fine indexer which resolves to a specific voxel within a @c MapRegion . Region indexing is
/// limited by the size of an @c int16_t supporting a spatial range of
/// ~`2^15 * regionVoxelDimensions() * resolution()` while the local key is limited by @c regionVoxelDimensions() .
///
/// @par Compression
/// The dense memory architecture used by this class creates an inherent memory overhead. Regions are readily
/// sparsely populated, but the memory allocation for a region is fixed. @c MapFlag::kCompressed may be used alleviate
/// the memory usage. In this mode an @c OccupancyMap uses a background compression thread to compress voxel data. The
/// background compression thread compresses regions which have not been touched for some time.
///
/// The background compression does impose a some CPU overhead and latency especially when iterating the map as a
/// whole to ensure voxel data are uncompressed when needed. The overhead is minimal when not using compression.
///
/// @todo Consider ways to modify the ohm API to better support `std::shared_ptr<OccupancyMap>`. Current occupancy map
/// usage encourages stack allocation or @c std::unique_ptr usage of the map, but then ends up passing ray pointer and
/// marks borrowed pointers to other objects - such as a @c GpuMap . This obfuscates ownership. The plan would be to
/// use @c std::shared_ptr<OccupancyMap> in these borrowed pointer cases. The ideal is to continue to allow stack
/// allocation of a map be ensuring the map lifetime exceeds any borrowed ownership. This needs testing and
/// verification, but the goal is to use smart pointers rather than raw pointers. At worst this may require
/// deriving @c std::enable_shared_from_this<OccupancyMap>
class ohm_API OccupancyMap
{
public:
  /// Base iterator class.
  class ohm_API base_iterator  // NOLINT
  {
  public:
    /// Invalid constructor.
    base_iterator();
    /// Base iterator into @p map starting at @p key. Map must remain unchanged during iteration.
    /// @param map The map to iterate in.
    /// @param key The key to start iterating at.
    base_iterator(OccupancyMap *map, const Key &key);
    /// Copy constructor.
    /// @param other Object to shallow copy.
    base_iterator(const base_iterator &other);
    /// Destructor.
    ~base_iterator();

    /// Key for the current voxel.
    /// @return Current voxel key.
    inline const Key &key() const { return key_; }

    /// Access the @c MapChunk the iterator is currently referencing.
    /// @return The current @c MapChunk .
    const MapChunk *chunk() const;

    /// Access the @c OccupancyMap the iterator is currently referencing.
    /// @return The current @c OccupancyMap .
    inline const OccupancyMap *map() const { return map_; }

    /// Copy assignment.
    /// @param other Object to shallow copy.
    base_iterator &operator=(const base_iterator &other);

    /// Comparison: equality. Comparison of invalid iterators is ill defined unless referencing the same map.
    /// @param other Iterator to compare against.
    /// @return True if @c this is logically equivalent to @p other.
    bool operator==(const base_iterator &other) const;

    /// Comparison: inequality.
    /// @param other Iterator to compare against.
    /// @return True if @c this is not logically equivalent to @p other.
    bool operator!=(const base_iterator &other) const;

    /// Is this iterator still valid?
    /// @return True if the iterator may be safely dereferenced.
    bool isValid() const;

    /// Dereference the iterator as a key.
    /// @return The current voxel key.
    inline const Key &operator*() const { return key_; }

    /// Dereference the iterator as a key.
    /// @return The current voxel key.
    inline const Key *operator->() const { return &key_; }

  protected:
    /// Move to the next voxel. Iterator becomes invalid if already referencing the last voxel.
    /// Iterator is unchanged if already invalid.
    void walkNext();

    OccupancyMap *map_ = nullptr;  ///< The referenced map.
    Key key_;                      ///< The current voxel key.
    /// Memory used to track an iterator into a hidden container type.
    /// We use an anonymous, fixed size memory chunk and placement new to prevent exposing STL
    /// types as part of the ABI.
    std::array<uint8_t, 32> chunk_mem_;  // NOLINT(readability-magic-numbers)
  };

  /// Non-constant iterator into an @c OccupancyMap.
  class ohm_API iterator : public base_iterator  // NOLINT
  {
  public:
    /// Constructor of an invalid iterator.
    iterator() = default;
    /// Iterator into @p map starting at @p key . Map must remain unchanged during iteration.
    /// @param map The map to iterate in.
    /// @param key The key to start iterating at.
    inline iterator(OccupancyMap *map, const Key &key)
      : base_iterator(map, key)
    {}
    /// Copy constructor.
    /// @param other Object to shallow copy.
    inline iterator(const iterator &other) = default;

    /// Empty destructor.
    ~iterator() = default;

    using base_iterator::chunk;
    /// @overload
    MapChunk *chunk() const;

    using base_iterator::map;
    /// @overload
    inline OccupancyMap *map() { return map_; }

    /// Prefix increment for the iterator. Iterator becomes invalid when incrementing an iterator
    /// referencing the last voxel in the map. Safe to call on an invalid iterator (no change).
    /// @return This iterator after the increment.
    inline iterator &operator++()
    {
      walkNext();
      return *this;
    }

    /// Postfix increment for the iterator. Iterator becomes invalid when incrementing an iterator
    /// referencing the last voxel in the map. Safe to call on an invalid iterator (no change).
    /// @return @c This iterator before the increment.
    inline iterator operator++(int)
    {
      iterator iter(*this);
      walkNext();
      return iter;
    }
  };

  /// Constant (read only) iterator for an @c OccupancyMap.
  class ohm_API const_iterator : public base_iterator  // NOLINT
  {
  public:
    /// Constructor of an invalid iterator.
    const_iterator() = default;
    /// Iterator into @p map starting at @p key. Map must remain unchanged during iteration.
    /// @param map The map to iterate in.
    /// @param key The key to start iterating at.
    inline const_iterator(OccupancyMap *map, const Key &key)
      : base_iterator(map, key)
    {}
    /// Copy constructor.
    /// @param other Object to shallow copy.
    inline const_iterator(const const_iterator &other) = default;
    /// Copy constructor.
    /// @param other Object to shallow copy.
    inline const_iterator(const iterator &other)  // NOLINT(google-explicit-constructor) want implicit conversion here
      : base_iterator(other)
    {}

    /// Prefix increment for the iterator. Iterator becomes invalid when incrementing an iterator
    /// referencing the last voxel in the map. Safe to call on an invalid iterator (no change).
    /// @return This iterator after the increment.
    inline const_iterator &operator++()
    {
      walkNext();
      return *this;
    }
    /// Postfix increment for the iterator. Iterator becomes invalid when incrementing an iterator
    /// referencing the last voxel in the map. Safe to call on an invalid iterator (no change).
    /// @return @c This iterator before the increment.
    inline const_iterator operator++(int)
    {
      const_iterator iter(*this);
      walkNext();
      return iter;
    }
  };

  /// Construct an @c OccupancyMap at the given voxels resolution.
  ///
  /// The @p regionVoxelDimensions controls the size of the map regions. The number of voxels
  /// along each axis is set by each axis of @p regionVoxelDimensions, thus the total volume of
  /// each region is set by the product of the axes. The memory usage of the region voxels is
  /// determined by multiplying the region volume by the size of:
  /// - float for the occupancy value
  /// - float for the clearance value
  /// There is also a sub-sampled clearance array in each chunk (size TBC).
  ///
  /// @param resolution The resolution for a single voxel in the map. Any zero value
  ///   dimension is replaced with its default value; e.g., @c OHM_DEFAULT_CHUNK_DIM_X.
  /// @param region_voxel_dimensions Sets the number of voxels in each map region.
  /// @param flags Map initialisation flags. See @c MapFlag .
  /// @param seed_layout The @p MapLayout to create the map with. The constructed map clones the @c seed_layout
  ///   object. Omit to use the default layout.
  OccupancyMap(double resolution, const glm::u8vec3 &region_voxel_dimensions, MapFlag flags,
               const MapLayout &seed_layout);

  /// @overload
  explicit OccupancyMap(double resolution = 1.0, const glm::u8vec3 &region_voxel_dimensions = glm::u8vec3(0, 0, 0),
                        MapFlag flags = MapFlag::kDefault);

  /// @overload
  OccupancyMap(double resolution, MapFlag flags, const MapLayout &seed_layout);

  /// @overload
  OccupancyMap(double resolution, MapFlag flags);

  /// Destructor.
  ~OccupancyMap();

  // Iterator.
  /// Create an iterator to the first voxel in the map. The map should not have voxels added or removed
  /// during iterator.
  /// @return An @c iterator to the first voxel in the map, or an invalid iterator when empty.
  iterator begin();
  /// Create a read only iterator to the first voxel in the map. The map should not have voxels added or removed
  /// during iterator.
  /// @return An @c const_iterator to the first voxel in the map, or an invalid iterator when empty.
  const_iterator begin() const;

  /// Create an iterator representing the end of iteration. See standard iteration patterns.
  /// @return An invalid iterator for this map.
  iterator end();
  /// Create a read only iterator representing the end of iteration. See standard iteration patterns.
  /// @return An invalid iterator for this map.
  const_iterator end() const;

  /// Calculate the approximate memory usage of this map in bytes.
  /// @return The approximate memory usage (bytes).
  size_t calculateApproximateMemory() const;

  /// Get the voxel resolution of the occupancy map. Voxels are cubes.
  /// @return The leaf voxel resolution.
  double resolution() const;

  /// A cyclic stamp value which changes whenever the map is touched. May be used to note when the map has
  /// been changed from a previous stamp value.
  /// @return The current cyclic stamp value.
  uint64_t stamp() const;

  /// Touches the map, progressing the @c stamp() value.
  /// @return The @c stamp() value after the touch.
  uint64_t touch();

  /// Query the timestmap for the first ray in this map. This time is used to as a timebase reference for rays with
  /// timestamps allowing time values to be store relative to this time. This time is normally updated once they
  /// left unchanged. Changing this value will invalidate the touch time layer.
  /// @note Timestamps may be unavailable.
  /// @return The first ray timestamp or a negative value if not set.
  double firstRayTime() const;
  /// Set the @c firstRayTime() regardless of the current value.
  /// Not threadsafe.
  /// @param time The first ray timestamp.
  void setFirstRayTime(double time);
  /// Update the @c firstRayTime() setting it only if it has yet to be set.
  /// Not threadsafe.
  /// @param time The first ray timestamp.
  /// @return The @c firstRayTime()
  double updateFirstRayTime(double time);

  /// Query the spatial resolution of each @c MapRegion. This represents the spatial extents of each region.
  /// @return The spatial resolution of a @c MapRegion.
  glm::dvec3 regionSpatialResolution() const;

  /// Query the number of voxels in each region, split by axis.
  /// @return A vector identifying the number of voxels in each region along each respective axis.
  glm::u8vec3 regionVoxelDimensions() const;

  /// Query the total number of voxels per region.
  /// This is simply the product of the @p regionVoxelDimensions().
  /// @return The number of voxels in each region.
  size_t regionVoxelVolume() const;

  /// Calculate the minimum spatial coordinate for the region identified by @p region_key.
  ///
  /// The region need not be present in the map for this calculation.
  /// @param region_key The region of interest.
  /// @return The minimum spatial extent coordinate for the region.
  glm::dvec3 regionSpatialMin(const glm::i16vec3 &region_key) const;

  /// Calculate the maximum spatial coordinate for the region identified by @p region_key.
  ///
  /// The region need not be present in the map for this calculation.
  /// @param region_key The region of interest.
  /// @return The maximum spatial extent coordinate for the region.
  glm::dvec3 regionSpatialMax(const glm::i16vec3 &region_key) const;

  /// Calculate the spatial centre for the region identified by @p region_key.
  ///
  /// The region need not be present in the map for this calculation.
  /// @param region_key The region of interest.
  /// @return The coordinates of the spatial centre of the region.
  glm::dvec3 regionSpatialCentre(const glm::i16vec3 &region_key) const;

  /// Sets the map origin. All point references are converted to be relative to this origin.
  /// Changing the origin will effectively shift all existing voxels.
  /// @param origin The map origin.
  void setOrigin(const glm::dvec3 &origin);

  /// Gets the map origin.
  /// @return The map origin.
  const glm::dvec3 &origin() const;

  /// Calculate the extents of the map based on existing regions containing known data.
  /// @param[out] min_ext Set to the minimum corner of the axis aligned extents. May be nullptr.
  /// @param[out] max_ext Set to the maximum corner of the axis aligned extents. May be nullptr.
  /// @param[out] key_range The key range enclosing the key extents.
  /// @return True if the map is non-empty and resulting extents are valid. False for an empty map in which case the
  ///   out values are undefined.
  bool calculateExtents(glm::dvec3 *min_ext, glm::dvec3 *max_ext, KeyRange *key_range = nullptr) const;

  /// Calculate the extents of the map based on existing regions containing known data.
  /// @param[out] min_ext Set to the minimum corner of the axis aligned extents. May be nullptr.
  /// @param[out] max_ext Set to the maximum corner of the axis aligned extents. May be nullptr.
  /// @param[out] min_key Set to the voxel key of the minimum corner of the axis aligned extents. May be nullptr.
  /// @param[out] max_key Set to the voxel key of the maximum corner of the axis aligned extents. May be nullptr.
  /// @return True if the map is non-empty and resulting extents are valid. False for an empty map in which case the
  ///   out values are undefined.
  bool calculateExtents(glm::dvec3 *min_ext, glm::dvec3 *max_ext, Key *min_key, Key *max_key = nullptr) const;

  /// Access to the map info structure for storing general meta data.
  ///
  /// This structure is serialised with the map.
  ///
  /// Some values are reserved for heightmap. See @c Heightmap.
  ///
  /// @return The map info structure.
  MapInfo &mapInfo();

  /// @overload
  const MapInfo &mapInfo() const;

  /// Get the initialisation flags for the map.
  /// @return Initialisation flags.
  MapFlag flags() const;

  //-------------------------------------------------------
  // Region management.
  //-------------------------------------------------------

  /// May layout controlling layers.
  /// @return Layout details.
  const MapLayout &layout() const;

  /// May layout controlling layers. Note: define a custom layout the existing layout may need to be cleared, or
  // reset to only preserve the occupancy layer.
  /// @return Layout details.
  MapLayout &layout();

  /// Adds the voxel layer required to track voxel mean position (@c VoxelMean). This invalidates any existing
  /// @c Voxel or direct data references.
  ///
  /// Note note that changing the requires all map chunks have additional voxel layers allocated.
  ///
  /// Does nothing if already present.
  void addVoxelMeanLayer();

  /// Is voxel mean positioning enabled on this map?
  /// @return True if the @c VoxelMean layer is enabled.
  bool voxelMeanEnabled() const;

  /// Add the "traversal" layer to this map. This adds @c default_layer::traversalLayerName() containing one float
  /// per voxel. This value accumulates the distance travelled through the voxels by all previously intersected rays.
  ///
  /// Note note that changing the requires all map chunks have additional voxel layers allocated.
  ///
  /// Does nothing if the layer is already present.
  void addTraversalLayer();

  /// Is traversal calculation enabled on this map?
  /// @return True if the "traversal" layer is enabled.
  bool traversalEnabled() const;

  /// Add the "touch_time" layer to the map. See @c addTouchTime() . Does nothing if the layer is already present.
  void addTouchTimeLayer();

  /// Check if the "touch_time" layer exists.
  /// @return True if the "touch_time" layer is enabled.
  bool touchTimeEnabled() const;

  /// Add the "incident_normal" layer to the map. See @c addIncidentNormal() . Does nothing if the layer is already
  /// present.
  void addIncidentNormalLayer();

  /// Check if the "incident_normal" layer exists.
  /// @return True if the "incident_normal" layer is enabled.
  bool incidentNormalEnabled() const;

  /// Ensure a voxel layer called @p layer_name is present, invoking @p add_layer_function to add it if necessary.
  ///
  /// If the layer is not present, then a copy of the @c MapLayout is first made and modified by calling
  /// @p add_layer_function . The layer changes are committed using @c updateLayout() . It is assumed that
  /// @p add_layer_function only modifies the @c MapLayout by adding an appropriate layer matching @p layer_name .
  ///
  /// No action is performed if a voxel layer named @p layer_name is already present, under the assumption that there
  /// is only one valid function to use to add a layer matching @p layer_name .
  ///
  /// @param layer_name Name of the layer to be added.
  /// @param add_layer_function Function to invoke to add the layer if required.
  void addLayer(const char *layer_name, const std::function<void(MapLayout &)> &add_layer_function);

  /// Update the memory layout to match that in this map's @c MapLayout. Must be called after updating
  /// the @p layout() after construction.
  ///
  /// By default this will attempt to preserve all voxel layers which are equivalent between the two layouts (see
  /// @c MapLayer::checkEquivalent() ). Voxel layers not present in the new layer are destroyed, while new voxel
  /// layers are allocated. This may take some time to process.
  ///
  /// This behaviour may be modified, by setting @c preserve_map to @c false , in which case this call destroys the
  /// current map content.
  ///
  /// In both cases the GPU cache is invalidated.
  ///
  /// @param new_layout The map layout to update to.
  /// @param preserve_map Try to preserve the map content for equivalent layers?
  void updateLayout(const MapLayout &new_layout, bool preserve_map = true);

  /// Query the number of regions in the map which have been touched.
  /// @return The number of regions in the map.
  size_t regionCount() const;

  /// Expire @c MapRegion sections which have not been touched after @p timestamp.
  /// Such regions are removed from the map.
  ///
  /// This compares each @c MapRegion::touched_time against @p timestamp, removing regions
  /// with a @c touched_time before @p timestamp. The region touch time is updated via
  /// @c Voxel::touch(), @c touchRegionTimestamp() or @c touchRegionTimestampByKey().
  ///
  /// @param timestamp The reference time.
  /// @return The number of removed regions.
  unsigned expireRegions(double timestamp);

  /// Remove @c MapRegion chunks which are sufficiently far from @p relativeTo.
  ///
  /// Regions are removed if their centre is further than @p distance from @p relativeTo.
  ///
  /// @param relative_to The spatial reference point to measure distance relative to.
  /// @param distance The distance threshold.
  /// @return The number of removed regions.
  unsigned removeDistanceRegions(const glm::dvec3 &relative_to, float distance);

  /// Remove @c MapRegion chunks which do not overalp the given axis aligned box.
  ///
  /// @param min_extents The AABB minimum extents.
  /// @param max_extents The AABB minimum extents.
  /// @return The number of removed regions.
  unsigned cullRegionsOutside(const glm::dvec3 &min_extents, const glm::dvec3 &max_extents);

  /// Touch the @c MapRegion which contains @p point .
  /// @param point A spatial point from which to resolve a containing region. There may be border case issues.
  /// @param timestamp The timestamp to update the region touch time to.
  /// @param allow_create Create the region (all uncertain) if it doesn't exist?
  /// @see @c touchRegionTimestampByKey()
  inline void touchRegionTimestamp(const glm::vec3 &point, double timestamp, bool allow_create = false)
  {
    touchRegionTimestamp(voxelKey(point), timestamp, allow_create);
  }

  /// Touch the @c MapRegion which contains @p voxel_key .
  /// @param voxel_key A voxel key from which to resolve a containing region.
  /// @param timestamp The timestamp to update the region touch time to.
  /// @param allow_create Create the region (all uncertain) if it doesn't exist?
  /// @see @c touchRegionTimestampByKey()
  inline void touchRegionTimestamp(const Key &voxel_key, double timestamp, bool allow_create = false)
  {
    touchRegionTimestampByKey(voxel_key.regionKey(), timestamp, allow_create);
  }

  /// Touch the @c MapRegion identified by @p regionKey.
  ///
  /// If @p regionKey resolves to a valid @c MapRegion, then its @c MapRegion::touched_time is set
  /// to @p timestamp. Otherwise the call is ignored.
  ///
  /// @param region_key The key for the region.
  /// @param timestamp The timestamp to update the region touch time to.
  /// @param allow_create Create the region (all uncertain) if it doesn't exist?
  void touchRegionTimestampByKey(const glm::i16vec3 &region_key, double timestamp, bool allow_create = false);

  /// Returns the centre of the region identified by @p regionKey.
  ///
  /// The region may or may not be present in the map. This function simply calculates where this region would lie.
  ///
  /// @param region_key The region of interest.
  /// @return The centre of the region @p regionKey in global coordinates.
  glm::dvec3 regionCentreGlobal(const glm::i16vec3 &region_key) const;

  /// Returns the centre of the region identified by @p regionKey in map local coordinates.
  ///
  /// The region may or may not be present in the map. This function simply calculates where this region would lie.
  ///
  /// @param region_key The region of interest.
  /// @return The centre of the region @p regionKey in map local coordinates.
  glm::dvec3 regionCentreLocal(const glm::i16vec3 &region_key) const;

  /// Calculates the region key for the key containing @p point (global coordinates).
  /// @param point The global coordinate point of interest.
  /// @return The key of the region containing @p point.
  glm::i16vec3 regionKey(const glm::dvec3 &point) const;

  //-------------------------------------------------------
  // Probabilistic map functions.
  //-------------------------------------------------------

  /// Defines the value adjustment made to a voxel when integrating a hit into the map.
  /// This is equivalent to <tt>valueToProbability(hitProbability())</tt>.
  ///
  /// The value must be positive to represent an increase in probability.
  ///
  /// @return The value adjustment made to a voxel on recording a 'hit'.
  /// @see integrateHit()
  float hitValue() const;
  /// The probability adjustment made to a voxel when integrating a hit into the map.
  /// @return The probability adjustment represented by a 'hit' in the map.
  /// @see integrateHit()
  float hitProbability() const;
  /// Sets the voxel occupancy probability used to represent a single hit in the map.
  ///
  /// The probability of voxel occupancy and the accumulation of such is equivalent to the
  /// logic used by Octomap, which serves as good reference material.
  ///
  /// Note the @p probabilty must be at least 0.5 for well define behaviour. Otherwise each hit
  /// will actually decrease the occupancy probability of a voxel.
  ///
  /// Probability do not affect already populated voxels in the map, only future updates.
  ///
  /// @param probability The new probability associated with a hit. Should be in the range (0.5, 1.0].
  void setHitProbability(float probability);
  /// Set the voxel occupancy hit value.
  /// @param value New hit value; must be > 0 for well defined behaviour.
  /// @see setHitProbability()
  void setHitValue(float value);

  /// Defines the value adjustment made to a voxel when integrating a miss or free space into the map.
  /// This is equivalent to <tt>valueToProbability(missProbability())</tt>.
  ///
  /// The value must be negative to represent an decrease in probability.
  ///
  /// @return The value adjustment made to a voxel on recording a 'hit'.
  /// @see integrateMiss()
  float missValue() const;
  /// The probability adjustment made to a voxel when integrating a miss into the map.
  /// @return The probability adjustment represented by a 'miss' in the map.
  /// @see integrateMiss()
  float missProbability() const;
  /// Sets the voxel occupancy probability used to represent a single miss in the map.
  ///
  /// The probability of voxel occupancy and the accumulation of such is equivalent to the
  /// logic used by Octomap, which serves as good reference material.
  ///
  /// Note the @p probabilty must be less than 0.5 for well define behaviour. Otherwise each miss
  /// will actually increase the occupancy probability of a voxel.
  ///
  /// Probability do not affect already populated voxels in the map, only future updates.
  ///
  /// @param probability The new probability associated with a miss. Should be in the range [0, 0.5).
  void setMissProbability(float probability);
  /// Set the voxel occupancy miss value.
  /// @param value New miss value; must be < 0 for well defined behaviour.
  /// @see setMissProbability()
  void setMissValue(float value);

  /// Get threshold value at which a voxel is considered occupied.
  /// This is equivalent to <tt>valueToProbability(occupancyThresholdProbability())</tt>.
  /// Use of this value is not generally recommended as @c occupancyThresholdProbability() provides
  /// a more meaningful value.
  /// @return The value at which a voxel is considered occupied.
  float occupancyThresholdValue() const;
  /// Get threshold probability at which a voxel is considered occupied.
  /// @return The probabilty [0, 1] at which a voxel is considered occupied.
  float occupancyThresholdProbability() const;
  /// Set the threshold at which to considere a voxel occupied.
  ///
  /// Setting a value less than 0.5 is not recommended as this can include "miss" results integrated
  /// into the map.
  ///
  /// @param probability The new occupancy threshold [0, 1].
  void setOccupancyThresholdProbability(float probability);

  /// The minimum value a voxel can have. Value adjustments are clamped to this minimum.
  /// @return The minimum voxel value.
  float minVoxelValue() const;

  /// Set the minimum value a voxel can have. Value adjustments are clamped to this minimum.
  /// Changing the minimum value does not affect any existing voxels with smaller values.
  /// @param value The new minimum value.
  void setMinVoxelValue(float value);

  /// Set the minimum value for a voxel expressed as a probability.
  /// @see @c setMinVoxelValue
  /// @param probability The minimum probability value allowed.
  void setMinVoxelProbability(float probability);

  /// Get the minimum value for a voxel expressed as a probability.
  /// @return The minimum voxel occupancy probability.
  float minVoxelProbability() const;

  /// Do voxels saturate at the minimum value, preventing further adjustment?
  /// @return True if voxels saturate and maintain state at the minimum value.
  bool saturateAtMinValue() const;

  /// Set the voxel minimum value saturation value. Saturation behaviour can be circumvented
  /// using some value adjustment functions.
  /// @param saturate True to have voxels prevent further value adjustments at the minimum value.
  void setSaturateAtMinValue(bool saturate);

  /// The maximum value a voxel can have. Value adjustments are clamped to this maximum.
  /// @return The maximum voxel value.
  float maxVoxelValue() const;

  /// Set the maximum value a voxel can have. Value adjustments are clamped to this maximum.
  /// Changing the maximum value does not affect any existing voxels with larger values.
  /// @param value The new maximum value.
  void setMaxVoxelValue(float value);

  /// Set the maximum value for a voxel expressed as a probability.
  /// @see @c setMinVoxelValue
  /// @param probability The maximum probability value allowed.
  void setMaxVoxelProbability(float probability);

  /// Get the maximum value for a voxel expressed as a probability.
  /// @return The maximum voxel occupancy probability.
  float maxVoxelProbability() const;

  /// Do voxels saturate at the maximum value, preventing further adjustment?
  /// @return True if voxels saturate and maintain state at the maximum value.
  bool saturateAtMaxValue() const;

  /// Set the voxel maximum value saturation value. Saturation behaviour can be circumvented
  /// using some value adjustment functions.
  /// @param saturate True to have voxels prevent further value adjustments at the minimum value.
  void setSaturateAtMaxValue(bool saturate);

  //-------------------------------------------------------
  // General map manipulation.
  //-------------------------------------------------------

  /// Retrieve the coordinates for the centre of the voxel identified by @p key local to the map origin.
  ///
  /// @param key The voxel of interest.
  /// @return The voxel coordinates, relative to the map @c origin().
  glm::dvec3 voxelCentreLocal(const Key &key) const;

  /// Calculate a voxel centre position.
  ///
  /// This is used by @c voxelCentreLocal() as `voxelCentre(key, map.voxelResolution())` and @c voxelCentreGlobal() as
  /// follows:
  ///
  /// @code
  /// void voxelCentres(const ohm::OccupancyMap &map, const ohm::Key &key,
  ///                   glm::dvec3 *local_centre, glm::dvec3 *global_centre)
  /// {
  ///   // voxelCentreLocal() equivalent call.
  ///   *local_centre = ohm::OccupancyMap::voxelCentre(key, map.resolution(), map.regionSpatialResolution());
  ///   // voxelCentreGlobal() equivalent call.
  ///   *global_centre = ohm::OccupancyMap::voxelCentre(key, map.resolution(), map.regionSpatialResolution(),
  ///                                                   map.origin());
  /// }
  /// @endcode
  ///
  ///
  /// @param key Voxel key for which to resolve the voxel centre.
  /// @param voxel_resolution The size of the voxels. From @c resolution() .
  /// @param region_spatial_dimensions The spatial dimensions of each @c MapRegion . From @c regionSpatialResolution() .
  /// @param map_origin The origin of the occupancy map. From @c origin() .
  static inline glm::dvec3 voxelCentre(const Key &key, double voxel_resolution,
                                       const glm::dvec3 &region_spatial_dimensions,
                                       const glm::dvec3 &map_origin = glm::dvec3(0.0))
  {
    glm::dvec3 centre;
    // Region centre
    centre = glm::vec3(key.regionKey());
    // Note: converting imp_->region_spatial_dimensions to glm::vec3 then multiplying to vec3 values resulted in
    // additional floating point error. The following component wise multiplication of float/int generates better
    // values.
    centre.x *= region_spatial_dimensions.x;
    centre.y *= region_spatial_dimensions.y;
    centre.z *= region_spatial_dimensions.z;
    // Offset to the lower extents of the region.
    centre -= 0.5 * region_spatial_dimensions;
    // Map offset.
    centre += map_origin;
    // Local offset.
    centre += glm::dvec3(key.localKey()) * voxel_resolution;
    centre += glm::dvec3(0.5 * voxel_resolution);
    return centre;
  }

  /// Retrieve the global coordinates for the centre of the voxel identified by @p key. This includes
  /// the map @c origin().
  ///
  /// @param key The voxel of interest.
  /// @return The global voxel coordinates.
  glm::dvec3 voxelCentreGlobal(const Key &key) const;

  /// Convert a global coordinate to the key value for the containing voxel.
  /// The voxel may not currently exist in the map.
  /// @param point A global coordinate to convert.
  /// @return The @c Key for the voxel containing @p point.
  Key voxelKey(const glm::dvec3 &point) const;

  /// @overload
  Key voxelKey(const glm::vec3 &point) const;

  /// Convert a local coordinate to the key value for the containing voxel.
  /// @param local_point A map local coordinate (relative to @c origin()) to convert.
  /// @return The @c Key for the voxel containing @p localPoint.
  Key voxelKeyLocal(const glm::vec3 &local_point) const;

  /// Move an @c Key along a selected axis.
  ///
  /// This function moves @p key along the selected @p axis. The direction and
  /// magnitude of the movement is determined by the sign and magnitude of @p step.
  /// Valid @p axis values are [0, 1, 2] corresponding to the X, Y, Z axes respectively.
  ///
  /// For example, to move @p key to it's previous neighbour along the X axis, call:
  /// <tt>moveKeyAlongAxis(key, 0, -1)</tt>. To move a key 16 voxels along the Y axis,
  /// call <tt>moveKeyAlongAxis(key, 1, 16)</tt>.
  ///
  /// Use of this method ensures the @p key region is updated as required when at the limits
  /// of the local indexing.
  ///
  /// @param key The key to move.
  /// @param axis The axis to move along: [0, 1, 2] mapping to X, Y, Z respectively.
  /// @param step Defines the step direction and magnitude along the selected @p axis.
  void moveKeyAlongAxis(Key &key, int axis, int step) const;

  /// Step the @p key along the @p axis in the given @p dir.
  ///
  /// This is a faster version of @c moveKeyAlongAxis() for when stepping just one voxel at a time.
  ///
  /// @param key The key to modify.
  /// @param axis The axis to modify. Must be [0, 2] mapping to XYZ respectively, or behaviour is undefined.
  /// @param dir Direction to step. Must be 1 or -1 or behaviour is undefined.
  /// @param region_voxel_dimensions The number of voxels in each region given per axis. See @c regionVoxelDimensions().
  static inline void stepKey(Key &key, int axis, int dir, const glm::ivec3 &region_voxel_dimensions)
  {
    int local_key = key.localKey()[axis] + dir;
    int region_key = key.regionKey()[axis];

    if (local_key < 0)
    {
      --region_key;
      local_key = region_voxel_dimensions[axis] - 1;
    }
    else if (local_key >= region_voxel_dimensions[axis])
    {
      ++region_key;
      local_key = 0;
    }

    key.setLocalAxis(axis, uint8_t(local_key));
    key.setRegionAxis(axis, uint16_t(region_key));
  }

  /// @overload
  void stepKey(Key &key, int axis, int dir) const;

  /// Move an @c Key by a given offset.
  ///
  /// This function moves @p key by the given @p x, @p y, @p z voxel offsets.
  /// Each of the offsets determine the direction and magnitude of movement along the
  /// X, Y and Z axes respectively.
  ///
  /// For example, to move @p key to it's previous neighbour along the X axis, call:
  /// <tt>moveKey(key, -1, 0, 0)</tt>. To move a key 16 voxels along the Y axis and -2
  /// voxels along Z, call <tt>moveKey(key, 0, 16, -2)</tt>.
  ///
  /// Use of this method ensures the @p key region is updated as required when at the limits
  /// of the local indexing.
  ///
  /// @param key The key to move.
  /// @param x The voxel offset to apply to @p key on the X axis.
  /// @param y The voxel offset to apply to @p key on the Y axis.
  /// @param z The voxel offset to apply to @p key on the X axis.
  void moveKey(Key &key, int x, int y, int z) const;

  /// @overload
  template <typename VecType>
  inline void moveKey(Key &key, const VecType &v) const
  {
    moveKey(key, v.x, v.y, v.z);
  }

  /// Calculate the number of voxels along each axis between two keys. Semantically, <tt>b - a</tt>.
  /// @param from The first key.
  /// @param to The second key.
  /// @return The voxel offset from @p from to @p to along each axis.
  glm::ivec3 rangeBetween(const Key &from, const Key &to) const;

  /// A static overload which requires the region dimensions in voxels for a region.
  /// @param from The first key.
  /// @param to The second key.
  /// @param region_voxel_dimensions The number of voxels in each region given per axis. See @c regionVoxelDimensions().
  /// @return The voxel offset from @p from to @p to along each axis.
  static inline glm::ivec3 rangeBetween(const Key &from, const Key &to, const glm::ivec3 &region_voxel_dimensions)
  {
    // First diff the regions.
    const glm::ivec3 region_diff = to.regionKey() - from.regionKey();
    glm::ivec3 voxel_diff;

    // Voxel difference is the sum of the local difference plus the region step difference.
    for (int i = 0; i < 3; ++i)
    {
      voxel_diff[i] =
        int(to.localKey()[i]) - int(from.localKey()[i]) + region_diff[i] * int(region_voxel_dimensions[i]);
    }

    return voxel_diff;
  }

  /// Set the range filter applied to all rays to be integrated into the map. @c RayMapper implementations must
  /// respect this filter in @c RayMapper::integrateRays() .
  /// @param ray_filter The range filter to install and filter rays with. Accepts an empty, which clears the filter.
  void setRayFilter(const RayFilterFunction &ray_filter);

  /// Get the range filter applied to all rays given to @c integrateRays().
  /// The map starts with a filter configured to remove any infinite or NaN points by rejecting rays longer than
  /// @c 1e10.
  /// @return The current ray filter.
  const RayFilterFunction &rayFilter() const;

  /// Clears the @c rayFilter().
  void clearRayFilter();

  /// Integrate the given @p rays into the map. The @p rays form a list of origin/sample pairs for which
  /// we generally consider the sample voxel as a hit when (increasing occupancy) and all other voxels as misses
  /// (free space). The sample may be treated as a miss when @p endPointsAsOccupied is false.
  ///
  /// @param rays Array of origin/sample point pairs.
  /// @param element_count The number of points in @p rays. The ray count is half this value.
  /// @param intensities An array of intensity values matching the @p rays items. There is one intensity value per ray
  ///   so there are @c element_count/2 items. May be null to omit intensity values.
  /// @param timestamps An array of timestamp values matching the @p rays items. There is one timestamp value per ray
  ///   so there are @c element_count/2 items. May be null to omit timestamp values in which case the touch time layer
  ///   will not be updated.
  /// @param ray_update_flags Flags controlling ray integration behaviour. See @c RayFlag.
  void integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities = nullptr,
                     const double *timestamps = nullptr, unsigned ray_update_flags = kRfDefault);

  //-------------------------------------------------------
  // Data copy/cloning
  //-------------------------------------------------------

  /// Clone the entire map.
  /// @return A deep clone of this map. Caller takes ownership.
  OccupancyMap *clone() const;

  /// Clone the map within the given extents.
  ///
  /// This creates a deep clone of this may, copying only regions which overlap the given extents.
  /// Note that any region which partially overmaps the extents is copied in its entirety.
  ///
  /// @param min_ext The minimum spatial extents to over.
  /// @param max_ext The maximum spatial extents to over.
  /// @return A deep clone of this map. Caller takes ownership.
  OccupancyMap *clone(const glm::dvec3 &min_ext, const glm::dvec3 &max_ext) const;

  //-------------------------------------------------------
  // Internal
  //-------------------------------------------------------

  /// Internal occupancy map detail access.
  /// @return Internal map details.
  inline OccupancyMapDetail *detail() { return imp_; }
  /// Internal occupancy map detail access.
  /// @return Internal map details.
  inline const OccupancyMapDetail *detail() const { return imp_; }

  /// Enumerate the regions within this map.
  /// @param[out] chunks The enumerated chunks are added to this container.
  void enumerateRegions(std::vector<const MapChunk *> &chunks) const;

  /// Fetch a region, potentially creating it. For internal use.
  /// @param region_key The key of the region to fetch.
  /// @param allow_create Create the region if it doesn't exist?
  /// @return A pointer to the requested region. Null if it doesn't exist and @p allowCreate is @c false.
  MapChunk *region(const glm::i16vec3 &region_key, bool allow_create = false);

  /// @overload
  const MapChunk *region(const glm::i16vec3 &region_key) const;

  /// Populate @c regions with a list of regions who's touch stamp is greater than the given value.
  ///
  /// Adds to @p regions without clearing it, thus there may be redundancy.
  ///
  /// @param from_stamp The map stamp value from which to fetch regions.
  /// @param regions The list to add to.
  unsigned collectDirtyRegions(uint64_t from_stamp, std::vector<std::pair<uint64_t, glm::i16vec3>> &regions) const;

  /// Experimental: calculate the extents of regions which have been changed since @c from_stamp .
  /// @param from_stamp The base stamp used to determine dirty regions.
  /// @param min_ext The region key which identifies the minimum extents of the dirty regions.
  /// @param max_ext The region key which identifies the maximum extents of the dirty regions.
  /// @return The most up to date stamp value for the dirty regions.
  uint64_t calculateDirtyExtents(uint64_t from_stamp, glm::i16vec3 *min_ext, glm::i16vec3 *max_ext) const;

  /// Experimental: calculate the dirty region extents for the clearance layer.
  /// @param min_ext The region key which identifies the minimum extents of the dirty regions.
  /// @param max_ext The region key which identifies the maximum extents of the dirty regions.
  /// @param region_padding Number of regions to pad the returned extents.
  void calculateDirtyClearanceExtents(glm::i16vec3 *min_ext, glm::i16vec3 *max_ext, unsigned region_padding = 0) const;

  /// Clear the map content and release map memory.
  void clear();

private:
  Key firstIterationKey() const;
  MapChunk *newChunk(const Key &for_key);
  static void releaseChunk(const MapChunk *chunk);

  /// Culling function for @c cullRegions().
  using RegionCullFunc = std::function<bool(const MapChunk &)>;

  /// Remove regions/chunks for which @c cull_func returns true.
  /// @param cull_func The culling criteria.
  /// @return The number of regions removed.
  unsigned cullRegions(const RegionCullFunc &cull_func);

  OccupancyMapDetail *imp_;
};

inline void OccupancyMap::setMinVoxelProbability(float probability)
{
  setMinVoxelValue(probabilityToValue(probability));
}

inline float OccupancyMap::minVoxelProbability() const
{
  return valueToProbability(minVoxelValue());
}

inline void OccupancyMap::setMaxVoxelProbability(float probability)
{
  setMaxVoxelValue(probabilityToValue(probability));
}

inline float OccupancyMap::maxVoxelProbability() const
{
  return valueToProbability(maxVoxelValue());
}
}  // namespace ohm

#endif  // OCCUPANCYMAP_H


================================================
FILE: ohm/OccupancyType.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OccupancyType.h"

#include <array>

namespace ohm
{
const char *occupancyTypeToString(int occupancy_type)
{
  static const std::array<const char *, 4> type_names =  //
    {
      "null",       //
      "uncertain",  //
      "free",       //
      "occupied"    //
    };

  const int index = occupancy_type - ohm::kNull;
  if (index >= 0 && unsigned(index) < type_names.size())
  {
    return type_names[index];
  }

  return "<unknown>";
}
}  // namespace ohm


================================================
FILE: ohm/OccupancyType.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OCCUPANCYTYPE_H
#define OCCUPANCYTYPE_H

#include "OhmConfig.h"

namespace ohm
{
/// An enumeration of the types of @c Voxel states available.
enum OccupancyType
{
  /// Invalid/null voxel.
  kNull = -2,
  /// Unobserved: no data recorded or available for the voxel.
  kUnobserved = -1,
  /// Know to be empty or free (traversable).
  kFree = 0,
  /// Occupied voxel.
  kOccupied = 1
};

const char ohm_API *occupancyTypeToString(int occupancy_type);
}  // namespace ohm

#endif  // OCCUPANCYTYPE_H_


================================================
FILE: ohm/OccupancyUtil.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OccupancyUtil.h"

#ifdef TES_ENABLE
namespace ohm
{
tes::Server *g_tes = nullptr;
}  // namespace ohm
#endif  // TES_ENABLE


================================================
FILE: ohm/OccupancyUtil.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OCCUPANCYUTIL
#define OCCUPANCYUTIL

#include "OhmConfig.h"

#include "Key.h"

#include <iostream>

#include <glm/glm.hpp>


namespace ohm
{
// From GLM 0.9.9.1, numbering starting including a patch number.
// So 0.9.8 was version 98, 0.9.9 was 99 and 0.9.9.1 was 991
// This assumes version 1.0 will be 1000
#if GLM_VERSION < 99
using GlmQualifier = glm::precision;
#else   // GLM_VERSION
using GlmQualifier = glm::qualifier;
#endif  // GLM_VERSION

template <typename T, GlmQualifier QUALITY>
inline T volumeOf(const glm::tvec3<T, QUALITY> &expanse)
{
  return expanse.x * expanse.y * expanse.z;
}
}  // namespace ohm

#endif  // OCCUPANCYUTIL


================================================
FILE: ohm/OhmConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMCONFIG_H
#define OHMCONFIG_H

#include "OhmExport.h"

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

#include <cmath>

// clang-format off

// Enable various validation tests throughout this library.
#cmakedefine OHM_VALIDATION
#cmakedefine OHM_FEATURE_THREADS
#cmakedefine OHM_PROFILE
#cmakedefine OHM_EMBED_GPU_CODE
#cmakedefine OHM_FEATURE_EIGEN

#ifdef OHM_PROFILE
#define PROFILING 1
#endif  // OHM_PROFILE

#cmakedefine TES_ENABLE
#ifdef TES_ENABLE
namespace tes
{
class Server;
}  // namespace tes
namespace ohm
{
/// Debug visualisation server pointer. Must be set by the executable to enable its use in this library.
/// That is, this is considered a borrowed pointer in this library.
extern tes::Server ohm_API *g_tes;  // Symbol defined in occupancyutil.cpp.
}  // namespace ohm
#endif  // TES_ENABLE

#include <memory>

/// Enable experimental parts of GLM, like `glm::length2()` (length squared)
#define GLM_ENABLE_EXPERIMENTAL

// clang-format on
#endif  // OHMCONFIG_H


================================================
FILE: ohm/Query.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "Query.h"

#include "private/QueryDetail.h"

#include "QueryFlag.h"

namespace ohm
{
Query::Query(QueryDetail *detail)
  : imp_(detail)
{
  if (!imp_)
  {
    imp_ = new QueryDetail;
  }
}


Query::~Query()
{
  delete imp_;
  imp_ = nullptr;
}


const OccupancyMap *Query::map() const
{
  return imp_->map;
}


void Query::setMap(OccupancyMap *map)
{
  imp_->map = map;
  onSetMap();
}


unsigned Query::queryFlags() const
{
  return imp_->query_flags;
}


void Query::setQueryFlags(unsigned flags)
{
  imp_->query_flags = flags;
}


size_t Query::numberOfResults() const
{
  return imp_->number_of_results;
}


const Key *Query::intersectedVoxels() const
{
  return (!imp_->intersected_voxels.empty()) ? imp_->intersected_voxels.data() : nullptr;
}


const double *Query::ranges() const
{
  return (!imp_->ranges.empty()) ? imp_->ranges.data() : nullptr;
}


bool Query::execute()
{
  reset(false);
  return onExecute();
}


bool Query::executeAsync()
{
  return onExecuteAsync();
}


void Query::reset(bool hard_reset)
{
  wait();
  imp_->intersected_voxels.clear();
  imp_->ranges.clear();
  imp_->number_of_results = 0u;
  onReset(hard_reset);
}


bool Query::wait(unsigned timeout_ms)
{
  return onWaitAsync(timeout_ms);
}


bool Query::onWaitAsync(unsigned /*timeout_ms*/)
{
  return false;
}
}  // namespace ohm


================================================
FILE: ohm/Query.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OCCUPANCYQUERY_H
#define OCCUPANCYQUERY_H

#include "OhmConfig.h"

#include <cstddef>

namespace ohm
{
class OccupancyMap;
class Key;
struct QueryDetail;

/// Base class for a query operation on an @p OccupancyMap.
///
/// Typical usage is to setup the derived query, then call @c exec() or @c execAsync() and @c wait().
/// Results may be available via methods such as @c numberOfResults() and @p intersectedVoxels(). Query behaviour may
/// be modified by setting various @c QueryFlag values in @c setQueryFlags(), although not all flags are honoured by
/// all query implementations.
///
/// Most queries operate on searching for occupied voxels, as defined by @c OccupancyMap::occupancyType(). By default
/// only voxels of type @c OccupancyType::kOccupied are considered occupied. For most queries, setting the query flag
/// @c QF_UnknownAsOccupied ensures that @c OccupancyType::kUnobserved are also considered as relevant obstructions.
/// We define the set of such relevant voxels as the set of @em obstructed voxels.
///
/// @todo Create a base class for the @c Query called @c MapOperation. @c Query adds the concept of results, which
/// @c MapOperation does not have.
class ohm_API Query
{
protected:
  /// Constructor. The @p detail is stored in @p imp_, allowing derived classes to a allocate
  /// derived detail structure. When null, the base implementation is allocated by this constructor.
  /// @param detail The @c Query data detail structure.
  explicit Query(QueryDetail *detail = nullptr);

public:
  /// Virtual destructor.
  virtual ~Query();

  /// Get the map the query has been associated with.
  /// @return The target map.
  const OccupancyMap *map() const;

  /// Set the map to operate on.
  /// @param map The map to query.
  void setMap(OccupancyMap *map);

  /// Access the @c QueryFlag bits set for this query.
  /// @return The @c QueryFlag set for this query.
  unsigned queryFlags() const;

  /// Set the @c QueryFlag bits for this query.
  /// @param flags The new @c QueryFlag value set for this query.
  void setQueryFlags(unsigned flags);

  /// Query the number of available results.
  ///
  /// This affects the number of items available from methods such as @p intersectedVoxel().
  /// It is valid after execution completes until the next call to @p exec() or @c execAsync().
  ///
  /// @return The number of available results.
  size_t numberOfResults() const;

  /// Query the voxels intersected by the query, where voxels are identified by their @c Key.
  ///
  /// When non-null, the number of items available in @p intersectedVoxels() is available via
  /// @c numberOfResults().
  ///
  /// Sort order and semantics may vary slightly for specific queries.
  ///
  /// @return A pointer to the array of voxels intersected by the query.
  const Key *intersectedVoxels() const;

  /// Get the array of distances associated with the @c intersectedVoxels().
  /// This may not be valid for some queries and could return null.
  ///
  /// When non-null, the array contains @c numberOfResults() elements, where each element corresponds
  /// to a voxel reported by @c intersectedVoxels().
  ///
  /// @return The array of distances to the line segment for each @c intersectedVoxel().
  const double *ranges() const;

  /// Execute a synchronous query.
  ///
  /// This calls through to the implementation in @p onExecute().
  ///
  /// @return True on success.
  bool execute();

  /// Execute an asynchronous query.
  ///
  /// This calls through to the implementation in @p onExecuteAsync(). On success, completion
  /// can be synchronised by calling @p wait() with an optional wait timeout.
  ///
  /// The method will fail when already executing a query.
  ///
  /// @return True on successfully starting query execution.
  bool executeAsync();

  /// Wait for/terminate any asynchronous query and clear results data. This will also wait
  /// for any oustanding asynchronous query.
  ///
  /// Some queries may differentiate between a hard and soft reset (@p hardReset @c true or @c false
  /// respectively). In a hard reset, all information from the previous query are discraded. In a soft
  /// reset, some data may be cached. For example, @c VoxelRanges supports a soft reset where it will
  /// not recalculate obstacle ranges for recently queried regions.
  ///
  /// @param hard_reset True for a hard reset, false for a soft reset.
  void reset(bool hard_reset = true);

  /// Wait for an asynchronous query to complete.
  ///
  /// @param timeout_ms Maximum amount of time to wait for completion (milliseconds).
  /// @return True if on return there is no asynchronous query running. This does not mean that there was
  ///   one running to begin with.
  bool wait(unsigned timeout_ms = ~0u);

protected:
  /// Virtual call for when a map is set.
  /// This is only called from @p setMap(), not from the constructor.
  virtual inline void onSetMap() {}

  /// Virtual function called to execute the query from @c execute().
  ///
  /// Derived classes must implement a synchronous query in this function returning only on failure or once
  /// the query completes. Results must be available by the end of a successful call.
  ///
  /// @return True on successfully completing a query.
  virtual bool onExecute() = 0;

  /// Virtual function called to execute the query asynchronously from @c executeAsync().
  ///
  /// Derived classes must implement an asynchronous query in this function returning only on failure or once
  /// the query has started. The call must fail when there is already an asynchronous query running from this
  /// instance.
  ///
  /// @return True on successfully starting an asynchronous query.
  virtual bool onExecuteAsync() = 0;

  /// Wait for an asynchronous query to complete.
  /// @param timeout_ms Maximum time to wait (milliseconds) - zero to wait indefinitely.
  /// @return True if the query completed before the timeout.
  /// @todo (KS): make pure virtual
  virtual bool onWaitAsync(unsigned timeout_ms);

  /// Called from @c reset(bool hardReset) to complete or terminate any
  /// outstanding asynchronous query and clear results.
  /// @param hard_reset True for a hard reset, false for a soft reset.
  virtual void onReset(bool hard_reset) = 0;

  QueryDetail *imp_;  ///< Internal implementation details.
};
}  // namespace ohm

#endif  // OCCUPANCYQUERY_H


================================================
FILE: ohm/QueryFlag.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMQUERYFLAG_H
#define OHMQUERYFLAG_H

#include "OhmConfig.h"

namespace ohm
{
// Disable clang-format so it doesn't split the table in the comments below
// clang-format off

  /// Flags controlling the behaviour of queries.
  ///
  /// Some queries have up to four modes of operation relating to CPU and GPU base calculations.
  /// This relates to where the calculations are performed, CPU or GPU, and whether cached values may be used.
  /// Caching primarily relates to whether data stored in different voxel layers may be used or must be
  /// recalculated. Specific behaviour may vary, but the following table defines the general expected behaviour
  /// based on the flags @c QF_GpuEvaluate and @c QF_NoCache.
  ///
  /// Flags                 | Behaviour
  /// --------------------- | ----------------------------------------------------------------------------------------
  /// Neither               | All query calculations performed in CPU and may used cached or precalculated values.
  /// @c QF_GpuEvaluate     | Invoke the required GPU calculation before collating the results. Cached values may be used and only dirty regions are recalculated.
  /// @c QF_NoCache         | CPU is used to calculate the requested values regardless of whether cached values are available or not.
  /// @c QF_GpuEvaluate, @c QF_NoCache | GPU used to forcibly recalculate affected data even if not dirty. No cached values are used.
  ///
  /// Note that these two flags are essentially mutually exclusive and setting both may result in undefined behaviour.
  enum QueryFlag : unsigned
  {
  // clang-format on

  /// Zero flag value for completeness. Treat unknown voxels as occupied. Otherwise they are considered free.
  kQfZero = 0,
  kQfUnknownAsOccupied = (1u << 0u),
  /// Only report a single result, choosing the closest result.
  kQfNearestResult = (1u << 1u),
  /// Request GPU usage for a query.
  kQfGpuEvaluate = (1u << 2u),

  /// Do not allow use of cached values. Some queries support using cached data either in the query itself
  /// or in the voxel layout. Setting this flag forces the re-evaluation of data regardless of whether or
  /// not cached data may be up to date. When disabled, queries should recalculate only dirty regions.
  kQfNoCache = (1u << 3u),

  /// Report ranges without applying scaling, but calculate with scaling. Used in range based queries.
  kQfReportUnscaledResults = (1u << 4u),

  /// Represents the first specialised or non generic query flag. May be extended by specific queries.
  kQfSpecialised = (1u << 16u),

  // Legacy flag for backwards compatibility
  kQfGpu = kQfGpuEvaluate,

  /// The default query flags applied.
  kQfDefaultFlags = kQfUnknownAsOccupied
};
}  // namespace ohm

#endif  // OHMQUERYFLAG_H


================================================
FILE: ohm/RayFilter.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RayFilter.h"

#include <Aabb.h>

namespace ohm
{
bool goodRay(const glm::dvec3 &start, const glm::dvec3 &end, double max_range)
{
  bool is_good = true;
  is_good = is_good && !glm::any(glm::isnan(start)) && !glm::any(glm::isinf(start));
  is_good = is_good && !glm::any(glm::isnan(end)) && !glm::any(glm::isinf(end));

  const glm::dvec3 ray = end - start;
  is_good = is_good && (max_range <= 0 || glm::dot(ray, ray) <= max_range * max_range);

  return is_good;
}


bool goodRayFilter(glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags, double max_range)
{
  if (goodRay(*start, *end, max_range))
  {
    return true;
  }

  *filter_flags |= kRffInvalid;
  return false;
}


bool clipRayFilter(glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags, double max_length)
{
  bool is_good = true;
  is_good = is_good && !glm::any(glm::isnan(*start)) && !glm::any(glm::isinf(*start));
  is_good = is_good && !glm::any(glm::isnan(*end)) && !glm::any(glm::isinf(*end));

  glm::dvec3 ray = *end - *start;
  const double ray_length_sqr = glm::dot(ray, ray);
  if (is_good && max_length > 0 && ray_length_sqr > max_length * max_length)
  {
    // Ray is good, but too long. Clip it.
    // Normalise ray.
    ray /= std::sqrt(ray_length_sqr);
    // Clip and mark
    *end = *start + ray * max_length;
    *filter_flags |= kRffClippedEnd;
  }

  *filter_flags |= !is_good * kRffInvalid;
  return is_good;
}


bool clipBounded(glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags, const ohm::Aabb &clip_box)
{
  unsigned line_clip_flags = 0;
  if (clip_box.clipLine(*start, *end, &line_clip_flags))
  {
    if (!clip_box.contains(*start) && !clip_box.contains(*end))
    {
      return false;
    }
  }


  // Lint(KS): everything is unsigned.
  *filter_flags |= !!(line_clip_flags & Aabb::kClippedStart) * kRffClippedStart;  // NOLINT(hicpp-signed-bitwise)
  *filter_flags |= !!(line_clip_flags & Aabb::kClippedEnd) * kRffClippedEnd;      // NOLINT(hicpp-signed-bitwise)

  return true;
}


bool clipToBounds(glm::dvec3 * /*start*/, glm::dvec3 *end, unsigned *filter_flags, const ohm::Aabb &clip_box)
{
  const bool clipped = clip_box.contains(*end);
  // Lint(KS): everything is unsigned.
  *filter_flags |= !!(clipped)*kRffClippedEnd;  // NOLINT(hicpp-signed-bitwise)
  return true;
}
}  // namespace ohm


================================================
FILE: ohm/RayFilter.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef RAYFILTER_H
#define RAYFILTER_H

#include "OhmConfig.h"

#include <glm/fwd.hpp>

#include <functional>

namespace ohm
{
class Aabb;


/// Flags used with @c RayFilterFunction.
enum RayFilterFlag
{
  /// Ray is invalid: should be skipped.
  kRffInvalid = (1u << 0u),
  /// Sample ray start point has been clipped (and moved).
  kRffClippedStart = (1u << 1u),
  /// Sample ray end point has been clipped (and moved). Prevents the end point from being treated as a hit result.
  kRffClippedEnd = (1u << 2u),
};

/// Function prototype used in ray filtering for @c OccupancyMap::integrateRays().
///
/// When installed, each ray is passed ot the @c RayFilterFunction and the function may modify or cull the ray.
///
/// A ray is culled (thereby omitted from the map) when this function returns false. When returning true, the ray
/// @c start and @c end points may be left as is, or may be modified first. When modifying the points, it is critical
/// to adjust the @c filter_flags appropriately. Set @c kRffClippedStart when @c start is modified and set
/// @c kRffClippedEnd when end is modified *and* no longer falls in the sample voxel (or if it should not be treated
/// as a sample).
///
/// @param start A pointer to the sample ray start coordinate. May be modified.
/// @param end A pointer to the sample ray end coordinate. May be modified.
/// @param filter_flags A pointer to the flags. May be added to using flags from @c RayFilterFlag.
/// @return True if the sample ray may be processed, false to skip this sample.
using RayFilterFunction = std::function<bool(glm::dvec3 *, glm::dvec3 *, unsigned *)>;

/// A helper function to validate a sample ray. Fails on rays which have start or end point as infinite or NaN or
/// if the ray length exceeds @p max_range (skipped if @p max_range <= 0).
/// @param start The sample ray start coordinate.
/// @param end The sample ray end coordinate.
/// @param max_range Optional maximum range limit for the ray.
/// @return True if the ray is ok to be included.
bool ohm_API goodRay(const glm::dvec3 &start, const glm::dvec3 &end, double max_range = 0);

/// A wrapper around @c goodRay() for use with @c RayFilterFunction.
/// On failure, @c filter_flags has the @c kRffInvalid flag set.
///
/// This is the default ray filter applied.
///
/// @param start A pointer to the sample ray start coordinate. May be modified.
/// @param end A pointer to the sample ray end coordinate. May be modified.
/// @param filter_flags A pointer to the flags. May be added to using flags from @c RayFilterFlag.
/// @param max_range Optional maximum range limit for the ray.
/// @return True if the sample ray may be processed, false to skip this sample.
bool ohm_API goodRayFilter(glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags, double max_range);

/// A filter function for clipping rays to a given minimum/maximum range.
///
/// This may modify the ray @p end value to ensure `|end - start| <= max_length`. The @p filter_flags bit
/// @p kRffClippedEnd is set whenever @p end is modified.
///
/// Any bad rays (NaN or infinite) are set the @p filter_flag @c kRffInvalid and return false.
bool ohm_API clipRayFilter(glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags, double max_length);

/// A @c RayFilterFunction which clips rays a bounding box.
///
/// May set the folling bits of @p filter_flags:
/// - @c kRffClippedStart if @p start is modified.
/// - @c kRffClippedEnd if @p end is modified.
///
/// @param start A pointer to the sample ray start coordinate. May be modified.
/// @param end A pointer to the sample ray end coordinate. May be modified.
/// @param filter_flags A pointer to the flags. May be added to using flags from @c RayFilterFlag.
/// @param clip_box The axis aligned box to clip rays to.
/// @return True if the sample ray may be processed, false to skip this sample.
bool ohm_API clipBounded(glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags, const ohm::Aabb &clip_box);

/// A @c RayFilterFunction which test sample end points ensuring they do not lie within @c clip_box
///
/// May set the folling bits of @p filter_flags:
/// - @c kRffClippedEnd if @p start falls within @p clip_box.
///
/// Never modifies @p start or @p end and never returns false.
///
/// @param start A pointer to the sample ray start coordinate. May be modified.
/// @param end A pointer to the sample ray end coordinate. May be modified.
/// @param filter_flags A pointer to the flags. May be added to using flags from @c RayFilterFlag.
/// @param max_range Optional maximum range limit for the ray.
/// @return True
bool ohm_API clipToBounds(glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags, const ohm::Aabb &clip_box);

}  // namespace ohm

#endif  // RAYFILTER_H


================================================
FILE: ohm/RayFlag.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef RAYFLAG_H
#define RAYFLAG_H

// Used as a GPU header.

#if !GPUTIL_DEVICE
namespace ohm
{
#endif  // !GPUTIL_DEVICE
/// Flags affecting the behaviour of how rays are integrated into the map.
enum RayFlag
#if !GPUTIL_DEVICE
  // Unsigned type specification not valid for NVidia OpenCL code.
  : unsigned
#endif  // !GPUTIL_DEVICE
{
  /// Default behaviour.
  kRfDefault = 0,
  /// Change behaviour such that the end point is considered another free voxel, rather than occupied.
  kRfEndPointAsFree = (1u << 0u),
  /// Change behaviour such that traversal stops as soon as an occupied voxel is reached. Ray traversal terminates
  /// after adjusting the occupied voxel.
  kRfStopOnFirstOccupied = (1u << 1u),
  /// Skip the first, non-sample voxel in the ray. Useful for dealing with secondary returns where multiple samples
  /// lie along the same ray. This way, secondary samples can start at the previous/primary sample point without
  /// affecting the occupancy of this voxel. However, note that in this use case, the sensor voxel will also always be
  /// skipped.
  kRfExcludeOrigin = (1u << 2u),
  /// Do not process the sample voxel.
  kRfExcludeSample = (1u << 3u),
  /// Exclude the ray part, integrating only the sample. This flag is only recommended in debugging or validation.
  /// @c RayMapperBase code is not optimised for this flag.
  kRfExcludeRay = (1u << 4u),
  /// Do not adjust the occupancy value of currently unobserved voxels.
  kRfExcludeUnobserved = (1u << 5u),
  /// Do not adjust the occupancy value of currently free voxels.
  kRfExcludeFree = (1u << 6u),
  /// Do not adjust the occupancy value of currently occupied voxels.
  kRfExcludeOccupied = (1u << 7u),

  /// Trace each ray backwards, from sample to origin? The default is to trace rays from origin to sample. Reverse
  /// tracing can reduce GPU voxel contention near sensor origins and improve performance. However, forward tracing
  /// must be used for ray clearing patterns.
  ///
  /// Note: even when reversed, the sample voxel is updated to help avoid contention where many samples fall in the
  /// same voxel.
  ///
  // This may be ignored by some algorithms, such as ray queries.
  kRfReverseWalk = (1u << 8u),

  /// Internal use flag values start here (not to be set by user).
  kRfInternal = (1u << 16u),
  /// Marks that timestamps are available for GPU. This is an internal flag.
  kRfInternalTimestamps = (kRfInternal << 0u)
};
#if !GPUTIL_DEVICE
}  // namespace ohm
#endif  // !GPUTIL_DEVICE

#endif  // RAYFLAG_H


================================================
FILE: ohm/RayMapper.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RayMapper.h"

namespace ohm
{
RayMapper::RayMapper() = default;

RayMapper::~RayMapper() = default;
}  // namespace ohm


================================================
FILE: ohm/RayMapper.h
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2020
//
#ifndef RAYMAPPER_H
#define RAYMAPPER_H

#include "OhmConfig.h"

#include "RayFlag.h"

#include <glm/fwd.hpp>

namespace ohm
{
class OccupancyMap;
class KeyList;

/// A @c RayMapper serves to provide a unified interface for integrating rays into an @c OccupancyMap .
///
/// This interfaces solely serves to define the @p integrateRays() function.
class ohm_API RayMapper
{
public:
  /// Default constructor.
  RayMapper();
  /// Default, virtual destructor.
  virtual ~RayMapper();

  /// Has the map been successfully validated?
  /// @return True if validated and @c integrateRays() is safe to call.
  virtual bool valid() const = 0;

  /// This function integrates the set of @p rays into the underlying map interface. The details of how this is
  /// achieved is entirely up to the dervied implementation. Some examples include probabilistic occupancy update
  /// only, probably with update with voxel mean, occupancy update with covariance using a normal distribution
  /// transform or GPU implementations of the previous methods
  ///
  /// The @p rays array is expected to consist of ray origin, ray end point pairs. These correlate with a range sensor
  /// position and sample point pair. The @p element_count identifies the number of @c glm::dvec3 elements in @p rays
  /// thus is expected to be an even number with the ray count being `element_count / 2`.
  ///
  /// The @p ray_update_flags are used to modify the behaviour as par @c RayFlag values. However, not all
  /// implementations will respect all flags.
  ///
  /// Should only be called if @c validated() is true.
  ///
  /// @param rays The array of start/end point pairs to integrate.
  /// @param element_count The number of @c glm::dvec3 elements in @p rays, which is twice the ray count.
  /// @param intensities An array of intensity values matching the @p rays items. There is one intensity value per ray
  ///   so there are @c element_count/2 items. May be null to omit intensity values.
  /// @param timestamps An array of timestamp values matching the @p rays items. There is one timestamp value per ray
  ///   so there are @c element_count/2 items. May be null to omit timestamp values in which case the touch time layer
  ///   will not be updated.
  /// @param ray_update_flags @c RayFlag bitset used to modify the behaviour of this function.
  /// @return The number of samples processed.
  virtual size_t integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities,
                               const double *timestamps, unsigned ray_update_flags) = 0;

  /// @overload
  virtual inline size_t integrateRays(const glm::dvec3 *rays, size_t element_count)
  {
    return integrateRays(rays, element_count, nullptr, nullptr, kRfDefault);
  }
};
}  // namespace ohm


#endif  // RAYMAPPER_H


================================================
FILE: ohm/RayMapperNdt.cpp
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2020
//
#include "RayMapperNdt.h"

#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

#include "CovarianceVoxel.h"

#include "CalculateSegmentKeys.h"
#include "KeyList.h"
#include "LineWalk.h"
#include "MapLayer.h"
#include "MapLayout.h"
#include "NdtMap.h"
#include "OccupancyMap.h"
#include "RayFilter.h"
#include "VoxelBuffer.h"
#include "VoxelData.h"
#include "VoxelIncident.h"
#include "VoxelTouchTime.h"

#include <iostream>

namespace ohm
{
RayMapperNdt::RayMapperNdt(NdtMap *map)
  : map_(map)
  , occupancy_layer_(map_->map().layout().occupancyLayer())
  , mean_layer_(map_->map().layout().meanLayer())
  , traversal_layer_(map_->map().layout().traversalLayer())
  , covariance_layer_(map_->map().layout().covarianceLayer())
  , intensity_layer_(map_->map().layout().intensityLayer())
  , hit_miss_count_layer_(map_->map().layout().hitMissCountLayer())
  , touch_time_layer_(map_->map().layout().layerIndex(default_layer::touchTimeLayerName()))
  , incident_normal_layer_(map_->map().layout().layerIndex(default_layer::incidentNormalLayerName()))
  , ndt_tm_(map->mode() == NdtMode::kTraversability)
{
  OccupancyMap *map_ptr = &map_->map();

  Voxel<const float> occupancy(map_ptr, occupancy_layer_);
  Voxel<const VoxelMean> mean(map_ptr, mean_layer_);
  Voxel<const CovarianceVoxel> cov(map_ptr, covariance_layer_);
  Voxel<const float> traversal(map_ptr, traversal_layer_);
  Voxel<const uint32_t> touch_time_layer(map_ptr, touch_time_layer_);
  Voxel<const uint32_t> incident_normal_layer(map_ptr, incident_normal_layer_);

  occupancy_dim_ = (occupancy.isLayerValid()) ? occupancy.layerDim() : occupancy_dim_;

  // Validate we have occupancy, mean and covariance layers and their dimensions match.
  valid_ = occupancy.isLayerValid() && mean.isLayerValid() && cov.isLayerValid() &&
           occupancy.layerDim() == mean.layerDim() && occupancy.layerDim() == cov.layerDim();
  // Validate the traversal layer in a simliar fashion.
  valid_ = occupancy.isLayerValid() && !traversal.isLayerValid() ||
           occupancy.isLayerValid() && traversal.isLayerValid() && occupancy.layerDim() == traversal.layerDim();

  if (touch_time_layer.isLayerValid())
  {
    valid_ = valid_ && occupancy.layerDim() == touch_time_layer.layerDim();
  }

  if (incident_normal_layer.isLayerValid())
  {
    valid_ = valid_ && occupancy.layerDim() == incident_normal_layer.layerDim();
  }

  if (ndt_tm_)
  {
    Voxel<const IntensityMeanCov> intensity(map_ptr, intensity_layer_);
    Voxel<const HitMissCount> hit_miss(map_ptr, hit_miss_count_layer_);
    valid_ = valid_ && intensity.isLayerValid() && occupancy.layerDim() == intensity.layerDim() &&
             hit_miss.isLayerValid() && occupancy.layerDim() == hit_miss.layerDim();
  }
}


RayMapperNdt::~RayMapperNdt() = default;


size_t RayMapperNdt::integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities,
                                   const double *timestamps, unsigned ray_update_flags)
{
  KeyList keys;
  MapChunk *last_chunk = nullptr;
  VoxelBuffer<VoxelBlock> occupancy_buffer;
  VoxelBuffer<VoxelBlock> mean_buffer;
  VoxelBuffer<VoxelBlock> cov_buffer;
  VoxelBuffer<VoxelBlock> intensity_buffer;
  VoxelBuffer<VoxelBlock> hit_miss_count_buffer;
  VoxelBuffer<VoxelBlock> traversal_buffer;
  VoxelBuffer<VoxelBlock> touch_time_buffer;
  VoxelBuffer<VoxelBlock> incidents_buffer;
  double last_exit_range = 0;
  bool stop_adjustments = false;

  OccupancyMap &occupancy_map = map_->map();
  const RayFilterFunction ray_filter = occupancy_map.rayFilter();
  const bool use_filter = bool(ray_filter);
  const auto occupancy_layer = occupancy_layer_;
  const auto occupancy_dim = occupancy_dim_;
  const auto miss_value = occupancy_map.missValue();
  const auto hit_value = occupancy_map.hitValue();
  const auto resolution = occupancy_map.resolution();
  const auto voxel_min = occupancy_map.minVoxelValue();
  const auto voxel_max = occupancy_map.maxVoxelValue();
  const auto saturation_min = occupancy_map.saturateAtMinValue() ? voxel_min : std::numeric_limits<float>::lowest();
  const auto saturation_max = occupancy_map.saturateAtMaxValue() ? voxel_max : std::numeric_limits<float>::max();
  const auto sensor_noise = map_->sensorNoise();
  const auto ndt_adaptation_rate = map_->adaptationRate();
  const auto ndt_sample_threshold = map_->ndtSampleThreshold();

  // Mean and covariance layers must exists.
  const auto mean_layer = mean_layer_;
  const auto traversal_layer = traversal_layer_;
  const auto covariance_layer = covariance_layer_;

  // Compulsory if using NdtMode::kTraversability
  const auto intensity_layer = intensity_layer_;
  const auto hit_miss_count_layer = hit_miss_count_layer_;

  // Touch the map to flag changes.
  const auto touch_stamp = occupancy_map.touch();

  glm::dvec3 start;
  glm::dvec3 sample;
  float intensity = 0.0f;
  double time_base = 0;

  if (timestamps)
  {
    // Update first ray time if not yet set.
    occupancy_map.updateFirstRayTime(*timestamps);
  }
  time_base = occupancy_map.firstRayTime();

  const auto visit_func = [&](const Key &key, double enter_range, double exit_range) -> bool  //
  {
    //
    // The update logic here is a little unclear as it tries to avoid outright branches.
    // The intended logic is described as follows:
    // 1. Select direct write or additive adjustment.
    //    - Make a direct, non-additive adjustment if one of the following conditions are met:
    //      - stop_adjustments is true
    //      - the voxel is uncertain
    //      - voxel is saturated
    //    - Otherwise add to present value.
    // 2. Select the value adjustment
    //    - current_value if one of the following conditions are met:
    //      - stop_adjustments is true (no longer making adjustments)
    //    - miss_value otherwise
    // 3. Calculate new value
    // 4. Apply saturation logic: only min saturation relevant
    //    -
    MapChunk *chunk = (last_chunk && key.regionKey() == last_chunk->region.coord) ?
                        last_chunk :
                        occupancy_map.region(key.regionKey(), true);
    if (chunk != last_chunk)
    {
      occupancy_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[occupancy_layer]);
      mean_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[mean_layer]);
      cov_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[covariance_layer]);
      if (ndt_tm_)
      {
        // Intensity not required for miss update, but we need it in sync for the update later.
        intensity_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[intensity_layer_]);
        hit_miss_count_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[hit_miss_count_layer]);
      }
      if (traversal_layer >= 0)
      {
        traversal_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[traversal_layer]);
      }
      if (touch_time_layer_ >= 0 && timestamps)
      {
        // Touch time not required for miss update, but we need it in sync for the update later.
        touch_time_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[touch_time_layer_]);
      }
      if (incident_normal_layer_ >= 0)
      {
        // Incidents not required for miss update, but we need it in sync for the update later.
        incidents_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[incident_normal_layer_]);
      }
    }
    last_chunk = chunk;
    const unsigned voxel_index = ohm::voxelIndex(key, occupancy_dim);
    float occupancy_value;
    CovarianceVoxel cov;
    VoxelMean voxel_mean;
    occupancy_buffer.readVoxel(voxel_index, &occupancy_value);
    cov_buffer.readVoxel(voxel_index, &cov);
    mean_buffer.readVoxel(voxel_index, &voxel_mean);
    const glm::dvec3 mean =
      subVoxelToLocalCoord<glm::dvec3>(voxel_mean.coord, resolution) + occupancy_map.voxelCentreGlobal(key);
    const float initial_value = occupancy_value;
    float adjusted_value = initial_value;

    bool is_miss = false;
    calculateMissNdt(&cov, &adjusted_value, &is_miss, start, sample, mean, voxel_mean.count, unobservedOccupancyValue(),
                     miss_value, ndt_adaptation_rate, sensor_noise, ndt_sample_threshold);

    if (ndt_tm_)
    {
      // Note we don't need hit count in miss calculation.
      HitMissCount hit_miss_count_voxel;
      hit_miss_count_buffer.readVoxel(voxel_index, &hit_miss_count_voxel);
      hit_miss_count_voxel.miss_count += (is_miss) ? 1u : 0u;
      hit_miss_count_buffer.writeVoxel(voxel_index, hit_miss_count_voxel);
    }

    occupancyAdjustDown(&occupancy_value, initial_value, adjusted_value, unobservedOccupancyValue(), voxel_min,
                        saturation_min, saturation_max, stop_adjustments);
    occupancy_buffer.writeVoxel(voxel_index, occupancy_value);

    // Accumulate traversal
    if (traversal_layer >= 0)
    {
      float traversal;
      traversal_buffer.readVoxel(voxel_index, &traversal);
      traversal += float(exit_range - enter_range);
      traversal_buffer.writeVoxel(voxel_index, traversal);
    }

    // Lint(KS): The analyser takes some branches which are not possible in practice.
    // NOLINTNEXTLINE(clang-analyzer-core.CallAndMessage)
    chunk->updateFirstValid(voxel_index);

    stop_adjustments = stop_adjustments;
    chunk->dirty_stamp = touch_stamp;
    // Update the touched_stamps with relaxed memory ordering. The important thing is to have an update,
    // not so much the sequencing. We really don't want to synchronise here.
    chunk->touched_stamps[occupancy_layer].store(touch_stamp, std::memory_order_relaxed);
    if (ndt_tm_)
    {
      chunk->touched_stamps[hit_miss_count_layer].store(touch_stamp, std::memory_order_relaxed);
    }

    // Store last exit range for final traversal accumulation.
    last_exit_range = exit_range;

    return true;
  };

  unsigned filter_flags;
  float min_int = std::numeric_limits<float>::max();
  float max_int = 0.0f;
  for (size_t i = 0; i < element_count; i += 2)
  {
    filter_flags = 0;
    start = rays[i];
    sample = rays[i + 1];
    if (intensities)
    {
      intensity = intensities[i >> 1];
    }

    if (use_filter)
    {
      if (!ray_filter(&start, &sample, &filter_flags))
      {
        // Bad ray.
        continue;
      }
    }

    // Explicit update of the end voxel if it's a sample, include in ray if clipped.
    const bool include_sample_in_ray = (filter_flags & kRffClippedEnd) || (ray_update_flags & kRfEndPointAsFree);
    unsigned walk_flags = (!include_sample_in_ray) ? kExcludeEndVoxel : 0u;
    // Skip the start voxel according to ray_update_flags.
    walk_flags |= (ray_update_flags & kRfExcludeOrigin) ? kExcludeStartVoxel : 0u;

    if (!(ray_update_flags & kRfExcludeRay))
    {
      stop_adjustments = false;
      walkSegmentKeys(LineWalkContext(occupancy_map, visit_func), start, sample, walk_flags);
    }

    if (!stop_adjustments && !include_sample_in_ray)
    {
      // Like the miss logic, we have similar obfuscation here to avoid branching. It's a little simpler though,
      // because we do have a branch above, which will filter some of the conditions catered for in miss integration.
      const ohm::Key key = occupancy_map.voxelKey(sample);
      MapChunk *chunk = (last_chunk && key.regionKey() == last_chunk->region.coord) ?
                          last_chunk :
                          occupancy_map.region(key.regionKey(), true);
      if (chunk != last_chunk)
      {
        occupancy_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[occupancy_layer]);
        mean_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[mean_layer]);
        cov_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[covariance_layer_]);
        if (ndt_tm_)
        {
          intensity_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[intensity_layer_]);
          hit_miss_count_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[hit_miss_count_layer_]);
        }
        if (traversal_layer_ >= 0)
        {
          traversal_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[traversal_layer]);
        }
        if (touch_time_layer_ >= 0 && timestamps)
        {
          touch_time_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[touch_time_layer_]);
        }
        if (incident_normal_layer_ >= 0)
        {
          incidents_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[incident_normal_layer_]);
        }
      }
      last_chunk = chunk;
      const unsigned voxel_index = ohm::voxelIndex(key, occupancy_dim);
      const glm::dvec3 voxel_centre = occupancy_map.voxelCentreGlobal(key);
      float occupancy_value;
      CovarianceVoxel cov;
      VoxelMean voxel_mean;
      occupancy_buffer.readVoxel(voxel_index, &occupancy_value);
      cov_buffer.readVoxel(voxel_index, &cov);
      mean_buffer.readVoxel(voxel_index, &voxel_mean);
      const glm::dvec3 mean = subVoxelToLocalCoord<glm::dvec3>(voxel_mean.coord, resolution) + voxel_centre;
      const float initial_value = occupancy_value;
      float adjusted_value = initial_value;

      IntensityMeanCov intensity_voxel;
      HitMissCount hit_miss_count_voxel;
      if (ndt_tm_)
      {
        intensity_buffer.readVoxel(voxel_index, &intensity_voxel);
        hit_miss_count_buffer.readVoxel(voxel_index, &hit_miss_count_voxel);

        const bool reinitialise_permeability_with_covariance = true;  // TODO: make a parameter of map
        calculateHitMissUpdateOnHit(&cov, adjusted_value, &hit_miss_count_voxel, start, sample, mean, voxel_mean.count,
                                    unobservedOccupancyValue(), reinitialise_permeability_with_covariance,
                                    ndt_adaptation_rate, sensor_noise, map_->reinitialiseCovarianceThreshold(),
                                    map_->reinitialiseCovariancePointCount(), ndt_sample_threshold);

        calculateIntensityUpdateOnHit(&intensity_voxel, adjusted_value, intensity, map_->initialIntensityCovariance(),
                                      voxel_mean.count, map_->reinitialiseCovarianceThreshold(),
                                      map_->reinitialiseCovariancePointCount());

        min_int = std::fmin(min_int, intensity_voxel.intensity_mean);
        max_int = std::fmax(max_int, intensity_voxel.intensity_mean);
      }

      const bool reset_mean = calculateHitWithCovariance(
        &cov, &adjusted_value, sample, mean, voxel_mean.count, hit_value, unobservedOccupancyValue(), float(resolution),
        map_->reinitialiseCovarianceThreshold(), map_->reinitialiseCovariancePointCount());
      occupancyAdjustUp(&occupancy_value, initial_value, adjusted_value, unobservedOccupancyValue(), voxel_max,
                        saturation_min, saturation_max, stop_adjustments);

      voxel_mean.count = (!reset_mean) ? voxel_mean.count : 0;
      voxel_mean.coord = subVoxelUpdate(voxel_mean.coord, voxel_mean.count, sample - voxel_centre, resolution);
      ++voxel_mean.count;

      occupancy_buffer.writeVoxel(voxel_index, occupancy_value);
      cov_buffer.writeVoxel(voxel_index, cov);
      mean_buffer.writeVoxel(voxel_index, voxel_mean);
      if (ndt_tm_)
      {
        intensity_buffer.writeVoxel(voxel_index, intensity_voxel);
        hit_miss_count_buffer.writeVoxel(voxel_index, hit_miss_count_voxel);
      }

      // Accumulate traversal
      if (traversal_layer >= 0)
      {
        float traversal;
        traversal_buffer.readVoxel(voxel_index, &traversal);
        traversal += float(glm::length(sample - start) - last_exit_range);
        traversal_buffer.writeVoxel(voxel_index, traversal);
      }

      if (touch_time_layer_ >= 0 && timestamps)
      {
        const unsigned touch_time = encodeVoxelTouchTime(time_base, timestamps[i >> 1]);
        touch_time_buffer.writeVoxel(voxel_index, touch_time);
      }

      if (incident_normal_layer_ >= 0)
      {
        unsigned packed_normal{};
        incidents_buffer.readVoxel(voxel_index, &packed_normal);
        // Point count has already been incremented so subtract one to get the right calculation.s
        packed_normal = updateIncidentNormal(packed_normal, start - sample, voxel_mean.count - 1);
        incidents_buffer.writeVoxel(voxel_index, packed_normal);
      }

      // Lint(KS): The analyser takes some branches which are not possible in practice.
      // NOLINTNEXTLINE(clang-analyzer-core.CallAndMessage)
      chunk->updateFirstValid(voxel_index);

      chunk->dirty_stamp = touch_stamp;
      // Update the touched_stamps with relaxed memory ordering. The important thing is to have an update,
      // not so much the sequencing. We really don't want to synchronise here.
      chunk->touched_stamps[occupancy_layer].store(touch_stamp, std::memory_order_relaxed);
      chunk->touched_stamps[mean_layer].store(touch_stamp, std::memory_order_relaxed);
      chunk->touched_stamps[covariance_layer].store(touch_stamp, std::memory_order_relaxed);
      if (ndt_tm_)
      {
        chunk->touched_stamps[intensity_layer].store(touch_stamp, std::memory_order_relaxed);
        chunk->touched_stamps[hit_miss_count_layer].store(touch_stamp, std::memory_order_relaxed);
      }
    }
  }

  return element_count / 2;
}
}  // namespace ohm


================================================
FILE: ohm/RayMapperNdt.h
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2020
//
#ifndef RAYMAPPERNDT_H
#define RAYMAPPERNDT_H

#include "OhmConfig.h"

#include "RayFlag.h"
#include "RayMapper.h"

#include <glm/vec3.hpp>

namespace ohm
{
class NdtMap;

/// A @c RayMapper implementation built around updating a map in CPU. This mapper supports occupancy population
/// using a normal distributions transform methodology. The given map must support the following layers:
/// @c MayLayout::occupancyLayer() - float occupancy values - , @c MapLayout::meanLayer() - @c VoxelMean - and
/// @c MapLayout::covarianceLayer() - @c CovarianceVoxel .
///
/// The @c integrateRays() implementation performs a single threaded walk of the voxels to update and touches
/// those voxels one at a time, updating their occupancy value. Occupancy values are updated using
/// @c calculateMissNdt() for voxels the rays pass through and @c calculateHitWithCovariance() for the sample/end
/// voxels. Sample voxels also have their @c CovarianceVoxel and @c VoxelMean layers updated.
///
/// For reference see:
/// 3D Normal Distributions Transform Occupancy Maps: An Efficient Representation for Mapping in Dynamic Environments
class ohm_API RayMapperNdt : public RayMapper
{
public:
  /// Constructor, wrapping the interface around the given @p map .
  ///
  /// @param map The target map. Must outlive this class.
  explicit RayMapperNdt(NdtMap *map);

  /// Destructor
  ~RayMapperNdt() override;

  /// Has the map been successfully validated?
  /// @return True if valid and @c integrateRays() is safe to call.
  inline bool valid() const override { return valid_; }

  /// Performs the ray integration.
  ///
  /// This is updated in a single threaded fashion similar to @c RayMapperOccupancy with modified value updates as
  /// described in the class documentation.
  ///
  /// This function supports the following @c RayFlag values:
  /// - kRfExcludeRay
  ///
  /// Should only be called if @c valid() is true.
  ///
  /// @param rays The array of start/end point pairs to integrate.
  /// @param element_count The number of @c glm::dvec3 elements in @p rays, which is twice the ray count.
  /// @param intensities An array of intensity values matching the @p rays items. There is one intensity value per ray
  ///   so there are @c element_count/2 items. Required for the @c NdtMode::kTraversability model.
  /// @param timestamps An array of timestamp values matching the @p rays items. There is one timestamp value per ray
  ///   so there are @c element_count/2 items. May be null to omit timestamp values in which case the touch time layer
  ///   will not be updated.
  /// @param ray_update_flags Not supported.
  size_t integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities, const double *timestamps,
                       unsigned ray_update_flags) override;

  using RayMapper::integrateRays;

protected:
  NdtMap *map_;                     ///< Target map.
  int occupancy_layer_ = -1;        ///< Cached occupancy layer index.
  int mean_layer_ = -1;             ///< Cached voxel mean layer index.
  int traversal_layer_ = -1;        ///< The traversal layer index.
  int covariance_layer_ = -1;       ///< Cached covariance layer index.
  int intensity_layer_ = -1;        ///< Cached intensity layer index.
  int hit_miss_count_layer_ = -1;   ///< Cached hit miss count layer index.
  int touch_time_layer_ = -1;       ///< Cache touch time layer index.
  int incident_normal_layer_ = -1;  ///< Cache incident normal layer index.
  /// Cached occupancy layer voxel dimensions. Voxel mean and covariance layers must exactly match.
  glm::u8vec3 occupancy_dim_{ 0, 0, 0 };
  bool valid_ = false;  ///< Has layer validation passed?
  const bool ndt_tm_;   ///< Does map implement ndt-tm?
};

}  // namespace ohm


#endif  // RAYMAPPERNDT_H


================================================
FILE: ohm/RayMapperOccupancy.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RayMapperOccupancy.h"

#include "DefaultLayer.h"
#include "LineWalk.h"
#include "MapChunk.h"
#include "MapLayer.h"
#include "MapLayout.h"
#include "OccupancyMap.h"
#include "Voxel.h"
#include "VoxelBuffer.h"
#include "VoxelIncident.h"
#include "VoxelMean.h"
#include "VoxelOccupancy.h"
#include "VoxelTouchTime.h"

// TODO (KS): RayMapperOccupancy::lookupRays() is deprecated. Use RaysQuery for less code maintenance, but it creates
// a poor dependency.
#include "RaysQuery.h"

namespace ohm
{
RayMapperOccupancy::RayMapperOccupancy(OccupancyMap *map)
  : map_(map)
  , occupancy_layer_(map_->layout().occupancyLayer())
  , mean_layer_(map_->layout().meanLayer())
  , traversal_layer_(map_->layout().traversalLayer())
  , touch_time_layer_(map_->layout().layerIndex(default_layer::touchTimeLayerName()))
  , incident_normal_layer_(map_->layout().layerIndex(default_layer::incidentNormalLayerName()))
{
  // Use Voxel to validate the layers.
  // In processing we use VoxelBuffer instead of Voxel objects. While Voxel makes for a neater API, using VoxelBuffer
  // makes for less overhead and yields better performance.
  Voxel<const float> occupancy(map_, occupancy_layer_);
  Voxel<const VoxelMean> mean(map_, mean_layer_);
  Voxel<const float> traversal(map_, traversal_layer_);
  Voxel<const uint32_t> touch_time_layer(map_, touch_time_layer_);
  Voxel<const uint32_t> incident_normal_layer(map_, incident_normal_layer_);

  occupancy_dim_ = occupancy.isLayerValid() ? occupancy.layerDim() : occupancy_dim_;

  // Validate we only have an occupancy layer or we also have a mean layer and the layer dimesions match.
  valid_ = occupancy.isLayerValid() && !mean.isLayerValid() ||
           occupancy.isLayerValid() && mean.isLayerValid() && occupancy.layerDim() == mean.layerDim();
  // Validate the traversal layer in a simliar fashion.
  valid_ = occupancy.isLayerValid() && !traversal.isLayerValid() ||
           occupancy.isLayerValid() && traversal.isLayerValid() && occupancy.layerDim() == traversal.layerDim();

  if (touch_time_layer.isLayerValid())
  {
    valid_ = valid_ && occupancy.layerDim() == touch_time_layer.layerDim();
  }

  if (incident_normal_layer.isLayerValid())
  {
    valid_ = valid_ && occupancy.layerDim() == incident_normal_layer.layerDim();
  }
}


RayMapperOccupancy::~RayMapperOccupancy() = default;


size_t RayMapperOccupancy::integrateRays(const glm::dvec3 *rays, size_t element_count, const float * /*intensities*/,
                                         const double *timestamps, unsigned ray_update_flags)
{
  KeyList keys;
  MapChunk *last_chunk = nullptr;
  MapChunk *last_mean_chunk = nullptr;
  VoxelBuffer<VoxelBlock> occupancy_buffer;
  VoxelBuffer<VoxelBlock> mean_buffer;
  VoxelBuffer<VoxelBlock> traversal_buffer;
  VoxelBuffer<VoxelBlock> touch_time_buffer;
  VoxelBuffer<VoxelBlock> incidents_buffer;
  double last_exit_range = 0;
  bool stop_adjustments = false;

  const RayFilterFunction ray_filter = map_->rayFilter();
  const bool use_filter = bool(ray_filter);
  const auto occupancy_layer = occupancy_layer_;
  const auto mean_layer = mean_layer_;
  const auto traversal_layer = traversal_layer_;
  const auto occupancy_dim = occupancy_dim_;
  const auto occupancy_threshold_value = map_->occupancyThresholdValue();
  const auto miss_value = map_->missValue();
  const auto hit_value = map_->hitValue();
  const auto resolution = map_->resolution();
  const auto voxel_min = map_->minVoxelValue();
  const auto voxel_max = map_->maxVoxelValue();
  const auto saturation_min = map_->saturateAtMinValue() ? voxel_min : std::numeric_limits<float>::lowest();
  const auto saturation_max = map_->saturateAtMaxValue() ? voxel_max : std::numeric_limits<float>::max();
  // Touch the map to flag changes.
  const auto touch_stamp = map_->touch();

  if (timestamps)
  {
    // Update first ray time if not yet set.
    map_->updateFirstRayTime(*timestamps);
  }

  const auto visit_func = [&](const Key &key, double enter_range, double exit_range) -> bool  //
  {
    // The update logic here is a little unclear as it tries to avoid outright branches.
    // The intended logic is described as follows:
    // 1. Select direct write or additive adjustment.
    //    - Make a direct, non-additive adjustment if one of the following conditions are met:
    //      - stop_adjustments is true
    //      - the voxel is uncertain
    //      - ray_update_flags and kRfExclude<Type> flags pass.
    //      - voxel is saturated
    //    - Otherwise add to present value.
    // 2. Select the value adjustment
    //    - current_value if one of the following conditions are met:
    //      - stop_adjustments is true (no longer making adjustments)
    //      - ray_update_flags and kRfExclude<Type> flags pass.
    //    - miss_value otherwise
    // 3. Calculate new value
    // 4. Apply saturation logic: only min saturation relevant
    //    -
    MapChunk *chunk =
      (last_chunk && key.regionKey() == last_chunk->region.coord) ? last_chunk : map_->region(key.regionKey(), true);
    if (chunk != last_chunk)
    {
      occupancy_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[occupancy_layer]);
      if (traversal_layer >= 0)
      {
        traversal_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[traversal_layer]);
      }
      if (touch_time_layer_ >= 0 && timestamps)
      {
        // Touch time not required for miss update, but we need it in sync for the update later.
        touch_time_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[touch_time_layer_]);
      }
      if (incident_normal_layer_ >= 0)
      {
        // Incidents not required for miss update, but we need it in sync for the update later.
        incidents_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[incident_normal_layer_]);
      }
    }
    last_chunk = chunk;
    const unsigned voxel_index = ohm::voxelIndex(key, occupancy_dim);
    float occupancy_value;
    occupancy_buffer.readVoxel(voxel_index, &occupancy_value);
    const float initial_value = occupancy_value;

    const bool initially_unobserved = initial_value == unobservedOccupancyValue();
    const bool initially_free = !initially_unobserved && initial_value < occupancy_threshold_value;
    const bool initially_occupied = !initially_unobserved && initial_value >= occupancy_threshold_value;

    // Calculate the adjustment to make based on the initial occupancy value, various exclusion flags and the configured
    // value adjustment.
    float miss_adjustment = miss_value;
    // The next series of statements are designed to modify the miss_adjustment according to the current voxel state
    // and the kRfExclude<Type> values. Note that for kRfExcludeUnobserved we set the miss_adjustment such that it keeps
    // the observesed value, whereas in other cases we set to zero to make for no change. This is because unobserved
    // values have a value written, whereas other voxels have use addition to adjust the value.
    miss_adjustment = (initially_unobserved && (ray_update_flags & kRfExcludeUnobserved)) ? unobservedOccupancyValue() :
                                                                                            miss_adjustment;
    miss_adjustment = (initially_free && (ray_update_flags & kRfExcludeFree)) ? 0.0f : miss_adjustment;
    miss_adjustment = (initially_occupied && (ray_update_flags & kRfExcludeOccupied)) ? 0.0f : miss_adjustment;

    occupancyAdjustMiss(&occupancy_value, initial_value, miss_adjustment, unobservedOccupancyValue(), voxel_min,
                        saturation_min, saturation_max, stop_adjustments);
    occupancy_buffer.writeVoxel(voxel_index, occupancy_value);

    // Accumulate traversal
    if (traversal_layer >= 0)
    {
      float traversal;
      traversal_buffer.readVoxel(voxel_index, &traversal);
      traversal += float(exit_range - enter_range);
      traversal_buffer.writeVoxel(voxel_index, traversal);
    }

    // Lint(KS): The analyser takes some branches which are not possible in practice.
    // NOLINTNEXTLINE(clang-analyzer-core.CallAndMessage)
    chunk->updateFirstValid(voxel_index);

    stop_adjustments = stop_adjustments || ((ray_update_flags & kRfStopOnFirstOccupied) && initially_occupied);
    chunk->dirty_stamp = touch_stamp;
    // Update the touched_stamps with relaxed memory ordering. The important thing is to have an update,
    // not so much the sequencing. We really don't want to synchronise here.
    chunk->touched_stamps[occupancy_layer].store(touch_stamp, std::memory_order_relaxed);

    // Store last exit range for final traversal accumulation.
    last_exit_range = exit_range;

    return true;
  };

  glm::dvec3 start;
  glm::dvec3 end;
  unsigned filter_flags;
  double time_base = 0;

  if (timestamps)
  {
    // Update first ray time if not yet set.
    map_->updateFirstRayTime(*timestamps);
  }
  time_base = map_->firstRayTime();

  for (size_t i = 0; i < element_count; i += 2)
  {
    filter_flags = 0;
    start = rays[i];
    end = rays[i + 1];

    if (use_filter)
    {
      if (!ray_filter(&start, &end, &filter_flags))
      {
        // Bad ray.
        continue;
      }
    }

    // Explicit update of the end voxel if it's a sample, include in ray if clipped.
    const bool include_sample_in_ray = (filter_flags & kRffClippedEnd) || (ray_update_flags & kRfEndPointAsFree);
    unsigned walk_flags = (!include_sample_in_ray) ? kExcludeEndVoxel : 0u;
    // Skip the start voxel according to ray_update_flags.
    walk_flags |= (ray_update_flags & kRfExcludeOrigin) ? kExcludeStartVoxel : 0u;

    if (!(ray_update_flags & kRfExcludeRay))
    {
      stop_adjustments = false;
      walkSegmentKeys(LineWalkContext(*map_, visit_func), start, end, walk_flags);
    }

    if (!stop_adjustments && !include_sample_in_ray && !(ray_update_flags & kRfExcludeSample))
    {
      // Like the miss logic, we have similar obfuscation here to avoid branching. It's a little simpler though,
      // because we do have a branch above, which will filter some of the conditions catered for in miss integration.
      const ohm::Key key = map_->voxelKey(end);
      MapChunk *chunk =
        (last_chunk && key.regionKey() == last_chunk->region.coord) ? last_chunk : map_->region(key.regionKey(), true);
      if (chunk != last_chunk)
      {
        occupancy_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[occupancy_layer]);
        if (traversal_layer >= 0)
        {
          traversal_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[traversal_layer]);
        }
        if (touch_time_layer_ >= 0 && timestamps)
        {
          touch_time_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[touch_time_layer_]);
        }
        if (incident_normal_layer_ >= 0)
        {
          incidents_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[incident_normal_layer_]);
        }
      }
      last_chunk = chunk;
      const unsigned voxel_index = ohm::voxelIndex(key, occupancy_dim);

      float occupancy_value;
      occupancy_buffer.readVoxel(voxel_index, &occupancy_value);
      const float initial_value = occupancy_value;

      const bool initially_unobserved = initial_value == unobservedOccupancyValue();
      const bool initially_free = !initially_unobserved && initial_value < occupancy_threshold_value;
      const bool initially_occupied = !initially_unobserved && initial_value >= occupancy_threshold_value;

      // Calculate the adjustment to make based on the initial occupancy value, various exclusion flags and the
      // configured value adjustment (see the equivalent section for the miss update). Note the adjustment for skipping
      // an initially_unobserved voxel is not zero - it's unobservedOccupancyValue()/infinity to keep the state
      // unchanged.
      float hit_adjustment = hit_value;
      hit_adjustment = (initially_unobserved && (ray_update_flags & kRfExcludeUnobserved)) ?
                         unobservedOccupancyValue() :
                         hit_adjustment;
      hit_adjustment = (initially_free && (ray_update_flags & kRfExcludeFree)) ? 0.0f : hit_adjustment;
      hit_adjustment = (initially_occupied && (ray_update_flags & kRfExcludeOccupied)) ? 0.0f : hit_adjustment;

      occupancyAdjustHit(&occupancy_value, initial_value, hit_adjustment, unobservedOccupancyValue(), voxel_max,
                         saturation_min, saturation_max, stop_adjustments);

      // update voxel mean if present.
      unsigned sample_count = 0;
      if (mean_layer >= 0)
      {
        if (chunk != last_mean_chunk)
        {
          mean_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[mean_layer]);
        }
        last_mean_chunk = chunk;
        VoxelMean voxel_mean;
        mean_buffer.readVoxel(voxel_index, &voxel_mean);
        voxel_mean.coord =
          subVoxelUpdate(voxel_mean.coord, voxel_mean.count, end - map_->voxelCentreGlobal(key), resolution);
        sample_count = voxel_mean.count;
        ++voxel_mean.count;
        mean_buffer.writeVoxel(voxel_index, voxel_mean);
        // Lint(KS): The analyser takes some branches which are not possible in practice.
        // NOLINTNEXTLINE(clang-analyzer-core.CallAndMessage)
        chunk->touched_stamps[mean_layer].store(touch_stamp, std::memory_order_relaxed);
      }
      occupancy_buffer.writeVoxel(voxel_index, occupancy_value);

      // Accumulate traversal
      if (traversal_layer >= 0)
      {
        float traversal;
        traversal_buffer.readVoxel(voxel_index, &traversal);
        traversal += float(glm::length(end - start) - last_exit_range);
        traversal_buffer.writeVoxel(voxel_index, traversal);
      }

      if (touch_time_layer_ >= 0 && timestamps)
      {
        const unsigned touch_time = encodeVoxelTouchTime(time_base, timestamps[i >> 1]);
        touch_time_buffer.writeVoxel(voxel_index, touch_time);
      }

      if (incident_normal_layer_ >= 0)
      {
        unsigned packed_normal{};
        incidents_buffer.readVoxel(voxel_index, &packed_normal);
        packed_normal = updateIncidentNormal(packed_normal, start - end, sample_count);
        incidents_buffer.writeVoxel(voxel_index, packed_normal);
      }

      // Lint(KS): The analyser takes some branches which are not possible in practice.
      // NOLINTNEXTLINE(clang-analyzer-core.CallAndMessage)
      chunk->updateFirstValid(voxel_index);

      chunk->dirty_stamp = touch_stamp;
      // Update the touched_stamps with relaxed memory ordering. The important thing is to have an update,
      // not so much the sequencing. We really don't want to synchronise here.
      chunk->touched_stamps[occupancy_layer].store(touch_stamp, std::memory_order_relaxed);
    }
  }

  return element_count / 2;
}


size_t RayMapperOccupancy::lookupRays(const glm::dvec3 *rays, size_t element_count, float *newly_observed_volumes,
                                      float *ranges, OccupancyType *terminal_states)
{
  RaysQuery query;
  query.setMap(map_);
  query.setVolumeCoefficient((float)(1.0 / 180.0 * M_PI * 1.0 / 180.0 * M_PI));
  query.setRays(rays, element_count);

  query.execute();

  if (newly_observed_volumes)
  {
    memcpy(newly_observed_volumes, query.unobservedVolumes(),
           query.numberOfResults() * sizeof(*newly_observed_volumes));
  }

  if (ranges)
  {
    memcpy(ranges, query.ranges(), query.numberOfResults() * sizeof(*ranges));
  }

  if (terminal_states)
  {
    memcpy(terminal_states, query.terminalOccupancyTypes(), query.numberOfResults() * sizeof(*terminal_states));
  }

  return query.numberOfResults();
}
}  // namespace ohm


================================================
FILE: ohm/RayMapperOccupancy.h
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2020
//
#ifndef RAYMAPPEROCCUPANCY_H
#define RAYMAPPEROCCUPANCY_H

#include "OhmConfig.h"

#include "CalculateSegmentKeys.h"
#include "KeyList.h"
#include "RayFilter.h"
#include "RayFlag.h"
#include "RayMapper.h"
#include "Voxel.h"

#include <glm/vec3.hpp>

namespace ohm
{
/// A @c RayMapper implementation built around updating a map in CPU. This mapper supports basic occupancy population
/// and @c VoxelMean update (if enabled by the map) - @c MayLayout::occupancyLayer() and @c MapLayout::meanLayer()
/// respectively.
///
/// The @c integrateRays() implementation performs a single threaded walk of the voxels to update and touches
/// those voxels one at a time, updating their occupancy value. The given @c OccupancyMap must have an occupancy
/// layer and may have a @c VoxelMean layer.
class ohm_API RayMapperOccupancy : public RayMapper
{
public:
  /// Constructor, wrapping the interface around the given @p map .
  ///
  /// @param map The target map. Must outlive this class.
  explicit RayMapperOccupancy(OccupancyMap *map);

  /// Destructor
  ~RayMapperOccupancy() override;

  /// Has the map been successfully validated?
  /// @return True if valid and @c integrateRays() is safe to call.
  inline bool valid() const override { return valid_; }

  /// Performs the ray integration.
  ///
  /// This is updated in a single threaded fashion. For each ray we walk the affected voxel @c Key set and
  /// update those voxels. Voxels along each line segment have their occupancy probability diminished, while
  /// the end voxel of each segment has the probability increase. The end voxel will also have its @c VoxelMean
  /// updated if the map has a @c MapLayout::meanLayer() . This behaviour may be modified by the @p RayFlag
  /// bits in @p ray_update_flags .
  ///
  /// Should only be called if @c valid() is true.
  ///
  /// @param rays The array of start/end point pairs to integrate.
  /// @param element_count The number of @c glm::dvec3 elements in @p rays , which is twice the ray count.
  /// @param intensities An array of intensity values matching the @p rays items. There is one intensity value per ray
  ///   so there are @c element_count/2 items. May be null to omit intensity values.
  /// @param timestamps An array of timestamp values matching the @p rays items. There is one timestamp value per ray
  ///   so there are @c element_count/2 items. May be null to omit timestamp values in which case the touch time layer
  ///   will not be updated.
  /// @param ray_update_flags @c RayFlag bitset used to modify the behaviour of this function. All flags are
  /// implemented.
  size_t integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities, const double *timestamps,
                       unsigned ray_update_flags) override;

  /// Lookup the given @p rays in the map. The @p rays form a list of origin/sample pairs, where the sample represents
  /// the maximum range to scan.
  ///
  /// @deprecated This functionality can now be found in @c RaysQuery .
  ///
  /// @param rays Array of origin/sample point pairs.
  /// @param element_count The number of points in @p rays. The ray count is half this value.
  /// @param[out] newly_observed_volumes Array to be populated with previously unobserved volume observed in each ray.
  /// Optional; pass in nullptr if not required. Length element_count/2.
  /// @param[out] ranges Array to be populated with range traced for each ray. Will be length of origin sample/pair
  /// unless an occupied voxel was encountered. Optional; pass in nullptr if not required. Length element_count/2.
  /// @param[out] terminal_states Array to be populated with the state of the final voxel traced. Optional; pass in
  /// nullptr if not required. Length element_count/2.
  size_t lookupRays(const glm::dvec3 *rays, size_t element_count, float *newly_observed_volumes, float *ranges,
                    OccupancyType *terminal_states);

  using RayMapper::integrateRays;

protected:
  OccupancyMap *map_ = nullptr;           ///< Target map.
  int occupancy_layer_ = -1;              ///< Cached occupancy layer index.
  int mean_layer_ = -1;                   ///< Cached voxel mean layer index.
  int traversal_layer_ = -1;              ///< The traversal layer index.
  int touch_time_layer_ = -1;             ///< Cache touch time layer index.
  int incident_normal_layer_ = -1;        ///< Cache incident normal layer index.
  glm::u8vec3 occupancy_dim_{ 0, 0, 0 };  ///< Cached occupancy layer voxel dimensions. Voxel mean must exactly match.
  bool valid_ = false;                    ///< Has layer validation passed?
};

}  // namespace ohm


#endif  // RAYMAPPEROCCUPANCY_H


================================================
FILE: ohm/RayMapperSecondarySample.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RayMapperSecondarySample.h"

#include "DefaultLayer.h"
#include "MapChunk.h"
#include "MapLayer.h"
#include "MapLayout.h"
#include "OccupancyMap.h"
#include "Voxel.h"
#include "VoxelBuffer.h"
#include "VoxelSecondarySample.h"

namespace ohm
{
RayMapperSecondarySample::RayMapperSecondarySample(OccupancyMap *map)
  : map_(map)
  , secondary_samples_layer_(map_->layout().layerIndex(default_layer::secondarySamplesLayerName()))
{
  // Use Voxel to validate the layers.
  // In processing we use VoxelBuffer instead of Voxel objects. While Voxel makes for a neater API, using VoxelBuffer
  // makes for less overhead and yields better performance.
  Voxel<const VoxelSecondarySample> secondary_sample(map_, secondary_samples_layer_);

  layer_dim_ = secondary_sample.isLayerValid() ? secondary_sample.layerDim() : layer_dim_;
  valid_ = secondary_sample.isLayerValid();
}


RayMapperSecondarySample::~RayMapperSecondarySample() = default;


size_t RayMapperSecondarySample::integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities,
                                               const double *timestamps, unsigned ray_update_flags)
{
  (void)intensities;
  (void)timestamps;
  (void)ray_update_flags;

  MapChunk *last_chunk = nullptr;
  VoxelBuffer<VoxelBlock> secondary_sample_buffer;
  VoxelSecondarySample voxel{};

  const auto secondary_samples_layer = secondary_samples_layer_;
  const auto layer_dim = layer_dim_;
  // Touch the map to flag changes.
  const auto touch_stamp = map_->touch();

  for (size_t i = 0; i < element_count; i += 2)
  {
    const double range = glm::length(rays[i + 1] - rays[i]);
    const ohm::Key key = map_->voxelKey(rays[i + 1]);
    MapChunk *chunk =
      (last_chunk && key.regionKey() == last_chunk->region.coord) ? last_chunk : map_->region(key.regionKey(), true);

    if (chunk != last_chunk)
    {
      secondary_sample_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[secondary_samples_layer]);
    }
    last_chunk = chunk;
    const unsigned voxel_index = ohm::voxelIndex(key, layer_dim);

    secondary_sample_buffer.readVoxel(voxel_index, &voxel);
    addSecondarySample(voxel, range);
    secondary_sample_buffer.writeVoxel(voxel_index, voxel);

    assert(chunk);
    chunk->dirty_stamp = touch_stamp;
    chunk->touched_stamps[secondary_samples_layer].store(touch_stamp, std::memory_order_relaxed);
  }

  return element_count / 2;
}
}  // namespace ohm


================================================
FILE: ohm/RayMapperSecondarySample.h
================================================
//
// Author: Kazys Stepanas
// Copyright (c) CSIRO 2020
//
#ifndef OHM_RAYMAPPERSECONDARYSAMPLE_H
#define OHM_RAYMAPPERSECONDARYSAMPLE_H

#include "OhmConfig.h"

#include "CalculateSegmentKeys.h"
#include "KeyList.h"
#include "RayFilter.h"
#include "RayFlag.h"
#include "RayMapper.h"
#include "Voxel.h"

#include <glm/vec3.hpp>

namespace ohm
{
/// A @c RayMapper implementation built to update the secondary sample voxel layer. Secondary samples are generally
/// used for lidar dual returns.
///
/// This mapper performs no ray tracing, instead only updating the @c VoxelSecondarySample data for each sample voxel.
/// A ray is still required for each voxel in order to calculate the required ranges for the update. The provided rays
/// are expected to originate from the previous/primary voxel to the secondary voxel instead of from the sensor.
/// Intensity and timestamp values are ignored. @c RayFlag values have no effect.
///
/// The @c integrateRays() implementation performs a single threaded walk of just the sample voxels.
class ohm_API RayMapperSecondarySample : public RayMapper
{
public:
  /// Constructor, wrapping the interface around the given @p map .
  ///
  /// @param map The target map. Must outlive this class.
  explicit RayMapperSecondarySample(OccupancyMap *map);

  /// Destructor
  ~RayMapperSecondarySample() override;

  /// Has the map been successfully validated?
  /// @return True if valid and @c integrateRays() is safe to call.
  inline bool valid() const override { return valid_; }

  /// Performs the ray integration.
  ///
  /// This is updated in a single threaded fashion. For each ray we update the sample voxel based on the distance
  /// between the ray origin and sample. Each ray origin is expected to be the location of the primary/previous sample
  /// along the ray, and the second point is the secondary sample location of interest.
  ///
  /// Should only be called if @c valid() is true.
  ///
  /// @param rays The array of start/end or primary/secondary sample pairs to integrate.
  /// @param element_count The number of @c glm::dvec3 elements in @p rays , which is twice the ray count.
  /// @param intensities Ignored.
  /// @param timestamps Ignored.
  /// @param ray_update_flags Ignored.
  /// @return The number of samples processed.
  size_t integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities, const double *timestamps,
                       unsigned ray_update_flags) override;

  using RayMapper::integrateRays;

protected:
  OccupancyMap *map_ = nullptr;       ///< Target map.
  int secondary_samples_layer_ = -1;  ///< Cached secondary samples layer index.
  glm::u8vec3 layer_dim_{ 0, 0, 0 };  ///< Cached layer voxel dimensions.
  bool valid_ = false;                ///< Has layer validation passed?
};

}  // namespace ohm

#endif  // OHM_RAYMAPPERSECONDARYSAMPLE_H


================================================
FILE: ohm/RayMapperTrace.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RayMapperTrace.h"

#include "CalculateSegmentKeys.h"
#include "Key.h"
#include "KeyList.h"
#include "MapLayout.h"
#include "MapRegionCache.h"
#include "OccupancyMap.h"
#include "Trace.h"
#include "VoxelData.h"

#include "private/OccupancyMapDetail.h"

#ifdef TES_ENABLE
#include <3esmeshmessages.h>
#include <3esservermacros.h>
#include <shapes/3esmeshresource.h>

#include <glm/gtc/type_ptr.hpp>

#include <unordered_set>

namespace ohm
{
using KeyToIndexMap = std::unordered_map<Key, uint32_t, Key::Hash>;
using KeySet = std::unordered_set<Key, Key::Hash>;

struct OccupancyMeshDetail
{
  OccupancyMap *map;
  uint32_t id;

  std::vector<tes::Vector3d> vertices;
  // Define the render extents for the voxels.
  std::vector<tes::Vector3d> normals;
  std::vector<uint32_t> colours;
  /// Tracks indices of unused vertices in the vertex array.
  std::vector<uint32_t> unused_vertex_list;
  /// Maps voxel keys to their vertex indices.
  KeyToIndexMap voxel_index_map;
};

/// Defines and maintains a 3rd Eye Scene mesh resource based on an octomap.
///
/// Renders as a point cloud of occupied voxels.
class OccupancyMesh : public tes::MeshResource
{
public:
  explicit OccupancyMesh(OccupancyMap *map);
  ~OccupancyMesh() override;

  uint32_t id() const override;
  tes::Transform transform() const override;
  uint32_t tint() const override;
  uint8_t drawType(int stream) const override;

  unsigned vertexCount(int stream) const override;
  unsigned indexCount(int stream) const override;

  tes::DataBuffer vertices(int stream) const override;
  tes::DataBuffer indices(int stream) const override;
  tes::DataBuffer normals(int stream) const override;
  tes::DataBuffer uvs(int stream) const override;
  tes::DataBuffer colours(int stream) const override;

  tes::Resource *clone() const override;

  int transfer(tes::PacketWriter &packet, unsigned byte_limit, tes::TransferProgress &progress) const override;

  /// Updates noted changes to the debug view.
  /// @param newly_occupied Keys of voxels which have become occupied from free or uncertain since the last update.
  /// @param newly_free Keys of voxels which have become free from occupied since the last update.
  /// @param touched_occupied Keys of voxels which have changed occupied probability.
  void update(const KeySet &newly_occupied, const KeySet &newly_free, const KeySet &touched_occupied);

private:
  std::unique_ptr<OccupancyMeshDetail> imp_;
};

/// Assigns a colour to a voxel based on it's occupancy value.
/// @param occupancy_voxel The voxel of interest. Must be valid.
uint32_t voxelColour(const Voxel<const float> &occupancy_voxel)
{
  float occupancy;
  if (occupancy_voxel.isValid())
  {
    occupancy_voxel.read(&occupancy);
    occupancy = valueToProbability(occupancy);
    const auto intensity = float((occupancy - occupancy_voxel.map()->occupancyThresholdProbability()) /
                                 (1.0 - occupancy_voxel.map()->occupancyThresholdProbability()));
    const int c = int(255 * intensity);
    return tes::Colour(c, c, c).c;
  }
  return tes::Colour(0, 0, 0).c;
}

OccupancyMesh::OccupancyMesh(OccupancyMap *map)
  : imp_(std::make_unique<OccupancyMeshDetail>())
{
  imp_->map = map;
  imp_->id = tes::Id(this).id();
}

OccupancyMesh::~OccupancyMesh() = default;

uint32_t OccupancyMesh::id() const
{
  return imp_->id;
}

tes::Transform OccupancyMesh::transform() const
{
  return tes::Transform::identity(true);
}

uint32_t OccupancyMesh::tint() const
{
  return tes::Colour::Colours[tes::Colour::White].c;
}

uint8_t OccupancyMesh::drawType(int stream) const
{
  TES_UNUSED(stream);
  return tes::DtVoxels;
}

unsigned OccupancyMesh::vertexCount(int stream) const
{
  TES_UNUSED(stream);
  return unsigned(imp_->vertices.size());
}

unsigned OccupancyMesh::indexCount(int stream) const
{
  TES_UNUSED(stream);
  return 0;
}

tes::DataBuffer OccupancyMesh::vertices(int stream) const
{
  TES_UNUSED(stream);
  return tes::DataBuffer(imp_->vertices);
}

tes::DataBuffer OccupancyMesh::indices(int stream) const
{
  TES_UNUSED(stream);
  return tes::DataBuffer();
}

tes::DataBuffer OccupancyMesh::normals(int stream) const
{
  TES_UNUSED(stream);
  return tes::DataBuffer(imp_->normals);
}

tes::DataBuffer OccupancyMesh::uvs(int stream) const
{
  TES_UNUSED(stream);
  return tes::DataBuffer();
}

tes::DataBuffer OccupancyMesh::colours(int stream) const
{
  TES_UNUSED(stream);
  return tes::DataBuffer(imp_->colours);
}

tes::Resource *OccupancyMesh::clone() const
{
  auto *copy = new OccupancyMesh(imp_->map);
  *copy->imp_ = *imp_;
  return copy;
}

int OccupancyMesh::transfer(tes::PacketWriter &packet, unsigned byte_limit, tes::TransferProgress &progress) const
{
  // Build the voxel set if required.
  if (imp_->voxel_index_map.empty())
  {
    imp_->vertices.clear();
    imp_->colours.clear();
    Voxel<const float> occupancy_voxel(imp_->map, imp_->map->layout().occupancyLayer());
    const OccupancyMap &map = *imp_->map;
    for (auto iter = map.begin(); iter != map.end(); ++iter)
    {
      occupancy_voxel.setKey(iter);
      if (isOccupied(occupancy_voxel))
      {
        // Add voxel.
        imp_->voxel_index_map.insert(std::make_pair(*iter, uint32_t(imp_->vertices.size())));
        imp_->vertices.emplace_back(glm::value_ptr(map.voxelCentreGlobal(*iter)));
        // Normals represent voxel half extents.
        imp_->normals.emplace_back(0.5 * map.resolution());
        imp_->colours.push_back(voxelColour(occupancy_voxel));
      }
    }
  }

  return tes::MeshResource::transfer(packet, byte_limit, progress);
}

void OccupancyMesh::update(const KeySet &newly_occupied, const KeySet &newly_free, const KeySet &touched_occupied)
{
  if (newly_occupied.empty() && newly_free.empty() && touched_occupied.empty())
  {
    // Nothing to do.
    return;
  }

  if (g_tes->connectionCount() == 0)
  {
    // No-one to send to.
    imp_->vertices.clear();
    imp_->normals.clear();
    imp_->colours.clear();
    // indices.clear();
    imp_->unused_vertex_list.clear();
    imp_->voxel_index_map.clear();
    return;
  }

  // Start by removing freed nodes.
  size_t initial_unused_vertex_count = imp_->unused_vertex_list.size();
  std::vector<uint32_t> modified_vertices;
  for (const auto &key : newly_free)
  {
    // Resolve the index for this voxel.
    auto voxel_lookup = imp_->voxel_index_map.find(key);
    if (voxel_lookup != imp_->voxel_index_map.end())
    {
      // Invalidate the voxel.
      imp_->colours[voxel_lookup->second] = 0u;
      imp_->unused_vertex_list.push_back(voxel_lookup->second);
      modified_vertices.push_back(voxel_lookup->second);
      imp_->voxel_index_map.erase(voxel_lookup);
    }
  }

  // Now added occupied nodes, initially from the free list.
  Voxel<const float> occupancy_voxel(imp_->map, imp_->map->layout().occupancyLayer());
  size_t processed_occupied_count = 0;
  auto occupied_iter = newly_occupied.begin();
  while (!imp_->unused_vertex_list.empty() && occupied_iter != newly_occupied.end())
  {
    const uint32_t vertex_index = imp_->unused_vertex_list.back();
    const Key key = *occupied_iter;
    const bool mark_as_modified = imp_->unused_vertex_list.size() <= initial_unused_vertex_count;
    imp_->unused_vertex_list.pop_back();
    ++occupied_iter;
    ++processed_occupied_count;
    occupancy_voxel.setKey(key);
    imp_->vertices[vertex_index] = tes::Vector3d(glm::value_ptr(imp_->map->voxelCentreGlobal(key)));
    imp_->colours[vertex_index] = voxelColour(occupancy_voxel);
    imp_->voxel_index_map.insert(std::make_pair(key, vertex_index));
    // Only mark as modified if this vertex wasn't just invalidate by removal.
    // It will already be on the list otherwise.
    if (mark_as_modified)
    {
      modified_vertices.push_back(vertex_index);
    }
  }

  // Send messages for individually changed voxels.
  // Start a mesh redefinition message.
  std::vector<uint8_t> buffer(0xffffu);  // NOLINT(readability-magic-numbers)
  tes::PacketWriter packet(buffer.data(), static_cast<uint16_t>(buffer.size()));
  tes::MeshRedefineMessage msg{};
  tes::MeshComponentMessage cmpmsg{};
  tes::MeshFinaliseMessage finalmsg{};
  tes::ObjectAttributesd attributes{};
  tes::DataBuffer data_buffer;

  // Work out how many vertices we'll have after all modifications are done.
  size_t old_vertex_count = imp_->vertices.size();
  size_t new_vertex_count = imp_->vertices.size();
  if (newly_occupied.size() - processed_occupied_count > imp_->unused_vertex_list.size())
  {
    // We have more occupied vertices than available in the free list.
    // This means we will add new vertices.
    new_vertex_count += newly_occupied.size() - processed_occupied_count - imp_->unused_vertex_list.size();
  }

  msg.meshId = imp_->id;
  msg.vertexCount = uint32_t(new_vertex_count);
  msg.indexCount = 0;
  msg.drawType = drawType(0);
  attributes.identity();

  packet.reset(tes::MtMesh, tes::MeshRedefineMessage::MessageId);
  msg.write(packet, attributes);

  packet.finalise();
  g_tes->send(packet);

  // Next update changed triangles.
  cmpmsg.meshId = id();

  // Update modified vertices, one at a time.
  for (uint32_t vertex_index : modified_vertices)
  {
    packet.reset(tes::MtMesh, tes::MmtVertex);
    cmpmsg.write(packet);
    data_buffer = tes::DataBuffer(&imp_->vertices[vertex_index], 1);
    data_buffer.write(packet, 0, packet.bytesRemaining(), vertex_index);
    packet.finalise();
    g_tes->send(packet);

    // Send colour and position update.
    packet.reset(tes::MtMesh, tes::MmtVertexColour);
    cmpmsg.write(packet);
    data_buffer = tes::DataBuffer(&imp_->colours[vertex_index], 1);
    data_buffer.write(packet, 0, packet.bytesRemaining(), vertex_index);
    packet.finalise();
    g_tes->send(packet);
  }

  // Add remaining vertices and send a bulk modification message.
  // Continue iteration from where we left off.
  for (; occupied_iter != newly_occupied.end(); ++occupied_iter, ++processed_occupied_count)
  {
    const auto vertex_index = uint32_t(imp_->vertices.size());
    const Key key = *occupied_iter;
    imp_->voxel_index_map.insert(std::make_pair(key, vertex_index));
    imp_->vertices.emplace_back(glm::value_ptr(imp_->map->voxelCentreGlobal(key)));
    // Normals represent voxel half extents.
    imp_->normals.emplace_back(0.5 * imp_->map->resolution());
    imp_->colours.emplace_back(tes::Colour::Colours[tes::Colour::White].c);
  }

  // Send bulk messages for new vertices.
  if (old_vertex_count != new_vertex_count)
  {
    // Send colour and position update.
    auto offset = uint32_t(old_vertex_count);
    data_buffer = tes::DataBuffer(imp_->vertices);

    while (offset < new_vertex_count)
    {
      packet.reset(tes::MtMesh, tes::MmtVertex);
      cmpmsg.write(packet);
      offset += data_buffer.writePacked(packet, offset, 0.5 * imp_->map->resolution(), packet.bytesRemaining());
      packet.finalise();
      g_tes->send(packet);
    }

    offset = uint32_t(old_vertex_count);
    data_buffer = tes::DataBuffer(imp_->normals);
    while (offset < new_vertex_count)
    {
      packet.reset(tes::MtMesh, tes::MmtNormal);
      cmpmsg.write(packet);
      offset += data_buffer.write(packet, offset, packet.bytesRemaining());
      packet.finalise();
      g_tes->send(packet);
    }

    offset = uint32_t(old_vertex_count);
    data_buffer = tes::DataBuffer(imp_->colours);
    while (offset < new_vertex_count)
    {
      packet.reset(tes::MtMesh, tes::MmtVertexColour);
      cmpmsg.write(packet);
      offset += data_buffer.write(packet, offset, packet.bytesRemaining());
      packet.finalise();
      g_tes->send(packet);
    }
  }

  // Update colours for touched occupied
  if (!touched_occupied.empty())
  {
    for (const auto &key : touched_occupied)
    {
      occupancy_voxel.setKey(key);
      auto index_search = imp_->voxel_index_map.find(key);
      if (index_search != imp_->voxel_index_map.end())
      {
        const unsigned voxel_index = index_search->second;
        imp_->colours[voxel_index] = voxelColour(occupancy_voxel);

        packet.reset(tes::MtMesh, tes::MmtVertexColour);
        cmpmsg.write(packet);
        data_buffer = tes::DataBuffer(&imp_->colours[voxel_index], 1);
        data_buffer.write(packet, 0, packet.bytesRemaining(), voxel_index);
        packet.finalise();
        g_tes->send(packet);
      }
    }
  }

  // Finalise the modifications.
  finalmsg.meshId = imp_->id;
  finalmsg.flags = 0;
  packet.reset(tes::MtMesh, finalmsg.MessageId);
  finalmsg.write(packet);
  packet.finalise();
  g_tes->send(packet);
}


/// Draw an NDT visualisation for the given "sector key".
///
/// This sends ellipsoids for the occupied voxels in the sector. See @p RayMapperTrace::SectorSet .
void drawNdt(const glm::i16vec4 &sector_key, const OccupancyMap &map)
{
  std::vector<tes::Sphere> ellipsoids;
  std::vector<tes::Shape *> ellipsoid_ptrs;

  const glm::i16vec3 region_key(sector_key);
  const MapChunk *chunk = map.region(region_key);

  if (chunk)
  {
    // Extract occupied voxels in the region
    Voxel<const float> occ_voxel(&map, map.layout().occupancyLayer());
    Voxel<const VoxelMean> mean_voxel(&map, map.layout().meanLayer());
    Voxel<const CovarianceVoxel> cov_voxel(&map, map.layout().covarianceLayer());

    const auto dim = map.regionVoxelDimensions();
    const tes::Id shape_id = tes::Id(chunk, kTcNdt) + sector_key.w;  // Each sector has a unique key

    // Work out the sector indexing range.
    glm::ivec3 start_index(0);
    glm::ivec3 end_index(0);

    for (int i = 0; i < 3; ++i)
    {
      start_index[i] = !!(sector_key.w & (1 << i));  // NOLINT(hicpp-signed-bitwise)
      end_index[i] = (start_index[i]) ? dim[i] : dim[i] / 2;
      start_index[i] = (start_index[i]) ? dim[i] / 2 : 0;
    }

    for (int z = start_index.z; z < end_index.z; ++z)
    {
      for (int y = start_index.y; y < end_index.y; ++y)
      {
        for (int x = start_index.x; x < end_index.x; ++x)
        {
          const Key key(region_key, x, y, z);
          setVoxelKey(key, occ_voxel, mean_voxel, cov_voxel);
          if (isOccupied(occ_voxel))
          {
            const CovarianceVoxel cov_info = cov_voxel.data();
            glm::dquat rotation;
            glm::dvec3 scale;
            const glm::dvec3 pos = positionUnsafe(mean_voxel);
            if (covarianceUnitSphereTransformation(&cov_info, &rotation, &scale))
            {
              // Use single precision for smaller data size
              ellipsoids.emplace_back(
                tes::Sphere(shape_id, tes::Transform(tes::Vector3f(glm::value_ptr(pos)),
                                                     tes::Quaternionf(float(rotation.x), float(rotation.y),
                                                                      float(rotation.z), float(rotation.w)),
                                                     tes::Vector3f(glm::value_ptr(scale)))));
              ellipsoids.back().setColour(tes::Colour::Colours[tes::Colour::MediumSeaGreen]);
            }
          }
        }
      }
    }

    if (!ellipsoids.empty())
    {
      for (tes::Sphere &shape : ellipsoids)
      {
        ellipsoid_ptrs.emplace_back(&shape);
      }

      // Create the multi-shape to replace the existing one for the region.
      g_tes->create(tes::MultiShape(ellipsoid_ptrs.data(), ellipsoid_ptrs.size()).setReplace(true));
    }
    else
    {
      // Destroy any existing representation for this region.
      g_tes->destroy(tes::Sphere(shape_id));
    }
  }
}

/// Draw an NDT-TM visualisation for the given "sector key".
///
/// This sends ellipsoids for the occupied voxels in the sector. See @p RayMapperTrace::SectorSet .
void drawNdtTm(const glm::i16vec4 &sector_key, const OccupancyMap &map)
{
  std::vector<tes::Sphere> ellipsoids;
  std::vector<tes::Shape *> ellipsoid_ptrs;
  static float min_intensity = std::numeric_limits<float>::infinity(),
               max_intensity = -std::numeric_limits<float>::infinity();

  const glm::i16vec3 region_key(sector_key);
  const MapChunk *chunk = map.region(region_key);

  if (chunk)
  {
    // Extract occupied voxels in the region
    Voxel<const float> occ_voxel(&map, map.layout().occupancyLayer());
    Voxel<const VoxelMean> mean_voxel(&map, map.layout().meanLayer());
    Voxel<const CovarianceVoxel> cov_voxel(&map, map.layout().covarianceLayer());
    Voxel<const IntensityMeanCov> intensity_voxel(&map, map.layout().intensityLayer());
    Voxel<const HitMissCount> hit_miss_voxel(&map, map.layout().hitMissCountLayer());

    const tes::Id shape_id = tes::Id(chunk, kTcNdt) + sector_key.w;  // Each sector has a unique key

    // Work out the sector indexing range.
    glm::ivec3 start_index(0);
    glm::ivec3 end_index(0);

    for (int i = 0; i < 3; ++i)
    {
      start_index[i] = !!(sector_key.w & (1 << i));
      end_index[i] = (start_index[i]) ? map.regionVoxelDimensions()[i] : map.regionVoxelDimensions()[i] / 2;
      start_index[i] = (start_index[i]) ? map.regionVoxelDimensions()[i] / 2 : 0;
    }

    for (int z = start_index.z; z < end_index.z; ++z)
    {
      for (int y = start_index.y; y < end_index.y; ++y)
      {
        for (int x = start_index.x; x < end_index.x; ++x)
        {
          const Key key(region_key, x, y, z);
          setVoxelKey(key, occ_voxel, mean_voxel, cov_voxel, intensity_voxel, hit_miss_voxel);
          if (isOccupied(occ_voxel))
          {
            const CovarianceVoxel cov_info = cov_voxel.data();
            glm::dquat rotation;
            glm::dvec3 scale;
            const glm::dvec3 pos = positionUnsafe(mean_voxel);
            const IntensityMeanCov intensity_mean_cov = intensity_voxel.data();
            min_intensity = std::fmin(min_intensity, intensity_mean_cov.intensity_mean);
            max_intensity = std::fmax(max_intensity, intensity_mean_cov.intensity_mean);
            const float scaled_intensity =
              float(M_PI) * (-1.0f + 1.5f * (intensity_mean_cov.intensity_mean - min_intensity) /
                                       std::fmax(1.0f, max_intensity - min_intensity));
            const float sin_sc = std::sin(scaled_intensity), cos_sc = std::cos(scaled_intensity);
            const HitMissCount hit_miss_count = hit_miss_voxel.data();

            if (covarianceUnitSphereTransformation(&cov_info, &rotation, &scale))
            {
              // Use single precision for smaller data size
              ellipsoids.emplace_back(
                tes::Sphere(shape_id, tes::Transform(tes::Vector3f(glm::value_ptr(pos)),
                                                     tes::Quaternionf((float)rotation.x, (float)rotation.y,
                                                                      (float)rotation.z, (float)rotation.w),
                                                     tes::Vector3f(glm::value_ptr(scale)))));
              const float alpha = 0.9f * float(hit_miss_count.hit_count) /
                                  std::fmax(1.0f, float(hit_miss_count.hit_count + hit_miss_count.miss_count));
              ellipsoids.back().setColour(tes::Colour(0.1f + alpha * 0.5f * (1.0f + sin_sc),
                                                      0.1f + alpha * 0.5f * (1.0f + cos_sc),
                                                      0.1f + alpha * 0.5f * (1.0f - sin_sc), 1.0f));
            }
          }
        }
      }
    }

    if (!ellipsoids.empty())
    {
      for (tes::Sphere &shape : ellipsoids)
      {
        ellipsoid_ptrs.emplace_back(&shape);
      }

      // Create the multi-shape to replace the existing one for the region.
      g_tes->create(tes::MultiShape(ellipsoid_ptrs.data(), ellipsoid_ptrs.size()).setReplace(true));
    }
    else
    {
      // Destroy any existing representation for this region.
      g_tes->destroy(tes::Sphere(shape_id));
    }
  }
}
}  // namespace ohm

#else   // TES_ENABLE
namespace ohm
{
class OccupancyMesh
{
public:
  OccupancyMesh(OccupancyMap *) {}
};
}  // namespace ohm
#endif  // TES_ENABLE

namespace ohm
{
RayMapperTrace::RayMapperTrace(OccupancyMap *map, RayMapper *true_mapper)
  : map_(map)
  , true_mapper_(true_mapper)
  , imp_(std::make_unique<OccupancyMesh>(map))
{
#ifdef TES_ENABLE
  if (g_tes)
  {
    // Instantiate the mesh resource.
    g_tes->referenceResource(imp_.get());
    // Create the mesh object representation.
    g_tes->create(tes::MeshSet(imp_.get(), tes::Id(this, kTcVoxels)));

    // We need to run an update to ensure the mesh is created before we start trying to update it.
    TES_SERVER_UPDATE(g_tes, 0.0f);
  }
#endif  // TES_ENABLE
}


RayMapperTrace::~RayMapperTrace()
{
#ifdef TES_ENABLE
  if (g_tes)
  {
    // Release the mesh object representation.
    g_tes->destroy(tes::MeshSet(imp_.get(), tes::Id(this, kTcVoxels)));
    // Release the mesh resource.
    g_tes->releaseResource(imp_.get());
  }
#endif  // TES_ENABLE
}


bool RayMapperTrace::valid() const
{
  return true_mapper_ != nullptr && true_mapper_->valid();
}

size_t RayMapperTrace::integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities,
                                     const double *timestamps, unsigned ray_update_flags)
{
#ifdef TES_ENABLE
  // Walk all the rays and cache the state of the (predicted) touched voxels.
  VoxelMap initial_state;
  VoxelMap updated_state;
  SectorSet sector_set;

  if (g_tes && element_count)
  {
    cacheState(rays, element_count, &initial_state, &sector_set);
  }
#endif  // TES_ENABLE

  const size_t result = true_mapper_->integrateRays(rays, element_count, intensities, timestamps, ray_update_flags);

#ifdef TES_ENABLE
  if (g_tes && element_count)
  {
    // Sync gpu cache to CPU
    if (map_->detail()->gpu_cache)
    {
      map_->detail()->gpu_cache->flush();
    }

    // Draw the rays
    g_tes->create(tes::MeshShape(tes::DtLines, tes::Id(0u, kTcRays),
                                 tes::DataBuffer(&rays->x, element_count, 3, sizeof(*rays) / sizeof(rays->x)))
                    .setColour(tes::Colour::Colours[tes::Colour::Yellow]));

    cacheState(rays, element_count, &updated_state);

    // Determine changes.
    KeySet newly_occupied;
    KeySet newly_freed;
    KeySet touched_occupied;

    for (const auto &voxel_info : updated_state)
    {
      const auto initial_info = initial_state.find(voxel_info.first);
      if (initial_info == initial_state.end())
      {
        // Something weird has happened. Let's ignore it!
        continue;
      }

      const OccupancyType initial_type = initial_info->second.type;
      if (voxel_info.second.type != initial_type)
      {
        // State has changed.
        switch (voxel_info.second.type)
        {
        case kOccupied:
          newly_occupied.insert(voxel_info.first);
          break;
        case kFree:
          if (initial_type == kOccupied)
          {
            newly_freed.insert(voxel_info.first);
          }
          break;
        default:
          // nothing to do here
          break;
        }
      }
      else if (voxel_info.second.type == kOccupied)
      {
        // Touched
        touched_occupied.insert(voxel_info.first);
      }
    }

    // Update the mesh changes
    imp_->update(newly_occupied, newly_freed, touched_occupied);

    // Update NDT representation.
    if (!sector_set.empty() && map_->layout().covarianceLayer() >= 0)
    {
      for (const auto &sector_key : sector_set)
      {
        if (map_->layout().intensityLayer() >= 0 && map_->layout().hitMissCountLayer() >= 0)
        {
          drawNdtTm(sector_key, *map_);
        }
        else
        {
          drawNdt(sector_key, *map_);
        }
      }
    }

    // Do full update (with connection management)
    TES_SERVER_UPDATE(g_tes, 0.0f);
  }
#endif  // TES_ENABLE

  return result;
}


glm::i16vec4 RayMapperTrace::sectorKey(const Key &key) const
{
  // We divide the MapChunk into 8 sectors, like a voxel. We need to convert the local key into a sector index
  glm::ivec3 local = key.localKey();
  // Each axis is converted to 0 or 1
  local[0] = !!(local[0] >= (map_->regionVoxelDimensions()[0] / 2));
  local[1] = !!(local[1] >= (map_->regionVoxelDimensions()[1] / 2));
  local[2] = !!(local[2] >= (map_->regionVoxelDimensions()[2] / 2));

  // Linearise the new key as the w value for the return key.
  glm::i16vec4 sector_key(key.regionKey(), local.x + local.y * 2 + local.z * 4);

  return sector_key;
}


void RayMapperTrace::cacheState(const glm::dvec3 *rays, size_t element_count, VoxelMap *voxels, SectorSet *sectors)
{
  KeyList keys;

  // Setup voxel references
  Voxel<const float> occupancy_voxel(map_, map_->layout().occupancyLayer());
  Voxel<const VoxelMean> mean_voxel(map_, map_->layout().meanLayer());
  Voxel<const CovarianceVoxel> covariance_voxel(map_, map_->layout().covarianceLayer());

  for (size_t ri = 0; ri < element_count / 2; ++ri)
  {
    keys.clear();
    calculateSegmentKeys(keys, *map_, rays[ri * 2 + 0], rays[ri * 2 + 1], true);

    // Walk the ray keys
    for (const auto &key : keys)
    {
      if (sectors)
      {
        sectors->insert(sectorKey(key));
      }

      if (voxels->find(key) == voxels->end())
      {
        // not already in the set.
        setVoxelKey(key, occupancy_voxel, mean_voxel, covariance_voxel);

        VoxelState voxel_info;
        voxel_info.type = occupancyType(occupancy_voxel);

        if (voxel_info.type == kOccupied && covariance_voxel.isValid() && mean_voxel.isValid())
        {
          CovarianceVoxel cov;
          covariance_voxel.read(&cov);

          voxel_info.ellipse_pos = positionUnsafe(mean_voxel);
          covarianceUnitSphereTransformation(&cov, &voxel_info.ellipse_rotation, &voxel_info.ellipse_scale);
        }

        voxels->insert(std::make_pair(key, voxel_info));
      }
    }
  }
}
}  // namespace ohm


================================================
FILE: ohm/RayMapperTrace.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_RAYMAPPERTRACE_H_
#define OHM_RAYMAPPERTRACE_H_

#include "OhmConfig.h"

#include "Key.h"
#include "OccupancyType.h"
#include "RayMapper.h"

#include <ohmutil/VectorHash.h>

#include <glm/vec3.hpp>
#include <glm/vec4.hpp>

#include <glm/gtc/quaternion.hpp>

#include <unordered_map>
#include <unordered_set>

namespace ohm
{
class OccupancyMesh;

/// A @c RayMapper wrapper which adds Third Eye Scene debug visualisation to the process.
///
/// This only functions correctly if ohm is built with the option @c OHM_TES_DEBUG .
///
/// The debug visualisation adds significant overhead. @c ohm::Trace also has to be enabled.
///
/// The visualisation shows rays being integrated, occupied voxels and NDT ellipsoids if present.
///
/// See https://github.com/csiro-robotics/3rdEyeScene
class ohm_API RayMapperTrace : public RayMapper
{
public:
  /// Utility structure tracking the status of a voxel including (optional) ellipsoid shape.
  struct ohm_API VoxelState
  {
    glm::dquat ellipse_rotation{ 1, 0, 0, 0 };  ///< Rotation applied to a scaled sphere to generate the ellipse.
    glm::dvec3 ellipse_pos{ 0 };                ///< Global position of the voxel ellipse.
    glm::dvec3 ellipse_scale{ 1 };              ///< Ellipse scaling.
    OccupancyType type{ kNull };                ///< Voxel type.
  };

  /// Hash for a @c glm::i16vec4
  struct ohm_API I16Vec4Hash
  {
    /// Hash operator
    /// @param v Vector to hash
    /// @return 64-bit hash value for v - probably really poor on bucketing.
    inline uint64_t operator()(const glm::i16vec4 &v) const
    {
      // NOLINTNEXTLINE(readability-magic-numbers)
      return (uint64_t(v.w) << 48u) | (uint64_t(v.z) << 32u) | (uint64_t(v.y) << 16u) | uint64_t(v.x);
    }
  };

  /// Set for tracking voxel state.
  using VoxelMap = std::unordered_map<Key, VoxelState, Key::Hash>;
  /// Tracks touched voxels for NDT sending. We use the region key for XYZ and a W component [0, 7] which identifies
  /// a sector in the region. Each of the first 3 bits selects the lower or upper half of the region with bits
  /// (0, 1, 2) mapping to the (x, y, z) axes.
  ///
  /// Explicitly, the sector mappings are:
  ///
  /// Sector  | X range | Y range | Z range
  /// ------- | ------- | ------- | --------
  /// 0       | lower   | lower   | lower
  /// 1       | upper   | lower   | lower
  /// 2       | lower   | upper   | lower
  /// 3       | upper   | upper   | lower
  /// 4       | lower   | lower   | upper
  /// 5       | upper   | lower   | upper
  /// 6       | lower   | upper   | upper
  /// 7       | upper   | upper   | upper
  using SectorSet = std::unordered_set<glm::i16vec4, I16Vec4Hash>;

  /// Create a trace around the given @p map and mapper. These must outlive this object.
  /// @param map The map the @p true_mapper operates on.
  /// @param true_mapper The wrapped mapper.
  RayMapperTrace(OccupancyMap *map, RayMapper *true_mapper);

  /// Destructor.
  ~RayMapperTrace() override;

  /// Access the target map.
  /// @return The target map object.
  inline OccupancyMap *map() const { return map_; }
  /// Access the wrapped @c RayMapper .
  /// @return The wrapped mapper.
  inline RayMapper *trueMapper() const { return true_mapper_; }

  /// Validity check - passthrough to the wrapped mapper.
  /// @return True if the wrapped mapper is valid.
  bool valid() const override;

  /// Integrate the given rays into the @c trueMapper() . This also performs debug visualisation around the operation.
  /// @param rays The array of start/end point pairs to integrate.
  /// @param element_count The number of @c glm::dvec3 elements in @p rays, which is twice the ray count.
  /// @param intensities An array of intensity values matching the @p rays items. There is one intensity value per ray
  ///   so there are @c element_count/2 items. May be null to omit intensity values.
  /// @param timestamps An array of timestamp values matching the @p rays items. There is one timestamp value per ray
  ///   so there are @c element_count/2 items. May be null to omit timestamp values in which case the touch time layer
  ///   will not be updated.
  /// @param ray_update_flags @c RayFlag bitset used to modify the behaviour of this function.
  size_t integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities, const double *timestamps,
                       unsigned ray_update_flags) override;

private:
  /// Work out the sector key associated with @p key . See @c SectorSet .
  /// @param key The voxel key to translate.
  glm::i16vec4 sectorKey(const Key &key) const;

  /// Cache the initial state of voxels affected by the given @p ray set.
  void cacheState(const glm::dvec3 *rays, size_t element_count, VoxelMap *voxels, SectorSet *sectors = nullptr);

  OccupancyMap *map_;
  RayMapper *true_mapper_;
  std::unique_ptr<OccupancyMesh> imp_;
};
}  // namespace ohm

#endif  // OHM_RAYMAPPERTRACE_H_


================================================
FILE: ohm/RayMapperTsdf.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RayMapperTsdf.h"

#include "DefaultLayer.h"
#include "LineWalk.h"
#include "MapChunk.h"
#include "MapLayer.h"
#include "MapLayout.h"
#include "OccupancyMap.h"
#include "Voxel.h"
#include "VoxelBuffer.h"
#include "VoxelTsdf.h"

namespace ohm
{
RayMapperTsdf::RayMapperTsdf(OccupancyMap *map)
  : map_(map)
  , tsdf_layer_(map_->layout().layerIndex(default_layer::tsdfLayerName()))
{
  // Use Voxel to validate the layers.
  // In processing we use VoxelBuffer instead of Voxel objects. While Voxel makes for a neater API, using VoxelBuffer
  // makes for less overhead and yields better performance.
  Voxel<const VoxelTsdf> tsdf(map_, tsdf_layer_);

  tsdf_dim_ = tsdf.isLayerValid() ? tsdf.layerDim() : tsdf_dim_;
  valid_ = tsdf.isLayerValid();
}


RayMapperTsdf::~RayMapperTsdf() = default;


void RayMapperTsdf::setTsdfOptions(const TsdfOptions &options)
{
  tsdf_options_ = options;
  if (map_)
  {
    updateMapInfo(map_->mapInfo(), tsdf_options_);
  }
}


void RayMapperTsdf::setMaxWeight(float max_weight)
{
  tsdf_options_.max_weight = max_weight;
  if (map_)
  {
    updateMapInfo(map_->mapInfo(), tsdf_options_);
  }
}


void RayMapperTsdf::setDefaultTruncationDistance(float default_truncation_distance)
{
  tsdf_options_.default_truncation_distance = default_truncation_distance;
  if (map_)
  {
    updateMapInfo(map_->mapInfo(), tsdf_options_);
  }
}


void RayMapperTsdf::setDropoffEpsilon(float dropoff_epsilon)
{
  tsdf_options_.dropoff_epsilon = dropoff_epsilon;
  if (map_)
  {
    updateMapInfo(map_->mapInfo(), tsdf_options_);
  }
}


void RayMapperTsdf::setSparsityCompensationFactor(float sparsity_compensation_factor)
{
  tsdf_options_.sparsity_compensation_factor = sparsity_compensation_factor;
  if (map_)
  {
    updateMapInfo(map_->mapInfo(), tsdf_options_);
  }
}


size_t RayMapperTsdf::integrateRays(const glm::dvec3 *rays, size_t element_count, const float * /*intensities*/,
                                    const double *timestamps, unsigned /*ray_update_flags*/)
{
  KeyList keys;
  MapChunk *last_chunk = nullptr;
  VoxelBuffer<VoxelBlock> tsdf_buffer;

  const RayFilterFunction ray_filter = map_->rayFilter();
  const bool use_filter = bool(ray_filter);
  const auto tsdf_layer = tsdf_layer_;
  const auto tsdf_dim = tsdf_dim_;
  // Touch the map to flag changes.
  const auto touch_stamp = map_->touch();

  // Cached values for current ray to use in the update closure.
  glm::dvec3 sensor{};
  glm::dvec3 sample{};
  // Filtered start/end points.
  glm::dvec3 ray_start{};
  glm::dvec3 ray_end{};

  if (timestamps)
  {
    // Update first ray time if not yet set.
    map_->updateFirstRayTime(*timestamps);
  }

  const auto visit_func = [&](const Key &key, double enter_range, double exit_range) -> bool  //
  {
    (void)enter_range;  // Unused
    (void)exit_range;   // Unused
    // The update logic here is a little unclear as it tries to avoid outright branches.
    // The intended logic is described as follows:
    // 1. Select direct write or additive adjustment.
    //    - Make a direct, non-additive adjustment if one of the following conditions are met:
    //      - stop_adjustments is true
    //      - the voxel is uncertain
    //      - ray_update_flags and kRfExclude<Type> flags pass.
    //      - voxel is saturated
    //    - Otherwise add to present value.
    // 2. Select the value adjustment
    //    - current_value if one of the following conditions are met:
    //      - stop_adjustments is true (no longer making adjustments)
    //      - ray_update_flags and kRfExclude<Type> flags pass.
    //    - miss_value otherwise
    // 3. Calculate new value
    // 4. Apply saturation logic: only min saturation relevant
    //    -
    MapChunk *chunk =
      (last_chunk && key.regionKey() == last_chunk->region.coord) ? last_chunk : map_->region(key.regionKey(), true);
    if (chunk != last_chunk)
    {
      tsdf_buffer = VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[tsdf_layer]);
    }
    last_chunk = chunk;
    const unsigned voxel_index = ohm::voxelIndex(key, tsdf_dim);
    VoxelTsdf tsdf_voxel;
    tsdf_buffer.readVoxel(voxel_index, &tsdf_voxel);

    calculateTsdf(sensor, sample, map_->voxelCentreGlobal(key), tsdf_options_.default_truncation_distance,
                  tsdf_options_.max_weight, tsdf_options_.dropoff_epsilon, tsdf_options_.sparsity_compensation_factor,
                  &tsdf_voxel.weight, &tsdf_voxel.distance);
    tsdf_buffer.writeVoxel(voxel_index, tsdf_voxel);

    // Lint(KS): The analyser takes some branches which are not possible in practice.
    // NOLINTNEXTLINE(clang-analyzer-core.CallAndMessage)
    chunk->updateFirstValid(voxel_index);

    chunk->dirty_stamp = touch_stamp;
    // Update the touched_stamps with relaxed memory ordering. The important thing is to have an update,
    // not so much the sequencing. We really don't want to synchronise here.
    chunk->touched_stamps[tsdf_layer].store(touch_stamp, std::memory_order_relaxed);

    return true;
  };

  for (size_t i = 0; i < element_count; i += 2)
  {
    unsigned filter_flags = 0;
    ray_start = sensor = rays[i];
    ray_end = sample = rays[i + 1];

    if (use_filter)
    {
      if (!ray_filter(&ray_start, &ray_end, &filter_flags))
      {
        // Bad ray.
        continue;
      }
    }

    walkSegmentKeys(LineWalkContext(*map_, visit_func), ray_start, ray_end);
  }

  return element_count / 2;
}
}  // namespace ohm


================================================
FILE: ohm/RayMapperTsdf.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef RAYMAPPERTSDF_H
#define RAYMAPPERTSDF_H

#include "OhmConfig.h"

#include "CalculateSegmentKeys.h"
#include "KeyList.h"
#include "RayFilter.h"
#include "RayFlag.h"
#include "RayMapper.h"
#include "Voxel.h"
#include "VoxelTsdf.h"

#include <glm/vec3.hpp>

namespace ohm
{
/// A @c RayMapper implementation built around updating a TSDF map in CPU. This mapper supports TSDF
/// population of @c VoxelTsdf .
///
/// The @c integrateRays() implementation performs a single threaded walk of the voxels to update and touches
/// those voxels one at a time, updating their tsdf values. The given @c OccupancyMap must have a @c VoxelTsdf
/// layer.
class ohm_API RayMapperTsdf : public RayMapper
{
public:
  /// Constructor, wrapping the interface around the given @p map .
  ///
  /// @param map The target map. Must outlive this class.
  explicit RayMapperTsdf(OccupancyMap *map);

  /// Destructor
  ~RayMapperTsdf() override;

  /// Has the map been successfully validated?
  /// @return True if valid and @c integrateRays() is safe to call.
  inline bool valid() const override { return valid_; }

  void setTsdfOptions(const TsdfOptions &options);
  const TsdfOptions &tsdfOptions() const { return tsdf_options_; }

  void setMaxWeight(float max_weight);
  inline float maxWeight() const { return tsdf_options_.max_weight; }
  void setDefaultTruncationDistance(float default_truncation_distance);
  inline float defaultTruncationDistance() const { return tsdf_options_.default_truncation_distance; }
  void setDropoffEpsilon(float dropoff_epsilon);
  inline float dropoffEpsilon() const { return tsdf_options_.dropoff_epsilon; }
  void setSparsityCompensationFactor(float sparsity_compensation_factor);
  inline float sparsityCompensationFactor() const { return tsdf_options_.sparsity_compensation_factor; }

  /// Performs the ray integration.
  ///
  /// This is updated in a single threaded fashion. For each ray we walk the affected voxel @c Key set and
  /// update those voxels. Voxels along each line segment have their tsdf value updated.
  ///
  /// Should only be called if @c valid() is true.
  ///
  /// @param rays The array of start/end point pairs to integrate.
  /// @param element_count The number of @c glm::dvec3 elements in @p rays , which is twice the ray count.
  /// @param intensities An array of intensity values matching the @p rays items. There is one intensity value per ray
  ///   so there are @c element_count/2 items. May be null to omit intensity values.
  /// @param timestamps An array of timestamp values matching the @p rays items. There is one timestamp value per ray
  ///   so there are @c element_count/2 items. May be null to omit timestamp values in which case the touch time layer
  ///   will not be updated.
  /// @param ray_update_flags @c RayFlag bitset used to modify the behaviour of this function. All flags are
  /// implemented.
  size_t integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities, const double *timestamps,
                       unsigned ray_update_flags) override;

  using RayMapper::integrateRays;

protected:
  OccupancyMap *map_ = nullptr;      ///< Target map.
  int tsdf_layer_ = -1;              ///< Cached tsdf layer index.
  glm::u8vec3 tsdf_dim_{ 0, 0, 0 };  ///< Cached tsdf layer voxel dimensions. Voxel mean must exactly match.
  TsdfOptions tsdf_options_;         ///< TSDF options.
  bool valid_ = false;               ///< Has layer validation passed?
};

}  // namespace ohm

#endif  // RAYMAPPERTSDF_H


================================================
FILE: ohm/RayPattern.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RayPattern.h"

#include "private/RayPatternDetail.h"

#include <glm/ext.hpp>

namespace ohm
{
RayPattern::RayPattern(ohm::RayPatternDetail *detail)
  : imp_(detail)
{}

RayPattern::RayPattern()
  : RayPattern(new ohm::RayPatternDetail)
{}

RayPattern::~RayPattern() = default;

void RayPattern::addPoints(const glm::dvec3 *points, size_t point_count)
{
  for (size_t pidx = 0; pidx < point_count; ++pidx)
  {
    imp_->sample_pairs.emplace_back(glm::dvec3(0.0));
    imp_->sample_pairs.push_back(points[pidx]);
  }
}

void RayPattern::addRays(const glm::dvec3 *ray_pairs, size_t elements)
{
  for (size_t ridx = 0; ridx + 1 < elements; ridx += 2)
  {
    imp_->sample_pairs.push_back(ray_pairs[ridx + 0]);
    imp_->sample_pairs.push_back(ray_pairs[ridx + 1]);
  }
}

void RayPattern::addRay(const glm::dvec3 &ray_start, const glm::dvec3 &ray_end)
{
  imp_->sample_pairs.push_back(ray_start);
  imp_->sample_pairs.push_back(ray_end);
}

size_t RayPattern::rayCount() const
{
  return imp_->sample_pairs.size() / 2;
}

const glm::dvec3 *RayPattern::rayPoints() const
{
  return imp_->sample_pairs.data();
}

size_t RayPattern::buildRays(std::vector<glm::dvec3> *rays, const glm::dvec3 &position, const glm::dquat &rotation,
                             double scaling) const
{
  rays->clear();
  rays->reserve(rayCount());
  for (glm::dvec3 sample : imp_->sample_pairs)
  {
    sample = rotation * (scaling * sample) + position;
    rays->push_back(sample);
  }

  return rays->size();
}

size_t RayPattern::buildRays(std::vector<glm::dvec3> *rays, const glm::dmat4 &pattern_transform) const
{
  rays->clear();
  rays->reserve(rayCount());
  for (glm::dvec3 sample : imp_->sample_pairs)
  {
    // Apply transformation.
    sample = glm::dvec3(pattern_transform * glm::dvec4(sample, 1.0));
    rays->push_back(sample);
  }

  return rays->size();
}
}  // namespace ohm


================================================
FILE: ohm/RayPattern.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef RAYPATTERN_H
#define RAYPATTERN_H

#include "OhmConfig.h"

#include <glm/fwd.hpp>

#include <memory>
#include <vector>

namespace ohm
{
struct RayPatternDetail;

/// A @c RayPattern defines a set of ray end points with a common origin. The class may be used to define a custom
/// point set or derived to define a pre-determined pattern.
///
/// The @c RayPattern is intended for use with the @c ClearingPattern utility.
class ohm_API RayPattern
{
protected:
  /// Constructor for sub-classes to derive the detail.
  /// @param detail Custom implementation or null to use the default..
  explicit RayPattern(ohm::RayPatternDetail *detail);

public:
  /// Create an empty ray pattern.
  RayPattern();
  /// Virtual destructor.
  virtual ~RayPattern();

  /// Add a set of points to the pattern. This implicitly adds a ray origin for each @p point at (0, 0, 0).
  /// @param points The array of points to add.
  /// @param point_count Number of elements in @p points.
  void addPoints(const glm::dvec3 *points, size_t point_count);

  /// Add a single point to the pattern.
  /// @param point The new point to add.
  inline void addPoint(const glm::dvec3 &point) { addPoints(&point, 1); }

  /// Add a set of ray start/end point pairs to the pattern.
  /// @param ray_pairs An array of ray start/end point pairs in sensor space.
  /// @param elements Number of elements in @p ray_pairs. Must be even for well defined behaviour.
  void addRays(const glm::dvec3 *ray_pairs, size_t elements);

  /// Add a single ray to the pattern.
  /// @param ray_start Origin of the ray in sensor space.
  /// @param ray_end End point of the ray in sensor space.
  void addRay(const glm::dvec3 &ray_start, const glm::dvec3 &ray_end);

  /// Query the number of rays in the pattern. This is the number of start/end point pairs.
  /// @return The number of ray pairs in the pattern.
  size_t rayCount() const;

  /// Access the ray sample array. This is an array of start/end point pairs (in that order).
  /// @return A pointer to the start of the array of ray pairs in the pattern. There are @c rayCount() pairs
  ///   meaning there are <tt>2 * rayCount()</tt> elements.
  const glm::dvec3 *rayPoints() const;

  /// Build the ray set from the base pattern of points. The @p rays container is populated with pairs of start/end
  /// points which can be used with @c OccupancyMap::intergratePoints(). The first item of every pair is equal to
  /// @p position, while the second is a point from the pattern, rotated by @p rotation and translated by @p position.
  /// @param rays The ray set to populate. Cleared before use.
  /// @param position The translation for the pattern application.
  /// @param rotation The rotation for the pattern application.
  /// @param scaling Optional uniform scaling to apply to the pattern.
  /// @return The number of elements added to @p rays (twice the @c pointCount()).
  size_t buildRays(std::vector<glm::dvec3> *rays, const glm::dvec3 &position, const glm::dquat &rotation,
                   double scaling = 1.0) const;

  /// @overload
  size_t buildRays(std::vector<glm::dvec3> *rays, const glm::dmat4 &pattern_transform) const;

private:
  std::unique_ptr<ohm::RayPatternDetail> imp_;
};
}  // namespace ohm

// 0. Pre-build pattern.
// 1. Position and rotate pattern.

#endif  // RAYPATTERN_H


================================================
FILE: ohm/RayPatternConical.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RayPatternConical.h"

#include <3esservermacros.h>

#include <glm/ext.hpp>

#define DEBUG_VISUALISE 0

#if defined(TES_ENABLE) && !DEBUG_VISUALISE
#undef TES_ENABLE
#endif  // defined(TES_ENABLE) && !DEBUG_VISUALISE

namespace ohm
{
RayPatternConical::RayPatternConical(const glm::dvec3 &cone_axis, double cone_angle, double range,
                                     double angular_resolution, double min_range)
{
  TES_STMT(const double cone_radius = range * std::tan(cone_angle));
  TES_CONE_T(g_tes, TES_COLOUR_A(YellowGreen, 128), tes::Id(this),
             tes::Directional(tes::Vector3d(0.0), tes::Vector3d(glm::value_ptr(cone_axis)), cone_radius, range));
  TES_CONE_W(g_tes, TES_COLOUR_A(YellowGreen, 128), tes::Id(this) + 100,
             tes::Directional(tes::Vector3d(0.0), tes::Vector3d(glm::value_ptr(cone_axis)), cone_radius, range));

  // First ensure the cone axis is normalised.
  const glm::dvec3 &cone_normal = glm::normalize(cone_axis);

  TES_ARROW(g_tes, TES_COLOUR(Yellow), tes::Id(this),
            tes::Directional(tes::Vector3d(0.0), tes::Vector3d(glm::value_ptr(cone_normal))));

  // Add the cone axis.
  addRay(cone_normal * min_range, cone_normal * range);

  // Find a perpendicular to the cone normal. We will use this to define a radius vector around the unit circle through
  // the cone.
  // To generate, we simply sizzle the cone normal components.
  const glm::dvec3 deflection_base(cone_normal.z, cone_normal.x, cone_normal.y);

  TES_ARROW(g_tes, TES_COLOUR(Green), tes::Id(this) + 1,
            tes::Directional(tes::Vector3d(0.0), tes::Vector3d(glm::value_ptr(deflection_base))));

  TES_SERVER_UPDATE(g_tes, 0.0f);

  // Now walk inscribe the circle which makes the base.
  // NOLINTNEXTLINE(clang-analyzer-security.FloatLoopCounter)
  for (double circle_angle = 0; circle_angle < 2 * M_PI; circle_angle += angular_resolution)
  {
    // Define a deflection angle for this circle angle.
    const glm::dquat deflection_rotation = glm::angleAxis(circle_angle, cone_normal);
    // Define the deflection vector which we'll use to deflect the cone axis.
    const glm::dvec3 deflection_axis = deflection_rotation * deflection_base;

    TES_ARROW(g_tes, TES_COLOUR(Cyan), tes::Id(),
              tes::Directional(tes::Vector3d(0.0), tes::Vector3d(glm::value_ptr(deflection_axis))));

    // Now create deflected rates starting at angular_resolution up to the cone angle.
    // Lint(KS): use of float for loop checks out. Precision isn't an issue here.
    // NOLINTNEXTLINE(clang-analyzer-security.FloatLoopCounter, readability-magic-numbers)
    for (double deflection_angle = angular_resolution; deflection_angle <= 0.5 * cone_angle;
         deflection_angle += angular_resolution)
    {
      // Rotate the cone_normal by the deflection angle around the deflection axis and scale by range.
      const glm::dquat rotation = glm::angleAxis(deflection_angle, deflection_axis);
      const glm::dvec3 ray_dir = rotation * cone_normal;
      addRay(ray_dir * min_range, ray_dir * range);

      TES_LINE(g_tes, TES_COLOUR(PowderBlue), tes::Id(), tes::Vector3d(0.0), tes::Vector3d(glm::value_ptr(ray_dir)));
    }

    TES_SERVER_UPDATE(g_tes, 0.0f);
  }

#ifdef TES_ENABLE
  std::vector<glm::vec3> debug_points;
  debug_points.reserve(2 * pointCount());
  const glm::dvec3 *points = this->points();
  for (size_t i = 0; i < pointCount(); ++i)
  {
    debug_points.push_back(glm::vec3(0.0f));
    debug_points.push_back(glm::vec3(points[i]));
  }
  TES_IF(!debug_points.empty())
  {
    TES_LINES(g_tes, TES_COLOUR(SteelBlue), glm::value_ptr(debug_points.front()), debug_points.size(),
              sizeof(debug_points.front()));
  }
#endif  // TES_ENABLE

  TES_SERVER_UPDATE(g_tes, 0.0f);
  TES_ARROW_END(g_tes, tes::Id(this));
  TES_ARROW_END(g_tes, tes::Id(this) + 1);
  TES_CONE_END(g_tes, tes::Id(this));
  TES_CONE_END(g_tes, tes::Id(this) + 100);
}
}  // namespace ohm


================================================
FILE: ohm/RayPatternConical.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef RAYPATTERNCONICAL_H
#define RAYPATTERNCONICAL_H

#include "OhmConfig.h"

#include "RayPattern.h"

namespace ohm
{
/// A conical @c RayPattern construction
class ohm_API RayPatternConical : public RayPattern
{
public:
  /// Constructor.
  /// @param cone_axis The cone's primary axis/directory - expected to be a unit vector.
  /// @param cone_angle The angle between the cone's axis and the cone wall (radians).
  /// @param range Length of the rays in the cone. This makes the cone bottom spherical.
  /// @param angular_resolution Approximate angular separation between rays (radians).
  /// @param min_range Controls the near point of each line segment. Zero starts all rays at the apex, while
  /// increasing this value moves the starting points out. Must be less than @p range .
  RayPatternConical(const glm::dvec3 &cone_axis, double cone_angle, double range, double angular_resolution,
                    double min_range = 0);
};
}  // namespace ohm

#endif  // RAYPATTERNCONICAL_H


================================================
FILE: ohm/RaysQuery.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RaysQuery.h"

#include "private/RaysQueryDetail.h"

#include "CalculateSegmentKeys.h"
#include "KeyList.h"
#include "LineWalk.h"
#include "MapLayer.h"
#include "MapLayout.h"
#include "OccupancyMap.h"
#include "Voxel.h"
#include "VoxelBuffer.h"
#include "VoxelMean.h"
#include "VoxelOccupancy.h"

namespace ohm
{
RaysQuery::RaysQuery(RaysQueryDetail *detail)
  : Query(detail)
{}


RaysQuery::RaysQuery()
  : RaysQuery(new RaysQueryDetail)
{}


RaysQuery::~RaysQuery() = default;


void RaysQuery::setVolumeCoefficient(double coefficient)
{
  RaysQueryDetail *d = imp();
  d->volume_coefficient = coefficient;
}


double RaysQuery::volumeCoefficient() const
{
  const RaysQueryDetail *d = imp();
  return d->volume_coefficient;
}


void RaysQuery::setRays(const glm::dvec3 *rays, size_t element_count)
{
  RaysQueryDetail *d = imp();
  d->rays_in.clear();
  addRays(rays, element_count);
}


void RaysQuery::addRays(const glm::dvec3 *rays, size_t element_count)
{
  RaysQueryDetail *d = imp();
  // Ensure we add in pairs.
  for (size_t i = 0; i < element_count; i += 2)
  {
    d->rays_in.emplace_back(rays[i]);
    d->rays_in.emplace_back(rays[i + 1]);
  }
}


void RaysQuery::addRay(const glm::dvec3 &origin, const glm::dvec3 &end_point)
{
  RaysQueryDetail *d = imp();
  d->rays_in.emplace_back(origin);
  d->rays_in.emplace_back(end_point);
}

void RaysQuery::clearRays()
{
  RaysQueryDetail *d = imp();
  d->rays_in.clear();
}


const glm::dvec3 *RaysQuery::rays(size_t *count) const
{
  const RaysQueryDetail *d = imp();
  if (count)
  {
    *count = d->rays_in.size();
  }
  return d->rays_in.data();
}


size_t RaysQuery::numberOfRays() const
{
  const RaysQueryDetail *d = imp();
  return d->rays_in.size() / 2;
}


bool RaysQuery::onExecute()
{
  RaysQueryDetail *d = imp();

  if (!d->map && !d->valid_layers)
  {
    return false;
  }

  KeyList keys;
  MapChunk *last_chunk = nullptr;
  VoxelBuffer<const VoxelBlock> occupancy_buffer;
  double unobserved_volume = 0;
  float range = 0;
  OccupancyType terminal_state = OccupancyType::kNull;
  Key terminal_key(nullptr);

  auto *map = d->map;
  const RayFilterFunction ray_filter = map->rayFilter();
  const bool use_filter = bool(ray_filter);
  const auto occupancy_layer = d->occupancy_layer;
  const auto occupancy_dim = d->occupancy_dim;
  const auto occupancy_threshold_value = map->occupancyThresholdValue();
  const auto volume_coefficient = d->volume_coefficient;

  const auto visit_func = [&](const Key &key, double enter_range, double exit_range) -> bool  //
  {
    // Work out the index of the voxel in it's region.
    const unsigned voxel_index = ohm::voxelIndex(key, occupancy_dim);
    float occupancy_value = unobservedOccupancyValue();
    // Ensure the MapChunk pointer is up to date.
    MapChunk *chunk =
      (last_chunk && key.regionKey() == last_chunk->region.coord) ? last_chunk : map->region(key.regionKey(), false);
    if (chunk)
    {
      if (chunk != last_chunk)
      {
        occupancy_buffer = VoxelBuffer<const VoxelBlock>(chunk->voxel_blocks[occupancy_layer]);
      }
      occupancy_buffer.readVoxel(voxel_index, &occupancy_value);
    }
    last_chunk = chunk;
    // Check voxel occupancy status.
    const bool is_unobserved = occupancy_value == unobservedOccupancyValue();
    const bool is_occupied = !is_unobserved && occupancy_value > occupancy_threshold_value;
    unobserved_volume +=
      is_unobserved ?
        (volume_coefficient * (exit_range * exit_range * exit_range - enter_range * enter_range * enter_range)) :
        0.0f;
    range = (!is_occupied) ? float(exit_range) : range;
    // Resolve the voxel state.
    terminal_state =
      is_unobserved ? OccupancyType::kUnobserved : (is_occupied ? OccupancyType::kOccupied : OccupancyType::kFree);
    terminal_key = key;

    return !is_occupied;
  };

  // Size the output arrays.
  d->ranges.reserve(d->rays_in.size() / 2);
  d->intersected_voxels.reserve(d->rays_in.size() / 2);
  d->unobserved_volumes_out.reserve(d->rays_in.size() / 2);
  d->terminal_states_out.reserve(d->rays_in.size() / 2);

  glm::dvec3 start;
  glm::dvec3 end;
  unsigned filter_flags;
  for (size_t i = 0; i < d->rays_in.size(); i += 2)
  {
    filter_flags = 0;
    start = d->rays_in[i];
    end = d->rays_in[i + 1];

    unobserved_volume = 0.0f;
    range = 0.0f;

    if (use_filter && !ray_filter(&start, &end, &filter_flags))
    {
      // Filtered ray.
      d->ranges.emplace_back(range);
      d->unobserved_volumes_out.emplace_back(unobserved_volume);
      d->terminal_states_out.emplace_back(OccupancyType::kNull);
      d->intersected_voxels.emplace_back(Key::kNull);
      continue;
    }

    walkSegmentKeys(LineWalkContext(*map, visit_func), start, end);

    d->ranges.emplace_back(range);
    d->unobserved_volumes_out.emplace_back(unobserved_volume);
    d->terminal_states_out.emplace_back(terminal_state);
    d->intersected_voxels.emplace_back(terminal_key);
  }

  d->number_of_results = d->ranges.size();

  return true;
}


const double *RaysQuery::unobservedVolumes() const
{
  const RaysQueryDetail *d = imp();
  return d->unobserved_volumes_out.data();
}


const OccupancyType *RaysQuery::terminalOccupancyTypes() const
{
  const RaysQueryDetail *d = imp();
  return d->terminal_states_out.data();
}


void RaysQuery::onSetMap()
{
  RaysQueryDetail *d = imp();
  auto map = d->map;
  if (!map)
  {
    d->occupancy_layer = -1;
    d->valid_layers = false;
    return;
  }

  d->occupancy_layer = map->layout().occupancyLayer();

  // Use Voxel to validate the layers.
  // In processing we use VoxelBuffer instead of Voxel objects. While Voxel makes for a neater API, using VoxelBuffer
  // makes for less overhead and yields better performance.
  Voxel<const float> occupancy(map, d->occupancy_layer);

  d->occupancy_dim = occupancy.isLayerValid() ? occupancy.layerDim() : d->occupancy_dim;

  // Validate we only have an occupancy layer.
  d->valid_layers = occupancy.isLayerValid();
}


bool RaysQuery::onExecuteAsync()
{
  return false;
}


void RaysQuery::onReset(bool hard_reset)
{
  RaysQueryDetail *d = imp();
  d->unobserved_volumes_out.clear();
  d->terminal_states_out.clear();
  if (hard_reset)
  {
    d->rays_in.clear();
  }
}


RaysQueryDetail *RaysQuery::imp()
{
  return static_cast<RaysQueryDetail *>(imp_);
}


const RaysQueryDetail *RaysQuery::imp() const
{
  return static_cast<const RaysQueryDetail *>(imp_);
}
}  // namespace ohm


================================================
FILE: ohm/RaysQuery.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_RAYSQUERY_H
#define OHM_RAYSQUERY_H

#include "OhmConfig.h"

#include "OccupancyType.h"
#include "Query.h"
#include "QueryFlag.h"

#include <glm/fwd.hpp>

#include <vector>

namespace ohm
{
struct RaysQueryDetail;

/// A query which takes a set of rays and searches for existing obstructions along that ray without modifying the input
/// map.
///
/// The query provides several pieces of information about each ray:
/// - @c ranges() contains the distance until reaching either an occupied voxel, or the end of the ray.
/// - @c unobservedVolumes() is an approximate volumetric accumulation of the voxels traversed by the ray.
/// - @c terminalOccupancyTypes() yields the type of voxel each ray query terminated on.
/// - @c intersectedVoxels() contains the voxel key for each terminating voxel. Contains null keys for filtered rays
///   (@c OccupancyMap::rayFilter()).
///
/// The number of results @c numberOfResults() should match the @c numberOfRays() .
///
/// The @c unobservedVolumes() traces along each ray and accrues the previously unobserved volume that would be observed
/// (assuming no new obstacles are encountered). This is calculated by accruing `(exit_range^3 - enter_range^3)` for
/// each previously unobserved voxel through which the ray passes prior to encountering an occupied voxel. This can be
/// utilised to perform integration in spherical coordinates, where the angular weights and normalisation are applied
/// externally for each ray. The calculation is optionally scaled by @c volumeCoefficient() , which defaults to one.
///
/// Where @c enter_range and @c exit_range are the ranges at which the ray enters and leaves a voxel respectively.
/// This value is accumulated for each unobserved or null voxel.
///
/// Note: on a hard reset, the set of rays is cleared, while a soft reset leaves the ray set unchanged.
class ohm_API RaysQuery : public Query
{
public:
  /// Default flags to execute this query with.
  static const unsigned kDefaultFlags = kQfNoCache;

protected:
  /// Constructor used for inherited objects. This supports deriving @p LineQueryDetail into
  /// more specialised forms.
  /// @param detail pimple style data structure. When null, a @c LineQueryDetail is allocated by
  /// this method.
  explicit RaysQuery(RaysQueryDetail *detail);

public:
  /// Constructor. The map and rays must be set before using.
  RaysQuery();

  /// Destructor
  ~RaysQuery() override;

  // --- Parameterisation ---

  /// Set the coefficient used in calculating the @c unobservedVolumes() .
  /// See class documentation for detail.
  /// @param coefficient The new coefficient.
  void setVolumeCoefficient(double coefficient);
  /// Get the coefficient used in calculating the @c unobservedVolumes() .
  /// See class documentation for detail.
  /// @return The set coefficient.
  double volumeCoefficient() const;

  /// Set the rays to intersect.
  /// @param rays Origin/end point pairs.
  /// @param element_count Number of elements in @p rays . Expected to be even to account for the origin/end pairing.
  void setRays(const glm::dvec3 *rays, size_t element_count);
  /// Set the rays to intersect.
  /// @param rays Origin/end point pairs. The size is expected to be even to account for the origin/end pairing.
  void setRays(const std::vector<glm::dvec3> &rays);

  /// Add rays to the existing set.
  /// @param rays Origin/end point pairs.
  /// @param element_count Number of elements in @p rays . Expected to be even to account for the origin/end pairing.
  void addRays(const glm::dvec3 *rays, size_t element_count);
  /// Add rays to the existing set.
  /// @param rays Origin/end point pairs. The size is expected to be even to account for the origin/end pairing.
  void addRays(const std::vector<glm::dvec3> &rays);
  /// Add a single ray to the existing set.
  /// @param origin The ray origin.
  /// @param end_point The ray end_point.
  void addRay(const glm::dvec3 &origin, const glm::dvec3 &end_point);

  /// Clear the existing ray set. Also cleared on a hard @c reset(true) .
  void clearRays();

  /// Query the array of query rays.
  const glm::dvec3 *rays(size_t *count = nullptr) const;
  /// Query the number of query rays.
  size_t numberOfRays() const;

  // --- Results ---

  /// An approximation of the volume of previously unobserved space each ray traverses. The number of elements matches
  /// @c numberOfResults() .
  /// @return The unobserved volume traversed by the rays.
  const double *unobservedVolumes() const;

  /// The occupancy type of the voxels at which each ray terminates. This will be @c OccupancyType::kOccupied if an
  /// occupied voxel is encountered along the ray, otherwise it will be the type of the last voxel along the ray. The
  /// number of elements matches @c numberOfResults() .
  /// @return The set of end voxel states for the rays.
  const OccupancyType *terminalOccupancyTypes() const;

protected:
  void onSetMap() override;
  bool onExecute() override;
  bool onExecuteAsync() override;
  void onReset(bool hard_reset) override;

  /// Access internal details.
  /// @return Internal details.
  RaysQueryDetail *imp();
  /// Access internal details.
  /// @return Internal details.
  const RaysQueryDetail *imp() const;
};

inline void RaysQuery::setRays(const std::vector<glm::dvec3> &rays)
{
  return setRays(rays.data(), rays.size());
}

inline void RaysQuery::addRays(const std::vector<glm::dvec3> &rays)
{
  addRays(rays.data(), rays.size());
}
}  // namespace ohm

#endif  // OHM_RAYSQUERY_H


================================================
FILE: ohm/Stream.cpp
================================================
// Copyright (c) 2015
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "Stream.h"

#ifdef OHM_ZIP
#include <zlib.h>
#endif  // OHM_ZIP

#include <cstring>
#include <fstream>
#include <string>

namespace ohm
{
#ifdef OHM_ZIP
struct Compression
{
  z_stream stream;
  Byte *buffer = nullptr;
  unsigned buffer_size = 0;
  bool initialised = false;

  explicit Compression(unsigned buffer_size = 16 * 1024u);
  ~Compression();

  int initDeflate();
  int doneDeflate();

  int initInflate();
  int doneInflate();
};


Compression::Compression(unsigned buffer_size)  // NOLINT
  : buffer_size(buffer_size)
{
  memset(&stream, 0u, sizeof(stream));
  buffer = new Byte[buffer_size];
}


Compression::~Compression()
{
  delete[] buffer;
}


int Compression::initDeflate()
{
  if (!initialised)
  {
    memset(&stream, 0u, sizeof(stream));
    initialised = true;
    return deflateInit(&stream, Z_DEFAULT_COMPRESSION);
  }
  return Z_OK;
}


int Compression::doneDeflate()
{
  if (initialised)
  {
    initialised = false;
    return deflateEnd(&stream);
  }
  return Z_OK;
}


int Compression::initInflate()
{
  if (!initialised)
  {
    memset(&stream, 0u, sizeof(stream));
    initialised = true;
    return inflateInit(&stream);
  }
  return Z_OK;
}


int Compression::doneInflate()
{
  if (initialised)
  {
    initialised = false;
    return inflateEnd(&stream);
  }
  return Z_OK;
}

#endif  // OHM_ZIP

struct StreamPrivate
{
  std::string file_path;
  unsigned flags = 0;
};

struct InputStreamPrivate : StreamPrivate
{
  std::ifstream in;
#ifdef OHM_ZIP
  Compression compress;
#endif  // OHM_ZIP
};

struct OutputStreamPrivate : StreamPrivate
{
  std::ofstream out;
#ifdef OHM_ZIP
  Compression compress;
  bool needs_flush = false;
#endif  // OHM_ZIP
};


const std::string &Stream::filePath() const
{
  return imp_->file_path;
}


unsigned Stream::flags() const
{
  return imp_->flags;
}


bool Stream::open(const std::string &file_path, unsigned flags)
{
  close();
  return doOpen(file_path, flags);
}


void Stream::close()
{
  if (isOpen())
  {
    flush();
    doClose();
  }
}


void Stream::seek(size_t pos)
{
  flush();
  doSeek(pos);
}


size_t Stream::tell()
{
  return doTell();
}


Stream::Stream(StreamPrivate *imp)
  : imp_(imp)
{
  imp_->flags = 0u;
}


Stream::~Stream() = default;


InputStream::InputStream(const std::string &file_path, unsigned flags)
  : Stream(new InputStreamPrivate)
{
  if (!file_path.empty())
  {
    InputStream::doOpen(file_path, flags);
  }
}


InputStream::~InputStream()
{
  close();
  delete imp();
}


void InputStream::setCompressedFlag()
{
#ifdef OHM_ZIP
  if (!isOpen())
  {
    return;
  }

  unsigned &flags = imp()->flags;
  bool changed = !(flags & kSfCompress);
  flags |= kSfCompress;
  if (changed)
  {
    imp()->compress.initInflate();
  }
#endif  // OHM_ZIP
}


unsigned InputStream::read(void *buffer, unsigned max_bytes)
{
#ifdef OHM_ZIP
  if (imp()->flags & kSfCompress)
  {
    InputStreamPrivate &imp = *this->imp();
    int ret;
    unsigned have;

    imp.compress.stream.avail_out = max_bytes;
    imp.compress.stream.next_out = static_cast<unsigned char *>(buffer);

    do
    {
      if (!imp.compress.stream.avail_in)
      {
        // No data currently available. Prime the buffer.
        const unsigned read_bytes = readRaw(imp.compress.buffer, imp.compress.buffer_size);
        imp.compress.stream.avail_in = read_bytes;
        imp.compress.stream.next_in = imp.compress.buffer;

        if (!imp.compress.stream.avail_in)
        {
          // Nothing available.
          break;
        }
      }

      ret = inflate(&imp.compress.stream, Z_NO_FLUSH);

      if (ret != Z_OK && ret == Z_STREAM_END)
      {
        return max_bytes - imp.compress.stream.avail_out;
      }
    } while (imp.compress.stream.avail_out);

    have = max_bytes - imp.compress.stream.avail_out;
    return have;
  }
#endif  // OHM_ZIP
  return readRaw(buffer, max_bytes);
}


unsigned InputStream::readRaw(void *buffer, unsigned max_bytes)
{
  std::istream &in = imp()->in;
  const std::ifstream::pos_type initial_pos = in.tellg();
  in.read(static_cast<char *>(buffer), max_bytes);
  const std::ifstream::pos_type end_pos = in.tellg();
  auto read = unsigned(end_pos - initial_pos);
  return read;
}


bool InputStream::isOpen() const
{
  return imp()->in.is_open();
}


void InputStream::flush()
{}


bool InputStream::doOpen(const std::string &file_path, unsigned flags)
{
  imp()->in.open(file_path.c_str(), std::ios_base::binary);
  imp()->file_path = file_path;
#ifndef OHM_ZIP
  flags &= ~SF_Compress;
#endif  // OHM_ZIP
  imp()->flags = flags;
#ifdef OHM_ZIP
  if (flags & kSfCompress)
  {
    imp()->compress.initInflate();
  }
#endif  // OHM_ZIP
  return imp()->in.is_open();
}


void InputStream::doClose()
{
#ifdef OHM_ZIP
  if (imp_->flags & kSfCompress)
  {
    imp()->compress.doneInflate();
  }
#endif  // OHM_ZIP
  imp()->in.close();
  imp_->flags = 0;
  imp_->file_path = std::string();
}


void InputStream::doSeek(size_t pos)
{
  imp()->in.seekg(pos, std::ios_base::beg);
}


size_t InputStream::doTell()
{
  return imp()->in.tellg();
}


InputStreamPrivate *InputStream::imp()
{
  return static_cast<InputStreamPrivate *>(imp_);
}


const InputStreamPrivate *InputStream::imp() const
{
  return static_cast<const InputStreamPrivate *>(imp_);
}


OutputStream::OutputStream(const std::string &file_path, unsigned flags)
  : Stream(new OutputStreamPrivate)
{
  if (!file_path.empty())
  {
    OutputStream::doOpen(file_path, flags);
  }

#ifdef OHM_ZIP
  imp()->needs_flush = false;
#endif  // OHM_ZIP
}


OutputStream::~OutputStream()
{
  close();
  delete imp();
}


unsigned OutputStream::write(const void *buffer, unsigned max_bytes)
{
#ifdef OHM_ZIP
  if (imp()->flags & kSfCompress)
  {
    OutputStreamPrivate &imp = *this->imp();
    int ret;

    imp.compress.stream.next_in = (Bytef *)buffer;  // NOLINT
    imp.compress.stream.avail_in = max_bytes;

    if (imp.compress.stream.avail_out == 0)
    {
      imp.compress.stream.avail_out = imp.compress.buffer_size;
      imp.compress.stream.next_out = imp.compress.buffer;
    }

    do
    {
      ret = deflate(&imp.compress.stream, Z_NO_FLUSH);
      if (ret != Z_OK)
      {
        return ~unsigned(0u);
      }
      imp.needs_flush = true;

      if (imp.compress.stream.avail_out == 0)
      {
        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
        imp.out.write(reinterpret_cast<char *>(imp.compress.buffer), imp.compress.buffer_size);
        imp.compress.stream.avail_out = imp.compress.buffer_size;
        imp.compress.stream.next_out = imp.compress.buffer;
      }
    } while (imp.compress.stream.avail_in);

    return max_bytes;
  }
#endif  // OHM_ZIP
  return writeUncompressed(buffer, max_bytes);
}


unsigned OutputStream::writeUncompressed(const void *buffer, unsigned max_bytes)
{
  imp()->out.write(static_cast<const char *>(buffer), max_bytes);
  if (imp()->out.good())
  {
    return max_bytes;
  }

  return 0;
}


bool OutputStream::isOpen() const
{
  return imp()->out.is_open();
}


void OutputStream::flush()
{
  OutputStreamPrivate &imp = *this->imp();
#ifdef OHM_ZIP
  // Finish deflating.
  if (imp.needs_flush && imp.flags & kSfCompress)
  {
    int ret;
    imp.compress.stream.next_in = nullptr;
    imp.compress.stream.avail_in = 0;
    do
    {
      ret = deflate(&imp.compress.stream, Z_FINISH);
      unsigned have = imp.compress.buffer_size - imp.compress.stream.avail_out;
      if (have)
      {
        // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
        imp.out.write(reinterpret_cast<char *>(imp.compress.buffer), have);
      }

      imp.compress.stream.avail_out = imp.compress.buffer_size;
      imp.compress.stream.next_out = imp.compress.buffer;
    } while (ret == Z_OK);

    imp.needs_flush = false;
  }
#endif  // OHM_ZIP
  imp.out.flush();
}


bool OutputStream::doOpen(const std::string &file_path, unsigned flags)
{
  imp()->out.open(file_path.c_str(), std::ios_base::binary);
  imp()->file_path = file_path;
#ifndef OHM_ZIP
  flags &= ~SF_Compress;
#endif  // OHM_ZIP
  imp()->flags = flags;
#ifdef OHM_ZIP
  if (flags & kSfCompress)
  {
    imp()->compress.initDeflate();
  }
#endif  // OHM_ZIP
  return imp()->out.is_open();
}


void OutputStream::doClose()
{
#ifdef OHM_ZIP
  if (imp_->flags & kSfCompress)
  {
    imp()->compress.doneDeflate();
  }
#endif  // OHM_ZIP
  imp()->out.close();
  imp_->flags = 0;
  imp_->file_path = std::string();
}


void OutputStream::doSeek(size_t pos)
{
  imp()->out.seekp(pos, std::ios_base::beg);
}


size_t OutputStream::doTell()
{
  return imp()->out.tellp();
}


OutputStreamPrivate *OutputStream::imp()
{
  return static_cast<OutputStreamPrivate *>(imp_);
}


const OutputStreamPrivate *OutputStream::imp() const
{
  return static_cast<const OutputStreamPrivate *>(imp_);
}
}  // namespace ohm


================================================
FILE: ohm/Stream.h
================================================
// Copyright (c) 2015
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMSTREAM_H
#define OHMSTREAM_H

#include "OhmConfig.h"

#include <cstddef>
#include <string>

#define OHM_ZIP 1

namespace ohm
{
struct InputStreamPrivate;
struct OutputStreamPrivate;
struct StreamPrivate;


/// Flags for use with @c InputStream and @c OutputStream.
enum StreamFlag : unsigned
{
  /// Compression is enabled.
  kSfCompress = (1u << 0u),
};


/// Base class for @c InputStream and @c OutputStream used to encapsulate file access and compression.
/// The stream is tailored towards file access, not a generalised stream.
class ohm_API Stream
{
public:
  /// Returns true if the stream (file) is open.
  /// @return True when open.
  virtual bool isOpen() const = 0;

  /// Returns the file path given when the stream was opened.
  /// @return The file path, or an empty string when not open.
  const std::string &filePath() const;

  /// Returns the current @c StreamFlag settings.
  /// @return Current flags.
  unsigned flags() const;

  /// Opens the stream to @p filePath.
  /// @param file_path The relative or absolute file path.
  /// @param flags @c StreamFlag values to open with.
  /// @return True on success.
  bool open(const std::string &file_path, unsigned flags = 0u);

  /// Closes the file if open. This forces a @c flush().
  void close();

  /// Flushes the file. This forces completion of compression and writes out for output streams.
  /// Input streams generally do nothing on a flush.
  ///
  /// Writing with compression enabled after a flush may cause undefined behaviour.
  virtual void flush() = 0;

  /// Seeks to the given stream position. Use with care, as it can invalidate the compression/decompression
  /// state.
  ///
  /// It is generally best to only seek when not using compression, either on the stream or when using
  /// raw reading and writing (uncompressed). A @c flush() is also best performed before seeking.
  /// @param pos Absolute position to seek to.
  void seek(size_t pos);

  /// Tells the current stream position. This may be undefined when using compression.
  ///
  /// The position may be invalid when using compression as there may be data pending
  /// in the compression buffer. A @c flush() may force an accurate position, but can
  /// invalidate further compression.
  /// @return The current absolute stream position.
  size_t tell();

protected:
  /// Called from @p open() to perform the open operation.
  /// @param file_path The relative or absolute file path.
  /// @param flags @c StreamFlag values to open with.
  /// @return True on success.
  virtual bool doOpen(const std::string &file_path, unsigned flags) = 0;

  /// Called to close the file (after a @c flush()).
  virtual void doClose() = 0;

  /// Called to perform the @c seek() operation.
  /// @param pos Absolute position to seek to.
  virtual void doSeek(size_t pos) = 0;

  /// Called to perform the @c tell() operation.
  /// @return The current absolute stream position.
  virtual size_t doTell() = 0;

  /// Constructor.
  /// @param imp The implementation pointer, set to @c imp_. Derivations must clean it up.
  explicit Stream(StreamPrivate *imp);
  /// Empty virtual destructor.
  virtual ~Stream();

  /// The stream implementation pointer. Derivations must clean this up.
  StreamPrivate *imp_;
};


/// An input file stream supporting compression.
///
/// Compression may be enabled on @c open() using @c SF_Compress, or later by calling
/// @c setCompressedFlag().
///
/// Reading compressed and uncompressed data is supported, with judicious use. Uncompressed data
/// can be read only if there are no compressed data pending in the compression buffer.
/// Essentially, uncompressed data can be read:
/// - Before any compressed data are read.
/// - After finishing reading all compressed data.
//.
/// It is not advised to read more than one compressed data section.
class ohm_API InputStream : public Stream
{
public:
  /// Constructor, optionally opening a file.
  ///
  /// @c open() is called only if @p filePath is specified. Success is determined by
  /// checking @c isOpen().
  ///
  /// @param file_path Optionally specifies the file to open.
  /// @param flags The @c StreamFlag set to open with.
  explicit InputStream(const std::string &file_path = std::string(), unsigned flags = 0u);

  /// Destructor. Flushes and closes the file.
  ~InputStream() override;

  /// Enables reading compressed data after opening.
  ///
  /// Intended for use when compression state is determined by data in a file header.
  /// In this case, the header is read uncompressed, then the stream is switched to
  /// reading compressed data.
  void setCompressedFlag();

  /// Read from the file. Decompression enabled if supported.
  /// @param buffer Buffer to read into.
  /// @param max_bytes Maximum number of bytes to read. @p buffer must be large enough for this content.
  /// @return The number of bytes read. Zero indicates no bytes available, or an decompression error.
  unsigned read(void *buffer, unsigned max_bytes);

  /// Read raw bytes from the file, no decompression.
  ///
  /// This may only be used before calls to @c read(), or after a call to
  /// @c flush().
  ///
  /// @param buffer Buffer to read into.
  /// @param max_bytes Maximum number of bytes to read. @p buffer must be large enough for this content.
  /// @return The number of bytes read. Zero indicates no bytes available, or an decompression error.
  unsigned readRaw(void *buffer, unsigned max_bytes);

  /// Returns true if the stream (file) is open.
  /// @return True when open.
  bool isOpen() const override;

  /// No operation required.
  void flush() override;

protected:
  /// Called from @p open() to perform the open operation.
  /// @param file_path The relative or absolute file path.
  /// @param flags @c StreamFlag values to open with.
  /// @return True on success.
  bool doOpen(const std::string &file_path, unsigned flags) override;

  /// Called to close the file (after a @c flush()).
  void doClose() override;

  /// Called to perform the @c seek() operation.
  /// @param pos Absolute position to seek to.
  void doSeek(size_t pos) override;

  /// Called to perform the @c tell() operation.
  /// @return The current absolute stream position.
  size_t doTell() override;

  /// Conversion to the underlying implementation.
  /// @return The underlying implementation.
  InputStreamPrivate *imp();

  /// Conversion to the underlying implementation.
  /// @return The underlying implementation.
  const InputStreamPrivate *imp() const;
};

/// An output file stream supporting compression.
///
/// Compression may be enabled on @c open() using @c SF_Compress, or later by calling
/// @c setCompressedFlag().
///
/// Writing compressed and uncompressed data is supported, with judicious use. Uncompressed data
/// can be written only if there are no compressed data pending in the compression buffer.
/// Essentially, uncompressed data can be written:
/// - Before any compressed data are written.
/// - After finishing reading all compressed data.
//.
/// It is not advised to write more than one compressed data section.
class ohm_API OutputStream : public Stream
{
public:
  /// Constructor, optionally opening a file.
  ///
  /// @c open() is called only if @p filePath is specified. Success is determined by
  /// checking @c isOpen().
  ///
  /// @param file_path Optionally specifies the file to open.
  /// @param flags The @c StreamFlag set to open with.
  explicit OutputStream(const std::string &file_path = std::string(), unsigned flags = 0u);

  /// Destructor. Flushes and closes the file.
  ~OutputStream() override;

  /// Write bytes to the file, compression enabled.
  unsigned write(const void *buffer, unsigned max_bytes);

  /// Write bytes to the file, with no compression.
  ///
  /// This may only be used before calls to @c write(), or after a call to
  /// @c flush().
  unsigned writeUncompressed(const void *buffer, unsigned max_bytes);

  /// Returns true if the stream (file) is open.
  /// @return True when open.
  bool isOpen() const override;

  /// Flushes the file. This forces completion of compression and flushes the file.
  ///
  /// Writing with compression enabled after a flush may cause undefined behaviour.
  void flush() override;

private:
  /// Called from @p open() to perform the open operation.
  /// @param file_path The relative or absolute file path.
  /// @param flags @c StreamFlag values to open with.
  /// @return True on success.
  bool doOpen(const std::string &file_path, unsigned flags) override;

  /// Called to close the file (after a @c flush()).
  void doClose() override;

  /// Called to perform the @c seek() operation.
  /// @param pos Absolute position to seek to.
  void doSeek(size_t pos) override;

  /// Called to perform the @c tell() operation.
  /// @return The current absolute stream position.
  size_t doTell() override;

  /// Conversion to the underlying implementation.
  /// @return The underlying implementation.
  OutputStreamPrivate *imp();

  /// Conversion to the underlying implementation.
  /// @return The underlying implementation.
  const OutputStreamPrivate *imp() const;
};
}  // namespace ohm

#endif  // OHMSTREAM_H


================================================
FILE: ohm/Trace.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "Trace.h"

#include <3esservermacros.h>

void ohm::trace::init(const std::string &file_stream)
{
  (void)file_stream;
  // Initialise TES
  TES_SETTINGS(settings, tes::SF_Compress | tes::SF_Collate);
  // Initialise server info.
  TES_SERVER_INFO(info, tes::XYZ);
  TES_STMT(info.defaultFrameTime = 1);
  // Create the server. Use tesServer declared globally above.
  TES_SERVER_CREATE(ohm::g_tes, settings, &info);

  // Start the server and wait for the connection monitor to start.
  TES_SERVER_START(ohm::g_tes, tes::ConnectionMonitor::Asynchronous);

  TES_IF(!file_stream.empty()) { TES_LOCAL_FILE_STREAM(ohm::g_tes, file_stream.c_str()); }
  TES_SERVER_START_WAIT(g_tes, 1000);

  TES_CATEGORY(g_tes, "Map", kTcMap, 0, true);
  TES_CATEGORY(g_tes, "Rays", kTcRays, kTcMap, true);
  TES_CATEGORY(g_tes, "Voxels", kTcVoxels, kTcMap, true);
  TES_CATEGORY(g_tes, "Ndt", kTcNdt, kTcMap, true);
  TES_CATEGORY(g_tes, "Heightmap", kTcHeightmap, 0, true);
  TES_CATEGORY(g_tes, "Voxels", kTcHmVoxel, kTcHeightmap, true);
  TES_CATEGORY(g_tes, "Surface", kTcHmSurface, kTcHmVoxel, true);
  TES_CATEGORY(g_tes, "Vacant", kTcHmVacant, kTcHmVoxel, true);
  TES_CATEGORY(g_tes, "Virtual", kTcHmVirtualSurface, kTcHmVoxel, true);
  TES_CATEGORY(g_tes, "Clearance", kTcHmClearance, kTcHeightmap, false);
  TES_CATEGORY(g_tes, "Visit", kTcHmVisit, kTcHeightmap, true);
  TES_CATEGORY(g_tes, "Info", kTcHmInfo, kTcHeightmap, true);
}


void ohm::trace::done()
{
  TES_SERVER_UPDATE(ohm::g_tes, 0.0f);
  TES_SERVER_STOP(ohm::g_tes);
}


bool ohm::trace::available()
{
#ifdef TES_ENABLE
  return true;
#else   // TES_ENABLE
  return false;
#endif  // TES_ENABLE
}


================================================
FILE: ohm/Trace.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef TRACE_H
#define TRACE_H

#include "OhmConfig.h"

#include <string>

namespace ohm
{
/// Debug visualisation trace categories.
enum TraceCategory : uint16_t
{
  kTcDefault,           ///< Default category (catchall)
  kTcMap,               ///< Parent category for occupancy map generation.
  kTcRays,              ///< Ray insertion
  kTcVoxels,            ///< Voxel state
  kTcNdt,               ///< NDT shapes
  kTcHeightmap,         ///< Parent category for heightmap generation
  kTcHmVoxel,           ///< Heightmap voxel - parent to suface and virtual surface
  kTcHmVirtualSurface,  ///< Heightmap virtual surface voxel
  kTcHmSurface,         ///< Heightmap surface voxel
  kTcHmVacant,          ///< Heightmap vacant voxel
  kTcHmClearance,       ///< Heightmap clearance
  kTcHmVisit,           ///< Heightmap visit/open lists
  kTcHmInfo,            ///< Heightmap info text
};

namespace trace
{
/// Initialise the trace system. This enables debug rendering connections via 3rd Eye Scene.
///
/// Requires `TES_ENABLE` cmake option to be on.
///
/// @todo Allow the server reference frame to be specified: default is right handled, XYZ = right, forward, up.
///
/// @param file_stream When set, opens a file stream to record trace to this file.
void ohm_API init(const std::string &file_stream = std::string());

/// Finalise the trace system.
void ohm_API done();

/// Query if debug tracing via 3rd Eye Scene is available.
/// @return True if 3es tracing is available.
bool ohm_API available();
}  // namespace trace

/// A utility class which calls @c trace::init() and @c trace::done() on construction and destruction respectively.
/// This supports stack based trace initialisation and shutdown.
class ohm_API Trace
{
public:
  /// Constructor: calls through to @c trace::init()
  inline Trace(const std::string &file_stream, bool enable = true)  // NOLINT(google-explicit-constructor)
  {
    if (enable)
    {
      trace::init(file_stream);
    }
  }
  /// Destructor: calls through to @c trace::done()
  inline ~Trace() { trace::done(); }

  Trace(const Trace &) = delete;
  Trace &operator=(const Trace &) = delete;
};
}  // namespace ohm

#endif  // TRACE_H


================================================
FILE: ohm/Voxel.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "Voxel.h"


================================================
FILE: ohm/Voxel.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef OHMVOXEL_H
#define OHMVOXEL_H

#include "OhmConfig.h"

#include "Key.h"
#include "MapChunk.h"
#include "MapLayer.h"
#include "MapLayout.h"
#include "OccupancyMap.h"
#include "VoxelBlock.h"

#include <cinttypes>
#include <cstring>
#include <type_traits>

#include <glm/vec3.hpp>

namespace ohm
{
namespace detail
{
/// Internal helper to manage updating const/mutable chunks with the base as the mutable version.
template <typename T>
struct VoxelChunkAccess
{
  /// Resolve a mutable chunk for @p key from @p map , creating the chunk if required.
  /// @param map The map of interest.
  /// @param key The key to resolve the chunk for.
  /// @return The chunk for @p key .
  static MapChunk *chunk(OccupancyMap *map, const Key &key) { return map->region(key.regionKey(), true); }

  /// Update the first valid index for @p chunk using @p voxel_index .
  /// @param chunk The map chunk being touched: must be valid.
  /// @param voxel_index The linear index of the modified voxel.
  static void touch(MapChunk *chunk, unsigned voxel_index) { chunk->updateFirstValid(voxel_index); }

  /// Mark @p chunk as having been updated within @p layer_index .
  /// This will @c OccupancyMap::touch() the @p map and update the stamps in @p chunk relevant to @p layer_index .
  /// @param map The map of interest.
  /// @param chunk The map chunk being touched: must be valid.
  /// @param layer_index The voxel memory index in chunk which has been modified.
  static void touch(OccupancyMap *map, MapChunk *chunk, int layer_index)
  {
    chunk->dirty_stamp = map->touch();
    chunk->touched_stamps[layer_index].store(chunk->dirty_stamp, std::memory_order_relaxed);
  }

  /// Write the @p value to the voxel at @p voxel_index within @p voxel_memory .
  /// @param voxel_memory Start of the voxel memory.
  /// @param voxel_index Index of the voxel within @p voxel_memory - strided by @c T .
  /// @param value The value to write.
  /// @param flags_change Flags to set in @p flags .
  /// @param flags Flags to modify by setting @p flags_change .
  static void writeVoxel(uint8_t *voxel_memory, unsigned voxel_index, const T &value, unsigned flags_change,
                         uint16_t *flags)
  {
    memcpy(voxel_memory + sizeof(T) * voxel_index, &value, sizeof(T));
    *flags |= flags_change;
  }
};

/// Internal helper to manage const chunks. Supports fetching existing chunks, but no modification.
template <typename T>
struct VoxelChunkAccess<const T>
{
  /// Query the @c MapChunk pointer for @p key.
  /// @param map The Occupancy map of interest
  /// @param key The key to get a chunk for.
  /// @return The @c MapChunk for key, or null if the chunk does not exist.
  static const MapChunk *chunk(const OccupancyMap *map, const Key &key) { return map->region(key.regionKey()); }

  /// Noop.
  /// @param chunk Ignored.
  /// @param voxel_index Ignored.
  static void touch(const MapChunk *chunk, unsigned voxel_index)
  {
    (void)chunk;
    (void)voxel_index;
  }

  /// Noop.
  /// @param map Ignored.
  /// @param chunk Ignored.
  /// @param layer_index Ignored.
  static void touch(const OccupancyMap *map, const MapChunk *chunk, int layer_index)
  {
    (void)map;
    (void)chunk;
    (void)layer_index;
  }

  static void writeVoxel(uint8_t voxel_memory, unsigned voxel_index, const T &value, unsigned flags_change,
                         uint16_t *flags) = delete;
};
}  // namespace detail

/// The @c Voxel interface provides a semi optimal abstraction and book keeping for accessing voxel data.
///
/// The @c Voxel interface deals directly with the @c MapLayer abstraction of the @c OccupancyMap , providing access
/// to the data within a single @p MapLayer . The template type is used to resolve the data within the layer as
/// the template type, supporting mutable and const access (see below). The template type is validated against
/// the data in the proposed layer index by checking the size of @c T against the size of the data stored in the
/// layer. The layer index is invalidated when the sizes do not match.
///
/// A mutable @c Voxel is one where the template type @c T is non-const, while a const @c Voxel has a const template
/// type @c T . Only a mutable @c Voxel can create new @c MapChunks within the @c OccupancyMap . This occurs
/// immediately on setting a key, generally via @c setKey() . Additional book keeping is managed by the mutable
/// @c Voxel to ensure correct update of @c OccupancyMap::stamp() (via @c OccupancyMap::touch() ) as well as
/// updating appropriate @c MapChunk stamps. The @c MapChunk::first_valid_index is also updated for the occupancy
/// layer.
///
/// A @c Voxel is initialised to reference a specific @c MapLayer within a specific @c OccupancyMap . At this point
/// the @c Voxel should pass @c isValidLayer() to ensure that the map is valid, the layer reference is valid and
/// that the size of @c T matches the @c MayLayer voxel size. Specific voxels can then be referenced via @c setKey()
/// to identify which voxel to reference. This resolves the @c MapChunk , creating the chunk in a mutable @c Voxel
/// if required. A const @c Voxel will never create a new @c MapChunk and may have a valid @c Key , with a null
/// @c MapChunk .
///
/// From this we see that a @c Voxel may be in one of several states. Below we list the states and various methods
/// which can be used to check these states.
///
/// State description               | `isLayerValid()`  | `isValidReference()`  | `isValid()` | `errorFlags()`
/// ------------------------------- | ----------------  | --------------------- | ----------- | -------------
/// Null map                        | false             | false                 | false       | `NullMap`
/// `layerIndex()` out of range     | false             | false                 | false       | `InvalidLayerIndex`
/// `sizeof(T)!=voxelByteSize()` *  | false             | false                 | false       | `VoxelSizeMismatch`
/// Initialised                     | true              | false                 | false       | 0
/// Key set, chunk null (const only)| true              | true                  | false       | 0
/// Key set, chunk resolved         | true              | true                  | true        | 0
///
/// * see @c MapLayer::voxelByteSize()
///
/// There are various ways to set a @c Voxel to reference a specific voxel using @c setKey() or some constructors.
/// The @c setKey() function supports various overloads. The first accepts a @c Key which references a voxel. This
/// key is immediately used to resolve the @c MapChunk then the specific voxel @c data() on request. Setting to a key
/// in the same @c MapChunk as the current maintains the current @c MapChunk pointer. @c setKey() also accepts a
/// @c Key with a @c MapChunk for cases where the caller already has a reference to the correct @c MapChunk . This
/// chunk pointer is assumed to be correct and is not validated.
///
/// The key may also be set from another @c Voxel even one for a different layer. This copies the @c Key and
/// @c MapChunk from the other @c Voxel saving on a looking into the map to resolve the chunk. This call is validated
/// only to ensure that the @c OccupancyMap pointers match, setting @c Error::kMapMismatch on failure.
///
/// Finally a voxel reference may be set from an @c OccupancyMap::iterator for a mutable voxel or an
/// @c OccupancyMap::const_iterator for a const voxel. This call also assumes the @c MapChunk in the iterator is valid
/// and will save a looking into the map. Note that the iterator is assumed to be valid by the @c Voxel and is not
/// validated. That is, a @c Voxel must not be initialised from an @c end() or otherwise invalid iterator.
///
/// A similar set of parametrisation of the @c Voxel constructors also exist.
///
/// For convenience the free function @c setVoxelKey() may be used to initialise a number of @c Voxel references,
/// presumably to different layers, from a single key reference. This is a variadic template function accepting
/// at least two arguments. The first is the key reference object - a @c Key , iterator or valid @c Voxel reference -
/// followed by any number of @c Voxel objects. Take care to ensure that the first argument is always a valid key
/// reference.
///
/// For optimal memory access a @c MapChunk should be accessed as linearly as possible. This is mostly only applicable
/// to reading only access. For write access some gains are to be made by referencing voxels in a spatially coherent
/// fashion, ones which are likely to fall in the same @c MapChunk . This helps keep @c Voxel overheads to a
/// minimum, but direct access to and linear traversal of voxel data may be more optimal with manual book keeping
/// to manage the various stamps and @c MapChunk::first_valid_index .
///
/// Typical usage is illustrated in the example below, populating a line in the map and reading a line of voxels.
///
/// @code
/// void populateLine(ohm::OccupancyMap &map)
///{
///  // Create a voxel reference for accessing voxel occupancy.
///  ohm::Voxel<float> occupancy(&map, map.layout().occupancyLayer());
///  // Create a voxel reference for accessing VoxelMean.
///  ohm::Voxel<ohm::VoxelMean> mean(&map, map.layout().meanLayer());
///
///  // Ensure the occupancy layer is ok.
///  if (!occupancy.isLayerValid())
///  {
///    return;
///  }
///
///  glm::dvec3 sample(0, 0, 0);
///  for (int i = 0; i < 10; ++i)
///  {
///    sample.x = i * map.resolution();
///    const Key key = map.voxelKey(sample);
///
///    // Below we look at several methods for setting the keys for the Voxel objects. Note that they have equivalent
///    // results and only one need be used. The process of setting the Key instantiates the MapChunk in map.
///
///    // 1. Set each voxel individually. This has some slight additional overhead as each Voxel resolved the chunk.
///    occupancy.setKey(key);
///    mean.setKey(key);
///
///    // 2. Set one Voxel key directly, the subsequent items are chained and the chunk is resolved once.
///    mean.setKey(occupancy.setKey(key));
///
///    // 3. Equivalent, but slightly more readable version of 2. Note this version is open to mistakenly passing a
///    // Voxel for the first argument rather than a Key and not actually setting a key.
///    ohm::setVoxelKey(key, occupancy, mean);
///
///    // Next we look at some different ways of setting occupancy. Again, only 1 need be used.
///    // A) Set occupancy directly:
///    occupancy.data() = map.hitValue();
///
///    // B) Use helper function. Assumes occupancy.isValid() is true
///    ohm::integrateHit(occupancy);  // in VoxelOccupancy.h
///
///    // Finally update the voxel mean. Using both techniques below results in adding the sample twice, so the
///    // mean count will be 2.
///    // i. Safe version, with mean.isValid() may be false.
///    ohm::updatePositionSafe(mean, sample);  // in VoxelMean.h
///    // ii. Unchecked version, where mean.isValid() is assumed to be true. We check explicitly because we have not
///    // checked mean.isLayerValid() above.
///    if (mean.isValid())
///    {
///      ohm::updatePositionUnsafe(mean, sample);  // in VoxelMean.h
///    }
///  }
///}
///
/// void walkLine(const ohm::OccupancyMap &map)
///{
///  // Create a voxel reference for accessing voxel occupancy.
///  ohm::Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
///  // Create a voxel reference for accessing VoxelMean.
///  ohm::Voxel<const ohm::VoxelMean> mean(&map, map.layout().meanLayer());
///
///  glm::dvec3 sample(0, 0, 0);
///  for (int i = 0; i < 12; ++i)  // iterate further than the original line
///  {
///    sample.x = i * map.resolution();
///    const Key key = map.voxelKey(sample);
///
///    // Use option 3 from above to set the key.
///    // This will not create the chunk if it does not exist because we are using const template types for
///    // Voxel.
///    ohm::setVoxelKey(key, occupancy, mean);
///
///    std::cout << "check map at (" << sample.x << "," << sample.y << "," << sample.z << ")" << std::endl;
///    std::cout << "occupancy: ";
///    if (!ohm::isUnobservedOrNull(occupancy))  // in VoxelOccupancy.h
///    {
///      std::cout << occupancy.data() << std::endl;
///    }
///    else
///    {
///      std::cout << "unobserved" << std::endl;
///    }
///
///    // The the voxel position, safely. This will be the voxel centre if VoxelMean is not supported.
///    const glm::dvec3 voxel_pos = ohm::positionSafe(mean);  // in VoxelMean.h
///    std::cout << "position: (" << voxel_pos.x << "," << voxel_pos.y << "," << voxel_pos.z << ")" << std::endl;
///  }
///}
/// @endcode
///
/// @par Lazy update
/// A @c Voxel object keeps track of changes made to it's current chunk and voxel. However, it uses a lazy update for
/// changing @c MapChunk members such as @c MapChunk::dirty_stamp, @c MapChunk::touched_stamps and
/// @c MapChunk::first_valid_index. Retaining a @c Voxel object can lead to cirsumstances whereby these values are out
/// of date and subsequent map operations do not consider the @c MapChunk modifier (incorrect stamp values), or being
/// indexing the chunk at the wrong location (@c first_valid_index). This lazy update is particularly important to
/// consider when using a @c GpuMap as the @c GpuMapCache only uploads data from CPU to GPU memory based on changes to
/// the @c MapChunk::touched_stamp values.
///
/// A @c Voxel is considered committed when one of the following occurs:
///
/// - The @c Voxel is destructed
/// - The @c Voxel is @c reset()
/// - A call to @c commit() is made
/// - The @c Voxel key changes (see below)
///
/// Changes to the @c Voxel key provide further subtle complications in that the @c MapChunk stamps will only be updated
/// when the new key references a different @c MapChunk (or a null region).
///
/// @note The map must outlive the voxel. Generally this will be true when using @c Voxel objects within a limited
/// scope. However care must be taken when an @c OccupancyMap and a @c Voxel reference to that map exist in the same
/// scope or when performing operations which can invalidate the @c MapLayout or clear the map. In these cases,
/// all @c Voxel objects should be explicitly released via @c Voxel::release() .
///
/// @tparam T The data type expected to be contained in the @c MapLayer to operate on. Checked for size match with
/// layer content. Note that compiler alignment of structures may cause mismatches between the data added to a
/// @c MapLayer and the size of @c T as @c MapLayer content will not be padded while @c T may be.
///
/// @todo Add validation debug compile flag which validate the various initalisations and data access calls.
template <typename T>
class Voxel
{
public:
  /// Non-const data type for @c T
  using DataType = typename std::remove_const<T>::type;
  /// Const or non-const @c OccupancyMap iterator to support construction from an iterator. Saves on resolving
  /// the @c MapChunk again.
  using MapIteratorType =
    typename std::conditional<std::is_const<T>::value, OccupancyMap::const_iterator, OccupancyMap::iterator>::type;
  /// Const or non-const @c OccupancyMap pointer based on @c T constness
  using MapTypePtr = typename std::conditional<std::is_const<T>::value, const OccupancyMap *, OccupancyMap *>::type;
  /// Const or non-const @c MapChunk pointer based on @c T constness
  using MapChunkPtr = typename std::conditional<std::is_const<T>::value, const MapChunk *, MapChunk *>::type;
  /// Voxel data pointer - always a @c uint8_t type with @c const added if @c T is @c const .
  using VoxelDataPtr = typename std::conditional<std::is_const<T>::value, const uint8_t *, uint8_t *>::type;

  /// Error flag values indicating why initialisation may have failed.
  enum class Error : uint16_t
  {
    kNone = 0,                        ///< No error
    kNullMap = (1u << 0u),            ///< @c OccupancyMap is null
    kInvalidLayerIndex = (1u << 1u),  ///< The given layer index is invalid.
    kVoxelSizeMismatch = (1u << 2u),  ///< The @c MapLayer voxel size does not match the size of @c T
    kMapMismatch = (1u << 3u)         ///< When two @c Voxel object maps do not match such as in @c setKey() chaining.
  };

  /// Book keeping flags.
  enum class Flag : uint16_t
  {
    kNone = 0,                       ///< Nothing of note
    kIsOccupancyLayer = (1u << 0u),  ///< Marks that this @c Voxel points to the occupancy layer of @c OccupancyMap.
    kCompressionLock = (1u << 1u),   ///< Indicates the layer's @c VoxelBlock has been retained in @c chunk_.
    kTouchedChunk = (1u << 2u),      ///< Marks that the current @c MapChunk data has been accessed for mutation.
    kTouchedVoxel = (1u << 3u),      ///< Marks that the current voxel data has been accessed for mutation.

    /// Flag values which are not propagated in copy assignment.
    kNonPropagatingFlags = kTouchedChunk | kTouchedVoxel | kCompressionLock  // NOLINT(hicpp-signed-bitwise)
  };

  /// Empty constructor generating an invalid voxel with no map.
  Voxel() = default;
  /// Copy constructor - same type.
  /// @param other The voxel to copy.
  Voxel(const Voxel<T> &other);
  /// RValue constructor - same type.
  /// @param other The voxel to copy.
  Voxel(Voxel<T> &&other) noexcept;
  /// Copy constructor from a different type. Copies the map, chunk and key, but references a different layer/type.
  /// @param other The base voxel to copy.
  /// @param layer_index The @c MapLayer to access for the type @c T .
  template <typename U>
  Voxel(const Voxel<U> &other, int layer_index);
  /// Create a @c Voxel reference for @c map and the specified @c layer_index . The map and layer are validated
  /// (see @c Error flags) and @c isLayerValid() will be true on success. The key is not set so @c isValid() remains
  /// false.
  /// @param map The map to access and mutate for non-const @c Voxel types.
  /// @param layer_index The @c MapLayer to access for the type @c T .
  Voxel(MapTypePtr map, int layer_index);
  /// Create a @c Voxel reference for @c map and the specified @c layer_index and reference the voxel at @c key. The
  /// map and layer are validated (see @c Error flags) and @c isLayerValid() will be true on success. The key is also
  /// set and @c isValid() will be true for a mutable @c Voxel , creating the @c MapChunk if required. A non-mutable
  /// @c Voxel will still not be valid in the case where the @p key references a @c MapChunk which does not exist.
  /// @param map The map to access and mutate for non-const @c Voxel types.
  /// @param layer_index The @c MapLayer to access for the type @c T .
  /// @param key The key for the voxel to initially reference. May be changed layer with @c setKey() .
  Voxel(MapTypePtr map, int layer_index, const Key &key);

  /// Set the @c Voxel to reference the start of the voxel buffer within the @c MapChunk associated with the given
  /// @p region_key . This is equivalent to using @c Key(region_key,0,0,0) .
  /// @param map The map to access and mutate for non-const @c Voxel types.
  /// @param layer_index The @c MapLayer to access for the type @c T .
  /// @param region_key The region coordinate key for the chunk to reference.
  Voxel(MapTypePtr map, int layer_index, const glm::i16vec3 &region_key);

  /// Create a @c Voxel reference from a @c OccupancyMap::iterator (mutable @c Voxel ) or a
  /// @c OccupancyMap::const_iterator (const @c Voxel ). This is similar to using the
  /// @c Voxel(MapTypePtr,layer_index,key) constructor, with the map, chunk and key always coming from the iterator.
  /// This will never create a @c MapChunk as it is assumed to be valid in the iterator.
  Voxel(const MapIteratorType &iter, int layer_index);

  /// Destructor, ensures book keeping operations are completed on the @c MapChunk .
  inline ~Voxel() { updateTouch(false); }

  /// Check if the map and layer references are valid and error flags are clear.
  /// @return True if the @c map() is not null, the @c layerIndex() is valid and @c errorFlags() are zero.
  inline bool isLayerValid() const { return map_ && layer_index_ >= 0 && error_flags_ == 0; }
  /// Check if the voxel reference is valid for @c data() calls.
  /// @return True if @c isLayerValid() and the @c chunk() and @c key() values are  non-null.
  inline bool isValid() const { return isLayerValid() && chunk_ && key_ != Key::kNull && voxel_memory_ != nullptr; }
  /// Check if the voxel reference is invalid.
  /// @return The logical negation of @c isValid()
  inline bool isNull() const { return isLayerValid() && (!chunk_ || key_ == Key::kNull || voxel_memory_ == nullptr); }
  /// Check if the @c Voxel is a valid voxel reference. This does not check the @c chunk() .
  /// @return True if @c isValidLayer() and the key is non-null.
  inline bool isValidReference() const { return isLayerValid() && key_ != Key::kNull; }

  /// Nullify the @c Voxel . This clears the map, layer, chunk and key values. Book keeping is performed as required.
  inline void reset() { *this = Voxel<T>(); }

  /// Commit any oustanding write operations. A @c Voxel object keeps track of changes made to it's current chunk and
  /// voxel. However, it uses a lazy update for changing @c MapChunk members such as @c MapChunk::dirty_stamp,
  /// @c MapChunk::touched_stamps and @c MapChunk::first_valid_index . This function ensures
  inline void commit() { updateTouch(true); }

  /// Query the pointer to the @c OccupancyMap .
  /// @return The map pointer.
  inline MapTypePtr map() const { return map_; }
  /// Query the pointer to the @c MapChunk .
  /// @return The chunk pointer.
  inline MapChunkPtr chunk() const { return chunk_; }
  /// Query the current @c Key reference.
  /// @return The current key value.
  inline Key key() const { return key_; }
  /// Query the index of the target @c MapLayer in the @c OccupancyMap .
  /// @return The map layer to target.
  inline int layerIndex() const { return layer_index_; }
  /// Query the cached @c MapLayer::dimensions() .
  /// @return The map layer voxel dimensions.
  inline glm::u8vec3 layerDim() const { return layer_dim_; }
  /// Query the status @c Flag values for the voxel. These are generally book keeping flags.
  /// @return The current status flags.
  inline unsigned flags() const { return flags_; }
  /// Query the status @c Error flag values for the voxel.
  /// @return The current error flags.
  inline unsigned errorFlags() const { return error_flags_; }

  /// Set the voxel @c Key . This may create a @c MapChunk for a mutable @c Voxel .
  /// @param key The key for the voxel to reference. Must be non-null and in range.
  /// @return `*this`
  Voxel<T> &setKey(const Key &key);
  /// Set the voxel @c Key with a pre-resolved @c MapChunk .
  /// @param key The key for the voxel to reference. Must be non-null and in range.
  /// @param chunk A pointer to the correct chunk for the @c Key . This must be the correct chunk.
  /// @return `*this`
  Voxel<T> &setKey(const Key &key, MapChunkPtr chunk);
  template <typename U>
  /// Set the voxel key from another @c Voxel reference. This copies the @c MapChunk from @p other .
  ///
  /// Will set the @c Error::kMapMismatch flag if the map pointers do not match between @c this and @p other.
  /// @param other The voxel to initialise from.
  /// @return `*this`
  Voxel<T> &setKey(const Voxel<U> &other);
  /// Set the voxel reference from an @c OccupancyMap::iterator (mutable) or @c OccupancyMap::const_iterator (const).
  /// @param iter The iterator to set the voxel reference from. Must be a valid voxel iterator.
  /// @return `*this`
  Voxel<T> &setKey(const MapIteratorType &iter);

  /// Resolve the linearised voxel index into the @c MapChunk layer. @c isValidReference() must be true before
  /// calling.
  /// @return The linear voxel index resolved from the key.
  inline unsigned voxelIndex() const { return ohm::voxelIndex(key_, layer_dim_); }

  /// Access the data for the current voxel. This is a convenience wrapper for the @c read() function which returns
  /// the template data type. Only call if @c isValid() is true.
  /// @return The data for the current voxel.
  DataType data() const
  {
    DataType d;
    read(&d);
    return d;
  }

  /// Read the current voxel data value.
  ///
  /// This reads the data for the current @c voxelIndex() into @p value . This call does no error checking and
  /// assumes that all pointers and index values have been validated.
  ///
  /// @param[out] value Pointer where to write the voxel data of type @c T for the currently referenced voxel.
  /// @return The read value - i.e., `*value`.
  inline const DataType &read(DataType *value) const
  {
    memcpy(value, voxel_memory_ + sizeof(T) * voxelIndex(), sizeof(T));
    return *value;
  }

  /// Write the current voxel data @p value .
  ///
  /// This writes @p value to the current @c voxelIndex() location. This call does no error checking and
  /// assumes that all pointers and index values have been validated. This can only be called if @c T is a non const
  /// type.
  ///
  /// @param[in] value Value to write for the current voxel.
  inline void write(const DataType &value)
  {
    detail::VoxelChunkAccess<T>::writeVoxel(voxel_memory_, voxelIndex(), value,
                                            unsigned(Flag::kTouchedChunk) | unsigned(Flag::kTouchedVoxel), &flags_);
  }

  /// Return a pointer to the start of the voxel memory for the current chunk.
  /// @c isValid() must be true before calling.
  /// @return A pointer to the voxel memory for the currently referenced chunk.
  inline VoxelDataPtr voxelMemory() const { return voxel_memory_; }

  /// Attempt to step the voxel reference to the next voxel in the current @c MapChunk .
  ///
  /// This first validates @c isValidReference() before attempting to modify the @c key() . On success, the
  /// key will reference the next @c voxelIndex() in the @c MapChunk .
  ///
  /// @return True on success.
  bool nextInRegion();

  /// Swap this voxel content with @p other .
  /// @param other The voxel to exchange data with.
  void swap(Voxel &other);

  /// Assignment operator. Performs book keeping before assignment.
  ///
  /// Note that some @c Flag values will not be copied by the assignment. See @c Flag::kNonPropagatingFlags .
  /// @param other The voxel reference to assign from.
  /// @return `*this`
  Voxel &operator=(const Voxel<T> &other);
  /// Move assignent operator.
  /// @param other The rvalue reference to move data from.
  /// @return `*this`
  Voxel &operator=(Voxel<T> &&other) noexcept;

protected:
  /// Internal key set function. Performs book keeping for @c Flag::kIsOccupancyLayer and @c Flag::kTouchedVoxel .
  /// @param key The key value to set.
  inline void setKeyInternal(const Key &key)
  {
    updateVoxelTouch();
    key_ = key;
  }

  /// Internal chunk set function. Performs book keeping for @c Flag::kTouchedChunk .
  /// @param chunk The chunk to set the voxel to reference.
  inline void setChunk(MapChunkPtr chunk)
  {
    if (chunk_ != chunk)
    {
      updateChunkTouchAndCompression(false);
      chunk_ = chunk;
      if (chunk_ && layer_index_ != -1)
      {
        chunk_->voxel_blocks[layer_index_]->retain();
        flags_ |= unsigned(Flag::kCompressionLock);
        voxel_memory_ = chunk_->voxel_blocks[layer_index_]->voxelBytes();
      }
      else
      {
        voxel_memory_ = nullptr;
      }
    }
  }

  /// Validate the @c layerIndex() . May invalidate the layer index and set @c Error flag values.
  void validateLayer();
  /// Perform book keeping for the currently referenced voxel. Handles @c Flag::kIsOccupancyLayer and
  /// @c Flag::kTouchedVoxel . This only needs to do work when @c Flag::kIsOccupancyLayer is true by updating
  /// @c MapChunk::first_valid_index the @c Flag::kTouchedVoxel has been set. @c Flag::kTouchedVoxel is cleared.
  ///
  /// Safe to call when no book keeping needs to be done or the voxel reference is invalid.
  void updateVoxelTouch();
  /// Perform book keeping for the current chunk. Handles @c Flag::kTouchedChunk by ensuring @c OccupancyMap::touch()
  /// is called then updating the @c MapChunk::dirty_stamp and the @c MapChunk::touched_stamps for the referenced
  /// layer. @c Flag::kTouchedChunk is cleared.
  ///
  /// Safe to call when no book keeping needs to be done or the voxel reference is invalid.
  ///
  /// @param retain_chunk True to keep the chunk memory retained (uncompressed), false to release the chunk and set it
  ///   to null. Should only be true for @c commit(), where the voxel reference is also retained.
  void updateChunkTouchAndCompression(bool retain_chunk);
  /// Full book keeping calling @c updateVoxelTouch() and @c updateChunkTouchAndCompression() .
  ///
  /// Safe to call when no book keeping needs to be done or the voxel reference is invalid.
  /// @param retain_chunk True to keep the chunk memory retained (uncompressed), false to release the chunk and set it
  ///   to null. Should only be true for @c commit(), where the voxel reference is also retained.
  inline void updateTouch(bool retain_chunk)
  {
    updateVoxelTouch();
    updateChunkTouchAndCompression(retain_chunk);
  }

private:
  VoxelDataPtr voxel_memory_ = nullptr;  ///< A pointer to the start of voxel memory within the current chunk.
  MapTypePtr map_ = nullptr;             ///< @c OccupancyMap pointer
  MapChunkPtr chunk_ = nullptr;          ///< Current @c MapChunk pointer - may be null even with a valid key reference.
  Key key_ = Key::kNull;                 ///< Current voxel @c Key reference.
  int layer_index_ = -1;                 ///< The target map layer. Validated on construction.
  glm::u8vec3 layer_dim_{ 0, 0, 0 };     ///< The voxel dimensions of the layer.
  uint16_t flags_ = 0;                   ///< Current status/book keeping flags
  uint16_t error_flags_ = 0;             ///< Current error flags.
};


/// @overload
template <typename KeyType, typename T>
void setVoxelKey2(const KeyType &key, Voxel<T> &voxel)
{
  voxel.setKey(key);
}

/// A less safe version of @c setVoxelKey(). This version isn't as safe as it accepts any @c KeyType , which opens up
/// up bugs where the first argument is an invalid @c Voxel reference, rather than the expected usage where the first
/// argument is a @c Key or @c OccupancyMap::iterator .
/// @param key The object from which to set the key value. Expected to be a @c Key , @c OccupancyMap::iterator or
/// @c OccupancyMap::const_iterator
/// @param voxel The first @c Voxel to set the key for.
/// @param args Additional @c Voxel references.
template <typename KeyType, typename T, typename... Args>
void setVoxelKey2(const KeyType &key, Voxel<T> &voxel, Args &...args)
{
  setVoxelKey2(voxel.setKey(key), args...);
}

/// @overload
template <typename T>
void setVoxelKey(const Key &key, Voxel<T> &voxel)
{
  voxel.setKey(key);
}

/// @overload
template <typename T>
void setVoxelKey(const OccupancyMap::iterator &iter, Voxel<T> &voxel)
{
  voxel.setKey(iter);
}

/// @overload
template <typename T>
void setVoxelKey(const OccupancyMap::const_iterator &iter, Voxel<T> &voxel)
{
  voxel.setKey(iter);
}

/// Set the key value for multiple @c Voxel objects with reduced overhead by resolving the @c MapChunk once.
/// @param key The object from which to set the key value.
/// @param voxel The first @c Voxel to set the key for.
/// @param args Additional @c Voxel references.
template <typename T, typename... Args>
void setVoxelKey(const Key &key, Voxel<T> &voxel, Args &...args)
{
  setVoxelKey2(key, voxel, args...);
}

/// Set the key value for multiple @c Voxel from a map iterator.
/// @param iter The occupancy map iterator to set from.
/// @param voxel The first @c Voxel to set the key for. Must be a mutable voxel.
/// @param args Additional @c Voxel references.
template <typename T, typename... Args>
void setVoxelKey(const OccupancyMap::iterator &iter, Voxel<T> &voxel, Args &...args)
{
  setVoxelKey2(iter, voxel, args...);
}

/// Set the key value for multiple @c Voxel from a map iterator.
/// @param iter The occupancy map iterator to set from.
/// @param voxel The first @c Voxel to set the key for. Must be a const voxel.
/// @param args Additional @c Voxel references.
template <typename T, typename... Args>
void setVoxelKey(const OccupancyMap::const_iterator &iter, Voxel<T> &voxel, Args &...args)
{
  setVoxelKey2(iter, voxel, args...);
}


template <typename T>
Voxel<T>::Voxel(const Voxel<T> &other)
  : map_(other.map_)
  , chunk_(nullptr)
  , key_(other.key_)
  , layer_index_(other.layer_index_)
  , layer_dim_(other.layer_dim_)
  , flags_(other.flags_ & ~unsigned(Flag::kNonPropagatingFlags))
  , error_flags_(other.error_flags_)
{
  // Do not set chunk or voxel_memory_ pointers directly. Use the method call to ensure flags are correctly
  // maintained.
  setChunk(other.chunk_);
}


template <typename T>
Voxel<T>::Voxel(Voxel<T> &&other) noexcept
  : voxel_memory_(std::exchange(other.voxel_memory_, nullptr))
  , map_(std::exchange(other.map_, nullptr))
  , chunk_(std::exchange(other.chunk_, nullptr))
  , key_(std::exchange(other.key_, Key::kNull))
  , layer_index_(std::exchange(other.layer_index_, -1))
  , layer_dim_(std::exchange(other.layer_dim_, glm::u8vec3(0, 0, 0)))
  , flags_(std::exchange(other.flags_, 0u))
  , error_flags_(std::exchange(other.error_flags_, 0u))
{}


template <typename T>
template <typename U>
Voxel<T>::Voxel(const Voxel<U> &other, int layer_index)
  : map_(other.map())
  , chunk_(nullptr)
  , key_(other.key())
  , layer_index_(layer_index)
  , flags_(other.flags() & ~unsigned(Flag::kNonPropagatingFlags))
  , error_flags_(other.errorFlags())
{
  validateLayer();
  // Do not set chunk or voxel_memory_ pointers directly. Use the method call to ensure flags are correctly
  // maintained.
  setChunk(other.chunk());
}


template <typename T>
Voxel<T>::Voxel(MapTypePtr map, int layer_index)
  : map_(map)
  , layer_index_(layer_index)
{
  validateLayer();
}


template <typename T>
Voxel<T>::Voxel(MapTypePtr map, int layer_index, const Key &key)
  : Voxel<T>(map, layer_index)
{
  if (map)
  {
    setKey(key);
  }
}


template <typename T>
Voxel<T>::Voxel(MapTypePtr map, int layer_index, const glm::i16vec3 &region_key)
  : Voxel<T>(map, layer_index, Key(region_key, 0, 0, 0))
{}


template <typename T>
Voxel<T>::Voxel(const MapIteratorType &iter, int layer_index)
  : Voxel<T>(iter.map(), layer_index, iter.key())
{
  setChunk(iter.chunk());
}


template <typename T>
Voxel<T> &Voxel<T>::setKey(const Key &key)
{
  setKeyInternal(key);
  if (!chunk_ || chunk_->region.coord != key.regionKey())
  {
    // Create chunk if not read only access.
    setChunk(detail::VoxelChunkAccess<T>::chunk(map_, key));
  }
  return *this;
}


template <typename T>
Voxel<T> &Voxel<T>::setKey(const Key &key, MapChunkPtr chunk)
{
  setKeyInternal(key);
  setChunk(chunk);
  return *this;
}


template <typename T>
Voxel<T> &Voxel<T>::setKey(const MapIteratorType &iter)
{
  setKeyInternal(*iter);
  setChunk(iter.chunk());
  return *this;
}


template <typename T>
template <typename U>
Voxel<T> &Voxel<T>::setKey(const Voxel<U> &other)
{
  setKeyInternal(other.key());
  setChunk(other.chunk());
  error_flags_ |= !(map_ == other.map()) * unsigned(Error::kMapMismatch);
  return *this;
}


template <typename T>
bool Voxel<T>::nextInRegion()
{
  if (isValidReference())
  {
    if (key_.localKey().x + 1 == layer_dim_.x)
    {
      if (key_.localKey().y + 1 == layer_dim_.y)
      {
        if (key_.localKey().z + 1 == layer_dim_.z)
        {
          return false;
        }

        key_.setLocalKey(glm::u8vec3(0, 0, key_.localKey().z + 1));
      }
      else
      {
        key_.setLocalKey(glm::u8vec3(0, key_.localKey().y + 1, key_.localKey().z));
      }
    }
    else
    {
      key_.setLocalAxis(0, key_.localKey().x + 1);
    }

    return true;
  }

  return false;
}


template <typename T>
void Voxel<T>::swap(Voxel &other)
{
  std::swap(voxel_memory_, other.voxel_memory_);
  std::swap(map_, other.map_);
  std::swap(chunk_, other.chunk_);
  std::swap(key_, other.key_);
  std::swap(layer_index_, other.layer_index_);
  std::swap(layer_dim_, other.layer_dim_);
  std::swap(flags_, other.flags_);
  std::swap(error_flags_, other.error_flags_);
}


template <typename T>
Voxel<T> &Voxel<T>::operator=(const Voxel<T> &other)
{
  if (&other != this)
  {
    updateTouch(false);  // Note: this will set the chunk to null.
    map_ = other.map_;
    setKeyInternal(other.key_);
    layer_index_ = other.layer_index_;
    layer_dim_ = other.layer_dim_;
    flags_ = other.flags_ & ~unsigned(Flag::kNonPropagatingFlags);
    error_flags_ = other.error_flags_;
    // Do not set chunk or voxel_memory_ pointers directly. Use the method call to ensure flags are correctly
    // maintained.
    voxel_memory_ = nullptr;  // About to be resolved in setChunk()
    setChunk(other.chunk_);
  }
  return *this;
}


template <typename T>
Voxel<T> &Voxel<T>::operator=(Voxel<T> &&other) noexcept
{
  Voxel<T> copy(std::move(other));
  swap(copy);
  return *this;
}


template <typename T>
void Voxel<T>::validateLayer()
{
  if (layer_index_ >= 0)
  {
    const MapLayer *layer = map_ ? map_->layout().layerPtr(layer_index_) : nullptr;
    if (!map_)
    {
      error_flags_ |= unsigned(Error::kNullMap);
    }
    else if (!layer)
    {
      // Invalid layer.
      error_flags_ |= unsigned(Error::kInvalidLayerIndex);
    }
    else if (layer->voxelByteSize() != sizeof(T))
    {
      // Incorrect layer size.
      error_flags_ |= unsigned(Error::kVoxelSizeMismatch);
    }
    else
    {
      layer_dim_ = layer->dimensions(map_->regionVoxelDimensions());
    }

    flags_ &= ~unsigned(Flag::kIsOccupancyLayer);
    flags_ |= (layer && map_ && layer_index_ == map_->layout().occupancyLayer()) * unsigned(Flag::kIsOccupancyLayer);
  }
}


template <typename T>
void Voxel<T>::updateVoxelTouch()
{
  if ((flags_ & unsigned(Flag::kIsOccupancyLayer) | unsigned(Flag::kTouchedVoxel)) && chunk_)
  {
    detail::VoxelChunkAccess<T>::touch(chunk_, voxelIndex());
  }
  flags_ &= ~unsigned(Flag::kTouchedVoxel);
}


template <typename T>
void Voxel<T>::updateChunkTouchAndCompression(bool retain_chunk)
{
  if (map_ && chunk_)
  {
    if (flags_ & unsigned(Flag::kTouchedChunk))
    {
      detail::VoxelChunkAccess<T>::touch(map_, chunk_, layer_index_);
    }
    if (!retain_chunk && (flags_ & unsigned(Flag::kCompressionLock)))
    {
      chunk_->voxel_blocks[layer_index_]->release();
      chunk_ = nullptr;
    }
  }
  // Always clear Flag::kTouchedChunk however, we only clear Flag::kCompressionLock if we are not retaining the chunk.
  auto clear_flags = unsigned(Flag::kTouchedChunk);
  clear_flags |= !!retain_chunk * unsigned(Flag::kCompressionLock);
  flags_ &= ~clear_flags;
}
}  // namespace ohm

#endif  // OHMVOXEL_H


================================================
FILE: ohm/VoxelBlock.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "VoxelBlock.h"

#include "MapLayer.h"
#include "VoxelBlockCompressionQueue.h"

#include "private/OccupancyMapDetail.h"

#include <zlib.h>

#include <algorithm>
#include <cstring>

namespace ohm
{
namespace
{
const unsigned kDefaultBufferSize = 1024u;
unsigned g_minimum_buffer_size = kDefaultBufferSize;
int g_zlib_compression_level = Z_BEST_SPEED;
int g_zlib_gzip_flag = 0;  // Use 16 to enable GZip.
const int kWindowBits = 14;
const int kZLibMemLevel = 8;
const int kCompressionStrategy = Z_DEFAULT_STRATEGY;

const int kGZipCompressionFlag = 16;
const unsigned kReleaseDelayMs = 500;
/// When reserving compressed buffer space, device the uncompressed size by this factor.
const unsigned kBufferReservationQutient = 10;
}  // namespace


void VoxelBlock::getCompressionControls(CompressionControls *controls)
{
  controls->minimum_buffer_size = g_minimum_buffer_size;

  switch (g_zlib_compression_level)
  {
  default:
  case Z_DEFAULT_COMPRESSION:
    controls->compression_level = kCompressBalanced;
    break;
  case Z_BEST_SPEED:
    controls->compression_level = kCompressFast;
    break;
  case Z_BEST_COMPRESSION:
    controls->compression_level = kCompressMax;
    break;
  }

  if (g_zlib_gzip_flag)
  {
    controls->compression_type = kCompressGZip;
  }
  else
  {
    controls->compression_type = kCompressDeflate;
  }
}

void VoxelBlock::setCompressionControls(const CompressionControls &controls)
{
  g_minimum_buffer_size = (controls.minimum_buffer_size > 0) ? controls.minimum_buffer_size : g_minimum_buffer_size;
  switch (controls.compression_level)
  {
  default:
  case kCompressFast:
    g_zlib_compression_level = Z_BEST_SPEED;
    break;
  case kCompressBalanced:
    g_zlib_compression_level = Z_DEFAULT_COMPRESSION;
    break;
  case kCompressMax:
    g_zlib_compression_level = Z_BEST_COMPRESSION;
    break;
  }

  g_zlib_gzip_flag = (controls.compression_type == kCompressGZip) ? kGZipCompressionFlag : 0;
}


VoxelBlock::VoxelBlock(const OccupancyMapDetail *map, const MapLayer &layer)
  : map_(map)
  , layer_index_(layer.layerIndex())
  , uncompressed_byte_size_(layer.layerByteSize(map->region_voxel_dimensions))
{
  initUncompressed(voxel_bytes_, layer);
  flags_ |= kFUncompressed;
  // Try add to compression process if the map uses compression.
  if ((map->flags & MapFlag::kCompressed) == MapFlag::kCompressed)
  {
    VoxelBlockCompressionQueue::instance().push(this);
  }
}


VoxelBlock::~VoxelBlock() = default;


void VoxelBlock::destroy()
{
  // Don't use scoped lock as we will delete this which would make releasing the lock invalid.
  access_guard_.lock();
  if (flags_ & kFManagedForCompression)
  {
    // Currently queue. Mark for death. The compression queue will destroy it.
    flags_ |= kFMarkedForDeath;
    access_guard_.unlock();
  }
  else
  {
    // Not managed for compression. Delete now.
    // fprintf(stderr, "0x%" PRIXPTR ", VoxelBlock::destroy()\n", (uintptr_t)this);
    delete this;
  }
}


void VoxelBlock::retain()
{
  std::unique_lock<Mutex> guard(access_guard_);
  ++reference_count_;
  flags_ |= kFLocked;  // Ensure block is lock to prevent compression.
  // Ensure uncompressed data are available.
  if (!(flags_ & kFUncompressed))
  {
    std::vector<uint8_t> working_buffer;
    uncompressUnguarded(working_buffer);
    voxel_bytes_.swap(working_buffer);
    flags_ |= kFUncompressed;
  }
}

void VoxelBlock::release()
{
  std::unique_lock<Mutex> guard(access_guard_);
  if (reference_count_ > 0)
  {
    --reference_count_;
    if (reference_count_ == 0)
    {
      // Unlock to allow compression.
      flags_ &= ~kFLocked;
    }
  }
}

#if 0
void VoxelBlock::compressInto(std::vector<uint8_t> &compression_buffer)
{
  std::unique_lock<Mutex> guard(access_guard_);

  // Handle uninitialised buffer. We may not have initialised the buffer yet, but this call requires data to be
  // compressed such as when used for serialisation to disk.
  if (voxel_bytes_.empty())
  {
    initUncompressed(voxel_bytes_, map_->layout.layer(layer_index_));
    flags_ |= kFUncompressed;
  }

  compressUnguarded(compression_buffer);
}
#endif  // 0

size_t VoxelBlock::compress()
{
  std::vector<uint8_t> compression_buffer;
  return compressWithTemporaryBuffer(compression_buffer);
}

size_t VoxelBlock::compressWithTemporaryBuffer(std::vector<uint8_t> &compression_buffer)
{
  std::unique_lock<Mutex> guard(access_guard_);

  if (!reference_count_ && !(flags_ & kFLocked))
  {
    // Handle uninitialised buffer. We may not have initialised the buffer yet, but this call requires data to be
    // compressed such as when used for serialisation to disk.
    if (voxel_bytes_.empty())
    {
      initUncompressed(voxel_bytes_, map_->layout.layer(layer_index_));
      flags_ |= kFUncompressed;
    }

    compressUnguarded(compression_buffer);
    setCompressedBytesUnguarded(compression_buffer);
    return compression_buffer.size();
  }
  return 0;
}

void VoxelBlock::updateLayerIndex(unsigned layer_index)
{
  std::unique_lock<Mutex> guard(access_guard_);
  layer_index_ = layer_index;
}

bool VoxelBlock::supportsCompression() const
{
  return (map_->flags & MapFlag::kCompressed) == MapFlag::kCompressed;
}

bool VoxelBlock::compressUnguarded(std::vector<uint8_t> &compression_buffer)
{
  if (flags_ & kFUncompressed)
  {
    int ret = Z_OK;
    z_stream stream;
    memset(&stream, 0u, sizeof(stream));
    // NOLINTNEXTLINE(hicpp-signed-bitwise)
    deflateInit2(&stream, g_zlib_compression_level, Z_DEFLATED, kWindowBits | g_zlib_gzip_flag, kZLibMemLevel,
                 kCompressionStrategy);

    stream.next_in = static_cast<Bytef *>(voxel_bytes_.data());
    stream.avail_in = unsigned(voxel_bytes_.size());

    compression_buffer.reserve(
      std::max(voxel_bytes_.size() / kBufferReservationQutient, static_cast<size_t>(g_minimum_buffer_size)));
    compression_buffer.resize(compression_buffer.capacity());

    stream.avail_out = unsigned(compression_buffer.size());
    stream.next_out = compression_buffer.data();

    int flush_flag = Z_NO_FLUSH;
    do
    {
      ret = deflate(&stream, flush_flag);

      switch (ret)
      {
      case Z_OK:
        // Done with input data. Make sure we change to flushing.
        if (stream.avail_in == 0)
        {
          flush_flag = Z_FINISH;
        }

        // Check for insufficient output data before Z_STREAM_END.
        if (stream.avail_out == 0)
        {
          // Output buffer too small.
          const size_t bytes_so_far = compression_buffer.size();
          compression_buffer.resize(2 * bytes_so_far);
          stream.avail_out = unsigned(compression_buffer.size() - bytes_so_far);
          stream.next_out = compression_buffer.data() + bytes_so_far;
        }
        break;
      case Z_STREAM_END:
        break;
      default:
        // Failed.
        deflateEnd(&stream);
        return false;
      }
    } while (stream.avail_in || ret != Z_STREAM_END);

    // Ensure flush.
    if (flush_flag != Z_FINISH)
    {
      deflate(&stream, Z_FINISH);
    }

    ret = deflateEnd(&stream);
    if (ret != Z_OK)
    {
      return false;
    }

    // Resize compressed buffer.
    compression_buffer.resize(compression_buffer.size() - stream.avail_out);
  }
  else
  {
    // Already compressed. Copy buffer.
    compression_buffer.resize(voxel_bytes_.size());
    if (!voxel_bytes_.empty())
    {
      memcpy(compression_buffer.data(), voxel_bytes_.data(), sizeof(*voxel_bytes_.data()) * voxel_bytes_.size());
    }
  }

  return true;
}

bool VoxelBlock::uncompressUnguarded(std::vector<uint8_t> &expanded_buffer)
{
  if (voxel_bytes_.empty())
  {
    initUncompressed(voxel_bytes_, map_->layout.layer(layer_index_));
    flags_ |= kFUncompressed;
  }

  if (flags_ & kFUncompressed)
  {
    // Simply copy existing bytes.
    expanded_buffer.resize(voxel_bytes_.size());
    if (!voxel_bytes_.empty())
    {
      memcpy(expanded_buffer.data(), voxel_bytes_.data(), sizeof(*voxel_bytes_.data()) * voxel_bytes_.size());
    }
    return true;
  }

  expanded_buffer.resize(uncompressed_byte_size_);

  int ret = Z_OK;
  z_stream stream;
  memset(&stream, 0u, sizeof(stream));
  inflateInit2(&stream, kWindowBits | g_zlib_gzip_flag);  // NOLINT(hicpp-signed-bitwise)

  stream.avail_in = unsigned(voxel_bytes_.size());
  stream.next_in = voxel_bytes_.data();

  stream.avail_out = unsigned(expanded_buffer.size());
  stream.next_out = static_cast<unsigned char *>(expanded_buffer.data());

  int flush_flag = Z_NO_FLUSH;
  do
  {
    ret = inflate(&stream, flush_flag);

    switch (ret)
    {
    case Z_OK:
      // Check for insufficient output data on flush or before finishing input data. This is an error an error condition
      // as we know how large it should be.
      if (stream.avail_out == 0 && (flush_flag == Z_FINISH || stream.avail_in))
      {
        // Failed.
        inflateEnd(&stream);
        return false;
      }

      // Transition to flush if there is no more input data.
      if (stream.avail_in == 0)
      {
        flush_flag = Z_FINISH;
      }
      break;
    case Z_STREAM_END:
      break;
    default:
      // Failed.
      inflateEnd(&stream);
      return false;
    }
  } while (stream.avail_in || ret != Z_STREAM_END);

  // Ensure flush.
  if (flush_flag != Z_FINISH)
  {
    inflate(&stream, Z_FINISH);
  }

  // Resize compressed buffer.
  expanded_buffer.resize(expanded_buffer.size() - stream.avail_out);
  inflateEnd(&stream);

  return true;
}


void VoxelBlock::initUncompressed(std::vector<uint8_t> &expanded_buffer, const MapLayer &layer)
{
  expanded_buffer.resize(uncompressedByteSize());
  layer.clear(expanded_buffer.data(), map_->region_voxel_dimensions);
}


void VoxelBlock::setCompressedBytesUnguarded(const std::vector<uint8_t> &compressed_voxels)
{
  voxel_bytes_.resize(compressed_voxels.size());
  if (!compressed_voxels.empty())
  {
    memcpy(voxel_bytes_.data(), compressed_voxels.data(), sizeof(*compressed_voxels.data()) * compressed_voxels.size());
  }
  voxel_bytes_.shrink_to_fit();
  compressed_byte_size_ = voxel_bytes_.size();
  // Clear uncompressed flag.
  flags_ &= ~(kFUncompressed);
}
}  // namespace ohm


================================================
FILE: ohm/VoxelBlock.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef VOXELBLOCK_H
#define VOXELBLOCK_H

#include "OhmConfig.h"

#include "Mutex.h"

#include <glm/fwd.hpp>
#include <glm/vec3.hpp>

#include <atomic>
#include <chrono>
#include <mutex>
#include <vector>

namespace ohm
{
class MapLayer;
class VoxelBlockCompressionQueue;
struct OccupancyMapDetail;

/// A utility class used to track the memory for a dense voxel layer in a @c MapChunk. This class ensures voxel memory
/// is uncompressed when requested and compressed using the background compression thread when no longer needed.
///
/// Internally the @c VoxelBlock allocates or decompresses voxel memory for its associated @c MapLayer
/// (@c layerInfo()) when @c retain() is called. It then maintains a reference count for the number of @c retain()
/// calls ensuring uncompressed voxel data remain valid until all references are by calling @c release(). The block is
/// then passed to the background compression thread when the last reference is released. The level of compression
/// can be globally set using the static @c setCompressionControls() function.
///
/// Typically, @c retain() and @c release() should not be called directly. Instead user code should use the @c Voxel
/// data access object. This object manages multiple aspects of voxel data access including ensuring @c retain() and
/// @c release() are called as needed.
///
/// When a @c VoxelBlock is pushed onto the background compression queue, it is assigned a release time before which
/// it cannot be compressed. The background thread cannot compress the block before this time elapses. The time is
/// updated on each release, ensuring that the block is not compressed until some time after the most recent release.
/// This is currently based on wall clock time and may change in future.
///
/// The block also deals with cases where the background thread is in the process of compressing the voxel data while
/// the reference count is non zero or when the background thread is processing the block when the map chunk is
/// deleted.
class ohm_API VoxelBlock
{
  friend VoxelBlockCompressionQueue;

public:
  /// Deleter object for correctly releasing a @c VoxelBlock
  class ohm_API Deleter
  {
  public:
    /// Deletion operator calling through to @c VoxelBlock::destroy()
    /// @param block The object to destroy.
    inline void operator()(VoxelBlock *block) { block->destroy(); }
  };

  /// Unique pointer definition for a @c VoxelBlock . Ensures correct custom deletion.
  using Ptr = std::unique_ptr<VoxelBlock, Deleter>;

  /// The mutex used to protect threaded access.
  using Mutex = ohm::SpinMutex;
  /// The clock to use for tracking release time.
  using Clock = std::chrono::steady_clock;

  /// Flags marking the @c VoxelBlock status.
  enum Flag : unsigned
  {
    /// Memory buffer currently holds uncompressed voxel data.
    kFUncompressed = (1u << 0u),
    /// Block is locked and ineligible for compression.
    kFLocked = (1u << 1u),
    // /// Block is queued for compression.
    // kFCompressionQueued = (1u << 1u),
    /// Block is to be deleted. Only set when the block should be deleted but is currently on the compression thread.
    kFMarkedForDeath = (1u << 2u),
    /// Block is part of the compression system.
    kFManagedForCompression = (1u << 3u)
  };

  /// Compression level options
  enum CompressionLevel
  {
    /// Use the fastest compression (default).
    kCompressFast,
    /// Used balanced size/speed compression
    kCompressBalanced,
    /// Use maximum compression (slowest)
    kCompressMax
  };

  /// Compression type.
  enum CompressionType
  {
    /// ZLib deflate.
    kCompressDeflate,
    /// GZip compression.
    kCompressGZip
  };

  /// Static compression controls.
  struct ohm_API CompressionControls
  {
    /// Minimum initial buffer size used when compressing a voxel block.
    unsigned minimum_buffer_size = 0;
    /// Voxel block compression level.
    CompressionLevel compression_level = kCompressFast;
    /// Voxel block compression technique.
    CompressionType compression_type = kCompressDeflate;
  };

  /// Get the current compression controls.
  /// @param[out] controls A non-null pointer to the structure in which current compression settings are returned.
  static void getCompressionControls(CompressionControls *controls);
  /// Set the voxel block compression controls. Should only be called before maps are created and voxel compression
  /// begins.
  /// @param controls New compression settings.
  static void setCompressionControls(const CompressionControls &controls);

  /// Create a voxel block within the given @p map for the given @p layer_index.
  ///
  /// @note At the time of the call, there are cases where the @p layer will not match the layout in @p map. This
  /// occurs when updating the layout. The calling code must rectify this situation before other code may use the
  /// @c VoxelBlock .
  /// @param map Details of the occupancy map to which the block belongs.
  /// @param layer The @p MapLayer which the voxel block represents.
  VoxelBlock(const OccupancyMapDetail *map, const MapLayer &layer);

private:
  /// Hidden destructor for dealing with the processing queue safely.
  /// Use destroy().
  ~VoxelBlock();

public:
  /// Delete and destroy this object; use in place of <tt>delete voxel_block</tt>.
  ///
  /// This will either immediately delete the @c VoxelBlock or mark it for deletion by the compression thread.
  /// As such, the object instance should no longer be used after making this call, but it may persist until the
  /// background thread releases it.
  void destroy();

  /// Static overload to destroy the @p block for use with @c std::unique_ptr . Calls through to the instance overload
  /// of @c VoxelBlock::destroy() .
  /// @param block The object to destroy.
  inline static void destroy(VoxelBlock *block) { block->destroy(); }

  /// Size of a single voxel in the map.
  /// @return The size of a voxel in bytes.
  size_t perVoxelByteSize() const;

  /// Uncompressed data size.
  /// @return The uncompressed size of the voxel map in bytes.
  inline size_t uncompressedByteSize() const { return uncompressed_byte_size_; }

  /// Query current flag values.
  inline unsigned flags() const { return flags_; }

  /// Retain the uncompressed voxel memory until a corresponding @c release() call. Not recommended; use
  /// @c voxelBuffer().
  ///
  /// This call may block while the voxel memory is uncompressed or allocated an initialised.
  void retain();

  /// Release the uncompressed voxel memory until a corresponding @c release() call. Not recommended; use
  /// @c voxelBuffer().
  void release();

#if 0
  // This function could be useful, but I'm not keen on maintaining it.

  /// Compress the voxel data into @p compression_buffer. Writes the current voxel bytes when already compressed.
  ///
  /// @note This may initialise the buffer if empty, so that this method can be used for serialisation of the map to
  ///   disk.
  /// @param[in,out] compression_buffer Buffer to write compression data into. Resized to the compressed data size.
  void compressInto(std::vector<uint8_t> &compression_buffer);
#endif  // #

  /// Attempt to compress the @c VoxelBlock memory.
  ///
  /// This call can only succeed if the current reference count is zero (and the kFLocked flag is clear). The compressed
  /// data size is returned on success.
  ///
  /// Threadsafe.
  ///
  /// @return The compressed data size on success, zero on failure.
  size_t compress();

  /// Attempt to compress the @c VoxelBlock memory. Uses the @p compression_buffer as temporary storage during
  /// compression.
  ///
  /// This call can only succeed if the current reference count is zero (and the kFLocked flag is clear). The compressed
  /// data size is returned on success.
  ///
  /// Threadsafe.
  ///
  /// @param compression_buffer Temporary buffer to use during compression. As a side effect of success, this will
  ///     contain the compressed voxel data, but this behaviour is not guaranteed and may change.
  /// @return The compressed data size on success, zero on failure.
  size_t compressWithTemporaryBuffer(std::vector<uint8_t> &compression_buffer);

  /// Direct access to the voxel bytes. Should be retained first. For internal use.
  /// @return Voxel bytes.
  uint8_t *voxelBytes();

  /// Direct access to the voxel bytes. Should be retained first. For internal use.
  /// @return Voxel bytes.
  const uint8_t *voxelBytes() const;

  /// Internal function for updating the layer index value when remapping layouts. For internal use.
  /// @param layer_index The new layer index.
  void updateLayerIndex(unsigned layer_index);

private:
  /// True if the map configuration supports compression of this voxel block.
  bool supportsCompression() const;

  /// Perform the compression operation into @p compression_buffer without locking the mutex . This is called from
  /// @c compressInto() after locking the @c access_guard_ .
  ///
  /// @param compression_buffer The buffer to compress into. Final size will exactly match the compressed data size
  ///   though the capacity may be larger.
  /// @return True if compressio into @p compression_buffer succeeded.
  bool compressUnguarded(std::vector<uint8_t> &compression_buffer);
  /// Decompress voxel data into @p expanded_buffer without locking the mutex. This is called from @c retain() after
  /// the mutex is locked.
  /// @param expanded_buffer The buffer to populate with uncompressed data.
  /// @return True on successfully decompressing.
  bool uncompressUnguarded(std::vector<uint8_t> &expanded_buffer);
  /// Initialise the given buffer to uncompressed voxel data. The voxel data is cleared to the appropriate pattern
  /// for the voxel layer.
  /// @param expanded_buffer The buffer to initialised.
  /// @param layer The layer used to initialise the memory. Must be explicitly passed to handle map layout changes.
  void initUncompressed(std::vector<uint8_t> &expanded_buffer, const MapLayer &layer);
  /// Swap the voxel bytes with the given compressed voxel bytes, but only if there are currently no retained
  /// references. This is for use byte the @c VoxelBlockCompressionQueue.
  /// @param compressed_voxels The compressed voxel data.
  /// @return The compressed byte size on success, zero on failure or inability to compress.
  void setCompressedBytesUnguarded(const std::vector<uint8_t> &compressed_voxels);

  /// Voxel data.
  ///
  /// This data can be in one of three states:
  /// 1. Empty implying no changes have been made from the default initialised values.
  /// 2. Uncompressed when not empty and `flags_ & kFUncompressed` set.
  /// 3. Compressed when not emtpy and `flags_ & kFUncompressed` clear.
  std::vector<uint8_t> voxel_bytes_;
  /// Data access mutex
  mutable Mutex access_guard_;
  /// Number of oustandting @c retain() calls. Cannot be compressed while no zero.
  std::atomic_uint32_t reference_count_{ 0 };
  /// Block status @c Flag values.
  std::atomic_uint32_t flags_{ 0 };
  /// Timepoint after which the block may be compressed.
  Clock::time_point release_after_;
  /// The owning occupancy map detail.
  const OccupancyMapDetail *map_ = nullptr;
  /// The index into the @c MapLayout represented by this voxel data.
  unsigned layer_index_ = 0;
  /// Byte size of this voxel block when uncompressed.
  size_t uncompressed_byte_size_ = 0;
  /// Byte size of this voxel block when uncompressed.
  size_t compressed_byte_size_ = 0;
};

inline uint8_t *VoxelBlock::voxelBytes()
{
  return voxel_bytes_.data();
}

inline const uint8_t *VoxelBlock::voxelBytes() const
{
  return voxel_bytes_.data();
}
}  // namespace ohm

#endif  // VOXELBLOCK_H


================================================
FILE: ohm/VoxelBlockCompressionQueue.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "VoxelBlockCompressionQueue.h"

#include "VoxelBlock.h"

#include "private/VoxelBlockCompressionQueueDetail.h"

#include <logutil/Logger.h>

#include <algorithm>
#include <chrono>
#include <cinttypes>
#include <iostream>

namespace ohm
{
const int kSleepIntervalMs = 50;

VoxelBlockCompressionQueue &VoxelBlockCompressionQueue::instance()
{
  static VoxelBlockCompressionQueue queue_instance;
  return queue_instance;
}

VoxelBlockCompressionQueue::VoxelBlockCompressionQueue(bool test_mode)
  : imp_(new VoxelBlockCompressionQueueDetail)
{
  imp_->test_mode = test_mode;
}

VoxelBlockCompressionQueue::~VoxelBlockCompressionQueue()
{
  if (imp_->running)
  {
    std::unique_lock<VoxelBlockCompressionQueueDetail::Mutex> guard(imp_->ref_lock);
    joinCurrentThread();
  }
}

void VoxelBlockCompressionQueue::retain()
{
  std::unique_lock<VoxelBlockCompressionQueueDetail::Mutex> guard(imp_->ref_lock);
  if (++imp_->reference_count == 1)
  {
    // Start the thread.
    imp_->running = true;
    imp_->processing_thread = std::thread([this]() { this->run(); });
  }
}


void VoxelBlockCompressionQueue::release()
{
  std::unique_lock<VoxelBlockCompressionQueueDetail::Mutex> guard(imp_->ref_lock);
  int ref = --imp_->reference_count;
  if (ref == 0)
  {
    joinCurrentThread();
  }
  else if (ref < 0)
  {
    imp_->reference_count = 0;
  }
}


uint64_t VoxelBlockCompressionQueue::highTide() const
{
  return imp_->high_tide;
}


void VoxelBlockCompressionQueue::setHighTide(uint64_t tide)
{
  imp_->high_tide = tide;
}


uint64_t VoxelBlockCompressionQueue::lowTide() const
{
  return imp_->low_tide;
}


void VoxelBlockCompressionQueue::setLowTide(uint64_t tide)
{
  imp_->low_tide = tide;
}


uint64_t VoxelBlockCompressionQueue::estimatedAllocationSize() const
{
  return imp_->estimated_allocated_size;
}


void VoxelBlockCompressionQueue::push(VoxelBlock *block)
{
  if (imp_->running || imp_->test_mode)
  {
    block->flags_ |= VoxelBlock::kFManagedForCompression;
    ohm::push(*imp_, block);
  }
}


bool VoxelBlockCompressionQueue::testMode() const
{
  return imp_->test_mode;
}


void VoxelBlockCompressionQueue::__tick(std::vector<uint8_t> &compression_buffer)
{
  // Process any new items added to the compression queue by adding them to the block list.
  {
    VoxelBlock *voxels = nullptr;

    while (ohm::tryPop(*imp_, &voxels))
    {
      imp_->blocks.emplace_back(CompressionEntry{ voxels, 0u });
    }
  }

  // Estimate the current memory usage and release items marked for death.
  uint64_t memory_usage = 0;
  const uint64_t high_tide = imp_->high_tide;
  const uint64_t low_tide = imp_->low_tide;
  for (auto iter = imp_->blocks.begin(); iter != imp_->blocks.end();)
  {
    CompressionEntry &entry = *iter;
    // Check if marked for death.
    if (!(entry.voxels->flags_ & VoxelBlock::kFMarkedForDeath))
    {
      // Still alive. Update th entry's allocation size.
      if (entry.voxels->flags_ & VoxelBlock::kFUncompressed)
      {
        entry.allocation_size = entry.voxels->uncompressed_byte_size_;
      }
      else
      {
        entry.allocation_size = entry.voxels->compressed_byte_size_;
      }

      memory_usage += entry.allocation_size;
      ++iter;
    }
    else
    {
      // Marked for death. Remove this entry;
      // Block no longer required. Remove it.
      // Marked for death. Clean it up.
      // Lock access guard to make sure the code that sets the flag has completed
      iter->voxels->access_guard_.lock();
      // fprintf(stderr, "0x%" PRIXPTR ", VoxelBlockCompressionQueue\n", (uintptr_t)iter->voxels);
      delete iter->voxels;
      iter = imp_->blocks.erase(iter);
    }
  }

  // Check if we are over the high tide and release what we can.
  if (memory_usage >= high_tide)
  {
    // Sort all blocks by allocation size.
    // TODO(KS): consider including the queued for compression flag so we push items to be compressed to the front
    // of the sort results.
    // TODO(KS): try using a partial sort.
    std::sort(imp_->blocks.begin(), imp_->blocks.end(), [](const CompressionEntry &a, const CompressionEntry &b) {
      return a.allocation_size > b.allocation_size;
    });

    auto iter = imp_->blocks.begin();
    // Free until we reach the low tide.
    while (iter != imp_->blocks.end() && memory_usage >= low_tide)
    {
      // Check if marked for death. The status may have changed since we updated the allocation size.
      if (!(iter->voxels->flags_ & VoxelBlock::kFMarkedForDeath))
      {
        // Not maked for death. Check lock flag and compress this item if we can
        if (!(iter->voxels->flags_ & VoxelBlock::kFLocked))
        {
          // Block is not locked.
          logutil::trace("compress\n");
          // Try compress the current item. This could fail as the flag can have changed. On failure, the
          // compressed_size will be zero. We call compressWithTemporaryBuffer() to re-use the compression buffer
          // memory.
          size_t compressed_size = iter->voxels->compressWithTemporaryBuffer(compression_buffer);
          if (compressed_size)
          {
            // Compression succeeded.
            // Adjust memory_usage down in a way which guarantees no underflow. Paranoia.
            memory_usage = (memory_usage > iter->allocation_size) ? memory_usage - iter->allocation_size : 0u;
            memory_usage += compressed_size;
          }
        }

        ++iter;
      }
      else
      {
        // Block no longer required. Remove it.
        // Marked for death. Clean it up.
        // Lock access guard to make sure the code that sets the flag has completed
        iter->voxels->access_guard_.lock();
        // fprintf(stderr, "0x%" PRIXPTR ", VoxelBlockCompressionQueue\n", (uintptr_t)iter->voxels);
        delete iter->voxels;
        // Adjust memory_usage down in a way which guarantees no underflow. Paranoia.
        memory_usage = (memory_usage > iter->allocation_size) ? memory_usage - iter->allocation_size : 0u;
        iter = imp_->blocks.erase(iter);
      }
    }
  }

  imp_->estimated_allocated_size = memory_usage;
}

void VoxelBlockCompressionQueue::joinCurrentThread()
{
  // Mark thread for quit.
  if (imp_->running)
  {
    imp_->quit_flag = true;
    imp_->processing_thread.join();
    // Clear the running and quit flags.
    imp_->running = false;
    imp_->quit_flag = false;
  }
}


void VoxelBlockCompressionQueue::run()
{
  std::vector<uint8_t> compression_buffer;
  while (!imp_->quit_flag)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(kSleepIntervalMs));
    __tick(compression_buffer);
  }
}
}  // namespace ohm


================================================
FILE: ohm/VoxelBlockCompressionQueue.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef VOXELBLOCKCOMPRESSIONQUEUE_H
#define VOXELBLOCKCOMPRESSIONQUEUE_H

#include "OhmConfig.h"

#include <vector>

namespace ohm
{
class VoxelBlock;
struct VoxelBlockCompressionQueueDetail;

/// Background compression thread used to manage compression of @c VoxelBlock data.
///
/// The queue supports reference counting in order to allow a single queue to be used accross multiple maps.
/// Each map should call @c retain() on construction and @c release() when done. The queue supports releasing the
/// last reference then attaining a new reference to start a new background thread.
///
/// An @c OccupancyMap will call @c retain() and @c release() on construction and destruction respectively.
class ohm_API VoxelBlockCompressionQueue
{
public:
  /// Singleton access.
  static VoxelBlockCompressionQueue &instance();

  /// Constructor.
  ///
  /// The @p test_mode must only be used for (unit) testing. This puts the queue into a synchronous mode and requires
  /// explicit calls to @c __tick().
  /// @param test_mode When true, creates a queue for testing.
  explicit VoxelBlockCompressionQueue(bool test_mode = false);
  /// Destructor. Ensures thread is joined.
  ~VoxelBlockCompressionQueue();

  /// Retain the compression queue. The first reference starts the background thread.
  void retain();

  /// Release the compression queue. Releasing the last reference joins the background thread.
  /// The next @c retain() call after the last @c release() reference will start a new thread.
  void release();

  /// Memory threshold (bytes) at which the queue starts compressing voxel blocks.
  uint64_t highTide() const;
  /// Set the @c highTide().
  /// @param tide The high tide (bytes) to start compressing once exceeded.
  void setHighTide(uint64_t tide);
  /// Memory level (bytes) to which the queue tries to compress down to once the high tide is exceeded.
  uint64_t lowTide() const;
  /// Set the @c lowTide().
  /// @param tide The low tide (bytes) to stop compressing once below.
  void setLowTide(uint64_t tide);
  /// Query the number of bytes allocated to voxel blocks managed by this compressor (byte).
  uint64_t estimatedAllocationSize() const;

  /// Push a @c VoxelBlock on the queue for compression.
  /// @param block The block to compress.
  void push(VoxelBlock *block);

  /// True if this object has been created in test mode.
  bool testMode() const;

  /// Test mode use only: called to perform a compression cycle. Must only be called when in @c testMode().
  /// @param compression_buffer Buffer used to compress into.
  void __tick(std::vector<uint8_t> &compression_buffer);

private:
  void joinCurrentThread();

  /// Main compression loop. This is the thread entry point.
  void run();

  std::unique_ptr<VoxelBlockCompressionQueueDetail> imp_;
};
}  // namespace ohm

#endif  // VOXELBLOCKCOMPRESSIONQUEUE_H


================================================
FILE: ohm/VoxelBuffer.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "VoxelBuffer.h"

namespace ohm
{
template <typename VoxelBlock>
VoxelBuffer<VoxelBlock>::VoxelBuffer(ohm::VoxelBlock *block)
  : voxel_block_(block)
{
  if (block)
  {
    block->retain();
    voxel_memory_size_ = block->uncompressedByteSize();
    voxel_memory_ = block->voxelBytes();
  }
}

template <typename VoxelBlock>
VoxelBuffer<VoxelBlock>::VoxelBuffer(VoxelBuffer<VoxelBlock> &&other) noexcept
  : voxel_memory_(std::exchange(other.voxel_memory_, nullptr))
  , voxel_memory_size_(std::exchange(other.voxel_memory_size_, 0))
  , voxel_block_(std::exchange(other.voxel_block_, nullptr))
{}

template <typename VoxelBlock>
VoxelBuffer<VoxelBlock>::VoxelBuffer(const VoxelBuffer<VoxelBlock> &other)
  : voxel_memory_(other.voxel_memory_)
  , voxel_memory_size_(other.voxel_memory_size_)
  , voxel_block_(other.voxel_block_)
{
  if (voxel_block_)
  {
    voxel_block_->retain();
  }
}

template <typename VoxelBlock>
VoxelBuffer<VoxelBlock>::~VoxelBuffer()
{
  release();
}

template <typename VoxelBlock>
VoxelBuffer<VoxelBlock> &VoxelBuffer<VoxelBlock>::operator=(VoxelBuffer<VoxelBlock> &&other) noexcept
{
  std::swap(voxel_block_, other.voxel_block_);
  std::swap(voxel_memory_size_, other.voxel_memory_size_);
  std::swap(voxel_memory_, other.voxel_memory_);
  return *this;
}

template <typename VoxelBlock>
VoxelBuffer<VoxelBlock> &VoxelBuffer<VoxelBlock>::operator=(const VoxelBuffer<VoxelBlock> &other)
{
  if (this != &other)
  {
    release();
    voxel_block_ = other.voxel_block_;
    if (voxel_block_)
    {
      voxel_block_->retain();
      voxel_memory_size_ = voxel_block_->uncompressedByteSize();
      voxel_memory_ = voxel_block_->voxelBytes();
    }
  }
  return *this;
}

template <typename VoxelBlock>
void VoxelBuffer<VoxelBlock>::release()
{
  if (voxel_block_)
  {
    voxel_block_->release();
    voxel_block_ = nullptr;
    voxel_memory_ = nullptr;
    voxel_memory_size_ = 0;
  }
}

// Export explicit template instantiations
#ifdef _MSC_VER
// Need to export the template instantiations for the Microsoft compiler only.
template class ohm_API VoxelBuffer<VoxelBlock>;
template class ohm_API VoxelBuffer<const VoxelBlock>;
#else   // _MSC_VER
template class VoxelBuffer<VoxelBlock>;
template class VoxelBuffer<const VoxelBlock>;
#endif  // _MSC_VER
}  // namespace ohm


================================================
FILE: ohm/VoxelBuffer.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef VOXELBUFFER_H
#define VOXELBUFFER_H

#include "OhmConfig.h"

#include "VoxelBlock.h"

#include <cinttypes>
#include <cstring>
#include <memory>
#include <type_traits>

namespace ohm
{
/// Base class for @c VoxelBuffer and @c VoxelBufferConst with common functionality between the two.
///
/// The voxel buffer classes ensure voxel data from a @c VoxelBlock are retained in uncompressed form so long as the
/// buffer object persists. The @c VoxelBlock data are releases, allowing for background compression once the buffer
/// object is released/out of scope.
template <typename VoxelBlock>
class VoxelBuffer
{
public:
  /// Define the pointer to the voxel memory. This is `const` if @c VoxelBlock is a const template argument.
  using VoxelPtr = typename std::conditional<std::is_const<VoxelBlock>::value, const uint8_t *, uint8_t *>::type;

  /// Default constructor : creates an invalid buffer reference object.
  inline VoxelBuffer() = default;
  /// Constructor wrapping data from the given @c block . Immediately calls @c ohm::VoxelBuffer::retain() .
  /// @param block A pointer to the block to retain. Must not be null.
  explicit VoxelBuffer(ohm::VoxelBlock *block);
  /// Overloaded constructor handling a @c std::unique_ptr wrapper around a @c ohm::VoxelBlock .
  /// @param block A unique pointer to the block to retain. Must not be null.
  inline explicit VoxelBuffer(const ohm::VoxelBlock::Ptr &block)
    : VoxelBuffer(block.get())
  {}
  /// RValue constructor.
  /// @param other The object to construct from.
  VoxelBuffer(VoxelBuffer<VoxelBlock> &&other) noexcept;
  /// Copy constructor constructor.
  /// @param other The object to construct from.
  VoxelBuffer(const VoxelBuffer<VoxelBlock> &other);

  /// Destructor, ensuring a call to @c ohm::VoxelBlock::release() for the wrapped block.
  ~VoxelBuffer();

  /// RValue assignment operator.
  /// @param other The object to assign from.
  /// @return `*this`
  VoxelBuffer<VoxelBlock> &operator=(VoxelBuffer<VoxelBlock> &&other) noexcept;

  /// Copy assignment operator.
  /// @param other The object to assign from
  /// @return `*this`
  VoxelBuffer<VoxelBlock> &operator=(const VoxelBuffer<VoxelBlock> &other);

  /// Does the current object reference a valid @c ohm::VoxelBlock ?
  /// @return True if referencing a valid object.
  bool isValid() const { return voxel_memory_ != nullptr; }

  /// Access the raw memory stored by the wrapped @c VoxelBlock . The returned point is `const` when the template type
  /// is `const`.
  ///
  /// Must only be called if @c isValid() is `true`.
  /// @return A pointer to the referenced voxel memory.
  VoxelPtr voxelMemory() const { return voxel_memory_; }
  /// Query the uncompressed byte size of the voxel memory for the retained layer.
  /// @return The uncompressed size for the voxel memory, or zero when @c isValid() is `false`.
  size_t voxelMemorySize() const { return voxel_memory_size_; }
  /// Access the wrapped @c VoxelBlock pointer.
  /// @return The wrapped object pointer.
  VoxelBlock *voxelBlock() const { return voxel_block_; }

  /// Read the content for a voxel in the buffer. Must only be called if @c isValid() , @c voxel_index is in range
  /// and @c T is the contained data type, exactly matching the voxel data size.
  /// @param voxel_index The index of the voxel to read data for. Must be in range.
  /// @param[out] value The voxel content is written to this address.
  /// @tparam T The data type to read. Must exactly match the voxel size and content for the referenced voxel layer.
  template <typename T>
  void readVoxel(unsigned voxel_index, T *value)
  {
    memcpy(value, voxelMemory() + sizeof(T) * voxel_index, sizeof(T));
  }

  /// Write the content for a voxel in the buffer. Must only be called if @c isValid() , @c voxel_index is in range
  /// and @c T is the contained data type, exactly matching the voxel data size.
  /// @param voxel_index The index of the voxel to read data for. Must be in range.
  /// @param value The voxel content to write at @p voxel_index .
  /// @tparam T The data type to read. Must exactly match the voxel size and content for the referenced voxel layer.
  template <typename T>
  void writeVoxel(unsigned voxel_index, const T &value)
  {
    memcpy(voxelMemory() + sizeof(T) * voxel_index, &value, sizeof(T));
  }

  /// Explicitly release the buffer. Further usage is invalid and @c isValid() will return `false`.
  void release();

protected:
  VoxelPtr voxel_memory_{ nullptr };         ///< Pointer to the uncompressed voxel memory.
  size_t voxel_memory_size_{ 0 };            ///< Number of bytes referenced by the @c voxel_memory_ .
  ohm::VoxelBlock *voxel_block_{ nullptr };  ///< The @c VoxelBlock object owning the voxel memory.
};

extern template class VoxelBuffer<VoxelBlock>;
extern template class VoxelBuffer<const VoxelBlock>;
}  // namespace ohm

#endif  // VOXELBUFFER_H


================================================
FILE: ohm/VoxelData.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMVOXELDATA_H
#define OHMVOXELDATA_H

// A convenience header for including supporting data and functions for all known @c ohm voxel types.

#include "CovarianceVoxel.h"
#include "DefaultLayer.h"
#include "OccupancyType.h"
#include "Voxel.h"
#include "VoxelMean.h"
#include "VoxelOccupancy.h"
#include "VoxelSecondarySample.h"
#include "VoxelTsdf.h"

#endif  // OHMVOXELDATA_H


================================================
FILE: ohm/VoxelIncident.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_VOXEL_INCIDENT_H
#define OHM_VOXEL_INCIDENT_H

#include "OhmConfig.h"

#include <glm/vec3.hpp>

namespace ohm
{
#include "VoxelIncidentCompute.h"
}

#endif  // OHM_VOXEL_INCIDENT_H


================================================
FILE: ohm/VoxelIncidentCompute.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef OHM_VOXEL_INCIDENT_COMPUTE_H
#define OHM_VOXEL_INCIDENT_COMPUTE_H

#if !GPUTIL_DEVICE
using Vec3 = glm::vec3;
#define OHM_NORMAL_STD std::
#define OHM_DEVICE_HOST
#else  // GPUTIL_DEVICE
#if !defined(VEC3)
#define VEC3
typedef float3 Vec3;
#endif  // !defined(VEC3)
#define OHM_NORMAL_STD
#define OHM_DEVICE_HOST __device__ __host__
#endif  // GPUTIL_DEVICE

#define OHM_NORMAL_QUAT       16383.0f
#define OHM_NORMAL_MASK       0x3FFF
#define OHM_NORMAL_SHIFT_X    0
#define OHM_NORMAL_SHIFT_Y    15
#define OHM_NORMAL_SET_BIT  (1u << 30)
#define OHM_NORMAL_SIGN_BIT_Z (1u << 31)

/// Decode the 32-bit quantised incident normal into a floating point vector.
///
/// See @c encodeNormal()
///
/// @param packed_normal The packed and quantised normal value.
inline Vec3 OHM_DEVICE_HOST decodeNormal(unsigned packed_normal)
{
  Vec3 n;

  n.x = (2.0f * (((packed_normal >> OHM_NORMAL_SHIFT_X) & OHM_NORMAL_MASK) / OHM_NORMAL_QUAT)) - 1.0f;
  n.y = (2.0f * (((packed_normal >> OHM_NORMAL_SHIFT_Y) & OHM_NORMAL_MASK) / OHM_NORMAL_QUAT)) - 1.0f;

  n.x = OHM_NORMAL_STD max(-1.0f, OHM_NORMAL_STD min(n.x, 1.0f));
  n.y = OHM_NORMAL_STD max(-1.0f, OHM_NORMAL_STD min(n.y, 1.0f));
  n.z = OHM_NORMAL_STD max(-1.0f, OHM_NORMAL_STD min(1.0f - (n.x * n.x + n.y * n.y), 1.0f));

  n.x = (packed_normal & OHM_NORMAL_SET_BIT) ? n.x : 0.0f;
  n.y = (packed_normal & OHM_NORMAL_SET_BIT) ? n.y : 0.0f;
  n.z = (packed_normal & OHM_NORMAL_SET_BIT) ? sqrt(n.z) : 0.0f;

  n.z *= (packed_normal & OHM_NORMAL_SIGN_BIT_Z) ? -1.0f : 1.0f;

  return n;
}

/// Encode a normalised vector into a 32-bit floating point value.
///
/// We use 15-bits each to encode X and Y channels. We use the most significant bit (31) to encode the sign of Z.
/// Bit 30 is used to incidate if there is a valid normal stored. We recover the Z channel by extracting X and Y, then
/// calculating Z such that the result is a normal vector. This yields the magnitude of Z and the Z-sign bit sets the
/// sign.
///
/// | Bit range | Quantised range | float range | Description |
/// | - | - | - | - |
/// | 0-14 | [-8192, 8191] | [-1, 1] | Quantised X channel |
/// | 15-29 | [-8192, 8191] | [-1, 1] | Quantised Y channel |
/// | 30 | [0, 1] | [0, 1] | Validity bit. Set if we have encoded a normal |
/// | 31 | [0, 1] | [0, 1] | Z sign bit |
inline unsigned OHM_DEVICE_HOST encodeNormal(Vec3 normal)
{
  unsigned n = 0;

  // Adjust normal range from [-1, 1] -> [0, 2] -> [0, 1]
  normal.x = 0.5f * (OHM_NORMAL_STD max(-1.0f, OHM_NORMAL_STD min(normal.x, 1.0f)) + 1.0f);
  normal.y = 0.5f * (OHM_NORMAL_STD max(-1.0f, OHM_NORMAL_STD min(normal.y, 1.0f)) + 1.0f);

  unsigned i = (unsigned)(normal.x * OHM_NORMAL_QUAT);
  n |= (i & OHM_NORMAL_MASK) << OHM_NORMAL_SHIFT_X;
  i = (unsigned)(normal.y * OHM_NORMAL_QUAT);
  n |= (i & OHM_NORMAL_MASK) << OHM_NORMAL_SHIFT_Y;

  // Clear the information bits.
  n &= ~(OHM_NORMAL_SET_BIT | OHM_NORMAL_SIGN_BIT_Z);
  // Set the information bits.
  n |= (normal.z < 0) ? OHM_NORMAL_SIGN_BIT_Z : 0;
  // mark as set if the input normal is non-zero.
  n |= (normal.x || normal.y || normal.z) ? OHM_NORMAL_SET_BIT : 0;

  return n;
}

inline Vec3 OHM_DEVICE_HOST updateIncidentNormalV3(Vec3 normal, Vec3 incident_ray, unsigned point_count)
{
  // Handle having a zero normal as an initialiastion pass regardless of point count.
  point_count = ((normal.x != 0 || normal.y != 0 || normal.z != 0) && point_count) ? point_count : 0;
  const float one_on_count_plus_one = 1.0f / (float)(point_count + 1);
  float normal_length_2 =
    incident_ray.x * incident_ray.x + incident_ray.y * incident_ray.y + incident_ray.z * incident_ray.z;
  incident_ray *= (normal_length_2 > 1e-6f) ? 1.0f / sqrt(normal_length_2) : 0.0f;
  normal.x += (incident_ray.x - normal.x) * one_on_count_plus_one;
  normal.y += (incident_ray.y - normal.y) * one_on_count_plus_one;
  normal.z += (incident_ray.z - normal.z) * one_on_count_plus_one;
  normal_length_2 = normal.x * normal.x + normal.y * normal.y + normal.z * normal.z;
  normal *= (normal_length_2 > 1e-6f) ? 1.0f / sqrt(normal_length_2) : 0.0f;
  return normal;
}

inline unsigned OHM_DEVICE_HOST updateIncidentNormal(unsigned packed_normal, Vec3 incident_ray, unsigned point_count)
{
  Vec3 normal = decodeNormal(packed_normal);
  normal = updateIncidentNormalV3(normal, incident_ray, point_count);
  return encodeNormal(normal);
}

#undef OHM_DEVICE_HOST

#endif  // OHM_VOXEL_INCIDENT_COMPUTE_H


================================================
FILE: ohm/VoxelLayout.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "VoxelLayout.h"

#include "private/VoxelLayoutDetail.h"

#include <cinttypes>
#include <cstring>
#include <string>

namespace ohm
{
template <typename T>
const char *VoxelLayoutT<T>::memberName(size_t member_index) const
{
  if (member_index >= detail_->members.size())
  {
    return nullptr;
  }

  return detail_->members[member_index].name.data();
}


template <typename T>
size_t VoxelLayoutT<T>::memberOffset(size_t member_index) const
{
  if (member_index >= detail_->members.size())
  {
    return 0;
  }

  return detail_->members[member_index].offset;
}


template <typename T>
int VoxelLayoutT<T>::indexOf(const char *name) const
{
  const std::string name_str(name);
  for (size_t i = 0; i < memberCount(); ++i)
  {
    if (name_str == memberName(i))
    {
      return int(i);
    }
  }

  return -1;
}


template <typename T>
DataType::Type VoxelLayoutT<T>::memberType(size_t member_index) const
{
  if (member_index >= detail_->members.size())
  {
    return DataType::kUnknown;
  }

  return DataType::Type(detail_->members[member_index].type);
}


template <typename T>
size_t VoxelLayoutT<T>::memberSize(size_t member_index) const
{
  if (member_index >= detail_->members.size())
  {
    return 0;
  }

  return DataType::size(detail_->members[member_index].type);
}


template <typename T>
uint64_t VoxelLayoutT<T>::memberClearValue(size_t member_index) const
{
  return detail_->members[member_index].clear_value;
}


template <typename T>
size_t VoxelLayoutT<T>::voxelByteSize() const
{
  return detail_->voxel_byte_size;
}


template <typename T>
size_t VoxelLayoutT<T>::memberCount() const
{
  return detail_->members.size();
}


template <typename T>
const void *VoxelLayoutT<T>::memberPtr(size_t member_index, const void *mem) const
{
  if (member_index >= detail_->members.size())
  {
    return nullptr;
  }

  const auto *bytes = reinterpret_cast<const uint8_t *>(mem);
  return &bytes[memberOffset(member_index)];
}


template <typename T>
void *VoxelLayoutT<T>::memberPtr(size_t member_index, void *mem) const
{
  if (member_index >= detail_->members.size())
  {
    return nullptr;
  }

  auto *bytes = reinterpret_cast<uint8_t *>(mem);
  return &bytes[memberOffset(member_index)];
}

namespace
{
/// Determine the byte alignment for a type of size @p data_size to one of {1, 2, 4, 8}.
/// @param data_size The byte size of the data size of interest.
/// @return The best alignment value >= @c base_alignment from {1, 2, 4} or 8 when @p data_size >= 88.
uint16_t getAlignmentForSize(uint16_t data_size)
{
  // Support 1, 2, 4, 8 byte alignments.
  switch (data_size)
  {
  case 1:
    return 1;
  case 2:
    return 2;
  case 3:
  case 4:
    return 4;
  default:
    break;
  }

  return 8;  // NOLINT(readability-magic-numbers)
}

/// A helper function for updating the @c VoxelLayoutDetail and @c VoxelMember offset, alignment and size values.
///
/// This function sets the @c VoxelMember::offset and updates @c VoxelLayoutDetail::next_offset and
/// @c VoxelLayoutDetail::voxel_byte_size based on the @pcVoxelLayoutDetail::next_offset and@c VoxelMember::type size.
///
/// The @c VoxelMember::offset is set to @p VoxelLayoutDetail::next_offset, the rounded up to the nearest suitable
/// alignment for the data size (base don @c getAlignmentForSize()). This yields an alignment of one of {1, 2, 4, 8}.
/// The @c VoxelLayoutDetail::voxel_byte_size is the sum of the member offset and it's data size aligned to the
/// nearest 8 byte boundary with the exception of sizes [1, 4], for which the data size is 4 bytes.
///
/// @param[in,out] detail The @c VoxelLayoutDetail to update the @c next_offset and @c voxel_byte_size for.
/// @param[in,out] member The member to set the @c offset for.
void updateOffsets(VoxelLayoutDetail *detail, VoxelMember *member)
{
  auto member_size = uint16_t(DataType::size(member->type));

  // Resolve packing. Member size dictates the required alignment at the next increment of 1, 2, 4 or 8 bytes.
  uint16_t alignment = getAlignmentForSize(member_size);
  if (alignment == 0)
  {
    alignment = 4;
  }
  // Set the initial member offset to the next target offset.
  member->offset = detail->next_offset;

  // Is the proposed offset correctly aligned?
  if (member->offset % alignment != 0)
  {
    // Not aligned. Pad to the required alignment.
    member->offset += alignment - member->offset % alignment;
  }

  // Adjust the next offset to the member offset + the size.
  detail->next_offset = uint16_t(member->offset + member_size);

  // Now we calculate the aligned voxel size to be aligned to 4 or 8 bytes.
  detail->voxel_byte_size = detail->next_offset;
  const unsigned word_alignment = 8;
  const unsigned half_word_alignment = 4;
  if (detail->voxel_byte_size <= half_word_alignment)
  {
    detail->voxel_byte_size = half_word_alignment;
  }
  else
  {
    detail->voxel_byte_size = word_alignment * ((detail->next_offset + word_alignment - 1) / word_alignment);
  }
}
}  // namespace

VoxelLayout::VoxelLayout()
  : VoxelLayoutT<VoxelLayoutDetail>(nullptr)
{}


VoxelLayout::VoxelLayout(VoxelLayoutDetail *detail)
  : VoxelLayoutT<VoxelLayoutDetail>(detail)
{}


VoxelLayout::VoxelLayout(const VoxelLayout &other) = default;


void VoxelLayout::addMember(const char *name, DataType::Type type, uint64_t clear_value)
{
  VoxelMember member{};
  strncpy(member.name.data(), name, sizeof(member.name));
  member.name[sizeof(member.name) - 1] = '\0';
  member.clear_value = clear_value;
  member.type = type;
  member.offset = 0;

  updateOffsets(detail_, &member);
  detail_->members.push_back(member);
}


bool VoxelLayout::removeMember(const char *name)
{
  std::string name_str(name);
  bool removed = false;

  // Recalculate next offset and voxel size as we go.
  detail_->next_offset = detail_->voxel_byte_size = 0;
  for (auto iter = detail_->members.begin(); iter != detail_->members.end();)
  {
    VoxelMember &member = *iter;
    if (name_str == member.name.data())
    {
      iter = detail_->members.erase(iter);
      removed = true;
    }
    else
    {
      // Update offsets.
      member.offset = detail_->next_offset;
      updateOffsets(detail_, &member);
      ++iter;
    }
  }

  return removed;
}


VoxelLayoutConst::VoxelLayoutConst()
  : VoxelLayoutT<const VoxelLayoutDetail>(nullptr)
{}


VoxelLayoutConst::VoxelLayoutConst(const VoxelLayoutDetail *detail)
  : VoxelLayoutT<const VoxelLayoutDetail>(detail)
{}


VoxelLayoutConst::VoxelLayoutConst(const VoxelLayoutConst &other) = default;


VoxelLayoutConst::VoxelLayoutConst(const VoxelLayout &other)
  : VoxelLayoutT<const VoxelLayoutDetail>(other.detail())
{}


// Export explicit template instantiations
template class ohm_API VoxelLayoutT<VoxelLayoutDetail>;
template class ohm_API VoxelLayoutT<const VoxelLayoutDetail>;
}  // namespace ohm


================================================
FILE: ohm/VoxelLayout.h
================================================
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// Copyright (c) 2018
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMVOXELLAYOUT_H
#define OHMVOXELLAYOUT_H

#include "OhmConfig.h"

#include "DataType.h"

#include <cinttypes>

#ifdef _MSC_VER
// After some deliberation I've disabled warning 4661. This warning arises because a template function does not have
// a definition available. Howver, this is a red herring in this case. We explicitly instantiate both VoxelLayoutT
// signatures we need - VoxelLayoutDetail and const VoxelLayoutDetail - and export from the DLL. I can't work out what
// the "correct" way to resolve this warning in this case is, but it is erroneous in this case, as far as I can tell.
#pragma warning(push)
#pragma warning(disable : 4661)
#endif  // _MSC_VER

namespace ohm
{
struct VoxelLayoutDetail;

/// Common implementation for accessing the voxel structure within a @c MapLayer.
///
/// This class is setup to define the same implementation for a writable @c VoxelLayout and a read only
/// @c VoxelLayoutConst.
///
/// See @c MapLayout for more details.
///
/// @tparam T Either <tt>VoxelLayoutDetail</tt> or <tt>const VoxelLayoutDetail</tt> from the private API.
template <typename T>
class VoxelLayoutT
{
public:
  // Default constructor.
  VoxelLayoutT() = default;

  /// Create a new layout structure around the given data.
  /// @param detail The @c VoxelLayoutDetail.
  explicit VoxelLayoutT(T *detail);

  /// Copy constructor supporting casting from non-const to const @c VoxelLayoutDetail.
  /// @param detail The @c VoxelLayoutDetail.
  template <typename U>
  inline explicit VoxelLayoutT(U *detail = nullptr)
    : detail_(detail)
  {}

  /// Copy constructor.
  /// @param other Object to shallow copy.
  VoxelLayoutT(const VoxelLayoutT<T> &other);
  /// Move constructor.
  /// @param other Object to move.
  VoxelLayoutT(VoxelLayoutT<T> &&other) noexcept;

  /// Looking the index of a member by name.
  /// @param name The name to lookup.
  /// @return The index of the member matching @p name, or -1 on failure.
  int indexOf(const char *name) const;

  /// Query the name of the member at @p memberIndex.
  /// @param member_index The index of the member to query.
  /// @return The member name or null if @p memberIndex is out of range.
  const char *memberName(size_t member_index) const;

  /// Query the byte offset to the member at @p memberIndex.
  /// @param member_index The index of the member to query.
  /// @return The byte offset to @p memberIndex or zero if out of range.
  size_t memberOffset(size_t member_index) const;

  /// Query the data type of the member at @p memberIndex.
  /// @param member_index The index of the member to query.
  /// @return The @c DataType of @p memberIndex or @c DataType::kUnknown if out of range.
  DataType::Type memberType(size_t member_index) const;

  /// Query the byte size to the member at @p memberIndex.
  /// @param member_index The index of the member to query.
  /// @return The byte size to @p memberIndex or zero if out of range.
  size_t memberSize(size_t member_index) const;

  /// Query the desired clear value for the member at member at @p memberIndex.
  /// @param member_index The index of the member to query. Must be in range [0, @c memberCount()).
  /// @return The clear value for @p memberIndex.
  uint64_t memberClearValue(size_t member_index) const;

  /// Get a pointer to the given @p memberIndex given a base voxel pointer at @p mem.
  /// @param member_index The index of the member to query. Must be in range [0, @c memberCount()).
  /// @param mem The address of the voxel data.
  /// @return A pointer to <tt>mem + @c memberOffset(memberIndex)</tt>.
  const void *memberPtr(size_t member_index, const void *mem) const;
  /// @overload
  void *memberPtr(size_t member_index, void *mem) const;

  /// Query the size of the voxel structure defined by this object.
  /// @return The byte size of the defined voxel structure.
  size_t voxelByteSize() const;

  /// Query the number of registered data members.
  /// @return The number of members.
  size_t memberCount() const;

  /// Internal data access.
  /// @return Internal details.
  inline T *detail() const { return detail_; }

protected:
  T *detail_ = nullptr;  ///< Internal implementation detail.
};

/// Template instantiation.
extern template class VoxelLayoutT<VoxelLayoutDetail>;
/// Template instantiation.
extern template class VoxelLayoutT<const VoxelLayoutDetail>;

/// A writable voxel layout for use in @p MapLayout.
class ohm_API VoxelLayout : public VoxelLayoutT<VoxelLayoutDetail>
{
public:
  /// Constructor: invalid object.
  VoxelLayout();

  /// Constructor wrapping @p detail.
  /// @param detail Underlying voxel data structure.
  explicit VoxelLayout(VoxelLayoutDetail *detail);

  /// Copy constructor (shallow).
  /// @param other Object to shallow copy.
  VoxelLayout(const VoxelLayout &other);

  /// Add a member to the structure.
  ///
  /// The name is added at a byte offset of @c voxelByteSize(), with some data alignment padding.
  /// Padding is added to ensure a particular alignment depending on the data size of @p type as show below.
  ///
  /// Type Size (bytes) | Supported Minimum Alignment (bytes)
  /// ----------------- | -----------------------------------
  /// 1                 | 1
  /// 2                 | 2
  /// 3                 | 4
  /// 4                 | 4
  /// 5+                | 8
  ///
  /// That is, padding is added to ensure the @c memberOffset() of the added member matches that indicated above.
  ///
  /// @param name The data member. Should be unique, but not checked.
  /// @param type The member data type.
  /// @param clear_value The value used with memset to initialise the data member.
  void addMember(const char *name, DataType::Type type, uint64_t clear_value);

  /// Remove a member from the layout by name.
  ///
  /// Warning: this should be used with great care and may yield unintended side effects and undefined behaviour.
  /// This method is best left to internal use only.
  ///
  /// Offsets of other members are updated as required.
  ///
  /// @param name The name of the member to remove.
  /// @return True if a member matching @p name was found and remove.
  bool removeMember(const char *name);

  /// Assignment operator.
  /// @param other Object to shallow copy.
  inline VoxelLayout &operator=(const VoxelLayout &other)
  {
    detail_ = other.detail();
    return *this;
  }
};

/// A readonly voxel layout for use in @p MapLayout.
class ohm_API VoxelLayoutConst : public VoxelLayoutT<const VoxelLayoutDetail>
{
public:
  /// Constructor: invalid object.
  VoxelLayoutConst();

  /// Constructor wrapping @p detail.
  /// @param detail Underlying voxel data structure.
  explicit VoxelLayoutConst(const VoxelLayoutDetail *detail);

  /// Copy constructor (shallow).
  /// @param other Object to shallow copy.
  VoxelLayoutConst(const VoxelLayoutConst &other);

  /// Copy constructor from writable object (shallow).
  /// @param other Object to shallow copy.
  explicit VoxelLayoutConst(const VoxelLayout &other);

  /// Assignment operator.
  /// @param other Object to shallow copy.
  inline VoxelLayoutConst &operator=(const VoxelLayoutConst &other)
  {
    detail_ = other.detail();
    return *this;
  }

  /// Assignment operator from writable object.
  /// @param other Object to shallow copy.
  inline VoxelLayoutConst &operator=(const VoxelLayout &other)
  {
    detail_ = other.detail();
    return *this;
  }
};


template <typename T>
inline VoxelLayoutT<T>::VoxelLayoutT(T *detail)
  : detail_(detail)
{}


template <typename T>
inline VoxelLayoutT<T>::VoxelLayoutT(const VoxelLayoutT &other)
  : detail_(other.detail_)
{}


template <typename T>
inline VoxelLayoutT<T>::VoxelLayoutT(VoxelLayoutT &&other) noexcept
  : detail_(other.detail_)
{
  other.detail_ = nullptr;
}
}  // namespace ohm

#ifdef _MSC_VER
#pragma warning(pop)
#endif  // _MSC_VER

#endif  // OHMVOXELLAYOUT_H


================================================
FILE: ohm/VoxelMean.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef VOXELMEAN_H
#define VOXELMEAN_H

#include "OhmConfig.h"

#include "MapCoord.h"
#include "Voxel.h"
#include "VoxelOccupancy.h"

#include <glm/vec3.hpp>

/// @defgroup voxelmean Voxel Mean Position
/// These functions are used to manipulate the voxel mean positioning voxel fields. @c VoxelMean allows a voxel to store
/// a position field offset from the centre of the voxel, thereby refining the precision of the voxel position.
///
/// To support voxel mean positioning, an @c OccupancyMap must first enable this feature by the constructor flag, or
/// by calling @c OccupancyMap::setVoxelMeanEnabled(true). This ensures a voxel layer is added to the map containing
/// to hold the @c VoxelMean data. This structure contains a quantised voxel mean coordinate pattern and a point count.
/// The voxel mean coordinate pattern is defined as follows:
/// - bits [0, 9]: voxel mean x coord
/// - bits [10, 19]: voxel mean y coord
/// - bits [20, 29]: voxel mean z coord
/// - bit 30: unused
/// - bit 31: marks voxel mean data as present
///
/// XYZ coordinates are assigned 10 bits each to store a quantised voxel mean coordinate. This coordinate is quantised
/// accross the range of 10-bits allowing for 1024 possible positions along each axis. If no voxel mean pattern is
/// present, then the voxel centre is given as the voxel position.
///
/// When samples are integrated into a voxel, the voxel mean position is calculated via a progressive average
/// combining the existing coordinate and the new sample value.
/// This is given as:
/// @code{.unparsed}
///   new_mean_coord = current_mean_coord + (sample - current_mean_coord) / (point_count + 1)
/// @endcode

namespace ohm
{
#include "VoxelMeanCompute.h"

template <typename V>
inline glm::dvec3 positionUnsafeT(const Voxel<V> &voxel)
{
  VoxelMean mean_info;
  voxel.read(&mean_info);
  glm::dvec3 mean = voxel.map()->voxelCentreGlobal(voxel.key());
  mean += subVoxelToLocalCoord<glm::dvec3>(mean_info.coord, voxel.map()->resolution());
  return mean;
}

/// @ingroup voxelmean
/// Calculate the mean position within the given @p voxel .
///
/// <tt>Voxel<VoxelMean>::isValid()</tt> must be true before calling.
/// @param voxel The voxel to query the mean coordinate for.
inline glm::dvec3 positionUnsafe(const Voxel<VoxelMean> &voxel)
{
  return positionUnsafeT(voxel);
}

/// @ingroup voxelmean
/// Calculate the mean position within the given @p voxel .
///
/// <tt>Voxel<const VoxelMean>::isValid()</tt> must be true before calling.
/// @param voxel The voxel to query the mean coordinate for.
inline glm::dvec3 positionUnsafe(const Voxel<const VoxelMean> &voxel)
{
  return positionUnsafeT(voxel);
}

template <typename V>
inline glm::dvec3 positionSafeT(const Voxel<V> &voxel)
{
  if (voxel.isValid())
  {
    return positionUnsafeT(voxel);
  }

  if (!voxel.key().isNull() && voxel.map())
  {
    return voxel.map()->voxelCentreGlobal(voxel.key());
  }
  return glm::dvec3{ 0 };
}

inline glm::dvec3 position(const VoxelMean &mean_info, const glm::dvec3 &voxel_centre, double voxel_resolution)
{
  const glm::dvec3 mean = voxel_centre + subVoxelToLocalCoord<glm::dvec3>(mean_info.coord, voxel_resolution);
  return mean;
}

/// Query the position of @c voxel if the @p voxel might be invalid or the voxel layer might be invalid.
///
/// In order of availability, the return value will be:
/// - The @c VoxelMean position - @p voxel must be fully valid.
/// - The voxel centre of the @p voxel.key() - @p voxel map and key must be valid.
/// - `(0, 0, 0)`
/// @param voxel The voxel to query.
/// @return The best available coordinate for the voxel (see remarks).
inline glm::dvec3 positionSafe(const Voxel<VoxelMean> &voxel)
{
  return positionSafeT(voxel);
}
/// @overload
inline glm::dvec3 positionSafe(const Voxel<const VoxelMean> &voxel)
{
  return positionSafeT(voxel);
}

/// @ingroup voxelmean
/// Explicitly set the mean position for @p voxel .
///
/// <tt>Voxel<const VoxelMean>::isValid()</tt> must be true before calling.
///
/// @param voxel The voxel to modify.
/// @param pos The new mean position for @p voxel . Must be within the bounds of @p voxel .
/// @param count Optional argument to explicitly set the number of samples gather to attain the position.
///   This affects subsequent mean updates, with larger values reducing the influence of new positions.
///   Use 0 to leave the current count value as is.
inline void setPositionUnsafe(Voxel<VoxelMean> &voxel, const glm::dvec3 &pos, unsigned count = 0)
{
  VoxelMean mean_info;
  voxel.read(&mean_info);
  mean_info.coord = subVoxelCoord(pos - voxel.map()->voxelCentreGlobal(voxel.key()), voxel.map()->resolution());
  mean_info.count = (count) ? count : mean_info.count;
  voxel.write(mean_info);
}

/// @ingroup voxelmean
/// A validated version of @c setPositionUnsafe() ensuring @c voxel is valid before attepting to write.
/// @param voxel The voxel to modify. May be in invalid.
/// @param pos The new mean position for @p voxel . Must be within the bounds of @p voxel .
/// @param count Optional argument to explicitly set the number of samples gathered to attain the position.
///   This affects subsequent mean updates, with larger values reducing the influence of new positions.
///   Use 0 to leave the current count value as is.
inline void setPositionSafe(Voxel<VoxelMean> &voxel, const glm::dvec3 &pos, unsigned count = 0)
{
  if (voxel.isValid())
  {
    setPositionUnsafe(voxel, pos, count);
  }
}

/// @ingroup voxelmean
/// Update the mean position for a voxel, adjusting the mean with the new coordinate @p pos .
///
/// <tt>Voxel<const VoxelMean>::isValid()</tt> must be true before calling.
///
/// @param mean_info Details of the voxel mean.
/// @param pos The new coordinate to incorporate into the mean.
/// @param voxel_centre Centre of the referenced voxel.
/// @param voxel_resolution Voxel resolution (size along each edge).
inline void updatePosition(VoxelMean *mean_info, const glm::dvec3 &pos, const glm::dvec3 &voxel_centre,
                           double voxel_resolution)
{
  mean_info->coord = subVoxelUpdate(mean_info->coord, mean_info->count, pos - voxel_centre, voxel_resolution);
  mean_info->count = std::min(mean_info->count, std::numeric_limits<unsigned>::max() - 1u) + 1;
}

/// @ingroup voxelmean
/// Update the mean position for a voxel, adjusting the mean with the new coordinate @p pos .
///
/// <tt>Voxel<const VoxelMean>::isValid()</tt> must be true before calling.
///
/// @param voxel The voxel to modify.
/// @param pos The new coordinate to incorporate into the mean.
inline void updatePositionUnsafe(Voxel<VoxelMean> &voxel, const glm::dvec3 &pos)
{
  VoxelMean mean_info;
  voxel.read(&mean_info);
  updatePosition(&mean_info, pos, voxel.map()->voxelCentreGlobal(voxel.key()), voxel.map()->resolution());
  voxel.write(mean_info);
}

/// @ingroup voxelmean
/// A validated version of @c updatePositionUnsafe() ensuring @c voxel is valid before attepting to write.
/// @param voxel The voxel to modify. May be invalid.
/// @param pos The new coordinate to incorporate into the mean.
inline void updatePositionSafe(Voxel<VoxelMean> &voxel, const glm::dvec3 &pos)
{
  if (voxel.isValid())
  {
    updatePositionUnsafe(voxel, pos);
  }
}

/// A convenience function for integrating a single hit into @p map with a voxel mean update.
/// Note: this is a sub-optimal way of updating the @p map . Use a @c RayMapper or the @c Voxel API for batch updates.
/// @param map The map to update. Must have an occupancy layer, but the mean layer is optional.
/// @param sample The sample where the hit occurs.
inline void integrateHit(ohm::OccupancyMap &map, const glm::dvec3 &sample)
{
  const Key key(map.voxelKey(sample));
  Voxel<float> occupancy(&map, map.layout().occupancyLayer(), key);
  integrateHit(occupancy);
  Voxel<VoxelMean> mean(&map, map.layout().meanLayer(), key);
  updatePositionSafe(mean, sample);
}
}  // namespace ohm

#endif  // VOXELMEAN_H


================================================
FILE: ohm/VoxelMeanCompute.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef VOXELMEANCOMPUTE_H
#define VOXELMEANCOMPUTE_H

// Note: Must include MapCoord.h before this header.

// Note: this header is included in GPU code.
// Because of this "OhmConfig.h" and <cmath> cannot be included here and you may need to include those first.

#if !GPUTIL_DEVICE
#ifndef __device__
#define __device__
#endif  // __device__
#ifndef __host__
#define __host__
#endif  // __host__

#define SUB_VOX_FUNC_PREFACE template <typename Vec3, typename coord_real>
using Vec3 = glm::vec3;

/// @ingroup voxelmean
/// The data structure used to hold the voxel mean coordinate and sample count. See @ref voxelmean for details on usage
/// and the mean quantisation in @c coord .
/// @todo Rename this and associated code as VoxelMean
struct VoxelMean
{
  uint32_t coord;  ///< Quantised voxel mean coordinate.
  uint32_t count;  ///< Number of samples used to generate the current @c coord value.
};

#else  // GPUTIL_DEVICE

#if !defined(COORD_REAL)
#define COORD_REAL
typedef float coord_real;
#endif  // !defined(COORD_REAL)

#if !defined(VEC3)
#define VEC3
typedef float3 Vec3;
#endif  // !defined(VEC3)

#define SUB_VOX_FUNC_PREFACE


// Voxel mean structure used on GPU. Must match the CPU structure, but the types are modified to support atomic
// operations.
typedef struct VoxelMean_t
{
  atomic_uint coord;
  atomic_uint count;
} VoxelMean;

#endif  // GPUTIL_DEVICE


/// @ingroup voxelmean
/// Convert @p voxel_local_coord into a voxel mean positioning pattern.
/// @param voxel_local_coord The new coordinate to store. This position should be within the voxel bounds relative
///   to the centre of the voxel; i.e., [-0.5 * resolution, 0.5 * resolution]. The position is clamped to the voxel
///   bounds.
/// @param resolution The length of each voxel cube edge.
/// @return The voxel mean pattern approximating @p voxel_local_coord.
SUB_VOX_FUNC_PREFACE
inline __device__ __host__ unsigned subVoxelCoord(Vec3 voxel_local_coord, coord_real resolution)
{
  // We divide the voxel into a voxel_local_coord, 3D grid, then assign 1 bit per cell.
  const unsigned bits_per_axis = 10;
  const int mean_positions = (1 << bits_per_axis) - 1;  // NOLINT(hicpp-signed-bitwise)
  const unsigned used_bit = (1u << 31u);
  const coord_real mean_resolution = resolution / (coord_real)mean_positions;  // NOLINT
  const coord_real offset = (coord_real)0.5f * resolution;                      // NOLINT

  int pos_x = pointToRegionCoord(voxel_local_coord.x + offset, mean_resolution);
  int pos_y = pointToRegionCoord(voxel_local_coord.y + offset, mean_resolution);
  int pos_z = pointToRegionCoord(voxel_local_coord.z + offset, mean_resolution);

  pos_x = (pos_x >= 0 ? (pos_x < (1 << bits_per_axis) ? pos_x : mean_positions) : 0);  // NOLINT(hicpp-signed-bitwise)
  pos_y = (pos_y >= 0 ? (pos_y < (1 << bits_per_axis) ? pos_y : mean_positions) : 0);  // NOLINT(hicpp-signed-bitwise)
  pos_z = (pos_z >= 0 ? (pos_z < (1 << bits_per_axis) ? pos_z : mean_positions) : 0);  // NOLINT(hicpp-signed-bitwise)

  unsigned pattern = 0;
  pattern |= (unsigned)pos_x;                           // NOLINT
  pattern |= ((unsigned)pos_y << bits_per_axis);        // NOLINT
  pattern |= ((unsigned)pos_z << (2 * bits_per_axis));  // NOLINT
  pattern |= used_bit;
  return pattern;
}


/// @ingroup voxelmean
/// Convert a voxel mean pattern into a coordinate, relative to the voxel centre.
/// @param pattern The voxel mean pattern.
/// @param resolution The length of each voxel cube edge.
/// @return The unpacked coordinate, relative to the voxel centre, in the range [-0.5 * resolution, 0.5 resolution].
///   The result is (0, 0, 0) if @p pattern has yet to be used (bit-31 not set).
SUB_VOX_FUNC_PREFACE
inline __device__ __host__ Vec3 subVoxelToLocalCoord(unsigned pattern, coord_real resolution)
{
  const unsigned bits_per_axis = 10;
  const int mean_positions = (1 << bits_per_axis) - 1;  // NOLINT(hicpp-signed-bitwise)
  const unsigned used_bit = (1u << 31u);
  const coord_real mean_resolution = resolution / (coord_real)mean_positions;  // NOLINT
  const coord_real offset = (coord_real)0.5f * resolution;                      // NOLINT

  Vec3 coord;  // NOLINT
  // NOLINTNEXTLINE
  coord.x = (used_bit) ? regionCentreCoord((int)(pattern & mean_positions), mean_resolution) - offset : 0;
  coord.y = (used_bit) ?
              // NOLINTNEXTLINE
              regionCentreCoord((int)((pattern >> bits_per_axis) & mean_positions), mean_resolution) - offset :
              0;
  coord.z = (used_bit) ?
              // NOLINTNEXTLINE
              regionCentreCoord((int)((pattern >> (2 * bits_per_axis)) & mean_positions), mean_resolution) - offset :
              0;
  return coord;
}


/// @ingroup voxelmean
/// Update the @c VoxelMean for a voxel adding @c voxel_local_coord to the coordinate. The @c voxel_local_coord
/// coordiate is sample coordinate to add relative to the voxel centre.
///
/// @param coord The subvoxel coordinate to update
/// @param point_count The number of samples currently contributing to @p coord .
/// @param voxel_local_coord The coordinate to add to the mean local to the voxel centre.
/// @param resolution The voxel resolution (size long edges).
SUB_VOX_FUNC_PREFACE
inline __device__ __host__ unsigned subVoxelUpdate(unsigned coord, unsigned point_count, Vec3 voxel_local_coord,
                                                   coord_real resolution)
{
  Vec3 mean =
#if !GPUTIL_DEVICE
    subVoxelToLocalCoord<Vec3>(coord, resolution)
#else   //  GPUTIL_DEVICE
    subVoxelToLocalCoord(coord, resolution)
#endif  //  GPUTIL_DEVICE
    ;

  // Lint(KS): have to use C style casting as this code is used on OpenCL as well.
  // NOLINTNEXTLINE(google-readability-casting)
  const coord_real one_on_count_plus_one = (coord_real)1 / (coord_real)(point_count + 1);
  mean.x += (voxel_local_coord.x - mean.x) * one_on_count_plus_one;
  mean.y += (voxel_local_coord.y - mean.y) * one_on_count_plus_one;
  mean.z += (voxel_local_coord.z - mean.z) * one_on_count_plus_one;
  return subVoxelCoord(mean, resolution);
}

#endif  // VOXELMEANCOMPUTE_H


================================================
FILE: ohm/VoxelOccupancy.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef VOXELOCCUPANCY_H
#define VOXELOCCUPANCY_H

#include "OhmConfig.h"

#include "OccupancyMap.h"
#include "OccupancyType.h"
#include "Voxel.h"

#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX
#include <cmath>


/// @defgroup voxeloccupancy Voxel Occupancy Functions
/// These functions are used to update the voxel occupancy value in a consistent way. This deals with various update
/// conditions including:
/// - uninitialised voxel
/// - min/max value caps
/// - saturation
/// - null update (noop)

namespace ohm
{
/// Define fmin for use in `VoxelOccpuancyCompute.h` functions. @c fmin() is overloaded in GPU for float and double,
/// but the global C symbol is double only. We alias @c std::fmin() , which is overloaded, so we can call just @c fmin()
/// in both CPU and GPU code.
using std::fmax;
using std::fmin;
/// Define fmax for use in `VoxelOccpuancyCompute.h` as per @c fmin()
#include "VoxelOccupancyCompute.h"

/// @ingroup voxeloccupancy
/// Value used to identify invalid or uninitialised voxels.
/// @return A numeric value considered invalid for a voxel value.
constexpr float unobservedOccupancyValue()
{
  return std::numeric_limits<float>::infinity();
}


/// @ingroup voxeloccupancy
/// Integrate a hit into the referenced @p voxel .
/// This adjust the occupancy data - @c Voxel<float>::data() - increasing it by the @c OccupancyMap::hitValue() .
///
/// Note @c voxel is self contained so long as it is valid.
///
/// @c Voxel<float>::isValid() must be true before calling.
///
/// @param voxel The voxel to update.
inline void integrateHit(Voxel<float> &voxel)
{
  float occupancy;
  voxel.read(&occupancy);
  const float initial_value = occupancy;
  const OccupancyMap &map = *voxel.map();
  occupancyAdjustHit(&occupancy, initial_value, map.hitValue(), unobservedOccupancyValue(), map.maxVoxelValue(),
                     map.saturateAtMinValue() ? map.minVoxelValue() : std::numeric_limits<float>::lowest(),
                     map.saturateAtMaxValue() ? map.maxVoxelValue() : std::numeric_limits<float>::max(), false);
  voxel.write(occupancy);
}

/// A convenience function for integrating a single hit into @p map . Note this is a sub-optimal way of updating
/// the @p map . Use a @c RayMapper or the @c Voxel API for batch updates.
/// @param map The map to update.
/// @param key The key for the voxel to create/update.
inline void integrateHit(ohm::OccupancyMap &map, const ohm::Key &key)
{
  Voxel<float> voxel(&map, map.layout().occupancyLayer(), key);
  integrateHit(voxel);
}

/// @ingroup voxeloccupancy
/// Integrate a miss into the referenced @p voxel .
/// This adjusts the occupancy data - @c Voxel<float>::data() - decreasing it by adding @c OccupancyMap::missValue() .
///
/// Note @c voxel is self contained so long as it is valid.
///
/// @c Voxel<float>::isValid() must be true before calling.
///
/// @param voxel The voxel to update.
inline void integrateMiss(Voxel<float> &voxel)
{
  float occupancy;
  voxel.read(&occupancy);
  const float initial_value = occupancy;
  const OccupancyMap &map = *voxel.map();
  occupancyAdjustMiss(&occupancy, initial_value, map.missValue(), unobservedOccupancyValue(), map.minVoxelValue(),
                      map.saturateAtMinValue() ? map.minVoxelValue() : std::numeric_limits<float>::lowest(),
                      map.saturateAtMaxValue() ? map.maxVoxelValue() : std::numeric_limits<float>::max(), false);
  voxel.write(occupancy);
}

/// A convenience function for integrating a single miss into @p map . Note this is a sub-optimal way of updating
/// the @p map . Use a @c RayMapper or the @c Voxel API for batch updates.
/// @param map The map to update.
/// @param key The key for the voxel to create/update.
inline void integrateMiss(ohm::OccupancyMap &map, const Key &key)
{
  Voxel<float> voxel(&map, map.layout().occupancyLayer(), key);
  integrateMiss(voxel);
}

/// @ingroup voxeloccupancy
/// Determine the @c OccupancyType for an occupancy @p value
///
/// @param value The value to categorise.
/// @param map The map within which the value sits.
/// @return The @c OccupancyType for the @p value .
inline OccupancyType occupancyType(float value, const OccupancyMap &map)
{
  if (value < unobservedOccupancyValue())
  {
    if (value < map.occupancyThresholdValue())
    {
      return kFree;
    }

    return kOccupied;
  }

  return kUnobserved;
}

template <typename T>
inline OccupancyType occupancyTypeT(const Voxel<T> &voxel)
{
  if (voxel.isValid())
  {
    float occupancy;
    voxel.read(&occupancy);
    return occupancyType(occupancy, *voxel.map());
  }
  return kNull;
}

/// Query the @c OccupancyType for @p voxel , which may be null/invalid.
/// @param voxel The voxel of interest by occupancy layer.
/// @return The occupancy type for the voxel given it's value. May be @c kNull when @p voxel is null.
inline OccupancyType occupancyType(const Voxel<float> &voxel)
{
  return occupancyTypeT(voxel);
}
/// @overload
inline OccupancyType occupancyType(const Voxel<const float> &voxel)
{
  return occupancyTypeT(voxel);
}

/// @ingroup voxeloccupancy
/// Return @c true if @p value represents an occupied voxel within @p map .
/// @param value The occupancy value to test.
/// @param map The map context within which to operate.
/// @return True if occupied.
inline bool isOccupied(float value, const OccupancyMap &map)
{
  return value != unobservedOccupancyValue() && value >= map.occupancyThresholdValue();
}

template <typename T>
inline bool isOccupiedT(const Voxel<T> &voxel)
{
  float occupancy;
  voxel.read(&occupancy);
  return isOccupied(occupancy, *voxel.map());
}

/// @ingroup voxeloccupancy
/// Return @c true if @p voxel represents an occupied voxel.
/// @param voxel The occupancy voxel test. Must not be null.
/// @return True if occupied.
inline bool isOccupied(const Voxel<float> &voxel)
{
  return voxel.isValid() && isOccupiedT(voxel);
}
/// @overload
inline bool isOccupied(const Voxel<const float> &voxel)
{
  return voxel.isValid() && isOccupiedT(voxel);
}


/// @ingroup voxeloccupancy
/// Return @c true if @p value represents a free voxel within @p map .
/// @param value The occupancy value to test. Must not be null.
/// @param map The map context within which to operate.
/// @return True if free.
inline bool isFree(float value, const OccupancyMap &map)
{
  return value != unobservedOccupancyValue() && value < map.occupancyThresholdValue();
}

template <typename T>
inline bool isFreeT(const Voxel<T> &voxel)
{
  float occupancy;
  voxel.read(&occupancy);
  return isFree(occupancy, *voxel.map());
}

/// @ingroup voxeloccupancy
/// Return @c true if @p voxel represents a free voxel.
/// @param voxel The occupancy voxel to test. Must not be null.
/// @return True if free.
inline bool isFree(const Voxel<float> &voxel)
{
  return voxel.isValid() && isFreeT(voxel);
}
/// @overload
inline bool isFree(const Voxel<const float> &voxel)
{
  return voxel.isValid() && isFreeT(voxel);
}


/// @ingroup voxeloccupancy
/// Return @c true if @p value represents an unobserved voxel value.
/// @param value The occupancy value to test.
/// @param map The map context within which to operate. Not used.
/// @return True if unobserved.
inline bool isUnobserved(float value, const OccupancyMap &map)
{
  (void)map;
  return value == unobservedOccupancyValue();
}
/// @overload
inline bool isUnobserved(float value)
{
  return value == unobservedOccupancyValue();
}

template <typename T>
inline bool isUnobservedT(const Voxel<T> &voxel)
{
  float occupancy;
  voxel.read(&occupancy);
  return isUnobserved(occupancy, *voxel.map());
}

/// @ingroup voxeloccupancy
/// Return @c true if @p voxel represents an unobserved, but not null voxel.
/// @param voxel The occupancy voxel to test. Must not be null.
/// @return True if unobserved.
inline bool isUnobserved(const Voxel<float> &voxel)
{
  return isUnobservedT(voxel);
}

/// @overload
inline bool isUnobserved(const Voxel<const float> &voxel)
{
  return isUnobservedT(voxel);
}

template <typename T>
inline bool isUnobservedOrNullT(const Voxel<T> &voxel)
{
  // Note: the call to voxel.isValid() is effectively redundant as it's the negated check of voxel.isNull()
  // but it does silence a clang-tidy warning. Perhaps this will help with branch prediction as well because the explict
  // check is clearer?
  return voxel.isNull() || (voxel.isValid() && isUnobservedT(voxel));
}

/// @ingroup voxeloccupancy
/// Return @c true if @p voxel represents an unobserved, but not null voxel.
/// @param voxel The occupancy voxel to test. May be null.
/// @return True if unobserved or null.
inline bool isUnobservedOrNull(const Voxel<float> &voxel)
{
  return isUnobservedOrNullT(voxel);
}
/// @overload
inline bool isUnobservedOrNull(const Voxel<const float> &voxel)
{
  return isUnobservedOrNullT(voxel);
}
}  // namespace ohm

#endif  // VOXELOCCUPANCY_H


================================================
FILE: ohm/VoxelOccupancyCompute.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef VOXELOCCUPANCYCOMPUTE_H
#define VOXELOCCUPANCYCOMPUTE_H

// Note: this header is included in GPU code.
// Because of this "OhmConfig.h" and <cmath> cannot be included here and you may need to include those first.

#if !GPUTIL_DEVICE
#ifndef __device__
#define __device__
#endif  // __device__
#ifndef __host__
#define __host__
#endif  // __host__
#else   // !GPUTIL_DEVICE
#define UNOBSERVED INFINITY
#endif  // !GPUTIL_DEVICE

/// @ingroup voxeloccupancy
/// Adjust the @p occupancy value up by adding @c hit_adjustment , assuming that @c hit_adjustment is positive.
///
/// The resulting occupancy is:
/// - unchanged if @p null_update is true
/// - unchanged if @p initial_value is at @c saturation_min
/// - unchanged if @p initial_value is at @c saturation_max
/// - min of @p hit_adjustment and @p max_value if @p initial_value is equal to @p uninitialised_value
/// - min of `initial_value + hit_adjustment` and @p max_value otherwise
///
/// @param occupancy The occupancy value to adjust.
/// @param initial_value The initial value for @p occupancy (i.e., `*occupancy`)
/// @param hit_adjustment The value to add to @p initial_value when initialised. Set this value when not initialised.
/// @param uninitialised_value The special value used to indicate an uninitialised occupancy value. Typically +inf
/// @param max_value The maximum value allowed for occupancy. Assumed to be positive.
/// @param saturation_min The minimum saturation value. Occupancy at this (low) value should not be modified.
///   Use -inf to disable (or @c std::numeric_limits<float>::lowest() ).
/// @param saturation_max The maximum saturation value. Occupancy at this (high) value should not be modified.
///   Use +inf to disable (or @c std::numeric_limits<float>::max() ).
/// @param null_update Flag indicating nothing should be modified by this function. Used for loops which may have
///   such noop conditions.
inline void occupancyAdjustHit(float *occupancy, float initial_value, float hit_adjustment, float uninitialised_value,
                               float max_value, float saturation_min, float saturation_max, bool null_update)
{
  const bool uninitialised = initial_value == uninitialised_value;
  const float base_value = (null_update || !uninitialised) ? initial_value : 0.0f;
  hit_adjustment =
    (!null_update && (uninitialised || saturation_min < initial_value && initial_value < saturation_max)) ?
      hit_adjustment :
      0.0f;
  *occupancy = (base_value != uninitialised_value) ? fmin(base_value + hit_adjustment, max_value) : base_value;
}

/// @ingroup voxeloccupancy
/// Adjust the @p occupancy value by setting @p adjusted_value which is assumed to be an increase from the current
/// value (or from an uninitialised value). Note that @c adjusted_value may be negative so long as it effects an
/// increase in value.
///
/// The resulting occupancy is:
/// - unchanged if @p null_update is true
/// - unchanged if @p initial_value is at @c saturation_min
/// - unchanged if @p initial_value is at @c saturation_max
/// - min of @p adjusted_value and @p max_value otherwise
///
/// @param occupancy The occupancy value to adjust.
/// @param initial_value The initial value for @p occupancy (i.e., `*occupancy`)
/// @param adjusted_value The new value for @p occupancy assumed to be an increase from the @p initial_value .
/// @param uninitialised_value The special value used to indicate an uninitialised occupancy value. Typically +inf
/// @param max_value The maximum value allowed for occupancy. Assumed to be positive.
/// @param saturation_min The minimum saturation value. Occupancy at this (low) value should not be modified.
///   Use -inf to disable (or @c std::numeric_limits<float>::lowest() ).
/// @param saturation_max The maximum saturation value. Occupancy at this (high) value should not be modified.
///   Use +inf to disable (or @c std::numeric_limits<float>::max() ).
/// @param null_update Flag indicating nothing should be modified by this function. Used for loops which may have
///   such noop conditions.
inline void occupancyAdjustUp(float *occupancy, float initial_value, float adjusted_value, float uninitialised_value,
                              float max_value, float saturation_min, float saturation_max, bool null_update)
{
  const bool uninitialised = initial_value == uninitialised_value;
  adjusted_value =
    (!null_update && (uninitialised || saturation_min < initial_value && initial_value < saturation_max)) ?
      adjusted_value :
      initial_value;
  *occupancy = (adjusted_value != uninitialised_value) ? fmin(max_value, adjusted_value) : adjusted_value;
}

/// @ingroup voxeloccupancy
/// Adjust the @p occupancy value down by adding @c miss_adjustment , assuming that @c miss_adjustment is negative.
///
/// The resulting occupancy is:
/// - unchanged if @p null_update is true
/// - unchanged if @p initial_value is at @c saturation_min
/// - unchanged if @p initial_value is at @c saturation_max
/// - max of @p miss_adjustment and @p min_value if @p initial_value is equal to @p uninitialised_value
/// - max of `initial_value + miss_adjustment` and @p min_value otherwise
///
/// @param occupancy The occupancy value to adjust.
/// @param initial_value The initial value for @p occupancy (i.e., `*occupancy`)
/// @param miss_adjustment The value to add to @p initial_value when initialised. Set this value when not initialised.
/// @param uninitialised_value The special value used to indicate an uninitialised occupancy value. Typically +inf
/// @param min_value The maximum value allowed for occupancy. Assumed to be negative.
/// @param saturation_min The minimum saturation value. Occupancy at this (low) value should not be modified.
///   Use -inf to disable (or @c std::numeric_limits<float>::lowest() ).
/// @param saturation_max The maximum saturation value. Occupancy at this (high) value should not be modified.
///   Use +inf to disable (or @c std::numeric_limits<float>::max() ).
/// @param null_update Flag indicating nothing should be modified by this function. Used for loops which may have
///   such noop conditions.
inline void occupancyAdjustMiss(float *occupancy, float initial_value, float miss_adjustment, float uninitialised_value,
                                float min_value, float saturation_min, float saturation_max, bool null_update)
{
  const bool uninitialised = initial_value == uninitialised_value;
  const float base_value = (null_update || !uninitialised) ? initial_value : 0.0f;
  miss_adjustment =
    (!null_update && (uninitialised || saturation_min < initial_value && initial_value < saturation_max)) ?
      miss_adjustment :
      0.0f;
  *occupancy = (base_value != uninitialised_value) ? fmax(min_value, base_value + miss_adjustment) : base_value;
}

/// @ingroup voxeloccupancy
/// Adjust the @p occupancy value by setting @p adjusted_value which is assumed to be a decrease from the current
/// value (or from an uninitialised value). Note that @c adjusted_value may be positive so long as it effects a
/// decrease in value.
///
/// The resulting occupancy is:
/// - unchanged if @p null_update is true
/// - unchanged if @p initial_value is at @c saturation_min
/// - unchanged if @p initial_value is at @c saturation_max
/// - max of @p adjusted_value and @p min_value otherwise
///
/// @param occupancy The occupancy value to adjust.
/// @param initial_value The initial value for @p occupancy (i.e., `*occupancy`)
/// @param adjusted_value The new value for @p occupancy assumed to be an increase from the @p initial_value .
/// @param uninitialised_value The special value used to indicate an uninitialised occupancy value. Typically +inf
/// @param min_value The maximum value allowed for occupancy. Assumed to be positive.
/// @param saturation_min The minimum saturation value. Occupancy at this (low) value should not be modified.
///   Use -inf to disable (or @c std::numeric_limits<float>::lowest() ).
/// @param saturation_max The maximum saturation value. Occupancy at this (high) value should not be modified.
///   Use +inf to disable (or @c std::numeric_limits<float>::max() ).
/// @param null_update Flag indicating nothing should be modified by this function. Used for loops which may have
///   such noop conditions.
inline void occupancyAdjustDown(float *occupancy, float initial_value, float adjusted_value, float uninitialised_value,
                                float min_value, float saturation_min, float saturation_max, bool null_update)
{
  const bool uninitialised = initial_value == uninitialised_value;
  adjusted_value =
    (!null_update && (uninitialised || saturation_min < initial_value && initial_value < saturation_max)) ?
      adjusted_value :
      initial_value;
  *occupancy = (adjusted_value != uninitialised_value) ? fmax(min_value, adjusted_value) : adjusted_value;
}

#endif  // VOXELOCCUPANCYCOMPUTE_H


================================================
FILE: ohm/VoxelSecondarySample.h
================================================
// Copyright (c) 2022
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_DUAL_RETURN_H
#define OHM_DUAL_RETURN_H

#include "OhmConfig.h"

#include <cinttypes>
#include <limits>

namespace ohm
{
/// Stores information about secondary samples (dual returns) collected in a voxel.
///
/// Lidar dual returns may be considered as secondary samples in OHM provdied the secondary sample layer is used.
/// Each voxel stores the information provided in this structure. This stores the number of secondary samples recorded
/// by a voxel and additional values to provide the mean distance between primary and secondary samples for this voxel
/// and the standard deviation thereof.
///
/// @note The @c range_mean is stored as a @c uint16_t value in order to keep the voxel size at 8 bytes. The range is
/// quantised to 1000th of the input value. OHM generally expects metres, so the range is generally in millimetres.
/// The quantisation factor is exposed via @c secondarySampleQuantisationFactor() .
///
/// Use @c secondarySampleRangeMean() and @c secondarySampleRangeStdDev() in order to extract the range mean and
/// standard deviation values.
struct VoxelSecondarySample
{
  /// Used to calculate the standard deviation, @c m2 aggregates the squared distance from the mean.
  float m2;
  /// Standard mean distance between the primary sample and the secondary sample for secondary samples falling in this
  /// voxel.
  uint16_t range_mean;
  /// The number of secondary samples which have been collected in this voxel.
  uint16_t count;
};

/// Quantisation factor used for @c VoxelSecondarySample::range_mean . This relates the normal occupancy map units to
/// the quantised value using:
///
/// @code{.unparsed}
///   range_units = secondarySampleQuantisationFactor() * map_units
/// @endcode
///
/// @return A scalar for transforming from map units to @c VoxelSecondarySample::range_mean units.
inline constexpr double secondarySampleQuantisationFactor()
{
  return 1000.0;
}

/// The maximum range value which can be stored in @c VoxelSecondarySample::range_mean .
/// @return The maximum range value for @c VoxelSecondarySample::range_mean .
inline constexpr double secondarySampleMaxRange()
{
  return (std::numeric_limits<decltype(VoxelSecondarySample::range_mean)>::max() - 1u) /
         secondarySampleQuantisationFactor();
}

/// Extract the mean distance between primary and secondary samplex for @p voxel .
///
/// This converts @c VoxelSecondarySample::range_mean back into map units.
///
/// @param voxel The voxel to read.
/// @return The average distance between primary and secondary samples for @p voxel .
inline double secondarySampleRangeMean(const VoxelSecondarySample &voxel)
{
  return voxel.range_mean / secondarySampleQuantisationFactor();
}

/// Extract the standard deviation of the distance between primary and secondary samplex for @p voxel .
///
/// This converts @c VoxelSecondarySample::m2 into a standard deviation.
///
/// @param voxel The voxel to read.
/// @return The standard deviation of the distance between primary and secondary samples for @p voxel .
inline double secondarySampleRangeStdDev(const VoxelSecondarySample &voxel)
{
  return std::sqrt(double(voxel.m2) / double(voxel.count));
}

/// Update statistics for an additional secondary sample in @p voxel .
///
/// @param voxel The voxel to update in which the secondary sample lies.
/// @param range The distance between the primary and secondary samples. Must be positive.
inline void addSecondarySample(VoxelSecondarySample &voxel, double range)
{
  // Using Wellford's algorithm.
  // Clamp range
  range = std::min(range, secondarySampleMaxRange());
  double range_mean = voxel.range_mean / secondarySampleQuantisationFactor();
  ++voxel.count;
  const double delta = range - range_mean;
  range_mean += delta / voxel.count;
  voxel.range_mean = uint16_t(range_mean * secondarySampleQuantisationFactor());
  const double delta2 = range - range_mean;
  voxel.m2 += float(delta * delta2);
}

/// @overload
inline void addSecondarySample(Voxel<VoxelSecondarySample> &voxel, double range)
{
  VoxelSecondarySample data = voxel.data();
  addSecondarySample(data, range);
  voxel.write(data);
}
}  // namespace ohm

#endif  // OHM_DUAL_RETURN_H


================================================
FILE: ohm/VoxelTouchTime.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_VOXEL_TOUCH_TIME_H
#define OHM_VOXEL_TOUCH_TIME_H

#include "OhmConfig.h"

namespace ohm
{
#include "VoxelTouchTimeCompute.h"
}

#endif  // OHM_VOXEL_TOUCH_TIME_H


================================================
FILE: ohm/VoxelTouchTimeCompute.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_VOXEL_TOUCH_TIME_COMPUTE_H
#define OHM_VOXEL_TOUCH_TIME_COMPUTE_H

// Do not include config header. This can be used from GPU code.

#if !GPUTIL_DEVICE
#define OHM_DEVICE_HOST
#else  // GPUTIL_DEVICE
#define OHM_DEVICE_HOST __device__ __host__
#endif  // GPUTIL_DEVICE

/// Encode as milliseconds
#define OHM_VOXEL_TOUCH_TIME_SCALE 0.001

/// Encode a voxel timestamp. Encoded as milliseconds since @p timebase .
/// @param timebase The base time to encode relative to.
/// @param timestamp The timestamp to encode.
/// @return Milliseconds elapsed between @p timebase and @p timestamp .
inline unsigned OHM_DEVICE_HOST encodeVoxelTouchTime(double timebase, double timestamp)
{
  return (unsigned)((timestamp - timebase) / OHM_VOXEL_TOUCH_TIME_SCALE);
}

/// Decode a voxel timestamp. Decoded as milliseconds since @p timebase .
/// @param timebase The base time to decode relative to.
/// @param touch_time Encoded time - milliseconds elapsed since @p timebase .
/// @return The decoded tiemstamp.
inline double OHM_DEVICE_HOST decodeVoxelTouchTime(double timebase, unsigned touch_time)
{
  return touch_time * OHM_VOXEL_TOUCH_TIME_SCALE + timebase;
}

#undef OHM_DEVICE_HOST

#endif  // OHM_VOXEL_TOUCH_TIME_COMPUTE_H


================================================
FILE: ohm/VoxelTsdf.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "VoxelTsdf.h"

#include "MapInfo.h"

namespace ohm
{
void updateMapInfo(MapInfo &info, const TsdfOptions &tsdf_options)
{
  info.set(MapValue("tsdf-max-weight", tsdf_options.max_weight));
  info.set(MapValue("tsdf-default-truncation-distance", tsdf_options.default_truncation_distance));
  info.set(MapValue("tsdf-dropoff-epsilon", tsdf_options.dropoff_epsilon));
  info.set(MapValue("tsdf-sparsity-compensation-factor", tsdf_options.sparsity_compensation_factor));
}


void fromMapInfo(TsdfOptions &tsdf_options, const MapInfo &info)
{
  tsdf_options.max_weight =
    static_cast<float>(info.get("tsdf-max-weight", MapValue("tsdf-max-weight", tsdf_options.max_weight)));
  tsdf_options.default_truncation_distance = static_cast<float>(
    info.get("tsdf-default-truncation-distance",
             MapValue("tsdf-default-truncation-distance", tsdf_options.default_truncation_distance)));
  tsdf_options.dropoff_epsilon = static_cast<float>(
    info.get("tsdf-dropoff-epsilon", MapValue("tsdf-dropoff-epsilon", tsdf_options.dropoff_epsilon)));
  tsdf_options.sparsity_compensation_factor = static_cast<float>(
    info.get("tsdf-sparsity-compensation-factor",
             MapValue("tsdf-sparsity-compensation-factor", tsdf_options.sparsity_compensation_factor)));
}
}  // namespace ohm


================================================
FILE: ohm/VoxelTsdf.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef VOXELTSDF_H
#define VOXELTSDF_H

#include "OhmConfig.h"

#include "Voxel.h"

#include <glm/vec3.hpp>

/// @defgroup voxeltsdf Voxel Truncated Signed Distance Fields
/// These functions are used to manipulate the voxel TSDF values. @c VoxelTsdf allows a voxel to store
/// a weight and distance value for the TSDF algorithm.
///
/// The TSDF algorithm used here is based on Voxblox TSDF and is licensed under BSD 3-clause license.
///

namespace ohm
{
class MapInfo;

/// TSDF mapping options.
struct TsdfOptions
{
  /// Maximum TSDF voxel weight.
  float max_weight = 1e4f;
  /// Default truncation distance.
  float default_truncation_distance = 0.1f;
  /// Dropoff for the dropoff adjustment. Zero or negative to disable.
  float dropoff_epsilon = 0.0f;
  /// Compensation for sparse data sets. Zero or negative to disable.
  float sparsity_compensation_factor = 1.0;
};

/// Write the given tsdf options to @c MapInfo .
void ohm_API updateMapInfo(MapInfo &info, const TsdfOptions &tsdf_options);

/// Read the given tsdf options from @c MapInfo .
void ohm_API fromMapInfo(TsdfOptions &tsdf_options, const MapInfo &info);

#include "VoxelTsdfCompute.h"
}  // namespace ohm

#endif  // VOXELTSDF_H


================================================
FILE: ohm/VoxelTsdfCompute.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef VOXELTSDFCOMPUTE_H
#define VOXELTSDFCOMPUTE_H

#if !GPUTIL_DEVICE
#ifndef __device__
#define __device__
#endif  // __device__
#ifndef __host__
#define __host__
#endif  // __host__

#define TSDF_VOX_FUNC_PREFACE template <typename Vec3>

/// Voxel data for maintaining truncated signed distance field.
struct VoxelTsdf
{
  float weight;    ///< TSDF weight.
  float distance;  ///< TSDF distance.
};

#else  // !GPUTIL_DEVICE

#if !defined(VEC3)
#define VEC3
typedef float3 Vec3;
#endif  // !defined(VEC3)

#if !defined(Int3)
typedef int3 Int3;
#endif  // !defined(Int3)

#define TSDF_VOX_FUNC_PREFACE

/// Voxel data for maintaining truncated signed distance field.
typedef struct VoxelTsdf_t
{
  float weight;   ///< TSDF weight.
  float distance; ///< TSDF distance.
} VoxelTsdf;

#endif  // !GPUTIL_DEVICE

/// Check if @p voxel has valid data in it.
/// @return True if @p voxel is non-null and has valid data, false when null or unused.
inline __device__ __host__ bool isValidTsdf(const VoxelTsdf *voxel)
{
  return voxel && (voxel->weight != 0.0f || voxel->distance != 0.0f);
}


TSDF_VOX_FUNC_PREFACE
inline __device__ __host__ float computeDistance(const Vec3 sensor, const Vec3 sample, const Vec3 voxel_centre)
{
  const Vec3 sensor_to_voxel = voxel_centre - sensor;
  const Vec3 sensor_to_sample = sample - sensor;

  const float distance_g = (float)sqrt(dot(sensor_to_sample, sensor_to_sample));
  // Project sensor to voxel onto the sample ray.
  const float distance_g_v = (float)dot(sensor_to_voxel, sensor_to_sample) / distance_g;

  const float sdf = distance_g - distance_g_v;
  return sdf;
}


/// Calculate the truncated signed distance fields (TSDF) update for a voxel.
///
/// This code is based on Voxblox fast TSDF update and is licensed under BSD 3-clause license.
///
/// @param sensor Sensor position/the start of the sample ray.
/// @param sample The sample position and the end of the sample ray.
/// @param voxel_centre The center of the voxel being updated. The voxel lies along the ray.
/// @param max_weight The maximum weight allowed for a TSDF voxel.
/// @param dropoff_epsilon Used to calculate weight dropoff. The recommended value is to match the voxel size.
///   See voxblox for details. Ignored if zero or less.
/// @param use_sparsity_compensation_factor Use compensation for sparse point clouds?
/// @param sparsity_compensation_factor The sparsity compensation factor to apply. Ignored if zero or less.
/// @param[in,out] voxel_weight Initial TSDF voxel weight which is then modified with the TSDF calculation for this ray.
/// @param[in,out] voxel_distance Initial TSDF voxel distance, then modified with the TSDF calculation for this ray.
/// @return True if the calculation modified the @p voxel_weight and/or @p voxel_distance.
TSDF_VOX_FUNC_PREFACE
inline __device__ __host__ bool calculateTsdf(const Vec3 sensor, const Vec3 sample, const Vec3 voxel_centre,
                                              float default_truncation_distance, float max_weight,
                                              float dropoff_epsilon, float sparsity_compensation_factor,
                                              float *voxel_weight, float *voxel_distance)
{
  // Calculate the TSDF adjustment.
#if !GPUTIL_DEVICE
  using namespace std;
#else   // !GPUTIL_DEVICE
#endif  // !GPUTIL_DEVICE
  const float sdf = computeDistance(sensor, sample, voxel_centre);

  const float initial_weight = *voxel_weight;
  float updated_weight = 1.0f;  // Equivalent to voxblox use_constant_weight

  // Comment from voxblox
  // Compute updated weight in case we use weight dropoff. It's easier here
  // that in getVoxelWeight as here we have the actual SDF for the voxel
  // already computed.
  updated_weight *= (dropoff_epsilon > 0) ?
                      ((default_truncation_distance + sdf) / (default_truncation_distance - dropoff_epsilon)) :
                      1.0f;
  updated_weight = max(updated_weight, 0.0f);

  // Comment from voxblox
  // Compute the updated weight in case we compensate for sparsity. By
  // multiplicating the weight of occupied areas (|sdf| < truncation distance)
  // by a factor, we prevent to easily fade out these areas with the free
  // space parts of other rays which pass through the corresponding voxels.
  // This can be useful for creating a TSDF map from sparse sensor data (e.g.
  // visual features from a SLAM system). By default, this option is disabled.
  updated_weight *=
    (sparsity_compensation_factor > 0 && fabs(sdf) < default_truncation_distance) ? sparsity_compensation_factor : 1.0f;

  const float new_weight = initial_weight + updated_weight;

  // Comment from voxblox
  // it is possible to have weights very close to zero, due to the limited
  // precision of floating points dividing by this small value can cause nans
  const float abs_new_weight = fabs(new_weight);
  const bool near_zero_weight = abs_new_weight < 0.00001f;
  const float new_sdf =
    (!near_zero_weight) ? (sdf * updated_weight + *voxel_distance * initial_weight) / new_weight : 0.0f;

  *voxel_distance = (!near_zero_weight) ? ((new_sdf > 0.0f) ? min(default_truncation_distance, new_sdf) :
                                                             max(-default_truncation_distance, new_sdf)) :
                                          *voxel_distance;
  *voxel_weight = (!near_zero_weight) ? min(new_weight, max_weight) : initial_weight;
  return !near_zero_weight;
}

#endif  // VOXELTSDFCOMPUTE_H


================================================
FILE: ohm/private/ClearingPatternDetail.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef CLEARINGPATTERNDETAIL_H
#define CLEARINGPATTERNDETAIL_H

#include "OhmConfig.h"

#include <glm/glm.hpp>

#include <vector>

namespace ohm
{
class RayPattern;

struct ClearingPatternDetail
{
  std::vector<glm::dvec3> ray_set;
  const RayPattern *pattern = nullptr;
  unsigned ray_flags = 0u;  // Default value should be ClearingPattern::kDefaultFlags.
  bool has_pattern_ownership = false;
};
}  // namespace ohm

#endif  // CLEARINGPATTERNDETAIL_H


================================================
FILE: ohm/private/LineKeysQueryDetail.h
================================================

// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_LINEKEYSQUERYDETAIL_H_
#define OHM_LINEKEYSQUERYDETAIL_H_

#include "OhmConfig.h"

#include "QueryDetail.h"

namespace ohm
{
/// Pimpl data for @c LineKeysQuery
struct ohm_API LineKeysQueryDetail : QueryDetail
{
  std::vector<glm::dvec3> rays;  ///< Ray origin/end point pairs to query for.
  /// Results vector, identifying the offsets for each ray into @c intersected_voxels where the results for that ray
  /// begin.
  std::vector<size_t> result_indices;
  /// Results vector, identifying the number of voxels for each ray in @c intersected_voxels.
  std::vector<size_t> result_counts;
};
}  // namespace ohm

#endif  // OHM_LINEKEYSQUERYDETAIL_H_


================================================
FILE: ohm/private/LineQueryDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_LINEQUERYDETAIL_H
#define OHM_LINEQUERYDETAIL_H

#include "OhmConfig.h"

#include "QueryDetail.h"

#include "ohm/KeyList.h"

namespace ohm
{
class ClearanceProcess;

struct ohm_API LineQueryDetail : QueryDetail
{
  glm::dvec3 start_point = glm::dvec3(0);
  glm::dvec3 end_point = glm::dvec3(0);
  // Internal: calculated on execute.
  glm::dvec3 segment_dir = glm::dvec3(0);
  glm::dvec3 axis_scaling = glm::dvec3(1, 1, 1);
  KeyList segment_keys;
  // Internal: calculated on execute.
  double segment_length = 0;
  /// Range reported for unobstructed voxels.
  float default_range = -1;
  float search_radius = 0;
};
}  // namespace ohm

#endif  // OHM_LINEQUERYDETAIL_H


================================================
FILE: ohm/private/MapLayerDetail.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_MAPLAYERDETAIL_H
#define OHM_MAPLAYERDETAIL_H

#include "OhmConfig.h"

#include <string>

namespace ohm
{
struct VoxelLayoutDetail;

struct ohm_API MapLayerDetail
{
  std::string name;
  VoxelLayoutDetail *voxel_layout = nullptr;
  uint16_t layer_index = 0;
  uint16_t subsampling = 0;
  unsigned flags = 0;
};
}  // namespace ohm

#endif  // OHM_MAPLAYERDETAIL_H


================================================
FILE: ohm/private/MapLayoutDetail.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_MAPLAYOUTDETAIL_H
#define OHM_MAPLAYOUTDETAIL_H

#include "OhmConfig.h"

#include "ohm/MapLayer.h"

#include <vector>

namespace ohm
{
struct ohm_API MapLayoutDetail
{
  std::vector<MapLayer *> layers;
  int occupancy_layer = -1;
  int mean_layer = -1;
  int traversal_layer = -1;
  int covariance_layer = -1;
  int clearance_layer = -1;
  int intensity_layer = -1;
  int hit_miss_count_layer = -1;

  inline ~MapLayoutDetail() { clear(); }

  inline void clear()
  {
    for (MapLayer *layer : layers)
    {
      delete layer;
    }
    layers.clear();
    occupancy_layer = mean_layer = traversal_layer = covariance_layer = clearance_layer = intensity_layer =
      hit_miss_count_layer = -1;
  }
};
}  // namespace ohm

#endif  // OHM_MAPLAYOUTDETAIL_H


================================================
FILE: ohm/private/MapperDetail.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef MAPPERDETAIL_H_
#define MAPPERDETAIL_H_

#include "OhmConfig.h"

#include <vector>

namespace ohm
{
class MappingProcess;
class OccupancyMap;

struct ohm_API MapperDetail
{
  std::vector<MappingProcess *> processes;
  OccupancyMap *map = nullptr;
  unsigned next_process = 0;
};
}  // namespace ohm

#endif  // MAPPERDETAIL_H_


================================================
FILE: ohm/private/NdtMapDetail.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef NDTMAPDETAIL_H
#define NDTMAPDETAIL_H

#include "OhmConfig.h"

#include "MapProbability.h"
#include "NdtMode.h"

namespace ohm
{
class OccupancyMap;

/// Internal details associated with a @c NdtMap extension to an @c OccupancyMap .
struct NdtMapDetail
{
  /// The target occupancy map.
  OccupancyMap *map = nullptr;
  /// Range sensor noise estimate
  float sensor_noise = 0.05f;  // NOLINT(readability-magic-numbers)
  /// Number of samples required before using NDT logic in a miss integration.
  unsigned sample_threshold = 3;
  /// Rate at which ray intersections with NDT ellipsoids errode voxels. Range [0, 1] with 1 yielding stronger
  /// effects. Initialise to zero here. Should be reset on NDT map construction to match the normal occupancy miss
  /// value by default, otherwise we will yield stronger erosion from NDT misses.
  ///
  /// See NdtMap::ndtAdaptationRateFromMissProbability()
  float adaptation_rate = 0;  // NOLINT(readability-magic-numbers)
  /// Low probability value threshold used to re-initialise covariance matrix and mean.
  /// Used with @c reinitialise_covariance_point_count in @c calculateHitWithCovariance()
  float reinitialise_covariance_threshold = probabilityToValue(0.2f);  // NOLINT(readability-magic-numbers)
  /// Upper point count limit required to reinitialise the covariance matrix. Used with
  /// @c reinitialise_covariance_threshold in @c calculateHitWithCovariance()
  unsigned reinitialise_covariance_point_count = 100;
  /// Covariance of intensity (for NDT-TM) for initialisation upon receipt of first hit.
  float initial_intensity_covariance = 1.0f;
  /// The NDT mapping mode. Must not be @c kNone in the @c NdtMap usage case.
  NdtMode mode = NdtMode::kOccupancy;
  /// True if @p map is a borrowed pointer, false to take ownership and delete it.
  bool borrowed_map = false;
  /// Debug tracing enabled? Requires 3es
  bool trace = false;
};
}  // namespace ohm

#endif  // NDTMAPDETAIL_H


================================================
FILE: ohm/private/NearestNeighboursDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_NEARESTNEIGHBOURSDETAIL_H
#define OHM_NEARESTNEIGHBOURSDETAIL_H

#include "OhmConfig.h"

#include "QueryDetail.h"

namespace ohm
{
struct ohm_API NearestNeighboursDetail : QueryDetail
{
  glm::dvec3 near_point = glm::dvec3(0);
  float search_radius = 0;
};
}  // namespace ohm

#endif  // OHM_NEARESTNEIGHBOURSDETAIL_H


================================================
FILE: ohm/private/OccupancyMapDetail.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OccupancyMapDetail.h"

#include "DefaultLayer.h"
#include "MapLayer.h"
#include "MapLayout.h"
#include "MapRegionCache.h"
#include "OccupancyMap.h"
#include "VoxelLayout.h"
#include "VoxelOccupancy.h"

#include <algorithm>
#include <cstring>

namespace ohm
{
OccupancyMapDetail::~OccupancyMapDetail()
{
  delete gpu_cache;
}


void OccupancyMapDetail::moveKeyAlongAxis(Key &key, int axis, int step, const glm::ivec3 &region_voxel_dimensions)
{
  const glm::ivec3 local_limits = region_voxel_dimensions;

  if (step == 0)
  {
    // No change.
    return;
  }

  // We first step within the chunk region. If we can't then we step the region and reset
  // stepped local axis value.
  glm::i16vec3 region_key = key.regionKey();
  glm::ivec3 local_key = key.localKey();
  if (step > 0)
  {
    // Positive step.
    local_key[axis] += step;
    region_key[axis] += local_key[axis] / local_limits[axis];
    local_key[axis] %= local_limits[axis];
  }
  else
  {
    // Negative step.
    local_key[axis] += step;
    // Create a region step which simulates a floating point floor.
    region_key[axis] += ((local_key[axis] - (local_limits[axis] - 1)) / local_limits[axis]);
    if (local_key[axis] < 0)
    {
      // This is nuts. In C/C++, the % operator is not actually a modulus operator.
      // It's a "remainder" operator. A modulus operator should only give positive results,
      // but in C a negative input will generate a negative output. Through the magic of
      // StackOverflow, here's a good explanation:
      //  https://stackoverflow.com/questions/11720656/modulo-operation-with-negative-numbers
      // This means that here, given localKey[axis] is negative, the modulus:
      //    localKey[axis] % LocalLimits[axis]
      // will give results in the range (-LocalLimits[axis], 0]. So, lets set the limit
      // to 4, then we get the following: like follows:
      //
      // i  i%4   4 - i%4
      //  0  0    4
      // -1 -1    3
      // -2 -2    2
      // -3 -3    1
      // -4  0    4
      // -5 -1    3
      // -6 -2    2
      // -7 -3    1
      // -8  0    4
      //
      // The last column above shows the results of the following line of code.
      // This generates the wrong results in that the '4' results in the last
      // column should be 0. We could apply another "% LocalLimits[axis]" to
      // the result or just add the if statement below.
      local_key[axis] = local_limits[axis] + local_key[axis] % local_limits[axis];
      if (local_key[axis] == local_limits[axis])
      {
        local_key[axis] = 0;
      }
    }
    // else
    // {
    //   localKey[axis] %= LocalLimits[axis];
    // }
  }

  key = Key(region_key, local_key.x, local_key.y, local_key.z);
}


void OccupancyMapDetail::setDefaultLayout(MapFlag init_flags)
{
  // Setup the default layers
  layout.clear();

  addOccupancy(layout);

  if ((init_flags & MapFlag::kVoxelMean) != MapFlag::kNone)
  {
    addVoxelMean(layout);
    flags |= MapFlag::kVoxelMean;
  }
  else
  {
    flags &= ~MapFlag::kVoxelMean;
  }

  if ((init_flags & MapFlag::kTraversal) != MapFlag::kNone)
  {
    addTraversal(layout);
    flags |= MapFlag::kTraversal;
  }
  else
  {
    flags &= ~MapFlag::kTraversal;
  }

  if ((init_flags & MapFlag::kTouchTime) != MapFlag::kNone)
  {
    addTouchTime(layout);
    flags |= MapFlag::kTouchTime;
  }
  else
  {
    flags &= ~MapFlag::kTouchTime;
  }

  if ((init_flags & MapFlag::kIncidentNormal) != MapFlag::kNone)
  {
    addIncidentNormal(layout);
    flags |= MapFlag::kIncidentNormal;
  }
  else
  {
    flags &= ~MapFlag::kIncidentNormal;
  }

  if ((init_flags & MapFlag::kTsdf) != MapFlag::kNone)
  {
    addTsdf(layout);
    flags |= MapFlag::kTsdf;
  }
  else
  {
    flags &= ~MapFlag::kTsdf;
  }

  if ((init_flags & MapFlag::kSecondarySample) != MapFlag::kNone)
  {
    addSecondarySamples(layout);
    flags |= MapFlag::kSecondarySample;
  }
  else
  {
    flags &= ~MapFlag::kSecondarySample;
  }
}


void OccupancyMapDetail::copyFrom(const OccupancyMapDetail &other)
{
  origin = other.origin;
  region_spatial_dimensions = other.region_spatial_dimensions;
  region_voxel_dimensions = other.region_voxel_dimensions;
  resolution = other.resolution;
  stamp = other.stamp;
  occupancy_threshold_value = other.occupancy_threshold_value;
  hit_value = other.hit_value;
  miss_value = other.miss_value;
  min_voxel_value = other.min_voxel_value;
  max_voxel_value = other.max_voxel_value;
  saturate_at_min_value = other.saturate_at_min_value;
  saturate_at_max_value = other.saturate_at_max_value;
  layout = MapLayout(other.layout);
  flags = other.flags;
}
}  // namespace ohm


================================================
FILE: ohm/private/OccupancyMapDetail.h
================================================
//
// Author: Kazys Stepanas
//
#ifndef OHM_MAPDETAIL_H
#define OHM_MAPDETAIL_H

#include "OhmConfig.h"

#include <glm/glm.hpp>

#include "ohm/MapChunk.h"
#include "ohm/MapFlag.h"
#include "ohm/MapInfo.h"
#include "ohm/MapLayout.h"
#include "ohm/MapRegion.h"
#include "ohm/Mutex.h"
#include "ohm/RayFilter.h"

#include <ohmutil/VectorHash.h>

#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#endif  // __GNUC__
#include <ska/bytell_hash_map.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif  // __GNUC__

#include <mutex>
#include <unordered_map>
#include <vector>

namespace ohm
{
using ChunkMap = ska::bytell_hash_map<glm::i16vec3, MapChunk *, Vector3Hash<glm::i16vec3>>;

class MapRegionCache;
class OccupancyMap;

/// Internal details associated with an @c OccupancyMap .
struct ohm_API OccupancyMapDetail
{
  /// Mutex type to use to protect map access.
  using Mutex = ohm::Mutex;

  /// A global origin offset for data in the map. All data read from the map has this origin added.
  glm::dvec3 origin = glm::dvec3(0);
  /// The spatial dimensions of each region. Calculated as `region_voxel_dimensions * resolution`.
  /// Each axis may have a different spatial length.
  glm::dvec3 region_spatial_dimensions = glm::dvec3(0);
  /// The voxel dimensions of each region - i.e., the number of voxels along each axis for a map region.
  /// Each axis may have a different voxel length.
  glm::u8vec3 region_voxel_dimensions = glm::u8vec3(0);
  /// The size of a voxel cube edge. All voxels are uniform cubes.
  double resolution = 0.0;
  /// The timestamp of the first ray added to the map. This is only value if >= 0 and should be set once only.
  /// @note Timestamps may be unavailable.
  double first_ray_time = -1;
  /// Used to mark changes in the map. This is a monotonic value which is modified when the map is changed and is
  /// copied into associated @c MapChunk objects. This can be used to detect the recency of changes.
  uint64_t stamp = 0;
  /// The value threshold used to consider a voxel as occupied. Occupied voxels have a value equal to or greater than
  /// this value, but not equal to @c ohm::unobservedOccupancyValue() (infinity).
  /// @see @c ohm::valueToProbability()
  float occupancy_threshold_value = 0.0f;
  /// The value adjustment made to voxels in which samples fall (hits). Must be > 0
  float hit_value = 0.0f;
  /// The value adjustment made to voxels along rays leading up to sample points (misses). Must be < 0
  float miss_value = 0.0f;
  /// The maximum value clamp for a voxel value (excluding @c ohm::unobservedOccupancyValue() (infinity)).
  /// Zero to disable.
  float min_voxel_value = 0.0f;
  /// The minimum value clamp for a voxel value.
  /// Zero to disable.
  float max_voxel_value = 0.0f;
  /// Flag indicating voxels become locked and cannot be changed when they reach @c min_voxel_value .
  bool saturate_at_min_value = false;
  /// Flag indicating voxels become locked and cannot be changed when they reach @c max_voxel_value .
  bool saturate_at_max_value = false;
  /// Map control flags.
  MapFlag flags = MapFlag::kNone;
  /// The voxel memory layout information for the map.
  MapLayout layout;
  /// The hash map of @c MapChunk objects contained in this map.
  ChunkMap chunks;
  /// Data access mutex. Used to protect @c chunks .
  mutable Mutex mutex;
  // Region count at load time. Useful when only the header is loaded.
  size_t loaded_region_count = 0;

  /// GPU cache pointer. Note: this is declared here, but implemented in a dependent library. We simply ensure that
  /// the map detail supports a GPU cache.
  ///
  /// @todo Use @c std::unique_ptr
  MapRegionCache *gpu_cache = nullptr;

  /// Optional function to be called for each input ray before processing. See @c RayFilterFunction documentation.
  RayFilterFunction ray_filter;

  /// Meta information storage about the map.
  /// The data stored are arbitrary key/value pairs. Generally it is expected that this may hold data about how
  /// the map was generated or has been modified.
  MapInfo info;

  /// Default constructor.
  OccupancyMapDetail() = default;
  /// Destructor ensures @c gpu_cache is destroyed.
  ~OccupancyMapDetail();

  /// Move an @c Key along a selected axis.
  /// This is the implementation to @c OccupancyMap::moveKeyAlongAxis(). See that function for details.
  /// @param key The key to adjust.
  /// @param axis Axis ID to move along [0, 2].
  /// @param step How far to move/step.
  /// @param region_voxel_dimensions Region dimensions in voxels, as per @c OccupancyMapDetail::region_voxel_dimensions
  static void moveKeyAlongAxis(Key &key, int axis, int step, const glm::ivec3 &region_voxel_dimensions);

  /// @overload
  inline void moveKeyAlongAxis(Key &key, int axis, int step) const
  {
    moveKeyAlongAxis(key, axis, step, region_voxel_dimensions);
  }

  /// Setup the default @c MapLayout: occupancy layer and clearance layer.
  /// @param init_flags Flags identifying how to initialise the layers. Only considers flags relating to voxel layers.
  ///   The @p flags member is updated accordingly.
  void setDefaultLayout(MapFlag init_flags = MapFlag::kNone);

  /// Copy internal details from @p other. For cloning.
  /// @param other The map detail to copy from.
  void copyFrom(const OccupancyMapDetail &other);
};
}  // namespace ohm

#endif  // OHM_MAPDETAIL_H


================================================
FILE: ohm/private/OccupancyQueryAlg.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OCCUPANCYQUERYALG_H
#define OCCUPANCYQUERYALG_H

#include "OhmConfig.h"

#include "ohm/OccupancyMap.h"

#include "ohm/private/OccupancyMapDetail.h"
#include "ohm/private/QueryDetail.h"

#ifdef OHM_FEATURE_THREADS
#include <tbb/blocked_range.h>
#include <tbb/blocked_range3d.h>
#include <tbb/parallel_for.h>

#include <atomic>
#endif  // OHM_FEATURE_THREADS

#include <functional>

// This file contains various algorithms to help execute queries on GPU.
// Common code, structures and contracts are presented here.

namespace ohm
{
template <typename QUERY>
unsigned occupancyQueryRegions(
  OccupancyMap &map, QUERY &query, ClosestResult &closest, const glm::dvec3 &query_min_extents,
  const glm::dvec3 &query_max_extents,
  const std::function<unsigned(OccupancyMap &, QUERY &, const glm::i16vec3 &, ClosestResult &)> &region_query_func)
{
  glm::i16vec3 min_region_key;
  glm::i16vec3 max_region_key;
  glm::i16vec3 region_key;
  unsigned current_neighbours = 0;

  // Determine the maximum deltas in region indexing we can have based on the provided extents.
  min_region_key = map.regionKey(query_min_extents);
  max_region_key = map.regionKey(query_max_extents);

  // Iterate the regions, invoking region_query_func for each.
  for (int16_t z = min_region_key.z; z <= max_region_key.z; ++z)
  {
    region_key.z = z;
    for (int16_t y = min_region_key.y; y <= max_region_key.y; ++y)
    {
      region_key.y = y;
      for (int16_t x = min_region_key.x; x <= max_region_key.x; ++x)
      {
        region_key.x = x;
        current_neighbours += region_query_func(map, query, region_key, closest);
      }
    }
  }

  return current_neighbours;
}

template <typename QUERY>
inline unsigned occupancyQueryRegions(OccupancyMap &map, QUERY &query, ClosestResult &closest,
                                      const glm::dvec3 &query_min_extents, const glm::dvec3 &query_max_extents,
                                      unsigned (*region_query_func)(OccupancyMap &, QUERY &, const glm::i16vec3 &,
                                                                    ClosestResult &))
{
  const std::function<unsigned(OccupancyMap &, QUERY &, const glm::i16vec3 &, ClosestResult &)> region_op =
    region_query_func;
  return occupancyQueryRegions(map, query, closest, query_min_extents, query_max_extents, region_op);
}

template <typename QUERY>
unsigned occupancyQueryRegionsParallel(
  OccupancyMap &map, QUERY &query, ClosestResult &closest, const glm::dvec3 &query_min_extents,
  const glm::dvec3 &query_max_extents,
  const std::function<unsigned(OccupancyMap &, QUERY &, const glm::i16vec3 &, ClosestResult &)> &region_query_func)
{
#ifdef OHM_FEATURE_THREADS
  glm::i16vec3 min_region_key;
  glm::i16vec3 max_region_key;
  std::atomic_uint current_neighbours(0u);

  // Determine the maximum deltas in region indexing we can have based on the provided extents.
  min_region_key = map.regionKey(query_min_extents);
  max_region_key = map.regionKey(query_max_extents);

  tbb::parallel_for(
    tbb::blocked_range3d<size_t>(max_region_key.z, max_region_key.z + 1, max_region_key.y, max_region_key.y + 1,
                                 max_region_key.x, max_region_key.x + 1),
    [&current_neighbours, &map, &query, &closest, &region_query_func](const tbb::blocked_range3d<int> &range) {
      glm::i16vec3 region_key;
      for (int z = range.pages().begin(); z != range.pages().end(); ++z)
      {
        region_key.z = z;
        for (int y = range.rows().begin(); y != range.rows().end(); ++y)
        {
          region_key.y = y;
          for (int x = range.cols().begin(); x != range.cols().end(); ++x)
          {
            region_key.x = x;
            current_neighbours += region_query_func(map, query, region_key, closest);
          }
        }
      }
    });

  return current_neighbours;
#else   // OHM_FEATURE_THREADS
  return occupancyQueryRegions(map, query, closest, query_min_extents, query_max_extents, region_query_func);
#endif  // OHM_FEATURE_THREADS
}

template <typename QUERY>
inline unsigned occupancyQueryRegionsParallel(OccupancyMap &map, QUERY &query, ClosestResult &closest,
                                              const glm::dvec3 &query_min_extents, const glm::dvec3 &query_max_extents,
                                              unsigned (*region_query_func)(OccupancyMap &, QUERY &,
                                                                            const glm::i16vec3 &, ClosestResult &))
{
  const std::function<unsigned(OccupancyMap &, QUERY &, const glm::i16vec3 &, ClosestResult &)> region_op =
    region_query_func;
  return occupancyQueryRegionsParallel(map, query, closest, query_min_extents, query_max_extents, region_op);
}
}  // namespace ohm

#endif  // OCCUPANCYQUERYALG_H


================================================
FILE: ohm/private/QueryDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_QUERYDETAIL_H
#define OHM_QUERYDETAIL_H

#include "OhmConfig.h"

#include "ohm/Key.h"

#include <limits>
#include <vector>

namespace ohm
{
class OccupancyMap;

/// Pimpl data for @c Query objects .
struct ohm_API QueryDetail
{
  /// The map to perform the query on.
  OccupancyMap *map = nullptr;
  /// The voxels intersected by the query, identified by their @c Key (if used).
  std::vector<Key> intersected_voxels;
  /// Distances associated with the query results (if used).
  std::vector<double> ranges;
  /// Number of results in @c intersected_voxels and/or @c ranges .
  size_t number_of_results = 0;
  /// @c QueryFlag values for the query.
  unsigned query_flags = 0;

  /// Virtual destructor.
  virtual ~QueryDetail() = default;
};

/// Query result helper identifying the closest result index.
struct ohm_API ClosestResult
{
  size_t index = 0;                                   ///< Index into the results arrays.
  double range = std::numeric_limits<double>::max();  ///< (Closest) range value.
};
}  // namespace ohm

#endif  // OHM_QUERYDETAIL_H


================================================
FILE: ohm/private/RayPatternDetail.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef RAYPATTERNDETAIL_H
#define RAYPATTERNDETAIL_H

#include "OhmConfig.h"

#include <glm/glm.hpp>

#include <vector>

namespace ohm
{
struct RayPatternDetail
{
  /// Ray start/end point pairs in sensor space.
  std::vector<glm::dvec3> sample_pairs;
};
}  // namespace ohm

#endif  // RAYPATTERNDETAIL_H


================================================
FILE: ohm/private/RaysQueryDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_RAYSQUERYDETAIL_H
#define OHM_RAYSQUERYDETAIL_H

#include "OhmConfig.h"

#include "QueryDetail.h"

#include <ohm/OccupancyType.h>

#include <cmath>

namespace ohm
{
struct ohm_API RaysQueryDetail : QueryDetail
{
  /// Set of origin/end point pairs to lookup in the map.
  std::vector<glm::dvec3> rays_in;
  std::vector<double> unobserved_volumes_out;
  std::vector<OccupancyType> terminal_states_out;
  double volume_coefficient = 1.0f;
  int occupancy_layer = -1;              ///< Cached occupancy layer index.
  glm::u8vec3 occupancy_dim{ 0, 0, 0 };  ///< Cached occupancy layer voxel dimensions. Voxel mean must exactly match.
  bool valid_layers = false;             ///< Has layer validation passed?
};
}  // namespace ohm

#endif  // OHM_RAYSQUERYDETAIL_H


================================================
FILE: ohm/private/SerialiseUtil.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef SERIALISEUTIL_H
#define SERIALISEUTIL_H

#include "OhmConfig.h"

#include "ohm/Stream.h"

namespace ohm
{
/// Explicitly typed stream writing, uncompressed.
template <typename T, typename S>
inline bool writeUncompressed(OutputStream &stream, const S &val)
{
  const T val2 = static_cast<T>(val);
  return stream.writeUncompressed(&val2, unsigned(sizeof(val2))) == sizeof(val2);
}


/// Explicitly typed stream writing.
template <typename T, typename S>
inline bool write(OutputStream &stream, const S &val)
{
  const T val2 = static_cast<T>(val);
  return stream.write(&val2, unsigned(sizeof(val2))) == sizeof(val2);
}


/// Explicitly typed stream reading, uncompressed.
template <typename T, typename S>
inline bool readRaw(InputStream &stream, S &val)
{
  T val2{ 0 };
  if (stream.readRaw(&val2, unsigned(sizeof(val2))) != sizeof(val2))
  {
    return false;
  }
  val = static_cast<S>(val2);
  return true;
}


/// Explicitly typed stream reading.
template <typename T, typename S>
inline bool read(InputStream &stream, S &val)
{
  T val2{ 0 };
  if (stream.read(&val2, unsigned(sizeof(val2))) != sizeof(val2))
  {
    return false;
  }
  val = static_cast<S>(val2);
  return true;
}
}  // namespace ohm

#endif  // SERIALISEUTIL_H


================================================
FILE: ohm/private/VoxelAlgorithms.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "VoxelAlgorithms.h"

#include "Key.h"
#include "OccupancyMap.h"
#include "VoxelData.h"

#include <limits>

namespace ohm
{
glm::ivec3 calculateVoxelSearchHalfExtents(const OccupancyMap &map, float search_radius)
{
  return glm::ivec3(int(std::ceil(search_radius / map.resolution())));
}


float calculateNearestNeighbour(const Key &voxel_key, const OccupancyMap &map,
                                const glm::ivec3 &voxel_search_half_extents, bool unobserved_as_occupied,
                                bool ignore_self, float search_range, const glm::vec3 &axis_scaling,
                                bool report_unscaled_distance)
{
  Key search_key;
  Voxel<const float> test_voxel(&map, map.layout().occupancyLayer());
  glm::vec3 voxel_centre;
  glm::vec3 separation;

  float scaled_range_sqr;
  float scaled_closest_range_sqr;
  float range_sqr;
  float closest_range_sqr;

  scaled_closest_range_sqr = closest_range_sqr = std::numeric_limits<float>::infinity();

  voxel_centre = map.voxelCentreLocal(voxel_key);

  test_voxel.setKey(voxel_key);
  // First try early out if the target voxel is occupied.
  if (!ignore_self &&
      (test_voxel.isValid() && isOccupied(test_voxel) || unobserved_as_occupied && isUnobservedOrNull(test_voxel)))
  {
    return 0.0f;
  }

  for (int z = -voxel_search_half_extents.z; z <= voxel_search_half_extents.z; ++z)
  {
    for (int y = -voxel_search_half_extents.y; y <= voxel_search_half_extents.y; ++y)
    {
      for (int x = -voxel_search_half_extents.x; x <= voxel_search_half_extents.x; ++x)
      {
        search_key = voxel_key;
        map.moveKey(search_key, x, y, z);
        test_voxel.setKey(search_key);

        if (ignore_self && x == 0 && y == 0 && z == 0)
        {
          continue;
        }

        if (test_voxel.isValid() && isOccupied(test_voxel) || unobserved_as_occupied && isUnobservedOrNull(test_voxel))
        {
          separation = glm::vec3(map.voxelCentreLocal(search_key)) - voxel_centre;
          range_sqr = glm::dot(separation, separation);
          separation.x *= axis_scaling.x;
          separation.y *= axis_scaling.y;
          separation.z *= axis_scaling.z;
          scaled_range_sqr = glm::dot(separation, separation);
          if (!report_unscaled_distance)
          {
            range_sqr = scaled_range_sqr;
          }

          // Should this be scaledRangeSqr <= searchRange * searchRange?
          if (search_range == 0 || range_sqr <= search_range * search_range)
          {
            if (scaled_range_sqr < scaled_closest_range_sqr)
            {
              closest_range_sqr = range_sqr;
              scaled_closest_range_sqr = scaled_range_sqr;
            }
          }
        }
      }
    }
  }

  if (closest_range_sqr < std::numeric_limits<float>::infinity())
  {
    return std::sqrt(float(closest_range_sqr));
  }

  return -1.0f;
}
}  // namespace ohm


================================================
FILE: ohm/private/VoxelAlgorithms.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_NODEALGORITHMS_H
#define OHM_NODEALGORITHMS_H

#include "OhmConfig.h"

#include <glm/glm.hpp>

namespace ohm
{
class Key;
class OccupancyMap;

/// A utility function for calculating the @c voxel_search_half_extents parameter for @c calculateNearestNeighbour.
/// @param map The map being searched.
/// @param search_radius The search radius of interest.
/// @return The calculated @c voxel_search_half_extents argument.
glm::ivec3 ohm_API calculateVoxelSearchHalfExtents(const OccupancyMap &map, float search_radius);

/// Search for the nearest occupied neighbour of @p voxel and return the range.
/// @param voxel_key The voxel we are operating on.
/// @param map The map to which @p voxel belongs.
/// @param voxel_search_half_extents Defines how far along each axis to search neighbours for in both + and -
/// directions.
/// @param unobserved_as_occupied Treat unknown/unexplored voxels as occupied?
/// @param ignore_self Only consider neighbours. Ignore the voxel itself if it is occupied.
/// @param search_range Radius to search and report.
/// @param axis_scaling Scaling applied along each axis to distort the search space.
/// @param report_unscaled_distance Set to true in order to report the result without applying @c axis_scaling.
///     The same voxel is selected regardless of this value.
/// @return -1 if there are no occupied voxels within the @p voxel_search_half_extents range, or the range of
///   the nearest obstacle. Zero when @p voxel itself is occupied.
float ohm_API calculateNearestNeighbour(const Key &voxel_key, const OccupancyMap &map,
                                        const glm::ivec3 &voxel_search_half_extents, bool unobserved_as_occupied,
                                        bool ignore_self, float search_range = 0,
                                        const glm::vec3 &axis_scaling = glm::vec3(1.0f, 1.0f, 1.0f),
                                        bool report_unscaled_distance = false);
}  // namespace ohm

#endif  // OHM_NODEALGORITHMS_H


================================================
FILE: ohm/private/VoxelBlockCompressionQueueDetail.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef VOXELMAPCOMPRESSIONQUEUEDETAIL_H
#define VOXELMAPCOMPRESSIONQUEUEDETAIL_H

#include "OhmConfig.h"

#include "Mutex.h"

#ifdef OHM_FEATURE_THREADS
#include <tbb/concurrent_queue.h>
#else  // OHM_FEATURE_THREADS
#include <queue>
#endif  // OHM_FEATURE_THREADS

#include <atomic>
#include <condition_variable>
#include <deque>
#include <mutex>
#include <thread>
#include <unordered_map>

namespace ohm
{
class VoxelBlock;

/// Data structure used to track the blocks available for compression.
struct CompressionEntry
{
  VoxelBlock *voxels;      ///< [Compression enabled] voxel block.
  size_t allocation_size;  ///< Last calculated allocation size.
};

/// @c VoxelBlockCompressionQueue internals.
struct VoxelBlockCompressionQueueDetail
{
  using Mutex = ohm::Mutex;
  /// Reference count mutex.
  Mutex ref_lock;
#ifdef OHM_FEATURE_THREADS
  /// Queue used to push @c VoxelBlock candidates for compression.
  tbb::concurrent_queue<VoxelBlock *> compression_queue;
#else   // OHM_FEATURE_THREADS
  /// Mutex for @c compression_queue
  ohm::Mutex queue_lock;
  /// Queue used to push @c VoxelBlock candidates for compression.
  std::queue<VoxelBlock *> compression_queue;
#endif  // OHM_FEATURE_THREADS
  /// Full set of registered @c VoxelBlock items.
  std::vector<CompressionEntry> blocks;
  /// High tide to initiate compression at.
  std::atomic_uint64_t high_tide{ 12ull * 1024ull * 1024ull * 1024ull };
  /// Low tide to compression down to.
  std::atomic_uint64_t low_tide{ 6ull * 1024ull * 1024ull * 1024ull };
  /// Current allocation estimation.
  std::atomic_uint64_t estimated_allocated_size{ 0 };
  /// Thread reference count.
  std::atomic_int reference_count{ 0 };
  /// Thread quit flag.
  std::atomic_bool quit_flag{ false };
  /// Processing thread.
  std::thread processing_thread;
  /// True if @c processing_thread is running.
  bool running{ false };
  /// Set when instantiated for testing.
  bool test_mode{ false };
};

inline void push(VoxelBlockCompressionQueueDetail &detail, VoxelBlock *block)
{
#ifdef OHM_FEATURE_THREADS
  detail.compression_queue.push(block);
#else   // OHM_FEATURE_THREADS
  std::unique_lock<ohm::Mutex> guard(detail.queue_lock);
  detail.compression_queue.emplace(block);
#endif  // OHM_FEATURE_THREADS
}

inline bool tryPop(VoxelBlockCompressionQueueDetail &detail, VoxelBlock **block)
{
#ifdef OHM_FEATURE_THREADS
  return detail.compression_queue.try_pop(*block);
#else   // OHM_FEATURE_THREADS
  std::unique_lock<ohm::Mutex> guard(detail.queue_lock);
  if (!detail.compression_queue.empty())
  {
    *block = detail.compression_queue.front();
    detail.compression_queue.pop();
    return true;
  }

  return false;
#endif  // OHM_FEATURE_THREADS
}
}  // namespace ohm

#endif  // VOXELMAPCOMPRESSIONQUEUEDETAIL_H


================================================
FILE: ohm/private/VoxelLayoutDetail.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_VOXELLAYOUTDETAIL_H
#define OHM_VOXELLAYOUTDETAIL_H

#include "OhmConfig.h"

#include <array>
#include <vector>

namespace ohm
{
struct ohm_API VoxelMember
{
  /// Voxel member name field. Sized to set the overall structure size to 64 bytes.
  std::array<char, 52> name;  // NOLINT(readability-magic-numbers)
  uint64_t clear_value;
  uint16_t type;
  uint16_t offset;
};

struct ohm_API VoxelLayoutDetail
{
  std::vector<VoxelMember> members;
  uint16_t next_offset = 0u;
  uint16_t voxel_byte_size = 0u;
};
}  // namespace ohm

#endif  // OHM_VOXELLAYOUTDETAIL_H


================================================
FILE: ohm/serialise/MapSerialiseV0.1.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapSerialiseV0.1.h"

#include "private/OccupancyMapDetail.h"
#include "private/SerialiseUtil.h"

#include "DefaultLayer.h"
#include "MapChunk.h"
#include "MapLayer.h"
#include "MapSerialise.h"
#include "Stream.h"
#include "VoxelBlock.h"
#include "VoxelBuffer.h"

namespace ohm
{
namespace v0_1
{
int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count)
{
  return load(stream, detail, progress, version, region_count, v0_1::loadChunk);
}

int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion & /*version*/,
         size_t region_count, const ChunkFunc &load_chunk)
{
  int err = loadLayout(stream, detail);
  if (err)
  {
    return err;
  }

  if (progress)
  {
    if (region_count)
    {
      progress->setTargetProgress(unsigned(region_count));
    }
    else
    {
      progress->setTargetProgress(unsigned(1));
      progress->incrementProgress();
    }
  }

  MapChunk *chunk = nullptr;
  for (unsigned i = 0; i < region_count && (!progress || !progress->quit()); ++i)
  {
    chunk = new MapChunk(detail);
    err = load_chunk(stream, *chunk, detail);
    if (err)
    {
      delete chunk;
      return err;
    }

    // Resolve map chunk details.
    chunk->searchAndUpdateFirstValid(detail.region_voxel_dimensions);
    detail.chunks.insert(std::make_pair(chunk->region.coord, chunk));

    if (progress)
    {
      progress->incrementProgress();
    }
  }

  return err;
}


int loadLayout(InputStream &stream, OccupancyMapDetail &map)
{
  // Save details about the map layers.
  MapLayout &layout = map.layout;
  bool ok = true;
  uint32_t layer_count = 0;
  std::vector<char> layer_name;
  std::vector<char> member_name;
  uint32_t len;
  uint32_t layer_flags;
  uint16_t subsampling;

  layout.clear();

  ok = read<int32_t>(stream, layer_count) && ok;

  for (unsigned i = 0; ok && i < layer_count; ++i)
  {
    // Read the layer name.
    len = 0;
    ok = read<uint32_t>(stream, len) && ok;

    layer_name.resize(len + 1u);  // NOLINT
    ok = stream.read(layer_name.data(), len) == len && ok;
    layer_name[len] = '\0';

    // Read flags.
    layer_flags = 0;
    ok = read<uint32_t>(stream, layer_flags) && ok;

    // Read the subsampling
    subsampling = 0;
    ok = read<uint16_t>(stream, subsampling) && ok;

    MapLayer *layer = layout.addLayer(layer_name.data(), subsampling);

    // Read voxel layout.
    VoxelLayout voxel_layout = layer->voxelLayout();
    // Read voxel size.
    uint32_t voxel_size = 0;
    ok = read<uint32_t>(stream, voxel_size) && ok;
    // Read member count.
    uint32_t voxel_member_count = 0;
    ok = read<uint32_t>(stream, voxel_member_count) && ok;
    for (size_t j = 0; j < voxel_member_count; ++j)
    {
      // Read member name.
      len = 0;
      ok = read<uint32_t>(stream, len) && ok;

      member_name.resize(len + 1u);  // NOLINT
      ok = stream.read(member_name.data(), len) == len && ok;
      member_name[len] = '\0';

      // Read member type, offset and clear value.
      uint16_t type = 0;
      uint16_t offset = 0;
      uint64_t clear_value = 0;
      ok = read<uint16_t>(stream, type) && ok;
      ok = read<uint16_t>(stream, offset) && ok;
      ok = read<uint64_t>(stream, clear_value) && ok;

      // Add the data member.
      if (ok && type)
      {
        voxel_layout.addMember(member_name.data(), DataType::Type(type), clear_value);
        if (voxel_layout.memberOffset(j) != offset)
        {
          // ok = false;
          return kSeMemberOffsetError;
        }
      }
    }
  }

  return (ok) ? 0 : kSeFileReadFailure;
}


int loadChunk(InputStream &stream, MapChunk &chunk, const OccupancyMapDetail &detail)
{
  bool ok = true;

  // Write region details, then nodes. MapChunk members are derived.
  ok = read<int32_t>(stream, chunk.region.coord.x) && ok;
  ok = read<int32_t>(stream, chunk.region.coord.y) && ok;
  ok = read<int32_t>(stream, chunk.region.coord.z) && ok;
  ok = read<double>(stream, chunk.region.centre.x) && ok;
  ok = read<double>(stream, chunk.region.centre.y) && ok;
  ok = read<double>(stream, chunk.region.centre.z) && ok;
  ok = read<double>(stream, chunk.touched_time) && ok;

  if (ok)
  {
    const MapLayout &layout = detail.layout;
    for (size_t i = 0; i < layout.layerCount(); ++i)
    {
      const MapLayer &layer = layout.layer(i);
      VoxelBuffer<VoxelBlock> voxel_buffer(chunk.voxel_blocks[i]);
      uint8_t *layer_mem = voxel_buffer.voxelMemory();

      if (layer.flags() & MapLayer::kSkipSerialise)
      {
        // Not to be serialised. Clear instead.
        layer.clear(layer_mem, detail.region_voxel_dimensions);
        continue;
      }

      const size_t node_count = layer.volume(detail.region_voxel_dimensions);
      const size_t node_byte_count = layer.voxelByteSize() * node_count;
      if (node_byte_count != unsigned(node_byte_count))
      {
        return kSeValueOverflow;
      }

      ok = stream.read(layer_mem, unsigned(node_byte_count)) == node_byte_count && ok;
    }
  }

  return (ok) ? 0 : kSeFileReadFailure;
}
}  // namespace v0_1
}  // namespace ohm

================================================
FILE: ohm/serialise/MapSerialiseV0.1.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef MAPSERIALISEV0_1_H
#define MAPSERIALISEV0_1_H

#include "OhmConfig.h"

#include <functional>

namespace ohm
{
class InputStream;
struct MapChunk;
struct MapVersion;
struct OccupancyMapDetail;
class SerialiseProgress;

using ChunkFunc = std::function<int(InputStream &stream, MapChunk &, const OccupancyMapDetail &)>;

namespace v0_1
{
int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count);
int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count, const ChunkFunc &load_chunk);
int loadLayout(InputStream &stream, OccupancyMapDetail &map);
int loadChunk(InputStream &stream, MapChunk &chunk, const OccupancyMapDetail &detail);
}  // namespace v0_1
}  // namespace ohm

#endif  // MAPSERIALISEV0_1_H


================================================
FILE: ohm/serialise/MapSerialiseV0.2.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapSerialiseV0.2.h"

#include "private/OccupancyMapDetail.h"
#include "private/SerialiseUtil.h"

#include "DefaultLayer.h"
#include "MapChunk.h"
#include "MapInfo.h"
#include "MapLayer.h"
#include "MapSerialise.h"
#include "Stream.h"

namespace ohm
{
namespace v0_2
{
int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count)
{
  return load(stream, detail, progress, version, region_count, &v0_1::loadChunk);
}

int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count, const ChunkFunc &load_chunk)
{
  // From version 0.2 we have MapInfo.
  int err = loadMapInfo(stream, detail.info);
  if (err)
  {
    return err;
  }

  return v0_1::load(stream, detail, progress, version, region_count, load_chunk);
}


int loadMapInfo(InputStream &in, MapInfo &info)  //, const bool endianSwap)
{
  uint32_t item_count = 0;
  info.clear();

  if (!readRaw<uint32_t>(in, item_count))
  {
    return kSeInfoError;
  }

  if (!item_count)
  {
    return kSeOk;
  }

  int err = 0;
  for (unsigned i = 0; i < item_count; ++i)
  {
    MapValue value;
    err = loadItem(in, value);  //, endianSwap);
    if (err != kSeOk)
    {
      return err;
    }
    info.set(value);
  }

  return kSeOk;
}


int loadItem(InputStream &in, MapValue &value)  //, const bool endianSwap)
{
  uint16_t len16;
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  in.read(reinterpret_cast<char *>(&len16), sizeof(len16));
  // if (endianSwap)
  // {
  //   endian::endianSwap(&len16);
  // }

  char *str = new char[len16 + 1];
  if (len16)
  {
    in.read(str, len16);
  }
  str[len16] = '\0';
  value.setName(str);
  delete[] str;

  uint8_t type;
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  in.read(reinterpret_cast<char *>(&type), 1);

  switch (type)
  {
  case MapValue::kInt8: {
    int8_t val;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&val), 1);
    value = val;
    break;
  }
  case MapValue::kUInt8: {
    uint8_t val;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&val), 1);
    value = val;
    break;
  }
  case MapValue::kInt16: {
    int16_t val;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&val), sizeof(val));
    // if (endianSwap) { endian::endianSwap(&val); }
    value = val;
    break;
  }
  case MapValue::kUInt16: {
    uint16_t val;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&val), sizeof(val));
    // if (endianSwap) { endian::endianSwap(&val); }
    value = val;
    break;
  }
  case MapValue::kInt32: {
    int32_t val;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&val), sizeof(val));
    // if (endianSwap) { endian::endianSwap(&val); }
    value = val;
    break;
  }
  case MapValue::kUInt32: {
    uint32_t val;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&val), sizeof(val));
    // if (endianSwap) { endian::endianSwap(&val); }
    value = val;
    break;
  }
  case MapValue::kInt64: {
    int64_t val;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&val), sizeof(val));
    // if (endianSwap) { endian::endianSwap(&val); }
    value = val;
    break;
  }
  case MapValue::kUInt64: {
    uint64_t val;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&val), sizeof(val));
    // if (endianSwap) { endian::endianSwap(&val); }
    value = val;
    break;
  }
  case MapValue::kFloat32: {
    float val;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&val), sizeof(val));
    // if (endianSwap) { endian::endianSwap(&val); }
    value = val;
    break;
  }
  case MapValue::kFloat64: {
    double val;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&val), sizeof(val));
    // if (endianSwap) { endian::endianSwap(&val); }
    value = val;
    break;
  }
  case MapValue::kBoolean: {
    uint8_t val;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&val), 1);
    if (val)
    {
      value = true;
    }
    else
    {
      value = false;
    }
    break;
  }
  case MapValue::kString: {
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    in.read(reinterpret_cast<char *>(&len16), sizeof(len16));
    // if (endianSwap)
    // {
    //   endian::endianSwap(&len16);
    // }

    char *str = new char[len16 + 1];
    if (len16)
    {
      in.read(str, len16);
    }
    str[len16] = '\0';
    value = str;
    delete[] str;
    break;
  }

  default:
    return kSeUnknownDataType;
  }

  //{
  //  MapValue strValue = value.toStringValue();
  //  fprintf(stderr, "r: %s = %s\n", value.name(), static_cast<const char *>(strValue));
  //}
  return kSeOk;
}
}  // namespace v0_2
}  // namespace ohm

================================================
FILE: ohm/serialise/MapSerialiseV0.2.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef MAPSERIALISEV0_2_H
#define MAPSERIALISEV0_2_H

#include "OhmConfig.h"

#include "MapSerialiseV0.1.h"

namespace ohm
{
class InputStream;
struct MapChunk;
class MapInfo;
class MapValue;
struct MapVersion;
struct OccupancyMapDetail;
class SerialiseProgress;

namespace v0_2
{
int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count);
int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count, const ChunkFunc &load_chunk);

int loadMapInfo(InputStream &in, MapInfo &info);
int loadItem(InputStream &in, MapValue &value);
}  // namespace v0_2
}  // namespace ohm

#endif  // MAPSERIALISEV0_2_H


================================================
FILE: ohm/serialise/MapSerialiseV0.4.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapSerialiseV0.4.h"

#include "MapSerialiseV0.2.h"

#include "private/OccupancyMapDetail.h"
#include "private/SerialiseUtil.h"

#include "DefaultLayer.h"
#include "MapChunk.h"
#include "MapLayer.h"
#include "MapSerialise.h"
#include "Stream.h"
#include "VoxelBlock.h"
#include "VoxelBuffer.h"

namespace ohm
{
namespace v0_4
{
int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count)
{
  return load(stream, detail, progress, version, region_count, &v0_4::loadChunk);
}

int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count, const ChunkFunc &load_chunk)
{
  return v0_2::load(stream, detail, progress, version, region_count, load_chunk);
}

int loadChunk(InputStream &stream, MapChunk &chunk, const OccupancyMapDetail &detail)
{
  bool ok = true;

  // Write region details, then nodes. MapChunk members are derived.
  ok = read<int32_t>(stream, chunk.region.coord.x) && ok;
  ok = read<int32_t>(stream, chunk.region.coord.y) && ok;
  ok = read<int32_t>(stream, chunk.region.coord.z) && ok;
  ok = read<double>(stream, chunk.region.centre.x) && ok;
  ok = read<double>(stream, chunk.region.centre.y) && ok;
  ok = read<double>(stream, chunk.region.centre.z) && ok;
  ok = read<double>(stream, chunk.touched_time) && ok;

  if (ok)
  {
    const MapLayout &layout = detail.layout;
    for (size_t i = 0; i < layout.layerCount(); ++i)
    {
      const MapLayer &layer = layout.layer(i);
      VoxelBuffer<VoxelBlock> voxel_buffer(chunk.voxel_blocks[i]);
      // Get the layer memory.
      uint8_t *layer_mem = voxel_buffer.voxelMemory();

      if (layer.flags() & MapLayer::kSkipSerialise)
      {
        // Not to be serialised. Clear instead.
        layer.clear(layer_mem, detail.region_voxel_dimensions);
        continue;
      }

      uint64_t layer_touched_stamp = 0;
      ok = read<uint64_t>(stream, layer_touched_stamp) && ok;

      chunk.touched_stamps[i] = layer_touched_stamp;

      const size_t node_count = layer.volume(detail.region_voxel_dimensions);
      const size_t node_byte_count = layer.voxelByteSize() * node_count;
      if (node_byte_count != unsigned(node_byte_count))
      {
        return kSeValueOverflow;
      }

      ok = stream.read(layer_mem, unsigned(node_byte_count)) == node_byte_count && ok;
    }
  }

  return (ok) ? 0 : kSeFileReadFailure;
}
}  // namespace v0_4
}  // namespace ohm

================================================
FILE: ohm/serialise/MapSerialiseV0.4.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef MAPSERIALISEV0_4_H
#define MAPSERIALISEV0_4_H

#include "OhmConfig.h"

#include "MapSerialiseV0.1.h"

namespace ohm
{
class InputStream;
struct MapChunk;
struct MapVersion;
struct OccupancyMapDetail;
class SerialiseProgress;

namespace v0_4
{
int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count);

int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count, const ChunkFunc &load_chunk);

int loadChunk(InputStream &stream, MapChunk &chunk, const OccupancyMapDetail &detail);
}  // namespace v0_4
}  // namespace ohm

#endif  // MAPSERIALISEV0_3_H


================================================
FILE: ohm/serialise/MapSerialiseV0.5.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapSerialiseV0.5.h"


================================================
FILE: ohm/serialise/MapSerialiseV0.5.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef MAPSERIALISEV0_5_H
#define MAPSERIALISEV0_5_H

#include "OhmConfig.h"

#include "MapSerialiseV0.4.h"

namespace ohm
{
namespace v0_5
{
inline int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
                size_t region_count)
{
  return v0_4::load(stream, detail, progress, version, region_count);
}
}  // namespace v0_5
}  // namespace ohm

#endif  // MAPSERIALISEV0_3_H


================================================
FILE: ohm/serialise/MapSerialiseV0.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapSerialiseV0.h"

#include "private/OccupancyMapDetail.h"
#include "private/SerialiseUtil.h"

#include "DefaultLayer.h"
#include "MapChunk.h"
#include "MapFlag.h"
#include "MapLayer.h"
#include "MapSerialise.h"
#include "Stream.h"
#include "VoxelBlock.h"
#include "VoxelBuffer.h"

#include <cstring>

namespace ohm
{
namespace v0
{
int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion & /*version*/,
         size_t region_count)
{
  int err = kSeOk;

  detail.setDefaultLayout(MapFlag::kNone);
  addClearance(detail.layout);

  if (progress)
  {
    if (region_count)
    {
      progress->setTargetProgress(unsigned(region_count));
    }
    else
    {
      progress->setTargetProgress(unsigned(1));
      progress->incrementProgress();
    }
  }

  MapChunk *chunk = nullptr;
  for (unsigned i = 0; i < region_count && (!progress || !progress->quit()); ++i)
  {
    chunk = new MapChunk(detail);
    err = v0::loadChunk(stream, *chunk, detail);
    if (err)
    {
      delete chunk;
      return err;
    }

    // Resolve map chunk details.
    chunk->searchAndUpdateFirstValid(detail.region_voxel_dimensions);
    detail.chunks.insert(std::make_pair(chunk->region.coord, chunk));

    if (progress)
    {
      progress->incrementProgress();
    }
  }

  return err;
}


// Version zero chunk loading
int loadChunk(InputStream &stream, MapChunk &chunk, const OccupancyMapDetail &detail)
{
  bool ok = true;

  const MapLayer *occupancy_layer = chunk.layout().layerPtr(chunk.layout().occupancyLayer());
  const MapLayer *clearance_layer = chunk.layout().layerPtr(chunk.layout().clearanceLayer());
  // Use a hard coded reference to the legacy layer "coarseClearance". The layer was never used anywhere.
  const MapLayer *coarse_clearance_layer = chunk.layout().layer("coarseClearance");

  if (coarse_clearance_layer)
  {
    VoxelBuffer<VoxelBlock> voxel_buffer(chunk.voxel_blocks[coarse_clearance_layer->layerIndex()]);
    memset(voxel_buffer.voxelMemory(), 0, voxel_buffer.voxelMemorySize());
  }

  // Write region details, then nodes. MapChunk members are derived.
  ok = read<int32_t>(stream, chunk.region.coord.x) && ok;
  ok = read<int32_t>(stream, chunk.region.coord.y) && ok;
  ok = read<int32_t>(stream, chunk.region.coord.z) && ok;
  ok = read<double>(stream, chunk.region.centre.x) && ok;
  ok = read<double>(stream, chunk.region.centre.y) && ok;
  ok = read<double>(stream, chunk.region.centre.z) && ok;
  ok = read<double>(stream, chunk.touched_time) && ok;

  const unsigned node_count =
    detail.region_voxel_dimensions.x * detail.region_voxel_dimensions.y * detail.region_voxel_dimensions.z;
  const size_t node_byte_count = 2 * sizeof(float) * node_count;
  if (node_byte_count != unsigned(node_byte_count))
  {
    return kSeValueOverflow;
  }

  if (ok)
  {
    // Initial version used MapNode which contained two floats.
    // This interleaves occupancy/clearance, so we need to pull them out.
    std::vector<float> node_data(node_count * 2);
    ok = stream.read(node_data.data(), unsigned(node_byte_count)) == node_byte_count && ok;

    VoxelBuffer<VoxelBlock> occupancy_buffer(chunk.voxel_blocks[occupancy_layer->layerIndex()]);
    VoxelBuffer<VoxelBlock> clearance_buffer(chunk.voxel_blocks[clearance_layer->layerIndex()]);

    for (unsigned i = 0; i < unsigned(node_data.size() / 2); ++i)
    {
      // Unnecessary casting for clang-tidy
      occupancy_buffer.writeVoxel(i, node_data[unsigned((i << 1u) + 0u)]);
      clearance_buffer.writeVoxel(i, node_data[unsigned((i << 1u) + 1u)]);
    }
  }

  return (ok) ? 0 : kSeFileReadFailure;
}
}  // namespace v0
}  // namespace ohm


================================================
FILE: ohm/serialise/MapSerialiseV0.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef MAPSERIALISEV0_H
#define MAPSERIALISEV0_H

#include "OhmConfig.h"

namespace ohm
{
class InputStream;
struct MapChunk;
struct MapVersion;
struct OccupancyMapDetail;
class SerialiseProgress;

namespace v0
{
int load(InputStream &stream, OccupancyMapDetail &detail, SerialiseProgress *progress, const MapVersion &version,
         size_t region_count);
int loadChunk(InputStream &stream, MapChunk &chunk, const OccupancyMapDetail &detail);
}  // namespace v0
}  // namespace ohm

#endif  // MAPSERIALISEV0_H


================================================
FILE: ohm.natvis
================================================
<?xml version="1.0" encoding="utf-8"?>
<AutoVisualizer xmlns="http://schemas.microsoft.com/vstudio/debugger/natvis/2010">
  <Type Name="glm::vec&lt;2,char,*&gt;">
    <AlternativeType Name="glm::vec&lt;2,unsigned char,*&gt;"/>
    <AlternativeType Name="glm::u8vec2"/>
    <AlternativeType Name="glm::i8vec2"/>
    <DisplayString>x={(int)x} y={(int)y} z={(int)z}</DisplayString>
    <Expand>
      <Item Name="x">(int)x</Item>
      <Item Name="y">(int)y</Item>
      <Item Name="z">(int)z</Item>
      <Item Name="len2">(int)x*(int)x + (int)y*(int)y + (int)z*(int)z</Item>
    </Expand>
  </Type>

  <Type Name="glm::vec&lt;3,char,*&gt;">
    <AlternativeType Name="glm::vec&lt;3,unsigned char,*&gt;"/>
    <AlternativeType Name="glm::u8vec3"/>
    <AlternativeType Name="glm::i8vec3"/>
    <DisplayString>x={(int)x} y={(int)y} z={(int)z}</DisplayString>
    <Expand>
      <Item Name="x">(int)x</Item>
      <Item Name="y">(int)y</Item>
      <Item Name="z">(int)z</Item>
      <Item Name="len2">(int)x*(int)x + (int)y*(int)y + (int)z*(int)z</Item>
    </Expand>
  </Type>

  <Type Name="glm::vec&lt;4,char,*&gt;">
    <AlternativeType Name="glm::vec&lt;4,unsigned char,*&gt;"/>
    <AlternativeType Name="glm::u8vec4"/>
    <AlternativeType Name="glm::i8vec4"/>
    <DisplayString>x={(int)x} y={(int)y} z={(int)z}</DisplayString>
    <Expand>
      <Item Name="x">(int)x</Item>
      <Item Name="y">(int)y</Item>
      <Item Name="z">(int)z</Item>
      <Item Name="len2">(int)x*(int)x + (int)y*(int)y + (int)z*(int)z</Item>
    </Expand>
  </Type>

  <Type Name="glm::vec&lt;2,*&gt;">
    <AlternativeType Name="glm::vec2"/>
    <AlternativeType Name="glm::dvec2"/>
    <AlternativeType Name="glm::i16vec2"/>
    <AlternativeType Name="glm::ivec2"/>
    <AlternativeType Name="glm::u16vec2"/>
    <AlternativeType Name="glm::uvec2"/>
    <DisplayString>x={x} y={y} z={z}</DisplayString>
    <Expand>
      <Item Name="x">x</Item>
      <Item Name="y">y</Item>
      <Item Name="z">z</Item>
      <Item Name="len2">x*x + y*y + z*z</Item>
    </Expand>
  </Type>

  <Type Name="glm::vec&lt;3,*&gt;">
    <AlternativeType Name="glm::vec3"/>
    <AlternativeType Name="glm::dvec3"/>
    <AlternativeType Name="glm::i16vec3"/>
    <AlternativeType Name="glm::ivec3"/>
    <AlternativeType Name="glm::u16vec3"/>
    <AlternativeType Name="glm::uvec3"/>
    <DisplayString>x={x} y={y} z={z}</DisplayString>
    <Expand>
      <Item Name="x">x</Item>
      <Item Name="y">y</Item>
      <Item Name="z">z</Item>
      <Item Name="len2">x*x + y*y + z*z</Item>
    </Expand>
  </Type>

  <Type Name="glm::vec&lt;4,*&gt;">
    <AlternativeType Name="glm::vec4"/>
    <AlternativeType Name="glm::dvec4"/>
    <AlternativeType Name="glm::i16vec4"/>
    <AlternativeType Name="glm::ivec4"/>
    <AlternativeType Name="glm::u16vec4"/>
    <AlternativeType Name="glm::uvec4"/>
    <DisplayString>x={x} y={y} z={z}</DisplayString>
    <Expand>
      <Item Name="x">x</Item>
      <Item Name="y">y</Item>
      <Item Name="z">z</Item>
      <Item Name="len2">x*x + y*y + z*z</Item>
    </Expand>
  </Type>

  <Type Name="ohm::Key">
    <DisplayString Condition="local_.x==0 &amp;&amp; local_.y==0 &amp;&amp; local_.z==0 &amp;&amp; region_key_.x==-32768 &amp;&amp; region_key_.y==-32768 &amp;&amp; region_key_.z==-32768">(null-key)</DisplayString>
    <DisplayString>L:({(int)local_.x},{(int)local_.y},{(int)local_.z}) R:({region_key_.x},{region_key_.y},{region_key_.z})</DisplayString>
    <Expand>
      <Item Name="local_">local_</Item>
      <Item Name="region_key_">region_key_</Item>
    </Expand>
  </Type>

  <!--
    SKA hash map support: note, the correct solution is to use CustomListItems and code to decipher the elements.
    However, the VSCode MIEngine does not support Loop so we need an alternative. (Of course I learned this after I
    had it working).
    Also doesn't seem to work for GDB.
  -->
  <Type Name="ska::bytell_hash_map&lt;*&gt;">
    <DisplayString>items={num_elements}</DisplayString>
    <Expand>
      <!--
        The backup solution is to treat it like an indexed list and iterate the data blocks rather than the individual
        elements.

        Data begins at the `entries` pointer. There are a number of these blocks equal to the `num_slots_minus_one + 1`
        unless `num_slots_minus_one` is zero, in which case the container is empty.

        The data blocks can be expanded to look for valid data entries in the map, although there will be many
        malformed/invalid data items.
      -->
      <IndexListItems>
        <Size>(num_slots_minus_one ? num_slots_minus_one + 1 : 0)</Size>
        <ValueNode>
          entries[$i].data
        </ValueNode>
      </IndexListItems>
      <!--
        The correct solution is this one.
        This logic is based on the ska map iterator code.
        Note: If this is uncommented, the decrement operators need to be restored. They are currently written as
        `- -` because joining the dashes is malformed in an HTML comment. The extraneous space would need to be removed.
      -->
      <!-- <CustomListItems MaxItemsPerView="5000" ExcludeView="Test">
        <Variable Name="num_slots" InitialValue="(size_t)(num_slots_minus_one ? num_slots_minus_one + 1 : 0)"/>
        <Variable Name="BlockSize" InitialValue="ska::detailv8::CalculateBytellBlockSize&lt;$T1,$T2&gt;::value"/>
        <Variable Name="magic_for_empty" InitialValue="ska::detailv8::sherwood_v8_constants&lt;void&gt;::magic_for_empty"/>
        <Variable Name="index" InitialValue="num_slots"/>
        <Variable Name="current" InitialValue="entries + (num_slots / BlockSize)"/>
        <Variable Name="count" InitialValue="0"/>

        <Loop>
          <Loop>
            <If Condition="index % BlockSize == 0">
              <Exec>- -current</Exec>
            </If>
            <If Condition="index- - == 0">
              <Break/>
            </If>
            <Break Condition="current->control_bytes[index % BlockSize] != magic_for_empty"/>
          </Loop>
          <Item>&amp;current->data[index % BlockSize]</Item>
          <Break Condition="++count >= num_elements"/>
        </Loop>
      </CustomListItems> -->
    </Expand>
  </Type>

  <!-- SKA iterator item visualisation. Works for const and non-const. -->
  <Type Name="ska::detailv8::sherwood_v8_table&lt;std::pair&lt;*,*&gt;,*&gt;::templated_iterator&lt;*&gt;">
    <DisplayString>{current->data[index % ska::detailv8::CalculateBytellBlockSize&lt;$T1,$T1&gt;::value].first}:{current->data[index % ska::detailv8::CalculateBytellBlockSize&lt;$T1,$T1&gt;::value].second}</DisplayString>
    <Expand>
      <Item Name="[key]">current->data[index % ska::detailv8::CalculateBytellBlockSize&lt;$T1,$T1&gt;::value].first</Item>
      <Item Name="[value]">current->data[index % ska::detailv8::CalculateBytellBlockSize&lt;$T1,$T1&gt;::value].second</Item>
    </Expand>
  </Type>
</AutoVisualizer>


================================================
FILE: ohmapp/CMakeLists.txt
================================================

# Helper for building an application around ohm map generation.

include(GenerateExportHeader)

configure_file(OhmAppConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/ohmapp/OhmAppConfig.h")
configure_file(OhmAppGpuConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/ohmapp/OhmAppGpuConfig.h")

set(SOURCES
  DataSource.cpp
  DataSource.h
  MapHarness.cpp
  MapHarness.h
  OhmAppConfig.in.h
  OhmAppCpu.cpp
  OhmAppCpu.h
  ohmappmain.inl
  SlamIOSource.cpp
  SlamIOSource.h
)

set(SOURCES_GPU
  OhmAppGpu.cpp
  OhmAppGpu.h
)

set(PUBLIC_HEADERS
  DataSource.h
  MapHarness.h
  OhmAppCpu.h
  ohmappmain.inl
  SlamIOSource.h
  "${CMAKE_CURRENT_BINARY_DIR}/ohmapp/OhmAppConfig.h"
  "${CMAKE_CURRENT_BINARY_DIR}/ohmapp/OhmAppExport.h"
)

set(PUBLIC_HEADERS_GPU
  OhmAppGpu.h
  "${CMAKE_CURRENT_BINARY_DIR}/ohmapp/OhmAppGpuConfig.h"
  "${CMAKE_CURRENT_BINARY_DIR}/ohmapp/OhmAppGpuExport.h"
)

function(_ohmapp_setup_target TARGET_NAME OHM_GPU)
  # Because of the way we compile our GPU library twice with different names, we must explicitly define the export
  # macro. Curiously, there's a way to overide all the macros except the one used to control whether to export the
  # symbols or not. This puts us in a position where it could either be ohmcuda_EXPORTS or ohmocl_EXPORTS depending
  # on which targets are enabled. We build both the same way though, so define both symbols for all builds.
  if(OHM_GPU STREQUAL "cpu")
    target_compile_definitions(${TARGET_NAME} PRIVATE "-Dohmapp_EXPORTS")
    set(GPUTIL_LIBRARY)
    set(OHMGPU_LIBRARY)
    set(EXPORT_HEADER "OhmAppExport.h")
    set(EXPORT_PREFIX "ohmapp")
    set(EXPORT_STATIC "OHMAPP")
  else(OHM_GPU STREQUAL "cpu")
    # target_compile_definitions(${TARGET_NAME} PUBLIC "-DOHM_GPU=${OHM_GPU}")
    target_compile_definitions(${TARGET_NAME} PRIVATE "-Dohmapp_EXPORTS" "-Dohmcuda_EXPORTS" "-Dohmocl_EXPORTS")
    set(GPUTIL_LIBRARY gputil${OHM_GPU})
    set(OHMGPU_LIBRARY ohm${OHM_GPU})
    set(EXPORT_HEADER "OhmAppGpuExport.h")
    set(EXPORT_PREFIX "ohmappgpu")
    set(EXPORT_STATIC "OHMAPPGPU")
  endif(OHM_GPU STREQUAL "cpu")
  target_link_libraries(${TARGET_NAME}
    PUBLIC
      ${OHMGPU_LIBRARY}
    PRIVATE
      ${GPUTIL_LIBRARY})

  target_include_directories(${TARGET_NAME}
    PUBLIC
      $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}>
      $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
      $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/ohmapp>
      $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
  )

  clang_tidy_target(${TARGET_NAME})

  generate_export_header(${TARGET_NAME}
    EXPORT_MACRO_NAME "${EXPORT_PREFIX}_API"
    DEPRECATED_MACRO_NAME "${EXPORT_PREFIX}_DEPRECATED"
    NO_EXPORT_MACRO_NAME "${EXPORT_PREFIX}_NO_EXPORT"
    EXPORT_FILE_NAME "ohmapp/${EXPORT_HEADER}"
    NO_DEPRECATED_MACRO_NAME  "${EXPORT_PREFIX}_NO_DEPRECATED"
    STATIC_DEFINE "${EXPORT_STATIC}_STATIC")

  install(TARGETS ${TARGET_NAME} EXPORT ${CMAKE_PROJECT_NAME}-config-targets
    LIBRARY DESTINATION lib
    ARCHIVE DESTINATION lib
    RUNTIME DESTINATION bin
    INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/ohmapp
  )
endfunction(_ohmapp_setup_target)

add_library(ohmapp STATIC ${SOURCES})
_ohmapp_setup_target(ohmapp "cpu")
target_link_libraries(ohmapp PUBLIC ohm ohmutil ohmtools slamio)

# Add GPU sources and libarries
list(APPEND SOURCE OhmAppGpu.cpp OhmAppGpu.h)
if(OHM_FEATURE_CUDA)
  add_library(ohmappcuda STATIC ${SOURCES_GPU})
  _ohmapp_setup_target(ohmappcuda "cuda")
  target_link_libraries(ohmappcuda PUBLIC ohmapp)
endif(OHM_FEATURE_CUDA)
if(OHM_FEATURE_OPENCL)
  add_library(ohmappocl STATIC ${SOURCES_GPU})
  _ohmapp_setup_target(ohmappocl "ocl")
  target_link_libraries(ohmappocl PUBLIC ohmapp)
endif(OHM_FEATURE_OPENCL)

install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/ohmapp)

if(OHM_FEATURE_CUDA OR OHM_FEATURE_OPENCL)
  install(FILES ${PUBLIC_HEADERS_GPU} DESTINATION ${OHM_PREFIX_INCLUDE}/ohmapp)
endif(OHM_FEATURE_CUDA OR OHM_FEATURE_OPENCL)

source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})


================================================
FILE: ohmapp/DataSource.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "DataSource.h"

// Must be after argument streaming operators.
#include <ohmutil/Options.h>

#include <logutil/Logger.h>

using namespace ohm;

namespace ohmapp
{
DataSource::Options::~Options() = default;

void DataSource::Options::configure(cxxopts::Options &parser)
{
  cxxopts::OptionAdder adder = parser.add_options("Input");
  configure(adder);
}


void DataSource::Options::configure(cxxopts::OptionAdder &adder)
{
  // clang-format off
  adder
    ("point-limit", "Limit the number of points loaded.", optVal(point_limit))
    ("start-time", "Only process points time stamped later than the specified time.", optVal(start_time))
    ("time-limit", "Limit the elapsed time in the LIDAR data to process (seconds). Measured relative to the first data sample.", optVal(time_limit))
    ("stats", "Stats collection mode: [off,console,csv]", optVal(stats_mode)->implicit_value("console"))
    ("return_number", "Stats collection mode: [off,auto,explicit]", optVal(return_number_mode)->implicit_value("auto"))
    ;
  // clang-format on
}


void DataSource::Options::print(std::ostream &out)
{
  if (point_limit)
  {
    out << "Maximum point: " << point_limit << '\n';
  }

  if (start_time > 0)
  {
    out << "Process from timestamp: " << start_time << '\n';
  }

  if (time_limit > 0)
  {
    out << "Process to timestamp: " << time_limit << '\n';
  }

  out << "Stats: " << stats_mode << '\n';
  out << "Return numbers: " << return_number_mode << '\n';
}


DataSource::DataSource(std::unique_ptr<Options> &&options)
  : options_(std::move(options))
{}


void DataSource::configure(cxxopts::Options &parser)
{
  options_->configure(parser);
}


DataSource::~DataSource() = default;


std::string DataSource::getFileExtension(const std::string &file)
{
  const size_t last_dot = file.find_last_of('.');
  if (last_dot != std::string::npos)
  {
    return file.substr(last_dot + 1);
  }

  return "";
}


void DataSource::setSamplesOnly(bool samples_only)
{
  samples_only_ = samples_only;
}


int DataSource::validateOptions()
{
  return 0;
}


void DataSource::printConfiguration(std::ostream &out)
{
  options_->print(out);
}


unsigned DataSource::addBatchStats(const Stats &stats, Stats &global_stats, std::vector<Stats> &windowed_stats_buffer,
                                   unsigned windowed_stats_buffer_size, unsigned windowed_stats_buffer_next)
{
  // Update global stats.
  global_stats.data_time_start = std::min(stats.data_time_start, global_stats.data_time_start);
  global_stats.data_time_end = std::max(stats.data_time_end, global_stats.data_time_end);
  global_stats.process_time_start = std::min(stats.process_time_start, global_stats.process_time_start);
  global_stats.process_time_end = std::max(stats.process_time_end, global_stats.process_time_end);
  global_stats.ray_length_minimum = std::min(stats.ray_length_minimum, global_stats.ray_length_minimum);
  global_stats.ray_length_maximum = std::max(stats.ray_length_maximum, global_stats.ray_length_maximum);
  global_stats.ray_length_total += stats.ray_length_total;
  global_stats.ray_count += stats.ray_count;

  // Update window
  if (windowed_stats_buffer.size() >= windowed_stats_buffer_size)
  {
    windowed_stats_buffer[windowed_stats_buffer_next] = stats;
    windowed_stats_buffer_next = (windowed_stats_buffer_next + 1) % windowed_stats_buffer_size;
  }
  else
  {
    windowed_stats_buffer.emplace_back(stats);
    ++windowed_stats_buffer_next;
  }

  return windowed_stats_buffer_next;
}


std::ostream &DataSource::Stats::writeCsvHeader(std::ostream &out)
{
  const char delim = ',';
  out << "process_time_start" << delim << "process_time_end" << delim << "data_time_start" << delim << "data_time_end"
      << delim << "ray_count" << delim << "ray_length_minimum" << delim << "ray_length_maximum" << delim
      << "ray_length_average" << delim << "rays_per_second_data" << delim << "rays_per_second_process\n";
  return out;
}


std::ostream &operator<<(std::ostream &out, const ohmapp::DataSource::Stats &stats)
{
  const char delim = ',';
  const auto precision = out.precision();
  out.precision(std::numeric_limits<double>::max_digits10);
  out << stats.process_time_start << delim << stats.process_time_end << delim << stats.data_time_start << delim
      << stats.data_time_end << delim << stats.ray_count << delim << stats.ray_length_minimum << delim
      << stats.ray_length_maximum << delim << stats.rayLengthAverage() << delim << stats.dataRaysPerSecond() << delim
      << stats.processRaysPerSecond();
  out.precision(precision);
  return out;
}


std::ostream &operator<<(std::ostream &out, ohmapp::DataSource::StatsMode mode)
{
  using StatsMode = ohmapp::DataSource::StatsMode;
  switch (mode)
  {
  case StatsMode::Off:
    out << "off";
    break;
  case StatsMode::Console:
    out << "console";
    break;
  case StatsMode::Csv:
    out << "csv";
    break;
  default:
    out << "unknown";
    break;
  }

  return out;
}


std::istream &operator>>(std::istream &in, ohmapp::DataSource::StatsMode &mode)
{
  using StatsMode = ohmapp::DataSource::StatsMode;
  std::string mode_str;
  in >> mode_str;
  if (mode_str == "console")
  {
    mode = StatsMode::Console;
  }
  else if (mode_str == "csv")
  {
    mode = StatsMode::Csv;
  }
  else if (mode_str == "off")
  {
    mode = StatsMode::Off;
  }
  else
  {
    in.setstate(std::istream::failbit);
  }

  return in;
}


std::ostream &operator<<(std::ostream &out, ohmapp::DataSource::ReturnNumberMode mode)
{
  using ReturnNumberMode = ohmapp::DataSource::ReturnNumberMode;
  switch (mode)
  {
  case ReturnNumberMode::Off:
    out << "off";
    break;
  case ReturnNumberMode::Auto:
    out << "auto";
    break;
  case ReturnNumberMode::Explicit:
    out << "explicit";
    break;
  default:
    out << "unknown";
    break;
  }

  return out;
}


std::istream &operator>>(std::istream &in, ohmapp::DataSource::ReturnNumberMode &mode)
{
  using ReturnNumberMode = ohmapp::DataSource::ReturnNumberMode;
  std::string mode_str;
  in >> mode_str;
  if (mode_str == "auto")
  {
    mode = ReturnNumberMode::Auto;
  }
  else if (mode_str == "explicit")
  {
    mode = ReturnNumberMode::Explicit;
  }
  else if (mode_str == "off")
  {
    mode = ReturnNumberMode::Off;
  }
  else
  {
    in.setstate(std::istream::failbit);
  }

  return in;
}
}  // namespace ohmapp


================================================
FILE: ohmapp/DataSource.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMAPP_DATASOURCE_H
#define OHMAPP_DATASOURCE_H

#include "OhmAppConfig.h"

#include <ohm/Trace.h>

#include <ohmutil/ProgressMonitor.h>

#include <glm/vec3.hpp>
#include <glm/vec4.hpp>

#include <iosfwd>
#include <limits>
#include <memory>
#include <string>
#include <vector>

namespace cxxopts
{
class OptionAdder;
class Options;
class ParseResult;
}  // namespace cxxopts

namespace slamio
{
class SlamCloudLoader;
}

namespace ohmapp
{
/// A data source for ohm map data used with @c MapHarness . The data source is responsible for running a data loop
/// in @c run() feeding data to the @c BatchFunction as required.
///
/// The @c MapHarness uses this object as follows:
///
/// - Call @c configure()
/// - Call @c validateOptions()
/// - Optionally call @c printConfiguration()
/// - Call @c prepareForRun()
/// - Call @c run() to process the data.
class ohmapp_API DataSource
{
public:
  /// Describes how stats should be collected.
  enum class ohmapp_API StatsMode
  {
    /// No stats
    Off,
    /// Display to console only.
    Console,
    /// Log to CSV file.
    Csv
  };

  /// Options on how return numbers are treated in point clouds.
  enum class ohmapp_API ReturnNumberMode
  {
    /// No special handling of return number.
    Off,
    /// Use explicit fields if pressent, but allow infering return number based on timestamps. Sequential points sharing
    /// the same timestamp are marked as a second return.
    Auto,
    /// Supported if an explicit @c return_number field is present in the cloud.
    Explicit
  };

  /// Base options common to all data sources.
  struct ohmapp_API Options
  {
    /// Process no more that this number of points. Zero for unbounded.
    uint64_t point_limit = 0;
    /// Skip data before this timestamp (seconds). May be relative or absolute depending on the implementation.
    double start_time = 0;
    /// Process data within a time interval set by this value (seconds).
    double time_limit = 0;
    /// How to should statistics be collected?
    StatsMode stats_mode = StatsMode::Off;
    /// How return number is treated in point clouds.
    ReturnNumberMode return_number_mode = ReturnNumberMode::Off;

    /// Virtual destructor.
    virtual ~Options();

    /// Configure command line parsing.
    /// @param parser The parsing object.
    void configure(cxxopts::Options &parser);
    /// Configure command line parsing.
    /// @param adder The object to add options to.
    virtual void configure(cxxopts::OptionAdder &adder);
    /// Print configured options.
    /// @param out Stream to print to.
    virtual void print(std::ostream &out);
  };

  /// Statistics about rays. Scope of the statistics depends on the context.
  struct ohmapp_API Stats
  {
    /// Processing start time.
    double process_time_start = 0;
    /// Processing end time.
    double process_time_end = 0;
    /// Data start time.
    double data_time_start = 0;
    /// Data end time.
    double data_time_end = 0;
    /// Minimum sample ray length.
    double ray_length_minimum = std::numeric_limits<double>::max();
    /// Maximum sample ray length.
    double ray_length_maximum = 0;
    /// Total of the samples ray length - for calculating the average.
    double ray_length_total = 0;
    /// Number of sample rays.
    uint64_t ray_count = 0;

    /// Calculate the processing time range from start to end. Will (approximately) match @c dataTime() in real time
    /// processing.
    /// @return The process time range.
    inline double processTime() const { return process_time_end - process_time_start; }

    /// Calculate the data time range from start to end.
    /// @return The data time range.
    inline double dataTime() const { return data_time_end - data_time_start; }

    /// Calculate and return the average ray length. Returns zero when the average cannot be calcualted (no ray count).
    /// @return The average ray length or zero.
    inline double rayLengthAverage() const { return (ray_count) ? ray_length_total / double(ray_count) : 0.0; }

    /// Calculate the number of rays per second in processing time.
    /// @return The rays per second, or zero if no time range is available.
    inline double processRaysPerSecond() const { return (processTime() > 0) ? double(ray_count) / processTime() : 0.0; }

    /// Calculate the number of rays per second in data time.
    /// @return The rays per second, or zero if no time range is available.
    inline double dataRaysPerSecond() const { return (dataTime() > 0) ? double(ray_count) / dataTime() : 0.0; }

    inline void reset() { reset(0.0, 0.0); }

    /// Reset the stats setting the data and processing start times.
    /// @param time_start_reset Value to reset @c time_start to.
    inline void reset(double process_time_start_reset, double data_time_start_reset)
    {
      process_time_start = process_time_start_reset;
      data_time_start = data_time_start_reset;
      data_time_end = process_time_end = 0;
      ray_length_minimum = std::numeric_limits<double>::max();
      ray_length_maximum = ray_length_total = 0;
      ray_count = 0;
    }

    /// Write CSV format headers to @p out matching output of the streaming operator for @c Stats .
    static std::ostream &writeCsvHeader(std::ostream &out);
  };

  /// Create a harness with the given options specialisation.
  ///
  /// Takes ownership of the pointer. @p options should generally be a specialisation of @c Options .
  ///
  /// @param options Configuration options.
  DataSource(std::unique_ptr<Options> &&options);

  /// Virtual destructor.
  virtual ~DataSource();

  /// Get the configured options (mutable).
  inline Options &options() { return *options_; }
  /// Get the configured options.
  inline const Options &options() const { return *options_; }

  /// Get the extension for @p file excluding the leading '.' character.
  /// @param file File name to check.
  static std::string getFileExtension(const std::string &file);

  /// Should only samples be given to the first array argument of @c BatchFunction ( @c true) or sensor/sample pairs
  /// ( @c false ).
  inline bool samplesOnly() const { return samples_only_; }
  /// Set @c samplesOnly() .
  /// @param samples_only True to only pass samples to @c BatchFunction , false to interleave sensor/sample pairs.
  virtual void setSamplesOnly(bool samples_only);

  /// Return the name of the data source. For example, this may be the input file name without extension.
  /// @return A name for the data source.
  virtual std::string sourceName() const = 0;

  /// Query the number of points which have been processed.
  ///
  /// Threadsafe.
  ///
  /// @return The number of samples which have been submitted to the @c BatchFunction .
  virtual uint64_t processedPointCount() const = 0;
  /// Query the time interval covered by the sample points which have been processed.
  ///
  /// Threadsafe.
  ///
  /// @return The time range of samples which have been submitted to the @c BatchFunction .
  virtual double processedTimeRange() const = 0;

  /// Get the expected batch size for calls to @c BatchFunction . Zero if unknown.
  /// @return The expected sample batch size or zero if unknown.
  virtual unsigned expectedBatchSize() const = 0;

  /// Request that batches come in either the given @p batch_size or so as not to exceed the given  @p max_sensor_motion
  ///
  /// Implementations may ignore this setting.
  ///
  /// @param batch_size Target size for a batch.
  /// @param max_sensor_motion Maximum sensor motion for a batch. Zero to disable.
  virtual void requestBatchSettings(unsigned batch_size, double max_sensor_motion) = 0;

  /// Configure command line options for this data source.
  /// @param parser Command line parser.
  virtual void configure(cxxopts::Options &parser);

  /// Validate configured options.
  /// @param parsed Command line option parsing result.
  virtual int validateOptions();

  /// Print the command line configuration.
  /// @param out Stream to print to.
  virtual void printConfiguration(std::ostream &out);

  /// Prepare to execute, finalising the configured options.
  /// @param[out] predicted_point_count Set to the predicted number of points to process. Set to zero if unknown.
  /// @param reference_name Optional reference name which identifies the run. Generally used for additional file output
  /// such as stats logging.
  /// @return Zero on success, a non-zero value on failure which can be used as the program exit code.
  virtual int prepareForRun(uint64_t &predicted_point_count, const std::string &reference_name = std::string()) = 0;

  /// Function object invoked for each data sample batch.
  /// @param batch_origin The sensor position for the first point in the batch.
  /// @param sensor_and_samples Sensor/sample point pairs when @c samplesOnly() is @c false or just sample points when
  /// @c samplesOnly() is @c true . Sensor and sample points are in the same frame as the @c batch_origin .
  /// @param timestamps Time stamps for each sample point.
  /// @param intensities Intensity values for each sample point. Will be zero when the input data has no intensity.
  /// @param colour Colour values for each sample point. Will be zero when the input data has no colour.
  /// @param return_number Zero based, sample return number where available. Zero is the first/primary return, 1 is the
  /// second return, etc. Empty when this information is not available.
  /// @return True to continue processessing, @c false to stop.
  using BatchFunction =
    std::function<bool(const glm::dvec3 &batch_origin, const std::vector<glm::dvec3> &sensor_and_samples,
                       const std::vector<double> &timestamps, const std::vector<float> &intensities,
                       const std::vector<glm::vec4> &colours, const std::vector<uint8_t> &return_number)>;

  /// Run data sample loading calling the @p batch_function for each data batch loaded.
  ///
  /// Implementations block until all data samples have been processed by @p batch_function or the @p batch_function
  /// returns false.
  ///
  /// @note @c samplesOnly() affects data passed to @c batch_function . When @c true, the @p batch_function
  /// @p sensor_and_samples array contains only data samples. When @c false it interleavses sensor and sample pairs
  /// where the sensor location may be unique for each sample point.
  ///
  /// @param batch_function Function call for each data sample batch.
  /// @return Zero on success, a non-zero value on failure which can be used as the program exit code.
  virtual int run(BatchFunction batch_function, unsigned *quit_level_ptr) = 0;

  /// Retrieve the global stats on all data processed so far.
  ///
  /// This function is generally not thread safe. Use @c windowedStats() for thread safe, progressive statistics.
  ///
  /// @return Stats on the data processed.
  virtual Stats globalStats() const = 0;

  /// Retrieve windowed stats covering a recent time window determined by the implementation..
  ///
  /// This must be thread safe for asynchronous access.
  ///
  /// @return Stats on the recent sample window.
  virtual Stats windowedStats() const = 0;

  /// A helper for collating stats.
  ///
  /// Adds @p stats to the global stats and windowed stats buffer.
  ///
  /// @param stats Stats to add.
  /// @param global_stats Global stats to modify.
  /// @param windowed_stats_buffer Ring buffer of windowed stats items.
  /// @param windowed_stats_buffer_size Number of items to store in @p windowed_stats_buffer before cycling.
  /// @param windowed_stats_buffer_next Next insertion/overwrite index for @p windowed_stats_buffer once full.
  /// @return The next value of @c windowed_stats_buffer_next .
  static unsigned addBatchStats(const Stats &stats, Stats &global_stats, std::vector<Stats> &windowed_stats_buffer,
                                unsigned windowed_stats_buffer_size, unsigned windowed_stats_buffer_next);

  /// A helper function for calculating windowed stats.
  /// @param stats Object to calculate stats into.
  /// @param begin First collected stats iterator.
  /// @param end Collects stats end iterator.
  /// @tparam Iter An interatable type referencing a @c Stats range.
  template <typename Iter>
  inline static void calculateWindowedStats(Stats &stats, const Iter begin, const Iter end)
  {
    stats.reset();

    if (begin != end)
    {
      stats.data_time_start = begin->data_time_start;
      stats.data_time_end = begin->data_time_end;
      stats.process_time_start = begin->process_time_start;
      stats.process_time_end = begin->process_time_end;

      for (auto iter = begin; iter != end; ++iter)
      {
        stats.data_time_start = std::min(iter->data_time_start, stats.data_time_start);
        stats.data_time_end = std::max(iter->data_time_end, stats.data_time_end);
        stats.process_time_start = std::min(iter->process_time_start, stats.process_time_start);
        stats.process_time_end = std::max(iter->process_time_end, stats.process_time_end);
        stats.ray_length_minimum = std::min(iter->ray_length_minimum, stats.ray_length_minimum);
        stats.ray_length_maximum = std::max(iter->ray_length_maximum, stats.ray_length_maximum);
        stats.ray_length_total += iter->ray_length_total;
        stats.ray_count += iter->ray_count;
      }
    }
  }

private:
  /// User configurable options.
  std::unique_ptr<Options> options_;
  /// True to provide only data in the first array argument passed to @c processBatch() , false to include
  /// sensor/sample pairs.
  bool samples_only_ = true;
};

/// Streaming operator for @c ohmapp::DataSource::Stats logging comma separated values.
///
/// @param out The output stream.
/// @param stats The stats to print.
/// @return The output stream.
std::ostream ohmapp_API &operator<<(std::ostream &out, const ohmapp::DataSource::Stats &stats);

/// Output streaming for @c ohmapp::DataSource::StatsMode .
/// @param out Output stream.
/// @param mode Mode value.
/// @return The output stream.
std::ostream ohmapp_API &operator<<(std::ostream &out, ohmapp::DataSource::StatsMode mode);
/// Output streaming for @c ohmapp::DataSource::StatsMode .
/// @param in Intput stream.
/// @param mode Mode reference to read into.
/// @return The input stream.
std::istream ohmapp_API &operator>>(std::istream &in, ohmapp::DataSource::StatsMode &mode);

/// Output streaming for @c ohmapp::DataSource::ReturnNumberMode .
/// @param out Output stream.
/// @param mode Mode value.
/// @return The output stream.
std::ostream ohmapp_API &operator<<(std::ostream &out, ohmapp::DataSource::ReturnNumberMode mode);
/// Output streaming for @c ohmapp::DataSource::ReturnNumberMode .
/// @param in Intput stream.
/// @param mode Mode reference to read into.
/// @return The input stream.
std::istream ohmapp_API &operator>>(std::istream &in, ohmapp::DataSource::ReturnNumberMode &mode);
}  // namespace ohmapp

#endif  // OHMAPP_DATASOURCE_H


================================================
FILE: ohmapp/MapHarness.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "MapHarness.h"

#include "DataSource.h"

#include <logutil/LogUtil.h>

#include <glm/vec4.hpp>

#include <chrono>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <locale>

// Must be after argument streaming operators.
#include <ohmutil/Options.h>

using namespace ohm;

namespace ohmapp
{
using Clock = std::chrono::high_resolution_clock;


MapHarness::OutputOptions::OutputOptions() = default;
MapHarness::OutputOptions::~OutputOptions() = default;


void MapHarness::OutputOptions::configure(cxxopts::Options &parser)
{
  cxxopts::OptionAdder adder = parser.add_options("Output");
  configure(adder);
}


void MapHarness::OutputOptions::configure(cxxopts::OptionAdder &adder)
{
  // clang-format off
  adder
    ("cloud-colour", "Colour for points in the saved cloud (if saving).", optVal(cloud_colour))
    ("output", "Output base name. Saved file paths are derived from this string adding appropriate file extensions.", optVal(base_name))
    ("q,quiet", "Run in quiet mode. Suppresses progress messages.", optVal(quiet))
    ("save-cloud", "Save a point cloud after population?", optVal(save_cloud))
    ("save-info", "Write information on how the map was generated to text file?", optVal(save_info))
    ("save-map", "Save the map object after population?", optVal(save_map))
    ("trace", "Enable debug tracing to the given file name to generate a 3es file. High performance impact.", optVal(trace)->implicit_value("trace"))
    ("trace-final", "Only output final map in trace.", optVal(trace_final))
  ;
  // clang-format on
}


void MapHarness::OutputOptions::print(std::ostream &out)
{
  out << "Output base path: " << base_name << '\n';
  std::string save_items;
  if (save_map)
  {
    save_items += " map";
  }
  if (save_cloud)
  {
    save_items += " cloud";
    if (glm::all(glm::greaterThanEqual(cloud_colour, glm::vec3(0.0f))))
    {
      out << "Save cloud colour: " << cloud_colour << '\n';
    }
  }
  if (!save_items.empty())
  {
    out << "Will save: " << save_items << '\n';
  }
  if (!trace.empty())
  {
#ifdef TES_ENABLE
    out << "3es trace file: " << trace << (trace_final ? "(final only)\n" : "\n");
#else   //
    out << "3es trace ignore (not compiled)\n";
#endif  // TES_ENABLE
  }
}


MapHarness::MapOptions::~MapOptions() = default;


void MapHarness::MapOptions::configure(cxxopts::Options &parser)
{
  cxxopts::OptionAdder adder = parser.add_options("Map");
  configure(adder);
}


void MapHarness::MapOptions::configure(cxxopts::OptionAdder &adder)
{
  // clang-format off
  adder
    ("resolution", "The voxel resolution of the generated map.", optVal(resolution))
  ;
  // clang-format on
}


void MapHarness::MapOptions::print(std::ostream &out)
{
  out << "Map resolution: " << resolution << '\n';
}


MapHarness::Options::Options()
{
  output_ = std::make_unique<MapHarness::OutputOptions>();
  map_ = std::make_unique<MapHarness::MapOptions>();
}


MapHarness::Options::~Options() = default;


void MapHarness::Options::configure(cxxopts::Options &parser)
{
  parser.add_options()("h,help", "Display this help.");
  output_->configure(parser);
  map_->configure(parser);

  if (!positional_args.empty())
  {
    parser.parse_positional(positional_args);
  }
}


void MapHarness::Options::print(std::ostream &out)
{
  out.imbue(std::locale(""));
  out << std::boolalpha;
  output_->print(out);
  map_->print(out);
}


MapHarness::MapHarness(std::unique_ptr<Options> &&options, std::shared_ptr<DataSource> data_source)
  : options_(std::move(options))
  , data_source_(data_source)
{}


MapHarness::~MapHarness() = default;


int MapHarness::parseCommandLineOptions(int argc, const char *const *argv)
{
  cxxopts::Options parser(argv[0]);
  configureOptions(parser);
  if (on_configure_options_callback_)
  {
    on_configure_options_callback_();
  }

  int result = 0;
  try
  {
    cxxopts::ParseResult parsed = parser.parse(argc, argv);
    if (parsed.count("help") || parsed.arguments().empty())
    {
      // show usage.
      std::cout << parser.help(options_->default_help_sections) << std::endl;
      quit_level_ = maxQuitLevel();
      return 0;
    }

    result = validateOptions(parsed);
  }
  catch (const cxxopts::OptionException &e)
  {
    std::cerr << "Argument error\n" << e.what() << std::endl;
    result = -1;
  }

  return result;
}


int MapHarness::validateOptions(const cxxopts::ParseResult &parsed)
{
  (void)parsed;
  data_source_->validateOptions();

  // Generate output name based on input if not specified.
  if (options_->output().base_name.empty())
  {
    options_->output().base_name = data_source_->sourceName();
  }

#ifdef TES_ENABLE
  if (!options().output().trace.empty())
  {
    if (DataSource::getFileExtension(options().output().trace) != "3es")
    {
      options().output().trace += ".3es";
    }
  }
#endif  // TES_ENABLE

  return 0;
}


int MapHarness::run()
{
  int result_code = 0;

  if (quit_level_)
  {
    // Nothing to do.
    return result_code;
  }

#ifdef TES_ENABLE
  if (!options().output().trace.empty())
  {
    trace_ = std::make_unique<ohm::Trace>(options().output().trace.c_str());
  }
#endif  //  TES_ENABLE

  progress_.setDisplayFunction(
    std::bind(&MapHarness::displayProgress, this, std::placeholders::_1, std::placeholders::_2));

  std::ofstream info_file_out;
  std::array<std::ostream *, 2> info_streams = { nullptr };

  if (!quiet())
  {
    info_streams[0] = &std::cout;
  }

  if (options_->output().save_info)
  {
    const std::string output_txt = options_->output().base_name + ".txt";
    info_file_out.open(output_txt.c_str());
    info_streams[1] = &info_file_out;
  }

  result_code = prepareForRun();
  if (result_code)
  {
    tearDown();
    return result_code;
  }

  for (auto *out : info_streams)
  {
    if (out)
    {
      data_source_->options().print(*out);
      options_->print(*out);
    }
  }

  uint64_t predicted_point_count = 0;
  data_source_->prepareForRun(predicted_point_count, options_->output().base_name);

  if (on_start_callback_)
  {
    on_start_callback_();
  }

  logutil::info("Populating map\n");
  display_stats_in_progress_ = data_source_->options().stats_mode != DataSource::StatsMode::Off;
  progress_.beginProgress(ProgressMonitor::Info(predicted_point_count));
  progress_.startThread();

  const Clock::time_point start_time = Clock::now();
  data_source_->run(
    std::bind(&MapHarness::processBatch, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
              std::placeholders::_4, std::placeholders::_5, std::placeholders::_6),
    quitLevelPtr());
  progress_.endProgress();
  progress_.pause();
  display_stats_in_progress_ = false;
  finaliseMap();
  const Clock::time_point end_time = Clock::now();

  const double time_range = data_source_->processedTimeRange();
  const uint64_t processed_count = data_source_->processedPointCount();
  const double processing_time_sec =
    std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count() * 1e-3;

  if (quit_level_ < 2)
  {
    for (auto *out : info_streams)
    {
      if (out)
      {
        const double rtf_inv = (processing_time_sec > 0 && time_range > 0) ? time_range / processing_time_sec : 0.0;
        const double rtf = (rtf_inv) ? 1.0 / rtf_inv : 0;
        *out << "Sample count: " << processed_count << '\n';
        *out << "Data time: " << time_range << '\n';
        *out << "Total processing time: " << end_time - start_time << '\n';
        *out << "Realtime Factor: " << rtf << '\n';
        *out << "RTF inverse: " << rtf_inv << '\n';
        *out << "Average samples/sec: "
             << unsigned((processing_time_sec > 0) ? processed_count / processing_time_sec : 0.0) << '\n';
        // *out << "Memory (approx): " << map.calculateApproximateMemory() / (1024.0 * 1024.0) << " MiB\n";
        *out << std::flush;

        if (data_source_->options().stats_mode != DataSource::StatsMode::Off)
        {
          *out << "Ray length minimum: " << data_source_->globalStats().ray_length_minimum << '\n';
          *out << "Ray length maximum: " << data_source_->globalStats().ray_length_maximum << '\n';
          *out << "Ray length average: " << data_source_->globalStats().rayLengthAverage() << '\n';
        }
      }
    }
  }

  result_code = serialise();
  progress_.joinThread();
  tearDown();

  return 0;
}


void MapHarness::configureOptions(cxxopts::Options &parser)
{
  data_source_->configure(parser);
  options_->configure(parser);
}


int MapHarness::serialise()
{
  int result_code = 0;
  if (options_->output().save_map && quit_level_ < 2)
  {
    result_code = saveMap(options_->output().base_name);
    if (result_code)
    {
      return result_code;
    }
  }

  if (options_->output().save_cloud && quit_level_ < 2)
  {
    const std::string output_ply = options_->output().base_name + "_cloud.ply";
    result_code = saveCloud(output_ply);
    if (result_code)
    {
      return result_code;
    }
  }

  return result_code;
}


void MapHarness::displayProgress(const ProgressMonitor::Progress &progress, bool final)
{
  if (!quiet())
  {
    const double elapsed_sec = data_source_->processedTimeRange();
    const auto sec = unsigned(std::trunc(elapsed_sec));
    const auto ms = unsigned((elapsed_sec - std::trunc(elapsed_sec)) * 1000.0);

    std::ostringstream out;
    out.imbue(std::locale(""));
    out << '\r';

    if (!progress.info.info.empty())
    {
      out << progress.info.info << " : ";
    }

    out << std::setfill(' ') << std::setw(6) << sec << '.' << std::setfill('0') << std::setw(3) << ms << "s : ";

    const int fill_padding = 3;
    const auto fill_width = std::min(std::numeric_limits<decltype(progress.progress)>::digits10,
                                     std::numeric_limits<uint32_t>::digits10 + fill_padding);
    out << std::setfill(' ') << std::setw(fill_width) << progress.progress;
    if (progress.info.total)
    {
      out << " / " << std::setfill(' ') << std::setw(fill_width) << progress.info.total;
    }

    if (display_stats_in_progress_)
    {
      out << " ";
      // Show stats
      const DataSource::Stats &stats = data_source_->windowedStats();
      const double rays_per_second = stats.processRaysPerSecond();
      const uint64_t integer_rays_per_second = uint32_t(std::round(rays_per_second));
      out << std::setfill(' ') << std::setw(fill_width) << integer_rays_per_second << " rays/s ";
      out.precision(3);
      out << "avg len: " << std::fixed << stats.rayLengthAverage() << "     ";
    }

    if (final)
    {
      out << '\n';
    }
    logutil::info(out.str());
  }
}
}  // namespace ohmapp


================================================
FILE: ohmapp/MapHarness.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMAPP_MAPHARNESS_H
#define OHMAPP_MAPHARNESS_H

#include "OhmAppConfig.h"

#include <ohm/Trace.h>

#include <ohmutil/ProgressMonitor.h>

#include <logutil/Logger.h>

#include <glm/vec3.hpp>
#include <glm/vec4.hpp>

#include <functional>
#include <memory>
#include <string>
#include <vector>

namespace cxxopts
{
class OptionAdder;
class Options;
class ParseResult;
}  // namespace cxxopts

namespace ohmapp
{
class DataSource;

/// A base class which can be extended to define an application which populates an occupancy map.
///
/// Usage:
/// - instantiate the harness specialisation
/// - Call @c parseCommandLineOptions() or configure @c options() directly.
/// - Call @c run()
/// - Release/destroy
///
/// Derivations should implement the pure virtual funtions as detailed in those function comments. Derivations may also
/// extend various @c Options structures providing their own type. It is recommended that appropriate conversion
/// functions are put in place to access the underlying types. For example, if @c MapOptions is extended then
/// @c Options should first be extended to instantiate the specialisation. Additional @c Options should override
/// @c Options::map() to return the @c MapOptions specialisation and @c MapHarness should override @c options() to
/// return the @c Options specialisation.
class ohmapp_API MapHarness
{
public:
  /// Maximum value for @c quitLevel()
  constexpr static unsigned maxQuitLevel() { return 2u; }

  /// Callback function signature invoked from @c parseCommandLineOptions() after parsing, but before
  /// @c validateOptions() .
  using GeneralCallback = std::function<void()>;

  /// Options controlling output.
  struct ohmapp_API OutputOptions
  {
    /// Base output name. Typically used as a base file name/path where an extension may be added appropriate to the
    /// serialisation type. E.g., saving an ohm map will add @c '.ohm' while saving a ply would add @c '.ply'.
    std::string base_name;
    /// Explicit colour to use when exportin a pointcloud. Any negative colour channels indicate the explicit colour
    /// is not being used.
    glm::vec3 cloud_colour{ -1.0f };
    /// 3rd Eye Scene trace file. Enables 3es debugging if available (compile switch).
    std::string trace;
    /// Only use 3es to visualise the final map?
    bool trace_final = false;
    /// Save the map file?
    bool save_map = true;
    /// Save a point cloud form the map?
    bool save_cloud = true;
    /// Save statistics to file?
    bool save_info = false;
    /// Suppress console output.
    bool quiet = false;

    OutputOptions();
    virtual ~OutputOptions();

    /// Configure the command line options for the given @c parser . Calls @c `configure(const cxxopts::OptionAdder &)`
    /// @param parser The command line parser.
    void configure(cxxopts::Options &parser);
    /// Add command line options.
    /// Derivations should override this to add their own options as well as calling this base version.
    /// @param adder Object to add command line options to.
    virtual void configure(cxxopts::OptionAdder &adder);
    /// Print command line options to the given stream.
    /// Derivations should override this to print their own options as well as calling this base version.
    /// @param out Output stream to print configured options to.
    virtual void print(std::ostream &out);
  };

  /// Options controlling the map configuration.
  struct ohmapp_API MapOptions
  {
    /// Voxel size.
    double resolution = 0.1;

    virtual ~MapOptions();

    /// Configure the command line options for the given @c parser . Calls @c `configure(const cxxopts::OptionAdder &)`
    /// @param parser The command line parser.
    void configure(cxxopts::Options &parser);
    /// Add command line options. Derivations should override this to add their own options as well as calling this
    /// base version.
    /// @param adder Object to add command line options to.
    virtual void configure(cxxopts::OptionAdder &adder);
    /// Print command line options to the given stream.
    /// Derivations should override this to print their own options as well as calling this base version.
    /// @param out Output stream to print configured options to.
    virtual void print(std::ostream &out);
  };

  /// Collated options.
  struct ohmapp_API Options
  {
    /// The output options.
    std::unique_ptr<OutputOptions> output_;
    /// The map options.
    std::unique_ptr<MapOptions> map_;

    /// Positional argument names set when @c configure() is called.
    std::vector<std::string> positional_args = { "cloud", "trajectory", "output" };
    /// List of help sections to show when @c --help is used.
    std::vector<std::string> default_help_sections = { "", "Input", "Output", "Map" };

    Options();
    virtual ~Options();

    /// Access the output options by reference. Should be overriden by derivation to return their own output options
    /// specialisation.
    /// @return The @c OutputOptions .
    inline OutputOptions &output() { return *output_; }
    /// @overload
    inline const OutputOptions &output() const { return *output_; }

    /// Access the map options by reference. Should be overriden by derivation to return their own map options
    /// specialisation.
    /// @return The @c MapOptions .
    inline MapOptions &map() { return *map_; }
    /// @overload
    inline const MapOptions &map() const { return *map_; }

    /// Configure the command line options for the given @c parser . Calls @c `configure(const cxxopts::OptionAdder &)`
    /// @param parser The command line parser.
    virtual void configure(cxxopts::Options &parser);
    /// Print command line options to the given stream.
    /// Derivations should override this to print their own options as well as calling this base version.
    /// @param out Output stream to print configured options to.
    virtual void print(std::ostream &out);
  };

  /// Create a harness with the given options specialisation.
  ///
  /// Takes ownership of the pointer. @p options should generally be a specialisation of @c Options .
  MapHarness(std::unique_ptr<Options> &&options, std::shared_ptr<DataSource> data_source);
  /// Virtual destructor.
  virtual ~MapHarness();

  /// Running in quiet mode? Suppresses logging and progress display.
  inline bool quiet() const { return options_->output().quiet; }

  /// Access the @c ProgressMonitor object.
  /// @return The progress monitor.
  inline ProgressMonitor &progress() { return progress_; }
  /// @overload
  inline const ProgressMonitor &progress() const { return progress_; }

  /// Description string for command line help.
  virtual std::string description() const = 0;

  /// Setup the harness for execution, parsing the command line arguments using and preparing for execution.
  ///
  /// Calls
  /// - @c configureOptions()
  /// - @c validateOptions()
  ///
  /// @param argc Number of elements in @p argv .
  /// @param argv Command line arguments.
  /// @return Zero on success, a non-zero value on failure which can be used as the program exit code.
  virtual int parseCommandLineOptions(int argc, const char *const *argv);

  /// Run the map generation.
  ///
  /// The @c run() function manages execution as follows:
  /// - Binds the @c ProgressMonitor - @c progress() - display function to @c displayProgress()
  /// - Calls @c prepareForRun()
  /// - Displays @c DataSource options and the internal @c options() calling the @c print() function. May be called
  ///   multiple times displaying to different streams
  /// - Calls @c DataSource::prepareForRun()
  /// - Starts the @c progress() thread.
  /// - Calls @c DataSource::run() . This calls contains main execution loop.
  /// - Calls @c finaliseMap(). Serialisation may occur from here.
  /// - Displays statistics
  /// - Calls @c tearDown()
  ///
  /// Note that @c tearDown() is called out of sequence if any failures occur during execution. That is, @c tearDown()
  /// is called on error and the @c run() sequence terminated.
  ///
  /// @return Zero on success, a non-zero value on failure which can be used as the program exit code.
  int run();

  /// Request a quit, incrementing the quit level.
  ///
  /// Incrementing the quit level progressively terminates certain loops. Specifically:
  /// - level 1 quits the map generation loop (as controlled by the @c DataSource )
  /// - level 2 quits serialisation
  ///
  /// The quit level ay be incremented any number of types, but has no further sematic effect after reaching
  /// @c maxQuitLevel() .
  inline void requestQuit() { ++quit_level_; }
  /// Query the current quit level.
  /// @return The current quit level.
  inline unsigned quitLevel() const { return quit_level_; }

  /// Get the @c quitLevel() address - generally used for incrementing on @c SIGINT .
  /// @return The @c quitLevel() address.
  inline unsigned *quitLevelPtr() { return &quit_level_; }

  /// @overload
  inline const unsigned *quitLevelPtr() const { return &quit_level_; }

  /// Query if the @c quitLevel() is high enough to skip map population.
  /// @return True to quit map generation.
  inline bool quitPopulation() const { return quit_level_ != 0u; }

  /// Query if the @c quitLevel() is high enough to skip serialisation.
  /// @return True to quit map serialisation.
  inline bool quitSerialisation() const { return quit_level_ > 1; }

  /// Get the @c Options - read only.
  /// @return The configured options.
  const Options &options() const { return *options_; }

  /// Get the @c Options - read/write.
  /// @return The configured options.
  Options &options() { return *options_; }

  /// Access the @c DataSource source object.
  /// @return The data source object.
  std::shared_ptr<ohmapp::DataSource> dataSource() const { return data_source_; }

  /// Set the callback to invoke on configuration.
  ///
  /// Called after @c configureOptions(), but before @c validateOptions() .
  /// @param callback The function to invoke. May be set to empty for no callback.
  void setOnConfigureOptionsCallback(GeneralCallback callback) { on_configure_options_callback_ = callback; }

  /// Get the callback invoked on starting.
  /// @return The post configuration callback.
  GeneralCallback onConfigureOptionsCallback() const { return on_configure_options_callback_; }

  /// Set the callback to invoke on starting.
  ///
  /// Called after @c prepareForRun(), but before any data processing.
  /// @param callback The function to invoke. May be set to empty for no callback.
  void setOnStartCallback(GeneralCallback callback) { on_start_callback_ = callback; }

  /// Get the callback invoked on starting.
  /// @return The start callback.
  GeneralCallback onStartCallback() const { return on_start_callback_; }

protected:
  /// Configure the @p parser to parse command line options into @p options() .
  /// @param parser The command line parser.
  virtual void configureOptions(cxxopts::Options &parser);

  /// Validate and modify options after parsing. Called from @c parseCommandLineOptions() .
  ///
  /// The default implementation validates input/ouput arguments.
  ///
  /// @param parsed The results from the parsing call.
  /// @return Zero on success, a non-zero value on failure which can be used as the program exit code.
  virtual int validateOptions(const cxxopts::ParseResult &parsed);

  /// Perform custom setup for execution such as map creation - called from @c run() after @c createSlamLoader() .
  /// @return Zero on success or an exit code on error.
  virtual int prepareForRun() = 0;

  /// Process a batch of samples, adding them to the map.
  ///
  /// The implementation is to integrate the samples into the map. Note that @p sensor_and_samples will either be an
  /// array of sensor origin/sample pairs or samples only depending on @c DataSource::samplesOnly()
  /// ( @c dataSource()->samplesOnly() ). Sensor and sample points are both in the same frame as @c batch_origin and not
  /// in the sensor frame.
  ///
  /// @note It is the implementation's responsibility to call @c progress_.incrementProgressBy(timestamps.size()) .
  ///
  /// @param batch_origin The sensor position for the first point in the batch.
  /// @param sensor_and_samples Sensor/sample point pairs or just sample points (see above).Sensor and sample points are
  /// in the same frame as the @c batch_origin .
  /// @param timestamps Time stamps for each sample point.
  /// @param intensities Intensity values for each sample point. Will be zero when the input data has no intensity.
  /// @param colour Colour values for each sample point. Will be zero when the input data has no colour.
  /// @param return_numbers Array of return numbers for each sample. Zero is the primary sample, 1 is the
  /// second, etc. Empty when this information is not available.
  /// @return True to continue processing - generally should be `return !quitPopulation();`
  virtual bool processBatch(const glm::dvec3 &batch_origin, const std::vector<glm::dvec3> &sensor_and_samples,
                            const std::vector<double> &timestamps, const std::vector<float> &intensities,
                            const std::vector<glm::vec4> &colours, const std::vector<uint8_t> &return_numbers) = 0;

  /// Called after app data have been added to the map to finalise.
  inline virtual void finaliseMap() {}

  /// Serialise the map based on @c OutputOptions . Calls @c saveMap() and/or @c saveCloud() as indicated by the
  /// @c OutputOptions::save_flags .
  /// @return Zero on success, a non-zero value on failure which can be used as the program exit code.
  int serialise();

  /// Save map data. Called from @c serialise() .
  /// @return Zero on success, a non-zero value on failure which can be used as the program exit code.
  virtual int saveMap(const std::string &path_without_extension) = 0;
  /// Save point cloud from the map. Called from @c serialise() .
  /// @return Zero on success, a non-zero value on failure which can be used as the program exit code.
  virtual int saveCloud(const std::string &path_ply) = 0;

  /// Perform tear down after processing has completed. Called from @c run() after saving.
  ///
  /// Also called on failure to process if @c prepareForRun() has been called (including when that function fails).
  virtual void tearDown() = 0;

  /// Callback for @c ProgressMonitor::setDisplayFunction() which displays to @c std::cout reusing the same line (\\r).
  virtual void displayProgress(const ProgressMonitor::Progress &progress, bool final);

  /// Options for the populator. May be a derivation of @c MapHarness::Options .
  std::unique_ptr<Options> options_;
  /// Progress reporting thread helper.
  ProgressMonitor progress_;

  /// 3es debug trace pointer. Will only be available when TES_ENABLE is preprocessor defined.
  std::unique_ptr<ohm::Trace> trace_;

private:
  /// Time elapsed in the input data set timestamps (milliseconds).
  std::atomic<uint64_t> dataset_elapsed_ms_;
  /// Object from which samples are loaded.
  std::shared_ptr<DataSource> data_source_;
  /// Value set when a quit is requested. A quit value of 1 will stop map population, while 2 will also skip
  /// serialisation.
  unsigned quit_level_ = 0;
  /// Called after @c configureOptions() for external notification, but before @c validateOptions() .
  GeneralCallback on_configure_options_callback_;
  /// Called after @c prepareForRun() for external notification.
  GeneralCallback on_start_callback_;
  bool display_stats_in_progress_ = false;
};
}  // namespace ohmapp

#endif  // OHMAPP_MAPHARNESS_H


================================================
FILE: ohmapp/OhmAppConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMAPPCONFIG_H
#define OHMAPPCONFIG_H

#include "OhmAppExport.h"

#include <ohm/OhmConfig.h>

#endif  // OHMAPPCONFIG_H


================================================
FILE: ohmapp/OhmAppCpu.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmAppCpu.h"

#include <ohm/DebugDraw.h>
#include <ohm/DefaultLayer.h>
#include <ohm/MapSerialise.h>
#include <ohm/Mapper.h>
#include <ohm/NdtMap.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/RayMapperNdt.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/RayMapperSecondarySample.h>
#include <ohm/RayMapperTrace.h>
#include <ohm/RayMapperTsdf.h>
#include <ohm/Trace.h>
#include <ohm/VoxelBlockCompressionQueue.h>
#include <ohm/VoxelData.h>

#include <ohmapp/DataSource.h>

#ifdef TES_ENABLE
#include <ohm/RayMapperTrace.h>
#endif  // TES_ENABLE

#include <ohmtools/OhmCloud.h>

#include <logutil/LogUtil.h>
#include <ohmutil/PlyMesh.h>
#include <ohmutil/ProgressMonitor.h>
#include <ohmutil/SafeIO.h>
#include <ohmutil/ScopedTimeDisplay.h>

namespace ohm
{
std::istream &operator>>(std::istream &in, NdtMode &mode);
std::ostream &operator<<(std::ostream &out, const NdtMode mode);
}  // namespace ohm

// Must be after argument streaming operators.
#include <ohmutil/Options.h>

namespace ohm
{
std::istream &operator>>(std::istream &in, ohm::NdtMode &mode)
{
  // Note: we don't use ndtModeFromString() because we use abbreviations
  std::string mode_str;
  in >> mode_str;
  if (mode_str == "off")
  {
    mode = ohm::NdtMode::kNone;
  }
  else if (mode_str == "om")
  {
    mode = ohm::NdtMode::kOccupancy;
  }
  else if (mode_str == "tm")
  {
    mode = ohm::NdtMode::kTraversability;
  }
  else
  {
    throw cxxopts::invalid_option_format_error(mode_str);
  }
  return in;
}

std::ostream &operator<<(std::ostream &out, const ohm::NdtMode mode)
{
  // Note: we don't use ndtModeToString() because we use abbreviations
  switch (mode)
  {
  case ohm::NdtMode::kNone:
    out << "off";
    break;
  case ohm::NdtMode::kOccupancy:
    out << "om";
    break;
  case ohm::NdtMode::kTraversability:
    out << "tm";
    break;
  default:
    out << "<unknown>";
  }
  return out;
}
}  // namespace ohm

namespace ohmapp
{
OhmAppCpu::MapOptions::MapOptions()
{
  // Initialise defaults from map configurations.
  ohm::OccupancyMap defaults_map;

  region_voxel_dim = defaults_map.regionVoxelDimensions();
  prob_hit = defaults_map.hitProbability();
  prob_miss = defaults_map.missProbability();
  prob_thresh = defaults_map.occupancyThresholdProbability();
  prob_range[0] = defaults_map.minVoxelValue();
  prob_range[1] = defaults_map.maxVoxelValue();
}


void OhmAppCpu::MapOptions::configure(cxxopts::OptionAdder &adder)
{
  Super::MapOptions::configure(adder);
  // clang-format off
  adder
    ("ray-length-max", "Maximum ray length. Longer rays are clipped to this range. Use zero to disable.", optVal(ray_length_max))
    ("clamp", "Set probability clamping to the given min/max. Given as a value, not probability.", optVal(prob_range))
    ("dim", "Set the voxel dimensions of each region in the map. Range for each is [0, 255).", optVal(region_voxel_dim))
    ("hit", "The occupancy probability due to a hit. Must be >= 0.5.", optVal(prob_hit))
    ("miss", "The occupancy probability due to a miss. Must be < 0.5.", optVal(prob_miss))
    ("voxel-mean", "Enable voxel mean coordinates?", optVal(voxel_mean))
    ("traversal", "Enable traversal layer?", optVal(traversal))
    ("threshold", "Sets the occupancy threshold assigned when exporting the map to a cloud.", optVal(prob_thresh)->implicit_value(optStr(prob_thresh)))
    ("tsdf", "Build a tsdf map instead of an occupancy map. Incompatible with other voxel or occupancy options.", optVal(tsdf_enabled))
    ("tsdf-max-weight", "Maximum TSDF voxel weight.", optVal(tsdf.max_weight))
    ("tsdf-default-truncation", "Default truncation distance.", optVal(tsdf.default_truncation_distance))
    ("tsdf-dropoff", "Dropoff for the dropoff adjustment. Zero or negative to disable.", optVal(tsdf.dropoff_epsilon))
    ("tsdf-sparsity", "Compensation for sparse data sets. Zero or negative to disable.", optVal(tsdf.sparsity_compensation_factor))
    ("mode", "Controls the mapping mode [ normal, sample, erode ]. The 'normal' mode is the default, with the full ray "
              "being integrated into the map. 'sample' mode only adds samples to increase occupancy, while 'erode' "
              "only erodes free space by skipping the sample voxels.", optVal(mode))
    ;
  // clang-format on
}


void OhmAppCpu::MapOptions::print(std::ostream &out)
{
  Super::MapOptions::print(out);
  glm::i16vec3 region_dim = region_voxel_dim;
  region_dim.x = (region_dim.x) ? region_dim.x : OHM_DEFAULT_CHUNK_DIM_X;
  region_dim.y = (region_dim.y) ? region_dim.y : OHM_DEFAULT_CHUNK_DIM_Y;
  region_dim.z = (region_dim.z) ? region_dim.z : OHM_DEFAULT_CHUNK_DIM_Z;
  out << "Map region dimensions: " << region_dim << '\n';
  if (!tsdf_enabled)
  {
    out << "Mapping mode: " << mode << '\n';
    out << "Voxel mean position: " << (voxel_mean ? "on" : "off") << '\n';
    // out << "Map region memory: " << logutil::Bytes(ohm::OccupancyMap::voxelMemoryPerRegion(region_voxel_dim))
    //     << '\n';
    out << "Hit probability: " << prob_hit << " (" << ohm::probabilityToValue(prob_hit) << ")\n";
    out << "Miss probability: " << prob_miss << " (" << ohm::probabilityToValue(prob_miss) << ")\n";
    out << "Probability threshold: " << prob_thresh << '\n';
    out << "Probability range: [" << ohm::valueToProbability(prob_range[0]) << ' '
        << ohm::valueToProbability(prob_range[1]) << "]\n";
    out << "Value range      : [" << prob_range[0] << ' ' << prob_range[1] << "]\n";
  }
  else
  {
    out << "TSDF mode: fast\n";
    out << "TSDF max weight: " << tsdf.max_weight << '\n';
    out << "TSDF default truncation distance: " << tsdf.default_truncation_distance << '\n';
    out << "TSDF dropoff epsilon: ";
    if (tsdf.dropoff_epsilon > 0)
    {
      out << tsdf.dropoff_epsilon << '\n';
    }
    else
    {
      out << "off\n";
    }
    out << "TSDF sparsity compensation factor: ";
    if (tsdf.sparsity_compensation_factor > 0)
    {
      out << tsdf.sparsity_compensation_factor << '\n';
    }
    else
    {
      out << "off\n";
    }
  }

  out << "Ray length max: " << ray_length_max << '\n';
}


OhmAppCpu::NdtOptions::NdtOptions()
{
  ohm::OccupancyMap defaults_map;
  const ohm::NdtMap defaults_ndt(&defaults_map, true);
  // Default probabilities may differ for NDT.
  prob_hit = defaults_map.hitProbability();
  prob_miss = defaults_map.missProbability();
  adaptation_rate = defaults_ndt.adaptationRate();
  sensor_noise = defaults_ndt.sensorNoise();
  covariance_reset_probability = ohm::valueToProbability(defaults_ndt.reinitialiseCovarianceThreshold());
  covariance_reset_sample_count = defaults_ndt.reinitialiseCovariancePointCount();
  adaptation_rate = defaults_ndt.adaptationRate();
}


OhmAppCpu::NdtOptions::~NdtOptions() = default;


void OhmAppCpu::NdtOptions::configure(cxxopts::Options &parser)
{
  cxxopts::OptionAdder adder = parser.add_options("Ndt");
  configure(adder);
}


void OhmAppCpu::NdtOptions::configure(cxxopts::OptionAdder &adder)
{
  // clang-format off
  adder
    ("ndt", "Normal distribution transform (NDT) occupancy map generation mode {off,om,tm}. Mode om is the NDT occupancy mode, where tm adds traversability mapping data.", optVal(mode)->implicit_value(optStr(ohm::NdtMode::kOccupancy)))
    ("ndt-cov-point-threshold", "Minimum number of samples requires in order to allow the covariance to reset at --ndt-cov-prob-threshold..", optVal(covariance_reset_sample_count))
    ("ndt-cov-prob-threshold", "Low probability threshold at which the covariance can be reset as samples accumulate once more. See also --ndt-cov-point-threshold.", optVal(covariance_reset_probability))
    ("ndt-adaptation-rate", "NDT adaptation rate [0, 1]. Controls the clearing effect of the NDT. "
                            "Should be kept in proportion with miss value such as 'a(1 - 2 * miss)' "
                            "where 'a' is a chosen scale factor (1 for comparable strength to the miss value).", optVal(adaptation_rate))
    ("ndt-sensor-noise", "Range sensor noise used for Ndt mapping. Must be > 0.", optVal(sensor_noise))
    ;
  // clang-format on
}


void OhmAppCpu::NdtOptions::print(std::ostream &out)
{
  if (bool(mode))
  {
    out << "NDT mode: " << mode << '\n';
    out << "NDT adaptation rate: " << adaptation_rate << '\n';
    out << "NDT sensor noise: " << sensor_noise << '\n';
    out << "NDT covariance reset probability: " << covariance_reset_probability << '\n';
    out << "NDT covariance reset sample cout: " << covariance_reset_sample_count << '\n';
  }
}


OhmAppCpu::CompressionOptions::CompressionOptions()
{
  ohm::VoxelBlockCompressionQueue cq(true);  // Create in test mode.
  high_tide = logutil::Bytes(cq.highTide());
  low_tide = logutil::Bytes(cq.lowTide());
}


OhmAppCpu::CompressionOptions::~CompressionOptions() = default;


void OhmAppCpu::CompressionOptions::configure(cxxopts::Options &parser)
{
  cxxopts::OptionAdder adder = parser.add_options("Compression");
  configure(adder);
}

void OhmAppCpu::CompressionOptions::configure(cxxopts::OptionAdder &adder)
{
  // clang-format off
  adder
    ("high-tide", "Set the high memory tide which the background compression thread will try keep below.", optVal(high_tide))
    ("low-tide", "Set the low memory tide to which the background compression thread will try reduce to once high-tide is exceeded.", optVal(low_tide))
    ("uncompressed", "Maintain uncompressed map. By default, may regions may be compressed when no longer needed.", optVal(uncompressed))
  ;
  // clang-format on
}


void OhmAppCpu::CompressionOptions::print(std::ostream &out)
{
  out << "Compression: " << (uncompressed ? "off" : "on") << '\n';
  if (!uncompressed)
  {
    out << "  High tide: " << high_tide << '\n';
    out << "  Low tide: " << low_tide << '\n';
  }
}


OhmAppCpu::Options::Options()
{
  map_ = std::make_unique<OhmAppCpu::MapOptions>();
  ndt_ = std::make_unique<OhmAppCpu::NdtOptions>();
  compression_ = std::make_unique<OhmAppCpu::CompressionOptions>();
  default_help_sections.emplace_back("Ndt");
  default_help_sections.emplace_back("Compression");
}


void OhmAppCpu::Options::configure(cxxopts::Options &parser)
{
  Super::Options::configure(parser);
  ndt_->configure(parser);
  compression_->configure(parser);
}


void OhmAppCpu::Options::print(std::ostream &out)
{
  Super::Options::print(out);
  ndt_->print(out);
  compression_->print(out);
}


OhmAppCpu::OhmAppCpu(std::shared_ptr<ohmapp::DataSource> data_source)
  : Super(std::make_unique<Options>(), data_source)
{
  data_source->setSamplesOnly(false);
  data_source->requestBatchSettings(defaultBatchSize(), 0);
}


OhmAppCpu::OhmAppCpu(std::unique_ptr<Options> &&options, std::shared_ptr<ohmapp::DataSource> data_source)
  : Super(std::move(options), data_source)
{
  data_source->setSamplesOnly(false);
  data_source->requestBatchSettings(defaultBatchSize(), 0);
}


#if SLAMIO_HAVE_PDAL
#define CLOUD_TYPE "PDAL supported point cloud"
#else  // SLAMIO_HAVE_PDAL
#define CLOUD_TYPE "PLY point cloud"
#endif  // SLAMIO_HAVE_PDAL

std::string OhmAppCpu::description() const
{
  return "Generate an occupancy map from a ray cloud or a point cloud with accompanying "
         "trajectory file. The trajectory marks the scanner trajectory with timestamps "
         "loosely corresponding to cloud point timestamps. Trajectory points are "
         "interpolated for each cloud point based on corresponding times in the "
         "trajectory. A ray cloud uses the normals channel to provide a vector from "
         "point sample back to sensor location (see "
         "https://github.com/csiro-robotics/raycloudtools).\n"
         "\n"
         "The sample file is a " CLOUD_TYPE " file, while the trajectory is either a text "
         "trajectory containing [time x y z <additional>] items per line or is itself a "
         "point cloud file.";
}


int OhmAppCpu::validateOptions(const cxxopts::ParseResult &parsed)
{
  int return_code = Super::validateOptions(parsed);
  if (return_code)
  {
    return return_code;
  }

  // Derive ray_mode_flags from mode
  if (options().map().mode == "normal")
  {
    options().map().ray_mode_flags = ohm::kRfDefault;
  }
  else if (options().map().mode == "samples")
  {
    options().map().ray_mode_flags = ohm::kRfExcludeRay;
  }
  else if (options().map().mode == "erode")
  {
    options().map().ray_mode_flags = ohm::kRfExcludeSample;
  }
  else
  {
    std::cerr << "Unknown mode argument: " << options().map().mode << std::endl;
    return -1;
  }

  // Set default ndt probability if using.
  if (int(options().ndt().mode))
  {
    bool prob_hit_given = false;
    bool prob_miss_given = false;
    for (const auto &item : parsed.arguments())
    {
      if (item.key() == "hit")
      {
        prob_hit_given = true;
      }
      if (item.key() == "miss")
      {
        prob_miss_given = true;
      }
    }

    if (!prob_hit_given)
    {
      // Use ndt default hit prob
      options().map().prob_hit = options().ndt().prob_hit;
    }

    if (!prob_miss_given)
    {
      // Use ndt default hit prob
      options().map().prob_miss = options().ndt().prob_miss;
    }
  }
  return 0;
}


int OhmAppCpu::prepareForRun()
{
  ohm::MapFlag map_flags = ohm::MapFlag::kDefault;
  map_flags |= (options().map().voxel_mean) ? ohm::MapFlag::kVoxelMean : ohm::MapFlag::kNone;
  map_flags &= (options().compression().uncompressed) ? ~ohm::MapFlag::kCompressed : ~ohm::MapFlag::kNone;
  map_ = std::make_unique<ohm::OccupancyMap>(options().map().resolution, options().map().region_voxel_dim, map_flags);

  // Make sure we build layers before initialising any GPU map. Otherwise we can cache the wrong GPU programs.
  if (options().map().voxel_mean)
  {
    map_->addVoxelMeanLayer();
  }
  if (options().map().traversal)
  {
    map_->addTraversalLayer();
  }

  std::unique_ptr<ohm::NdtMap> ndt_map;
  if (bool(options().ndt().mode))
  {
    ndt_map_ = std::make_unique<ohm::NdtMap>(map_.get(), true, options().ndt().mode);
    ndt_map_->setAdaptationRate(options().ndt().adaptation_rate);
    ndt_map_->setSensorNoise(options().ndt().sensor_noise);
    ndt_map_->setReinitialiseCovarianceThreshold(ohm::probabilityToValue(options().ndt().covariance_reset_probability));
    ndt_map_->setReinitialiseCovariancePointCount(options().ndt().covariance_reset_sample_count);

    true_mapper_ = std::make_unique<ohm::RayMapperNdt>(ndt_map_.get());
  }
  else if (options().map().tsdf_enabled)
  {
    // Create a new layout with just the TSDF layer. We won't need occupancy.
    ohm::MapLayout layout;
    addTsdf(layout);
    map_->updateLayout(layout);
    auto tsdf_mapper = std::make_unique<ohm::RayMapperTsdf>(map_.get());
    tsdf_mapper->setTsdfOptions(options().map().tsdf);
    true_mapper_ = std::move(tsdf_mapper);
  }
  else
  {
    true_mapper_ = std::make_unique<ohm::RayMapperOccupancy>(map_.get());
  }

  map_->setHitProbability(options().map().prob_hit);
  map_->setOccupancyThresholdProbability(options().map().prob_thresh);
  map_->setMissProbability(options().map().prob_miss);
  if (options().map().prob_range[0] || options().map().prob_range[1])
  {
    map_->setMinVoxelValue(options().map().prob_range[0]);
    map_->setMaxVoxelValue(options().map().prob_range[1]);
  }
  // map_->setSaturateAtMinValue(options().map().saturateMin);
  // map_->setSaturateAtMaxValue(options().map().saturateMax);

  // Prevent ready saturation to free.
  // map_->setClampingThresMin(0.01);

  // Ensure options reflect map flags.
  options().map().voxel_mean = map_->voxelMeanEnabled();
  options().map().traversal = map_->traversalEnabled();

  if (options().map().ray_length_max > 0)
  {
    const auto ray_length_max = options().map().ray_length_max;
    map_->setRayFilter([ray_length_max](glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags) {
      return ohm::clipRayFilter(start, end, filter_flags, ray_length_max);
    });
  }
  else
  {
    // No ray length filter installed, but make sure we skip bad rays.
    map_->setRayFilter([](glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags) {
      return ohm::goodRayFilter(start, end, filter_flags, 0);
    });
  }

  mapper_ = true_mapper_.get();
#ifdef TES_ENABLE
  if (!options().output().trace.empty() && !options().output().trace_final)
  {
    trace_mapper_ = std::make_unique<ohm::RayMapperTrace>(map_.get(), true_mapper_.get());
    mapper_ = trace_mapper_.get();
  }
#endif  // TES_ENABLE

  if (dataSource()->options().return_number_mode != DataSource::ReturnNumberMode::Off)
  {
    map_->addLayer(ohm::default_layer::secondarySamplesLayerName(), ohm::addSecondarySamples);

    secondary_sample_mapper_ = std::make_unique<ohm::RayMapperSecondarySample>(map_.get());
    if (!secondary_sample_mapper_->valid())
    {
      secondary_sample_mapper_ = nullptr;
    }
  }

  return 0;
}


bool OhmAppCpu::processBatch(const glm::dvec3 &batch_origin, const std::vector<glm::dvec3> &sensor_and_samples,
                             const std::vector<double> &timestamps, const std::vector<float> &intensities,
                             const std::vector<glm::vec4> &colours, const std::vector<uint8_t> &return_numbers)
{
  (void)batch_origin;
  (void)colours;
  unsigned ray_mode_flags = options().map().ray_mode_flags;
  if (secondary_sample_mapper_ && !return_numbers.empty())
  {
    // Using dual returns. Add kRfExcludeOrigin flag.
    ray_mode_flags |= ohm::kRfExcludeOrigin;
  }
  mapper_->integrateRays(sensor_and_samples.data(), unsigned(sensor_and_samples.size()), intensities.data(),
                         timestamps.data(), ray_mode_flags);

  if (secondary_sample_mapper_ && !return_numbers.empty())
  {
    // Extract the secondary returns.
    secondary_returns_.clear();
    secondary_returns_.reserve(sensor_and_samples.size());
    for (size_t i = 0; i < return_numbers.size(); ++i)
    {
      if (return_numbers[i])
      {
        secondary_returns_.emplace_back(sensor_and_samples[i * 2 + 0]);
        secondary_returns_.emplace_back(sensor_and_samples[i * 2 + 1]);
      }
    }

    if (!secondary_returns_.empty())
    {
      secondary_sample_mapper_->integrateRays(secondary_returns_.data(), unsigned(secondary_returns_.size()), nullptr,
                                              nullptr, 0u);
    }
  }

  progress_.incrementProgressBy(timestamps.size());
  return !quitPopulation();
}


void OhmAppCpu::finaliseMap()
{
#ifdef TES_ENABLE
  if (map_)
  {
    ohm::debugDraw(*map_);
  }
#endif  // TES_ENABLE
}


int OhmAppCpu::saveMap(const std::string &path_without_extension)
{
  std::string output_file = path_without_extension + ".ohm";
  std::ostringstream out;
  out.imbue(std::locale(""));
  logutil::info("Saving map to ", output_file, '\n');

  std::unique_ptr<SerialiseMapProgress> save_progress;
  save_progress = std::make_unique<SerialiseMapProgress>(progress_, quitLevelPtr());
  progress_.unpause();

  int err = ohm::save(output_file.c_str(), *map_, save_progress.get());

  progress_.endProgress();
  if (!quiet())
  {
    std::cout << std::endl;
  }

  if (err)
  {
    logutil::error("Failed to save map: ", err, '\n');
  }

  return err;
}


int OhmAppCpu::saveCloud(const std::string &path_ply)
{
  // Save a cloud representation.
  logutil::info("Converting to point cloud.\n");

  ohmtools::ProgressCallback save_progress_callback;
  uint64_t point_count = 0;

  logutil::info("Saving point cloud to ", path_ply, '\n');
  progress_.beginProgress(ProgressMonitor::Info(map_->regionCount()));
  save_progress_callback = [this](size_t progress, size_t /*target*/) { progress_.updateProgress(progress); };

  if (!options().map().tsdf_enabled)
  {
    ohmtools::ColourByHeight colour_by_height(*map_);
    ohmtools::SaveCloudOptions save_opt;

    if (glm::all(glm::greaterThanEqual(options().output().cloud_colour, glm::vec3(0.0f))))
    {
      const ohm::Colour uniform_colour = ohm::Colour::fromRgbf(
        options().output().cloud_colour.r, options().output().cloud_colour.g, options().output().cloud_colour.b);
      save_opt.colour_select = [uniform_colour](const ohm::Voxel<const float> &) { return uniform_colour; };
    }
    else
    {
      save_opt.colour_select = [&colour_by_height](const ohm::Voxel<const float> &occupancy) {
        return colour_by_height.select(occupancy);
      };
    }

    point_count = ohmtools::saveCloud(path_ply.c_str(), *map_, save_opt, save_progress_callback);
  }
  else
  {
    ohmtools::ColourByHeight colour_by_height(*map_);
    ohmtools::ColourSelectTsdf colour_select;

    if (glm::all(glm::greaterThanEqual(options().output().cloud_colour, glm::vec3(0.0f))))
    {
      const ohm::Colour uniform_colour = ohm::Colour::fromRgbf(
        options().output().cloud_colour.r, options().output().cloud_colour.g, options().output().cloud_colour.b);
      colour_select = [uniform_colour](const ohm::Voxel<const ohm::VoxelTsdf> &) { return uniform_colour; };
    }
    else
    {
      colour_select = [&colour_by_height](const ohm::Voxel<const ohm::VoxelTsdf> &voxel) {
        return colour_by_height.select(voxel.key());
      };
    }

    point_count = ohmtools::saveTsdfCloud(
      path_ply.c_str(), *map_,
      std::min(options().map().tsdf.default_truncation_distance, 1.0f * float(options().map().resolution)),
      colour_select, save_progress_callback);
  }

  progress_.endProgress();
  progress_.pause();

  if (!quiet())
  {
    logutil::info("\nExported ", point_count, " point(s)\n");
  }

  return 0;
}


void OhmAppCpu::tearDown()
{
  // ndt_map->debugDraw();
  mapper_ = nullptr;
#ifdef TES_ENABLE
  trace_mapper_.release();
#endif  // TES_ENABLE
  true_mapper_.release();
  ndt_map_.release();
  map_.release();
}
}  // namespace ohmapp


================================================
FILE: ohmapp/OhmAppCpu.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMAPP_OHMPOPCPU_H_
#define OHMAPP_OHMPOPCPU_H_

#include "OhmAppConfig.h"

#include <ohmapp/MapHarness.h>

#include <ohm/MapSerialise.h>
#include <ohm/NdtMap.h>
#include <ohm/NdtMode.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapper.h>
#include <ohm/VoxelTsdf.h>

#include <logutil/LogUtil.h>

namespace ohmapp
{
/// Helper to display map serialisation progress.
class ohmapp_API SerialiseMapProgress : public ohm::SerialiseProgress
{
public:
  /// Create with the given @p monitor using @p quit_level to track whether it should quit (when > 1).
  explicit SerialiseMapProgress(ProgressMonitor &monitor, const unsigned *quit_level = nullptr)
    : monitor_(monitor)
    , quit_level_(quit_level)
  {}

  /// Check if quit_level was set and is greater than 1.
  bool quit() const override { return quit_level_ && *quit_level_ > 1; }

  void setTargetProgress(unsigned target) override { monitor_.beginProgress(ProgressMonitor::Info(target)); }
  void incrementProgress(unsigned inc) override { monitor_.incrementProgressBy(inc); }

private:
  ProgressMonitor &monitor_;
  const unsigned *quit_level_;
};

/// Population harness to generate an @c ohm::OccupancyMap using ohm CPU algorithms.
class ohmapp_API OhmAppCpu : public ohmapp::MapHarness
{
public:
  /// Base class alias.
  using Super = ohmapp::MapHarness;

  inline static constexpr unsigned defaultBatchSize() { return 4096u; }

  /// Specialised map options.
  struct ohmapp_API MapOptions : public Super::MapOptions
  {
    /// Maximum ray length. Longer rays are truncated to this length.
    double ray_length_max = 0.0;
    /// Specifies the number of voxels along each dimension of a chunk.
    glm::u8vec3 region_voxel_dim = glm::u8vec3(0);  // will be re-initialised from a default map
    /// Probability for an occupancy hit.
    float prob_hit = 0.0f;  // will be re-initialised from a default map
    /// Probability for an occupancy miss.
    float prob_miss = 0.0f;  // will be re-initialised from a default map
    /// Occupancy probability threshold. Occupied when occupancy reaches this probability.
    float prob_thresh = 0.5f;                // will be re-initialised from a default map
    glm::vec2 prob_range = glm::vec2(0, 0);  // will be re-initialised from a default map
    /// String value for the "--mode" argument. This sets the value of @c ray_mode_flags - see that member.
    std::string mode = "normal";
    /// @c ohm::RayFlag selection based on the "--mode" argument which is mapped into the @c mode member.
    ///
    /// Supported modes:
    /// - "normal" (default) => @c ohm::kRfDefault
    /// - "sample" (default) => @c ohm::kRfExcludeRay
    /// - "erode" (default) => @c ohm::kRfExcludeSample
    unsigned ray_mode_flags = ohm::kRfDefault;
    /// Generate the map with voxel mean sub-voxel positioning?
    bool voxel_mean = false;
    /// Generate the map with voxel traversal for density queries?
    bool traversal = false;

    /// TSDF options.
    ohm::TsdfOptions tsdf{};
    bool tsdf_enabled = false;  ///< Enable TSDF mapping? Not compatible with occupancy.

    MapOptions();

    void configure(cxxopts::OptionAdder &adder) override;
    void print(std::ostream &out) override;
  };

  /// NDT map options.
  struct ohmapp_API NdtOptions
  {
    // NDT map probabilities should be much narrower. The NDT process is more precise.
    /// Probability for an NDT map occupancy hit.
    float prob_hit = 0.0;  // will be re-initialised from a default map
    /// Probability for an NDT map occupancy miss.
    float prob_miss = 0.0;  // will be re-initialised from a default map
    /// NDT adaptation rate.
    float adaptation_rate = 0.0f;  // will be re-initialised from a default map
    /// Expected sensor noise for NDT model.
    float sensor_noise = 0.0f;  // will be re-initialised from a default map
    /// Reset covariance probability.
    float covariance_reset_probability = 0.0f;  // will be re-initialised from a default map
    /// Reset covariance sample count.
    unsigned covariance_reset_sample_count = 0;  // will be re-initialised from a default map
    /// NDT mode
    ohm::NdtMode mode = ohm::NdtMode::kNone;

    NdtOptions();
    virtual ~NdtOptions();

    /// Configure the command line options for the given @c parser . Calls @c `configure(const cxxopts::OptionAdder &)`
    /// @param parser The command line parser.
    void configure(cxxopts::Options &parser);
    /// Add command line options.
    /// Derivations should override this to add their own options as well as calling this base version.
    /// @param adder Object to add command line options to.
    /// Print command line options to the given stream.
    /// Derivations should override this to print their own options as well as calling this base version.
    /// @param out Output stream to print configured options to.
    virtual void configure(cxxopts::OptionAdder &adder);
    /// Print command line options to the given stream.
    /// Derivations should override this to print their own options as well as calling this base version.
    /// @param out Output stream to print configured options to.
    virtual void print(std::ostream &out);
  };

  /// Background compression options.
  struct ohmapp_API CompressionOptions
  {
    /// High tide: invoke compression when this is exceeded.
    logutil::Bytes high_tide;
    /// Low tide: compress until this is reached.
    logutil::Bytes low_tide;
    /// True to disable compression.
    bool uncompressed = false;

    CompressionOptions();
    virtual ~CompressionOptions();

    /// Configure the command line options for the given @c parser . Calls @c `configure(const cxxopts::OptionAdder &)`
    /// @param parser The command line parser.
    void configure(cxxopts::Options &parser);
    /// Add command line options.
    /// Derivations should override this to add their own options as well as calling this base version.
    /// @param adder Object to add command line options to.
    /// Print command line options to the given stream.
    /// Derivations should override this to print their own options as well as calling this base version.
    /// @param out Output stream to print configured options to.
    virtual void configure(cxxopts::OptionAdder &adder);
    /// Print command line options to the given stream.
    /// Derivations should override this to print their own options as well as calling this base version.
    /// @param out Output stream to print configured options to.
    virtual void print(std::ostream &out);
  };

  /// Specialise collated options.
  struct ohmapp_API Options : public Super::Options
  {
    /// NDT mapping options
    std::unique_ptr<NdtOptions> ndt_;
    /// Compression options
    std::unique_ptr<CompressionOptions> compression_;

    inline MapOptions &map() { return static_cast<MapOptions &>(*map_); }
    inline const MapOptions &map() const { return static_cast<const MapOptions &>(*map_); }

    /// Access the ndt mapping options by reference. Should be overriden by derivation to return their own ndt options
    /// specialisation.
    /// @return The @c NdtOptions .
    inline NdtOptions &ndt() { return *ndt_; }
    /// @overload
    inline const NdtOptions &ndt() const { return *ndt_; }

    /// Access the compressions options by reference. Should be overriden by derivation to return their own options
    /// specialisation.
    /// @return The @c CompressionOptions .
    inline CompressionOptions &compression() { return *compression_; }
    /// @overload
    inline const CompressionOptions &compression() const { return *compression_; }

    Options();

    void configure(cxxopts::Options &parser) override;
    void print(std::ostream &out) override;
  };

  /// Default constructor.
  OhmAppCpu(std::shared_ptr<ohmapp::DataSource> data_source);

  std::string description() const override;

  /// Access options by reference.
  /// @return The application @c Options .
  Options &options() { return static_cast<Options &>(Super::options()); }

  /// @overload
  const Options &options() const { return static_cast<const Options &>(Super::options()); }

  /// Access the map object.
  /// @return The @c OccupancyMap .
  inline ohm::OccupancyMap *map() { return map_.get(); }
  /// @overload
  inline const ohm::OccupancyMap *map() const { return map_.get(); }

protected:
  /// Constructor for derivied classes.
  /// @param options Options pointer.
  /// @param data_source Data source pointer.
  OhmAppCpu(std::unique_ptr<Options> &&options, std::shared_ptr<ohmapp::DataSource> data_source);

  int validateOptions(const cxxopts::ParseResult &parsed) override;
  int prepareForRun() override;
  bool processBatch(const glm::dvec3 &batch_origin, const std::vector<glm::dvec3> &sensor_and_samples,
                    const std::vector<double> &timestamps, const std::vector<float> &intensities,
                    const std::vector<glm::vec4> &colours, const std::vector<uint8_t> &return_numbers) override;
  void finaliseMap() override;
  int saveMap(const std::string &path_without_extension) override;
  int saveCloud(const std::string &path_ply) override;
  void tearDown() override;

  /// Object used to process points from @c processBatch() . Normally the same as @c true_mapper_
  ohm::RayMapper *mapper_ = nullptr;
  /// The occupancy map
  std::unique_ptr<ohm::OccupancyMap> map_;
  /// NDT map used when NDT is enabled. Null when NDT is disabled.
  std::unique_ptr<ohm::NdtMap> ndt_map_;
  /// The @c RayMapper used to add points to the map.
  std::unique_ptr<ohm::RayMapper> true_mapper_;
  /// Debug drawing mapper. Installed in @c mapper_ when tracing is enabled.
  std::unique_ptr<ohm::RayMapper> trace_mapper_;
  /// Mapper for seconary sample points (dual returns).
  std::unique_ptr<ohm::RayMapper> secondary_sample_mapper_;
  /// Dual returns buffer.
  std::vector<glm::dvec3> secondary_returns_;
};
}  // namespace ohmapp

#endif  // OHMAPP_OHMPOPCPU_H_


================================================
FILE: ohmapp/OhmAppGpu.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmAppGpu.h"

#include <logutil/LogUtil.h>

#include <ohmgpu/GpuCache.h>
#include <ohmgpu/GpuMap.h>
#include <ohmgpu/GpuNdtMap.h>
#include <ohmgpu/GpuTsdfMap.h>
#include <ohmgpu/OhmGpu.h>

#include <ohmapp/DataSource.h>

#include <ohm/DefaultLayer.h>
#include <ohm/RayMapperSecondarySample.h>

#ifdef TES_ENABLE
#include <ohm/RayMapperTrace.h>
#endif  // TES_ENABLE

#include <gputil/gpuDevice.h>

// Must be after any argument type streaming operators.
#include <ohmutil/Options.h>

namespace ohmapp
{
OhmAppGpu::GpuOptions::GpuOptions()
{
  // Build GPU options set.
  device_option_types.resize(ohm::gpuArgsInfo(nullptr, nullptr, 0));
  device_options.resize(device_option_types.size() * 2);
  ohm::gpuArgsInfo(device_options.data(), device_option_types.data(), unsigned(device_option_types.size()));
}


size_t OhmAppGpu::GpuOptions::gpuCacheSizeBytes() const
{
  return size_t(cache_size_gb * double(ohm::GpuCache::kGiB));
}


void OhmAppGpu::GpuOptions::configure(cxxopts::Options &parser)
{
  auto adder = parser.add_options("Gpu");
  // clang-format off
  adder
    ("gpu-cache-size", "Configured the GPU cache size used to cache regions for GPU update. Floating point value specified in GiB. A zero value uses half the available GPU RAM, 1GiB or 3/4 of RAM in order of preference.", optVal(cache_size_gb))
    ("gpu-ray-segment-length", "Configure the maximum allowed ray length for a single GPU thread to process. Longer rays are broken into multiple segments.", optVal(ray_segment_length))
    ("forward", "Perform forward ray tracing. GPU defaults to reverse ray tracing for performance (lower voxel contention).", optVal(forward_trace))
    ;

  // clang-format on

  if (!device_options.empty())
  {
    for (size_t i = 0; i < device_options.size(); i += 2)
    {
      adder(device_options[i + 0], device_options[i + 1],
            device_option_types[i / 2] == 0 ? ::cxxopts::value<bool>() : ::cxxopts::value<std::string>());
    }
  }
}


void OhmAppGpu::GpuOptions::print(std::ostream &out)
{
  out << "Gpu cache size: " << logutil::Bytes(gpuCacheSizeBytes()) << '\n';
  out << "Gpu max ray segment: " << ray_segment_length << '\n';
  out << "Gpu ray tracing: " << (forward_trace ? "forward" : "reverse") << '\n';
}


OhmAppGpu::Options::Options()
{
  gpu_ = std::make_unique<GpuOptions>();
  default_help_sections.emplace_back("Gpu");
}

void OhmAppGpu::Options::configure(cxxopts::Options &parser)
{
  Super::Options::configure(parser);
  gpu_->configure(parser);
}


void OhmAppGpu::Options::print(std::ostream &out)
{
  Super::Options::print(out);
  gpu_->print(out);
}


OhmAppGpu::OhmAppGpu(std::shared_ptr<ohmapp::DataSource> data_source)
  : Super(std::make_unique<Options>(), data_source)
{}


OhmAppGpu::OhmAppGpu(std::unique_ptr<Options> &&options, std::shared_ptr<ohmapp::DataSource> data_source)
  : Super(std::move(options), data_source)
{}


int OhmAppGpu::parseCommandLineOptions(int argc, const char *const *argv)
{
  int return_code = Super::parseCommandLineOptions(argc, argv);
  if (return_code)
  {
    return return_code;
  }

  return_code = ohm::configureGpuFromArgs(argc, argv);

  // Handle GPU cache auto sizing.
  auto &opt_gpu = options().gpu();
  if (opt_gpu.cache_size_gb <= 0)
  {
    // Calculate the GPU cache size as half GPU RAM size or 1GiB - whichever is larger. 1 GiB is too large use
    // 3/4 of the GPU RAM.
    const uint64_t total_device_memory = ohm::gpuDevice().deviceMemory();
    // Seed with 3/4 of device memory.
    uint64_t target_gpu_cache_size = (total_device_memory * 3) / 4;
    // If this is more than 1GiB, use either 1GiB or half device memory.
    if (target_gpu_cache_size > ohm::GpuCache::kGiB)
    {
      target_gpu_cache_size = std::max<uint64_t>(total_device_memory / 2, ohm::GpuCache::kGiB);
    }
    opt_gpu.cache_size_gb = double(target_gpu_cache_size) / double(ohm::GpuCache::kGiB);
    // Cap the max auto allocation to 4GiB. Some embedded systems with unified RAM can try for very large cache size
    // which exceeds the maximum allowed allocation size. While GpuLayerCache caps this, we limit the auto selected size
    // to help avoid giving a false impression of having a very large amount of GPU memory allocated.
    opt_gpu.cache_size_gb = std::min(opt_gpu.cache_size_gb, 4.0);
    std::cout << "Auto select GPU cache upper bound: " << logutil::Bytes(opt_gpu.gpuCacheSizeBytes()) << std::endl;
  }

  return return_code;
}


ohm::GpuMap *OhmAppGpu::gpuMap()
{
  if (true_mapper_)
  {
    return static_cast<ohm::GpuMap *>(true_mapper_.get());
  }
  return nullptr;
}


const ohm::GpuMap *OhmAppGpu::gpuMap() const
{
  if (true_mapper_)
  {
    return static_cast<const ohm::GpuMap *>(true_mapper_.get());
  }
  return nullptr;
}


int OhmAppGpu::prepareForRun()
{
  int return_code = Super::prepareForRun();
  if (return_code)
  {
    return return_code;
  }

  // Will use GpuNdtMap instead.
  ndt_map_.release();

  const size_t gpu_cache_size = options().gpu().gpuCacheSizeBytes();
  unsigned reserve_batch_size = dataSource()->expectedBatchSize();
  if (!reserve_batch_size)
  {
    reserve_batch_size = defaultBatchSize();
  }

  if (!options().gpu().forward_trace)
  {
    options().map().ray_mode_flags |= ohm::kRfReverseWalk;
  }

  if (options().ndt().mode != ohm::NdtMode::kNone)
  {
    true_mapper_ =
      std::make_unique<ohm::GpuNdtMap>(map_.get(), true, reserve_batch_size, gpu_cache_size, options().ndt().mode);
  }
  else if (options().map().tsdf_enabled)
  {
    // Create a new layout with just the TSDF layer. We won't need occupancy.
    ohm::MapLayout layout;
    ohm::addTsdf(layout);
    map_->updateLayout(layout);
    auto tsdf_mapper = std::make_unique<ohm::GpuTsdfMap>(map_.get(), true, reserve_batch_size, gpu_cache_size);
    tsdf_mapper->setTsdfOptions(options().map().tsdf);
    true_mapper_ = std::move(tsdf_mapper);
  }
  else
  {
    true_mapper_ = std::make_unique<ohm::GpuMap>(map_.get(), true, reserve_batch_size, gpu_cache_size);
  }
  ohm::GpuMap *gpu_map = gpuMap();
  gpu_map->setRaySegmentLength(options().gpu().ray_segment_length);

  if (bool(options().ndt().mode))
  {
    ohm::NdtMap *ndt_map = &static_cast<ohm::GpuNdtMap *>(gpu_map)->ndtMap();
    ndt_map->setAdaptationRate(options().ndt().adaptation_rate);
    ndt_map->setSensorNoise(options().ndt().sensor_noise);
    ndt_map->setReinitialiseCovarianceThreshold(ohm::probabilityToValue(options().ndt().covariance_reset_probability));
    ndt_map->setReinitialiseCovariancePointCount(options().ndt().covariance_reset_sample_count);
  }

  // Ensure options reflect map flags.
  options().map().voxel_mean = map_->voxelMeanEnabled();
  options().map().traversal = map_->traversalEnabled();

  if (options().map().ray_length_max > 0)
  {
    const auto ray_length_max = options().map().ray_length_max;
    map_->setRayFilter([ray_length_max](glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags) {
      return ohm::clipRayFilter(start, end, filter_flags, ray_length_max);
    });
  }
  else
  {
    // No ray length filter installed, but make sure we skip bad rays.
    // GPU deals very poorly with Inf/NaN values.
    map_->setRayFilter([](glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags) {
      return ohm::goodRayFilter(start, end, filter_flags, 0);
    });
  }

  mapper_ = true_mapper_.get();
#ifdef TES_ENABLE
  if (!options().output().trace.empty() && !options().output().trace_final)
  {
    trace_mapper_ = std::make_unique<ohm::RayMapperTrace>(map_.get(), true_mapper_.get());
    mapper_ = trace_mapper_.get();
  }
#endif  // TES_ENABLE

  if (dataSource()->options().return_number_mode != DataSource::ReturnNumberMode::Off)
  {
    map_->addLayer(ohm::default_layer::secondarySamplesLayerName(), ohm::addSecondarySamples);

    secondary_sample_mapper_ = std::make_unique<ohm::RayMapperSecondarySample>(map_.get());
    if (!secondary_sample_mapper_->valid())
    {
      secondary_sample_mapper_ = nullptr;
    }
  }

  return return_code;
}


void OhmAppGpu::finaliseMap()
{
  if (auto *gpu_map = gpuMap())
  {
    logutil::info("syncing GPU voxels\n");
    gpu_map->syncVoxels();
  }
  Super::finaliseMap();
}
}  // namespace ohmapp


================================================
FILE: ohmapp/OhmAppGpu.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMAPP_OHMPOPGPU_H_
#define OHMAPP_OHMPOPGPU_H_

#include "OhmAppGpuConfig.h"

#include "OhmAppCpu.h"

#include <ohmgpu/GpuMap.h>
#include <ohmgpu/GpuNdtMap.h>

namespace ohmapp
{
/// Population harness to generate an @c ohm::OccupancyMap using ohm GPU algorithms.
class ohmapp_API OhmAppGpu : public OhmAppCpu
{
public:
  /// Base class alias.
  using Super = OhmAppCpu;

  /// GPU options.
  struct ohmapp_API GpuOptions
  {
    /// See @c ohm::gpuArgsInfo()
    std::vector<int> device_option_types;
    /// See @c ohm::gpuArgsInfo()
    std::vector<const char *> device_options;
    /// Size of the GPU cache in GiB - supports non integer values.
    double cache_size_gb = 0;
    /// Maximum ray length before breaking up into multiple rays before GPU processing. Zero to submit rays as is.
    double ray_segment_length = 0;
    /// Trace rays forwards. GPU defaults to reverse ray tracing for performance.
    bool forward_trace = false;

    GpuOptions();

    /// Requested GPU cache size in bytes.
    size_t gpuCacheSizeBytes() const;

    /// Configure the command line options for the given @c parser . Calls @c `configure(const cxxopts::OptionAdder &)`
    /// @param parser The command line parser.
    void configure(cxxopts::Options &parser);
    /// Print command line options to the given stream.
    /// Derivations should override this to print their own options as well as calling this base version.
    /// @param out Output stream to print configured options to.
    void print(std::ostream &out);
  };

  /// Specialise collated options.
  struct ohmapp_API Options : public Super::Options
  {
    /// GPU options.
    std::unique_ptr<GpuOptions> gpu_;

    /// Access the GPU options by reference. Should be overriden by derivation to return their own options
    /// specialisation.
    /// @return The @c GpuOptions .
    inline GpuOptions &gpu() { return *gpu_; }
    /// @overload
    inline const GpuOptions &gpu() const { return *gpu_; }

    Options();

    void configure(cxxopts::Options &parser) override;
    void print(std::ostream &out) override;
  };

  /// Default constructor.
  /// @param data_source Options which will manage the data loop and provide data to @c progressBatch()
  OhmAppGpu(std::shared_ptr<ohmapp::DataSource> data_source);

  int parseCommandLineOptions(int argc, const char *const *argv) override;

  /// Access options by reference.
  /// @return The application @c Options .
  Options &options() { return static_cast<Options &>(Super::options()); }
  /// @overload
  const Options &options() const { return static_cast<const Options &>(Super::options()); }

  /// Access the GPU map object.
  /// @return The @c GpuMap .
  ohm::GpuMap *gpuMap();
  /// @overload
  const ohm::GpuMap *gpuMap() const;

protected:
  /// Constructor for derivied classes.
  /// @param options Options pointer.
  /// @param data_source Data source pointer.
  OhmAppGpu(std::unique_ptr<Options> &&options, std::shared_ptr<ohmapp::DataSource> data_source);

  int prepareForRun() override;
  void finaliseMap() override;
};
}  // namespace ohmapp

#endif  // OHMAPP_OHMPOPGPU_H_


================================================
FILE: ohmapp/OhmAppGpuConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMAPPGPUCONFIG_H
#define OHMAPPGPUCONFIG_H

#include "OhmAppGpuExport.h"

#include <ohmapp/OhmAppConfig.h>

#endif  // OHMAPPGPUCONFIG_H


================================================
FILE: ohmapp/SlamIOSource.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "SlamIOSource.h"

#include <logutil/Logger.h>

#include <slamio/SlamCloudLoader.h>

#include <glm/glm.hpp>

#include <chrono>
#include <fstream>

// Must be after argument streaming operators.
#include <ohmutil/Options.h>

using namespace ohm;
namespace ohmapp
{
void SlamIOSource::Options::configure(cxxopts::OptionAdder &adder)
{
  Super::Options::configure(adder);
  // clang-format off
  adder
    ("batch-delta", "Maximum delta in the sensor movement before forcing a batch up. Zero/negative to disable.", optVal(sensor_batch_delta))
    ("batch-size", "The number of points to process in each batch. Controls debug display. In GPU mode, this controls the GPU grid size.", optVal(batch_size))
    ("cloud", "The input cloud (las/laz) to load.", cxxopts::value(cloud_file))
    ("points-only", "Assume the point cloud is providing points only. Otherwise a cloud file with no trajectory is considered a ray cloud.", optVal(point_cloud_only))
    ("preload", "Preload this number of points before starting processing. -1 for all. May be used for separating processing and loading time.", optVal(preload_count)->default_value("0")->implicit_value("-1"))
    ("sensor", "Offset from the trajectory to the sensor position. Helps correct trajectory to the sensor centre for better rays.", optVal(sensor_offset))
    ("trajectory", "The trajectory (text) file to load.", cxxopts::value(trajectory_file))
    ;
  // clang-format on
}


void SlamIOSource::Options::print(std::ostream &out)
{
  out << "Cloud: " << cloud_file;
  if (!trajectory_file.empty() && !point_cloud_only)
  {
    out << " + " << trajectory_file << '\n';
  }
  else
  {
    if (point_cloud_only)
    {
      out << " (no trajectory)\n";
    }
    else
    {
      out << " (ray cloud)\n";
    }
  }

  if (preload_count)
  {
    out << "Preload: ";
    if (preload_count < 0)
    {
      out << "all";
    }
    else
    {
      out << preload_count;
    }
    out << '\n';
  }

  if (sensor_batch_delta >= 0)
  {
    out << "Sensor batch delta: " << sensor_batch_delta << '\n';
  }
  if (batch_size)
  {
    out << "Points batch size: " << batch_size << '\n';
  }

  Super::Options::print(out);
}

SlamIOSource::SlamIOSource()
  : Super(std::make_unique<Options>())
{}


SlamIOSource::~SlamIOSource() = default;


std::string SlamIOSource::sourceName() const
{
  const auto extension_start = options().cloud_file.find_last_of('.');
  if (extension_start != std::string::npos)
  {
    return options().cloud_file.substr(0, extension_start);
  }
  return options().cloud_file;
}


uint64_t SlamIOSource::processedPointCount() const
{
  return processed_point_count_;
}


double SlamIOSource::processedTimeRange() const
{
  return processed_time_range_;
}


unsigned SlamIOSource::expectedBatchSize() const
{
  return options().batch_size;
}


void SlamIOSource::requestBatchSettings(unsigned batch_size, double max_sensor_motion)
{
  options().batch_size = batch_size;
  options().sensor_batch_delta = max_sensor_motion;
}


int SlamIOSource::validateOptions()
{
  if (options().cloud_file.empty())
  {
    logutil::error("Missing input cloud\n");
    return -1;
  }

  return 0;
}


int SlamIOSource::prepareForRun(uint64_t &point_count, const std::string &reference_name)
{
  loader_ = std::make_unique<slamio::SlamCloudLoader>();
  loader_->enableReturnNumberInference(options().return_number_mode == ReturnNumberMode::Auto);

  loader_->setErrorLog([this](const char *msg) { logutil::error(msg); });
  if (!options().trajectory_file.empty())
  {
    if (!loader_->openWithTrajectory(options().cloud_file.c_str(), options().trajectory_file.c_str()))
    {
      logutil::error("Error loading cloud ", options().cloud_file, " with trajectory ", options().trajectory_file,
                     '\n');
      return 1;
    }
  }
  else if (!options().point_cloud_only)
  {
    if (!loader_->openRayCloud(options().cloud_file.c_str()))
    {
      logutil::error("Error loading ray ", options().cloud_file, '\n');
      return 1;
    }
  }
  else if (options().point_cloud_only)
  {
    if (!loader_->openPointCloud(options().cloud_file.c_str()))
    {
      logutil::error("Error loading point cloud ", options().cloud_file, '\n');
      return 1;
    }
  }

  loader_->setSensorOffset(options().sensor_offset);

  Clock::time_point end_time;

  if (options().preload_count)
  {
    int64_t preload_count = options().preload_count;
    if (preload_count < 0 && options().point_limit)
    {
      preload_count = options().point_limit;
    }

    logutil::info("Preloading points");

    const auto start_time = Clock::now();
    if (preload_count < 0)
    {
      logutil::info('\n');
      loader_->preload();
    }
    else
    {
      logutil::info(" ", preload_count, '\n');
      loader_->preload(preload_count);
    }
    const auto end_time = Clock::now();
    const double preload_time =
      std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count() * 1e-3;
    logutil::info("Preload completed over ", preload_time, " seconds.", '\n');
  }

  const auto point_limit = options().point_limit;
  point_count = (point_limit) ? std::min<uint64_t>(point_limit, loader_->numberOfPoints()) : loader_->numberOfPoints();

  if (!reference_name.empty() && options().stats_mode == StatsMode::Csv)
  {
    const auto csv_stats_file = reference_name + "_stats.csv";
    stats_csv_ = std::make_unique<std::ofstream>(csv_stats_file.c_str());
    Stats::writeCsvHeader(*stats_csv_);
  }

  return 0;
}


int SlamIOSource::run(BatchFunction batch_function, unsigned *quit_level_ptr)
{
  const auto flush_on_exit = [this]() {
    if (stats_csv_)
    {
      stats_csv_->flush();
      stats_csv_.reset();
    };
  };

  time_point_start_ = Clock::now();
  if (!loader_)
  {
    flush_on_exit();
    return 1;
  }

  // Data samples array, optionall interleaved with sensor position.
  std::vector<glm::dvec3> sensor_and_samples;
  std::vector<glm::vec4> colours;
  std::vector<float> intensities;
  std::vector<double> timestamps;
  std::vector<uint8_t> return_numbers;
  glm::dvec3 batch_origin(0);
  glm::dvec3 last_batch_origin(0);
  // Update map visualisation every N samples.
  const size_t ray_batch_size = options().batch_size;
  double timebase = -1;
  double last_batch_timestamp = -1;
  double accumulated_motion = 0;
  double delta_motion = 0;
  bool warned_no_motion = false;

  //------------------------------------
  // Population loop.
  //------------------------------------
  Stats batch_stats{};
  slamio::SamplePoint sample{};
  bool point_pending = false;
  bool have_processed = false;
  bool finish = false;
  // Cache control variables
  const auto point_limit = options().point_limit;
  const auto time_limit = options().time_limit;
  const auto input_start_time = options().start_time;
  const auto sensor_batch_delta = options().sensor_batch_delta;

  // Get to the first sample tiem.
  // Read the first sample and set the time base.
  if (!loader_->nextSample(sample))
  {
    // No work to do.
    logutil::info("No points to process\n");
    flush_on_exit();
    return 0;
  }

  timebase = sample.timestamp;

  while (sample.timestamp - timebase < input_start_time)
  {
    if (!loader_->nextSample(sample))
    {
      logutil::info("No sample points before selected start time ", input_start_time, ". Nothign to do.\n");
      flush_on_exit();
      return 0;
    }
  }

  batch_origin = sample.origin;

  point_pending = true;
  global_stats_.data_time_start = batch_stats.data_time_start = timebase;
  global_stats_.process_time_start = batch_stats.process_time_start = 0;

  uint64_t process_points_local = 0;
  const uint8_t max_return_number =
    (options().return_number_mode != ReturnNumberMode::Off) ? std::numeric_limits<uint8_t>::max() : 0u;
  const bool use_return_number = loader_->hasReturnNumber() || loader_->returnNumberInference();
  processed_point_count_ = 0;
  processed_time_range_ = 0;
  while ((process_points_local < point_limit || point_limit == 0) &&
         (last_batch_timestamp - timebase < time_limit || time_limit == 0) && point_pending && !finish)
  {
    const double sensor_delta_sq = glm::dot(sample.origin - batch_origin, sample.origin - batch_origin);
    const bool sensor_delta_exceeded =
      sensor_batch_delta > 0 && sensor_delta_sq > sensor_batch_delta * sensor_batch_delta;

    // Add sample to the batch.
    if (!sensor_delta_exceeded)
    {
      if (!samplesOnly())
      {
        sensor_and_samples.emplace_back(sample.origin);
      }
      sensor_and_samples.emplace_back(sample.sample);
      colours.emplace_back(sample.colour);
      intensities.emplace_back(sample.intensity);
      timestamps.emplace_back(sample.timestamp);
      if (use_return_number)
      {
        return_numbers.emplace_back(std::min(sample.return_number, max_return_number));
      }

      const double ray_length = glm::length(sample.sample - sample.origin);
      batch_stats.ray_length_minimum = std::min(ray_length, batch_stats.ray_length_minimum);
      batch_stats.ray_length_maximum = std::max(ray_length, batch_stats.ray_length_maximum);
      batch_stats.ray_length_total += ray_length;

      point_pending = false;
    }
    else
    {
      // Flag a point as being pending so it's added on the next loop.
      point_pending = true;
    }

    if (!timestamps.empty() && (sensor_delta_exceeded || timestamps.size() >= ray_batch_size ||
                                point_limit && process_points_local + timestamps.size() >= point_limit))
    {
      finish = !processBatch(batch_function, batch_origin, sensor_and_samples, timestamps, intensities, colours,
                             return_numbers, batch_stats);

      delta_motion = glm::length(batch_origin - last_batch_origin);
      accumulated_motion += delta_motion;
      last_batch_origin = batch_origin;

      if (have_processed && !warned_no_motion && delta_motion == 0 && !timestamps.empty())
      {
        // Precisely zero motion seems awfully suspicious.
        logutil::warn("\nWarning: Precisely zero motion in batch\n");
        warned_no_motion = true;
      }
      have_processed = true;
      process_points_local += timestamps.size();
      processed_point_count_ = process_points_local;
      processed_time_range_ = timestamps.back() - timebase;

      sensor_and_samples.clear();
      timestamps.clear();
      intensities.clear();
      colours.clear();
      return_numbers.clear();
    }

    if (!point_pending)
    {
      // Fetch next sample.
      point_pending = loader_->nextSample(sample);
    }
    batch_origin = (timestamps.empty()) ? sample.origin : batch_origin;

    finish = finish || (quit_level_ptr && *quit_level_ptr != 0u);
  }

  if (point_pending && (!point_limit || process_points_local < point_limit))
  {
    // Final point processing.
    if (!samplesOnly())
    {
      sensor_and_samples.emplace_back(sample.origin);
    }
    sensor_and_samples.emplace_back(sample.sample);
    colours.emplace_back(sample.colour);
    intensities.emplace_back(sample.intensity);
    timestamps.emplace_back(sample.timestamp);

    const double ray_length = glm::length(sample.sample - sample.origin);
    batch_stats.ray_length_minimum = std::min(ray_length, batch_stats.ray_length_minimum);
    batch_stats.ray_length_maximum = std::max(ray_length, batch_stats.ray_length_maximum);
    batch_stats.ray_length_total += ray_length;

    point_pending = false;
  }

  // Process the final batch.
  if (!timestamps.empty() && !finish)
  {
    processBatch(batch_function, last_batch_origin, sensor_and_samples, timestamps, intensities, colours,
                 return_numbers, batch_stats);
    process_points_local += timestamps.size();
    processed_point_count_ = process_points_local;
    processed_time_range_ = timestamps.back() - timebase;
  }

  const double motion_epsilon = 1e-6;
  if (accumulated_motion < motion_epsilon)
  {
    logutil::warn("Warning: very low accumulated motion: ", accumulated_motion, '\n');
  }

  loader_->close();
  loader_.release();

  flush_on_exit();
  return 0;
}


DataSource::Stats SlamIOSource::globalStats() const
{
  return global_stats_;
}


DataSource::Stats SlamIOSource::windowedStats() const
{
  std::unique_lock<std::mutex> guard(stats_lock_);
  return windowed_stats_;
}


void SlamIOSource::updateWindowedStats()
{
  Stats stats{};
  calculateWindowedStats(stats, windowed_stats_buffer_.begin(), windowed_stats_buffer_.end());
  std::unique_lock<std::mutex> guard(stats_lock_);
  windowed_stats_ = stats;
  guard.unlock();
  if (stats_csv_)
  {
    *stats_csv_ << stats << '\n';
  }
}

void SlamIOSource::addBatchStats(const Stats &stats)
{
  windowed_stats_buffer_next_ = DataSource::addBatchStats(stats, global_stats_, windowed_stats_buffer_,
                                                          windowed_stats_buffer_size_, windowed_stats_buffer_next_);

  // Update the windowed stats whenever the buffer is full. We essentially write whenever the ring buffer *next* write
  // index wraps around to 0.
  if (windowed_stats_buffer_next_ == 0)
  {
    updateWindowedStats();
  }
}


// Yes, try inline this private function.
inline bool SlamIOSource::processBatch(const BatchFunction &batch_function, const glm::dvec3 &batch_origin,
                                       const std::vector<glm::dvec3> &sensor_and_samples,
                                       const std::vector<double> &timestamps, const std::vector<float> &intensities,
                                       const std::vector<glm::vec4> &colours, const std::vector<uint8_t> &return_number,
                                       Stats &stats)
{
  const auto time_now = Clock::now();
  stats.process_time_end = std::chrono::duration<double>(time_now - time_point_start_).count();
  stats.data_time_end = (!timestamps.empty()) ? timestamps.back() : stats.data_time_start;
  stats.ray_count = unsigned(timestamps.size());
  const bool keep_processing =
    batch_function(batch_origin, sensor_and_samples, timestamps, intensities, colours, return_number);
  if (options().stats_mode != StatsMode::Off)
  {
    addBatchStats(stats);
  }
  stats.reset(stats.process_time_end, stats.data_time_end);

  return keep_processing;
}
}  // namespace ohmapp


================================================
FILE: ohmapp/SlamIOSource.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmAppConfig.h"

#include "DataSource.h"

#include <glm/vec3.hpp>

#include <atomic>
#include <chrono>
#include <memory>
#include <mutex>

namespace slamio
{
class SlamCloudLoader;
}

namespace ohmapp
{
/// A @c DataSource which loads from point clouds or ray clouds using @c slamio::SlamCloudLoader .
class ohmapp_API SlamIOSource : public DataSource
{
public:
  /// Base class alias.
  using Super = DataSource;

  /// Options specialisation.
  struct ohmapp_API Options : public Super::Options
  {
    /// Point cloud data file. May alternatively specify a ray cloud file where the normals prepresent a ray back to
    /// the sensor location. Ray clouds do not require a trajectory file.
    std::string cloud_file;
    /// Trajectory file. May be a point cloud or a text trajectory file. See @c slamio library.
    std::string trajectory_file;
    /// Translation offset to apply to the trajectory positions to get to resolve the sensor position.
    glm::dvec3 sensor_offset = glm::dvec3(0.0);
    /// Preload this number of points before starting map generation. -1 to preload all points. Can be used for timing.
    int64_t preload_count = 0;
    /// Delta sensor motion required before triggering a batch.
    double sensor_batch_delta = 0.0;
    /// Trigger a batch whenever this number of points have been loaded.
    unsigned batch_size = 4096;
    /// True to process a point cloud without a trajectory. No sensor positions are known, and the sensor positions are
    /// given as the sample positions.
    bool point_cloud_only = false;

    void configure(cxxopts::OptionAdder &adder) override;
    void print(std::ostream &out) override;
  };

  /// Default constructor.
  SlamIOSource();
  /// Destructor.
  ~SlamIOSource();

  /// Access options by reference.
  /// @return The application @c Options .
  inline Options &options() { return static_cast<Options &>(Super::options()); }
  /// @overload
  inline const Options &options() const { return static_cast<const Options &>(Super::options()); }

  std::string sourceName() const override;

  uint64_t processedPointCount() const override;
  double processedTimeRange() const override;
  unsigned expectedBatchSize() const override;
  void requestBatchSettings(unsigned batch_size, double max_sensor_motion) override;

  int validateOptions() override;
  int prepareForRun(uint64_t &point_count, const std::string &reference_name) override;

  int run(BatchFunction batch_function, unsigned *quit_level_ptr) override;

  Stats globalStats() const override;

  Stats windowedStats() const override;

private:
  /// Update and cache the @c windowedStats() .
  void updateWindowedStats();

  /// Add the given batch stats.
  /// @param stats Batch stats to add.
  void addBatchStats(const Stats &stats);

  /// Process a data batch, collecting stats.
  bool processBatch(const BatchFunction &batch_function, const glm::dvec3 &batch_origin,
                    const std::vector<glm::dvec3> &sensor_and_samples, const std::vector<double> &timestamps,
                    const std::vector<float> &intensities, const std::vector<glm::vec4> &colours,
                    const std::vector<uint8_t> &return_number, Stats &stats);

  using Clock = std::chrono::high_resolution_clock;

  /// Slam cloud loader. Valid after calling @c createSlamLoader() as called from @c run() .
  std::unique_ptr<slamio::SlamCloudLoader> loader_;
  /// Number of points processed. Must be kept up to date during @c run() for display and statistics.
  std::atomic<uint64_t> processed_point_count_{};
  /// Time range processed. Must be kept up to date during @c run() for display and statistics.
  std::atomic<double> processed_time_range_{};
  /// Stats window ring buffer.
  std::vector<Stats> windowed_stats_buffer_;
  /// Target buffer size for the stats window ring buffer.
  unsigned windowed_stats_buffer_size_ = 20;
  /// Next insertion index into the @c windowed_stats_buffer_ ring buffer.
  unsigned windowed_stats_buffer_next_ = 0;
  /// Global data stats.
  Stats global_stats_;
  /// Windowed data stats.
  Stats windowed_stats_;
  /// Processing (real) start time.
  Clock::time_point time_point_start_;
  /// Access guard for @c windowed_stats_
  mutable std::mutex stats_lock_;
  /// CSV logging stream for stats.
  std::unique_ptr<std::ostream> stats_csv_;
};
}  // namespace ohmapp


================================================
FILE: ohmapp/ohmappmain.inl
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
//
// Provdes a structure for an application which populates a map using MapHarness
#include "MapHarness.h"

#include <csignal>
#include <iostream>
#include <locale>
#include <memory>

namespace
{
/// Global cache of @c ohmapp::MapHarness::quitLevelPtr() updated in @c onSignal()
unsigned *g_quit = nullptr;

/// Handle @c SIGINT and @c SIGTERM by incrementing @c g_quit
void onSignal(int arg)
{
  if (arg == SIGINT || arg == SIGTERM)
  {
    if (g_quit)
    {
      ++(*g_quit);
    }
  }
}
}  // namespace

/// Main program implementation.
///
/// @param argc Command line argument count.
/// @param argv Command line arguments.
/// @param populator Map generation option.
int ohmappMain(int argc, const char *const *argv, ohmapp::MapHarness &populator)
{
  g_quit = populator.quitLevelPtr();
  signal(SIGINT, onSignal);
  signal(SIGTERM, onSignal);

  std::cout.imbue(std::locale(""));

  int exit_code = populator.parseCommandLineOptions(argc, argv);
  if (exit_code)
  {
    return exit_code;
  }

  exit_code = populator.run();
  return exit_code;
}

/// Overload using the template type as a @c ohmapp::MapHarness .
/// @param argc Command line argument count.
/// @param argv Command line arguments.
/// @param data_source Data source object.
template <typename OhmPop>
int ohmappMain(int argc, const char *const *argv, std::shared_ptr<ohmapp::DataSource> data_source)
{
  OhmPop populator(data_source);
  return ohmappMain(argc, argv, populator);
}

/// Overload using the template types as a @c ohmapp::MapHarness and @c ohmapp::DataSource .
/// @param argc Command line argument count.
/// @param argv Command line arguments.
template <typename OhmPop, typename DataSource>
int ohmappMain(int argc, const char *const *argv)
{
  return ohmappMain<OhmPop>(argc, argv, std::make_shared<DataSource>());
}


================================================
FILE: ohmgpu/CMakeLists.txt
================================================

include(GenerateExportHeader)
include(TextFileResource)


find_package(ZLIB)
if(OHM_FEATURE_THREADS)
  find_package(TBB)
endif(OHM_FEATURE_THREADS)

if(OHM_FEATURE_CUDA AND OHM_USE_DEPRECATED_CMAKE_CUDA)
  # Need to find CUDA for deprecated project configuration
  find_package(CUDA)
endif(OHM_FEATURE_CUDA AND OHM_USE_DEPRECATED_CMAKE_CUDA)

set(TES_ENABLE ${OHM_TES_DEBUG})
configure_file(OhmGpuConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/ohmgpu/OhmGpuConfig.h")

set(SOURCES
  private/ClearanceProcessDetail.cpp
  private/ClearanceProcessDetail.h
  private/GpuMapDetail.cpp
  private/GpuMapDetail.h
  private/GpuProgramRef.cpp
  private/GpuProgramRef.h
  private/GpuTransformSamplesDetail.h
  private/GpuTsdfMapDetail.h
  private/LineKeysQueryDetailGpu.h
  private/LineQueryDetailGpu.h
  private/RaysQueryDetailGpu.cpp
  private/RaysQueryDetailGpu.h
  private/RoiRangeFill.cpp
  private/RoiRangeFill.h
  ClearanceProcess.cpp
  ClearanceProcess.h
  GpuCache.cpp
  GpuCache.h
  GpuCachePostSyncHandler.h
  GpuCacheStats.h
  GpuKey.h
  GpuLayerCache.cpp
  GpuLayerCache.h
  GpuLayerCacheParams.cpp
  GpuLayerCacheParams.h
  GpuMap.cpp
  GpuMap.h
  GpuNdtMap.cpp
  GpuNdtMap.h
  GpuTransformSamples.cpp
  GpuTransformSamples.h
  GpuTsdfMap.cpp
  GpuTsdfMap.h
  LineKeysQueryGpu.cpp
  LineKeysQueryGpu.h
  LineQueryGpu.cpp
  LineQueryGpu.h
  OhmGpu.cpp
  OhmGpu.h
  RaysQueryGpu.cpp
  RaysQueryGpu.h
)

set(GPU_SOURCES
  gpu/CovarianceHitNdt.cl
  gpu/LineKeys.cl
  gpu/RaysQuery.cl
  gpu/RegionUpdate.cl
  gpu/RoiRangeFill.cl
  gpu/TransformSamples.cl
  gpu/TsdfUpdate.cl
)

get_target_property(OHM_SOURCE_DIR ohm SOURCE_DIR)
set(GPU_HEADERS
  gpu/AdjustNdt.cl
  gpu/AdjustOccupancy.cl
  gpu/CovarianceHitNdt_h.cl
  gpu/Traversal.cl
  gpu/LineWalk.cl
  gpu/LineWalkMarkers.cl
  gpu/VoxelIncident.cl
  gpu/VoxelMean.cl
  gpu/RaysQueryResult.h
  GpuKey.h
  # Need some headers from the OHM core project.
  ${OHM_SOURCE_DIR}/CovarianceVoxelCompute.h
  ${OHM_SOURCE_DIR}/LineWalkCompute.h
  ${OHM_SOURCE_DIR}/MapCoord.h
  ${OHM_SOURCE_DIR}/RayFlag.h
  ${OHM_SOURCE_DIR}/VoxelIncidentCompute.h
  ${OHM_SOURCE_DIR}/VoxelMeanCompute.h
  ${OHM_SOURCE_DIR}/VoxelTouchTimeCompute.h
  ${OHM_SOURCE_DIR}/VoxelTsdfCompute.h
)

set(PUBLIC_HEADERS
  ClearanceProcess.h
  GpuCache.h
  GpuLayerCacheParams.h
  GpuCachePostSyncHandler.h
  GpuCacheStats.h
  GpuKey.h
  GpuLayerCache.h
  GpuMap.h
  GpuNdtMap.h
  GpuTransformSamples.h
  LineKeysQueryGpu.h
  LineQueryGpu.h
  OhmGpu.h
  RaysQueryGpu.h
  "${CMAKE_CURRENT_BINARY_DIR}/ohmgpu/OhmGpuConfig.h"
  "${CMAKE_CURRENT_BINARY_DIR}/ohmgpu/OhmGpuExport.h"
  )

function(_ohmgpu_setup_target TARGET_NAME GPUTIL_LIBRARY OHM_GPU)
  target_compile_definitions(${TARGET_NAME} PUBLIC "-DOHM_GPU=${OHM_GPU}")
  # Because of the way we compile our GPU library twice with different names, we must explicitly define the export
  # macro. Curiously, there's a way to overide all the macros except the one used to control whether to export the
  # symbols or not. This puts us in a position where it could either be ohmcuda_EXPORTS or ohmocl_EXPORTS depending
  # on which targets are enabled. We build both the same way though, so define both symbols for all builds.
  target_compile_definitions(${TARGET_NAME} PRIVATE "-Dohmcuda_EXPORTS" "-Dohmocl_EXPORTS")
  target_link_libraries(${TARGET_NAME}
    PUBLIC
      ohm
      ohmutil
      $<$<BOOL:${OHM_FEATURE_THREADS}>:TBB::tbb>
    PRIVATE
      ${GPUTIL_LIBRARY}
  )

    if(BUILD_SHARED)
      # Link additional dependencies when building shared ohm.
      # Because ohm is shared and these are private, static depenencies, we do not need to propagate
      # the dependencies and limit this with $<BUILD_INTERFACE:>
      target_link_libraries(${TARGET_NAME}
        PRIVATE
          # Link 3es if enabled.
          $<BUILD_INTERFACE:$<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>>
      )
    else(BUILD_SHARED)
      # With ohm static, we link depdencencies in such as way that the will propagate and need to be
      # found when ohm is linked.
      target_link_libraries(${TARGET_NAME}
        PRIVATE
          # Link 3es if enabled.
          $<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>
      )
    endif(BUILD_SHARED)
  
  target_include_directories(${TARGET_NAME}
    PUBLIC
      $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}>
      # Access private ohm interfaces
      $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
      $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/ohmgpu>
      $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
    PRIVATE
      $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}>
      # The following include directories are required to get the GPU code building in all different context.
      # Otherwise we have a lot of book keeping in managing include statements.
      $<BUILD_INTERFACE:${OHM_SOURCE_DIR}>
      $<BUILD_INTERFACE:${OHM_SOURCE_DIR}/private>
    )

  target_include_directories(${TARGET_NAME}
    SYSTEM PRIVATE
    "${CMAKE_CURRENT_LIST_DIR}/3rdparty"
      $<$<NOT:$<BOOL:${OHM_TES_DEBUG}>>:${3ES_INCLUDE_DIRS}>
    )

  clang_tidy_target(${TARGET_NAME} EXCLUDE_MATCHES ".*\.cl")

  generate_export_header(${TARGET_NAME}
    EXPORT_MACRO_NAME ohmgpu_API
    DEPRECATED_MACRO_NAME ohmgpu_DEPRECATED
    NO_EXPORT_MACRO_NAME ohmgpu_NO_EXPORT
    EXPORT_FILE_NAME ohmgpu/OhmGpuExport.h
    NO_DEPRECATED_MACRO_NAME ohmgpu_NO_DEPRECATED
    STATIC_DEFINE OHMGPU_STATIC)

  install(TARGETS ${TARGET_NAME} EXPORT ${CMAKE_PROJECT_NAME}-config-targets
    LIBRARY DESTINATION lib
    ARCHIVE DESTINATION lib
    RUNTIME DESTINATION bin
    INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/ohmgpu
  )
endfunction(_ohmgpu_setup_target)


if(OHM_FEATURE_OPENCL)
  get_target_property(OHM_INCLUDE_DIRS ohm SOURCE_DIR)

  # Add gputil:gpu_ext.h to this project for correct marshalling.
  get_target_property(GPUTIL_INCLUDE_DIRS gputilocl SOURCE_DIR)
  get_target_property(GPUTIL_SOURCE_DIR gputilocl SOURCE_DIR)
  list(APPEND GPU_HEADERS ${GPUTIL_SOURCE_DIR}/gpu_ext.h)

  if(OHM_EMBED_GPU_CODE)
    # Embedding GPU code. Generate source file resources.
    # Only need to embed for OpenCL.
    foreach(CLSOURCE ${GPU_SOURCES})
      get_filename_component(CLSOURCENAME "${CLSOURCE}" NAME)
      get_filename_component(CLSOURCENAME_WE "${CLSOURCE}" NAME_WE)
      text_file_resource("${CLSOURCE}" "${CLSOURCENAME_WE}Code"
        TYPE opencl
        # ECHO
        PATHS "${CMAKE_CURRENT_LIST_DIR}" "${CMAKE_CURRENT_LIST_DIR}/gpu" "${OHM_INCLUDE_DIRS}" "${PROJECT_SOURCE_DIR}" "${GPUTIL_INCLUDE_DIRS}"
        # Append to the SOURCES list.
        FILELIST SOURCES
      )
    endforeach(CLSOURCE)
  else(OHM_EMBED_GPU_CODE)
    list(APPEND GPU_SOURCES gpu/LineWalk.cl)
    list(APPEND GPU_SOURCES gpu/LineWalkMarkers.cl)
    list(APPEND GPU_SOURCES gpu/Regions.cl)
  endif(OHM_EMBED_GPU_CODE)

  add_library(ohmocl ${SOURCES} ${GPU_SOURCES} ${GPU_HEADERS})
  _ohmgpu_setup_target(ohmocl gputilocl ${OHM_GPU_OPENCL})

  if(NOT OHM_EMBED_GPU_CODE)
    # Not embedding GPU source code. Make sure we marshal GPU headers as well as code.
    list(APPEND GPU_SOURCES "${GPU_HEADERS}")
    foreach(CLSOURCE ${GPU_SOURCES})
      get_filename_component(CLSOURCENAME "${CLSOURCE}" NAME)
      get_filename_component(CLSOURCEPATH "${CLSOURCE}" ABSOLUTE)
      # Not embedding GPU code. Marshal GPU source files.
      add_custom_command(
        OUTPUT "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/${CLSOURCENAME}"
        COMMAND "${CMAKE_COMMAND}" -E copy_if_different "${CLSOURCEPATH}" "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/${CLSOURCENAME}"
        MAIN_DEPENDENCY "${CLSOURCE}"
        COMMENT "Copying ${CLSOURCE}"
      )
    endforeach(CLSOURCE)
  endif(NOT OHM_EMBED_GPU_CODE)
endif(OHM_FEATURE_OPENCL)

if(OHM_FEATURE_CUDA)
  list(APPEND GPU_SOURCES
    gpu/CovarianceHitNdt.cu
    gpu/LineKeys.cu
    gpu/RaysQuery.cu
    gpu/RegionUpdate.cu
    gpu/RegionUpdateNdt.cu
    gpu/RoiRangeFill.cu
    gpu/TransformSamples.cu
    gpu/TsdfUpdate.cu
  )

  if(OHM_USE_DEPRECATED_CMAKE_CUDA)
    cuda_add_library(ohmcuda ${SOURCES} ${GPU_SOURCES} ${GPU_HEADERS})
  else(OHM_USE_DEPRECATED_CMAKE_CUDA)
    add_library(ohmcuda ${SOURCES} ${GPU_SOURCES} ${GPU_HEADERS})
    target_link_libraries(ohmcuda PUBLIC "${OHM_CUDA_LIBRARY}")
  endif(OHM_USE_DEPRECATED_CMAKE_CUDA)

  _ohmgpu_setup_target(ohmcuda gputilcuda ${OHM_GPU_CUDA})
endif(OHM_FEATURE_CUDA)

install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/ohmgpu)

source_group("source" REGULAR_EXPRESSION ".*$")
source_group("source\\cl" REGULAR_EXPRESSION "/cl/.*$")
source_group("source\\gpu" REGULAR_EXPRESSION "/gpu/.*$")
source_group("source\\private" REGULAR_EXPRESSION "/private/.*$")


================================================
FILE: ohmgpu/ClearanceProcess.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "ClearanceProcess.h"

#include "GpuCache.h"
#include "GpuLayerCache.h"
#include "GpuMap.h"
#include "OhmGpu.h"

#include "private/ClearanceProcessDetail.h"

#include <ohm/DefaultLayer.h>
#include <ohm/Key.h>
#include <ohm/MapChunk.h>
#include <ohm/MapLayout.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/QueryFlag.h>
#include <ohm/VoxelData.h>

#include <ohm/private/MapLayoutDetail.h>
#include <ohm/private/OccupancyMapDetail.h>
#include <ohm/private/OccupancyQueryAlg.h>
#include <ohm/private/VoxelAlgorithms.h>

#include <ohmutil/Profile.h>

#include <3esservermacros.h>

#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>

#ifdef OHM_FEATURE_THREADS
#include <tbb/blocked_range3d.h>
#include <tbb/parallel_for.h>
#endif  // OHM_FEATURE_THREADS

#include <algorithm>
#include <functional>
#include <iostream>
#include <memory>

#define VALIDATE_VALUES_UNCHANGED 0

namespace ohm
{
namespace
{
void regionClearanceProcessCpuBlock(OccupancyMap &map, ClearanceProcessDetail &query, const glm::ivec3 &block_start,
                                    const glm::ivec3 &block_end, const glm::i16vec3 &region_key, MapChunk *chunk,
                                    const glm::ivec3 &voxel_search_half_extents)
{
  Key voxel_key(nullptr);
  Voxel<float> voxel(&map, map.layout().clearanceLayer());
  float range;

  if (!voxel.isLayerValid())
  {
    return;
  }

  voxel_key.setRegionKey(region_key);
  for (int z = block_start.z; z < block_end.z; ++z)
  {
    voxel_key.setLocalAxis(2, z);
    for (int y = block_start.y; y < block_end.y; ++y)
    {
      voxel_key.setLocalAxis(1, y);
      for (int x = block_start.x; x < block_end.x; ++x)
      {
        voxel_key.setLocalAxis(0, x);
        voxel.setKey(voxel_key, chunk);
        assert(voxel.isValid() && voxel.voxelMemory());
        // if (!voxel.isNull()) // Don't think this is possible any more.
        {
          range = calculateNearestNeighbour(voxel_key, map, voxel_search_half_extents,
                                            (query.query_flags & kQfUnknownAsOccupied) != 0, false, query.search_radius,
                                            query.axis_scaling, (query.query_flags & kQfReportUnscaledResults) != 0);
          voxel.write(range);
        }
      }
    }
  }
}


void regionSeedFloodFillCpuBlock(OccupancyMap &map, ClearanceProcessDetail &query, const glm::ivec3 &block_start,
                                 const glm::ivec3 &block_end, const glm::i16vec3 &region_key, MapChunk *chunk,
                                 const glm::ivec3 & /*voxel_search_half_extents*/)
{
  Key voxel_key(nullptr);

  if (!chunk)
  {
    return;
  }

  Voxel<float> occupancy(&map, map.layout().occupancyLayer());
  Voxel<float> clearance(&map, map.layout().clearanceLayer());

  if (!occupancy.isLayerValid() && !clearance.isLayerValid())
  {
    return;
  }

  voxel_key.setRegionKey(region_key);
  for (int z = block_start.z; z < block_end.z; ++z)
  {
    voxel_key.setLocalAxis(2, z);
    for (int y = block_start.y; y < block_end.y; ++y)
    {
      voxel_key.setLocalAxis(1, y);
      for (int x = block_start.x; x < block_end.x; ++x)
      {
        voxel_key.setLocalAxis(0, x);
        occupancy.setKey(voxel_key, chunk);
        clearance.setKey(voxel_key, chunk);
        assert(occupancy.isValid() && occupancy.voxelMemory());
        assert(clearance.isValid() && clearance.voxelMemory());
        // if (!clearance.isNull())
        {
          if (isOccupied(occupancy) || ((query.query_flags & kQfUnknownAsOccupied) != 0 && isUnobserved(occupancy)))
          {
            clearance.write(0.0f);
          }
          else
          {
            clearance.write(-1.0f);
          }
        }
      }
    }
  }
}


void regionFloodFillStepCpuBlock(OccupancyMap &map, ClearanceProcessDetail & /*query*/, const glm::ivec3 &block_start,
                                 const glm::ivec3 &block_end, const glm::i16vec3 &region_key, MapChunk *chunk,
                                 const glm::ivec3 & /*voxel_search_half_extents*/)
{
  Key voxel_key(nullptr);
  Key neighbour_key(nullptr);
  float voxel_range;

  if (!chunk)
  {
    return;
  }

  Voxel<float> voxel_clearance(&map, map.layout().clearanceLayer());
  Voxel<const float> neighbour(&map, map.layout().clearanceLayer());

  if (!voxel_clearance.isLayerValid() || !neighbour.isLayerValid())
  {
    return;
  }

  voxel_key.setRegionKey(region_key);
  for (int z = block_start.z; z < block_end.z; ++z)
  {
    voxel_key.setLocalAxis(2, z);
    for (int y = block_start.y; y < block_end.y; ++y)
    {
      voxel_key.setLocalAxis(1, y);
      for (int x = block_start.x; x < block_end.x; ++x)
      {
        voxel_key.setLocalAxis(0, x);
        voxel_clearance.setKey(voxel_key);
        assert(voxel_clearance.isValid() && voxel_clearance.voxelMemory());
        // if (!voxel_clearance.isNull())
        {
          voxel_clearance.read(&voxel_range);
          for (int nz = -1; nz <= 1; ++nz)
          {
            for (int ny = -1; ny <= 1; ++ny)
            {
              for (int nx = -1; nx <= 1; ++nx)
              {
                if (nx || ny || nz)
                {
                  // This is wrong. It will propagate changed from this iteration. Not what we want.
                  neighbour_key = voxel_key;
                  map.moveKey(neighbour_key, nx, ny, nz);
                  neighbour.setKey(neighbour_key);
                  // Get neighbour value.
                  if (!neighbour.isNull())
                  {
                    float neighbour_range = -1.0f;
                    if (!neighbour.isNull())
                    {
                      neighbour.read(&neighbour_range);
                    }
                    if (neighbour_range >= 0)
                    {
                      // Adjust by range to neighbour.
                      neighbour_range += glm::length(glm::vec3(nx, ny, nz));
                      if (neighbour_range < voxel_range)
                      {
                        voxel_clearance.write(neighbour_range);
                      }
                    }
                  }
                }
              }
            }
          }
        }
      }
    }
  }

  chunk->touched_stamps[map.layout().clearanceLayer()] = chunk->dirty_stamp = map.touch();
}


unsigned regionClearanceProcessCpu(OccupancyMap &map, ClearanceProcessDetail &query, const glm::i16vec3 &region_key)
{
  OccupancyMapDetail &map_data = *map.detail();
  const auto chunk_search = map_data.chunks.find(region_key);
  glm::ivec3 voxel_search_half_extents;

  if (chunk_search == map_data.chunks.end())
  {
    // The entire region is unknown space. Nothing to do as we can't write to anything.
    return 0;
  }

  voxel_search_half_extents = ohm::calculateVoxelSearchHalfExtents(map, query.search_radius);
  MapChunk *chunk = chunk_search->second;

#ifdef OHM_FEATURE_THREADS
  const auto parallel_query_func = [&query, &map, region_key, chunk,
                                    voxel_search_half_extents](const tbb::blocked_range3d<int> &range) {
    regionClearanceProcessCpuBlock(map, query,
                                   glm::ivec3(range.cols().begin(), range.rows().begin(), range.pages().begin()),
                                   glm::ivec3(range.cols().end(), range.rows().end(), range.pages().end()), region_key,
                                   chunk, voxel_search_half_extents);
  };
  tbb::parallel_for(
    tbb::blocked_range3d<int>(0, map_data.region_voxel_dimensions.z, 0, map_data.region_voxel_dimensions.y, 0,
                              map_data.region_voxel_dimensions.x),
    parallel_query_func);

#else   // OHM_FEATURE_THREADS
  regionClearanceProcessCpuBlock(map, query, glm::ivec3(0, 0, 0), map_data.region_voxel_dimensions, region_key, chunk,
                                 voxel_search_half_extents);
#endif  // OHM_FEATURE_THREADS

  return unsigned(map.regionVoxelVolume());
}

unsigned regionSeedFloodFillCpu(OccupancyMap &map, ClearanceProcessDetail &query, const glm::i16vec3 &region_key,
                                const glm::ivec3 & /*voxel_extents*/, const glm::ivec3 &calc_extents)
{
  OccupancyMapDetail &map_data = *map.detail();
  const auto chunk_search = map_data.chunks.find(region_key);
  glm::ivec3 voxel_search_half_extents;

  if (chunk_search == map_data.chunks.end())
  {
    // The entire region is unknown space. Nothing to do as we can't write to anything.
    return 0;
  }

  voxel_search_half_extents = ohm::calculateVoxelSearchHalfExtents(map, query.search_radius);
  MapChunk *chunk = chunk_search->second;

#ifdef OHM_FEATURE_THREADS
  const auto parallel_query_func = [&query, &map, region_key, chunk,
                                    voxel_search_half_extents](const tbb::blocked_range3d<int> &range) {
    regionSeedFloodFillCpuBlock(map, query,
                                glm::ivec3(range.cols().begin(), range.rows().begin(), range.pages().begin()),
                                glm::ivec3(range.cols().end(), range.rows().end(), range.pages().end()), region_key,
                                chunk, voxel_search_half_extents);
  };
  tbb::parallel_for(
    tbb::blocked_range3d<int>(0, map_data.region_voxel_dimensions.z, 0, map_data.region_voxel_dimensions.y, 0,
                              map_data.region_voxel_dimensions.x),
    parallel_query_func);

#else   // OHM_FEATURE_THREADS
  regionSeedFloodFillCpuBlock(map, query, glm::ivec3(0, 0, 0), map_data.region_voxel_dimensions, region_key, chunk,
                              voxel_search_half_extents);
#endif  // OHM_FEATURE_THREADS

  return calc_extents.x * calc_extents.y * calc_extents.z;
}

unsigned regionFloodFillStepCpu(OccupancyMap &map, ClearanceProcessDetail &query, const glm::i16vec3 &region_key,
                                const glm::ivec3 & /*voxel_extents*/, const glm::ivec3 &calc_extents)
{
  OccupancyMapDetail &map_data = *map.detail();
  const auto chunk_search = map_data.chunks.find(region_key);
  glm::ivec3 voxel_search_half_extents;

  if (chunk_search == map_data.chunks.end())
  {
    // The entire region is unknown space. Nothing to do as we can't write to anything.
    return 0;
  }

  voxel_search_half_extents = ohm::calculateVoxelSearchHalfExtents(map, query.search_radius);
  MapChunk *chunk = chunk_search->second;

#ifdef OHM_FEATURE_THREADS
  const auto parallel_query_func = [&query, &map, region_key, chunk,
                                    voxel_search_half_extents](const tbb::blocked_range3d<int> &range) {
    regionFloodFillStepCpuBlock(map, query,
                                glm::ivec3(range.cols().begin(), range.rows().begin(), range.pages().begin()),
                                glm::ivec3(range.cols().end(), range.rows().end(), range.pages().end()), region_key,
                                chunk, voxel_search_half_extents);
  };
  tbb::parallel_for(
    tbb::blocked_range3d<int>(0, map_data.region_voxel_dimensions.z, 0, map_data.region_voxel_dimensions.y, 0,
                              map_data.region_voxel_dimensions.x),
    parallel_query_func);

#else   // OHM_FEATURE_THREADS
  regionFloodFillStepCpuBlock(map, query, glm::ivec3(0, 0, 0), map_data.region_voxel_dimensions, region_key, chunk,
                              voxel_search_half_extents);
#endif  // OHM_FEATURE_THREADS

  return calc_extents.x * calc_extents.y * calc_extents.z;
}
}  // namespace


ClearanceProcess::ClearanceProcess()
  : imp_(new ClearanceProcessDetail)
{
  imp_->gpu_query = std::make_unique<RoiRangeFill>(gpuDevice());
}


ClearanceProcess::ClearanceProcess(float search_radius, unsigned query_flags)
  : ClearanceProcess()
{
  setSearchRadius(search_radius);
  setQueryFlags(query_flags);
}


ClearanceProcess::~ClearanceProcess()
{
  ClearanceProcessDetail *d = imp();
  delete d;
  imp_ = nullptr;
}


float ClearanceProcess::searchRadius() const
{
  const ClearanceProcessDetail *d = imp();
  return d->search_radius;
}


void ClearanceProcess::setSearchRadius(float range)
{
  ClearanceProcessDetail *d = imp();
  d->search_radius = range;
}


unsigned ClearanceProcess::queryFlags() const
{
  const ClearanceProcessDetail *d = imp();
  return d->query_flags;
}


void ClearanceProcess::setQueryFlags(unsigned flags)
{
  ClearanceProcessDetail *d = imp();
  d->query_flags = flags;
}


glm::vec3 ClearanceProcess::axisScaling() const
{
  const ClearanceProcessDetail *d = imp();
  return d->axis_scaling;
}


void ClearanceProcess::setAxisScaling(const glm::vec3 &scaling)
{
  ClearanceProcessDetail *d = imp();
  d->axis_scaling = scaling;
}


void ClearanceProcess::reset()
{
  ClearanceProcessDetail *d = imp();
  d->resetWorking();
}


void ClearanceProcess::ensureClearanceLayer(OccupancyMap &map)
{
  if (map.layout().clearanceLayer() != -1)
  {
    return;
  }

  // Duplicate the layout, add the layer and update the map, preserving the current map.
  MapLayout updated_layout(map.layout());
  addClearance(updated_layout);
  map.updateLayout(updated_layout, true);
}


int ClearanceProcess::update(OccupancyMap &map, double time_slice)
{
  ClearanceProcessDetail *d = imp();

  // Ensure clearnce layer is present.
  ensureClearanceLayer(map);

  using Clock = std::chrono::high_resolution_clock;
  const auto start_time = Clock::now();
  double elapsed_sec = 0;

  // Fetch outdated regions.
  // Results must be ordered by region touch stamp.
  // Add to previous results. There may be repeated regions.
  // FIXME: if a region is added, the so must its neighbours be due to the flooding effect of the update.

  // Drop existing cached occupancy values before continuing.
  GpuCache *gpu_cache = gpumap::gpuCache(map);
  if (gpu_cache)
  {
    GpuLayerCache *clearance_cache = gpu_cache->layerCache(kGcIdClearance);
    // Technically there's not need to clear this. It never stores data we need back to CPU.
    clearance_cache->syncToMainMemory();
    clearance_cache->clear();
  }

  if (!d->haveWork())
  {
    d->getWork(map);
    const auto cur_time = Clock::now();
    elapsed_sec = std::chrono::duration_cast<std::chrono::duration<double>>(cur_time - start_time).count();
  }

  unsigned total_processed = 0;
  const glm::i16vec3 step(1);
  while (d->haveWork() && (time_slice <= 0 || elapsed_sec < time_slice))
  {
    // Iterate dirty regions
    updateRegion(map, d->current_dirty_cursor, false);
    // updateExtendedRegion(map, d->mapStamp, d->currentDirtyCursor, d->currentDirtyCursor + step - glm::i16vec3(1));
    d->stepCursor(step);

    total_processed += volumeOf(step);

    if (!d->haveWork())
    {
      d->getWork(map);
    }

    const auto cur_time = Clock::now();
    elapsed_sec = std::chrono::duration_cast<std::chrono::duration<double>>(cur_time - start_time).count();
  }

  return (total_processed != 0 || d->haveWork()) ? kMprProgressing : kMprUpToDate;
}


void ohm::ClearanceProcess::calculateForExtents(OccupancyMap &map, const glm::dvec3 &min_extents,
                                                const glm::dvec3 &max_extents, bool force)
{
  // Ensure clearnce layer is present.
  ensureClearanceLayer(map);

  const glm::i16vec3 min_region = map.regionKey(min_extents);
  const glm::i16vec3 max_region = map.regionKey(max_extents);
  //// Process in blocks containing up to this many regions in each dimension.
  // const int blockMax = 5;

  glm::i16vec3 region_key;

  // Drop existing cached occupancy values before continuing.
  GpuCache *gpu_cache = gpumap::gpuCache(map);
  if (gpu_cache)
  {
    GpuLayerCache *clearance_cache = gpu_cache->layerCache(kGcIdClearance);
    // Technically there's not need to clear this. It never stores data we need back to CPU.
    clearance_cache->syncToMainMemory();
    clearance_cache->clear();
  }

  for (int z = min_region.z; z <= max_region.z; ++z)
  {
    region_key.z = z;
    for (int y = min_region.y; y <= max_region.y; ++y)
    {
      region_key.y = y;
      for (int x = min_region.x; x <= max_region.x; ++x)
      {
        region_key.x = x;
        updateRegion(map, region_key, force);
      }
    }
  }
}

bool ClearanceProcess::updateRegion(OccupancyMap &map, const glm::i16vec3 &region_key, bool force)
{
  // Get the next region.
  ClearanceProcessDetail *d = imp();

  MapChunk *region = map.region(region_key, (d->query_flags & kQfInstantiateUnknown));
  if (!region)
  {
    return false;
  }

  const int occupancy_layer_index = map.layout().occupancyLayer();
  const int clearance_layer_index = map.layout().clearanceLayer();

  if (occupancy_layer_index < 0 || clearance_layer_index < 0)
  {
    return false;
  }

  // Explore the region neighbours to see if they are out of date. That would invalidate this region.
  glm::i16vec3 neighbour_key;

  // We are dirty if any input region has updated occupancy values since the last
  // region->touchedStamps[clearance_layer_index] value. We iterate to work out the maximum
  // touchedStamps[occupancy_layer_index] value in the neighbourhood and compare that to our region
  // clearance_layer_index stamp. Dirty if clearance stamp is lower. The target value also sets the new stamp to apply
  // to the region clearance stamp.
  uint64_t target_update_stamp = region->touched_stamps[occupancy_layer_index];
  for (int z = -1; z <= 1; ++z)
  {
    neighbour_key.z = region_key.z + z;
    for (int y = -1; y <= 1; ++y)
    {
      neighbour_key.y = region_key.y + y;
      for (int x = -1; x <= 1; ++x)
      {
        neighbour_key.x = region_key.x + x;
        MapChunk *neighbour = map.region(neighbour_key, false);
        if (neighbour)
        {
          target_update_stamp =
            std::max(target_update_stamp, uint64_t(neighbour->touched_stamps[occupancy_layer_index]));
        }
      }
    }
  }

  if (!force && region->touched_stamps[clearance_layer_index] >= target_update_stamp)
  {
    // Nothing to update in these extents.
    return false;
  }

  // Debug highlight the region.
  TES_BOX_W(g_tes, TES_COLOUR(FireBrick), tes::Id(&map),
            tes::Transform(tes::Vector3d(glm::value_ptr(map.regionSpatialCentre(region_key))),
                           tes::Vector3d(glm::value_ptr(map.regionSpatialResolution()))));

  if ((d->query_flags & kQfGpuEvaluate))
  {
    PROFILE(occupancyClearanceProcessGpu);
    d->gpu_query->setAxisScaling(d->axis_scaling);
    d->gpu_query->setSearchRadius(d->search_radius);
    d->gpu_query->setQueryFlags(d->query_flags);
    d->gpu_query->calculateForRegion(map, region_key);
  }
  else
  {
    std::function<unsigned(OccupancyMap &, ClearanceProcessDetail &, const glm::i16vec3 &, ClosestResult &)> query_func;

#if 1
    query_func = [](OccupancyMap &map, ClearanceProcessDetail &query, const glm::i16vec3 &region_key, ClosestResult &
                    /* closest */) -> unsigned { return regionClearanceProcessCpu(map, query, region_key); };
    ClosestResult closest;  // Not used.
    ohm::occupancyQueryRegions(map, *d, closest, map.regionSpatialMin(region_key) - glm::dvec3(d->search_radius),
                               map.regionSpatialMin(region_key) + glm::dvec3(d->search_radius), query_func);
#else   // #
    // Flood fill experiment. Incorrect as it will propagate changes from the current iteration
    // to some neighbours. Protect against that it it can be much faster than the brute force method.
    queryFunc = [&voxel_extents, &calc_extents](
                  OccupancyMap &map, ClearanceProcessDetail &query, const glm::i16vec3 &region_key, ClosestResult &
                  /* closest */) -> unsigned {
      // return regionClearanceProcessCpu(map, query, region_key, voxel_extents, calc_extents);
      return regionSeedFloodFillCpu(map, query, region_key, voxel_extents, calc_extents);
    };

    ClosestResult closest;  // Not used.
    ohm::occupancyQueryRegions(map, *d, closest, minExtents - glm::dvec3(d->searchRadius),
                               maxExtents + glm::dvec3(d->searchRadius), queryFunc);

    queryFunc = [&voxel_extents, &calc_extents, map](
                  OccupancyMap &constMap, ClearanceProcessDetail &query, const glm::i16vec3 &region_key, ClosestResult &
                  /* closest */) -> unsigned {
      return regionFloodFillStepCpu(map, query, region_key, voxel_extents, calc_extents);
    };

    for (unsigned i = 0; i < voxelPadding; ++i)
    {
      ohm::occupancyQueryRegions(map, *d, closest, d->minExtents - glm::dvec3(d->searchRadius),
                                 d->maxExtents + glm::dvec3(d->searchRadius), queryFunc);
    }
#endif  // #
  }

  TES_SERVER_UPDATE(g_tes, 0.0f);
  // Delete debug objects.
  // TES_SPHERE_END(g_tes, uint32_t((size_t)&map));
  TES_BOX_END(g_tes, uint32_t((size_t)&map));

  // Regions are up to date *now*.
  region->touched_stamps[clearance_layer_index] = target_update_stamp;
  return true;
}


ClearanceProcessDetail *ClearanceProcess::imp()
{
  return static_cast<ClearanceProcessDetail *>(imp_);
}


const ClearanceProcessDetail *ClearanceProcess::imp() const
{
  return static_cast<const ClearanceProcessDetail *>(imp_);
}
}  // namespace ohm


================================================
FILE: ohmgpu/ClearanceProcess.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_CLEARANCEPROCESS_H
#define OHMGPU_CLEARANCEPROCESS_H

#include "OhmGpuConfig.h"

#include <ohm/MappingProcess.h>
#include <ohm/QueryFlag.h>

#include <glm/fwd.hpp>

namespace ohm
{
struct ClearanceProcessDetail;
class OccupancyMap;

/// This query calculates the @c Voxel::clearance() for all voxels within the query extents.
///
/// > This class is experimental. There are many issues associated with keeping the GPU cache in sync and suboptimal.
///
/// For each voxel in the query expanse, the range to the nearest obstructed voxel (see @c Query) is calculated and
/// written back into the voxel's @c clearance value. The range of the calculation is limited by the
/// @c searchRange() and is not exhaustive.
///
/// The query @c minExtents() and @c maxExtents() loosely limit the zone for which obstacle ranges are calculated.
/// All voxels from all regions within the extents are calculated. That is, even a partial overlap with a range
/// results in all voxels for that region being calculated.
///
/// The CPU implementation performs a brute force O(nmm) where n is defined by the number of voxels within all regions
/// in the search extents, an m is defined by the number of voxels required to reach the @c searchRadius(). The GPU
/// implementation is closer to worst case O(m) although it incurs additional, initial overhead. The GPU
/// implementation is recommended over the CPU implementation.
///
/// Both CPU and GPU implementations keep track of which regions have been previously calculated. Results are not
/// recalculated for a region unless a hard @c reset() is performed.
///
/// Note that the CPU implementation is more accurate than the GPU algorithm. The former directly calculates ranges
/// to the nearest obstacles, while the latter uses an approximated flood fill. As a result, the GPU algorithm is
/// optimistic and may report longer ranges than it should.
///
/// The results written to @c Voxel::clearance() should be interpreted as follows:
/// - 0.0 => The voxel in question is itself an obstruction.
/// - > 0 => There is an obstructed voxel within the @c searchRadius().
/// - < 0 => There are no obstructions within the @c searchRadius().
///
/// This query respects the @c QF_UnknownAsOccupied flag, allowing unknown obstacles to be considered as obstacles.
///
/// Note that for this @c Query the following methods are invalid, have different semantics or
/// have no meaning:
/// - @c Query::numberOfResults() not used.
/// - @c Query::intersectedVoxels() contains no data.
/// - @c Query::ranges() contains no data.
///
/// Results are instead available via @c VoxelConst::clearance() or  @c Voxel::clearance().
///
/// @note This class is experimental and poorly maintained. Using @c update() to make progressive updates is known
/// to have non-deterministic results.
class ohmgpu_API ClearanceProcess : public MappingProcess
{
public:
  /// Extended query flags for @c ClearanceProcess.
  enum QueryFlag : unsigned
  {
    /// Instantiate regions which are in unknown space.
    kQfInstantiateUnknown = (kQfSpecialised << 0u),
  };

  /// Empty constructor.
  ClearanceProcess();

  /// Construct a new query using the given parameters.
  ///
  /// This constructor will instantiate a GPU cache in order to aid in the query.
  ///
  /// @param search_radius Defines the search radius around @p nearPoint.
  /// @param query_flags Flags controlling the query behaviour. See @c QueryFlag and @c ClearanceProcess::QueryFlag.
  ClearanceProcess(float search_radius, unsigned query_flags);
  /// Destructor.
  ~ClearanceProcess() override;

  /// Get the search radius to which we look for obstructing voxels.
  /// @return The radius to look for obstacles within.
  float searchRadius() const;
  /// Set the search radius to which we look for obstructing voxels.
  ///
  /// Modifying this value may require a cache reset.
  /// @param range The new search radius.
  void setSearchRadius(float range);

  /// The @c QueryFlag values applied to the process.
  /// @return The value values.
  unsigned queryFlags() const;
  /// Set the @c QueryFlag values for the process.
  /// @param flags The flag values to set.
  void setQueryFlags(unsigned flags);

  /// Get the axis weightings applied when determining the nearest obstructing voxel.
  /// @return Current axis weighting.
  /// @see @c setAxisScaling()
  glm::vec3 axisScaling() const;

  /// Set the per axis scaling applied when determining the closest obstructing voxel.
  ///
  /// This vector effectively distorts the map using the following scaling matrix:
  /// @code{.unparsed}
  ///   axisScaling.x   0               0
  ///   0               axisScaling.y   0
  ///   0               0               axisScaling.z
  /// @endcode
  ///
  /// The default scaling of (1, 1, 1) has no overall impact, meaning that the closest obstructing voxel is
  /// selected. Scaling of (1, 1, 2) scales the Z axis by a factor of two, making the Z axis half as important as
  /// either the X or Y axes.
  ///
  /// Results are scaled by calculating the distance between two voxels as show in the following pseudo code:
  /// @code
  ///   separation = nearestObstacleCentre - voxelCentre;
  ///   separation.x *= 1.0f * axisScaling.x;
  ///   separation.y *= 1.0f * axisScaling.y;
  ///   separation.z *= 1.0f * axisScaling.z;
  ///   float range = length(separation)
  /// @endcode
  ///
  /// Note a zero value causes the scaling on that axis to be zero, making all voxels on that axis logically,
  /// equidistant. This may yield undefined resuts. Negative values are not recommended, but will effectively be
  /// equivalent to their absolute value.
  ///
  /// Modifying this value will require a cache reset (@c reset(true)) to recalculate ranges.
  ///
  /// @param scaling The new axis scaling to apply.
  void setAxisScaling(const glm::vec3 &scaling);

  void reset() override;

  /// Ensure the mapping clearance layer is present in @p map .
  /// @param map The map to ensure has a clearance layer.
  static void ensureClearanceLayer(OccupancyMap &map);

  /// Update the processing queue to process part of the dirty list.
  /// @param map The map to process.
  /// @param time_slice The amount of time available for processing (seconds). Stop if exceeded.
  /// @return See @c MappingProcessResult.
  int update(OccupancyMap &map, double time_slice) override;

  /// Calculate clearance values for all regions within the given extents.
  ///
  /// This is call ignores the processing list, and blocks until the calculations are complete.
  ///
  /// The @p force flag (default @c true) ensures the recalculation is forced regardless of whether
  /// there are existing values or not. The region is still marked as having up to date clearance values.
  ///
  /// @param map The map to process.
  /// @param min_extents The minimum extents corner of the region to calculate.
  /// @param max_extents The maximum extents corner of the region to calculate.
  /// @param force Force recalculation of the clearance values even if they seem up to date.
  ///   This is required if any of the clearance calculation parameters change.
  void calculateForExtents(OccupancyMap &map, const glm::dvec3 &min_extents, const glm::dvec3 &max_extents,
                           bool force = true);

protected:
  /// Update clearance for the given region.
  /// @param map The operating map.
  /// @param region_key The key of the region to update.
  /// @param force Force update => update even if not dirty.
  /// @return True if work was done. False if nothing need be done.
  bool updateRegion(OccupancyMap &map, const glm::i16vec3 &region_key, bool force);

  /// Internal data access
  /// @return The internal data members.
  ClearanceProcessDetail *imp();
  /// Internal data access
  /// @return The internal data members.
  const ClearanceProcessDetail *imp() const;

private:
  ClearanceProcessDetail *imp_;
};
}  // namespace ohm

#endif  // OHMGPU_CLEARANCEPROCESS_H


================================================
FILE: ohmgpu/GpuCache.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "GpuCache.h"

#include "GpuLayerCache.h"
#include "GpuLayerCacheParams.h"

#include "OhmGpu.h"

#include "private/GpuMapDetail.h"

#include <ohm/MapChunk.h>
#include <ohm/VoxelBlock.h>
#include <ohm/VoxelBuffer.h>

#include <gputil/gpuDevice.h>

#include <memory>
#include <vector>

namespace ohm
{
// Support pre C++17 ODR
constexpr size_t GpuCache::kMiB;
constexpr size_t GpuCache::kGiB;
constexpr size_t GpuCache::kDefaultLayerMemSize;
constexpr size_t GpuCache::kDefaultTargetMemSize;


struct GpuCacheDetail
{
  std::vector<std::unique_ptr<GpuLayerCache>> layer_caches;
  gputil::Device gpu;
  gputil::Queue gpu_queue;
  OccupancyMap *map = nullptr;
  size_t target_gpu_alloc_size = 0;
  unsigned flags = 0;
};


GpuCache::GpuCache(OccupancyMap &map, size_t target_gpu_alloc_size, unsigned flags)
  : imp_(new GpuCacheDetail)
{
  imp_->gpu = ohm::gpuDevice();
  imp_->gpu_queue = imp_->gpu.defaultQueue();
  imp_->map = &map;
  imp_->target_gpu_alloc_size = target_gpu_alloc_size;
  imp_->flags = flags;
}


GpuCache::~GpuCache()
{
  delete imp_;
}


void GpuCache::reinitialise()
{
  reinitialiseGpuCache(this, *imp_->map, imp_->flags);
}


void GpuCache::flush()
{
  for (auto &&layer : imp_->layer_caches)
  {
    if (layer)
    {
      layer->syncToMainMemory();
    }
  }
}


void GpuCache::clear()
{
  for (auto &&layer : imp_->layer_caches)
  {
    if (layer)
    {
      layer->clear();
    }
  }
}


void GpuCache::removeLayers()
{
  imp_->layer_caches.clear();
}


void GpuCache::remove(const glm::i16vec3 &region_key)
{
  for (auto &&layer : imp_->layer_caches)
  {
    if (layer)
    {
      layer->remove(region_key);
    }
  }
}


bool GpuCache::syncLayerTo(MapChunk &dst_chunk, unsigned dst_layer, const MapChunk &src_chunk, unsigned src_layer)
{
  // Iterate the layers looking for the appropriate one. Note we accept the first one which matches the src_layer index.
  // There are situations where this could be incorrect as two layer caches use the same source layer, however,
  // this should be ok with how the layer indices are placed. See assumptions on @c GpuCacheId .
  //
  // Note (KS): this iteration isn't particularly efficient, but we should have small enough layers sets. A mapping of
  // src_layer to cache may be useful later. In practice this should be fine. The function was written for copyMap()
  // but that implementation now avoids this inefficiency by using `findLayerCache()` first. This implementation remains
  // for completeness.
  for (auto &&layer : imp_->layer_caches)
  {
    if (layer && layer->layerIndex() == src_layer)
    {
      VoxelBuffer<VoxelBlock> dst(dst_chunk.voxel_blocks[dst_layer]);
      if (layer->syncToExternal(dst, src_chunk.region.coord))
      {
        return true;
      }
      break;
    }
  }

  return false;
}


MapRegionCache *GpuCache::findLayerCache(unsigned layer)
{
  // Iterate the layers looking for the appropriate one. Note we accept the first one which matches the src_layer index.
  // There are situations where this could be incorrect as two layer caches use the same source layer, however,
  // this should be ok with how the layer indices are placed. See assumptions on @c GpuCacheId .
  //
  // Note (KS): this iteration isn't particularly efficient, but we should have small enough layers sets. A mapping of
  // src_layer to cache may be useful later.
  for (auto &&layer_cache : imp_->layer_caches)
  {
    if (layer_cache && layer_cache->layerIndex() == layer)
    {
      return layer_cache.get();
    }
  }

  return nullptr;
}


size_t GpuCache::targetGpuAllocSize() const
{
  return imp_->target_gpu_alloc_size;
}


unsigned GpuCache::layerCount() const
{
  return unsigned(imp_->layer_caches.size());
}


GpuLayerCache *GpuCache::createCache(unsigned id, const GpuLayerCacheParams &params)
{
  while (id >= imp_->layer_caches.size())
  {
    imp_->layer_caches.push_back(nullptr);
  }

  if (imp_->layer_caches[id])
  {
    // Already allocated
    return nullptr;
  }

  const size_t layer_mem_size = (params.gpu_mem_size) ? params.gpu_mem_size : kDefaultLayerMemSize;
  imp_->layer_caches[id] = std::make_unique<GpuLayerCache>(imp_->gpu, imp_->gpu_queue, *imp_->map, params.map_layer,
                                                           layer_mem_size, params.flags, params.on_sync);

  return imp_->layer_caches[id].get();
}


GpuLayerCache *GpuCache::layerCache(unsigned id)
{
  if (id >= imp_->layer_caches.size())
  {
    return nullptr;
  }

  return imp_->layer_caches[id].get();
}


gputil::Device &GpuCache::gpu()
{
  return imp_->gpu;
}


const gputil::Device &GpuCache::gpu() const
{
  return imp_->gpu;
}


gputil::Queue &GpuCache::gpuQueue()
{
  return imp_->gpu_queue;
}


const gputil::Queue &GpuCache::gpuQueue() const
{
  return imp_->gpu_queue;
}
}  // namespace ohm


================================================
FILE: ohmgpu/GpuCache.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_GPUCACHE_H
#define OHMGPU_GPUCACHE_H

#include "OhmGpuConfig.h"

#include <ohm/MapRegionCache.h>

#include <cstddef>

namespace gputil
{
class Device;
class Queue;
}  // namespace gputil

namespace ohm
{
struct GpuCacheDetail;
struct GpuLayerCacheParams;
class GpuLayerCache;
class OccupancyMap;

/// IDs for GPU caches.
///
/// Note: there are assumptions made about the ordering of these indices:
///
/// - IDs for read/write layers appear first. For example, @c kGcIdOccupancy which reads/writes occupancy appears
///   before @c kGcIdClearance which only reads occupancy and writes clearance values.
///
/// Locations where these assumptions are most relevant:
///
/// - @c GpuCache::syncLayerTo()
enum GpuCacheId
{
  /// Cache used for populating the map occupancy values.
  kGcIdOccupancy,
  /// Cache of occupancy map values when calculating voxel clearance values. Does not write back to host.
  ///
  /// This cache uploads data from the voxel occupancy layer, but never downloads back to CPU. Instead, the
  /// @c RoiRangeFill class maintains results in it's own buffer and copies these results back to the CPU layer.
  /// This means that the @c GpuLayerCache for the "clearance" voxel layer is always out of sync between CPU and GPU -
  /// they are designed to store different data.
  kGcIdClearance,
  /// Cache used for sub voxel positioning.
  kGcIdVoxelMean,
  /// Cache used for @c CovarianceVoxel data.
  kGcIdCovariance,
  /// Cache used for @c IntensityMeanCov
  kGcIdIntensity,
  /// Cache used for @c HitMissCount
  kGcIdHitMiss,
  /// Cache used for traversal layer.
  kGcIdTraversal,
  /// Cache used for touch time layer.
  kGcIdTouchTime,
  /// Cache used for incident normal layer.
  kGcIdIncidentNormal,
  /// Cache used for @c VoxelTsdf data.
  kGcIdTsdf,
};

/// Provides access to the @c GpuLayerCache objects used to cache host voxel data in GPU memory and manage
/// synchronisation.
///
/// The GPU cache contains a number of @c GpuLayerCache objects, each of which is bound to a particular @c MapLayer.
/// Each layer cache is initialised in the same way; sized to @c layerGpuMemSize() bytes and optionally mappable
/// (@c mappableBuffers()) to host memory.
///
/// @c GpuLayerCache objects are instantiates by @c layerCache(). Each cache is identified by user defined ID
/// and set to cache data from a specific @c MapLayer.
///
/// The @c GpuCache also defines the @c gputil::Device and @p gputil::Queue associated with GPU operations.
///
/// For more information on layer cache usage see @c GpuLayerCache.
class ohmgpu_API GpuCache : public MapRegionCache
{
public:
  /// Define for 1 MiB in bytes.
  static constexpr size_t kMiB = 1024ull * 1024ull;
  /// Define for 1 GiB in bytes.
  static constexpr size_t kGiB = 1024ull * 1024ull * 1024ull;
  /// The default byte size of each GPU layer if not specified.
  static constexpr size_t kDefaultLayerMemSize = kGiB / 2;
  /// Default total memory size to target.
  static constexpr size_t kDefaultTargetMemSize = 1 * kGiB;

  /// Instantiate the @c GpuCache for @p map.
  /// @param map The map to cache data for.
  /// @param target_gpu_alloc_size The GPU memory target size, distributed across all allocated @c GpuLayerCache
  /// objects (bytes). See @c targetGpuAllocSize() .
  /// @param flags The @c GpuFlag values to initialise the cache with.
  explicit GpuCache(OccupancyMap &map, size_t target_gpu_alloc_size = kDefaultTargetMemSize, unsigned flags = 0);

  /// Destructor, cleaning up all owned @c GpuLayerCache objects.
  ~GpuCache() override;

  /// Flush and clear the cache, then reallocate GPU buffers. For use when voxel layout changes.
  void reinitialise() override;

  /// Sync to main memory.
  void flush() override;

  /// Flush sync to main memory then drop all cache entries. Cache layers are preserved.
  void clear() override;

  /// Remove all layer caches.
  void removeLayers();

  /// Remove a particular region from the cache.
  /// @param region_key The region to flush from the cache.
  void remove(const glm::i16vec3 &region_key) override;

  /// Implement synching from this cache to another location.
  ///
  /// @param dst_chunk The chunk object to sync to.
  /// @param dst_layer The index to sync to in @c MapChunk::voxel_blocks .
  /// @param src_chunk The chunk to sync from.
  /// @param src_layer The layer to sync from.
  /// @return True if the source chunk/layer pairing are cached by this object and have been copied to the destination
  ///   chunk/layer pairing.
  bool syncLayerTo(MapChunk &dst_chunk, unsigned dst_layer, const MapChunk &src_chunk, unsigned src_layer) override;

  /// Find the @c GpuLayerCache for @p layer .
  MapRegionCache *findLayerCache(unsigned layer) override;

  /// Query the target GPU memory allocation byte size. This is the target allocation across all @c GpuLayerCache
  /// objects and is distributed amongst these objects. The distribution is weighted so that layers requiring more
  /// voxel data are given more of the allocation.
  ///
  /// @note Specifying an allocation size too small to cover all regions expected to be accessed in a single GPU
  /// batch update will result in undefined behaviour.
  ///
  /// @return The default size of for a layer cache in bytes.
  size_t targetGpuAllocSize() const;

  /// Returns the number of indexable layers. Some may be null.
  /// @return The number of indexable layers.
  unsigned layerCount() const;

  /// Creates a new @c GpuLayerCache with the specified @p params.
  ///
  /// The @p id should always be in a relative low range as a @c vector is used to allocate and access
  /// cache pointers.
  ///
  /// See @c GpuLayerCacheParams for configuration details.
  ///
  /// Fails when:
  /// - A layer cache with @p id already exists.
  ///
  /// @param id The unique ID for the cache.
  /// @param params Cache configuration.
  /// @return A pointer to the new cache on success, null on failure.
  GpuLayerCache *createCache(unsigned id, const GpuLayerCacheParams &params);

  /// Request or create a new @c GpuLayerCache.
  ///
  /// The cache is uniquely identified by the specified @p id, the sematics of which depend on usage. A default
  /// set of IDs is defined in @c GpuCacheId The call also specifies a @p layer which identifies the @c MapLayer
  /// associated with the cache.
  GpuLayerCache *layerCache(unsigned id);

  /// Access the GPU @c gputil::Device associated with GPU operations.
  /// @return The bound @c gputil::Device.
  gputil::Device &gpu();
  /// @overload
  const gputil::Device &gpu() const;
  /// Access the GPU @c gputil::Queue associated with GPU operations.
  /// @return The bound @c gputil::Queue.
  gputil::Queue &gpuQueue();
  /// @overload
  const gputil::Queue &gpuQueue() const;

private:
  GpuCacheDetail *imp_;
};
}  // namespace ohm

#endif  // OHMGPU_GPUCACHE_H


================================================
FILE: ohmgpu/GpuCachePostSyncHandler.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUCACHEPOSTSYNCHANDLER_H
#define GPUCACHEPOSTSYNCHANDLER_H

#include <OhmConfig.h>

#include <glm/fwd.hpp>

#include <functional>

namespace ohm
{
struct MapChunk;

using GpuCachePostSyncHandler = std::function<void(MapChunk *, const glm::u8vec3 &)>;
}  // namespace ohm

#endif  // GPUCACHEPOSTSYNCHANDLER_H


================================================
FILE: ohmgpu/GpuCacheStats.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_GPUCACHESTATS_H
#define OHMGPU_GPUCACHESTATS_H

#include "OhmGpuConfig.h"

#include <cinttypes>

namespace ohm
{
/// Running stats on a @c GpuLayerCache .
struct ohmgpu_API GpuCacheStats
{
  uint32_t hits = 0;    ///< Number of cache hits
  uint32_t misses = 0;  ///< Number of cache misses.
  uint32_t full = 0;    ///< Number of misses where the cache was full and something had to be dropped.
};
}  // namespace ohm

#endif  // OHMGPU_GPUCACHESTATS_H


================================================
FILE: ohmgpu/GpuKey.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUKEY_H
#define GPUKEY_H

#include "MapCoord.h"

/// printf formating string macro for @c GpuKey.
#define KEY_F "[%d %d %d : %d %d %d]"
/// printf argument expansion macro for @c GpuKey.
///
/// @code
/// GpuKey key = { 1, 2, 3, 4, 5, 6, 7 };
/// printf("Key is: " KEY_F "\n", KEY_A(key));
/// @endcode
#define KEY_A(key) \
  (key).region[0], (key).region[1], (key).region[2], (int)(key).voxel[0], (int)(key).voxel[1], (int)(key).voxel[2]

#if !GPUTIL_DEVICE

#ifndef __device__
#define __device__
#endif  // __device__
#ifndef __host__
#define __host__
#endif  // __host__

namespace ohm
{
#endif  // !GPUTIL_DEVICE
/// @c ohm::Key representation in GPU.
///
/// This structure must exactly match the memory alignment of ohm::Key.
typedef struct GpuKey  // NOLINT(readability-identifier-naming, modernize-use-using)
{
  /// Region key.
  short region[3];  // NOLINT(modernize-avoid-c-arrays, google-runtime-int)
  /// Voxel key.
  /// Element 3 is used to identify clipped ray keys. That is, voxel[3] = 1 for a "sample voxel" indicates we have
  /// actually clipped the sample ray and, while the voxel is the last relevant voxel in the ray, it is not the
  /// sample voxel.
  unsigned char voxel[4];  // NOLINT(modernize-avoid-c-arrays)
} GpuKey;
#if !GPUTIL_DEVICE
}  // namespace ohm
#endif  // !GPUTIL_DEVICE

#ifdef GPUTIL_DEVICE

/// Test for equality between two @c GpuKey objects.
/// @param a The first key.
/// @param b The second key.
/// @return True if @p a and @p b are exactly equal.
inline __device__ __host__ bool equalKeys(const GpuKey *a, const GpuKey *b);

__device__ __host__ void stepKeyAlongAxis(GpuKey *key, int axis, int step, const int3 regionDim);
__device__ __host__ void moveKeyAlongAxis(GpuKey *key, int axis, int step, const int3 regionDim);

#if GPUTIL_DEVICE == GPUTIL_OPENCL
// For some reason, Intel OpenCL does not like the assignment of `*out = *in;` - it silently fails - so we manually
// unpack the keys instead.
// We implement the copy using a macro because of issues with trying to use function overloading with address space
// qualifiers.
#define copyKey(out, in)              \
  (out)->region[0] = (in)->region[0]; \
  (out)->region[1] = (in)->region[1]; \
  (out)->region[2] = (in)->region[2]; \
  (out)->voxel[0] = (in)->voxel[0];   \
  (out)->voxel[1] = (in)->voxel[1];   \
  (out)->voxel[2] = (in)->voxel[2];   \
  (out)->voxel[3] = (in)->voxel[3]

#else   // GPUTIL_DEVICE == GPUTIL_OPENCL
__device__ __host__ void copyKey(GpuKey *out, const GpuKey *in);

inline __device__ __host__ void copyKey(GpuKey *out, const GpuKey *in)
{
  *out = *in;
}
#endif  // GPUTIL_DEVICE == GPUTIL_OPENCL

inline __device__ __host__ bool equalKeys(const GpuKey *a, const GpuKey *b)
{
  return a->region[0] == b->region[0] && a->region[1] == b->region[1] && a->region[2] == b->region[2] &&
         a->voxel[0] == b->voxel[0] && a->voxel[1] == b->voxel[1] && a->voxel[2] == b->voxel[2];
}

inline __device__ __host__ void stepKeyAlongAxis(GpuKey *key, int axis, int step, const int3 regionDim)
{
  const int axisDim = (axis == 0) ? regionDim.x : ((axis == 1) ? regionDim.y : regionDim.z);
  int vind = key->voxel[axis] + step;
  // Manage region stepping.
  key->region[axis] += (vind < 0) ? -1 : 0;        // Step down a region
  key->region[axis] += (vind >= axisDim) ? 1 : 0;  // Step up a region
  vind = (vind >= 0) ? vind : axisDim - 1;         // Underflow voxel index.
  vind = (vind < axisDim) ? vind : 0;              // Overflow voxel index.
  key->voxel[axis] = (uchar)vind;
}


inline __device__ __host__ void moveKeyAlongAxis(GpuKey *key, int axis, int step, const int3 regionDim)
{
  const int axisDim = (axis == 0) ? regionDim.x : ((axis == 1) ? regionDim.y : regionDim.z);

  // We first step within the chunk region. If we can't then we step the region and reset
  // stepped local axis value. We need to expand the byte precision of the voxel key to support region changes.
  int localKey[3] = { key->voxel[0], key->voxel[1], key->voxel[2] };
  localKey[axis] += step;
  // glm::i16vec3 regionKey = key.regionKey();
  if (step >= 0)
  {
    // Positive step or zero step.
    key->region[axis] += localKey[axis] / axisDim;
    localKey[axis] %= axisDim;
  }
  else
  {
    // Negative step.
    // Create a region step which simulates a floating point floor.
    key->region[axis] += ((localKey[axis] - (axisDim - 1)) / axisDim);
    if (localKey[axis] < 0)
    {
      // This is nuts. In C/C++, the % operator is not actually a modulus operator.
      // It's a "remainder" operator. A modulus operator should only give positive results,
      // but in C a negative input will generate a negative output. Through the magic of
      // StackOverflow, here's a good explanation:
      //  https://stackoverflow.com/questions/11720656/modulo-operation-with-negative-numbers
      // This means that here, given localKey[axis] is negative, the modulus:
      //    localKey[axis] % axisDim
      // will give results in the range (-axisDim, 0]. So, lets set the limit
      // to 4, then we get the following: like follows:
      //
      // i  i%4   4 - i%4
      //  0  0    4
      // -1 -1    3
      // -2 -2    2
      // -3 -3    1
      // -4  0    4
      // -5 -1    3
      // -6 -2    2
      // -7 -3    1
      // -8  0    4
      //
      // The last column above shows the results of the following line of code.
      // This generates the wrong results in that the '4' results in the last
      // column should be 0. We could apply another "% axisDim" to
      // the result or just add the if statement below.
      localKey[axis] = axisDim + localKey[axis] % axisDim;
      localKey[axis] = (localKey[axis] != axisDim) ? localKey[axis] : 0;
    }
    // else
    // {
    //   localKey[axis] %= axisDim;
    // }
  }

  key->voxel[0] = localKey[0];
  key->voxel[1] = localKey[1];
  key->voxel[2] = localKey[2];
}

// GpuKey dir calculator for a single axis. Results are:
// . 0 => keys are equal on axis.
// . 1 => end key indexes a higher voxel along axis.
// . -1 => end key indexes a lower voxel along axis.
//
// Calculated as follows:
//
// . -1 end_region < start_region
// . 1 end_region > start_region
// . -1 end_region == start_region && end_voxel < start_voxel
// . 1 end_region == start_region && end_voxel > start_voxel
// . 0 end_region == start_region && end_voxel == start_voxel
#define _OHM_GPUKEY_AXIS_DIR(start, end, axis)                      \
  ((end)->region[axis] != (start)->region[axis] ?                   \
     ((end)->region[axis] > (start)->region[axis] ? 1.0f : -1.0f) : \
     ((end)->voxel[axis] != (end)->voxel[1] ? ((end)->voxel[axis] > (start)->voxel[axis] ? 1.0f : -1.0f) : 0.0f))

inline __device__ __host__ float3 keyDirection(const GpuKey *start, const GpuKey *end)
{
  return make_float3(_OHM_GPUKEY_AXIS_DIR(start, end, 0), _OHM_GPUKEY_AXIS_DIR(start, end, 1),
                     _OHM_GPUKEY_AXIS_DIR(start, end, 2));
}

// Calculate the number of voxels from start to end.
inline __device__ __host__ int3 keyDiff(const GpuKey *end, const GpuKey *start, const int3 regionDim)
{
  // First diff the regions.
  const int3 region_diff =
    make_int3(end->region[0] - start->region[0], end->region[1] - start->region[1], end->region[2] - start->region[2]);
  int3 voxel_diff;

  // Voxel difference is the sum of the local difference plus the region step difference.
  voxel_diff.x = end->voxel[0] - start->voxel[0] + region_diff.x * regionDim.x;
  voxel_diff.y = end->voxel[1] - start->voxel[1] + region_diff.y * regionDim.y;
  voxel_diff.z = end->voxel[2] - start->voxel[2] + region_diff.z * regionDim.z;

  return voxel_diff;
}

#endif  // GPUTIL_DEVICE

#endif  // GPUKEY_H


================================================
FILE: ohmgpu/GpuLayerCache.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "GpuLayerCache.h"

#include "GpuCacheStats.h"
#include "GpuLayerCacheParams.h"

#include <ohm/DefaultLayer.h>
#include <ohm/MapChunk.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/MapRegion.h>
#include <ohm/OccupancyMap.h>
#include <ohm/VoxelBlock.h>
#include <ohm/VoxelBuffer.h>

#include <gputil/gpuDevice.h>

#include <ohmutil/VectorHash.h>

#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#endif  // __GNUC__
#include <ska/bytell_hash_map.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif  // __GNUC__

#include <algorithm>
#include <cassert>
#include <memory>

namespace ohm
{
/// Data required for a single cache entry.
struct GpuCacheEntry  // NOLINT
{
  /// The cached chunk. May be null when the chunk does not exist in the map.
  MapChunk *chunk = nullptr;
  /// Region key for @c chunk.
  glm::i16vec3 region_key = glm::i16vec3(0);
  /// Offset into the GPU buffer at which this chunk's voxels have been uploaded (bytes).
  size_t mem_offset = 0;
  /// Event associated with the most recent operation on @c gpuMem.
  /// This may be an upload, download or kernel execution using the buffer.
  gputil::Event sync_event;
  /// Stamp value used to assess the oldest cache entry.
  uint64_t age_stamp = 0;
  /// Retains uncompressed voxel memory while the chunk remains in the cache.
  VoxelBuffer<VoxelBlock> voxel_buffer;
  // FIXME: (KS) Would be nice to resolve how chunk stamping is managed to sync between GPU and CPU.
  // Currently we must clear the GpuCache when updating on CPU.
  /// Tracks the @c MapChunk::touched_stamps value for the voxel layer. We know the layer has been modified in CPU
  /// this does not match and we must ignore the cached entry.
  uint64_t chunk_touch_stamp = 0;
  /// Most recent @c  batch_marker from @c upload().
  unsigned batch_marker = 0;
  /// Can/should download of this item be skipped?
  bool skip_download = true;
};

struct GpuLayerCacheDetail
{
  using CacheMap = ska::bytell_hash_map<glm::i16vec3, GpuCacheEntry, Vector3Hash<glm::i16vec3>>;

  /// Cache hit/miss stats.
  GpuCacheStats stats;

  // Not part of the public API. We can put whatever we want here.
  std::unique_ptr<gputil::Buffer> buffer;
  unsigned cache_size = 0;
  unsigned batch_marker = 1;
  CacheMap cache;
  /// List of memory offsets available for re-use. Populated when we remove entries from the cache rather than
  /// replacing them.
  std::vector<size_t> mem_offset_free_list;
  glm::u8vec3 region_size = glm::u8vec3(0);
  uint64_t age_stamp = 0;
  gputil::Queue gpu_queue;
  gputil::Device gpu;
  size_t chunk_mem_size = 0;
  /// Initial target allocation size.
  size_t target_gpu_mem_size = 0;
  /// Map layer from which we read or write data
  unsigned layer_index = 0;
  unsigned flags = 0;
  uint8_t *dummy_chunk = nullptr;
  OccupancyMap *map = nullptr;
  GpuCachePostSyncHandler on_sync;

  ~GpuLayerCacheDetail()
  {
    delete[] dummy_chunk;
    // We must clean up the cache explicitly. Otherwise it may be cleaned up after the _gpu device, in which case
    // the events will no longer be valid.
    cache.clear();
  }
};

GpuLayerCache::GpuLayerCache(const gputil::Device &gpu, const gputil::Queue &gpu_queue, OccupancyMap &map,
                             unsigned layer_index, size_t target_gpu_mem_size, unsigned flags,
                             GpuCachePostSyncHandler on_sync)
  : imp_(new GpuLayerCacheDetail)
{
  assert(layer_index < map.layout().layerCount());

  imp_->gpu = gpu;
  imp_->gpu_queue = gpu_queue;
  imp_->layer_index = layer_index;
  imp_->flags = flags;
  imp_->on_sync = std::move(on_sync);
  imp_->map = &map;

  allocateBuffers(map, map.layout().layer(layer_index), target_gpu_mem_size);
}


GpuLayerCache::~GpuLayerCache()
{
  delete imp_;
}


void GpuLayerCache::reinitialise()
{
  clear();
}


void GpuLayerCache::flush()
{
  syncToMainMemory();
}


unsigned GpuLayerCache::beginBatch()
{
  imp_->batch_marker += 2;
  return imp_->batch_marker;
}


void GpuLayerCache::beginBatch(unsigned batch_marker)
{
  imp_->batch_marker = batch_marker;
}


unsigned GpuLayerCache::layerIndex() const
{
  return imp_->layer_index;
}


size_t GpuLayerCache::allocate(OccupancyMap &map, const glm::i16vec3 &region_key, MapChunk *&chunk,
                               gputil::Event *event, CacheStatus *status, unsigned batch_marker, unsigned flags)
{
  GpuCacheEntry *entry = resolveCacheEntry(map, region_key, chunk, event, status, batch_marker, flags, false);
  if (entry)
  {
    return entry->mem_offset;
  }

  return ~size_t{ 0u };
}


size_t GpuLayerCache::upload(OccupancyMap &map, const glm::i16vec3 &region_key, MapChunk *&chunk, gputil::Event *event,
                             CacheStatus *status, unsigned batch_marker, unsigned flags)
{
  GpuCacheEntry *entry = resolveCacheEntry(map, region_key, chunk, event, status, batch_marker, flags, true);
  if (entry)
  {
    return entry->mem_offset;
  }

  return ~size_t{ 0u };
}


bool GpuLayerCache::lookup(OccupancyMap & /*map*/, const glm::i16vec3 &region_key, size_t *offset,
                           gputil::Event *current_event)
{
  // const MapLayer &layer = map.layout().layer(_layerIndex);
  GpuCacheEntry *entry = findCacheEntry(region_key);
  if (entry)
  {
    if (offset)
    {
      *offset = entry->mem_offset;
    }

    if (current_event)
    {
      *current_event = entry->sync_event;
    }
    return true;
  }

  return false;
}


gputil::Buffer *GpuLayerCache::buffer() const
{
  return imp_->buffer.get();
}


void GpuLayerCache::updateEvent(MapChunk &chunk, gputil::Event &event)
{
  GpuCacheEntry *entry = findCacheEntry(chunk);
  if (!entry)
  {
    // This is a logical error.
    return;
  }

  entry->sync_event = event;
  // Touch the chunk entry.
  entry->age_stamp = imp_->age_stamp++;
}


void GpuLayerCache::updateEvents(unsigned batch_marker, gputil::Event &event)
{
  for (auto &iter : imp_->cache)
  {
    if (iter.second.batch_marker == batch_marker)
    {
      iter.second.sync_event = event;
      // Touch the chunk entry.
      iter.second.age_stamp = imp_->age_stamp;
    }
  }
  ++imp_->age_stamp;
}


void GpuLayerCache::remove(const glm::i16vec3 &region_key)
{
  auto search_iter = imp_->cache.find(region_key);

  if (search_iter != imp_->cache.end())
  {
    GpuCacheEntry &entry = search_iter->second;
    // Wait for oustanding operations, but don't sync.
    entry.sync_event.wait();
    // Push the memory offset onto the free list for re-use.
    imp_->mem_offset_free_list.push_back(entry.mem_offset);
    imp_->cache.erase(search_iter);
  }
}


bool GpuLayerCache::syncLayerTo(MapChunk &dst_chunk, unsigned dst_layer, const MapChunk &src_chunk, unsigned src_layer)
{
  if (src_layer == layerIndex())
  {
    VoxelBuffer<VoxelBlock> dst(dst_chunk.voxel_blocks[dst_layer]);
    return syncToExternal(dst, src_chunk.region.coord) > 0;
  }

  return false;
}


MapRegionCache *GpuLayerCache::findLayerCache(unsigned layer)
{
  // This is part of making GpuLayerCache derive MapRegionCache. This type of MapRegionCache can have no children
  // (vs GpuCache which contains GpuLayerCache objects), so we caonly only return this if we match the layer index.
  return (layerIndex() == layer) ? this : nullptr;
}


void GpuLayerCache::syncToMainMemory(const MapChunk &chunk)
{
  GpuCacheEntry *entry = findCacheEntry(chunk);
  if (entry)
  {
    syncToMainMemory(*entry, true);
  }
}


void GpuLayerCache::syncToMainMemory(const glm::i16vec3 &region_key)
{
  GpuCacheEntry *entry = findCacheEntry(region_key);
  if (entry)
  {
    syncToMainMemory(*entry, true);
  }
}


void GpuLayerCache::syncToMainMemory()
{
  // Queue up memory transfers.
  for (auto &iter : imp_->cache)
  {
    GpuCacheEntry &entry = iter.second;
    syncToMainMemory(entry, false);
  }

  // Wait on the queued events.
  for (auto &iter : imp_->cache)
  {
    GpuCacheEntry &entry = iter.second;
    entry.sync_event.wait();
    if (entry.chunk && imp_->on_sync)
    {
      imp_->on_sync(entry.chunk, imp_->region_size);
    }
    // Up to date.
    entry.skip_download = true;
  }
}


size_t GpuLayerCache::syncToExternal(uint8_t *dst, size_t dst_size, const glm::i16vec3 &src_region_key)
{
  if (dst)
  {
    GpuCacheEntry *entry = findCacheEntry(src_region_key);
    if (entry)
    {
      if (entry->chunk && entry->voxel_buffer.isValid() && entry->voxel_buffer.voxelMemorySize() >= dst_size &&
          !entry->skip_download)
      {
        // Found a cached entry to sync.
        // Read voxel data, waiting on the chunk event to ensure it's up to date.
        // We use a synchronous copy to the destination location.
        return imp_->buffer->read(dst, entry->voxel_buffer.voxelMemorySize(), entry->mem_offset, nullptr,
                                  &entry->sync_event);
      }
    }
  }

  return 0;
}


size_t GpuLayerCache::syncToExternal(VoxelBuffer<VoxelBlock> &dst, const glm::i16vec3 &src_region_key)
{
  if (dst.isValid())
  {
    return syncToExternal(dst.voxelMemory(), dst.voxelMemorySize(), src_region_key);
  }
  return 0;
}


gputil::Device &GpuLayerCache::gpu()
{
  return imp_->gpu;
}


const gputil::Device &GpuLayerCache::gpu() const
{
  return imp_->gpu;
}


gputil::Queue &GpuLayerCache::gpuQueue()
{
  return imp_->gpu_queue;
}


const gputil::Queue &GpuLayerCache::gpuQueue() const
{
  return imp_->gpu_queue;
}


unsigned GpuLayerCache::cachedCount() const
{
  return unsigned(imp_->cache.size());
}


unsigned GpuLayerCache::cacheSize() const
{
  return imp_->cache_size;
}


unsigned GpuLayerCache::bufferSize() const
{
  return (imp_->buffer) ? unsigned(imp_->buffer->actualSize()) : 0;
}


unsigned GpuLayerCache::chunkSize() const
{
  return unsigned(imp_->chunk_mem_size);
}


void GpuLayerCache::reallocate(const OccupancyMap &map)
{
  clear();
  imp_->buffer.reset(nullptr);
  allocateBuffers(map, map.layout().layer(imp_->layer_index), imp_->target_gpu_mem_size);
}


void GpuLayerCache::clear()
{
  // Ensure all outstanding GPU transactions are complete, but do not sync.
  for (auto &&entry : imp_->cache)
  {
    entry.second.sync_event.wait();
  }
  imp_->cache.clear();
  imp_->stats.hits = imp_->stats.misses = imp_->stats.full;
}

void GpuLayerCache::queryStats(GpuCacheStats *stats)
{
  *stats = imp_->stats;
}

GpuCacheEntry *GpuLayerCache::resolveCacheEntry(OccupancyMap &map, const glm::i16vec3 &region_key, MapChunk *&chunk,
                                                gputil::Event *event, CacheStatus *status, unsigned batch_marker,
                                                unsigned flags, bool upload)
{
  const MapLayer &layer = map.layout().layer(imp_->layer_index);
  GpuCacheEntry *entry = findCacheEntry(region_key);
  if (entry)
  {
    ++imp_->stats.hits;
    // Already uploaded.
    chunk = entry->chunk;
    // Needs update?
    bool update_required = upload && (flags & kForceUpload) != 0;

    // Check if it was previously added, but without allowing creation.
    if (!entry->chunk)
    {
      // First check if it's been created on CPU.
      entry->chunk = chunk = map.region(region_key, false);
      if (!entry->chunk && (flags & kAllowRegionCreate))
      {
        // Now allowed to create. Do so.
        entry->chunk = chunk = map.region(region_key, true);
        update_required = upload;
      }
      else if (entry->chunk)
      {
        // Has been created on CPU. Require upload.
        update_required = true;
      }
    }

    // FIXME: (KS) resolve detecting CPU changes. Currently must clear the cache.
    if (upload && !update_required && chunk)
    {
      // Check if the chunk has changed in CPU.
      update_required = chunk->touched_stamps[imp_->layer_index] > entry->chunk_touch_stamp;
    }

    entry->skip_download = entry->skip_download && ((flags & kSkipDownload) != 0);

    if (update_required)
    {
      // Upload the chunk in case it has been created while it's been in the cache.
      gputil::Event wait_for_previous = entry->sync_event;
      gputil::Event *wait_for_ptr = (wait_for_previous.isValid()) ? &wait_for_previous : nullptr;

      // Need to hold the voxel buffer until we have written to the GPU buffer.
      if (entry->chunk && !entry->voxel_buffer.isValid())
      {
        entry->voxel_buffer = VoxelBuffer<VoxelBlock>(entry->chunk->voxel_blocks[imp_->layer_index]);
      }
      const uint8_t *voxel_mem =
        (entry->voxel_buffer.isValid()) ? entry->voxel_buffer.voxelMemory() : imp_->dummy_chunk;
      imp_->buffer->write(voxel_mem, layer.layerByteSize(map.regionVoxelDimensions()), entry->mem_offset,
                          &imp_->gpu_queue, wait_for_ptr, &entry->sync_event);
    }
    // We update the touched stamping even though the entry is already present and we may not need to upload anything.
    // We make the assumption that the request for a upload caching is being made because we are about to modify it.
    // Note we also check the skip_download flag in which case there's no need to update the touched_stamps entry.
    if (upload && chunk)
    {
      if (!entry->skip_download)
      {
        // Keeping in sync between GPU and CPU has been an ongoing issue. It probably needs a stamping system which
        // separates CPU and GPU changes, but we don't have that yet. As an interim solution to recognising GPU changes,
        // we update the dirty stamp for a chunk on both upload and download.
        chunk->dirty_stamp = chunk->touched_stamps[imp_->layer_index] = entry->chunk_touch_stamp = imp_->map->stamp();
      }
      else
      {
        entry->chunk_touch_stamp = chunk->touched_stamps[imp_->layer_index];
      }
    }

    if (event)
    {
      *event = entry->sync_event;
    }
    if (status)
    {
      *status = kCacheExisting;
    }
    entry->age_stamp = imp_->age_stamp++;
    if (batch_marker)
    {
      // Update the batch marker.
      entry->batch_marker = batch_marker;
    }
    return entry;
  }

  ++imp_->stats.misses;

  // Not in the cache yet.
  // Ensure the map chunk exists in the map if kAllowRegionCreate is set.
  // Otherwise chunk may be null.
  chunk = map.region(region_key, (flags & kAllowRegionCreate));

  // Now add the chunk to the cache.
  // Check if there are unallocated buffers.
  if (cachedCount() < cacheSize())
  {
    // Use the next buffer.
    GpuCacheEntry new_entry{};
    // First we try poping an entry off the free list.
    if (!imp_->mem_offset_free_list.empty())
    {
      new_entry.mem_offset = imp_->mem_offset_free_list.front();
      imp_->mem_offset_free_list.erase(imp_->mem_offset_free_list.begin());
    }
    else
    {
      new_entry.mem_offset = imp_->chunk_mem_size * cachedCount();
    }
    auto inserted = imp_->cache.insert(std::make_pair(region_key, new_entry));
    entry = &inserted.first->second;
  }
  else
  {
    ++imp_->stats.full;
    // Cache is full. Look for the oldest entry to sync back to main memory.
    auto oldest_entry = imp_->cache.begin();
    for (auto iter = imp_->cache.begin(); iter != imp_->cache.end(); ++iter)
    {
      // Check the age_stamp and the batch marker.
      if (iter->second.age_stamp < oldest_entry->second.age_stamp &&
          (batch_marker == 0 || iter->second.batch_marker != batch_marker))
      {
        oldest_entry = iter;
      }
    }

    if (batch_marker && oldest_entry != imp_->cache.end() && oldest_entry->second.batch_marker == batch_marker)
    {
      // All entries in the cache share the batch_marker. We cannot upload.
      if (status)
      {
        // Cache is full and we cannot release any old entries.
        *status = kCacheFull;
      }
      return nullptr;
    }

    // Synchronise the oldest entry back to main memory.
    syncToMainMemory(oldest_entry->second, true);

    GpuCacheEntry new_entry{};
    new_entry.mem_offset = oldest_entry->second.mem_offset;
    // Remove oldest entry from the cache
    imp_->cache.erase(oldest_entry);

    // Insert the new entry.
    auto inserted = imp_->cache.insert(std::make_pair(region_key, new_entry));
    entry = &inserted.first->second;
  }

  // Complete the cache entry.
  entry->chunk = chunk;  // May be null.
  // Lock chunk memory for the relevant layer. This will be retained while the chunk is in this cache.
  entry->voxel_buffer =
    (chunk) ? VoxelBuffer<VoxelBlock>(chunk->voxel_blocks[imp_->layer_index]) : VoxelBuffer<VoxelBlock>();
  entry->region_key = region_key;
  entry->age_stamp = imp_->age_stamp++;
  if (batch_marker)
  {
    // Update the batch marker.
    entry->batch_marker = batch_marker;
  }
  entry->skip_download = (flags & kSkipDownload);

  if (upload)
  {
    const uint8_t *voxel_mem = (entry->voxel_buffer.isValid()) ? entry->voxel_buffer.voxelMemory() : imp_->dummy_chunk;
    imp_->buffer->write(voxel_mem, imp_->chunk_mem_size, entry->mem_offset, &imp_->gpu_queue, nullptr,
                        &entry->sync_event);
    if (chunk)
    {
      if (!entry->skip_download)
      {
        // As above where we upload to update, we change the stamp for the chunk on both upload and download.
        chunk->dirty_stamp = chunk->touched_stamps[imp_->layer_index] = entry->chunk_touch_stamp = imp_->map->stamp();
      }
      else
      {
        entry->chunk_touch_stamp = chunk->touched_stamps[imp_->layer_index];
      }
    }
    else
    {
      entry->chunk_touch_stamp = imp_->map->stamp();
    }
  }

  if (event)
  {
    *event = entry->sync_event;
  }
  if (status)
  {
    *status = kCacheNew;
  }

  return entry;
}


void GpuLayerCache::allocateBuffers(const OccupancyMap &map, const MapLayer &layer, size_t target_gpu_mem_size)
{
  // Query maximum allocation size.
  auto mem_limit = imp_->gpu.maxAllocationSize();
  target_gpu_mem_size = (target_gpu_mem_size <= mem_limit) ? target_gpu_mem_size : mem_limit;

  imp_->target_gpu_mem_size = target_gpu_mem_size;
  imp_->region_size = layer.dimensions(map.regionVoxelDimensions());
  imp_->chunk_mem_size = layer.layerByteSize(map.regionVoxelDimensions());

  size_t allocated = 0;

  // Do loop to ensure we allocate at least one buffer.
  unsigned buffer_flags = gputil::kBfReadWrite;
  if (imp_->flags & kGcfMappable)
  {
    buffer_flags |= gputil::kBfHostAccess;
  }

  imp_->cache_size = 0;

  do
  {
    allocated += imp_->chunk_mem_size;
    ++imp_->cache_size;
  } while (allocated + imp_->chunk_mem_size <= target_gpu_mem_size);

  imp_->buffer = std::make_unique<gputil::Buffer>(imp_->gpu, allocated, buffer_flags);

  imp_->dummy_chunk = new uint8_t[layer.layerByteSize(map.regionVoxelDimensions())];
  layer.clear(imp_->dummy_chunk, map.regionVoxelDimensions());
}


void GpuLayerCache::syncToMainMemory(GpuCacheEntry &entry, bool wait_on_sync)
{
  if (entry.chunk && !entry.skip_download)
  {
    // Cache the current sync event. We will make the memory copy depend on this event.
    gputil::Event last_event = entry.sync_event;
    // Release the entry's sync event. We will git it a new one.
    entry.sync_event.release();
    // This should technically always be true if chunk is not null.
    if (entry.voxel_buffer.isValid())
    {
      // Queue memory read blocking on the last event and tracking a new one in entry.syncEvent
      uint8_t *voxel_mem = entry.voxel_buffer.voxelMemory();
      imp_->buffer->read(voxel_mem, imp_->chunk_mem_size, entry.mem_offset, &imp_->gpu_queue, &last_event,
                         &entry.sync_event);
      // Update the dirty stamp for the region
      entry.chunk->dirty_stamp = entry.chunk->touched_stamps[imp_->layer_index] = entry.chunk_touch_stamp =
        imp_->map->touch();
      // Also need to invalidate the MapChunk::first_valid_index as we don't know what it will be coming off the GPU.
      // We only apply this change for the occupancy layer
      if (imp_->layer_index == unsigned(imp_->map->layout().occupancyLayer()) ||
          imp_->layer_index == unsigned(imp_->map->layout().layerIndex(default_layer::tsdfLayerName())))
      {
        entry.chunk->invalidateFirstValidIndex();
      }
    }
  }

  // Do we block now on the sync? This could be changed to execute only when we don't skip download.
  // Must wait if we have an on_sync handler (to call it). Eventually we may be able to use a GPU post event hook, but
  /// there are thread-safety issues with that.
  if (wait_on_sync)
  {
    // Wait for operations to complete.
    entry.sync_event.wait();
    // Up to date.
    entry.skip_download = true;

    if (imp_->on_sync)
    {
      imp_->on_sync(entry.chunk, imp_->region_size);
    }
  }
}


namespace
{
template <typename ENTRY, typename T>
inline ENTRY *findCacheEntry(T &cache, const glm::i16vec3 &region_key)
{
  auto search_iter = cache.find(region_key);

  if (search_iter != cache.end())
  {
    return &search_iter->second;
  }

  // Not in the GPU cache.
  return nullptr;
}
}  // namespace


GpuCacheEntry *GpuLayerCache::findCacheEntry(const glm::i16vec3 &region_key)
{
  return ohm::findCacheEntry<GpuCacheEntry>(imp_->cache, region_key);
}


const GpuCacheEntry *GpuLayerCache::findCacheEntry(const glm::i16vec3 &region_key) const
{
  return ohm::findCacheEntry<const GpuCacheEntry>(imp_->cache, region_key);
}


GpuCacheEntry *GpuLayerCache::findCacheEntry(const MapChunk &chunk)
{
  return ohm::findCacheEntry<GpuCacheEntry>(imp_->cache, chunk.region.coord);
}


const GpuCacheEntry *GpuLayerCache::findCacheEntry(const MapChunk &chunk) const
{
  return ohm::findCacheEntry<const GpuCacheEntry>(imp_->cache, chunk.region.coord);
}

}  // namespace ohm


================================================
FILE: ohmgpu/GpuLayerCache.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_GPULAYERCACHE_H
#define OHMGPU_GPULAYERCACHE_H

#include "OhmGpuConfig.h"

#include "GpuCachePostSyncHandler.h"

#include <ohm/MapRegionCache.h>

#include <glm/glm.hpp>

#include <gputil/gpuBuffer.h>
#include <gputil/gpuDevice.h>
#include <gputil/gpuQueue.h>

namespace ohm
{
struct GpuCacheStats;
struct GpuCacheEntry;
struct GpuLayerCacheDetail;
struct MapChunk;
class MapLayer;
class OccupancyMap;
class VoxelBlock;
template <typename T>
class VoxelBuffer;

/// Defines a GPU memory cache of voxel data.
///
/// The cache may be used to ensure voxels from a @c MapChunk::voxel_maps associated with a @c MapLayer are uploaded
/// to GPU and optionally downloaded back to gpu. A layer cache should only be created via @c GpuCache with the
/// appropriate @c GpuLayerCacheParams and @c GpuLayerCacheFlag selection. Once created a cache is bound to transfer
/// voxel data to/from a specific @c MapLayer only.
///
/// The GPU memory is allocated as a single, large memory buffer. Voxel data are uploaded to available regions within
/// this buffer when calling @p upload(). The return value identified the byte offset into the buffer where data for
/// the specific region are located. The region data persist in the cache as long as possible. The oldest region is
/// dropped when @c upload() is called and the cache is full.
///
/// Typical usage is as follows:
/// - Start a batch with @c beginBatch()
/// - @c upload() or @c allocate()
///   - @c upload() the data from the required @c MapChunk sections using the batch from @c beginBatch().
///   - @c allocate() creates space for the chunk, but does not upload. This is for operations which download to the
///     chunk only.
/// - Execute GPU code.
/// - Synchronise data back to host memory.
///
/// Using @c beginBatch() returns a rolling batch marker. When this batch marker is passed to @p upload() the
/// cache ensures no region with the same batch marker is removed from the cache. In this way all regions required for
/// a GPU program may be locked in the cache. The @p upload() call may fail when using batch markers if the cache is
/// not large enough.
class ohmgpu_API GpuLayerCache : public MapRegionCache
{
public:
  /// Status return values for @c upload().
  enum CacheStatus
  {
    /// The region has been previously uploaded to the GPU and exists in the cache.
    kCacheExisting,
    /// The region has been newly uploaded to GPU (in progress).
    kCacheNew,
    /// The cache is full and the region will not be uploaded on this call.
    /// Only when @c upload() @c batchMarker is used.
    kCacheFull,
  };

  /// Flags for caching methods affecting @c upload() and @c allocate()
  enum CacheFlag : unsigned
  {
    /// Allow creation of the region chunk in the map if it does not exist?
    kAllowRegionCreate = (1u << 0u),
    /// Download of results is not required for this region.
    /// Note changes in this flag value on may force the cache to stall.
    kSkipDownload = (1u << 1u),
    /// Force upload even if already cached.
    kForceUpload = (1u << 2u),
  };

  /// Create a new layer cache.
  ///
  /// This allocated a GPU buffer up to @c targetGpuMemSize bytes. The actual target allocation is a multiple of
  /// the data size for a single map chunk. This may be padded slightly further to create an optimal GPU buffer size.
  /// The result will be set to a number of regions not to exceed @p targetGpuMemSize, however the padding may
  /// result is exceeding the target value. The allocation is also limited to half the GPU memory size.
  ///
  /// @param gpu The GPU device to allocate in.
  /// @param gpu_queue GPU queue used to execute memory transfer in.
  /// @param map The map to which the cache belongs.
  /// @param layer_index Identifies @c MapLayer from which to cache voxel data.
  /// @param target_gpu_mem_size The maximum allowed buffer size (bytes).
  /// @param flags Creation flags: see @c GpuLayerCacheFlag . @c GCFRead is currently mandatory.
  /// @param on_sync Defines a function to invoke after a @c MapChunk is synched to main memory (CPU).
  GpuLayerCache(const gputil::Device &gpu, const gputil::Queue &gpu_queue, OccupancyMap &map, unsigned layer_index,
                size_t target_gpu_mem_size, unsigned flags,
                GpuCachePostSyncHandler on_sync = GpuCachePostSyncHandler());

  /// Release the GPU cache. Does not synchronise to host memory.
  ~GpuLayerCache();

  /// Equivalent to a @c clear() .
  void reinitialise() override;

  /// Equivalent to a @c syncToMainMemory() .
  void flush() override;

  /// Generate a new batch marker for use with @c upload() @c batchMarker parameter.
  /// @return The next rolling batch marker.
  unsigned beginBatch();

  /// Start a new batch using the given marker.
  void beginBatch(unsigned batch_marker);

  /// Identifies the voxel layer the GPU cache operates on. See @c MapLayout and @c MapChunk::voxel_maps.
  /// @return The voxel layer index this cache operates on.
  unsigned layerIndex() const;

  /// Handler invoked after a MapChunk has been synchronised to main memory.
  /// @return The function object invoked on sync.
  const GpuCachePostSyncHandler &onSyncHandler() const;

  /// Allocate space for voxel data from the @c MapChunk identified by @c regionKey to GPU.
  /// The GPU memory is directly associated with the @c MapChunk voxel layer given by
  /// @c layerIndex().
  ///
  /// This allocates space for the voxel layer from the @c MapChunk defined by @c regionKey,
  /// leaving the GPU memory uninitialised. The assumption is that the GPU will set the correct
  /// values which are to be downloaded to CPU later. Allocation is skipped when the layer is
  /// already in the GPU cache.
  ///
  /// Allocation supports batching operations for a group of regions to be processed
  /// collectively. When @p batchMarker is non-zero, the @p batchMarker is associated with the
  /// region. The @c GpuCache will not remove any regions from the cache which share the same
  /// @p batchMarker. This means that a call to @c allocate() or @c upload() may fail to allocate the requested
  /// region. In this case, the return value is ~0u and the @p status is set to @c CacheFull.
  /// Canonically, only the @p status value should be reliably used.
  ///
  /// Note that each region may only have one @c batchMarker associated with it. Only the most
  /// recent @c batchMarker value is associated with a region in the cache. Also note that
  /// a @c batchMarker of zero will not overwrite the previously associated marker value.
  ///
  /// @param map The map from which we are uploading data.
  /// @param region_key The key for the region to upload.
  /// @param[out] chunk Set to the @c chunk identified by @p regionKey. Null if the region does not exist and @c
  ///     AllowRegionCreate is false.
  /// @param[out] event Optional to request the GPU event marking the any outstanding operations on the cache memory.
  ///     Only relevant when the region is already in the cache.
  /// @param[out] status Optional to request the status of the cache entry.
  /// @param batch_marker Optionally set to demarcate a group of upload operations.
  /// @param flags a combination of @c CacheFlag flag values.
  /// @return The byte offset into @c buffer() where the region is being uploaded. ~0u on
  ///     failure to upload as determined by the use of the @p batchMarker.
  size_t allocate(OccupancyMap &map, const glm::i16vec3 &region_key, MapChunk *&chunk, gputil::Event *event,
                  CacheStatus *status = nullptr, unsigned batch_marker = 0, unsigned flags = 0u);

  /// Queue upload of voxel data from the @c MapChunk identified by @c regionKey to GPU.
  /// Uploads voxel data for the @c MapChunk voxel layer given by @c layerIndex().
  ///
  /// This takes the associated voxel layer from the @c MapChunk defined by @c regionKey,
  /// demarcates space for the chunk in  the GPU cache and uploads the CPU data to GPU.
  /// When the region chunk does not exist, GPU memory is still allocated and initialised with the
  /// default clear value for that layer. Upload is skipped when the cache is already up to date
  /// for the specified region. Upload behaviour may be modified by setting @c flags as per @c CacheFlag.
  ///
  /// Data upload supports batching the upload for a group of regions to be processed
  /// collectively. When @p batchMarker is non-zero, the @p batchMarker is associated with the
  /// region. The @c GpuCache will not remove any regions from the cache which share the same
  /// @p batchMarker. This means that a call to @c allocate() or @c upload() may fail to allocate the requested
  /// region. In this case, the return value is ~0u and the @p status is set to @c CacheFull.
  /// Canonically, only the @p status value should be reliably used.
  ///
  /// Note that each region may only have one @c batchMarker associated with it. Only the most
  /// recent @c batchMarker value is associated with a region in the cache. Also note that
  /// a @c batchMarker of zero will not overwrite the previously associated marker value.
  ///
  /// @param map The map from which we are uploading data.
  /// @param region_key The key for the region to upload.
  /// @param[out] chunk Set to the @c chunk identified by @p regionKey. Null if the region does not exist and @c
  ///     AllowRegionCreate is false.
  /// @param[out] event Optional to request the GPU event marking the completion of the upload.
  /// @param[out] status Optional to request the status of the requested upload.
  /// @param batch_marker Optionally set to demarcate a group of upload operations.
  /// @param flags a combination of @c CacheFlag flag values.
  /// @return The byte offset into @c buffer() where the region is being uploaded. ~0u on
  ///     failure to upload as determined by the use of the @p batchMarker.
  size_t upload(OccupancyMap &map, const glm::i16vec3 &region_key, MapChunk *&chunk, gputil::Event *event,
                CacheStatus *status = nullptr, unsigned batch_marker = 0, unsigned flags = 0u);

  /// Lookup the cache to see if the chunk identified by @p regionKey is present in the cache.
  ///
  /// If the cache is present, then this method:
  /// - Sets @p offset to the byte offset into @p buffer() where the region is stored (if provided).
  /// - Sets @p currentEvent to the most recent @c gputil::Event associated with data transfer for this region
  ///   (if provided).
  /// - Returns @c true.
  ///
  /// @param map The map which owns the cache.
  /// @param region_key The key for the map chunk of interest.
  /// @param[out] offset Set to the byte offset in @c buffer() if in the cache. May be null.
  /// @param[out] current_event Set to the most recent GPU data manipulation event associated with this chunk.
  ///   May be null.
  /// @return True if the region is cached, false otherwise.
  bool lookup(OccupancyMap &map, const glm::i16vec3 &region_key, size_t *offset,
              gputil::Event *current_event = nullptr);

  /// Access the GPU memory buffer to which regions are uploaded. Required as an argument to GPU kernels.
  /// @return The cache's region GPU buffer.
  gputil::Buffer *buffer() const;

  /// Update the most recent event affecting the memory buffer used by @p chunk.
  ///
  /// Use case is for when executing a kernel that touched the GPU buffer associated with @p chunk.
  /// Any time such a kernel is executed, the chunk's event must be updated to the completion
  /// of that kernel, effectively locking the chunk on GPU until the kernel completes.
  ///
  /// Does nothing if @p chunk is not currently cached, though that is a logical error.
  ///
  /// @param chunk The map chunk to update the most recent event for.
  /// @param event The most recent event to associate with @p chunk.
  void updateEvent(MapChunk &chunk, gputil::Event &event);

  /// Update the event for all cached regions marked with @p batchMarker (see @c upload()).
  /// @param batch_marker The maker to match against.
  /// @param event The most recent event to associate.
  void updateEvents(unsigned batch_marker, gputil::Event &event);

  /// Remove data associated with @p region_key from the cache.
  /// This will block until outstanding operations relating to @p chunk complete, but will not explicitly sync data
  /// back to the host.
  /// @param region_key The key of the region to remove from the cache.
  void remove(const glm::i16vec3 &region_key) override;

  /// Implements the base class version. Succeeds only if @c src_chunk is in the cache and @c src_layer matches the
  /// @c layerIndex() . Invokes @c syncToExternal() .
  bool syncLayerTo(MapChunk &dst_chunk, unsigned dst_layer, const MapChunk &src_chunk, unsigned src_layer) override;

  /// Return this if @p layer matches @c layerCache() , null otherwise.
  MapRegionCache *findLayerCache(unsigned layer) override;

  /// Synchronise GPU memory for @p chunk back to main memory.
  ///
  /// Safe to call when @p chunk is not currently uploaded to GPU, in which case this method does little.
  ///
  /// This method blocks until synchronisation has completed.
  ///
  /// @param chunk The map chunk to synchronise.
  void syncToMainMemory(const MapChunk &chunk);

  /// An overload which accepts a chunk pointer, which may be null.
  /// @param chunk The map chunk to synchronise.
  inline void syncToMainMemory(const MapChunk *chunk)
  {
    if (chunk)
    {
      syncToMainMemory(*chunk);
    }
  }

  /// @overload
  void syncToMainMemory(const glm::i16vec3 &region_key);

  /// Synchronise all GPU chunk memory back to main memory. This may block while outstanding GPU operations complete.
  void syncToMainMemory();

  /// Try sync the content of the cached entry at @p src_region_key to the address at @p dst .
  ///
  /// This function supports synching from GPU memory to an alternative location rather than back to the owning
  /// @c VoxelBlock . This has no effect on the original map, however, the operation must block on the synchronisation
  /// event for the appropriate cache entry. That is, the operation may block for oustanding GPU operations targetting
  /// the cached entry.
  ///
  /// The operation may fail for the following reasons:
  ///
  /// - No entry for @p src_region_key is cached.
  /// - The specified region is marked to skip downloading (GPU read only).
  /// - @p dst is null
  /// - The @p dst_size is too small to fit the requested data.
  ///
  /// Note: the @c VoxelBuffer overload is the recommended version of this function, supporting copying from one map to
  /// another.
  ///
  /// @param dst The address to write to.
  /// @param dst_size The number of bytes available at @p dst .
  /// @param src_region_key The region key for the cache entry to sync from.
  /// @return The number of bytes copied to @p dst on success, zero on failure.
  size_t syncToExternal(uint8_t *dst, size_t dst_size, const glm::i16vec3 &src_region_key);

  /// @overload
  size_t syncToExternal(VoxelBuffer<VoxelBlock> &dst, const glm::i16vec3 &src_region_key);

  /// Access the GPU @c gputil::Device associated with GPU operations.
  /// @return The bound @c gputil::Device.
  gputil::Device &gpu();
  /// @overload
  const gputil::Device &gpu() const;

  /// Access the GPU @c gputil::Queue associated with GPU operations.
  /// @return The bound @c gputil::Queue.
  gputil::Queue &gpuQueue();
  /// @overload
  const gputil::Queue &gpuQueue() const;

  /// Query the number of regions currently in the cache.
  /// @return The number of cached regions.
  unsigned cachedCount() const;

  /// Query the number of regions the cache can hold.
  /// @return The number of regions the cache can hold at any one time.
  unsigned cacheSize() const;

  /// Total size of the GPU buffer(s) allocated by this layer cache.
  /// @return The byte size of this cache's GPU buffer(s).
  unsigned bufferSize() const;

  /// Bytes used by each cache entry in the GPU buffer.
  /// @return The byte size of each cached chunk in GPU.
  unsigned chunkSize() const;

  /// Clear the cache then reallocate using the initial constraints. This should be called whenever the layout of
  /// the associated @c MapLayer changes, although this should be a rare occurrence.
  ///
  /// This method does not perform a @c syncToMainMemory() before clearing the cache. When reorganising layers
  /// the sync should be performed before the reorganisation, not on reallocation.
  ///
  /// @param map The map to which the @c GpuLayerCache belongs.
  void reallocate(const OccupancyMap &map);

  /// Drop all cache entries. Call @c syncToMainMemory() first if data should be synched first.
  /// Resets @c GpuCacheStats - see @c queryStats() .
  void clear() override;

  /// Query cache hit/miss counts. The stats are reset on @c clear() .
  /// @param[out] stats Populated to the current cache stats.
  void queryStats(GpuCacheStats *stats);

private:
  /// Internal cache resolution/allocation function. The @p upload flag controls whether the call
  /// just makes space for the chunk, or if it uploads data s well.
  GpuCacheEntry *resolveCacheEntry(OccupancyMap &map, const glm::i16vec3 &region_key, MapChunk *&chunk,
                                   gputil::Event *event, CacheStatus *status, unsigned batch_marker, unsigned flags,
                                   bool upload);

  void allocateBuffers(const OccupancyMap &map, const MapLayer &layer, size_t target_gpu_mem_size);

  /// Sync a cache entry to main memory.
  ///
  /// Note: this method invoked the @c GpuCachePostSyncHandler only when @p wait_on_sync is true.
  /// When @p wait_on_sync is false, the caller must resolve calling the @c GpuCachePostSyncHandler.
  /// @param entry The cache entry.
  /// @param wait_on_sync Wait for sync to finish?
  void syncToMainMemory(GpuCacheEntry &entry, bool wait_on_sync);

  GpuCacheEntry *findCacheEntry(const glm::i16vec3 &region_key);
  const GpuCacheEntry *findCacheEntry(const glm::i16vec3 &region_key) const;

  GpuCacheEntry *findCacheEntry(const MapChunk &chunk);
  const GpuCacheEntry *findCacheEntry(const MapChunk &chunk) const;

  GpuLayerCacheDetail *imp_;
};
}  // namespace ohm

#endif  // OHMGPU_GPULAYERCACHE_H


================================================
FILE: ohmgpu/GpuLayerCacheParams.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "GpuLayerCacheParams.h"


================================================
FILE: ohmgpu/GpuLayerCacheParams.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPULAYERCACHEPARAMS_H
#define GPULAYERCACHEPARAMS_H

#include "OhmGpuConfig.h"

#include "GpuCachePostSyncHandler.h"

#include <algorithm>

namespace ohm
{
/// Flags used to create a @c GpuLayerCache.
enum GpuLayerCacheFlag : unsigned
{
  /// Will read voxel data from host to GPU memory.
  kGcfRead = (1u << 0u),
  /// Will write voxel data from GPU to host memory.
  kGcfWrite = (1u << 1u),
  /// Using buffers mappable to host memory? Can result in faster data transfer.
  kGcfMappable = (1u << 2u),

  /// Default creation flags.
  kGcfDefaultFlags = kGcfRead | kGcfMappable
};

/// Parameters used in creating a @c GpuCacheLayer in @c GpuCache::createCache().
struct ohmgpu_API GpuLayerCacheParams
{
  /// The size (bytes) of the GPU cache buffer. Use zero to choose the default size specified by the @c GpuCache.
  size_t gpu_mem_size = 0;
  /// The @c MapLayer index the cache pulls data from.
  int map_layer = 0;
  /// @c GpuLayerCacheFlag cache creation flags.
  unsigned flags = kGcfDefaultFlags;
  /// Callback invoked on synchronising memory back to CPU.
  /// The calling thread may not be the main thread, so the function must be threadsafe.
  GpuCachePostSyncHandler on_sync;

  /// Default constructor.
  inline GpuLayerCacheParams() = default;
  /// Construct with the given member values.
  /// @param mem_size Target GPU cache size.
  /// @param layer The @c MapLayer index which the GPU layer cache synchronised with.
  /// @param flags @c GpuLayerCacheFlag cache creation flags.
  /// @param on_sync Callback to invoke on synchronising back to CPU.
  inline GpuLayerCacheParams(size_t mem_size, int layer, unsigned flags,
                             GpuCachePostSyncHandler on_sync = GpuCachePostSyncHandler())
    : gpu_mem_size(mem_size)
    , map_layer(layer)
    , flags(flags)
    , on_sync(std::move(on_sync))
  {}
};
}  // namespace ohm

#endif  // GPULAYERCACHEPARAMS_H


================================================
FILE: ohmgpu/GpuMap.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research
// Organisation (CSIRO) ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "GpuMap.h"

#include "GpuCache.h"
#include "GpuKey.h"
#include "GpuLayerCache.h"
#include "GpuTransformSamples.h"
#include "OhmGpu.h"
#include "RayItem.h"

#include <ohm/Aabb.h>
#include <ohm/DefaultLayer.h>
#include <ohm/MapChunk.h>
#include <ohm/MapRegion.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/RayFilter.h>
#include <ohm/VoxelMean.h>
#include <ohm/VoxelTouchTime.h>

#include <ohmutil/GlmStream.h>

#include "private/GpuMapDetail.h"
#include "private/GpuProgramRef.h"

#include <ohm/private/OccupancyMapDetail.h>

#include <gputil/gpuBuffer.h>
#include <gputil/gpuEvent.h>
#include <gputil/gpuKernel.h>
#include <gputil/gpuPinnedBuffer.h>
#include <gputil/gpuPlatform.h>
#include <gputil/gpuProgram.h>

#include <logutil/Logger.h>

#include <glm/ext.hpp>

#include <array>
#include <cassert>
#include <cmath>
#include <functional>
#include <initializer_list>
#include <iostream>

/// Enable to verify ray sorting pushes unclipped samples to the begining of the list.
#define OHM_GPU_VERIFY_SORT 0

#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
#include "RegionUpdateResource.h"
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL

#if GPUTIL_TYPE == GPUTIL_CUDA
GPUTIL_CUDA_DECLARE_KERNEL(regionRayUpdateOccupancy);
#endif  // GPUTIL_TYPE == GPUTIL_CUDA

namespace ohm
{
namespace
{
#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref("RegionUpdate", GpuProgramRef::kSourceString, RegionUpdateCode,  // NOLINT
                            RegionUpdateCode_length);
#else   // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref("RegionUpdate", GpuProgramRef::kSourceFile, "RegionUpdate.cl");  // NOLINT
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL

const double kDefaultMaxRayRange = 1000.0;

#if OHM_GPU_VERIFY_SORT
/// Verify that rays are sorted such that all unclipped samples come first.
unsigned verifySort(const std::vector<RayItem> &rays)
{
  unsigned failure_count = 0;
  bool allow_samples = true;
  for (const auto &ray : rays)
  {
    if (!allow_samples && (ray.filter_flags & kRffClippedEnd) == 0)
    {
      ++failure_count;
    }
    if (ray.filter_flags & kRffClippedEnd)
    {
      allow_samples = false;
    }
  }

  if (failure_count)
  {
    throw failure_count;
  }

  return failure_count;
}
#endif  // OHM_GPU_VERIFY_SORT
}  // namespace

namespace gpumap
{
GpuCache *enableGpu(OccupancyMap &map)
{
  return enableGpu(map, GpuCache::kDefaultTargetMemSize, kGpuAllowMappedBuffers);
}


GpuCache *enableGpu(OccupancyMap &map, size_t target_gpu_mem_size, unsigned flags)
{
  OccupancyMapDetail &map_imp = *map.detail();
  if (map_imp.gpu_cache)
  {
    return static_cast<GpuCache *>(map_imp.gpu_cache);
  }

  initialiseGpuCache(map, target_gpu_mem_size, flags);
  return static_cast<GpuCache *>(map_imp.gpu_cache);
}


void sync(OccupancyMap &map)
{
  if (GpuCache *cache = gpuCache(map))
  {
    for (unsigned i = 0; i < cache->layerCount(); ++i)
    {
      if (GpuLayerCache *layer = cache->layerCache(i))
      {
        layer->syncToMainMemory();
      }
    }
  }
}


void sync(OccupancyMap &map, unsigned layer_index)
{
  if (GpuCache *cache = gpuCache(map))
  {
    if (GpuLayerCache *layer = cache->layerCache(layer_index))
    {
      layer->syncToMainMemory();
    }
  }
}


GpuCache *gpuCache(OccupancyMap &map)
{
  return static_cast<GpuCache *>(map.detail()->gpu_cache);
}

void walkRegions(const OccupancyMap &map, const glm::dvec3 &start_point, const glm::dvec3 &end_point,
                 const RegionWalkFunction &on_visit)
{
  // see "A Faster Voxel Traversal Algorithm for Ray
  // Tracing" by Amanatides & Woo
  const glm::i16vec3 start_point_key = map.regionKey(start_point);
  const glm::i16vec3 end_point_key = map.regionKey(end_point);
  const glm::dvec3 start_point_local = glm::vec3(start_point - map.origin());
  const glm::dvec3 end_point_local = glm::vec3(end_point - map.origin());

  glm::dvec3 direction = glm::vec3(end_point - start_point);
  double length = glm::dot(direction, direction);

  // Very small segments which straddle a voxel boundary can be problematic. We want to avoid
  // a sqrt on a very small number, but be robust enough to handle the situation.
  // To that end, we skip normalising the direction for directions below a tenth of the voxel.
  // Then we will exit either with start/end voxels being the same, or we will step from start
  // to end in one go.
  const bool valid_length = (length >= 0.1 * map.resolution() * 0.1 * map.resolution());
  if (valid_length)
  {
    length = std::sqrt(length);
    direction *= 1.0 / length;
  }

  if (start_point_key == end_point_key)  // || !valid_length)
  {
    on_visit(start_point_key, start_point, end_point);
    return;
  }

  if (!valid_length)
  {
    // Start/end points are in different, but adjacent voxels. Prevent issues with the loop by
    // early out.
    on_visit(start_point_key, start_point, end_point);
    on_visit(end_point_key, start_point, end_point);
    return;
  }

  std::array<int, 3> step = { 0, 0, 0 };
  glm::dvec3 region;
  std::array<double, 3> time_max;
  std::array<double, 3> time_delta;
  std::array<double, 3> time_limit;
  double next_region_border;
  double direction_axis_inv;
  const glm::dvec3 region_resolution = map.regionSpatialResolution();
  glm::i16vec3 current_key = start_point_key;

  region = map.regionCentreLocal(current_key);

  // Compute step direction, increments and maximums along
  // each axis.
  for (unsigned i = 0; i < 3; ++i)
  {
    if (direction[i] != 0)
    {
      direction_axis_inv = 1.0 / direction[i];
      step[i] = (direction[i] > 0) ? 1 : -1;
      // Time delta is the ray time between voxel
      // boundaries calculated for each axis.
      time_delta[i] = region_resolution[i] * std::abs(direction_axis_inv);
      // Calculate the distance from the origin to the
      // nearest voxel edge for this axis.
      next_region_border = region[i] + double(step[i]) * 0.5 * region_resolution[i];
      time_max[i] = (next_region_border - start_point_local[i]) * direction_axis_inv;
      time_limit[i] =
        std::abs((end_point_local[i] - start_point_local[i]) * direction_axis_inv);  // +0.5f *
                                                                                     // regionResolution[i];
    }
    else
    {
      time_max[i] = time_delta[i] = std::numeric_limits<double>::max();
      time_limit[i] = 0;
    }
  }

  bool limit_reached = false;
  int axis;
  while (!limit_reached && current_key != end_point_key)
  {
    on_visit(current_key, start_point, end_point);

    if (time_max[0] < time_max[2])
    {
      axis = (time_max[0] < time_max[1]) ? 0 : 1;
    }
    else
    {
      axis = (time_max[1] < time_max[2]) ? 1 : 2;
    }

    limit_reached = std::abs(time_max[axis]) > time_limit[axis];
    current_key[axis] += step[axis];
    time_max[axis] += time_delta[axis];
  }

  // Touch the last region.
  on_visit(current_key, start_point, end_point);
}
}  // namespace gpumap

GpuMap::GpuMap(GpuMapDetail *detail, unsigned expected_element_count, size_t gpu_mem_size)
  : imp_(detail)
{
  // Map and borrowed_map already set. Temporarily cache and clear them so we don't unnecessarily release them.
  auto *map = detail->map;
  bool borrowed_map = detail->borrowed_map;
  detail->map = nullptr;
  detail->borrowed_map = false;
  setMap(map, borrowed_map, expected_element_count, gpu_mem_size, true);
}


GpuMap::GpuMap(OccupancyMap *map, bool borrowed_map, unsigned expected_element_count, size_t gpu_mem_size)
  : GpuMap(new GpuMapDetail(map, borrowed_map), expected_element_count, gpu_mem_size)
{}


GpuMap::~GpuMap()
{
  GpuMap::releaseGpuProgram();
  delete imp_;
}


bool GpuMap::gpuOk() const
{
  return imp_->gpu_ok;
}


OccupancyMap &GpuMap::map()
{
  return *imp_->map;
}


const OccupancyMap &GpuMap::map() const
{
  return *imp_->map;
}


bool GpuMap::borrowedMap() const
{
  return imp_->borrowed_map;
}


void GpuMap::syncVoxels()
{
  const int sync_index = (imp_->next_buffers_index + 1) % GpuMapDetail::kBuffersCount;
  if (imp_->map)
  {
    // TODO(KS): split the logic for starting synching and waiting on completion.
    // This will allow us to kick synching off all layers in parallel and should reduce the overall latency.
    for (const auto &voxel_info : imp_->voxel_upload_info[sync_index])
    {
      if (!voxel_info.skip_cpu_sync)
      {
        gpumap::sync(*imp_->map, voxel_info.gpu_layer_id);
      }
    }
  }
  onSyncVoxels(sync_index);
}


void GpuMap::syncVoxels(const std::vector<int> &layer_indices)
{
  // Sync layers specified in layer_indices. Supports map copy functions.
  const int sync_index = (imp_->next_buffers_index + 1) % GpuMapDetail::kBuffersCount;
  auto *cache = gpuCache();
  if (imp_->map)
  {
    for (const auto &voxel_info : imp_->voxel_upload_info[sync_index])
    {
      const auto *layer_cache = cache->layerCache(voxel_info.gpu_layer_id);
      if (layer_cache && !voxel_info.skip_cpu_sync &&
          std::find(layer_indices.begin(), layer_indices.end(), int(layer_cache->layerIndex())) != layer_indices.end())
      {
        gpumap::sync(*imp_->map, voxel_info.gpu_layer_id);
      }
    }
  }
  onSyncVoxels(sync_index);
}


void GpuMap::setRayFilter(const RayFilterFunction &ray_filter)
{
  imp_->ray_filter = ray_filter;
  imp_->custom_ray_filter = true;
}


const RayFilterFunction &GpuMap::rayFilter() const
{
  return imp_->ray_filter;
}


const RayFilterFunction &GpuMap::effectiveRayFilter() const
{
  return (imp_->custom_ray_filter || !imp_->map) ? imp_->ray_filter : imp_->map->rayFilter();
}


void GpuMap::clearRayFilter()
{
  imp_->ray_filter = nullptr;
  imp_->custom_ray_filter = false;
}


float GpuMap::hitValue() const
{
  return imp_->map->hitValue();
}


void GpuMap::setHitValue(float value)
{
  imp_->map->setHitValue(value);
}


float GpuMap::missValue() const
{
  return imp_->map->missValue();
}


void GpuMap::setMissValue(float value)
{
  imp_->map->setMissValue(value);
}


double GpuMap::raySegmentLength() const
{
  return imp_->ray_segment_length;
}


void GpuMap::setRaySegmentLength(double length)
{
  imp_->ray_segment_length = length;
}


bool GpuMap::groupedRays() const
{
  return imp_->group_rays;
}


size_t GpuMap::integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities,
                             const double *timestamps, unsigned region_update_flags)
{
  return integrateRays(rays, element_count, intensities, timestamps, region_update_flags, effectiveRayFilter());
}


GpuCache *GpuMap::gpuCache() const
{
  return (imp_->map) ? static_cast<GpuCache *>(imp_->map->detail()->gpu_cache) : nullptr;
}


void GpuMap::setMap(OccupancyMap *map, bool borrowed_map, unsigned expected_element_count, size_t gpu_mem_size,
                    bool force_gpu_program_release)
{
  // Must ensure we aren't waiting on any GPU operations.
  if (imp_->map)
  {
    syncVoxels();

    // Delete the map if nt borrowed.
    if (!imp_->borrowed_map)
    {
      delete imp_->map;
    }
  }

  // Change the map
  imp_->map = map;
  imp_->borrowed_map = map && borrowed_map;

  if (map)
  {
    // Note: some uses of the GpuMap allow a null map pointer, but this is something of an edge case.
    GpuCache &gpu_cache = *gpumap::enableGpu(*imp_->map, gpu_mem_size, gpumap::kGpuAllowMappedBuffers);

    const unsigned prealloc_region_count = 1024u;
    for (unsigned i = 0; i < GpuMapDetail::kBuffersCount; ++i)
    {
      imp_->key_buffers[i] =
        gputil::Buffer(gpu_cache.gpu(), sizeof(GpuKey) * expected_element_count, gputil::kBfReadHost);
      imp_->ray_buffers[i] =
        gputil::Buffer(gpu_cache.gpu(), sizeof(gputil::float3) * expected_element_count, gputil::kBfReadHost);
      if (imp_->use_original_ray_buffers)
      {
        imp_->original_ray_buffers[i] =
          gputil::Buffer(gpu_cache.gpu(), sizeof(gputil::float3) * expected_element_count, gputil::kBfReadHost);
      }
      imp_->intensities_buffers[i] =
        gputil::Buffer(gpu_cache.gpu(), sizeof(float) * expected_element_count, gputil::kBfReadHost);
      imp_->timestamps_buffers[i] =
        gputil::Buffer(gpu_cache.gpu(), sizeof(uint32_t) * expected_element_count, gputil::kBfReadHost);
      imp_->region_key_buffers[i] =
        gputil::Buffer(gpu_cache.gpu(), sizeof(gputil::int3) * prealloc_region_count, gputil::kBfReadHost);

      // Add structures for managing uploads of regino offsets to the cache buffer.
      imp_->occupancy_uidx = int(imp_->voxel_upload_info[i].size());  // Set twice to the same value, but that's ok.
      imp_->voxel_upload_info[i].emplace_back(VoxelUploadInfo(kGcIdOccupancy, gpu_cache.gpu()));
    }

    cacheGpuProgram(imp_->support_voxel_mean && imp_->map->voxelMeanEnabled(),
                    imp_->support_traversal && imp_->map->traversalEnabled(), force_gpu_program_release);
  }
}


void GpuMap::setGroupedRays(bool group)
{
  imp_->group_rays = group;
}


void GpuMap::cacheGpuProgram(bool with_voxel_mean, bool with_traversal, bool force)
{
  if (imp_->program_ref)
  {
    if (!force && with_voxel_mean == imp_->cached_sub_voxel_program)
    {
      return;
    }
  }

  releaseGpuProgram();

  // Ensure voxel mean VoxelUploadInfo is present, or removed
  imp_->mean_uidx = enableVoxelUpload(int(kGcIdVoxelMean), with_voxel_mean);
  imp_->traversal_uidx = enableVoxelUpload(int(kGcIdTraversal), with_traversal);
  imp_->touch_time_uidx = enableVoxelUpload(int(kGcIdTouchTime), imp_->map->touchTimeEnabled());
  imp_->incident_normal_uidx = enableVoxelUpload(int(kGcIdIncidentNormal), imp_->map->incidentNormalEnabled());

  GpuCache &gpu_cache = *gpuCache();
  imp_->gpu_ok = true;
  imp_->cached_sub_voxel_program = with_voxel_mean;
  imp_->program_ref = &g_program_ref;

  if (imp_->program_ref->addReference(gpu_cache.gpu()))
  {
    imp_->update_kernel = GPUTIL_MAKE_KERNEL(imp_->program_ref->program(), regionRayUpdateOccupancy);
    imp_->update_kernel.calculateOptimalWorkGroupSize();
    imp_->gpu_ok = imp_->update_kernel.isValid();
  }
  else
  {
    imp_->gpu_ok = false;
  }
}


void GpuMap::releaseGpuProgram()
{
  if (imp_ && imp_->update_kernel.isValid())
  {
    imp_->update_kernel = gputil::Kernel();
  }

  if (imp_ && imp_->program_ref)
  {
    imp_->program_ref->releaseReference();
    imp_->program_ref = nullptr;
  }
}


size_t GpuMap::integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities,
                             const double *timestamps, unsigned region_update_flags, const RayFilterFunction &filter)
{
  if (!imp_->map)
  {
    return 0u;
  }

  if (!imp_->gpu_ok)
  {
    return 0u;
  }

  OccupancyMap &map = *imp_->map;
  GpuCache *gpu_cache = gpumap::enableGpu(map);

  if (!gpu_cache)
  {
    return 0u;
  }

  if (element_count == 0)
  {
    return 0u;
  }

  // Drop intensity and timestamps if we do not have the map layers to support it. This saves on uploading unnecesary
  // GPU data.
  if (map.layout().intensityLayer() < 0)
  {
    intensities = nullptr;
  }
  if (map.layout().layerIndex(default_layer::touchTimeLayerName()) < 0)
  {
    timestamps = nullptr;
  }

  // Ensure we are using the correct GPU program. Voxel mean support may have changed.
  cacheGpuProgram(imp_->support_voxel_mean && map.voxelMeanEnabled(), imp_->support_traversal && map.traversalEnabled(),
                  false);

  // Resolve the buffer index to use. We need to support cases where buffer is already one fo the imp_->ray_buffers.
  // Check this first.
  // We still need a buffer index for event tracking.
  const int buf_idx = imp_->next_buffers_index;
  waitOnPreviousOperation(buf_idx);

  // Touch the map to update stamping.
  map.touch();

  double timebase = 0;
  if (timestamps)
  {
    timebase = map.updateFirstRayTime(*timestamps);
    region_update_flags |= kRfInternalTimestamps;
  }

  // Get the GPU cache.
  GpuLayerCache *layer_cache = gpu_cache->layerCache(kGcIdOccupancy);
  if (!layer_cache)
  {
    layer_cache = gpu_cache->layerCache(kGcIdTsdf);
  }
  if (!layer_cache)
  {
    logutil::error("GpuMap cannot resolve occupancy or TSDF layer.\n");
    return 0u;
  }
  imp_->batch_marker = layer_cache->beginBatch();

  // Region walking function tracking which regions are
  // affected by a ray.
  const auto region_func = [this](const glm::i16vec3 &region_key, const glm::dvec3 & /*origin*/,
                                  const glm::dvec3 & /*sample*/) {
    if (imp_->regions.find(region_key) == imp_->regions.end())
    {
      imp_->regions.insert(region_key);
    }
  };

  // Declare pinned buffers for use in upload_ray delegate.
  gputil::PinnedBuffer keys_pinned;
  gputil::PinnedBuffer rays_pinned;
  gputil::PinnedBuffer original_rays_pinned;
  gputil::PinnedBuffer intensities_pinned;
  gputil::PinnedBuffer timestamps_pinned;

  // Build region set and upload rays.
  imp_->regions.clear();

  std::array<gputil::float3, 2> ray_gpu;
  unsigned uploaded_ray_count = 0u;
  unsigned unclipped_samples = 0u;
  GpuKey line_start_key_gpu{};
  GpuKey line_end_key_gpu{};

  const bool use_filter = bool(filter);

  // We have two loops we could run:
  // 1. processing rays as is
  // 2. grouping rays by sample voxel
  //
  // In either case we use the same code to actually upload
  // We set add_ray_upload to contain the "loop body" for both cases.
  using RayUploadFunc = std::function<void(const RayItem &)>;
  const RayUploadFunc upload_ray_core = [&](const RayItem &ray)  //
  {
    // Upload if not preloaded.
    line_start_key_gpu.region[0] = ray.origin_key.regionKey()[0];
    line_start_key_gpu.region[1] = ray.origin_key.regionKey()[1];
    line_start_key_gpu.region[2] = ray.origin_key.regionKey()[2];
    line_start_key_gpu.voxel[0] = ray.origin_key.localKey()[0];
    line_start_key_gpu.voxel[1] = ray.origin_key.localKey()[1];
    line_start_key_gpu.voxel[2] = ray.origin_key.localKey()[2];
    line_start_key_gpu.voxel[3] = 0;

    line_end_key_gpu.region[0] = ray.sample_key.regionKey()[0];
    line_end_key_gpu.region[1] = ray.sample_key.regionKey()[1];
    line_end_key_gpu.region[2] = ray.sample_key.regionKey()[2];
    line_end_key_gpu.voxel[0] = ray.sample_key.localKey()[0];
    line_end_key_gpu.voxel[1] = ray.sample_key.localKey()[1];
    line_end_key_gpu.voxel[2] = ray.sample_key.localKey()[2];
    line_end_key_gpu.voxel[3] = (ray.filter_flags & kRffClippedEnd) ? 1 : 0;

    keys_pinned.write(&line_start_key_gpu, sizeof(line_start_key_gpu), (uploaded_ray_count * 2 + 0) * sizeof(GpuKey));
    keys_pinned.write(&line_end_key_gpu, sizeof(line_end_key_gpu), (uploaded_ray_count * 2 + 1) * sizeof(GpuKey));

    // Localise the ray to single precision.
    // We change the ray coordinates to be relative to the end voxel centre. This assist later in voxel mean
    // calculations which are all relative to that voxel centre. Normally in CPU we have to make this adjustment
    // every time. We can avoid the adjustment via this logic.
    const glm::dvec3 end_voxel_centre = map.voxelCentreGlobal(ray.sample_key);
    ray_gpu[0].x = float(ray.origin.x - end_voxel_centre.x);
    ray_gpu[0].y = float(ray.origin.y - end_voxel_centre.y);
    ray_gpu[0].z = float(ray.origin.z - end_voxel_centre.z);
    ray_gpu[1].x = float(ray.sample.x - end_voxel_centre.x);
    ray_gpu[1].y = float(ray.sample.y - end_voxel_centre.y);
    ray_gpu[1].z = float(ray.sample.z - end_voxel_centre.z);
    rays_pinned.write(ray_gpu.data(), sizeof(ray_gpu), uploaded_ray_count * sizeof(ray_gpu));

    if (imp_->use_original_ray_buffers)
    {
      // Upload the original sample, made relative to this ray's end voxel.
      ray_gpu[0].x = float(ray.original_origin.x - end_voxel_centre.x);
      ray_gpu[0].y = float(ray.original_origin.y - end_voxel_centre.y);
      ray_gpu[0].z = float(ray.original_origin.z - end_voxel_centre.z);
      ray_gpu[1].x = float(ray.original_sample.x - end_voxel_centre.x);
      ray_gpu[1].y = float(ray.original_sample.y - end_voxel_centre.y);
      ray_gpu[1].z = float(ray.original_sample.z - end_voxel_centre.z);
      // Note: we are only uploading the first item from ray_gpu - the original sample position.
      original_rays_pinned.write(ray_gpu.data(), sizeof(ray_gpu), uploaded_ray_count * sizeof(ray_gpu));
    }

    ++uploaded_ray_count;

    // Increment unclipped_samples if this sample isn't clipped.
    unclipped_samples += (ray.filter_flags & kRffClippedEnd) == 0;

    // logutil::trace(i / 2, ' ', imp_->map->voxelKey(rays[i + 0]), " -> ", imp_->map->voxelKey(rays[i + 1]), ' ',
    //                 ray_start, ':', ray_end, "  <=>  ", rays[i + 0], " -> ", rays[i + 1], '\n');
    // logutil::trace("dirs: ", (ray_end - ray_start), " vs ", (ray_end_d - ray_start_d), '\n');
    gpumap::walkRegions(*imp_->map, ray.origin, ray.sample, region_func);
  };

  const RayUploadFunc upload_ray_with_intensity_or_timestamp = [&](const RayItem &ray)  //
  {
    // Note: upload_count tracks the number of float3 items uploaded, so it increments by 2 each step - sensor & sample.
    // Intensities are matched one per sample, so we halve it's value here.
    if (intensities)
    {
      intensities_pinned.write(&ray.intensity, sizeof(ray.intensity), uploaded_ray_count * sizeof(ray.intensity));
    }
    if (timestamps)
    {
      timestamps_pinned.write(&ray.timestamp, sizeof(ray.timestamp), uploaded_ray_count * sizeof(ray.timestamp));
    }
    upload_ray_core(ray);
  };

  const RayUploadFunc upload_ray =
    (intensities || timestamps) ? upload_ray_with_intensity_or_timestamp : upload_ray_core;
  // Reserve memory for the current ray set.
  imp_->grouped_rays.clear();
  imp_->grouped_rays.reserve(element_count / 2);

  // Take the input rays and move it into imp_->grouped_rays. In this process we do two things:
  // 1. Apply the ray filter (if any). This can change the origin and/or sample points.
  // 2. Break up long rays into multiple grouped_rays entries
  const double resolution = imp_->map->resolution();
  RayItem ray{};
  for (unsigned i = 0; i < element_count; i += 2)
  {
    ray.original_origin = ray.origin = rays[i + 0];
    ray.original_sample = ray.sample = rays[i + 1];
    ray.intensity = (intensities) ? intensities[i >> 1] : 0;
    ray.timestamp = (timestamps) ? encodeVoxelTouchTime(timebase, timestamps[i >> 1]) : 0;
    ray.filter_flags = 0;

    if (use_filter)
    {
      if (!filter(&ray.origin, &ray.sample, &ray.filter_flags))
      {
        // Bad ray.
        continue;
      }
    }

    if (imp_->ray_segment_length > resolution)
    {
      // ray_length starts as a squared value.
      double ray_length = glm::length2(ray.sample - ray.origin);
      // Ensure we maintain the kRffClippedEnd for the original filtered ray.
      const unsigned last_part_clipped_end = ray.filter_flags & kRffClippedEnd;
      if (ray_length >= imp_->ray_segment_length * imp_->ray_segment_length)
      {
        // We have a long ray. Break it up into even segments according to the ray_segment_length.
        // Get a true length (not squared)
        ray_length = std::sqrt(ray_length);
        // Divide by the segment length to work out how many segments we need.
        // Round up.
        const int part_count = int(std::ceil(ray_length / imp_->ray_segment_length));
        // Work out the part length.
        const double part_length = ray_length / double(part_count);
        const glm::dvec3 dir = (ray.sample - ray.origin) / ray_length;
        // Cache the initial origin and sample points. We're about to make modifications.
        const glm::dvec3 origin = ray.origin;
        const glm::dvec3 sample = ray.sample;
        // Change ray.sample to the ray.origin to make the loop logic consistent as we transition iterations.
        ray.sample = ray.origin;
        for (int i = 1; i < part_count; ++i)
        {
          // New origin is the previous 'sample'
          ray.origin = ray.sample;
          // Vector line equation.
          ray.sample = origin + double(i) * part_length * dir;

          ray.origin_key = map.voxelKey(ray.origin);
          ray.sample_key = map.voxelKey(ray.sample);

          // Mark the ray flags to ensure we don't update the sample voxel. It will appear in the next line segment
          // again. We'll update it there.
          ray.filter_flags |= kRffClippedEnd;

          imp_->grouped_rays.emplace_back(ray);

          // Mark the origin as having been moved for the next iteration. Not really used yet.
          ray.filter_flags |= kRffClippedStart;
        }

        // We still need to add the last segment. This will not have kRffClippedStart unless the filter did this.
        ray.filter_flags &= ~kRffClippedEnd;
        ray.filter_flags |= last_part_clipped_end;
        ray.origin = ray.sample;
        ray.sample = sample;
      }
    }

    // This always adds either the full, unsegmented ray, or the last part for a segmented ray.
    ray.origin_key = map.voxelKey(ray.origin);
    ray.sample_key = map.voxelKey(ray.sample);

    imp_->grouped_rays.emplace_back(ray);
  }

  if (imp_->group_rays)
  {
    // Sort the rays. Order does not matter asside from ensuring the rays are grouped by sample voxel.
    // Despite the extra CPU work, this has proven faster for NDT update in GpuNdtMap because the GPU can do much less
    // work.
    std::sort(imp_->grouped_rays.begin(), imp_->grouped_rays.end());
#if OHM_GPU_VERIFY_SORT
    verifySort(imp_->grouped_rays);
#endif  // OHM_GPU_VERIFY_SORT
  }

  // Reserve GPU memory for the rays.
  imp_->key_buffers[buf_idx].resize(sizeof(GpuKey) * 2 * imp_->grouped_rays.size());
  imp_->ray_buffers[buf_idx].resize(sizeof(gputil::float3) * 2 * imp_->grouped_rays.size());

  if (imp_->use_original_ray_buffers)
  {
    imp_->original_ray_buffers[buf_idx].resize(sizeof(gputil::float3) * 2 * imp_->grouped_rays.size());
  }

  // Declare pinned buffers for use in upload_ray delegate.
  keys_pinned = gputil::PinnedBuffer(imp_->key_buffers[buf_idx], gputil::kPinWrite);
  rays_pinned = gputil::PinnedBuffer(imp_->ray_buffers[buf_idx], gputil::kPinWrite);
  if (imp_->use_original_ray_buffers)
  {
    original_rays_pinned = gputil::PinnedBuffer(imp_->original_ray_buffers[buf_idx], gputil::kPinWrite);
  }
  if (intensities)
  {
    imp_->intensities_buffers[buf_idx].resize(sizeof(float) * imp_->grouped_rays.size());
    intensities_pinned = gputil::PinnedBuffer(imp_->intensities_buffers[buf_idx], gputil::kPinWrite);
  }
  if (timestamps)
  {
    imp_->timestamps_buffers[buf_idx].resize(sizeof(uint32_t) * imp_->grouped_rays.size());
    timestamps_pinned = gputil::PinnedBuffer(imp_->timestamps_buffers[buf_idx], gputil::kPinWrite);
  }

  // Upload to GPU.
  for (const RayItem &ray : imp_->grouped_rays)
  {
    upload_ray(ray);
  }

  // Asynchronous unpin. Kernels will wait on the associated event.
  keys_pinned.unpin(&gpu_cache->gpuQueue(), nullptr, &imp_->key_upload_events[buf_idx]);
  rays_pinned.unpin(&gpu_cache->gpuQueue(), nullptr, &imp_->ray_upload_events[buf_idx]);
  if (imp_->use_original_ray_buffers)
  {
    original_rays_pinned.unpin(&gpu_cache->gpuQueue(), nullptr, &imp_->original_ray_upload_events[buf_idx]);
  }
  if (intensities)
  {
    intensities_pinned.unpin(&gpu_cache->gpuQueue(), nullptr, &imp_->intensities_upload_events[buf_idx]);
  }
  if (timestamps)
  {
    timestamps_pinned.unpin(&gpu_cache->gpuQueue(), nullptr, &imp_->timestamps_upload_events[buf_idx]);
  }

  imp_->ray_counts[buf_idx] = uploaded_ray_count;
  imp_->unclipped_sample_counts[buf_idx] = unclipped_samples;

  if (uploaded_ray_count == 0)
  {
    return 0u;
  }

  enqueueRegions(buf_idx, region_update_flags);

  return uploaded_ray_count * 2;
}


void GpuMap::waitOnPreviousOperation(int buffer_index)
{
  // Wait first on the event known to complete last.
  imp_->region_update_events[buffer_index].wait();
  imp_->region_update_events[buffer_index].release();

  imp_->key_upload_events[buffer_index].wait();
  imp_->key_upload_events[buffer_index].release();
  imp_->ray_upload_events[buffer_index].wait();
  imp_->ray_upload_events[buffer_index].release();

  imp_->region_key_upload_events[buffer_index].wait();
  imp_->region_key_upload_events[buffer_index].release();

  for (VoxelUploadInfo &upload_info : imp_->voxel_upload_info[buffer_index])
  {
    upload_info.offset_upload_event.wait();
    upload_info.offset_upload_event.release();
  }
}


void GpuMap::enqueueRegions(int buffer_index, unsigned region_update_flags)
{
  // For each region we need to enqueue the voxel data for that region. Within the GpuCache, each GpuLayerCache
  // manages the voxel data for a voxel layer and uploads into a single buffer for that layer returning an offset into
  // that buffer. For each (relevant) layer, we need to record the memory offset and upload corresponding event. These
  // need to be later fed to the update kernel.

  // Size the region buffers.
  imp_->region_key_buffers[buffer_index].elementsResize<gputil::int3>(imp_->regions.size());

  for (VoxelUploadInfo &upload_info : imp_->voxel_upload_info[buffer_index])
  {
    upload_info.offsets_buffer.elementsResize<uint64_t>(imp_->regions.size());
    upload_info.offsets_buffer_pinned = gputil::PinnedBuffer(upload_info.offsets_buffer, gputil::kPinWrite);
  }

  // Execute on each region.
  gputil::PinnedBuffer regions_buffer(imp_->region_key_buffers[buffer_index], gputil::kPinWrite);

  GpuCache &gpu_cache = *this->gpuCache();
  for (const auto &region_key : imp_->regions)
  {
    const int try_limit = 2;
    for (int tries = 0; tries < try_limit; ++tries)
    {
      if (enqueueRegion(region_key, buffer_index))
      {
        // logutil::trace("region: [", region_key.x, ' ', region_key.y, ' ', region_key.z, "]\n");
        gputil::int3 gpu_region_key = { region_key.x, region_key.y, region_key.z };
        regions_buffer.write(&gpu_region_key, sizeof(gpu_region_key),
                             imp_->region_counts[buffer_index] * sizeof(gpu_region_key));
        ++imp_->region_counts[buffer_index];
        break;  // Break the tries loop into the outer loop.
      }

      if (tries + 1 < try_limit)
      {
        // Enqueue region failed. Flush pending all pending operations and before trying again in the current buffer.
        regions_buffer.unpin(&gpu_cache.gpuQueue(), nullptr, &imp_->region_key_upload_events[buffer_index]);
        for (VoxelUploadInfo &upload_info : imp_->voxel_upload_info[buffer_index])
        {
          upload_info.offsets_buffer_pinned.unpin(&gpu_cache.gpuQueue(), nullptr, &upload_info.offset_upload_event);
        }

        // We may have failed to enqueue anything. Only finalise if there's something to do.
        if (imp_->region_counts[buffer_index])
        {
          finaliseBatch(region_update_flags);
        }

        // Because we are reusing the same rays buffer, we need to cache the following two events and restore them
        // after waitOnPreviousOperation(), which would release them.
        gputil::Event key_upload_events = imp_->key_upload_events[buffer_index];
        gputil::Event ray_upload_events = imp_->ray_upload_events[buffer_index];

        // Since our cache memory is full, we should wait for all operations to complete as we may need a clear cache.
        // We re-use the same buffer_index to avoid copying ray data.
        for (int i = 0; i < int(GpuMapDetail::kBuffersCount); ++i)
        {
          waitOnPreviousOperation(i);
        }

        // Use the same buffer.
        // FIXME(KS): this is programming by side-effects. Should probably pass the buffer index to finaliseBatch()
        // instead of letting it read imp_->next_buffers_index.
        imp_->next_buffers_index = buffer_index;
        imp_->key_upload_events[buffer_index] = key_upload_events;
        imp_->ray_upload_events[buffer_index] = ray_upload_events;

        // Re-pin buffers.
        regions_buffer.pin();
        for (VoxelUploadInfo &upload_info : imp_->voxel_upload_info[buffer_index])
        {
          upload_info.offsets_buffer_pinned.pin();
        }
      }
      else
      {
        // TODO(KS): throw with more information.
        logutil::error("Failed to enqueue region data\n");
      }
    }
  }

  regions_buffer.unpin(&gpu_cache.gpuQueue(), nullptr, &imp_->region_key_upload_events[buffer_index]);
  for (VoxelUploadInfo &upload_info : imp_->voxel_upload_info[buffer_index])
  {
    upload_info.offsets_buffer_pinned.unpin(&gpu_cache.gpuQueue(), nullptr, &upload_info.offset_upload_event);
  }

  if (!imp_->region_key_upload_events[buffer_index].isValid())
  {
    logutil::error("Invalid region key upload event\n");
  }

  finaliseBatch(region_update_flags);
}


bool GpuMap::enqueueRegion(const glm::i16vec3 &region_key, int buffer_index)
{
  // Upload chunk to GPU.
  MapChunk *chunk = nullptr;
  // uint64_t mem_offset;
  GpuLayerCache::CacheStatus status;

  GpuCache &gpu_cache = *this->gpuCache();

  for (VoxelUploadInfo &voxel_info : imp_->voxel_upload_info[buffer_index])
  {
    GpuLayerCache &layer_cache = *gpu_cache.layerCache(voxel_info.gpu_layer_id);
    unsigned cache_flags = 0;
    cache_flags |= voxel_info.allow_region_creation * GpuLayerCache::kAllowRegionCreate;
    cache_flags |= voxel_info.skip_cpu_sync * GpuLayerCache::kSkipDownload;
    auto mem_offset = uint64_t(layer_cache.upload(*imp_->map, region_key, chunk, &voxel_info.voxel_upload_event,
                                                  &status, imp_->batch_marker, cache_flags));

    if (status == GpuLayerCache::kCacheFull)
    {
      logutil::warn("GpuLayerCache full: [", layer_cache.layerIndex(), "] ",
                    (layer_cache.layerIndex() < imp_->map->layout().layerCount()) ?
                      imp_->map->layout().layer(layer_cache.layerIndex()).name() :
                      "<out-of-range>",
                    "\n");
      return false;
    }

    // logutil::trace("region: [", region_key.x, ' ', region_key.y, ' ', region_key.z, "]\n");
    voxel_info.offsets_buffer_pinned.write(&mem_offset, sizeof(mem_offset),
                                           imp_->region_counts[buffer_index] * sizeof(mem_offset));
  }

  return true;
}


void GpuMap::finaliseBatch(unsigned region_update_flags)
{
  const int buf_idx = imp_->next_buffers_index;
  const OccupancyMapDetail *map = imp_->map->detail();

  // Complete region data upload.
  GpuCache &gpu_cache = *this->gpuCache();
  GpuLayerCache &occupancy_layer_cache = *gpu_cache.layerCache(kGcIdOccupancy);
  GpuLayerCache *mean_layer_cache = nullptr;
  GpuLayerCache *traversal_layer_cache = nullptr;
  GpuLayerCache *touch_times_layer_cache = nullptr;
  GpuLayerCache *incidents_layer_cache = nullptr;

  const int occ_uidx = imp_->occupancy_uidx;
  const int mean_uidx = imp_->mean_uidx;
  const int traversal_uidx = imp_->traversal_uidx;
  const int touch_time_uidx = imp_->touch_time_uidx;
  const int incident_normal_uidx = imp_->incident_normal_uidx;

  if (imp_->support_voxel_mean && imp_->map->voxelMeanEnabled())
  {
    mean_layer_cache = gpu_cache.layerCache(kGcIdVoxelMean);
  }

  if (imp_->support_traversal && imp_->map->traversalEnabled())
  {
    traversal_layer_cache = gpu_cache.layerCache(kGcIdTraversal);
  }

  if (imp_->map->touchTimeEnabled())
  {
    touch_times_layer_cache = gpu_cache.layerCache(kGcIdTouchTime);
  }

  if (imp_->map->incidentNormalEnabled())
  {
    incidents_layer_cache = gpu_cache.layerCache(kGcIdIncidentNormal);
  }

  // Enqueue update kernel.
  const gputil::int3 region_dim_gpu = { map->region_voxel_dimensions.x, map->region_voxel_dimensions.y,
                                        map->region_voxel_dimensions.z };

  const unsigned region_count = imp_->region_counts[buf_idx];
  const unsigned ray_count = imp_->ray_counts[buf_idx];
  int next_upload_buffer = 0;
  gputil::Dim3 global_size(ray_count);
  gputil::Dim3 local_size(std::min<size_t>(imp_->update_kernel.optimalWorkGroupSize(), ray_count));
  gputil::EventList wait({ imp_->key_upload_events[buf_idx], imp_->ray_upload_events[buf_idx],
                           imp_->region_key_upload_events[buf_idx],
                           imp_->voxel_upload_info[buf_idx][next_upload_buffer].offset_upload_event,
                           imp_->voxel_upload_info[buf_idx][next_upload_buffer].voxel_upload_event });
  ++next_upload_buffer;

  // Add supplemental buffer upload events
  // Note: we do not wait on original_ray_upload_events even if imp_->use_original_ray_buffers because we do not use the
  // original_ray_buffers in this algorithm. Ones that do should add it to the wait list here.
  if (imp_->intensities_upload_events[buf_idx].isValid())
  {
    wait.add(imp_->intensities_upload_events[buf_idx]);
  }
  if (imp_->timestamps_upload_events[buf_idx].isValid())
  {
    wait.add(imp_->timestamps_upload_events[buf_idx]);
  }

  // Add voxel mean offset upload events.
  if (mean_layer_cache)
  {
    wait.add(imp_->voxel_upload_info[buf_idx][next_upload_buffer].offset_upload_event);
    wait.add(imp_->voxel_upload_info[buf_idx][next_upload_buffer].voxel_upload_event);
    ++next_upload_buffer;
  }
  if (traversal_layer_cache)
  {
    wait.add(imp_->voxel_upload_info[buf_idx][next_upload_buffer].offset_upload_event);
    wait.add(imp_->voxel_upload_info[buf_idx][next_upload_buffer].voxel_upload_event);
    ++next_upload_buffer;
  }
  if (touch_times_layer_cache)
  {
    wait.add(imp_->voxel_upload_info[buf_idx][next_upload_buffer].offset_upload_event);
    wait.add(imp_->voxel_upload_info[buf_idx][next_upload_buffer].voxel_upload_event);
    ++next_upload_buffer;
  }
  if (incidents_layer_cache)
  {
    wait.add(imp_->voxel_upload_info[buf_idx][next_upload_buffer].offset_upload_event);
    wait.add(imp_->voxel_upload_info[buf_idx][next_upload_buffer].voxel_upload_event);
    ++next_upload_buffer;
  }

  // Supporting voxel mean and traversal are putting us at the limit of what we can support using this sort of
  // conditional invocation.
  imp_->update_kernel(
    global_size, local_size, wait, imp_->region_update_events[buf_idx], &gpu_cache.gpuQueue(),
    // Kernel args begin:
    // Occupancy voxels and offsets.
    gputil::BufferArg<float>(*occupancy_layer_cache.buffer()),
    gputil::BufferArg<uint64_t>(imp_->voxel_upload_info[buf_idx][occ_uidx].offsets_buffer),
    // Mean voxels and offsets.
    gputil::BufferArg<VoxelMean>(mean_layer_cache ? mean_layer_cache->buffer() : nullptr),
    gputil::BufferArg<uint64_t>(mean_layer_cache ? &imp_->voxel_upload_info[buf_idx][mean_uidx].offsets_buffer :
                                                   nullptr),
    // Traversal voxels and offsets.
    gputil::BufferArg<float>(traversal_layer_cache ? traversal_layer_cache->buffer() : nullptr),
    gputil::BufferArg<uint64_t>(
      traversal_layer_cache ? &imp_->voxel_upload_info[buf_idx][traversal_uidx].offsets_buffer : nullptr),
    // Touch times voxels and offsets.
    gputil::BufferArg<uint32_t>(touch_times_layer_cache ? touch_times_layer_cache->buffer() : nullptr),
    gputil::BufferArg<uint64_t>(
      touch_times_layer_cache ? &imp_->voxel_upload_info[buf_idx][touch_time_uidx].offsets_buffer : nullptr),
    // Incident normal voxels and offsets.
    gputil::BufferArg<uint32_t>(incidents_layer_cache ? incidents_layer_cache->buffer() : nullptr),
    gputil::BufferArg<uint64_t>(
      incidents_layer_cache ? &imp_->voxel_upload_info[buf_idx][incident_normal_uidx].offsets_buffer : nullptr),
    // Region keys and region count
    gputil::BufferArg<gputil::int3>(imp_->region_key_buffers[buf_idx]), region_count,
    // Ray start/end keys
    gputil::BufferArg<GpuKey>(imp_->key_buffers[buf_idx]),
    // Ray start end points, local to end voxel and ray count
    gputil::BufferArg<gputil::float3>(imp_->ray_buffers[buf_idx]), ray_count,
    // Input touch times buffer
    gputil::BufferArg<uint32_t>((region_update_flags & kRfInternalTimestamps) ? &imp_->timestamps_buffers[buf_idx] :
                                                                                nullptr),
    // Region dimensions, map resolution, ray adjustment (miss), sample adjustment (hit)
    region_dim_gpu, float(map->resolution), map->miss_value, map->hit_value,
    // Occupied threshold, min occupancy, max occupancy, update flags.
    map->occupancy_threshold_value, map->min_voxel_value, map->max_voxel_value, region_update_flags);

  // Update most recent chunk GPU event.
  occupancy_layer_cache.updateEvents(imp_->batch_marker, imp_->region_update_events[buf_idx]);
  if (mean_layer_cache)
  {
    mean_layer_cache->updateEvents(imp_->batch_marker, imp_->region_update_events[buf_idx]);
  }
  if (traversal_layer_cache)
  {
    traversal_layer_cache->updateEvents(imp_->batch_marker, imp_->region_update_events[buf_idx]);
  }

  // logutil::trace(imp_->region_counts[buf_idx], "regions\n");

  imp_->region_counts[buf_idx] = 0;
  // Start a new batch for the GPU layers.
  imp_->batch_marker = occupancy_layer_cache.beginBatch();
  if (mean_layer_cache)
  {
    mean_layer_cache->beginBatch(imp_->batch_marker);
  }
  if (traversal_layer_cache)
  {
    traversal_layer_cache->beginBatch(imp_->batch_marker);
  }
  imp_->next_buffers_index = (imp_->next_buffers_index + 1) % GpuMapDetail::kBuffersCount;
}


int GpuMap::enableVoxelUpload(int cache_id, bool enable)
{
  GpuCache &gpu_cache = *gpuCache();
  for (size_t i = 0; i < imp_->voxel_upload_info[0].size(); ++i)
  {
    if (imp_->voxel_upload_info[0][i].gpu_layer_id == cache_id)
    {
      if (enable)
      {
        // Request present and already is. Nothing to do.
        return int(i);
      }
      // Request removal and found present. Remove.
      imp_->voxel_upload_info[0].erase(imp_->voxel_upload_info[0].begin() + i);
      imp_->voxel_upload_info[1].erase(imp_->voxel_upload_info[1].begin() + i);
      return -1;
    }
  }

  // Not found.
  if (enable)
  {
    // Not found and requesting present. Add.
    imp_->voxel_upload_info[0].emplace_back(VoxelUploadInfo(cache_id, gpu_cache.gpu()));
    imp_->voxel_upload_info[1].emplace_back(VoxelUploadInfo(cache_id, gpu_cache.gpu()));
    return int(imp_->voxel_upload_info[0].size() - 1);
  }

  return -1;
}
}  // namespace ohm


================================================
FILE: ohmgpu/GpuMap.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_GPUMAP_H
#define OHMGPU_GPUMAP_H

#include "OhmGpuConfig.h"

#include <ohm/RayFilter.h>
#include <ohm/RayFlag.h>
#include <ohm/RayMapper.h>

#include <glm/glm.hpp>

#include <functional>
#include <vector>

namespace gputil
{
class Event;
class Buffer;
class PinnedBuffer;
}  // namespace gputil

namespace ohm
{
class Aabb;
class GpuCache;
struct GpuMapDetail;
class OccupancyMap;
class RayFilter;

struct VoxelUploadInfo;

namespace gpumap
{
/// Flags for GPU initialisation.
enum GpuFlag : unsigned
{
  /// Allow host (mappable) buffers. Used if device/host memory is unified.
  kGpuAllowMappedBuffers = (1u << 0u),
  /// Force mappable buffers.
  kGpuForceMappedBuffers = (1u << 1u),
};

/// Enable GPU usage for the given @p map. This creates a GPU cache for the @p map using the
/// default cache layer memory size and mappable GPU buffers.
///
/// Ignored if the @p map already is GPU enabled, simply returning the existing @c GpuMap.
///
/// @exception gputil::Exception Thrown when a @c gputil::ApiException is raised during GPU memory allocation. This
/// Generally indicates an out of memory issue, but can be caused by other API exceptions.
///
/// @param map The map to enable GPU usage on.
/// @return The @c GpuCache for the map. Null if GPU code is not enabled.
GpuCache ohmgpu_API *enableGpu(OccupancyMap &map);

/// Enable GPU usage for the given @p map.
///
/// Ignored if the @p map already is GPU enabled.
///
/// @exception gputil::Exception Thrown when a @c gputil::ApiException is raised during GPU memory allocation. This
/// Generally indicates an out of memory issue, but can be caused by other API exceptions.
///
/// @param map The map to enable GPU usage on.
/// @param target_gpu_mem_size Target GPU memory usage. This is split amongst the active, default layers.
/// @param gpu_flags @c GpuFlag values controlling initialisation.
/// @return The @c GpuCache for the map. Null if GPU code is not enabled.
GpuCache ohmgpu_API *enableGpu(OccupancyMap &map, size_t target_gpu_mem_size,
                               unsigned gpu_flags = kGpuAllowMappedBuffers);

// /// Reports the status of setting up the associated GPU program for populating the map.
// ///
// /// integrateRays() only functions when this function reports @c true. Otherwise calls to @c integrateRays() are
// /// ignored and this class is non-functional.
// ///
// /// @return True when the GPU has been successfully initialised.
// bool gpuOk();

/// Sync all GPU layer memory to main memory ensuring main memory is up to date.
///
/// Note: individual @c GpuLayerCache syncing is preferred to global synching.
///
/// Does nothing if the map has no GPU cache or GPU code is disabled.
/// @param map The map to sync GPU memory for.
void ohmgpu_API sync(OccupancyMap &map);

/// Sync a specific GPU memory layer to main memory.
///
/// Does nothing if the map has no GPU cache or GPU code is disabled.
/// @param map The map to sync GPU memory for.
/// @param layer_index The index of the layer to sync.
void ohmgpu_API sync(OccupancyMap &map, unsigned layer_index);

/// Retrieves the GPU cache used by @p map if GPU usage has been enabled for @p map.
/// @return The GPU cache for @p map.
GpuCache ohmgpu_API *gpuCache(OccupancyMap &map);

// --- Internal support items ---
/// Callback for walkRegions().
using RegionWalkFunction = std::function<void(const glm::i16vec3 &, const glm::dvec3 &, const glm::dvec3 &)>;

/// Walk the regions touched by the line connecting @p start_point and @p end_point invoking @c on_visit for each
/// touched region. This is similar @c walkSegmentKeys() in @c LineWalk.h .
///
/// @param map The map to walk.
/// @param start_point The line/ray origin.
/// @param end_point The line/ray end point.
/// @param on_visit Function to call for each visited region.
void ohmgpu_API walkRegions(const OccupancyMap &map, const glm::dvec3 &start_point, const glm::dvec3 &end_point,
                            const RegionWalkFunction &on_visit);
}  // namespace gpumap

/// A wrapper for an @c OccupancyMap that uses GPU to update and manage the wrapped map.
///
/// All map updates should go via this class, not directly to the @c OccupancyMap in order to ensure the map is
/// correctly maintained. The general usage is:
/// - Build batches for line segments/sample rays to integrate.
/// - Call @c integrateRays() with the collected line segments.
/// - Do the same with additional sample data.
/// - When required, call @c sync() to ensure CPU memory is up to date.
///
/// The call to @c sync() is required before operating on the @c OccupancyMap directly.
///
/// Use of this class can provide significant gains over pure CPU ray integration using @c OccupancyMap member
/// functions.
///
/// @par Known issues
/// - There can sometimes be a significant pause in syncing from GPU. This appears to relate to waiting on the
///   OpenCL event, which should already be complete. This may have been due to a now fixed memory overwrite.
/// - The GPU code may sometimes have excessive contention on updating the value of a single voxel. The GPU code is
///   written to abort after a large number of iterations. This also may have been worsened by the memory overwrite.
/// - With Visual Studio 15.4, the unit tests using this class, such as GpuMap.Populate, often crash in debug.
///   Release mode appears ok.
///
/// @todo Evaluate delay creation of @c MapChunk objects. This requires tracking whether a region has been updated
/// at all by GPU. We only download data for touched regions and create the @c MapChunk then.
///
/// @todo This class should be refactored as GPU cache is now included in the OccupancyMap and we are seeing non
/// occupancy based GPU algorithms.
class ohmgpu_API GpuMap : public RayMapper
{
protected:
  /// Constructor for derived classes to call.
  /// @param detail The pimpl data struture for the map. Must not be null. May be a derivation of @c GpuMapDetail .
  /// @param expected_element_count The expected point count for calls to @c integrateRays(). Used as a hint.
  /// @param gpu_mem_size Optionally specify the target GPU cache memory to allocate.
  explicit GpuMap(GpuMapDetail *detail, unsigned expected_element_count = 2048,  // NOLINT(readability-magic-numbers)
                  size_t gpu_mem_size = 0u);

public:
  /// Construct @c GpuMap support capabilities around @p map. The @p map pointer may be borrowed or owned.
  ///
  /// If @p gpuMemSize is not specified, then up to 1GiB or 3/4 of the GPU memory will be allocated for the GPU
  /// cache, whichever is smaller.
  ///
  /// @exception gputil::Exception Thrown when a @c gputil::ApiException is raised during GPU memory allocation. This
  /// Generally indicates an out of memory issue, but can be caused by other API exceptions.
  ///
  ///
  /// @param map The map to wrap.
  /// @param borrowed_map True to borrow the map, @c false for this object to take ownership.
  /// @param expected_element_count The expected point count for calls to @c integrateRays(). Used as a hint.
  /// @param gpu_mem_size Optionally specify the target GPU cache memory to allocate.
  explicit GpuMap(OccupancyMap *map, bool borrowed_map = true,
                  unsigned expected_element_count = 2048,  // NOLINT(readability-magic-numbers)
                  size_t gpu_mem_size = 0u);

  /// Destructor. Will wait on outstanding GPU operations first and destroy the @c map() if not using a
  /// @c borrowedPointer().
  ~GpuMap() override;

  /// Reports the status of setting up the associated GPU program for populating the map.
  ///
  /// integrateRays() only functions when this function reports @c true. Otherwise calls to @c integrateRays() are
  /// ignored and this class is non-functional.
  ///
  /// @return True when the GPU has been successfully initialised.
  bool gpuOk() const;

  /// Validate function from @c RayMapper . Based on @c gpuOk() result.
  /// @return True if validated and @c integrateRays() is safe to call.
  inline bool valid() const override { return gpuOk(); }

  /// Request the @c OccupancyMap this @c GpuMap wraps.
  /// @return The underlying @c OccupancyMap.
  OccupancyMap &map();
  /// Request the @c OccupancyMap this @c GpuMap wraps.
  /// @return The underlying @c OccupancyMap.
  const OccupancyMap &map() const;

  /// Is the @c GpuMap using a borrowed pointer to @c map()?
  /// If not borrowed, then the @c GpuMap will destroy the @c OccupancyMap on destruction.
  bool borrowedMap() const;

  /// Sync the GPU memory back to main memory for all GPU cached layers.
  void syncVoxels();

  /// Sync a subset of voxel layers from GPU to main memory. The @p layer_indices may be read from @c MapLayout .
  /// Any out of range index is ignored. This allows @c MapLayout::namedLayer() functions to be used for layers which
  /// are not present.
  void syncVoxels(const std::vector<int> &layer_indices);

  /// Set the range filter applied to all rays given to @c integrateRays(). Setting a null filter ensures no
  /// filtering is performed. The default behaviour is to use the same filter as the @c OccupancyMap.
  ///
  /// Note: not having a filter for a GpuMap is highly inadvisable; bad data such as infinite NaN points will cause
  /// the GPU to hang.
  ///
  /// @param ray_filter The range filter to install and apply to @c integrateRays().
  ///   Accepts a null pointer, which clears the filter.
  void setRayFilter(const RayFilterFunction &ray_filter);

  /// Get the installed range filter applied to all rays given to @c integrateRays().
  /// @return The installed range filter.
  const RayFilterFunction &rayFilter() const;

  /// Get the range filter actually being used. This will be the one belonging to the wrapped @c OccupancyMap when
  /// the @c GpuMap does not have an explicitly installed filter.
  /// @return The range filter currently in use.
  const RayFilterFunction &effectiveRayFilter() const;

  /// Clears the @c rayFilter(). Unlike the same method in @c OccupancyMap, this is not the same a setting a null
  /// filter. For the @p GpuMap, @c clearRayFilter() restores the default behaviour of using the same filter
  /// as the underlying @c OccupancyMap.
  void clearRayFilter();

  /// Access the @c OccupancyMap::hitValue() for API compatibility.
  /// @return @c OccupancyMap::hitValue().
  float hitValue() const;

  /// Pass-through to @c OccupancyMap::setHitValue() for API compatibility.
  /// @param value New hit value.
  void setHitValue(float value);

  /// Access the @c OccupancyMap::missValue() for API compatibility.
  /// @return @c OccupancyMap::missValue().
  float missValue() const;

  /// Pass-through to @c OccupancyMap::setMissValue() for API compatibility.
  /// @param value New miss value.
  void setMissValue(float value);

  /// Get the ray segment length used to break up long rays. See @c setRaySegmentLength()
  /// @return The ray length at which to break up rays.
  double raySegmentLength() const;

  /// Set the ray segment length used to break up long rays.
  ///
  /// Long rays in a GPU warp (CUDA terminology) can cause the warp to do very little work as a single long ray can
  /// cause many additional iteractions when the rest of the warp has finished its work. To combat this, we can break
  /// up long rays into multiple parts or segments. Setting @c setRaySegmentLength() to a value greater than the map
  /// resolution enables this feature. Note, however, this should be a "reasonable value" for the given resolution
  /// or the GPU will be overloaded doing very little work per thread with significant overheads.
  ///
  /// Each ray longer than the specified @p length is broken in to segments of approximately equal length. Essentially
  /// the @c raySegmentLength() sets the upper bound for each segment, but each segment will have equal length.
  ///
  /// @param length The ray length at which to break up rays.
  void setRaySegmentLength(double length);

  /// Query if rays are being grouped by sample before upload to GPU.
  ///
  /// This is only set for algorithms which require grouping of rays such as the NDT update used by @c GpuNdtMap .
  /// It is not a user configurable property because of its algorithmic dependency. Rays are grouped by pre sorting
  /// in CPU before GPU upload. This means that ray batches are reordered before upload.
  ///
  /// @return True if rays are grouped by sample voxel before upload to GPU.
  bool groupedRays() const;

  /// Integrate the given @p rays into the map. The @p rays form a list of origin/sample pairs for which
  /// we generally consider the sample voxel as a hit when (increasing occupancy) and all other voxels as misses
  /// (free space). The sample may be treated as a miss when @p endPointsAsOccupied is false.
  ///
  /// This function prepares and queues a GPU update of the affected regions. The time spent in this function is
  /// variable as it may have to wait for existing GPU operations to complete before queuing new operations.
  ///
  /// Points are filtered using the @c effectiveRayFilter(). It is highly advisable to always have a filter
  /// installed which removes infinite and NaN points and long sample rays.
  ///
  /// Note: This call is ignored if @c gpuOk() is @c false.
  ///
  /// @param rays Array of origin/sample point pairs.
  /// @param element_count The number of points in @p rays. The ray count is half this value.
  /// @param intensities An array of intensity values matching the @p rays items. There is one intensity value per ray
  ///   so there are @c element_count/2 items. May be null to omit intensity values.
  /// @param timestamps An array of timestamp values matching the @p rays items. There is one timestamp value per ray
  ///   so there are @c element_count/2 items. May be null to omit timestamp values in which case the touch time layer
  ///   will not be updated.
  /// @param region_update_flags Flags controlling ray integration behaviour. See @c RayFlag.
  /// @return The number of rays integrated. Zero indicates a failure when @p pointCount is not zero.
  ///   In this case either the GPU is unavailable, or all @p rays are invalid.
  size_t integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities, const double *timestamps,
                       unsigned region_update_flags) override;

  using RayMapper::integrateRays;

  /// Internal use: get the GPU cache used by this map.
  /// @return The GPU cache this map uses.
  GpuCache *gpuCache() const;

protected:
  /// Some derivation uses of the @c GpuMap allow initialisation with a null map, though this is an edge case.
  ///
  /// This function allows the map pointer to be set. It is expected that use cases where the map can change always have
  /// @p borrowed_map @c true .
  ///
  /// No public API @c GpuMap implementation should support a null map.
  void setMap(OccupancyMap *map, bool borrowed_map, unsigned expected_element_count, size_t gpu_mem_size,
              bool force_gpu_program_release);

  /// Called after @c syncVoxels() completes synching known data.
  virtual inline void onSyncVoxels(int buffer_index) { (void)buffer_index; }

  /// Set the status of @c sortedRays() .
  ///
  /// This should only be set for algorithms which require ray grouping. For example, @c GpuNdtMap sorts rays for
  /// performance reasons.
  ///
  /// @param group The grouping status to set.
  void setGroupedRays(bool group);

  /// Cache the correct GPU program to cater for @c with_voxel_mean. Releases the existing program first when
  /// @p force is true or @p with_voxel_mean does not match the cached program.
  /// @param with_voxel_mean True to cache the program which supports voxel mean positioning (@ref voxelmean).
  /// @param with_traversal Include traversal calculations? Requires "traversal" layer.
  /// @param force Force release and program caching even if already correct. Must be used on initialisation.
  virtual void cacheGpuProgram(bool with_voxel_mean, bool with_traversal, bool force);

  /// Release the current GPU program.
  virtual void releaseGpuProgram();

  /// Implementation for various ways we can integrate rays into the map. See @c integrateRays() for general detail.
  /// @param rays Array of origin/sample point pairs. Expect either @c glm::dvec3 (preferred) or @c glm::vec3.
  /// @param element_count The number of points in @p rays. The ray count is half this value.
  /// @param intensities Optional - for each ray, intensity of the return (element_count/2 elements).
  /// @param timestamps Optiona - the timestamp value for each ray (element_count/2 elements).
  /// @param region_update_flags Flags controlling ray integration behaviour. See @c RayFlag.
  /// @param filter Filter function apply to each ray before passing to GPU. May be empty.
  /// @return The number of rays integrated. Zero indicates a failure when @p pointCount is not zero.
  ///   In this case either the GPU is unavailable, or all @p rays are invalid.
  size_t integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities, const double *timestamps,
                       unsigned region_update_flags, const RayFilterFunction &filter);

  /// Wait for previous ray batch, as indicated by @p buffer_index, to complete.
  /// @param buffer_index Identifies the batch to wait on.
  void waitOnPreviousOperation(int buffer_index);

  /// Enqueue upload of voxel data for the regions specified in @c GpuMapDetail::voxel_upload_info .
  ///
  /// May trigger a limited number of attempts to  @c finaliseBatch() if @c enqueueRegion() fails.
  /// @param buffer_index Index of active data buffers from @c GpuMapDetail to used.
  /// @param region_update_flags Flags controlling ray integration behaviour. See @c RayFlag.
  virtual void enqueueRegions(int buffer_index, unsigned region_update_flags);

  /// Enqueue a region for update.
  ///
  /// Ensure the following:
  /// - The region is added to @p map()
  /// - The region is in the @c GpuCache
  /// - Details of the region are added to @p regionsBuffer and @p offsetsBuffer
  ///
  /// This will call @c finaliseBatch() when the chunk cache is full will the still unprocessed batch. This ensures
  /// the current batch executes. The function will then recurse once (using @c allowRetry) modifying the
  /// @p regionsBuffer and @p offsetsBuffer to fill alternative GPU buffers for the next batch.
  ///
  /// @param region_key The key for the region of interest.
  /// @param buffer_index Index of active data buffers from @c GpuMapDetail to used.
  virtual bool enqueueRegion(const glm::i16vec3 &region_key, int buffer_index);

  /// Finalise the current ray/region batch and start executing GPU kernel.
  /// @param region_update_flags Flags controlling ray integration behaviour. See @c RayFlag.
  virtual void finaliseBatch(unsigned region_update_flags);

  /// Helper to enable caching of a particular gpu cache item.
  /// @param cache_id Cache ID. Expect a @c GpuCacheId member, but not type constrained to reduce include statements.
  /// @param enable True to enable caching, false to disable caching.
  /// @return The index at which the @c VoxelUploadInfo for the cache is stored. -1 when disbled.
  int enableVoxelUpload(int cache_id, bool enable);

  GpuMapDetail *imp_;  ///< Internal pimpl data.
};
}  // namespace ohm

#endif  // OHMGPU_GPUMAP_H


================================================
FILE: ohmgpu/GpuNdtMap.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "GpuNdtMap.h"

#include "private/GpuNdtMapDetail.h"

#include "GpuCache.h"
#include "GpuKey.h"
#include "GpuLayerCache.h"

#include "private/GpuProgramRef.h"

#include <ohm/MapChunk.h>
#include <ohm/OccupancyMap.h>
#include <ohm/VoxelMean.h>

#include <ohm/private/OccupancyMapDetail.h>

#include <gputil/gpuBuffer.h>
#include <gputil/gpuEvent.h>
#include <gputil/gpuKernel.h>
#include <gputil/gpuPinnedBuffer.h>
#include <gputil/gpuPlatform.h>
#include <gputil/gpuProgram.h>

#include <logutil/Logger.h>

#include <glm/ext.hpp>
#include <glm/gtx/norm.hpp>

#include <iostream>

// Must come after glm includes due to usage on GPU.
#include <ohm/CovarianceVoxel.h>

#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
#include "CovarianceHitNdtResource.h"
#include "RegionUpdateResource.h"
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL

#if GPUTIL_TYPE == GPUTIL_CUDA
GPUTIL_CUDA_DECLARE_KERNEL(regionRayUpdateNdt);
GPUTIL_CUDA_DECLARE_KERNEL(covarianceHitNdt);
#endif  // GPUTIL_TYPE == GPUTIL_CUDA

namespace ohm
{
namespace
{
// Note on -DNDT=
// We need to define NDT to the correct value depending on the algorithm to use. See NdtModeDef.cl
// 0 => no NDT
// 1 => NDT occupancy map
// 2 => NDT traverability map
#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref_ndt_miss("RegionUpdate", GpuProgramRef::kSourceString, RegionUpdateCode,  // NOLINT
                                     RegionUpdateCode_length, { "-DNDT" });
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref_ndt_hit("CovarianceHitNdt", GpuProgramRef::kSourceString, CovarianceHitNdtCode,  // NOLINT
                                    CovarianceHitNdtCode_length);
#else   // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref_ndt_miss("RegionUpdate", GpuProgramRef::kSourceFile, "RegionUpdate.cl", 0u, { "-DNDT" });
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref_ndt_hit("CovarianceHitNdt", GpuProgramRef::kSourceFile, "CovarianceHitNdt.cl");  // NOLINT
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
}  // namespace


GpuNdtMap::GpuNdtMap(OccupancyMap *map, bool borrowed_map, unsigned expected_element_count, size_t gpu_mem_size,
                     NdtMode ndt_mode)
  : GpuMap(new GpuNdtMapDetail(map, borrowed_map, ndt_mode), expected_element_count, gpu_mem_size)
{
  // Ensure voxel mean and covariance layers are present.

  for (int i = 0; i < 2; ++i)
  {
    if (ndt_mode != NdtMode::kNone)
    {
      detail()->cov_uidx = int(imp_->voxel_upload_info[i].size());  // Set twice to the same value, but that's ok.
      imp_->voxel_upload_info[i].emplace_back(VoxelUploadInfo(kGcIdCovariance, gpuCache()->gpu()));
    }
    if (ndt_mode == NdtMode::kTraversability)
    {
      detail()->intensity_uidx = int(imp_->voxel_upload_info[i].size());  // Set twice to the same value, but that's ok.
      imp_->voxel_upload_info[i].emplace_back(VoxelUploadInfo(kGcIdIntensity, gpuCache()->gpu()));
      detail()->hit_miss_uidx = int(imp_->voxel_upload_info[i].size());  // Set twice to the same value, but that's ok.
      imp_->voxel_upload_info[i].emplace_back(VoxelUploadInfo(kGcIdHitMiss, gpuCache()->gpu()));
    }
  }

  // Cache the correct GPU program.
  cacheGpuProgram(true, map && imp_->support_traversal && map->traversalEnabled(), true);

  // Rays should be grouped by sample voxel for performance reasons. The GPU update assumes this grouping.
  setGroupedRays(true);
}  // namespace ohm


GpuNdtMap::~GpuNdtMap()
{
  GpuNdtMap::releaseGpuProgram();
}


void GpuNdtMap::setSensorNoise(float noise_range)
{
  GpuNdtMapDetail *imp = detail();
  imp->ndt_map.setSensorNoise(noise_range);
}


float GpuNdtMap::sensorNoise() const
{
  const GpuNdtMapDetail *imp = detail();
  return imp->ndt_map.sensorNoise();
}


NdtMap &GpuNdtMap::ndtMap()
{
  GpuNdtMapDetail *imp = detail();
  return imp->ndt_map;
}


const NdtMap &GpuNdtMap::ndtMap() const
{
  const GpuNdtMapDetail *imp = detail();
  return imp->ndt_map;
}


GpuNdtMapDetail *GpuNdtMap::detail()
{
  return static_cast<GpuNdtMapDetail *>(imp_);
}


const GpuNdtMapDetail *GpuNdtMap::detail() const
{
  return static_cast<const GpuNdtMapDetail *>(imp_);
}


void GpuNdtMap::cacheGpuProgram(bool /*with_voxel_mean*/, bool /*with_traversal_rate*/, bool force)
{
  if (imp_->program_ref)
  {
    if (!force)
    {
      return;
    }
  }

  releaseGpuProgram();

  GpuCache &gpu_cache = *gpuCache();
  GpuNdtMapDetail *imp = detail();
  imp->gpu_ok = true;
  imp->cached_sub_voxel_program = true;

  switch (imp->ndt_map.mode())
  {
  default:  // Unknown mode
    imp_->gpu_ok = false;
    break;
  case NdtMode::kOccupancy:
  case NdtMode::kTraversability:
    imp->program_ref = &g_program_ref_ndt_miss;
    imp->cov_hit_program_ref = &g_program_ref_ndt_hit;
    imp->gpu_ok = imp->program_ref->addReference(gpu_cache.gpu()) && imp_->gpu_ok;
    imp->gpu_ok = imp->cov_hit_program_ref->addReference(gpu_cache.gpu()) && imp_->gpu_ok;

    if (imp_->gpu_ok)
    {
      imp->update_kernel = GPUTIL_MAKE_KERNEL(imp->program_ref->program(), regionRayUpdateNdt);
      imp->cov_hit_kernel = GPUTIL_MAKE_KERNEL(imp->cov_hit_program_ref->program(), covarianceHitNdt);
    }
    break;
  }

  if (imp_->gpu_ok)
  {
    imp->update_kernel.calculateOptimalWorkGroupSize();
    imp->cov_hit_kernel.calculateOptimalWorkGroupSize();
    imp->gpu_ok = imp->update_kernel.isValid() && imp->cov_hit_kernel.isValid();
  }
}


void GpuNdtMap::finaliseBatch(unsigned region_update_flags)
{
  const int buf_idx = imp_->next_buffers_index;
  GpuNdtMapDetail *imp = detail();

  // Wait for: upload of ray keys, upload of rays, upload of region key mapping.
  gputil::EventList wait(
    { imp->key_upload_events[buf_idx], imp->ray_upload_events[buf_idx], imp->region_key_upload_events[buf_idx] });

  // Add supplemental buffer upload events
  // Note: we do not wait on original_ray_upload_events even if imp_->use_original_ray_buffers because we do not use the
  // original_ray_buffers in this algorithm. Ones that do should add it to the wait list here.
  if (imp_->intensities_upload_events[buf_idx].isValid())
  {
    wait.add(imp_->intensities_upload_events[buf_idx]);
  }
  if (imp_->timestamps_upload_events[buf_idx].isValid())
  {
    wait.add(imp_->timestamps_upload_events[buf_idx]);
  }

  // Add wait for region voxel offsets
  for (auto &upload_info : imp->voxel_upload_info[buf_idx])
  {
    wait.add(upload_info.offset_upload_event);
    wait.add(upload_info.voxel_upload_event);
  }

  // We will track which caches we depend on to manage event waiting.
  TouchedCacheSet touched_caches = { nullptr };

  switch (imp->ndt_map.mode())
  {
  case NdtMode::kOccupancy:
  case NdtMode::kTraversability:
    invokeNdt(region_update_flags, buf_idx, wait, touched_caches);
    break;
  default:
    // Invalid mode et.
    break;
  }

  // Update most recent chunk GPU event.
  for (auto &&layer_cache : touched_caches)
  {
    if (layer_cache)
    {
      layer_cache->updateEvents(imp->batch_marker, imp->region_update_events[buf_idx]);
    }
  }

  // logutil::trace(imp->region_counts[buf_idx], " regions\n");

  imp->region_counts[buf_idx] = 0;
  // Start a new batch for the GPU layers.
  bool have_batch = false;
  for (auto &&layer_cache : touched_caches)
  {
    if (layer_cache)
    {
      if (have_batch)
      {
        layer_cache->beginBatch(imp->batch_marker);
      }
      else
      {
        imp->batch_marker = layer_cache->beginBatch();
      }
      layer_cache->updateEvents(imp->batch_marker, imp->region_update_events[buf_idx]);
    }
  }

  imp->next_buffers_index = 1 - imp->next_buffers_index;
}


void GpuNdtMap::releaseGpuProgram()
{
  GpuMap::releaseGpuProgram();
  GpuNdtMapDetail *imp = detail();
  if (imp && imp->cov_hit_kernel.isValid())
  {
    imp->cov_hit_kernel = gputil::Kernel();
  }

  if (imp && imp->cov_hit_program_ref)
  {
    imp->cov_hit_program_ref->releaseReference();
    imp->cov_hit_program_ref = nullptr;
  }
}


void GpuNdtMap::invokeNdt(unsigned region_update_flags, int buf_idx, gputil::EventList &wait,
                          TouchedCacheSet &touched_caches)
{
  GpuNdtMapDetail *imp = detail();
  const OccupancyMapDetail *map = imp->map->detail();

  // Complete region data upload.
  GpuCache &gpu_cache = *gpuCache();

  GpuLayerCache &occupancy_layer_cache = *gpu_cache.layerCache(kGcIdOccupancy);
  GpuLayerCache &mean_layer_cache = *gpu_cache.layerCache(kGcIdVoxelMean);
  GpuLayerCache &cov_voxel_layer_cache = *gpu_cache.layerCache(kGcIdCovariance);
  GpuLayerCache *intensity_layer_cache = nullptr;
  GpuLayerCache *hit_miss_layer_cache = nullptr;
  GpuLayerCache *traversal_layer_cache = nullptr;
  GpuLayerCache *touch_times_layer_cache = nullptr;
  GpuLayerCache *incidents_layer_cache = nullptr;

  const int occ_uidx = detail()->occupancy_uidx;
  const int mean_uidx = detail()->mean_uidx;
  const int cov_uidx = detail()->cov_uidx;
  const int intense_uidx = detail()->intensity_uidx;
  const int hit_miss_uidx = detail()->hit_miss_uidx;
  const int traversal_uidx = detail()->traversal_uidx;
  const int touch_time_uidx = imp_->touch_time_uidx;
  const int incident_normal_uidx = imp_->incident_normal_uidx;

  if (imp->ndt_map.mode() == NdtMode::kTraversability)
  {
    intensity_layer_cache = gpu_cache.layerCache(kGcIdIntensity);
    hit_miss_layer_cache = gpu_cache.layerCache(kGcIdHitMiss);
  }

  if (imp_->support_traversal && imp_->map->traversalEnabled())
  {
    traversal_layer_cache = gpu_cache.layerCache(kGcIdTraversal);
  }

  if (imp_->map->touchTimeEnabled())
  {
    touch_times_layer_cache = gpu_cache.layerCache(kGcIdTouchTime);
  }

  if (imp_->map->incidentNormalEnabled())
  {
    incidents_layer_cache = gpu_cache.layerCache(kGcIdIncidentNormal);
  }

  size_t tidx = 0;
  touched_caches[tidx++] = &occupancy_layer_cache;
  touched_caches[tidx++] = &mean_layer_cache;
  touched_caches[tidx++] = &cov_voxel_layer_cache;
  if (intensity_layer_cache)
  {
    touched_caches[tidx++] = intensity_layer_cache;
  }
  if (hit_miss_layer_cache)
  {
    touched_caches[tidx++] = hit_miss_layer_cache;
  }
  if (traversal_layer_cache)
  {
    touched_caches[tidx++] = traversal_layer_cache;
  }
  if (touch_times_layer_cache)
  {
    touched_caches[tidx++] = touch_times_layer_cache;
  }
  if (incidents_layer_cache)
  {
    touched_caches[tidx++] = incidents_layer_cache;
  }
  for (; tidx < touched_caches.size(); ++tidx)
  {
    touched_caches[tidx] = nullptr;
  }

  // Enqueue update kernel.
  const gputil::int3 region_dim_gpu = { map->region_voxel_dimensions.x, map->region_voxel_dimensions.y,
                                        map->region_voxel_dimensions.z };

  const unsigned region_count = imp->region_counts[buf_idx];
  const unsigned ray_count = imp->ray_counts[buf_idx];
  // Rays are sorted such that unclipped entries come first. We only need to consider these.
  const unsigned sample_count = imp->unclipped_sample_counts[buf_idx];

  gputil::Event first_kernel_event;
  gputil::Dim3 global_size;
  gputil::Dim3 local_size;

  unsigned modify_flags = (!(region_update_flags & kRfEndPointAsFree)) ? kRfExcludeSample : 0u;

  if (!(region_update_flags & kRfExcludeRay))
  {
    global_size = gputil::Dim3(ray_count);
    local_size = gputil::Dim3(std::min<size_t>(imp->update_kernel.optimalWorkGroupSize(), ray_count));
    imp->update_kernel(
      global_size, local_size, wait, first_kernel_event, &gpu_cache.gpuQueue(),
      // Kernel args begin:
      // Occupancy voxels and offsets.
      gputil::BufferArg<float>(*occupancy_layer_cache.buffer()),
      gputil::BufferArg<uint64_t>(imp->voxel_upload_info[buf_idx][occ_uidx].offsets_buffer),
      // Mean voxels and offsets.
      gputil::BufferArg<VoxelMean>(*mean_layer_cache.buffer()),
      gputil::BufferArg<uint64_t>(imp->voxel_upload_info[buf_idx][mean_uidx].offsets_buffer),
      // Covariance voxels and offsets.
      gputil::BufferArg<CovarianceVoxel>(*cov_voxel_layer_cache.buffer()),
      gputil::BufferArg<uint64_t>(imp->voxel_upload_info[buf_idx][cov_uidx].offsets_buffer),
      // Hit/miss voxels and offsets.
      gputil::BufferArg<HitMissCount>(hit_miss_layer_cache ? hit_miss_layer_cache->buffer() : nullptr),
      gputil::BufferArg<uint64_t>(
        hit_miss_layer_cache ? &imp->voxel_upload_info[buf_idx][hit_miss_uidx].offsets_buffer : nullptr),
      // Traversal voxels and offsets.
      gputil::BufferArg<float>(traversal_layer_cache ? traversal_layer_cache->buffer() : nullptr),
      gputil::BufferArg<uint64_t>(
        traversal_layer_cache ? &imp->voxel_upload_info[buf_idx][traversal_uidx].offsets_buffer : nullptr),
      // Touch times voxels and offsets.
      gputil::BufferArg<uint32_t>(nullptr), gputil::BufferArg<uint64_t>(nullptr),  // No touch times for miss update
      // Incident normal voxels and offsets.
      gputil::BufferArg<uint32_t>(nullptr), gputil::BufferArg<uint64_t>(nullptr),  // No incidents for miss update
                                                                                   // Region keys and region count
      gputil::BufferArg<gputil::int3>(imp->region_key_buffers[buf_idx]), region_count,
      // Ray start/end keys
      gputil::BufferArg<GpuKey>(imp->key_buffers[buf_idx]),
      // Ray start end points, local to end voxel and ray count
      gputil::BufferArg<gputil::float3>(imp->ray_buffers[buf_idx]), ray_count,
      // Input touch times buffer
      gputil::BufferArg<uint32_t>(nullptr),  // No touch times for miss update
      // Region dimensions, map resolution, ray adjustment (miss), sample adjustment (hit)
      region_dim_gpu, float(map->resolution), map->miss_value, map->hit_value,
      // Occupied threshold, min occupancy, max occupancy.
      map->occupancy_threshold_value, map->min_voxel_value, map->max_voxel_value,
      // Update flags, NDT adaptation rate, NDT model sensor noise
      region_update_flags | modify_flags, imp->ndt_map.adaptationRate(), imp->ndt_map.sensorNoise());
    wait.clear();
    wait.add(first_kernel_event);
  }

  if (!(region_update_flags & (kRfExcludeSample | kRfEndPointAsFree)) && sample_count > 0)
  {
    // NDT can only have one CovarianceHitNdtTm batch in flight because it does not support contension. Ensure previous
    // one has completed and it waits on the kernel above to finish too.
    waitOnPreviousOperation(1 - buf_idx);

    global_size = gputil::Dim3(sample_count);
    local_size = gputil::Dim3(std::min<size_t>(imp->cov_hit_kernel.optimalWorkGroupSize(), sample_count));
    imp->cov_hit_kernel(
      global_size, local_size, wait, imp->region_update_events[buf_idx], &gpu_cache.gpuQueue(),
      // Kernel args begin:
      // Occupancy voxels and offsets.
      gputil::BufferArg<float>(*occupancy_layer_cache.buffer()),
      gputil::BufferArg<uint64_t>(imp->voxel_upload_info[buf_idx][occ_uidx].offsets_buffer),
      // Mean voxels and offsets.
      gputil::BufferArg<VoxelMean>(*mean_layer_cache.buffer()),
      gputil::BufferArg<uint64_t>(imp->voxel_upload_info[buf_idx][mean_uidx].offsets_buffer),
      // Covariance voxels and offsets.
      gputil::BufferArg<CovarianceVoxel>(*cov_voxel_layer_cache.buffer()),
      gputil::BufferArg<uint64_t>(imp->voxel_upload_info[buf_idx][cov_uidx].offsets_buffer),
      // Intensity voxels and offsets.
      gputil::BufferArg<IntensityMeanCov>(intensity_layer_cache ? intensity_layer_cache->buffer() : nullptr),
      gputil::BufferArg<uint64_t>(
        intensity_layer_cache ? &imp->voxel_upload_info[buf_idx][intense_uidx].offsets_buffer : nullptr),
      // Hit/miss voxels and offsets.
      gputil::BufferArg<HitMissCount>(hit_miss_layer_cache ? hit_miss_layer_cache->buffer() : nullptr),
      gputil::BufferArg<uint64_t>(
        hit_miss_layer_cache ? &imp->voxel_upload_info[buf_idx][hit_miss_uidx].offsets_buffer : nullptr),
      // Traversal voxels and offsets.
      gputil::BufferArg<float>(traversal_layer_cache ? traversal_layer_cache->buffer() : nullptr),
      gputil::BufferArg<uint64_t>(
        traversal_layer_cache ? &imp->voxel_upload_info[buf_idx][traversal_uidx].offsets_buffer : nullptr),
      // Touch times voxels and offsets.
      gputil::BufferArg<uint32_t>(touch_times_layer_cache ? touch_times_layer_cache->buffer() : nullptr),
      gputil::BufferArg<uint64_t>(
        touch_times_layer_cache ? &imp_->voxel_upload_info[buf_idx][touch_time_uidx].offsets_buffer : nullptr),
      // Incident normal voxels and offsets.
      gputil::BufferArg<uint32_t>(incidents_layer_cache ? incidents_layer_cache->buffer() : nullptr),
      gputil::BufferArg<uint64_t>(
        incidents_layer_cache ? &imp_->voxel_upload_info[buf_idx][incident_normal_uidx].offsets_buffer : nullptr),
      // Region keys and region count
      gputil::BufferArg<gputil::int3>(imp->region_key_buffers[buf_idx]), region_count,
      // Ray start/end keys
      gputil::BufferArg<GpuKey>(imp->key_buffers[buf_idx]),
      // Ray start end points, local to end voxel and sample count
      gputil::BufferArg<gputil::float3>(imp->ray_buffers[buf_idx]), sample_count,
      // Input touch times buffer
      gputil::BufferArg<uint32_t>((region_update_flags & kRfInternalTimestamps) ? &imp_->timestamps_buffers[buf_idx] :
                                                                                  nullptr),
      // Input intensities buffer
      gputil::BufferArg<float>(intensity_layer_cache ? &imp->intensities_buffers[buf_idx] : nullptr),
      // Region dimensions, map resolution, sample adjustment (hit), occupancy threshold, occupancy max value
      region_dim_gpu, float(map->resolution), map->hit_value, map->occupancy_threshold_value, map->max_voxel_value,
      // Intensity covariance initialisation, NDT sample threshold, NDT adaptation rate
      imp->ndt_map.initialIntensityCovariance(), imp->ndt_map.ndtSampleThreshold(), imp->ndt_map.adaptationRate(),
      // NDT model sensor noise, NDt reinitialisation covariance threshold
      imp->ndt_map.sensorNoise(), imp->ndt_map.reinitialiseCovarianceThreshold(),
      // NDT reinitialisation sample count
      imp->ndt_map.reinitialiseCovariancePointCount());
  }
  else
  {
    imp->region_update_events[buf_idx] = first_kernel_event;
  }

  // Mark the end of the batch and a start of a new batch.
  // FIXME(KS): tech debt here. This should be managed at a higher level by the thing which calls finaliseBatch(), but
  // that means we need to know what caches have been touched here.
  if (touched_caches[0])
  {
    imp_->batch_marker = touched_caches[0]->beginBatch();
    for (size_t i = 1; i < touched_caches.size() && touched_caches[i]; ++i)
    {
      touched_caches[i]->beginBatch(imp_->batch_marker);
    }
  }
}
}  // namespace ohm


================================================
FILE: ohmgpu/GpuNdtMap.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUNDTMAP_H
#define GPUNDTMAP_H

#include "OhmGpuConfig.h"

#include "GpuMap.h"

#include <ohm/NdtMode.h>

#include <array>

namespace gputil
{
struct Dim3;
class Event;
class EventList;
}  // namespace gputil

namespace ohm
{
class NdtMap;
class GpuLayerCache;
struct GpuNdtMapDetail;

/// A GPU implementation of the @c NdtMap algorithm.
///
/// For details of what the NDT algorithm is, see @c NdtMap. The comments here focus on how this algorithm is applied
/// in GPU.
///
/// The NDT algorithm requires voxels to have @c VoxelMean and @c CovarianceVoxel data in addition to occupancy
/// values. While the mean can still be reasonably updated with contension in GPU using atomic operations because of
/// it's relatively independent values, the covariance has 6 floating point values. These 6 floats are highly
/// dependent and must be calculated and updated together. This could be done in a lock free fashion using a double
/// buffered approach, but this would double the per voxel data size.
///
/// Instead we do not support contension when calculating and updating covariance values. Covariance is updated in
/// a second pass in @c integrateRays() . The first pass only integrates the voxels the rays pass through, skipping
/// the sample voxels. This pass has the NDT miss logic applied, where we adjust the probability based on the current
/// covariance value of each cell.
///
/// The second pass of @c integrateRays() processes only the sample voxels. We run one GPU thread per sample, however,
/// there may be multiple threads for each voxel as multiple samples are likely to fall in the same voxel. So we have
/// multiple threads calculating the same result for the same voxel. We only allow the first - lowest global id -
/// thread to write the result.
///
/// This is not the most efficient process with redundant calculations made by multiple threads, however, it ensures
/// that:
///
/// 1. We avoid GPU contention on the covariance
/// 2. We keep all the voxel data in GPU memory. Doing the work in CPU requires repeatedly synching GPU and CPU.
/// 3. We do not need to do additional CPU work such as sorting the samples by sample voxel.
///
/// The base @c GpuMap supports having two batches in flight. It supports a second batch being prepared while the
/// first is allowed to continue. The NDT changes do not support this and there is a sync point before executing
/// the NDT sample update.
///
/// The NDT changes are applied by overriding @c finaliseBatch()
class ohmgpu_API GpuNdtMap : public GpuMap
{
public:
  /// Create a @c GpuNdtMap around the given @p map representation.
  /// @param map The map to wrap.
  /// @param borrowed_map True to borrow the map, @c false for this object to take ownership.
  /// @param expected_element_count The expected point count for calls to @c integrateRays(). Used as a hint.
  /// @param gpu_mem_size Optionally specify the target GPU cache memory to allocate.
  /// @param ndt_mode Specified which NDT mode to use. Using @c kNone is invalid resulting in undefined behaviour.
  explicit GpuNdtMap(OccupancyMap *map, bool borrowed_map = true, unsigned expected_element_count = 2048u,
                     size_t gpu_mem_size = 0u, NdtMode ndt_mode = NdtMode::kOccupancy);

  /// @overload
  inline GpuNdtMap(OccupancyMap *map, bool borrowed_map, NdtMode mode)
    : GpuNdtMap(map, borrowed_map, 2048u, 0u, mode)
  {}

  /// @overload
  inline GpuNdtMap(OccupancyMap *map, bool borrowed_map, unsigned expected_element_count, NdtMode mode)
    : GpuNdtMap(map, borrowed_map, expected_element_count, 0u, mode)
  {}

  /// Destructor
  ~GpuNdtMap() override;

  /// Set the range sensor noise estimate. For example, the range noise for a lidar sensor.
  ///
  /// @param noise_range The sensor noise range. Must be greater than zero.
  void setSensorNoise(float noise_range);

  /// Read the range sensor noise estimate.
  float sensorNoise() const;

  /// Access the CPU based @c NdtMap wrapper. This should be used with great care as changes to the voxels via
  /// this interface may not be correctly reflected in GPU.
  /// @return The CPU based @c NdtMap
  NdtMap &ndtMap();

  /// Access the CPU based @c NdtMap wrapper.
  /// @return The CPU based @c NdtMap
  const NdtMap &ndtMap() const;

protected:
  /// Helper to access the internal pimpl cast to the correct type.
  GpuNdtMapDetail *detail();
  /// Helper to access the internal pimpl cast to the correct type.
  const GpuNdtMapDetail *detail() const;

  /// Cache the NDT kernel as well.
  /// @param with_voxel_mean True to cache the program which supports voxel mean positioning (@ref voxelmean). Redundant
  ///   for NDT as it is already required.
  /// @param with_traversal True to cache the program which supports the traversal layer.
  /// @param force Force release and program caching even if already correct. Must be used on initialisation.
  void cacheGpuProgram(bool with_voxel_mean, bool with_traversal, bool force) override;

  void finaliseBatch(unsigned region_update_flags) override;

  /// Typdef for tracking which @c GpuLayerCache objects have been touched.
  using TouchedCacheSet = std::array<GpuLayerCache *, 8>;
  /// Invoke the NDT kernels.
  /// @param region_update_flags Flags controlling ray integration behaviour. See @c RayFlag.
  /// @param buf_idx Which buffer set in @c GpuMapDetail to update [0, 1]
  /// @param wait Set of GPU events to wait for before executing the first kernel. Expected to hold events related to
  ///   buffer uploads.
  /// @param used_caches Modified to hold pointers to the set of @c GpuLayerCache objects which have been used/touched
  ///   by the kernel invocations. Expected to be null terminated or full.
  void invokeNdt(unsigned region_update_flags, int buf_idx, gputil::EventList &wait, TouchedCacheSet &used_caches);

  void releaseGpuProgram() override;
};
}  // namespace ohm

#endif  // GPUNDTMAP_H


================================================
FILE: ohmgpu/GpuTransformSamples.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "GpuTransformSamples.h"

#include "private/GpuTransformSamplesDetail.h"

#include "OhmGpu.h"

#include "private/GpuProgramRef.h"

#include <gputil/gpuEvent.h>
#include <gputil/gpuEventList.h>
#include <gputil/gpuKernel.h>
#include <gputil/gpuPinnedBuffer.h>
#include <gputil/gpuPlatform.h>
#include <gputil/gpuProgram.h>

#include <glm/ext.hpp>

#include <algorithm>
#include <mutex>

#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
#include "TransformSamplesResource.h"
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL

#if GPUTIL_TYPE == GPUTIL_CUDA
GPUTIL_CUDA_DECLARE_KERNEL(transformTimestampedPoints);
#endif  // GPUTIL_TYPE == GPUTIL_CUDA

namespace ohm
{
namespace
{
#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref("TransformSamples", GpuProgramRef::kSourceString, TransformSamplesCode,  // NOLINT
                            TransformSamplesCode_length);
#else   // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref("TransformSamples", GpuProgramRef::kSourceFile, "TransformSamples.cl");
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL

inline bool goodSample(const glm::dvec3 &sample, double max_range)
{
  if (glm::any(glm::isnan(sample)))
  {
    return false;
  }

  if (glm::dot(sample, sample) > max_range)
  {
    return false;
  }

  return true;
}
}  // namespace


GpuTransformSamples::GpuTransformSamples(gputil::Device &gpu)
  : imp_(new GpuTransformSamplesDetail)
{
  imp_->gpu = gpu;
  imp_->transform_positions_buffer = gputil::Buffer(gpu, sizeof(gputil::float3) * 8, gputil::kBfReadHost);
  imp_->transform_rotations_buffer = gputil::Buffer(gpu, sizeof(gputil::float4) * 8, gputil::kBfReadHost);
  imp_->transform_times_buffer = gputil::Buffer(gpu, sizeof(float) * 8, gputil::kBfReadHost);
  if (g_program_ref.addReference(gpu))
  {
    imp_->kernel = GPUTIL_MAKE_KERNEL(g_program_ref.program(), transformTimestampedPoints);
    imp_->kernel.calculateOptimalWorkGroupSize();
  }
}


GpuTransformSamples::GpuTransformSamples(GpuTransformSamples &&other) noexcept
  : imp_(other.imp_)
{
  other.imp_ = nullptr;
}


GpuTransformSamples::~GpuTransformSamples()
{
  if (imp_ && imp_->kernel.isValid())
  {
    imp_->kernel = gputil::Kernel();
    g_program_ref.releaseReference();
  }
  delete imp_;
}


unsigned GpuTransformSamples::transform(const double *transform_times, const glm::dvec3 *transform_translations,
                                        const glm::dquat *transform_rotations, unsigned transform_count,
                                        const double *sample_times, const glm::dvec3 *local_samples,
                                        unsigned point_count, gputil::Queue &gpu_queue, gputil::Buffer &output_buffer,
                                        gputil::Event &completion_event, double max_range)
{
  if (point_count == 0 || transform_count == 0)
  {
    return 0u;
  }

  // Wait on outstanding operations to complete.
  gputil::Event::wait(imp_->upload_events.data(), GpuTransformSamplesDetail::kUploadEventCount);

  // Reserve GPU memory for the rays. We reserve space for the provides samples paired with transformed origin point.
  // This results in an interleaved spacing with:
  //    reserved_ray_origin
  //    local_sample_point
  //    reserved_ray_origin
  //    local_sample_point
  //    reserved_ray_origin
  //    local_sample_point
  //    ...
  //
  // To preserve memory, we use the reserve_ray_origin.x value to store the associated timestamps for the samples.
  // To address lack of double precision support, we'll make the times relative to the first transform timestamp.
  output_buffer.resize(sizeof(gputil::float3) * 2 * point_count);
  gputil::PinnedBuffer ray_buffer(output_buffer, gputil::kPinWrite);
  glm::vec3 sample_time;
  glm::vec3 sample;

  unsigned upload_count = 0u;
  const double base_time = transform_times[0];
  const auto max_time = float(transform_times[transform_count - 1] - base_time);
  for (unsigned i = 0; i < point_count; ++i)
  {
    sample_time = glm::vec3(float(sample_times[i] - base_time));
    sample = glm::vec3(local_samples[i]);
    if (!goodSample(sample, max_range))
    {
      continue;
    }
    ray_buffer.write(glm::value_ptr(sample_time), sizeof(sample_time), (upload_count + 0) * sizeof(gputil::float3));
    ray_buffer.write(glm::value_ptr(sample), sizeof(sample), (upload_count + 1) * sizeof(gputil::float3));
    upload_count += 2;
  }

  if (upload_count == 0)
  {
    return 0u;
  }

  // Asynchronous unpin. Kernels will wait on the associated event.
  ray_buffer.unpin(&gpu_queue, nullptr, &imp_->upload_events[0]);

  // Upload transforms.
  // FIXME: resolve device
  imp_->transform_positions_buffer.resize(sizeof(gputil::float3) * transform_count);
  imp_->transform_rotations_buffer.resize(sizeof(gputil::float4) * transform_count);
  imp_->transform_times_buffer.resize(sizeof(float) * transform_count);
  gputil::PinnedBuffer positions_buffer(imp_->transform_positions_buffer, gputil::kPinWrite);
  gputil::PinnedBuffer rotations_buffer(imp_->transform_rotations_buffer, gputil::kPinWrite);
  gputil::PinnedBuffer times_buffer(imp_->transform_times_buffer, gputil::kPinWrite);

  gputil::float3 position{};
  gputil::float4 rotation{};
  float single_precision_timestamp;
  for (unsigned i = 0; i < transform_count; ++i)
  {
    position.x = float(transform_translations[i].x);
    position.y = float(transform_translations[i].y);
    position.z = float(transform_translations[i].z);
    // Looks like GLM (quaternion) memory layout has changed from w last to w first. We need to explicitly map into
    // gputil::float4 and upload that to rectify.
    rotation.x = float(transform_rotations[i].x);
    rotation.y = float(transform_rotations[i].y);
    rotation.z = float(transform_rotations[i].z);
    rotation.w = float(transform_rotations[i].w);
    positions_buffer.write(&position, sizeof(position), i * sizeof(gputil::float3));
    rotations_buffer.write(&rotation, sizeof(rotation), i * sizeof(gputil::float4));
    single_precision_timestamp = std::max(0.0f, std::min(float(transform_times[i] - base_time), max_time));
    times_buffer.write(&single_precision_timestamp, sizeof(single_precision_timestamp),
                       i * sizeof(single_precision_timestamp));
  }

  // Asynchronous unpin.
  positions_buffer.unpin(&gpu_queue, nullptr, &imp_->upload_events[1]);
  rotations_buffer.unpin(&gpu_queue, nullptr, &imp_->upload_events[2]);
  times_buffer.unpin(&gpu_queue, nullptr, &imp_->upload_events[3]);

  // Set a max global size.
  const unsigned sample_count = upload_count / 2;
  const unsigned max_global_threads = 8 * 1024u;
  const unsigned min_batch_size = 8;
  const unsigned batch_size = std::max((sample_count + max_global_threads - 1) / max_global_threads, min_batch_size);

  gputil::Dim3 global_size((sample_count + batch_size - 1) / batch_size);
  gputil::Dim3 local_size(std::min(imp_->kernel.optimalWorkGroupSize(), global_size.x));
  gputil::EventList wait(imp_->upload_events.data(), GpuTransformSamplesDetail::kUploadEventCount);
  imp_->kernel(global_size, local_size, wait, completion_event, &gpu_queue,
               gputil::BufferArg<gputil::float3 *>(output_buffer), sample_count,
               gputil::BufferArg<float *>(imp_->transform_times_buffer),
               gputil::BufferArg<gputil::float3 *>(imp_->transform_positions_buffer),
               gputil::BufferArg<gputil::float4 *>(imp_->transform_rotations_buffer), transform_count, batch_size);

  gpu_queue.flush();

  return upload_count;
}
}  // namespace ohm


================================================
FILE: ohmgpu/GpuTransformSamples.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPU_TRANSFORM_SAMPLES_H
#define GPU_TRANSFORM_SAMPLES_H

#include "OhmGpuConfig.h"

#include <glm/fwd.hpp>

#include <limits>

namespace gputil
{
class Buffer;
class Device;
class Event;
class Queue;
}  // namespace gputil

namespace ohm
{
struct GpuTransformSamplesDetail;

/// A utility class for transforming local samples into a global frame using the Gpu.
///
/// See @c transform() for details.
class ohmgpu_API GpuTransformSamples
{
public:
  /// Create a GPU transform operation.
  /// @param gpu The GPU device context to operate in.
  explicit GpuTransformSamples(gputil::Device &gpu);
  /// Move constructor
  /// @param other Object to move.
  GpuTransformSamples(GpuTransformSamples &&other) noexcept;

  ~GpuTransformSamples();

  /// Transform rays in the indicated buffer from local space representation into a global frame.
  ///
  /// This process represents transforming data samples from a local sensor frame into an odometry frame.
  ///
  /// This local sample points specified in @p local_samples with corresponding @p sample_times and transforms them
  /// into a global frame using the GPU. The transforms are specified by @p transform_times, @p transform_translations
  /// and @p transform_rotations - scaling is not supported. These transforms specify the origin local to global
  /// transformations. The @p transform_times range must encompass all @p sample_times. The number of transforms
  /// should generally be small and each GPU thread executed a binary search for its appropriate sample.
  ///
  /// On successful execution, @p output_buffer is resize to hold @p point_count * 2 @c gputil::float3 entries.
  /// These entries are line segment pairs in the output space. The first item of each pair is the sensor origin
  /// for the sample, and the second is the transformed sample point. The @p completion_event is may be used to
  /// mark completion of the transformation as the GPU execution is asynchronous.
  ///
  /// Input samples are filtered for infinite and NaN points, which are not uploaded to the GPU. This means the
  /// items in @p output_buffer may not directly correspond to the @p local_samples. The number of result pairs is
  /// given by the return value.
  ///
  /// @param transform_times Array of timestamps for the local to global transforms.
  /// @param transform_translations Array of translation components of the local to global transforms.
  /// @param transform_rotations Array of quaternion rotations of the local to global transforms.
  /// @param transform_count number of entries in @p transform_times, @p transform_translations and
  ///   @p transform_rotations.
  /// @param sample_times Array of timestamps for @p local_samples.
  /// @param local_samples The sample points to transform in local sensor space.
  /// @param point_count Number of items in @p local_samples and @p sample_times.
  /// @param gpu_queue The queue in which to execute the GPU operations.
  /// @param output_buffer GPU buffer to calculate the results in. Will be resized if too small.
  /// @param completion_event Event which may be used to monitor GPU completion of the translations.
  /// @param max_range Maximum allowed distance length of a valid ray (sensor to sample distance).
  ///   Longer rays are rejected.
  /// @return The number of valid samples queued for translation on GPU.
  unsigned transform(const double *transform_times, const glm::dvec3 *transform_translations,
                     const glm::dquat *transform_rotations, unsigned transform_count, const double *sample_times,
                     const glm::dvec3 *local_samples, unsigned point_count, gputil::Queue &gpu_queue,
                     gputil::Buffer &output_buffer, gputil::Event &completion_event,
                     double max_range = std::numeric_limits<double>::infinity());

private:
  GpuTransformSamplesDetail *imp_;
};

}  // namespace ohm

#endif  // GPU_TRANSFORM_SAMPLES_H


================================================
FILE: ohmgpu/GpuTsdfMap.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "GpuTsdfMap.h"

#include "private/GpuTsdfMapDetail.h"

#include "GpuCache.h"
#include "GpuKey.h"
#include "GpuLayerCache.h"

#include "private/GpuProgramRef.h"

#include <ohm/DefaultLayer.h>
#include <ohm/MapChunk.h>
#include <ohm/MapLayout.h>
#include <ohm/OccupancyMap.h>
#include <ohm/VoxelTsdf.h>

#include <ohm/private/OccupancyMapDetail.h>

#include <gputil/gpuBuffer.h>
#include <gputil/gpuEvent.h>
#include <gputil/gpuKernel.h>
#include <gputil/gpuPinnedBuffer.h>
#include <gputil/gpuPlatform.h>
#include <gputil/gpuProgram.h>

#include <logutil/Logger.h>

#include <glm/ext.hpp>
#include <glm/gtx/norm.hpp>

#include <iostream>

#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
#include "TsdfUpdateResource.h"
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL

#if GPUTIL_TYPE == GPUTIL_CUDA
GPUTIL_CUDA_DECLARE_KERNEL(tsdfRayUpdate);
#endif  // GPUTIL_TYPE == GPUTIL_CUDA

namespace ohm
{
namespace
{
// Note on -DNDT=
// We need to define NDT to the correct value depending on the algorithm to use. See NdtModeDef.cl
// 0 => no NDT
// 1 => NDT occupancy map
// 2 => NDT traverability map
#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref_tsdf("TsdfUpdate", GpuProgramRef::kSourceString, TsdfUpdateCode,  // NOLINT
                                 TsdfUpdateCode_length, {});
#else   // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref_tsdf("TsdfUpdate", GpuProgramRef::kSourceFile, "TsdfUpdate.cl", 0u, {});
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
}  // namespace


GpuTsdfMap::GpuTsdfMap(OccupancyMap *map, bool borrowed_map, unsigned expected_element_count, size_t gpu_mem_size)
  : GpuMap(new GpuTsdfMapDetail(map, borrowed_map), expected_element_count, gpu_mem_size)
{
  // Ensure tsdf layer is present.
  if (map->layout().layerIndex(default_layer::tsdfLayerName()) == -1)
  {
    // Copy and update layout then update in the map.
    MapLayout layout = map->layout();
    addTsdf(layout);
    map->updateLayout(layout);
  }

  // Buffer management is a bit messy because it's a retrofit.
  for (int i = 0; i < 2; ++i)
  {
    // We only want TSDF data, so clear what the base class added here.
    imp_->voxel_upload_info[i].clear();
    detail()->tsdf_uidx = int(imp_->voxel_upload_info[i].size());  // Set twice to the same value, but that's ok.
    imp_->voxel_upload_info[i].emplace_back(VoxelUploadInfo(kGcIdTsdf, gpuCache()->gpu()));
  }

  // Only using TSDF.
  imp_->occupancy_uidx = -1;
  imp_->mean_uidx = -1;
  imp_->traversal_uidx = -1;
  imp_->touch_time_uidx = -1;
  imp_->incident_normal_uidx = -1;

  // Cache the correct GPU program.
  cacheGpuProgram(false, false, true);
}  // namespace ohm


GpuTsdfMap::~GpuTsdfMap()
{
  GpuTsdfMap::releaseGpuProgram();
}


void GpuTsdfMap::setTsdfOptions(const TsdfOptions &options)
{
  GpuTsdfMapDetail *imp = detail();
  imp->tsdf_options = options;
  if (imp->map)
  {
    updateMapInfo(imp->map->mapInfo(), imp->tsdf_options);
  }
}


const TsdfOptions &GpuTsdfMap::tsdfOptions() const
{
  const GpuTsdfMapDetail *imp = detail();
  return imp->tsdf_options;
}


void GpuTsdfMap::setMaxWeight(float max_weight)
{
  GpuTsdfMapDetail *imp = detail();
  imp->tsdf_options.max_weight = max_weight;
  if (imp->map)
  {
    updateMapInfo(imp->map->mapInfo(), imp->tsdf_options);
  }
}


float GpuTsdfMap::maxWeight() const
{
  const GpuTsdfMapDetail *imp = detail();
  return imp->tsdf_options.max_weight;
}


void GpuTsdfMap::setDefaultTruncationDistance(float default_truncation_distance)
{
  GpuTsdfMapDetail *imp = detail();
  imp->tsdf_options.default_truncation_distance = default_truncation_distance;
  if (imp->map)
  {
    updateMapInfo(imp->map->mapInfo(), imp->tsdf_options);
  }
}


float GpuTsdfMap::defaultTruncationDistance() const
{
  const GpuTsdfMapDetail *imp = detail();
  return imp->tsdf_options.default_truncation_distance;
}


void GpuTsdfMap::setDropoffEpsilon(float dropoff_epsilon)
{
  GpuTsdfMapDetail *imp = detail();
  imp->tsdf_options.dropoff_epsilon = dropoff_epsilon;
  if (imp->map)
  {
    updateMapInfo(imp->map->mapInfo(), imp->tsdf_options);
  }
}


float GpuTsdfMap::dropoffEpsilon() const
{
  const GpuTsdfMapDetail *imp = detail();
  return imp->tsdf_options.dropoff_epsilon;
}


void GpuTsdfMap::setSparsityCompensationFactor(float sparsity_compensation_factor)
{
  GpuTsdfMapDetail *imp = detail();
  imp->tsdf_options.sparsity_compensation_factor = sparsity_compensation_factor;
  if (imp->map)
  {
    updateMapInfo(imp->map->mapInfo(), imp->tsdf_options);
  }
}


float GpuTsdfMap::sparsityCompensationFactor() const
{
  const GpuTsdfMapDetail *imp = detail();
  return imp->tsdf_options.sparsity_compensation_factor;
}


GpuTsdfMapDetail *GpuTsdfMap::detail()
{
  return static_cast<GpuTsdfMapDetail *>(imp_);
}


const GpuTsdfMapDetail *GpuTsdfMap::detail() const
{
  return static_cast<const GpuTsdfMapDetail *>(imp_);
}


void GpuTsdfMap::cacheGpuProgram(bool /*with_voxel_mean*/, bool /*with_traversal_rate*/, bool force)
{
  if (imp_->program_ref)
  {
    if (!force)
    {
      return;
    }
  }

  releaseGpuProgram();

  GpuCache &gpu_cache = *gpuCache();
  GpuTsdfMapDetail *imp = detail();
  imp->gpu_ok = true;
  imp->cached_sub_voxel_program = true;

  imp->program_ref = &g_program_ref_tsdf;
  imp->gpu_ok = imp->program_ref->addReference(gpu_cache.gpu()) && imp_->gpu_ok;

  if (imp_->gpu_ok)
  {
    imp->update_kernel = GPUTIL_MAKE_KERNEL(imp->program_ref->program(), tsdfRayUpdate);
  }

  if (imp_->gpu_ok)
  {
    imp->update_kernel.calculateOptimalWorkGroupSize();
    imp->gpu_ok = imp->update_kernel.isValid();
  }
}


void GpuTsdfMap::finaliseBatch(unsigned region_update_flags)
{
  const int buf_idx = imp_->next_buffers_index;
  const OccupancyMapDetail *map = imp_->map->detail();
  GpuTsdfMapDetail *imp = detail();

  // We will track which caches we depend on to manage event waiting.
  GpuCache &gpu_cache = *this->gpuCache();
  GpuLayerCache &tsdf_layer_cache = *gpu_cache.layerCache(kGcIdTsdf);
  const int tsdf_uidx = imp->tsdf_uidx;
  // Enqueue update kernel.
  const gputil::int3 region_dim_gpu = { map->region_voxel_dimensions.x, map->region_voxel_dimensions.y,
                                        map->region_voxel_dimensions.z };

  if (!imp_->use_original_ray_buffers)
  {
    logutil::error("TSDF algorithm requires GPU samples_buffers, but the flag has been disabled. Aborting.\n");
    return;
  }

  const unsigned region_count = imp_->region_counts[buf_idx];
  const unsigned ray_count = imp_->ray_counts[buf_idx];
  gputil::Dim3 global_size(ray_count);
  gputil::Dim3 local_size(std::min<size_t>(imp_->update_kernel.optimalWorkGroupSize(), ray_count));

  // Note: we also wait on original_ray_upload_events here as the samples are required to calculate the TSDF distances.
  gputil::EventList wait({ imp_->key_upload_events[buf_idx], imp_->ray_upload_events[buf_idx],
                           imp_->original_ray_upload_events[buf_idx], imp_->region_key_upload_events[buf_idx],
                           imp_->voxel_upload_info[buf_idx][tsdf_uidx].offset_upload_event,
                           imp_->voxel_upload_info[buf_idx][tsdf_uidx].voxel_upload_event });

  // Supporting voxel mean and traversal are putting us at the limit of what we can support using this sort of
  // conditional invocation.
  imp_->update_kernel(global_size, local_size, wait, imp_->region_update_events[buf_idx], &gpu_cache.gpuQueue(),
                      // Kernel args begin:
                      // Tsdf voxels and offsets.
                      gputil::BufferArg<VoxelTsdf>(*tsdf_layer_cache.buffer()),
                      gputil::BufferArg<uint64_t>(imp_->voxel_upload_info[buf_idx][tsdf_uidx].offsets_buffer),
                      // Region keys and region count
                      gputil::BufferArg<gputil::int3>(imp_->region_key_buffers[buf_idx]), region_count,
                      // Ray start/end keys
                      gputil::BufferArg<GpuKey>(imp_->key_buffers[buf_idx]),
                      // Ray start end points, local to end voxel and ray count
                      gputil::BufferArg<gputil::float3>(imp_->ray_buffers[buf_idx]),
                      // Original ray sensor/samples buffer.
                      gputil::BufferArg<gputil::float3>(imp_->original_ray_buffers[buf_idx]), ray_count,
                      // Region dimensions, map resolution, TSDF settings.
                      region_dim_gpu, float(map->resolution), imp->tsdf_options.max_weight,
                      imp->tsdf_options.default_truncation_distance, imp->tsdf_options.dropoff_epsilon,
                      imp->tsdf_options.sparsity_compensation_factor, region_update_flags);

  // Update most recent chunk GPU event.
  tsdf_layer_cache.updateEvents(imp_->batch_marker, imp_->region_update_events[buf_idx]);

  // logutil::trace(imp_->region_counts[buf_idx], "regions\n");

  imp_->region_counts[buf_idx] = 0;
  // Start a new batch for the GPU layers.
  imp_->batch_marker = tsdf_layer_cache.beginBatch();
  imp_->next_buffers_index = (imp_->next_buffers_index + 1) % GpuMapDetail::kBuffersCount;
}
}  // namespace ohm


================================================
FILE: ohmgpu/GpuTsdfMap.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUTSDFMAP_H
#define GPUTSDFMAP_H

#include "OhmGpuConfig.h"

#include "GpuMap.h"

#include <ohm/VoxelTsdf.h>

#include <array>

namespace gputil
{
struct Dim3;
class Event;
class EventList;
}  // namespace gputil

namespace ohm
{
class NdtMap;
class GpuLayerCache;
struct GpuTsdfMapDetail;

/// A GPU implementation of a TSDF algorithm.
///
/// The TSDF algorithm requires voxels to have @c VoxelTsdf but does not require any occupancy data.
///
/// Note: @c GpuTsdfMap only respects the following @c RayFlag values:
/// - @c kRfReverseWalk
/// - @c kRfExcludeOrigin
class ohmgpu_API GpuTsdfMap : public GpuMap
{
public:
  /// Create a @c GpuTsdfMap around the given @p map representation.
  /// @param map The map to wrap.
  /// @param borrowed_map True to borrow the map, @c false for this object to take ownership.
  /// @param expected_element_count The expected point count for calls to @c integrateRays(). Used as a hint.
  /// @param gpu_mem_size Optionally specify the target GPU cache memory to allocate.
  /// @param ndt_mode Specified which NDT mode to use. Using @c kNone is invalid resulting in undefined behaviour.
  explicit GpuTsdfMap(OccupancyMap *map, bool borrowed_map = true, unsigned expected_element_count = 2048u,
                      size_t gpu_mem_size = 0u);

  /// @overload
  inline GpuTsdfMap(OccupancyMap *map, bool borrowed_map)
    : GpuTsdfMap(map, borrowed_map, 2048u, 0u)
  {}

  /// @overload
  inline GpuTsdfMap(OccupancyMap *map, bool borrowed_map, unsigned expected_element_count)
    : GpuTsdfMap(map, borrowed_map, expected_element_count, 0u)
  {}

  /// Destructor
  ~GpuTsdfMap() override;

  /// Set TSDF mapping options.
  /// @param options Options to set.
  void setTsdfOptions(const TsdfOptions &options);
  /// Get the TSDF mapping options.
  ///
  /// Note this overlaps with various  @c set functions.
  ///
  /// @return The current mapping options.
  const TsdfOptions &tsdfOptions() const;

  void setMaxWeight(float max_weight);
  float maxWeight() const;
  void setDefaultTruncationDistance(float default_truncation_distance);
  float defaultTruncationDistance() const;
  void setDropoffEpsilon(float dropoff_epsilon);
  float dropoffEpsilon() const;
  void setSparsityCompensationFactor(float sparsity_compensation_factor);
  float sparsityCompensationFactor() const;

protected:
  /// Helper to access the internal pimpl cast to the correct type.
  GpuTsdfMapDetail *detail();
  /// Helper to access the internal pimpl cast to the correct type.
  const GpuTsdfMapDetail *detail() const;

  /// Cache the NDT kernel as well.
  /// @param with_voxel_mean Ignored.
  /// @param with_traversal Ignored.
  /// @param force Force release and program caching even if already correct. Must be used on initialisation.
  void cacheGpuProgram(bool with_voxel_mean, bool with_traversal, bool force) override;

  void finaliseBatch(unsigned region_update_flags) override;
};
}  // namespace ohm

#endif  // GPUTSDFMAP_H


================================================
FILE: ohmgpu/LineKeysQueryGpu.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "LineKeysQueryGpu.h"

#include "private/LineKeysQueryDetailGpu.h"

#include "OhmGpu.h"

#include "private/GpuProgramRef.h"

#include <ohm/CalculateSegmentKeys.h>
#include <ohm/KeyList.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>

#include <gputil/gpuKernel.h>
#include <gputil/gpuPinnedBuffer.h>
#include <gputil/gpuPlatform.h>
#include <gputil/gpuProgram.h>

#include <logutil/Logger.h>

#include <glm/gtc/type_ptr.hpp>

#include <algorithm>
#include <chrono>
#include <cstring>
#include <thread>

#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
#include "LineKeysResource.h"
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL

#if GPUTIL_TYPE == GPUTIL_CUDA
GPUTIL_CUDA_DECLARE_KERNEL(calculateLines);
#endif  // GPUTIL_TYPE == GPUTIL_CUDA

namespace ohm
{
namespace
{
#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref("LineKeys", GpuProgramRef::kSourceString, LineKeysCode, LineKeysCode_length);  // NOLINT
#else   // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref("LineKeys", GpuProgramRef::kSourceFile, "LineKeys.cl");
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL

bool readGpuResults(LineKeysQueryDetailGpu &query);

unsigned nextPow2(unsigned v)
{
  // compute the next highest power of 2 of 32-bit v
  v--;
  v |= v >> 1u;
  v |= v >> 2u;
  v |= v >> 4u;
  v |= v >> 8u;
  v |= v >> 16u;  // NOLINT(readability-magic-numbers)
  v++;
  return v;
}

// TODO(KS): Verify alignment.
const size_t kGpuKeySize = sizeof(GpuKey);

bool initialiseGpu(LineKeysQueryDetailGpu &query)
{
  if (query.gpu_ok)
  {
    return true;
  }

  query.gpu = gpuDevice();

  unsigned queue_flags = 0;
  //#ifdef OHM_PROFILE
  //    queueFlags |= gputil::Queue::Profile;
  //#endif // OHM_PROFILE
  query.queue = query.gpu.createQueue(queue_flags);

  if (!g_program_ref.addReference(query.gpu))
  {
    return false;
  }

  query.line_keys_kernel = GPUTIL_MAKE_KERNEL(g_program_ref.program(), calculateLines);
  query.line_keys_kernel.calculateOptimalWorkGroupSize();

  if (!query.line_keys_kernel.isValid())
  {
    return false;
  }

  // Initialise buffer to dummy size. We'll resize as required.
  query.lines_out = gputil::Buffer(query.gpu, 1 * kGpuKeySize, gputil::kBfReadWriteHost);
  query.line_points = gputil::Buffer(query.gpu, 1 * sizeof(gputil::float3), gputil::kBfReadHost);
  query.gpu_ok = true;

  return true;
}


bool lineKeysQueryGpu(LineKeysQueryDetailGpu &query, bool /*async*/)
{
  // logutil::trace("Prime kernel\n");
  // Size the buffers.
  query.max_keys_per_line = 1;
  const double voxel_res = query.map->resolution();
  for (size_t i = 0; i < query.rays.size(); i += 2)
  {
    query.max_keys_per_line = std::max<unsigned>(
      unsigned(std::ceil((glm::length(query.rays[i + 1] - query.rays[i + 0]) / voxel_res) * std::pow(3.0, 0.5)) + 1u),
      query.max_keys_per_line);
  }
  // logutil::trace("Worst case key requirement: ", query.max_keys_per_line, '\n');
  // logutil::trace("Occupancy Key size ", sizeof(Key), " GPU Key size: ", kGpuKeySize, '\n');

  size_t required_size = query.rays.size() / 2 * query.max_keys_per_line * kGpuKeySize;
  if (query.lines_out.size() < required_size)
  {
    // logutil::trace("Required bytes ", required_size, " for ", query.rays.size() / 2u, " lines\n");
    query.lines_out.resize(required_size);
  }
  required_size = query.rays.size() * sizeof(gputil::float3);
  if (query.line_points.size() < required_size)
  {
    // logutil::trace("line_points size: ", required_size, '\n');
    query.line_points.resize(required_size);
  }

  // Upload rays. Need to write one at a time due to precision change and size differences.
  glm::vec3 point_f;
  gputil::PinnedBuffer line_points_mem(query.line_points, gputil::kPinWrite);
  for (size_t i = 0; i < query.rays.size(); ++i)
  {
    point_f = query.rays[i] - query.map->origin();
    line_points_mem.write(glm::value_ptr(point_f), sizeof(point_f), i * sizeof(gputil::float3));
  }
  line_points_mem.unpin();

  // Execute.
  const gputil::int3 region_dim = { query.map->regionVoxelDimensions().x, query.map->regionVoxelDimensions().y,
                                    query.map->regionVoxelDimensions().z };

  // logutil::trace("Invoke kernel\n");
  gputil::Dim3 global_size(query.rays.size() / 2);
  gputil::Dim3 local_size(std::min<size_t>(query.line_keys_kernel.optimalWorkGroupSize(), query.rays.size() / 2));

  // Ensure all memory transfers have completed.
  query.queue.insertBarrier();
  int err = query.line_keys_kernel(global_size, local_size, &query.queue,
                                   // Kernel args
                                   gputil::BufferArg<GpuKey>(query.lines_out), query.max_keys_per_line,
                                   gputil::BufferArg<gputil::float3>(query.line_points),
                                   gputil::uint(query.rays.size() / 2), region_dim, float(query.map->resolution()));

  if (err)
  {
    return false;
  }

  query.inflight = true;
  return true;
}

bool readGpuResults(LineKeysQueryDetailGpu &query)
{
  // logutil::trace("Reading results\n");
  // Download results.
  gputil::PinnedBuffer gpu_mem(query.lines_out, gputil::kPinRead);

  query.result_indices.resize(query.rays.size() / 2);
  query.result_counts.resize(query.rays.size() / 2);

  const size_t ray_count = query.rays.size() / 2;
  size_t read_offset_count = 0;
  int16_t result_count = 0;
  for (size_t i = 0; i < ray_count; ++i)
  {
    // Read result count.
    gpu_mem.read(&result_count, sizeof(result_count), read_offset_count * kGpuKeySize);

    query.result_indices[i] = query.intersected_voxels.size();
    query.result_counts[i] = result_count;

    // Read keys.
    if (result_count)
    {
#if 1
      static_assert(sizeof(GpuKey) == sizeof(Key), "CPU/GPU key size mismatch.");
      if (query.intersected_voxels.capacity() < query.intersected_voxels.size() + result_count)
      {
        const size_t reserve = nextPow2(unsigned(query.intersected_voxels.capacity() + result_count));
        // const size_t reserve = (query.intersected_voxels.capacity() + resultCount) * 2;
        // logutil::trace("will reserve ", reserve, '\n');
        query.intersected_voxels.reserve(reserve);
      }
      query.intersected_voxels.resize(query.intersected_voxels.size() + result_count);
      gpu_mem.read(query.intersected_voxels.data() + query.result_indices[i], kGpuKeySize * result_count,
                   (read_offset_count + 1) * kGpuKeySize);
#else   // #
      GpuKey gpuKey;
      Key key;
      for (size_t j = 0; j < resultCount; ++j)
      {
        gpuMem.read(&gpuKey, GpuKeySize, (readOffsetCount + 1 + j) * GpuKeySize);
        key.setRegionKey(glm::i16vec3(gpuKey.region[0], gpuKey.region[1], gpuKey.region[2]));
        key.setLocalKey(glm::u8vec3(gpuKey.voxel[0], gpuKey.voxel[1], gpuKey.voxel[2]));
        query.intersected_voxels.push_back(key);
      }
#endif  // #
    }

    read_offset_count += query.max_keys_per_line;
  }

  gpu_mem.unpin();

  query.number_of_results = query.rays.size() / 2;

  query.inflight = false;
  // logutil::trace("Results ready\n");
  return true;
}
}  // namespace

LineKeysQueryGpu::LineKeysQueryGpu(LineKeysQueryDetailGpu *detail)
  : LineKeysQuery(detail)
{}


LineKeysQueryGpu::LineKeysQueryGpu(ohm::OccupancyMap &map, unsigned query_flags)
  : LineKeysQueryGpu(query_flags)
{
  setMap(&map);
}


LineKeysQueryGpu::LineKeysQueryGpu(unsigned query_flags)
  : LineKeysQuery(new LineKeysQueryDetailGpu)
{
  setQueryFlags(query_flags);
  auto *d = static_cast<LineKeysQueryDetailGpu *>(imp_);
  initialiseGpu(*d);
}


LineKeysQueryGpu::~LineKeysQueryGpu()
{
  auto *d = static_cast<LineKeysQueryDetailGpu *>(imp_);
  if (d && d->gpu_ok)
  {
    if (d->line_keys_kernel.isValid())
    {
      d->line_keys_kernel = gputil::Kernel();
      g_program_ref.releaseReference();
    }
  }
  delete d;
  imp_ = nullptr;
}


bool LineKeysQueryGpu::onExecute()
{
  auto *d = static_cast<LineKeysQueryDetailGpu *>(imp_);

  if (!(d->query_flags & kQfGpuEvaluate))
  {
    return LineKeysQuery::onExecute();
  }

  initialiseGpu(*d);

  if (d->gpu_ok)
  {
    bool ok = lineKeysQueryGpu(*d, false);
    if (ok)
    {
      d->queue.finish();
      ok = readGpuResults(*d);
    }
    return ok;
  }

  return false;
}


bool LineKeysQueryGpu::onExecuteAsync()
{
  auto *d = static_cast<LineKeysQueryDetailGpu *>(imp_);

  if ((d->query_flags & kQfGpuEvaluate))
  {
    initialiseGpu(*d);

    if (d->gpu_ok)
    {
      bool ok = lineKeysQueryGpu(*d, true);
      if (ok)
      {
        // d->queue.flush();
        // ok = readGpuResults(*d);
      }
      return ok;
    }

    static bool once = false;
    if (!once)
    {
      once = true;
      logutil::warn("GPU unavailable for LineKeysQuery. Failing async call.\n");
    }
  }

  return false;
}


void LineKeysQueryGpu::onReset(bool /*hard_reset*/)
{
  auto *d = static_cast<LineKeysQueryDetailGpu *>(imp_);
  d->result_indices.clear();
  d->result_counts.clear();
}


bool LineKeysQueryGpu::onWaitAsync(unsigned timeout_ms)
{
  auto *d = static_cast<LineKeysQueryDetailGpu *>(imp_);
  const auto sleep_interval = std::chrono::milliseconds(0);
  const auto start_time = std::chrono::system_clock::now();
  auto timeout = std::chrono::milliseconds(timeout_ms);
  while (d->inflight)
  {
    std::this_thread::sleep_for(sleep_interval);
    if (timeout_ms != ~0u)
    {
      if (std::chrono::system_clock::now() - start_time >= timeout)
      {
        break;
      }
    }
  }

  return !d->inflight;
}


LineKeysQueryDetailGpu *LineKeysQueryGpu::imp()
{
  return static_cast<LineKeysQueryDetailGpu *>(imp_);
}


const LineKeysQueryDetailGpu *LineKeysQueryGpu::imp() const
{
  return static_cast<const LineKeysQueryDetailGpu *>(imp_);
}
}  // namespace ohm


================================================
FILE: ohmgpu/LineKeysQueryGpu.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_LINEKEYSQUERY_H
#define OHMGPU_LINEKEYSQUERY_H

#include "OhmGpuConfig.h"

#include <ohm/LineKeysQuery.h>

#include <glm/fwd.hpp>

namespace ohm
{
struct LineKeysQueryDetailGpu;

/// This query calculates the voxels intersecting a batch of rays.
///
/// The results are similar to those of @c OccupancyMap::calculateSegmentKeys() (identical when using CPU),
/// but supports batched and GPU based calculation. The GPU calculation is generally only marginally faster
/// than CPU, but GPU supports @c executeAsync(), which is not supported for CPU.
///
/// In practice, this query isn't very useful as the GPU performance gains are minimal.
///
/// General usage is:
/// - Initialise the query object setting the map and the GPU flag if required.
/// - Call @c setRays() to define the ray start/end point pairs.
/// - Call @c execute() or @c executeAsync() followed by @c wait() (GPU only).
/// - Process results (see below).
///
/// The @c numberOfResults() will match the number of rays (@c pointCount given to @c setRays()) and for
/// each of these results, there is an entry in @c resultIndices() and @c resultCounts(). The indices indicate the
/// offsets into @c intersectedVoxels() for each ray in the same order they appear in @c setRays(). The
/// counts identify how many voxels are present for the current line counting from the associated index.
/// There should always be at least one voxel per ray for the start/end voxel. More generally, the first voxel
/// is the ray start voxel and the last voxel is the ray end voxel.
class ohmgpu_API LineKeysQueryGpu : public LineKeysQuery
{
protected:
  /// Constructor used for inherited objects. This supports deriving @p LineKeysQueryDetail into
  /// more specialised forms.
  /// @param detail pimple style data structure. When null, a @c LineKeysQueryDetail is allocated by
  /// this method.
  explicit LineKeysQueryGpu(LineKeysQueryDetailGpu *detail);

public:
  /// Construct a new query using the given parameters.
  /// @param map The map to operate on. Only the voxel resolution and region sizes are used.
  /// @param query_flags Flags controlling the query behaviour. See @c QueryFlag and @c LineKeysQuery::Flag.
  explicit LineKeysQueryGpu(ohm::OccupancyMap &map, unsigned query_flags = 0u);

  /// Construct a new query using the given parameters.
  /// @param query_flags Flags controlling the query behaviour. See @c QueryFlag and @c LineKeysQuery::Flag.
  explicit LineKeysQueryGpu(unsigned query_flags = 0);

  /// Destructor.
  ~LineKeysQueryGpu() override;

protected:
  bool onExecute() override;
  bool onExecuteAsync() override;
  void onReset(bool hard_reset) override;
  bool onWaitAsync(unsigned timeout_ms) override;

  /// Internal pimpl data access.
  /// @return Pimpl data pointer.
  LineKeysQueryDetailGpu *imp();
  /// Internal pimpl data access.
  /// @return Pimpl data pointer.
  const LineKeysQueryDetailGpu *imp() const;
};
}  // namespace ohm


#endif  // OHMGPU_LINEKEYSQUERY_H


================================================
FILE: ohmgpu/LineQueryGpu.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "LineQueryGpu.h"

#include "ClearanceProcess.h"
#include "GpuMap.h"

#include "private/LineQueryDetailGpu.h"

#include <ohm/CalculateSegmentKeys.h>
#include <ohm/Key.h>
#include <ohm/OccupancyMap.h>
#include <ohm/QueryFlag.h>
#include <ohm/VoxelData.h>
#include <ohm/private/OccupancyMapDetail.h>
#include <ohm/private/OccupancyQueryAlg.h>
#include <ohm/private/VoxelAlgorithms.h>

#include "GpuLayerCache.h"

#include <3esservermacros.h>

#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>

#ifdef OHM_FEATURE_THREADS
#include <tbb/blocked_range.h>
#include <tbb/parallel_for.h>
#endif  // OHM_FEATURE_THREADS

#include <algorithm>
#include <functional>
#include <iostream>
#include <limits>

namespace ohm
{
LineQueryGpu::LineQueryGpu(LineQueryDetailGpu *detail)
  : LineQuery(detail)
{}


LineQueryGpu::LineQueryGpu()
  : LineQuery(new LineQueryDetailGpu)
{}


LineQueryGpu::LineQueryGpu(OccupancyMap &map, const glm::dvec3 &start_point, const glm::dvec3 &end_point,
                           float search_radius, unsigned query_flags)
  : LineQuery(new LineQueryDetailGpu)
{
  setMap(&map);
  setStartPoint(start_point);
  setEndPoint(end_point);
  setSearchRadius(search_radius);
  setQueryFlags(query_flags);
}


LineQueryGpu::~LineQueryGpu()
{
  LineQueryDetailGpu *d = imp();
  if (d)
  {
    delete d->clearance_calculator;
    delete d;
  }
  // Clear pointer for base class.
  imp_ = nullptr;
}


bool LineQueryGpu::onExecute()
{
  LineQueryDetailGpu *d = imp();

  if (!d->map)
  {
    return false;
  }

  if (!(d->query_flags & kQfGpuEvaluate))
  {
    return LineQuery::onExecute();
  }

  ClosestResult closest;

  ClearanceProcess::ensureClearanceLayer(*d->map);
  gpumap::enableGpu(*d->map);

  // GPU evaluation requested. Use the ClearanceProcess to do so.
  glm::dvec3 min_ext;
  glm::dvec3 max_ext;
  for (int i = 0; i < 3; ++i)
  {
    min_ext[i] = std::min(d->start_point[i], d->end_point[i]);
    max_ext[i] = std::max(d->start_point[i], d->end_point[i]);
  }
  unsigned clearance_flags = kQfGpuEvaluate;
  if (d->query_flags & kQfUnknownAsOccupied)
  {
    clearance_flags |= kQfUnknownAsOccupied;
  }
  if (!d->clearance_calculator)
  {
    d->clearance_calculator = new ClearanceProcess();
  }
  d->clearance_calculator->setSearchRadius(d->search_radius);
  d->clearance_calculator->setQueryFlags(clearance_flags);
  d->clearance_calculator->setAxisScaling(d->axis_scaling);
  // Force recalculation if not using cached values. Otherwise we'll only calculate dirty regions.
  const bool force = (d->query_flags & kQfNoCache);
  d->clearance_calculator->calculateForExtents(*d->map, min_ext, max_ext, force);
  // QF_NoCache behaviour differs slightly for GPU calculation. The GPU only pushes
  // into the voxel clearance layer so set usedCacheClearance to read that information.

  // Calculate the voxels the line intersects.
  calculateSegmentKeys(d->segment_keys, *d->map, d->start_point, d->end_point);

  // Populate results.
  d->intersected_voxels.resize(d->segment_keys.size());
  d->ranges.resize(d->segment_keys.size());

  Voxel<const float> clearance(d->map, d->map->layout().clearanceLayer());

  if (!d->segment_keys.empty())
  {
    float range;
    closest.index = 0;
    closest.range = -1;
    for (size_t i = 0; i < d->segment_keys.size(); ++i)
    {
      d->intersected_voxels[i] = d->segment_keys[i];
      clearance.setKey(d->segment_keys[i]);

      if (clearance.isValid())
      {
        clearance.read(&range);
        // Range will be -1 from ClearanceProcess for unobstructed voxels (to the search radius).
        // Override the result with d->default_range, which defaults to -1 as well.
        if (range < 0)
        {
          range = d->default_range;
        }
      }
      else
      {
        range = d->default_range;
      }

      d->ranges[i] = range;
      if (i == 0 || range >= 0 && (range < closest.range || closest.range < 0))
      {
        closest.index = i;
        closest.range = range;
      }
    }
  }

  if ((d->query_flags & kQfNearestResult) && !d->intersected_voxels.empty())
  {
    d->intersected_voxels[0] = d->intersected_voxels[closest.index];
    d->intersected_voxels.resize(1);
    d->ranges[0] = d->ranges[closest.index];
    d->number_of_results = 1u;
    d->ranges.resize(1);
  }
  else
  {
    d->number_of_results = d->intersected_voxels.size();
  }

  return true;
}


bool LineQueryGpu::onExecuteAsync()
{
  return false;
}


void LineQueryGpu::onReset(bool /*hard_reset*/)
{
  // LineQueryDetailGpu *d = imp();
}


LineQueryDetailGpu *LineQueryGpu::imp()
{
  return static_cast<LineQueryDetailGpu *>(imp_);
}


const LineQueryDetailGpu *LineQueryGpu::imp() const
{
  return static_cast<const LineQueryDetailGpu *>(imp_);
}
}  // namespace ohm


================================================
FILE: ohmgpu/LineQueryGpu.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_LINEQUERY_H
#define OHMGPU_LINEQUERY_H

#include "OhmGpuConfig.h"

#include <ohm/LineQuery.h>

#include <glm/fwd.hpp>

namespace ohm
{
class GpuMap;
struct LineQueryDetailGpu;

/// A line segment intersection query for an @c OccupancyMap.
///
/// A line segment query determines the range of voxels for which intersect a given line segment and for each such
/// voxels, calculates the range to the nearest obstructed voxel. Obstructed voxels are defined as occupied voxels
/// and optionally unknown voxels when the query flag @c QF_UnknownAsOccupied is set.
///
/// The results are given in @c intersectedVoxels() and @c ranges(). The @c intersectedVoxels() identify the voxels
/// touched by the line segment, in order, from start to end. The corresponding @c ranges() yield the nearest
/// obstacle range for each voxel in @c intersectedVoxels(), or -1 if there is no voxel within the search range.
/// Setting the flag @c QF_NearestResult modifies the results to only report one voxel which has the shortest
/// range value (excluding unobstructed voxels). The result set may be empty in the case where @c QF_NearestResult is
/// set and all voxels along the line have no obstructions within the search range.
///
/// The CPU implementation is O(nmm) where n is the number of voxels in the line and m is defined by search radius
/// divided by the map resolution (i.e., the number of voxels required to reach the search radius). The GPU
/// implementation is closer to worst case O(m) although it incurs additional, initial overhead. The GPU
/// implementation supports caching to offset this overhead.
///
/// Using the GPU implementation, the @c LineQuery invokes the @c VoxelRanges query in order to cache the obstacle
/// ranges. On a soft @c reset(), the obstacle ranges are only recalculated for regions which have not yet
/// been calculated. A hard @c reset() should be invoked whenever the map changes to recalculate these ranges.
///
/// Note that a @c VoxelRanges query pointer may be given to the constructor to use an external @c VoxelRanges
/// query object. In this case this object does not take ownership of the external @c VoxelRanges query object.
///
/// @todo Add a flag which will early out of completing the query when the line enters a region which contains only
/// uncertain voxels (or does not exist). This is to help avoid creating those regions when using GPU queries
/// based on the assumption that we are not interested in continuing the query when it is wholly obstructed.
///
/// @bug Update the comment without reference to defunct VoxelRange, instead noting that ClearanceProcess is
/// required and precalculated clearance values may be assumed in GPU mode. searchRadius() is ingored in this
/// mode.
class ohmgpu_API LineQueryGpu : public LineQuery
{
public:
  /// Default flags to execute this query with.
  static const unsigned kDefaultFlags = kQfNoCache;

protected:
  /// Constructor used for inherited objects. This supports deriving @p LineQueryDetail into
  /// more specialised forms.
  /// @param detail pimple style data structure. When null, a @c LineQueryDetail is allocated by
  /// this method.
  explicit LineQueryGpu(LineQueryDetailGpu *detail);

public:
  LineQueryGpu();

  /// Construct a new query using the given parameters.
  /// @param map The map to perform the query on.
  /// @param start_point The global coordinate marking the start of the line segment.
  /// @param end_point The global coordinate marking the end of the line segment.
  /// @param search_radius Defines the "width" of the line. See @c searchRadius().
  /// @param query_flags Flags controlling the query behaviour. See @c QueryFlag and @c LineQuery::Flag.
  LineQueryGpu(OccupancyMap &map, const glm::dvec3 &start_point, const glm::dvec3 &end_point, float search_radius,
               unsigned query_flags = kDefaultFlags);

  /// Destructor.
  ~LineQueryGpu() override;

protected:
  bool onExecute() override;
  bool onExecuteAsync() override;
  void onReset(bool hard_reset) override;

  /// Internal pimpl data access.
  /// @return Pimpl data pointer.
  LineQueryDetailGpu *imp();
  /// Internal pimpl data access.
  /// @return Pimpl data pointer.
  const LineQueryDetailGpu *imp() const;
};
}  // namespace ohm

#endif  // OHMGPU_LINEQUERY_H


================================================
FILE: ohmgpu/OhmGpu.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmGpu.h"


#include <gputil/gpuDevice.h>

#if OHM_GPU == OHM_GPU_OPENCL
#include <gputil/cl/gpuDeviceDetail.h>
#endif  // OHM_GPU == OHM_GPU_OPENCL

#include <gputil/gpuProgram.h>

#include <logutil/Logger.h>

#include <array>
#include <cassert>
#include <iostream>
#include <mutex>
#include <sstream>

namespace
{
gputil::Device g_gpu_device;  // NOLINT(cert-err58-cpp)
std::mutex g_gpu_mutex;
std::string g_gpu_build_std_arg;
unsigned g_gpu_std_major = 0;
unsigned g_gpu_std_minor = 0;
bool g_gpu_initialised = false;
}  // namespace


namespace ohm
{
bool configureGpuFromArgsInternal(int argc, const char **argv, bool show_device)
{
  if (!g_gpu_initialised)
  {
    struct PreferredDevice
    {
      const char *device_name;
      unsigned type_flags = gputil::Device::kGpu;
    };

#if GPUTIL_TYPE == GPUTIL_OPENCL
    static const size_t preferred_device_count = 4;
#else                                                                                    // GPUTIL_TYPE == GPUTIL_OPENCL
    static const size_t preferred_device_count = 3;
#endif                                                                                   // GPUTIL_TYPE == GPUTIL_OPENCL
    static const std::array<PreferredDevice, preferred_device_count> preferred_device =  //
    {
//
#if GPUTIL_TYPE == GPUTIL_OPENCL
      PreferredDevice{ "Intel", gputil::Device::kGpu },          //
#endif                                                           // GPUTIL_TYPE == GPUTIL_OPENCL
      PreferredDevice{ nullptr, gputil::Device::kGpu },          //
      PreferredDevice{ nullptr, gputil::Device::kAccelerator },  //
      PreferredDevice{ nullptr, 0 },                             //
    };

    for (size_t i = 0; !g_gpu_initialised && i < preferred_device_count; ++i)
    {
      if (g_gpu_device.select(argc, argv, preferred_device[i].device_name, preferred_device[i].type_flags))
      {
        if (show_device)
        {
          logutil::info("Selected device ", g_gpu_device.description(), '\n');
        }

#if OHM_GPU == OHM_GPU_OPENCL
        // Is --clver on the command line?
        bool found_cl_ver = false;
        for (int i = 1; i < argc; ++i)
        {
          const char *cl_ver_arg = "--clver";
          const size_t cl_arg_len = strlen(cl_ver_arg);
          if (strncmp(argv[i], cl_ver_arg, cl_arg_len) == 0)
          {
            // Found --clver.
            // Read the version string.
            cl_uint ver_major;
            cl_uint ver_minor;
            std::string cl_ver = argv[i] + cl_arg_len + 1;
            found_cl_ver = clu::parseVersion(cl_ver.c_str(), &ver_major, &ver_minor);
            g_gpu_std_major = ver_major;
            g_gpu_std_minor = ver_minor;
          }
        }

        if (!found_cl_ver)
        {
          if (strcmp(OHM_OPENCL_STD, "max") == 0)
          {
            // Requesting the maximum available version.
            // Query the device.
            g_gpu_std_major = g_gpu_device.info().version.major;
            g_gpu_std_minor = g_gpu_device.info().version.minor;
          }
          else
          {
            cl_uint ver_major;
            cl_uint ver_minor;
            clu::parseVersion(OHM_OPENCL_STD, &ver_major, &ver_minor);
            g_gpu_std_major = ver_major;
            g_gpu_std_minor = ver_minor;
          }
        }

        // Validate OpenCL extensions for 2.x are available.
        std::ostringstream str;
        str << "-cl-std=CL" << g_gpu_std_major << "." << g_gpu_std_minor;
        g_gpu_build_std_arg = str.str();
        // Validate extensions.

        bool extensions_supported = true;
        if (g_gpu_std_major >= 2)
        {
          // Tokenise required OpenCL feature list
          const char *feature_str = OHM_OPENCL_2_FEATURES;
          do
          {
            const char *begin = feature_str;

            while (*feature_str != ';' && *feature_str)
            {
              ++feature_str;
            }

            if (*begin != '\0')
            {
              std::string feature(begin, feature_str);
              if (!g_gpu_device.supportsFeature(feature.c_str()))
              {
                std::cerr << "Missing OpenCL 2+ feature: " << feature << std::endl;
                extensions_supported = false;
              }
            }
          } while (*feature_str++ != '\0');
        }

        if (!extensions_supported)
        {
          std::cerr << "Fallback to OpenCL 1.2" << std::endl;
          g_gpu_build_std_arg = "-cl-std=CL1.2";
          g_gpu_std_major = 1;
          g_gpu_std_minor = 2;
        }
#endif  // OHM_GPU == OHM_GPU_OPENCL


        g_gpu_initialised = true;
      }
    }
  }

  if (!g_gpu_initialised && show_device)
  {
    std::cerr << "OHM GPU device selection failed" << std::endl;
  }

  return g_gpu_initialised;
}


int configureGpuInternal(unsigned accel, const char *device_name, bool show_device)
{
  int argc = 0;
  // Lint(KS): unusual usage. Best to leave as is.
  const char *argv[2] = { nullptr };  // NOLINT(modernize-avoid-c-arrays)

  std::string accel_str = "--accel=";
  if (accel == kGpuAccel)
  {
    accel_str += "gpu";
    argv[argc] = accel_str.c_str();
    ++argc;
  }
  else if (accel == kCpuAccel)
  {
    accel_str += "cpu";
    argv[argc] = accel_str.c_str();
    ++argc;
  }
  else if (accel == kAnyAccel)
  {
    accel_str += "any";
    argv[argc] = accel_str.c_str();
    ++argc;
  }

  if (device_name)
  {
    std::string device_str = "--device=";
    device_str += device_name;
    argv[argc] = device_str.c_str();
    ++argc;
  }
  assert(size_t(argc) <= sizeof(argv) / sizeof(argv[0]));

  return configureGpuFromArgsInternal(argc, static_cast<const char **>(argv), show_device);
}


int configureGpuFromArgs(int argc, const char **argv, bool show_device)
{
  int err = 0;
  std::unique_lock<std::mutex> lock(g_gpu_mutex);
  if (!configureGpuFromArgsInternal(argc, argv, show_device))
  {
    err = 1;
  }
  return err;
}


int configureGpu(unsigned accel, const char *device_name, bool show_device)
{
  int err = 0;
  std::unique_lock<std::mutex> lock(g_gpu_mutex);
  if (!configureGpuInternal(accel, device_name, show_device))
  {
    err = 1;
  }
  return err;
}


gputil::Device &gpuDevice()
{
  std::unique_lock<std::mutex> lock(g_gpu_mutex);
  if (!g_gpu_initialised)
  {
    configureGpuInternal(kGpuAccel, nullptr, true);
  }
  return g_gpu_device;
}


unsigned gpuArgsInfo(const char **args_info, int *arg_type, unsigned max_pairs)
{
  // clang-format off
    struct ArgInfo
    {
      const char *name;
      const char *desc;
      int type;
    };
#if OHM_GPU == OHM_GPU_OPENCL
    // Lint(KS): can change once std::make_array() is viable.
    const ArgInfo arg_pairs[] = // NOLINT(modernize-avoid-c-arrays)
    {
      { "accel", "Select the OpenCL accelerator type [any,cpu,gpu] (gpu).", 1 },
      { "clver", "Sets the OpenCL runtime version. Selected device must support target OpenCL version. Format via the regex /[1-9][0-9]*(.[1-9][0-9]*)?/.", 1 },
      { "device", "OpenCL device name must contain the given string (case insensitive).", 1 },
      { "gpu-debug", "Compile OpenCL GPU code for full debugging.", 0 },
      { "platform", "OpenCL platform name must contain the given string (case insensitive).", 1 },
      { "vendor", "OpenCL vendor name must contain the given string (case insensitive).", 1 },
    };
    const unsigned arg_pair_count = sizeof(arg_pairs) / sizeof(arg_pairs[0]);
#else  // OHM_GPU == OHM_GPU_OPENCL
    const std::array<ArgInfo, 1> arg_pairs = { ArgInfo{ "", "", 0 } };
    unsigned arg_pair_count = 0;
#endif // OHM_GPU == OHM_GPU_OPENCL
        // clang-format on

  if (args_info)
  {
    for (unsigned i = 0; i < arg_pair_count && i < max_pairs; ++i)
    {
      args_info[(i << 1u) + 0] = arg_pairs[i].name;
      args_info[(i << 1u) + 1] = arg_pairs[i].desc;
      if (arg_type)
      {
        arg_type[i] = arg_pairs[i].type;
      }
    }
  }

  return arg_pair_count;
}


const char *gpuBuildStdArg()
{
  return g_gpu_build_std_arg.c_str();
}


void setGpuBuildVersion(gputil::BuildArgs &build_args)
{
  // Resolve the requested from the configuration define.
  build_args.version_major = int(g_gpu_std_major);
  build_args.version_minor = int(g_gpu_std_minor);
}
}  // namespace ohm


================================================
FILE: ohmgpu/OhmGpu.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_H
#define OHMGPU_H

#include "OhmGpuConfig.h"

namespace gputil
{
struct BuildArgs;
class Device;
}  // namespace gputil

namespace ohm
{
/// GPU/accelerator selection types.
enum AccelType : unsigned
{
  kGpuAccel = (1u << 0u),
  kCpuAccel = (1u << 1u),
  kAnyAccel = kGpuAccel | kCpuAccel
};

/// Make a GPU/accelerator selection from command line arguments.
///
/// May be given the entire set of command line arguments, but it will only respect the following:
/// - --accel=[any,cpu,gpu] : choose accelerator type.
/// - --platform=<hint> : platform name hint. Partial, lower case match is enough.
/// - --device=<hint> : device name hint. Partial, lower case match is enough.
/// - --clver=<version> : Minimum OpenCL version string; e.g., "1.2", "2.0" "2".
/// - --gpu-debug : compile GPU code for debugging (Intel OpenCL)
///
/// @param argc Number of arguments in @p argv.
/// @param argv Command line arguments.
/// @param show_device Log selected device to standard output?
/// @return 0 on success.
int ohmgpu_API configureGpuFromArgs(int argc, const char **argv, bool show_device = true);

/// @overload
inline int ohmgpu_API configureGpuFromArgs(int argc, char **argv, bool show_device = true)
{
  return configureGpuFromArgs(argc, const_cast<const char **>(argv),  // NOLINT(cppcoreguidelines-pro-type-const-cast)
                              show_device);
}

/// @overload
inline int ohmgpu_API configureGpuFromArgs(int argc, const char *const *argv, bool show_device = true)
{
  return configureGpuFromArgs(argc, const_cast<const char **>(argv),  // NOLINT(cppcoreguidelines-pro-type-const-cast)
                              show_device);
}

/// Make a GPU/accelerator selection.
///
/// @param accel Accelerator types allowed.
/// @param device_name Device name hint. Same as "--device" in @c configureGpuFromArgs().
/// @param show_device Print the selected device to stdout.
/// @return 0 on success.
int ohmgpu_API configureGpu(unsigned accel = kGpuAccel, const char *device_name = nullptr, bool show_device = true);

/// Get the GPU device to use for GPU based code in this library.
/// @return A reference to the GPU device to use.
gputil::Device ohmgpu_API &gpuDevice();

/// Provides information about the available command line options which control GPU behaviour.
///
/// This populates @p argsInfo with an array of static string pointers arranges in pairs. The pairs
/// specify a valid GPU command line option, followed by its English description.
/// The number of pairs is given by the return value. Arguments are given without any command line prefix
/// (e.g. "--" or "/"), and only with one supporting form (long form).
///
/// The function may be called with @p argsInfo as @c nullptr in order to query the number of pairs.
///
/// Each argument type is identified in @c argType if given. Types are:
/// - 0 : switch (boolean)
/// - 1 : string
///
/// @param args_info Populated with pairs of command line arguments and their descriptions up to @c maxPairs items.
/// Null to query the
///     number of arguments.
/// @param max_pairs The number of pairs supported by the address at @c argsInfo.
/// @param arg_type Array identifying the type for each argument.
/// @return The total number of pairs available regardless of what has been written to @p argsInfo.
unsigned ohmgpu_API gpuArgsInfo(const char **args_info, int *arg_type, unsigned max_pairs);

/// GPU compilation string used to define the required GPU code standard.
///
/// Used to set "-cl-std=x.x" in OpenCL compilation. Validated required extended features.
///
/// This method will be deprecated once all kernels are migrated to use gputil::Program and gputil::Kernel.
///
/// @return An argument string which should be included when building GPU code.
const char ohmgpu_API *gpuBuildStdArg();

/// Set the GPU target versions in @p build_args to define the required GPU code standard.
///
/// Used to set "-cl-std=x.x" in OpenCL compilation. Validated required extended features.
///
/// @return An argument string which should be included when building GPU code.
void ohmgpu_API setGpuBuildVersion(gputil::BuildArgs &build_args);
}  // namespace ohm

#endif  // OHMGPU_H


================================================
FILE: ohmgpu/OhmGpuConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMGPUCONFIG_H
#define OHMGPUCONFIG_H

#include "OhmGpuExport.h"

#include <ohm/OhmConfig.h>

// clang-format off

#define OHM_GPU_NONE 0
#define OHM_GPU_OPENCL 1
#define OHM_GPU_CUDA 2

/// Target OpenCL standard. 'max' => maximum device version (min 1.2)
#define OHM_OPENCL_STD "@OHM_OPENCL_STD@"
/// OpenCL required features to enable OpenCL 2.0 code.
#define OHM_OPENCL_2_FEATURES "@OHM_OPENCL_2_FEATURES@"

// clang-format on

#endif  // OHMGPUCONFIG_H


================================================
FILE: ohmgpu/RayItem.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_RAYITEM_H
#define OHMGPU_RAYITEM_H

#include "OhmGpuConfig.h"

#include <ohm/Key.h>
#include <ohm/RayFilter.h>

#include <glm/vec3.hpp>

#include <cinttypes>

namespace ohm
{
/// This is an internal structure used in @c GpuMap for sorting algorithm to group rays before upload.
/// It is made available for use in unit tests to ensure the same order used in GPU can be used in CPU after rays have
/// been sorted.
struct RayItem
{
  /// Origin of the ray. Unless clipped, this is the sensor location.
  glm::dvec3 origin;
  /// End point of the sample ray. Unless clipped, this is the location of the sample detection.
  glm::dvec3 sample;
  /// The original origin/sensor point, before any filtering or clipping may have occurred.
  glm::dvec3 original_origin;
  /// The original sample point, before any filtering or clipping may have occurred.
  glm::dvec3 original_sample;
  /// Map @c Key corresponding to @p origin .
  Key origin_key;
  /// Map @c Key corresponding to @p sample .
  Key sample_key;
  /// Intensity value associated with the @p sample .
  float intensity;
  /// Quantised, relative timestamp.
  unsigned timestamp;
  /// @c RayFilterFlag values corresponding to any modification which have been made to @p origin and @p sample .
  unsigned filter_flags;

  /// Key comparison `a < b` to help with less than operator.
  static bool isLessKey(const Key &a, const Key &b);

  /// Sorting operator.
  bool operator<(const RayItem &other) const;
};

inline bool isLessKey(const Key &a, const Key &b)
{
  // Multiplier/shift value to move a region key axis such that each axis is in its own bit set.
  const int64_t region_stride = 0x10000u;
  // Shift and mask together the region key axes
  const int64_t region_index_a = static_cast<int64_t>(a.regionKey().x) +
                                 static_cast<int64_t>(region_stride * a.regionKey().y) +
                                 region_stride * region_stride * static_cast<int64_t>(a.regionKey().z);
  const int64_t region_index_b = static_cast<int64_t>(b.regionKey().x) +
                                 region_stride * static_cast<int64_t>(b.regionKey().y) +
                                 region_stride * region_stride * static_cast<int64_t>(b.regionKey().z);
  // Multiplier/shift value to move a local key axis such that each axis is in its own bit set.
  const uint32_t local_stride = 0x100u;
  // Shift and mask together the local key axes
  const uint32_t local_index_a = uint32_t(a.localKey().x) + local_stride * uint32_t(a.localKey().y) +
                                 local_stride * local_stride * uint32_t(a.localKey().z);
  const uint32_t local_index_b = uint32_t(b.localKey().x) + local_stride * uint32_t(b.localKey().y) +
                                 local_stride * local_stride * uint32_t(b.localKey().z);
  // Compare the results.
  // - By region index next
  // - Finally by local index.
  return (region_index_a < region_index_b) || (region_index_a == region_index_b && local_index_a < local_index_b);
}

inline bool RayItem::operator<(const RayItem &other) const
{
  // For the comparison, we merge the region key values into a single 64-bit value
  // and the same for the local index. Then we compare the resulting values.
  // The final ordering is irrelevant in terms of which is "less". The goal is to group
  // items with the same sample key.

  const RayItem &a = *this;
  const RayItem &b = other;
  const int clipped_a = !!(a.filter_flags & kRffClippedEnd);
  const int clipped_b = !!(b.filter_flags & kRffClippedEnd);

  // Compare the results. We sort such that:
  // - Items with unclipped end points come first.
  // - By sample key
  // - By origin key
  return clipped_a < clipped_b || (clipped_a == clipped_b && a.sample_key < b.sample_key) ||
         (clipped_a == clipped_b && a.sample_key == b.sample_key && a.origin_key < b.origin_key);
}

}  // namespace ohm

#endif  // OHMGPU_RAYITEM_H


================================================
FILE: ohmgpu/RaysQueryGpu.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RaysQueryGpu.h"

#include "private/RaysQueryDetailGpu.h"

#include "GpuKey.h"
#include "GpuLayerCache.h"

#include <ohm/private/OccupancyMapDetail.h>

namespace ohm
{
RaysQueryGpu::RaysQueryGpu(RaysQueryDetailGpu *detail)
  : RaysQuery(detail)
{}


RaysQueryGpu::RaysQueryGpu()
  : RaysQuery(new RaysQueryDetailGpu)
{
  RaysQueryDetailGpu *d = imp();
  d->gpu_interface = std::make_unique<RaysQueryMapWrapper>();
  d->query_flags |= kQfGpu;
}


RaysQueryGpu::~RaysQueryGpu()
{}


void RaysQueryGpu::onSetMap()
{
  RaysQuery::onSetMap();
  RaysQueryDetailGpu *d = imp();
  d->gpu_interface->setMap(d->map);
}


bool RaysQueryGpu::onExecute()
{
  RaysQueryDetailGpu *d = imp();

  if (!d->map)
  {
    return false;
  }

  if (!onExecuteAsync())
  {
    return false;
  }

  // Sync.
  sync();

  return true;
}


bool RaysQueryGpu::onExecuteAsync()
{
  RaysQueryDetailGpu *d = imp();

  if (!d->map || d->rays_in.empty())
  {
    return false;
  }

  if (!(d->query_flags & kQfGpuEvaluate))
  {
    return RaysQuery::onExecute();
  }

  // For GPU we use floating point precision.
  d->gpu_interface->setVolumeCoefficient(float(volumeCoefficient()));
  return d->gpu_interface->integrateRays(d->rays_in.data(), d->rays_in.size()) == d->rays_in.size();
}


void RaysQueryGpu::onReset(bool hard_reset)
{
  RaysQuery::onReset(hard_reset);
  // Need to wait on the GPU program.
  sync();
}

void RaysQueryGpu::sync()
{
  RaysQueryDetailGpu *d = imp();
  if (d->query_flags & kQfGpuEvaluate)
  {
    RaysQueryDetailGpu *d = imp();
    d->gpu_interface->syncVoxels();

    d->ranges.clear();
    d->unobserved_volumes_out.clear();
    d->terminal_states_out.clear();

    const auto number_of_results = d->gpu_interface->results().size();

    d->ranges.reserve(number_of_results);
    d->unobserved_volumes_out.reserve(number_of_results);
    d->terminal_states_out.reserve(number_of_results);

    // Copy to split output buffers.
    for (const auto &result : d->gpu_interface->results())
    {
      d->ranges.emplace_back(result.range);
      d->unobserved_volumes_out.emplace_back(result.unobserved_volume);
      d->terminal_states_out.emplace_back(OccupancyType(result.voxel_type));
    }
    d->number_of_results = number_of_results;
  }
}


RaysQueryDetailGpu *RaysQueryGpu::imp()
{
  return static_cast<RaysQueryDetailGpu *>(imp_);
}


const RaysQueryDetailGpu *RaysQueryGpu::imp() const
{
  return static_cast<const RaysQueryDetailGpu *>(imp_);
}
}  // namespace ohm


================================================
FILE: ohmgpu/RaysQueryGpu.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmGpuConfig.h"

#include "GpuMap.h"

#include <ohm/RaysQuery.h>

namespace ohm
{
struct RaysQueryDetailGpu;

/// A GPU implementation of the @c RaysQuery .
///
/// The GPU overheads mean that this will be slower than the CPU implementation for low counts. GPU begins to outperform
/// CPU when querying rays on the order of 2000 or more with significant benefits starting at 5000-10000 rays with ray
/// lengths approximately 100 times the voxel resolution. Doubling the ray length may require an order of magnitude
/// more sample rays to maintain GPU timing benefits.
///
/// The actual timing results will vary depending on GPU API, GPU architecture, voxel size, query ray lengths and map
/// environment.
class ohmgpu_API RaysQueryGpu : public RaysQuery
{
protected:
  /// Constructor used for inherited objects. This supports deriving @p RaysQueryDetail into
  /// more specialised forms.
  /// @param detail pimple style data structure. When null, a @c RaysQueryDetail is allocated by
  /// this method.
  explicit RaysQueryGpu(RaysQueryDetailGpu *detail);

public:
  /// Constructor. The map and rays must be set before using.
  RaysQueryGpu();

  /// Destructor.
  ~RaysQueryGpu() override;

protected:
  void onSetMap() override;
  bool onExecute() override;
  bool onExecuteAsync() override;
  void onReset(bool hard_reset) override;

  /// Synchronise GPU results
  void sync();

  /// Internal pimpl data access.
  /// @return Pimpl data pointer.
  RaysQueryDetailGpu *imp();
  /// Internal pimpl data access.
  /// @return Pimpl data pointer.
  const RaysQueryDetailGpu *imp() const;
};
}  // namespace ohm


================================================
FILE: ohmgpu/gpu/AdjustNdt.cl
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef ADJUSTNDT_CL
#define ADJUSTNDT_CL

#include "CovarianceVoxelCompute.h"

inline __device__ float calculateOccupancyAdjustment(const GpuKey *voxel_key, const GpuKey *end_key, bool is_end_voxel,
                                                     bool is_sample_voxel, float voxel_resolution,
                                                     LineWalkData *line_data)
{
  // Note: we always ignore voxels where is_sample_voxel or is_end_voxel is true. Samples are adjusted later while
  // a non-sample is_end_voxel is a split ray.

  const ulonglong vi_local = voxel_key->voxel[0] + voxel_key->voxel[1] * line_data->region_dimensions.x +
                             voxel_key->voxel[2] * line_data->region_dimensions.x * line_data->region_dimensions.y;
  ulonglong vi = (line_data->means_offsets[line_data->current_region_index] / sizeof(*line_data->means)) + vi_local;
  __global VoxelMean *mean_data = &line_data->means[vi];

  const uint voxel_mean_coord = gputilAtomicLoadU32(&mean_data->coord);
  const uint voxel_mean_count = gputilAtomicLoadU32(&mean_data->count);
  float3 voxel_mean = subVoxelToLocalCoord(voxel_mean_coord, voxel_resolution);
  // voxel_mean is currently relative to the voxel centre of the voxel_key voxel. We need to change it to be in the same
  // reference frame as the incoming rays, which is relative to the endKey voxel. For this we need to calculate the
  // additional displacement from the centre of endKey to the centre of voxel_key and add this displacement.

  // Calculate the number of voxel steps from endKey to the voxel_key
  const int3 voxel_diff = keyDiff(voxel_key, end_key, line_data->region_dimensions);
  // Scale by voxel resolution and add to voxel_mean
  voxel_mean.x += voxel_diff.x * voxel_resolution;
  voxel_mean.y += voxel_diff.y * voxel_resolution;
  voxel_mean.z += voxel_diff.z * voxel_resolution;

  vi = (line_data->cov_offsets[line_data->current_region_index] / sizeof(*line_data->cov_voxels)) + vi_local;
  CovarianceVoxel cov_voxel;
  // Manual copy of the NDT voxel: we had some issues with OpenCL assignment on structures.
  cov_voxel.trianglar_covariance[0] = line_data->cov_voxels[vi].trianglar_covariance[0];
  cov_voxel.trianglar_covariance[1] = line_data->cov_voxels[vi].trianglar_covariance[1];
  cov_voxel.trianglar_covariance[2] = line_data->cov_voxels[vi].trianglar_covariance[2];
  cov_voxel.trianglar_covariance[3] = line_data->cov_voxels[vi].trianglar_covariance[3];
  cov_voxel.trianglar_covariance[4] = line_data->cov_voxels[vi].trianglar_covariance[4];
  cov_voxel.trianglar_covariance[5] = line_data->cov_voxels[vi].trianglar_covariance[5];

  float adjustment = 0;
  const int min_sample_threshold = 4;  // Should be passed in.

  bool is_miss;
  const float3 voxel_maximum_likelihood = calculateMissNdt(
    &cov_voxel, &adjustment, &is_miss, line_data->sensor, line_data->sample, voxel_mean, voxel_mean_count, INFINITY,
    line_data->ray_adjustment, line_data->adaptation_rate, line_data->sensor_noise, min_sample_threshold);

  // We increment the miss if needed.
  if (line_data->hit_miss && is_miss)
  {
    __global atomic_uint *miss_count_ptr =
      (__global atomic_uint *)&line_data
        ->hit_miss[(line_data->hit_miss_offsets[line_data->current_region_index] / sizeof(*line_data->hit_miss)) +
                   vi_local]
        .miss_count;
    gputilAtomicAdd(miss_count_ptr, 1u);
  }

  // NDT should do sample update in a separate process in order to update the covariance, so we should not get here.
  return (is_end_voxel || (is_sample_voxel && !(line_data->region_update_flags & kRfEndPointAsFree))) ? 0 : adjustment;
}

#endif  // ADJUSTNDT_CL


================================================
FILE: ohmgpu/gpu/AdjustOccupancy.cl
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef ADJUSTOCCUPANCY_CL
#define ADJUSTOCCUPANCY_CL

inline __device__ float calculateOccupancyAdjustment(const GpuKey *voxel_key, const GpuKey *end_key, bool is_end_voxel,
                                                     bool is_sample_voxel, float voxel_resolution,
                                                     LineWalkData *line_data)
{
  // We need to handle broken ray segments. They will come through with is_end_voxel true, but is_sample_voxel false.
  // We need to make a zero adjustment in that case.
  const float adjustment = (!is_sample_voxel || line_data->region_update_flags & kRfEndPointAsFree) ?
                             line_data->ray_adjustment :
                             line_data->sample_adjustment;
  return (is_end_voxel && !is_sample_voxel) ? 0 : adjustment;
}

#endif  // ADJUSTOCCUPANCY_CL


================================================
FILE: ohmgpu/gpu/CovarianceHitNdt.cl
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "gpu_ext.h"  // Must be first

#include "CovarianceVoxelCompute.h"
#include "GpuKey.h"
#include "MapCoord.h"
#include "VoxelMeanCompute.h"

#include "Regions.cl"

#include "CovarianceHitNdt_h.cl"
#include "Traversal.cl"
#include "VoxelIncident.cl"

/// This kernel integrates the ray sample points only into the map and is executed one thread per sample.
///
/// This kernel works with the @c REGION_UPDATE_KERNEL compiled in NDT mode. That kernel adjusts only free space
/// skipping the sample voxels. This kernel is invoked afterwards in order to calculate the sample adjustments. This
/// is because the sample adjustment is unable to use atomic CAS semantics when updating the covariance. The six values
/// of the reduced covariance matrix - see @c CovarianceVoxel - cannot be updated atomically.
///
/// This performes the covariance update on @c CovarianceVoxel for each affected sample voxel. The kernel requires that
/// the @c line_keys and corresponding @c local_lines are grouped (or sorted) such that all samples affecting a
/// particular sample voxel appear in a contiguous range in the array - referred below as a "sample block".
///
/// We run one GPU thread per sample entry in @c line_keys and @c local_lines . Each thread starts by checking if it is
/// the first thread in a sample block. Only the first thread in the sample block continues execution with all other
/// threads returning. The remaining threads iterate over their sample block and perform the relevant updates to
/// @c CovarianceVoxel and @c VoxelMean before writing the value back to main memory.
///
/// While this results in many inactive threads, it has proven more efficient than the following other attempted
/// techniques:
/// - Performing NDT sample update on GPU resulted in too much memory synchronisation overhead between CPU/GPU
/// - Unordered GPU update with each thread iterating the entire sample range looking for relevant samples - only first
///   thread did the writing
///
/// @note Memory layout for voxel data - @p occupancy , @p means and @p cov_voxels - is the same as for the
/// @c REGION_UPDATE_KERNEL .
///
/// @param occupancy The GPU cached voxel occupancy block.
/// @param occupancy_region_mem_offsets_global Array of voxel region memory offsets into @p occupancy . Each element
///     corresponds to a key in occupancy_region_keys_global. The offsets are in bytes.
/// @param means The GPU cached @c VoxelMean data block.
/// @param means_region_mem_offsets_global Array of voxel region memory offsets into @p means . Each element
///     corresponds to a key in occupancy_region_keys_global. The offsets are in bytes.
/// @param cov_voxels The GPU cached @c CovarianceVoxel data block.
/// @param cov_region_mem_offsets_global Array of voxel region memory offsets into @p cov_voxels . Each element
///     corresponds to a key in occupancy_region_keys_global. The offsets are in bytes.
/// @param occupancy_region_keys_global Array of voxel region keys identifying regions available in GPU. There are
///     @c region_count elements in this array.
/// @param region_count Number of regions uploaded in GPU and addressable in @p occupancy_region_keys_global .
/// @param line_keys Array of origin/sample pairs converted into @c GpuKey references to integrate into the map.
///     Must be ordered such that all samples of the same value appear in a contiguous block.
/// @param local_lines Array array of origin/sample pairs which generated the @c line_keys. These are converted from the
///     original, double precision, map frame coordinates into a set of local frames. Each start/end point pair is
///     relative to the centre of the voxel containing the end point. This is to reduce floating point error in
///     double to single precision conversion and assist in voxel mean calculations which are in the same frame.
///     The original coordinates are not recoverable in this code.
/// @param line_count number of lines in @p line_keys and @p local_lines. These come in pairs, so the number of elements
///     in those arrays is double this value.
/// @param region_dimensions Specifies the size of any one region in voxels.
/// @param voxel_resolution Specifies the size of a voxel cube.
/// @param sample_adjustment Specifiest the value adjustment applied to voxels containing the sample point (line end
///     point). Should be > 0 to re-enforce as occupied.
/// @param occupied_threashold Voxel @p occupancy value at which point the voxel is considered occupied. Must be >= 0 .
///     Normally 0.
/// @param voxel_value_max Maximum clamping value for voxel adjustments.
/// @param sensor_noise Expected range sensor noise value in the same units as the line values (generally metres).
///     Used as part of the NDT model. Must be > 0
/// @param reinitialise_cov_threshold The occupancy threshold value at which the covariance matrix may be reinitialised.
///     See @c calculateHitWithCovariance()
/// @param reinitialise_cov_sample_count The point count required to allow @p reinitialise_cov_threshold to be
///     triggered. See @c calculateHitWithCovariance()
__kernel void covarianceHitNdt(
  __global atomic_float *occupancy, __global ulonglong *occupancy_region_mem_offsets_global,             //
  __global VoxelMean *means, __global ulonglong *means_region_mem_offsets_global,                        //
  __global CovarianceVoxel *cov_voxels, __global ulonglong *cov_region_mem_offsets_global,               //
  __global IntensityMeanCov *intensity_voxels, __global ulonglong *intensity_region_mem_offsets_global,  //
  __global HitMissCount *hit_miss_voxels, __global ulonglong *hit_miss_region_mem_offsets_global,        //
  __global atomic_float *traversal_voxels, __global ulonglong *traversal_region_mem_offsets_global,      //
  __global atomic_uint *touch_time_voxels, __global ulonglong *touch_times_region_mem_offsets_global,    //
  __global atomic_uint *incident_voxels, __global ulonglong *incidents_region_mem_offsets_global,        //
  __global int3 *occupancy_region_keys_global, uint region_count,                                        //
  __global GpuKey *line_keys, __global float3 *local_lines, uint line_count,                             //
  __global uint *touch_times, __global float *intensities,                                               //
  int3 region_dimensions, float voxel_resolution, float sample_adjustment, float occupied_threshold,
  float voxel_value_max, float initial_intensity_covariance, unsigned ndt_sample_threshold, float adaptation_rate,
  float sensor_noise, float reinitialise_cov_threshold, unsigned reinitialise_cov_sample_count)
{
  if (get_global_id(0) >= line_count)
  {
    return;
  }

  // Get the voxel for this thread. Remember, line_keys contains sensor/sample voxel pairs. Sample is second.
  GpuKey start_voxel;
  // BUG: Intel OpenCL 2.0 compiler does not effect an assignment of GpuKey. I've had to unrolled it in copyKey().
  copyKey(&start_voxel, &line_keys[get_global_id(0) * 2 + 1]);
  start_voxel.voxel[3] = 0;  // For now we can ignore clipped sample voxels. Will check during iteration below.

  // We assume a sorted set of input points, where sample points falling in the same voxel are grouped together.
  // We then only allow the first thread in each voxel group to do the update for that group.
  // We check this now.
  GpuKey target_voxel;
  // Initialise with the target voxel index to ensure it has a value.
  copyKey(&target_voxel, &start_voxel);
  // Now fetch the previous key if we can. Only can't for global thread 0.
  if (get_global_id(0) > 0)
  {
    // Note the -1 to get the previous sample voxel.
    // copyKey(&target_voxel, &line_keys[get_global_id(0) * 2 - 1]);
    copyKey(&target_voxel, &line_keys[(get_global_id(0) - 1) * 2 + 1]);
  }
  target_voxel.voxel[3] = 0;  // Again - don't care about clipping yet.

  if (equalKeys(&target_voxel, &start_voxel) && get_global_id(0) > 0)
  {
    // This is not the first thread for this voxel grouping. Abort. The first thread in the group will do the update.
    // While this results in many idle threads, it maximise throughput while minimising iteration.
    return;
  }

  const uint region_local_index = start_voxel.voxel[0] + start_voxel.voxel[1] * region_dimensions.x +
                                  start_voxel.voxel[2] * region_dimensions.x * region_dimensions.y;
  uint occupancy_index, mean_index, cov_index;
  uint intensity_index, hit_miss_index;

  // Resolve the read/write indices for the target voxel. We need indices into occupancy, means and cov_voxels.
  // Dummy arguments for regionsResolveRegion(). We will only perform one lookup for each data type.
  int3 dummy_region_key;
  uint region_index;

  regionsInitCurrent(&dummy_region_key, &region_index);
  if (!regionsResolveRegion(&start_voxel, &dummy_region_key, &region_index, occupancy_region_keys_global, region_count))
  {
    // Data not available in GPU memory.
    return;
  }

  occupancy_index = (uint)(region_local_index + occupancy_region_mem_offsets_global[region_index] / sizeof(*occupancy));
  mean_index = (uint)(region_local_index + means_region_mem_offsets_global[region_index] / sizeof(*means));
  cov_index = (uint)(region_local_index + cov_region_mem_offsets_global[region_index] / sizeof(*cov_voxels));
  intensity_index =
    (intensity_voxels) ?
      (uint)(region_local_index + intensity_region_mem_offsets_global[region_index] / sizeof(*intensity_voxels)) :
      0;
  hit_miss_index =
    (hit_miss_voxels) ?
      (uint)(region_local_index + hit_miss_region_mem_offsets_global[region_index] / sizeof(*hit_miss_voxels)) :
      0;
  uint traversal_index =
    (traversal_voxels) ?
      (uint)(region_local_index + traversal_region_mem_offsets_global[region_index] / sizeof(*traversal_voxels)) :
      0;
  float traversal = (traversal_voxels) ? gputilAtomicLoadF32(&traversal_voxels[traversal_index]) : 0.0f;
  uint touch_time_index =
    (touch_time_voxels) ?
      (uint)(region_local_index + touch_times_region_mem_offsets_global[region_index] / sizeof(*touch_time_voxels)) :
      0;
  uint touch_time = (touch_time_voxels) ? gputilAtomicLoadU32(&touch_time_voxels[touch_time_index]) : 0u;
  uint incident_index =
    (incident_voxels) ?
      (uint)(region_local_index + incidents_region_mem_offsets_global[region_index] / sizeof(*incident_voxels)) :
      0;
  uint incident = (incident_voxels) ? gputilAtomicLoadU32(&incident_voxels[incident_index]) : 0u;

  // Cache initial values.
  WorkItem work_item;
  work_item.occupancy = gputilAtomicLoadF32(&occupancy[occupancy_index]);
  work_item.mean = subVoxelToLocalCoord(gputilAtomicLoadU32(&means[mean_index].coord), voxel_resolution);
  work_item.sample_count = gputilAtomicLoadU32(&means[mean_index].count);

  // Manual copy of the NDT voxel: we had some issues with OpenCL assignment on structures.
  work_item.cov.trianglar_covariance[0] = cov_voxels[cov_index].trianglar_covariance[0];
  work_item.cov.trianglar_covariance[1] = cov_voxels[cov_index].trianglar_covariance[1];
  work_item.cov.trianglar_covariance[2] = cov_voxels[cov_index].trianglar_covariance[2];
  work_item.cov.trianglar_covariance[3] = cov_voxels[cov_index].trianglar_covariance[3];
  work_item.cov.trianglar_covariance[4] = cov_voxels[cov_index].trianglar_covariance[4];
  work_item.cov.trianglar_covariance[5] = cov_voxels[cov_index].trianglar_covariance[5];

  IntensityMeanCov intensity_cov;  // = intensity_voxels[intensity_index];
  HitMissCount hit_miss_count;     // = hit_miss_voxels[hit_miss_index];

  if (intensity_voxels)
  {
    intensity_cov.intensity_mean = intensity_voxels[intensity_index].intensity_mean;
    intensity_cov.intensity_cov = intensity_voxels[intensity_index].intensity_cov;
  }

  if (hit_miss_voxels)
  {
    hit_miss_count.hit_count = hit_miss_voxels[hit_miss_index].hit_count;
    hit_miss_count.miss_count = hit_miss_voxels[hit_miss_index].miss_count;
  }

  // Now update by iterating from the starting voxel until we change voxels or reach the end of the set.
  uint added = 0;
  for (uint i = get_global_id(0); i < line_count; ++i)
  {
    copyKey(&target_voxel, &line_keys[i * 2 + 1]);
    if (equalKeys(&target_voxel, &start_voxel))
    {
      // Now we consider rays which have been clipped. These will have Ignore voxels with voxel[3] != 0.
      // This indicates the end point is a truncated part of the ray and not a real sample.
      if (target_voxel.voxel[3] == 0)
      {
        if (hit_miss_voxels)
        {
          const bool reinitialise_permeability_with_covariance = true;  // TODO: make a parameter of map
          calculateHitMissUpdateOnHit(&work_item.cov, work_item.occupancy, &hit_miss_count, local_lines[i * 2],
                                      local_lines[i * 2 + 1], work_item.mean, work_item.sample_count, INFINITY,
                                      reinitialise_permeability_with_covariance, adaptation_rate, sensor_noise,
                                      reinitialise_cov_threshold, reinitialise_cov_sample_count, ndt_sample_threshold);
        }
        if (intensity_voxels)
        {
          calculateIntensityUpdateOnHit(&intensity_cov, work_item.occupancy, intensities[i],
                                        initial_intensity_covariance, work_item.sample_count,
                                        reinitialise_cov_threshold, reinitialise_cov_sample_count);
        }
        if (traversal_voxels)
        {
          traversal += calculateTraversal(local_lines[i * 2], local_lines[i * 2 + 1], voxel_resolution);
        }

        if (touch_times)
        {
          touch_time = max(touch_time, touch_times[i]);
        }

        if (incident_voxels)
        {
          // The sample count won't be increment for the current point yet. That's the behaviour we want for the
          // progressive average.
          incident =
            updateIncidentNormal(incident, local_lines[i * 2] - local_lines[i * 2 + 1], work_item.sample_count);
        }

        collateSample(&work_item, local_lines[i * 2], local_lines[i * 2 + 1], region_dimensions, voxel_resolution,
                      sample_adjustment, occupied_threshold, sensor_noise, reinitialise_cov_threshold,
                      reinitialise_cov_sample_count);
        ++added;
      }
    }
    else
    {
      // Change in voxel. Done collecting.
      break;
    }
  }

  // Cap occupancy to max.
  if (voxel_value_max > 0)
  {
    work_item.occupancy = min(work_item.occupancy, voxel_value_max);
  }

  // if (get_global_id(0) == 0)
  // {
  //   printf("added: %u\n", added);
  // }

  // Write results. We expect no contension at this point so we write results directly. No atomic operations.
  gputilAtomicStoreF32(&occupancy[occupancy_index], work_item.occupancy);
  gputilAtomicStoreU32(&means[mean_index].coord, subVoxelCoord(work_item.mean, voxel_resolution));
  gputilAtomicStoreU32(&means[mean_index].count, work_item.sample_count);
  cov_voxels[cov_index] = work_item.cov;
  if (intensity_voxels)
  {
    intensity_voxels[intensity_index] = intensity_cov;
  }
  if (hit_miss_voxels)
  {
    hit_miss_voxels[hit_miss_index] = hit_miss_count;
  }
  if (traversal_voxels)
  {
    gputilAtomicStoreF32(&traversal_voxels[traversal_index], traversal);
  }
  if (touch_time_voxels)
  {
    gputilAtomicStoreU32(&touch_time_voxels[touch_time_index], touch_time);
  }
  if (incident_voxels)
  {
    gputilAtomicStoreU32(&incident_voxels[incident_index], incident);
  }
}


================================================
FILE: ohmgpu/gpu/CovarianceHitNdt.cu
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include <gputil/cuda/cutil_importcl.h>
#include <gputil/gpu_ext.h>

#include "CovarianceHitNdt.cl"

GPUTIL_CUDA_DEFINE_KERNEL(covarianceHitNdt);


================================================
FILE: ohmgpu/gpu/CovarianceHitNdt_h.cl
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef OHMGPU_COVARIANCE_HIT_HDT_H_
#define OHMGPU_COVARIANCE_HIT_HDT_H_

#include "CovarianceVoxelCompute.h"
// #include "VoxelMeanCompute.h"

// #include "Regions.cl"

typedef struct WorkItem_t
{
  CovarianceVoxel cov;
  // Voxel mean relative to the voxel centre.
  float3 mean;
  uint sample_count;
  float occupancy;
} WorkItem;

// forward declarations

void __device__ collateSample(WorkItem *work_item, float3 sensor, float3 sample, int3 region_dimensions,
                              float voxel_resolution, float sample_adjustment, float occupied_threshold,
                              float sensor_noise, float reinitialise_cov_threshold,
                              unsigned reinitialise_cov_sample_count);


void __device__ collateSample(WorkItem *work_item, float3 sensor, float3 sample, int3 region_dimensions,
                              float voxel_resolution, float sample_adjustment, float occupied_threshold,
                              float sensor_noise, float reinitialise_cov_threshold,
                              unsigned reinitialise_cov_sample_count)
{
  // The sample is currently relative to the voxel centre of the end voxel. This is the same reference frame we need
  // to calculate the quantised mean, so no change is required.
  if (calculateHitWithCovariance(&work_item->cov, &work_item->occupancy, sample, work_item->mean,
                                 work_item->sample_count, sample_adjustment, INFINITY, voxel_resolution,
                                 reinitialise_cov_threshold, reinitialise_cov_sample_count))
  {
    // Covariance matrix has reset. Reset the point count to clear the mean value.
    work_item->sample_count = 0;
  }

  const float one_on_count_plus_one = 1.0f / (float)(work_item->sample_count + 1);
  work_item->mean = (work_item->sample_count * work_item->mean + sample) * one_on_count_plus_one;
  ++work_item->sample_count;
}

#endif  // OHMGPU_COVARIANCE_HIT_HDT_H_


================================================
FILE: ohmgpu/gpu/LineKeys.cl
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "gpu_ext.h"  // Must be first

#include "GpuKey.h"
#include "MapCoord.h"

#include "LineWalkMarkers.cl"

#define WALK_NAME        LineKeys
#define WALK_VISIT_VOXEL lineKeysVisitVoxel

__device__ bool lineKeysVisitVoxel(const GpuKey *voxel_key, const GpuKey *start_key, const GpuKey *end_key,
                                   int voxel_marker, float enter_range, float exit_range, void *user_data);

// Must be included after above defined
#include "LineWalk.cl"

typedef struct LineWalkData_t
{
  __global GpuKey *line_out;
  uint max_keys;
  uint key_count;
} LineWalkData;

__device__ void calculateLineKeys(__global GpuKey *line_out, uint max_keys, const GpuKey *start_key,
                                  const GpuKey *end_key, const float3 start_point, const float3 end_point,
                                  const int3 region_dim, float voxel_resolution);


__device__ bool lineKeysVisitVoxel(const GpuKey *voxel_key, const GpuKey *start_key, const GpuKey *end_key,
                                   int voxel_marker, float enter_range, float exit_range, void *user_data)
{
  LineWalkData *line_data = (LineWalkData *)user_data;
  // Dereferencing line_data->line_out[1 + line_data->key_count++] caused issues on Intel OpenCL. Defer the increment
  // and use pointer arithmetic as a workaround.
  copyKey(line_data->line_out + line_data->key_count + 1, voxel_key);
  // Increment both as a workaround.
  ++line_data->key_count;
  return line_data->key_count + 1 < line_data->max_keys;
}


__device__ void calculateLineKeys(__global GpuKey *line_out, uint max_keys, const GpuKey *start_key,
                                  const GpuKey *end_key, const float3 start_point, const float3 end_point,
                                  const int3 region_dim, float voxel_resolution)
{
  LineWalkData line_data;
  line_data.line_out = line_out;  // + 1;
  line_data.max_keys = max_keys;
  line_data.key_count = 0;

  walkVoxelsLineKeys(start_key, end_key, start_point, end_point, region_dim, voxel_resolution, kLineWalkFlagNone,
                     &line_data);

  // Write result count to the first entry.
  line_out[0].region[0] = line_out[0].region[1] = line_out[0].region[2] = line_data.key_count;
  // printf("%lu => %u keys\n", get_global_id(0), line_data.key_count);
}


__kernel void calculateLines(__global GpuKey *lines_out, uint max_keys_per_line,
                             const __global float3 *query_point_pairs, uint query_count, int3 region_dim,
                             float voxel_resolution)
{
  const bool valid_thread = (get_global_id(0) < query_count);

  if (!valid_thread)
  {
    return;
  }

  float3 start_point = (valid_thread) ? query_point_pairs[get_global_id(0) * 2 + 0] : make_float3(0, 0, 0);
  float3 end_point = (valid_thread) ? query_point_pairs[get_global_id(0) * 2 + 1] : make_float3(0, 0, 0);
  __global GpuKey *line_out = lines_out + (get_global_id(0) * max_keys_per_line);
  // We convert region_dim from an int3 to an array to allow indexed access.
  GpuKey start_key, end_key;

  coordToKey(&start_key, &start_point, &region_dim, voxel_resolution);
  coordToKey(&end_key, &end_point, &region_dim, voxel_resolution);

  // printf("Query Count: %u\nValid? %d\n", query_count, valid_thread ? 1 : 0);
  // printf("RD: %d %d %d   Res: %f\n", region_dim.x, region_dim.y, region_dim.z, voxel_resolution);
  // printf("From %f %f %f to %f %f %f\n", start_point.x, start_point.y, start_point.z, end_point.x, end_point.y,
  // end_point.z); printf("Keys: " KEY_F " to " KEY_F "\n", KEY_A(start_key), KEY_A(end_key));

  // Adjust start/end point to be relative to the centre of the start_key voxel.
  start_point -= voxelCentre(&start_key, &region_dim, voxel_resolution);
  end_point -= voxelCentre(&start_key, &region_dim, voxel_resolution);

  calculateLineKeys(line_out, max_keys_per_line, &start_key, &end_key, start_point, end_point, region_dim,
                    voxel_resolution);
}


================================================
FILE: ohmgpu/gpu/LineKeys.cu
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gputil/cuda/cutil_importcl.h>
#include <gputil/gpu_ext.h>

#include "LineKeys.cl"

GPUTIL_CUDA_DEFINE_KERNEL(calculateLines);


================================================
FILE: ohmgpu/gpu/LineWalk.cl
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef WALK_NAME
#error WALK_NAME must be used to define the voxel walking function.
#endif  // WALK_NAME

#ifndef WALK_VISIT_VOXEL
#error WALK_VISIT_VOXEL must be used to define the voxel visiting function
#endif  // WALK_VISIT_VOXEL

//------------------------------------------------------------------------------
/// Note on building this code.
/// This code may be used by multiple OpenCL kernels, but has to support the same for CUDA compilation. This is slightly
/// tricky because OpenCL has entirely separate compilation units, while CUDA does not. The preprocessor is used to
/// workaround this by aliasing the key function names. Before including, the following must be defined:
///
/// @code{.c}
/// #define WALK_NAME        MyWalkName
/// #define WALK_VISIT_VOXEL visitVoxelMyWalkName
/// @endcode
///
/// The first is the main function name and the second is the function to call for each voxel visited.
///
/// This function is called for each voxel traversed by the line walking function.
///
/// Note the @c WALK_VISIT_VOXEL() function is not implemented in this source file, and must be implemented by the
/// source including this file.
///
/// The @p walk_flags may be used to modify the algorithm behaviour and semantics as follows:
///
/// - @c kLineWalkFlagReverse reverse walk. The algorithm iterates from @p endVoxel to @p startVoxel instead.
/// - @c kLineWalkFlagForReportEndLast force reporting the @p endVoxel last in a reverse walk.
///
/// @param voxel_key The key for the voxel currently being traversed. This voxel is on the line.
/// @param walk_start_key The first voxel visited by the walk.
/// @param walk_end_key The last voxel to be visited by the walk.
/// @param voxel_marker Set to @p kLineWalkMarkerStart (1) if @p voxelKey is the @c start_key or
///   @c kLineWalkMarkerEnd (2) if it is the @c end_key . Zero otherwise.
/// @param enter_range How far from the origin has been traversed before entering @p voxelKey . Is adapted for
///   @c kLineWalkFlagReverse .
/// @param exit_range How far from the origin has been traversed when exiting @p voxelKey . Is adapted for
///   @c kLineWalkFlagReverse .
/// @param user_data A pointer to the user data given to @c walkVoxels()
/// @return True to continue traversing the line, false to abort traversal.
/// __device__ bool WALK_VISIT_VOXEL(const GpuKey *voxel_key, const GpuKey *walk_start_key, const GpuKey *walk_end_key,
///                                  int voxel_marker, float enter_range, float exit_range, void *user_data);
//------------------------------------------------------------------------------
#include "GpuKey.h"
#include "LineWalkMarkers.cl"

// Define the context based function names

#define WALK_BUILD_FUNC_NAME(A, B)  WALK_BUILD_FUNC_NAME_(A, B)
#define WALK_BUILD_FUNC_NAME_(A, B) A##B

#define walkVoxels     WALK_BUILD_FUNC_NAME(walkVoxels, WALK_NAME)
#define walkVisitVoxel WALK_BUILD_FUNC_NAME(walkVisit, WALK_NAME)

typedef struct WalkContext
{
  void *user_data;
  int region_dimensions[3];
  // Value used for deferring calling the sample voxel in a reverse trace.
  float first_exit_range;
} WalkContext;

/// Calculate the @p GpuKey for @p point local to the region's minimum extents corner.
/// @param[out] key The output key.
/// @param point The coordinate to calculate the @p key for in region local coordinates.
/// @paramregion_dimensions Defines the size of a region in voxels. Used to update the @p GpuKey.
/// @param voxel_resolution Size of a voxel from one face to another.
/// @return True if @c point lies in the region, false otherwise.
__device__ bool coordToKey(GpuKey *key, const float3 *point, const int3 *region_dimensions, float voxel_resolution);

/// Calculates the centre of the voxel defined by @p key (global space).
/// @param key The key marking the voxel of interest.
/// @param region_dimensions Defines the size of a region in voxels. Used to update the @p GpuKey.
/// @param voxel_resolution Size of a voxel from one face to another.
/// @return The centre of the voxel defined by @p key.
inline __device__ float3 voxelCentre(const GpuKey *key, const int3 *region_dimensions, float voxel_resolution);

/// Get the @p index component of @c float3 @p v
inline __device__ float getf3(const float3 *v, int index);
/// Get the @p index component of @c int3 @p v
inline __device__ int geti3(const int3 *v, int index);

/// Line walking function for use by kernels.
/// The algorithm walks the voxels from @p start_key to @p end_key. The line segment is defined relative to the centre
/// of the @p startkey voxel with line points @p start_point and @p end_point respectively.
///
/// @c WALK_VISIT_VOXEL() is invoked for each voxel traversed.
///
/// @param start_key The key for the voxel containing @p start_point.
/// @param end_key The key for the voxel containing @p end_point.
/// @param start_point The start point of the line segment to traverse, relative to the centre of the
///   start voxel (identified by start_key). That is the origin is the centre of the start_key voxel.
/// @param end_point The end point of the line segment to traverse, relative to the centre of the
///   start voxel (identified by start_key). That is the origin is the centre of the start_key voxel.
/// @param region_dimensions Defines the size of a region in voxels. Used to update the @p GpuKey.
/// @param voxel_resolution Size of a voxel from one face to another.
/// @param walk_flags Flags affecting the algorithm behaviour. See @c LineWalkFlag .
/// @param userData User pointer passed to @c walkLineVoxel().
__device__ void walkVoxels(const GpuKey *start_key, const GpuKey *end_key, const float3 start_point,
                           const float3 end_point, const int3 region_dimensions, float voxel_resolution, int walk_flags,
                           void *user_data);

// Psuedo header guard to prevent symbol duplication.
#ifndef LINE_WALK_CL
#define LINE_WALK_CL


inline __device__ bool coordToKey(GpuKey *key, const float3 *point, const int3 *region_dimensions,
                                  float voxel_resolution)
{
  // Quantise.
  key->region[0] = pointToRegionCoord(point->x, region_dimensions->x * voxel_resolution);
  key->region[1] = pointToRegionCoord(point->y, region_dimensions->y * voxel_resolution);
  key->region[2] = pointToRegionCoord(point->z, region_dimensions->z * voxel_resolution);

  // Localise.
  // Trying to minimise local variables for GPU.
  // The value we pass to pointToRegionVoxel is logically:
  //    point - regionCentre - regionHalfExtents
  // or
  //    point - regionMin
  // which equates to the point in region local coordinates.
  // printf("p.x(%f) = %f - (%f - %f)   : regionMin: %f\n",
  //        point->x - (regionCentreCoord(key->region[0], region_dimensions->x * voxel_resolution) - 0.5f *
  //        region_dimensions->x * voxel_resolution), point->x, regionCentreCoord(key->region[0], region_dimensions->x *
  //        voxel_resolution), 0.5f * region_dimensions->x * voxel_resolution, regionCentreCoord(key->region[0],
  //        region_dimensions->x * voxel_resolution) - 0.5f * region_dimensions->x * voxel_resolution);
  key->voxel[0] =
    pointToRegionVoxel(point->x - (regionCentreCoord(key->region[0], region_dimensions->x * voxel_resolution) -
                                   0.5f * region_dimensions->x * voxel_resolution),
                       voxel_resolution, region_dimensions->x * voxel_resolution);
  key->voxel[1] =
    pointToRegionVoxel(point->y - (regionCentreCoord(key->region[1], region_dimensions->y * voxel_resolution) -
                                   0.5f * region_dimensions->y * voxel_resolution),
                       voxel_resolution, region_dimensions->y * voxel_resolution);
  key->voxel[2] =
    pointToRegionVoxel(point->z - (regionCentreCoord(key->region[2], region_dimensions->z * voxel_resolution) -
                                   0.5f * region_dimensions->z * voxel_resolution),
                       voxel_resolution, region_dimensions->z * voxel_resolution);

  if (key->voxel[0] < region_dimensions->x && key->voxel[1] < region_dimensions->y &&
      key->voxel[2] < region_dimensions->z)
  {
    return true;
  }

// Out of range.
#if 0
  printf("%u Bad key: " KEY_F "\nfrom (%.16f,%.16f,%.16f)\n"
         "  quantisation: (%.16f,%.16f,%.16f)\n"
         "  region: (%.16f,%.16f,%.16f)\n",
         (uint)get_global_id(0), KEY_A(*key), point->x, point->y, point->z,
         point->x - (regionCentreCoord(key->region[0], region_dimensions->x * voxel_resolution) -
                     0.5f * region_dimensions->x * voxel_resolution),
         point->y - (regionCentreCoord(key->region[1], region_dimensions->y * voxel_resolution) -
                     0.5f * region_dimensions->y * voxel_resolution),
         point->z - (regionCentreCoord(key->region[2], region_dimensions->z * voxel_resolution) -
                     0.5f * region_dimensions->z * voxel_resolution),
         region_dimensions->x * voxel_resolution, region_dimensions->y * voxel_resolution,
         region_dimensions->z * voxel_resolution);
  printf("pointToRegionCoord(%.16f, %d * %.16f = %.16f)\n", point->y, region_dimensions->y, voxel_resolution,
         region_dimensions->y * voxel_resolution);
  printf("pointToRegionVoxel(%.16f - %.16f, ...)\n", point->y,
         regionCentreCoord(key->region[1], region_dimensions->y * voxel_resolution) -
           0.5f * (region_dimensions->y * voxel_resolution));
#endif  // #
  return false;
}


inline __device__ float3 voxelCentre(const GpuKey *key, const int3 *region_dimensions, float voxel_resolution)
{
  float3 voxel;

  // printf("voxelCentre(" KEY_F ", [%d %d %d], %f)\n", KEY_A(*key), region_dimensions->x, region_dimensions->y,
  // region_dimensions->z, voxel_resolution);

  // Calculation is:
  //  - region centre - region half extents => region min extents.
  //  - add voxel region local coordiate.
  // Using terse code to reduce local variable load.
  voxel.x = regionCentreCoord(key->region[0], region_dimensions->x * voxel_resolution) -
            0.5f * region_dimensions->x * voxel_resolution + key->voxel[0] * voxel_resolution + 0.5f * voxel_resolution;
  voxel.y = regionCentreCoord(key->region[1], region_dimensions->y * voxel_resolution) -
            0.5f * region_dimensions->y * voxel_resolution + key->voxel[1] * voxel_resolution + 0.5f * voxel_resolution;
  voxel.z = regionCentreCoord(key->region[2], region_dimensions->z * voxel_resolution) -
            0.5f * region_dimensions->z * voxel_resolution + key->voxel[2] * voxel_resolution + 0.5f * voxel_resolution;

  return voxel;
}

#endif  // LINE_WALK_CL

inline __device__ void walkKeyDiff(WalkContext *context, int *diff, const GpuKey *key_a, const GpuKey *key_b)
{
  const int3 diff_iv3 =
    keyDiff(key_a, key_b,
            make_int3(context->region_dimensions[0], context->region_dimensions[1], context->region_dimensions[2]));
  diff[0] = diff_iv3.x;
  diff[1] = diff_iv3.y;
  diff[2] = diff_iv3.z;
}

inline __device__ void walkStepKey(WalkContext *context, GpuKey *key, int axis, int step_dir)
{
  stepKeyAlongAxis(
    key, axis, step_dir,
    make_int3(context->region_dimensions[0], context->region_dimensions[1], context->region_dimensions[2]));
  // Do not propagate the clipping bit.
  key->voxel[3] = 0;
}

inline __device__ bool walkVisitVoxel(WalkContext *context, const GpuKey *voxel_key, const GpuKey *start_key,
                                      const GpuKey *end_key, int voxel_marker, float enter_range, float exit_range)
{
  return WALK_VISIT_VOXEL(voxel_key, start_key, end_key, voxel_marker, enter_range, exit_range, context->user_data);
  context->first_exit_range = (context->first_exit_range < 0) ? exit_range : context->first_exit_range;
  return true;
}

// Must be included after ther visit function is defined.
#include "LineWalkCompute.h"

__device__ void walkVoxels(const GpuKey *start_key, const GpuKey *end_key, const float3 start_point,
                           const float3 end_point, const int3 region_dimensions, float voxel_resolution, int walk_flags,
                           void *user_data)
{
  WalkContext context;
  context.region_dimensions[0] = region_dimensions.x;
  context.region_dimensions[1] = region_dimensions.y;
  context.region_dimensions[2] = region_dimensions.z;
  context.user_data = user_data;
  context.first_exit_range = -1.0f;

  const float3 voxel_resolution_3 = make_float3(voxel_resolution, voxel_resolution, voxel_resolution);
  const float length_epsilon = 1e-6f;
  if ((walk_flags & kLineWalkFlagReverse) == 0)
  {
    // We need to calculate the start voxel centre in the right coordinate space. All coordinates are relative to the
    // end voxel centre.
    // 1. Calculate the voxel step from endKey to startKey.
    // 2. Scale results by voxelResolution.
    const int3 voxel_diff = keyDiff(start_key, end_key, region_dimensions);
    const float3 start_voxel_centre =
      make_float3(voxel_diff.x * voxel_resolution, voxel_diff.y * voxel_resolution, voxel_diff.z * voxel_resolution);
    walkLineVoxels(&context, start_point, end_point, start_key, end_key, start_voxel_centre, voxel_resolution_3,
                   walk_flags, length_epsilon);
  }
  else
  {
    const bool defer_sample = (walk_flags & kLineWalkFlagForReportEndLast);
    walk_flags |= !!defer_sample * kExcludeStartVoxel;
    const float3 end_voxel_centre = make_float3(0, 0, 0);
    walkLineVoxels(&context, end_point, start_point, end_key, start_key, end_voxel_centre, voxel_resolution_3,
                   walk_flags, length_epsilon);
    if (defer_sample)
    {
      // Ensure valid exit range if not calculated.
      context.first_exit_range = max(context.first_exit_range, 0.0f);
      WALK_VISIT_VOXEL(end_key, start_key, end_key, kLineWalkMarkerStart, 0.0f, context.first_exit_range,
                       context.user_data);
    }
  }
}

#undef walkVisitVoxel
#undef walkVoxels
#undef WALK_NAME
#undef WALK_VISIT_VOXEL


================================================
FILE: ohmgpu/gpu/LineWalkMarkers.cl
================================================
// Copyright (c) 2022
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef LINE_WALK_MARKERS_CL
#define LINE_WALK_MARKERS_CL

/// Flags affecting line walking
typedef enum LineWalkFlag
{
  /// No options set: default behaviour.
  kLineWalkFlagNone = 0u,
  /// Skip reporting the voxel containing the start point if different from the end point?
  kExcludeStartVoxel = (1u << 0u),
  /// Skip reporting the voxel containing the end point if different from the start point.
  kExcludeEndVoxel = (1u << 1u),
  /// Option for reverse line walking. The line walks the segment in reverse.
  kLineWalkFlagReverse = (1u << 2u),
  /// Option to force reporting the end voxel last in a reverse walk. This occurs normally when @p kLineWalkFlagReverse
  /// is not set.
  kLineWalkFlagForReportEndLast = (1u << 3u)
} LineWalkFlag;

#endif  // LINE_WALK_MARKERS_CL


================================================
FILE: ohmgpu/gpu/RaysQuery.cl
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

//------------------------------------------------------------------------------
// Note on building this code.
// This code is intended to be used for updating regions both with and without
// sub voxel positioning. This is mostly the same, but the voxel type changes
// and we need an extract function call when updating a voxel with sub voxel
// positioning. To achive this, this code is intended to be includes into a
// compiled twice with the following defines changed on each compilation:
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// Debug switches for the compiling code to enable (or just uncomment here).
// Deliberately before incldues to configure those files.
//------------------------------------------------------------------------------

// Report regions we can't resolve via printf().
#define REPORT_MISSING_REGIONS

// Limit the number of cells we can traverse in the line traversal. This is a worst case limit.
//#define LIMIT_LINE_WALK_ITERATIONS
// Limit the number of times we try update a voxel value. Probably best to always have this enabled.
//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------
#include "gpu_ext.h"  // Must be first

#include "MapCoord.h"
#include "RayFlag.h"
#include "RaysQueryResult.h"

#include "LineWalkMarkers.cl"
#include "Regions.cl"

//------------------------------------------------------------------------------
// Declarations
//------------------------------------------------------------------------------

#define WALK_VISIT_VOXEL visitVoxelRayQuery
#define WALK_NAME        RayQuery

// User data for voxel visit callback.
typedef struct LineWalkData_t
{
  // Voxel occupancy memory. All regions use a shared buffer.
  __global atomic_float *occupancy;
  // Array of offsets for each regionKey into occupancy. These are byte offsets.
  __global ulonglong *occupancy_offsets;
  // Array of region keys for currently loaded regions.
  __global int3 *region_keys;
  // The region currently being traversed. Also used to reduce searching the region_keys and region_mem_offsets.
  int3 current_region;
  // Size of a region in voxels.
  int3 region_dimensions;
  /// Value threshold for occupied voxels.
  float occupied_threshold;
  // Number of regions in region_keys/region_mem_offsets.
  uint region_count;
  // Index of the @c current_region into region_keys and corresponding xxx_offsets arrays.
  uint current_region_index;
  /// Coefficient for calulating the unobserved_volume.
  float volume_coefficient;

  RaysQueryResult result;
} LineWalkData;

// Implement the voxel traversal function. We update the value of the voxel using atomic instructions.
__device__ bool visitVoxelRayQuery(const GpuKey *voxel_key, const GpuKey *start_key, const GpuKey *end_key,
                                   int voxel_marker, float enter_range, float exit_range, void *user_data)
{
  LineWalkData *line_data = (LineWalkData *)user_data;
  __global atomic_float *occupancy = line_data->occupancy;

  // Resolve memory offset for the region of interest.
  bool have_voxel_memory = regionsResolveRegion(voxel_key, &line_data->current_region, &line_data->current_region_index,
                                                line_data->region_keys, line_data->region_count);

#ifdef REPORT_MISSING_REGIONS
  if (!have_voxel_memory)
  {
    // We can fail to resolve regions along the in the line. This can occurs for several reasons:
    // - Floating point error differences between CPU and GPU line walking means that the GPU may walk into the edge
    //   of a region not hit when walking the regions on CPU.
    // - Regions may not be uploaded due to extents limiting on CPU.
    printf("%u region missing: " KEY_F "\n  Voxels: " KEY_F "->" KEY_F "\n", get_global_id(0), KEY_A(*voxel_key),
           KEY_A(*start_key), KEY_A(*end_key));
  }
#endif  // REPORT_MISSING_REGIONS

  // This voxel lies in the region. We will make a value adjustment.
  // Work out which voxel to modify.
  const ulonglong vi_local = voxel_key->voxel[0] + voxel_key->voxel[1] * line_data->region_dimensions.x +
                             voxel_key->voxel[2] * line_data->region_dimensions.x * line_data->region_dimensions.y;
  const ulonglong vi =
    (line_data->occupancy_offsets[line_data->current_region_index] / sizeof(*line_data->occupancy)) + vi_local;

  have_voxel_memory = have_voxel_memory && (voxel_key->voxel[0] < line_data->region_dimensions.x &&
                                            voxel_key->voxel[1] < line_data->region_dimensions.y &&
                                            voxel_key->voxel[2] < line_data->region_dimensions.z);

  __global atomic_float *occupancy_ptr = &occupancy[vi];

  // Lookup the voxel value.
  const float occupancy_value = (have_voxel_memory) ? gputilAtomicLoadF32(occupancy_ptr) : INFINITY;
  const int voxel_type = (occupancy_value == INFINITY) ?
                           RQ_OccUnobserved :
                           ((occupancy_value >= line_data->occupied_threshold) ? RQ_OccOccupied : RQ_OccFree);

  line_data->result.range = (voxel_type != RQ_OccOccupied) ? exit_range : line_data->result.range;
  line_data->result.unobserved_volume += (voxel_type == RQ_OccUnobserved) ?
                                           (line_data->volume_coefficient * (exit_range * exit_range * exit_range -
                                                                             enter_range * enter_range * enter_range)) :
                                           0.0f;
  line_data->result.voxel_type = voxel_type;

  // Stop traversal if occupied.
  return voxel_type != RQ_OccOccupied;
}

// Must be included after WALK_NAME and WALK_VISIT_VOXEL and the WALK_VISIT_VOXEL function is defined
#include "LineWalk.cl"

//------------------------------------------------------------------------------
// Kernel
//------------------------------------------------------------------------------

/// Integrate rays into voxel map regions.
///
/// Invoked one thread per ray (per @p line_keys pair).
///
/// Like keys are provided in start/end key pairs in @p line_keys where there are @p line_count pairs. Each thread
/// extracts it's start/end pair and performs a line walking algorithm from start to end key. The lines start end points
/// are also provided, relative to the centre of the starting voxel. These start/end coordinate pairs are in
/// @p local_lines. The coordinates for each line are local to the starting voxel centre in order to avoid precision
/// issues which may be introduced in converting from a common double precision frame on CPU into a single precision
/// frame in GPU (we do not support double precision GPU due to the limited driver support).
///
/// For each voxel key along the line, we resolve a voxel in @p occupancy by cross referencing in
/// @p occupancy_region_keys_global, @p occupancy_region_mem_offsets_global and @p region_count. Voxels are split into
/// regions in contiguous chunks in @p occupancy. The @c GpuKey::region for a voxel is matched in lookup @p
/// occupancy_region_keys_global and the index into @p occupancy_region_keys_global recorded. This index is used to
/// lookup @p occupancy_region_mem_offsets_global, which provides a byte offset (not elements) from @p occupancy at
/// which the voxel memory for this voxel begins. Each voxel region has a number of voxels equal to
/// <tt>region_dimensions.x * region_dimensions.y * region_dimensions.z</tt>.
///
/// Once voxel memory is resolved, the value of that voxel is updated by either adding @p ray_adjustment for all but
/// the last voxel in the line, or @p sample_adjustment for the last voxel (exception listed below). The value is
/// clamped to the range <tt>[voxel_value_min, voxel_value_max]</tt>. This adjustment is made in global memory using
/// atomic operations. Success is not guaranteed, but is highly probably. This contension has performance impacts, but
/// was found to be the best overall approach for performance.
///
/// The value adjustment of @p sample_adjustment is normally used for the last voxel in each line. This behaviour may
/// be changed per line, by setting the value of @p GpuKey::voxel[3] (normally unused) to 1. This indicates the line has
/// been clipped.
///
/// The line traversal is also affected by the region_update_flags, which are defined in the enum RayFlag.
/// These modify the line traversal as follows:
/// - kRfEndPointAsFree: use ray_adjustment for the last voxel rather than sample_adjustment.
/// - kRfStopOnFirstOccupied: terminate line walking after touching the first occupied voxel found.
/// - kRfExcludeSample: ignore the voxel containing the sample (the last voxel).
/// - kRfExcludeUnobserved: do not adjust voxels which are (initially) unobserved
/// - kRfExcludeFree: do not adjust voxels which are (initially) free
/// - kRfExcludeOccupied: do not adjust voxels which are (initially) occupied
///
/// @param occupancy Pointer to the dense voxel occupancy maps the currently available regions. Offsets for a
///     specific region are available by looking up a region key in @p occupancy_region_keys_global and using the
///     cooresponding @c occupancy_region_mem_offsets_global byte offset into this array.
/// @param occupancy_region_keys_global Array of voxel region keys identifying regions available in GPU. There are
///     @c region_count elements in this array.
/// @param occupancy_region_mem_offsets_global Array of voxel region memory offsets into @c occupancy. Each element
///     corresponds to a key in occupancy_region_keys_global. The offsets are in bytes.
/// @param line_keys Array of line segment pairs converted into @c GpuKey references to integrate into the map.
/// @param local_lines Array array of line segments which generated the @c line_keys. These are converted from the
///     original, double precision, map frame coordinates into a set of local frames. Each start/end point pair is
///     relative to the centre of the voxel containing the end point. This is to reduce floating point error in
///     double to single precision conversion and assist in voxel mean calculations which are in the same frame.
///     The original coordinates are not recoverable in this code.
/// @param line_count number of lines in @p line_keys and @p local_lines. These come in pairs, so the number of elements
///     in those arrays is double this value.
/// @param region_dimensions Specifies the size of any one region in voxels.
/// @param voxel_resolution Specifies the size of a voxel cube.
/// @param ray_adjustment Specifies the value adjustment to apply to voxels along the line segment leading up to the
///     final sample voxel. This should be < 0 to re-enforce as free.
/// @param sample_adjustment Specifiest the value adjustment applied to voxels containing the sample point (line end
///     point). Should be > 0 to re-enforce as occupied.
/// @param voxel_value_min Minimum clamping value for voxel adjustments.
/// @param voxel_value_max Maximum clamping value for voxel adjustments.
/// @param region_update_flags Update control values as per @c RayFlag.
__kernel void raysQuery(__global atomic_float *occupancy, __global ulonglong *occupancy_region_mem_offsets_global,
                        __global int3 *occupancy_region_keys_global, uint region_count, __global GpuKey *line_keys,
                        __global float3 *local_lines, uint line_count, int3 region_dimensions, float voxel_resolution,
                        float occupied_threshold, float volume_coefficient,
                        // Output buffers.
                        __global RaysQueryResult *results)
{
  // Only process valid lines.
  if (get_global_id(0) >= line_count)
  {
    return;
  }

  LineWalkData line_data;
  line_data.occupancy = occupancy;
  line_data.occupancy_offsets = occupancy_region_mem_offsets_global;
  line_data.region_keys = occupancy_region_keys_global;
  line_data.region_dimensions = region_dimensions;
  line_data.occupied_threshold = occupied_threshold;
  line_data.region_count = region_count;
  line_data.volume_coefficient = volume_coefficient;
  line_data.result.range = 0;
  line_data.result.unobserved_volume = 0;
  line_data.result.voxel_type = RQ_OccNull;

  regionsInitCurrent(&line_data.current_region, &line_data.current_region_index);

  // Now walk the clipped ray.
  GpuKey start_key, end_key;
  copyKey(&start_key, &line_keys[get_global_id(0) * 2 + 0]);
  copyKey(&end_key, &line_keys[get_global_id(0) * 2 + 1]);

  const float3 lineStart = local_lines[get_global_id(0) * 2 + 0];
  const float3 lineEnd = local_lines[get_global_id(0) * 2 + 1];

  walkVoxelsRayQuery(&start_key, &end_key, lineStart, lineEnd, region_dimensions, voxel_resolution, kLineWalkFlagNone,
                     &line_data);

  results[get_global_id(0)] = line_data.result;
}

#ifndef RAY_QUERY_BASE_CL
#define RAY_QUERY_BASE_CL
#endif  // RAY_QUERY_BASE_CL


================================================
FILE: ohmgpu/gpu/RaysQuery.cu
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include <gputil/cuda/cutil_importcl.h>
#include <gputil/gpu_ext.h>

// Build base without voxel means
#include "RaysQuery.cl"

GPUTIL_CUDA_DEFINE_KERNEL(raysQuery);


================================================
FILE: ohmgpu/gpu/RaysQueryResult.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_GPU_RAYQUERY_RESULT_H
#define OHMGPU_GPU_RAYQUERY_RESULT_H

#ifndef RQ_OccNull
// TODO(KS): include OccupancyType.h or formalise the types.
/// Invalid/null voxel.
#define RQ_OccNull (-2)
/// Unobserved: no data recorded or available for the voxel.
#define RQ_OccUnobserved (-1)
/// Know to be empty or free (traversable).
#define RQ_OccFree (0)
/// Occupied voxel.
#define RQ_OccOccupied (1)
#endif  // RQ_OccNull

/// Structure used to write results for a GpuRayQuery.
typedef struct RaysQueryResult_t  // NOLINT(readability-identifier-naming, modernize-use-using)
{
  /// Range the ray successfully traverses.
  float range;
  /// Unobserved volume metric.
  float unobserved_volume;
  /// Occupancy type of the final voxel.
  int voxel_type;
} RaysQueryResult;

#endif  // OHMGPU_GPU_RAYQUERY_RESULT_H


================================================
FILE: ohmgpu/gpu/RegionUpdate.cl
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

//------------------------------------------------------------------------------
// Note on building this code.
// This code is intended to be used for updating regions both with and without
// sub voxel positioning. This is mostly the same, but the voxel type changes
// and we need an extract function call when updating a voxel with sub voxel
// positioning. To achive this, this code is intended to be includes into a
// compiled twice with the following defines changed on each compilation:
// - REGION_UPDATE_KERNEL : the kernel name for the entry point
// - REGION_UPDATE_SUFFIX : suffix applied to distinguish potentially repeaded
//  code and types.
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// Debug switches for the compiling code to enable (or just uncomment here).
// Deliberately before incldues to configure those files.
//------------------------------------------------------------------------------

// Report regions we can't resolve via printf().
//#define REPORT_MISSING_REGIONS

// Limit the number of cells we can traverse in the line traversal. This is a worst case limit.
//#define LIMIT_LINE_WALK_ITERATIONS
// Limit the number of times we try update a voxel value. Probably best to always have this enabled.
#ifndef LIMIT_VOXEL_WRITE_ITERATIONS
#define LIMIT_VOXEL_WRITE_ITERATIONS
#endif  // LIMIT_VOXEL_WRITE_ITERATIONS

//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------
#include "gpu_ext.h"  // Must be first

#include "MapCoord.h"
#include "RayFlag.h"
#include "Traversal.cl"
#include "VoxelIncident.cl"
#include "VoxelMeanCompute.h"
#ifdef NDT
#include "CovarianceVoxelCompute.h"
#endif  // NDT

#include "LineWalkMarkers.cl"
#include "Regions.cl"

//------------------------------------------------------------------------------
// Declarations
//------------------------------------------------------------------------------

// This is begging for a refactor.
#ifdef NDT
#define REGION_UPDATE_KERNEL regionRayUpdateNdt
#define REGION_WALK_VOXELS   walkVoxelsNdt
#define REGION_VISIT_VOXEL   visitVoxelNdt
#define WALK_VISIT_VOXEL     visitVoxelNdt
#define WALK_NAME            Ndt

#else  // NDT
#define REGION_UPDATE_KERNEL regionRayUpdateOccupancy
#define REGION_WALK_VOXELS   walkVoxelsOccupancy
#define REGION_VISIT_VOXEL   visitVoxelOccupancy
#define WALK_VISIT_VOXEL     visitVoxelOccupancy
#define WALK_NAME            Occupancy
#endif  // NDT


#ifndef REGION_UPDATE_BASE_CL
// User data for voxel visit callback.
typedef struct LineWalkData_t
{
  // Voxel occupancy memory. All regions use a shared buffer.
  __global atomic_float *occupancy;
  // Array of offsets for each regionKey into occupancy. These are byte offsets.
  __global ulonglong *occupancy_offsets;
  __global VoxelMean *means;
  // Array of offsets for each regionKey into means. These are byte offsets.
  __global ulonglong *means_offsets;
  // Traversal voxel memory
  __global atomic_float *traversal;
  // Array of offsets for each regionKey into traversal. These are byte offsets.
  __global ulonglong *traversal_offsets;
  // Touch time voxel memory
  __global atomic_uint *touch_times;
  // Array of offsets for each regionKey into touch_times. These are byte offsets.
  __global ulonglong *touch_times_offsets;
  // Incidents voxel memory
  __global atomic_uint *incidents;
  // Array of offsets for each regionKey into incidents. These are byte offsets.
  __global ulonglong *incidents_offsets;
#ifdef NDT
  __global CovarianceVoxel *cov_voxels;
  // Array of offsets for each regionKey into cov_voxels. These are byte offsets.
  __global ulonglong *cov_offsets;
  // Number of hit/miss counts used in the ndt-tm model
  __global HitMissCount *hit_miss;
  // Array of offsets for each regionKey into hit_miss. These are byte offsets.
  __global ulonglong *hit_miss_offsets;
#endif  // NDT
  // Array of region keys for currently loaded regions.
  __global int3 *region_keys;
  // The region currently being traversed. Also used to reduce searching the region_keys and region_mem_offsets.
  int3 current_region;
  // Size of a region in voxels.
  int3 region_dimensions;
  /// Voxel size
  float voxel_resolution;
  // MapMode/voxel value adjustment for keys along the line segment, but not the sample voxel.
  float ray_adjustment;
  // MapMode/voxel value adjustment for the sample voxel.
  float sample_adjustment;
  /// Value threshold for occupied voxels.
  float occupied_threshold;
  // MapMode/voxel minimum allowed value.
  float voxel_value_min;
  // MapMode/voxel maximum allowed value.
  float voxel_value_max;
  // Number of regions in region_keys/region_mem_offsets.
  uint region_count;
  // Index of the @c current_region into region_keys and corresponding xxx_offsets arrays.
  uint current_region_index;
  uint region_update_flags;
  /// A reference sample position. This is in a frame local to the centre of the voxel containing this sample
  /// coordinate.
  float3 sample;
  /// A reference sensor position. This is in a frame local to the centre of the voxel containing the sample coordinate.
  float3 sensor;
  /// Encoded touch time value for this voxel.
  uint touch_time;
#ifdef NDT
  // Affects how quickly NDT removes voxels: [0, 1].
  float adaptation_rate;
  // An estimate on the sensor range noise error.
  float sensor_noise;
#endif  // NDT
} LineWalkData;
#endif  // REGION_UPDATE_BASE_CL

#include "VoxelMean.cl"

#ifdef NDT
#include "AdjustNdt.cl"
#else  // NDT
#include "AdjustOccupancy.cl"
#endif  // NDT

__device__ bool REGION_VISIT_VOXEL(const GpuKey *voxel_key, const GpuKey *start_key, const GpuKey *end_key,
                                   int voxel_marker, float enter_range, float exit_range, void *user_data);

// Must be included after WALK_NAME and WALK_VISIT_VOXEL function is define
#include "LineWalk.cl"

// Implement the voxel traversal function. We update the value of the voxel using atomic instructions.
__device__ bool REGION_VISIT_VOXEL(const GpuKey *voxel_key, const GpuKey *start_key, const GpuKey *end_key,
                                   int voxel_marker, float enter_range, float exit_range, void *user_data)
{
  float old_value, new_value;

  LineWalkData *line_data = (LineWalkData *)user_data;
  __global atomic_float *occupancy = line_data->occupancy;

  // Abort if this is the sample voxel and we are to exclude the sample. The sample voxel is detected when voxel_marker
  // is kLineWalkMarkerEnd is true and voxel[3] is zero. A value of 1 indicates a clipped ray and the end voxel does not
  // contain the sample.
  const bool reverse_walk = line_data->region_update_flags & kRfReverseWalk;
  const bool is_sample_candidate =
    (!reverse_walk && voxel_marker == kLineWalkMarkerEnd) || (reverse_walk && voxel_marker == kLineWalkMarkerStart);
  const bool is_sample_voxel = is_sample_candidate && voxel_key->voxel[3] == 0;
  if (is_sample_voxel && (line_data->region_update_flags & kRfExcludeSample))
  {
    return true;
  }

  if (!is_sample_voxel && (line_data->region_update_flags & kRfExcludeRay))
  {
    return true;
  }

  // Resolve memory offset for the region of interest.
  if (!regionsResolveRegion(voxel_key, &line_data->current_region, &line_data->current_region_index,
                            line_data->region_keys, line_data->region_count))
  {
    // We can fail to resolve regions along the in the line. This can occurs for several reasons:
    // - Floating point error differences between CPU and GPU line walking means that the GPU may walk into the edge
    //   of a region not hit when walking the regions on CPU.
    // - Regions may not be uploaded due to extents limiting on CPU.
#ifdef REPORT_MISSING_REGIONS
    printf("%u region missing: " KEY_F "\n  Voxels: " KEY_F "->" KEY_F "\n", get_global_id(0), KEY_A(*voxel_key),
           KEY_A(*start_key), KEY_A(*end_key));
#endif
    return true;
  }

  // Adjust value by ray_adjustment unless this is the sample voxel.
  float adjustment = calculateOccupancyAdjustment(voxel_key, (!reverse_walk) ? end_key : start_key, is_sample_candidate,
                                                  is_sample_voxel, line_data->voxel_resolution, line_data);

  // This voxel lies in the region. We will make a value adjustment.
  // Work out which voxel to modify.
  const ulonglong vi_local = voxel_key->voxel[0] + voxel_key->voxel[1] * line_data->region_dimensions.x +
                             voxel_key->voxel[2] * line_data->region_dimensions.x * line_data->region_dimensions.y;
  ulonglong vi =
    (line_data->occupancy_offsets[line_data->current_region_index] / sizeof(*line_data->occupancy)) + vi_local;

  if (voxel_key->voxel[0] < line_data->region_dimensions.x && voxel_key->voxel[1] < line_data->region_dimensions.y &&
      voxel_key->voxel[2] < line_data->region_dimensions.z)
  {
    __global atomic_float *occupancy_ptr = &occupancy[vi];

    bool was_occupied_voxel = false;

#ifdef LIMIT_VOXEL_WRITE_ITERATIONS
    // Under high contension we can end up repeatedly failing to write the voxel value.
    // The primary concern is not deadlocking the GPU, so we put a hard limit on the numebr of
    // attempts made.
    const int iteration_limit = 20;
    int iterations = 0;
#endif
    do
    {
#ifdef LIMIT_VOXEL_WRITE_ITERATIONS
      if (iterations++ > iteration_limit)
      {
        break;
      }
#endif
      // Calculate a new value for the voxel.
      old_value = new_value = gputilAtomicLoadF32(occupancy_ptr);

      const bool initially_unobserved = old_value == INFINITY;
      const bool initially_free = !initially_unobserved && old_value < line_data->occupied_threshold;
      const bool initially_occupied = !initially_unobserved && old_value >= line_data->occupied_threshold;
      was_occupied_voxel = initially_occupied;

      // Check exclusion flags and skip this voxel if excluded.
      // We skip by a 'break' statement which will break out of the compare and swap loop. Could return true.
      // Check skipping unobserved.
      // We also check for zero adjustment here.
      if (initially_unobserved && (line_data->region_update_flags & kRfExcludeUnobserved) || adjustment == 0)
      {
        adjustment = 0;  // Flag null adjustment. Prevents mean update.
        break;
      }

      // Check skipping free.
      if (initially_free && (line_data->region_update_flags & kRfExcludeFree))
      {
        adjustment = 0;  // Flag null adjustment. Prevents mean update.
        break;
      }

      // Check skipping occupied.
      if (initially_occupied && (line_data->region_update_flags & kRfExcludeOccupied))
      {
        adjustment = 0;  // Flag null adjustment. Prevents mean update.
        break;
      }

      // Uninitialised voxels start at INFINITY.
      new_value = (new_value != INFINITY) ? new_value + adjustment : adjustment;
      // Clamp the value.
      new_value = clamp(new_value, line_data->voxel_value_min, line_data->voxel_value_max);

      // Now try write the value, looping if we fail to write the new value.
      // mem_fence(CLK_GLOBAL_MEM_FENCE);
    } while (new_value != old_value && !gputilAtomicCasF32(occupancy_ptr, old_value, new_value));

    if (adjustment > 0)
    {
      uint sample_count = 0;
      if (line_data->means_offsets)
      {
        ulonglong vi =
          vi_local + (line_data->means_offsets[line_data->current_region_index] / sizeof(*line_data->means));
        sample_count = updateVoxelMeanPosition(&line_data->means[vi], line_data->sample, line_data->voxel_resolution);
      }

      if (line_data->touch_times)
      {
        ulonglong vi = vi_local + (line_data->touch_times_offsets[line_data->current_region_index] /
                                   sizeof(*line_data->touch_times));
        // Cast from atomic_uint to uint for OpenCL 2.0 compatibility. The atomic_max has not atomic_<type> signatures.
        gputilAtomicMax((__global uint *)&line_data->touch_times[vi], line_data->touch_time);
      }

      if (line_data->incidents)
      {
        ulonglong vi =
          vi_local + (line_data->incidents_offsets[line_data->current_region_index] / sizeof(*line_data->incidents));
        updateVoxelIncident(&line_data->incidents[vi], line_data->sensor - line_data->sample, sample_count);
      }
    }

    // Update traversal. There is no floating based atomic arithmetic, so we must do the same CAS style update.
    if (line_data->traversal_offsets)
    {
#ifdef LIMIT_VOXEL_WRITE_ITERATIONS
      iterations = 0;
#endif
      // Work out which voxel to modify.
      vi = (line_data->traversal_offsets[line_data->current_region_index] / sizeof(*line_data->traversal)) + vi_local;
      __global atomic_float *traversal = &line_data->traversal[vi];
      old_value = new_value = gputilAtomicLoadF32(traversal);
      do
      {
#ifdef LIMIT_VOXEL_WRITE_ITERATIONS
        if (iterations++ > iteration_limit)
        {
          break;
        }
#endif
        new_value += exit_range - enter_range;
      } while (new_value != old_value && !gputilAtomicCasF32(traversal, old_value, new_value));
    }

    if (was_occupied_voxel && (line_data->region_update_flags & kRfStopOnFirstOccupied))
    {
      // Found first occupied voxel and request is to stop on the first occupied voxel. Abort traversal.
      return false;
    }
  }
  else
  {
    // printf("%u Out of bounds: %u " KEY_F "\n", get_global_id(0), vi, KEY_A(*voxel_key));
    // Abort traversal
    return false;
  }

  // Continue traversal
  return true;
}

//------------------------------------------------------------------------------
// Kernel
//------------------------------------------------------------------------------

/// Integrate rays into voxel map regions.
///
/// Invoked one thread per ray (per @p line_keys pair).
///
/// Like keys are provided in start/end key pairs in @p line_keys where there are @p line_count pairs. Each thread
/// extracts it's start/end pair and performs a line walking algorithm from start to end key. The lines start end points
/// are also provided, relative to the centre of the starting voxel. These start/end coordinate pairs are in
/// @p local_lines. The coordinates for each line are local to the starting voxel centre in order to avoid precision
/// issues which may be introduced in converting from a common double precision frame on CPU into a single precision
/// frame in GPU (we do not support double precision GPU due to the limited driver support).
///
/// For each voxel key along the line, we resolve a voxel in @p occupancy by cross referencing in
/// @p occupancy_region_keys_global, @p occupancy_region_mem_offsets_global and @p region_count. Voxels are split into
/// regions in contiguous chunks in @p occupancy. The @c GpuKey::region for a voxel is matched in lookup @p
/// occupancy_region_keys_global and the index into @p occupancy_region_keys_global recorded. This index is used to
/// lookup @p occupancy_region_mem_offsets_global, which provides a byte offset (not elements) from @p occupancy at
/// which the voxel memory for this voxel begins. Each voxel region has a number of voxels equal to
/// <tt>region_dimensions.x * region_dimensions.y * region_dimensions.z</tt>.
///
/// Once voxel memory is resolved, the value of that voxel is updated by either adding @p ray_adjustment for all but
/// the last voxel in the line, or @p sample_adjustment for the last voxel (exception listed below). The value is
/// clamped to the range <tt>[voxel_value_min, voxel_value_max]</tt>. This adjustment is made in global memory using
/// atomic operations. Success is not guaranteed, but is highly probably. This contension has performance impacts, but
/// was found to be the best overall approach for performance.
///
/// The value adjustment of @p sample_adjustment is normally used for the last voxel in each line. This behaviour may
/// be changed per line, by setting the value of @p GpuKey::voxel[3] (normally unused) to 1. This indicates the line has
/// been clipped.
///
/// The line traversal is also affected by the region_update_flags, which are defined in the enum RayFlag.
/// These modify the line traversal as follows:
/// - kRfEndPointAsFree: use ray_adjustment for the last voxel rather than sample_adjustment.
/// - kRfStopOnFirstOccupied: terminate line walking after touching the first occupied voxel found.
/// - kRfExcludeSample: ignore the voxel containing the sample (the last voxel).
/// - kRfExcludeUnobserved: do not adjust voxels which are (initially) unobserved
/// - kRfExcludeFree: do not adjust voxels which are (initially) free
/// - kRfExcludeOccupied: do not adjust voxels which are (initially) occupied
///
/// @param occupancy Pointer to the dense voxel occupancy maps the currently available regions. Offsets for a
///     specific region are available by looking up a region key in @p occupancy_region_keys_global and using the
///     cooresponding @c occupancy_region_mem_offsets_global byte offset into this array.
/// @param occupancy_region_keys_global Array of voxel region keys identifying regions available in GPU. There are
///     @c region_count elements in this array.
/// @param occupancy_region_mem_offsets_global Array of voxel region memory offsets into @c occupancy. Each element
///     corresponds to a key in occupancy_region_keys_global. The offsets are in bytes.
/// @param line_keys Array of line segment pairs converted into @c GpuKey references to integrate into the map.
/// @param local_lines Array array of line segments which generated the @c line_keys. These are converted from the
///     original, double precision, map frame coordinates into a set of local frames. Each start/end point pair is
///     relative to the centre of the voxel containing the end point. This is to reduce floating point error in
///     double to single precision conversion and assist in voxel mean calculations which are in the same frame.
///     The original coordinates are not recoverable in this code.
/// @param line_count number of lines in @p line_keys and @p local_lines. These come in pairs, so the number of elements
///     in those arrays is double this value.
/// @param region_dimensions Specifies the size of any one region in voxels.
/// @param voxel_resolution Specifies the size of a voxel cube.
/// @param ray_adjustment Specifies the value adjustment to apply to voxels along the line segment leading up to the
///     final sample voxel. This should be < 0 to re-enforce as free.
/// @param sample_adjustment Specifiest the value adjustment applied to voxels containing the sample point (line end
///     point). Should be > 0 to re-enforce as occupied.
/// @param voxel_value_min Minimum clamping value for voxel adjustments.
/// @param voxel_value_max Maximum clamping value for voxel adjustments.
/// @param region_update_flags Update control values as per @c RayFlag.
__kernel void REGION_UPDATE_KERNEL(
  __global atomic_float *occupancy, __global ulonglong *occupancy_region_mem_offsets_global,  //
  __global VoxelMean *means, __global ulonglong *means_region_mem_offsets_global,             //
#ifdef NDT
  __global CovarianceVoxel *cov_voxels, __global ulonglong *cov_region_mem_offsets_global,         //
  __global HitMissCount *hit_miss_voxels, __global ulonglong *hit_miss_region_mem_offsets_global,  //
#endif
  __global atomic_float *traversal_voxels, __global ulonglong *traversal_region_mem_offsets_global,       //
  __global atomic_uint *touch_time_voxels, __global ulonglong *touch_times_region_mem_offsets_global,     //
  __global atomic_uint *incident_voxels, __global ulonglong *incidents_region_mem_offsets_global,         //
  __global int3 *occupancy_region_keys_global, uint region_count,                                         //
  __global GpuKey *line_keys, __global float3 *local_lines, uint line_count, __global uint *touch_times,  //
  int3 region_dimensions, float voxel_resolution, float ray_adjustment, float sample_adjustment,
  float occupied_threshold, float voxel_value_min, float voxel_value_max, uint region_update_flags
#ifdef NDT
  ,
  float adaptation_rate, float sensor_noise
#endif  // NDT
)
{
  // Only process valid lines.
  if (get_global_id(0) >= line_count)
  {
    return;
  }

  LineWalkData line_data;
  line_data.occupancy = occupancy;
  line_data.occupancy_offsets = occupancy_region_mem_offsets_global;
  line_data.means = means;
  line_data.means_offsets = means_region_mem_offsets_global;
#ifdef NDT
  line_data.cov_voxels = cov_voxels;
  line_data.cov_offsets = cov_region_mem_offsets_global;
  line_data.adaptation_rate = adaptation_rate;
  line_data.sensor_noise = sensor_noise;
  line_data.hit_miss = hit_miss_voxels;
  line_data.hit_miss_offsets = hit_miss_region_mem_offsets_global;
#endif  // NDT
  line_data.traversal = traversal_voxels;
  line_data.traversal_offsets = traversal_region_mem_offsets_global;
  line_data.touch_times = touch_time_voxels;
  line_data.touch_times_offsets = touch_times_region_mem_offsets_global;
  line_data.incidents = incident_voxels;
  line_data.incidents_offsets = incidents_region_mem_offsets_global;
  line_data.region_keys = occupancy_region_keys_global;
  line_data.region_dimensions = region_dimensions;
  line_data.voxel_resolution = voxel_resolution;
  line_data.ray_adjustment = ray_adjustment;
  line_data.sample_adjustment = sample_adjustment;
  line_data.occupied_threshold = occupied_threshold;
  line_data.voxel_value_min = voxel_value_min;
  line_data.voxel_value_max = voxel_value_max;
  line_data.region_count = region_count;
  line_data.region_update_flags = region_update_flags;

  regionsInitCurrent(&line_data.current_region, &line_data.current_region_index);

  // Now walk the clipped ray.
  GpuKey start_key, end_key;
  copyKey(&start_key, &line_keys[get_global_id(0) * 2 + 0]);
  copyKey(&end_key, &line_keys[get_global_id(0) * 2 + 1]);

  const float3 line_start = local_lines[get_global_id(0) * 2 + 0];
  const float3 line_end = local_lines[get_global_id(0) * 2 + 1];

  line_data.sample = line_end;
  line_data.sensor = line_start;
  line_data.touch_time = (touch_times) ? touch_times[get_global_id(0)] : 0;

  // For reverse line walk, the start voxel centre is always (0, 0, 0).
  float3 start_voxel_centre = make_float3(0.0f, 0.0f, 0.0f);
  int walk_flags = 0;
  if (region_update_flags & kRfReverseWalk)
  {
    walk_flags |= kLineWalkFlagReverse;
#ifndef NDT
    // For non-NDT (pure occupancy) updates, force reporting the sample last. This yields better occupancy behaviour
    // and less erosion.
    walk_flags |= kLineWalkFlagForReportEndLast;
#endif  // !NDT
    walk_flags |= !!(region_update_flags & kRfExcludeOrigin) * kExcludeEndVoxel;
  }
  else
  {
    walk_flags |= !!(region_update_flags & kRfExcludeOrigin) * kExcludeStartVoxel;
  }

  // Call the line walking function.
  REGION_WALK_VOXELS(&start_key, &end_key, line_start, line_end, region_dimensions, voxel_resolution, walk_flags,
                     &line_data);
}

#undef REGION_UPDATE_KERNEL
#undef REGION_WALK_VOXELS
#undef REGION_VISIT_VOXEL
#undef VOXEL_TYPE

#ifndef REGION_UPDATE_BASE_CL
#define REGION_UPDATE_BASE_CL
#endif  // REGION_UPDATE_BASE_CL


================================================
FILE: ohmgpu/gpu/RegionUpdate.cu
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include <gputil/cuda/cutil_importcl.h>
#include <gputil/gpu_ext.h>

// Build base without voxel means
#include "RegionUpdate.cl"

GPUTIL_CUDA_DEFINE_KERNEL(regionRayUpdateOccupancy);


================================================
FILE: ohmgpu/gpu/RegionUpdateNdt.cu
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include <gputil/cuda/cutil_importcl.h>
#include <gputil/gpu_ext.h>

// Build base with voxel means and NDT
#define NDT
#include "RegionUpdate.cl"

GPUTIL_CUDA_DEFINE_KERNEL(regionRayUpdateNdt);


================================================
FILE: ohmgpu/gpu/Regions.cl
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "GpuKey.h"

/// Initialises the @c currentRegion and @c regionIndex parameters for @c regionsResolveRegion().
__device__ void regionsInitCurrent(int3 *currentRegion, uint *regionIndex);

/// A utility function for resolving the correct memory address index for voxels of the region containing @p voxelKey.
///
/// This function supports the @c GpuCache and @c GpuLayerCache code on CPU. The cache structure is such that voxel
/// data for regions are uploaded into a single memory buffer with the cache tracking memory offsets for each uploaded
/// region into the voxel buffer. On GPU we require three buffers in order to resolve voxels for a specific region:
///
/// 1. The GPU cache voxel buffer, which contains the voxel data.
/// 2. @p voxelKey : an array of @c GpuKey items which is used to resolve corresponding memory offsets into the voxel
///   buffer.
/// 3. An array of memory offsets into the voxel buffer which dirrectly correspond to the @c GpuKey array.
///
/// With this information we can lookup the voxel memory for a region as follows:
/// 1. Traverse the @p voxelKey array searching for the key of interest.
/// 2. If a match is found, lookup the memory offset at the same index. This gives the memory offset into the voxel
///   memory buffer as a byte offset.
/// 3. Lookup the voxel memory buffer with the offset from 2.
///
/// This function supports resolving the index into the @p voxelKey array. This index may then be used for steps 2 and
/// 3 which are done in the calling code.
///
/// There an expectation that multiple subsequent region lookups will be required as we walk a line of voxels. Further,
/// it is expected that voxels from the same region will generally be required. For this reason, this function first
/// checks @c currentRegion to see if it matches @c voxelKey and we can early out. The @c currentRegion is maintained
/// by this function, updated as required.
///
/// Before the first call, @p currentRegion should be initialised by calling @c regionsInitCurrent().
///
/// @param voxelKey The voxel for which we want to find the region data memory offset.
/// @param[in,out] currentRegion On enter, the current/last region reference. On successful exit, the region containing
///     @p voxelKey.
/// @param[in,out] regionIndex On enter, the current/last region voxel index offset. On successful exit, the
///     voxel index offset to the region containing @p voxelKey.
/// @param regionKeys Array of region keys associated with @p regionMemOffsets.
/// @param regionCount The number of regions in @p regionKeys and @p regionMemOffsets.
/// @return True if a region for @p voxelKey is found, false otherwise.
__device__ bool regionsResolveRegion(const GpuKey *voxelKey, int3 *currentRegion, uint *regionIndex,
                                     __global int3 *regionKeys, uint regionCount);

#ifndef REGIONS_CL
#define REGIONS_CL
inline __device__ void regionsInitCurrent(int3 *currentRegion, uint *regionIndex)
{
  currentRegion->x = currentRegion->y = currentRegion->z = 2147483647;
  *regionIndex = 0u;
}

inline __device__ bool regionsResolveRegion(const GpuKey *voxelKey, int3 *currentRegion, uint *regionIndex,
                                            __global int3 *regionKeys, uint regionCount)
{
  // Check if the current region is the same as the last. This will generally be the case.
  if (voxelKey->region[0] == currentRegion->x && voxelKey->region[1] == currentRegion->y &&
      voxelKey->region[2] == currentRegion->z)
  {
    // Same region. No update required.
    return true;
  }

  // printf("%u searching: " KEY_F "\n", get_global_id(0), KEY_A(*voxelKey));
  // Need to search for the region.
  for (uint i = 0; i < regionCount; ++i)
  {
    if (voxelKey->region[0] == regionKeys[i].x && voxelKey->region[1] == regionKeys[i].y &&
        voxelKey->region[2] == regionKeys[i].z)
    {
      // Found the region for voxelKey.
      *currentRegion = regionKeys[i];
      // Voxel offset is an index offset.
      *regionIndex = i;  // regionMemOffsets[i] / voxelSizeBytes;
      // printf("%u found: [ %i %i %i ] @ %u\n", get_global_id(0),
      //        regionKeys[i].x, regionKeys[i].y, regionKeys[i].z,
      //        i);
      // if (*regionIndex * voxelSizeBytes > 1073741824ul)
      // {
      //   printf("%u Bad region address %u\n", get_global_id(0), *regionIndex);
      // }
      return true;
    }
    // printf("%u not: [ %i %i %i ]\n", get_global_id(0), regionKeys[i].x, regionKeys[i].y, regionKeys[i].z);
  }

  return false;
}

// inline __device__ uint regionVoxelIndex(uint regionIndex, __global ulonglong *regionMemOffsets, uint voxelSizeBytes)
// {
//   return regionMemOffsets[regionIndex] / voxelSizeBytes;
// }

#endif  // REGIONS_CL


================================================
FILE: ohmgpu/gpu/RoiRangeFill.cl
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "gpu_ext.h"  // Must be first

// Explicitly include MapCoord.h first.
#include "MapCoord.h"

#include "Regions.cl"

/// @defgroup voxelClearanceGpu Voxel Clearance GPU
/// @{
/// @brief Documentation on GPU code used to calculate the clearance to the nearest obstacle for each voxel.
///
/// This program has each voxel calculate the range to the nearest obstructed voxel, including itself. An obstructed
/// voxel is an occupied voxel, or an uncertain voxel if @c unknownAsOccupied is set. Clearance is only calculated to
/// the requested range, reporting -1 for any voxel with no obstruction within that range.
///
/// The calculation works using a flood fill algorithm, as follows:
/// -# Seed voxels using @c seedObstacleVoxels() to contain an @c char4 with the following information:
///   - @c xyz defines the voxel coordinates relative to the 3D voxel block being processed.
///     The lower, bottom, left corner is (0, 0, 0).
///   - @c w set to 1 for an occupied voxel and zero for a free voxel. Uncertain voxels attain either depending @c
///   unknownAsOccupied
/// -# Iteratively call @c propagateObstacles to propagate obstacle flood fill. Each voxel does the following:
///   - Inspect immediate neighbours (8) and select the closest obstacle from these and its current value.
///     - If voxel does not contain an obstacle (w = 0) select an occupied neighbour with closest @c xyz values.
///     - If voxel contains an obstacle (w = 1) select an occupied neighbour closer than the current @c xyz value.
///       - Maintain current @c xyz no neighbour tracks a closer voxel.
/// -# Repeat 2. as many times as required to ensure we have propagated the wave fronts to the required range.
///
/// For efficiency, voxels for all required regions are maintained in a single GPU buffer cache (see @c GpuLayerCache).
/// This buffer is given as the @c voxelsMem parameter. A region may at any fixed interface within this buffer
/// (interval determined by the region memory size). The @c regionKeysGlobal and @c regionMemOffsetsGlobal parameters
/// are required to map from a region key (in the @c regionKeysGlobal array) to its corresponding memory offset from
/// @c regionMemOffsetGlobal. This offset is used to find the region memory block in @c voxelsMem.
///
/// An optional axis scaling may be applied using @p axisScaling affecting how ranges is calculated along each
/// primary axis. This vector effectively distorts the space by the following matrix:
/// @code{.unparsed}
///   axisScaling.x   0               0
///   0               axisScaling.y   0
///   0               0               axisScaling.z
/// @endcode
///
/// Note the scaling may optionally be omitted from the reported range by using scaling vector other than (1, 1, 1)
/// during propagation, but using the vector (1, 1, 1) for @c migrateResults().

#ifndef ROI_RANGE_FILL_BASE_CL
//-----------------------------------------------------------------------------
// Compile switches
//-----------------------------------------------------------------------------
// Validate we touch all local memory required.
#define VALIDATE_LOCAL_MEM_LOAD 0

#define DBG_X 34
#define DBG_Y 28
#define DBG_Z 28

// TODO: include flags header instead of repeating definitions here.
#define kUnknownAsOccupied (1 << 0)

// Limit the number of cells we can traverse in the line traversal. This is a worst case limit.
//#define LIMIT_LINE_WALK_ITERATIONS
// Limit the number of times we try update a voxel value. Probably best to always have this enabled.
#define LIMIT_VOXEL_WRITE_ITERATIONS
#ifdef LIMIT_VOXEL_WRITE_ITERATIONS
// Store additional debug information in LineWalkData for error reporting.
//#define STORE_DEBUG_INFO
#endif  // LIMIT_VOXEL_WRITE_ITERATIONS

//-----------------------------------------------------------------------------
// Types
//-----------------------------------------------------------------------------
typedef atomic_uint voxel_type;

#endif  // ROI_RANGE_FILL_BASE_CL

//-----------------------------------------------------------------------------
// Function prototypes.
//-----------------------------------------------------------------------------
bool __device__ isOccupied(const float value, float threshold, unsigned flags);
/// Get the index of a voxel within the provided extents.
int __device__ getGlobalVoxelIndex(int3 coord, int3 voxelExtents);
int3 __device__ linearIndexToCoord(unsigned linearIndex, int3 expanse);
bool __device__ isInExpanse(int3 coord, int3 expanseMin, int3 expanseMax);
bool __device__ onROIEdge(int3 coord, int3 roiExtents);
/// Calculate the volume of @p expanse: <tt>x * y * z</tt>.
unsigned __device__ volumeOf(int3 expanse);

/// Select the best obstruction considering the neighbour of the current voxel offset
/// at <tt>(nx, ny, nz)</tt>. The current voxel coordinate is determined by the
/// get_local_id(N) indices.
///
/// @param nx Neighbour offset in x.
/// @param ny Neighbour offset in y.
/// @param nz Neighbour offset in z.
/// @param currentClosest Coordinates of the current best obstruction with w=1 for obstacle, w=0 otherwise.
/// @param localVoxel Cache of voxels with their current best results.
/// @param axisScaling Scaling applied to each axis when calculating the nearest obstruction. See comments above.
/// @return The closest obstructing voxel selected from the neighbour and @p currentClosest.
char4 __device__ selectObstructionForNeighbour(int nx, int ny, int nz, char4 currentClosest, float *currentDistSqr,
                                               __local char4 *localVoxels, float3 axisScaling);

/// Precalculate parameters for @c resolvePaddingVoxelIndex().
void __device__ calculateFacePadding(int3 workingVoxelExtents, int3 outerRegionPadding, __local uint *faceVolumeSizes,
                                     __local int3 *minFaceExtents, __local int3 *maxFaceExtents,
                                     __local int3 *faceVolumes);

/// Convert an index into the padding region into a 3D index local to the @p workingVoxelExtents.
///
/// The @p workingVoxelExtents defines a target region of voxels of interest with 3D indices in the range
/// [0, workingVoxelExtents). Meanwhile the region [-outerRegionPadding, workingVoxelExtents + outerRegionPadding)
/// defines a padded region of interest (to read from). This function assumes we have a 1D @p index into the
/// padding region, skipping over the region [0, workingVoxelExtents). This @p index is converted into a 3D
/// index in the range [-outerRegionPadding, workingVoxelExtents + outerRegionPadding), excluding indices
/// [0, workingVoxelExtents).
///
/// @param index The 1D index into the padding.
/// @param workingVoxelExtents Defines the void region to skip.
/// @param outerRegionPadding Defines the padding added to @p workingVoxelExtents in which we do index.
/// @return A 3D index in the range [-outerRegionPadding, workingVoxelExtents + outerRegionPadding), excluding the
///   range [0, workingVoxelExtents). Returns (0, 0, 0) on failure.
int3 __device__ resolvePaddingVoxelIndex(uint index, int3 workingVoxelExtents, int3 outerRegionPadding,
                                         __local uint *faceVolumeSizes, __local int3 *minFaceExtents,
                                         __local int3 *maxFaceExtents, __local int3 *faceVolumes);

/// Find the voxel index within the @p voxelExtents closest to @p voxelIndex3 (outside the region).
///
/// @param voxelIndex3 Index to find a valid voxel in @p voxelExtents. General usage expects this is
///     outside the @p voxelExtents.
/// @param voxelExtents Defines the voxel region of interest. A valid index lies within [0, voxelExtents).
/// @return The closest voxel in [0, voxelExtents) to @p voxelIndex3, or @p voxelIndex on failure.
int3 __device__ findClosestRegionVoxel(int3 voxelIndex3, int3 voxelExtents);

/// Update a working voxel obstruction using compare and swap semantics.
///
/// The new value, @p newObstruction, is only written if it reresents a voxel closer than the existing value at
/// @p voxel. This method supports write contention on @p voxel.
///
/// @param voxelIndex The voxel coordiantes in the region of interest.
/// @param voxel A pointer to the @p voxel data.
/// @param newObstruction The new obstruction to write. XYZ are relative offsets from @p voxelIndex, while W must be 1.
/// @param axisScaling Per axis scaling applied to distance calculations.
bool __device__ updateVoxelObstructionCas(int3 voxelIndex, __global voxel_type *voxel, char4 newObstruction,
                                          float3 axisScaling);

// Load voxel data into local memory.
void __device__ loadPropagationLocalVoxels(__global char4 *srcVoxels, __local char4 *localVoxels, int3 voxelExtents,
                                           int3 globalWorkItem, int3 localWorkItem, int3 localExpance);

/// Convert from obstruction value to uint for use in atomic operations in @c seedFromOuterRegions()
/// @param obstruction Offset to nearest current obstruction, with w = 1 if there is an obstruction, w = 0 otherwise.
/// @return A 32-bit uint representation of obstruction.
inline uint __device__ obstructionToVoxel(char4 obstruction);
/// Convert from 32-bit uint voxel representation to a char4 obstruction representation for use in atomic operations in
/// @c seedFromOuterRegions()
/// @param voxel The voxel representation to convert.
/// @retrun Offset to nearest current obstruction, with w = 1 if there is an obstruction, w = 0 otherwise.
inline char4 __device__ voxelToObstruction(uint voxel);

//-----------------------------------------------------------------------------
// Implementation
//-----------------------------------------------------------------------------
bool __device__ isOccupied(const float occupancy, float threshold, unsigned flags)
{
  return (occupancy == INFINITY && (flags & kUnknownAsOccupied)) || (occupancy != INFINITY && occupancy >= threshold);
}

#ifndef ROI_RANGE_FILL_BASE_CL
int __device__ getGlobalVoxelIndex(int3 coord, int3 voxelExtents)
{
  if (isInExpanse(coord, make_int3(0, 0, 0), voxelExtents))
  {
    return coord.x + coord.y * voxelExtents.x + coord.z * voxelExtents.x * voxelExtents.y;
  }

  return -1;
}


int3 __device__ linearIndexToCoord(unsigned linearIndex, int3 expanse)
{
  int3 coord;
  coord.x = (int)(linearIndex % expanse.x);
  linearIndex /= expanse.x;
  coord.y = (int)(linearIndex % expanse.y);
  linearIndex /= expanse.y;
  coord.z = (int)linearIndex;
  return coord;
}

bool __device__ isInExpanse(int3 coord, int3 expanseMin, int3 expanseMax)
{
  return expanseMin.x <= coord.x && coord.x < expanseMax.x && expanseMin.y <= coord.y && coord.y < expanseMax.y &&
         expanseMin.z <= coord.z && coord.z < expanseMax.z;
}

bool __device__ onROIEdge(int3 coord, int3 roiExtents)
{
  return
    // in region
    0 <= coord.x && coord.x < roiExtents.x && 0 <= coord.y && coord.y < roiExtents.y && 0 <= coord.z &&
    coord.z < roiExtents.z &&
    // on edge
    (coord.x == 0 || coord.y == 0 || coord.z == 0 || coord.x == roiExtents.x - 1 || coord.y == roiExtents.y - 1 ||
     coord.z == roiExtents.z - 1);
}

unsigned __device__ volumeOf(int3 expanse)
{
  return (unsigned)(expanse.x) * (unsigned)(expanse.y) * (unsigned)(expanse.z);
}


char4 __device__ selectObstructionForNeighbour(int nx, int ny, int nz, char4 currentClosest, float *currentDistSqr,
                                               __local char4 *localVoxels, float3 axisScaling)
{
  // We need to add 2 to the local size lookups to cater for the 1 voxel padding either side
  // of the local voxels. Similarly, we add one to the get_local_id() calls for the same
  // reason.
  const int linearIndexN = (get_local_id(0) + 1 + nx) + (get_local_id(1) + 1 + ny) * (get_local_size(0) + 2) +
                           (get_local_id(2) + 1 + nz) * (get_local_size(0) + 2) * (get_local_size(1) + 2);
  float3 scaledSeparation;

  // Get the neighbour's closest obstacle voxel coordinate.
  char4 neighbourObstacle = localVoxels[linearIndexN];

  // Localise the obstacle from the neighbour frame.
  neighbourObstacle = neighbourObstacle + make_char4(nx, ny, nz, 0);

  // Use neighbourObstacle if:
  // 1. It is an obstacle (w == 1)
  // 2. It is closer than currentClosest or currentClosest is not an obstacle (w == 0).
  scaledSeparation = make_float3(neighbourObstacle.x, neighbourObstacle.y, neighbourObstacle.z) * axisScaling;
  float neighbourObstacleRangeSqr = dot(scaledSeparation, scaledSeparation);
  // Set up for item 2 above. We set neighbourObstacleRangeSqr to currentDistSqr when neighbourObstacleRangeSqr is
  // out of range.
  const bool replaceCurrent =
    neighbourObstacle.w && (currentClosest.w == 0 || neighbourObstacleRangeSqr < *currentDistSqr);

  if (replaceCurrent)
  {
    *currentDistSqr = neighbourObstacleRangeSqr;
    // Neighbour has a closer, in range obstacle than currentClosest.
    return neighbourObstacle;
  }

  return currentClosest;
}


void __device__ calculateFacePadding(int3 workingVoxelExtents, int3 outerRegionPadding, __local uint *faceVolumeSizes,
                                     __local int3 *minFaceExtents, __local int3 *maxFaceExtents,
                                     __local int3 *faceVolumes)
{
  // Indexing here is tricky. We essentially have a working volume in the range:
  // [workingVoxelExtents - outerRegionPadding, workingVoxelExtents + outerRegionPadding)
  // However, we need to skip voxels in the range [0, workingVoxelExtents).
  //
  // We take the following approach:
  // - Define the padding region as the voxels outside of workingVoxelExtents.
  // - Split the padding region into 6 faces.
  //  - Bottom:
  //    [ -outerRegionPadding.x, workingVoxelExtents.x + outerRegionPadding.x )
  //    [ -outerRegionPadding.y, workingVoxelExtents.y + outerRegionPadding.y )
  //    [ -outerRegionPadding.z, 0                                            )
  //  - Top:
  //    [ -outerRegionPadding.x, workingVoxelExtents.x + outerRegionPadding.x )
  //    [ -outerRegionPadding.y, workingVoxelExtents.y + outerRegionPadding.y )
  //    [ workingVoxelExtents.z, workingVoxelExtents.z + outerRegionPadding.z )
  //  - Back:
  //    [ -outerRegionPadding.x, workingVoxelExtents.x + outerRegionPadding.x )
  //    [ -outerRegionPadding.y, 0                                            )
  //    [ 0,                     workingVoxelExtents.z                        )
  //  - Front:
  //    [ -outerRegionPadding.x, workingVoxelExtents.x + outerRegionPadding.x )
  //    [ workingVoxelExtents.y, workingVoxelExtents.y + outerRegionPadding.y )
  //    [ 0,                     workingVoxelExtents.z                        )
  //  - Left:
  //    [ -outerRegionPadding.x, 0                     )
  //    [ 0,                     workingVoxelExtents.y )
  //    [ 0,                     workingVoxelExtents.z )
  //  - Right:
  //    [ workingVoxelExtents.x, workingVoxelExtents.x + outerRegionPadding.x )
  //    [ 0,                     workingVoxelExtents.y                        )
  //    [ 0,                     workingVoxelExtents.z                        )
  //
  // Other ideas to consider:
  // https://en.wikipedia.org/wiki/Net_(polyhedron)

  enum Face
  {
    FaceBottom,
    FaceTop,
    FaceBack,
    FaceFront,
    FaceLeft,
    FaceRight,
  };

  // Cacluate which face the index falls into. For this we first calculate all the face volumes
  // Calculate volumes into local memory to share in the work group.
  if (get_local_id(0) < 6)
  {
    switch (get_local_id(0))
    {
    case FaceBottom:
      minFaceExtents[FaceBottom] = -outerRegionPadding;
      maxFaceExtents[FaceBottom] =
        make_int3(workingVoxelExtents.x + outerRegionPadding.x, workingVoxelExtents.y + outerRegionPadding.y, 0);
      break;
    case FaceTop:
      minFaceExtents[FaceTop] = make_int3(-outerRegionPadding.x, -outerRegionPadding.y, workingVoxelExtents.z);
      maxFaceExtents[FaceTop] = workingVoxelExtents + outerRegionPadding;
      break;
    case FaceBack:
      minFaceExtents[FaceBack] = make_int3(-outerRegionPadding.x, -outerRegionPadding.y, 0);
      maxFaceExtents[FaceBack] = make_int3(workingVoxelExtents.x + outerRegionPadding.x, 0, workingVoxelExtents.z);
      break;
    case FaceFront:
      minFaceExtents[FaceFront] = make_int3(-outerRegionPadding.x, workingVoxelExtents.y, 0);
      maxFaceExtents[FaceFront] = make_int3(workingVoxelExtents.x + outerRegionPadding.x,
                                            workingVoxelExtents.y + outerRegionPadding.y, workingVoxelExtents.z);
      break;
    case FaceLeft:
      minFaceExtents[FaceLeft] = make_int3(-outerRegionPadding.x, 0, 0);
      maxFaceExtents[FaceLeft] = make_int3(0, workingVoxelExtents.y, workingVoxelExtents.z);
      break;
    case FaceRight:
      minFaceExtents[FaceRight] = make_int3(workingVoxelExtents.x, 0, 0);
      maxFaceExtents[FaceRight] =
        make_int3(workingVoxelExtents.x + outerRegionPadding.x, workingVoxelExtents.y, workingVoxelExtents.z);
      break;
    default:
      break;
    }

    faceVolumes[get_local_id(0)] = maxFaceExtents[get_local_id(0)] - minFaceExtents[get_local_id(0)];
    faceVolumeSizes[get_local_id(0)] = volumeOf(faceVolumes[get_local_id(0)]);
    // printf("[%d] %d %d %d to %d %d %d\t=>(%d %d %d)\n", get_local_id(0),
    //   minFaceExtents[get_local_id(0)].x, minFaceExtents[get_local_id(0)].y, minFaceExtents[get_local_id(0)].z,
    //   maxFaceExtents[get_local_id(0)].x, maxFaceExtents[get_local_id(0)].y, maxFaceExtents[get_local_id(0)].z,
    //   faceVolumes[get_local_id(0)].x, faceVolumes[get_local_id(0)].y, faceVolumes[get_local_id(0)].z);
  }

  barrier(CLK_LOCAL_MEM_FENCE);
}


int3 __device__ resolvePaddingVoxelIndex(uint index, int3 workingVoxelExtents, int3 outerRegionPadding,
                                         __local uint *faceVolumeSizes, __local int3 *minFaceExtents,
                                         __local int3 *maxFaceExtents, __local int3 *faceVolumes)
{
  int face = -1;
  uint faceIndex1D = index;
  for (int i = 0; i < 6; ++i)
  {
    if (face == -1)
    {
      if (faceIndex1D < faceVolumeSizes[i])
      {
        face = i;
      }
      else
      {
        faceIndex1D -= faceVolumeSizes[i];
      }
    }
  }

  if (face < 0)
  {
    // Out of range. Should have already been culled.
    printf("No face found on thread %u\n", index);
    return make_int3(0, 0, 0);
  }

  // Now generate an index into the face region.
  // maxFaceExtents is exclusive, so use >=
  if (faceIndex1D >= volumeOf(maxFaceExtents[face] - minFaceExtents[face]))
  {
    printf("Face index out of range on thread %u\n", index);
    return make_int3(0, 0, 0);
  }

  // Convert the 1D face index into a 3D index in the face volume.
  int3 faceIndex3;
  faceIndex3.z = faceIndex1D / (faceVolumes[face].x * faceVolumes[face].y);
  const uint faceIndex1D2 = faceIndex1D - faceIndex3.z * faceVolumes[face].x * faceVolumes[face].y;
  faceIndex3.y = faceIndex1D2 / faceVolumes[face].x;
  faceIndex3.x = faceIndex1D2 % faceVolumes[face].x;

  // if (index == 144)
  // {
  //   printf("[%u]: F[%d]: if[%u], if3[%d %d %d] => [%d %d %d]\n", index, face, faceIndex1D,
  //       faceIndex3.x, faceIndex3.y, faceIndex3.z,
  //       faceIndex3.x + minFaceExtents[face].x,
  //       faceIndex3.y + minFaceExtents[face].y,
  //       faceIndex3.z + minFaceExtents[face].z);
  // }

  // Offset by the minFaceExtents for the face.
  return faceIndex3 + minFaceExtents[face];
}


inline float __device__ calcTimeVal(float limit, float origin, float direction)
{
  return (limit - origin) * direction;
}


inline bool __device__ rangesOverlap(float2 range1, float2 range2, float2 *overlap)
{
  overlap->x = max(range1.x, range2.x);
  overlap->y = min(range1.y, range2.y);
  return range1.x <= range2.y && range2.x <= range1.y;
}


int3 __device__ findClosestRegionVoxel(int3 voxelIndex3, int3 voxelExtents)
{
  // Convert to a ray/box interection test.

  // Define the box. Technically the box ranges from (0,0,0) to voxelExtents. However, when converting
  // to integer results, we'll get results at the voxel extents. We avoid this by bringing in the box edges
  // by half a voxel.
  const float3 box[2] = { make_float3(0.5f, 0.5f, 0.5f), convert_float3(voxelExtents) - 0.5f };
  // Define the ray as starting at the centre of the source voxel.
  const float3 origin = convert_float3(voxelIndex3) + make_float3(0.5f, 0.5f, 0.5f);
  // Tagret the centre of the box.
  const float3 end = 0.5f * convert_float3(voxelExtents);
  const float3 direction = end - origin;
  const float3 inv_direction = make_float3(1.0f, 1.0f, 1.0f) / direction;
  const int3 sign = make_int3(inv_direction.x < 0 ? 1 : 0, inv_direction.y < 0 ? 1 : 0, inv_direction.z < 0 ? 1 : 0);

  float2 hitTimes;
  float2 tx, ty, tz;
  bool validIntersection = true;

  // Perform various segment overlap calculations.
  tx.x = calcTimeVal(box[sign.x].x, origin.x, inv_direction.x);
  tx.y = calcTimeVal(box[1 - sign.x].x, origin.x, inv_direction.x);
  ty.x = calcTimeVal(box[sign.y].y, origin.y, inv_direction.y);
  ty.y = calcTimeVal(box[1 - sign.y].y, origin.y, inv_direction.y);
  tz.x = calcTimeVal(box[sign.z].z, origin.z, inv_direction.z);
  tz.y = calcTimeVal(box[1 - sign.z].z, origin.z, inv_direction.z);

  validIntersection = validIntersection && rangesOverlap(tx, ty, &hitTimes);
  validIntersection = validIntersection && rangesOverlap(hitTimes, tz, &hitTimes);

  const float hitTime = hitTimes.x >= 0 ? hitTimes.x : hitTimes.y;
  return validIntersection ? convert_int3(origin + hitTime * direction) : voxelIndex3;
}


inline __device__ uint obstructionToVoxel(char4 obstruction)
{
#if __ENDIAN_LITTLE__
  return (uint)((uchar)obstruction.x) | ((uchar)obstruction.y << 8) | ((uchar)obstruction.z << 16) |
         ((uchar)obstruction.w << 24);
#else   // __ENDIAN_LITTLE__
  return (uint)((uchar)obstruction.x << 24) | ((uchar)obstruction.y << 16) | ((uchar)obstruction.z << 8) |
         ((uchar)obstruction.w);
#endif  // __ENDIAN_LITTLE__
}


inline __device__ char4 voxelToObstruction(uint voxel)
{
#if __ENDIAN_LITTLE__
  return make_char4((char)((voxel)&0xFFu), (char)((voxel >> 8) & 0xFFu), (char)((voxel >> 16) & 0xFFu),
                    (char)((voxel >> 24) & 0xFFu));
#else   // __ENDIAN_LITTLE__
  return make_char4((char)((voxel >> 24) & 0xFFu), (char)((voxel >> 16) & 0xFFu), (char)((voxel >> 8) & 0xFFu),
                    (char)((voxel)&0xFFu));
#endif  // __ENDIAN_LITTLE__
}


bool __device__ updateVoxelObstructionCas(int3 voxelIndex, __global voxel_type *voxel, char4 newObstruction,
                                          float3 axisScaling)
{
  uint reference_value, new_value;
  __global voxel_type *voxel_ptr;

  // Prepare the new value we may write.
  new_value = obstructionToVoxel(newObstruction);
  voxel_ptr = voxel;

  float3 offset = make_float3(newObstruction.x, newObstruction.y, newObstruction.z) * axisScaling;
  const float distanceToObstacleSqr = dot(offset, offset);

  const int iterationLimit = 500;
  int iteration = 0;
  bool needsUpdate = false, updated = false;
  // Begin the contended write loop:
  do
  {
    // Cache the current value as a reference value.
    reference_value = gputilAtomicLoadU32(voxel_ptr);

    // Evaluate the range to the voxel currently considered the closest obstruction for the target voxel.
    // No need to apply voxelResolution as we are making relative comparisons.
    const char4 ref = voxelToObstruction(reference_value);
    offset = make_float3(ref.x, ref.y, ref.z) * axisScaling;

    const bool existing_obstruction = ref.w != 0;

    // Apply axis scaling to the distance calculation.
    const float currentDistSqr = dot(offset, offset);

    // Only important when the new obstruction is closer than the current one or the current is not an obstruction.
    if (distanceToObstacleSqr < currentDistSqr || !existing_obstruction)
    {
      // Attempt to write to the target location. This is done with contention, so we use
      // atomics to test for success. On failure we'll iterate again until we hit the iteration
      // limit.
      needsUpdate = !gputilAtomicCasU32(voxel_ptr, reference_value, new_value);
      updated = !needsUpdate;
    }
    else
    {
      // No need to touch the voxel. Existing value is closer.
      needsUpdate = false;
    }
  } while (needsUpdate && ++iteration < iterationLimit);

#ifdef LIMIT_VOXEL_WRITE_ITERATIONS
  if (iteration == iterationLimit)
  {
    printf("%u excessive voxel update iterations (%d %d %d).\n", get_global_id(0), voxelIndex.x, voxelIndex.y,
           voxelIndex.z);
  }
#endif  // LIMIT_VOXEL_WRITE_ITERATIONS

  return updated;
}


/// Load voxels into local memory for propagation kernels.
void __device__ loadPropagationLocalVoxels(__global char4 *srcVoxels, __local char4 *localVoxels, int3 voxelExtents,
                                           int3 globalWorkItem, int3 localWorkItem, int3 localExpanse)
{
  const int3 localGroupExpanse = localExpanse;
  // We pad the local memory by one voxel around the region expanse. This is to fetch data from neighbouring regions.
  const int3 localMemExpanse = localGroupExpanse + make_int3(2, 2, 2);
  const uint totalLoad = volumeOf(localMemExpanse);
  const char4 unobstructedVoxel = make_char4(0, 0, 0, 0);

#if VALIDATE_LOCAL_MEM_LOAD
  const char4 localMemClearValue = make_char4(1, 2, 3, 4);
  const int3 debugGroup = make_int3(0, 0, 0);
  const int3 debugThread = make_int3(0, 0, 0);
  // if (isLocalThread(0, 0, 0) && isInGroup(debugGroup.x, debugGroup.y, debugGroup.z))
  if (isGlobalThread(debugThread.x, debugThread.y, debugThread.z))
  {
    // Validate we touch all local mem required.
    for (uint i = 0; i < volumeOf(localMemExpanse); ++i)
    {
      localVoxels[i] = localMemClearValue;
    }
  }
  barrier(CLK_LOCAL_MEM_FENCE);
#endif  // VALIDATE_LOCAL_MEM_LOAD

  // if (isGlobalThread(0, 0, 0))
  // {
  //   printf("voxelExt: %d %d %d, localExp %d %d %d, localMem %d %d %d\n",
  //     voxelExtents.x, voxelExtents.y, voxelExtents.z,
  //     localExpanse.x, localExpanse.y, localExpanse.z,
  //     localMemExpanse.x, localMemExpanse.y, localMemExpanse.z);
  // }

  for (uint i = 0; i < totalLoad; i += volumeOf(localGroupExpanse))
  {
    // Cache voxels into local memory.
    // Work out the local source coordinate.
    // Note the linear index will normally be out of range in the last iteration as the
    // padded 3D expanse doesn't nicely match the local size.
    // There is also an oddity in the linearisation. We use the localGroupExpanse to generate a linear
    // index (matching the work group size) then convert that to a 3D index in the localMemExpanse,
    // which includes padding. We then iterate and get additional voxels, essentially accounting for
    // padding. Odd, but it works out.
    uint localLinearIndex = i + localWorkItem.x + localWorkItem.y * localGroupExpanse.x +
                            localWorkItem.z * localGroupExpanse.x * localGroupExpanse.y;

    // Convert the linear index into a 3D index into the padded local expanse.
    int3 localSrcCoord = linearIndexToCoord(localLinearIndex, localMemExpanse);
    // Convert the localSrcCoord into the unpadded coordinates. Remember, we only need pad one voxel from neighbouring
    // regions as we only propagate one voxel at a time.
    localSrcCoord -= make_int3(1, 1, 1);

    // Convert the local coordinate to a global voxel coordinate.
    // This may under of over flow.
    int3 globalSrcCoord =
      make_int3(localSrcCoord.x + get_group_id(0) * localExpanse.x, localSrcCoord.y + get_group_id(1) * localExpanse.y,
                localSrcCoord.z + get_group_id(2) * localExpanse.z);

    // Convert the global 3D voxel coordinate into a linear value. It will come out as -1
    // when the global coordinate is out of range.
    int globalLinearSrcIndex = getGlobalVoxelIndex(globalSrcCoord, voxelExtents);

    // Now make the load into local memory when the target index is in range.
    if (localLinearIndex < volumeOf(localMemExpanse))
    {
      // Cache the range of the global voxel, but assign -1 when the global voxel is out of range.
      localVoxels[localLinearIndex] = (globalLinearSrcIndex >= 0) ? srcVoxels[globalLinearSrcIndex] : unobstructedVoxel;
    }

    barrier(CLK_LOCAL_MEM_FENCE);
  }

#if VALIDATE_LOCAL_MEM_LOAD
  if (isGlobalThread(debugThread.x, debugThread.y, debugThread.z))
  {
    // Validate we touch all local mem required.
    bool touchedAllMem = true;
    for (uint i = 0; i < volumeOf(localMemExpanse); ++i)
    {
      if (localVoxels[i].x == localMemClearValue.x && localVoxels[i].y == localMemClearValue.y &&
          localVoxels[i].z == localMemClearValue.z && localVoxels[i].w == localMemClearValue.w)
      {
        printf("Did not touch %u\n", i);
        touchedAllMem = false;
      }
    }

    if (!touchedAllMem)
    {
      printf("Did not touch all local memory\n");
    }
  }
  barrier(CLK_LOCAL_MEM_FENCE);
#endif  // VALIDATE_LOCAL_MEM_LOAD
}

#endif  // ROI_RANGE_FILL_BASE_CL

//-----------------------------------------------------------------------------
// Kernels
//-----------------------------------------------------------------------------

/// Build obstacle information into workingVoxels. We store the voxel coordinates in the GPU global group and
/// w = 1 for occupied voxels, zero for free. Each voxel in workingVoxels directly matches the dimensions of the
/// space we are to calculate clearance values for. This will always match directly with a number of OccupancyMap
/// regions in CPU.
///
/// This kernel does not consider any voxels outside the regions we are calculating clearance values for. That
/// occurs in the @c seedFromOuterRegions() kernel below.
///
/// @par Invocation
/// In layer batches, one thread per X/Z coordinate, processing zbatch items in Z.
///
/// @param cornerVoxelKey Key for the lower extents corner of the global work group. All other GPU threads can resolve
/// their key by
///   adjusting this key using their 3D global ID.
/// @param workingVoxels Identifies the offset to the nearest obstructed voxel in voxel units.
///   The w coordinate is zero if there is no obstructed in range and the voxel itself isn't and obstructed.
///   The w coordinate is 1 if there is an obstructed to consider.
__kernel void seedRegionVoxels(__global GpuKey *cornerVoxelKey, __global float *voxelOccupancy,
                               __global char4 *workingVoxels, __global int3 *regionKeysGlobal,
                               __global ulonglong *regionMemOffsetsGlobal, uint regionCount, int3 regionVoxelDimensions,
                               int3 workingVoxelExtents, float occupancyThresholdValue, uint flags, int zbatch)
{
  int3 effectiveGlobalId = make_int3(get_global_id(0), get_global_id(1), get_global_id(2) * zbatch);
  int3 voxelRegion;
  uint regionVoxelOffset;
  regionsInitCurrent(&voxelRegion, &regionVoxelOffset);

  GpuKey voxelKey = *cornerVoxelKey;

  // Offset the voxel key.
  moveKeyAlongAxis(&voxelKey, 0, effectiveGlobalId.x, regionVoxelDimensions);
  moveKeyAlongAxis(&voxelKey, 1, effectiveGlobalId.y, regionVoxelDimensions);
  moveKeyAlongAxis(&voxelKey, 2, effectiveGlobalId.z, regionVoxelDimensions);

  for (int z = 0; z < zbatch; ++z)
  {
    if (effectiveGlobalId.x < (uint)workingVoxelExtents.x && effectiveGlobalId.y < (uint)workingVoxelExtents.y &&
        effectiveGlobalId.z < (uint)workingVoxelExtents.z)
    {
      // Find the region for voxelKey
      if (regionsResolveRegion(&voxelKey, &voxelRegion, &regionVoxelOffset, regionKeysGlobal, regionCount))
      {
        // Found the region memory.
        // Index into voxels
        const uint vidx_local = voxelKey.voxel[0] + voxelKey.voxel[1] * regionVoxelDimensions.x +
                                voxelKey.voxel[2] * regionVoxelDimensions.x * regionVoxelDimensions.y;
        const uint vidx = (regionMemOffsetsGlobal[regionVoxelOffset] / sizeof(*voxelOccupancy)) + vidx_local;
        // Index into workingVoxels.
        const uint widx = effectiveGlobalId.x + effectiveGlobalId.y * workingVoxelExtents.x +
                          effectiveGlobalId.z * workingVoxelExtents.x * workingVoxelExtents.y;
        const float occupancy = voxelOccupancy[vidx];
        const bool isObstruction = isOccupied(occupancy, occupancyThresholdValue, flags);
        workingVoxels[widx] = make_char4(0, 0, 0, (isObstruction) ? 1 : 0);
      }
    }

    // Move to the next Z item.
    ++effectiveGlobalId.z;
    moveKeyAlongAxis(&voxelKey, 2, 1, regionVoxelDimensions);
  }
}


/// Seeds obstacle information taken from voxels outside of the region of interest. Each obstructing voxels
/// finds the voxel inside the working region closest to itself and attempts to push propagate into this voxel.
/// An obstacle voxel can only propagate into the working region if:
/// - It is within the search range of the closest working voxel.
/// - It is closer than any previously recorded voxel in the working voxel.
///
/// This process raises memory contention so obstucting voxels are popagated using compare and swap semantics.
///
/// Each element in @p workingVoxels stores the 3D coordinates to the nearest obstructing voxel in XYZ and either
/// 1 (obstacle) or 0 (no obstacles) in W. The @p workingVoxels array has dimensions of
/// <tt>(regionVoxelDimensions.x, regionVoxelDimensions.y, regionVoxelDimensions.z) and the first entry 3D index is
/// (0, 0, 0). Note that the @c seedFromOuterRegions() kernel will incorporate data from oustide the @p workingVoxels
/// so indexing on each axis may be out of range, either negative or greater than @p regionVoxelDimensions.
///
/// Source data from CPU are held in @p voxelOccupancy (occupancy values) and referenced via @p regionKeysGlobal and
/// @p regionMemOffsetsGlobal using each voxel's @c GpuKey. Each voxel can map to CPU coordinates by offsetting
/// @p cornerVoxel by its local coordiantes in @p workingVoxels. The region part of @c GpuKey is matched against
/// an element of @p regionkeysGlobal. This index is then used to lookup @p regionMemOffsetGlobal from which we
/// get an offset into @p voxelOccupancy. Finally, we convert the voxel part of @c GpuKey into a 1D index which is
/// added to the offset into @p voxelOccupancy. (Needs diagram)
///
/// @param cornerVoxelKey Key for the lower extents corner of the global work group. All other GPU threads can resolve
///   their key by adjusting this key using their 3D global ID.
/// @param voxelOccupancy Voxel occupancy data from the CPU. Indexing is done in conjunction with @p regionKeysGlobal
///    and @p regionMemOffsetGlobal.
/// @param workingVoxels Represents the region of interest and the region(s) for which we will export calculated
///   clearance values.
/// @param regionKeysGlobal Array of region keys corresponding to entries in @p regionMemOffsetsGlobal. See above.
/// @param regionMemOffsetsGlobal Array of index offsets into @c voxelOccupancy marking the start of each region.
/// @param regionCount Number of elements in both @p regionKeysGlobal and @p regionMemOffsetsGlobal.
/// @param regionVoxelDimensions The voxel dimensions of a single region in CPU.
/// @param workingVoxelExtents The dimensions of @p workingVoxels.
/// @param outerRegionPadding The amount of padding required around @c regionVoxelDimensions to correctly consider
///   external obstacles.
/// @param occupancyThresholdValue Requires @p voxelOccupancy threshold value to consider a voxel occupied. Any larger
///   value is considered occupied except for @c INFINITY, which is considered unknown.
/// @param flags Flags modifying behaviour. See @c QF_ flags above.
/// @param zbatch Number of items each thread should process.
__kernel void seedFromOuterRegions(__global GpuKey *cornerVoxelKey, __global float *voxelOccupancy,
                                   __global voxel_type *workingVoxels, __global int3 *regionKeysGlobal,
                                   __global ulonglong *regionMemOffsetsGlobal, uint regionCount,
                                   int3 regionVoxelDimensions, int3 workingVoxelExtents, int3 outerRegionPadding,
                                   float3 axisScaling, float occupancyThresholdValue, uint flags, int batch)
{
  local uint faceVolumeSizes[6];
  local int3 minFaceExtents[6];
  local int3 maxFaceExtents[6];
  local int3 faceVolumes[6];

  // Work out whether this thread indexes a valid padding voxel.
  const unsigned voxelCount = volumeOf(workingVoxelExtents + 2 * outerRegionPadding) - volumeOf(workingVoxelExtents);

  int3 voxelRegion;
  uint regionVoxelOffset;
  regionsInitCurrent(&voxelRegion, &regionVoxelOffset);
  calculateFacePadding(workingVoxelExtents, outerRegionPadding, faceVolumeSizes, minFaceExtents, maxFaceExtents,
                       faceVolumes);

  for (int i = 0; i < batch; ++i)
  {
    uint voxelIndex = get_global_id(0) * batch + i;
    if (voxelIndex >= voxelCount)
    {
      // printf("v out of range: %u < %u\n", voxelIndex, voxelCount);
      // Out of range.
      return;
    }

    // Resolve the padding voxel index relative to the workingVoxels region.
    const int3 workingIndex3 = resolvePaddingVoxelIndex(voxelIndex, workingVoxelExtents, outerRegionPadding,
                                                        faceVolumeSizes, minFaceExtents, maxFaceExtents, faceVolumes);

    // Validate the indexing. Failure shows a logic error.
    if (workingIndex3.x < -outerRegionPadding.x || workingIndex3.y < -outerRegionPadding.y ||
        workingIndex3.z < -outerRegionPadding.z || workingIndex3.x >= workingVoxelExtents.x + outerRegionPadding.x ||
        workingIndex3.y >= workingVoxelExtents.y + outerRegionPadding.y ||
        workingIndex3.z >= workingVoxelExtents.z + outerRegionPadding.z ||
        (0 <= workingIndex3.x && workingIndex3.x < workingVoxelExtents.x && 0 <= workingIndex3.y &&
         workingIndex3.y < workingVoxelExtents.y && 0 <= workingIndex3.z && workingIndex3.z < workingVoxelExtents.z))
    {
      printf("Bad working index: %u => %d %d %d: region %d %d %d, padding %d %d %d\n", voxelIndex, workingIndex3.x,
             workingIndex3.y, workingIndex3.z, workingVoxelExtents.x, workingVoxelExtents.y, workingVoxelExtents.z,
             outerRegionPadding.x, outerRegionPadding.y, outerRegionPadding.z);
      return;
    }

    // Offset the voxel key.
    GpuKey voxelKey = *cornerVoxelKey;
    moveKeyAlongAxis(&voxelKey, 0, workingIndex3.x, regionVoxelDimensions);
    moveKeyAlongAxis(&voxelKey, 1, workingIndex3.y, regionVoxelDimensions);
    moveKeyAlongAxis(&voxelKey, 2, workingIndex3.z, regionVoxelDimensions);

    // Find the region for voxelKey
    if (regionsResolveRegion(&voxelKey, &voxelRegion, &regionVoxelOffset, regionKeysGlobal, regionCount))
    {
      // Found the region memory.
      // Index into voxels
      const uint vidx_local = voxelKey.voxel[0] + voxelKey.voxel[1] * regionVoxelDimensions.x +
                              voxelKey.voxel[2] * regionVoxelDimensions.x * regionVoxelDimensions.y;
      const uint vidx = (regionMemOffsetsGlobal[regionVoxelOffset] / sizeof(*voxelOccupancy)) + vidx_local;
      const float occupancy = voxelOccupancy[vidx];
      const bool isObstruction = isOccupied(occupancy, occupancyThresholdValue, flags);

      if (isObstruction)
      {
        // printf("outer obstruction from key " KEY_F " @: %d %d %d\n",
        //   KEY_A(voxelKey),
        //   workingIndex3.x, workingIndex3.y, workingIndex3.z);

        // Find the edge voxel in the ROI closest to the source voxel.
        // Note: There are rounding errors on this, there are different behaviours depending on the ray direction.
        // This was most notable when testing a single region ROI with obstacles being sourced from the voxel layers
        // directly above and below the ROI. The pattern on the bottom was such that the nearest voxel was generally
        // offset by exactly 1 voxel along Z. The pattern on the top tended to have diagonal offsets.
        // This results in an effective down sampling and a loss of information. We cater for this below by targeting
        // neighbours as well as the edgeVoxel3.
        const int3 edgeVoxel3 = findClosestRegionVoxel(workingIndex3, workingVoxelExtents);

        // Exhaustive neighbours.
        for (int z = -1; z <= 1; ++z)
        {
          for (int y = -1; y <= 1; ++y)
          {
            for (int x = -1; x <= 1; ++x)
            {
              const int3 regionVoxel3 = edgeVoxel3 + make_int3(x, y, z);
              if (onROIEdge(regionVoxel3, workingVoxelExtents))
              {
                // Index into workingVoxels.
                const uint ridx = regionVoxel3.x + regionVoxel3.y * workingVoxelExtents.x +
                                  regionVoxel3.z * workingVoxelExtents.x * workingVoxelExtents.y;


                // Value to write; relative offset from regionVoxel3 to workingIndex3.
                char4 obstructionValue = make_char4(workingIndex3.x - regionVoxel3.x, workingIndex3.y - regionVoxel3.y,
                                                    workingIndex3.z - regionVoxel3.z,
                                                    1);  // Must be 1 for an obstruction.

                // printf("obstruction at %d %d %d => %d %d %d : %d %d %d %d\n", workingIndex3.x, workingIndex3.y,
                // workingIndex3.z,
                //   regionVoxel3.x, regionVoxel3.y, regionVoxel3.z,
                //   obstructionValue.x, obstructionValue.y, obstructionValue.z, obstructionValue.w);

                // Write into the voxel with contention.
                updateVoxelObstructionCas(workingIndex3, &workingVoxels[ridx], obstructionValue, axisScaling);
              }
            }
          }
        }
      }
    }
  }
}


/// Propagate obstacles range value from neighbouring voxels.
///
/// Invoking this kernel once will ensure every voxel looks at its immediate face, edge
/// and corner neighbours, and minimises its range value (where -1/negative is an irrelevant
/// range value). The range value of a neighbour has the range from the neighbour to the
/// voxel of interest added to it before being considered for update.
///
/// When called iteratively, this effectively calculated the range to the closest obstacle
/// voxel. The number of times this is invoked determines the maximum obstacle range.
///
/// The seedObstacleVoxels() kernel should be invoked before this kernel.
///
/// Local memory requirement:
/// - @p localVoxels group volume expanded by 2 around each axis yielding a 1 voxel padding.
///
/// @par Invocation
/// In layer batches, one thread per X/Z coordinate, processing zbatch items in Z.
__kernel void propagateObstacles(__global char4 *srcVoxels, __global char4 *dstVoxels, int3 voxelExtents,
                                 float searchRange, float3 axisScaling LOCAL_ARG(char4 *, localVoxels))
{
  LOCAL_MEM_ENABLE();
  LOCAL_VAR(char4 *, localVoxels,
            0);  // FIXME(KS): match size to CPU size. Won't actually have effect unless more locals are used.


  int3 effectiveGlobalId = make_int3(get_global_id(0), get_global_id(1), get_global_id(2));
  int3 effectiveLocalId = make_int3(get_local_id(0), get_local_id(1), get_local_id(2));
  const int3 effectiveLocalSize = make_int3(get_local_size(0), get_local_size(1), get_local_size(2));

  const uint voxelIndex =
    get_global_id(0) + get_global_id(1) * voxelExtents.x + get_global_id(2) * voxelExtents.x * voxelExtents.y;
  const bool validVoxel = voxelIndex < volumeOf(voxelExtents) && effectiveGlobalId.x < voxelExtents.x &&
                          effectiveGlobalId.y < voxelExtents.y && effectiveGlobalId.z < voxelExtents.z;

  // if (isGlobalThread(0, 0, 0))
  // {
  //   printf("axisScaling: %f %f %f\n", axisScaling.x, axisScaling.y, axisScaling.z);
  // }
  loadPropagationLocalVoxels(srcVoxels, localVoxels, voxelExtents, effectiveGlobalId, effectiveLocalId,
                             effectiveLocalSize);

  if (!validVoxel)
  {
    // Stop invalid voxel references here.
    return;
  }

  // Set range for an unobstructed voxel at the maximum range.
  char4 closestObstruction = srcVoxels[voxelIndex];
  const float3 currentScaledSeparation =
    make_float3(closestObstruction.x, closestObstruction.y, closestObstruction.z) * axisScaling;
  float currentClosestDistSqr = dot(currentScaledSeparation, currentScaledSeparation);

  // 2. Consider each neighbour range value and select the best result.
  for (int z = -1; z <= 1; ++z)
  {
    for (int y = -1; y <= 1; ++y)
    {
      for (int x = -1; x <= 1; ++x)
      {
        if (x || y || z)  // Only do neighbours.
        {
          closestObstruction = selectObstructionForNeighbour(x, y, z, closestObstruction, &currentClosestDistSqr,
                                                             localVoxels, axisScaling);
        }
      }
    }
  }

  // 3. Update the voxel range.
  if (validVoxel)
  {
    dstVoxels[voxelIndex] = closestObstruction;
  }
}


/// Migrate from the working voxel data to the voxel clearance (distance) memory data.
///
/// @par Invocation
/// One thread per region voxel.
///
/// @param cornerVoxelKey Key for the lower extents corner of the global work group. All other GPU threads can resolve
/// their key by
///   adjusting this key using their 3D global ID.
/// @param voxelsMem Memory pointer to the memory block holding voxels. See notes in @ref voxelClearanceGpu
/// @param workingVoxels The flood fill map from @c propagateObstacles()
///   The w coordinate is zero if there is no obstructed in range and the voxel itself isn't and obstructed.
///   The w coordinate is 1 if there is an obstructed to consider.
/// @param regionKeysGlobal Array of region keys matched to elements in @p regionMemOffsetsGlobal (see @ref
/// voxelClearanceGpu).
/// @param regionMemOffsetsGlobal Array of memory offsets into @p voxelsMem where to find each region (see @ref
/// voxelClearanceGpu).
/// @param regionCount Number of regions in @p regionKesyGlobal and @p regionMemOffsetsGlobal.
/// @param regionVoxelDim Number of voxels along each axis in a single region.
/// @param workingVoxelExtents Extents within @p workingVoxels for which we actually want to generate results. This is
///   the non-padded region.
/// @param searchRange The maximum search range for the overall query. Obstacles beyond this range are ignored.
/// @param voxelResolution Physical dimensions along each edge of each voxel.
__kernel void migrateResults(__global float *clearanceVoxels, __global char4 *workingVoxels, int3 regionVoxelDimensions,
                             int3 workingVoxelExtents, float searchRange, float voxelResolution, float3 axisScaling,
                             uint flags)
{
  // NOTE: Experimentation with invoking smaller global sizes was attempted, but to no performance benefit.
  // Tried:
  //  - Each worker thread processed a number of Z layers.
  //  - Each worker thread processed a whole X/Y layer.

  const int3 regionIndex3 = make_int3(get_global_id(0), get_global_id(1), get_global_id(2));
  if (0 <= regionIndex3.x && regionIndex3.x < regionVoxelDimensions.x && 0 <= regionIndex3.y &&
      regionIndex3.y < regionVoxelDimensions.y && 0 <= regionIndex3.z && regionIndex3.z < regionVoxelDimensions.z)
  {
    const int3 workingIndex3 = make_int3(regionIndex3.x, regionIndex3.y, regionIndex3.z);

    // if (isGlobalThread(0, 0, 0))
    // {
    //   // printf("axisScaling: %f %f %f\n", axisScaling.x, axisScaling.y, axisScaling.z);
    //   printf("regionIndex: %d %d %d, of %d %d %d\n", regionIndex3.x, regionIndex3.y, regionIndex3.z,
    //     regionVoxelDimensions.x, regionVoxelDimensions.y, regionVoxelDimensions.z);
    //   printf("workingIndex: %d %d %d of %d %d %d\n", workingIndex3.x, workingIndex3.y, workingIndex3.z,
    //     workingVoxelExtents.x, workingVoxelExtents.y, workingVoxelExtents.z);
    // }

    // Find working voxel index to read from.
    const uint widx = workingIndex3.x + workingIndex3.y * workingVoxelExtents.x +
                      workingIndex3.z * workingVoxelExtents.x * workingVoxelExtents.y;
    const char4 nearestObstacleOffset = workingVoxels[widx];
    const float3 nearestObstacle = make_float3(nearestObstacleOffset.x * voxelResolution * axisScaling.x,
                                               nearestObstacleOffset.y * voxelResolution * axisScaling.y,
                                               nearestObstacleOffset.z * voxelResolution * axisScaling.z);
    float clearance = (nearestObstacleOffset.w) ? length(nearestObstacle) : -1.0f;
    clearance = (clearance <= searchRange) ? clearance : -1.0f;

    // if (isGlobalThread(0, 0, 31))
    // {
    //   printf("[%u,%u,%u] : %d %d %d %d => %f\n",
    //     get_global_id(0), get_global_id(1), get_global_id(2),
    //     nearestObstacleOffset.x, nearestObstacleOffset.y, nearestObstacleOffset.z, nearestObstacleOffset.w,
    //     clearance);
    // }

    // Index to write to. Assume writing only to regionVoxelDimensions
    const uint vidx = regionIndex3.x + regionIndex3.y * regionVoxelDimensions.x +
                      regionIndex3.z * regionVoxelDimensions.x * regionVoxelDimensions.y;

    clearanceVoxels[vidx] = clearance;
  }
}

#ifndef ROI_RANGE_FILL_BASE_CL
#define ROI_RANGE_FILL_BASE_CL
#endif  // ROI_RANGE_FILL_BASE_CL

/// @}


================================================
FILE: ohmgpu/gpu/RoiRangeFill.cu
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include <gputil/cuda/cutil_importcl.h>
#include <gputil/gpu_ext.h>

// Build base without voxel means
#include "RoiRangeFill.cl"

GPUTIL_CUDA_DEFINE_KERNEL(seedRegionVoxels);
GPUTIL_CUDA_DEFINE_KERNEL(seedFromOuterRegions);
GPUTIL_CUDA_DEFINE_KERNEL(propagateObstacles);
GPUTIL_CUDA_DEFINE_KERNEL(migrateResults);


================================================
FILE: ohmgpu/gpu/TransformSamples.cl
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "gpu_ext.h"  // Must be first

__device__ float4 slerp(float4 from, float4 to, float interpolation_factor);
__device__ float4 quaternion_rotate_quaterion(float4 a, float4 b);
__device__ float3 quaternion_rotate_point(float4 rotation, float3 point);


__device__ float4 slerp(float4 from, float4 to, float interpolation_factor)
{
  // bool b = all(isequal(from, to));
  const bool all_equal = all(isequal(from, to));
  // if (all(isequal(from, to)))
  if (all_equal)
  {
    return from;
  }

  float coeff0, coeff1, angle, sin_angle, cos_angle, inv_sin;

  cos_angle = dot(from, to);

  float4 temp = (cos_angle >= 0) ? to : -1.0f * to;
  cos_angle = (cos_angle >= 0) ? cos_angle : -1.0f * cos_angle;

  // numerical round-off error could create problems in call to acos
  if (1.0f - cos_angle > 1e-12f)
  {
    angle = acos(cos_angle);
    sin_angle = sin(angle);  // fSin >= 0 since fCos >= 0

    inv_sin = 1.0f / sin_angle;
    coeff0 = sin((1.0f - interpolation_factor) * angle) * inv_sin;
    coeff1 = sin(interpolation_factor * angle) * inv_sin;
  }
  else
  {
    coeff0 = 1.0f - interpolation_factor;
    coeff1 = interpolation_factor;
  }

  temp.x = coeff0 * from.x + coeff1 * temp.x;
  temp.y = coeff0 * from.y + coeff1 * temp.y;
  temp.z = coeff0 * from.z + coeff1 * temp.z;
  temp.w = coeff0 * from.w + coeff1 * temp.w;

  return temp;
}


__device__ float4 quaternion_rotate_quaterion(float4 a, float4 b)
{
  float4 q;
  q.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
  q.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
  q.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
  q.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
  return q;
}


__device__ float3 quaternion_rotate_point(float4 rotation, float3 v)
{
  const float xx = rotation.x * rotation.x;
  const float xy = rotation.x * rotation.y;
  const float xz = rotation.x * rotation.z;
  const float xw = rotation.x * rotation.w;
  const float yy = rotation.y * rotation.y;
  const float yz = rotation.y * rotation.z;
  const float yw = rotation.y * rotation.w;
  const float zz = rotation.z * rotation.z;
  const float zw = rotation.z * rotation.w;

  float3 res;

  res.x = (1 - 2 * (yy + zz)) * v.x + (2 * (xy - zw)) * v.y + (2 * (xz + yw)) * v.z;

  res.y = (2 * (xy + zw)) * v.x + (1 - 2 * (xx + zz)) * v.y + (2 * (yz - xw)) * v.z;

  res.z = (2 * (xz - yw)) * v.x + (2 * (yz + xw)) * v.y + (1 - 2 * (xx + yy)) * v.z;

  return res;
}


__kernel void transformTimestampedPoints(__global float3 *points, uint point_count,
                                         __global float *transform_timestamps, __global float3 *transform_positions,
                                         __global float4 *transform_rotations, uint transform_count, uint batch_size)
{
  // // Load transform timestamps into local memory.
  // // This is the only data accessed multiple times.
  // const uint transform_step = min(transform_count / get_global_size(0), 1);
  // for (uint i = 0; i < transform_step; ++i)
  // {
  //   const uint transform_index = get_global_id(0) + i * get_global_size(0);
  //   if (transform_index < transform_count)
  //   {
  //     local_transform_times[transform_index] = timestamps[transform_index];
  //   }
  // }

  // barrier(CLK_LOCAL_MEM_FENCE);

  // printf("hi %u / %u\n", get_global_id(0), get_global_size(0));

  for (uint i = 0; i < batch_size; ++i)
  {
    const unsigned sample_index = get_global_id(0) * batch_size + i;
    if (sample_index >= point_count)
    {
      // Out of range.
      // printf("Should return %u : %u\n", get_global_id(0), sample_index);
      return;
    }

    // printf("Thread %u processing %u\n", get_global_id(0), sample_index);

    float3 sample_point = points[sample_index * 2 + 1];
    float sample_time = points[sample_index * 2 + 0].x;

    // if (isGlobalThread(0, 0, 0))
    // {
    //   printf("%u / %u : %f %f %f\n", sample_index, point_count, sample_point.x, sample_point.y, sample_point.z);
    // }

    // Find the appropriate transforms. Binary search the transforms.
    uint from_index = 0;
    uint to_index = transform_count - 1;

    if (transform_count > 2)
    {
      // Binary search.
      const uint iter_limit = 100000;
      uint iter_count = 0;

      if (transform_timestamps[0] <= sample_time && sample_time <= transform_timestamps[transform_count - 1])
      {
        while (from_index <= to_index && iter_count < iter_limit)
        {
          ++iter_count;
          const uint mid_low = (from_index + to_index) / 2;
          const uint mid_high = min(mid_low + 1, transform_count - 1);
          // Adapted binary search for the index braketing sample_time.
          if (sample_time >= transform_timestamps[mid_low] && sample_time <= transform_timestamps[mid_high])
          {
            from_index = mid_low;
            to_index = mid_high;
            break;
          }
          else if (sample_time <= transform_timestamps[mid_low])
          {
            to_index = mid_low - 1;
          }
          else
          {
            from_index = mid_low + 1;
          }
        }

#ifdef DEBUG
        if (iter_count >= iter_limit)
        {
          printf("transformTimestampedPoints(): Binary search failure (%u): %u / %u. search-bound(%u, %u), max(%u)",
                 get_global_id(0), iter_count, iter_limit, from_index, to_index, transform_count);
          printf("Search Time: %f\n", sample_time);
          printf("Times[%u]:\n", transform_count);
          for (uint i = 0; i < transform_count; ++i)
          {
            printf("  %f\n", transform_timestamps[i]);
          }
        }
#endif  // DEBUG
      }
      else
      {
#if DEBUG
        printf("transformTimestampedPoints()[%u]: out of range %f: [%f, %f]\n", get_global_id(0), sample_time,
               transform_timestamps[0], transform_timestamps[transform_count - 1]);
#endif  // DEBUG
        if (sample_time < transform_timestamps[0])
        {
          sample_time = transform_timestamps[0];
          from_index = to_index = 0;
        }
        else
        {
          sample_time = transform_timestamps[transform_count - 1];
          from_index = to_index = transform_count - 1;
        }
      }
    }

    // Have resolved the transform. Linearly interpoloate position and spherically rotation.
    const float interpolation_factor = (sample_time - transform_timestamps[from_index]) /
                                       (transform_timestamps[to_index] - transform_timestamps[from_index]);
    const float3 sensor_position =
      transform_positions[from_index] +
      interpolation_factor * (transform_positions[to_index] - transform_positions[from_index]);
    const float4 sensor_rotation = quaternion_rotate_quaterion(
      transform_rotations[from_index],
      slerp(transform_rotations[from_index], transform_rotations[to_index], interpolation_factor));

    // printf("GPU: %f(%f)  T(%f %f %f) R(%f %f %f %f)\n", sample_time, interpolation_factor, sensor_position.x,
    // sensor_position.y,
    //         sensor_position.z, sensor_rotation.w, sensor_rotation.x, sensor_rotation.y, sensor_rotation.z);

    // Rotate and translate the local sample.
    sample_point = sensor_position + quaternion_rotate_point(sensor_rotation, sample_point);

    // Record the results.
    points[sample_index * 2 + 0] = sensor_position;
    points[sample_index * 2 + 1] = sample_point;
  }
}


================================================
FILE: ohmgpu/gpu/TransformSamples.cu
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include <gputil/cuda/cutil_importcl.h>
#include <gputil/gpu_ext.h>

#include "TransformSamples.cl"

GPUTIL_CUDA_DEFINE_KERNEL(transformTimestampedPoints);


================================================
FILE: ohmgpu/gpu/Traversal.cl
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef TRAVERSAL_CL_H
#define TRAVERSAL_CL_H

/// Calculates the traversal for the line [start, end]. The reference frame of @p start and @p end is assumed to be
/// relative to the centre of the voxel containing @p end (the sample point). This means we can calculate the distance
/// traversed through this voxel by constructing six planes axis aligned around the origin at a distance of
/// @p 0.5*voxel_resolution . We find the distance travelled by intersecting the ray from @p end to @p start with these
/// planes and finding the first hit. The distance to this hit point marks the distance travelled through the voxel.
///
/// @param start The ray start point. This lies outside the end voxel and it's position is relative to the end voxel
///   centre.
/// @param end The ray end point. This lies in the end voxel and it's position is relative to the end voxel centre.
__device__ float calculateTraversal(float3 start, float3 end, float voxel_resolution);

inline __device__ float calculateTraversal(float3 start, float3 end, float voxel_resolution)
{
  // Define the planes which mark the edge of the voxel. The normals out of the voxel.
  // The distance part of the plane equation is implied below as 0.5*voxel_resolution
  const float3 voxel_plane_normals[6] = { make_float3(-1, 0, 0), make_float3(1, 0, 0),  make_float3(0, -1, 0),
                                          make_float3(0, 1, 0),  make_float3(0, 0, -1), make_float3(0, 0, 1) };

  // Seed with the length of the line segment. Doing so handles the case where both points lie within the voxel as we
  // minimise this value in our plane checks.
  float first_hit_time = length(start - end);
  // We trace a ray out of the voxel from end/sample point to start/origin.
  float3 dir = (start - end) / (first_hit_time > 0 ? first_hit_time : 1.0f);

  for (int i = 0; i < 6; ++i)
  {
    const float ray_dot = dot(voxel_plane_normals[i], dir);
    // Generate some point on the plane. It's the voxel centre plus the n*resolution/2 . We know the rays are passed to
    // GPU such that the start and end points are relative to the end voxel centre, so it's (0, 0, 0)
    // Modify p so that is the vector from end to some point on the plane.
    const float3 p = voxel_plane_normals[i] * 0.5f * voxel_resolution - end;
    const float plane_hit_time = (ray_dot > 1e-6f) ? dot(p, voxel_plane_normals[i]) / ray_dot : INFINITY;
    first_hit_time = min(first_hit_time, plane_hit_time);
  }

  return first_hit_time;
}

#endif  // TRAVERSAL_CL_H


================================================
FILE: ohmgpu/gpu/TsdfUpdate.cl
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

//------------------------------------------------------------------------------
// Note on building this code.
// This code is intended to be used for updating regions both with and without
// sub voxel positioning. This is mostly the same, but the voxel type changes
// and we need an extract function call when updating a voxel with sub voxel
// positioning. To achive this, this code is intended to be includes into a
// compiled twice with the following defines changed on each compilation:
// - REGION_UPDATE_KERNEL : the kernel name for the entry point
// - REGION_UPDATE_SUFFIX : suffix applied to distinguish potentially repeaded
//  code and types.
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// Debug switches for the compiling code to enable (or just uncomment here).
// Deliberately before incldues to configure those files.
//------------------------------------------------------------------------------

// Report regions we can't resolve via printf().
// #define REPORT_MISSING_REGIONS

// Limit the number of cells we can traverse in the line traversal. This is a worst case limit.
//#define LIMIT_LINE_WALK_ITERATIONS
// Limit the number of times we try update a voxel value. Probably best to always have this enabled.
#ifndef LIMIT_VOXEL_WRITE_ITERATIONS
#define LIMIT_VOXEL_WRITE_ITERATIONS
#endif  // LIMIT_VOXEL_WRITE_ITERATIONS


//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------
#define GPUTIL_ATOMICS_64 1
#include "gpu_ext.h"  // Must be first

#include "MapCoord.h"
#include "RayFlag.h"
#include "VoxelTsdfCompute.h"

#include "LineWalkMarkers.cl"
#include "Regions.cl"

//------------------------------------------------------------------------------
// Declarations
//------------------------------------------------------------------------------

#define WALK_VISIT_VOXEL visitVoxelTsdf
#define WALK_NAME        Tsdf

#ifndef TSDF_UPDATE_BASE_CL
// User data for voxel visit callback.
typedef struct TsdfWalkData_t
{
  // Voxel occupancy memory. All regions use a shared buffer.
  __global VoxelTsdf *tsdf_voxels;
  // Array of offsets for each regionKey into tsdf. These are byte offsets.
  __global ulonglong *tsdf_offsets;
  // Array of region keys for currently loaded regions.
  __global int3 *region_keys;
  /// The voxel size.
  float voxel_resolution;
  /// Maximum TSDF voxel weight.
  float max_weight;
  /// Default TSDF truncation distance
  float default_truncation_distance;
  /// Non-zero/+ve to enable voxel dropoff. Recommended to set to the voxel size.
  float dropoff_epsilon;
  /// Non-zero/+ve to enable sparsity compensation.
  float sparsity_compensation_factor;
  // The region currently being traversed. Also used to reduce searching the region_keys and region_mem_offsets.
  int3 current_region;
  // Size of a region in voxels.
  int3 region_dimensions;
  // Number of regions in region_keys/region_mem_offsets.
  uint region_count;
  // Index of the @c current_region into region_keys and corresponding xxx_offsets arrays.
  uint current_region_index;
  uint region_update_flags;
  /// A reference sensor position. This is in a frame local to the centre of the voxel containing the sample coordinate.
  float3 sensor;
  /// A reference sample position. This is in a frame local to the centre of the voxel containing this sample
  /// coordinate. Note: for TSDF this is not always the same as the voxel identified by @c end_key in the visit
  /// function. Ray truncation and filtering may modify the @pc end_key so we only process part of the ray.
  float3 sample;
} TsdfWalkData;
#endif  // TSDF_UPDATE_BASE_CL

// A somewhat tacked on implementation of Truncated Signed Distance Fields.
// Implement the voxel traversal function. We update the value of the voxel using atomic instructions.
//
// Note: TSDF ray tracing is actually done in reverse. This can greatly reduce voxel contension improving TSDF
// performance (as the CAS loop limit is hit less often) and quality (as be abandon data less often).
__device__ bool visitVoxelTsdf(const GpuKey *voxel_key, const GpuKey *start_key, const GpuKey *end_key,
                               int voxel_marker, float enter_range, float exit_range, void *user_data)
{
  TsdfWalkData *tsdf_data = (TsdfWalkData *)user_data;

  // Resolve memory offset for the region of interest.
  if (!regionsResolveRegion(voxel_key, &tsdf_data->current_region, &tsdf_data->current_region_index,
                            tsdf_data->region_keys, tsdf_data->region_count))
  {
    // We can fail to resolve regions along the in the line. This can occurs for several reasons:
    // - Floating point error differences between CPU and GPU line walking means that the GPU may walk into the edge
    //   of a region not hit when walking the regions on CPU.
    // - Regions may not be uploaded due to extents limiting on CPU.
#ifdef REPORT_MISSING_REGIONS
    printf("%u region missing: " KEY_F "\n  Voxels: " KEY_F "->" KEY_F "\n", get_global_id(0), KEY_A(*voxel_key),
           KEY_A(*start_key), KEY_A(*end_key));
#endif
    return true;
  }

  // We assume this voxel lies in the tsdf_data->current_region.
  // Work out which voxel to modify.
  const ulonglong vi_local = voxel_key->voxel[0] + voxel_key->voxel[1] * tsdf_data->region_dimensions.x +
                             voxel_key->voxel[2] * tsdf_data->region_dimensions.x * tsdf_data->region_dimensions.y;
  ulonglong vi =
    (tsdf_data->tsdf_offsets[tsdf_data->current_region_index] / sizeof(*tsdf_data->tsdf_voxels)) + vi_local;

  if (voxel_key->voxel[0] < tsdf_data->region_dimensions.x && voxel_key->voxel[1] < tsdf_data->region_dimensions.y &&
      voxel_key->voxel[2] < tsdf_data->region_dimensions.z)
  {
#ifdef LIMIT_VOXEL_WRITE_ITERATIONS
    // Under high contension we can end up repeatedly failing to write the voxel value.
    // The primary concern is not deadlocking the GPU, so we put a hard limit on the numebr of
    // attempts made.
    const int iteration_limit = 20;
    int iterations = 0;
#endif

    // Calculate the current voxel centre in the same space as tsdf_data->sensor and tsdf_data->sample. Remember,
    // those values are both calculated relative to the centre of the voxel containing tsdf_data->sample
    const bool reverse_walk = tsdf_data->region_update_flags & kRfReverseWalk;
    const int3 voxel_diff = keyDiff(voxel_key, (!reverse_walk) ? end_key : start_key, tsdf_data->region_dimensions);
    const float3 voxel_centre =
      make_float3(voxel_diff.x * tsdf_data->voxel_resolution, voxel_diff.y * tsdf_data->voxel_resolution,
                  voxel_diff.z * tsdf_data->voxel_resolution);

    /// Use a union of VoxelTsdf and atomic_ulong (64-bits) so we can write the value back in one operation.
    union
    {
      VoxelTsdf voxel;
      ulonglong value;
    } initial, updated_tsdf;

    __global atomic_ulong *tsdf_voxel_ptr = (__global atomic_ulong *)&tsdf_data->tsdf_voxels[vi];

    do
    {
#ifdef LIMIT_VOXEL_WRITE_ITERATIONS
      if (iterations++ > iteration_limit)
      {
        break;
      }
#endif  // LIMIT_VOXEL_WRITE_ITERATIONS

      initial.value = gputilAtomicLoadU64(tsdf_voxel_ptr);
      updated_tsdf.voxel.weight = initial.voxel.weight;
      updated_tsdf.voxel.distance = initial.voxel.distance;

      if (!calculateTsdf(tsdf_data->sensor, tsdf_data->sample, voxel_centre, tsdf_data->default_truncation_distance,
                         tsdf_data->max_weight, tsdf_data->dropoff_epsilon, tsdf_data->sparsity_compensation_factor,
                         &updated_tsdf.voxel.weight, &updated_tsdf.voxel.distance))
      {
        // Weight too low. Nothing more to do for this voxel.
        return true;
      }
      // Now try write the value, looping if we fail to write the new value.
      // mem_fence(CLK_GLOBAL_MEM_FENCE);
    } while (
      (updated_tsdf.voxel.distance != initial.voxel.distance || updated_tsdf.voxel.weight != initial.voxel.weight) &&
      !gputilAtomicCasU64(tsdf_voxel_ptr, initial.value, updated_tsdf.value));
  }

  return true;
}

// Must be included after WALK_NAME and WALK_VISIT_VOXEL and the WALK_VISIT_VOXEL function is defined
#include "LineWalk.cl"

//------------------------------------------------------------------------------
// Kernel
//------------------------------------------------------------------------------

/// Integrate rays into voxel map regions.
///
/// Invoked one thread per ray (per @p line_keys pair).
///
/// Like keys are provided in start/end key pairs in @p line_keys where there are @p line_count pairs. Each thread
/// extracts it's start/end pair and performs a line walking algorithm from start to end key. The lines start end
/// points are also provided, relative to the centre of the starting voxel. These start/end coordinate pairs are in
/// @p local_lines. The coordinates for each line are local to the starting voxel centre in order to avoid precision
/// issues which may be introduced in converting from a common double precision frame on CPU into a single precision
/// frame in GPU (we do not support double precision GPU due to the limited driver support).
///
/// For each voxel key along the line, we resolve a voxel in @p occupancy by cross referencing in
/// @p occupancy_region_keys_global, @p occupancy_region_mem_offsets_global and @p region_count. Voxels are split into
/// regions in contiguous chunks in @p occupancy. The @c GpuKey::region for a voxel is matched in lookup @p
/// occupancy_region_keys_global and the index into @p occupancy_region_keys_global recorded. This index is used to
/// lookup @p occupancy_region_mem_offsets_global, which provides a byte offset (not elements) from @p occupancy at
/// which the voxel memory for this voxel begins. Each voxel region has a number of voxels equal to
/// <tt>region_dimensions.x * region_dimensions.y * region_dimensions.z</tt>.
///
/// Once voxel memory is resolved, the value of that voxel is updated by either adding @p ray_adjustment for all but
/// the last voxel in the line, or @p sample_adjustment for the last voxel (exception listed below). The value is
/// clamped to the range <tt>[voxel_value_min, voxel_value_max]</tt>. This adjustment is made in global memory using
/// atomic operations. Success is not guaranteed, but is highly probably. This contension has performance impacts, but
/// was found to be the best overall approach for performance.
///
/// The value adjustment of @p sample_adjustment is normally used for the last voxel in each line. This behaviour may
/// be changed per line, by setting the value of @p GpuKey::voxel[3] (normally unused) to 1. This indicates the line
/// has been clipped.
///
/// The line traversal is also affected by the region_update_flags, which are defined in the enum RayFlag.
/// These modify the line traversal as follows:
/// - kRfEndPointAsFree: use ray_adjustment for the last voxel rather than sample_adjustment.
/// - kRfStopOnFirstOccupied: terminate line walking after touching the first occupied voxel found.
/// - kRfExcludeSample: ignore the voxel containing the sample (the last voxel).
/// - kRfExcludeUnobserved: do not adjust voxels which are (initially) unobserved
/// - kRfExcludeFree: do not adjust voxels which are (initially) free
/// - kRfExcludeOccupied: do not adjust voxels which are (initially) occupied
///
/// @param occupancy Pointer to the dense voxel occupancy maps the currently available regions. Offsets for a
///     specific region are available by looking up a region key in @p occupancy_region_keys_global and using the
///     cooresponding @c occupancy_region_mem_offsets_global byte offset into this array.
/// @param occupancy_region_keys_global Array of voxel region keys identifying regions available in GPU. There are
///     @c region_count elements in this array.
/// @param occupancy_region_mem_offsets_global Array of voxel region memory offsets into @c occupancy. Each element
///     corresponds to a key in occupancy_region_keys_global. The offsets are in bytes.
/// @param line_keys Array of line segment pairs converted into @c GpuKey references to integrate into the map.
/// @param local_lines Array array of line segments which generated the @c line_keys. These are converted from the
///     original, double precision, map frame coordinates into a set of local frames. Each start/end point pair is
///     relative to the centre of the voxel containing the end point. This is to reduce floating point error in
///     double to single precision conversion and assist in voxel mean calculations which are in the same frame.
///     The original coordinates are not recoverable in this code.
/// @param unclipped_lines A companion array to @p local_lines which contains the original, unclipped sensor/sample
///     pairs with @p line_count such pairs. These are the original sensor/sample points used to generate
///     @p local_lines. They may differ from the items in @p local_lines because the lines may be truncated or the
///     original rays may be split into multiple segments (in CPU). We need the corresponding original points
///     to ensure the TSDF distance calculation is correct. Like @p local_lines, the @p samples are provided
///     relative to centre of each @p line_keys end voxels (second voxel key in each pair).
/// @param line_count number of lines in @p line_keys and @p local_lines. These come in pairs, so the number of
/// elements in those arrays is double this value.
/// @param region_dimensions Specifies the size of any one region in voxels.
/// @param voxel_resolution Specifies the size of a voxel cube.
/// @param ray_adjustment Specifies the value adjustment to apply to voxels along the line segment leading up to the
///     final sample voxel. This should be < 0 to re-enforce as free.
/// @param sample_adjustment Specifiest the value adjustment applied to voxels containing the sample point (line end
///     point). Should be > 0 to re-enforce as occupied.
/// @param voxel_value_min Minimum clamping value for voxel adjustments.
/// @param voxel_value_max Maximum clamping value for voxel adjustments.
/// @param region_update_flags Update control values as per @c RayFlag. Only respects @c kRfReverseWalk and
/// @c kRfExcludeOrigin.
__kernel void tsdfRayUpdate(__global VoxelTsdf *tsdf_voxels, __global ulonglong *tsdf_region_mem_offsets_global,  //
                            __global int3 *tsdf_region_keys_global, uint region_count,                            //
                            __global GpuKey *line_keys, __global float3 *local_lines,                             //
                            __global float3 *unclipped_lines, uint line_count,                                    //
                            int3 region_dimensions, float voxel_resolution, float max_weight,
                            float default_truncation_distance, float dropoff_epsilon,
                            float sparsity_compensation_factor, uint region_update_flags)
{
  // Only process valid lines.
  if (get_global_id(0) >= line_count)
  {
    return;
  }

  TsdfWalkData tsdf_data;
  tsdf_data.tsdf_voxels = tsdf_voxels;
  tsdf_data.tsdf_offsets = tsdf_region_mem_offsets_global;
  tsdf_data.region_keys = tsdf_region_keys_global;
  tsdf_data.region_dimensions = region_dimensions;
  tsdf_data.voxel_resolution = voxel_resolution;
  tsdf_data.max_weight = max_weight;
  tsdf_data.default_truncation_distance = default_truncation_distance;
  tsdf_data.dropoff_epsilon = dropoff_epsilon;
  tsdf_data.sparsity_compensation_factor = sparsity_compensation_factor;
  tsdf_data.region_count = region_count;
  tsdf_data.region_update_flags = region_update_flags;

  regionsInitCurrent(&tsdf_data.current_region, &tsdf_data.current_region_index);

  // Now walk the clipped ray.
  GpuKey start_key, end_key;
  copyKey(&start_key, &line_keys[get_global_id(0) * 2 + 0]);
  copyKey(&end_key, &line_keys[get_global_id(0) * 2 + 1]);

  tsdf_data.sensor = unclipped_lines[get_global_id(0) * 2 + 0];
  tsdf_data.sample = unclipped_lines[get_global_id(0) * 2 + 1];

  const float3 line_start = local_lines[get_global_id(0) * 2 + 0];
  const float3 line_end = local_lines[get_global_id(0) * 2 + 1];

  int walk_flags = 0;
  if (region_update_flags & kRfReverseWalk)
  {
    walk_flags |= kLineWalkFlagReverse;
    walk_flags |= !!(region_update_flags & kRfExcludeOrigin) * kExcludeEndVoxel;
  }
  else
  {
    walk_flags |= !!(region_update_flags & kRfExcludeOrigin) * kExcludeStartVoxel;
  }

  walkVoxelsTsdf(&start_key, &end_key, line_start, line_end, region_dimensions, voxel_resolution, walk_flags,
                 &tsdf_data);
}

#ifndef TSDF_UPDATE_BASE_CL
#define TSDF_UPDATE_BASE_CL
#endif  // TSDF_UPDATE_BASE_CL


================================================
FILE: ohmgpu/gpu/TsdfUpdate.cu
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include <gputil/cuda/cutil_importcl.h>
#include <gputil/gpu_ext.h>

// Build base without voxel means
#include "TsdfUpdate.cl"

GPUTIL_CUDA_DEFINE_KERNEL(tsdfRayUpdate);


================================================
FILE: ohmgpu/gpu/VoxelIncident.cl
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "VoxelIncidentCompute.h"

/// Update the voxel mean pattern at @p target_address by including the bit(s) from @p pattern_to_add.
/// This is done using atomic operations.
///
/// Each bit in the pattern indicates occupancy at a particular voxel mean location.
/// @param voxel The @c VoxelMean to update.
/// @param incident_ray The incident ray from sensor to sample (un-normalised).
/// @param sample_count How many samples in the voxel *before* adding the current one.
__device__ void updateVoxelIncident(__global atomic_uint *voxel, float3 incident_ray, uint sample_count);

#ifndef VOXEL_INCIDENT_CL
#define VOXEL_INCIDENT_CL

//------------------------------------------------------------------------------
// Functions
//------------------------------------------------------------------------------

// Psuedo header guard to prevent function implementation duplication.
inline __device__ void updateVoxelIncident(__global atomic_uint *voxel, float3 incident_ray, uint sample_count)
{
  uint old_value;
  uint new_value;

  // Few iterations as it's less important to get this right.
  const int iteration_limit = 10;
  int iteration_count = 0;

  // First update the mean position, then the point count. There is a level of contension here, and the results may be
  // somewhat out.
  do
  {
    old_value = gputilAtomicLoadU32(voxel);
    new_value = updateIncidentNormal(old_value, incident_ray, sample_count);
    ++iteration_count;
  } while (!gputilAtomicCasU32(voxel, old_value, new_value) && iteration_limit < iteration_count);
}

#endif  // VOXEL_INCIDENT_CL


================================================
FILE: ohmgpu/gpu/VoxelMean.cl
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

/// Update the voxel mean pattern at @p target_address by including the bit(s) from @p pattern_to_add.
/// This is done using atomic operations.
///
/// Each bit in the pattern indicates occupancy at a particular voxel mean location.
/// @param voxel The @c VoxelMean to update.
/// @param sample_pos The sample position local to the centre of the voxel in falls in.
/// @param voxel_resolution Voxel size.
__device__ uint updateVoxelMeanPosition(__global VoxelMean *voxel, float3 sample_pos, float voxel_resolution);

#ifndef VOXEL_MEAN_CL
#define VOXEL_MEAN_CL

//------------------------------------------------------------------------------
// Functions
//------------------------------------------------------------------------------

// Psuedo header guard to prevent function implementation duplication.
inline __device__ uint updateVoxelMeanPosition(__global VoxelMean *voxel, float3 sample_pos, float voxel_resolution)
{
  uint point_count;
  uint old_value;
  uint new_value;

  // Few iterations as it's less important to get this right.
  const int iteration_limit = 10;
  int iteration_count = 0;

  // First update the mean position, then the point count. There is a level of contension here, and the results may be
  // somewhat out.
  do
  {
    // point_count = gputilAtomicLoadU32(&voxel->count);
    point_count = gputilAtomicLoadU32(&voxel->count);
    old_value = gputilAtomicLoadU32(&voxel->coord);
    new_value = subVoxelUpdate(old_value, point_count, sample_pos, voxel_resolution);
    ++iteration_count;
  } while (!gputilAtomicCasU32(&voxel->coord, old_value, new_value) && iteration_limit < iteration_count);

  // Atomic increment for the point count.
  return gputilAtomicInc(&voxel->count);
}

#endif  // VOXEL_MEAN_CL


================================================
FILE: ohmgpu/private/ClearanceProcessDetail.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "ClearanceProcessDetail.h"

#include <ohm/OccupancyMap.h>

#include <algorithm>

namespace ohm
{
void ClearanceProcessDetail::getWork(OccupancyMap &map)
{
  map.calculateDirtyClearanceExtents(&min_dirty_region, &max_dirty_region, 1);
  current_dirty_cursor = min_dirty_region;
}


void ClearanceProcessDetail::stepCursor(const glm::i16vec3 &step)
{
  if (current_dirty_cursor.x < max_dirty_region.x)
  {
    current_dirty_cursor.x = std::min<int>(current_dirty_cursor.x + step.x, max_dirty_region.x);
  }
  else
  {
    current_dirty_cursor.x = min_dirty_region.x;
    if (current_dirty_cursor.y < max_dirty_region.y)
    {
      current_dirty_cursor.y = std::min<int>(current_dirty_cursor.y + step.y, max_dirty_region.y);
    }
    else
    {
      current_dirty_cursor.y = min_dirty_region.y;
      if (current_dirty_cursor.z < max_dirty_region.z)
      {
        current_dirty_cursor.z = std::min<int>(current_dirty_cursor.z + step.z, max_dirty_region.z);
      }
      else
      {
        current_dirty_cursor = max_dirty_region + glm::i16vec3(1);
      }
    }
  }
}
}  // namespace ohm


================================================
FILE: ohmgpu/private/ClearanceProcessDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_CLEARANCEPROCESSEDETAIL_H
#define OHMGPU_CLEARANCEPROCESSEDETAIL_H

#include "OhmGpuConfig.h"

#include <glm/glm.hpp>

#include "RoiRangeFill.h"

#include <memory>

// #define CACHE_LOCAL_RESULTS
// #define VALIDATE_KEYS

namespace ohm
{
class OccupancyMap;
struct MapChunk;

struct ClearanceProcessDetail
{
  unsigned query_flags = 0;
  glm::vec3 axis_scaling = glm::vec3(1);
  glm::i16vec3 min_dirty_region = glm::i16vec3(1);
  glm::i16vec3 max_dirty_region = glm::i16vec3(0);
  glm::i16vec3 current_dirty_cursor = glm::i16vec3(0);
  /// Last value of the @c OccupancyMap::stamp(). Used to see if the cache needs to be cleared.
  uint64_t map_stamp = 0;
  float search_radius = 0;

  std::unique_ptr<RoiRangeFill> gpu_query;

  inline bool haveWork() const
  {
    return glm::all(glm::lessThanEqual(min_dirty_region, max_dirty_region)) &&
           glm::all(glm::lessThanEqual(current_dirty_cursor, max_dirty_region));
  }

  inline void resetWorking()
  {
    min_dirty_region = glm::i16vec3(1);
    max_dirty_region = current_dirty_cursor = glm::i16vec3(0);
    map_stamp = 0;
  }

  void stepCursor(const glm::i16vec3 &step = glm::i16vec3(1));

  void getWork(OccupancyMap &map);
};
}  // namespace ohm

#endif  // OHMGPU_CLEARANCEPROCESSEDETAIL_H


================================================
FILE: ohmgpu/private/GpuMapDetail.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "GpuMapDetail.h"

#include "GpuCache.h"
#include "GpuLayerCacheParams.h"
#include "GpuMap.h"
#include "GpuTransformSamples.h"

#include <logutil/LogUtil.h>

#include <ohm/DefaultLayer.h>
#include <ohm/MapLayer.h>
#include <ohm/OccupancyMap.h>
#include <ohm/private/OccupancyMapDetail.h>

#include <gputil/gpuDevice.h>

#include <map>

namespace ohm
{
namespace
{
void onOccupancyLayerChunkSync(MapChunk *chunk, const glm::u8vec3 &region_dimensions)
{
  chunk->searchAndUpdateFirstValid(region_dimensions);
}
}  // namespace

GpuMapDetail::~GpuMapDetail()
{
  if (!borrowed_map)
  {
    delete map;
  }
}

GpuCache *initialiseGpuCache(OccupancyMap &map, size_t target_gpu_mem_size, unsigned flags)
{
  OccupancyMapDetail *detail = map.detail();
  auto *gpu_cache = static_cast<GpuCache *>(detail->gpu_cache);
  if (!gpu_cache)
  {
    target_gpu_mem_size = (target_gpu_mem_size) ? target_gpu_mem_size : GpuCache::kDefaultTargetMemSize;
    gpu_cache = new GpuCache(map, target_gpu_mem_size, flags);
    detail->gpu_cache = gpu_cache;

    reinitialiseGpuCache(gpu_cache, map, flags);
  }

  return gpu_cache;
}


void reinitialiseGpuCache(GpuCache *gpu_cache, OccupancyMap &map, unsigned flags)
{
  if (gpu_cache)
  {
    gpu_cache->clear();
    gpu_cache->removeLayers();

    // Create default layers.
    unsigned mappable_flag = 0;
    if (flags & gpumap::kGpuForceMappedBuffers)
    {
      mappable_flag |= kGcfMappable;
    }
    else if (flags & gpumap::kGpuAllowMappedBuffers)
    {
      // Use mapped buffers if device has unified host memory.
      if (gpu_cache->gpu().unifiedMemory())
      {
        mappable_flag |= kGcfMappable;
      }
    }

    // Setup known layers.
    const int occupancy_layer = map.layout().occupancyLayer();
    const int mean_layer = map.layout().meanLayer();
    const int covariance_layer = map.layout().covarianceLayer();
    const int intensity_layer = map.layout().intensityLayer();
    const int hit_miss_layer = map.layout().hitMissCountLayer();
    const int clearance_layer = map.layout().clearanceLayer();
    const int traversal_layer = map.layout().traversalLayer();
    const int touch_times_layer = map.layout().layerIndex(default_layer::touchTimeLayerName());
    const int incidents_layer = map.layout().layerIndex(default_layer::incidentNormalLayerName());
    const int tsdf_layer = map.layout().layerIndex(default_layer::tsdfLayerName());
    std::array<int, 10> known_layers = { occupancy_layer, mean_layer,      covariance_layer, intensity_layer,
                                         hit_miss_layer,  clearance_layer, traversal_layer,  touch_times_layer,
                                         incidents_layer, tsdf_layer };

    // Calculate the relative layer memory sizes.
    std::map<int, size_t> layer_mem_weight;
    size_t total_mem_weight = 0;

    for (int layer_index : known_layers)
    {
      if (layer_index >= 0)
      {
        const size_t layer_size = map.layout().layer(layer_index).layerByteSize(map.regionVoxelDimensions());
        layer_mem_weight[layer_index] = layer_size;
        total_mem_weight += layer_size;
      }
    }

    // Distribute the target member size.
    for (auto &layer_weight : layer_mem_weight)
    {
      // Logic is: layer_mem = target mem * (layer_weight / total_weight)
      layer_weight.second =
        (total_mem_weight) ? layer_weight.second * gpu_cache->targetGpuAllocSize() / total_mem_weight : 0;
    }

    // Estimate the amount of memory that will be allocated for the user's
    // benefit
    size_t estimated_mem = 0;
    for (const int layer_index : known_layers)
    {
      if (layer_index >= 0)
      {
        // Each layer is allocated once so is limited by the GPU's max
        // allocation size
        estimated_mem += std::min(layer_mem_weight[layer_index], gpu_cache->gpu().maxAllocationSize());
      }
    }

    // Create the caches
    try
    {
      if (occupancy_layer >= 0)
      {
        gpu_cache->createCache(kGcIdOccupancy,
                               // On sync, ensure the first valid voxel is updated.
                               GpuLayerCacheParams{ layer_mem_weight[occupancy_layer], occupancy_layer,
                                                    kGcfRead | kGcfWrite | mappable_flag, &onOccupancyLayerChunkSync });
      }

      // Initialise the voxel mean layer.
      if (mean_layer >= 0)
      {
        gpu_cache->createCache(kGcIdVoxelMean, GpuLayerCacheParams{ layer_mem_weight[mean_layer], mean_layer,
                                                                    kGcfRead | kGcfWrite | mappable_flag });
      }

      if (covariance_layer >= 0)
      {
        // TODO(KS): add the write flag if we move to being able to process the samples on GPU too.
        gpu_cache->createCache(kGcIdCovariance,
                               GpuLayerCacheParams{ layer_mem_weight[covariance_layer], covariance_layer,
                                                    kGcfRead | kGcfWrite | mappable_flag });
      }

      // Intensity mean and covaraince.
      if (intensity_layer >= 0)
      {
        gpu_cache->createCache(kGcIdIntensity, GpuLayerCacheParams{ layer_mem_weight[intensity_layer], intensity_layer,
                                                                    kGcfRead | kGcfWrite | mappable_flag });
      }

      // Ndt-tm hit/miss count
      if (hit_miss_layer >= 0)
      {
        gpu_cache->createCache(kGcIdHitMiss, GpuLayerCacheParams{ layer_mem_weight[hit_miss_layer], hit_miss_layer,
                                                                  kGcfRead | kGcfWrite | mappable_flag });
      }

      // Note: we create the clearance gpu cache if we have a clearance layer, but it caches the occupancy_layer as that
      // is the information it reads.
      if (clearance_layer >= 0)
      {
        // Use of occupancy_layer below is correct. See the comment on the kGcIdClearance delcaration and the brief note
        // above.
        gpu_cache->createCache(kGcIdClearance, GpuLayerCacheParams{ layer_mem_weight[clearance_layer], occupancy_layer,
                                                                    kGcfRead | mappable_flag });
      }

      if (traversal_layer >= 0)
      {
        gpu_cache->createCache(kGcIdTraversal, GpuLayerCacheParams{ layer_mem_weight[traversal_layer], traversal_layer,
                                                                    kGcfRead | kGcfWrite | mappable_flag });
      }

      if (touch_times_layer >= 0)
      {
        gpu_cache->createCache(kGcIdTouchTime,
                               GpuLayerCacheParams{ layer_mem_weight[touch_times_layer], touch_times_layer,
                                                    kGcfRead | kGcfWrite | mappable_flag });
      }

      if (incidents_layer >= 0)
      {
        gpu_cache->createCache(kGcIdIncidentNormal,
                               GpuLayerCacheParams{ layer_mem_weight[incidents_layer], incidents_layer,
                                                    kGcfRead | kGcfWrite | mappable_flag });
      }

      if (tsdf_layer >= 0)
      {
        gpu_cache->createCache(kGcIdTsdf,
                               GpuLayerCacheParams{ layer_mem_weight[tsdf_layer], tsdf_layer,
                                                    kGcfRead | kGcfWrite | mappable_flag, &onOccupancyLayerChunkSync });
      }
    }
    catch (const gputil::ApiException &exception)
    {
      const logutil::Bytes estimated_bytes{ estimated_mem, logutil::ByteMagnitude::kByte };
      const std::string message = "Failed to create cache, expected to allocated about " + estimated_bytes.toString() +
                                  " of GPU memory. " + exception.what();
      std::throw_with_nested(gputil::Exception(message, __FILE__, __LINE__));
    }
  }
}
}  // namespace ohm


================================================
FILE: ohmgpu/private/GpuMapDetail.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_GPUMAPDETAIL_H
#define OHMGPU_GPUMAPDETAIL_H

#include "OhmGpuConfig.h"

#include "GpuCache.h"
#include "RayItem.h"

#include <ohm/Key.h>
#include <ohm/RayFilter.h>

#include <glm/glm.hpp>

#include <gputil/gpuBuffer.h>
#include <gputil/gpuEvent.h>
#include <gputil/gpuKernel.h>
#include <gputil/gpuPinnedBuffer.h>

#include <ohmutil/VectorHash.h>

#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#endif                              // __GNUC__
#include <ska/bytell_hash_map.hpp>  // NOLINT
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif  // __GNUC__

#include <array>
#include <vector>

namespace ohm
{
class OccupancyMap;
class GpuProgramRef;
class GpuTransformSamples;

/// Tracks information about voxel data being uploaded.
struct VoxelUploadInfo
{
  /// A GPU buffer which tracks the voxel memory offsets for each region being uploaded. This contains uint64_t
  /// offsets, one for each region being uploaded. It corresponds directly to the region keys in
  /// @c GpuMapDetail::region_offset_buffers .
  ///
  /// @todo comment about how this contains offsets in voxels, not bytes as relating to data in the a
  /// @c GpuLayerCache buffer.
  gputil::Buffer offsets_buffer;
  /// Pinned wrapper to @c offsets_buffer
  gputil::PinnedBuffer offsets_buffer_pinned;
  /// Last upload event to @c offsets_buffer
  gputil::Event offset_upload_event;
  /// Last voxel upload event vai the corresponding @c GpuLayerCache .
  gputil::Event voxel_upload_event;
  /// The @c GpuLayerCache in the @c GpuCache which this structure references.
  int gpu_layer_id = 0;
  /// Allow the region to be instantiated in CPU before uploading? Otherwise the default memory block is created.
  bool allow_region_creation = true;
  /// Skip syncing this @c gpu_layer_id back to CPU on voxel sync?
  bool skip_cpu_sync = false;

  inline VoxelUploadInfo() = default;

  inline VoxelUploadInfo(int gpu_layer_id, const gputil::Device &gpu,
                         unsigned prealloc_elements = 1024u)  // NOLINT(readability-magic-numbers)
    : offsets_buffer(gpu, sizeof(uint64_t) * prealloc_elements, gputil::kBfReadHost)
    , gpu_layer_id(gpu_layer_id)
  {}
};

struct GpuMapDetail
{
  using RegionSet = ska::bytell_hash_set<glm::i16vec3, Vector3Hash<glm::i16vec3>>;

  static const unsigned kBuffersCount = 2;
  OccupancyMap *map;
  // Ray/key buffer upload event pairs.
  /// Events for @p key_buffers
  std::array<gputil::Event, kBuffersCount> key_upload_events;
  /// Buffers for start/end voxel keys for each ray pair: GpuKey
  std::array<gputil::Buffer, kBuffersCount> key_buffers;
  /// Events for @p ray_buffers
  std::array<gputil::Event, kBuffersCount> ray_upload_events;
  /// Buffers of rays to process float3 pairs. Coordinates are local to the centre of the start voxel for each pair.
  std::array<gputil::Buffer, kBuffersCount> ray_buffers;
  /// Buffers holding only sample points for each ray in @p ray_buffers. Occupancy algorithms do not need this
  /// information only the current voxel and whether it's the sample voxelor not  - flagged in the key_buffers -
  /// are important. However, algorithms such as TSDF need to know the distance to the final sample point. We may
  /// lose this information either by GPU ray segmentation (rays split into multiple segments) or by ray filtering
  /// where rays may be truncated. This buffer is populated when @p use_original_sample_buffers is set and contains
  /// one item per ray. The coordinates uploaded are relative to the corresponding ray end voxel (second item in each
  /// ray/voxel pair), using the voxel centre as the local origin. For unclipped rays, this will be the same as the end
  /// point.
  std::array<gputil::Buffer, kBuffersCount> original_ray_buffers;
  /// Events for @p original_ray_buffers
  std::array<gputil::Event, kBuffersCount> original_ray_upload_events;
  /// Buffers to upload sample intensities - a single floating point value per sample.
  std::array<gputil::Buffer, kBuffersCount> intensities_buffers;
  /// Events for @p intensities_buffers
  std::array<gputil::Event, kBuffersCount> intensities_upload_events;
  /// Buffers to upload sample timestamps - a uint32 value per sample - quantised, relative time.
  std::array<gputil::Buffer, kBuffersCount> timestamps_buffers;
  /// Events for @p timestamps_buffers
  std::array<gputil::Event, kBuffersCount> timestamps_upload_events;

  std::array<gputil::Event, kBuffersCount> region_key_upload_events;
  std::array<gputil::Buffer, kBuffersCount> region_key_buffers;
  std::array<gputil::Event, kBuffersCount> region_update_events;

  // Item 0 is always the occupancy layer.
  std::array<std::vector<VoxelUploadInfo>, kBuffersCount> voxel_upload_info;
  /// Vector used to group/sort rays when @c group_rays is `true`.
  std::vector<RayItem> grouped_rays;

  GpuProgramRef *program_ref = nullptr;
  gputil::Kernel update_kernel;

  RayFilterFunction ray_filter;
  bool custom_ray_filter = false;

  /// Number of rays (origin/sample pairs) in the corresponding ray_buffers items.
  std::array<unsigned, kBuffersCount> ray_counts = { 0, 0 };
  /// Number of rays (origin/sample pairs) in the corresponding ray_buffers items which contain unclipped end (sample)
  /// points.
  std::array<unsigned, kBuffersCount> unclipped_sample_counts = { 0, 0 };
  unsigned transform_count = 0;
  std::array<unsigned, kBuffersCount> region_counts = { 0, 0 };

  int next_buffers_index = 0;
  /// Set of processing regions.
  RegionSet regions;

  /// Long rays are broken into segments of this length or smaller (when value is > 0).
  double ray_segment_length = 0;

  /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the occupancy layer.
  int occupancy_uidx = -1;
  /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the mean layer.
  int mean_uidx = -1;
  /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the traversal layer.
  int traversal_uidx = -1;
  /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the touch time layer.
  int touch_time_uidx = -1;
  /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the incident normal layer.
  int incident_normal_uidx = -1;

  /// Used as @c GpuLayerCache::upload() @c batchMarker argument.
  unsigned batch_marker = 1;  // Will cycle odd numbers to avoid zero.
  /// Should rays be grouped (sorted) before GPU upload. Should only be set for some algorthims, like NDT (required).
  bool group_rays = false;
  /// Should we populate @p original_ray_buffers, storing the unclipped rays points for each ray? See
  /// comments on @p original_ray_buffers.
  bool use_original_ray_buffers = false;
  /// True if the @p map is borrowed. False, if @p map is owned here and must be deleted on destruction.
  bool borrowed_map = false;
  /// Have GPU resources been initialised?
  bool gpu_ok = false;
  /// Have cached a GPU program with sub-voxel positioning ( @c VoxelMean )?
  /// @todo I think this is defunct.
  bool cached_sub_voxel_program = false;
  /// Support voxel mean GPU cache layer? This is enabled by default, but can be disabled in specific derivations.
  bool support_voxel_mean = true;
  /// Support traversal GPU cache layer? This is enabled by default, but can be disabled in specific derivations.
  bool support_traversal = true;

  GpuMapDetail(OccupancyMap *map, bool borrowed_map)
    : map(map)
    , borrowed_map(borrowed_map)
  {}

  virtual ~GpuMapDetail();
};


/// Ensure the GPU cache is initialised. Ok to call if already initialised.
/// @param flags @c GpuFlag values.
GpuCache *initialiseGpuCache(OccupancyMap &map, size_t target_gpu_mem_size, unsigned flags);


/// (Re)initialise the given GPU @p gpu_cache to reflect the given @p map layout.
/// @param flags @c GpuFlag values.
void reinitialiseGpuCache(GpuCache *gpu_cache, OccupancyMap &map, unsigned flags);
}  // namespace ohm

#endif  // OHMGPU_GPUMAPDETAIL_H


================================================
FILE: ohmgpu/private/GpuNdtMapDetail.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUNDTMAPDETAIL_H
#define GPUNDTMAPDETAIL_H

#include "OhmGpuConfig.h"

#include "private/GpuMapDetail.h"

#include <ohm/NdtMap.h>
#include <ohm/NdtMode.h>

namespace ohm
{
struct GpuNdtMapDetail : public GpuMapDetail
{
  GpuProgramRef *cov_hit_program_ref = nullptr;
  gputil::Kernel cov_hit_kernel;
  NdtMap ndt_map;

  /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the covariance layer.
  int cov_uidx = -1;
  /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the intensity layer.
  int intensity_uidx = -1;
  /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the hit/hiss layer.
  int hit_miss_uidx = -1;

  GpuNdtMapDetail(OccupancyMap *map, bool borrowed_map, NdtMode mode)
    : GpuMapDetail(map, borrowed_map)
    , ndt_map(map, true, mode)  // Ensures correct initialisation for NDT operations.
  {}
};
}  // namespace ohm

#endif  // GPUNDTMAPDETAIL_H


================================================
FILE: ohmgpu/private/GpuProgramRef.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "GpuProgramRef.h"
#include "OhmGpu.h"

#include <gputil/gpuKernel.h>

namespace ohm
{
GpuProgramRef::GpuProgramRef(const char *name, SourceType source_type, const char *source_str, size_t source_str_length)
  : name_(name)
  , source_str_(source_str, source_str_length)
  , source_type_(source_type)
{
  // Handle no length provided by setting again. Constructor will not have copied anything.
  if (!source_str_length)
  {
    source_str_ = source_str;
  }
}


GpuProgramRef::GpuProgramRef(const char *name, SourceType source_type, const char *source_str, size_t source_str_length,
                             const std::initializer_list<std::string> &build_args)
  : GpuProgramRef(name, source_type, source_str, source_str_length)
{
  build_args_ = build_args;
}


GpuProgramRef::~GpuProgramRef()
{
  releaseReference();
}


bool GpuProgramRef::addReference(gputil::Device &gpu)
{
  std::unique_lock<std::mutex> guard(program_mutex_);
  if (program_ref_ == 0)
  {
    gputil::BuildArgs build_args;
    ohm::setGpuBuildVersion(build_args);
    build_args.args = &build_args_;

    int err = 0;
    program_ = gputil::Program(gpu, name_.c_str());
    // Lint(KS): macro may generate the same code, but depends on GPU API.
    if (source_type_ == kSourceFile)  // NOLINT(bugprone-branch-clone)
    {
      err = GPUTIL_BUILD_FROM_FILE(program_, source_str_.c_str(), build_args);
    }
    else
    {
      err = GPUTIL_BUILD_FROM_SOURCE(program_, source_str_.c_str(), source_str_.size(), build_args);
    }

    if (err)
    {
      program_ = gputil::Program();
      return false;
    }
  }

  ++program_ref_;
  return true;
}


void GpuProgramRef::releaseReference()
{
  std::unique_lock<std::mutex> guard(program_mutex_);

  if (program_ref_ > 0)
  {
    --program_ref_;
    if (!program_ref_)
    {
      program_ = gputil::Program();
    }
  }
}


bool GpuProgramRef::isValid()
{
  std::unique_lock<std::mutex> guard(program_mutex_);
  return program_ref_ > 0;
}
}  // namespace ohm


================================================
FILE: ohmgpu/private/GpuProgramRef.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_GPUPROGRAMREF_H
#define OHMGPU_GPUPROGRAMREF_H

#include "OhmGpuConfig.h"

#include <gputil/gpuProgram.h>

#include <mutex>
#include <vector>

namespace gputil
{
class Device;
}  // namespace gputil

namespace ohm
{
/// A helper class for reference counting a gpu program.
///
/// Usage:
/// - Declare a static GpuProgramRef, initialising with either
///   - A source file path relative to the executable
///   - An inline source string
/// - For each desired reference to the source, call addReference().
///   - E.g., in the constructor of the file using the program.
/// - Call release when each reference is done with the program.
///   - E.g., in the constructor of the file using the program.
class ohmgpu_API GpuProgramRef
{
public:
  /// Type of source being used/referenced
  enum SourceType
  {
    /// Null value. For internal use.
    kSourceNull,
    /// Loading source from a file name.
    kSourceFile,
    /// Building from a provided source string.
    kSourceString
  };

  /// Create a program reference.
  /// @param name Program's reference name.
  /// @param source_type Identified how to treat @p source_str
  /// @param source_str Either a file name or string containing to code to build.
  /// @param source_str_length Optional length of @p source_str.
  /// @bug Needs to be `noexcept`
  GpuProgramRef(const char *name, SourceType source_type, const char *source_str, size_t source_str_length = 0u);
  /// Create a program reference with build arguments.
  /// @param name Program's reference name.
  /// @param source_type Identified how to treat @p source_str
  /// @param source_str Either a file name or string containing to code to build.
  /// @param source_str_length Optional length of @p source_str.
  /// @param build_args Additional build arguments.
  /// @bug Needs to be `noexcept`
  GpuProgramRef(const char *name, SourceType source_type, const char *source_str, size_t source_str_length,
                const std::initializer_list<std::string> &build_args);
  ~GpuProgramRef();

  inline gputil::Program &program() { return program_; }

  bool addReference(gputil::Device &gpu);
  void releaseReference();

  bool isValid();

private:
  std::mutex program_mutex_;
  gputil::Program program_;
  volatile int program_ref_ = 0;
  std::string name_;
  std::string source_str_;
  SourceType source_type_;
  std::vector<std::string> build_args_;
};
}  // namespace ohm

#endif  // OHMGPU_GPUPROGRAMREF_H


================================================
FILE: ohmgpu/private/GpuTransformSamplesDetail.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmGpuConfig.h"

#include <gputil/gpuBuffer.h>
#include <gputil/gpuDevice.h>
#include <gputil/gpuEvent.h>
#include <gputil/gpuKernel.h>

#include <array>

namespace ohm
{
struct GpuTransformSamplesDetail
{
  static const unsigned kUploadEventCount = 4;

  gputil::Buffer transform_positions_buffer;
  gputil::Buffer transform_rotations_buffer;
  gputil::Buffer transform_times_buffer;
  std::array<gputil::Event, kUploadEventCount> upload_events;
  gputil::Device gpu;
  gputil::Kernel kernel;
};
}  // namespace ohm


================================================
FILE: ohmgpu/private/GpuTsdfMapDetail.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef GPUTSDFMAPDETAIL_H
#define GPUTSDFMAPDETAIL_H

#include "OhmGpuConfig.h"

#include "private/GpuMapDetail.h"

#include <ohm/VoxelTsdf.h>

namespace ohm
{
struct GpuTsdfMapDetail : public GpuMapDetail
{
  /// Index into @c voxel_upload_info buffers at which we have the @c VoxelUploadInfo for the tsdf layer.
  int tsdf_uidx = -1;
  /// Tsdf mapping options.
  TsdfOptions tsdf_options;

  GpuTsdfMapDetail(OccupancyMap *map, bool borrowed_map)
    : GpuMapDetail(map, borrowed_map)
  {
    // Require original samples for TSDF for the distance calculations.
    use_original_ray_buffers = true;
  }
};
}  // namespace ohm

#endif  // GPUTSDFMAPDETAIL_H


================================================
FILE: ohmgpu/private/LineKeysQueryDetailGpu.h
================================================

// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_LINEKEYSQUERYDETAILGPU_H_
#define OHMGPU_LINEKEYSQUERYDETAILGPU_H_OHMGPU_LINEKEYSQUERYDETAILGPU_HOHMGPU_LINEKEYSQUERYDETAILGPU_H_

#include "OhmGpuConfig.h"

#include <ohm/private/LineKeysQueryDetail.h>

// Include GPU structure definition.
#include "GpuKey.h"

#include <gputil/gpuBuffer.h>
#include <gputil/gpuDevice.h>
#include <gputil/gpuKernel.h>
#include <gputil/gpuQueue.h>

#include <atomic>

namespace ohm
{
struct LineKeysQueryDetailGpu : public LineKeysQueryDetail
{
  gputil::Kernel line_keys_kernel;
  gputil::Device gpu;

  gputil::Queue queue;

  gputil::Buffer lines_out;
  gputil::Buffer line_points;
  unsigned max_keys_per_line = 0;
  std::atomic_bool inflight{ false };

  bool gpu_ok = false;
};
}  // namespace ohm

#endif  // OHMGPU_LINEKEYSQUERYDETAILGPU_H_


================================================
FILE: ohmgpu/private/LineQueryDetailGpu.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_LINEQUERYDETAILGPU_H
#define OHMGPU_LINEQUERYDETAILGPU_H

#include "OhmGpuConfig.h"

#include <ohm/private/LineQueryDetail.h>

#include <ohm/KeyList.h>

namespace ohm
{
class ClearanceProcess;

struct LineQueryDetailGpu : LineQueryDetail
{
  ClearanceProcess *clearance_calculator = nullptr;
};
}  // namespace ohm

#endif  // OHMGPU_LINEQUERYDETAILGPU_H


================================================
FILE: ohmgpu/private/RaysQueryDetailGpu.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RaysQueryDetailGpu.h"

#include "private/GpuProgramRef.h"

#include "GpuKey.h"
#include "GpuLayerCache.h"

#include <ohm/OccupancyMap.h>

#include <ohm/private/OccupancyMapDetail.h>

#include <gputil/gpuBuffer.h>
#include <gputil/gpuEventList.h>
#include <gputil/gpuKernel.h>
#include <gputil/gpuPinnedBuffer.h>
#include <gputil/gpuPlatform.h>
#include <gputil/gpuProgram.h>

#include <logutil/Logger.h>

#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
#include "RaysQueryResource.h"
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL

#if GPUTIL_TYPE == GPUTIL_CUDA
GPUTIL_CUDA_DECLARE_KERNEL(raysQuery);
#endif  // GPUTIL_TYPE == GPUTIL_CUDA

namespace ohm
{
namespace
{
#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref("RaysQuery", GpuProgramRef::kSourceString, RaysQueryCode,  // NOLINT
                            RaysQueryCode_length);
#else   // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref("RaysQuery", GpuProgramRef::kSourceFile, "RaysQuery.cl", 0u);
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
}  // namespace

RaysQueryMapWrapper::RaysQueryMapWrapper()
  : GpuMap(new RaysQueryMapWrapperDetail)
{
  RaysQueryMapWrapperDetail *imp = detail();
  imp->support_voxel_mean = false;
}


RaysQueryMapWrapper::~RaysQueryMapWrapper()
{}


void RaysQueryMapWrapper::setMap(OccupancyMap *map)
{
  const unsigned expected_query_count = 2048u;
  GpuMap::setMap(map, true, expected_query_count, 0, false);
  RaysQueryMapWrapperDetail *imp = detail();
  if (map)
  {
    if (GpuCache *gpu_cache = gpuCache())
    {
      if (!imp->results_gpu.isValid())
      {
        imp->results_gpu =
          gputil::Buffer(gpu_cache->gpu(), sizeof(RaysQueryResult) * expected_query_count, gputil::kBfWriteHost);
      }
    }

    // Disable instantiation of regions and syncing voxels back to CPU. This is a read only operation.
    for (size_t i = 0; i < imp->voxel_upload_info.size(); ++i)
    {
      for (auto &voxel_info : imp->voxel_upload_info[i])
      {
        voxel_info.allow_region_creation = false;
        voxel_info.skip_cpu_sync = true;
      }
    }
  }
  imp->results_cpu.clear();
}


void RaysQueryMapWrapper::setVolumeCoefficient(float coefficient)
{
  detail()->volume_coefficient = coefficient;
}


float RaysQueryMapWrapper::volumeCoefficient() const
{
  return detail()->volume_coefficient;
}


const std::vector<RaysQueryResult> &RaysQueryMapWrapper::results() const
{
  return detail()->results_cpu;
}


size_t RaysQueryMapWrapper::integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities,
                                          const double *timestamps, unsigned ray_update_flags)
{
  RaysQueryMapWrapperDetail *imp = detail();
  imp->results_cpu.clear();
  imp->needs_sync = true;
  // Queries must be traced forward for the correct result - ray start to end - and cannot be reverse traced.
  return GpuMap::integrateRays(rays, element_count, intensities, timestamps, ray_update_flags & ~kRfReverseWalk);
}


void RaysQueryMapWrapper::onSyncVoxels(int buffer_index)
{
  (void)buffer_index;  // unused
  RaysQueryMapWrapperDetail *imp = detail();
  if (imp->results_event.isValid())
  {
    imp->results_event.wait();
  }
}


RaysQueryMapWrapperDetail *RaysQueryMapWrapper::detail()
{
  return static_cast<RaysQueryMapWrapperDetail *>(imp_);
}


const RaysQueryMapWrapperDetail *RaysQueryMapWrapper::detail() const
{
  return static_cast<const RaysQueryMapWrapperDetail *>(imp_);
}


void RaysQueryMapWrapper::cacheGpuProgram(bool with_voxel_mean, bool with_traversal, bool force)
{
  (void)with_voxel_mean;  // unused
  (void)with_traversal;   // unused
  if (imp_->program_ref)
  {
    if (!force)
    {
      return;
    }
  }

  releaseGpuProgram();

  GpuCache &gpu_cache = *gpuCache();
  imp_->gpu_ok = true;
  imp_->cached_sub_voxel_program = with_voxel_mean;
  imp_->program_ref = &g_program_ref;

  if (imp_->program_ref->addReference(gpu_cache.gpu()))
  {
    imp_->update_kernel = GPUTIL_MAKE_KERNEL(imp_->program_ref->program(), raysQuery);
    imp_->update_kernel.calculateOptimalWorkGroupSize();

    imp_->gpu_ok = imp_->update_kernel.isValid();
  }
  else
  {
    imp_->gpu_ok = false;
  }
}


void RaysQueryMapWrapper::finaliseBatch(unsigned region_update_flags)
{
  (void)region_update_flags;  // unused

  RaysQueryMapWrapperDetail *imp = detail();
  const int buf_idx = imp->next_buffers_index;
  const OccupancyMapDetail *map = imp->map->detail();

  // Complete region data upload.
  GpuCache &gpu_cache = *this->gpuCache();
  GpuLayerCache &occupancy_layer_cache = *gpu_cache.layerCache(kGcIdOccupancy);

  // Enqueue update kernel.
  const gputil::int3 region_dim_gpu = { map->region_voxel_dimensions.x, map->region_voxel_dimensions.y,
                                        map->region_voxel_dimensions.z };

  const unsigned region_count = imp->region_counts[buf_idx];
  const unsigned ray_count = imp->ray_counts[buf_idx];
  imp->results_gpu.elementsResize<RaysQueryResult>(ray_count);
  gputil::Dim3 global_size(ray_count);
  gputil::Dim3 local_size(std::min<size_t>(imp->update_kernel.optimalWorkGroupSize(), ray_count));
  gputil::EventList wait(
    { imp->key_upload_events[buf_idx], imp->ray_upload_events[buf_idx], imp->region_key_upload_events[buf_idx],
      imp->voxel_upload_info[buf_idx][0].offset_upload_event, imp->voxel_upload_info[buf_idx][0].voxel_upload_event });


  imp->update_kernel(global_size, local_size, wait, imp->region_update_events[buf_idx], &gpu_cache.gpuQueue(),
                     // Kernel args begin:
                     gputil::BufferArg<float>(*occupancy_layer_cache.buffer()),
                     gputil::BufferArg<uint64_t>(imp->voxel_upload_info[buf_idx][0].offsets_buffer),
                     gputil::BufferArg<gputil::int3>(imp->region_key_buffers[buf_idx]), region_count,
                     gputil::BufferArg<GpuKey>(imp->key_buffers[buf_idx]),
                     gputil::BufferArg<gputil::float3>(imp->ray_buffers[buf_idx]), ray_count, region_dim_gpu,
                     float(map->resolution), map->occupancy_threshold_value, imp->volume_coefficient,
                     gputil::BufferArg<RaysQueryResult>(imp->results_gpu));
  // gpu_cache.gpuQueue().flush();


  // Update most recent chunk GPU event.
  occupancy_layer_cache.updateEvents(imp->batch_marker, imp->region_update_events[buf_idx]);

  // Enqueu reading the results.
  imp->results_cpu.resize(ray_count);
  imp->results_gpu.readElements(imp->results_cpu.data(), ray_count, 0, &gpu_cache.gpuQueue(),
                                &imp->region_update_events[buf_idx], &imp->results_event);

  gpu_cache.gpuQueue().finish();

  // logutil::trace(imp->region_counts[buf_idx], "regions\n");

  imp->region_counts[buf_idx] = 0;
  // Start a new batch for the GPU layers.
  imp->batch_marker = occupancy_layer_cache.beginBatch();
  imp->next_buffers_index = 1 - imp->next_buffers_index;
}

}  // namespace ohm


================================================
FILE: ohmgpu/private/RaysQueryDetailGpu.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmGpuConfig.h"

#include "GpuMapDetail.h"

#include <ohmgpu/GpuMap.h>
#include <ohmgpu/gpu/RaysQueryResult.h>

#include <ohm/private/RaysQueryDetail.h>


namespace ohm
{
struct RaysQueryMapWrapperDetail : public GpuMapDetail
{
  gputil::Event results_event;
  gputil::Buffer results_gpu;
  std::vector<RaysQueryResult> results_cpu;
  float volume_coefficient = 1.0f;
  bool needs_sync = false;

  inline RaysQueryMapWrapperDetail()
    : GpuMapDetail(nullptr, false)
  {}
};

/// This is a creative use of the @c GpuMap where we use the standard @c GpuMap logic to marshal our rays, but
/// override the GPU kernel invocation to perform the rays query instead.
///
/// Notes:
/// - Only supports one inflight query.
/// - Rays cannot be removed by filtering, only clipped.
/// - Behaviour is undefined if the ray query does not fit in a single GPU batch.
class ohmgpu_API RaysQueryMapWrapper final : public GpuMap
{
public:
  /// Create a @c RaysQueryMapWrapper . Note we initialise with a null map and set the map pointer later.
  RaysQueryMapWrapper();

  /// Destructor
  ~RaysQueryMapWrapper() final;

  void setMap(OccupancyMap *map);
  void setVolumeCoefficient(float coefficient);
  float volumeCoefficient() const;

  const std::vector<RaysQueryResult> &results() const;

  using RayMapper::integrateRays;

protected:
  size_t integrateRays(const glm::dvec3 *rays, size_t element_count, const float *intensities, const double *timestamps,
                       unsigned ray_update_flags) override;

  void onSyncVoxels(int buffer_index) override;

  /// Helper to access the internal pimpl cast to the correct type.
  RaysQueryMapWrapperDetail *detail();
  /// Helper to access the internal pimpl cast to the correct type.
  const RaysQueryMapWrapperDetail *detail() const;

  /// Load and cache the required GPU program. The @p with_voxel_mean value is irrelevant.
  void cacheGpuProgram(bool with_voxel_mean, bool with_traversal, bool force) final;

  /// Override the GPU kernenel invocation to perform the rays query.
  void finaliseBatch(unsigned region_update_flags) final;
};

struct RaysQueryDetailGpu : public RaysQueryDetail
{
  std::unique_ptr<RaysQueryMapWrapper> gpu_interface;
};
}  // namespace ohm


================================================
FILE: ohmgpu/private/RoiRangeFill.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RoiRangeFill.h"

#include "GpuCache.h"
#include "GpuKey.h"
#include "GpuLayerCache.h"
#include "GpuMap.h"

#include "private/GpuMapDetail.h"
#include "private/GpuProgramRef.h"

#include <ohm/DefaultLayer.h>
#include <ohm/Key.h>
#include <ohm/MapLayer.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/QueryFlag.h>
#include <ohm/VoxelBlock.h>
#include <ohm/VoxelBuffer.h>
#include <ohm/private/OccupancyMapDetail.h>

#include <ohmutil/GlmStream.h>

#include <gputil/gpuPinnedBuffer.h>
#include <gputil/gpuPlatform.h>

#include <logutil/Logger.h>

#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>

#include <algorithm>
#include <limits>

#define KERNEL_PROFILING 0
#include <ohmutil/Profile.h>

#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
#include "RoiRangeFillResource.h"
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL

#if GPUTIL_TYPE == GPUTIL_CUDA
GPUTIL_CUDA_DECLARE_KERNEL(seedRegionVoxels);
GPUTIL_CUDA_DECLARE_KERNEL(seedFromOuterRegions);
GPUTIL_CUDA_DECLARE_KERNEL(propagateObstacles);
GPUTIL_CUDA_DECLARE_KERNEL(migrateResults);
#endif  // GPUTIL_TYPE == GPUTIL_CUDA

namespace ohm
{
namespace
{
#if defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref("RoiRangeFill", GpuProgramRef::kSourceString,  // NOLINT
                            RoiRangeFillCode, RoiRangeFillCode_length);
#else   // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
// NOLINTNEXTLINE(cert-err58-cpp)
GpuProgramRef g_program_ref("RoiRangeFill", GpuProgramRef::kSourceFile, "RoiRangeFill.cl", 0u);
#endif  // defined(OHM_EMBED_GPU_CODE) && GPUTIL_TYPE == GPUTIL_OPENCL
}  // namespace

RoiRangeFill::RoiRangeFill(gputil::Device &gpu)
{
  gpu_ = gpu;

  // Initialise buffer to dummy size. We'll resize as required.
  gpu_corner_voxel_key_ = gputil::Buffer(gpu, sizeof(GpuKey), gputil::kBfReadHost);
  // NOLINTNEXTLINE(readability-magic-numbers)
  gpu_region_keys_ = gputil::Buffer(gpu, 32 * sizeof(gputil::int3), gputil::kBfReadHost);
  // NOLINTNEXTLINE(readability-magic-numbers)
  gpu_occupancy_region_offsets_ = gputil::Buffer(gpu, 32 * sizeof(uint64_t), gputil::kBfReadHost);
  // Place holder allocation:
  gpu_region_clearance_buffer_ = gputil::Buffer(gpu, 4, gputil::kBfReadHost);
  for (auto &gpu_work : gpu_work_)
  {
    gpu_work = gputil::Buffer(gpu, 1 * sizeof(gputil::char4), gputil::kBfReadWrite);
  }
}


RoiRangeFill::~RoiRangeFill()
{
  // Release buffers first.
  gpu_corner_voxel_key_ = gputil::Buffer();
  gpu_region_keys_ = gputil::Buffer();
  gpu_occupancy_region_offsets_ = gputil::Buffer();
  gpu_region_clearance_buffer_ = gputil::Buffer();
  gpu_work_[0] = gputil::Buffer();
  gpu_work_[1] = gputil::Buffer();
  gpu_ = gputil::Device();

  releaseGpuProgram();
}


bool RoiRangeFill::calculateForRegion(OccupancyMap &map, const glm::i16vec3 &region_key)
{
  PROFILE(RoiRangeFill);

  const int clearance_layer_index = map.layout().clearanceLayer();
  if (clearance_layer_index < 0)
  {
    return false;
  }

  PROFILE(prime);

  // Calculate the voxel extents of the query. This size depends on the size of a single region plus the padding
  // required to ensure we reach the search range.
  const auto voxel_padding = unsigned(std::ceil(search_radius_ / map.resolution()));

  // Ensure cache is initialised.
  GpuCache *gpu_cache = initialiseGpuCache(map, GpuCache::kDefaultTargetMemSize, gpumap::kGpuAllowMappedBuffers);
  GpuLayerCache *occupancy_cache = gpu_cache->layerCache(kGcIdOccupancy);
  GpuLayerCache *clearance_cache = gpu_cache->layerCache(kGcIdClearance);

  // // Set the occupancyCache to read only mode. We need to copy query from it, but not back.
  // // This supports multiple threads.
  // GpuLayerCacheWriteLock occupancyWriteLock(*occupancyCache);

  // 1. voxelExtents contains the full range of the query.
  // 2. Determine the optimal buffer size to cover this range. This is based on GPU capabilities.
  const unsigned max_batch_size = std::min(occupancy_cache->cacheSize(), clearance_cache->cacheSize());
  const glm::ivec3 region_padding((voxel_padding + map.regionVoxelDimensions().x - 1) / map.regionVoxelDimensions().x,
                                  (voxel_padding + map.regionVoxelDimensions().y - 1) / map.regionVoxelDimensions().y,
                                  (voxel_padding + map.regionVoxelDimensions().z - 1) / map.regionVoxelDimensions().z);

  // Voxel grid we need to perform the calculations for.
  const glm::ivec3 batch_calc_extents(map.regionVoxelDimensions());
  // Voxel grid we need to upload (padded on the calc extents).
  const glm::ivec3 batch_voxel_extents = batch_calc_extents + 2 * glm::ivec3(voxel_padding);
  assert(map.regionVoxelDimensions().x <= batch_voxel_extents.x &&
         map.regionVoxelDimensions().y <= batch_voxel_extents.y &&
         map.regionVoxelDimensions().z <= batch_voxel_extents.z);

  const unsigned required_cache_size = volumeOf(glm::ivec3(1, 1, 1) + 2 * region_padding);
  // Must be able to support uploading 1 region plus enough padding regions to complete the query.
  if (max_batch_size < required_cache_size)
  {
    std::cerr << "RoiRangeFill: Gpu cache too small. Required " << required_cache_size
              << " supported: " << max_batch_size << std::endl;
    return false;
  }

  PROFILE_END(prime);
  PROFILE(gpuExec);
  std::vector<gputil::Event> upload_events;
  // Iterate the region grid, uploading the batch.
  const glm::ivec3 region_min = glm::ivec3(region_key) - region_padding;
  const glm::ivec3 region_max = glm::ivec3(region_key) + region_padding;

  // const glm::ivec3 batchMin(x, y, z);
  // const glm::ivec3 batchMax = batchMin + regionStep - glm::ivec3(1);
  // const unsigned occupancy_batch_marker = occupancy_cache->beginBatch();
  unsigned clearance_batch_marker = clearance_cache->beginBatch();

  // Set up the key marking the lower corner of the working group.
  const Key corner_voxel_key(region_key, glm::u8vec3(0));
  const GpuKey gpu_key = { corner_voxel_key.regionKey().x, corner_voxel_key.regionKey().y,
                           corner_voxel_key.regionKey().z, corner_voxel_key.localKey().x,
                           corner_voxel_key.localKey().y,  corner_voxel_key.localKey().z };
  gpu_corner_voxel_key_.write(&gpu_key, sizeof(gpu_key));

  PROFILE(upload);
  // Prepare region key and offset buffers.
  // Size the region buffers.
  gpu_region_keys_.elementsResize<gputil::int3>(volumeOf(glm::ivec3(1) + 2 * region_padding));
  gpu_occupancy_region_offsets_.elementsResize<uint64_t>(volumeOf(glm::ivec3(1) + 2 * region_padding));

  gpu_region_clearance_buffer_.resize(
    map.layout().layer(clearance_layer_index).layerByteSize(map.regionVoxelDimensions()));

  gputil::PinnedBuffer region_keys(gpu_region_keys_, gputil::kPinWrite);
  gputil::PinnedBuffer occupancy_region_offsets(gpu_occupancy_region_offsets_, gputil::kPinWrite);
  region_count_ = 0;

  for (int z = region_min.z; z <= region_max.z; ++z)
  {
    for (int y = region_min.y; y <= region_max.y; ++y)
    {
      for (int x = region_min.x; x <= region_max.x; ++x)
      {
        const glm::ivec3 current_region_coord = glm::ivec3(x, y, z);
        MapChunk *chunk = nullptr;
        gputil::Event upload_event;
        GpuLayerCache::CacheStatus clearance_status;

        const unsigned gpu_cache_flags = GpuLayerCache::kSkipDownload;

        // Copy region occupancy voxels into the clearanceCache. This may come from either
        // a. The occupancyCache
        // b. Main memory.
        uint64_t clearance_mem_offset = 0u;
        size_t occupancy_mem_offset = 0u;

        gputil::Event occupancy_event;
        if (occupancy_cache->lookup(map, current_region_coord, &occupancy_mem_offset, &occupancy_event))
        {
          // Queue copying from occupancy cache.
          clearance_mem_offset = clearance_cache->allocate(map, current_region_coord, chunk, nullptr, &clearance_status,
                                                           clearance_batch_marker, gpu_cache_flags);

          gputil::Buffer *occupancy_buffer = occupancy_cache->buffer();
          gputil::Buffer *clearance_buffer = clearance_cache->buffer();

          gputil::copyBuffer(
            *clearance_buffer, clearance_mem_offset, *occupancy_buffer, occupancy_mem_offset,
            map.layout().layer(map.layout().occupancyLayer()).layerByteSize(map.regionVoxelDimensions()),
            &clearance_cache->gpuQueue(), &occupancy_event, &upload_event);
          clearance_cache->updateEvent(*chunk, upload_event);
        }
        else
        {
          // Copy from main memory.
          clearance_mem_offset = clearance_cache->upload(map, current_region_coord, chunk, &upload_event,
                                                         &clearance_status, clearance_batch_marker, gpu_cache_flags);
        }
        assert(clearance_status != GpuLayerCache::kCacheFull);

        if (clearance_status != GpuLayerCache::kCacheFull)
        {
          upload_events.push_back(upload_event);
          region_keys.write(glm::value_ptr(current_region_coord), sizeof(current_region_coord),
                            region_count_ * sizeof(gputil::int3));
          occupancy_region_offsets.write(&clearance_mem_offset, sizeof(clearance_mem_offset),
                                         region_count_ * sizeof(uint64_t));
          ++region_count_;
        }
        else
        {
          std::cerr << "RoiRangeFill: GPU cache full. Results invalid.\n" << std::flush;
          return false;
        }
      }
    }
  }

  PROFILE_END(upload)

  // All regions for this patch pushed. Make the calculation.
  // TODO(KS): async unpin.
  region_keys.unpin();
  occupancy_region_offsets.unpin();
  finishRegion(region_key, map, *this, *gpu_cache, *clearance_cache, batch_voxel_extents, upload_events);
  upload_events.clear();

  return true;
}


void RoiRangeFill::cacheGpuProgram(bool force)
{
  if (!force && program_ref_ != nullptr)
  {
    // Already loaded.
    return;
  }

  releaseGpuProgram();

  program_ref_ = &g_program_ref;
  if (program_ref_->addReference(gpu_))
  {
    seed_kernel_ = GPUTIL_MAKE_KERNEL(program_ref_->program(), seedRegionVoxels);
    seed_outer_kernel_ = GPUTIL_MAKE_KERNEL(program_ref_->program(), seedFromOuterRegions);
    propagate_kernel_ = GPUTIL_MAKE_KERNEL(program_ref_->program(), propagateObstacles);
    migrate_kernel_ = GPUTIL_MAKE_KERNEL(program_ref_->program(), migrateResults);

    if (!seed_kernel_.isValid() || !seed_outer_kernel_.isValid() || !propagate_kernel_.isValid() ||
        !migrate_kernel_.isValid())
    {
      releaseGpuProgram();
    }
    else
    {
      seed_kernel_.calculateOptimalWorkGroupSize();
      seed_outer_kernel_.calculateOptimalWorkGroupSize();
      // Add local voxels cache.
      propagate_kernel_.addLocal([](size_t workgroup_size) {
        // Convert workgroupSize to a cubic dimension (conservatively) then add
        // padding of 1 either size. This forms the actual size, but ends up being
        // a conservative estimate of the memory requirement.
        const size_t cubic_size = size_t(std::ceil(std::pow(double(workgroup_size), 1.0 / 3.0))) + 2;
        return sizeof(gputil::char4) * cubic_size * cubic_size * cubic_size;
      });

      propagate_kernel_.calculateOptimalWorkGroupSize();

      migrate_kernel_.calculateOptimalWorkGroupSize();
    }
  }
}


void RoiRangeFill::releaseGpuProgram()
{
  seed_kernel_ = gputil::Kernel();
  seed_outer_kernel_ = gputil::Kernel();
  propagate_kernel_ = gputil::Kernel();
  migrate_kernel_ = gputil::Kernel();

  if (program_ref_)
  {
    program_ref_->releaseReference();
  }
}


void RoiRangeFill::finishRegion(const glm::i16vec3 &region_key, OccupancyMap &map, RoiRangeFill &query,
                                GpuCache &gpu_cache, GpuLayerCache &clearance_cache,
                                const glm::ivec3 &batch_voxel_extents, const std::vector<gputil::Event> &upload_events)
{
  PROFILE(finishRegion);

  PROFILE(invoke);
  // Ensure sufficient working voxel memory size.
  for (int i = 0; i < 2; ++i)
  {
    query.gpuWork(i).elementsResize<gputil::char4>(volumeOf(batch_voxel_extents));
  }

  invoke(*map.detail(), query, gpu_cache, clearance_cache, batch_voxel_extents, upload_events);
  PROFILE_END(invoke);

  PROFILE(download);
  // Download back to main memory.
  MapChunk *region = map.region(region_key);
  if (region)
  {
    gputil::PinnedBuffer clearance_buffer(query.gpuRegionClearanceBuffer(), gputil::kPinRead);
    VoxelBuffer<VoxelBlock> voxels(region->voxel_blocks[map.layout().clearanceLayer()]);
    uint8_t *dst = voxels.voxelMemory();
    clearance_buffer.read(dst, voxels.voxelMemorySize());
  }
  PROFILE_END(download);
}


int RoiRangeFill::invoke(const OccupancyMapDetail &map, RoiRangeFill &query, GpuCache &gpu_cache,
                         GpuLayerCache &clearance_layer_cache, const glm::ivec3 &input_data_extents,
                         const std::vector<gputil::Event> &upload_events)
{
  int err = 0;

  // zbatch: how do we batch layers in Z to increase work per thread?
  const int zbatch = 1;  // std::max<int>(map.regionVoxelDimensions.z, 32);

  // Convert to CL types and inputs.
  // Region voxel dimensions
  const gputil::int3 region_voxel_extents_gpu = { map.region_voxel_dimensions.x, map.region_voxel_dimensions.y,
                                                  map.region_voxel_dimensions.z };
  // Padding voxel extents from ROI.
  const gputil::int3 padding_gpu = { (input_data_extents.x - map.region_voxel_dimensions.x) / 2,
                                     (input_data_extents.y - map.region_voxel_dimensions.y) / 2,
                                     (input_data_extents.z - map.region_voxel_dimensions.z) / 2 };

  gputil::float3 axis_scaling_gpu = { query.axisScaling().x, query.axisScaling().y, query.axisScaling().z };

  gputil::Queue &queue = gpu_cache.gpuQueue();

  // Select kernels based on voxel mean usage or not.
  cacheGpuProgram(false);

  PROFILE(seed);
  // For now just wait on the sync events here.
  // The alternative is to repack the events into kernelEvents via a cl::Event conversion.
  // Ultimately, I need to remove the use of the C++ OpenCL wrapper if I'm using gputil.
  gputil::Event::wait(upload_events.data(), upload_events.size());

  gputil::Event seed_kernel_event;
  gputil::Event seed_outer_kernel_event;

  unsigned kernel_algorithm_flags = 0;
  if (query.queryFlags() & kQfUnknownAsOccupied)
  {
    kernel_algorithm_flags |= 1U;
  }

  int src_buffer_index = 0;
  // Initial seeding is just a single region extents in X/Y, with Z divided by the batch size (round up to ensure
  // coverage).
  const gputil::Dim3 seed_grid(size_t(region_voxel_extents_gpu.x), size_t(region_voxel_extents_gpu.y),
                               size_t((region_voxel_extents_gpu.z + zbatch - 1) / zbatch));

  gputil::Dim3 global_size;
  gputil::Dim3 local_size;
  seed_kernel_.calculateGrid(&global_size, &local_size, seed_grid);

  err = seed_kernel_(
    global_size, local_size, seed_kernel_event, &queue,
    // Kernel arguments
    gputil::BufferArg<GpuKey>(gpu_corner_voxel_key_), gputil::BufferArg<float>(*clearance_layer_cache.buffer()),
    gputil::BufferArg<gputil::char4>(query.gpuWork(src_buffer_index)),
    gputil::BufferArg<gputil::int3>(query.gpuRegionKeys()),
    gputil::BufferArg<uint64_t>(query.gpuOccupancyRegionOffsets()), query.regionCount(), region_voxel_extents_gpu,
    region_voxel_extents_gpu, float(map.occupancy_threshold_value), kernel_algorithm_flags, zbatch);
  if (err)
  {
    return err;
  }

  // Seed from data outside of the ROI.
  const int seed_outer_batch = 32;
  const size_t padding_volume = volumeOf(input_data_extents) - volumeOf(map.region_voxel_dimensions);

  global_size = gputil::Dim3((padding_volume + seed_outer_batch - 1) / seed_outer_batch);
  local_size = gputil::Dim3(256);  // NOLINT(readability-magic-numbers)

  err = seed_outer_kernel_(
    global_size, local_size, gputil::EventList({ seed_kernel_event }), seed_outer_kernel_event, &queue,
    // Kernel arguments
    gputil::BufferArg<GpuKey>(query.gpuCornerVoxelKey()), gputil::BufferArg<float *>(*clearance_layer_cache.buffer()),
    gputil::BufferArg<gputil::char4>(query.gpuWork(src_buffer_index)),
    gputil::BufferArg<gputil::int3>(query.gpuRegionKeys()),
    gputil::BufferArg<uint64_t>(query.gpuOccupancyRegionOffsets()), query.regionCount(), region_voxel_extents_gpu,
    region_voxel_extents_gpu, padding_gpu, axis_scaling_gpu, float(map.occupancy_threshold_value),
    kernel_algorithm_flags, seed_outer_batch);

  if (err)
  {
    return err;
  }

  queue.flush();

  // #ifdef OHM_PROFILE
  //   seed_kernel_event.wait();
  // #endif // OHM_PROFILE
  PROFILE_END(seed);

  PROFILE(propagate);

  propagate_kernel_.calculateGrid(
    &global_size, &local_size,
    gputil::Dim3(region_voxel_extents_gpu.x, region_voxel_extents_gpu.y, region_voxel_extents_gpu.z));

  gputil::Event previous_event = seed_outer_kernel_event;
  gputil::Event propagate_event;

  const int propagation_iterations = int(std::ceil(query.searchRadius() / map.resolution));
  logutil::trace("Iterations: ", propagation_iterations, '\n');
  for (int i = 0; i < propagation_iterations; ++i)
  {
    err = propagate_kernel_(global_size, local_size, { previous_event }, propagate_event, &queue,
                            // Kernel args
                            gputil::BufferArg<gputil::char4>(query.gpuWork(src_buffer_index)),
                            gputil::BufferArg<gputil::char4>(query.gpuWork(1 - src_buffer_index)),
                            region_voxel_extents_gpu, float(query.searchRadius()), axis_scaling_gpu
                            // , __local char4 *localVoxels
    );

    if (err)
    {
      return err;
    }

    previous_event = propagate_event;
    src_buffer_index = 1 - src_buffer_index;

    queue.flush();
  }

  // #ifdef OHM_PROFILE
  //   previous_event.wait();
  // #endif // OHM_PROFILE

  PROFILE_END(propagate);
  if (query.queryFlags() & kQfReportUnscaledResults)
  {
    axis_scaling_gpu = { 1, 1, 1 };
  }

  PROFILE(migrate);

  // Only queue migration kernel for the target region.
  migrate_kernel_.calculateGrid(
    &global_size, &local_size,
    gputil::Dim3(region_voxel_extents_gpu.x, region_voxel_extents_gpu.y, region_voxel_extents_gpu.z));

  gputil::Event migrate_event;
  err = migrate_kernel_(global_size, local_size, gputil::EventList({ previous_event }), migrate_event, &queue,
                        // Kernel args
                        gputil::BufferArg<gputil::char4>(query.gpuRegionClearanceBuffer()),
                        gputil::BufferArg<gputil::char4>(query.gpuWork(src_buffer_index)), region_voxel_extents_gpu,
                        region_voxel_extents_gpu, float(query.searchRadius()), float(map.resolution), axis_scaling_gpu,
                        unsigned(query.queryFlags()));

  if (err)
  {
    return err;
  }

  queue.flush();

  previous_event = migrate_event;
  previous_event.wait();
  PROFILE_END(migrate);

  queue.finish();

  return err;
}
}  // namespace ohm


================================================
FILE: ohmgpu/private/RoiRangeFill.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMGPU_ROIRANGEFILL_H
#define OHMGPU_ROIRANGEFILL_H

#include "OhmGpuConfig.h"

#include <glm/glm.hpp>

#include <gputil/gpuBuffer.h>
#include <gputil/gpuDevice.h>
#include <gputil/gpuKernel.h>

#include <array>

namespace ohm
{
class OccupancyMap;
struct OccupancyMapDetail;
class GpuCache;
class GpuLayerCache;
class GpuProgramRef;

/// GPU algorithm to calculate per voxel clearance values (range to nearest obstruction).
class ohmgpu_API RoiRangeFill
{
public:
  explicit RoiRangeFill(gputil::Device &gpu);
  ~RoiRangeFill();

  gputil::Buffer &gpuCornerVoxelKey() { return gpu_corner_voxel_key_; }
  const gputil::Buffer &gpuCornerVoxelKey() const { return gpu_corner_voxel_key_; }

  gputil::Buffer &gpuRegionKeys() { return gpu_region_keys_; }
  const gputil::Buffer &gpuRegionKeys() const { return gpu_region_keys_; }

  gputil::Buffer &gpuOccupancyRegionOffsets() { return gpu_occupancy_region_offsets_; }
  const gputil::Buffer &gpuOccupancyRegionOffsets() const { return gpu_occupancy_region_offsets_; }

  gputil::Buffer &gpuRegionClearanceBuffer() { return gpu_region_clearance_buffer_; }
  const gputil::Buffer &gpuRegionClearanceBuffer() const { return gpu_region_clearance_buffer_; }

  gputil::Buffer &gpuWork(int buffer) { return gpu_work_[buffer]; }
  const gputil::Buffer &gpuWork(int buffer) const { return gpu_work_[buffer]; }

  const gputil::Device &gpu() const { return gpu_; }

  const glm::vec3 &axisScaling() const { return axis_scaling_; }
  void setAxisScaling(const glm::vec3 &scaling) { axis_scaling_ = scaling; }

  float searchRadius() const { return search_radius_; }
  void setSearchRadius(float radius) { search_radius_ = radius; }

  unsigned queryFlags() const { return query_flags_; }
  void setQueryFlags(unsigned flags) { query_flags_ = flags; }

  unsigned regionCount() const { return region_count_; }
  // void setRegionCount(unsigned count) { region_count_ = count; }

  bool calculateForRegion(OccupancyMap &map, const glm::i16vec3 &region_key);

private:
  void cacheGpuProgram(bool force);
  void releaseGpuProgram();

  void finishRegion(const glm::i16vec3 &region_key, OccupancyMap &map, RoiRangeFill &query, GpuCache &gpu_cache,
                    GpuLayerCache &clearance_cache, const glm::ivec3 &batch_voxel_extents,
                    const std::vector<gputil::Event> &upload_events);

  int invoke(const OccupancyMapDetail &map, RoiRangeFill &query, GpuCache &gpu_cache,
             GpuLayerCache &clearance_layer_cache, const glm::ivec3 &input_data_extents,
             const std::vector<gputil::Event> &upload_events);

  /// Key for the lower extents corner of the global work group. All other GPU threads can resolve their key by
  /// adjusting this key using their 3D global ID.
  gputil::Buffer gpu_corner_voxel_key_;
  /// Array of keys of the uploaded regions. May be used to resolve indexing to regions and vise-versa.
  gputil::Buffer gpu_region_keys_;
  /// Memory offsets into the GPU cache memory holding voxel occupancy values. Order matches @c gpu_region_keys_.
  gputil::Buffer gpu_occupancy_region_offsets_;
  /// Buffer holding the clearance values for the region we are working on.
  gputil::Buffer gpu_region_clearance_buffer_;
  /// Buffer of int4 used to propagate obstacles.
  std::array<gputil::Buffer, 2> gpu_work_;
  gputil::Device gpu_;
  gputil::Kernel seed_kernel_;
  gputil::Kernel seed_outer_kernel_;
  gputil::Kernel propagate_kernel_;
  gputil::Kernel migrate_kernel_;
  GpuProgramRef *program_ref_ = nullptr;
  glm::vec3 axis_scaling_ = glm::vec3(1.0f);
  float search_radius_ = 0.0f;
  unsigned query_flags_ = 0;
  unsigned region_count_ = 0;
};
}  // namespace ohm

#endif  // OHMGPU_ROIRANGEFILL_H


================================================
FILE: ohmheightmap/3rdparty/delaunator.hpp
================================================

// MIT License
//
// Copyright (c) 2018 Volodymyr Bilonenko
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
// Original source:
// https://github.com/delfrrr/delaunator-cpp @ v0.4.0
// clang-format off

#pragma once

#include <algorithm>
#include <cmath>
#include <exception>
#include <iostream>
#include <limits>
#include <memory>
#include <utility>
#include <vector>
#include <tuple>

namespace delaunator {

// Kahan and Babuska summation, Neumaier variant; accumulates less FP error
inline double sum(const std::vector<double>& x) {
    double sum = x[0];
    double err = 0.0;

    for (size_t i = 1; i < x.size(); i++) {
        const double k = x[i];
        const double m = sum + k;
        err += std::fabs(sum) >= std::fabs(k) ? sum - m + k : k - m + sum;
        sum = m;
    }
    return sum + err;
}

inline double dist(
    const double ax,
    const double ay,
    const double bx,
    const double by) {
    const double dx = ax - bx;
    const double dy = ay - by;
    return dx * dx + dy * dy;
}

inline double circumradius(
    const double ax,
    const double ay,
    const double bx,
    const double by,
    const double cx,
    const double cy) {
    const double dx = bx - ax;
    const double dy = by - ay;
    const double ex = cx - ax;
    const double ey = cy - ay;

    const double bl = dx * dx + dy * dy;
    const double cl = ex * ex + ey * ey;
    const double d = dx * ey - dy * ex;

    const double x = (ey * bl - dy * cl) * 0.5 / d;
    const double y = (dx * cl - ex * bl) * 0.5 / d;

    if ((bl > 0.0 || bl < 0.0) && (cl > 0.0 || cl < 0.0) && (d > 0.0 || d < 0.0)) {
        return x * x + y * y;
    } else { // NOLINT(readability-else-after-return)
        return std::numeric_limits<double>::max();
    }
}

inline bool orient(
    const double px,
    const double py,
    const double qx,
    const double qy,
    const double rx,
    const double ry) {
    return (qy - py) * (rx - qx) - (qx - px) * (ry - qy) < 0.0;
}

inline std::pair<double, double> circumcenter(
    const double ax,
    const double ay,
    const double bx,
    const double by,
    const double cx,
    const double cy) {
    const double dx = bx - ax;
    const double dy = by - ay;
    const double ex = cx - ax;
    const double ey = cy - ay;

    const double bl = dx * dx + dy * dy;
    const double cl = ex * ex + ey * ey;
    const double d = dx * ey - dy * ex;

    const double x = ax + (ey * bl - dy * cl) * 0.5 / d;
    const double y = ay + (dx * cl - ex * bl) * 0.5 / d;

    return std::make_pair(x, y);
}

inline double compare(
    std::vector<double> const& coords,
    std::size_t i,
    std::size_t j,
    double cx,
    double cy) {
    const double d1 = dist(coords[2 * i], coords[2 * i + 1], cx, cy);
    const double d2 = dist(coords[2 * j], coords[2 * j + 1], cx, cy);
    const double diff1 = d1 - d2;
    const double diff2 = coords[2 * i] - coords[2 * j];
    const double diff3 = coords[2 * i + 1] - coords[2 * j + 1];

    if (diff1 > 0.0 || diff1 < 0.0) {
        return diff1;
    } else if (diff2 > 0.0 || diff2 < 0.0) { // NOLINT(readability-else-after-return)
        return diff2;
    } else {
        return diff3;
    }
}

struct sort_to_center { // NOLINT(readability-identifier-naming)

    std::vector<double> const& coords;
    double cx;
    double cy;

    bool operator()(std::size_t i, std::size_t j) {
        return compare(coords, i, j, cx, cy) < 0;
    }
};

inline bool in_circle( // NOLINT(readability-identifier-naming)
    double ax,
    double ay,
    double bx,
    double by,
    double cx,
    double cy,
    double px,
    double py) {
    const double dx = ax - px;
    const double dy = ay - py;
    const double ex = bx - px;
    const double ey = by - py;
    const double fx = cx - px;
    const double fy = cy - py;

    const double ap = dx * dx + dy * dy;
    const double bp = ex * ex + ey * ey;
    const double cp = fx * fx + fy * fy;

    return (dx * (ey * cp - bp * fy) -
            dy * (ex * cp - bp * fx) +
            ap * (ex * fy - ey * fx)) < 0.0;
}

constexpr double EPSILON = std::numeric_limits<double>::epsilon();
constexpr std::size_t INVALID_INDEX = std::numeric_limits<std::size_t>::max(); // NOLINT(readability-identifier-naming)

inline bool check_pts_equal(double x1, double y1, double x2, double y2) { // NOLINT(readability-identifier-naming)
    return std::fabs(x1 - x2) <= EPSILON &&
           std::fabs(y1 - y2) <= EPSILON;
}

// monotonically increases with real angle, but doesn't need expensive trigonometry
inline double pseudo_angle(double dx, double dy) { // NOLINT(readability-identifier-naming)
    const double p = dx / (std::abs(dx) + std::abs(dy));
    return (dy > 0.0 ? 3.0 - p : 1.0 + p) / 4.0; // [0..1)
}

struct DelaunatorPoint {
    std::size_t i;
    double x;
    double y;
    std::size_t t;
    std::size_t prev;
    std::size_t next;
    bool removed;
};

class Delaunator {

public:
    std::vector<double> const& coords;
    std::vector<std::size_t> triangles;
    std::vector<std::size_t> halfedges;
    std::vector<std::size_t> hull_prev;
    std::vector<std::size_t> hull_next;
    std::vector<std::size_t> hull_tri;
    std::size_t hull_start;

    Delaunator(std::vector<double> const& in_coords);

    double get_hull_area(); // NOLINT(readability-identifier-naming)

private:
    std::vector<std::size_t> m_hash; // NOLINT(readability-identifier-naming)
    double m_center_x; // NOLINT(readability-identifier-naming)
    double m_center_y; // NOLINT(readability-identifier-naming)
    std::size_t m_hash_size; // NOLINT(readability-identifier-naming)

    std::size_t legalize(std::size_t a);
    std::size_t hash_key(double x, double y); // NOLINT(readability-identifier-naming)
    std::size_t add_triangle( // NOLINT(readability-identifier-naming)
        std::size_t i0,
        std::size_t i1,
        std::size_t i2,
        std::size_t a,
        std::size_t b,
        std::size_t c);
    void link(std::size_t a, std::size_t b);
};

Delaunator::Delaunator(std::vector<double> const& in_coords)
    : coords(in_coords),
      triangles(), // NOLINT(readability-redundant-member-init)
      halfedges(), // NOLINT(readability-redundant-member-init)
      hull_prev(), // NOLINT(readability-redundant-member-init)
      hull_next(), // NOLINT(readability-redundant-member-init)
      hull_tri(), // NOLINT(readability-redundant-member-init)
      hull_start(),
      m_hash(), // NOLINT(readability-redundant-member-init)
      m_center_x(),
      m_center_y(),
      m_hash_size() {
    std::size_t n = coords.size() >> 1;

    double max_x = std::numeric_limits<double>::min();
    double max_y = std::numeric_limits<double>::min();
    double min_x = std::numeric_limits<double>::max();
    double min_y = std::numeric_limits<double>::max();
    std::vector<std::size_t> ids;
    ids.reserve(n);

    for (std::size_t i = 0; i < n; i++) {
        const double x = coords[2 * i];
        const double y = coords[2 * i + 1];

        if (x < min_x) min_x = x; // NOLINT(readability-braces-around-statements)
        if (y < min_y) min_y = y; // NOLINT(readability-braces-around-statements)
        if (x > max_x) max_x = x; // NOLINT(readability-braces-around-statements)
        if (y > max_y) max_y = y; // NOLINT(readability-braces-around-statements)

        ids.push_back(i);
    }
    const double cx = (min_x + max_x) / 2;
    const double cy = (min_y + max_y) / 2;
    double min_dist = std::numeric_limits<double>::max();

    std::size_t i0 = INVALID_INDEX;
    std::size_t i1 = INVALID_INDEX;
    std::size_t i2 = INVALID_INDEX;

    // pick a seed point close to the centroid
    for (std::size_t i = 0; i < n; i++) {
        const double d = dist(cx, cy, coords[2 * i], coords[2 * i + 1]);
        if (d < min_dist) {
            i0 = i;
            min_dist = d;
        }
    }

    const double i0x = coords[2 * i0];
    const double i0y = coords[2 * i0 + 1];

    min_dist = std::numeric_limits<double>::max();

    // find the point closest to the seed
    for (std::size_t i = 0; i < n; i++) {
        if (i == i0) continue; // NOLINT(readability-braces-around-statements)
        const double d = dist(i0x, i0y, coords[2 * i], coords[2 * i + 1]);
        if (d < min_dist && d > 0.0) {
            i1 = i;
            min_dist = d;
        }
    }

    double i1x = coords[2 * i1];
    double i1y = coords[2 * i1 + 1];

    double min_radius = std::numeric_limits<double>::max();

    // find the third point which forms the smallest circumcircle with the first two
    for (std::size_t i = 0; i < n; i++) {
        if (i == i0 || i == i1) continue; // NOLINT(readability-braces-around-statements)

        const double r = circumradius(
            i0x, i0y, i1x, i1y, coords[2 * i], coords[2 * i + 1]);

        if (r < min_radius) {
            i2 = i;
            min_radius = r;
        }
    }

    if (!(min_radius < std::numeric_limits<double>::max())) {
        throw std::runtime_error("not triangulation");
    }

    double i2x = coords[2 * i2];
    double i2y = coords[2 * i2 + 1];

    if (orient(i0x, i0y, i1x, i1y, i2x, i2y)) {
        std::swap(i1, i2);
        std::swap(i1x, i2x);
        std::swap(i1y, i2y);
    }

    std::tie(m_center_x, m_center_y) = circumcenter(i0x, i0y, i1x, i1y, i2x, i2y);

    // sort the points by distance from the seed triangle circumcenter
    std::sort(ids.begin(), ids.end(), sort_to_center{ coords, m_center_x, m_center_y });

    // initialize a hash table for storing edges of the advancing convex hull
    m_hash_size = static_cast<std::size_t>(std::llround(std::ceil(std::sqrt(n))));
    m_hash.resize(m_hash_size);
    std::fill(m_hash.begin(), m_hash.end(), INVALID_INDEX);

    // initialize arrays for tracking the edges of the advancing convex hull
    hull_prev.resize(n);
    hull_next.resize(n);
    hull_tri.resize(n);

    hull_start = i0;

    size_t hull_size = 3;

    hull_next[i0] = hull_prev[i2] = i1;
    hull_next[i1] = hull_prev[i0] = i2;
    hull_next[i2] = hull_prev[i1] = i0;

    hull_tri[i0] = 0;
    hull_tri[i1] = 1;
    hull_tri[i2] = 2;

    m_hash[hash_key(i0x, i0y)] = i0;
    m_hash[hash_key(i1x, i1y)] = i1;
    m_hash[hash_key(i2x, i2y)] = i2;

    std::size_t max_triangles = n < 3 ? 1 : 2 * n - 5;
    triangles.reserve(max_triangles * 3);
    halfedges.reserve(max_triangles * 3);
    add_triangle(i0, i1, i2, INVALID_INDEX, INVALID_INDEX, INVALID_INDEX);
    double xp = std::numeric_limits<double>::quiet_NaN();
    double yp = std::numeric_limits<double>::quiet_NaN();
    for (std::size_t k = 0; k < n; k++) {
        const std::size_t i = ids[k];
        const double x = coords[2 * i];
        const double y = coords[2 * i + 1];

        // skip near-duplicate points
        if (k > 0 && check_pts_equal(x, y, xp, yp)) continue; // NOLINT(readability-braces-around-statements)
        xp = x;
        yp = y;

        // skip seed triangle points
        if (
            check_pts_equal(x, y, i0x, i0y) ||
            check_pts_equal(x, y, i1x, i1y) ||
            check_pts_equal(x, y, i2x, i2y)) continue; // NOLINT(readability-braces-around-statements)

        // find a visible edge on the convex hull using edge hash
        std::size_t start = 0;

        size_t key = hash_key(x, y);
        for (size_t j = 0; j < m_hash_size; j++) {
            start = m_hash[(key + j) % m_hash_size];
            if (start != INVALID_INDEX && start != hull_next[start]) break; // NOLINT(readability-braces-around-statements)
        }

        start = hull_prev[start];
        size_t e = start;
        size_t q;

        while (q = hull_next[e], !orient(x, y, coords[2 * e], coords[2 * e + 1], coords[2 * q], coords[2 * q + 1])) { //TODO: does it works in a same way as in JS
            e = q;
            if (e == start) {
                e = INVALID_INDEX;
                break;
            }
        }

         // NOLINTNEXTLINE(readability-braces-around-statements)
        if (e == INVALID_INDEX) continue; // likely a near-duplicate point; skip it

        // add the first triangle from the point
        std::size_t t = add_triangle(
            e,
            i,
            hull_next[e],
            INVALID_INDEX,
            INVALID_INDEX,
            hull_tri[e]);

        hull_tri[i] = legalize(t + 2);
        hull_tri[e] = t;
        hull_size++;

        // walk forward through the hull, adding more triangles and flipping recursively
        std::size_t next = hull_next[e];
        while (
            q = hull_next[next],
            orient(x, y, coords[2 * next], coords[2 * next + 1], coords[2 * q], coords[2 * q + 1])) {
            t = add_triangle(next, i, q, hull_tri[i], INVALID_INDEX, hull_tri[next]);
            hull_tri[i] = legalize(t + 2);
            hull_next[next] = next; // mark as removed
            hull_size--;
            next = q;
        }

        // walk backward from the other side, adding more triangles and flipping
        if (e == start) {
            while (
                q = hull_prev[e],
                orient(x, y, coords[2 * q], coords[2 * q + 1], coords[2 * e], coords[2 * e + 1])) {
                t = add_triangle(q, i, e, INVALID_INDEX, hull_tri[e], hull_tri[q]);
                legalize(t + 2);
                hull_tri[q] = t;
                hull_next[e] = e; // mark as removed
                hull_size--;
                e = q;
            }
        }

        // update the hull indices
        hull_prev[i] = e;
        hull_start = e;
        hull_prev[next] = i;
        hull_next[e] = i;
        hull_next[i] = next;

        m_hash[hash_key(x, y)] = i;
        m_hash[hash_key(coords[2 * e], coords[2 * e + 1])] = e;
    }
}

double Delaunator::get_hull_area() { // NOLINT(readability-identifier-naming)
    std::vector<double> hull_area;
    size_t e = hull_start;
    do {
        hull_area.push_back((coords[2 * e] - coords[2 * hull_prev[e]]) * (coords[2 * e + 1] + coords[2 * hull_prev[e] + 1]));
        e = hull_next[e];
    } while (e != hull_start);
    return sum(hull_area);
}

std::size_t Delaunator::legalize(std::size_t a) {
    const std::size_t b = halfedges[a];

    /* if the pair of triangles doesn't satisfy the Delaunay condition
    * (p1 is inside the circumcircle of [p0, pl, pr]), flip them,
    * then do the same check/flip recursively for the new pair of triangles
    *
    *           pl                    pl
    *          /||\                  /  \
    *       al/ || \bl            al/    \a
    *        /  ||  \              /      \
    *       /  a||b  \    flip    /___ar___\
    *     p0\   ||   /p1   =>   p0\---bl---/p1
    *        \  ||  /              \      /
    *       ar\ || /br             b\    /br
    *          \||/                  \  /
    *           pr                    pr
    */
    const std::size_t a0 = a - a % 3;
    const std::size_t b0 = b - b % 3;

    const std::size_t al = a0 + (a + 1) % 3;
    const std::size_t ar = a0 + (a + 2) % 3;
    const std::size_t bl = b0 + (b + 2) % 3;

    const std::size_t p0 = triangles[ar];
    const std::size_t pr = triangles[a];
    const std::size_t pl = triangles[al];
    const std::size_t p1 = triangles[bl];

    if (b == INVALID_INDEX) {
        return ar;
    }

    const bool illegal = in_circle(
        coords[2 * p0],
        coords[2 * p0 + 1],
        coords[2 * pr],
        coords[2 * pr + 1],
        coords[2 * pl],
        coords[2 * pl + 1],
        coords[2 * p1],
        coords[2 * p1 + 1]);

    if (illegal) {
        triangles[a] = p1;
        triangles[b] = p0;

        auto hbl = halfedges[bl];

        // edge swapped on the other side of the hull (rare); fix the halfedge reference
        if (hbl == INVALID_INDEX) {
            std::size_t e = hull_start;
            do {
                if (hull_tri[e] == bl) {
                    hull_tri[e] = a;
                    break;
                }
                e = hull_next[e];
            } while (e != hull_start);
        }
        link(a, hbl);
        link(b, halfedges[ar]);
        link(ar, bl);

        std::size_t br = b0 + (b + 1) % 3;

        legalize(a);
        return legalize(br);
    }
    return ar;
}

std::size_t Delaunator::hash_key(double x, double y) { // NOLINT(readability-identifier-naming)
    const double dx = x - m_center_x;
    const double dy = y - m_center_y;
    return static_cast<std::size_t>(std::llround(
               std::floor(pseudo_angle(dx, dy) * static_cast<double>(m_hash_size)))) %
           m_hash_size;
}

std::size_t Delaunator::add_triangle( // NOLINT(readability-identifier-naming)
    std::size_t i0,
    std::size_t i1,
    std::size_t i2,
    std::size_t a,
    std::size_t b,
    std::size_t c) {
    std::size_t t = triangles.size();
    triangles.push_back(i0);
    triangles.push_back(i1);
    triangles.push_back(i2);
    link(t, a);
    link(t + 1, b);
    link(t + 2, c);
    return t;
}

void Delaunator::link(std::size_t a, std::size_t b) {
    std::size_t s = halfedges.size();
    if (a == s) {
        halfedges.push_back(b);
    } else if (a < s) {
        halfedges[a] = b;
    } else {
        throw std::runtime_error("Cannot link edge");
    }
    if (b != INVALID_INDEX) {
        std::size_t s2 = halfedges.size();
        if (b == s2) {
            halfedges.push_back(a);
        } else if (b < s2) {
            halfedges[b] = a;
        } else {
            throw std::runtime_error("Cannot link edge");
        }
    }
}

} //namespace delaunator

// clang-format on


================================================
FILE: ohmheightmap/CMakeLists.txt
================================================

include(GenerateExportHeader)
include(TextFileResource)


find_package(ZLIB)
if(OHM_FEATURE_THREADS)
  find_package(TBB)
endif(OHM_FEATURE_THREADS)

set(TES_ENABLE ${OHM_TES_DEBUG})
configure_file(OhmHeightmapConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/ohmheightmap/OhmHeightmapConfig.h")

set(SOURCES
  private/HeightmapDetail.cpp
  private/HeightmapDetail.h
  private/HeightmapOperations.cpp
  private/HeightmapOperations.h
  Heightmap.cpp
  Heightmap.h
  HeightmapMesh.cpp
  HeightmapMesh.h
  HeightmapMode.cpp
  HeightmapMode.h
  HeightmapSerialise.cpp
  HeightmapSerialise.h
  HeightmapUtil.cpp
  HeightmapUtil.h
  HeightmapVoxel.cpp
  HeightmapVoxel.h
  HeightmapVoxelType.h
  PlaneFillLayeredWalker.cpp
  PlaneFillLayeredWalker.h
  PlaneFillWalker.cpp
  PlaneFillWalker.h
  PlaneWalker.cpp
  PlaneWalker.h
  PlaneWalkVisitMode.h
  TriangleEdge.h
  TriangleNeighbours.h
  UpAxis.h
)

set(PUBLIC_HEADERS
  Heightmap.h
  HeightmapMesh.h
  HeightmapMode.h
  HeightmapSerialise.h
  HeightmapUtil.h
  HeightmapVoxel.h
  HeightmapVoxelType.h
  PlaneFillLayeredWalker.h
  PlaneFillWalker.h
  PlaneWalker.h
  PlaneWalkVisitMode.h
  TriangleEdge.h
  TriangleNeighbours.h
  UpAxis.h
  "${CMAKE_CURRENT_BINARY_DIR}/ohmheightmap/OhmHeightmapConfig.h"
  "${CMAKE_CURRENT_BINARY_DIR}/ohmheightmap/OhmHeightmapExport.h"
  )

add_library(ohmheightmap ${SOURCES})
clang_tidy_target(ohmheightmap)

target_link_libraries(ohmheightmap PUBLIC ohm ohmutil)

target_link_libraries(ohmheightmap
  PUBLIC
    glm::glm
    $<$<BOOL:${OHM_FEATURE_THREADS}>:TBB::tbb>
  PRIVATE
    $<BUILD_INTERFACE:$<$<BOOL:${OHM_FEATURE_EIGEN}>:Eigen3::Eigen>>
)

if(BUILD_SHARED)
  # Link additional dependencies when building shared ohm.
  # Because ohm is shared and these are private, static depenencies, we do not need to propagate
  # the dependencies and limit this with $<BUILD_INTERFACE:>
  target_link_libraries(ohmheightmap
    PRIVATE
      # Link 3es if enabled.
      $<BUILD_INTERFACE:$<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>>
      $<BUILD_INTERFACE:ZLIB::ZLIB>
  )
else(BUILD_SHARED)
  # With ohm static, we link depdencencies in such as way that the will propagate and need to be
  # found when ohm is linked.
  target_link_libraries(ohmheightmap
    PRIVATE
      # Link 3es if enabled.
      $<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>
      ZLIB::ZLIB
  )
endif(BUILD_SHARED)

target_include_directories(ohmheightmap
  PUBLIC
    $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/ohmheightmap>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
  PRIVATE
    $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}>
)

target_include_directories(ohmheightmap SYSTEM
  PRIVATE
    # Link static and header only dependencies as a private link target under the BUILD_INTERFACE. This imports inlcude
    # directories and links the libraries, but does not chain the dependency downstream.
    "${CMAKE_CURRENT_LIST_DIR}/3rdparty"
    $<$<NOT:$<BOOL:${OHM_TES_DEBUG}>>:${3ES_INCLUDE_DIRS}>
)

generate_export_header(ohmheightmap
  EXPORT_MACRO_NAME ohmheightmap_API
  EXPORT_FILE_NAME ohmheightmap/OhmHeightmapExport.h
  STATIC_DEFINE OHMHEIGHTMAP_STATIC)

install(TARGETS ohmheightmap EXPORT ${CMAKE_PROJECT_NAME}-config-targets
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/ohmheightmap
)

install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/ohmheightmap)

source_group("source" REGULAR_EXPRESSION ".*$")
source_group("source\\private" REGULAR_EXPRESSION "/private/.*$")
source_group("source\\serialise" REGULAR_EXPRESSION "/serialise/.*$")


================================================
FILE: ohmheightmap/Heightmap.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "Heightmap.h"

#include "private/HeightmapDetail.h"
#include "private/HeightmapOperations.h"

#include "HeightmapUtil.h"
#include "PlaneFillLayeredWalker.h"
#include "PlaneFillWalker.h"
#include "PlaneWalker.h"

#include <ohm/Aabb.h>
#include <ohm/CovarianceVoxel.h>
#include <ohm/DefaultLayer.h>
#include <ohm/MapChunk.h>
#include <ohm/MapCoord.h>
#include <ohm/MapInfo.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/Trace.h>

#include <glm/vec3.hpp>

// Include after GLM types for glm type streaming operators.
#include "ohmutil/GlmStream.h"

#define PROFILING 0
#include <ohmutil/Profile.h>

#include <3esservermacros.h>

#include <algorithm>
#include <cassert>
#include <cstring>
#include <iostream>
#include <sstream>
#include <string>

// Enable code to support breaking on a specific voxel.
#define HM_DEBUG_VOXEL 0

namespace ohm
{
namespace heightmap
{
/// Helper function for visiting a heightmap node. This expands into the neighbours as required and performs debug
/// rendering.
/// @param walker The class used to walk the heightmap region. Examples; @c PlaneWalker , @c PlanerFillWalker ,
///     @c PlanerFillLayeredWalker
/// @param imp Heightmap implementation.
/// @param walk_key The initial key extracted from the @c walker before calculating the candidate and/or ground keys.
/// @param candidate_key The initial ground candidate key calculated from @p walk_key as the starting point to search
///   for the @p ground_key . May be null.
/// @param ground_key The selected ground key. May be null.
template <typename Walker>
void onVisitWalker(Walker &walker, const HeightmapDetail &imp, const Key &walk_key, const Key &candidate_key,
                   const Key &ground_key)
{
  (void)candidate_key;  // Unused
  (void)imp;            // Unused without TES_ENABLE
  (void)walk_key;       // Unused without TES_ENABLE
  // Add neighbours for walking.
  std::array<Key, 8> neighbours;
  const auto mode = (!candidate_key.isNull()) ? PlaneWalkVisitMode::kAddUnvisitedNeighbours :
                                                PlaneWalkVisitMode::kAddUnvisitedColumnNeighbours;
  size_t added_count = walker.visit(ground_key, mode, neighbours);
  (void)added_count;  // Unused unless TES_ENABLE is defined.
#ifdef TES_ENABLE
  if (g_tes)
  {
    // Add the neighbours for debug visualisation
    KeyRange source_map_range;
    uint32_t voxel_id;
    imp.occupancy_map->calculateExtents(nullptr, nullptr, &source_map_range);
    for (size_t i = 0; i < added_count; ++i)
    {
      const Key &nkey = neighbours[i];
      voxel_id = uint32_t(source_map_range.indexOf(nkey));
      voxel_id |= kNeighbourIdMask;
      const glm::dvec3 pos = imp.occupancy_map->voxelCentreGlobal(nkey);
      tes::Box neighbour(tes::Id(voxel_id, kTcHmVisit), tes::Transform(tes::Vector3d(glm::value_ptr(pos)),
                                                                       tes::Vector3d(imp.heightmap->resolution())));
      neighbour.setColour(tes::Colour::Colours[tes::Colour::CornflowerBlue]).setWireframe(true);
      neighbour.setReplace(true);
      g_tes->create(neighbour);
    }

    // Delete the previous candidate voxel visualisation.
    voxel_id = uint32_t(source_map_range.indexOf(walk_key));
    voxel_id |= kNeighbourIdMask;
    g_tes->destroy(tes::Box(tes::Id(voxel_id, kTcHmVisit)));

    /// Visualise the candiate with a transient box (single frame)
    const glm::dvec3 pos = imp.occupancy_map->voxelCentreGlobal(walk_key);
    tes::Box candidate(tes::Id(0, kTcHmVisit),
                       tes::Transform(tes::Vector3d(glm::value_ptr(pos)), tes::Vector3d(imp.heightmap->resolution())));
    candidate.setColour(tes::Colour::Colours[tes::Colour::LightGoldenrodYellow]).setWireframe(true);
    g_tes->create(candidate);

    std::ostringstream info;
    info << "R" << walk_key.regionKey() << " L" << walk_key.localKey();
    const std::string str = info.str();
    tes::Text3D info_text(str.c_str(), tes::Id(0, kTcHmInfo), tes::Directional(tes::Vector3d(glm::value_ptr(pos))));
    info_text.setScreenFacing(true).setColour(candidate.colour());
    g_tes->create(info_text);
  }
#endif  // neighbours
}
}  // namespace heightmap


Heightmap::Heightmap()
  : Heightmap(0.2, 2.0, UpAxis::kZ)  // NOLINT(readability-magic-numbers)
{}


Heightmap::Heightmap(double grid_resolution, double min_clearance, UpAxis up_axis, unsigned region_size)
  : imp_(new HeightmapDetail)
{
  region_size = region_size ? region_size : kDefaultRegionSize;

  imp_->min_clearance = min_clearance;

  if (up_axis < UpAxis::kNegZ || up_axis > UpAxis::kZ)
  {
    std::cerr << "Unknown up axis ID: " << int(up_axis) << std::endl;
    up_axis = UpAxis::kZ;
  }

  // Cache the up axis normal.
  imp_->up_axis_id = up_axis;
  imp_->updateAxis();

  // Use an OccupancyMap to store grid cells. Each region is 1 voxel thick.
  glm::u8vec3 region_dim(region_size);
  region_dim[imp_->vertical_axis_index] = 1;
  // Note: Compression is disabled for the heightmap.
  imp_->heightmap = std::make_unique<OccupancyMap>(grid_resolution, region_dim, MapFlag::kNone);
  // The multilayer heightmap expects more entries. Default to having room for N layers per chunk.
  region_dim[imp_->vertical_axis_index] = 4;
  imp_->multilayer_heightmap = std::make_unique<OccupancyMap>(grid_resolution, region_dim);

  imp_->heightmap_voxel_layer = heightmap::setupHeightmap(*imp_->heightmap, *imp_);
  heightmap::setupHeightmap(*imp_->multilayer_heightmap, *imp_);
}


Heightmap::~Heightmap() = default;


void Heightmap::setOccupancyMap(const OccupancyMap *map)
{
  imp_->occupancy_map = map;
}


const OccupancyMap *Heightmap::occupancyMap() const
{
  return imp_->occupancy_map;
}


OccupancyMap &Heightmap::heightmap() const
{
  return *imp_->heightmap;
}


void Heightmap::setCeiling(double ceiling)
{
  imp_->ceiling = ceiling;
}


double Heightmap::ceiling() const
{
  return imp_->ceiling;
}


void Heightmap::setFloor(double floor)
{
  imp_->floor = floor;
}


double Heightmap::floor() const
{
  return imp_->floor;
}


void Heightmap::setMinClearance(double clearance)
{
  imp_->min_clearance = clearance;
}


double Heightmap::minClearance() const
{
  return imp_->min_clearance;
}


void Heightmap::setIgnoreVoxelMean(bool ignore)
{
  imp_->ignore_voxel_mean = ignore;
}


bool Heightmap::ignoreVoxelMean() const
{
  return imp_->ignore_voxel_mean;
}


void Heightmap::setGenerateVirtualSurface(bool enable)
{
  imp_->generate_virtual_surface = enable;
}


bool Heightmap::generateVirtualSurface() const
{
  return imp_->generate_virtual_surface;
}


void Heightmap::setPromoteVirtualBelow(bool enable)
{
  imp_->promote_virtual_below = enable;
}


bool Heightmap::promoteVirtualBelow() const
{
  return imp_->promote_virtual_below;
}


void Heightmap::setMode(HeightmapMode mode)
{
  imp_->mode = mode;
}


HeightmapMode Heightmap::mode() const
{
  return imp_->mode;
}


UpAxis Heightmap::upAxis() const
{
  return UpAxis(imp_->up_axis_id);
}


int Heightmap::upAxisIndex() const
{
  return imp_->vertical_axis_index;
}


const glm::dvec3 &Heightmap::upAxisNormal() const
{
  return imp_->up;
}


int Heightmap::surfaceAxisIndexA() const
{
  return HeightmapDetail::surfaceIndexA(imp_->up_axis_id);
}


const glm::dvec3 &Heightmap::surfaceAxisA() const
{
  return HeightmapDetail::surfaceNormalA(imp_->up_axis_id);
}


int Heightmap::surfaceAxisIndexB() const
{
  return HeightmapDetail::surfaceIndexB(imp_->up_axis_id);
}


const glm::dvec3 &Heightmap::surfaceAxisB() const
{
  return HeightmapDetail::surfaceNormalB(imp_->up_axis_id);
}


const glm::dvec3 &Heightmap::upAxisNormal(UpAxis axis_id)
{
  return HeightmapDetail::upAxisNormal(axis_id);
}


const glm::dvec3 &Heightmap::surfaceAxisA(UpAxis axis_id)
{
  return HeightmapDetail::surfaceNormalA(axis_id);
}


const glm::dvec3 &Heightmap::surfaceAxisB(UpAxis axis_id)
{
  return HeightmapDetail::surfaceNormalB(axis_id);
}


void Heightmap::setVirtualSurfaceFilterThreshold(unsigned threshold)
{
  imp_->virtual_surface_filter_threshold = threshold;
}


unsigned Heightmap::virtualSurfaceFilterThreshold() const
{
  return imp_->virtual_surface_filter_threshold;
}


int Heightmap::heightmapVoxelLayer() const
{
  return imp_->heightmap_voxel_layer;
}


bool Heightmap::buildHeightmap(const glm::dvec3 &reference_pos, const ohm::Aabb &cull_to)
{
  if (!imp_->occupancy_map)
  {
    return false;
  }

  PROFILE(buildHeightmap);

  // 1. Calculate the map extents.
  //  a. Calculate occupancy map extents.
  //  b. Project occupancy map extents onto heightmap plane.
  // 2. Populate heightmap voxels

  const OccupancyMap &src_map = *imp_->occupancy_map;
  ohm::Aabb src_region;
  src_map.calculateExtents(&src_region.minExtentsMutable(), &src_region.maxExtentsMutable());

  // Clip to the cull box.
  for (int i = 0; i < 3; ++i)
  {
    if (cull_to.diagonal()[i] > 0)
    {
      src_region.minExtentsMutable()[i] = cull_to.minExtents()[i];
      src_region.maxExtentsMutable()[i] = cull_to.maxExtents()[i];
    }
  }

  // Generate keys for these extents.
  const Key min_ext_key = src_map.voxelKey(src_region.minExtents());
  const Key max_ext_key = src_map.voxelKey(src_region.maxExtents());

  unsigned processed_count = 0;
  unsigned supporting_voxel_flags = !!imp_->generate_virtual_surface * heightmap::kVirtualSurfaces |
                                    !!imp_->promote_virtual_below * heightmap::kPromoteVirtualBelow;
  switch (imp_->mode)
  {
  case HeightmapMode::kPlanar:  //
  {
    // Pure planar walk must prefer below and does better ignoring virtual surfaces above the plane.
    supporting_voxel_flags |= heightmap::kIgnoreVirtualAbove;
    const Key planar_key = src_map.voxelKey(reference_pos);
    PlaneWalker walker(src_map, min_ext_key, max_ext_key, imp_->up_axis_id, &planar_key);
    processed_count = buildHeightmapT(walker, reference_pos, supporting_voxel_flags, supporting_voxel_flags);
    break;
  }
  case HeightmapMode::kSimpleFill:  //
  {
    // We should prefer voxels below for the first iteration, when seeding, after that we prefer the closest candidate
    // supporting voxel to the seed voxel.
    const unsigned initial_supporting_voxel_flags = supporting_voxel_flags;
    supporting_voxel_flags |= heightmap::kBiasAbove;
    PlaneFillWalker walker(src_map, min_ext_key, max_ext_key, imp_->up_axis_id);
    processed_count = buildHeightmapT(walker, reference_pos, initial_supporting_voxel_flags, supporting_voxel_flags);
    break;
  }
  case HeightmapMode::kLayeredFillUnordered:  //
  // No break
  case HeightmapMode::kLayeredFill:  //
  {
    // We should prefer voxels below for the first iteration, when seeding, after that we prefer the closest candidate
    // supporting voxel to the seed voxel.
    const unsigned initial_supporting_voxel_flags = supporting_voxel_flags;
    supporting_voxel_flags |= heightmap::kBiasAbove;
    PlaneFillLayeredWalker walker(src_map, min_ext_key, max_ext_key, imp_->up_axis_id);
    processed_count = buildHeightmapT(walker, reference_pos, initial_supporting_voxel_flags, supporting_voxel_flags);
    break;
  }
  default:
    break;
  }

#if PROFILING
  ohm::Profile::instance().report();
#endif  // PROFILING

  return processed_count;
}


HeightmapVoxelType Heightmap::getHeightmapVoxelInfo(const Key &key, glm::dvec3 *pos, HeightmapVoxel *voxel_info) const
{
  if (!key.isNull())
  {
    Voxel<const float> heightmap_occupancy(imp_->heightmap.get(), imp_->heightmap->layout().occupancyLayer(), key);

    if (heightmap_occupancy.isValid())
    {
      Voxel<const HeightmapVoxel> heightmap_voxel(imp_->heightmap.get(), imp_->heightmap_voxel_layer, key);
      Voxel<const VoxelMean> mean_voxel(imp_->heightmap.get(), imp_->heightmap->layout().meanLayer(), key);

      const glm::dvec3 voxel_centre = imp_->heightmap->voxelCentreGlobal(key);
      *pos = mean_voxel.isLayerValid() ? positionSafe(mean_voxel) : voxel_centre;
      float occupancy;
      heightmap_occupancy.read(&occupancy);
      const bool is_uncertain = occupancy == ohm::unobservedOccupancyValue();
      const float heightmap_voxel_value = (!is_uncertain) ? occupancy : -1.0f;
      // isValid() is somewhat redundant, but it silences a clang-tidy check.
      if (!is_uncertain && heightmap_voxel.isValid())
      {
        // Get height info.
        HeightmapVoxel heightmap_info;
        heightmap_voxel.read(&heightmap_info);
        *pos = voxel_centre + imp_->up * double(heightmap_info.height);
        if (voxel_info)
        {
          *voxel_info = heightmap_info;
        }

        if (heightmap_voxel_value == 0)
        {
          // kVacant
          return HeightmapVoxelType::kVacant;
        }

        if (heightmap_voxel_value > 0)
        {
          return HeightmapVoxelType::kSurface;
        }
      }

      return (!is_uncertain) ? HeightmapVoxelType::kVirtualSurface : HeightmapVoxelType::kUnknown;
    }
    return HeightmapVoxelType::kUnknown;
  }
  return HeightmapVoxelType::kUnknown;
}


double Heightmap::getVoxelHeight(const Key &key, double height) const
{
  return heightmap::getVoxelHeight(*imp_->heightmap, imp_->up, key, height);
}


double Heightmap::getVoxelHeight(const Key &key, const HeightmapVoxel &info) const
{
  return heightmap::getVoxelHeight(*imp_->heightmap, imp_->up, key, double(info.height));
}


void Heightmap::checkForBaseLayerDuplicates(std::ostream &out) const
{
  heightmap::checkForBaseLayerDuplicates(out, *imp_);
}


void Heightmap::updateMapInfo(MapInfo &info) const
{
  imp_->toMapInfo(info);
}


Key &Heightmap::project(Key *key) const
{
  return heightmap::project(key, upAxisIndex());
}


KeyRange Heightmap::buildReferencePlaneSlice(Key min_key, Key max_key, const glm::dvec3 &reference_pos) const
{
  const OccupancyMap &src_map = *imp_->occupancy_map;
  // Calculate the key range which marks the vertical slice through the reference map which covers the vertical range
  //  [reference_pos_height - floor, reference_pos_height + ceiling]
  const double height_sign = (int(imp_->up_axis_id) >= 0) ? 1.0 : -1.0;
  glm::dvec2 planar_slice(reference_pos[imp_->vertical_axis_index] - height_sign * imp_->floor,
                          reference_pos[imp_->vertical_axis_index] + height_sign * imp_->ceiling);
  if (planar_slice[0] > planar_slice[1])
  {
    std::swap(planar_slice[0], planar_slice[1]);
  }

  // Adjust range min/max to match the planar_slice.
  glm::dvec3 ref_pos = reference_pos;
  ref_pos[imp_->vertical_axis_index] = planar_slice[0];
  Key ref_key = src_map.voxelKey(ref_pos);
  min_key.setAxisFrom(imp_->vertical_axis_index, ref_key);

  ref_pos = reference_pos;
  ref_pos[imp_->vertical_axis_index] = planar_slice[1];
  ref_key = src_map.voxelKey(ref_pos);
  max_key.setAxisFrom(imp_->vertical_axis_index, ref_key);

  return KeyRange(min_key, max_key, src_map.regionVoxelDimensions());
}


template <typename KeyWalker>
bool Heightmap::buildHeightmapT(KeyWalker &walker, const glm::dvec3 &reference_pos, unsigned initial_supporting_flags,
                                unsigned iterating_supporting_flags)
{
  // Brute force initial approach.
  const OccupancyMap &src_map = *imp_->occupancy_map;
  OccupancyMap &heightmap = *imp_->heightmap;
  const double seed_height = glm::dot(imp_->up, reference_pos);

  updateMapInfo(heightmap.mapInfo());

  // Clear previous results.
  heightmap.clear();

  // Encode the base height of the heightmap in the origin.
  // heightmap.setOrigin(upAxisNormal() * glm::dot(upAxisNormal(), reference_pos));

  // Allow voxel mean positioning.
  const bool use_voxel_mean = src_map.voxelMeanEnabled() && !imp_->ignore_voxel_mean;
  if (use_voxel_mean)
  {
    heightmap.addVoxelMeanLayer();
  }

  PROFILE(walk)

  // Set the initial key.
  Key walk_key = src_map.voxelKey(reference_pos);

  // Bound the walk_key to the search bounds.
  if (!walk_key.isBounded(walker.minKey(), walker.maxKey()))
  {
    walk_key.clampToAxis(surfaceAxisIndexA(), walker.minKey(), walker.maxKey());
    walk_key.clampToAxis(surfaceAxisIndexB(), walker.minKey(), walker.maxKey());
  }

  if (!walker.begin(walk_key))
  {
    return false;
  }

  // Walk the 2D extraction region in a spiral around walk_key.
  unsigned populated_count = 0;
  // Convert the search floor and ceiling values to voxel counts.
  const int voxel_floor = ohm::pointToRegionCoord(imp_->floor, src_map.resolution());
  const int voxel_ceiling = ohm::pointToRegionCoord(imp_->ceiling, src_map.resolution());
  const int clearance_voxel_count_permissive =
    std::max(1, ohm::pointToRegionCoord(imp_->min_clearance, src_map.resolution()) - 1);

  heightmap::SrcVoxel src_voxel(src_map, use_voxel_mean);
  heightmap::DstVoxel hm_voxel(heightmap, imp_->heightmap_voxel_layer, use_voxel_mean);
  // Track generated extents. Seed with zero keys and correct dimensions.
  KeyRange dst_range_2d(Key(0), Key(0), heightmap.regionVoxelDimensions());

#if HM_DEBUG_VOXEL
  const glm::dvec3 debug_pos(2.05, 0.75, 0);
  const Key debug_src_key(1, -5, 0, 14, 28, 19);
#endif  // HM_DEBUG_VOXEL
  unsigned supporting_voxel_flags = initial_supporting_flags;
  // Tracks voxels which have results at multiple layers for a heightmap support isMultiLayered()
  std::set<ohm::Key> multi_layer_keys;
  // We use this map to collect data for virtual surface filtering step. It maps from source voxel keys to heightmap
  // key and voxel type.
  std::unordered_map<ohm::Key, heightmap::HeightmapKeyType> src_to_heightmap_keys;
  const bool ordered_layers = areLayersSorted();  // True to sort multi-layered configurations.
  bool abort = false;
  do
  {
#if HM_DEBUG_VOXEL
    const glm::dvec3 ref_pos = src_map.voxelCentreGlobal(walk_key);
    if (std::abs(ref_pos.x - debug_pos.x) < 0.5 * src_map.resolution() &&
        std::abs(ref_pos.y - debug_pos.x) < 0.5 * src_map.resolution())
    {
      int stopme = 1;
    }

    if (walk_key == debug_src_key)
    {
      int stopme = 2;
    }
#endif  // HM_DEBUG_VOXEL

    // Find the nearest voxel to the current key which may be a ground candidate.
    // This is key closest to the walk_key which could be ground. This will be either an occupied voxel, or virtual
    // ground voxel.
    // Virtual ground is where a free is supported by an uncertain or null voxel below it.
    Key candidate_key = heightmap::findNearestSupportingVoxel(src_voxel, walk_key, upAxis(), walker.minKey(),
                                                              walker.maxKey(), voxel_floor, voxel_ceiling,
                                                              clearance_voxel_count_permissive, supporting_voxel_flags);

    // Walk the column of candidate_key to find the first occupied voxel with sufficent clearance. A virtual voxel
    // with sufficient clearance may be given if there is no valid occupied voxel.
    heightmap::GroundCandidate ground;
    if (!candidate_key.isNull())
    {
      findGround(ground, src_voxel, candidate_key, walker.minKey(), walker.maxKey(), *imp_);
    }
    const Key ground_key = (ground.isValid()) ? ground.key : walk_key;

    // Mark whether this voxel may be a base layer candidate. This is always true for non-layered heightmaps.
    // For a layered map, we need a valid ground key with observations above.
    const bool is_base_layer_candidate =
      !isMultiLayered() || ground.isValid() && (ground.clearance > 0 || ground.observed_above);

    heightmap::onVisitWalker(walker, *imp_, walk_key, candidate_key, ground_key);

    // Write to the heightmap.
    src_voxel.setKey(ground_key);
    src_voxel.syncKey();
    // There's a fairly subtle issue we can resolve here. Using say a planar walk, we can end up with a voxel where
    // candidate_key is null so ground_key will be the same as walk_key. If that's of type kFree, then we can
    // incorrectly generate very flat sections of virtual surface. Although this could be fixed above, by ensuring the
    // ground_key is null, some other walk techniques can rely on it no being null. So, we apply a late fix and consider
    // our vxoel to be kNull when candidate_key is null. This actually has a positive impact on layered generation too
    // eliminating some undesirable "streamers" of virtual surface.
    const OccupancyType voxel_type = (!candidate_key.isNull()) ? src_voxel.occupancyType() : OccupancyType::kNull;

    if (voxel_type == kOccupied || (voxel_type == kFree && imp_->generate_virtual_surface))
    {
      // We use the voxel centre for lookup in the local cache for better consistency. Otherwise lateral drift in
      // subvoxel positioning can result in looking up the wrong cell.
      glm::dvec3 src_voxel_centre =
        (src_voxel.occupancy.isValid()) ? src_voxel.centre() : src_voxel.map().voxelCentreGlobal(ground_key);
      // We only use voxel mean positioning for occupied voxels. The information is unreliable for free voxels.
      glm::dvec3 voxel_pos = (voxel_type == kOccupied) ? src_voxel.position() : src_voxel_centre;

      HeightmapVoxelType hm_voxel_type =
        addSurfaceVoxel(hm_voxel, src_voxel, voxel_type, ground, voxel_pos, multi_layer_keys, is_base_layer_candidate);
      if (hm_voxel_type != HeightmapVoxelType::kUnknown)
      {
        if (populated_count > 0)
        {
          // Expand to hold additional sample.
          dst_range_2d.expand(hm_voxel.heightmap.key());
        }
        else
        {
          // First sample. Set min and max extents.
          dst_range_2d.setMinKey(hm_voxel.heightmap.key());
          dst_range_2d.setMaxKey(hm_voxel.heightmap.key());
        }
        ++populated_count;
        // Populate src_to_heightmap_keys if we are using it.
        if (ordered_layers && imp_->virtual_surface_filter_threshold > 0)
        {
          src_to_heightmap_keys.emplace(ground_key,
                                        heightmap::HeightmapKeyType{ hm_voxel.occupancy.key(), hm_voxel_type });
        }
      }
    }

    TES_SERVER_UPDATE(g_tes, 0.0f);
    supporting_voxel_flags = iterating_supporting_flags;
  } while (!abort && walker.walkNext(walk_key));

  if (ordered_layers)
  {
    // Mark virtual surface voxels for removal.
    if (imp_->virtual_surface_filter_threshold > 0)
    {
      heightmap::filterVirtualVoxels(*imp_, imp_->virtual_surface_filter_threshold, src_to_heightmap_keys);
    }

    // Ensure finalised extents is 2D only. We may have stacked some of the layers.
    Key key = dst_range_2d.minKey();
    key.setRegionAxis(imp_->vertical_axis_index, 0);
    key.setLocalAxis(imp_->vertical_axis_index, 0);
    dst_range_2d.setMinKey(key);
    key = dst_range_2d.maxKey();
    key.setRegionAxis(imp_->vertical_axis_index, 0);
    key.setLocalAxis(imp_->vertical_axis_index, 0);
    dst_range_2d.setMaxKey(key);

    // Finalise layers and remove filtered virtual surface voxels.
    heightmap::finaliseLayeredHeightmap(*imp_, dst_range_2d, multi_layer_keys, use_voxel_mean, &seed_height);
  }

  return populated_count != 0;
}


HeightmapVoxelType Heightmap::addSurfaceVoxel(heightmap::DstVoxel &hm_voxel, const heightmap::SrcVoxel &src_voxel,
                                              const OccupancyType voxel_type, const heightmap::GroundCandidate &ground,
                                              glm::dvec3 voxel_pos, std::set<ohm::Key> &multi_layer_keys,
                                              bool is_base_layer_candidate)
{
  if (voxel_type != kUnobserved && voxel_type != kNull)
  {
    OccupancyMap &heightmap = *imp_->heightmap;

    // Real or virtual surface.
    const HeightmapVoxelType add_voxel_type =
      (voxel_type == kOccupied) ? HeightmapVoxelType::kSurface : HeightmapVoxelType::kVirtualSurface;
    float surface_value = (voxel_type == kOccupied) ? kHeightmapSurfaceValue : kHeightmapVirtualSurfaceValue;

    // Cache the height then clear from the position.
    const double src_height = glm::dot(imp_->up, voxel_pos);
    voxel_pos[upAxisIndex()] = 0;

    // Get the heightmap voxel to update.
    Key hm_key = heightmap.voxelKey(voxel_pos);
    project(&hm_key);
    // TODO(KS): Using the Voxel interface here is highly suboptimal. This needs to be modified to access the
    // MapChunk directly for efficiency.
    hm_voxel.setKey(hm_key);

    // We can do a direct occupancy value write with no checks for the heightmap. The value is explicit.
    assert(hm_voxel.occupancy.isValid() && hm_voxel.occupancy.voxelMemory());

    bool should_add = true;
    // For multi-layered heightmaps, we need to check occupancy and not overwrite existing results.
    if (isMultiLayered())
    {
      if (hm_voxel.occupancy.data() != ohm::unobservedOccupancyValue())
      {
        // It's possible to visit the same 2D voxel at different candidate heights, but generate the same result
        // as a previous visitation. We check for this below.
        if (hm_voxel.haveRecordedHeight(src_height, upAxisIndex(), imp_->up))
        {
          // It's a repeat. Don't add.
          should_add = false;
        }
        else
        {
          // Store the base key to ensure we update multi_layer_keys if we are still adding this voxel.
          const auto base_key = hm_key;

          // We need to insert the voxel at the next available voxel in the target column. This results in unsorted
          // insertion with layer sorting optionally occuring as a post process.
          //
          // During this walk up the column we also validate that a virtual surface voxel is not being added too close
          // to another virtual surface voxel.
          double nearest_voxel_below = 0;
          double nearest_voxel_above = 0;
          do
          {
            // Check the distance to the existing voxel.
            const double current_voxel_height =
              hm_voxel.heightmap.data().height + glm::dot(imp_->up, heightmap.voxelCentreGlobal(hm_key));
            const double current_voxel_delta = current_voxel_height - src_height;
            nearest_voxel_below =
              (current_voxel_delta < 0 && (nearest_voxel_below <= 0 || -current_voxel_delta < nearest_voxel_below)) ?
                -current_voxel_delta :
                nearest_voxel_below;
            nearest_voxel_above =
              (current_voxel_delta > 0 && (nearest_voxel_above <= 0 || current_voxel_delta < nearest_voxel_above)) ?
                current_voxel_delta :
                nearest_voxel_above;

            // Walk to the next key up in the heightmap. We always move the key up as the height cells are
            // independent.
            heightmap.moveKeyAlongAxis(hm_key, upAxisIndex(), 1);
            hm_voxel.setKey(hm_key);
            assert(hm_voxel.occupancy.isValid() && hm_voxel.occupancy.voxelMemory());
          } while (hm_voxel.occupancy.data() != ohm::unobservedOccupancyValue());

          // Prevent adding a virtual surface voxel which is too close to an existing voxel.
          // We use the clearance height as the threshold.
          if ((nearest_voxel_below > 0 && nearest_voxel_below <= imp_->min_clearance) ||
              (nearest_voxel_above > 0 && nearest_voxel_above <= imp_->min_clearance))
          {
            should_add = false;
          }

          // We've now found where to insert and validated the insertion. Ensure we have base_key in multi_layer_keys
          // if we are still adding.
          if (should_add && areLayersSorted())
          {
            multi_layer_keys.insert(base_key);
          }
        }
      }
    }

    if (should_add)
    {
      hm_voxel.occupancy.write(surface_value);
      // Set voxel mean position as required. Will be skipped if not enabled.
      hm_voxel.setPosition(voxel_pos);

      // Write the height and clearance values.
      HeightmapVoxel height_info{};
      // Calculate the relative voxel height now that we have a target voxel key which may consider multi-layering.
      // Later sorting may change the HeightmapVoxel::height value as the voxel may be changed.
      height_info.height = heightmap::relativeVoxelHeight(src_height, hm_key, heightmap, imp_->up);
      height_info.clearance = float(ground.clearance);
      height_info.normal_x = height_info.normal_y = height_info.normal_z = 0;
      height_info.layer = (is_base_layer_candidate) ? kHvlBaseLayer : kHvlExtended;
      height_info.flags = 0;
      height_info.flags |= (ground.observed_above) ? kHvfObservedAbove : 0;
      height_info.contributing_samples =
        uint16_t((src_voxel.mean.isValid()) ? std::min(src_voxel.mean.data().count, 0xffffu) : 0u);

      if (voxel_type == kOccupied && src_voxel.covariance.isValid())
      {
        CovarianceVoxel cov;
        src_voxel.covariance.read(&cov);
        glm::dvec3 normal;
        covarianceEstimatePrimaryNormal(&cov, &normal);
        const double flip = (glm::dot(normal, upAxisNormal()) >= 0) ? 1.0 : -1.0;
        normal *= flip;
        height_info.normal_x = float(normal.x);
        height_info.normal_y = float(normal.y);
        height_info.normal_z = float(normal.z);
      }
      hm_voxel.heightmap.write(height_info);

      hm_voxel.debugDraw(imp_->debug_level, imp_->vertical_axis_index, int(imp_->up_axis_id) >= 0 ? 1.0 : -1.0);
      return add_voxel_type;
    }
  }

  return HeightmapVoxelType::kUnknown;
}
}  // namespace ohm


================================================
FILE: ohmheightmap/Heightmap.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_HEIGHTMAP_H
#define OHMHEIGHTMAP_HEIGHTMAP_H

#include "OhmHeightmapConfig.h"

#include "HeightmapMode.h"
#include "HeightmapVoxelType.h"
#include "UpAxis.h"

#include <ohm/Aabb.h>
#include <ohm/KeyRange.h>
#include <ohm/OccupancyType.h>

#include <memory>

#include <glm/fwd.hpp>

#include <functional>
#include <set>
#include <vector>

namespace ohm
{
class Key;
class MapInfo;
class OccupancyMap;
class VoxelConst;
struct HeightmapDetail;
struct HeightmapVoxel;

namespace heightmap
{
struct DstVoxel;
struct GroundCandidate;
struct SrcVoxel;
}  // namespace heightmap

/// A 2D voxel map variant which calculates a heightmap surface from another @c OccupancyMap .
///
/// The heightmap is built from an @c OccupancyMap and forms an axis aligned collapse of that map. The up axis may be
/// specified on construction of the heightmap, but must be aligned to a primary axis. The heightmap is built in
/// its own @c OccupancyMap , which consists of a single layer of voxels. The @c MapLayout for the heightmap is
/// two layers:
/// - **occupancy** layer
///   - float occupancy
/// - *heightmap* layer (named from @c HeightmapVoxel::kHeightmapLayer )
///   - @c HeightmapVoxel
///
/// The height specifies the absolute height of the surface, while clearance denotes how much room there is above
/// the surface voxel before the next obstruction. Note that the height values always increase going up, so the
/// height value will be inverted when using any @c UpAxis::kNegN @c UpAxis value. Similarly, the clearance is always
/// positive unless there are no further voxels above the surface, in which case the clearance is zero
/// (no information).
///
/// Each voxel in the heightmap represents a collapse of the source @c OccupancyMap based on a seed reference
/// position - see @c buildHeightmap() . The heightmap is generated by considering each column in the source map
/// relative to a reference height based on the seed position and neighbouring cells. When a valid supporting surface
/// is found, a heightmap voxel is marked as occupied and given a height associated with the supporting surface. This
/// supporting surface is the closest occupied voxel to the current reference position also having sufficient
/// clearance above it, @c minClearance() .
///
/// @par Virtual surfaces
/// The heightmap may also generate a 'virtual surface' from the interface between uncertain and free voxels when
/// @c generateVirtualSurface() is set. A 'virtual surface' voxel is simply a free voxel with an uncertain voxel below
/// it, but only in a column which does not have an occupied voxel within the search range. Virtual surface voxels
/// are marked as free in the heightmap.
///
/// The heightmap is generated either using a planar seach or a flood fill from the reference position. The planar
/// search operates at a fixed reference height at each column, while the flood fill search height is dependent on
/// the height of neighbour voxels. The flood fill is better at following surfaces, however it is significantly
/// slower.
///
/// Some variables limit the search for a supporting voxel in each column. To be considered as a support candidate, a
/// voxel must;
///
/// - Lie within the extents given to @c buildHeightmap()
/// - Must not be higher than the @c ceiling() height above its starting search position.
///
/// The generated heightmap may be accessed via @c heightmap() and voxel positions should be retrieved using
/// @c getHeightmapVoxelPosition() .
///
/// The @c OccupancyMap used to represent the heightmap has additional meta data stored in its @c MapInfo :
///
/// | Meta data                   | Description                                                                       |
/// | --------------------------- | --------------------------------------------------------------------------------  |
/// | *heightmap*                 | Present and true if this is a heightmap.                                          |
/// | *heightmap-axis*            | The up axis ID for a heightmap.                                                   |
/// | *heightmap-axis-x*          | The up axis X value for a heightmap.                                              |
/// | *heightmap-axis-y*          | The up axis Y value for a heightmap.                                              |
/// | *heightmap-axis-z*          | The up axis Z value for a heightmap.                                              |
/// | *heightmap-ceiling*         | The @c ceiling() value used in generating the heightmap.                          |
/// | *heightmap-clearance*       | The @c clearance() value used to generate the heightamp.                          |
/// | *heightmap-floor*           | The @c floor() value used in generating the heightmap.                            |
/// | *heightmap-mode*            | The @c mode() value used in generating the heightmap.                             |
/// | *heightmap-mode-name*       | An English translation of @c mode() .                                             |
/// | *heightmap-virtual-surface* | True if @c generateVirtualSurface() is set.                                       |
/// | *heightmap-virtual-surface-filter-threshold* | Value of @c virtualSurfaceFilterThreshold() .                    |
/// | *heightmap-virtual-surface-promote* | True if @c promoteVirtualBelow() is set                                   |
/// | *heightmap_seed_x*          | The X coordinate of the seed/reference position passed to @c buildHeightmap()     |
/// | *heightmap_seed_y*          | The Y coordinate of the seed/reference position passed to @c buildHeightmap()     |
/// | *heightmap_seed_z*          | The Z coordinate of the seed/reference position passed to @c buildHeightmap()     |
class ohmheightmap_API Heightmap
{
public:
  /// Size of regions in the heightmap. This is a 2D voxel extent. The region height is always one voxel.
  static const unsigned kDefaultRegionSize = 128;
  /// Voxel value assigned to heightmap cells which represent a real surface extracted from the source map.
  static constexpr float kHeightmapSurfaceValue = 1.0f;
  /// Voxel value assigned to heightmap cells which represent a virtual surface extracted from the source map.
  /// Virtual surfaces may be formed by the interface between a free voxel supported by an uncertain/null voxel.
  static constexpr float kHeightmapVirtualSurfaceValue = -1.0f;
  /// Voxel value assigned to heightmap cells which are deliberately vacant in the heightmap. This may occur when
  /// the corresponding voxels have no valid voxel in the entire column from the source map or when no ground voxel
  /// can be found for a layered search. In a layered heightmap there may be a mix of vacant and surface voxels in a
  /// single column in the heightmap.
  static constexpr float kHeightmapVacantValue = 0.0f;

  /// Construct a default initialised heightmap.
  Heightmap();

  /// Construct a new heightmap optionally tied to a specific @p map .
  /// @param grid_resolution The grid resolution for the heightmap. Should match the source map for best results.
  /// @param min_clearance The minimum clearance value expected above each surface voxel.
  /// @param up_axis Identifies the up axis for the map.
  /// @param region_size Grid size of each region in the heightmap.
  Heightmap(double grid_resolution, double min_clearance, UpAxis up_axis = UpAxis::kZ, unsigned region_size = 0);

  /// Destructor.
  ~Heightmap();

  /// Set number of threads to use in heightmap generation, enabling multi-threaded code path as required.
  ///
  /// Setting the @p thread_count to zero enables multi-threading using the maximum number of threads. Setting the
  /// @p thread_count to 1 disables threads (default).
  ///
  /// Using multiple threads may not yield significant gains.
  ///
  /// @param thread_count The number of threads to set.
  /// @return True if mult-threading is available. False when no mult-threading is available and @p thread_count is
  /// ignored.
  bool setThreadCount(unsigned thread_count);

  /// Get the number of threads to use.
  ///
  /// - 0: use all available
  /// - 1: force single threaded, or no multi-threading is available.
  /// - n: Use n threads.
  /// @return The number of threads to use.
  unsigned threadCount() const;

  /// Set the occupancy map on which to base the heightmap. The heightmap does not take ownership of the pointer so
  /// the @p map must persist until @c buildHeightmap() is called.
  void setOccupancyMap(const OccupancyMap *map);

  /// Access the current source occupancy map.
  const OccupancyMap *occupancyMap() const;

  /// Access the currently generated heightmap.
  OccupancyMap &heightmap() const;

  /// Set the search ceiling level. Searches are constrained to this ceiling height from the seed voxel. The effective
  /// ceiling varies depending on the active @c mode() . For @c HeightmapMode::kPlanar , the imposes a fixed limit. For
  /// all fill modes, the ceiling is relative to the current seed voxel. This means the absolute ceiling will run
  /// parallel to a slope.
  /// @param ceiling The new ceiling value in global map units. Positive to enable, but must be larger than half the
  /// source map voxel resolution to have any effect.
  void setCeiling(double ceiling);

  /// Get the search ceiling level. See @c setCeiling() .
  /// @return The search ceiling value in global map units.
  double ceiling() const;

  /// Set the search floor level. Searches are constrained to search down only to this floor height from the seed voxel.
  /// The same effective constraints apply for the floor as do for @c setCeiling() .
  /// @param floor The new floor value in global map units. Positive to enable, but must be larger than half the source
  /// map voxel resolution to have any effect.
  void setFloor(double floor);

  /// Get the search floor level. See @c setCeiling() .
  /// @return The search floor value in global map units.
  double floor() const;

  /// Set the minimum clearance required above a voxel in order to consider it a heightmap voxel.
  /// @param clearance The new clearance value.
  void setMinClearance(double clearance);

  /// Get the minimum clearance required above a voxel in order to consider it a heightmap voxel.
  /// @return The height clearance value.
  double minClearance() const;

  /// Sets whether voxel mean positions are ignored (true) forcing the use of voxel centres.
  /// @param ignore True to force voxel centres even when voxel mean positions are present.
  void setIgnoreVoxelMean(bool ignore);

  /// Force voxel centres even when voxel mean positions are present?
  /// @return True to ignore voxel mean positioning.
  /// @see @ref voxelmean
  bool ignoreVoxelMean() const;

  /// Set the generation of a heightmap floor around the transition from unknown to free voxels?
  ///
  /// This option allows a heightmap floor to be generated in columns where there is no clear occupied floor voxel.
  /// When enabled, the heightmap generates a floor level at the lowest transition point from unknown to free voxel.
  ///
  /// @param enable Enable this option?
  void setGenerateVirtualSurface(bool enable);

  /// Allow the generation of a heightmap floor around the transition from unknown to free voxels?
  ///
  /// @see @c setGenerateVirtualSurface()
  ///
  /// @return True if this option is enabled.
  bool generateVirtualSurface() const;

  /// Set whether virtual surface candidates below the reference position are preferred to real above.
  ///
  /// When building a heightmap column, the default behaviour is for virtual surfaces to be reported only if the
  /// search expanse does not include a real occupied voxel from which a real surface can be derived. This option
  /// changes the behaviour to make a virtual surface candidate which lies below the reference position a preferred
  /// seed candidate to an occupied voxel which lies above the reference position. This can generate better ground
  /// results where the ground cannot be properly observed.
  ///
  /// @param enable True to promote virtual candidate below the reference position.
  void setPromoteVirtualBelow(bool enable);

  /// Query whether virtual surface voxels below the reference position are preferred to real voxels above.
  ///
  /// @see @c setPromoteVirtualBelow()
  ///
  /// @return True to prefer virtual voxels below the reference position.
  bool promoteVirtualBelow() const;

  /// Sets the heightmap generation mode. May be modified between calls to @c buildHeightmap()
  /// @param mode The target mode.
  void setMode(HeightmapMode mode);

  /// Query the heightmap generation mode. The default is @c kPlanar .
  /// @return The current heightmap generation mode.
  HeightmapMode mode() const;

  /// Query if this object is set to generate a heightmap with multiple layers.
  /// @return True when generating a multi-layered heightmap.
  inline bool isMultiLayered() const
  {
    return mode() == HeightmapMode::kLayeredFillUnordered || mode() == HeightmapMode::kLayeredFill;
  }

  /// Query if the resulting multi-layered heightmap has each column ordered by height. Implies @c isMultiLayered() .
  /// @return True if the heightmap contains columns sorted in height order.
  inline bool areLayersSorted() const { return mode() == HeightmapMode::kLayeredFill; }

  /// Set the number of occupied neighbours a virtual surface voxel needs to be kept in a @c HeightmapMode::kLayeredFill
  /// map.
  ///
  /// When non-zero and using @c mode() @c HeightmapMode::kLayeredFill , this value enables filtering out of virtual
  /// surface voxels which have fewer occupied neighours than this value. This can help reduce "virtual surface noise"
  /// in maps which have sparse free space observations.
  ///
  /// This setting only works with @c mode() @c HeightmapMode::kLayeredFill .
  ///
  /// Neighbours are the 26 voxels sharing a face, edge or vertex with the voxel of interest.
  ///
  /// @param threshold Number of occupied neighbours required to keep a virtual surface voxel. Theoretical range is
  /// [0, 26], with a pragmatic range of approximately [0, 8].
  void setVirtualSurfaceFilterThreshold(unsigned threshold);

  /// Query the virtual surface filtering threshold. See @c setVirtualSurfaceFilterThreshold() .
  unsigned virtualSurfaceFilterThreshold() const;

  /// Set the heightmap generation to flood fill ( @c true ) or planar ( @c false ).
  ///
  /// @deprecated Use @c setMode(HeightmapMode::kSimpleFill) .
  ///
  /// @param flood_fill True to enable the flood fill technique.
  inline void setUseFloodFill(bool flood_fill)
  {
    setMode(flood_fill ? HeightmapMode::kSimpleFill : HeightmapMode::kPlanar);
  }

  /// Is the flood fill generation technique in use ( @c true ) or planar technique ( @c false ).
  ///
  /// @deprecated Check @c mode() against @c HeightmapMode::kSimpleFill .
  ///
  /// @return True when using flood fill.
  inline bool useFloodFill() const { return mode() == HeightmapMode::kSimpleFill; }

  /// The layer number which contains @c HeightmapVoxel structures.
  /// @return The heightmap layer index or -1 on error (not present).
  /// @see @ref voxelmean
  int heightmapVoxelLayer() const;

  /// The layer number which contains @c HeightmapVoxel structures during heightmap construction.
  /// @return The heightmap build layer index or -1 on error (not present).
  int heightmapVoxelBuildLayer() const;

  /// Get the up axis identifier used to generate the heightmap.
  UpAxis upAxis() const;

  /// Get the up axis index [0, 2] marking XYZ respectively. Ignores direction.
  int upAxisIndex() const;

  /// Get the normal vector for the up axis used with last @c buildHeightmap() .
  const glm::dvec3 &upAxisNormal() const;

  /// Component index of the first surface axis normal [0, 2].
  int surfaceAxisIndexA() const;

  /// Get a unit vector which lies along the surface of the heightmap, perpendicular to @c surfaceAxisB() and
  /// upAxisNormal().
  const glm::dvec3 &surfaceAxisA() const;

  /// Component of the second surface axis normal [0, 2].
  int surfaceAxisIndexB() const;

  /// Get a unit vector which lies along the surface of the heightmap, perpendicular to @c surfaceAxisA() and
  /// upAxisNormal().
  const glm::dvec3 &surfaceAxisB() const;

  /// Static resolution of @c Axis to a normal.
  /// @param axis_id The @c Axis ID.
  static const glm::dvec3 &upAxisNormal(UpAxis axis_id);

  /// Get a unit vector which lies along the surface of the heightmap, perpendicular to @c surfaceAxisB() and
  /// upAxisNormal().
  static const glm::dvec3 &surfaceAxisA(UpAxis axis_id);

  /// Get a unit vector which lies along the surface of the heightmap, perpendicular to @c surfaceAxisA() and
  /// upAxisNormal().
  static const glm::dvec3 &surfaceAxisB(UpAxis axis_id);

  /// Generate the heightmap around a reference position. This sets the @c base_height as in the overload, but also
  /// changes the behaviour to flood fill out from the reference position.
  ///
  /// @param reference_pos The staring position to build a heightmap around. Nominally a vehicle or sensor position.
  /// @param cull_to Build the heightmap only from within these extents in the source map.
  /// @return true on success.
  bool buildHeightmap(const glm::dvec3 &reference_pos, const ohm::Aabb &cull_to = ohm::Aabb(0.0));

  /// Query the information about a voxel in the @c heightmap() occupancy map.
  ///
  /// Heightmap voxel values, positions and semantics are specialised from the general @c OccupancyMap usage. This
  /// method may be used to ensure the correct position values are retrieved and consistent voxel interpretations
  /// are made. All position queries should be made via this function. The return value is used indicate whether
  /// the voxel is relevant/occupied within the occupancy map.
  ///
  /// @param key The key of the voxel to test for validity and to retrieve the position and details of. If this key
  ///            does not map to a valid voxel in the @p heightmap() of this object a @c HeightmapVoxel::Unknown
  ///            value will be returned.
  /// @param[out] pos The retrieved position of @p heightmap_voxel . Only valid when this function returns something
  ///             other than @c HeightmapVoxel::Unknown .
  /// @param[out] voxel_info Clearance and height details of the voxel associated with @p key. Only valid when this
  ///             function returns something other than @c HeightmapVoxel::Unknown .
  /// @return The type of the voxel in question. May return @c HeightmapVoxel::Unknown if @p key is invalid.
  HeightmapVoxelType getHeightmapVoxelInfo(const Key &key, glm::dvec3 *pos, HeightmapVoxel *voxel_info = nullptr) const;

  /// Calculate the height of the voxel at @p key with matching local @p height value.
  ///
  /// This calculates the centre of the voxel at @p key then calculates `dot(up, centre) + height`.
  ///
  /// @param key The heightmap voxel key of interest. The up axis should always be (0, 0) for non-layered heightmaps.
  /// @param height The local voxel height delta.
  /// @return The global voxel height value.
  double getVoxelHeight(const Key &key, double height) const;

  /// Calculate the height of the voxel at @p key with matching @c HeightmapVoxel @p info .
  ///
  /// This calls through to @c getVoxelHeight(key,height) using the value @p info.height .
  ///
  /// @param key The heightmap voxel key of interest. The up axis should always be (0, 0) for non-layered heightmaps.
  /// @param info The voxel heightmap data.
  /// @return The global voxel height value.
  double getVoxelHeight(const Key &key, const HeightmapVoxel &info) const;

  /// Debug function which logs any columns with multiple base layer voxels.
  /// @param out Stream to log to.
  void checkForBaseLayerDuplicates(std::ostream &out) const;

  //-------------------------------------------------------
  // Internal
  //-------------------------------------------------------

  /// Internal heightmap detail access.
  /// @return Internal heightmap details.
  inline HeightmapDetail *detail() { return imp_.get(); }
  /// Internal heightmap detail access.
  /// @return Internal heightmap details.
  inline const HeightmapDetail *detail() const { return imp_.get(); }

  /// Update @c info to reflect the details of how the heightmap is generated. See class comments.
  /// @param info The info object to update.
  void updateMapInfo(MapInfo &info) const;

  /// Ensure that @p key is referencing a voxel within the heightmap plane.
  /// @param[in,out] key The key to project. May be modified by this call. Must not be null.
  /// @return A reference to @p key .
  Key &project(Key *key) const;

private:
  /// Build a key range which covers the source map extents in 2D, but limits the vertical range by the floor/ceiling
  /// around @p reference_pos .
  /// @param min_key The source map minimum extents key.
  /// @param max_key The source map maximum extents key.
  /// @param reference_pos The reference position to build the heightmap from. Only the vertical component is used.
  /// @return The key range creating an extended planar slice through the source map.
  KeyRange buildReferencePlaneSlice(Key min_key, Key max_key, const glm::dvec3 &reference_pos) const;

  /// Internal implementation of heightmap construction. Supports the different key walking techniques available.
  /// @param walker The key walker used to iterate the source map and heightmap overlap.
  /// @param reference_pos Reference position around which to generate the heightmap
  /// @param on_visit Optional callback invoked for each key visited. Parameters are: @p walker , this object's
  ///   internal details, the candidate key first evaluated for the column search start, the ground key to be migrated
  ///   to the heightmap. Both keys reference the source map.
  template <typename KeyWalker>
  bool buildHeightmapT(KeyWalker &walker, const glm::dvec3 &reference_pos, unsigned initial_supporting_flags,
                       unsigned iterating_supporting_flags);

  /// Helper function for adding a surface, or virtual surface voxel from @c buildHeightmapT() .
  ///
  /// @param hm_voxel Destination voxel management structure - references the target heightmap @c OccupancyMap .
  ///     This will be modified to match the heightmap voxel corresponding to @p src_voxel . It is passed as an argument
  ///     for caching efficiency and could otherwise be omitted.
  /// @param src_voxel Source voxel management structure - references the source @c OccupancyMap and identifies the
  ///     voxel from the source map to be added to the heightmap. Must be valid.
  /// @param voxel_type The @c OccupancyType associated with @c src_voxel .
  /// @param clearance The available height clearance above @p src_voxel being added.
  /// @param voxel_pos The position of @c src_voxel . Will use sub-voxel positioning if available and allowed otherwise
  ///     marks the voxel centre.
  /// @param multi_layer_keys Set of heightmap map keys which identify columns containing more than one voxel. Will
  ///     be added to if the @p hm_voxel already has voxel data and we are building a layered heightmap.
  /// @param is_base_layer_candidate Should be true if the @c src_voxel falls within the allowed range for being
  ///     included in base surface layer. Should always be true for non-layered heightmaps.
  /// @return The voxel type in the heightmap. One of @c kSurface, @c kVirtualSurface, @c kUnknown where the latter
  /// indicates no voxel has not been added to the heightmap.
  HeightmapVoxelType addSurfaceVoxel(heightmap::DstVoxel &hm_voxel, const heightmap::SrcVoxel &src_voxel,
                                     OccupancyType voxel_type, const heightmap::GroundCandidate &ground,
                                     glm::dvec3 voxel_pos, std::set<ohm::Key> &multi_layer_keys,
                                     bool is_base_layer_candidate);

  std::unique_ptr<HeightmapDetail> imp_;
};  // namespace ohm
}  // namespace ohm

#endif  // OHMHEIGHTMAP_HEIGHTMAP_H


================================================
FILE: ohmheightmap/HeightmapMesh.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "HeightmapMesh.h"

// Must come first for various include issues.
#include "delaunator.hpp"

#include <ohmheightmap/Heightmap.h>
#include <ohmheightmap/HeightmapVoxel.h>
#include <ohmheightmap/TriangleEdge.h>
#include <ohmheightmap/TriangleNeighbours.h>

#include <ohm/Aabb.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/OccupancyMap.h>
#include <ohm/VoxelData.h>

#include <ohmutil/PlyMesh.h>
#include <ohmutil/Profile.h>

#include <glm/ext.hpp>
#include <glm/glm.hpp>

#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/normal.hpp>

#include <vector>

namespace ohm
{
class HeightmapMeshDetail
{
public:
  std::vector<glm::dvec3> vertices;
  std::vector<glm::vec3> vertex_normals;
  std::vector<glm::vec3> tri_normals;
  std::vector<unsigned> triangles;
  std::vector<TriangleNeighbours> triangle_neighbours;
  std::vector<TriangleEdge> edges;
  std::vector<double> coords_2d;
  HeightmapMesh::NormalsMode normals_mode = HeightmapMesh::kNormalsAverage;
  /// Loose mesh extents enclosing the generating 2D voxels. The extents are tight along the height axis.
  Aabb loose_mesh_extents = Aabb(0.0);
  /// Tight mesh extents exactly enclosing the mesh vertices.
  Aabb tight_mesh_extents = Aabb(0.0);
  double resolution = 0.0;

  void clear()
  {
    vertices.clear();
    vertex_normals.clear();
    tri_normals.clear();
    triangles.clear();
    triangle_neighbours.clear();
    edges.clear();
    coords_2d.clear();
    loose_mesh_extents = tight_mesh_extents = Aabb(0.0);
    resolution = 0.0;
  }
};


HeightmapMesh::HeightmapMesh(NormalsMode normals_mode)
  : imp_(std::make_unique<HeightmapMeshDetail>())
{
  imp_->normals_mode = normals_mode;
}


HeightmapMesh::~HeightmapMesh() = default;


HeightmapMesh::NormalsMode HeightmapMesh::normalsMode()
{
  return imp_->normals_mode;
}


void HeightmapMesh::setNormalsMode(NormalsMode mode)
{
  imp_->normals_mode = mode;
}


bool HeightmapMesh::buildMesh(const Heightmap &heightmap, const MeshVoxelModifier &voxel_modifier)
{
  PROFILE(HeightmapMesh_buildMesh);
  // Populate the coordinates.
  imp_->clear();

  // Walk heightmap voxels.
  const OccupancyMap &heightmap_occupancy = heightmap.heightmap();
  const MapLayer *heightmap_layer = heightmap_occupancy.layout().layer(HeightmapVoxel::kHeightmapLayer);
  const int heightmap_layer_index = int(heightmap_layer->layerIndex());

  if (heightmap_layer_index < 0)
  {
    // Fail.
    return false;
  }

  imp_->resolution = heightmap_occupancy.resolution();

  const glm::dvec3 up = heightmap.upAxisNormal();
  const glm::vec3 upf(up);
  const double heightmap_resolution = heightmap_occupancy.resolution();
  glm::dvec3 point;
  glm::dvec3 min_map_ext;
  glm::dvec3 max_map_ext;
  glm::dvec3 min_vert_ext;
  glm::dvec3 max_vert_ext;
  glm::dvec3 voxel_centre;

  min_map_ext = min_vert_ext = glm::dvec3(std::numeric_limits<double>::max());
  max_map_ext = max_vert_ext = glm::dvec3(-std::numeric_limits<double>::max());

  // Build vertices and map extents.
  for (auto voxel_iter = heightmap_occupancy.begin(); voxel_iter != heightmap_occupancy.end(); ++voxel_iter)
  {
    HeightmapVoxel voxel_info;
    auto voxel_type = heightmap.getHeightmapVoxelInfo(*voxel_iter, &point, &voxel_info);
    if (voxel_modifier)
    {
      voxel_type = voxel_modifier(*voxel_iter, voxel_type, &point, &voxel_info.clearance);
    }

    if (voxel_type != HeightmapVoxelType::kUnknown && voxel_type != HeightmapVoxelType::kVacant)
    {
      imp_->coords_2d.push_back(point.x);
      imp_->coords_2d.push_back(point.y);
      imp_->vertices.push_back(point);

      // Adjust tight extents
      min_vert_ext.x = std::min(point.x, min_vert_ext.x);
      min_vert_ext.y = std::min(point.y, min_vert_ext.y);
      min_vert_ext.z = std::min(point.z, min_vert_ext.z);

      max_vert_ext.x = std::max(point.x, max_vert_ext.x);
      max_vert_ext.y = std::max(point.y, max_vert_ext.y);
      max_vert_ext.z = std::max(point.z, max_vert_ext.z);

      // Adjust loose extents
      voxel_centre = heightmap_occupancy.voxelCentreGlobal(*voxel_iter);
      min_map_ext.x = std::min(voxel_centre.x - 0.5 * heightmap_resolution, min_map_ext.x);
      min_map_ext.y = std::min(voxel_centre.y - 0.5 * heightmap_resolution, min_map_ext.y);
      min_map_ext.z = std::min(voxel_centre.z - 0.5 * heightmap_resolution, min_map_ext.z);

      max_map_ext.x = std::max(voxel_centre.x + 0.5 * heightmap_resolution, max_map_ext.x);
      max_map_ext.y = std::max(voxel_centre.y + 0.5 * heightmap_resolution, max_map_ext.y);
      max_map_ext.z = std::max(voxel_centre.z + 0.5 * heightmap_resolution, max_map_ext.z);
    }
  }

  if (!imp_->vertices.empty())
  {
    // Fixup the loose extents along the height axis. This matches that of the tight extents.
    const int up_axis_index = heightmap.upAxisIndex();
    min_map_ext[up_axis_index] = min_vert_ext[up_axis_index];
    max_map_ext[up_axis_index] = max_vert_ext[up_axis_index];
  }
  else
  {
    min_map_ext = max_map_ext = min_vert_ext = max_vert_ext = glm::dvec3(0.0);
  }

  imp_->loose_mesh_extents = Aabb(min_map_ext, max_map_ext);
  imp_->tight_mesh_extents = Aabb(min_vert_ext, max_vert_ext);

  // Need at least 3 points to triangulate.
  if (imp_->coords_2d.size() < 3)
  {
    return false;
  }

  // Triangulate using Delaunay triangulation.
  try
  {
    delaunator::Delaunator delaunay(imp_->coords_2d);

    // Prime vertex normal calculations.
    imp_->vertex_normals.clear();
    imp_->vertex_normals.reserve(imp_->vertices.size());
    for (size_t i = 0; i < imp_->vertices.size(); ++i)
    {
      imp_->vertex_normals.emplace_back(glm::vec3(0.0f));
    }

    // Extract Delaunay triangles into imp_->triangles.
    imp_->triangles.resize(delaunay.triangles.size());
    if (!delaunay.triangles.empty())
    {
      imp_->triangles.clear();
      imp_->triangles.reserve(delaunay.triangles.size());
      imp_->tri_normals.reserve(delaunay.triangles.size() / 3);
      imp_->triangle_neighbours.reserve(delaunay.triangles.size() / 3);
      imp_->edges.reserve(delaunay.triangles.size());

      std::array<glm::dvec3, 3> tri;
      glm::vec3 normal;
      std::array<unsigned, 3> indices;
      unsigned tri_count = 0;
      TriangleNeighbours neighbour_info{};

      // Initialise empty neighbour information.
      neighbour_info.neighbours[0] = neighbour_info.neighbours[1] = neighbour_info.neighbours[2] = ~0u;
      neighbour_info.neighbour_edge_indices[0] = neighbour_info.neighbour_edge_indices[1] =
        neighbour_info.neighbour_edge_indices[2] = -1;

      for (size_t i = 0; i < delaunay.triangles.size(); i += 3)
      {
        indices[0] = unsigned(delaunay.triangles[i + 0]);
        indices[1] = unsigned(delaunay.triangles[i + 1]);
        indices[2] = unsigned(delaunay.triangles[i + 2]);
        tri[0] = imp_->vertices[indices[0]];
        tri[1] = imp_->vertices[indices[1]];
        tri[2] = imp_->vertices[indices[2]];

        // Calculate the triangle normal.
        normal = glm::triangleNormal(tri[0], tri[1], tri[2]);

        // Adjust winding to match the heightmap axis.
        if (glm::dot(normal, glm::vec3(up)) < 0)
        {
          std::swap(indices[1], indices[2]);
          normal *= -1.0f;
        }

        imp_->triangles.push_back(indices[0]);
        imp_->triangles.push_back(indices[1]);
        imp_->triangles.push_back(indices[2]);

        imp_->triangle_neighbours.push_back(neighbour_info);

        imp_->edges.emplace_back(TriangleEdge(indices[0], indices[1], tri_count, 0));
        imp_->edges.emplace_back(TriangleEdge(indices[1], indices[2], tri_count, 1));
        imp_->edges.emplace_back(TriangleEdge(indices[2], indices[0], tri_count, 2));

        // Vertex normals generated by considering all faces.
        if (imp_->normals_mode == kNormalsAverage)
        {
          imp_->vertex_normals[indices[0]] += normal;
          imp_->vertex_normals[indices[1]] += normal;
          imp_->vertex_normals[indices[2]] += normal;
        }
        else if (imp_->normals_mode == kNormalsWorst)
        {
          // Vertex normals by least horizontal.
          for (int j = 0; j < 3; ++j)
          {
            const glm::vec3 existing_normal = imp_->vertex_normals[indices[j]];
            const float existing_dot = glm::dot(existing_normal, upf);
            const float new_dot = glm::dot(normal, upf);
            if (existing_normal == glm::vec3(0.0f) || existing_dot > new_dot)
            {
              // No existing normal or existing is more horizontal. Override.
              imp_->vertex_normals[indices[j]] = normal;
            }
          }
        }

        imp_->tri_normals.push_back(normal);
        ++tri_count;
      }

      if (!imp_->edges.empty())
      {
        // Use the vertex to edges to build the triangle neighbour information.
        // First sort to ensure all triangle mappings for a vertex are adjacent.
        std::sort(imp_->edges.begin(), imp_->edges.end());

        // Edges should end up paired and we know from how the mesh is built that there can only be either one or two
        // triangles per edge.
        const TriangleEdge *previous_edge = &imp_->edges[0];
        const TriangleEdge *current_edge;
        for (size_t i = 1; i < imp_->edges.size(); ++i)
        {
          current_edge = &imp_->edges[i];
          if (current_edge->v0 == previous_edge->v0 && current_edge->v1 == previous_edge->v1)
          {
            // We have a shared edge. Update neighbour information for both triangles.
            TriangleNeighbours &n0 = imp_->triangle_neighbours[current_edge->triangle];
            TriangleNeighbours &n1 = imp_->triangle_neighbours[previous_edge->triangle];

            n0.neighbours[current_edge->triangle_edge_index] = previous_edge->triangle;
            n0.neighbour_edge_indices[current_edge->triangle_edge_index] = previous_edge->triangle_edge_index;

            n1.neighbours[previous_edge->triangle_edge_index] = current_edge->triangle;
            n1.neighbour_edge_indices[previous_edge->triangle_edge_index] = current_edge->triangle_edge_index;
          }
          // else We have an open edge and need do nothing.
          previous_edge = current_edge;
        }
      }
    }

    // Normalise data stored in vertex_normals to get the final normals.
    for (auto &vertex_normal : imp_->vertex_normals)
    {
      vertex_normal = glm::normalize(vertex_normal);
    }

    return true;
  }
  catch (const std::runtime_error &)
  {
    // Triangulation has failed. Can come about due to co-linear coordinates.
  }

  return false;
}


double HeightmapMesh::resolution() const
{
  return imp_->resolution;
}


size_t HeightmapMesh::vertexCount() const
{
  return imp_->vertices.size();
}


size_t HeightmapMesh::triangleCount() const
{
  return imp_->tri_normals.size();
}


const glm::dvec3 *HeightmapMesh::vertices() const
{
  return imp_->vertices.data();
}


const glm::vec3 *HeightmapMesh::vertexNormals() const
{
  return imp_->vertex_normals.data();
}


const glm::vec3 *HeightmapMesh::triangleNormals() const
{
  return imp_->tri_normals.data();
}


const unsigned *HeightmapMesh::triangles() const
{
  return imp_->triangles.data();
}


const Aabb &HeightmapMesh::meshBoundingBox() const
{
  return imp_->loose_mesh_extents;
}


const Aabb &HeightmapMesh::tightMeshBoundingBox() const
{
  return imp_->tight_mesh_extents;
}


const TriangleNeighbours *HeightmapMesh::triangleNeighbours() const
{
  return imp_->triangle_neighbours.data();
}


bool HeightmapMesh::extractPlyMesh(PlyMesh &mesh, bool offset_by_extents)
{
  if (triangleCount() == 0)
  {
    return false;
  }

  mesh.clear();

  const glm::dvec3 offset = (offset_by_extents) ? imp_->loose_mesh_extents.minExtents() : glm::dvec3(0.0);

  for (const glm::dvec3 &vert : imp_->vertices)
  {
    // Offset by mesh min extents to support single precision convertion.
    mesh.addVertex(glm::vec3(vert - offset));
  }

  mesh.addTriangles(imp_->triangles.data(), unsigned(imp_->triangles.size() / 3));

  return true;
}
}  // namespace ohm


================================================
FILE: ohmheightmap/HeightmapMesh.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_HEIGHTMAPMESH_H
#define OHMHEIGHTMAP_HEIGHTMAPMESH_H

#include "OhmHeightmapConfig.h"

#include "HeightmapVoxelType.h"

#include <glm/fwd.hpp>

#include <algorithm>
#include <functional>
#include <memory>

namespace ohm
{
class Aabb;
class Heightmap;
class HeightmapMeshDetail;
class Key;
class PlyMesh;
struct TriangleNeighbours;

/// A utility class for generating a triangle mesh from a @c Heightmap occupancy map.
///
/// The mesh is generated in @c buildMesh() and provides the following data:
/// - Number of vertices
/// - Number of triangles (number of indices is 3 * triangleCount())
/// - Double precision vertices
/// - Single precision per vertex normals, generated by @c NormalsMode
/// - Single precision per triangle normals.
/// - Mesh axis aligned bounds.
/// - Triangle neighbour information: see @c TriangleNeighbours.
class ohmheightmap_API HeightmapMesh
{
public:
  /// Vertex normal generation mode.
  enum NormalsMode
  {
    /// Average each vertex triangle normals.
    kNormalsAverage,
    /// Select the "worst" triangle normal where "worst" is the least horizontal.
    kNormalsWorst,
  };

  /// Optional modifier function which can be applied to a voxel before moving it the mesh.
  /// @param voxel The current heightmap voxel.
  /// @param voxel_type The initial voxel type.
  /// @param voxel_position The voxel position. May be modified.
  /// @param clearance The voxel overhead clearance. May be modified.
  /// @return The @c HeightmapVoxelType after filtering or @p voxel_type if no modification is required.
  using MeshVoxelModifier = std::function<HeightmapVoxelType(const Key &, HeightmapVoxelType, glm::dvec3 *, float *)>;

  /// Construct a heightmap mesh using the given vertex normal generation mode.
  /// @param normals_mode The initial vertex generation mode.
  explicit HeightmapMesh(NormalsMode normals_mode = kNormalsAverage);

  /// Destructor.
  ~HeightmapMesh();

  /// Query the active vertex normal generation mode.
  /// @return The vertex normal mode.
  NormalsMode normalsMode();

  /// Set the active vertex normal generation mode.
  /// @param mode The new mode.
  void setNormalsMode(NormalsMode mode);

  /// Build a mesh from @p heightmap. This clears previous data.
  ///
  /// Possible failure conditions:
  /// - The heightmap occupancy map does not contain the required voxel layers.
  ///
  /// @param heightmap The heightmap to generate a mesh for.
  /// @param voxel_modifier Optional modifier function applied to each voxel before moving it to the mesh.
  /// @return True on success.
  bool buildMesh(const Heightmap &heightmap, const MeshVoxelModifier &voxel_modifier = MeshVoxelModifier());

  /// Get the voxel resolution fo the heightmap from which the last mesh was generated.
  /// @return The heightmap voxel resolution.
  double resolution() const;

  /// Get the number of vertices in the generated mesh.
  ///
  /// Only valid when @c buildMesh() is successful.
  ///
  /// @return The mesh vertex count.
  size_t vertexCount() const;

  /// Get the number of triangles in the generated mesh.
  ///
  /// Only valid when @c buildMesh() is successful.
  ///
  /// @return The mesh triangle count.
  size_t triangleCount() const;

  /// Get the vertex array of the generated mesh. The vertex count is given by @c vertexCount().
  ///
  /// Only valid when @c buildMesh() is successful.
  ///
  /// @return A pointer to the vertex array.
  const glm::dvec3 *vertices() const;

  /// Get the vertex normal array of the generated mesh. The vertex count is given by @c vertexCount().
  ///
  /// Note: vertex normals are single precision.
  ///
  ///
  /// Only valid when @c buildMesh() is successful.
  ///
  /// @return A pointer to the vertex array.
  const glm::vec3 *vertexNormals() const;

  /// Get the triangle normal array of the generated mesh. The triangle count is given by @p triangleCount().
  ///
  /// Note: triangle normals are single precision.
  ///
  ///
  /// Only valid when @c buildMesh() is successful.
  ///
  /// @return A pointer to the vertex array.
  const glm::vec3 *triangleNormals() const;

  /// Get triangle index array of the generated mesh. The number of triangles is given by @c triangleCount()
  ///
  /// The returned array comes in index triples (three vertices per triangle), so the number of elements in the
  /// returned array is <tt>3 * triangleCount()</tt>.
  ///
  /// Only valid when @c buildMesh() is successful.
  ///
  /// @return A pointer to the triangle index array.
  const unsigned *triangles() const;

  /// Get per triangle neighbour information. The number of triangles is given by @c triangleCount().
  ///
  /// Only valid when @c buildMesh() is successful.
  ///
  /// @return A pointer to the triangle index array.
  const TriangleNeighbours *triangleNeighbours() const;

  /// Get the axis aligned bounding box enclosing the resulting mesh and the 2D grid of voxels which generated it.
  /// The box will be slightly larger than the mesh to enclose the 2D voxel grid which generated it. The height axis
  /// tightly encloses the vertices used to generate the mesh.
  ///
  /// Only valid when @c buildMesh() is successful.
  ///
  /// @return The mesh axis aligned bounding box.
  const Aabb &meshBoundingBox() const;

  /// Get the axis aligned bounding box which tightly encloses the vertices of the mesh. Whereas @c meshBoundingBox()
  /// encloses the generating voxels, this version tightly encompasses the vertices.
  ///
  /// Only valid when @c buildMesh() is successful.
  ///
  /// @return The mesh axis aligned bounding box.
  const Aabb &tightMeshBoundingBox() const;

  /// Extract the heightmap mesh into a @c PlyMesh object for saving to file.
  ///
  /// Note the resulting mesh only supports single precision, so the default behaviour is to relocate all vertices
  /// such that they are relative to the @c meshBoundingBox() minimum extents. To preserve the original coordinates,
  /// pass false to @c offset_by_extents.
  ///
  /// @param[in,out] mesh The mesh object to populate. Mesh cleared first.
  /// @param offset_by_extents When true, relocate vertices relative to @c meshBoundingBox() minimum extents.
  /// @return True on success, false when empty.
  bool extractPlyMesh(PlyMesh &mesh, bool offset_by_extents = true);

private:
  std::unique_ptr<HeightmapMeshDetail> imp_;
};
}  // namespace ohm

#endif  // OHMHEIGHTMAP_HEIGHTMAPMESH_H


================================================
FILE: ohmheightmap/HeightmapMode.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "HeightmapMode.h"

#include <array>

namespace ohm
{
namespace
{
static const std::array<std::string, 4> mode_names =  //
  {
    //
    "planar",             //
    "fill",               //
    "layered-unordered",  //
    "layered",            //
  };
}

std::string heightmapModeToString(HeightmapMode mode)
{
  if (size_t(mode) < mode_names.size())
  {
    return mode_names[int(mode)];
  }

  return "<unknown>";
}
HeightmapMode heightmapModeFromString(const std::string &str, bool *valid_string)
{
  int mode_index = 0;
  for (const auto &name : mode_names)
  {
    if (str == name)
    {
      if (valid_string)
      {
        *valid_string = true;
      }
      return HeightmapMode(mode_index);
    }
    ++mode_index;
  }

  if (valid_string)
  {
    *valid_string = false;
  }
  return HeightmapMode::kPlanar;
}
}  // namespace ohm


================================================
FILE: ohmheightmap/HeightmapMode.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef OHMHEIGHTMAP_HEIGHTMAPMODE_H
#define OHMHEIGHTMAP_HEIGHTMAPMODE_H

#include "OhmHeightmapConfig.h"

#include <string>

namespace ohm
{
/// Enumeration of the available heightmap generation modes.
enum class HeightmapMode
{
  /// Simple planar traversal from the reference position. Heightmap grid cells are only visited once each.
  kPlanar,
  /// Use a simple flood fill out from the reference position. The resulting heightmap may have better continuity than
  /// @c kPlanar. Voxels may be revisited when a lower candidate voxel is found.
  kSimpleFill,
  /// Use a flood fill which supports layering. The fill attempts to expand on all available surfaces and can generate
  /// a multi layer heightmap. The resulting heightmap is 2.5D and each column can contain multiple entries. The height
  /// values of entries in each column are unsorted with undefined height ordering.
  kLayeredFillUnordered,
  /// Same as @c kLayeredFillUnordered except that the resulting heightmap ensures each column of voxels in the
  /// generated heightmap are in ascending height order.
  kLayeredFill,

  /// First mode value
  kFirst = kPlanar,
  /// Last mode value.
  kLast = kLayeredFill
};

/// Convert a @c HeightmapMode to a string.
/// @param mode The mode value to convert.
/// @return The string name for @p mode or `"<unknown>"` if @p mode is out of range.
std::string ohmheightmap_API heightmapModeToString(HeightmapMode mode);
/// Convert a string to a @c HeightampMode - reverse of @c heightmapModeToString().
/// @param str The string to convert.
/// @param valid_string Optional - set to true if @p str is a valid mode name, false otherwise.
/// @return The mode matching @p str or @c HeightmapMode::kPlanar when @p str is not a valid mode name.
HeightmapMode ohmheightmap_API heightmapModeFromString(const std::string &str, bool *valid_string = nullptr);
}  // namespace ohm

#endif  // OHMHEIGHTMAP_HEIGHTMAPMODE_H


================================================
FILE: ohmheightmap/HeightmapSerialise.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "HeightmapSerialise.h"

#include "Heightmap.h"

#include "private/HeightmapDetail.h"

#include <ohm/MapInfo.h>
#include <ohm/OccupancyMap.h>

namespace
{
struct RegisterExtensionCodes
{
  RegisterExtensionCodes()
  {
    ohm::registerSerialiseExtensionErrorCodeString(int(ohm::kSeHeightmapInfoMismatch), "heightmap info mismatch");
  }
};

const RegisterExtensionCodes s_register_heightmap_errors;
}  // namespace

namespace ohm
{
int load(const std::string &filename, Heightmap &heightmap, SerialiseProgress *progress, MapVersion *version_out)
{
  HeightmapDetail &detail = *heightmap.detail();

  int err = load(filename, *detail.heightmap, progress, version_out);
  if (err)
  {
    return err;
  }

  // TODO(KS): Set axis from map info.
  const MapInfo &info = detail.heightmap->mapInfo();
  if (!bool(info.get("heightmap")))
  {
    return kSeHeightmapInfoMismatch;
  }

  detail.fromMapInfo(info);

  return err;
}
}  // namespace ohm


================================================
FILE: ohmheightmap/HeightmapSerialise.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_HEIGHTMAPMAPSERIALISE_H
#define OHMHEIGHTMAP_HEIGHTMAPMAPSERIALISE_H

#include "OhmHeightmapConfig.h"

#include <ohm/MapSerialise.h>

#include <glm/fwd.hpp>

#include <cinttypes>
#include <string>

namespace ohm
{
class Heightmap;

/// An enumeration of potential serialisation errors.
enum HeightmapSerialisationError
{
  /// @c MapInfo does not represent a heightmap.
  kSeHeightmapInfoMismatch = kSeExtensionCode + 1,
};

/// Load a save heightmap into a @c Heightmap object. Saving can be done directly on the @c Occupancy map stored in
/// @c Heightmap::heightmap() .
/// @param filename The heightmap file path to load.
/// @param heightmap The heightmap object to load into.
/// @param progress Optional progress reporting interface.
/// @param[out] version_out Set to the map file version number if provided.
int ohmheightmap_API load(const std::string &filename, Heightmap &heightmap, SerialiseProgress *progress = nullptr,
                          MapVersion *version_out = nullptr);
}  // namespace ohm

#endif  // OHMHEIGHTMAP_HEIGHTMAPMAPSERIALISE_H


================================================
FILE: ohmheightmap/HeightmapUtil.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "HeightmapUtil.h"

#include "private/HeightmapDetail.h"

#include "HeightmapVoxel.h"

#include <ohm/DefaultLayer.h>
#include <ohm/MapInfo.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/OccupancyMap.h>
#include <ohm/VoxelLayout.h>

#include <cassert>
#include <sstream>

namespace ohm
{
namespace heightmap
{
int setupHeightmap(ohm::OccupancyMap &heightmap, HeightmapDetail &detail)
{
  // Setup the heightmap voxel layout.
  MapLayout &layout = heightmap.layout();

  layout.filterLayers({ default_layer::occupancyLayerName(), default_layer::meanLayerName() });

  MapLayer *layer;
  VoxelLayout voxels;

  layer = layout.addLayer(HeightmapVoxel::kHeightmapLayer, 0);
  int layer_index = static_cast<int>(layer->layerIndex());
  voxels = layer->voxelLayout();
  voxels.addMember("height", DataType::kFloat, 0);
  voxels.addMember("clearance", DataType::kFloat, 0);
  voxels.addMember("normal_x", DataType::kFloat, 0);
  voxels.addMember("normal_y", DataType::kFloat, 0);
  voxels.addMember("normal_z", DataType::kFloat, 0);
  voxels.addMember("layer", DataType::kUInt8, 0);
  voxels.addMember("flags", DataType::kUInt8, 0);
  voxels.addMember("contributing_samples", DataType::kUInt16, 0);

  // Apply any padding bytes as "reserved_N".
  int r = 0;
  while (voxels.voxelByteSize() < sizeof(HeightmapVoxel))
  {
    std::ostringstream name_stream;
    name_stream << "reserved" << r++;
    const auto name = name_stream.str();
    voxels.addMember(name.c_str(), DataType::kUInt8, 0);
  }
  assert(voxels.voxelByteSize() == sizeof(HeightmapVoxel));

  detail.toMapInfo(heightmap.mapInfo());

  return layer_index;
}

UpAxis queryHeightmapAxis(const MapInfo &info)
{
  const MapValue value = info.get("heightmap-axis");
  if (value.isValid())
  {
    return UpAxis(int(value));
  }

  return UpAxis::kZ;
}

double queryHeightmapClearance(const MapInfo &info)
{
  const MapValue value = info.get("heightmap-clearance");
  if (value.isValid())
  {
    return double(value);
  }

  return 0.0;
}

std::array<int, 3> heightmapAxisIndices(UpAxis up_axis)
{
  std::array<int, 3> axis_indices;
  switch (up_axis)
  {
  case UpAxis::kX:
    /* fallthrough */
  case UpAxis::kNegX:
    axis_indices[0] = 1;
    axis_indices[1] = 2;
    axis_indices[2] = 0;
    break;
  case UpAxis::kY:
    /* fallthrough */
  case UpAxis::kNegY:
    axis_indices[0] = 0;
    axis_indices[1] = 2;
    axis_indices[2] = 1;
    break;
  default:
  case UpAxis::kZ:
    /* fallthrough */
  case UpAxis::kNegZ:
    axis_indices[0] = 0;
    axis_indices[1] = 1;
    axis_indices[2] = 2;
    break;
  }

  return axis_indices;
}
}  // namespace heightmap
}  // namespace ohm


================================================
FILE: ohmheightmap/HeightmapUtil.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_HEIGHTMAPUTIL_H
#define OHMHEIGHTMAP_HEIGHTMAPUTIL_H

#include "OhmHeightmapConfig.h"

#include "UpAxis.h"

#include <ohm/Key.h>

namespace ohm
{
struct HeightmapDetail;
class OccupancyMap;
class MapInfo;

/// This namespace contains utility functions used to help build and maintain heightmap instances.
///
/// These functions are intended to only be used as part of the internal ohm API.
namespace heightmap
{
/// Initialise the @p heightmap voxel layout.
int ohmheightmap_API setupHeightmap(ohm::OccupancyMap &heightmap, HeightmapDetail &detail);

/// Query the heightmap axis configured in the given @c MapInfo.
/// @param info The map info data to query.
/// @return The configured heigthmap axis or @c UpAxis::kZ if nothing is configured.
UpAxis ohmheightmap_API queryHeightmapAxis(const MapInfo &info);

/// Query the configured heightmap clearance value.
/// @param info The map info data to query.
/// @return The configured clearance used to generate the heightmap. Zero if not present.
double ohmheightmap_API queryHeightmapClearance(const MapInfo &info);

/// Resolve an array of indices which may be used to reference XYZ axes from a given heightmap up axis.
///
/// Each element in the returned array indexes one of the {X, Y, Z} axes as {0, 1, 2} respectively. The resulting
/// array is constructed such that the element at index 2 always matches the axis specified by @p up, while the other
/// two elements - at indices {0, 1} - define the axes of the heightmap plane.
///
/// Given the statement `auto array = heightmapAxisIndices(up_axis)`, the full set of possible results is tabulated
/// below;
///
/// `up_axis`       | `array[0]`  | `array[1]`  | `array[2]`  |
/// --------------- | ----------: | ----------: | ----------: |
/// `UpAxis::kX     | 1           | 2           | 0           |
/// `UpAxis::kNegX  | 1           | 2           | 0           |
/// `UpAxis::kY     | 0           | 2           | 1           |
/// `UpAxis::kNegY  | 0           | 2           | 1           |
/// `UpAxis::kZ     | 0           | 1           | 2           |
/// `UpAxis::kNegX  | 0           | 1           | 2           |
///
/// @param up_axis The up axis to generate a set of indices for.
/// @return A set of indices which can be used to reference axes as described above.
std::array<int, 3> ohmheightmap_API heightmapAxisIndices(UpAxis up_axis);
}  // namespace heightmap
}  // namespace ohm

#endif  // OHMHEIGHTMAP_HEIGHTMAPUTIL_H


================================================
FILE: ohmheightmap/HeightmapVoxel.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "HeightmapVoxel.h"

namespace ohm
{
const char *const HeightmapVoxel::kHeightmapLayer = "heightmap";
const char *const HeightmapVoxel::kHeightmapBuildLayer = "heightmap_build";
}  // namespace ohm


================================================
FILE: ohmheightmap/HeightmapVoxel.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_HEIGHTMAPVOXEL_H
#define OHMHEIGHTMAP_HEIGHTMAPVOXEL_H

#include "OhmHeightmapConfig.h"

#include <cstdint>

namespace ohm
{
/// Voxel layer values for @c HeightmapVoxel::type.
enum HeightmapVoxelLayer : uint8_t
{
  /// A surface (candidate) voxel. For non-layered heightmaps, this is always used. For layered heightmaps this
  /// value is used during construction to indicate there are observations above the voxel. A @c kLayeredFill heightmap
  /// ensure at most one voxel per column is marked as @c kHvlBaseLayer with other voxels marked @c kHvlExtended .
  kHvlBaseLayer = 0,
  /// A layered heightmap voxel could be an extended surface layer. A @c kLayeredFill heightmap marks all bar one voxels
  /// in a column with this value marking the other @c kHvlBaseLayer as a best guess preferred surface. Columns
  /// will not have this value.
  /// @c kHvlExtended if there are no observations above it.
  kHvlExtended,
  kHvlInvalid  ///< Voxel has been determined to be invalid and should be removed.
};

/// Flag values for @c HeightmapVoxel::flags
enum HeightmapVoxelFlag : uint8_t
{
  /// Indicates that the voxel has some amount of clear/free space above it. Most of the time the clearance yields the
  /// same information. However, consider the following figure: (section view)
  ///
  /// @code{.unparsed}
  ///
  ///              --------     surface B
  ///             /
  ///            /
  /// ----------------------    surface A
  /// @endcode
  ///
  /// Where surface A lies below surface B, the voxels will have an appropriate clearance value. However, surface B
  /// may have a zero clearance value - no known obstructions - when there are no observations above if. This can occur
  /// in an outdoor setting as a sky generates no returns.
  ///
  /// If surface B is properly observed, then we can expect some free voxels above each surface occupied voxel which are
  /// generated by the sensor rays coming from above to make those observations. However, this may fail at very shallow
  /// angles of incidence.
  ///
  /// Note this flag is only assessed to a limited range above the heighmap voxel, limited by the heightmap ceiling
  /// range.
  kHvfObservedAbove = (1 << 0)
};

/// A voxel within the heightmap.
///
/// @note Use of @c alignas will need to be removed should this structure be needed in GPU code. OpenCL 1.2 supports
/// `__attribute__ ((aligned (n)))` style alignment. A macro may be used to manage this.
/// See https://www.khronos.org/registry/OpenCL/sdk/1.2/docs/man/xhtml/
///
/// CUDA compatibility would also need to be assessed.
///
/// @internal Note(KS): ohmheightmap_API is not added to HeightmapVoxel. This fails to comple with GCC 7 when
/// __attribute__ is also specified. Just removed as library export is not needed for a POD struct (other than the
/// static member which we explicitly tag for export).
struct alignas(8) HeightmapVoxel
{
  /// The name of the layer which stores these voxels.
  static const char ohmheightmap_API *const kHeightmapLayer;
  /// The name of the layer used to build the first pass heightmap. This is the layer without blur.
  /// Only used when using blur.
  static const char ohmheightmap_API *const kHeightmapBuildLayer;

  /// The voxel height, relative to the voxel centre.
  float height;
  /// Clearance above the voxel height to the next, occupied voxel. Zero indicates no known overhead obstruction.
  float clearance;
  /// Local surface normal X coordinate.
  ///
  /// The three normal coordinates are only valid when the heightmap has been generated from an occupancy map
  /// which includes @c CovarianceVoxel . The normal is an estimate from the covariance.
  float normal_x;
  /// Local surface normal Y coordinate.
  float normal_y;
  /// Local surface normal Z coordinate.
  float normal_z;
  /// Layer information. Currently restricted to values of @c HeightmapVoxelLayer.
  uint8_t layer;
  /// Alignment padding to add @c contributing_samples without changing the data structure.
  uint8_t flags;
  /// The number of samples in the source voxel which contributed to this result - only available if @c VoxelMean
  /// is available to accumulate the sample count. Note that this may be lower than the value in @c VoxelMean
  /// because it is of lower precision. In this case, the value is set to @c 0xffffu.
  uint16_t contributing_samples;
};
}  // namespace ohm

#endif  // OHMHEIGHTMAP_HEIGHTMAPVOXEL_H


================================================
FILE: ohmheightmap/HeightmapVoxelType.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_HEIGHTMAP_VOXEL_TYPE_H_
#define OHMHEIGHTMAP_HEIGHTMAP_VOXEL_TYPE_H_

#include "OhmHeightmapConfig.h"

namespace ohm
{
/// Type enumeration for voxels in a heightmap.
enum class HeightmapVoxelType : uint8_t
{
  /// Unkown or unobserved voxel.
  kUnknown = 0,
  /// A voxel which is being kept forcibly vacant. Normally this represents voxels which have yet to be able to
  /// observed at the startup location.
  kVacant,
  /// Represents a real surface voxel in the heightmap.
  kSurface,
  /// Represents a virtual surface voxel in the heightmap. Virtual surfaces are created at the interface between
  /// free and unknown voxels (free supported by unknown).
  kVirtualSurface,
  /// A voxel which has been inferred as fatal. This occurs where virtual surfaces appear near the reference point.
  kInferredFatal,
  /// An otherwise forcibly fatal cost voxel.
  kFatal
};
}  // namespace ohm

#endif  // OHMHEIGHTMAP_HEIGHTMAP_VOXEL_TYPE_H_


================================================
FILE: ohmheightmap/OhmHeightmapConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMHEIGHTMAPCONFIG_H
#define OHMHEIGHTMAPCONFIG_H

#include "OhmHeightmapExport.h"

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

#include <cmath>

#include <ohm/OhmConfig.h>

#endif  // OHMHEIGHTMAPCONFIG_H


================================================
FILE: ohmheightmap/PlaneFillLayeredWalker.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "PlaneFillLayeredWalker.h"

#include "HeightmapUtil.h"

#include <ohm/Key.h>
#include <ohm/OccupancyMap.h>

#include <algorithm>

namespace ohm
{
PlaneFillLayeredWalker::PlaneFillLayeredWalker(const OccupancyMap &map, const Key &min_ext_key, const Key &max_ext_key,
                                               UpAxis up_axis)
  : map(map)
  , range(KeyRange(min_ext_key, max_ext_key, map))
  , key_range(map.rangeBetween(min_ext_key, max_ext_key) + glm::ivec3(1, 1, 1))
  , axis_indices(ohm::heightmap::heightmapAxisIndices(up_axis))
  , up_sign((int(up_axis) >= 0) ? 1 : -1)
{}


bool PlaneFillLayeredWalker::begin(Key &key)
{
  // Clear existing data.
  open_list_.clear();
  touched_grid_.clear();
  touched_list_.clear();

  if (range.isValid())
  {
    // Size the 2D grid (fixed size)
    touched_grid_.resize(size_t(key_range[axis_indices[0]]) * size_t(key_range[axis_indices[1]]));
    // Reserve the open_list_ with double the initial capacity of the grid.
    touched_list_.reserve(2 * touched_grid_.size());

    // Clear the grid.
    std::fill(touched_grid_.begin(), touched_grid_.end(), 0u);

    // Ensure the key is in range.
    key.clampTo(range.minKey(), range.maxKey());

    return true;
  }

  return false;
}


bool PlaneFillLayeredWalker::walkNext(Key &key)
{
  // Pop the open list.
  if (!open_list_.empty())
  {
    key = open_list_.front();
    open_list_.pop_front();
    return true;
  }

  return false;
}


size_t PlaneFillLayeredWalker::visit(const Key &key, PlaneWalkVisitMode mode, std::array<Key, 8> &added_neighbours)
{
  size_t added = 0;
  if (mode != PlaneWalkVisitMode::kIgnoreNeighbours)
  {
    Key start_key = key;
    for (int row_delta = -1; row_delta <= 1; ++row_delta)
    {
      for (int col_delta = -1; col_delta <= 1; ++col_delta)
      {
        Key n_key = start_key;
        map.moveKeyAlongAxis(n_key, axis_indices[1], row_delta);
        map.moveKeyAlongAxis(n_key, axis_indices[0], col_delta);

        const auto idx = gridIndexForKey(n_key);
        if (idx != ~0u && (row_delta != 0 || col_delta != 0))
        {
          const int n_visit_height = keyHeight(n_key);
          if (mode == PlaneWalkVisitMode::kAddUnvisitedNeighbours && !hasTouched(idx, n_visit_height) ||
              mode == PlaneWalkVisitMode::kAddUnvisitedColumnNeighbours && !hasTouched(idx))
          {
            // Neighbour in range and not touched. Add to touched and open lists.
            touch(idx, n_visit_height);
            open_list_.push_back(n_key);
            assert(added < added_neighbours.size());
            added_neighbours[added] = n_key;
            ++added;
          }
        }
      }
    }
  }

  return added;
}


unsigned PlaneFillLayeredWalker::gridIndexForKey(const Key &key)
{
  // Get the offset for the key.
  const auto offset_to_key = map.rangeBetween(range.minKey(), key);

  if (offset_to_key[axis_indices[0]] >= 0 && offset_to_key[axis_indices[1]] >= 0 &&
      offset_to_key[axis_indices[0]] < key_range[axis_indices[0]] &&
      offset_to_key[axis_indices[1]] < key_range[axis_indices[1]])
  {
    return unsigned(offset_to_key[axis_indices[0]]) +
           unsigned(offset_to_key[axis_indices[1]]) * unsigned(key_range[axis_indices[0]]);
  }

  // Key out of range.
  return ~0u;
}


int PlaneFillLayeredWalker::keyHeight(const Key &key) const
{
  return map.rangeBetween(range.minKey(), key)[axis_indices[2]];
}


void PlaneFillLayeredWalker::touch(int grid_index, int visit_height)
{
  touched_list_.emplace_back();
  Touched &new_touch = touched_list_.back();
  new_touch.height = visit_height;
  new_touch.next = touched_grid_[grid_index];
  // Note: it is correct to use the touched_list_.size() to find the index of the new item as we are using a 1-based
  // index.
  touched_grid_[grid_index] = unsigned(touched_list_.size());
}


bool PlaneFillLayeredWalker::hasTouched(int grid_index, int visit_height) const
{
  // Traverse the linked list of items for this grid index.
  // Get the item head.
  unsigned current = touched_grid_[grid_index];
  // current is a 1-based index in to touched_list_. Zero is a terminating value.
  while (current > 0)
  {
    const Touched &touched = touched_list_[current - 1];
    if (touched.height == visit_height)
    {
      return true;
    }
    current = touched.next;
  }

  return false;
}
}  // namespace ohm


================================================
FILE: ohmheightmap/PlaneFillLayeredWalker.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_PLANEFILLLAYEREDWALKER_H
#define OHMHEIGHTMAP_PLANEFILLLAYEREDWALKER_H

#include "OhmHeightmapConfig.h"

#include "PlaneWalkVisitMode.h"
#include "UpAxis.h"

#include <ohm/Key.h>
#include <ohm/KeyRange.h>

#include <glm/vec3.hpp>

#include <array>
#include <cinttypes>
#include <deque>
#include <vector>

namespace ohm
{
class Key;
class OccupancyMap;

/// Helper class for walking heightmap generation in a floodfill pattern with the intention of generating a multi-layer
/// heightmap.
///
/// Manages walking the correct axis based on the @c UpAxis .
///
/// Usage:
/// - Initialise the walker and a key value to start iteration from.
/// - call @c begin(key) with the initial key value. The @p key will be clamped to the range.
/// - Start loop:
///   - do work on key, refining it to be the actual key of interest.
///   - call @c visit(key) - where @c key value may be a refinement of the original key. This will push it's neighbours.
///   - call @c walkNext(key) and loop if true.
class ohmheightmap_API PlaneFillLayeredWalker
{
public:
  /// Entry used to track node visiting in the @c touched_list_ . The @c height is the minimum height at which a voxel
  /// has been visited as a vertical offset from the @c minKey() . A negative value indices no visit, a zero or positive
  /// value indicates the offset from the @c minKey() at which the node was visited.
  ///
  /// The @c next value creates a linked list to manage multiple visitations to the same 2D cell location.
  struct Touched
  {
    /// Open list type.
    using List = std::vector<Touched>;

    /// Height at which the cell has been visited.
    int height;
    /// A pseudo linked next next pointer - represents the next index into the visit list using 1-based indexing. This
    /// makes zero a null value.
    unsigned next;
  };

  const OccupancyMap &map;     ///< Map to walk voxels in.
  const KeyRange range;        ///< Specifies the key extents to visit. The up axis is used to limit visiting heights.
  const glm::ivec3 key_range;  ///< The key range covered by @c range.
  /// Mapping of the indices to walk, supporting various heightmap up axes. Element 2 is always the up axis, where
  /// elements 0 and 1 are the horizontal axes.
  const std::array<int, 3> axis_indices;
  /// Sign of the up axis [1, -1].
  const int up_sign;

  /// Constructor.
  /// @param map The map to walk voxels in.
  /// @param min_ext_key The minimal extents to visit.
  /// @param max_ext_key The maximal extents to visit.
  /// @param up_axis Defines the up axis for the plane being visited.
  PlaneFillLayeredWalker(const OccupancyMap &map, const Key &min_ext_key, const Key &max_ext_key, UpAxis up_axis);

  /// Query the minimum key value to fill to.
  /// @return The mimimum key value.
  inline const Key &minKey() const { return range.minKey(); }
  /// Query the maximum key value to fill to.
  /// @return The maximum key value.
  inline const Key &maxKey() const { return range.maxKey(); }

  /// Begin walking keys starting from the given @p key .
  ///
  /// After calling @c begin() , it is expected that the key value will be processed and possibly modified along the
  /// up axis. For example in heightmap generation, the key may initially define the seed key for finding a valid
  /// surface. The surface itself may appear at a different height. It is the modified height key which should be passed
  /// to @c visit() before calling @c walkNext() . In the case where no surface is found, the unmodified key may be
  /// given to @c visit() .
  ///
  /// @param[in,out] key The seed key for walking. The key will be clamped to @c range .
  /// @return True if the range defines a valid region to walk.
  bool begin(Key &key);

  /// Walk the next key in the sequence.
  /// @param[in,out] key Modifies to be the next key to be walked.
  /// @return True if the key is valid, false if walking is complete.
  bool walkNext(Key &key);

  /// Call this function when visiting a voxel at the given @p key. The keys neighbouring @p key (on the walk plane)
  /// are added to the open list, provided they are not already on the open list. The added neighbouring keys are
  /// filled in @p neighbours with the number of neighbours added given in the return value.
  ///
  /// Note: when @p mode is @c kAddUnvisitedNeighbours the neighbours in the layer *above* @c key are added. This
  /// encourages traversal up slopes connected to the ground especially when there is no clearance constraint.
  /// @p mode should be @c kAddUnvisitedColumnNeighbours when a valid ground candidate cound not be found in the column
  /// of @p key . This mode will add neighbours on the same level as @p key , but only if the column itself has not been
  /// visited. This encourages traversal of unobserved space.
  ///
  /// @param key The key being visited. Must fall within the @c range.minKey() and @c range.maxKey() bounds.
  /// @param mode Affects how to expand neighbours when visiting the voxel at @p key.
  /// @param neighbours Populated with any neighbours of @p key added to the open list.
  /// @return The number of neighbours added.
  size_t visit(const Key &key, PlaneWalkVisitMode mode, std::array<Key, 8> &neighbours);
  /// @overload
  inline size_t visit(const Key &key, PlaneWalkVisitMode mode)
  {
    std::array<Key, 8> discard_neighbours;
    return visit(key, mode, discard_neighbours);
  }

private:
  /// Mark the item at @p grid_index in the @c touched_grid_ as being touched at the given @c visit_height .
  /// @param grid_index An index into @c touched_grid_
  /// @param visit_height The height at which to visit.
  void touch(int grid_index, int visit_height);
  bool hasTouched(int grid_index, int visit_height) const;
  inline bool hasTouched(int grid_index) const { return touched_grid_[grid_index] > 0; }

  /// Calculate the @c touched_grid_ index for the given @p key .
  unsigned gridIndexForKey(const Key &key);
  /// Query the Touched entry height for @p key .
  int keyHeight(const Key &key) const;

  /// Remaining voxels to (re)process. This dequeue is used to fetch the next voxel for processing.
  std::deque<Key> open_list_;
  /// Identifies which bounded keys have been touched. This is an unordered list of the heights at which voxels have
  /// been touched extending the @c touched_grid_ up into a third dimension.
  Touched::List touched_list_;
  /// A grid of 1-based indices into the @c touched_list_. The @c touched_grid_ is sized to match grid of keys defined
  /// by the 2D region from @c range.minKey() to @c range.maxKey(). The values are 1-based indices into the
  /// @c touched_list_ with zero marking a null value.
  ///
  /// A cell is touched if it has a non-zero value. The visit height can be found by following the index into the
  /// @c touched_list_. Multiple visits can be followed by chaining indices using @c Touched::next.
  std::vector<int> touched_grid_;
};
}  // namespace ohm

#endif  // OHMHEIGHTMAP_PLANEFILLLAYEREDWALKER_H


================================================
FILE: ohmheightmap/PlaneFillWalker.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "PlaneFillWalker.h"

#include "HeightmapUtil.h"

#include <ohm/OccupancyMap.h>

#include <algorithm>

namespace ohm
{
PlaneFillWalker::PlaneFillWalker(const OccupancyMap &map, const Key &min_ext_key, const Key &max_ext_key,
                                 UpAxis up_axis, Revisit revisit_behaviour)
  : map(map)
  , min_ext_key(min_ext_key)
  , max_ext_key(max_ext_key)
  , key_range(map.rangeBetween(min_ext_key, max_ext_key) + glm::ivec3(1, 1, 1))
  , axis_indices(ohm::heightmap::heightmapAxisIndices(up_axis))
  , revisit_behaviour(revisit_behaviour)
{}


bool PlaneFillWalker::begin(Key &key)
{
  open_list_.clear();
  visit_grid_.clear();
  visit_grid_.resize(size_t(key_range[axis_indices[0]]) * size_t(key_range[axis_indices[1]]));

  if (visit_grid_.empty())
  {
    // Key out of range.
    return false;
  }

  key.clampTo(min_ext_key, max_ext_key);
  return true;
}


bool PlaneFillWalker::walkNext(Key &key)
{
  // Pop the open list.
  while (!open_list_.empty())
  {
    key = open_list_.front();
    open_list_.pop_front();

    if (glm::all(glm::greaterThan(key_range, glm::ivec3(0))))
    {
      key.clampTo(min_ext_key, max_ext_key);
      const unsigned grid_index = gridIndex(key);
      visit_grid_[grid_index].visit(keyHeight(key));
      return true;
    }
  }

  return false;
}


size_t PlaneFillWalker::visit(const Key &key, PlaneWalkVisitMode mode, std::array<Key, 8> &added_neighbours)
{
  size_t added = 0;

  if (mode != PlaneWalkVisitMode::kIgnoreNeighbours)
  {
    const unsigned grid_index = gridIndex(key);
    if (grid_index != ~0u)
    {
      // Note: we do not update the visit height for key. This can result in recurring loops. We only want to track the
      // best height at which a voxel was added to the open list.
      for (int row_delta = -1; row_delta <= 1; ++row_delta)
      {
        for (int col_delta = -1; col_delta <= 1; ++col_delta)
        {
          Key n_key = key;
          map.moveKeyAlongAxis(n_key, axis_indices[1], row_delta);
          map.moveKeyAlongAxis(n_key, axis_indices[0], col_delta);

          const unsigned n_grid_index = gridIndex(n_key);
          // Skip over the self reference.
          if (n_grid_index != ~0u && n_grid_index != grid_index)
          {
            bool add_to_open = false;
            const int n_visit_height = keyHeight(n_key);
            switch (revisit_behaviour)
            {
            case Revisit::kHigher:
              add_to_open = visit_grid_[n_grid_index].revisitHigher(n_visit_height);
              break;

            case Revisit::kLower:
              add_to_open = visit_grid_[n_grid_index].revisitLower(n_visit_height);
              break;

            case Revisit::kNone:
            default:
              add_to_open = visit_grid_[n_grid_index].revisitNone(n_visit_height);
              break;
            }

            if (add_to_open)
            {
              // Neighbour in range and not touched. Add to open list.
              open_list_.push_back(n_key);
              visit_grid_[n_grid_index].visit(n_visit_height);
              assert(added < added_neighbours.size());
              added_neighbours[added] = n_key;
              ++added;
            }
          }
        }
      }
    }
  }

  return added;
}


void PlaneFillWalker::touch(const Key &key)
{
  const auto idx = gridIndex(key);
  if (idx != ~0u)
  {
    visit_grid_[idx].visit(keyHeight(key));
  }
}


unsigned PlaneFillWalker::gridIndex(const Key &key)
{
  // Get the offset for the key.
  const auto offset_to_key = map.rangeBetween(min_ext_key, key);

  if (offset_to_key[axis_indices[0]] >= 0 && offset_to_key[axis_indices[1]] >= 0 &&
      offset_to_key[axis_indices[0]] < key_range[axis_indices[0]] &&
      offset_to_key[axis_indices[1]] < key_range[axis_indices[1]])
  {
    return unsigned(offset_to_key[axis_indices[0]]) +
           unsigned(offset_to_key[axis_indices[1]]) * unsigned(key_range[axis_indices[0]]);
  }

  // Key out of range.
  return ~0u;
}


int PlaneFillWalker::keyHeight(const Key &key) const
{
  return map.rangeBetween(min_ext_key, key)[axis_indices[2]];
}
}  // namespace ohm


================================================
FILE: ohmheightmap/PlaneFillWalker.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_PLANEFILLWALKER_H
#define OHMHEIGHTMAP_PLANEFILLWALKER_H

#include "OhmHeightmapConfig.h"

#include "PlaneWalkVisitMode.h"
#include "UpAxis.h"

#include <ohm/Key.h>

#include <glm/vec3.hpp>

#include <array>
#include <cinttypes>
#include <deque>
#include <vector>

namespace ohm
{
class Key;
class OccupancyMap;

/// Helper class for walking a plane in the heightmap given any up axis using a flood fill pattern.
/// Manages walking the correct axis based on the @c UpAxis.
///
/// Usage:
/// - Initialise the walker and a key to start iteration from.
/// - call @c begin(key) with the initial key value. The @p key will be clamped to the range.
/// - Start loop:
///   - do work on key, refining it to be the actual key of interest.
///   - call @c visit(key) - where @c key value may be a refinement of the original key. This will push it's neighbours.
///   - call @c walkNext(key) and loop if true.
class ohmheightmap_API PlaneFillWalker
{
public:
  /// Revisit behaviour for @c visit()
  enum class Revisit : int
  {
    kNone = 0,  ///< No revisiting allowed.
    kLower,     ///< Revisit if the key appears lower along the up axis.
    kHigher     ///< Revisit if the key is higher along the up axis.
  };

  /// Entry used to track node visiting in the @c visit_list. This tracks the height at which the cell has been visited
  /// with -1 indicating an unvisited cell. The height is calculated by @c keyHeight() and marks the vertical voxel
  /// offset from the @c minKey().
  struct Visit
  {
    /// Height at which the cell has been visited. The height is an offset from the @c minKey() height. -1 denotes an
    /// unvisited cell.
    int height = -1;

    /// Mark the voxel as being visited at the specified height.
    /// @param visit_height The the visiting height.
    inline void visit(int visit_height) { height = visit_height; }
    /// Query if the voxel has been visited.
    /// @return True if visited.
    inline bool visited() const { return height >= 0; }
    /// Query whether we can revisit a voxel at the given height allowing revisiting if the new height is greater than
    /// any previous visit height.
    /// @param visit_height The new height we wish to visit at.
    /// @return True if the voxel should be revisited.
    inline bool revisitHigher(int visit_height) const { return !visited() || visit_height > height; }
    /// Query whether we can revisit a voxel at the given height allowing revisiting if the new height is less than
    /// any previous visit height.
    /// @param visit_height The new height we wish to visit at.
    /// @return True if the voxel should be revisited.
    inline bool revisitLower(int visit_height) const { return !visited() || visit_height < height; }
    /// For revisiting API compatibility - semantically equivalent to @c visited().
    /// @return True if the voxel can be (re)visited.
    inline bool revisitNone(int /*visit_height*/) const { return !visited(); }
  };

  const OccupancyMap &map;     ///< Map to walk voxels in.
  const Key min_ext_key;       ///< The starting voxel key (inclusive).
  const Key max_ext_key;       ///< The last voxel key (inclusive).
  const glm::ivec3 key_range;  ///< The range between @c min_ext_key and @c max_ext_key .
  /// Mapping of the indices to walk, supporting various heightmap up axes. Element 2 is always the up axis, where
  /// elements 0 and 1 are the horizontal axes.
  const std::array<int, 3> axis_indices;
  /// Defines how to treat attempts to revisit a node.
  Revisit revisit_behaviour = Revisit::kLower;

  /// Constructor.
  /// @param map The map to walk voxels in.
  /// @param min_ext_key The starting voxel key (inclusive).
  /// @param max_ext_key The last voxel key (inclusive).
  /// @param up_axis Specifies the up axis for the map.
  /// @param revisit_behaviour Controls what to do when visiting voxels in a column which has already been visited.
  PlaneFillWalker(const OccupancyMap &map, const Key &min_ext_key, const Key &max_ext_key, UpAxis up_axis,
                  Revisit revisit_behaviour = Revisit::kLower);

  /// Query the minimum key value to walk from.
  /// @return The mimimum key value.
  inline const Key &minKey() const { return min_ext_key; }
  /// Query the maximum key value to walk to.
  /// @return The maximum key value.
  inline const Key &maxKey() const { return max_ext_key; }

  /// Initialise @p key To the first voxel to walk.
  /// @param[out] key Set to the first key to be walked.
  /// @return True if the key is valid, false if there is nothing to walk.
  bool begin(Key &key);

  /// Walk the next key in the sequence.
  /// @param[in,out] key Modifies to be the next key to be walked.
  /// @return True if the key is valid, false if walking is complete.
  bool walkNext(Key &key);

  /// Call this function when visiting a voxel at the given @p key. The keys neighbouring @p key (on the walk plane)
  /// are added to the open list, provided they are not already on the open list. The added neighbouring keys are
  /// filled in @p neighbours with the number of neighbours added given in the return value.
  /// @param key The key being visited. Must fall within the @c range.minKey() and @c range.maxKey() bounds.
  /// @param neighbours Populated with any neighbours of @p key added to the open list.
  /// @param mode Affects how to expand neighbours when visiting the voxel at @p key.
  /// @return The number of neighbours added.
  size_t visit(const Key &key, PlaneWalkVisitMode mode, std::array<Key, 8> &neighbours);
  /// @overload
  inline size_t visit(const Key &key, PlaneWalkVisitMode mode)
  {
    std::array<Key, 8> discard_neighbours;
    return visit(key, mode, discard_neighbours);
  }

  /// Marks the given key as visited.
  /// @param key The key to visit.
  void touch(const Key &key);

private:
  unsigned gridIndex(const Key &key);
  /// Query the Visit entry height for @p key.
  int keyHeight(const Key &key) const;

  std::deque<Key> open_list_;  ///< Remaining voxels to (re)process.
  /// A grid tracking the heights at which each planar item has been visited. The grid is sized for the 2D region from
  /// @c min_ext_key to @c max_ext_key. A height value < 0 indicates not having been visited.
  std::vector<Visit> visit_grid_;
};
}  // namespace ohm

#endif  // OHMHEIGHTMAP_PLANEFILLWALKER_H


================================================
FILE: ohmheightmap/PlaneWalkVisitMode.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_PLANEWALKVISITMODE_H
#define OHMHEIGHTMAP_PLANEWALKVISITMODE_H

#include "OhmHeightmapConfig.h"

namespace ohm
{
/// Mode selection for handling neighbours in various @c PlaneWalker implementations used by @c Heightmap .
/// Determines now neighbours are handled when visiting a node.
enum class PlaneWalkVisitMode
{
  /// Do nothing with neighbours. Not added.
  kIgnoreNeighbours,
  /// Add (planar) neighbours for processing.
  kAddUnvisitedNeighbours,
  /// Add (planar) neighbours for processing as long as they have not yet been visited in a 2D sense.
  ///
  /// This option should be used with @c PlaneFillLayeredWalk when a column cannot find a valid ground candidate. The
  /// layered fill can then traverse unobserved space.
  kAddUnvisitedColumnNeighbours
};
}  // namespace ohm

#endif  // OHMHEIGHTMAP_PLANEWALKVISITMODE_H


================================================
FILE: ohmheightmap/PlaneWalker.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "PlaneWalker.h"

#include "HeightmapUtil.h"

#include <ohm/OccupancyMap.h>

namespace ohm
{
PlaneWalker::PlaneWalker(const OccupancyMap &map, const Key &min_ext_key, const Key &max_ext_key, UpAxis up_axis,
                         const Key *plane_key_ptr)
  : map(map)
  , min_ext_key(min_ext_key)
  , max_ext_key(max_ext_key)
  , plane_key(plane_key_ptr ? *plane_key_ptr : min_ext_key)
  , axis_indices(ohm::heightmap::heightmapAxisIndices(up_axis))
{}


bool PlaneWalker::begin(Key &key) const
{
  key = min_ext_key;
  // Flatten the key onto the plane.
  key.setRegionAxis(axis_indices[2], plane_key.regionKey()[axis_indices[2]]);
  key.setLocalAxis(axis_indices[2], plane_key.localKey()[axis_indices[2]]);
  key.clampToAxis(axis_indices[2], min_ext_key, max_ext_key);
  return true;
}


bool PlaneWalker::walkNext(Key &key) const
{
  map.stepKey(key, axis_indices[0], 1);
  if (!key.isBounded(axis_indices[0], min_ext_key, max_ext_key))
  {
    // Finished walking this axis. Reset and walk outer axis.
    key.setLocalAxis(axis_indices[0], min_ext_key.localKey()[axis_indices[0]]);
    key.setRegionAxis(axis_indices[0], min_ext_key.regionKey()[axis_indices[0]]);

    map.stepKey(key, axis_indices[1], 1);
    if (!key.isBounded(axis_indices[1], min_ext_key, max_ext_key))
    {
      return false;
    }
  }

  return true;
}
}  // namespace ohm


================================================
FILE: ohmheightmap/PlaneWalker.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_PLANEWALKER_H
#define OHMHEIGHTMAP_PLANEWALKER_H

#include "OhmHeightmapConfig.h"

#include "PlaneWalkVisitMode.h"
#include "UpAxis.h"

#include <ohm/Key.h>

#include <array>

namespace ohm
{
class OccupancyMap;

/// Helper class for walking a plane in the heightmap given any up axis.
/// Manages walking the correct axis based on the @c UpAxis.
///
/// Usage:
/// - Initialise
/// - call @c begin().
/// - do work
/// - call @c walkNext() and loop if true.
class ohmheightmap_API PlaneWalker
{
public:
  const OccupancyMap &map;  ///< Map to walk voxels in.
  const Key min_ext_key;    ///< The starting voxel key (inclusive).
  const Key max_ext_key;    ///< The last voxel key (inclusive).
  const Key plane_key;      ///< The key which was used to seed the plane, setting the height.
  /// Mapping of the indices to walk, supporting various heightmap up axes. Element 2 is always the up axis, where
  /// elements 0 and 1 are the horizontal axes.
  const std::array<int, 3> axis_indices;

  /// Constructor.
  /// @param map The map to walk voxels in.
  /// @param min_ext_key The starting voxel key (inclusive).
  /// @param max_ext_key The last voxel key (inclusive).
  /// @param up_axis Specifies the up axis for the map.
  /// @param plane_key_ptr Optional key specification for the @c plane_key .
  PlaneWalker(const OccupancyMap &map, const Key &min_ext_key, const Key &max_ext_key, UpAxis up_axis,
              const Key *plane_key_ptr = nullptr);

  /// Query the minimum key value to walk from.
  /// @return The mimimum key value.
  inline const Key &minKey() const { return min_ext_key; }
  /// Query the maximum key value to walk to.
  /// @return The maximum key value.
  inline const Key &maxKey() const { return max_ext_key; }

  /// Initialise @p key To the first voxel to walk.
  /// @param[out] key Set to the first key to be walked.
  /// @return True if the key is valid, false if there is nothing to walk.
  bool begin(Key &key) const;

  /// Walk the next key in the sequence.
  /// @param[in,out] key Modifies to be the next key to be walked.
  /// @return True if the key is valid, false if walking is complete.
  bool walkNext(Key &key) const;

  /// For API compatibility. Does nothing.
  /// @return 0
  inline size_t visit(const Key & /*key*/, PlaneWalkVisitMode /*mode*/) { return 0u; }  // NOLINT
  /// For API compatibility. Does nothing.
  /// @return 0
  inline size_t visit(const Key & /*key*/, PlaneWalkVisitMode /*mode*/, std::array<Key, 8> & /*neighbours*/)  // NOLINT
  {
    return 0u;
  }
};
}  // namespace ohm

#endif  // OHMHEIGHTMAP_PLANEWALKER_H


================================================
FILE: ohmheightmap/TriangleEdge.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_TRIANGLEEDGE_H
#define OHMHEIGHTMAP_TRIANGLEEDGE_H

#include "OhmHeightmapConfig.h"

#include <algorithm>

namespace ohm
{
/// A helper for tracking edges in mesh generation. Stores edge vertices such that v0 < v1 regardless of the input
/// ordering. This allows the edge array to be sorted to clump the edge pairings where two triangles share an edge.
/// Only two triangles can share one edge because we build the mesh without T intersections.
struct ohmheightmap_API TriangleEdge
{
  unsigned v0;                   ///< The lower magnitude vertex index.
  unsigned v1;                   ///< The higher magnitude vertex index.
  unsigned triangle;             ///< The index of the triangle which generated this edge.
  unsigned triangle_edge_index;  ///< The index of this edge in @p triangle.

  /// Constructor; v0 and v1 will be reordered such that v0 < v1.
  /// @param v0 First vertex index.
  /// @param v1 Second vertex index.
  /// @param triangle Triangle index.
  /// @param edge_index The index of the edge in @p triangle [0, 2].
  inline TriangleEdge(unsigned v0, unsigned v1, unsigned triangle, unsigned edge_index)
    : triangle(triangle)
    , triangle_edge_index(edge_index)
  {
    this->v0 = std::min(v0, v1);
    this->v1 = std::max(v0, v1);
  }

  /// Comparison operator for sorting.
  inline bool operator<(const TriangleEdge &other) const { return v0 < other.v0 || v0 == other.v0 && v1 < other.v1; }
};
}  // namespace ohm

#endif  // OHMHEIGHTMAP_TRIANGLEEDGE_H


================================================
FILE: ohmheightmap/TriangleNeighbours.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_TRIANGLENEIGHBOURS_H
#define OHMHEIGHTMAP_TRIANGLENEIGHBOURS_H

#include "OhmHeightmapConfig.h"

#include <array>
#include <cstdint>

namespace ohm
{
/// Stores the neighbours information for a triangle.
///
/// Identifies the @p neighbour triangle indices (multiply by 3 when indexing HeightmapMesh::triangles()) and the
/// edge ID of the shared edge in the neighbour. The index into @c neighbours [0, 2] identifies the edge ID in the
/// current triangle.
struct ohmheightmap_API TriangleNeighbours
{
  /// Indices to the neighbouring triangles.
  /// ~0u for each open edge (-1).
  ///
  /// Ordered by the shared edge as {v[0], v[1]}, {v[1], v[2]}, {v[2], v[0]}.
  std::array<unsigned, 3> neighbours;
  /// Identifies the shared edge indices in each neighbour triangle. For example, @c neighbour_edge_indices[0]
  /// contains edge information for neighbour[0]. The value of [0, 2] indicates which vertex pairing in neighbour[0]
  /// identifies the shared edge. -1 is used for empty edges.
  std::array<int8_t, 3> neighbour_edge_indices;
};
}  // namespace ohm

#endif  // OHMHEIGHTMAP_TRIANGLENEIGHBOURS_H


================================================
FILE: ohmheightmap/UpAxis.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef UPAXIS_H
#define UPAXIS_H

#include "OhmConfig.h"

namespace ohm
{
/// Up axis identification values. Used for @c Heightmap generation.
///
/// Documentation for each ID identifies the up axis.
enum class UpAxis : int
{
  /// (0, 0, -1)
  kNegZ = -3,
  /// (0, -1, 0)
  kNegY = -2,
  /// (-1, 0, 0)
  kNegX = -1,
  /// (1, 0, 0)
  kX,
  /// (0, 1, 0)
  kY,
  /// (0, 0, 1)
  kZ,
};
}  // namespace ohm

#endif  // UPAXIS_H


================================================
FILE: ohmheightmap/private/HeightmapDetail.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "HeightmapDetail.h"

#include <ohm/MapInfo.h>
#include <ohm/OccupancyMap.h>

#include <array>

namespace ohm
{
HeightmapDetail::~HeightmapDetail() = default;

const glm::dvec3 &HeightmapDetail::upAxisNormal(UpAxis axis_id)
{
  static const std::array<glm::dvec3, 7> axes =  //
    {
      glm::dvec3(0, 0, -1),  // -Z
      glm::dvec3(0, -1, 0),  // -Y
      glm::dvec3(-1, 0, 0),  // -X
      glm::dvec3(1, 0, 0),   // X
      glm::dvec3(0, 1, 0),   // Y
      glm::dvec3(0, 0, 1),   // Z
      glm::dvec3(0, 0, 0),   // Dummy
    };

  int lookup_index = int(axis_id) + 3;
  if (lookup_index < 0 || lookup_index >= int(axes.size()))
  {
    // Reference the dummy index.
    lookup_index = int(axes.size() - 1);
  };

  return axes[lookup_index];
}


int HeightmapDetail::surfaceIndexA(UpAxis up_axis_id)
{
  static const std::array<int, 7> indices =  //
    {
      0,  // -Z
      0,  // -Y
      1,  // -X
      1,  // X
      0,  // Y
      0,  // Z
      0,  // Dummy
    };

  int lookup_index = int(up_axis_id) + 3;
  if (lookup_index < 0 || lookup_index >= int(indices.size()))
  {
    // Reference the dummy index.
    lookup_index = int(indices.size() - 1);
  };

  return indices[lookup_index];
}


const glm::dvec3 &HeightmapDetail::surfaceNormalA(UpAxis axis_id)
{
  static const std::array<glm::dvec3, 7> axes =  //
    {
      glm::dvec3(1, 0, 0),   // -Z
      glm::dvec3(1, 0, 0),   // -Y
      glm::dvec3(0, -1, 0),  // -X
      glm::dvec3(0, 1, 0),   // X
      glm::dvec3(-1, 0, 0),  // Y
      glm::dvec3(1, 0, 0),   // Z
      glm::dvec3(0, 0, 0),   // Dummy
    };

  int lookup_index = int(axis_id) + 3;
  if (lookup_index < 0 || lookup_index >= int(axes.size()))
  {
    // Reference the dummy index.
    lookup_index = int(axes.size() - 1);
  };

  return axes[lookup_index];
}


int HeightmapDetail::surfaceIndexB(UpAxis up_axis_id)
{
  static const std::array<int, 7> indices =  //
    {
      1,  // -Z
      2,  // -Y
      2,  // -X
      2,  // X
      2,  // Y
      1,  // Z
      1,  // Dummy
    };

  int lookup_index = int(up_axis_id) + 3;
  if (lookup_index < 0 || lookup_index >= int(indices.size()))
  {
    // Reference the dummy index.
    lookup_index = int(indices.size() - 1);
  };

  return indices[lookup_index];
}


const glm::dvec3 &HeightmapDetail::surfaceNormalB(UpAxis axis_id)
{
  static const std::array<glm::dvec3, 7> axes =  //
    {
      glm::dvec3(0, -1, 0),  // -Z
      glm::dvec3(0, 0, 1),   // -Y
      glm::dvec3(0, 0, 1),   // -X
      glm::dvec3(0, 0, 1),   // X
      glm::dvec3(0, 0, 1),   // Y
      glm::dvec3(0, 1, 0),   // Z
      glm::dvec3(0, 0, 0),   // Dummy
    };

  int lookup_index = int(axis_id) + 3;
  if (lookup_index < 0 || lookup_index >= int(axes.size()))
  {
    // Reference the dummy index.
    lookup_index = int(axes.size() - 1);
  };

  return axes[lookup_index];
}


void HeightmapDetail::fromMapInfo(const MapInfo &info)
{
  up_axis_id = UpAxis(int(info.get("heightmap-axis")));
  ceiling = double(info.get("heightmap-ceiling"));
  min_clearance = double(info.get("heightmap-clearance"));
  floor = double(info.get("heightmap-floor"));
  ignore_voxel_mean = bool(info.get("heightmap-ignore-voxel-mean"));
  generate_virtual_surface = bool(info.get("heightmap-virtual-surface"));
  mode = HeightmapMode(int(info.get("heightmap-mode")));
  generate_virtual_surface = bool(info.get("heightmap-virtual-surface"));
  virtual_surface_filter_threshold = bool(info.get("heightmap-virtual-surface-filter-threshold"));
  promote_virtual_below = bool(info.get("heightmap-virtual-surface-promote"));
  updateAxis();
}


void HeightmapDetail::toMapInfo(MapInfo &info) const
{
  const std::string mode_name = heightmapModeToString(mode);
  info.set(MapValue("heightmap", true));
  info.set(MapValue("heightmap-axis-x", up.x));
  info.set(MapValue("heightmap-axis-y", up.y));
  info.set(MapValue("heightmap-axis-z", up.z));
  info.set(MapValue("heightmap-axis", int(up_axis_id)));
  info.set(MapValue("heightmap-ceiling", ceiling));
  info.set(MapValue("heightmap-clearance", min_clearance));
  info.set(MapValue("heightmap-floor", floor));
  info.set(MapValue("heightmap-ignore-voxel-mean", ignore_voxel_mean));
  info.set(MapValue("heightmap-mode", int(mode)));
  info.set(MapValue("heightmap-mode-name", mode_name.c_str()));
  info.set(MapValue("heightmap-virtual-surface", generate_virtual_surface));
  info.set(MapValue("heightmap-virtual-surface-filter-threshold", int(virtual_surface_filter_threshold)));
  info.set(MapValue("heightmap-virtual-surface-promote", promote_virtual_below));
}
}  // namespace ohm


================================================
FILE: ohmheightmap/private/HeightmapDetail.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAP_HEIGHTMAPDETAIL_H
#define OHMHEIGHTMAP_HEIGHTMAPDETAIL_H

#include "OhmHeightmapConfig.h"

#include "ohmheightmap/HeightmapMode.h"
#include "ohmheightmap/UpAxis.h"

#include <ohm/Aabb.h>

#include <glm/glm.hpp>

#include <memory>

namespace ohm
{
class OccupancyMap;
class MapInfo;

/// Pimpl data for @c Heightmap .
struct ohmheightmap_API HeightmapDetail
{
  /// Source occupancy map pointer from which we are building a heightmap.
  const OccupancyMap *occupancy_map = nullptr;
  /// Use a very thin occupancy map for the heightmap representation.
  ///
  /// Within the heightmap the occupancy values are used independently of any of the threshold values.
  /// For each voxel the values have the following meanings:
  /// - @c ohm::unobservedOccupancyValue() => unobserved region (standard semantics)
  /// - <tt>value < 0</tt> => virtual surface
  /// - <tt>value == 0</tt> => vacant due to local cache seeding
  /// - <tt>value > 0</tt> => real surface
  ///
  /// The case where @c value is zero indicates that there are no observations for the column and it has not been
  /// able to be observed.
  std::unique_ptr<OccupancyMap> heightmap;
  /// Similar to @c heightmap , this is an occupancy map into which we build a multilayer heightmap.
  std::unique_ptr<OccupancyMap> multilayer_heightmap;
  /// The direct of up used in heightmap generation. Must be aligned to a specific access.
  glm::dvec3 up = glm::dvec3(0, 0, 1);
  /// Ignore all source voxels which lie higher than this above the seed voxel height.
  /// Enable by setting a positive, non zero value.
  double ceiling = 0;
  /// Ignore all source voxels which lie a distance greater than this below the seed voxel height.
  /// Enable by setting a positive, non zero value.
  double floor = 0;
  /// Minimum clearance above a potential ground/surface voxel required to accept the voxel as a viable surface.
  double min_clearance = 1.0;
  /// Voxel layer containing the @c HeightmapVoxel data in the @c heightmap.
  int heightmap_voxel_layer = -1;
  /// Identifies the up axis: @c UpAxis
  UpAxis up_axis_id = UpAxis::kZ;
  /// Identifies the up axis as aligned to XYZ, [0, 2] but ignores sign/direction.
  /// Same as up_axis_id if that value is >= 0.
  int vertical_axis_index = int(UpAxis::kZ);
  /// Enables post process filtering for layered heightmaps, removing virtual surface voxels with fewer 26-connected
  /// populated neighbours.
  unsigned virtual_surface_filter_threshold = 0;
  /// Level of debugging information provided in generating the heightmap. Only has an effect if
  /// @c OHM_TES_DEBUG is configured in CMake.
  int debug_level = 0;
  /// Specifies the heightmap generation mode.
  HeightmapMode mode = HeightmapMode::kPlanar;
  /// Should heightmap generation ignore the presence of voxel mean positions, forcing voxel centres instead?
  bool ignore_voxel_mean = false;
  /// Allow the generation of a virtual heightmap floor around the transition from unknown to free voxels?
  ///
  /// @see @c Heightmap::setGenerateVirtualFloor()
  bool generate_virtual_surface = false;
  /// Prefer a virtual surface below the reference position to a real surface above.
  /// @see @c Heightmap::setPromoteVirtualBelow()
  bool promote_virtual_below = false;

  ~HeightmapDetail();

  void updateAxis();
  static const glm::dvec3 &upAxisNormal(UpAxis axis_id);
  static int surfaceIndexA(UpAxis up_axis_id);
  static const glm::dvec3 &surfaceNormalA(UpAxis axis_id);
  static int surfaceIndexB(UpAxis up_axis_id);
  static const glm::dvec3 &surfaceNormalB(UpAxis axis_id);

  void fromMapInfo(const MapInfo &info);
  void toMapInfo(MapInfo &info) const;
};


inline void HeightmapDetail::updateAxis()
{
  up = upAxisNormal(up_axis_id);
  vertical_axis_index = (int(up_axis_id) >= 0) ? int(up_axis_id) : -(int(up_axis_id) + 1);
}
}  // namespace ohm

#endif  // OHMHEIGHTMAP_HEIGHTMAPDETAIL_H


================================================
FILE: ohmheightmap/private/HeightmapOperations.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "HeightmapOperations.h"

#include "HeightmapDetail.h"

#include "ohmheightmap/Heightmap.h"  // For TES_ENABLE
#include "ohmheightmap/HeightmapVoxelType.h"

#include <ohm/OccupancyUtil.h>
#include <ohm/Trace.h>  // For TES_ENABLE

#define PROFILING 0
#include <ohmutil/Profile.h>

#include <3esservermacros.h>

#include <glm/gtc/type_ptr.hpp>  // For TES_ENABLE

namespace ohm
{
namespace heightmap
{
#ifdef TES_ENABLE
const uint32_t kNeighbourIdMask = 0x80000000u;
#endif  // TES_ENABLE


bool DstVoxel::haveRecordedHeight(double height, int up_axis_index, const glm::dvec3 &up) const
{
  // Check if the current heightmap voxel has already recorded a result at the given height. Only call for valid
  // voxels from multi-layered heightmaps.

  // Create a voxel for iteration. Seed from this.
  const double epsilon = 1e-3 * occupancy.map()->resolution();
  DstVoxel walker;
  walker.occupancy = occupancy;
  walker.heightmap = heightmap;
  // Not interested in mean

  Key key = occupancy.key();
  double voxel_height;
  while (walker.occupancy.isValid() && walker.occupancy.data() != ohm::unobservedOccupancyValue())
  {
    // Convert from voxel relative to absolute height. This can introduce floating point error.
    voxel_height = walker.heightmap.data().height;
    voxel_height += glm::dot(walker.occupancy.map()->voxelCentreGlobal(walker.occupancy.key()), up);
    if (std::abs(voxel_height - height) < epsilon)
    {
      return true;
    }

    // Next voxel in the column.
    occupancy.map()->moveKeyAlongAxis(key, up_axis_index, 1);
    walker.setKey(key);
  }

  return false;
}


uint32_t DstVoxel::get3esVoxelId(const Key &heightmap_key, const OccupancyMap &heightmap, int up_axis_index)
{
  // Generate an ID for the voxel.
  // For this we take the extents of the occupancy map (in this case the heightmap) and assign an ID based on the
  // voxel position in that range.
  KeyRange heightmap_range;
  heightmap.calculateExtents(nullptr, nullptr, &heightmap_range);
  // Note: the vertical extents of the heightmap can change for a layered map as we add layers. We cater for this
  // here by ensuring the vertical extents of heightmap_range is padded out to allow for 32 layers. This effectively
  // reserves 5 bits for vertical indexing, 1 bits masking open list voxels, leaving 26 for horizontal indexing. This
  // supports a 2D region of approximately 8Kx8K voxels and should support maps which can reasonably be debugged using
  // 3es. Larger maps create excessively large 3es data sets.
  // Anything more really needs to use a tes::MutableMesh object.
  Key adjusted_vertical_key(0, 0, 0, 0, 0, 0);
  heightmap.moveKeyAlongAxis(adjusted_vertical_key, up_axis_index, 32);
  heightmap_range.expand(adjusted_vertical_key);
  return uint32_t(heightmap_range.indexOf(heightmap_key));
}

void DstVoxel::debugDraw(int level, int up_axis_index, double up_scale)
{
  (void)level;
  (void)up_axis_index;
  (void)up_scale;
#ifdef TES_ENABLE
  if (occupancy.isValid() && g_tes)
  {
    uint32_t voxel_id = get3esVoxelId(up_axis_index);
    glm::dvec3 voxel_pos = occupancy.map()->voxelCentreGlobal(occupancy.key());
    voxel_pos[up_axis_index] += up_scale * heightmap.data().height;

    tes::Id box_id(voxel_id, kTcHmSurface);
    tes::Colour box_colour = tes::Colour::Colours[tes::Colour::Green];
    const float voxel_value = occupancy.data();
    if (voxel_value == Heightmap::kHeightmapVirtualSurfaceValue)
    {
      box_id.setCategory(kTcHmVirtualSurface);
      box_colour = tes::Colour::Colours[tes::Colour::Orange];
    }
    else if (voxel_value == Heightmap::kHeightmapVacantValue)
    {
      box_id.setCategory(kTcHmVacant);
      box_colour = tes::Colour::Colours[tes::Colour::LightSlateGrey];
    }

    tes::Box voxel(box_id, tes::Transform(glm::value_ptr(voxel_pos), tes::Vector3d(occupancy.map()->resolution())));
    voxel.setReplace(true);
    // voxel.setTransparent(true);
    // Colour surface green, virtual orange, vacant grey
    voxel.setColour(box_colour);
    g_tes->create(voxel);

    // Create a line for the clearance height.
    const double clearance_height = heightmap.data().clearance;
    if (clearance_height > 0)
    {
      glm::dvec3 clearance_dir(0);
      clearance_dir[up_axis_index] = up_scale;

      tes::Arrow clearance(tes::Id(voxel_id, kTcHmClearance),
                           tes::Directional(tes::Vector3d(glm::value_ptr(voxel_pos)),
                                            tes::Vector3d(glm::value_ptr(clearance_dir)), 0.005, clearance_height));
      clearance.setColour(tes::Colour::Colours[tes::Colour::Yellow]);
      clearance.setReplace(true);
      g_tes->create(clearance);
    }
  }
#endif  // TES_ENABLE
}

bool BaseLayerCandidate::isOtherCandidateBetter(const BaseLayerCandidate &other, const double *seed_height) const
{
  // Check for no currently recorded entry.
  if (key.isNull())
  {
    // This entry is null. Prefer other.
    return true;
  }

  if (other.isValid())
  {
    /// Check for clearance above.
    if (!clearAbove() && other.clearAbove())
    {
      // Other entry has a voxel clearance, but this voxel does not. Prefer other.
      return true;
    }

    // Finally select by distance to seed height, but only if both voxels have the same clearAbove() resolution.
    if (clearAbove() == other.clearAbove() && seed_height)
    {
      if (std::abs(other.height - *seed_height) < std::abs(height - *seed_height))
      {
        return true;
      }
    }
  }

  // This is better.
  return false;
}

OccupancyType sourceVoxelHeight(glm::dvec3 *voxel_position, double *height, SrcVoxel &voxel, const glm::dvec3 &up)
{
  OccupancyType voxel_type = voxel.occupancyType();
  if (voxel_type == ohm::kOccupied)
  {
    // Determine the height offset for voxel.
    voxel.syncKey();
    *voxel_position = voxel.position();
  }
  else
  {
    // Return the voxel centre. Voxel may be invalid, so use the map interface on the key.
    *voxel_position = voxel.map().voxelCentreGlobal(voxel.occupancy.key());
  }
  *height = glm::dot(*voxel_position, up);

  return voxel_type;
}

Key findNearestSupportingVoxel2(SrcVoxel &voxel, const Key &from_key, const Key &to_key, int up_axis_index,
                                int step_limit, bool search_up, unsigned flags, int *offset, bool *is_virtual)
{
  const bool allow_virtual_surface = (flags & kVirtualSurfaces) != 0;
  // Calculate the vertical range we will be searching.
  // Note: the vertical_range sign may not be what you expect. It will match search_up (true === +, false === -)
  // when the up axis is +X, +Y, or +Z. It will not match when the up axis is -X, -Y, or -Z.
  int vertical_range = voxel.map().rangeBetween(from_key, to_key)[up_axis_index] + 1;
  // Step direction is based on the vertical_range sign.
  const int step = (vertical_range >= 0) ? 1 : -1;
  vertical_range = (vertical_range >= 0) ? vertical_range : -vertical_range;
  if (step_limit > 0)
  {
    vertical_range = std::min(vertical_range, step_limit);
  }

  Key best_virtual(nullptr);
  bool last_unobserved = false;
  bool last_free = false;

  Key last_key(nullptr);
  Key current_key = from_key;

  if (search_up)
  {
    // In the case of searching up, we need to check the seed_key to see if it is unobserved, and therefore potentially
    // of a virtual surface, but not count it as part of our traversal. This is in part because for a virtual surface
    // we report the supporting unobserved voxel, not the free voxel, for the next phase to detect (final reporting is
    // of the free voxel).
    voxel.setKey(from_key);
    last_unobserved = ohm::isUnobservedOrNull(voxel.occupancy);
    last_key = from_key;

    // Now move the key up one voxel for the next iteration.
    voxel.map().moveKeyAlongAxis(current_key, up_axis_index, step);
  }

  // Note: there is an asymmetry to how we use vertical_range. We would tend to explore up one more voxel than down
  // without expanding the vertical range down by 1.
  if (!search_up)
  {
    ++vertical_range;
  }

  *offset = 0;

  // Confusion over iteration counters.
  // We logically have three iteration counters which all do similar things: i, offset and current_key. While similar
  // they are all slightly different:
  // - i is a very simple loop counter. Not much special here, except that we can make it jump towards the end of the
  //  loop in order to skip null regions.
  // - offset marks how far we've searched and is reported to the caller. It is biased such that on the first iteration
  //  searching up has a lower offset value than searching down. After that they fall in sync.
  // - current_key steps up/down following i, but needs separate management since keys have multiple indexing values per
  //  axis : a region index and a local index.

  for (int i = 0; i < vertical_range; ++i)
  {
    // We want to bias the initial voxel selection such that calling code will to choose the upper voxel over the lower
    // one (where they are both occupied) so that we can follow ramps up, rather than choosing the ground below. This is
    // only relevant when the clearance is zero, but is important.
    //
    // In order to create the bias, we manage the offset value in a non-linear fashion where it may be either 0 or 1 on
    // the first iteration - searching up or down respectively. On subsequent iterations it is linear with a value
    // matching `i + 1`.
    //
    // Note when searching up, first voxel checked is the one above the seed voxel.
    //
    // The offset parameter is used to report this information for the caller.
    //
    // The resulting offset series is:
    //
    //             | i=0 | 1 | 2 | 3 | 4 | 5 |
    // ----------- | --- | - | - | - | - | - |
    // Search up   |   0 | 2 | 3 | 4 | 5 | 6 |
    // Search down |   1 | 2 | 3 | 4 | 5 | 6 |
    *offset = (i > 0) ? i + 1 : !search_up;
    voxel.setKey(current_key);

    // This line yields performance issues likely due to the stochastic memory access.
    // For a true performance gain we'd have to access chunks linearly.
    // Read the occupancy value for the voxel.
    float occupancy = unobservedOccupancyValue();
    if (voxel.occupancy.chunk())
    {
      voxel.occupancy.read(&occupancy);
    }
    // Categorise the voxel.
    const bool occupied = occupancy >= voxel.occupancy_threshold && occupancy != unobservedOccupancyValue();
    const bool free = occupancy < voxel.occupancy_threshold;
    const bool unobserved = !occupied && !free;

    if (occupied)
    {
      // Voxel is occupied. We've found our candidate.
      *is_virtual = false;
      return current_key;
    }

    // No occupied voxel. Update the best (virtual) voxel.
    // We either keep the current best_virtual, or we select the current_voxel as a new best candidate.
    // We split this work into two. The first check is for the upward search where always select the first viable
    // virtual surface voxel and will not overwrite it. The conditions for the upward search are:
    // - virtual surface is allowed
    // - searching up
    // - the current voxel is free
    // - the previous voxel was unknown
    // - we do not already have a virtual voxel
    best_virtual = (allow_virtual_surface && search_up && free && last_unobserved && best_virtual.isNull()) ?
                     last_key :
                     best_virtual;

    // This is the case for searching down. In this case we are always looking for the lowest virtual voxel. More
    // specifically, we are looking for the voxel *below* the virtual voxel. We need to return the unobserved voxel
    // supporting the virtual surface voxel in order for later algorithms to detect it as a virtual voxel because a
    // virtual voxel is characterised by the transition from unobserved to free, but the free voxel is the one of
    // (later) interest.
    //
    // We progressively select the current voxel as the new virtual voxel provided the last voxel was considered free
    // and the current voxel is unknown (not free and not occupied). We only need to check free as we will have exited
    // on an occupied voxel. The conditions here are:
    // - virtual surface is allowed
    // - searching down (!search_up)
    // - the last voxel was free
    // - the current voxel is unknown - we only need check !free at this point
    // Otherwise we keep the current candidate
    best_virtual = (allow_virtual_surface && !search_up && unobserved && last_free) ? current_key : best_virtual;

    // Cache values for the next iteration.
    last_unobserved = unobserved;
    last_free = free;
    last_key = current_key;

    // Calculate the next voxel.
    int next_step = step;
    if (!voxel.occupancy.chunk())
    {
      // The current voxel is an empty chunk implying all unknown voxels. We will skip to the last voxel in this
      // chunk. We don't skip the whole chunk to allow the virtual voxel calculation to take effect.
      next_step = (step > 0) ? voxel.occupancy.layerDim()[up_axis_index] - current_key.localKey()[up_axis_index] :
                               -(1 + current_key.localKey()[up_axis_index]);
      i += std::abs(next_step) - 1;
    }

    // Single step in the current region.
    voxel.map().moveKeyAlongAxis(current_key, up_axis_index, next_step);
  }

  if (best_virtual.isNull())
  {
    *offset = -1;
  }

  *is_virtual = !best_virtual.isNull();

  // We only get here if we haven't found an occupied voxel. Return the best virtual one.
  return best_virtual;
}


Key findNearestSupportingVoxel(SrcVoxel &voxel, const Key &seed_key, UpAxis up_axis, const Key &min_key,
                               const Key &max_key, int voxel_floor_limit, int voxel_ceiling_limit,
                               int clearance_voxel_count_permissive, unsigned flags)
{
  PROFILE(findNearestSupportingVoxel);
  Key above;
  Key below;
  int offset_below = -1;
  int offset_above = -1;
  bool virtual_below = false;
  bool virtual_above = false;

  const int up_axis_index = (int(up_axis) >= 0) ? int(up_axis) : -int(up_axis) - 1;
  const Key &search_down_to = (int(up_axis) >= 0) ? min_key : max_key;
  const Key &search_up_to = (int(up_axis) >= 0) ? max_key : min_key;

  below = findNearestSupportingVoxel2(voxel, seed_key, search_down_to, up_axis_index, voxel_floor_limit, false, flags,
                                      &offset_below, &virtual_below);
  above = findNearestSupportingVoxel2(voxel, seed_key, search_up_to, up_axis_index, voxel_ceiling_limit, true, flags,
                                      &offset_above, &virtual_above);

  const bool have_candidate_below = offset_below >= 0;
  const bool have_candidate_above = offset_above >= 0;

  // Ignore the fact that the voxel below is virtual when prefer_virtual_below is set.
  const bool promote_virtual_below = (flags & heightmap::kPromoteVirtualBelow) != 0;
  virtual_below = have_candidate_below && virtual_below && !promote_virtual_below;

  if (flags & heightmap::kBiasAbove)
  {
    // Prefering the closer voxel.
    if (have_candidate_below && have_candidate_above)
    {
      if (offset_below < offset_above)
      {
        return below;
      }

      return above;
    }
  }

  // Prefer non-virtual over virtual. Prefer the closer result.
  if (have_candidate_below && virtual_above && !virtual_below)
  {
    return below;
  }

  if (have_candidate_above && !virtual_above && virtual_below)
  {
    return above;
  }

  if (flags & heightmap::kIgnoreVirtualAbove)
  {
    // Inore virtual voxels above. This this generates better heightmaps from a fixed reference plane. Virtual surfaces
    // are more interesting when approaching a slope down than any such information above.
    if (have_candidate_below && virtual_above && virtual_below)
    {
      return below;
    }
  }

  // When both above and below have valid candidates. We prefer the lower one if there is sufficient clearance from
  // it to the higher one (should be optimistic). Otherwise we prefer the one which has had less searching.
  if (have_candidate_below && (!have_candidate_above || offset_below <= offset_above ||
                               have_candidate_below && have_candidate_above && !virtual_above &&
                                 offset_below + offset_above >= clearance_voxel_count_permissive))
  {
    return below;
  }

  return above;
}


bool findGround(GroundCandidate &ground, SrcVoxel &voxel, const Key &seed_key, const Key &min_key, const Key &max_key,
                const HeightmapDetail &imp)
{
  PROFILE(findGround);
  ground.invalidate();  // Invalidate the result.
  bool observed_above = false;
  // Start with the seed_key and look for ground. We only walk up from the seed key.
  double column_height = std::numeric_limits<double>::max();
  double column_clearance_height = column_height;

  // Start walking the voxels in the source map.
  // glm::dvec3 column_reference = heightmap.voxelCentreGlobal(target_key);

  // Walk the src column up.
  const int up_axis_index = imp.vertical_axis_index;
  // Select walking direction based on the up axis being aligned with the primary axis or not.
  const int step_dir = (int(imp.up_axis_id) >= 0) ? 1 : -1;
  glm::dvec3 sub_voxel_pos(0);
  glm::dvec3 column_voxel_pos(0);
  double height = 0;
  OccupancyType candidate_voxel_type = ohm::kNull;
  OccupancyType last_voxel_type = ohm::kNull;

  Key ground_key = Key::kNull;
  for (Key key = seed_key; key.isBounded(up_axis_index, min_key, max_key);
       voxel.map().stepKey(key, up_axis_index, step_dir))
  {
    // PROFILE(column);
    voxel.setKey(key);

    const OccupancyType voxel_type = sourceVoxelHeight(&sub_voxel_pos, &height, voxel, imp.up);

    // We check the clearance and consider a new candidate if we have encountered an occupied voxel, or
    // we are considering virtual surfaces. When considering virtual surfaces, we also check clearance where we
    // have transitioned from unobserved to free and we do not already have a candidate voxel. In this way
    // only occupied voxels can obstruct the clearance value and only the lowest virtual voxel will be considered as
    // a surface.
    const bool last_is_unobserved = last_voxel_type == ohm::kUnobserved || last_voxel_type == ohm::kNull;
    // Even if we have insufficient clearnace, we can flag that we've made an observation above the ground voxel
    // so long as we have encountered a non-null/non-unobserved voxel.
    // Note we will set this on the first valid ground candidate we encounter, but it will be immediately cleared.
    observed_above = observed_above || voxel_type != ohm::kNull && voxel_type != ohm::kUnobserved;
    if (voxel_type == ohm::kOccupied || imp.generate_virtual_surface && last_is_unobserved &&
                                          voxel_type == ohm::kFree && candidate_voxel_type == ohm::kNull)
    {
      if (candidate_voxel_type != ohm::kNull)
      {
        // Branch condition where we have a candidate ground_key, but have yet to check or record its clearance.
        // Clearance height is the height of the current voxel associated with key.
        column_clearance_height = height;
        if (column_clearance_height - column_height >= imp.min_clearance)
        {
          // Found our heightmap voxels.
          // We have sufficient clearance so ground_key is our ground voxel.
          break;
        }

        // Insufficient clearance. The current voxel becomes our new base voxel; keep looking for clearance.
        column_height = column_clearance_height = height;
        column_voxel_pos = sub_voxel_pos;
        // Current voxel becomes our new ground candidate voxel.
        ground_key = key;
        candidate_voxel_type = voxel_type;
        observed_above = false;
      }
      else
      {
        // Branch condition only for the first voxel in column.
        ground_key = key;
        column_height = column_clearance_height = height;
        column_voxel_pos = sub_voxel_pos;
        candidate_voxel_type = voxel_type;
        observed_above = false;
      }
    }

    last_voxel_type = voxel_type;
  }

  // Did we find a valid candidate?
  if (candidate_voxel_type != ohm::kNull)
  {
    ground.key = ground_key;
    ground.height = height;
    ground.clearance = column_clearance_height - column_height;
    ground.observed_above = observed_above;
    return true;
  }

  return false;
}


void filterVirtualVoxels(ohm::HeightmapDetail &detail, unsigned threshold,
                         const std::unordered_map<ohm::Key, HeightmapKeyType> &src_to_heightmap_keys)
{
  PROFILE(filterVirtualVoxels);

  const std::array<glm::ivec3, 26> neighbour_offsets = {
    glm::ivec3(-1, -1, -1), glm::ivec3(0, -1, -1), glm::ivec3(1, -1, -1),  //
    glm::ivec3(-1, 0, -1),  glm::ivec3(0, 0, -1),  glm::ivec3(1, 0, -1),   //
    glm::ivec3(-1, 1, -1),  glm::ivec3(0, 1, -1),  glm::ivec3(1, 1, -1),   //

    glm::ivec3(-1, -1, 0),  glm::ivec3(0, -1, 0),  glm::ivec3(1, -1, 0),  //
    glm::ivec3(-1, 0, 0),   glm::ivec3(1, 0, 0),                          //
    glm::ivec3(-1, 1, 0),   glm::ivec3(0, 1, 0),   glm::ivec3(1, 1, 0),   //

    glm::ivec3(-1, -1, 1),  glm::ivec3(0, -1, 1),  glm::ivec3(1, -1, 1),  //
    glm::ivec3(-1, 0, 1),   glm::ivec3(0, 0, 1),   glm::ivec3(1, 0, 1),   //
    glm::ivec3(-1, 1, 1),   glm::ivec3(0, 1, 1),   glm::ivec3(1, 1, 1)    //
  };

#ifdef TES_ENABLE
  KeyRange heightmap_range;
  detail.occupancy_map->calculateExtents(nullptr, nullptr, &heightmap_range);
#endif  // TES_ENABLE

  ohm::OccupancyMap &heightmap = *detail.heightmap;
  Voxel<float> occupancy_voxel(&heightmap, heightmap.layout().occupancyLayer());
  Voxel<HeightmapVoxel> heightmap_voxel(&heightmap, detail.heightmap_voxel_layer);
  assert(occupancy_voxel.isLayerValid());
  assert(heightmap_voxel.isLayerValid());
  // Walk each key in src_to_heightmap_keys looking for virtual voxels. We'll then check it's neighbour keys for
  // presence in the same map.
  for (const auto &key_mapping : src_to_heightmap_keys)
  {
    const Key &src_key = key_mapping.first;
    const HeightmapKeyType &hm_key_type = key_mapping.second;

    if (hm_key_type.type == HeightmapVoxelType::kVirtualSurface)
    {
      unsigned n_count = 0;  // Populated neighbour count.
      // We have a voxel to check.
      // Lookup it's neighbours counting how many are present in src_to_heightmap_keys.
      for (const auto &n_offset : neighbour_offsets)
      {
        Key n_key = src_key;
        // Remember, we are investigating neighbours in the source map, not the heightmap. We need to move keys in that
        // source map.
        detail.occupancy_map->moveKey(n_key, n_offset);
        if (src_to_heightmap_keys.find(n_key) != src_to_heightmap_keys.end())
        {
          ++n_count;
        }
      }

      // Mark for removal?
      if (n_count < threshold)
      {
        // Mark the voxel as invalid. To be removed during sorting.
        ohm::setVoxelKey(hm_key_type.key, occupancy_voxel, heightmap_voxel);
        assert(occupancy_voxel.isValid());
        assert(heightmap_voxel.isValid());
        auto hmv = heightmap_voxel.data();
        hmv.layer = kHvlInvalid;
        heightmap_voxel.write(hmv);
        // Write special occupancy value.
        occupancy_voxel.write(kHeightmapVirtualSurfaceFilteredValue());

#ifdef TES_ENABLE
        if (g_tes)
        {
          // Remove from debug visualisation.
          uint32_t voxel_id = DstVoxel::get3esVoxelId(hm_key_type.key, *detail.heightmap, detail.vertical_axis_index);
          const tes::Id box_id(voxel_id, kTcHmVirtualSurface);
          g_tes->destroy(tes::Box(box_id));  // Only the ID matters for destruction/removal.
          // Also remove any clearance ray.
          if (hmv.clearance > 0)
          {
            const tes::Id arrow_id(voxel_id, kTcHmClearance);
            g_tes->destroy(tes::Arrow(arrow_id));
          }
          TES_SERVER_UPDATE(g_tes, 0.0f);
        }
#endif  // TES_ENABLE
      }
    }
  }
}


void finaliseLayeredHeightmap(ohm::HeightmapDetail &detail, const KeyRange &key_extents_2d,
                              const std::set<ohm::Key> &multi_voxel_column_keys, const bool use_voxel_mean,
                              const double *seed_height)
{
  PROFILE(finaliseLayeredHeightmap);

  /// Structure used to extract heightmap data for sorting. Contains all information possible from the heightmap.
  struct SortingVoxel
  {
    double height = 0;
    HeightmapVoxel height_info{};
    float occupancy = 0;
    VoxelMean mean{};
    /// Value used to disambiguate the same height value. This only happens when marking voxels for removal.
    /// Without this the sorting algorithm may yield undefined behaviour.
    int order = 0;
    bool base_layer_candidate = false;

    /// Sorting operator.
    inline bool operator<(const SortingVoxel &other) const
    {
      return height < other.height || height == other.height && order < other.order;
    }
  };

  ohm::OccupancyMap &heightmap = *detail.heightmap;
  DstVoxel voxel(heightmap, detail.heightmap_voxel_layer, use_voxel_mean);
  // Will only ever be small.
  std::vector<SortingVoxel> sorting_list;

  for (const auto &base_key : key_extents_2d)
  {
    Key key = base_key;  // We may modify key and restore to base_key later.
    voxel.setKey(key);

    assert(voxel.occupancy.isValid() && voxel.heightmap.isValid() && (!use_voxel_mean || voxel.mean.isValid()));

    // Handle single key columns
    if (multi_voxel_column_keys.find(key) == multi_voxel_column_keys.end())
    {
      // Single voxel column. Ensure the voxel is marked with kHvlBaseLayer or remove it if it is kHvlInvalid - filtered
      /// for removal.
      HeightmapVoxel hmv = voxel.heightmap.data();
      switch (hmv.layer)
      {
      case kHvlBaseLayer:
        // Already fine. Nothing to do.
        break;
      case kHvlInvalid:
        // Invalid clear/remove the voxel.
        voxel.heightmap.write(HeightmapVoxel{});
        voxel.occupancy.write(ohm::unobservedOccupancyValue());
        if (use_voxel_mean)
        {
          voxel.mean.write(VoxelMean{});
        }
        break;
      default:
        // Not removing, not marked as base layer. Mark it as base layer.
        hmv.layer = kHvlBaseLayer;
        voxel.heightmap.write(hmv);
        break;
      }
      continue;
    }

    sorting_list.clear();

    int order = 0;
    while (voxel.occupancy.isValid() && voxel.occupancy.data() != ohm::unobservedOccupancyValue())
    {
      // Extract voxel data.
      const HeightmapVoxel hmv = voxel.heightmap.data();
      sorting_list.emplace_back();
      SortingVoxel &sorting_info = sorting_list.back();
      sorting_info.order = order++;

      sorting_info.occupancy = voxel.occupancy.data();
      sorting_info.height_info = hmv;
      sorting_info.mean = use_voxel_mean ? voxel.mean.data() : VoxelMean{};
      sorting_info.base_layer_candidate = (sorting_info.height_info.layer == kHvlBaseLayer);

      // Add kHvlInvalid voxels with an infinite height, pushing them to the end. We will not re-insert them when
      // processing the sorted list. Instead we will clear a voxel every time we encounter an invalid voxel.
      if (hmv.layer != kHvlInvalid)
      {
        // The height value is stored as a relative to the centre of the voxel in which it resides. We need to
        // convert this to an absolute height.
        sorting_info.height =
          heightmap::getVoxelHeight(heightmap, detail.up, key, double(sorting_info.height_info.height));
      }
      else
      {
        sorting_info.height = std::numeric_limits<decltype(sorting_info.height)>::infinity();
      }

      heightmap.moveKeyAlongAxis(key, detail.vertical_axis_index, 1);
      voxel.setKey(key);
    }

    // Enter sort/rebuild block if we have a list to sort or we have just the vacant voxel we've artificially added.
    if (sorting_list.size() > 1)
    {
      // Sort the voxels
      key = base_key;
      std::sort(sorting_list.begin(), sorting_list.end());

      /// Current best base layer candidate.
      BaseLayerCandidate best_base_candidate;
      // Now write back the sorted results.
      // At the same time we finalise the base layer.
      // Use non-const, non-reference SortingVoxel as we modify content before writing back to the OccupancyMap voxel.
      for (SortingVoxel voxel_info : sorting_list)
      {
        voxel.setKey(key);

        // Handle valid/unfiltered voxels.
        if (voxel_info.height_info.layer != kHvlInvalid)
        {
          assert(voxel.occupancy.isValid() && voxel.heightmap.isValid() && (!use_voxel_mean || voxel.mean.isValid()));
          // Now that we have a new voxel key, we need to convert the HeightmapVoxel heigth value to be relative to
          // the new voxel centre.
          voxel_info.height_info.height =
            float(voxel_info.height - glm::dot(detail.up, detail.heightmap->voxelCentreGlobal(key)));

          // Only one item can be in the base layer. Track the best candidate.
          if (voxel_info.base_layer_candidate)
          {
            const BaseLayerCandidate current_base_candidate(key, voxel_info.height_info, voxel_info.height);

            // Check if this is the best candidate.
            if (best_base_candidate.isOtherCandidateBetter(current_base_candidate, seed_height))
            {
              // We have found either the first candidate, or a better candidate.
              best_base_candidate = current_base_candidate;
            }
          }

          // We always write kHvlExtended here. The primary surface layer is finalised after the loop.
          voxel_info.height_info.layer = kHvlExtended;

          voxel.occupancy.write(voxel_info.occupancy);
          voxel.heightmap.write(voxel_info.height_info);
          if (use_voxel_mean)
          {
            voxel.mean.write(voxel_info.mean);
          }
        }
        else
        {
          // Filtered voxel. Ensure we clear data from the heightmap.
          voxel.occupancy.write(ohm::unobservedOccupancyValue());
          voxel.heightmap.write(HeightmapVoxel{});
          if (use_voxel_mean)
          {
            voxel.mean.write(VoxelMean{});
          }
        }
        heightmap.moveKeyAlongAxis(key, detail.vertical_axis_index, 1);
      }

      // Finalise the base layer.
      if (best_base_candidate.isValid())
      {
        voxel.setKey(best_base_candidate.key);
        best_base_candidate.voxel.layer = kHvlBaseLayer;
        voxel.heightmap.write(best_base_candidate.voxel);
      }
    }
  }
}


void checkForBaseLayerDuplicates(std::ostream &out, const ohm::HeightmapDetail &detail)
{
  ohm::Voxel<const float> occupancy(detail.heightmap.get(), detail.heightmap->layout().occupancyLayer());
  ohm::Voxel<const ohm::HeightmapVoxel> voxel(detail.heightmap.get(), detail.heightmap_voxel_layer);

  if (!occupancy.isLayerValid() || !voxel.isLayerValid())
  {
    return;
  }

  KeyRange key_extents;
  detail.heightmap->calculateExtents(nullptr, nullptr, &key_extents);
  project(&key_extents, detail.vertical_axis_index);

  for (const auto &base_key : key_extents)
  {
    Key key = base_key;
    ohm::setVoxelKey(key, voxel, occupancy);

    int base_count = 0;
    float value = (occupancy.isValid()) ? occupancy.data() : ohm::unobservedOccupancyValue();

    while (!voxel.isNull())
    {
      if (value != ohm::unobservedOccupancyValue())
      {
        // Extract voxel data.
        const HeightmapVoxel hmv = voxel.data();

        if (hmv.layer == kHvlBaseLayer)
        {
          ++base_count;
        }
      }

      detail.heightmap->moveKeyAlongAxis(key, detail.vertical_axis_index, 1);
      ohm::setVoxelKey(key, voxel, occupancy);
      value = (occupancy.isValid()) ? occupancy.data() : ohm::unobservedOccupancyValue();
    }

    if (base_count > 1)
    {
      out << "Found multiple base layers at " << base_key << std::endl;
    }
  }
}
}  // namespace heightmap
}  // namespace ohm


================================================
FILE: ohmheightmap/private/HeightmapOperations.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHM_HEIGHTMAPOPERATIONS_H_
#define OHM_HEIGHTMAPOPERATIONS_H_

#include "OhmHeightmapConfig.h"

#include "ohmheightmap/HeightmapVoxel.h"
#include "ohmheightmap/HeightmapVoxelType.h"
#include "ohmheightmap/UpAxis.h"

#include <ohm/KeyRange.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyType.h>
#include <ohm/VoxelData.h>

#include <glm/vec3.hpp>

#include <iosfwd>
#include <set>
#include <unordered_map>

namespace ohm
{
struct HeightmapDetail;

namespace heightmap
{
#ifdef TES_ENABLE
extern const uint32_t kNeighbourIdMask;
#endif  // TES_ENABLE

/// Internal value used to mark virtual surface voxels for removal. Necessary to differ from
/// @c ohm::unobservedOccupancyValue().
/// @return Negative infinity.
inline constexpr float kHeightmapVirtualSurfaceFilteredValue()
{
  return -std::numeric_limits<float>::infinity();
}

/// Flags for @c findNearestSupportingVoxel() calls
enum SupportingVoxelFlag : unsigned
{
  /// Allow virtual surface candidate voxels to be reported as potential ground candidates.
  ///
  /// A virtual surface candidate is a free voxel supported by an unobserved voxel.
  kVirtualSurfaces = (1u << 0u),
  /// When selecting a candidate voxel from one above and one below and two candidates are found at the same range,
  /// then bias the selection to the one above.
  ///
  /// This flag is turned when selecting the first voxel in the heightmap, then subsequentyly is turned on for all but
  /// the planar heightmap generation (@c PlaneWalker used). This helps guide fill expansions up sloped surfaces.
  kBiasAbove = (1u << 1u),
  /// Flag used when finding a virtual surface candidate in the lower voxel and an occupied voxel above it. This mode
  /// prefers the virtual voxel candidate. Virtual surfaces must be enabled.
  kPromoteVirtualBelow = (1u << 2u),
  /// Ignore virtual surface candidates above the seed voxel. This generates better heightmaps based on a fixed plane -
  /// using @c PlaneWalker
  kIgnoreVirtualAbove = (1u << 3u),
};

/// Helper structure for managing voxel data access from the source heightmap.
struct SrcVoxel
{
  Voxel<const float> occupancy;             ///< Occupancy value (required)
  Voxel<const VoxelMean> mean;              ///< Voxel mean layer (optional)
  Voxel<const CovarianceVoxel> covariance;  ///< Covariance layer used for surface normal estimation (optional)
  float occupancy_threshold;                ///< Occupancy threshold cached from the source map.

  SrcVoxel(const OccupancyMap &map, bool use_voxel_mean)
    : occupancy(&map, map.layout().occupancyLayer())
    , mean(&map, use_voxel_mean ? map.layout().meanLayer() : -1)
    , covariance(&map, map.layout().covarianceLayer())
    , occupancy_threshold(map.occupancyThresholdValue())
  {}

  /// Set the key, but only for the occupancy layer.
  inline void setKey(const Key &key) { occupancy.setKey(key); }

  /// Sync the key from the occupancy layer to the other layers.
  inline void syncKey()
  {
    // Chain the occupancy values which maximise data caching.
    covariance.setKey(mean.setKey(occupancy));
  }

  /// Query the target map.
  inline const OccupancyMap &map() const { return *occupancy.map(); }

  /// Query the occupancy classification of the current voxel.
  inline OccupancyType occupancyType() const
  {
    float value = unobservedOccupancyValue();
#ifdef __clang_analyzer__
    if (occupancy.voxelMemory())
#else   // __clang_analyzer__
    if (occupancy.isValid())
#endif  // __clang_analyzer__
    {
      occupancy.read(&value);
    }
    OccupancyType type = (value >= occupancy_threshold) ? kOccupied : kFree;
    type = value != unobservedOccupancyValue() ? type : kUnobserved;
    return occupancy.chunk() ? type : kNull;
  }

  /// Query the voxel position. Must call @c syncKey() first if using voxel mean.
  inline glm::dvec3 position() const
  {
    glm::dvec3 pos = occupancy.map()->voxelCentreGlobal(occupancy.key());
#ifdef __clang_analyzer__
    if (mean.voxelMemory())
#else   // __clang_analyzer__
    if (mean.isValid())
#endif  // __clang_analyzer__
    {
      VoxelMean mean_info;
      mean.read(&mean_info);
      pos += subVoxelToLocalCoord<glm::dvec3>(mean_info.coord, occupancy.map()->resolution());
    }
    return pos;
  }

  /// Query the voxel centre for the current voxel.
  inline glm::dvec3 centre() const { return occupancy.map()->voxelCentreGlobal(occupancy.key()); }
};

/// A utility for tracking the voxel being written in the heightmap.
struct DstVoxel
{
  /// Occupancy voxel in the heightmap: writable
  Voxel<float> occupancy;
  /// Heightmap extension data.
  Voxel<HeightmapVoxel> heightmap;
  /// Voxel mean (if being used.)
  Voxel<VoxelMean> mean;

  inline DstVoxel() = default;

  DstVoxel(OccupancyMap &map, int heightmap_voxel_layer, bool use_mean)
    : occupancy(&map, map.layout().occupancyLayer())
    , heightmap(&map, heightmap_voxel_layer)
    , mean(&map, use_mean ? map.layout().meanLayer() : -1)
  {}

  inline void setKey(const Key &key) { mean.setKey(heightmap.setKey(occupancy.setKey(key))); }

  /// Get the target (height)map
  inline const OccupancyMap &map() const { return *occupancy.map(); }

  /// Query the position from the heightmap.
  inline glm::dvec3 position() const
  {
    glm::dvec3 pos = occupancy.map()->voxelCentreGlobal(occupancy.key());
    if (mean.isLayerValid())
    {
      const VoxelMean mean_info = mean.data();
      pos += subVoxelToLocalCoord<glm::dvec3>(mean_info.coord, occupancy.map()->resolution());
    }
    return pos;
  }

  /// Set the position in the heightmap
  inline void setPosition(const glm::dvec3 &pos)
  {
    if (mean.isValid())
    {
      VoxelMean voxel_mean;
      mean.read(&voxel_mean);
      voxel_mean.coord = subVoxelCoord(pos - mean.map()->voxelCentreGlobal(mean.key()), mean.map()->resolution());
      voxel_mean.count = 1;
      mean.write(voxel_mean);
    }
  }

  inline glm::dvec3 centre() const { return occupancy.map()->voxelCentreGlobal(occupancy.key()); }

  bool haveRecordedHeight(double height, int up_axis_index, const glm::dvec3 &up) const;

  /// Generate a debug voxel ID for a key in the @p heightmap for use with 3es.
  /// @param heightmap_key The voxel key in the @p heightmap .
  /// @param heightmap The heightmap object.
  /// @param up_axis_index The index of the up axis {0, 1, 2} mapping to {X, Y, Z}.
  static uint32_t get3esVoxelId(const Key &heightmap_key, const OccupancyMap &heightmap, int up_axis_index);

  /// @overload
  uint32_t get3esVoxelId(int up_axis_index) { return get3esVoxelId(occupancy.key(), *occupancy.map(), up_axis_index); }

  void debugDraw(int level, int up_axis_index, double up_scale = 1.0);
};

/// A helper class used to track the best base layer candidate voxel in a multi layered heightmap.
struct BaseLayerCandidate
{
  /// Details of the heightmap voxel selected.
  /// Note this is a direct copy of the voxel data, so the @c height value is relative to the voxel @c key .
  /// Use the @c height member of this structure instead.
  HeightmapVoxel voxel = {};
  /// The key of the voxel in the heightmap @c OccupancyMap .
  Key key = Key(nullptr);
  /// Absolute height of this voxel.
  double height = 0;

  inline BaseLayerCandidate(const Key &key, const HeightmapVoxel &voxel, double height)
    : voxel(voxel)
    , key(key)
    , height(height)
  {}

  inline BaseLayerCandidate() = default;
  inline BaseLayerCandidate(const BaseLayerCandidate &other) = default;
  inline BaseLayerCandidate &operator=(const BaseLayerCandidate &other) = default;

  /// Are the current candidate details valid?
  /// @return True if valid.
  inline bool isValid() const { return !key.isNull(); }

  /// Check if the recorded voxel has clearance above it. Undefined if @c isValid() is false.
  /// @return True if there is clearance above the voxel.
  inline bool clearAbove() const { return voxel.clearance > 0 || (voxel.flags & kHvfObservedAbove) != 0; }

  /// Check if @p other is a better candidate than this candidate.
  /// @param other The other candidate to consider.
  /// @param seed_height Seed height used to generate the heightmap. If given, candidates closer to this height are
  /// preferred.
  /// @return True this candidate is better than this candidate.
  bool isOtherCandidateBetter(const BaseLayerCandidate &other, const double *seed_height = nullptr) const;
};

/// Calculate the height of voxel @p heightmap_key in the @p heightmap relative to the centre of the voxel.
/// @param absolute_height The absolute height value.
/// @param heightmap_key Identifies the voxel of interest.
/// @param heightmap The heightmap.
/// @param up The up vector in the heightmap.
/// @return The @c absolute_height converted to be relative to the centre of the voxel at @p heightmap_key .
inline float relativeVoxelHeight(double absolute_height, const Key &heightmap_key, const OccupancyMap &heightmap,
                                 const glm::dvec3 &up)
{
  const glm::dvec3 voxel_centre = heightmap.voxelCentreGlobal(heightmap_key);
  const float relative_height = float(absolute_height - glm::dot(voxel_centre, up));
  return relative_height;
}

/// Project @p key onto the heightmap plane. This zeros the vertical axis.
/// @param[in,out] key The key to modify.
/// @param vertical_axis_index Heightmap vertical axis index.
/// @return A reference to @p key after being modified.
inline Key &project(Key *key, int vertical_axis_index)
{
  key->setRegionAxis(vertical_axis_index, 0);
  key->setLocalAxis(vertical_axis_index, 0);
  return *key;
}

/// Project @p key_range onto the heightmap plane. This zeros the vertical axis.
/// @param[in,out] key_range The key range to modify.
/// @param vertical_axis_index Heightmap vertical axis index.
/// @return A reference to @p key_range after being modified.
inline KeyRange &project(KeyRange *key_range, int vertical_axis_index)
{
  Key key = key_range->minKey();
  project(&key, vertical_axis_index);
  key_range->setMinKey(key);
  key = key_range->maxKey();
  project(&key, vertical_axis_index);
  key_range->setMaxKey(key);
  return *key_range;
}

/// Calculate the absolute height of a voxel in a source occupancy map - before heightmap coversion. This essentially
/// takes the 3D position of @p voxel and extracts the height along the @p up axis.
/// @param[out] voxel_position The 3D position calculated for @p voxel . May include @c VoxelMean if available.
/// @param[out] height The height of @p voxel_position along the @p up axis.
/// @param voxel The voxel details in the source occupancy map from which we are generating a heightmap.
/// @param up The heightmap up axis.
/// @return The @c OccupancyType of the source @p voxel .
OccupancyType sourceVoxelHeight(glm::dvec3 *voxel_position, double *height, SrcVoxel &voxel, const glm::dvec3 &up);

/// Calculate the height of the voxel at @p key with matching local @p height value.
///
/// This calculates the centre of the voxel at @p key then calculates `dot(up, centre) + height`.
///
/// @param heightmap The heightmap which @p key references.
/// @param up The up axis for @p heightmap - must be a unit vector aligned with X, Y or Z axes.
/// @param key The heightmap voxel key of interest. The up axis should always be (0, 0) for non-layered heightmaps.
/// @param height The local voxel height delta.
/// @return The global voxel height value.
inline double getVoxelHeight(const OccupancyMap &heightmap, const glm::dvec3 &up, const Key &key, double height)
{
  const glm::dvec3 voxel_centre = heightmap.voxelCentreGlobal(key);
  return glm::dot(up, voxel_centre) + height;
}


/// A secondary operation for @c findNearestSupportingVoxel() which finds the first occupied or virtual voxel in the
/// column of @p from_key . This function can search either up or down from @p from_key until a candidate is found, the
/// @p step_limit number of voxels have been considered or after @p to_key has been considered - whichever condition is
/// met first.
///
/// A valid candidate voxel is one which is occupied or a virtual surface voxel. See the virtual surfaces section of the
/// @c Heightmap class documentation.
///
/// @param voxel Configured to allow access to various aspects of the source map voxels.
/// @param from_key Voxel key identifying where to start the search from. Only voxels in the same column are considered
///   up to @p to_key .
/// @param to_key The final key to consider. This must be in the same column as @p from_key .
/// @param up_axis_index Index of the up axis: 0=>x, 1=>y, 2=>z.
/// @param step_limit Limits the number of voxels to be visited. Zero to visit all voxels in the range
///   `[from_key, to_key]`
/// @param search_up A flag used to indicate if we are searching up a column or not. The sign of the difference between
///   @p from_key and @p to_key cannot be used to infer this as it is defined by the heightmap
///   primary axis, for which up may be alinged with an increasing, negative magnitude.
/// @param flags Flags affecting reporting. Only @c SupportingVoxelFlag::kVirtualSurfaces is considered here. When
///   set, consider virtual surface voxels - free voxels "supported" by unobserved voxels representing the best possible
///   surface estimate.
/// @param[out] offset On success, set the number of voxels between @p from_key and the selected voxel. Undefined on
///   failure.
/// @param[out] is_virtual On success, set to true if the selected voxel is a virtual surface voxel. False when the
///   selected voxel is an occupied voxel. Undefined on failure.
/// @return The first voxel found which is either occupied or a virtual surface voxel when @c kVirtualSurfaces is
///   set. The result is a null key on failure to find such a voxel within the search limits.
Key findNearestSupportingVoxel2(SrcVoxel &voxel, const Key &from_key, const Key &to_key, int up_axis_index,
                                int step_limit, bool search_up, unsigned flags, int *offset, bool *is_virtual);

/// Search the column containing @p seed_key in the source occupancy map for a potential supporting voxel.
///
/// A supporting voxel is one which is either occupied or a virtual surface voxel (if enabled). The source map details
/// are contained in the @c SrcVoxel structure passed via @p voxel . That structure is configured to reference the
/// relevant voxel layers. The actual voxels referenced by @c voxel will be modified by this function, starting at
/// @c seed_key .
///
/// The search process searches above and below @c seed_key - with up defined by @c up_axis - for an occupied or
/// virtual surface voxel. The final voxel selection is guided by several factors:
///
/// - Prefer occupied voxels over virtual surface voxels
///   - Except where @c promote_virtual_below is true
/// - Prefer below to above.
///   - Except where the distance between the candidates below and above is less than
///     @p clearance_voxel_count_permissive .
/// - Limit the search expansion to search up @c voxel_ceiling voxels (this is a voxel count value).
/// - Limit the search down to the map extents.
///
/// The resulting key can be used to identify the voxel from which to start searching for an actual ground candidate
/// with consideration given to clearance above.
///
/// The selected key is expected to be used as the seed for @c findGround() .
///
/// @param voxel Configured to allow access to various aspects of the source map voxels.
/// @param seed_key Voxel key identifying where to start the search from. Only voxels in the same column are considered.
/// @param up_axis Identifies the up axis - XYZ - and direction.
/// @param min_key Min extents limits. Searches are bounded by this value.
/// @param max_key Max extents limits. Searches are bounded by this value.
/// @param voxel_ceiling The number of voxels the function is allowed to consider above the @p seed_key .
/// @param clearance_voxel_count_permissive The number of voxels required to pass the clearance value. Used to
///   discriminate the voxel below when the candidate above is within this range, but non-virtual.
/// @param flags Control flags from @c SupportingVoxelFlag .
/// @return A new seed key from which to start searching for a valid ground voxel (@c findGround()).
Key findNearestSupportingVoxel(SrcVoxel &voxel, const Key &seed_key, UpAxis up_axis, const Key &min_key,
                               const Key &max_key, int voxel_floor_limit, int voxel_ceiling_limit,
                               int clearance_voxel_count_permissive, unsigned flags);

/// Output details for @c findGround() .
struct GroundCandidate
{
  Key key = Key(nullptr);  ///< Ground candidate key.
  double height = 0;       ///< Ground voxel absolute height. Based on mean position if available, centre otherwise.
  double clearance = 0;    ///< Clearance value above the voxel. Zero if clearance is unknown.
  bool observed_above = false;  ///< Have we made observations above this. Will always be true when `clearance > 0`

  /// Is this candidate/observation valid?
  /// @return True if valid.
  inline bool isValid() const { return !key.isNull(); }

  /// Invalidate this object, ensuring @c isValid() returns @c false .
  inline void invalidate() { key = Key(nullptr); }
};

/// Search for the best ground voxel for the column containing @p seed_key . The search begins at @p seed_key , normally
/// generated by @c findNearestSupportingVoxel() . This function considers the configured
/// @c HeightmapDetail::min_clearance from @p imp and may also consider virtual surface voxels if configured to do so.
///
/// @param[out] ground Set provide details of the first viable ground candidate. Invalid when the return value is]
///     @c false
/// @param voxel Configured to allow access to various aspects of the source map voxels.
/// @param seed_key Voxel key identifying where to start the search from. Only voxels in the same column are considered.
/// @param min_key Min extents limits. Searches are bounded by this value.
/// @param max_key Max extents limits. Searches are bounded by this value.
/// @param imp Implementation details of the heightmap object being operated on.
/// @return True if a @p ground candidate has been resolved within the search constraints.
bool findGround(GroundCandidate &ground, SrcVoxel &voxel, const Key &seed_key, const Key &min_key, const Key &max_key,
                const HeightmapDetail &imp);

/// A pairing of heightmap voxel key and heightmap voxel type.
struct HeightmapKeyType
{
  Key key;
  HeightmapVoxelType type;
};

/// Filter a layered heightmap removing virtual voxels which have fewer occupied 26-connected neighbours than
/// @p threshold . The call assumes that the check for layered heightmap mode has already been made.
///
/// @param detail Target @c Heightmap private detail.
/// @param threshold The number of non-empty neighbours required to retain a virtual voxel.
/// @param multi_layer_keys The set of heightmap keys which contain multiple voxels in the column.
/// @param src_to_heightmap_keys A mapping of source map key to heightmap voxel key and type.
void filterVirtualVoxels(ohm::HeightmapDetail &detail, unsigned threshold,
                         const std::unordered_map<ohm::Key, HeightmapKeyType> &src_to_heightmap_keys);

/// A utility function which finalised voxels in a layered heightmap, resolving the base layer and sorting voxels such
/// that lower heights appear lower in the heightmap.
///
/// In a multi layered heightmap, there may be multiple surfaced heights at any 2D voxel coordinate. This function
/// ensures that each 2D block is sorted such that the height value increases. The heightmap generates unsorted results.
/// The @p multi_voxel_column_keys identify which voxels need to be sorted. The keys are assumed to address the current
/// bottom voxel of each column only.
///
/// During sorting, the base layer is also resolved. Candidates have already been marked as voxels lying in a horizontal
/// slice around the the heightmap seed position, extending up and down to the ceiling and floor heights respectively.
/// During sorting, we finalise the base layer as either the lowest candidate (when @p seed_height is null) or the voxel
/// closest to the @p seed_height .
///
/// @param detail Target @c Heightmap private detail.
/// @param key_extents_2d 2D key extents of the generated heightmap. The height axis must have a range of 0 to be valid.
/// @param multi_voxel_column_keys Keys of columns which contain multiple voxels.
/// @param use_voxel_mean Does the map support voxel mean positioning?
/// @param seed_height Optional height to constrain the base layer near.
void finaliseLayeredHeightmap(ohm::HeightmapDetail &detail, const KeyRange &key_extents_2d,
                              const std::set<ohm::Key> &multi_voxel_column_keys, const bool use_voxel_mean,
                              const double *seed_height = nullptr);


/// Debug function which logs any columns with multiple base layer voxels.
/// @param out Stream to log to.
/// @param detail Heightmap detail.
void checkForBaseLayerDuplicates(std::ostream &out, const ohm::HeightmapDetail &detail);
}  // namespace heightmap
}  // namespace ohm

#endif  // OHM_HEIGHTMAPOPERATIONS_H_


================================================
FILE: ohmheightmapimage/CMakeLists.txt
================================================

# ohmheightmapimage provides additional functionality for extracting data from an ohm heightmap.
include(GenerateExportHeader)

find_package(OpenGL REQUIRED)
find_package(GLEW REQUIRED)
find_package(glfw3 REQUIRED)
if(OHM_FEATURE_THREADS)
  find_package(TBB)
endif(OHM_FEATURE_THREADS)

configure_file(OhmHeightmapImageConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/ohmheightmapimage/OhmHeightmapImageConfig.h")

set(SOURCES
  HeightmapImage.cpp
  HeightmapImage.h
  OhmHeightmapImageConfig.in.h
)

set(PUBLIC_HEADERS
  HeightmapImage.h
  "${CMAKE_CURRENT_BINARY_DIR}/ohmheightmapimage/OhmHeightmapImageConfig.h"
  "${CMAKE_CURRENT_BINARY_DIR}/ohmheightmapimage/OhmHeightmapUtilExport.h"
)
add_library(ohmheightmapimage ${SOURCES})
clang_tidy_target(ohmheightmapimage)

generate_export_header(ohmheightmapimage
      EXPORT_MACRO_NAME ohmheightmapimage_API
      EXPORT_FILE_NAME ohmheightmapimage/OhmHeightmapUtilExport.h
      STATIC_DEFINE OHMTOOLS_STATIC)

target_link_libraries(ohmheightmapimage PUBLIC ohmheightmap ohm ohmutil)
target_link_libraries(ohmheightmapimage
  PUBLIC
    glm::glm
    OpenGL::GL
    OpenGL::GLU
    $<$<BOOL:${OHM_FEATURE_THREADS}>:TBB::tbb>
)
if(BUILD_SHARED)
  # Link additional dependencies when building shared ohm.
  # Because ohm is shared and these are private, static depenencies, we do not need to propagate
  # the dependencies and limit this with $<BUILD_INTERFACE:>
  target_link_libraries(ohmheightmapimage
    PRIVATE
      # Link 3es if enabled.
      $<BUILD_INTERFACE:$<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>>
      $<BUILD_INTERFACE:GLEW::GLEW>
      $<BUILD_INTERFACE:glfw>
  )
else(BUILD_SHARED)
  # With ohm static, we link depdencencies in such as way that the will propagate and need to be
  # found when ohm is linked.
  target_link_libraries(ohmheightmapimage
    PRIVATE
      # Link 3es if enabled.
      $<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>
      GLEW::GLEW
      glfw
  )
endif(BUILD_SHARED)

target_include_directories(ohmheightmapimage
  PUBLIC
    $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/ohmheightmapimage>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
)

target_include_directories(ohmheightmapimage
  SYSTEM PRIVATE
    $<$<NOT:$<BOOL:${OHM_TES_DEBUG}>>:${3ES_INCLUDE_DIRS}>
)

install(TARGETS ohmheightmapimage EXPORT ${CMAKE_PROJECT_NAME}-config-targets
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/ohmheightmapimage
)

install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/ohmheightmapimage)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})


================================================
FILE: ohmheightmapimage/HeightmapImage.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "HeightmapImage.h"

#include <ohmheightmap/HeightmapMesh.h>

#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/OccupancyMap.h>
#include <ohm/Voxel.h>

#include <ohmutil/Colour.h>
#include <ohmutil/Profile.h>

#include <3esservermacros.h>

#include <iostream>
#include <mutex>

// Include GLEW
#include <GL/glew.h>

// Include GLFW
#include <GLFW/glfw3.h>

#include <glm/ext.hpp>

#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/normal.hpp>

#include <array>

namespace
{
/// Relevant texture formats.
enum AttachmentFormat
{
  /// RGB 3 bytes per pixel, uint8_t per channel.
  kAfRgb8,
  /// RGB 12 bytes per pixel, float32 per channel.
  kAfRgb32f,
  /// Mono 4 bytes per pixel, float32 per channel.
  kAfMono32f
};

// Fragment shader colour by input.
const char *const kNormalsFragmentShader = "#version 330 core\n"
                                           "in vec3 v_normal;\n"
                                           "in vec3 v_colour;\n"
                                           "in float v_depth;\n"
                                           "// Ouput data\n"
                                           "out vec3 colour;\n"
                                           "void main()\n"
                                           "{\n"
                                           // "  colour = vec3(v_depth, v_depth, v_depth);\n"
                                           "  colour = 0.5 * (v_normal + vec3(1, 1, 1));\n"
                                           "}";
const char *const kColoursFragmentShader = "#version 330 core\n"
                                           "in vec3 v_normal;\n"
                                           "in vec3 v_colour;\n"
                                           "in float v_depth;\n"
                                           "// Ouput data\n"
                                           "out vec3 colour;\n"
                                           "void main()\n"
                                           "{\n"
                                           "  colour = v_colour;\n"
                                           "}";

// Vertex shader.
const char *const kVertexShader = "#version 330 core\n"
                                  "// Input vertex data, different for all executions of this shader.\n"
                                  "layout(location = 0) in vec3 vertexPosition_modelspace;\n"
                                  "layout(location = 1) in vec3 vertexNormal_modelspace;\n"
                                  "layout(location = 2) in vec3 vertexColour;\n"
                                  "uniform mat4 MVP;\n"
                                  "uniform mat4 V;\n"
                                  "uniform mat4 M;\n"
                                  "out vec3 v_normal;\n"
                                  "out vec3 v_colour;\n"
                                  "out float v_depth;\n"
                                  "void main()\n"
                                  "{\n"
                                  "  gl_Position = gl_Position =  MVP * vec4(vertexPosition_modelspace,1);\n"
                                  "  v_normal =  (V * M * vec4(vertexNormal_modelspace,0)).xyz;\n"
                                  // "  v_normal =  vertexNormal_modelspace;\n"
                                  "  v_colour = vertexColour;\n"
                                  "  v_depth = 1.0 - (1.0 + gl_Position.z/gl_Position.w) / 2.0;\n"
                                  "}\n";

const char *const kQuadFragmentShader = "#version 330 core\n"
                                        "// Ouput data\n"
                                        "layout(location = 0) out vec4 color;\n"
                                        "uniform sampler2D render_texture;\n"
                                        "in vec2 UV;\n"
                                        "void main()\n"
                                        "{\n"
                                        "  color = texture(render_texture, UV);\n"
                                        "}";

// Full screen quad vertex shader.
const char *const kQuadVertexShader = "#version 330 core\n"
                                      "// Input vertex data, different for all executions of this shader.\n"
                                      "layout(location = 0) in vec3 vertexPosition_modelspace;\n"
                                      "// Output data ; will be interpolated for each fragment.\n"
                                      "out vec2 UV;\n"
                                      "void main()\n"
                                      "{\n"
                                      "    gl_Position = vec4(vertexPosition_modelspace, 1);\n"
                                      "    UV = (vertexPosition_modelspace.xy + vec2(1, 1)) / 2.0;\n"
                                      "}\n";

// The fullscreen quad's FBO
const std::array<GLfloat, 3 * 6> kQuadVertexBufferData =  //
  {
    -1.0f, -1.0f, 0.0f,  //
    1.0f,  -1.0f, 0.0f,  //
    -1.0f, 1.0f,  0.0f,  //
    -1.0f, 1.0f,  0.0f,  //
    1.0f,  -1.0f, 0.0f,  //
    1.0f,  1.0f,  0.0f,  //
  };

// Load vertex and fragment shader strings into a program.
GLuint loadShaders(const char * /*name*/, const char *vertex_shader_code, const char *fragment_shader_code)
{
  struct OnExit
  {
    std::function<void()> on_exit;
    explicit OnExit(std::function<void()> on_exit)
      : on_exit(std::move(on_exit))
    {}
    ~OnExit() { on_exit(); }
  };

  // Create the shaders
  GLuint vertex_shader_id = 0;
  GLuint fragment_shader_id = 0;
  GLuint program_id = 0;
  GLint compile_success = GL_FALSE;

  // Setup error cleanup handling.
  OnExit on_exit(  //
    [&]()          //
    {
      if (!compile_success)
      {
        if (program_id)
        {
          if (vertex_shader_id)
          {
            glDetachShader(program_id, vertex_shader_id);
          }
          if (fragment_shader_id)
          {
            glDetachShader(program_id, fragment_shader_id);
          }
          glDeleteProgram(program_id);
        }

        if (vertex_shader_id)
        {
          glDeleteShader(vertex_shader_id);
          vertex_shader_id = 0;
        }

        if (fragment_shader_id)
        {
          glDeleteShader(fragment_shader_id);
          fragment_shader_id = 0;
        }
      }
    });

  int info_msg_length = 0;

  // Compile Vertex Shader
  // printf("Compiling shader vertex : %s\n", name);
  vertex_shader_id = glCreateShader(GL_VERTEX_SHADER);
  glShaderSource(vertex_shader_id, 1, &vertex_shader_code, nullptr);
  glCompileShader(vertex_shader_id);

  // Check Vertex Shader
  glGetShaderiv(vertex_shader_id, GL_COMPILE_STATUS, &compile_success);
  glGetShaderiv(vertex_shader_id, GL_INFO_LOG_LENGTH, &info_msg_length);
  if (info_msg_length > 0)
  {
    std::vector<char> error_msg(info_msg_length + 1);
    glGetShaderInfoLog(vertex_shader_id, info_msg_length, nullptr, error_msg.data());
    std::cerr << error_msg.data() << std::endl;
  }

  if (!compile_success)
  {
    return 0;
  }

  // Compile Fragment Shader
  // printf("Compiling shader fragment: %s\n", name);
  fragment_shader_id = glCreateShader(GL_FRAGMENT_SHADER);
  glShaderSource(fragment_shader_id, 1, &fragment_shader_code, nullptr);
  glCompileShader(fragment_shader_id);

  // Check Fragment Shader
  glGetShaderiv(fragment_shader_id, GL_COMPILE_STATUS, &compile_success);
  glGetShaderiv(fragment_shader_id, GL_INFO_LOG_LENGTH, &info_msg_length);
  if (info_msg_length > 0)
  {
    std::vector<char> error_msg(info_msg_length + 1);
    glGetShaderInfoLog(fragment_shader_id, info_msg_length, nullptr, &error_msg[0]);
    std::cerr << error_msg.data() << std::endl;
  }

  if (!compile_success)
  {
    return 0;
  }

  // Link the program
  // printf("Linking program\n");
  program_id = glCreateProgram();
  glAttachShader(program_id, vertex_shader_id);
  glAttachShader(program_id, fragment_shader_id);
  glLinkProgram(program_id);

  // Check the program
  glGetProgramiv(program_id, GL_LINK_STATUS, &compile_success);
  glGetProgramiv(program_id, GL_INFO_LOG_LENGTH, &info_msg_length);
  if (info_msg_length > 0)
  {
    std::vector<char> error_msg(info_msg_length + 1);
    glGetProgramInfoLog(program_id, info_msg_length, nullptr, &error_msg[0]);
    std::cerr << error_msg.data() << std::endl;
  }

  if (!compile_success)
  {
    return 0;
  }

  glDetachShader(program_id, vertex_shader_id);
  glDetachShader(program_id, fragment_shader_id);

  glDeleteShader(vertex_shader_id);
  vertex_shader_id = 0;
  glDeleteShader(fragment_shader_id);
  fragment_shader_id = 0;

  return program_id;
}


template <typename T>
class ColourConverter
{
public:
  static glm::vec3 vec3(const T &c) { return glm::vec3(c); }
  static glm::vec4 vec4(const T &c) { return glm::vec4(c); }
};

template <>
class ColourConverter<ohm::Colour>
{
public:
  static glm::vec3 vec3(const ohm::Colour &c) { return glm::vec3(c.rf(), c.gf(), c.bf()); }
  static glm::vec4 vec4(const ohm::Colour &c) { return glm::vec4(vec3(c), 1.0f); }
};

std::mutex g_shared_init_guard;
volatile unsigned g_shared_ref_count = 0;

void sharedInit()
{
  std::unique_lock<std::mutex> guard(g_shared_init_guard);

  if (g_shared_ref_count == 0)
  {
    glfwInit();
  }

  ++g_shared_ref_count;
}

void sharedRelease()
{
  std::unique_lock<std::mutex> guard(g_shared_init_guard);

  if (g_shared_ref_count)
  {
    --g_shared_ref_count;
    if (g_shared_ref_count == 0)
    {
      glfwTerminate();
    }
  }
}

void textureBufferInfo(GLint texture_id)
{
  struct TexParam
  {
    int id;
    const char *name;
  };

  static const std::array<TexParam, 18> params =  //
    {                                             //
      TexParam{ GL_TEXTURE_WIDTH, "width" },
      TexParam{ GL_TEXTURE_HEIGHT, "height" },
      TexParam{ GL_TEXTURE_DEPTH, "depth" },
      TexParam{ GL_TEXTURE_INTERNAL_FORMAT, "format-internal" },
      TexParam{ GL_TEXTURE_RED_TYPE, "red-type" },
      TexParam{ GL_TEXTURE_GREEN_TYPE, "green-type" },
      TexParam{ GL_TEXTURE_BLUE_TYPE, "blue-type" },
      TexParam{ GL_TEXTURE_ALPHA_TYPE, "alpha-type" },
      TexParam{ GL_TEXTURE_DEPTH_TYPE, "depth-type" },
      TexParam{ GL_TEXTURE_RED_SIZE, "red-size" },
      TexParam{ GL_TEXTURE_GREEN_SIZE, "green-size" },
      TexParam{ GL_TEXTURE_BLUE_SIZE, "blue-size" },
      TexParam{ GL_TEXTURE_ALPHA_SIZE, "alpha-size" },
      TexParam{ GL_TEXTURE_DEPTH_SIZE, "depth-size" },
      TexParam{ GL_TEXTURE_COMPRESSED, "compressed" },
      TexParam{ GL_TEXTURE_COMPRESSED_IMAGE_SIZE, "compressed-size" },
      TexParam{ GL_TEXTURE_BUFFER_OFFSET, "buffer-offset" },
      TexParam{ GL_TEXTURE_BUFFER_SIZE, "buffer-size" }
    };

  glBindTexture(GL_TEXTURE_2D, texture_id);
  int param_value = 0;
  printf("texture-info:\n");
  for (const auto &param : params)
  {
    glGetTexLevelParameteriv(GL_TEXTURE_2D, 0, param.id, &param_value);
    printf("  %s: %d\n", param.name, param_value);
  }
  glBindTexture(GL_TEXTURE_2D, 0);
}

unsigned makeMultipleOf(unsigned value, unsigned of)
{
  const unsigned overflow = value % of;
  if (overflow)
  {
    return value + of - overflow;
  }
  return value;
}
}  // namespace

namespace ohm
{
struct HeightmapImageDetail
{
  std::vector<glm::vec3> vertices;
  std::vector<glm::vec3> vertex_normals;
  std::vector<glm::vec3> vertex_colours;

  HeightmapImage::BitmapInfo image_info;
  std::vector<uint8_t> image;

  HeightmapImage::ImageType desired_type = HeightmapImage::kImageNormals;
  HeightmapImage::ImageType generated_type = HeightmapImage::kImageNormals;
  unsigned pixels_per_voxel = 1;

  struct RenderData
  {
    GLFWwindow *window = nullptr;
    GLuint mesh_normals_program_id = 0;
    GLuint mesh_colours_program_id = 0;
    GLuint quad_program_id = 0;
    GLuint quad_vertex_buffer = 0;
    GLuint fbo_tex_id = 0xffffffffu;  // NOLINT(readability-magic-numbers)
    // Debug visualisation flag.
    bool show_window = false;
    bool block_on_show_window = false;

    bool init();

    ~RenderData() { clear(); }

    void clear();
  } render_data;
};


bool HeightmapImageDetail::RenderData::init()
{
  PROFILE(HeightmapImageDetail_init);
  if (window)
  {
    return true;
  }

  ::sharedInit();

  glfwWindowHint(GLFW_SAMPLES, 4);
  glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
  glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
  glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);  // To make MacOS happy; should not be needed
  glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
  glfwWindowHint(GLFW_VISIBLE, show_window ? GLFW_TRUE : GLFW_FALSE);

  window = glfwCreateWindow(64, 64, "ohm", nullptr, nullptr);  // NOLINT(readability-magic-numbers)

  if (!window)
  {
    sharedRelease();
    return false;
  }

  glfwMakeContextCurrent(window);

  // Initialize GLEW
  glewExperimental = true;  // Needed for core profile
  if (glewInit() != GLEW_OK)
  {
    std::cerr << "Failed to initialize GLEW" << std::endl;
    clear();
    return false;
  }

  // Black background
  glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

  // // Enable depth test
  glEnable(GL_DEPTH_TEST);
  // Accept fragment if it closer to the camera than the former one
  glDepthFunc(GL_LESS);

  // Cull triangles which normal is not towards the camera
  glEnable(GL_CULL_FACE);

  mesh_normals_program_id = loadShaders("mesh_normals_shader", kVertexShader, kNormalsFragmentShader);
  mesh_colours_program_id = loadShaders("mesh_colours_shader", kVertexShader, kColoursFragmentShader);

  // Set the list of draw buffers.
  std::array<GLenum, 1> quad_draw_buffers = { GL_COLOR_ATTACHMENT0 };
  glDrawBuffers(GLsizei(quad_draw_buffers.size()), quad_draw_buffers.data());

  glGenBuffers(1, &quad_vertex_buffer);
  glBindBuffer(GL_ARRAY_BUFFER, quad_vertex_buffer);
  glBufferData(GL_ARRAY_BUFFER, sizeof(kQuadVertexBufferData), kQuadVertexBufferData.data(), GL_STATIC_DRAW);

  // Create and compile our GLSL program from the shaders
  quad_program_id = loadShaders("fbo", kQuadVertexShader, kQuadFragmentShader);
  fbo_tex_id = glGetUniformLocation(quad_program_id, "render_texture");

  return true;
}

void HeightmapImageDetail::RenderData::clear()
{
  PROFILE(HeightmapImageDetail_clear);
  if (mesh_normals_program_id)
  {
    glDeleteProgram(mesh_normals_program_id);
    mesh_normals_program_id = 0;
  }

  if (mesh_colours_program_id)
  {
    glDeleteProgram(mesh_colours_program_id);
    mesh_colours_program_id = 0;
  }

  if (quad_program_id)
  {
    glDeleteProgram(quad_program_id);
    quad_program_id = 0;
  }

  if (quad_vertex_buffer)
  {
    glDeleteBuffers(1, &quad_vertex_buffer);
  }

  fbo_tex_id = 0xffffffffu;  // NOLINT(readability-magic-numbers)

  if (window)
  {
    glfwDestroyWindow(window);
    window = nullptr;

    sharedRelease();
  }
}


HeightmapImage::HeightmapImage(ImageType type, unsigned pixels_per_voxel)
  : imp_(new HeightmapImageDetail)
{
  PROFILE(HeightmapImageDetail);
  imp_->desired_type = imp_->generated_type = type;
  imp_->pixels_per_voxel = pixels_per_voxel;
  imp_->render_data.init();
}


HeightmapImage::~HeightmapImage()
{
  PROFILE(_HeightmapImageDetail);
}


HeightmapImage::ImageType HeightmapImage::desiredImageType()
{
  return imp_->desired_type;
}


void HeightmapImage::setDesiredImageType(ImageType type)
{
  imp_->desired_type = type;
}


unsigned HeightmapImage::pixelsPerVoxel()
{
  return imp_->pixels_per_voxel;
}


void HeightmapImage::setPixelsPerVoxel(unsigned ppv)
{
  imp_->pixels_per_voxel = ppv;
}


bool HeightmapImage::showWindow() const
{
  return imp_->render_data.show_window;
}


void HeightmapImage::setShowWindow(bool show_window)
{
  if (imp_->render_data.show_window != show_window)
  {
    imp_->render_data.show_window = show_window;
    if (show_window)
    {
      glfwShowWindow(imp_->render_data.window);
    }
    else
    {
      glfwHideWindow(imp_->render_data.window);
    }
  }
}


bool HeightmapImage::blockOnShowWindow() const
{
  return imp_->render_data.block_on_show_window;
}


void HeightmapImage::setBlockOnShowWindow(bool block)
{
  imp_->render_data.block_on_show_window = block;
}


const uint8_t *HeightmapImage::bitmap(BitmapInfo *info) const
{
  *info = imp_->image_info;
  return (!imp_->image.empty()) ? imp_->image.data() : nullptr;
}


bool HeightmapImage::generateBitmap(const Aabb &extents, double resolution, const glm::dvec3 *vertices,
                                    size_t vertex_count, const unsigned *indices, size_t index_count,
                                    const glm::dvec3 *vertex_normals, const Colour *colours, UpAxis up_axis)
{
  PROFILE(HeightmapImage_generateBitmap);
  // Render the mesh to a depth buffer.
  return renderHeightMesh(imp_->desired_type, extents, resolution, vertices, vertex_count, indices, index_count,
                          vertex_normals, colours, up_axis);
}


bool HeightmapImage::generateBitmap(const Aabb &extents, double resolution, const glm::dvec3 *vertices,
                                    size_t vertex_count, const unsigned *indices, size_t index_count,
                                    const glm::dvec3 *vertex_normals, const glm::vec3 *colours, UpAxis up_axis)
{
  PROFILE(HeightmapImage_generateBitmap);
  // Render the mesh to a depth buffer.
  return renderHeightMesh(imp_->desired_type, extents, resolution, vertices, vertex_count, indices, index_count,
                          vertex_normals, colours, up_axis);
}


bool HeightmapImage::generateBitmap(const Aabb &extents, double resolution, const glm::dvec3 *vertices,
                                    size_t vertex_count, const unsigned *indices, size_t index_count,
                                    const glm::dvec3 *vertex_normals, const glm::dvec3 *colours, UpAxis up_axis)
{
  PROFILE(HeightmapImage_generateBitmap);
  // Render the mesh to a depth buffer.
  return renderHeightMesh(imp_->desired_type, extents, resolution, vertices, vertex_count, indices, index_count,
                          vertex_normals, colours, up_axis);
}


bool HeightmapImage::generateBitmap(const HeightmapMesh &mesh, UpAxis up_axis)
{
  PROFILE(HeightmapImage_generateBitmap);
  const glm::vec3 *colours_ptr = nullptr;
#if 0
  std::vector<glm::vec3> colours(mesh.vertexCount());
  for (size_t i = 0; i < colours.size(); ++i)
  {
    colours[i] = (mesh.vertices()[i] - mesh.meshBoundingBox().minExtents()) / (mesh.meshBoundingBox().maxExtents() - mesh.meshBoundingBox().minExtents());
  }
  colours_ptr = colours.data();
#endif  // #
  return renderHeightMesh(imp_->desired_type, mesh.meshBoundingBox(), mesh.resolution(), mesh.vertices(),
                          mesh.vertexCount(), mesh.triangles(), mesh.triangleCount() * 3, mesh.vertexNormals(),
                          colours_ptr, up_axis);
}


template <typename NormalVec3, typename ColourVec>
bool HeightmapImage::renderHeightMesh(ImageType image_type, const Aabb &spatial_extents, double voxel_resolution,
                                      const glm::dvec3 *vertices, size_t vertex_count, const unsigned *indices,
                                      size_t index_count, const NormalVec3 *vertex_normals, const ColourVec *colours,
                                      UpAxis up_axis)
{
  PROFILE(HeightmapImage_renderHeightMesh);
  if (vertex_count == 0 || index_count == 0)
  {
    return false;
  }

  // First convert vertices to single precision coordinates local to spatial_extents.minExtents(). We need to render on
  // single precision.
  const glm::dvec3 *end_vertex = vertices + vertex_count;
  imp_->vertices.clear();
  imp_->vertices.reserve(vertex_count);
  for (const auto *v = vertices; v < end_vertex; ++v)
  {
    imp_->vertices.emplace_back(glm::vec3(*v - spatial_extents.minExtents()));
  }

  imp_->vertex_normals.clear();
  imp_->vertex_normals.reserve(vertex_count);
  if (vertex_normals)
  {
    const NormalVec3 *end_normal = vertex_normals + vertex_count;
    for (const auto *n = vertex_normals; n < end_normal; ++n)
    {
      imp_->vertex_normals.push_back(glm::vec3(*n));
    }
  }
  else
  {
    for (size_t i = 0; i < vertex_count; ++i)
    {
      imp_->vertex_normals.emplace_back(glm::vec3(0, 0, 1));
    }
  }

  imp_->vertex_colours.clear();
  imp_->vertex_colours.reserve(vertex_count);
  if (colours)
  {
    const ColourVec *end_colour = colours + vertex_count;
    for (const auto *c = colours; c < end_colour; ++c)
    {
      imp_->vertex_colours.push_back(ColourConverter<ColourVec>::vec3(*c));
    }
  }
  else
  {
    for (size_t i = 0; i < vertex_count; ++i)
    {
      imp_->vertex_colours.emplace_back(glm::vec3(1.0f));
    }
  }

  // Get local extents. Vertices are always relative to spatial_extents.minExtents()
  glm::vec3 min_ext_vertices = glm::vec3(0.0f);
  glm::vec3 max_ext_vertices = glm::vec3(spatial_extents.maxExtents() - spatial_extents.minExtents());

  TES_TRIANGLES(g_tes, TES_COLOUR(White), tes::Id(),
                tes::DataBuffer(glm::value_ptr(*imp_->vertices.data()), imp_->vertices.size(), 3,
                                sizeof(glm::vec3) / sizeof(float)),
                tes::DataBuffer(indices, index_count));
  TES_SERVER_UPDATE(g_tes, 0.0f);
  TES_SERVER_UPDATE(g_tes, 0.0f);

  //----------------------------------------------------------------------------
  // Initialise GLFW
  //----------------------------------------------------------------------------

  // Resolve horizontal and vertical axes.
  // Set axes[0] and axes[1] to index the horizontal axes (across the heightmap) and axes[2] to th vertical.
  std::array<int, 3> axes;
  switch (up_axis)
  {
  case UpAxis::kNegZ:
    axes[0] = 1;
    axes[1] = 0;
    axes[2] = 2;
    break;
  case UpAxis::kNegY:
    axes[0] = 2;
    axes[1] = 0;
    axes[2] = 1;
    break;
  case UpAxis::kNegX:
    axes[0] = 2;
    axes[1] = 1;
    axes[2] = 0;
    break;
  case UpAxis::kX:
    axes[0] = 1;
    axes[1] = 2;
    axes[2] = 0;
    break;
  case UpAxis::kY:
    axes[0] = 0;
    axes[1] = 2;
    axes[2] = 1;
    break;
  default:
  case UpAxis::kZ:
    axes[0] = 0;
    axes[1] = 1;
    axes[2] = 2;
    break;
  }

  // Open a window and create its OpenGL context
  const unsigned pixels_per_voxel = std::max(imp_->pixels_per_voxel, 1u);
  const unsigned target_width =
    pixels_per_voxel * unsigned(std::ceil(spatial_extents.diagonal()[axes[0]] / voxel_resolution));
  const unsigned target_height =
    pixels_per_voxel * unsigned(std::ceil(spatial_extents.diagonal()[axes[1]] / voxel_resolution));

  // For some reason it seems the either the buffer size must be a multiple of 16, or the individual dimensions of 8.
  // Making both of 8 addresses both.
  // Otherwise the render texture reports the correct size, but overruns when getting the image back.
  const unsigned render_width = makeMultipleOf(target_width, 8);
  const unsigned render_height = makeMultipleOf(target_height, 8);

  // Adjust the extents to cover the additional pixels.
  max_ext_vertices[axes[0]] += float(render_width - target_width) / float(pixels_per_voxel) * float(voxel_resolution);
  max_ext_vertices[axes[1]] += float(render_height - target_height) / float(pixels_per_voxel) * float(voxel_resolution);

  //----------------------------------------------------------------------------
  // Rendering setup.
  //----------------------------------------------------------------------------

  glfwSetWindowSize(imp_->render_data.window, int(render_width), int(render_height));
  glfwMakeContextCurrent(imp_->render_data.window);
  glViewport(0, 0, render_width, render_height);

  //----------------------------------------------------------------------------
  // Render data setup
  //----------------------------------------------------------------------------
  GLuint vertex_array_id;
  glGenVertexArrays(1, &vertex_array_id);
  glBindVertexArray(vertex_array_id);

  static_assert(sizeof(*indices) == sizeof(GLuint), "GLuint/indices type size mismatch");

  if (imp_->vertex_normals.empty())
  {
    return false;
  }

  // Bind vertex buffer
  GLuint vertex_buffer;
  glGenBuffers(1, &vertex_buffer);
  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer);
  glBufferData(GL_ARRAY_BUFFER, imp_->vertices.size() * sizeof(*imp_->vertices.data()), imp_->vertices.data(),
               GL_STATIC_DRAW);

  // Bind normals buffer.
  GLuint normals_buffer;
  glGenBuffers(1, &normals_buffer);
  glBindBuffer(GL_ARRAY_BUFFER, normals_buffer);
  glBufferData(GL_ARRAY_BUFFER, vertex_count * sizeof(*imp_->vertex_normals.data()), imp_->vertex_normals.data(),
               GL_STATIC_DRAW);

  // Bind colour buffer.
  GLuint colours_buffer;
  glGenBuffers(1, &colours_buffer);
  glBindBuffer(GL_ARRAY_BUFFER, colours_buffer);
  glBufferData(GL_ARRAY_BUFFER, vertex_count * sizeof(*imp_->vertex_colours.data()), imp_->vertex_colours.data(),
               GL_STATIC_DRAW);

  // Generate a buffer for the indices as well
  GLuint index_buffer;
  glGenBuffers(1, &index_buffer);
  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffer);
  glBufferData(GL_ELEMENT_ARRAY_BUFFER, index_count * sizeof(*indices), indices, GL_STATIC_DRAW);

  //----------------------------------------------------------------------------
  // FBO setup.
  //----------------------------------------------------------------------------
  // The framebuffer, which regroups 0, 1, or more textures, and 0 or 1 depth buffer.
  GLuint frame_buffer_id = 0;
  glGenFramebuffers(1, &frame_buffer_id);
  glBindFramebuffer(GL_FRAMEBUFFER, frame_buffer_id);

  // Setup a colour attachment texture.
  GLuint render_texture;
  AttachmentFormat render_texture_format = kAfRgb8;
  glGenTextures(1, &render_texture);

  // "Bind" the newly created texture : all future texture functions will modify this texture
  glBindTexture(GL_TEXTURE_2D, render_texture);

  if (image_type == kImageNormals && colours == nullptr)
  {
    render_texture_format = kAfRgb32f;
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB32F, render_width, render_height, 0, GL_RGB, GL_FLOAT, nullptr);
  }
  else
  {
    render_texture_format = kAfRgb8;
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, render_width, render_height, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr);
  }

  // Poor filtering
  // glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  // glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);

  glBindTexture(GL_TEXTURE_2D, 0);

  // // The depth buffer
  // GLuint depth_render_buffer;
  // glGenRenderbuffers(1, &depth_render_buffer);
  // glBindRenderbuffer(GL_RENDERBUFFER, depth_render_buffer);
  // glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT, render_width, render_height);
  // glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, depth_render_buffer);

  // Alternative : Depth texture. Slower, but you can sample it later in your shader
  GLuint depth_texture;
  AttachmentFormat depth_texture_format = kAfMono32f;
  glGenTextures(1, &depth_texture);
  glBindTexture(GL_TEXTURE_2D, depth_texture);
  glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT32, render_width, render_height, 0, GL_DEPTH_COMPONENT, GL_FLOAT,
               nullptr);
  // glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  // glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);

  glBindTexture(GL_TEXTURE_2D, 0);

  // Set "render_texture" as our colour attachment #0
  glFramebufferTexture(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, render_texture, 0);

  // Depth texture alternative :
  glFramebufferTexture(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, depth_texture, 0);

  glBindFramebuffer(GL_FRAMEBUFFER, 0);

  // Always check that our framebuffer is ok
  if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE)
  {
    return false;
  }

  const bool output_render_texture = (colours != nullptr || image_type != kImageHeights);
  GLuint output_texture_id = (output_render_texture) ? render_texture : depth_texture;
  AttachmentFormat output_texture_format = (output_render_texture) ? render_texture_format : depth_texture_format;
  GLenum output_format_type = (output_render_texture) ? GL_RGB : GL_DEPTH_COMPONENT;

  //----------------------------------------------------------------------------
  // Camera setup
  //----------------------------------------------------------------------------
  int mesh_program_id =
    (colours) ? imp_->render_data.mesh_colours_program_id : imp_->render_data.mesh_normals_program_id;
  glUseProgram(mesh_program_id);

  // Get a handle for our "MVP" uniform
  GLuint mvp_matrix_id = glGetUniformLocation(mesh_program_id, "MVP");
  GLuint v_matrix_id = glGetUniformLocation(mesh_program_id, "V");
  GLuint m_matrix_id = glGetUniformLocation(mesh_program_id, "M");

  const float near_clip = 0.0f;
  const float camera_offset = 0.0f;
  // Near and far clip planes require sufficient buffering to exceed the min/max extents range.
  // So near is at 1.0f (to avoid some depth precision issues), far is near clip (1) + range + camera_offset (2)
  const glm::mat4 projection_matrix =
    glm::ortho(-0.5f * max_ext_vertices[axes[0]] - min_ext_vertices[axes[0]],
               0.5f * max_ext_vertices[axes[0]] - min_ext_vertices[axes[0]],
               -0.5f * max_ext_vertices[axes[1]] - min_ext_vertices[axes[1]],
               0.5f * max_ext_vertices[axes[1]] - min_ext_vertices[axes[1]], near_clip,
               near_clip + camera_offset + max_ext_vertices[axes[2]] - min_ext_vertices[axes[2]]);
  // Look down from above.
  glm::vec3 eye = 0.5f * glm::vec3(min_ext_vertices + max_ext_vertices);
  glm::vec3 target = eye;
  eye[axes[2]] = max_ext_vertices[axes[2]] + camera_offset;
  target[axes[2]] = 0;
  glm::vec3 view_up(0.0f);
  view_up[axes[1]] = (int(up_axis) >= 0) ? 1.0f : -1.0f;
  const glm::mat4 view_matrix = glm::lookAt(eye, target, view_up);

  const glm::mat4 model_matrix = glm::mat4(1.0);
  const glm::mat4 mvp_matrix = projection_matrix * view_matrix * model_matrix;

  // Send our transformation to the currently bound shader,
  // in the "mvp_matrix" uniform
  glUniformMatrix4fv(mvp_matrix_id, 1, GL_FALSE, &mvp_matrix[0][0]);
  glUniformMatrix4fv(m_matrix_id, 1, GL_FALSE, &model_matrix[0][0]);
  glUniformMatrix4fv(v_matrix_id, 1, GL_FALSE, &view_matrix[0][0]);

  // Render to our framebuffer
  glBindFramebuffer(GL_FRAMEBUFFER, frame_buffer_id);

  glViewport(0, 0, render_width, render_height);

  // Clear the screen
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);  // NOLINT(hicpp-signed-bitwise)

  //-------------------------------------------
  // Render heightmap to FBO
  //-------------------------------------------
  glUseProgram(mesh_program_id);

  // 1rst attribute buffer : vertices
  glEnableVertexAttribArray(0);
  glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer);
  glVertexAttribPointer(0,         // index
                        3,         // size
                        GL_FLOAT,  // type
                        GL_FALSE,  // normalized?
                        0,         // stride
                        nullptr    // array buffer offset
  );

  // Normals stream.
  glEnableVertexAttribArray(1);
  glBindBuffer(GL_ARRAY_BUFFER, normals_buffer);
  glVertexAttribPointer(1,         // index
                        3,         // size
                        GL_FLOAT,  // type
                        GL_TRUE,   // normalized?
                        0,         // stride
                        nullptr    // array buffer offset
  );

  // Vertex colour stream.
  glEnableVertexAttribArray(2);
  glBindBuffer(GL_ARRAY_BUFFER, colours_buffer);
  glVertexAttribPointer(2,         // index
                        3,         // size
                        GL_FLOAT,  // type
                        GL_FALSE,  // normalized?
                        0,         // stride
                        nullptr    // array buffer offset
  );

  // Index buffer
  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffer);

  // Draw the triangles !
  glDrawElements(GL_TRIANGLES,          // mode
                 GLsizei(index_count),  // count
                 GL_UNSIGNED_INT,       // type
                 nullptr                // element array buffer offset
  );

  glDisableVertexAttribArray(0);

  glBindFramebuffer(GL_FRAMEBUFFER, 0);

  // Render to window
  if (imp_->render_data.show_window)
  {
    // Set the list of draw buffers.
    std::array<GLenum, 1> quad_draw_buffers = { GL_COLOR_ATTACHMENT0 };
    glDrawBuffers(GLsizei(quad_draw_buffers.size()), quad_draw_buffers.data());

    do
    {
      // Render to the screen
      glBindFramebuffer(GL_FRAMEBUFFER, 0);
      // Render on the whole framebuffer, complete from the lower left corner to the upper right
      glViewport(0, 0, render_width, render_height);

      // Clear the screen
      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);  // NOLINT(hicpp-signed-bitwise)

      // Use our shader
      glUseProgram(imp_->render_data.quad_program_id);

      // Bind our texture in Texture Unit 0
      glActiveTexture(GL_TEXTURE0);
      glBindTexture(GL_TEXTURE_2D, output_texture_id);
      // Set our "render_texture" sampler to use Texture Unit 0
      glUniform1i(imp_->render_data.fbo_tex_id, 0);

      // 1rst attribute buffer : vertices
      glEnableVertexAttribArray(0);
      glBindBuffer(GL_ARRAY_BUFFER, imp_->render_data.quad_vertex_buffer);
      glVertexAttribPointer(0,  // attribute 0. No particular reason for 0, but must match the layout in the shader.
                            3,  // size
                            GL_FLOAT,  // type
                            GL_FALSE,  // normalized?
                            0,         // stride
                            nullptr    // array buffer offset
      );

      // Draw the triangles !
      glDrawArrays(GL_TRIANGLES, 0, 6);  // 2*3 indices starting at 0 -> 2 triangles

      glDisableVertexAttribArray(0);

      // Swap buffers
      glfwSwapBuffers(imp_->render_data.window);
      glfwSwapBuffers(imp_->render_data.window);
      glfwPollEvents();
    } while (imp_->render_data.block_on_show_window &&
             glfwGetKey(imp_->render_data.window, GLFW_KEY_ESCAPE) != GLFW_PRESS &&
             glfwWindowShouldClose(imp_->render_data.window) == 0);
  }

  imp_->image_info.image_width = render_width;
  imp_->image_info.image_height = render_height;

  imp_->image_info.image_extents =
    Aabb(spatial_extents.minExtents(), spatial_extents.minExtents() + glm::dvec3(max_ext_vertices));
  imp_->image_info.type = (colours) ? kImageVertexColours888 : image_type;

  // Read pixels:
  switch (output_texture_format)
  {
  case kAfRgb8:
    imp_->image_info.bpp = 3;
    imp_->image_info.byte_count =
      size_t(imp_->image_info.image_width) * size_t(imp_->image_info.image_height) * size_t(imp_->image_info.bpp);
    imp_->image.resize(imp_->image_info.byte_count);
    // imp_->image.resize(imp_->image_info.byte_count * 2);
    // memset(imp_->image.data() + imp_->image_info.byte_count, 0xcf, imp_->image_info.byte_count);

    // textureBufferInfo(output_texture_id);
    glBindTexture(GL_TEXTURE_2D, output_texture_id);
    glGetTexImage(GL_TEXTURE_2D, 0, output_format_type, GL_UNSIGNED_BYTE, imp_->image.data());
    // if (imp_->image[imp_->image_info.image_width * imp_->image_info.image_height * imp_->image_info.bpp] != 0xcf)
    // {
    //   unsigned last_bad_index = imp_->image_info.image_width * imp_->image_info.image_height * imp_->image_info.bpp;
    //   while (last_bad_index < imp_->image_info.image_width * imp_->image_info.image_height * imp_->image_info.bpp * 2
    //   &&
    //          imp_->image[last_bad_index] != 0xcf)
    //   {
    //     ++last_bad_index;
    //   }
    //   printf("(%u,%u) x %u -> %u : prev(%u,%u) bad pixel to %u\n\n", render_width, render_height, image_size,
    //          image_size * imp_->image_info.bpp, prev_width, prev_height,
    //          (last_bad_index - imp_->image_info.image_width * imp_->image_info.image_height * imp_->image_info.bpp) /
    //            imp_->image_info.bpp);
    // }
    // else
    // {
    //   printf("(%u,%u) x %u -> %u : prev(%u,%u)\n",
    //          render_width, render_height, image_size, image_size * imp_->image_info.bpp, prev_width, prev_height);
    // }
    // savePng(imp_->image, render_width, render_height);
    break;
  case kAfRgb32f:
    imp_->image_info.bpp = 3 * sizeof(float);
    imp_->image_info.byte_count =
      size_t(imp_->image_info.image_width) * size_t(imp_->image_info.image_height) * size_t(imp_->image_info.bpp);
    imp_->image.resize(imp_->image_info.byte_count);
    glBindTexture(GL_TEXTURE_2D, output_texture_id);
    glGetTexImage(GL_TEXTURE_2D, 0, output_format_type, GL_FLOAT, imp_->image.data());
    break;
  case kAfMono32f:
    // We are reading the depth buffer, which is float format. We size the image bytes appropriately.
    imp_->image_info.bpp = sizeof(float);
    imp_->image_info.byte_count =
      size_t(imp_->image_info.image_width) * size_t(imp_->image_info.image_height) * size_t(imp_->image_info.bpp);
    imp_->image.resize(imp_->image_info.byte_count);
    glBindTexture(GL_TEXTURE_2D, output_texture_id);
    glGetTexImage(GL_TEXTURE_2D, 0, output_format_type, GL_FLOAT, imp_->image.data());
    break;
  default:
    // Unkown type;
    imp_->image_info.bpp = 0;
    imp_->image_info.byte_count = 0;
    imp_->image_info.image_width = 0;
    imp_->image_info.image_height = 0;
    imp_->image.resize(0);
    break;
  }

  // Cleanup
  glDeleteBuffers(1, &vertex_buffer);
  glDeleteBuffers(1, &normals_buffer);
  glDeleteBuffers(1, &colours_buffer);
  glDeleteBuffers(1, &index_buffer);
  glDeleteVertexArrays(1, &vertex_array_id);

  glDeleteFramebuffers(1, &frame_buffer_id);
  glDeleteTextures(1, &render_texture);
  glDeleteTextures(1, &depth_texture);
  // glDeleteRenderbuffers(1, &depth_render_buffer);

  return true;
}
}  // namespace ohm
#include <utility>


================================================
FILE: ohmheightmapimage/HeightmapImage.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMHEIGHTMAPIMAGE_HEIGHTMAPIMAGE_H
#define OHMHEIGHTMAPIMAGE_HEIGHTMAPIMAGE_H

#include "OhmHeightmapImageConfig.h"

#include <ohm/Aabb.h>

#include <ohmheightmap/UpAxis.h>

#include <glm/fwd.hpp>

#include <memory>

namespace ohm
{
struct Colour;
class Heightmap;
class HeightmapMesh;

struct HeightmapImageDetail;

/// Experimental conversion of a @c Heightmap into an image by rendering to an OpenGL FGO.
///
/// The resulting image may be either an RGB image where the RGB values map the local surface normals (like a
/// normal map in rendering) or a grey scale depth value, relative to the min/max for the input data set.
/// The RGB image may be provided either as a pure RGB uint8 format (1 byte per channel) using @c kImageNormals888
/// or as 32-bit floating point colour channels ranging [0, 1] using @c kImageNormals. Using @c kImageNormals, the
/// colour channels need to be remapped from [0, 1] to [-1, 1] by a shift and scale (2 * c - 1).
///
/// Surface normals are derived from the local vertex normals and interpolated across the surface. The vertex normals
/// are calculated according to @c NormalsMode.
///
/// Typical usage is:
/// - Generate a @c Heightmap.
/// - Construct a @c HeightmapImage around the @c Heightmap.
/// - Call @c generateBitmap()
/// - Fetch the results using @c bitmap().
/// - Dispose of the @c HeightmapImage before the @c Heightmap.
class ohmheightmapimage_API HeightmapImage
{
public:
  enum ohmheightmapimage_API ImageType
  {
    /// Extracted image consists of three floats per pixel. A normal can be derived from each pixel by taking the
    /// RGB float values and converting each channel by multiplying it by 2 and subtracting 1. This is to convert from
    /// the range [0, 1] to the range [-1, 1]. This is shown below.
    /// @code
    /// glm::vec3 convertColourToNormal(const glm::vec3 &c)
    /// {
    ///   return 2.0f * c - glm::vec3(1.0f);
    /// }
    /// @endcode
    kImageNormals,
    /// Extract RBG 888 image where colours represent surface normals.
    kImageNormals888,
    /// Extract a depth image with 4-byte float values for each pixel (depth).
    kImageHeights,
    /// Image has been generated using vertex colours. This is not be to used in the constructor, but may be returned
    /// as @c generatedImageType() after @c generateBitmap().
    kImageVertexColours888
  };

  /// Information about the generated bitmap.
  struct ohmheightmapimage_API BitmapInfo
  {
    /// Image pixel width.
    unsigned image_width = 0;
    /// Image pixel height.
    unsigned image_height = 0;
    /// Number of bytes per pixel.
    unsigned bpp = 0;
    /// Details on the pixel format.
    ImageType type = kImageNormals;
    /// Number of bytes in the image.
    size_t byte_count = 0;
    /// Image spatial extents.
    Aabb image_extents = Aabb(0.0);
  };

  explicit HeightmapImage(ImageType type = kImageNormals, unsigned pixels_per_voxel = 1);
  ~HeightmapImage();

  ImageType desiredImageType();
  void setDesiredImageType(ImageType type);

  unsigned pixelsPerVoxel();
  void setPixelsPerVoxel(unsigned ppv);

  bool showWindow() const;
  void setShowWindow(bool show_window);

  bool blockOnShowWindow() const;
  void setBlockOnShowWindow(bool block);

  /// Query the number of bytes required to extract the heightmap into a bitmap using @c extractBitmap().
  ///
  /// Note the @c ImageType in @p info will differ from @c desiredImageType() when vertex colours were provided in
  /// generation. In this case the @c ImageType will be @c kImageVertexColours888.
  ///
  /// @return The number of bytes required for the bitmap.
  const uint8_t *bitmap(BitmapInfo *info) const;

  /// Render an image from the given heightmap mesh.
  ///
  /// Pixels are coloured according to @c desiredImageType() or using the provided @c colours. When @p colours are
  /// provided, the image is rendered using vertex colouring, interpolated across the mesh. The colour format is RGB24
  /// using 3 bytes per pixel using 1 byte each for RGB colour channels.
  ///
  /// Withour explicit @c colours, both @c kImageNormals888 and @c kImageNormals the colours represent the surface
  /// normal. A horizontal surface maps to the colour (0.5, 0.5, 1.0) and deviations from horizontal decrease the
  /// blue channel towards 0.5, while increasing the red and green channels. The surface normal is essentially
  /// recovered by remapping each channel from the range [0, 1] to the range [-1, 1].
  ///
  /// The bitmap format for @c kImageNormals888 is RGB24 as described above. When using @c kImageNormals the colour
  /// format is RGB float 32, where each colour channel is represented by a 32-bit float.
  ///
  /// In @p kImageHeights, each pixel is a 32-bit float representing the relative depth.
  ///
  /// Note the generated image format is only attainable by calling @c bitmap() and reading the @c BitmapInfo.
  ///
  ///
  /// @param extents Defines the axis aligned bounding box enclosing the mesh to render. This may be larger than that
  ///   required, in which case the image will feature a black border.
  /// @param resolution The spatial resolution of each vertex. This maps to the resolution fo the voxel map used to
  ///   generate the mesh and is used to resolve @p pixelsPerVoxel().
  /// @param vertices The vertex array.
  /// @param vertex_count Number of elements in @p vertices.
  /// @param indices The triangle index array, three indices per triangle.
  /// @param index_count Number of indices; must be a multiple of 3.
  /// @param vertex_normals The normal at each vertex. Required for @c kImageNormals and kImagenormals888.
  /// @param colours Per vertex colours override. Forces the generated image to be @c kImageVertexColours888.
  ///     Various overloads support different input colour formats.
  /// @param up_axis Identifies the up axis.
  bool generateBitmap(const Aabb &extents, double resolution, const glm::dvec3 *vertices, size_t vertex_count,
                      const unsigned *indices, size_t index_count, const glm::dvec3 *vertex_normals = nullptr,
                      const Colour *colours = nullptr, UpAxis up_axis = UpAxis::kZ);

  /// @overload
  bool generateBitmap(const Aabb &extents, double resolution, const glm::dvec3 *vertices, size_t vertex_count,
                      const unsigned *indices, size_t index_count, const glm::dvec3 *vertex_normals,
                      const glm::vec3 *colours, UpAxis up_axis = UpAxis::kZ);

  /// @overload
  bool generateBitmap(const Aabb &extents, double resolution, const glm::dvec3 *vertices, size_t vertex_count,
                      const unsigned *indices, size_t index_count, const glm::dvec3 *vertex_normals,
                      const glm::dvec3 *colours, UpAxis up_axis = UpAxis::kZ);

  /// Generate a bitmap image from a @c HeightmapMesh. Does not support @c kImageVertexColours888 image mode.
  /// @param mesh The mesh to generate an image of.
  /// @param up_axis Identifies the up axis.
  bool generateBitmap(const HeightmapMesh &mesh, UpAxis up_axis = UpAxis::kZ);

private:
  template <typename NormalVec3, typename ColourVec>
  bool renderHeightMesh(ImageType type, const Aabb &spatial_extents, double voxel_resolution,
                        const glm::dvec3 *vertices, size_t vertex_count, const unsigned *indices, size_t index_count,
                        const NormalVec3 *vertex_normals, const ColourVec *colours, UpAxis up_axis);

  std::unique_ptr<HeightmapImageDetail> imp_;
};
}  // namespace ohm

#endif  // OHMHEIGHTMAPIMAGE_HEIGHTMAPIMAGE_H


================================================
FILE: ohmheightmapimage/OhmHeightmapImageConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMHEIGHTMAPIMAGECONFIG_H
#define OHMHEIGHTMAPIMAGECONFIG_H

#include "OhmHeightmapUtilExport.h"

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX
#include <cmath>

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

// Enable various validation tests throughout this library.
//#cmakedefine OHM_FEATURE_THREADS

#include "OhmConfig.h"

#endif  // OHMHEIGHTMAPIMAGECONFIG_H


================================================
FILE: ohmtools/CMakeLists.txt
================================================

include(GenerateExportHeader)



configure_file(OhmToolsConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/ohmtools/OhmToolsConfig.h")

set(SOURCES
  OhmCloud.cpp
  OhmCloud.h
  OhmGen.cpp
  OhmGen.h
  OhmToolsConfig.in.h
)

set(PUBLIC_HEADERS
  OhmCloud.h
  OhmGen.h
  "${CMAKE_CURRENT_BINARY_DIR}/ohmtools/OhmToolsConfig.h"
  "${CMAKE_CURRENT_BINARY_DIR}/ohmtools/OhmToolsExport.h"
)

add_library(ohmtools ${SOURCES})
clang_tidy_target(ohmtools)

generate_export_header(ohmtools
      EXPORT_MACRO_NAME ohmtools_API
      EXPORT_FILE_NAME ohmtools/OhmToolsExport.h
      STATIC_DEFINE OHMTOOLS_STATIC)

target_link_libraries(ohmtools
  PUBLIC
    ohm
    ohmheightmap
    ohmutil
    glm::glm
)

target_include_directories(ohmtools
  PUBLIC
    $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/ohmtools>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
)

install(TARGETS ohmtools EXPORT ${CMAKE_PROJECT_NAME}-config-targets
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/ohmtools
)

install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/ohmtools)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})


================================================
FILE: ohmtools/OhmCloud.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmCloud.h"

#include <ohm/Density.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyType.h>
#include <ohm/Query.h>
#include <ohm/VoxelData.h>

#include <ohmheightmap/Heightmap.h>
#include <ohmheightmap/HeightmapUtil.h>
#include <ohmheightmap/HeightmapVoxel.h>

#include <ohmheightmap/private/HeightmapDetail.h>

#include <ohmutil/Colour.h>
#include <ohmutil/PlyMesh.h>
#include <ohmutil/PlyPointStream.h>

#include <algorithm>
#include <fstream>
#include <limits>

namespace
{
const char *const kPropertyRed = "red";
const char *const kPropertyGreen = "green";
const char *const kPropertyBlue = "blue";

ohm::PlyPointStream setupPlyStream(
  bool enable_colour, const std::initializer_list<ohm::PlyPointStream::Property> &additional_properties = {})
{
  std::vector<ohm::PlyPointStream::Property> ply_properties = {
    ohm::PlyPointStream::Property{ "x", ohm::PlyPointStream::Type::kFloat64 },
    ohm::PlyPointStream::Property{ "y", ohm::PlyPointStream::Type::kFloat64 },
    ohm::PlyPointStream::Property{ "z", ohm::PlyPointStream::Type::kFloat64 }
  };

  if (enable_colour)
  {
    ply_properties.emplace_back(ohm::PlyPointStream::Property{ kPropertyRed, ohm::PlyPointStream::Type::kUInt8 });
    ply_properties.emplace_back(ohm::PlyPointStream::Property{ kPropertyGreen, ohm::PlyPointStream::Type::kUInt8 });
    ply_properties.emplace_back(ohm::PlyPointStream::Property{ kPropertyBlue, ohm::PlyPointStream::Type::kUInt8 });
  }

  for (const auto &additional_property : additional_properties)
  {
    ply_properties.emplace_back(additional_property);
  }

  return ohm::PlyPointStream(ply_properties);
}


struct ExtractedVoxel
{
  glm::dvec3 position = glm::dvec3(0);
  ohm::Colour colour = ohm::Colour(0);
};

enum SaveWithFlags
{
  WithColour = (1 << 0)
};

uint64_t saveAnyCloud(const std::string &file_name, const ohm::OccupancyMap &map,
                      std::function<bool(ExtractedVoxel &, const ohm::OccupancyMap::const_iterator &)> extract_voxel,
                      unsigned with_flags, const ohmtools::ProgressCallback &prog)
{
  std::ofstream out(file_name, std::ios::binary);

  if (!out.is_open())
  {
    return 0;
  }

  ExtractedVoxel voxel{};
  const size_t region_count = map.regionCount();
  size_t processed_region_count = 0;
  glm::i16vec3 last_region = (map.regionCount() > 0) ? map.begin().key().regionKey() : glm::i16vec3(0);

  // Setup the Ply stream.
  ohm::PlyPointStream ply = setupPlyStream((with_flags & WithColour) != 0);
  ply.open(out);

  uint64_t point_count = 0;
  for (auto iter = map.begin(); iter != map.end(); ++iter)
  {
    // Progress update.
    if (last_region != iter.key().regionKey())
    {
      ++processed_region_count;
      if (prog)
      {
        prog(processed_region_count, region_count);
      }
      last_region = iter.key().regionKey();
    }

    if (extract_voxel(voxel, iter))
    {
      ply.setPointPosition(voxel.position);
      if (with_flags & WithColour)
      {
        ply.setProperty(kPropertyRed, voxel.colour.r());
        ply.setProperty(kPropertyGreen, voxel.colour.g());
        ply.setProperty(kPropertyBlue, voxel.colour.b());
      }

      ply.writePoint();
      ++point_count;
    }
  }

  ply.close();
  out.close();

  return point_count;
}

void addVoxel(ohm::PlyMesh &ply, const glm::dvec3 &position, double resolution, const ohm::Colour &colour)
{
  const std::array<glm::dvec3, 8> vertices = {
    position + glm::dvec3(-0.5 * resolution, -0.5 * resolution, -0.5 * resolution),
    position + glm::dvec3(0.5 * resolution, -0.5 * resolution, -0.5 * resolution),
    position + glm::dvec3(0.5 * resolution, 0.5 * resolution, -0.5 * resolution),
    position + glm::dvec3(-0.5 * resolution, 0.5 * resolution, -0.5 * resolution),
    position + glm::dvec3(-0.5 * resolution, -0.5 * resolution, 0.5 * resolution),
    position + glm::dvec3(0.5 * resolution, -0.5 * resolution, 0.5 * resolution),
    position + glm::dvec3(0.5 * resolution, 0.5 * resolution, 0.5 * resolution),
    position + glm::dvec3(-0.5 * resolution, 0.5 * resolution, 0.5 * resolution)
  };

  const std::array<ohm::Colour, 8> colours = { colour, colour, colour, colour, colour, colour, colour, colour };

  const unsigned base_index = ply.addVertices(vertices.data(), unsigned(vertices.size()), colours.data());

  std::array<unsigned, 6 * 2 * 3> indices =  //
    {
      0, 2, 1, 0, 3, 2,  // Bottom
      4, 5, 6, 4, 6, 7,  // Top
      0, 1, 5, 0, 5, 4,  // Front
      2, 3, 7, 2, 7, 6,  // Back
      0, 7, 3, 0, 4, 7,  // Left
      1, 2, 6, 1, 6, 5   // Right
    };
  for (auto &index : indices)
  {
    index += base_index;
  }
  ply.addTriangles(indices.data(), 12, colours.data());
}  // namespace ohmtools


uint64_t saveAnyVoxels(const std::string &file_name, const ohm::OccupancyMap &map,
                       std::function<bool(ExtractedVoxel &, const ohm::OccupancyMap::const_iterator &)> extract_voxel,
                       unsigned with_flags, const ohmtools::ProgressCallback &prog)
{
  std::ofstream out(file_name, std::ios::binary);

  if (!out.is_open())
  {
    return 0;
  }

  // Ply voxel mesh.
  ohm::PlyMesh ply;

  ExtractedVoxel voxel{};
  const double resolution = map.resolution();
  const size_t region_count = map.regionCount();
  size_t processed_region_count = 0;
  glm::i16vec3 last_region = (map.regionCount() > 0) ? map.begin().key().regionKey() : glm::i16vec3(0);

  uint64_t voxel_count = 0;
  for (auto iter = map.begin(); iter != map.end(); ++iter)
  {
    // Progress update.
    if (last_region != iter.key().regionKey())
    {
      ++processed_region_count;
      if (prog)
      {
        prog(processed_region_count, region_count);
      }
      last_region = iter.key().regionKey();
    }

    if (extract_voxel(voxel, iter))
    {
      const ohm::Colour c = (with_flags & WithColour) ? voxel.colour : ohm::Colour(255, 255, 255);
      addVoxel(ply, voxel.position, resolution, c);
      ++voxel_count;
    }
  }

  if (!ply.save(out, true))
  {
    // Failed.
    voxel_count = 0;
  }

  out.close();
  return voxel_count;
}


}  // namespace

namespace ohmtools
{
const ohm::Colour ColourByHeight::kDefaultFrom(128, 255, 0);
const ohm::Colour ColourByHeight::kDefaultTo(120, 0, 255);

ColourByHeight::ColourByHeight(const ohm::OccupancyMap &map)
  : ColourByHeight(map, kDefaultFrom, kDefaultTo)
{}


ColourByHeight::ColourByHeight(const ohm::OccupancyMap &map, const ohm::Colour &from, const ohm::Colour &to)
  : colours({ from, to })
{
  map.calculateExtents(nullptr, nullptr, &range_);
}


ColourByHeight::ColourByHeight(const ohm::KeyRange &extents)
  : ColourByHeight(extents, kDefaultFrom, kDefaultTo)
{}


ColourByHeight::ColourByHeight(const ohm::KeyRange &extents, const ohm::Colour &from, const ohm::Colour &to)
  : colours({ from, to })
  , range_(extents)
{}


ohm::Colour ColourByHeight::select(const ohm::Key &key) const
{
  const double vertical_range = std::max(1.0, double(range_.range()[up_axis_]));
  const double offset = std::max(
    0.0, std::min(double(ohm::OccupancyMap::rangeBetween(range_.minKey(), key, range_.regionDimensions())[up_axis_]),
                  vertical_range));
  const auto factor = float(offset / vertical_range);
  return ohm::Colour::lerp(colours[0], colours[1], factor);
}


ohm::Colour ColourByHeight::select(const ohm::Voxel<const float> &occupancy) const
{
  return select(occupancy.key());
}


ColourByType::ColourByType(const ohm::OccupancyMap &map)
  : occupancy_threshold_(map.occupancyThresholdValue())
{}


ohm::Colour ColourByType::select(const ohm::Voxel<const float> &occupancy) const
{
  if (occupancy.isValid())
  {
    if (occupancy.data() >= occupancy_threshold_)
    {
      return occupied_colour;
    }
    return free_colour;
  }

  return ohm::Colour(0, 0, 0, 255);
}


ColourByOccupancy::ColourByOccupancy(const ohm::OccupancyMap &map, bool ramp_occupied_range)
  : ramp_occupied_range(ramp_occupied_range)
  , occupancy_threshold_probability_(map.occupancyThresholdProbability())
{
  const ColourByType default_colours(map);
  colours[0] = default_colours.free_colour;
  colours[1] = default_colours.occupied_colour;
}


ColourByOccupancy::ColourByOccupancy(const ohm::OccupancyMap &map, const ohm::Colour &from, const ohm::Colour &to,
                                     bool ramp_occupied_range)
  : ColourByOccupancy(map, ramp_occupied_range)
{
  colours[0] = from;
  colours[1] = to;
}


ohm::Colour ColourByOccupancy::select(const ohm::Voxel<const float> &occupancy)
{
  return select(occupancy.isValid() ? ohm::valueToProbability(occupancy.data()) : 0.0f);
}


ohm::Colour ColourByOccupancy::select(const float occupancy) const
{
  float factor = occupancy;
  if (ramp_occupied_range)
  {
    factor = (factor - occupancy_threshold_probability_) / (1.0f - occupancy_threshold_probability_);
  }
  factor = std::max(0.0f, std::min(factor, 1.0f));
  return ohm::Colour::lerp(colours[0], colours[1], factor);
}


const ohm::Colour ColourByIntensity::kDefaultFrom(154, 52, 3);
const ohm::Colour ColourByIntensity::kDefaultTo(255, 255, 212);

ColourByIntensity::ColourByIntensity(const ohm::OccupancyMap &map, float max_intensity)
  : max_intensity(max_intensity)
  , intensity_(&map, map.layout().intensityLayer())
{
  colours[0] = kDefaultFrom;
  colours[1] = kDefaultTo;
}


ColourByIntensity::ColourByIntensity(const ohm::OccupancyMap &map, const ohm::Colour &from, const ohm::Colour &to,
                                     float max_intensity)
  : ColourByIntensity(map, max_intensity)
{
  colours[0] = from;
  colours[1] = to;
}


ohm::Colour ColourByIntensity::select(const ohm::Voxel<const float> &occupancy)
{
  intensity_.setKey(occupancy);
  if (intensity_.isValid())
  {
    return select(intensity_.data().intensity_mean);
  }
  return colours[1];
}


ohm::Colour ColourByIntensity::select(const float intensity) const
{
  const float factor = std::max(0.0f, std::min(intensity, max_intensity)) / max_intensity;
  return ohm::Colour::lerp(colours[0], colours[1], factor);
}


ColourHeightmapLayer::ColourHeightmapLayer(const ohm::OccupancyMap &map)
{
  ohm::HeightmapDetail heightmap_detail;
  heightmap_detail.fromMapInfo(map.mapInfo());
  heightmap_up_axis_ = heightmap_detail.vertical_axis_index;
}


ohm::Colour ColourHeightmapLayer::select(const ohm::Voxel<const float> &occupancy) const
{
  using Colour = ohm::Colour;
  static const std::array<Colour, 10> colour_cycle =  //
    {                                                 //
      Colour::kColours[Colour::kPaleVioletRed], Colour::kColours[Colour::kFireBrick],
      Colour::kColours[Colour::kOrange],        Colour::kColours[Colour::kLightYellow],
      Colour::kColours[Colour::kWheat],         Colour::kColours[Colour::kChartreuse],
      Colour::kColours[Colour::kTurquoise],     Colour::kColours[Colour::kSkyBlue],
      Colour::kColours[Colour::kPlum],          Colour::kColours[Colour::kMediumBlue]
    };

  // Many assumptions here that each heightmap region is 1 voxel tall.
  int index = occupancy.key().regionKey()[heightmap_up_axis_];
  return colour_cycle[index % colour_cycle.size()];
}


ColourHeightmapType::ColourHeightmapType(const ohm::OccupancyMap &map)
{
  heightmap_layer_ = map.layout().layerIndex(ohm::HeightmapVoxel::kHeightmapLayer);
}


ohm::Colour ColourHeightmapType::select(const ohm::Voxel<const float> &occupancy) const
{
  if (heightmap_layer_ >= 0 && occupancy.isValid())
  {
    ohm::Voxel<const ohm::HeightmapVoxel> heightmap_voxel(occupancy.map(), heightmap_layer_);
    heightmap_voxel.setKey(occupancy);
    if (heightmap_voxel.isValid())
    {
      const float occ_value = occupancy.data();
      const bool is_base_layer = heightmap_voxel.data().layer == ohm::kHvlBaseLayer;
      const float colour_adjust = (is_base_layer) ? 1.0f : 0.4f;
      if (occ_value == ohm::Heightmap::kHeightmapSurfaceValue)
      {
        // Darken if not the base layer.
        return surface_colour.adjust(colour_adjust);
      }
      if (occ_value == ohm::Heightmap::kHeightmapVirtualSurfaceValue)
      {
        return virtual_colour.adjust(colour_adjust);
      }
    }
  }

  return ohm::Colour(0, 0, 0, 255);
}


const ohm::Colour ColourByHeightmapClearance::kDefaultLow(255, 128, 0);
const ohm::Colour ColourByHeightmapClearance::kDefaultHigh(255, 0, 0);

ColourByHeightmapClearance::ColourByHeightmapClearance(const ohm::OccupancyMap &map, double clearance_scale)
  : ColourByHeightmapClearance(map, kDefaultLow, kDefaultHigh, clearance_scale)
{}


ColourByHeightmapClearance::ColourByHeightmapClearance(const ohm::OccupancyMap &map, const ohm::Colour &low,
                                                       const ohm::Colour &high, double clearance_scale)
{
  colours[0] = low;
  colours[1] = high;
  min_clearance_ = ohm::heightmap::queryHeightmapClearance(map.mapInfo());
  max_clearance_ = min_clearance_ * clearance_scale;
  heightmap_layer_ = map.layout().layerIndex(ohm::HeightmapVoxel::kHeightmapLayer);
}


ohm::Colour ColourByHeightmapClearance::select(const ohm::Voxel<const float> &occupancy) const
{
  if (heightmap_layer_ >= 0 && occupancy.isValid())
  {
    ohm::Voxel<const ohm::HeightmapVoxel> heightmap_voxel(occupancy.map(), heightmap_layer_);
    heightmap_voxel.setKey(occupancy);
    if (heightmap_voxel.isValid())
    {
      const double clearance = heightmap_voxel.data().clearance;
      const double lerp_range = (max_clearance_ - min_clearance_);
      const double lerp_factor =
        (lerp_range > std::numeric_limits<double>::epsilon()) ? (clearance - min_clearance_) / lerp_range : 1.0;
      return ohm::Colour::lerp(colours[0], colours[1], float(lerp_factor));
    }
  }

  return ohm::Colour(255, 255, 255, 255);
}


uint64_t saveCloud(const std::string &file_name, const ohm::OccupancyMap &map, const SaveCloudOptions &opt,
                   const ProgressCallback &prog)
{
  // Work out if we need colour.
  unsigned with_flags = 0;
  auto colour_select = opt.colour_select;
  std::unique_ptr<ColourByHeight> colour_by_height;
  if (!colour_select && opt.allow_default_colour_selection)
  {
    colour_by_height = std::make_unique<ColourByHeight>(map);
    colour_select = [&colour_by_height](const ohm::Voxel<const float> &occupancy) {
      return colour_by_height->select(occupancy);
    };
  }

  if (colour_select)
  {
    with_flags |= WithColour;
  }

  ohm::Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  auto mean = (opt.ignore_voxel_mean) ? ohm::Voxel<const ohm::VoxelMean>() :
                                        ohm::Voxel<const ohm::VoxelMean>(&map, map.layout().meanLayer());

  const auto extract_voxel = [&map, &opt, &occupancy, &mean, colour_select](
                               ExtractedVoxel &voxel, const ohm::OccupancyMap::const_iterator &iter) -> bool {
    ohm::setVoxelKey(iter, occupancy, mean);
    if (isOccupied(occupancy) || opt.export_free && isFree(occupancy))
    {
      voxel.position = (mean.isLayerValid()) ? positionSafe(mean) : map.voxelCentreGlobal(*iter);
      if (colour_select)
      {
        voxel.colour = colour_select(occupancy);
      }
      return true;
    }
    return false;
  };

  return ::saveAnyCloud(file_name, map, extract_voxel, with_flags, prog);
}


uint64_t saveDensityCloud(const std::string &file_name, const ohm::OccupancyMap &map,
                          const SaveDensityCloudOptions &opt, const ProgressCallback &prog)
{
  ohm::Voxel<const float> traversal(&map, map.layout().traversalLayer());
  ohm::Voxel<const ohm::VoxelMean> mean(&map, map.layout().meanLayer());
  unsigned with_flags = 0;

  if (!traversal.isLayerValid() || !mean.isLayerValid())
  {
    return 0;
  }

  // Work out if we need colour.
  auto colour_select = opt.colour_select;
  std::unique_ptr<ColourByHeight> colour_by_height;
  if (!colour_select && opt.allow_default_colour_selection)
  {
    colour_by_height = std::make_unique<ColourByHeight>(map);
    colour_select = [&colour_by_height](const ohm::Voxel<const float> &traversal) {
      return colour_by_height->select(traversal);
    };
  }

  if (colour_select)
  {
    with_flags |= WithColour;
  }

  const auto extract_voxel = [&map, &opt, &traversal, &mean, colour_select](
                               ExtractedVoxel &voxel, const ohm::OccupancyMap::const_iterator &iter) -> bool {
    const float density = voxelDensity(traversal, mean);
    if (density >= opt.density_threshold)
    {
      voxel.position = (mean.isLayerValid()) ? positionSafe(mean) : map.voxelCentreGlobal(*iter);
      voxel.position = (!opt.ignore_voxel_mean) ? positionSafe(mean) : map.voxelCentreGlobal(*iter);
      if (colour_select)
      {
        voxel.colour = colour_select(traversal);
      }
      return true;
    }
    return false;
  };

  return ::saveAnyCloud(file_name, map, extract_voxel, with_flags, prog);
}


uint64_t ohmtools_API saveVoxels(const std::string &file_name, const ohm::OccupancyMap &map,
                                 const SaveCloudOptions &opt, const ProgressCallback &prog)
{
  unsigned with_flags = 0;

  // Work out if we need colour.
  auto colour_select = opt.colour_select;
  std::unique_ptr<ColourByHeight> colour_by_height;
  if (!colour_select && opt.allow_default_colour_selection)
  {
    colour_by_height = std::make_unique<ColourByHeight>(map);
    colour_select = [&colour_by_height](const ohm::Voxel<const float> &occupancy) {
      return colour_by_height->select(occupancy);
    };
  }

  if (colour_select)
  {
    with_flags |= WithColour;
  }

  ohm::Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  auto mean = (opt.ignore_voxel_mean) ? ohm::Voxel<const ohm::VoxelMean>() :
                                        ohm::Voxel<const ohm::VoxelMean>(&map, map.layout().meanLayer());

  const auto extract_voxel = [&map, &opt, &occupancy, &mean, colour_select](
                               ExtractedVoxel &voxel, const ohm::OccupancyMap::const_iterator &iter) -> bool {
    ohm::setVoxelKey(iter, occupancy, mean);
    if (isOccupied(occupancy) || opt.export_free && isFree(occupancy))
    {
      voxel.position = (mean.isLayerValid()) ? positionSafe(mean) : map.voxelCentreGlobal(*iter);
      if (colour_select)
      {
        voxel.colour = colour_select(occupancy);
      }
      return true;
    }
    return false;
  };

  return ::saveAnyVoxels(file_name, map, extract_voxel, with_flags, prog);
}


uint64_t saveDensityVoxels(const std::string &file_name, const ohm::OccupancyMap &map,
                           const SaveDensityCloudOptions &opt, const ProgressCallback &prog)
{
  ohm::Voxel<const float> traversal(&map, map.layout().traversalLayer());
  ohm::Voxel<const ohm::VoxelMean> mean(&map, map.layout().meanLayer());
  unsigned with_flags = 0;

  if (!traversal.isLayerValid() || !mean.isLayerValid())
  {
    return 0;
  }

  // Work out if we need colour.
  auto colour_select = opt.colour_select;
  std::unique_ptr<ColourByHeight> colour_by_height;
  if (!colour_select && opt.allow_default_colour_selection)
  {
    colour_by_height = std::make_unique<ColourByHeight>(map);
    colour_select = [&colour_by_height](const ohm::Voxel<const float> &traversal) {
      return colour_by_height->select(traversal);
    };
  }

  if (colour_select)
  {
    with_flags |= WithColour;
  }

  const auto extract_voxel = [&map, &opt, &traversal, &mean, colour_select](
                               ExtractedVoxel &voxel, const ohm::OccupancyMap::const_iterator &iter) -> bool {
    const float density = voxelDensity(traversal, mean);
    if (density >= opt.density_threshold)
    {
      voxel.position = (mean.isLayerValid()) ? positionSafe(mean) : map.voxelCentreGlobal(*iter);
      voxel.position = (!opt.ignore_voxel_mean) ? positionSafe(mean) : map.voxelCentreGlobal(*iter);
      if (colour_select)
      {
        voxel.colour = colour_select(traversal);
      }
      return true;
    }
    return false;
  };

  return ::saveAnyVoxels(file_name, map, extract_voxel, with_flags, prog);
}


uint64_t saveHeightmapCloud(const std::string &file_name, const ohm::OccupancyMap &map,
                            const SaveHeightmapCloudOptions &opt, const ProgressCallback &prog)
{
  std::ofstream out(file_name, std::ios::binary);

  if (!out.is_open())
  {
    return 0;
  }

  auto colour_select = opt.colour_select;
  std::unique_ptr<ColourByHeightmapClearance> colour_by_height;
  if (!colour_select && opt.allow_default_colour_selection)
  {
    colour_by_height = std::make_unique<ColourByHeightmapClearance>(map);
    colour_select = [&colour_by_height](const ohm::Voxel<const float> &occupancy) {
      return colour_by_height->select(occupancy);
    };
  }

  ohm::PlyPointStream ply = setupPlyStream(static_cast<bool>(colour_select));
  ply.open(out);

  glm::dvec3 pos;
  const size_t region_count = map.regionCount();
  size_t processed_region_count = 0;
  glm::i16vec3 last_region = map.begin().key().regionKey();

  ohm::Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  auto mean = (opt.ignore_voxel_mean) ? ohm::Voxel<const ohm::VoxelMean>() :
                                        ohm::Voxel<const ohm::VoxelMean>(&map, map.layout().meanLayer());
  ohm::Voxel<const ohm::HeightmapVoxel> heightmap_voxel(&map,
                                                        map.layout().layerIndex(ohm::HeightmapVoxel::kHeightmapLayer));

  if (!heightmap_voxel.isLayerValid())
  {
    // Invalid format.
    return 0;
  }

  // Resolve the heightmap axis from the mapInfo if relevant.
  int heightmap_axis = 2;
  float height_flip = 1.0f;  // Set to -1 if we need to negate the height value to get a coordinate out of it.
  ohm::UpAxis up_axis = ohm::heightmap::queryHeightmapAxis(map.mapInfo());
  if (int(up_axis) >= 0)
  {
    heightmap_axis = int(up_axis);
  }
  else
  {
    heightmap_axis = 1 - int(up_axis);
    height_flip = -1.0f;
  }

  uint64_t point_count = 0;
  for (auto iter = map.begin(); iter != map.end(); ++iter)
  {
    ohm::setVoxelKey(iter, occupancy, mean, heightmap_voxel);
    if (last_region != iter.key().regionKey())
    {
      ++processed_region_count;
      if (prog)
      {
        prog(processed_region_count, region_count);
      }
      last_region = iter.key().regionKey();
    }

    // Respect collapse option. When collapsing, we ignore voxels which are not in the base layer.
    if (!opt.collapse || (heightmap_voxel.isValid() && heightmap_voxel.data().layer == ohm::kHvlBaseLayer))
    {
      // Note: an occupancy value of 0 will come up as occupied, but in a heightmap represents a vacant voxel which we
      // want to skip unless exporting "free".
      if (isOccupied(occupancy) && occupancy.data() != 0 ||
          opt.export_free && (isFree(occupancy) || occupancy.data() == 0))
      {
        pos = (mean.isLayerValid()) ? positionSafe(mean) : map.voxelCentreGlobal(*iter);
        if (heightmap_voxel.isValid())
        {
          pos[heightmap_axis] =
            map.voxelCentreGlobal(*iter)[heightmap_axis] + double(height_flip * heightmap_voxel.data().height);
        }

        ply.setPointPosition(pos);

        if (colour_select)
        {
          const ohm::Colour c = colour_select(occupancy);
          ply.setProperty(kPropertyRed, c.r());
          ply.setProperty(kPropertyGreen, c.g());
          ply.setProperty(kPropertyBlue, c.b());
        }

        ply.writePoint();
        ++point_count;
      }
    }
  }

  ply.close();
  out.close();

  return point_count;
}


uint64_t saveHeightmapVoxels(const std::string &file_name, const ohm::OccupancyMap &map,
                             const SaveHeightmapCloudOptions &opt, const ProgressCallback &prog)
{
  std::ofstream out(file_name, std::ios::binary);

  if (!out.is_open())
  {
    return 0;
  }

  auto colour_select = opt.colour_select;
  std::unique_ptr<ColourByHeightmapClearance> colour_by_height;
  if (!colour_select && opt.allow_default_colour_selection)
  {
    colour_by_height = std::make_unique<ColourByHeightmapClearance>(map);
    colour_select = [&colour_by_height](const ohm::Voxel<const float> &occupancy) {
      return colour_by_height->select(occupancy);
    };
  }

  // Ply voxel mesh.
  ohm::PlyMesh ply;

  glm::dvec3 pos;
  const size_t region_count = map.regionCount();
  const double resolution = map.resolution();
  size_t processed_region_count = 0;
  glm::i16vec3 last_region = map.begin().key().regionKey();

  ohm::Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  auto mean = (opt.ignore_voxel_mean) ? ohm::Voxel<const ohm::VoxelMean>() :
                                        ohm::Voxel<const ohm::VoxelMean>(&map, map.layout().meanLayer());
  ohm::Voxel<const ohm::HeightmapVoxel> heightmap_voxel(&map,
                                                        map.layout().layerIndex(ohm::HeightmapVoxel::kHeightmapLayer));

  if (!heightmap_voxel.isLayerValid())
  {
    // Invalid format.
    return 0;
  }

  // Resolve the heightmap axis from the mapInfo if relevant.
  int heightmap_axis = 2;
  float height_flip = 1.0f;  // Set to -1 if we need to negate the height value to get a coordinate out of it.
  ohm::UpAxis up_axis = ohm::heightmap::queryHeightmapAxis(map.mapInfo());
  if (int(up_axis) >= 0)
  {
    heightmap_axis = int(up_axis);
  }
  else
  {
    heightmap_axis = 1 - int(up_axis);
    height_flip = -1.0f;
  }

  uint64_t voxel_count = 0;
  for (auto iter = map.begin(); iter != map.end(); ++iter)
  {
    ohm::setVoxelKey(iter, occupancy, mean, heightmap_voxel);
    if (last_region != iter.key().regionKey())
    {
      ++processed_region_count;
      if (prog)
      {
        prog(processed_region_count, region_count);
      }
      last_region = iter.key().regionKey();
    }

    // Respect collapse option. When collapsing, we ignore voxels which are not in the base layer.
    if (!opt.collapse || (heightmap_voxel.isValid() && heightmap_voxel.data().layer == ohm::kHvlBaseLayer))
    {
      // Note: an occupancy value of 0 will come up as occupied, but in a heightmap represents a vacant voxel which we
      // want to skip unless exporting "free".
      if (isOccupied(occupancy) && occupancy.data() != 0 ||
          opt.export_free && (isFree(occupancy) || occupancy.data() == 0))
      {
        pos = (mean.isLayerValid()) ? positionSafe(mean) : map.voxelCentreGlobal(*iter);
        if (heightmap_voxel.isValid())
        {
          pos[heightmap_axis] =
            map.voxelCentreGlobal(*iter)[heightmap_axis] + double(height_flip * heightmap_voxel.data().height);
        }

        const ohm::Colour c = (colour_select) ? colour_select(occupancy) : ohm::Colour(255, 255, 255);
        addVoxel(ply, pos, resolution, c);
        ++voxel_count;
      }
    }
  }

  if (!ply.save(out, true))
  {
    // Failed.
    voxel_count = 0;
  }

  out.close();

  return voxel_count;
}


void saveQueryCloud(const std::string &file_name, const ohm::OccupancyMap &map, const ohm::Query &query,
                    float colour_range, const ProgressCallback &prog)
{
  const size_t result_count = query.numberOfResults();
  const ohm::Key *keys = query.intersectedVoxels();
  const double *ranges = query.ranges();
  glm::dvec3 voxel_pos;

  ohm::PlyMesh ply;
  for (size_t i = 0; i < result_count; ++i)
  {
    const ohm::Key &key = keys[i];
    uint8_t c = std::numeric_limits<uint8_t>::max();
    if (colour_range > 0 && ranges)
    {
      double range_value = ranges[i];
      if (range_value < 0)
      {
        range_value = colour_range;
      }
      c = uint8_t(std::numeric_limits<uint8_t>::max() * std::max(0.0, (colour_range - range_value) / colour_range));
    }
    voxel_pos = map.voxelCentreGlobal(key);
    ply.addVertex(voxel_pos, ohm::Colour(c, std::numeric_limits<uint8_t>::max() / 2, 0));
    if (prog)
    {
      prog(i + 1, result_count);
    }
  }

  ply.save(file_name, true);
}


size_t saveClearanceCloud(const std::string &file_name, const ohm::OccupancyMap &map, const glm::dvec3 &min_extents,
                          const glm::dvec3 &max_extents, float colour_range, int export_type,
                          const ProgressCallback &prog)
{
  const size_t region_count = map.regionCount();
  size_t processed_region_count = 0;
  glm::dvec3 v;
  glm::i16vec3 last_region = map.begin().key().regionKey();
  ohm::PlyMesh ply;
  size_t point_count = 0;

  glm::i16vec3 min_region = map.regionKey(min_extents);
  glm::i16vec3 max_region = map.regionKey(max_extents);

  ohm::Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  ohm::Voxel<const float> clearance(&map, map.layout().clearanceLayer());

  if (!clearance.isLayerValid())
  {
    // No clearance layer.
    return 0;
  }

  const float colour_scale = colour_range;
  const auto map_end_iter = map.end();
  for (auto iter = map.begin(); iter != map_end_iter; ++iter)
  {
    clearance.setKey(occupancy.setKey(*iter));
    if (last_region != iter.key().regionKey())
    {
      ++processed_region_count;
      if (prog)
      {
        prog(processed_region_count, region_count);
      }
      last_region = iter.key().regionKey();
    }

    // Ensure the voxel is in a region we have calculated data for.
    if (min_region.x <= last_region.x && last_region.x <= max_region.x &&  //
        min_region.y <= last_region.y && last_region.y <= max_region.y &&  //
        min_region.z <= last_region.z && last_region.z <= max_region.z)
    {
      const bool export_match = !occupancy.isNull() && occupancyType(occupancy) >= export_type;
      if (export_match)
      {
        float range_value;
        assert(clearance.isValid());  // More for clang-tidy. We've already checked the layer validity.
        clearance.read(&range_value);
        if (range_value < 0)
        {
          range_value = colour_range;
        }
        if (range_value >= 0)
        {
          uint8_t c =
            uint8_t(std::numeric_limits<uint8_t>::max() * std::max(0.0f, (colour_scale - range_value) / colour_scale));
          v = map.voxelCentreLocal(*iter);
          ply.addVertex(v, ohm::Colour(c, std::numeric_limits<uint8_t>::max() / 2, 0));
          ++point_count;
        }
      }
    }
  }

  ply.save(file_name, true);

  return point_count;
}


size_t saveTsdfCloud(const std::string &file_name, const ohm::OccupancyMap &map, float surface_distance,
                     const ColourSelectTsdf &colour_select, const ProgressCallback &prog)
{
  // Work out if we need colour.
  unsigned with_flags = 0;
  std::unique_ptr<ColourByHeight> colour_by_height;

  ohm::Voxel<const ohm::VoxelTsdf> tsdf_voxel(&map, map.layout().layerIndex(ohm::default_layer::tsdfLayerName()));

  if (colour_select)
  {
    with_flags |= WithColour;
  }

  if (!tsdf_voxel.isLayerValid())
  {
    return 0;
  }

  const auto extract_voxel = [&map, surface_distance, &tsdf_voxel, colour_select](
                               ExtractedVoxel &voxel, const ohm::OccupancyMap::const_iterator &iter) -> bool {
    ohm::setVoxelKey(iter, tsdf_voxel);
    const auto tsdf = tsdf_voxel.data();
    const bool export_match = tsdf.weight > 0 && std::abs(tsdf.distance) < surface_distance;
    if (export_match)
    {
      voxel.position = map.voxelCentreLocal(*iter);
      if (colour_select)
      {
        voxel.colour = colour_select(tsdf_voxel);
      }
      return true;
    }
    return false;
  };

  return ::saveAnyCloud(file_name, map, extract_voxel, with_flags, prog);
}


size_t saveTsdfVoxels(const std::string &file_name, const ohm::OccupancyMap &map, float surface_distance,
                      const ColourSelectTsdf &colour_select, const ProgressCallback &prog)
{
  // Work out if we need colour.
  unsigned with_flags = 0;
  std::unique_ptr<ColourByHeight> colour_by_height;
  ohm::Voxel<const ohm::VoxelTsdf> tsdf_voxel(&map, map.layout().layerIndex(ohm::default_layer::tsdfLayerName()));

  if (colour_select)
  {
    with_flags |= WithColour;
  }

  if (!tsdf_voxel.isLayerValid())
  {
    return 0;
  }

  const auto extract_voxel = [&map, surface_distance, &tsdf_voxel, colour_select](
                               ExtractedVoxel &voxel, const ohm::OccupancyMap::const_iterator &iter) -> bool {
    ohm::setVoxelKey(iter, tsdf_voxel);
    const auto tsdf = tsdf_voxel.data();
    const bool export_match = tsdf.weight > 0 && std::abs(tsdf.distance) < surface_distance;
    if (export_match)
    {
      voxel.position = map.voxelCentreLocal(*iter);
      if (colour_select)
      {
        voxel.colour = colour_select(tsdf_voxel);
      }
      return true;
    }
    return false;
  };

  return ::saveAnyVoxels(file_name, map, extract_voxel, with_flags, prog);
}
}  // namespace ohmtools


================================================
FILE: ohmtools/OhmCloud.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMTOOLS_OHMCLOUD_H
#define OHMTOOLS_OHMCLOUD_H

#include "OhmToolsConfig.h"

#include <ohm/CovarianceVoxel.h>
#include <ohm/KeyRange.h>
#include <ohm/VoxelTsdf.h>

#include <ohmutil/Colour.h>

#include <glm/glm.hpp>

#include <array>
#include <functional>
#include <string>

namespace ohm
{
class Key;
class OccupancyMap;
class Query;
template <typename T>
class Voxel;
}  // namespace ohm

namespace ohmtools
{
/// Callback used by @c saveCloud() etc to report on progress.
/// The arguments passed are the current progress and the target progress respectively.
using ProgressCallback = std::function<void(size_t, size_t)>;

/// Colour selection filter function. May be used to override colour assignment.
using ColourSelect = std::function<ohm::Colour(const ohm::Voxel<const float> &)>;

/// Colour selection filter function for TSDF. May be used to override colour assignment.
using ColourSelectTsdf = std::function<ohm::Colour(const ohm::Voxel<const ohm::VoxelTsdf> &)>;

/// Options used to adjust how a cloud is saved from an occupancy map.
struct ohmtools_API SaveCloudOptions
{
  /// Overrides the default colour selection.
  ColourSelect colour_select{};
  /// When true and @c colour_select is empty, uses the default colour selection strategy which best suites the
  /// cloud type. For example, a general cloud may @c ColourByHeight while a heightmap is better suited to
  /// @c ColourByHeightmapClearance .
  bool allow_default_colour_selection = true;
  /// Export free space voxels? Required to get virtual surfaces from heightmaps.
  bool export_free = false;
  /// Ignore voxel mean forcing voxel centres for positions?
  bool ignore_voxel_mean = false;
};

/// Options for saving a density cloud.
struct SaveDensityCloudOptions : SaveCloudOptions
{
  /// Density rate threshold to pass.
  float density_threshold = 0;
};

/// Specialised options for saving heightmap clouds. Supports construction from a @c SaveCloudOptions setting default
/// values for heightmap extended parameters.
struct ohmtools_API SaveHeightmapCloudOptions : SaveCloudOptions
{
  bool collapse = false;  ///< Collapse to a 2.5D heightmap, reducing layered heightmaps to 2D?

  /// Instantiate default options.
  inline SaveHeightmapCloudOptions() = default;
  /// Copy constructor
  /// @param other Object to copy.
  inline SaveHeightmapCloudOptions(const SaveHeightmapCloudOptions &other) = default;
  /// Copy constructor copying the base @c SaveCloudOptions.
  /// @param other Object to copy.
  inline SaveHeightmapCloudOptions(const SaveCloudOptions &other)  // NOLINT(google-explicit-constructor)
    : SaveCloudOptions(other)
  {}

  /// Assignment operator.
  /// @param other Object to copy.
  inline SaveHeightmapCloudOptions &operator=(const SaveHeightmapCloudOptions &other) = default;
  /// Assignment operator copying the base @c SaveCloudOptions.
  /// @param other Object to copy.
  inline SaveHeightmapCloudOptions &operator=(const SaveCloudOptions &other)
  {
    SaveCloudOptions::operator=(other);
    return *this;
  }
};

//------------------------------------------------------------------------------
// Colour selection helpers
//------------------------------------------------------------------------------
/// A helper for @c saveCloud() to colour a cloud by height.
class ohmtools_API ColourByHeight
{
public:
  /// Default colour for the mimimum height value
  static const ohm::Colour kDefaultFrom;
  /// Default colour for the maximum height value
  static const ohm::Colour kDefaultTo;

  /// Colours to interpolate from `[0]` and to `[1]` across the height range.
  std::array<ohm::Colour, 2> colours;

  /// Create a height based colouriser for the given @p map.
  /// @param map The map to colour by - extracts extents.
  explicit ColourByHeight(const ohm::OccupancyMap &map);
  /// Create a height based colouriser for the given @p map using custom colours.
  /// @param map The map to colour by - extracts extents.
  /// @param from The lowest height colour.
  /// @param to The highest height colour.
  ColourByHeight(const ohm::OccupancyMap &map, const ohm::Colour &from, const ohm::Colour &to);
  /// Create a height based colouriser across the given @p extents.
  /// @param extents Key range overwhich voxels are to be coloured.
  explicit ColourByHeight(const ohm::KeyRange &extents);
  /// Create a height based colouriser across the given @p extents using custom colours.
  /// @param extents Key range overwhich voxels are to be coloured.
  /// @param from The lowest height colour.
  /// @param to The highest height colour.
  ColourByHeight(const ohm::KeyRange &extents, const ohm::Colour &from, const ohm::Colour &to);

  /// Select a colour for the given voxel @p key.
  /// @param key The voxel to colour.
  /// @return The colour for the voxel at @p key.
  ohm::Colour select(const ohm::Key &key) const;
  /// Select a colour for the given voxel @p occupancy.
  /// @param occupancy The voxel to colour.
  /// @return The colour for the voxel at @p occupancy.
  ohm::Colour select(const ohm::Voxel<const float> &occupancy) const;

private:
  ohm::KeyRange range_;
  int up_axis_ = 2;
};

/// Colour heightmap voxels by @c ohm::OccupancyType colouring occupied and free voxels.
class ohmtools_API ColourByType
{
public:
  /// Colour for occupied voxels
  ohm::Colour occupied_colour{ 32, 255, 32 };
  /// Colour for free occupied voxels (if enabled)
  ohm::Colour free_colour{ 255, 128, 32 };

  /// Create a voxel type colouriser for @p map.
  /// @param map The map to be colourised.
  explicit ColourByType(const ohm::OccupancyMap &map);

  /// Select a colour for the given voxel @p occupancy.
  /// @param occupancy The voxel to colour.
  /// @return The colour for the voxel at @p occupancy.
  ohm::Colour select(const ohm::Voxel<const float> &occupancy) const;

private:
  float occupancy_threshold_ = 0;
};

/// A helper for @c saveCloud() to colour a cloud by occupancy probability.
///
/// Colours from @c colours[0] to @c colours[1] across the occupancy probability range [0, 1].
class ohmtools_API ColourByOccupancy
{
public:
  /// Colours to interpolate from `[0]` and to `[1]` across the intensity range.
  std::array<ohm::Colour, 2> colours;
  /// Set the interpolation range to the default occupied probability only [0.5, 1.0]?
  bool ramp_occupied_range = true;

  /// Create a occupancy probability based colouriser.
  /// @param map The occupancy map to colour voxels for.
  /// @param ramp_occupied_range Set the interpolation range to the default occupied probability only [0.5, 1.0]?
  ColourByOccupancy(const ohm::OccupancyMap &map, bool ramp_occupied_range = true);
  /// Create a occupancy probability based colouriser using custom colours.
  /// @param map The occupancy map to colour voxels for.
  /// @param from The lowest occupancy probablility colour.
  /// @param to The highest occupancy probablility colour.
  /// @param ramp_occupied_range Set the interpolation range to the default occupied probability only [0.5, 1.0]?
  ColourByOccupancy(const ohm::OccupancyMap &map, const ohm::Colour &from, const ohm::Colour &to,
                    bool ramp_occupied_range = true);

  /// Select a colour for the given voxel @p occupancy.
  /// @param occupancy The voxel to colour.
  /// @return The colour for the voxel at @p occupancy.
  ohm::Colour select(const ohm::Voxel<const float> &occupancy);

  /// Select a colour for the given @p occupancy value.
  /// @param occupancy Occupancy probability to colour for [0, 1].
  /// @return The colour for the voxel.
  ohm::Colour select(const float occupancy) const;

private:
  float occupancy_threshold_probability_ = 0;
};

/// A helper for @c saveCloud() to colour a cloud by intensity.
///
/// Colours from @c colours[0] to @c colours[1] across the intensity range [0, max_intensity].
///
/// Note the @c select() function accepts an occupancy voxel and resolves the corresponding @c IntensityMeanCov value.
class ohmtools_API ColourByIntensity
{
public:
  /// Default colour for the mimimum intensity value
  static const ohm::Colour kDefaultFrom;
  /// Default colour for the maximum intensity value
  static const ohm::Colour kDefaultTo;

  /// Colours to interpolate from `[0]` and to `[1]` across the intensity range.
  std::array<ohm::Colour, 2> colours;
  /// Maximum expected intensity value. Apply @c colour[1] for points at or above this intensity value.
  float max_intensity = 1.0f;

  /// Create a intensity based colouriser.
  /// @param map The map to be colourised.
  /// @param max_intensity The maximum expected intensity value.
  explicit ColourByIntensity(const ohm::OccupancyMap &map, float max_intensity = 100.0f);
  /// Create a intensity based colouriser using custom colours.
  /// @param map The map to be colourised.
  /// @param from The lowest intensity colour.
  /// @param to The highest intensity colour.
  /// @param max_intensity The maximum expected intensity value.
  ColourByIntensity(const ohm::OccupancyMap &map, const ohm::Colour &from, const ohm::Colour &to,
                    float max_intensity = 100.0f);

  /// Check if colouring by intensity is valid for the configured map object.
  /// @return True if the map used on construction has an appropiate intensity layer.
  inline bool isValid() const { return intensity_.isLayerValid(); }

  /// Select a colour for the given voxel @p occupancy.
  /// @param occupancy The voxel to colour.
  /// @return The colour for the voxel at @p occupancy.
  ohm::Colour select(const ohm::Voxel<const float> &occupancy);

  /// Select a colour for the given @p intensity value.
  /// @param intensity Intensity value to colour for [0, max_intensity].
  /// @return The colour for the voxel @p intensity.
  ohm::Colour select(const float intensity) const;

private:
  ohm::Voxel<const ohm::IntensityMeanCov> intensity_;
};

/// Colour heightmap voxels such that voxels in matching heightmap layers have the same colour.
class ohmtools_API ColourHeightmapLayer
{
public:
  /// Create a layer colouriser for @p map.
  /// @param map The map to be colourised.
  explicit ColourHeightmapLayer(const ohm::OccupancyMap &map);

  /// Select a colour for the given voxel @p occupancy.
  /// @param occupancy The voxel to colour.
  /// @return The colour for the voxel at @p occupancy.
  ohm::Colour select(const ohm::Voxel<const float> &occupancy) const;

private:
  int heightmap_up_axis_ = 2;
};

/// Colour heightmap voxels by @c HeightmapVoxelType::kSurface or @c HeightmapVoxelType::kVirtualSurface type.
class ohmtools_API ColourHeightmapType
{
public:
  /// Colour to use for surface voxels.
  ohm::Colour surface_colour{ 32, 255, 32 };
  /// Colour to use for virtual surface voxels.
  ohm::Colour virtual_colour{ 255, 128, 32 };

  /// Create a heightmap voxel type colouriser.
  /// @param map The map to colour by - extracts extents.
  explicit ColourHeightmapType(const ohm::OccupancyMap &map);

  /// Select a colour for the given voxel @p occupancy.
  /// @param occupancy The voxel to colour.
  /// @return The colour for the voxel at @p occupancy.
  ohm::Colour select(const ohm::Voxel<const float> &occupancy) const;

private:
  int heightmap_layer_ = -1;
};

/// A helper for @c saveCloud() to colour a cloud by height.
class ohmtools_API ColourByHeightmapClearance
{
public:
  /// Colour used when there is zero clearance.
  static const ohm::Colour kDefaultLow;
  /// Colour used when the maximum clearance height is available.
  static const ohm::Colour kDefaultHigh;

  /// Colours to interpolate from `[0]` and to `[1]` across the clearance range.
  std::array<ohm::Colour, 2> colours;

  /// Create a clearance based colouriser for the given @p map.
  /// @param map The map to colour by - extracts extents.
  /// @param clearance_scale The maximum clearance height requested.
  explicit ColourByHeightmapClearance(const ohm::OccupancyMap &map, double clearance_scale = 2.0);
  /// Create a clearance based colouriser for the given @p map with custom colours.
  /// @param map The map to colour by - extracts extents.
  /// @param low The colour to use at zero clearance.
  /// @param high The colour to use at @p clearance_scale available clearance.
  /// @param clearance_scale The maximum clearance height requested.
  ColourByHeightmapClearance(const ohm::OccupancyMap &map, const ohm::Colour &low, const ohm::Colour &high,
                             double clearance_scale = 2.0);

  /// Select a colour for the given voxel @p occupancy.
  /// @param occupancy The voxel to colour.
  /// @return The colour for the voxel at @p occupancy.
  ohm::Colour select(const ohm::Voxel<const float> &occupancy) const;

private:
  double min_clearance_ = 0;
  double max_clearance_ = 0;
  int heightmap_layer_ = -1;
};

//------------------------------------------------------------------------------
// Cloud ply saving functions.
//------------------------------------------------------------------------------

/// Save @p map to a ply file, exporting only Occupied voxels.
/// @param file_name File to save to. Please add the .ply extension.
/// @param map The map to save.
/// @param opt Additional export controls.
/// @param prog Optional function called to report on progress.
/// @return The number of points saved.
uint64_t ohmtools_API saveCloud(const std::string &file_name, const ohm::OccupancyMap &map,
                                const SaveCloudOptions &opt = SaveCloudOptions(),
                                const ProgressCallback &prog = ProgressCallback());

/// Save @p map to a ply file using the density rate model.
/// @param file_name File to save to. Please add the .ply extension.
/// @param map The map to save.
/// @param opt Additional export controls.
/// @param prog Optional function called to report on progress.
/// @return The number of points saved.
uint64_t ohmtools_API saveDensityCloud(const std::string &file_name, const ohm::OccupancyMap &map,
                                       const SaveDensityCloudOptions &opt = SaveDensityCloudOptions(),
                                       const ProgressCallback &prog = ProgressCallback());

/// Similar to @c saveCloud() exporting voxels as a series of cube meshes.
/// @param file_name File to save to. Please add the .ply extension.
/// @param map The map to save.
/// @param opt Additional export controls.
/// @param prog Optional function called to report on progress.
/// @return The number of voxels saved.
uint64_t ohmtools_API saveVoxels(const std::string &file_name, const ohm::OccupancyMap &map,
                                 const SaveCloudOptions &opt = SaveCloudOptions(),
                                 const ProgressCallback &prog = ProgressCallback());

/// Save @p map assuming it is a heightmap (contains @c HeightmapVoxel data) to a ply cloud.
/// @param file_name File to save to. Please add the .ply extension.
/// @param map The map to save.
/// @param opt Additional export controls.
/// @param prog Optional function called to report on progress.
/// @return The number of points saved.
uint64_t ohmtools_API saveHeightmapCloud(const std::string &file_name, const ohm::OccupancyMap &heightmap,
                                         const SaveHeightmapCloudOptions &opt = SaveHeightmapCloudOptions(),
                                         const ProgressCallback &prog = ProgressCallback());

/// Similar to @c saveHeightmapCloud() exporting voxels as a series of cube meshes.
/// @param file_name File to save to. Please add the .ply extension.
/// @param map The map to save.
/// @param opt Additional export controls.
/// @param prog Optional function called to report on progress.
/// @return The number of voxels saved.
uint64_t ohmtools_API saveHeightmapVoxels(const std::string &file_name, const ohm::OccupancyMap &heightmap,
                                          const SaveHeightmapCloudOptions &opt = SaveHeightmapCloudOptions(),
                                          const ProgressCallback &prog = ProgressCallback());

/// Save the results of @p query on @p map to a ply file, exporting all intersected voxels.
/// Voxels are coloured by the reported query range values where voxels at or beyond @p colourRange
/// are green, tending to orange closer to zero.
/// @param file_name File to save to. Please add the .ply extension.
/// @param map The map to save voxels from.
/// @param query The query to save results from.
/// @param colour_range Affects voxel colouring as described above. Green at this range.
/// @param prog Optional function called to report on progress.
void ohmtools_API saveQueryCloud(const std::string &file_name, const ohm::OccupancyMap &map, const ohm::Query &query,
                                 float colour_range = 0.0f, const ProgressCallback &prog = ProgressCallback());

/// Save a point cloud representing the @c Voxel::clearance() values for voxels in @p map.
///
/// Voxels are coloured by the reported query range values where voxels at or beyond @p colourRange
/// are green, tending to orange closer to zero.
///
/// Only saves data from regions overlapping the @p minExtents and @p maxExtents, though all voxels from
/// such regions are exported, not just those within the extents.
///
/// @param file_name File to save to. Please add the .ply extension.
/// @param map The map to save voxels from.
/// @param min_extents Min extents to save overlapping regions from.
/// @param max_extents Max extents to save overlapping regions from.
/// @param colour_range Affects voxel colouring as described above. Green at this range.
/// @param export_type Type of voxels to export. Voxels of this @c OccupancyType or greater are exported.
/// @param prog Optional function called to report on progress.
size_t ohmtools_API saveClearanceCloud(const std::string &file_name, const ohm::OccupancyMap &map,
                                       const glm::dvec3 &min_extents, const glm::dvec3 &max_extents,
                                       float colour_range = 0.0f, int export_type = 0,
                                       const ProgressCallback &prog = ProgressCallback());

/// Save a point cloud from TSDF layer data.
///
/// @param file_name File to save to. Please add the .ply extension.
/// @param map The map to save voxels from.
/// @param surface_distance Surface distance threshold to export with.
/// @param prog Optional function called to report on progress.
size_t ohmtools_API saveTsdfCloud(const std::string &file_name, const ohm::OccupancyMap &map, float surface_distance,
                                  const ColourSelectTsdf &colour_select = {},
                                  const ProgressCallback &prog = ProgressCallback());

/// Save a point cloud from TSDF layer data.
///
/// @param file_name File to save to. Please add the .ply extension.
/// @param map The map to save voxels from.
/// @param surface_distance Surface distance threshold to export with.
/// @param prog Optional function called to report on progress.
size_t ohmtools_API saveTsdfVoxels(const std::string &file_name, const ohm::OccupancyMap &map, float surface_distance,
                                   const ColourSelectTsdf &colour_select = {},
                                   const ProgressCallback &prog = ProgressCallback());
}  // namespace ohmtools

#endif  // OHMTOOLS_OHMCLOUD_H


================================================
FILE: ohmtools/OhmGen.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmGen.h"

#include <ohm/Key.h>
#include <ohm/KeyList.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/VoxelData.h>

namespace ohmgen
{
void fillWithValue(ohm::OccupancyMap &map, const ohm::Key &min_key, const ohm::Key &max_key, float fill_value,
                   const float *expect_value, int step)
{
  ohm::Voxel<float> voxel(&map, map.layout().occupancyLayer());
  float initial_value;

  if (!voxel.isLayerValid())
  {
    return;
  }

  ohm::Key key = min_key;
  for (; key.isBoundedZ(min_key, max_key); map.moveKeyAlongAxis(key, 2, step))
  {
    key.setRegionAxis(1, min_key.regionKey()[1]);
    key.setLocalAxis(1, min_key.localKey()[1]);

    for (; key.isBoundedY(min_key, max_key); map.moveKeyAlongAxis(key, 1, step))
    {
      key.setRegionAxis(0, min_key.regionKey()[0]);
      key.setLocalAxis(0, min_key.localKey()[0]);

      for (; key.isBoundedX(min_key, max_key); map.moveKeyAlongAxis(key, 0, step))
      {
        voxel.setKey(key);
        initial_value = ohm::unobservedOccupancyValue();
        if (expect_value && initial_value != *expect_value)
        {
          throw std::logic_error("Voxel should start uncertain.");
        }
        assert(voxel.isValid());
        voxel.write(fill_value);
      }
    }
  }
}


void fillMapWithEmptySpace(ohm::OccupancyMap &map, int x1, int y1, int z1, int x2, int y2, int z2,
                           bool expect_empty_map)
{
  const float expect_initial_value = ohm::unobservedOccupancyValue();
  const float *expect_value_ptr = (expect_empty_map) ? &expect_initial_value : nullptr;

  ohm::Key min_key;
  ohm::Key max_key;
  min_key = max_key = ohm::Key(0, 0, 0, 0, 0, 0);

  map.moveKey(min_key, x1, y1, z1);
  map.moveKey(max_key, x2 - 1, y2 - 1, z2 - 1);

  fillWithValue(map, min_key, max_key, map.missValue(), expect_value_ptr, 1);
}


void buildWall(ohm::OccupancyMap &map, int a0, int a1, int a2, int a0min, int a1min, int a0max, int a1max, int a2val)
{
  ohm::Voxel<float> voxel(&map, map.layout().occupancyLayer());
  ohm::Key key;

  if (!voxel.isLayerValid())
  {
    return;
  }

  for (int val0 = a0min; val0 < a0max; ++val0)
  {
    for (int val1 = a1min; val1 < a1max; ++val1)
    {
      key = ohm::Key(0, 0, 0, 0, 0, 0);
      map.moveKeyAlongAxis(key, a0, val0);
      map.moveKeyAlongAxis(key, a1, val1);
      map.moveKeyAlongAxis(key, a2, a2val);
      voxel.setKey(key);
      assert(voxel.isValid());
      voxel.write(map.occupancyThresholdValue());
    }
  }
}

void boxRoom(ohm::OccupancyMap &map, const glm::dvec3 &min_ext, const glm::dvec3 &max_ext, int voxel_step)
{
  const ohm::Key min_key = map.voxelKey(min_ext);
  const ohm::Key max_key = map.voxelKey(max_ext);
  ohm::Key wall_key;

  fillWithValue(map, min_key, max_key, map.missValue(), nullptr, 1);

  // Left wall.
  wall_key = max_key;
  wall_key.setLocalAxis(0, min_key.localKey()[0]);
  wall_key.setRegionAxis(0, min_key.regionKey()[0]);
  fillWithValue(map, min_key, wall_key, map.occupancyThresholdValue(), nullptr, voxel_step);
  // Front wall
  wall_key = max_key;
  wall_key.setLocalAxis(1, min_key.localKey()[1]);
  wall_key.setRegionAxis(1, min_key.regionKey()[1]);
  fillWithValue(map, min_key, wall_key, map.occupancyThresholdValue(), nullptr, voxel_step);
  // Bottom wall
  wall_key = max_key;
  wall_key.setLocalAxis(2, min_key.localKey()[2]);
  wall_key.setRegionAxis(2, min_key.regionKey()[2]);
  fillWithValue(map, min_key, wall_key, map.occupancyThresholdValue(), nullptr, voxel_step);


  // Left wall.
  wall_key = min_key;
  wall_key.setLocalAxis(0, max_key.localKey()[0]);
  wall_key.setRegionAxis(0, max_key.regionKey()[0]);
  fillWithValue(map, wall_key, max_key, map.occupancyThresholdValue(), nullptr, voxel_step);
  // Front wall
  wall_key = min_key;
  wall_key.setLocalAxis(1, max_key.localKey()[1]);
  wall_key.setRegionAxis(1, max_key.regionKey()[1]);
  fillWithValue(map, wall_key, max_key, map.occupancyThresholdValue(), nullptr, voxel_step);
  // Bottom wall
  wall_key = min_key;
  wall_key.setLocalAxis(2, max_key.localKey()[2]);
  wall_key.setRegionAxis(2, max_key.regionKey()[2]);
  fillWithValue(map, wall_key, max_key, map.occupancyThresholdValue(), nullptr, voxel_step);
}


void slope(ohm::OccupancyMap &map, double angle_deg, const glm::dvec3 &min_ext, const glm::dvec3 &max_ext,
           int voxel_step)
{
  ohm::Key o_key = map.voxelKey(min_ext);
  int range_x = int(std::ceil((max_ext.x - min_ext.x) / map.resolution()));
  int range_y = int(std::ceil((max_ext.y - min_ext.y) / map.resolution()));

  const double tan_theta = std::tan(angle_deg * M_PI / 180.0);
  glm::dvec3 coord;
  ohm::Voxel<float> voxel(&map, map.layout().occupancyLayer());
  if (!voxel.isLayerValid())
  {
    return;
  }

  for (int y = 0; y < range_y; y += voxel_step)
  {
    for (int x = 0; x < range_x; x += voxel_step)
    {
      ohm::Key key = o_key;
      map.moveKey(key, x, y, 0);
      coord = map.voxelCentreGlobal(key);
      coord.z = min_ext.z + double(coord.y) * tan_theta;
      voxel.setKey(map.voxelKey(coord));
      assert(voxel.isValid());
      voxel.write(map.occupancyThresholdValue());
    }
  }
}
}  // namespace ohmgen


================================================
FILE: ohmtools/OhmGen.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMTOOLS_OHMGEN_H
#define OHMTOOLS_OHMGEN_H

#include "OhmToolsConfig.h"

#include <glm/fwd.hpp>

namespace ohm
{
class OccupancyMap;
}  // namespace ohm

namespace ohmgen
{
/// Fill @p map with empty space over the specified rectangular region.
///
/// Each voxel has one miss integrated.
///
/// @param map The map to fill.
/// @param x1 The lower extents X coordinate in @em voxels, included.
/// @param y1 The lower extents Y coordinate in @em voxels, included.
/// @param z1 The lower extents Z coordinate in @em voxels, included.
/// @param x2 The upper extents X coordinate in @em voxels, excluded.
/// @param y2 The upper extents Y coordinate in @em voxels, excluded.
/// @param z2 The upper extents Z coordinate in @em voxels, excluded.
/// @param expect_empty_map Do we expect the map to begin empty?
void ohmtools_API fillMapWithEmptySpace(ohm::OccupancyMap &map, int x1, int y1, int z1, int x2, int y2, int z2,
                                        bool expect_empty_map = true);

/// Fill @p map as if we had a sensor in the middle of a box.
///
/// For each voxel on the walls, we simulate integrating a ray sample from the origin to the wall voxel.
/// We integrate a hit in the wall sample and misses in the voxels connecting the wall voxel to the origin.
///
/// @param map The map to fill.
/// @param min_ext The minimum extents for the box. Defines the lower wall corner.
/// @param max_ext The maximum extents for the box. Defines the lower wall corner.
/// @param voxel_step Specifies the voxel step to make along the walls. This allows wholes
///   to be created in the wall sampling.
void ohmtools_API boxRoom(ohm::OccupancyMap &map, const glm::dvec3 &min_ext, const glm::dvec3 &max_ext,
                          int voxel_step = 1);

/// Generate a map containing a slope within the given extents. The slope is built in X/Y with Z adjusted to the
/// desired slope. The Z value to @p max_ext is ignored, when the Z value of @p min_ext seeds the initial height.
/// @param map The map to generate a slope in.
/// @param angle_deg The angle of the slope in degrees.
/// @param min_ext Defines the minimum of the slope extents.
/// @param max_ext Defines the maximum of the slope extents.
/// @param voxel_step Specifies the voxel step to make along the slope plane.
void ohmtools_API slope(ohm::OccupancyMap &map, double angle_deg, const glm::dvec3 &min_ext, const glm::dvec3 &max_ext,
                        int voxel_step = 1);
}  // namespace ohmgen

#endif  // OHMTOOLS_OHMGEN_H


================================================
FILE: ohmtools/OhmToolsConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMTOOLSCONFIG_H
#define OHMTOOLSCONFIG_H

#include "OhmToolsExport.h"

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX
#include <cmath>

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

// Enable various validation tests throughout this library.
//#cmakedefine OHM_FEATURE_THREADS

#include "OhmConfig.h"

#endif  // OHMTOOLSCONFIG_H


================================================
FILE: ohmutil/3rdparty/cxxopts/cxxopts.hpp
================================================
/*

Copyright (c) 2014, 2015, 2016, 2017 Jarryd Beck

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.

*/

#ifndef CXXOPTS_HPP_INCLUDED
#define CXXOPTS_HPP_INCLUDED

// clang-format off

#include <cctype>
#include <cstring>
#include <exception>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <memory>
#include <regex>
#include <sstream>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#ifdef __cpp_lib_optional
#include <optional>
#define CXXOPTS_HAS_OPTIONAL
#endif

#if __cplusplus >= 201603L
#define CXXOPTS_NODISCARD [[nodiscard]]
#else
#define CXXOPTS_NODISCARD
#endif

#ifndef CXXOPTS_VECTOR_DELIMITER
#define CXXOPTS_VECTOR_DELIMITER ','
#endif

#define CXXOPTS__VERSION_MAJOR 3
#define CXXOPTS__VERSION_MINOR 0
#define CXXOPTS__VERSION_PATCH 0

namespace cxxopts
{
  static constexpr struct {
    uint8_t major, minor, patch;
  } version = {
    CXXOPTS__VERSION_MAJOR,
    CXXOPTS__VERSION_MINOR,
    CXXOPTS__VERSION_PATCH
  };
} // namespace cxxopts

//when we ask cxxopts to use Unicode, help strings are processed using ICU,
//which results in the correct lengths being computed for strings when they
//are formatted for the help output
//it is necessary to make sure that <unicode/unistr.h> can be found by the
//compiler, and that icu-uc is linked in to the binary.

#ifdef CXXOPTS_USE_UNICODE
#include <unicode/unistr.h>

namespace cxxopts
{
  using String = icu::UnicodeString;

  inline
  String
  toLocalString(std::string s)
  {
    return icu::UnicodeString::fromUTF8(std::move(s));
  }

  class UnicodeStringIterator : public
    std::iterator<std::forward_iterator_tag, int32_t>
  {
    public:

    UnicodeStringIterator(const icu::UnicodeString* string, int32_t pos)
    : s(string)
    , i(pos)
    {
    }

    value_type
    operator*() const
    {
      return s->char32At(i);
    }

    bool
    operator==(const UnicodeStringIterator& rhs) const
    {
      return s == rhs.s && i == rhs.i;
    }

    bool
    operator!=(const UnicodeStringIterator& rhs) const
    {
      return !(*this == rhs);
    }

    UnicodeStringIterator&
    operator++()
    {
      ++i;
      return *this;
    }

    UnicodeStringIterator
    operator+(int32_t v)
    {
      return UnicodeStringIterator(s, i + v);
    }

    private:
    const icu::UnicodeString* s;
    int32_t i;
  };

  inline
  String&
  stringAppend(String&s, String a)
  {
    return s.append(std::move(a));
  }

  inline
  String&
  stringAppend(String& s, size_t n, UChar32 c)
  {
    for (size_t i = 0; i != n; ++i)
    {
      s.append(c);
    }

    return s;
  }

  template <typename Iterator>
  String&
  stringAppend(String& s, Iterator begin, Iterator end)
  {
    while (begin != end)
    {
      s.append(*begin);
      ++begin;
    }

    return s;
  }

  inline
  size_t
  stringLength(const String& s)
  {
    return s.length();
  }

  inline
  std::string
  toUTF8String(const String& s)
  {
    std::string result;
    s.toUTF8String(result);

    return result;
  }

  inline
  bool
  empty(const String& s)
  {
    return s.isEmpty();
  }
}

namespace std
{
  inline
  cxxopts::UnicodeStringIterator
  begin(const icu::UnicodeString& s)
  {
    return cxxopts::UnicodeStringIterator(&s, 0);
  }

  inline
  cxxopts::UnicodeStringIterator
  end(const icu::UnicodeString& s)
  {
    return cxxopts::UnicodeStringIterator(&s, s.length());
  }
}

//ifdef CXXOPTS_USE_UNICODE
#else

namespace cxxopts
{
  using String = std::string;

  template <typename T>
  T
  toLocalString(T&& t)
  {
    return std::forward<T>(t);
  }

  inline
  size_t
  stringLength(const String& s)
  {
    return s.length();
  }

  inline
  String&
  stringAppend(String&s, const String& a)
  {
    return s.append(a);
  }

  inline
  String&
  stringAppend(String& s, size_t n, char c)
  {
    return s.append(n, c);
  }

  template <typename Iterator>
  String&
  stringAppend(String& s, Iterator begin, Iterator end)
  {
    return s.append(begin, end);
  }

  template <typename T>
  std::string
  toUTF8String(T&& t)
  {
    return std::forward<T>(t);
  }

  inline
  bool
  empty(const std::string& s)
  {
    return s.empty();
  }
} // namespace cxxopts

//ifdef CXXOPTS_USE_UNICODE
#endif

namespace cxxopts
{
  namespace
  {
#ifdef _WIN32
    const std::string LQUOTE("\'");
    const std::string RQUOTE("\'");
#else
    const std::string LQUOTE("‘");
    const std::string RQUOTE("’");
#endif
  } // namespace

#if defined(__GNUC__)
// GNU GCC with -Weffc++ will issue a warning regarding the upcoming class, we want to silence it:
// warning: base class 'class std::enable_shared_from_this<cxxopts::Value>' has accessible non-virtual destructor
#pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
#pragma GCC diagnostic push
// This will be ignored under other compilers like LLVM clang.
#endif
  class Value : public std::enable_shared_from_this<Value>
  {
    public:

    virtual ~Value() = default;

    virtual
    std::shared_ptr<Value>
    clone() const = 0;

    virtual void
    parse(const std::string& text) const = 0;

    virtual void
    parse() const = 0;

    virtual bool
    has_default() const = 0;

    virtual bool
    is_container() const = 0;

    virtual bool
    has_implicit() const = 0;

    virtual std::string
    get_default_value() const = 0;

    virtual std::string
    get_implicit_value() const = 0;

    virtual std::shared_ptr<Value>
    default_value(const std::string& value) = 0;

    virtual std::shared_ptr<Value>
    implicit_value(const std::string& value) = 0;

    virtual std::shared_ptr<Value>
    no_implicit_value() = 0;

    virtual bool
    is_boolean() const = 0;
  };
#if defined(__GNUC__)
#pragma GCC diagnostic pop
#endif
  class OptionException : public std::exception
  {
    public:
    explicit OptionException(std::string  message)
    : m_message(std::move(message))
    {
    }

    CXXOPTS_NODISCARD
    const char*
    what() const noexcept override
    {
      return m_message.c_str();
    }

    private:
    std::string m_message;
  };

  class OptionSpecException : public OptionException
  {
    public:

    explicit OptionSpecException(const std::string& message)
    : OptionException(message)
    {
    }
  };

  class OptionParseException : public OptionException
  {
    public:
    explicit OptionParseException(const std::string& message)
    : OptionException(message)
    {
    }
  };

  class option_exists_error : public OptionSpecException
  {
    public:
    explicit option_exists_error(const std::string& option)
    : OptionSpecException("Option " + LQUOTE + option + RQUOTE + " already exists")
    {
    }
  };

  class invalid_option_format_error : public OptionSpecException
  {
    public:
    explicit invalid_option_format_error(const std::string& format)
    : OptionSpecException("Invalid option format " + LQUOTE + format + RQUOTE)
    {
    }
  };

  class option_syntax_exception : public OptionParseException {
    public:
    explicit option_syntax_exception(const std::string& text)
    : OptionParseException("Argument " + LQUOTE + text + RQUOTE +
        " starts with a - but has incorrect syntax")
    {
    }
  };

  class option_not_exists_exception : public OptionParseException
  {
    public:
    explicit option_not_exists_exception(const std::string& option)
    : OptionParseException("Option " + LQUOTE + option + RQUOTE + " does not exist")
    {
    }
  };

  class missing_argument_exception : public OptionParseException
  {
    public:
    explicit missing_argument_exception(const std::string& option)
    : OptionParseException(
        "Option " + LQUOTE + option + RQUOTE + " is missing an argument"
      )
    {
    }
  };

  class option_requires_argument_exception : public OptionParseException
  {
    public:
    explicit option_requires_argument_exception(const std::string& option)
    : OptionParseException(
        "Option " + LQUOTE + option + RQUOTE + " requires an argument"
      )
    {
    }
  };

  class option_not_has_argument_exception : public OptionParseException
  {
    public:
    option_not_has_argument_exception
    (
      const std::string& option,
      const std::string& arg
    )
    : OptionParseException(
        "Option " + LQUOTE + option + RQUOTE +
        " does not take an argument, but argument " +
        LQUOTE + arg + RQUOTE + " given"
      )
    {
    }
  };

  class option_not_present_exception : public OptionParseException
  {
    public:
    explicit option_not_present_exception(const std::string& option)
    : OptionParseException("Option " + LQUOTE + option + RQUOTE + " not present")
    {
    }
  };

  class option_has_no_value_exception : public OptionException
  {
    public:
    explicit option_has_no_value_exception(const std::string& option)
    : OptionException(
        option.empty() ?
        ("Option " + LQUOTE + option + RQUOTE + " has no value") :
        "Option has no value")
    {
    }
  };

  class argument_incorrect_type : public OptionParseException
  {
    public:
    explicit argument_incorrect_type
    (
      const std::string& arg
    )
    : OptionParseException(
        "Argument " + LQUOTE + arg + RQUOTE + " failed to parse"
      )
    {
    }
  };

  class option_required_exception : public OptionParseException
  {
    public:
    explicit option_required_exception(const std::string& option)
    : OptionParseException(
        "Option " + LQUOTE + option + RQUOTE + " is required but not present"
      )
    {
    }
  };

  template <typename T>
  void throw_or_mimic(const std::string& text)
  {
    static_assert(std::is_base_of<std::exception, T>::value,
                  "throw_or_mimic only works on std::exception and "
                  "deriving classes");

#ifndef CXXOPTS_NO_EXCEPTIONS
    // If CXXOPTS_NO_EXCEPTIONS is not defined, just throw
    throw T{text};
#else
    // Otherwise manually instantiate the exception, print what() to stderr,
    // and exit
    T exception{text};
    std::cerr << exception.what() << std::endl;
    std::exit(EXIT_FAILURE);
#endif
  }

  namespace values
  {
    namespace
    {
      std::basic_regex<char> integer_pattern
        ("(-)?(0x)?([0-9a-zA-Z]+)|((0x)?0)");
      std::basic_regex<char> truthy_pattern
        ("(t|T)(rue)?|1");
      std::basic_regex<char> falsy_pattern
        ("(f|F)(alse)?|0");
    } // namespace

    namespace detail
    {
      template <typename T, bool B>
      struct SignedCheck;

      template <typename T>
      struct SignedCheck<T, true>
      {
        template <typename U>
        void
        operator()(bool negative, U u, const std::string& text)
        {
          if (negative)
          {
            if (u > static_cast<U>((std::numeric_limits<T>::min)()))
            {
              throw_or_mimic<argument_incorrect_type>(text);
            }
          }
          else
          {
            if (u > static_cast<U>((std::numeric_limits<T>::max)()))
            {
              throw_or_mimic<argument_incorrect_type>(text);
            }
          }
        }
      };

      template <typename T>
      struct SignedCheck<T, false>
      {
        template <typename U>
        void
        operator()(bool, U, const std::string&) const {}
      };

      template <typename T, typename U>
      void
      check_signed_range(bool negative, U value, const std::string& text)
      {
        SignedCheck<T, std::numeric_limits<T>::is_signed>()(negative, value, text);
      }
    } // namespace detail

    template <typename R, typename T>
    void
    checked_negate(R& r, T&& t, const std::string&, std::true_type)
    {
      // if we got to here, then `t` is a positive number that fits into
      // `R`. So to avoid MSVC C4146, we first cast it to `R`.
      // See https://github.com/jarro2783/cxxopts/issues/62 for more details.
      r = static_cast<R>(-static_cast<R>(t-1)-1);
    }

    template <typename R, typename T>
    void
    checked_negate(R&, T&&, const std::string& text, std::false_type)
    {
      throw_or_mimic<argument_incorrect_type>(text);
    }

    template <typename T>
    void
    integer_parser(const std::string& text, T& value)
    {
      std::smatch match;
      std::regex_match(text, match, integer_pattern);

      if (match.length() == 0)
      {
        throw_or_mimic<argument_incorrect_type>(text);
      }

      if (match.length(4) > 0)
      {
        value = 0;
        return;
      }

      using US = typename std::make_unsigned<T>::type;

      constexpr bool is_signed = std::numeric_limits<T>::is_signed;
      const bool negative = match.length(1) > 0;
      const uint8_t base = match.length(2) > 0 ? 16 : 10;

      auto value_match = match[3];

      US result = 0;

      for (auto iter = value_match.first; iter != value_match.second; ++iter)
      {
        US digit = 0;

        if (*iter >= '0' && *iter <= '9')
        {
          digit = static_cast<US>(*iter - '0');
        }
        else if (base == 16 && *iter >= 'a' && *iter <= 'f')
        {
          digit = static_cast<US>(*iter - 'a' + 10);
        }
        else if (base == 16 && *iter >= 'A' && *iter <= 'F')
        {
          digit = static_cast<US>(*iter - 'A' + 10);
        }
        else
        {
          throw_or_mimic<argument_incorrect_type>(text);
        }

        const US next = static_cast<US>(result * base + digit);
        if (result > next)
        {
          throw_or_mimic<argument_incorrect_type>(text);
        }

        result = next;
      }

      detail::check_signed_range<T>(negative, result, text);

      if (negative)
      {
        checked_negate<T>(value, result, text, std::integral_constant<bool, is_signed>());
      }
      else
      {
        value = static_cast<T>(result);
      }
    }

    template <typename T>
    void stringstream_parser(const std::string& text, T& value)
    {
      std::stringstream in(text);
      in >> value;
      if (!in) {
        throw_or_mimic<argument_incorrect_type>(text);
      }
    }

    inline
    void
    parse_value(const std::string& text, uint8_t& value)
    {
      integer_parser(text, value);
    }

    inline
    void
    parse_value(const std::string& text, int8_t& value)
    {
      integer_parser(text, value);
    }

    inline
    void
    parse_value(const std::string& text, uint16_t& value)
    {
      integer_parser(text, value);
    }

    inline
    void
    parse_value(const std::string& text, int16_t& value)
    {
      integer_parser(text, value);
    }

    inline
    void
    parse_value(const std::string& text, uint32_t& value)
    {
      integer_parser(text, value);
    }

    inline
    void
    parse_value(const std::string& text, int32_t& value)
    {
      integer_parser(text, value);
    }

    inline
    void
    parse_value(const std::string& text, uint64_t& value)
    {
      integer_parser(text, value);
    }

    inline
    void
    parse_value(const std::string& text, int64_t& value)
    {
      integer_parser(text, value);
    }

    inline
    void
    parse_value(const std::string& text, bool& value)
    {
      std::smatch result;
      std::regex_match(text, result, truthy_pattern);

      if (!result.empty())
      {
        value = true;
        return;
      }

      std::regex_match(text, result, falsy_pattern);
      if (!result.empty())
      {
        value = false;
        return;
      }

      throw_or_mimic<argument_incorrect_type>(text);
    }

    inline
    void
    parse_value(const std::string& text, std::string& value)
    {
      value = text;
    }

    // The fallback parser. It uses the stringstream parser to parse all types
    // that have not been overloaded explicitly.  It has to be placed in the
    // source code before all other more specialized templates.
    template <typename T>
    void
    parse_value(const std::string& text, T& value) {
      stringstream_parser(text, value);
    }

    template <typename T>
    void
    parse_value(const std::string& text, std::vector<T>& value)
    {
      std::stringstream in(text);
      std::string token;
      while(!in.eof() && std::getline(in, token, CXXOPTS_VECTOR_DELIMITER)) {
        T v;
        parse_value(token, v);
        value.emplace_back(std::move(v));
      }
    }

#ifdef CXXOPTS_HAS_OPTIONAL
    template <typename T>
    void
    parse_value(const std::string& text, std::optional<T>& value)
    {
      T result;
      parse_value(text, result);
      value = std::move(result);
    }
#endif

    inline
    void parse_value(const std::string& text, char& c)
    {
      if (text.length() != 1)
      {
        throw_or_mimic<argument_incorrect_type>(text);
      }

      c = text[0];
    }

    template <typename T>
    struct type_is_container
    {
      static constexpr bool value = false;
    };

    template <typename T>
    struct type_is_container<std::vector<T>>
    {
      static constexpr bool value = true;
    };

    template <typename T>
    class abstract_value : public Value
    {
      using Self = abstract_value<T>;

      public:
      abstract_value()
      : m_result(std::make_shared<T>())
      , m_store(m_result.get())
      {
      }

      explicit abstract_value(T* t)
      : m_store(t)
      {
      }

      ~abstract_value() override = default;

      abstract_value& operator=(const abstract_value&) = default;

      abstract_value(const abstract_value& rhs)
      {
        if (rhs.m_result)
        {
          m_result = std::make_shared<T>();
          m_store = m_result.get();
        }
        else
        {
          m_store = rhs.m_store;
        }

        m_default = rhs.m_default;
        m_implicit = rhs.m_implicit;
        m_default_value = rhs.m_default_value;
        m_implicit_value = rhs.m_implicit_value;
      }

      void
      parse(const std::string& text) const override
      {
        parse_value(text, *m_store);
      }

      bool
      is_container() const override
      {
        return type_is_container<T>::value;
      }

      void
      parse() const override
      {
        parse_value(m_default_value, *m_store);
      }

      bool
      has_default() const override
      {
        return m_default;
      }

      bool
      has_implicit() const override
      {
        return m_implicit;
      }

      std::shared_ptr<Value>
      default_value(const std::string& value) override
      {
        m_default = true;
        m_default_value = value;
        return shared_from_this();
      }

      std::shared_ptr<Value>
      implicit_value(const std::string& value) override
      {
        m_implicit = true;
        m_implicit_value = value;
        return shared_from_this();
      }

      std::shared_ptr<Value>
      no_implicit_value() override
      {
        m_implicit = false;
        return shared_from_this();
      }

      std::string
      get_default_value() const override
      {
        return m_default_value;
      }

      std::string
      get_implicit_value() const override
      {
        return m_implicit_value;
      }

      bool
      is_boolean() const override
      {
        return std::is_same<T, bool>::value;
      }

      const T&
      get() const
      {
        if (m_store == nullptr)
        {
          return *m_result;
        }
        return *m_store;
      }

      protected:
      std::shared_ptr<T> m_result{};
      T* m_store{};

      bool m_default = false;
      bool m_implicit = false;

      std::string m_default_value{};
      std::string m_implicit_value{};
    };

    template <typename T>
    class standard_value : public abstract_value<T>
    {
      public:
      using abstract_value<T>::abstract_value;

      CXXOPTS_NODISCARD
      std::shared_ptr<Value>
      clone() const override
      {
        return std::make_shared<standard_value<T>>(*this);
      }
    };

    template <>
    class standard_value<bool> : public abstract_value<bool>
    {
      public:
      ~standard_value() override = default;

      standard_value()
      {
        set_default_and_implicit();
      }

      explicit standard_value(bool* b)
      : abstract_value(b)
      {
        set_default_and_implicit();
      }

      std::shared_ptr<Value>
      clone() const override
      {
        return std::make_shared<standard_value<bool>>(*this);
      }

      private:

      void
      set_default_and_implicit()
      {
        m_default = true;
        m_default_value = "false";
        m_implicit = true;
        m_implicit_value = "true";
      }
    };
  } // namespace values

  template <typename T>
  std::shared_ptr<Value>
  value()
  {
    return std::make_shared<values::standard_value<T>>();
  }

  template <typename T>
  std::shared_ptr<Value>
  value(T& t)
  {
    return std::make_shared<values::standard_value<T>>(&t);
  }

  class OptionAdder;

  class OptionDetails
  {
    public:
    OptionDetails
    (
      std::string short_,
      std::string long_,
      String desc,
      std::shared_ptr<const Value> val
    )
    : m_short(std::move(short_))
    , m_long(std::move(long_))
    , m_desc(std::move(desc))
    , m_value(std::move(val))
    , m_count(0)
    {
      m_hash = std::hash<std::string>{}(m_long + m_short);
    }

    OptionDetails(const OptionDetails& rhs)
    : m_desc(rhs.m_desc)
    , m_value(rhs.m_value->clone())
    , m_count(rhs.m_count)
    {
    }

    OptionDetails(OptionDetails&& rhs) = default;

    CXXOPTS_NODISCARD
    const String&
    description() const
    {
      return m_desc;
    }

    CXXOPTS_NODISCARD
    const Value&
    value() const {
        return *m_value;
    }

    CXXOPTS_NODISCARD
    std::shared_ptr<Value>
    make_storage() const
    {
      return m_value->clone();
    }

    CXXOPTS_NODISCARD
    const std::string&
    short_name() const
    {
      return m_short;
    }

    CXXOPTS_NODISCARD
    const std::string&
    long_name() const
    {
      return m_long;
    }

    size_t
    hash() const
    {
      return m_hash;
    }

    private:
    std::string m_short{};
    std::string m_long{};
    String m_desc{};
    std::shared_ptr<const Value> m_value{};
    int m_count;

    size_t m_hash{};
  };

  struct HelpOptionDetails
  {
    std::string s;
    std::string l;
    String desc;
    bool has_default;
    std::string default_value;
    bool has_implicit;
    std::string implicit_value;
    std::string arg_help;
    bool is_container;
    bool is_boolean;
  };

  struct HelpGroupDetails
  {
    std::string name{};
    std::string description{};
    std::vector<HelpOptionDetails> options{};
  };

  class OptionValue
  {
    public:
    void
    parse
    (
      const std::shared_ptr<const OptionDetails>& details,
      const std::string& text
    )
    {
      ensure_value(details);
      ++m_count;
      m_value->parse(text);
      m_long_name = &details->long_name();
    }

    void
    parse_default(const std::shared_ptr<const OptionDetails>& details)
    {
      ensure_value(details);
      m_default = true;
      m_long_name = &details->long_name();
      m_value->parse();
    }

#if defined(__GNUC__)
#if __GNUC__ <= 10 && __GNUC_MINOR__ <= 1
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Werror=null-dereference"
#endif
#endif
    
    CXXOPTS_NODISCARD
    size_t
    count() const noexcept
    {
      return m_count;
    }
    
#if defined(__GNUC__)
#if __GNUC__ <= 10 && __GNUC_MINOR__ <= 1
#pragma GCC diagnostic pop
#endif
#endif

    // TODO: maybe default options should count towards the number of arguments
    CXXOPTS_NODISCARD
    bool
    has_default() const noexcept
    {
      return m_default;
    }

    template <typename T>
    const T&
    as() const
    {
      if (m_value == nullptr) {
          throw_or_mimic<option_has_no_value_exception>(
              m_long_name == nullptr ? "" : *m_long_name);
      }

#ifdef CXXOPTS_NO_RTTI
      return static_cast<const values::standard_value<T>&>(*m_value).get();
#else
      return dynamic_cast<const values::standard_value<T>&>(*m_value).get();
#endif
    }

    private:
    void
    ensure_value(const std::shared_ptr<const OptionDetails>& details)
    {
      if (m_value == nullptr)
      {
        m_value = details->make_storage();
      }
    }


    const std::string* m_long_name = nullptr;
    // Holding this pointer is safe, since OptionValue's only exist in key-value pairs,
    // where the key has the string we point to.
    std::shared_ptr<Value> m_value{};
    size_t m_count = 0;
    bool m_default = false;
  };

  class KeyValue
  {
    public:
    KeyValue(std::string key_, std::string value_)
    : m_key(std::move(key_))
    , m_value(std::move(value_))
    {
    }

    CXXOPTS_NODISCARD
    const std::string&
    key() const
    {
      return m_key;
    }

    CXXOPTS_NODISCARD
    const std::string&
    value() const
    {
      return m_value;
    }

    template <typename T>
    T
    as() const
    {
      T result;
      values::parse_value(m_value, result);
      return result;
    }

    private:
    std::string m_key;
    std::string m_value;
  };

  using ParsedHashMap = std::unordered_map<size_t, OptionValue>;
  using NameHashMap = std::unordered_map<std::string, size_t>;

  class ParseResult
  {
    public:

    ParseResult() = default;
    ParseResult(const ParseResult&) = default;

    ParseResult(NameHashMap&& keys, ParsedHashMap&& values, std::vector<KeyValue> sequential, std::vector<std::string>&& unmatched_args)
    : m_keys(std::move(keys))
    , m_values(std::move(values))
    , m_sequential(std::move(sequential))
    , m_unmatched(std::move(unmatched_args))
    {
    }

    ParseResult& operator=(ParseResult&&) = default;
    ParseResult& operator=(const ParseResult&) = default;

    size_t
    count(const std::string& o) const
    {
      auto iter = m_keys.find(o);
      if (iter == m_keys.end())
      {
        return 0;
      }

      auto viter = m_values.find(iter->second);

      if (viter == m_values.end())
      {
        return 0;
      }

      return viter->second.count();
    }

    const OptionValue&
    operator[](const std::string& option) const
    {
      auto iter = m_keys.find(option);

      if (iter == m_keys.end())
      {
        throw_or_mimic<option_not_present_exception>(option);
      }

      auto viter = m_values.find(iter->second);

      if (viter == m_values.end())
      {
        throw_or_mimic<option_not_present_exception>(option);
      }

      return viter->second;
    }

    const std::vector<KeyValue>&
    arguments() const
    {
      return m_sequential;
    }

    const std::vector<std::string>&
    unmatched() const
    {
      return m_unmatched;
    }

    private:
    NameHashMap m_keys{};
    ParsedHashMap m_values{};
    std::vector<KeyValue> m_sequential{};
    std::vector<std::string> m_unmatched{};
  };

  struct Option
  {
    Option
    (
      std::string opts,
      std::string desc,
      std::shared_ptr<const Value>  value = ::cxxopts::value<bool>(),
      std::string arg_help = ""
    )
    : opts_(std::move(opts))
    , desc_(std::move(desc))
    , value_(std::move(value))
    , arg_help_(std::move(arg_help))
    {
    }

    std::string opts_;
    std::string desc_;
    std::shared_ptr<const Value> value_;
    std::string arg_help_;
  };

  using OptionMap = std::unordered_map<std::string, std::shared_ptr<OptionDetails>>;
  using PositionalList = std::vector<std::string>;
  using PositionalListIterator = PositionalList::const_iterator;

  class OptionParser
  {
    public:
    OptionParser(const OptionMap& options, const PositionalList& positional, bool allow_unrecognised)
    : m_options(options)
    , m_positional(positional)
    , m_allow_unrecognised(allow_unrecognised)
    {
    }

    ParseResult
    parse(int argc, const char* const* argv);

    bool
    consume_positional(const std::string& a, PositionalListIterator& next);

    void
    checked_parse_arg
    (
      int argc,
      const char* const* argv,
      int& current,
      const std::shared_ptr<OptionDetails>& value,
      const std::string& name
    );

    void
    add_to_option(OptionMap::const_iterator iter, const std::string& option, const std::string& arg);

    void
    parse_option
    (
      const std::shared_ptr<OptionDetails>& value,
      const std::string& name,
      const std::string& arg = ""
    );

    void
    parse_default(const std::shared_ptr<OptionDetails>& details);

    private:

    void finalise_aliases();

    const OptionMap& m_options;
    const PositionalList& m_positional;

    std::vector<KeyValue> m_sequential{};
    bool m_allow_unrecognised;

    ParsedHashMap m_parsed{};
    NameHashMap m_keys{};
  };

  class Options
  {
    public:

    explicit Options(std::string program, std::string help_string = "")
    : m_program(std::move(program))
    , m_help_string(toLocalString(std::move(help_string)))
    , m_custom_help("[OPTION...]")
    , m_positional_help("positional parameters")
    , m_show_positional(false)
    , m_allow_unrecognised(false)
    , m_width(76)
    , m_tab_expansion(false)
    , m_options(std::make_shared<OptionMap>())
    {
    }

    Options&
    positional_help(std::string help_text)
    {
      m_positional_help = std::move(help_text);
      return *this;
    }

    Options&
    custom_help(std::string help_text)
    {
      m_custom_help = std::move(help_text);
      return *this;
    }

    Options&
    show_positional_help()
    {
      m_show_positional = true;
      return *this;
    }

    Options&
    allow_unrecognised_options()
    {
      m_allow_unrecognised = true;
      return *this;
    }

    Options&
    set_width(size_t width)
    {
      m_width = width;
      return *this;
    }

    Options&
    set_tab_expansion(bool expansion=true)
    {
      m_tab_expansion = expansion;
      return *this;
    }

    ParseResult
    parse(int argc, const char* const* argv);

    OptionAdder
    add_options(std::string group = "");

    void
    add_options
    (
      const std::string& group,
      std::initializer_list<Option> options
    );

    void
    add_option
    (
      const std::string& group,
      const Option& option
    );

    void
    add_option
    (
      const std::string& group,
      const std::string& s,
      const std::string& l,
      std::string desc,
      const std::shared_ptr<const Value>& value,
      std::string arg_help
    );

    //parse positional arguments into the given option
    void
    parse_positional(std::string option);

    void
    parse_positional(std::vector<std::string> options);

    void
    parse_positional(std::initializer_list<std::string> options);

    template <typename Iterator>
    void
    parse_positional(Iterator begin, Iterator end) {
      parse_positional(std::vector<std::string>{begin, end});
    }

    std::string
    help(const std::vector<std::string>& groups = {}) const;

    std::vector<std::string>
    groups() const;

    const HelpGroupDetails&
    group_help(const std::string& group) const;

    private:

    void
    add_one_option
    (
      const std::string& option,
      const std::shared_ptr<OptionDetails>& details
    );

    String
    help_one_group(const std::string& group) const;

    void
    generate_group_help
    (
      String& result,
      const std::vector<std::string>& groups
    ) const;

    void
    generate_all_groups_help(String& result) const;

    std::string m_program{};
    String m_help_string{};
    std::string m_custom_help{};
    std::string m_positional_help{};
    bool m_show_positional;
    bool m_allow_unrecognised;
    size_t m_width;
    bool m_tab_expansion;

    std::shared_ptr<OptionMap> m_options;
    std::vector<std::string> m_positional{};
    std::unordered_set<std::string> m_positional_set{};

    //mapping from groups to help options
    std::map<std::string, HelpGroupDetails> m_help{};

    std::list<OptionDetails> m_option_list{};
    std::unordered_map<std::string, decltype(m_option_list)::iterator> m_option_map{};
  };

  class OptionAdder
  {
    public:

    OptionAdder(Options& options, std::string group)
    : m_options(options), m_group(std::move(group))
    {
    }

    OptionAdder&
    operator()
    (
      const std::string& opts,
      const std::string& desc,
      const std::shared_ptr<const Value>& value
        = ::cxxopts::value<bool>(),
      std::string arg_help = ""
    );

    private:
    Options& m_options;
    std::string m_group;
  };

  namespace
  {
    constexpr size_t OPTION_LONGEST = 30;
    constexpr size_t OPTION_DESC_GAP = 2;

    std::basic_regex<char> option_matcher
      ("--([[:alnum:]][-_[:alnum:]]+)(=(.*))?|-([[:alnum:]]+)");

    std::basic_regex<char> option_specifier
      ("(([[:alnum:]]),)?[ ]*([[:alnum:]][-_[:alnum:]]*)?");

    String
    format_option
    (
      const HelpOptionDetails& o
    )
    {
      const auto& s = o.s;
      const auto& l = o.l;

      String result = "  ";

      if (!s.empty())
      {
        result += "-" + toLocalString(s);
        if (!l.empty())
        {
          result += ",";
        }
      }
      else
      {
        result += "   ";
      }

      if (!l.empty())
      {
        result += " --" + toLocalString(l);
      }

      auto arg = !o.arg_help.empty() ? toLocalString(o.arg_help) : "arg";

      if (!o.is_boolean)
      {
        if (o.has_implicit)
        {
          result += " [=" + arg + "(=" + toLocalString(o.implicit_value) + ")]";
        }
        else
        {
          result += " " + arg;
        }
      }

      return result;
    }

    String
    format_description
    (
      const HelpOptionDetails& o,
      size_t start,
      size_t allowed,
      bool tab_expansion
    )
    {
      auto desc = o.desc;

      if (o.has_default && (!o.is_boolean || o.default_value != "false"))
      {
        if(!o.default_value.empty())
        {
          desc += toLocalString(" (default: " + o.default_value + ")");
        }
        else
        {
          desc += toLocalString(" (default: \"\")");
        }
      }

      String result;

      if (tab_expansion)
      {
        String desc2;
        auto size = size_t{ 0 };
        for (auto c = std::begin(desc); c != std::end(desc); ++c)
        {
          if (*c == '\n')
          {
            desc2 += *c;
            size = 0;
          }
          else if (*c == '\t')
          {
            auto skip = 8 - size % 8;
            stringAppend(desc2, skip, ' ');
            size += skip;
          }
          else
          {
            desc2 += *c;
            ++size;
          }
        }
        desc = desc2;
      }

      desc += " ";

      auto current = std::begin(desc);
      auto previous = current;
      auto startLine = current;
      auto lastSpace = current;

      auto size = size_t{};

      bool appendNewLine;
      bool onlyWhiteSpace = true;

      while (current != std::end(desc))
      {
        appendNewLine = false;

        if (std::isblank(*previous))
        {
          lastSpace = current;
        }

        if (!std::isblank(*current))
        {
          onlyWhiteSpace = false;
        }

        while (*current == '\n')
        {
          previous = current;
          ++current;
          appendNewLine = true;
        }

        if (!appendNewLine && size >= allowed)
        {
          if (lastSpace != startLine)
          {
            current = lastSpace;
            previous = current;
          }
          appendNewLine = true;
        }

        if (appendNewLine)
        {
          stringAppend(result, startLine, current);
          startLine = current;
          lastSpace = current;

          if (*previous != '\n')
          {
            stringAppend(result, "\n");
          }

          stringAppend(result, start, ' ');

          if (*previous != '\n')
          {
            stringAppend(result, lastSpace, current);
          }

          onlyWhiteSpace = true;
          size = 0;
        }

        previous = current;
        ++current;
        ++size;
      }

      //append whatever is left but ignore whitespace
      if (!onlyWhiteSpace)
      {
        stringAppend(result, startLine, previous);
      }

      return result;
    }
  } // namespace

inline
void
Options::add_options
(
  const std::string &group,
  std::initializer_list<Option> options
)
{
 OptionAdder option_adder(*this, group);
 for (const auto &option: options)
 {
   option_adder(option.opts_, option.desc_, option.value_, option.arg_help_);
 }
}

inline
OptionAdder
Options::add_options(std::string group)
{
  return OptionAdder(*this, std::move(group));
}

inline
OptionAdder&
OptionAdder::operator()
(
  const std::string& opts,
  const std::string& desc,
  const std::shared_ptr<const Value>& value,
  std::string arg_help
)
{
  std::match_results<const char*> result;
  std::regex_match(opts.c_str(), result, option_specifier);

  if (result.empty())
  {
    throw_or_mimic<invalid_option_format_error>(opts);
  }

  const auto& short_match = result[2];
  const auto& long_match = result[3];

  if (!short_match.length() && !long_match.length())
  {
    throw_or_mimic<invalid_option_format_error>(opts);
  } else if (long_match.length() == 1 && short_match.length())
  {
    throw_or_mimic<invalid_option_format_error>(opts);
  }

  auto option_names = []
  (
    const std::sub_match<const char*>& short_,
    const std::sub_match<const char*>& long_
  )
  {
    if (long_.length() == 1)
    {
      return std::make_tuple(long_.str(), short_.str());
    }
    return std::make_tuple(short_.str(), long_.str());
  }(short_match, long_match);

  m_options.add_option
  (
    m_group,
    std::get<0>(option_names),
    std::get<1>(option_names),
    desc,
    value,
    std::move(arg_help)
  );

  return *this;
}

inline
void
OptionParser::parse_default(const std::shared_ptr<OptionDetails>& details)
{
  // TODO: remove the duplicate code here
  auto& store = m_parsed[details->hash()];
  store.parse_default(details);
}

inline
void
OptionParser::parse_option
(
  const std::shared_ptr<OptionDetails>& value,
  const std::string& /*name*/,
  const std::string& arg
)
{
  auto hash = value->hash();
  auto& result = m_parsed[hash];
  result.parse(value, arg);

  m_sequential.emplace_back(value->long_name(), arg);
}

inline
void
OptionParser::checked_parse_arg
(
  int argc,
  const char* const* argv,
  int& current,
  const std::shared_ptr<OptionDetails>& value,
  const std::string& name
)
{
  if (current + 1 >= argc)
  {
    if (value->value().has_implicit())
    {
      parse_option(value, name, value->value().get_implicit_value());
    }
    else
    {
      throw_or_mimic<missing_argument_exception>(name);
    }
  }
  else
  {
    if (value->value().has_implicit())
    {
      parse_option(value, name, value->value().get_implicit_value());
    }
    else
    {
      parse_option(value, name, argv[current + 1]);
      ++current;
    }
  }
}

inline
void
OptionParser::add_to_option(OptionMap::const_iterator iter, const std::string& option, const std::string& arg)
{
  parse_option(iter->second, option, arg);
}

inline
bool
OptionParser::consume_positional(const std::string& a, PositionalListIterator& next)
{
  while (next != m_positional.end())
  {
    auto iter = m_options.find(*next);
    if (iter != m_options.end())
    {
      if (!iter->second->value().is_container())
      {
        auto& result = m_parsed[iter->second->hash()];
        if (result.count() == 0)
        {
          add_to_option(iter, *next, a);
          ++next;
          return true;
        }
        ++next;
        continue;
      }
      add_to_option(iter, *next, a);
      return true;
    }
    throw_or_mimic<option_not_exists_exception>(*next);
  }

  return false;
}

inline
void
Options::parse_positional(std::string option)
{
  parse_positional(std::vector<std::string>{std::move(option)});
}

inline
void
Options::parse_positional(std::vector<std::string> options)
{
  m_positional = std::move(options);

  m_positional_set.insert(m_positional.begin(), m_positional.end());
}

inline
void
Options::parse_positional(std::initializer_list<std::string> options)
{
  parse_positional(std::vector<std::string>(options));
}

inline
ParseResult
Options::parse(int argc, const char* const* argv)
{
  OptionParser parser(*m_options, m_positional, m_allow_unrecognised);

  return parser.parse(argc, argv);
}

inline ParseResult
OptionParser::parse(int argc, const char* const* argv)
{
  int current = 1;
  bool consume_remaining = false;
  auto next_positional = m_positional.begin();

  std::vector<std::string> unmatched;

  while (current != argc)
  {
    if (strcmp(argv[current], "--") == 0)
    {
      consume_remaining = true;
      ++current;
      break;
    }

    std::match_results<const char*> result;
    std::regex_match(argv[current], result, option_matcher);

    if (result.empty())
    {
      //not a flag

      // but if it starts with a `-`, then it's an error
      if (argv[current][0] == '-' && argv[current][1] != '\0') {
        if (!m_allow_unrecognised) {
          throw_or_mimic<option_syntax_exception>(argv[current]);
        }
      }

      //if true is returned here then it was consumed, otherwise it is
      //ignored
      if (consume_positional(argv[current], next_positional))
      {
      }
      else
      {
        unmatched.emplace_back(argv[current]);
      }
      //if we return from here then it was parsed successfully, so continue
    }
    else
    {
      //short or long option?
      if (result[4].length() != 0)
      {
        const std::string& s = result[4];

        for (std::size_t i = 0; i != s.size(); ++i)
        {
          std::string name(1, s[i]);
          auto iter = m_options.find(name);

          if (iter == m_options.end())
          {
            if (m_allow_unrecognised)
            {
              continue;
            }
            //error
            throw_or_mimic<option_not_exists_exception>(name);
          }

          auto value = iter->second;

          if (i + 1 == s.size())
          {
            //it must be the last argument
            checked_parse_arg(argc, argv, current, value, name);
          }
          else if (value->value().has_implicit())
          {
            parse_option(value, name, value->value().get_implicit_value());
          }
          else
          {
            //error
            throw_or_mimic<option_requires_argument_exception>(name);
          }
        }
      }
      else if (result[1].length() != 0)
      {
        const std::string& name = result[1];

        auto iter = m_options.find(name);

        if (iter == m_options.end())
        {
          if (m_allow_unrecognised)
          {
            // keep unrecognised options in argument list, skip to next argument
            unmatched.emplace_back(argv[current]);
            ++current;
            continue;
          }
          //error
          throw_or_mimic<option_not_exists_exception>(name);
        }

        auto opt = iter->second;

        //equals provided for long option?
        if (result[2].length() != 0)
        {
          //parse the option given

          parse_option(opt, name, result[3]);
        }
        else
        {
          //parse the next argument
          checked_parse_arg(argc, argv, current, opt, name);
        }
      }

    }

    ++current;
  }

  for (auto& opt : m_options)
  {
    auto& detail = opt.second;
    const auto& value = detail->value();

    auto& store = m_parsed[detail->hash()];

    if(value.has_default() && !store.count() && !store.has_default()){
      parse_default(detail);
    }
  }

  if (consume_remaining)
  {
    while (current < argc)
    {
      if (!consume_positional(argv[current], next_positional)) {
        break;
      }
      ++current;
    }

    //adjust argv for any that couldn't be swallowed
    while (current != argc) {
      unmatched.emplace_back(argv[current]);
      ++current;
    }
  }

  finalise_aliases();

  ParseResult parsed(std::move(m_keys), std::move(m_parsed), std::move(m_sequential), std::move(unmatched));
  return parsed;
}

inline
void
OptionParser::finalise_aliases()
{
  for (auto& option: m_options)
  {
    auto& detail = *option.second;
    auto hash = detail.hash();
    m_keys[detail.short_name()] = hash;
    m_keys[detail.long_name()] = hash;

    m_parsed.emplace(hash, OptionValue());
  }
}

inline
void
Options::add_option
(
  const std::string& group,
  const Option& option
)
{
    add_options(group, {option});
}

inline
void
Options::add_option
(
  const std::string& group,
  const std::string& s,
  const std::string& l,
  std::string desc,
  const std::shared_ptr<const Value>& value,
  std::string arg_help
)
{
  auto stringDesc = toLocalString(std::move(desc));
  auto option = std::make_shared<OptionDetails>(s, l, stringDesc, value);

  if (!s.empty())
  {
    add_one_option(s, option);
  }

  if (!l.empty())
  {
    add_one_option(l, option);
  }

  m_option_list.push_front(*option.get());
  auto iter = m_option_list.begin();
  m_option_map[s] = iter;
  m_option_map[l] = iter;

  //add the help details
  auto& options = m_help[group];

  options.options.emplace_back(HelpOptionDetails{s, l, stringDesc,
      value->has_default(), value->get_default_value(),
      value->has_implicit(), value->get_implicit_value(),
      std::move(arg_help),
      value->is_container(),
      value->is_boolean()});
}

inline
void
Options::add_one_option
(
  const std::string& option,
  const std::shared_ptr<OptionDetails>& details
)
{
  auto in = m_options->emplace(option, details);

  if (!in.second)
  {
    throw_or_mimic<option_exists_error>(option);
  }
}

inline
String
Options::help_one_group(const std::string& g) const
{
  using OptionHelp = std::vector<std::pair<String, String>>;

  auto group = m_help.find(g);
  if (group == m_help.end())
  {
    return "";
  }

  OptionHelp format;

  size_t longest = 0;

  String result;

  if (!g.empty())
  {
    result += toLocalString(" " + g + " options:\n");
  }

  for (const auto& o : group->second.options)
  {
    if (m_positional_set.find(o.l) != m_positional_set.end() &&
        !m_show_positional)
    {
      continue;
    }

    auto s = format_option(o);
    longest = (std::max)(longest, stringLength(s));
    format.push_back(std::make_pair(s, String()));
  }
  longest = (std::min)(longest, OPTION_LONGEST);

  //widest allowed description -- min 10 chars for helptext/line
  size_t allowed = 10;
  if (m_width > allowed + longest + OPTION_DESC_GAP)
  {
    allowed = m_width - longest - OPTION_DESC_GAP;
  }

  auto fiter = format.begin();
  for (const auto& o : group->second.options)
  {
    if (m_positional_set.find(o.l) != m_positional_set.end() &&
        !m_show_positional)
    {
      continue;
    }

    auto d = format_description(o, longest + OPTION_DESC_GAP, allowed, m_tab_expansion);

    result += fiter->first;
    if (stringLength(fiter->first) > longest)
    {
      result += '\n';
      result += toLocalString(std::string(longest + OPTION_DESC_GAP, ' '));
    }
    else
    {
      result += toLocalString(std::string(longest + OPTION_DESC_GAP -
        stringLength(fiter->first),
        ' '));
    }
    result += d;
    result += '\n';

    ++fiter;
  }

  return result;
}

inline
void
Options::generate_group_help
(
  String& result,
  const std::vector<std::string>& print_groups
) const
{
  for (size_t i = 0; i != print_groups.size(); ++i)
  {
    const String& group_help_text = help_one_group(print_groups[i]);
    if (empty(group_help_text))
    {
      continue;
    }
    result += group_help_text;
    if (i < print_groups.size() - 1)
    {
      result += '\n';
    }
  }
}

inline
void
Options::generate_all_groups_help(String& result) const
{
  std::vector<std::string> all_groups;

  std::transform(
    m_help.begin(),
    m_help.end(),
    std::back_inserter(all_groups),
    [] (const std::map<std::string, HelpGroupDetails>::value_type& group)
    {
      return group.first;
    }
  );

  generate_group_help(result, all_groups);
}

inline
std::string
Options::help(const std::vector<std::string>& help_groups) const
{
  String result = m_help_string + "\nUsage:\n  " +
    toLocalString(m_program) + " " + toLocalString(m_custom_help);

  if (!m_positional.empty() && !m_positional_help.empty()) {
    result += " " + toLocalString(m_positional_help);
  }

  result += "\n\n";

  if (help_groups.empty())
  {
    generate_all_groups_help(result);
  }
  else
  {
    generate_group_help(result, help_groups);
  }

  return toUTF8String(result);
}

inline
std::vector<std::string>
Options::groups() const
{
  std::vector<std::string> g;

  std::transform(
    m_help.begin(),
    m_help.end(),
    std::back_inserter(g),
    [] (const std::map<std::string, HelpGroupDetails>::value_type& pair)
    {
      return pair.first;
    }
  );

  return g;
}

inline
const HelpGroupDetails&
Options::group_help(const std::string& group) const
{
  return m_help.at(group);
}

} // namespace cxxopts

// clang-format on

#endif  // CXXOPTS_HPP_INCLUDED

================================================
FILE: ohmutil/3rdparty/ska/bytell_hash_map.hpp
================================================
//          Copyright Malte Skarupke 2017.
// Distributed under the Boost Software License, Version 1.0.
//    (See http://www.boost.org/LICENSE_1_0.txt)

#pragma once

#include <cstdint>
#include <cstddef>
#include <cmath>
#include <algorithm>
#include <iterator>
#include <utility>
#include <type_traits>
#include "flat_hash_map.hpp"
#include <vector>
#include <array>

namespace ska
{

namespace detailv8
{
using ska::detailv3::functor_storage;
using ska::detailv3::KeyOrValueHasher;
using ska::detailv3::KeyOrValueEquality;
using ska::detailv3::AssignIfTrue;
using ska::detailv3::HashPolicySelector;

template<typename = void>
struct sherwood_v8_constants
{
    static constexpr int8_t magic_for_empty = int8_t(0b11111111);
    static constexpr int8_t magic_for_reserved = int8_t(0b11111110);
    static constexpr int8_t bits_for_direct_hit = int8_t(0b10000000);
    static constexpr int8_t magic_for_direct_hit = int8_t(0b00000000);
    static constexpr int8_t magic_for_list_entry = int8_t(0b10000000);

    static constexpr int8_t bits_for_distance = int8_t(0b01111111);
    inline static int distance_from_metadata(int8_t metadata)
    {
        return metadata & bits_for_distance;
    }

    static constexpr int num_jump_distances = 126;
    // jump distances chosen like this:
    // 1. pick the first 16 integers to promote staying in the same block
    // 2. add the next 66 triangular numbers to get even jumps when
    // the hash table is a power of two
    // 3. add 44 more triangular numbers at a much steeper growth rate
    // to get a sequence that allows large jumps so that a table
    // with 10000 sequential numbers doesn't endlessly re-allocate
    static constexpr size_t jump_distances[num_jump_distances]
    {
        0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15,

        21, 28, 36, 45, 55, 66, 78, 91, 105, 120, 136, 153, 171, 190, 210, 231,
        253, 276, 300, 325, 351, 378, 406, 435, 465, 496, 528, 561, 595, 630,
        666, 703, 741, 780, 820, 861, 903, 946, 990, 1035, 1081, 1128, 1176,
        1225, 1275, 1326, 1378, 1431, 1485, 1540, 1596, 1653, 1711, 1770, 1830,
        1891, 1953, 2016, 2080, 2145, 2211, 2278, 2346, 2415, 2485, 2556,

        3741, 8385, 18915, 42486, 95703, 215496, 485605, 1091503, 2456436,
        5529475, 12437578, 27986421, 62972253, 141700195, 318819126, 717314626,
        1614000520, 3631437253, 8170829695, 18384318876, 41364501751,
        93070021080, 209407709220, 471167588430, 1060127437995, 2385287281530,
        5366895564381, 12075513791265, 27169907873235, 61132301007778,
        137547673121001, 309482258302503, 696335090510256, 1566753939653640,
        3525196427195653, 7931691866727775, 17846306747368716,
        40154190394120111, 90346928493040500, 203280588949935750,
        457381324898247375, 1029107980662394500, 2315492957028380766,
        5209859150892887590,
    };
};
template<typename T>
constexpr int8_t sherwood_v8_constants<T>::magic_for_empty;
template<typename T>
constexpr int8_t sherwood_v8_constants<T>::magic_for_reserved;
template<typename T>
constexpr int8_t sherwood_v8_constants<T>::bits_for_direct_hit;
template<typename T>
constexpr int8_t sherwood_v8_constants<T>::magic_for_direct_hit;
template<typename T>
constexpr int8_t sherwood_v8_constants<T>::magic_for_list_entry;

template<typename T>
constexpr int8_t sherwood_v8_constants<T>::bits_for_distance;

template<typename T>
constexpr int sherwood_v8_constants<T>::num_jump_distances;
template<typename T>
constexpr size_t sherwood_v8_constants<T>::jump_distances[num_jump_distances];

template<typename T, uint8_t BlockSize>
struct sherwood_v8_block
{
    sherwood_v8_block()
    {
    }
    ~sherwood_v8_block()
    {
    }
    int8_t control_bytes[BlockSize];
    union
    {
        T data[BlockSize];
    };

    static sherwood_v8_block * empty_block()
    {
        static std::array<int8_t, BlockSize> empty_bytes = []
        {
            std::array<int8_t, BlockSize> result;
            result.fill(sherwood_v8_constants<>::magic_for_empty);
            return result;
        }();
        return reinterpret_cast<sherwood_v8_block *>(&empty_bytes);
    }

    int first_empty_index() const
    {
        for (int i = 0; i < BlockSize; ++i)
        {
            if (control_bytes[i] == sherwood_v8_constants<>::magic_for_empty)
                return i;
        }
        return -1;
    }

    void fill_control_bytes(int8_t value)
    {
        std::fill(std::begin(control_bytes), std::end(control_bytes), value);
    }
};

template<typename T, typename FindKey, typename ArgumentHash, typename Hasher, typename ArgumentEqual, typename Equal, typename ArgumentAlloc, typename ByteAlloc, uint8_t BlockSize>
class sherwood_v8_table : private ByteAlloc, private Hasher, private Equal
{
    using AllocatorTraits = std::allocator_traits<ByteAlloc>;
    using BlockType = sherwood_v8_block<T, BlockSize>;
    using BlockPointer = BlockType *;
    using BytePointer = typename AllocatorTraits::pointer;
    struct convertible_to_iterator;
    using Constants = sherwood_v8_constants<>;

public:

    using value_type = T;
    using size_type = size_t;
    using difference_type = std::ptrdiff_t;
    using hasher = ArgumentHash;
    using key_equal = ArgumentEqual;
    using allocator_type = ByteAlloc;
    using reference = value_type &;
    using const_reference = const value_type &;
    using pointer = value_type *;
    using const_pointer = const value_type *;

    sherwood_v8_table()
    {
    }
    explicit sherwood_v8_table(size_type bucket_count, const ArgumentHash & hash = ArgumentHash(), const ArgumentEqual & equal = ArgumentEqual(), const ArgumentAlloc & alloc = ArgumentAlloc())
        : ByteAlloc(alloc), Hasher(hash), Equal(equal)
    {
        if (bucket_count)
            rehash(bucket_count);
    }
    sherwood_v8_table(size_type bucket_count, const ArgumentAlloc & alloc)
        : sherwood_v8_table(bucket_count, ArgumentHash(), ArgumentEqual(), alloc)
    {
    }
    sherwood_v8_table(size_type bucket_count, const ArgumentHash & hash, const ArgumentAlloc & alloc)
        : sherwood_v8_table(bucket_count, hash, ArgumentEqual(), alloc)
    {
    }
    explicit sherwood_v8_table(const ArgumentAlloc & alloc)
        : ByteAlloc(alloc)
    {
    }
    template<typename It>
    sherwood_v8_table(It first, It last, size_type bucket_count = 0, const ArgumentHash & hash = ArgumentHash(), const ArgumentEqual & equal = ArgumentEqual(), const ArgumentAlloc & alloc = ArgumentAlloc())
        : sherwood_v8_table(bucket_count, hash, equal, alloc)
    {
        insert(first, last);
    }
    template<typename It>
    sherwood_v8_table(It first, It last, size_type bucket_count, const ArgumentAlloc & alloc)
        : sherwood_v8_table(first, last, bucket_count, ArgumentHash(), ArgumentEqual(), alloc)
    {
    }
    template<typename It>
    sherwood_v8_table(It first, It last, size_type bucket_count, const ArgumentHash & hash, const ArgumentAlloc & alloc)
        : sherwood_v8_table(first, last, bucket_count, hash, ArgumentEqual(), alloc)
    {
    }
    sherwood_v8_table(std::initializer_list<T> il, size_type bucket_count = 0, const ArgumentHash & hash = ArgumentHash(), const ArgumentEqual & equal = ArgumentEqual(), const ArgumentAlloc & alloc = ArgumentAlloc())
        : sherwood_v8_table(bucket_count, hash, equal, alloc)
    {
        if (bucket_count == 0)
            rehash(il.size());
        insert(il.begin(), il.end());
    }
    sherwood_v8_table(std::initializer_list<T> il, size_type bucket_count, const ArgumentAlloc & alloc)
        : sherwood_v8_table(il, bucket_count, ArgumentHash(), ArgumentEqual(), alloc)
    {
    }
    sherwood_v8_table(std::initializer_list<T> il, size_type bucket_count, const ArgumentHash & hash, const ArgumentAlloc & alloc)
        : sherwood_v8_table(il, bucket_count, hash, ArgumentEqual(), alloc)
    {
    }
    sherwood_v8_table(const sherwood_v8_table & other)
        : sherwood_v8_table(other, AllocatorTraits::select_on_container_copy_construction(other.get_allocator()))
    {
    }
    sherwood_v8_table(const sherwood_v8_table & other, const ArgumentAlloc & alloc)
        : ByteAlloc(alloc), Hasher(other), Equal(other), _max_load_factor(other._max_load_factor)
    {
        rehash_for_other_container(other);
        try
        {
            insert(other.begin(), other.end());
        }
        catch(...)
        {
            clear();
            deallocate_data(entries, num_slots_minus_one);
            throw;
        }
    }
    sherwood_v8_table(sherwood_v8_table && other) noexcept
        : ByteAlloc(std::move(other)), Hasher(std::move(other)), Equal(std::move(other))
        , _max_load_factor(other._max_load_factor)
    {
        swap_pointers(other);
    }
    sherwood_v8_table(sherwood_v8_table && other, const ArgumentAlloc & alloc) noexcept
        : ByteAlloc(alloc), Hasher(std::move(other)), Equal(std::move(other))
        , _max_load_factor(other._max_load_factor)
    {
        swap_pointers(other);
    }
    sherwood_v8_table & operator=(const sherwood_v8_table & other)
    {
        if (this == std::addressof(other))
            return *this;

        clear();
        if (AllocatorTraits::propagate_on_container_copy_assignment::value)
        {
            if (static_cast<ByteAlloc &>(*this) != static_cast<const ByteAlloc &>(other))
            {
                reset_to_empty_state();
            }
            AssignIfTrue<ByteAlloc, AllocatorTraits::propagate_on_container_copy_assignment::value>()(*this, other);
        }
        _max_load_factor = other._max_load_factor;
        static_cast<Hasher &>(*this) = other;
        static_cast<Equal &>(*this) = other;
        rehash_for_other_container(other);
        insert(other.begin(), other.end());
        return *this;
    }
    sherwood_v8_table & operator=(sherwood_v8_table && other) noexcept
    {
        if (this == std::addressof(other))
            return *this;
        else if (AllocatorTraits::propagate_on_container_move_assignment::value)
        {
            clear();
            reset_to_empty_state();
            AssignIfTrue<ByteAlloc, AllocatorTraits::propagate_on_container_move_assignment::value>()(*this, std::move(other));
            swap_pointers(other);
        }
        else if (static_cast<ByteAlloc &>(*this) == static_cast<ByteAlloc &>(other))
        {
            swap_pointers(other);
        }
        else
        {
            clear();
            _max_load_factor = other._max_load_factor;
            rehash_for_other_container(other);
            for (T & elem : other)
                emplace(std::move(elem));
            other.clear();
        }
        static_cast<Hasher &>(*this) = std::move(other);
        static_cast<Equal &>(*this) = std::move(other);
        return *this;
    }
    ~sherwood_v8_table()
    {
        clear();
        deallocate_data(entries, num_slots_minus_one);
    }

    const allocator_type & get_allocator() const
    {
        return static_cast<const allocator_type &>(*this);
    }
    const ArgumentEqual & key_eq() const
    {
        return static_cast<const ArgumentEqual &>(*this);
    }
    const ArgumentHash & hash_function() const
    {
        return static_cast<const ArgumentHash &>(*this);
    }

    template<typename ValueType>
    struct templated_iterator
    {
    private:
        friend class sherwood_v8_table;
        BlockPointer current = BlockPointer();
        size_t index = 0;

    public:
        templated_iterator()
        {
        }
        templated_iterator(BlockPointer entries, size_t index)
            : current(entries)
            , index(index)
        {
        }

        using iterator_category = std::forward_iterator_tag;
        using value_type = ValueType;
        using difference_type = ptrdiff_t;
        using pointer = ValueType *;
        using reference = ValueType &;

        friend bool operator==(const templated_iterator & lhs, const templated_iterator & rhs)
        {
            return lhs.index == rhs.index;
        }
        friend bool operator!=(const templated_iterator & lhs, const templated_iterator & rhs)
        {
            return !(lhs == rhs);
        }

        templated_iterator & operator++()
        {
            do
            {
                if (index % BlockSize == 0)
                    --current;
                if (index-- == 0)
                    break;
            }
            while(current->control_bytes[index % BlockSize] == Constants::magic_for_empty);
            return *this;
        }
        templated_iterator operator++(int)
        {
            templated_iterator copy(*this);
            ++*this;
            return copy;
        }

        ValueType & operator*() const
        {
            return current->data[index % BlockSize];
        }
        ValueType * operator->() const
        {
            return current->data + index % BlockSize;
        }

        operator templated_iterator<const value_type>() const
        {
            return { current, index };
        }
    };
    using iterator = templated_iterator<value_type>;
    using const_iterator = templated_iterator<const value_type>;

    iterator begin()
    {
        size_t num_slots = num_slots_minus_one ? num_slots_minus_one + 1 : 0;
        return ++iterator{ entries + num_slots / BlockSize, num_slots };
    }
    const_iterator begin() const
    {
        size_t num_slots = num_slots_minus_one ? num_slots_minus_one + 1 : 0;
        return ++iterator{ entries + num_slots / BlockSize, num_slots };
    }
    const_iterator cbegin() const
    {
        return begin();
    }
    iterator end()
    {
        return { entries - 1, std::numeric_limits<size_t>::max() };
    }
    const_iterator end() const
    {
        return { entries - 1, std::numeric_limits<size_t>::max() };
    }
    const_iterator cend() const
    {
        return end();
    }

    inline iterator find(const FindKey & key)
    {
        size_t index = hash_object(key);
        size_t num_slots_minus_one = this->num_slots_minus_one;
        BlockPointer entries = this->entries;
        index = hash_policy.index_for_hash(index, num_slots_minus_one);
        bool first = true;
        for (;;)
        {
            size_t block_index = index / BlockSize;
            int index_in_block = index % BlockSize;
            BlockPointer block = entries + block_index;
            int8_t metadata = block->control_bytes[index_in_block];
            if (first)
            {
                if ((metadata & Constants::bits_for_direct_hit) != Constants::magic_for_direct_hit)
                    return end();
                first = false;
            }
            if (compares_equal(key, block->data[index_in_block]))
                return { block, index };
            int8_t to_next_index = metadata & Constants::bits_for_distance;
            if (to_next_index == 0)
                return end();
            index += Constants::jump_distances[to_next_index];
            index = hash_policy.keep_in_range(index, num_slots_minus_one);
        }
    }
    inline const_iterator find(const FindKey & key) const
    {
        return const_cast<sherwood_v8_table *>(this)->find(key);
    }
    size_t count(const FindKey & key) const
    {
        return find(key) == end() ? 0 : 1;
    }
    std::pair<iterator, iterator> equal_range(const FindKey & key)
    {
        iterator found = find(key);
        if (found == end())
            return { found, found };
        else
            return { found, std::next(found) };
    }
    std::pair<const_iterator, const_iterator> equal_range(const FindKey & key) const
    {
        const_iterator found = find(key);
        if (found == end())
            return { found, found };
        else
            return { found, std::next(found) };
    }


    template<typename Key, typename... Args>
    inline std::pair<iterator, bool> emplace(Key && key, Args &&... args)
    {
        size_t index = hash_object(key);
        size_t num_slots_minus_one = this->num_slots_minus_one;
        BlockPointer entries = this->entries;
        index = hash_policy.index_for_hash(index, num_slots_minus_one);
        bool first = true;
        for (;;)
        {
            size_t block_index = index / BlockSize;
            int index_in_block = index % BlockSize;
            BlockPointer block = entries + block_index;
            int8_t metadata = block->control_bytes[index_in_block];
            if (first)
            {
                if ((metadata & Constants::bits_for_direct_hit) != Constants::magic_for_direct_hit)
                    return emplace_direct_hit({ index, block }, std::forward<Key>(key), std::forward<Args>(args)...);
                first = false;
            }
            if (compares_equal(key, block->data[index_in_block]))
                return { { block, index }, false };
            int8_t to_next_index = metadata & Constants::bits_for_distance;
            if (to_next_index == 0)
                return emplace_new_key({ index, block }, std::forward<Key>(key), std::forward<Args>(args)...);
            index += Constants::jump_distances[to_next_index];
            index = hash_policy.keep_in_range(index, num_slots_minus_one);
        }
    }

    std::pair<iterator, bool> insert(const value_type & value)
    {
        return emplace(value);
    }
    std::pair<iterator, bool> insert(value_type && value)
    {
        return emplace(std::move(value));
    }
    template<typename... Args>
    iterator emplace_hint(const_iterator, Args &&... args)
    {
        return emplace(std::forward<Args>(args)...).first;
    }
    iterator insert(const_iterator, const value_type & value)
    {
        return emplace(value).first;
    }
    iterator insert(const_iterator, value_type && value)
    {
        return emplace(std::move(value)).first;
    }

    template<typename It>
    void insert(It begin, It end)
    {
        for (; begin != end; ++begin)
        {
            emplace(*begin);
        }
    }
    void insert(std::initializer_list<value_type> il)
    {
        insert(il.begin(), il.end());
    }

    void rehash(size_t num_items)
    {
        num_items = std::max(num_items, static_cast<size_t>(std::ceil(num_elements / static_cast<double>(_max_load_factor))));
        if (num_items == 0)
        {
            reset_to_empty_state();
            return;
        }
        auto new_prime_index = hash_policy.next_size_over(num_items);
        if (num_items == num_slots_minus_one + 1)
            return;
        size_t num_blocks = num_items / BlockSize;
        if (num_items % BlockSize)
            ++num_blocks;
        size_t memory_requirement = calculate_memory_requirement(num_blocks);
        unsigned char * new_memory = &*AllocatorTraits::allocate(*this, memory_requirement);

        BlockPointer new_buckets = reinterpret_cast<BlockPointer>(new_memory);

        BlockPointer special_end_item = new_buckets + num_blocks;
        for (BlockPointer it = new_buckets; it <= special_end_item; ++it)
            it->fill_control_bytes(Constants::magic_for_empty);
        using std::swap;
        swap(entries, new_buckets);
        swap(num_slots_minus_one, num_items);
        --num_slots_minus_one;
        hash_policy.commit(new_prime_index);
        num_elements = 0;
        if (num_items)
            ++num_items;
        size_t old_num_blocks = num_items / BlockSize;
        if (num_items % BlockSize)
            ++old_num_blocks;
        for (BlockPointer it = new_buckets, end = new_buckets + old_num_blocks; it != end; ++it)
        {
            for (int i = 0; i < BlockSize; ++i)
            {
                int8_t metadata = it->control_bytes[i];
                if (metadata != Constants::magic_for_empty && metadata != Constants::magic_for_reserved)
                {
                    emplace(std::move(it->data[i]));
                    AllocatorTraits::destroy(*this, it->data + i);
                }
            }
        }
        deallocate_data(new_buckets, num_items - 1);
    }

    void reserve(size_t num_elements)
    {
        size_t required_buckets = num_buckets_for_reserve(num_elements);
        if (required_buckets > bucket_count())
            rehash(required_buckets);
    }

    // the return value is a type that can be converted to an iterator
    // the reason for doing this is that it's not free to find the
    // iterator pointing at the next element. if you care about the
    // next iterator, turn the return value into an iterator
    convertible_to_iterator erase(const_iterator to_erase)
    {
        LinkedListIt current = { to_erase.index, to_erase.current };
        if (current.has_next())
        {
            LinkedListIt previous = current;
            LinkedListIt next = current.next(*this);
            while (next.has_next())
            {
                previous = next;
                next = next.next(*this);
            }
            AllocatorTraits::destroy(*this, std::addressof(*current));
            AllocatorTraits::construct(*this, std::addressof(*current), std::move(*next));
            AllocatorTraits::destroy(*this, std::addressof(*next));
            next.set_metadata(Constants::magic_for_empty);
            previous.clear_next();
        }
        else
        {
            if (!current.is_direct_hit())
                find_parent_block(current).clear_next();
            AllocatorTraits::destroy(*this, std::addressof(*current));
            current.set_metadata(Constants::magic_for_empty);
        }
        --num_elements;
        return { to_erase.current, to_erase.index };
    }

    iterator erase(const_iterator begin_it, const_iterator end_it)
    {
        if (begin_it == end_it)
            return { begin_it.current, begin_it.index };
        if (std::next(begin_it) == end_it)
            return erase(begin_it);
        if (begin_it == begin() && end_it == end())
        {
            clear();
            return { end_it.current, end_it.index };
        }
        std::vector<std::pair<int, LinkedListIt>> depth_in_chain;
        for (const_iterator it = begin_it; it != end_it; ++it)
        {
            LinkedListIt list_it(it.index, it.current);
            if (list_it.is_direct_hit())
                depth_in_chain.emplace_back(0, list_it);
            else
            {
                LinkedListIt root = find_direct_hit(list_it);
                int distance = 1;
                for (;;)
                {
                    LinkedListIt next = root.next(*this);
                    if (next == list_it)
                        break;
                    ++distance;
                    root = next;
                }
                depth_in_chain.emplace_back(distance, list_it);
            }
        }
        std::sort(depth_in_chain.begin(), depth_in_chain.end(), [](const auto & a, const auto & b) { return a.first < b.first; });
        for (auto it = depth_in_chain.rbegin(), end = depth_in_chain.rend(); it != end; ++it)
        {
            erase(it->second.it());
        }

        if (begin_it.current->control_bytes[begin_it.index % BlockSize] == Constants::magic_for_empty)
            return ++iterator{ begin_it.current, begin_it.index };
        else
            return { begin_it.current, begin_it.index };
    }

    size_t erase(const FindKey & key)
    {
        auto found = find(key);
        if (found == end())
            return 0;
        else
        {
            erase(found);
            return 1;
        }
    }

    void clear()
    {
        if (!num_slots_minus_one)
            return;
        size_t num_slots = num_slots_minus_one + 1;
        size_t num_blocks = num_slots / BlockSize;
        if (num_slots % BlockSize)
            ++num_blocks;
        for (BlockPointer it = entries, end = it + num_blocks; it != end; ++it)
        {
            for (int i = 0; i < BlockSize; ++i)
            {
                if (it->control_bytes[i] != Constants::magic_for_empty)
                {
                    AllocatorTraits::destroy(*this, std::addressof(it->data[i]));
                    it->control_bytes[i] = Constants::magic_for_empty;
                }
            }
        }
        num_elements = 0;
    }

    void shrink_to_fit()
    {
        rehash_for_other_container(*this);
    }

    void swap(sherwood_v8_table & other)
    {
        using std::swap;
        swap_pointers(other);
        swap(static_cast<ArgumentHash &>(*this), static_cast<ArgumentHash &>(other));
        swap(static_cast<ArgumentEqual &>(*this), static_cast<ArgumentEqual &>(other));
        if (AllocatorTraits::propagate_on_container_swap::value)
            swap(static_cast<ByteAlloc &>(*this), static_cast<ByteAlloc &>(other));
    }

    size_t size() const
    {
        return num_elements;
    }
    size_t max_size() const
    {
        return (AllocatorTraits::max_size(*this)) / sizeof(T);
    }
    size_t bucket_count() const
    {
        return num_slots_minus_one ? num_slots_minus_one + 1 : 0;
    }
    size_type max_bucket_count() const
    {
        return (AllocatorTraits::max_size(*this)) / sizeof(T);
    }
    size_t bucket(const FindKey & key) const
    {
        return hash_policy.index_for_hash(hash_object(key), num_slots_minus_one);
    }
    float load_factor() const
    {
        return static_cast<double>(num_elements) / (num_slots_minus_one + 1);
    }
    void max_load_factor(float value)
    {
        _max_load_factor = value;
    }
    float max_load_factor() const
    {
        return _max_load_factor;
    }

    bool empty() const
    {
        return num_elements == 0;
    }

private:
    BlockPointer entries = BlockType::empty_block();
    size_t num_slots_minus_one = 0;
    typename HashPolicySelector<ArgumentHash>::type hash_policy;
    float _max_load_factor = 0.9375f;
    size_t num_elements = 0;

    size_t num_buckets_for_reserve(size_t num_elements) const
    {
        return static_cast<size_t>(std::ceil(num_elements / static_cast<double>(_max_load_factor)));
    }
    void rehash_for_other_container(const sherwood_v8_table & other)
    {
        rehash(std::min(num_buckets_for_reserve(other.size()), other.bucket_count()));
    }
    bool is_full() const
    {
        if (!num_slots_minus_one)
            return true;
        else
            return num_elements + 1 > (num_slots_minus_one + 1) * static_cast<double>(_max_load_factor);
    }

    void swap_pointers(sherwood_v8_table & other)
    {
        using std::swap;
        swap(hash_policy, other.hash_policy);
        swap(entries, other.entries);
        swap(num_slots_minus_one, other.num_slots_minus_one);
        swap(num_elements, other.num_elements);
        swap(_max_load_factor, other._max_load_factor);
    }

    struct LinkedListIt
    {
        size_t index = 0;
        BlockPointer block = nullptr;

        LinkedListIt()
        {
        }
        LinkedListIt(size_t index, BlockPointer block)
            : index(index), block(block)
        {
        }

        iterator it() const
        {
            return { block, index };
        }
        int index_in_block() const
        {
            return index % BlockSize;
        }
        bool is_direct_hit() const
        {
            return (metadata() & Constants::bits_for_direct_hit) == Constants::magic_for_direct_hit;
        }
        bool is_empty() const
        {
            return metadata() == Constants::magic_for_empty;
        }
        bool has_next() const
        {
            return jump_index() != 0;
        }
        int8_t jump_index() const
        {
            return Constants::distance_from_metadata(metadata());
        }
        int8_t metadata() const
        {
            return block->control_bytes[index_in_block()];
        }
        void set_metadata(int8_t metadata)
        {
            block->control_bytes[index_in_block()] = metadata;
        }

        LinkedListIt next(sherwood_v8_table & table) const
        {
            int8_t distance = jump_index();
            size_t next_index = table.hash_policy.keep_in_range(index + Constants::jump_distances[distance], table.num_slots_minus_one);
            return { next_index, table.entries + next_index / BlockSize };
        }
        void set_next(int8_t jump_index)
        {
            int8_t & metadata = block->control_bytes[index_in_block()];
            metadata = (metadata & ~Constants::bits_for_distance) | jump_index;
        }
        void clear_next()
        {
            set_next(0);
        }

        value_type & operator*() const
        {
            return block->data[index_in_block()];
        }
        bool operator!() const
        {
            return !block;
        }
        explicit operator bool() const
        {
            return block != nullptr;
        }
        bool operator==(const LinkedListIt & other) const
        {
            return index == other.index;
        }
        bool operator!=(const LinkedListIt & other) const
        {
            return !(*this == other);
        }
    };

    template<typename... Args>
    SKA_NOINLINE(std::pair<iterator, bool>) emplace_direct_hit(LinkedListIt block, Args &&... args)
    {
        using std::swap;
        if (is_full())
        {
            grow();
            return emplace(std::forward<Args>(args)...);
        }
        if (block.metadata() == Constants::magic_for_empty)
        {
            AllocatorTraits::construct(*this, std::addressof(*block), std::forward<Args>(args)...);
            block.set_metadata(Constants::magic_for_direct_hit);
            ++num_elements;
            return { block.it(), true };
        }
        else
        {
            LinkedListIt parent_block = find_parent_block(block);
            std::pair<int8_t, LinkedListIt> free_block = find_free_index(parent_block);
            if (!free_block.first)
            {
                grow();
                return emplace(std::forward<Args>(args)...);
            }
            value_type new_value(std::forward<Args>(args)...);
            for (LinkedListIt it = block;;)
            {
                AllocatorTraits::construct(*this, std::addressof(*free_block.second), std::move(*it));
                AllocatorTraits::destroy(*this, std::addressof(*it));
                parent_block.set_next(free_block.first);
                free_block.second.set_metadata(Constants::magic_for_list_entry);
                if (!it.has_next())
                {
                    it.set_metadata(Constants::magic_for_empty);
                    break;
                }
                LinkedListIt next = it.next(*this);
                it.set_metadata(Constants::magic_for_empty);
                block.set_metadata(Constants::magic_for_reserved);
                it = next;
                parent_block = free_block.second;
                free_block = find_free_index(free_block.second);
                if (!free_block.first)
                {
                    grow();
                    return emplace(std::move(new_value));
                }
            }
            AllocatorTraits::construct(*this, std::addressof(*block), std::move(new_value));
            block.set_metadata(Constants::magic_for_direct_hit);
            ++num_elements;
            return { block.it(), true };
        }
    }

    template<typename... Args>
    SKA_NOINLINE(std::pair<iterator, bool>) emplace_new_key(LinkedListIt parent, Args &&... args)
    {
        if (is_full())
        {
            grow();
            return emplace(std::forward<Args>(args)...);
        }
        std::pair<int8_t, LinkedListIt> free_block = find_free_index(parent);
        if (!free_block.first)
        {
            grow();
            return emplace(std::forward<Args>(args)...);
        }
        AllocatorTraits::construct(*this, std::addressof(*free_block.second), std::forward<Args>(args)...);
        free_block.second.set_metadata(Constants::magic_for_list_entry);
        parent.set_next(free_block.first);
        ++num_elements;
        return { free_block.second.it(), true };
    }

    LinkedListIt find_direct_hit(LinkedListIt child) const
    {
        size_t to_move_hash = hash_object(*child);
        size_t to_move_index = hash_policy.index_for_hash(to_move_hash, num_slots_minus_one);
        return { to_move_index, entries + to_move_index / BlockSize };
    }
    LinkedListIt find_parent_block(LinkedListIt child)
    {
        LinkedListIt parent_block = find_direct_hit(child);
        for (;;)
        {
            LinkedListIt next = parent_block.next(*this);
            if (next == child)
                return parent_block;
            parent_block = next;
        }
    }

    std::pair<int8_t, LinkedListIt> find_free_index(LinkedListIt parent) const
    {
        for (int8_t jump_index = 1; jump_index < Constants::num_jump_distances; ++jump_index)
        {
            size_t index = hash_policy.keep_in_range(parent.index + Constants::jump_distances[jump_index], num_slots_minus_one);
            BlockPointer block = entries + index / BlockSize;
            if (block->control_bytes[index % BlockSize] == Constants::magic_for_empty)
                return { jump_index, { index, block } };
        }
        return { 0, {} };
    }

    void grow()
    {
        rehash(std::max(size_t(10), 2 * bucket_count()));
    }

    size_t calculate_memory_requirement(size_t num_blocks)
    {
        size_t memory_required = sizeof(BlockType) * num_blocks;
        memory_required += BlockSize; // for metadata of past-the-end pointer
        return memory_required;
    }

    void deallocate_data(BlockPointer begin, size_t num_slots_minus_one)
    {
        if (begin == BlockType::empty_block())
            return;

        ++num_slots_minus_one;
        size_t num_blocks = num_slots_minus_one / BlockSize;
        if (num_slots_minus_one % BlockSize)
            ++num_blocks;
        size_t memory = calculate_memory_requirement(num_blocks);
        unsigned char * as_byte_pointer = reinterpret_cast<unsigned char *>(begin);
        AllocatorTraits::deallocate(*this, typename AllocatorTraits::pointer(as_byte_pointer), memory);
    }

    void reset_to_empty_state()
    {
        deallocate_data(entries, num_slots_minus_one);
        entries = BlockType::empty_block();
        num_slots_minus_one = 0;
        hash_policy.reset();
    }

    template<typename U>
    size_t hash_object(const U & key)
    {
        return static_cast<Hasher &>(*this)(key);
    }
    template<typename U>
    size_t hash_object(const U & key) const
    {
        return static_cast<const Hasher &>(*this)(key);
    }
    template<typename L, typename R>
    bool compares_equal(const L & lhs, const R & rhs)
    {
        return static_cast<Equal &>(*this)(lhs, rhs);
    }

    struct convertible_to_iterator
    {
        BlockPointer it;
        size_t index;

        operator iterator()
        {
            if (it->control_bytes[index % BlockSize] == Constants::magic_for_empty)
                return ++iterator{it, index};
            else
                return { it, index };
        }
        operator const_iterator()
        {
            if (it->control_bytes[index % BlockSize] == Constants::magic_for_empty)
                return ++iterator{it, index};
            else
                return { it, index };
        }
    };
};
template<typename T, typename Enable = void>
struct AlignmentOr8Bytes
{
    static constexpr size_t value = 8;
};
template<typename T>
struct AlignmentOr8Bytes<T, typename std::enable_if<alignof(T) >= 1>::type>
{
    static constexpr size_t value = alignof(T);
};
template<typename... Args>
struct CalculateBytellBlockSize;
template<typename First, typename... More>
struct CalculateBytellBlockSize<First, More...>
{
    static constexpr size_t this_value = AlignmentOr8Bytes<First>::value;
    static constexpr size_t base_value = CalculateBytellBlockSize<More...>::value;
    static constexpr size_t value = this_value > base_value ? this_value : base_value;
};
template<>
struct CalculateBytellBlockSize<>
{
    static constexpr size_t value = 8;
};
}

template<typename K, typename V, typename H = std::hash<K>, typename E = std::equal_to<K>, typename A = std::allocator<std::pair<K, V> > >
class bytell_hash_map
        : public detailv8::sherwood_v8_table
        <
            std::pair<K, V>,
            K,
            H,
            detailv8::KeyOrValueHasher<K, std::pair<K, V>, H>,
            E,
            detailv8::KeyOrValueEquality<K, std::pair<K, V>, E>,
            A,
            typename std::allocator_traits<A>::template rebind_alloc<unsigned char>,
            detailv8::CalculateBytellBlockSize<K, V>::value
        >
{
    using Table = detailv8::sherwood_v8_table
    <
        std::pair<K, V>,
        K,
        H,
        detailv8::KeyOrValueHasher<K, std::pair<K, V>, H>,
        E,
        detailv8::KeyOrValueEquality<K, std::pair<K, V>, E>,
        A,
        typename std::allocator_traits<A>::template rebind_alloc<unsigned char>,
        detailv8::CalculateBytellBlockSize<K, V>::value
    >;
public:

    using key_type = K;
    using mapped_type = V;

    using Table::Table;
    bytell_hash_map()
    {
    }

    inline V & operator[](const K & key)
    {
        return emplace(key, convertible_to_value()).first->second;
    }
    inline V & operator[](K && key)
    {
        return emplace(std::move(key), convertible_to_value()).first->second;
    }
    V & at(const K & key)
    {
        auto found = this->find(key);
        if (found == this->end())
            throw std::out_of_range("Argument passed to at() was not in the map.");
        return found->second;
    }
    const V & at(const K & key) const
    {
        auto found = this->find(key);
        if (found == this->end())
            throw std::out_of_range("Argument passed to at() was not in the map.");
        return found->second;
    }

    using Table::emplace;
    std::pair<typename Table::iterator, bool> emplace()
    {
        return emplace(key_type(), convertible_to_value());
    }
    template<typename M>
    std::pair<typename Table::iterator, bool> insert_or_assign(const key_type & key, M && m)
    {
        auto emplace_result = emplace(key, std::forward<M>(m));
        if (!emplace_result.second)
            emplace_result.first->second = std::forward<M>(m);
        return emplace_result;
    }
    template<typename M>
    std::pair<typename Table::iterator, bool> insert_or_assign(key_type && key, M && m)
    {
        auto emplace_result = emplace(std::move(key), std::forward<M>(m));
        if (!emplace_result.second)
            emplace_result.first->second = std::forward<M>(m);
        return emplace_result;
    }
    template<typename M>
    typename Table::iterator insert_or_assign(typename Table::const_iterator, const key_type & key, M && m)
    {
        return insert_or_assign(key, std::forward<M>(m)).first;
    }
    template<typename M>
    typename Table::iterator insert_or_assign(typename Table::const_iterator, key_type && key, M && m)
    {
        return insert_or_assign(std::move(key), std::forward<M>(m)).first;
    }

    friend bool operator==(const bytell_hash_map & lhs, const bytell_hash_map & rhs)
    {
        if (lhs.size() != rhs.size())
            return false;
        for (const typename Table::value_type & value : lhs)
        {
            auto found = rhs.find(value.first);
            if (found == rhs.end())
                return false;
            else if (value.second != found->second)
                return false;
        }
        return true;
    }
    friend bool operator!=(const bytell_hash_map & lhs, const bytell_hash_map & rhs)
    {
        return !(lhs == rhs);
    }

private:
    struct convertible_to_value
    {
        operator V() const
        {
            return V();
        }
    };
};

template<typename T, typename H = std::hash<T>, typename E = std::equal_to<T>, typename A = std::allocator<T> >
class bytell_hash_set
        : public detailv8::sherwood_v8_table
        <
            T,
            T,
            H,
            detailv8::functor_storage<size_t, H>,
            E,
            detailv8::functor_storage<bool, E>,
            A,
            typename std::allocator_traits<A>::template rebind_alloc<unsigned char>,
            detailv8::CalculateBytellBlockSize<T>::value
        >
{
    using Table = detailv8::sherwood_v8_table
    <
        T,
        T,
        H,
        detailv8::functor_storage<size_t, H>,
        E,
        detailv8::functor_storage<bool, E>,
        A,
        typename std::allocator_traits<A>::template rebind_alloc<unsigned char>,
        detailv8::CalculateBytellBlockSize<T>::value
    >;
public:

    using key_type = T;

    using Table::Table;
    bytell_hash_set()
    {
    }

    template<typename... Args>
    std::pair<typename Table::iterator, bool> emplace(Args &&... args)
    {
        return Table::emplace(T(std::forward<Args>(args)...));
    }
    std::pair<typename Table::iterator, bool> emplace(const key_type & arg)
    {
        return Table::emplace(arg);
    }
    std::pair<typename Table::iterator, bool> emplace(key_type & arg)
    {
        return Table::emplace(arg);
    }
    std::pair<typename Table::iterator, bool> emplace(const key_type && arg)
    {
        return Table::emplace(std::move(arg));
    }
    std::pair<typename Table::iterator, bool> emplace(key_type && arg)
    {
        return Table::emplace(std::move(arg));
    }

    friend bool operator==(const bytell_hash_set & lhs, const bytell_hash_set & rhs)
    {
        if (lhs.size() != rhs.size())
            return false;
        for (const T & value : lhs)
        {
            if (rhs.find(value) == rhs.end())
                return false;
        }
        return true;
    }
    friend bool operator!=(const bytell_hash_set & lhs, const bytell_hash_set & rhs)
    {
        return !(lhs == rhs);
    }
};

} // end namespace ska


================================================
FILE: ohmutil/3rdparty/ska/flat_hash_map.hpp
================================================
//          Copyright Malte Skarupke 2017.
// Distributed under the Boost Software License, Version 1.0.
//    (See http://www.boost.org/LICENSE_1_0.txt)

#pragma once

#include <cstdint>
#include <cstddef>
#include <functional>
#include <cmath>
#include <algorithm>
#include <iterator>
#include <utility>
#include <type_traits>

#ifdef _MSC_VER
#define SKA_NOINLINE(...) __declspec(noinline) __VA_ARGS__
#else
#define SKA_NOINLINE(...) __VA_ARGS__ __attribute__((noinline))
#endif

namespace ska
{
struct prime_number_hash_policy;
struct power_of_two_hash_policy;
struct fibonacci_hash_policy;

namespace detailv3
{
template<typename Result, typename Functor>
struct functor_storage : Functor
{
    functor_storage() = default;
    functor_storage(const Functor & functor)
        : Functor(functor)
    {
    }
    template<typename... Args>
    Result operator()(Args &&... args)
    {
        return static_cast<Functor &>(*this)(std::forward<Args>(args)...);
    }
    template<typename... Args>
    Result operator()(Args &&... args) const
    {
        return static_cast<const Functor &>(*this)(std::forward<Args>(args)...);
    }
};
template<typename Result, typename... Args>
struct functor_storage<Result, Result (*)(Args...)>
{
    typedef Result (*function_ptr)(Args...);
    function_ptr function;
    functor_storage(function_ptr function)
        : function(function)
    {
    }
    Result operator()(Args... args) const
    {
        return function(std::forward<Args>(args)...);
    }
    operator function_ptr &()
    {
        return function;
    }
    operator const function_ptr &()
    {
        return function;
    }
};
template<typename key_type, typename value_type, typename hasher>
struct KeyOrValueHasher : functor_storage<size_t, hasher>
{
    typedef functor_storage<size_t, hasher> hasher_storage;
    KeyOrValueHasher() = default;
    KeyOrValueHasher(const hasher & hash)
        : hasher_storage(hash)
    {
    }
    size_t operator()(const key_type & key)
    {
        return static_cast<hasher_storage &>(*this)(key);
    }
    size_t operator()(const key_type & key) const
    {
        return static_cast<const hasher_storage &>(*this)(key);
    }
    size_t operator()(const value_type & value)
    {
        return static_cast<hasher_storage &>(*this)(value.first);
    }
    size_t operator()(const value_type & value) const
    {
        return static_cast<const hasher_storage &>(*this)(value.first);
    }
    template<typename F, typename S>
    size_t operator()(const std::pair<F, S> & value)
    {
        return static_cast<hasher_storage &>(*this)(value.first);
    }
    template<typename F, typename S>
    size_t operator()(const std::pair<F, S> & value) const
    {
        return static_cast<const hasher_storage &>(*this)(value.first);
    }
};
template<typename key_type, typename value_type, typename key_equal>
struct KeyOrValueEquality : functor_storage<bool, key_equal>
{
    typedef functor_storage<bool, key_equal> equality_storage;
    KeyOrValueEquality() = default;
    KeyOrValueEquality(const key_equal & equality)
        : equality_storage(equality)
    {
    }
    bool operator()(const key_type & lhs, const key_type & rhs)
    {
        return static_cast<equality_storage &>(*this)(lhs, rhs);
    }
    bool operator()(const key_type & lhs, const value_type & rhs)
    {
        return static_cast<equality_storage &>(*this)(lhs, rhs.first);
    }
    bool operator()(const value_type & lhs, const key_type & rhs)
    {
        return static_cast<equality_storage &>(*this)(lhs.first, rhs);
    }
    bool operator()(const value_type & lhs, const value_type & rhs)
    {
        return static_cast<equality_storage &>(*this)(lhs.first, rhs.first);
    }
    template<typename F, typename S>
    bool operator()(const key_type & lhs, const std::pair<F, S> & rhs)
    {
        return static_cast<equality_storage &>(*this)(lhs, rhs.first);
    }
    template<typename F, typename S>
    bool operator()(const std::pair<F, S> & lhs, const key_type & rhs)
    {
        return static_cast<equality_storage &>(*this)(lhs.first, rhs);
    }
    template<typename F, typename S>
    bool operator()(const value_type & lhs, const std::pair<F, S> & rhs)
    {
        return static_cast<equality_storage &>(*this)(lhs.first, rhs.first);
    }
    template<typename F, typename S>
    bool operator()(const std::pair<F, S> & lhs, const value_type & rhs)
    {
        return static_cast<equality_storage &>(*this)(lhs.first, rhs.first);
    }
    template<typename FL, typename SL, typename FR, typename SR>
    bool operator()(const std::pair<FL, SL> & lhs, const std::pair<FR, SR> & rhs)
    {
        return static_cast<equality_storage &>(*this)(lhs.first, rhs.first);
    }
};
static constexpr int8_t min_lookups = 4;
template<typename T>
struct sherwood_v3_entry
{
    sherwood_v3_entry()
    {
    }
    sherwood_v3_entry(int8_t distance_from_desired)
        : distance_from_desired(distance_from_desired)
    {
    }
    ~sherwood_v3_entry()
    {
    }
    static sherwood_v3_entry * empty_default_table()
    {
        static sherwood_v3_entry result[min_lookups] = { {}, {}, {}, {special_end_value} };
        return result;
    }

    bool has_value() const
    {
        return distance_from_desired >= 0;
    }
    bool is_empty() const
    {
        return distance_from_desired < 0;
    }
    bool is_at_desired_position() const
    {
        return distance_from_desired <= 0;
    }
    template<typename... Args>
    void emplace(int8_t distance, Args &&... args)
    {
        new (std::addressof(value)) T(std::forward<Args>(args)...);
        distance_from_desired = distance;
    }

    void destroy_value()
    {
        value.~T();
        distance_from_desired = -1;
    }

    int8_t distance_from_desired = -1;
    static constexpr int8_t special_end_value = 0;
    union { T value; };
};

inline int8_t log2(size_t value)
{
    static constexpr int8_t table[64] =
    {
        63,  0, 58,  1, 59, 47, 53,  2,
        60, 39, 48, 27, 54, 33, 42,  3,
        61, 51, 37, 40, 49, 18, 28, 20,
        55, 30, 34, 11, 43, 14, 22,  4,
        62, 57, 46, 52, 38, 26, 32, 41,
        50, 36, 17, 19, 29, 10, 13, 21,
        56, 45, 25, 31, 35, 16,  9, 12,
        44, 24, 15,  8, 23,  7,  6,  5
    };
    value |= value >> 1;
    value |= value >> 2;
    value |= value >> 4;
    value |= value >> 8;
    value |= value >> 16;
    value |= value >> 32;
    return table[((value - (value >> 1)) * 0x07EDD5E59A4E28C2) >> 58];
}

template<typename T, bool>
struct AssignIfTrue
{
    void operator()(T & lhs, const T & rhs)
    {
        lhs = rhs;
    }
    void operator()(T & lhs, T && rhs)
    {
        lhs = std::move(rhs);
    }
};
template<typename T>
struct AssignIfTrue<T, false>
{
    void operator()(T &, const T &)
    {
    }
    void operator()(T &, T &&)
    {
    }
};

inline size_t next_power_of_two(size_t i)
{
    --i;
    i |= i >> 1;
    i |= i >> 2;
    i |= i >> 4;
    i |= i >> 8;
    i |= i >> 16;
    i |= i >> 32;
    ++i;
    return i;
}

template<typename...> using void_t = void;

template<typename T, typename = void>
struct HashPolicySelector
{
    typedef fibonacci_hash_policy type;
};
template<typename T>
struct HashPolicySelector<T, void_t<typename T::hash_policy>>
{
    typedef typename T::hash_policy type;
};

template<typename T, typename FindKey, typename ArgumentHash, typename Hasher, typename ArgumentEqual, typename Equal, typename ArgumentAlloc, typename EntryAlloc>
class sherwood_v3_table : private EntryAlloc, private Hasher, private Equal
{
    using Entry = detailv3::sherwood_v3_entry<T>;
    using AllocatorTraits = std::allocator_traits<EntryAlloc>;
    using EntryPointer = typename AllocatorTraits::pointer;
    struct convertible_to_iterator;

public:

    using value_type = T;
    using size_type = size_t;
    using difference_type = std::ptrdiff_t;
    using hasher = ArgumentHash;
    using key_equal = ArgumentEqual;
    using allocator_type = EntryAlloc;
    using reference = value_type &;
    using const_reference = const value_type &;
    using pointer = value_type *;
    using const_pointer = const value_type *;

    sherwood_v3_table()
    {
    }
    explicit sherwood_v3_table(size_type bucket_count, const ArgumentHash & hash = ArgumentHash(), const ArgumentEqual & equal = ArgumentEqual(), const ArgumentAlloc & alloc = ArgumentAlloc())
        : EntryAlloc(alloc), Hasher(hash), Equal(equal)
    {
        rehash(bucket_count);
    }
    sherwood_v3_table(size_type bucket_count, const ArgumentAlloc & alloc)
        : sherwood_v3_table(bucket_count, ArgumentHash(), ArgumentEqual(), alloc)
    {
    }
    sherwood_v3_table(size_type bucket_count, const ArgumentHash & hash, const ArgumentAlloc & alloc)
        : sherwood_v3_table(bucket_count, hash, ArgumentEqual(), alloc)
    {
    }
    explicit sherwood_v3_table(const ArgumentAlloc & alloc)
        : EntryAlloc(alloc)
    {
    }
    template<typename It>
    sherwood_v3_table(It first, It last, size_type bucket_count = 0, const ArgumentHash & hash = ArgumentHash(), const ArgumentEqual & equal = ArgumentEqual(), const ArgumentAlloc & alloc = ArgumentAlloc())
        : sherwood_v3_table(bucket_count, hash, equal, alloc)
    {
        insert(first, last);
    }
    template<typename It>
    sherwood_v3_table(It first, It last, size_type bucket_count, const ArgumentAlloc & alloc)
        : sherwood_v3_table(first, last, bucket_count, ArgumentHash(), ArgumentEqual(), alloc)
    {
    }
    template<typename It>
    sherwood_v3_table(It first, It last, size_type bucket_count, const ArgumentHash & hash, const ArgumentAlloc & alloc)
        : sherwood_v3_table(first, last, bucket_count, hash, ArgumentEqual(), alloc)
    {
    }
    sherwood_v3_table(std::initializer_list<T> il, size_type bucket_count = 0, const ArgumentHash & hash = ArgumentHash(), const ArgumentEqual & equal = ArgumentEqual(), const ArgumentAlloc & alloc = ArgumentAlloc())
        : sherwood_v3_table(bucket_count, hash, equal, alloc)
    {
        if (bucket_count == 0)
            rehash(il.size());
        insert(il.begin(), il.end());
    }
    sherwood_v3_table(std::initializer_list<T> il, size_type bucket_count, const ArgumentAlloc & alloc)
        : sherwood_v3_table(il, bucket_count, ArgumentHash(), ArgumentEqual(), alloc)
    {
    }
    sherwood_v3_table(std::initializer_list<T> il, size_type bucket_count, const ArgumentHash & hash, const ArgumentAlloc & alloc)
        : sherwood_v3_table(il, bucket_count, hash, ArgumentEqual(), alloc)
    {
    }
    sherwood_v3_table(const sherwood_v3_table & other)
        : sherwood_v3_table(other, AllocatorTraits::select_on_container_copy_construction(other.get_allocator()))
    {
    }
    sherwood_v3_table(const sherwood_v3_table & other, const ArgumentAlloc & alloc)
        : EntryAlloc(alloc), Hasher(other), Equal(other), _max_load_factor(other._max_load_factor)
    {
        rehash_for_other_container(other);
        try
        {
            insert(other.begin(), other.end());
        }
        catch(...)
        {
            clear();
            deallocate_data(entries, num_slots_minus_one, max_lookups);
            throw;
        }
    }
    sherwood_v3_table(sherwood_v3_table && other) noexcept
        : EntryAlloc(std::move(other)), Hasher(std::move(other)), Equal(std::move(other))
    {
        swap_pointers(other);
    }
    sherwood_v3_table(sherwood_v3_table && other, const ArgumentAlloc & alloc) noexcept
        : EntryAlloc(alloc), Hasher(std::move(other)), Equal(std::move(other))
    {
        swap_pointers(other);
    }
    sherwood_v3_table & operator=(const sherwood_v3_table & other)
    {
        if (this == std::addressof(other))
            return *this;

        clear();
        if (AllocatorTraits::propagate_on_container_copy_assignment::value)
        {
            if (static_cast<EntryAlloc &>(*this) != static_cast<const EntryAlloc &>(other))
            {
                reset_to_empty_state();
            }
            AssignIfTrue<EntryAlloc, AllocatorTraits::propagate_on_container_copy_assignment::value>()(*this, other);
        }
        _max_load_factor = other._max_load_factor;
        static_cast<Hasher &>(*this) = other;
        static_cast<Equal &>(*this) = other;
        rehash_for_other_container(other);
        insert(other.begin(), other.end());
        return *this;
    }
    sherwood_v3_table & operator=(sherwood_v3_table && other) noexcept
    {
        if (this == std::addressof(other))
            return *this;
        else if (AllocatorTraits::propagate_on_container_move_assignment::value)
        {
            clear();
            reset_to_empty_state();
            AssignIfTrue<EntryAlloc, AllocatorTraits::propagate_on_container_move_assignment::value>()(*this, std::move(other));
            swap_pointers(other);
        }
        else if (static_cast<EntryAlloc &>(*this) == static_cast<EntryAlloc &>(other))
        {
            swap_pointers(other);
        }
        else
        {
            clear();
            _max_load_factor = other._max_load_factor;
            rehash_for_other_container(other);
            for (T & elem : other)
                emplace(std::move(elem));
            other.clear();
        }
        static_cast<Hasher &>(*this) = std::move(other);
        static_cast<Equal &>(*this) = std::move(other);
        return *this;
    }
    ~sherwood_v3_table()
    {
        clear();
        deallocate_data(entries, num_slots_minus_one, max_lookups);
    }

    const allocator_type & get_allocator() const
    {
        return static_cast<const allocator_type &>(*this);
    }
    const ArgumentEqual & key_eq() const
    {
        return static_cast<const ArgumentEqual &>(*this);
    }
    const ArgumentHash & hash_function() const
    {
        return static_cast<const ArgumentHash &>(*this);
    }

    template<typename ValueType>
    struct templated_iterator
    {
        templated_iterator() = default;
        templated_iterator(EntryPointer current)
            : current(current)
        {
        }
        EntryPointer current = EntryPointer();

        using iterator_category = std::forward_iterator_tag;
        using value_type = ValueType;
        using difference_type = ptrdiff_t;
        using pointer = ValueType *;
        using reference = ValueType &;

        friend bool operator==(const templated_iterator & lhs, const templated_iterator & rhs)
        {
            return lhs.current == rhs.current;
        }
        friend bool operator!=(const templated_iterator & lhs, const templated_iterator & rhs)
        {
            return !(lhs == rhs);
        }

        templated_iterator & operator++()
        {
            do
            {
                ++current;
            }
            while(current->is_empty());
            return *this;
        }
        templated_iterator operator++(int)
        {
            templated_iterator copy(*this);
            ++*this;
            return copy;
        }

        ValueType & operator*() const
        {
            return current->value;
        }
        ValueType * operator->() const
        {
            return std::addressof(current->value);
        }

        operator templated_iterator<const value_type>() const
        {
            return { current };
        }
    };
    using iterator = templated_iterator<value_type>;
    using const_iterator = templated_iterator<const value_type>;

    iterator begin()
    {
        for (EntryPointer it = entries;; ++it)
        {
            if (it->has_value())
                return { it };
        }
    }
    const_iterator begin() const
    {
        for (EntryPointer it = entries;; ++it)
        {
            if (it->has_value())
                return { it };
        }
    }
    const_iterator cbegin() const
    {
        return begin();
    }
    iterator end()
    {
        return { entries + static_cast<ptrdiff_t>(num_slots_minus_one + max_lookups) };
    }
    const_iterator end() const
    {
        return { entries + static_cast<ptrdiff_t>(num_slots_minus_one + max_lookups) };
    }
    const_iterator cend() const
    {
        return end();
    }

    iterator find(const FindKey & key)
    {
        size_t index = hash_policy.index_for_hash(hash_object(key), num_slots_minus_one);
        EntryPointer it = entries + ptrdiff_t(index);
        for (int8_t distance = 0; it->distance_from_desired >= distance; ++distance, ++it)
        {
            if (compares_equal(key, it->value))
                return { it };
        }
        return end();
    }
    const_iterator find(const FindKey & key) const
    {
        return const_cast<sherwood_v3_table *>(this)->find(key);
    }
    size_t count(const FindKey & key) const
    {
        return find(key) == end() ? 0 : 1;
    }
    std::pair<iterator, iterator> equal_range(const FindKey & key)
    {
        iterator found = find(key);
        if (found == end())
            return { found, found };
        else
            return { found, std::next(found) };
    }
    std::pair<const_iterator, const_iterator> equal_range(const FindKey & key) const
    {
        const_iterator found = find(key);
        if (found == end())
            return { found, found };
        else
            return { found, std::next(found) };
    }

    template<typename Key, typename... Args>
    std::pair<iterator, bool> emplace(Key && key, Args &&... args)
    {
        size_t index = hash_policy.index_for_hash(hash_object(key), num_slots_minus_one);
        EntryPointer current_entry = entries + ptrdiff_t(index);
        int8_t distance_from_desired = 0;
        for (; current_entry->distance_from_desired >= distance_from_desired; ++current_entry, ++distance_from_desired)
        {
            if (compares_equal(key, current_entry->value))
                return { { current_entry }, false };
        }
        return emplace_new_key(distance_from_desired, current_entry, std::forward<Key>(key), std::forward<Args>(args)...);
    }

    std::pair<iterator, bool> insert(const value_type & value)
    {
        return emplace(value);
    }
    std::pair<iterator, bool> insert(value_type && value)
    {
        return emplace(std::move(value));
    }
    template<typename... Args>
    iterator emplace_hint(const_iterator, Args &&... args)
    {
        return emplace(std::forward<Args>(args)...).first;
    }
    iterator insert(const_iterator, const value_type & value)
    {
        return emplace(value).first;
    }
    iterator insert(const_iterator, value_type && value)
    {
        return emplace(std::move(value)).first;
    }

    template<typename It>
    void insert(It begin, It end)
    {
        for (; begin != end; ++begin)
        {
            emplace(*begin);
        }
    }
    void insert(std::initializer_list<value_type> il)
    {
        insert(il.begin(), il.end());
    }

    void rehash(size_t num_buckets)
    {
        num_buckets = std::max(num_buckets, static_cast<size_t>(std::ceil(num_elements / static_cast<double>(_max_load_factor))));
        if (num_buckets == 0)
        {
            reset_to_empty_state();
            return;
        }
        auto new_prime_index = hash_policy.next_size_over(num_buckets);
        if (num_buckets == bucket_count())
            return;
        int8_t new_max_lookups = compute_max_lookups(num_buckets);
        EntryPointer new_buckets(AllocatorTraits::allocate(*this, num_buckets + new_max_lookups));
        EntryPointer special_end_item = new_buckets + static_cast<ptrdiff_t>(num_buckets + new_max_lookups - 1);
        for (EntryPointer it = new_buckets; it != special_end_item; ++it)
            it->distance_from_desired = -1;
        special_end_item->distance_from_desired = Entry::special_end_value;
        std::swap(entries, new_buckets);
        std::swap(num_slots_minus_one, num_buckets);
        --num_slots_minus_one;
        hash_policy.commit(new_prime_index);
        int8_t old_max_lookups = max_lookups;
        max_lookups = new_max_lookups;
        num_elements = 0;
        for (EntryPointer it = new_buckets, end = it + static_cast<ptrdiff_t>(num_buckets + old_max_lookups); it != end; ++it)
        {
            if (it->has_value())
            {
                emplace(std::move(it->value));
                it->destroy_value();
            }
        }
        deallocate_data(new_buckets, num_buckets, old_max_lookups);
    }

    void reserve(size_t num_elements)
    {
        size_t required_buckets = num_buckets_for_reserve(num_elements);
        if (required_buckets > bucket_count())
            rehash(required_buckets);
    }

    // the return value is a type that can be converted to an iterator
    // the reason for doing this is that it's not free to find the
    // iterator pointing at the next element. if you care about the
    // next iterator, turn the return value into an iterator
    convertible_to_iterator erase(const_iterator to_erase)
    {
        EntryPointer current = to_erase.current;
        current->destroy_value();
        --num_elements;
        for (EntryPointer next = current + ptrdiff_t(1); !next->is_at_desired_position(); ++current, ++next)
        {
            current->emplace(next->distance_from_desired - 1, std::move(next->value));
            next->destroy_value();
        }
        return { to_erase.current };
    }

    iterator erase(const_iterator begin_it, const_iterator end_it)
    {
        if (begin_it == end_it)
            return { begin_it.current };
        for (EntryPointer it = begin_it.current, end = end_it.current; it != end; ++it)
        {
            if (it->has_value())
            {
                it->destroy_value();
                --num_elements;
            }
        }
        if (end_it == this->end())
            return this->end();
        ptrdiff_t num_to_move = std::min(static_cast<ptrdiff_t>(end_it.current->distance_from_desired), end_it.current - begin_it.current);
        EntryPointer to_return = end_it.current - num_to_move;
        for (EntryPointer it = end_it.current; !it->is_at_desired_position();)
        {
            EntryPointer target = it - num_to_move;
            target->emplace(it->distance_from_desired - num_to_move, std::move(it->value));
            it->destroy_value();
            ++it;
            num_to_move = std::min(static_cast<ptrdiff_t>(it->distance_from_desired), num_to_move);
        }
        return { to_return };
    }

    size_t erase(const FindKey & key)
    {
        auto found = find(key);
        if (found == end())
            return 0;
        else
        {
            erase(found);
            return 1;
        }
    }

    void clear()
    {
        for (EntryPointer it = entries, end = it + static_cast<ptrdiff_t>(num_slots_minus_one + max_lookups); it != end; ++it)
        {
            if (it->has_value())
                it->destroy_value();
        }
        num_elements = 0;
    }

    void shrink_to_fit()
    {
        rehash_for_other_container(*this);
    }

    void swap(sherwood_v3_table & other)
    {
        using std::swap;
        swap_pointers(other);
        swap(static_cast<ArgumentHash &>(*this), static_cast<ArgumentHash &>(other));
        swap(static_cast<ArgumentEqual &>(*this), static_cast<ArgumentEqual &>(other));
        if (AllocatorTraits::propagate_on_container_swap::value)
            swap(static_cast<EntryAlloc &>(*this), static_cast<EntryAlloc &>(other));
    }

    size_t size() const
    {
        return num_elements;
    }
    size_t max_size() const
    {
        return (AllocatorTraits::max_size(*this)) / sizeof(Entry);
    }
    size_t bucket_count() const
    {
        return num_slots_minus_one ? num_slots_minus_one + 1 : 0;
    }
    size_type max_bucket_count() const
    {
        return (AllocatorTraits::max_size(*this) - min_lookups) / sizeof(Entry);
    }
    size_t bucket(const FindKey & key) const
    {
        return hash_policy.index_for_hash(hash_object(key), num_slots_minus_one);
    }
    float load_factor() const
    {
        size_t buckets = bucket_count();
        if (buckets)
            return static_cast<float>(num_elements) / bucket_count();
        else
            return 0;
    }
    void max_load_factor(float value)
    {
        _max_load_factor = value;
    }
    float max_load_factor() const
    {
        return _max_load_factor;
    }

    bool empty() const
    {
        return num_elements == 0;
    }

private:
    EntryPointer entries = Entry::empty_default_table();
    size_t num_slots_minus_one = 0;
    typename HashPolicySelector<ArgumentHash>::type hash_policy;
    int8_t max_lookups = detailv3::min_lookups - 1;
    float _max_load_factor = 0.5f;
    size_t num_elements = 0;

    static int8_t compute_max_lookups(size_t num_buckets)
    {
        int8_t desired = detailv3::log2(num_buckets);
        return std::max(detailv3::min_lookups, desired);
    }

    size_t num_buckets_for_reserve(size_t num_elements) const
    {
        return static_cast<size_t>(std::ceil(num_elements / std::min(0.5, static_cast<double>(_max_load_factor))));
    }
    void rehash_for_other_container(const sherwood_v3_table & other)
    {
        rehash(std::min(num_buckets_for_reserve(other.size()), other.bucket_count()));
    }

    void swap_pointers(sherwood_v3_table & other)
    {
        using std::swap;
        swap(hash_policy, other.hash_policy);
        swap(entries, other.entries);
        swap(num_slots_minus_one, other.num_slots_minus_one);
        swap(num_elements, other.num_elements);
        swap(max_lookups, other.max_lookups);
        swap(_max_load_factor, other._max_load_factor);
    }

    template<typename Key, typename... Args>
    SKA_NOINLINE(std::pair<iterator, bool>) emplace_new_key(int8_t distance_from_desired, EntryPointer current_entry, Key && key, Args &&... args)
    {
        using std::swap;
        if (num_slots_minus_one == 0 || distance_from_desired == max_lookups || num_elements + 1 > (num_slots_minus_one + 1) * static_cast<double>(_max_load_factor))
        {
            grow();
            return emplace(std::forward<Key>(key), std::forward<Args>(args)...);
        }
        else if (current_entry->is_empty())
        {
            current_entry->emplace(distance_from_desired, std::forward<Key>(key), std::forward<Args>(args)...);
            ++num_elements;
            return { { current_entry }, true };
        }
        value_type to_insert(std::forward<Key>(key), std::forward<Args>(args)...);
        swap(distance_from_desired, current_entry->distance_from_desired);
        swap(to_insert, current_entry->value);
        iterator result = { current_entry };
        for (++distance_from_desired, ++current_entry;; ++current_entry)
        {
            if (current_entry->is_empty())
            {
                current_entry->emplace(distance_from_desired, std::move(to_insert));
                ++num_elements;
                return { result, true };
            }
            else if (current_entry->distance_from_desired < distance_from_desired)
            {
                swap(distance_from_desired, current_entry->distance_from_desired);
                swap(to_insert, current_entry->value);
                ++distance_from_desired;
            }
            else
            {
                ++distance_from_desired;
                if (distance_from_desired == max_lookups)
                {
                    swap(to_insert, result.current->value);
                    grow();
                    return emplace(std::move(to_insert));
                }
            }
        }
    }

    void grow()
    {
        rehash(std::max(size_t(4), 2 * bucket_count()));
    }

    void deallocate_data(EntryPointer begin, size_t num_slots_minus_one, int8_t max_lookups)
    {
        if (begin != Entry::empty_default_table())
        {
            AllocatorTraits::deallocate(*this, begin, num_slots_minus_one + max_lookups + 1);
        }
    }

    void reset_to_empty_state()
    {
        deallocate_data(entries, num_slots_minus_one, max_lookups);
        entries = Entry::empty_default_table();
        num_slots_minus_one = 0;
        hash_policy.reset();
        max_lookups = detailv3::min_lookups - 1;
    }

    template<typename U>
    size_t hash_object(const U & key)
    {
        return static_cast<Hasher &>(*this)(key);
    }
    template<typename U>
    size_t hash_object(const U & key) const
    {
        return static_cast<const Hasher &>(*this)(key);
    }
    template<typename L, typename R>
    bool compares_equal(const L & lhs, const R & rhs)
    {
        return static_cast<Equal &>(*this)(lhs, rhs);
    }

    struct convertible_to_iterator
    {
        EntryPointer it;

        operator iterator()
        {
            if (it->has_value())
                return { it };
            else
                return ++iterator{it};
        }
        operator const_iterator()
        {
            if (it->has_value())
                return { it };
            else
                return ++const_iterator{it};
        }
    };

};
}

struct prime_number_hash_policy
{
    static size_t mod0(size_t) { return 0llu; }
    static size_t mod2(size_t hash) { return hash % 2llu; }
    static size_t mod3(size_t hash) { return hash % 3llu; }
    static size_t mod5(size_t hash) { return hash % 5llu; }
    static size_t mod7(size_t hash) { return hash % 7llu; }
    static size_t mod11(size_t hash) { return hash % 11llu; }
    static size_t mod13(size_t hash) { return hash % 13llu; }
    static size_t mod17(size_t hash) { return hash % 17llu; }
    static size_t mod23(size_t hash) { return hash % 23llu; }
    static size_t mod29(size_t hash) { return hash % 29llu; }
    static size_t mod37(size_t hash) { return hash % 37llu; }
    static size_t mod47(size_t hash) { return hash % 47llu; }
    static size_t mod59(size_t hash) { return hash % 59llu; }
    static size_t mod73(size_t hash) { return hash % 73llu; }
    static size_t mod97(size_t hash) { return hash % 97llu; }
    static size_t mod127(size_t hash) { return hash % 127llu; }
    static size_t mod151(size_t hash) { return hash % 151llu; }
    static size_t mod197(size_t hash) { return hash % 197llu; }
    static size_t mod251(size_t hash) { return hash % 251llu; }
    static size_t mod313(size_t hash) { return hash % 313llu; }
    static size_t mod397(size_t hash) { return hash % 397llu; }
    static size_t mod499(size_t hash) { return hash % 499llu; }
    static size_t mod631(size_t hash) { return hash % 631llu; }
    static size_t mod797(size_t hash) { return hash % 797llu; }
    static size_t mod1009(size_t hash) { return hash % 1009llu; }
    static size_t mod1259(size_t hash) { return hash % 1259llu; }
    static size_t mod1597(size_t hash) { return hash % 1597llu; }
    static size_t mod2011(size_t hash) { return hash % 2011llu; }
    static size_t mod2539(size_t hash) { return hash % 2539llu; }
    static size_t mod3203(size_t hash) { return hash % 3203llu; }
    static size_t mod4027(size_t hash) { return hash % 4027llu; }
    static size_t mod5087(size_t hash) { return hash % 5087llu; }
    static size_t mod6421(size_t hash) { return hash % 6421llu; }
    static size_t mod8089(size_t hash) { return hash % 8089llu; }
    static size_t mod10193(size_t hash) { return hash % 10193llu; }
    static size_t mod12853(size_t hash) { return hash % 12853llu; }
    static size_t mod16193(size_t hash) { return hash % 16193llu; }
    static size_t mod20399(size_t hash) { return hash % 20399llu; }
    static size_t mod25717(size_t hash) { return hash % 25717llu; }
    static size_t mod32401(size_t hash) { return hash % 32401llu; }
    static size_t mod40823(size_t hash) { return hash % 40823llu; }
    static size_t mod51437(size_t hash) { return hash % 51437llu; }
    static size_t mod64811(size_t hash) { return hash % 64811llu; }
    static size_t mod81649(size_t hash) { return hash % 81649llu; }
    static size_t mod102877(size_t hash) { return hash % 102877llu; }
    static size_t mod129607(size_t hash) { return hash % 129607llu; }
    static size_t mod163307(size_t hash) { return hash % 163307llu; }
    static size_t mod205759(size_t hash) { return hash % 205759llu; }
    static size_t mod259229(size_t hash) { return hash % 259229llu; }
    static size_t mod326617(size_t hash) { return hash % 326617llu; }
    static size_t mod411527(size_t hash) { return hash % 411527llu; }
    static size_t mod518509(size_t hash) { return hash % 518509llu; }
    static size_t mod653267(size_t hash) { return hash % 653267llu; }
    static size_t mod823117(size_t hash) { return hash % 823117llu; }
    static size_t mod1037059(size_t hash) { return hash % 1037059llu; }
    static size_t mod1306601(size_t hash) { return hash % 1306601llu; }
    static size_t mod1646237(size_t hash) { return hash % 1646237llu; }
    static size_t mod2074129(size_t hash) { return hash % 2074129llu; }
    static size_t mod2613229(size_t hash) { return hash % 2613229llu; }
    static size_t mod3292489(size_t hash) { return hash % 3292489llu; }
    static size_t mod4148279(size_t hash) { return hash % 4148279llu; }
    static size_t mod5226491(size_t hash) { return hash % 5226491llu; }
    static size_t mod6584983(size_t hash) { return hash % 6584983llu; }
    static size_t mod8296553(size_t hash) { return hash % 8296553llu; }
    static size_t mod10453007(size_t hash) { return hash % 10453007llu; }
    static size_t mod13169977(size_t hash) { return hash % 13169977llu; }
    static size_t mod16593127(size_t hash) { return hash % 16593127llu; }
    static size_t mod20906033(size_t hash) { return hash % 20906033llu; }
    static size_t mod26339969(size_t hash) { return hash % 26339969llu; }
    static size_t mod33186281(size_t hash) { return hash % 33186281llu; }
    static size_t mod41812097(size_t hash) { return hash % 41812097llu; }
    static size_t mod52679969(size_t hash) { return hash % 52679969llu; }
    static size_t mod66372617(size_t hash) { return hash % 66372617llu; }
    static size_t mod83624237(size_t hash) { return hash % 83624237llu; }
    static size_t mod105359939(size_t hash) { return hash % 105359939llu; }
    static size_t mod132745199(size_t hash) { return hash % 132745199llu; }
    static size_t mod167248483(size_t hash) { return hash % 167248483llu; }
    static size_t mod210719881(size_t hash) { return hash % 210719881llu; }
    static size_t mod265490441(size_t hash) { return hash % 265490441llu; }
    static size_t mod334496971(size_t hash) { return hash % 334496971llu; }
    static size_t mod421439783(size_t hash) { return hash % 421439783llu; }
    static size_t mod530980861(size_t hash) { return hash % 530980861llu; }
    static size_t mod668993977(size_t hash) { return hash % 668993977llu; }
    static size_t mod842879579(size_t hash) { return hash % 842879579llu; }
    static size_t mod1061961721(size_t hash) { return hash % 1061961721llu; }
    static size_t mod1337987929(size_t hash) { return hash % 1337987929llu; }
    static size_t mod1685759167(size_t hash) { return hash % 1685759167llu; }
    static size_t mod2123923447(size_t hash) { return hash % 2123923447llu; }
    static size_t mod2675975881(size_t hash) { return hash % 2675975881llu; }
    static size_t mod3371518343(size_t hash) { return hash % 3371518343llu; }
    static size_t mod4247846927(size_t hash) { return hash % 4247846927llu; }
    static size_t mod5351951779(size_t hash) { return hash % 5351951779llu; }
    static size_t mod6743036717(size_t hash) { return hash % 6743036717llu; }
    static size_t mod8495693897(size_t hash) { return hash % 8495693897llu; }
    static size_t mod10703903591(size_t hash) { return hash % 10703903591llu; }
    static size_t mod13486073473(size_t hash) { return hash % 13486073473llu; }
    static size_t mod16991387857(size_t hash) { return hash % 16991387857llu; }
    static size_t mod21407807219(size_t hash) { return hash % 21407807219llu; }
    static size_t mod26972146961(size_t hash) { return hash % 26972146961llu; }
    static size_t mod33982775741(size_t hash) { return hash % 33982775741llu; }
    static size_t mod42815614441(size_t hash) { return hash % 42815614441llu; }
    static size_t mod53944293929(size_t hash) { return hash % 53944293929llu; }
    static size_t mod67965551447(size_t hash) { return hash % 67965551447llu; }
    static size_t mod85631228929(size_t hash) { return hash % 85631228929llu; }
    static size_t mod107888587883(size_t hash) { return hash % 107888587883llu; }
    static size_t mod135931102921(size_t hash) { return hash % 135931102921llu; }
    static size_t mod171262457903(size_t hash) { return hash % 171262457903llu; }
    static size_t mod215777175787(size_t hash) { return hash % 215777175787llu; }
    static size_t mod271862205833(size_t hash) { return hash % 271862205833llu; }
    static size_t mod342524915839(size_t hash) { return hash % 342524915839llu; }
    static size_t mod431554351609(size_t hash) { return hash % 431554351609llu; }
    static size_t mod543724411781(size_t hash) { return hash % 543724411781llu; }
    static size_t mod685049831731(size_t hash) { return hash % 685049831731llu; }
    static size_t mod863108703229(size_t hash) { return hash % 863108703229llu; }
    static size_t mod1087448823553(size_t hash) { return hash % 1087448823553llu; }
    static size_t mod1370099663459(size_t hash) { return hash % 1370099663459llu; }
    static size_t mod1726217406467(size_t hash) { return hash % 1726217406467llu; }
    static size_t mod2174897647073(size_t hash) { return hash % 2174897647073llu; }
    static size_t mod2740199326961(size_t hash) { return hash % 2740199326961llu; }
    static size_t mod3452434812973(size_t hash) { return hash % 3452434812973llu; }
    static size_t mod4349795294267(size_t hash) { return hash % 4349795294267llu; }
    static size_t mod5480398654009(size_t hash) { return hash % 5480398654009llu; }
    static size_t mod6904869625999(size_t hash) { return hash % 6904869625999llu; }
    static size_t mod8699590588571(size_t hash) { return hash % 8699590588571llu; }
    static size_t mod10960797308051(size_t hash) { return hash % 10960797308051llu; }
    static size_t mod13809739252051(size_t hash) { return hash % 13809739252051llu; }
    static size_t mod17399181177241(size_t hash) { return hash % 17399181177241llu; }
    static size_t mod21921594616111(size_t hash) { return hash % 21921594616111llu; }
    static size_t mod27619478504183(size_t hash) { return hash % 27619478504183llu; }
    static size_t mod34798362354533(size_t hash) { return hash % 34798362354533llu; }
    static size_t mod43843189232363(size_t hash) { return hash % 43843189232363llu; }
    static size_t mod55238957008387(size_t hash) { return hash % 55238957008387llu; }
    static size_t mod69596724709081(size_t hash) { return hash % 69596724709081llu; }
    static size_t mod87686378464759(size_t hash) { return hash % 87686378464759llu; }
    static size_t mod110477914016779(size_t hash) { return hash % 110477914016779llu; }
    static size_t mod139193449418173(size_t hash) { return hash % 139193449418173llu; }
    static size_t mod175372756929481(size_t hash) { return hash % 175372756929481llu; }
    static size_t mod220955828033581(size_t hash) { return hash % 220955828033581llu; }
    static size_t mod278386898836457(size_t hash) { return hash % 278386898836457llu; }
    static size_t mod350745513859007(size_t hash) { return hash % 350745513859007llu; }
    static size_t mod441911656067171(size_t hash) { return hash % 441911656067171llu; }
    static size_t mod556773797672909(size_t hash) { return hash % 556773797672909llu; }
    static size_t mod701491027718027(size_t hash) { return hash % 701491027718027llu; }
    static size_t mod883823312134381(size_t hash) { return hash % 883823312134381llu; }
    static size_t mod1113547595345903(size_t hash) { return hash % 1113547595345903llu; }
    static size_t mod1402982055436147(size_t hash) { return hash % 1402982055436147llu; }
    static size_t mod1767646624268779(size_t hash) { return hash % 1767646624268779llu; }
    static size_t mod2227095190691797(size_t hash) { return hash % 2227095190691797llu; }
    static size_t mod2805964110872297(size_t hash) { return hash % 2805964110872297llu; }
    static size_t mod3535293248537579(size_t hash) { return hash % 3535293248537579llu; }
    static size_t mod4454190381383713(size_t hash) { return hash % 4454190381383713llu; }
    static size_t mod5611928221744609(size_t hash) { return hash % 5611928221744609llu; }
    static size_t mod7070586497075177(size_t hash) { return hash % 7070586497075177llu; }
    static size_t mod8908380762767489(size_t hash) { return hash % 8908380762767489llu; }
    static size_t mod11223856443489329(size_t hash) { return hash % 11223856443489329llu; }
    static size_t mod14141172994150357(size_t hash) { return hash % 14141172994150357llu; }
    static size_t mod17816761525534927(size_t hash) { return hash % 17816761525534927llu; }
    static size_t mod22447712886978529(size_t hash) { return hash % 22447712886978529llu; }
    static size_t mod28282345988300791(size_t hash) { return hash % 28282345988300791llu; }
    static size_t mod35633523051069991(size_t hash) { return hash % 35633523051069991llu; }
    static size_t mod44895425773957261(size_t hash) { return hash % 44895425773957261llu; }
    static size_t mod56564691976601587(size_t hash) { return hash % 56564691976601587llu; }
    static size_t mod71267046102139967(size_t hash) { return hash % 71267046102139967llu; }
    static size_t mod89790851547914507(size_t hash) { return hash % 89790851547914507llu; }
    static size_t mod113129383953203213(size_t hash) { return hash % 113129383953203213llu; }
    static size_t mod142534092204280003(size_t hash) { return hash % 142534092204280003llu; }
    static size_t mod179581703095829107(size_t hash) { return hash % 179581703095829107llu; }
    static size_t mod226258767906406483(size_t hash) { return hash % 226258767906406483llu; }
    static size_t mod285068184408560057(size_t hash) { return hash % 285068184408560057llu; }
    static size_t mod359163406191658253(size_t hash) { return hash % 359163406191658253llu; }
    static size_t mod452517535812813007(size_t hash) { return hash % 452517535812813007llu; }
    static size_t mod570136368817120201(size_t hash) { return hash % 570136368817120201llu; }
    static size_t mod718326812383316683(size_t hash) { return hash % 718326812383316683llu; }
    static size_t mod905035071625626043(size_t hash) { return hash % 905035071625626043llu; }
    static size_t mod1140272737634240411(size_t hash) { return hash % 1140272737634240411llu; }
    static size_t mod1436653624766633509(size_t hash) { return hash % 1436653624766633509llu; }
    static size_t mod1810070143251252131(size_t hash) { return hash % 1810070143251252131llu; }
    static size_t mod2280545475268481167(size_t hash) { return hash % 2280545475268481167llu; }
    static size_t mod2873307249533267101(size_t hash) { return hash % 2873307249533267101llu; }
    static size_t mod3620140286502504283(size_t hash) { return hash % 3620140286502504283llu; }
    static size_t mod4561090950536962147(size_t hash) { return hash % 4561090950536962147llu; }
    static size_t mod5746614499066534157(size_t hash) { return hash % 5746614499066534157llu; }
    static size_t mod7240280573005008577(size_t hash) { return hash % 7240280573005008577llu; }
    static size_t mod9122181901073924329(size_t hash) { return hash % 9122181901073924329llu; }
    static size_t mod11493228998133068689(size_t hash) { return hash % 11493228998133068689llu; }
    static size_t mod14480561146010017169(size_t hash) { return hash % 14480561146010017169llu; }
    static size_t mod18446744073709551557(size_t hash) { return hash % 18446744073709551557llu; }

    using mod_function = size_t (*)(size_t);

    mod_function next_size_over(size_t & size) const
    {
        // prime numbers generated by the following method:
        // 1. start with a prime p = 2
        // 2. go to wolfram alpha and get p = NextPrime(2 * p)
        // 3. repeat 2. until you overflow 64 bits
        // you now have large gaps which you would hit if somebody called reserve() with an unlucky number.
        // 4. to fill the gaps for every prime p go to wolfram alpha and get ClosestPrime(p * 2^(1/3)) and ClosestPrime(p * 2^(2/3)) and put those in the gaps
        // 5. get PrevPrime(2^64) and put it at the end
        static constexpr const size_t prime_list[] =
        {
            2llu, 3llu, 5llu, 7llu, 11llu, 13llu, 17llu, 23llu, 29llu, 37llu, 47llu,
            59llu, 73llu, 97llu, 127llu, 151llu, 197llu, 251llu, 313llu, 397llu,
            499llu, 631llu, 797llu, 1009llu, 1259llu, 1597llu, 2011llu, 2539llu,
            3203llu, 4027llu, 5087llu, 6421llu, 8089llu, 10193llu, 12853llu, 16193llu,
            20399llu, 25717llu, 32401llu, 40823llu, 51437llu, 64811llu, 81649llu,
            102877llu, 129607llu, 163307llu, 205759llu, 259229llu, 326617llu,
            411527llu, 518509llu, 653267llu, 823117llu, 1037059llu, 1306601llu,
            1646237llu, 2074129llu, 2613229llu, 3292489llu, 4148279llu, 5226491llu,
            6584983llu, 8296553llu, 10453007llu, 13169977llu, 16593127llu, 20906033llu,
            26339969llu, 33186281llu, 41812097llu, 52679969llu, 66372617llu,
            83624237llu, 105359939llu, 132745199llu, 167248483llu, 210719881llu,
            265490441llu, 334496971llu, 421439783llu, 530980861llu, 668993977llu,
            842879579llu, 1061961721llu, 1337987929llu, 1685759167llu, 2123923447llu,
            2675975881llu, 3371518343llu, 4247846927llu, 5351951779llu, 6743036717llu,
            8495693897llu, 10703903591llu, 13486073473llu, 16991387857llu,
            21407807219llu, 26972146961llu, 33982775741llu, 42815614441llu,
            53944293929llu, 67965551447llu, 85631228929llu, 107888587883llu,
            135931102921llu, 171262457903llu, 215777175787llu, 271862205833llu,
            342524915839llu, 431554351609llu, 543724411781llu, 685049831731llu,
            863108703229llu, 1087448823553llu, 1370099663459llu, 1726217406467llu,
            2174897647073llu, 2740199326961llu, 3452434812973llu, 4349795294267llu,
            5480398654009llu, 6904869625999llu, 8699590588571llu, 10960797308051llu,
            13809739252051llu, 17399181177241llu, 21921594616111llu, 27619478504183llu,
            34798362354533llu, 43843189232363llu, 55238957008387llu, 69596724709081llu,
            87686378464759llu, 110477914016779llu, 139193449418173llu,
            175372756929481llu, 220955828033581llu, 278386898836457llu,
            350745513859007llu, 441911656067171llu, 556773797672909llu,
            701491027718027llu, 883823312134381llu, 1113547595345903llu,
            1402982055436147llu, 1767646624268779llu, 2227095190691797llu,
            2805964110872297llu, 3535293248537579llu, 4454190381383713llu,
            5611928221744609llu, 7070586497075177llu, 8908380762767489llu,
            11223856443489329llu, 14141172994150357llu, 17816761525534927llu,
            22447712886978529llu, 28282345988300791llu, 35633523051069991llu,
            44895425773957261llu, 56564691976601587llu, 71267046102139967llu,
            89790851547914507llu, 113129383953203213llu, 142534092204280003llu,
            179581703095829107llu, 226258767906406483llu, 285068184408560057llu,
            359163406191658253llu, 452517535812813007llu, 570136368817120201llu,
            718326812383316683llu, 905035071625626043llu, 1140272737634240411llu,
            1436653624766633509llu, 1810070143251252131llu, 2280545475268481167llu,
            2873307249533267101llu, 3620140286502504283llu, 4561090950536962147llu,
            5746614499066534157llu, 7240280573005008577llu, 9122181901073924329llu,
            11493228998133068689llu, 14480561146010017169llu, 18446744073709551557llu
        };
        static constexpr size_t (* const mod_functions[])(size_t) =
        {
            &mod0, &mod2, &mod3, &mod5, &mod7, &mod11, &mod13, &mod17, &mod23, &mod29, &mod37,
            &mod47, &mod59, &mod73, &mod97, &mod127, &mod151, &mod197, &mod251, &mod313, &mod397,
            &mod499, &mod631, &mod797, &mod1009, &mod1259, &mod1597, &mod2011, &mod2539, &mod3203,
            &mod4027, &mod5087, &mod6421, &mod8089, &mod10193, &mod12853, &mod16193, &mod20399,
            &mod25717, &mod32401, &mod40823, &mod51437, &mod64811, &mod81649, &mod102877,
            &mod129607, &mod163307, &mod205759, &mod259229, &mod326617, &mod411527, &mod518509,
            &mod653267, &mod823117, &mod1037059, &mod1306601, &mod1646237, &mod2074129,
            &mod2613229, &mod3292489, &mod4148279, &mod5226491, &mod6584983, &mod8296553,
            &mod10453007, &mod13169977, &mod16593127, &mod20906033, &mod26339969, &mod33186281,
            &mod41812097, &mod52679969, &mod66372617, &mod83624237, &mod105359939, &mod132745199,
            &mod167248483, &mod210719881, &mod265490441, &mod334496971, &mod421439783,
            &mod530980861, &mod668993977, &mod842879579, &mod1061961721, &mod1337987929,
            &mod1685759167, &mod2123923447, &mod2675975881, &mod3371518343, &mod4247846927,
            &mod5351951779, &mod6743036717, &mod8495693897, &mod10703903591, &mod13486073473,
            &mod16991387857, &mod21407807219, &mod26972146961, &mod33982775741, &mod42815614441,
            &mod53944293929, &mod67965551447, &mod85631228929, &mod107888587883, &mod135931102921,
            &mod171262457903, &mod215777175787, &mod271862205833, &mod342524915839,
            &mod431554351609, &mod543724411781, &mod685049831731, &mod863108703229,
            &mod1087448823553, &mod1370099663459, &mod1726217406467, &mod2174897647073,
            &mod2740199326961, &mod3452434812973, &mod4349795294267, &mod5480398654009,
            &mod6904869625999, &mod8699590588571, &mod10960797308051, &mod13809739252051,
            &mod17399181177241, &mod21921594616111, &mod27619478504183, &mod34798362354533,
            &mod43843189232363, &mod55238957008387, &mod69596724709081, &mod87686378464759,
            &mod110477914016779, &mod139193449418173, &mod175372756929481, &mod220955828033581,
            &mod278386898836457, &mod350745513859007, &mod441911656067171, &mod556773797672909,
            &mod701491027718027, &mod883823312134381, &mod1113547595345903, &mod1402982055436147,
            &mod1767646624268779, &mod2227095190691797, &mod2805964110872297, &mod3535293248537579,
            &mod4454190381383713, &mod5611928221744609, &mod7070586497075177, &mod8908380762767489,
            &mod11223856443489329, &mod14141172994150357, &mod17816761525534927,
            &mod22447712886978529, &mod28282345988300791, &mod35633523051069991,
            &mod44895425773957261, &mod56564691976601587, &mod71267046102139967,
            &mod89790851547914507, &mod113129383953203213, &mod142534092204280003,
            &mod179581703095829107, &mod226258767906406483, &mod285068184408560057,
            &mod359163406191658253, &mod452517535812813007, &mod570136368817120201,
            &mod718326812383316683, &mod905035071625626043, &mod1140272737634240411,
            &mod1436653624766633509, &mod1810070143251252131, &mod2280545475268481167,
            &mod2873307249533267101, &mod3620140286502504283, &mod4561090950536962147,
            &mod5746614499066534157, &mod7240280573005008577, &mod9122181901073924329,
            &mod11493228998133068689, &mod14480561146010017169, &mod18446744073709551557
        };
        const size_t * found = std::lower_bound(std::begin(prime_list), std::end(prime_list) - 1, size);
        size = *found;
        return mod_functions[1 + found - prime_list];
    }
    void commit(mod_function new_mod_function)
    {
        current_mod_function = new_mod_function;
    }
    void reset()
    {
        current_mod_function = &mod0;
    }

    size_t index_for_hash(size_t hash, size_t /*num_slots_minus_one*/) const
    {
        return current_mod_function(hash);
    }
    size_t keep_in_range(size_t index, size_t num_slots_minus_one) const
    {
        return index > num_slots_minus_one ? current_mod_function(index) : index;
    }

private:
    mod_function current_mod_function = &mod0;
};

struct power_of_two_hash_policy
{
    size_t index_for_hash(size_t hash, size_t num_slots_minus_one) const
    {
        return hash & num_slots_minus_one;
    }
    size_t keep_in_range(size_t index, size_t num_slots_minus_one) const
    {
        return index_for_hash(index, num_slots_minus_one);
    }
    int8_t next_size_over(size_t & size) const
    {
        size = detailv3::next_power_of_two(size);
        return 0;
    }
    void commit(int8_t)
    {
    }
    void reset()
    {
    }

};

struct fibonacci_hash_policy
{
    size_t index_for_hash(size_t hash, size_t /*num_slots_minus_one*/) const
    {
        return (11400714819323198485ull * hash) >> shift;
    }
    size_t keep_in_range(size_t index, size_t num_slots_minus_one) const
    {
        return index & num_slots_minus_one;
    }

    int8_t next_size_over(size_t & size) const
    {
        size = std::max(size_t(2), detailv3::next_power_of_two(size));
        return 64 - detailv3::log2(size);
    }
    void commit(int8_t shift)
    {
        this->shift = shift;
    }
    void reset()
    {
        shift = 63;
    }

private:
    int8_t shift = 63;
};

template<typename K, typename V, typename H = std::hash<K>, typename E = std::equal_to<K>, typename A = std::allocator<std::pair<K, V> > >
class flat_hash_map
        : public detailv3::sherwood_v3_table
        <
            std::pair<K, V>,
            K,
            H,
            detailv3::KeyOrValueHasher<K, std::pair<K, V>, H>,
            E,
            detailv3::KeyOrValueEquality<K, std::pair<K, V>, E>,
            A,
            typename std::allocator_traits<A>::template rebind_alloc<detailv3::sherwood_v3_entry<std::pair<K, V>>>
        >
{
    using Table = detailv3::sherwood_v3_table
    <
        std::pair<K, V>,
        K,
        H,
        detailv3::KeyOrValueHasher<K, std::pair<K, V>, H>,
        E,
        detailv3::KeyOrValueEquality<K, std::pair<K, V>, E>,
        A,
        typename std::allocator_traits<A>::template rebind_alloc<detailv3::sherwood_v3_entry<std::pair<K, V>>>
    >;
public:

    using key_type = K;
    using mapped_type = V;

    using Table::Table;
    flat_hash_map()
    {
    }

    inline V & operator[](const K & key)
    {
        return emplace(key, convertible_to_value()).first->second;
    }
    inline V & operator[](K && key)
    {
        return emplace(std::move(key), convertible_to_value()).first->second;
    }
    V & at(const K & key)
    {
        auto found = this->find(key);
        if (found == this->end())
            throw std::out_of_range("Argument passed to at() was not in the map.");
        return found->second;
    }
    const V & at(const K & key) const
    {
        auto found = this->find(key);
        if (found == this->end())
            throw std::out_of_range("Argument passed to at() was not in the map.");
        return found->second;
    }

    using Table::emplace;
    std::pair<typename Table::iterator, bool> emplace()
    {
        return emplace(key_type(), convertible_to_value());
    }
    template<typename M>
    std::pair<typename Table::iterator, bool> insert_or_assign(const key_type & key, M && m)
    {
        auto emplace_result = emplace(key, std::forward<M>(m));
        if (!emplace_result.second)
            emplace_result.first->second = std::forward<M>(m);
        return emplace_result;
    }
    template<typename M>
    std::pair<typename Table::iterator, bool> insert_or_assign(key_type && key, M && m)
    {
        auto emplace_result = emplace(std::move(key), std::forward<M>(m));
        if (!emplace_result.second)
            emplace_result.first->second = std::forward<M>(m);
        return emplace_result;
    }
    template<typename M>
    typename Table::iterator insert_or_assign(typename Table::const_iterator, const key_type & key, M && m)
    {
        return insert_or_assign(key, std::forward<M>(m)).first;
    }
    template<typename M>
    typename Table::iterator insert_or_assign(typename Table::const_iterator, key_type && key, M && m)
    {
        return insert_or_assign(std::move(key), std::forward<M>(m)).first;
    }

    friend bool operator==(const flat_hash_map & lhs, const flat_hash_map & rhs)
    {
        if (lhs.size() != rhs.size())
            return false;
        for (const typename Table::value_type & value : lhs)
        {
            auto found = rhs.find(value.first);
            if (found == rhs.end())
                return false;
            else if (value.second != found->second)
                return false;
        }
        return true;
    }
    friend bool operator!=(const flat_hash_map & lhs, const flat_hash_map & rhs)
    {
        return !(lhs == rhs);
    }

private:
    struct convertible_to_value
    {
        operator V() const
        {
            return V();
        }
    };
};

template<typename T, typename H = std::hash<T>, typename E = std::equal_to<T>, typename A = std::allocator<T> >
class flat_hash_set
        : public detailv3::sherwood_v3_table
        <
            T,
            T,
            H,
            detailv3::functor_storage<size_t, H>,
            E,
            detailv3::functor_storage<bool, E>,
            A,
            typename std::allocator_traits<A>::template rebind_alloc<detailv3::sherwood_v3_entry<T>>
        >
{
    using Table = detailv3::sherwood_v3_table
    <
        T,
        T,
        H,
        detailv3::functor_storage<size_t, H>,
        E,
        detailv3::functor_storage<bool, E>,
        A,
        typename std::allocator_traits<A>::template rebind_alloc<detailv3::sherwood_v3_entry<T>>
    >;
public:

    using key_type = T;

    using Table::Table;
    flat_hash_set()
    {
    }

    template<typename... Args>
    std::pair<typename Table::iterator, bool> emplace(Args &&... args)
    {
        return Table::emplace(T(std::forward<Args>(args)...));
    }
    std::pair<typename Table::iterator, bool> emplace(const key_type & arg)
    {
        return Table::emplace(arg);
    }
    std::pair<typename Table::iterator, bool> emplace(key_type & arg)
    {
        return Table::emplace(arg);
    }
    std::pair<typename Table::iterator, bool> emplace(const key_type && arg)
    {
        return Table::emplace(std::move(arg));
    }
    std::pair<typename Table::iterator, bool> emplace(key_type && arg)
    {
        return Table::emplace(std::move(arg));
    }

    friend bool operator==(const flat_hash_set & lhs, const flat_hash_set & rhs)
    {
        if (lhs.size() != rhs.size())
            return false;
        for (const T & value : lhs)
        {
            if (rhs.find(value) == rhs.end())
                return false;
        }
        return true;
    }
    friend bool operator!=(const flat_hash_set & lhs, const flat_hash_set & rhs)
    {
        return !(lhs == rhs);
    }
};


template<typename T>
struct power_of_two_std_hash : std::hash<T>
{
    typedef ska::power_of_two_hash_policy hash_policy;
};

} // end namespace ska


================================================
FILE: ohmutil/3rdparty/ska/ska_sort.hpp
================================================
//          Copyright Malte Skarupke 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See http://www.boost.org/LICENSE_1_0.txt)

#pragma once

#include <cstdint>
#include <algorithm>
#include <type_traits>
#include <tuple>
#include <utility>

namespace detail
{
template<typename count_type, typename It, typename OutIt, typename ExtractKey>
void counting_sort_impl(It begin, It end, OutIt out_begin, ExtractKey && extract_key)
{
    count_type counts[256] = {};
    for (It it = begin; it != end; ++it)
    {
        ++counts[extract_key(*it)];
    }
    count_type total = 0;
    for (count_type & count : counts)
    {
        count_type old_count = count;
        count = total;
        total += old_count;
    }
    for (; begin != end; ++begin)
    {
        std::uint8_t key = extract_key(*begin);
        out_begin[counts[key]++] = std::move(*begin);
    }
}
template<typename It, typename OutIt, typename ExtractKey>
void counting_sort_impl(It begin, It end, OutIt out_begin, ExtractKey && extract_key)
{
    counting_sort_impl<std::uint64_t>(begin, end, out_begin, extract_key);
}
inline bool to_unsigned_or_bool(bool b)
{
    return b;
}
inline unsigned char to_unsigned_or_bool(unsigned char c)
{
    return c;
}
inline unsigned char to_unsigned_or_bool(signed char c)
{
    return static_cast<unsigned char>(c) + 128;
}
inline unsigned char to_unsigned_or_bool(char c)
{
    return static_cast<unsigned char>(c);
}
inline std::uint16_t to_unsigned_or_bool(char16_t c)
{
    return static_cast<std::uint16_t>(c);
}
inline std::uint32_t to_unsigned_or_bool(char32_t c)
{
    return static_cast<std::uint32_t>(c);
}
inline std::uint32_t to_unsigned_or_bool(wchar_t c)
{
    return static_cast<std::uint32_t>(c);
}
inline unsigned short to_unsigned_or_bool(short i)
{
    return static_cast<unsigned short>(i) + static_cast<unsigned short>(1 << (sizeof(short) * 8 - 1));
}
inline unsigned short to_unsigned_or_bool(unsigned short i)
{
    return i;
}
inline unsigned int to_unsigned_or_bool(int i)
{
    return static_cast<unsigned int>(i) + static_cast<unsigned int>(1 << (sizeof(int) * 8 - 1));
}
inline unsigned int to_unsigned_or_bool(unsigned int i)
{
    return i;
}
inline unsigned long to_unsigned_or_bool(long l)
{
    return static_cast<unsigned long>(l) + static_cast<unsigned long>(1l << (sizeof(long) * 8 - 1));
}
inline unsigned long to_unsigned_or_bool(unsigned long l)
{
    return l;
}
inline unsigned long long to_unsigned_or_bool(long long l)
{
    return static_cast<unsigned long long>(l) + static_cast<unsigned long long>(1ll << (sizeof(long long) * 8 - 1));
}
inline unsigned long long to_unsigned_or_bool(unsigned long long l)
{
    return l;
}
inline std::uint32_t to_unsigned_or_bool(float f)
{
    union
    {
        float f;
        std::uint32_t u;
    } as_union = { f };
    std::uint32_t sign_bit = -std::int32_t(as_union.u >> 31);
    return as_union.u ^ (sign_bit | 0x80000000);
}
inline std::uint64_t to_unsigned_or_bool(double f)
{
    union
    {
        double d;
        std::uint64_t u;
    } as_union = { f };
    std::uint64_t sign_bit = -std::int64_t(as_union.u >> 63);
    return as_union.u ^ (sign_bit | 0x8000000000000000);
}
template<typename T>
inline size_t to_unsigned_or_bool(T * ptr)
{
    return reinterpret_cast<size_t>(ptr);
}

template<size_t>
struct SizedRadixSorter;

template<>
struct SizedRadixSorter<1>
{
    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt buffer_begin, ExtractKey && extract_key)
    {
        counting_sort_impl(begin, end, buffer_begin, [&](auto && o)
        {
            return to_unsigned_or_bool(extract_key(o));
        });
        return true;
    }

    static constexpr size_t pass_count = 2;
};
template<>
struct SizedRadixSorter<2>
{
    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt buffer_begin, ExtractKey && extract_key)
    {
        std::ptrdiff_t num_elements = end - begin;
        if (num_elements <= (1ll << 32))
            return sort_inline<uint32_t>(begin, end, buffer_begin, buffer_begin + num_elements, extract_key);
        else
            return sort_inline<uint64_t>(begin, end, buffer_begin, buffer_begin + num_elements, extract_key);
    }

    template<typename count_type, typename It, typename OutIt, typename ExtractKey>
    static bool sort_inline(It begin, It end, OutIt out_begin, OutIt out_end, ExtractKey && extract_key)
    {
        count_type counts0[256] = {};
        count_type counts1[256] = {};

        for (It it = begin; it != end; ++it)
        {
            uint16_t key = to_unsigned_or_bool(extract_key(*it));
            ++counts0[key & 0xff];
            ++counts1[(key >> 8) & 0xff];
        }
        count_type total0 = 0;
        count_type total1 = 0;
        for (int i = 0; i < 256; ++i)
        {
            count_type old_count0 = counts0[i];
            count_type old_count1 = counts1[i];
            counts0[i] = total0;
            counts1[i] = total1;
            total0 += old_count0;
            total1 += old_count1;
        }
        for (It it = begin; it != end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it));
            out_begin[counts0[key]++] = std::move(*it);
        }
        for (OutIt it = out_begin; it != out_end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it)) >> 8;
            begin[counts1[key]++] = std::move(*it);
        }
        return false;
    }

    static constexpr size_t pass_count = 3;
};
template<>
struct SizedRadixSorter<4>
{

    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt buffer_begin, ExtractKey && extract_key)
    {
        std::ptrdiff_t num_elements = end - begin;
        if (num_elements <= (1ll << 32))
            return sort_inline<uint32_t>(begin, end, buffer_begin, buffer_begin + num_elements, extract_key);
        else
            return sort_inline<uint64_t>(begin, end, buffer_begin, buffer_begin + num_elements, extract_key);
    }
    template<typename count_type, typename It, typename OutIt, typename ExtractKey>
    static bool sort_inline(It begin, It end, OutIt out_begin, OutIt out_end, ExtractKey && extract_key)
    {
        count_type counts0[256] = {};
        count_type counts1[256] = {};
        count_type counts2[256] = {};
        count_type counts3[256] = {};

        for (It it = begin; it != end; ++it)
        {
            uint32_t key = to_unsigned_or_bool(extract_key(*it));
            ++counts0[key & 0xff];
            ++counts1[(key >> 8) & 0xff];
            ++counts2[(key >> 16) & 0xff];
            ++counts3[(key >> 24) & 0xff];
        }
        count_type total0 = 0;
        count_type total1 = 0;
        count_type total2 = 0;
        count_type total3 = 0;
        for (int i = 0; i < 256; ++i)
        {
            count_type old_count0 = counts0[i];
            count_type old_count1 = counts1[i];
            count_type old_count2 = counts2[i];
            count_type old_count3 = counts3[i];
            counts0[i] = total0;
            counts1[i] = total1;
            counts2[i] = total2;
            counts3[i] = total3;
            total0 += old_count0;
            total1 += old_count1;
            total2 += old_count2;
            total3 += old_count3;
        }
        for (It it = begin; it != end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it));
            out_begin[counts0[key]++] = std::move(*it);
        }
        for (OutIt it = out_begin; it != out_end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it)) >> 8;
            begin[counts1[key]++] = std::move(*it);
        }
        for (It it = begin; it != end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it)) >> 16;
            out_begin[counts2[key]++] = std::move(*it);
        }
        for (OutIt it = out_begin; it != out_end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it)) >> 24;
            begin[counts3[key]++] = std::move(*it);
        }
        return false;
    }

    static constexpr size_t pass_count = 5;
};
template<>
struct SizedRadixSorter<8>
{
    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt buffer_begin, ExtractKey && extract_key)
    {
        std::ptrdiff_t num_elements = end - begin;
        if (num_elements <= (1ll << 32))
            return sort_inline<uint32_t>(begin, end, buffer_begin, buffer_begin + num_elements, extract_key);
        else
            return sort_inline<uint64_t>(begin, end, buffer_begin, buffer_begin + num_elements, extract_key);
    }
    template<typename count_type, typename It, typename OutIt, typename ExtractKey>
    static bool sort_inline(It begin, It end, OutIt out_begin, OutIt out_end, ExtractKey && extract_key)
    {
        count_type counts0[256] = {};
        count_type counts1[256] = {};
        count_type counts2[256] = {};
        count_type counts3[256] = {};
        count_type counts4[256] = {};
        count_type counts5[256] = {};
        count_type counts6[256] = {};
        count_type counts7[256] = {};

        for (It it = begin; it != end; ++it)
        {
            uint64_t key = to_unsigned_or_bool(extract_key(*it));
            ++counts0[key & 0xff];
            ++counts1[(key >> 8) & 0xff];
            ++counts2[(key >> 16) & 0xff];
            ++counts3[(key >> 24) & 0xff];
            ++counts4[(key >> 32) & 0xff];
            ++counts5[(key >> 40) & 0xff];
            ++counts6[(key >> 48) & 0xff];
            ++counts7[(key >> 56) & 0xff];
        }
        count_type total0 = 0;
        count_type total1 = 0;
        count_type total2 = 0;
        count_type total3 = 0;
        count_type total4 = 0;
        count_type total5 = 0;
        count_type total6 = 0;
        count_type total7 = 0;
        for (int i = 0; i < 256; ++i)
        {
            count_type old_count0 = counts0[i];
            count_type old_count1 = counts1[i];
            count_type old_count2 = counts2[i];
            count_type old_count3 = counts3[i];
            count_type old_count4 = counts4[i];
            count_type old_count5 = counts5[i];
            count_type old_count6 = counts6[i];
            count_type old_count7 = counts7[i];
            counts0[i] = total0;
            counts1[i] = total1;
            counts2[i] = total2;
            counts3[i] = total3;
            counts4[i] = total4;
            counts5[i] = total5;
            counts6[i] = total6;
            counts7[i] = total7;
            total0 += old_count0;
            total1 += old_count1;
            total2 += old_count2;
            total3 += old_count3;
            total4 += old_count4;
            total5 += old_count5;
            total6 += old_count6;
            total7 += old_count7;
        }
        for (It it = begin; it != end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it));
            out_begin[counts0[key]++] = std::move(*it);
        }
        for (OutIt it = out_begin; it != out_end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it)) >> 8;
            begin[counts1[key]++] = std::move(*it);
        }
        for (It it = begin; it != end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it)) >> 16;
            out_begin[counts2[key]++] = std::move(*it);
        }
        for (OutIt it = out_begin; it != out_end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it)) >> 24;
            begin[counts3[key]++] = std::move(*it);
        }
        for (It it = begin; it != end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it)) >> 32;
            out_begin[counts4[key]++] = std::move(*it);
        }
        for (OutIt it = out_begin; it != out_end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it)) >> 40;
            begin[counts5[key]++] = std::move(*it);
        }
        for (It it = begin; it != end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it)) >> 48;
            out_begin[counts6[key]++] = std::move(*it);
        }
        for (OutIt it = out_begin; it != out_end; ++it)
        {
            std::uint8_t key = to_unsigned_or_bool(extract_key(*it)) >> 56;
            begin[counts7[key]++] = std::move(*it);
        }
        return false;
    }

    static constexpr size_t pass_count = 9;
};

template<typename>
struct RadixSorter;
template<>
struct RadixSorter<bool>
{
    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt buffer_begin, ExtractKey && extract_key)
    {
        size_t false_count = 0;
        for (It it = begin; it != end; ++it)
        {
            if (!extract_key(*it))
                ++false_count;
        }
        size_t true_position = false_count;
        false_count = 0;
        for (; begin != end; ++begin)
        {
            if (extract_key(*begin))
                buffer_begin[true_position++] = std::move(*begin);
            else
                buffer_begin[false_count++] = std::move(*begin);
        }
        return true;
    }

    static constexpr size_t pass_count = 2;
};
template<>
struct RadixSorter<signed char> : SizedRadixSorter<sizeof(signed char)>
{
};
template<>
struct RadixSorter<unsigned char> : SizedRadixSorter<sizeof(unsigned char)>
{
};
template<>
struct RadixSorter<signed short> : SizedRadixSorter<sizeof(signed short)>
{
};
template<>
struct RadixSorter<unsigned short> : SizedRadixSorter<sizeof(unsigned short)>
{
};
template<>
struct RadixSorter<signed int> : SizedRadixSorter<sizeof(signed int)>
{
};
template<>
struct RadixSorter<unsigned int> : SizedRadixSorter<sizeof(unsigned int)>
{
};
template<>
struct RadixSorter<signed long> : SizedRadixSorter<sizeof(signed long)>
{
};
template<>
struct RadixSorter<unsigned long> : SizedRadixSorter<sizeof(unsigned long)>
{
};
template<>
struct RadixSorter<signed long long> : SizedRadixSorter<sizeof(signed long long)>
{
};
template<>
struct RadixSorter<unsigned long long> : SizedRadixSorter<sizeof(unsigned long long)>
{
};
template<>
struct RadixSorter<float> : SizedRadixSorter<sizeof(float)>
{
};
template<>
struct RadixSorter<double> : SizedRadixSorter<sizeof(double)>
{
};
template<>
struct RadixSorter<char> : SizedRadixSorter<sizeof(char)>
{
};
template<>
struct RadixSorter<wchar_t> : SizedRadixSorter<sizeof(wchar_t)>
{
};
template<>
struct RadixSorter<char16_t> : SizedRadixSorter<sizeof(char16_t)>
{
};
template<>
struct RadixSorter<char32_t> : SizedRadixSorter<sizeof(char32_t)>
{
};
template<typename K, typename V>
struct RadixSorter<std::pair<K, V>>
{
    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt buffer_begin, ExtractKey && extract_key)
    {
        bool first_result = RadixSorter<V>::sort(begin, end, buffer_begin, [&](auto && o)
        {
            return extract_key(o).second;
        });
        auto extract_first = [&](auto && o)
        {
            return extract_key(o).first;
        };

        if (first_result)
        {
            return !RadixSorter<K>::sort(buffer_begin, buffer_begin + (end - begin), begin, extract_first);
        }
        else
        {
            return RadixSorter<K>::sort(begin, end, buffer_begin, extract_first);
        }
    }

    static constexpr size_t pass_count = RadixSorter<K>::pass_count + RadixSorter<V>::pass_count;
};
template<typename K, typename V>
struct RadixSorter<const std::pair<K, V> &>
{
    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt buffer_begin, ExtractKey && extract_key)
    {
        bool first_result = RadixSorter<V>::sort(begin, end, buffer_begin, [&](auto && o) -> const V &
        {
            return extract_key(o).second;
        });
        auto extract_first = [&](auto && o) -> const K &
        {
            return extract_key(o).first;
        };

        if (first_result)
        {
            return !RadixSorter<K>::sort(buffer_begin, buffer_begin + (end - begin), begin, extract_first);
        }
        else
        {
            return RadixSorter<K>::sort(begin, end, buffer_begin, extract_first);
        }
    }

    static constexpr size_t pass_count = RadixSorter<K>::pass_count + RadixSorter<V>::pass_count;
};
template<size_t I, size_t S, typename Tuple>
struct TupleRadixSorter
{
    using NextSorter = TupleRadixSorter<I + 1, S, Tuple>;
    using ThisSorter = RadixSorter<typename std::tuple_element<I, Tuple>::type>;

    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt out_begin, OutIt out_end, ExtractKey && extract_key)
    {
        bool which = NextSorter::sort(begin, end, out_begin, out_end, extract_key);
        auto extract_i = [&](auto && o)
        {
            return std::get<I>(extract_key(o));
        };
        if (which)
            return !ThisSorter::sort(out_begin, out_end, begin, extract_i);
        else
            return ThisSorter::sort(begin, end, out_begin, extract_i);
    }

    static constexpr size_t pass_count = ThisSorter::pass_count + NextSorter::pass_count;
};
template<size_t I, size_t S, typename Tuple>
struct TupleRadixSorter<I, S, const Tuple &>
{
    using NextSorter = TupleRadixSorter<I + 1, S, const Tuple &>;
    using ThisSorter = RadixSorter<typename std::tuple_element<I, Tuple>::type>;

    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt out_begin, OutIt out_end, ExtractKey && extract_key)
    {
        bool which = NextSorter::sort(begin, end, out_begin, out_end, extract_key);
        auto extract_i = [&](auto && o) -> decltype(auto)
        {
            return std::get<I>(extract_key(o));
        };
        if (which)
            return !ThisSorter::sort(out_begin, out_end, begin, extract_i);
        else
            return ThisSorter::sort(begin, end, out_begin, extract_i);
    }

    static constexpr size_t pass_count = ThisSorter::pass_count + NextSorter::pass_count;
};
template<size_t I, typename Tuple>
struct TupleRadixSorter<I, I, Tuple>
{
    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It, It, OutIt, OutIt, ExtractKey &&)
    {
        return false;
    }

    static constexpr size_t pass_count = 0;
};
template<size_t I, typename Tuple>
struct TupleRadixSorter<I, I, const Tuple &>
{
    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It, It, OutIt, OutIt, ExtractKey &&)
    {
        return false;
    }

    static constexpr size_t pass_count = 0;
};

template<typename... Args>
struct RadixSorter<std::tuple<Args...>>
{
    using SorterImpl = TupleRadixSorter<0, sizeof...(Args), std::tuple<Args...>>;

    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt buffer_begin, ExtractKey && extract_key)
    {
        return SorterImpl::sort(begin, end, buffer_begin, buffer_begin + (end - begin), extract_key);
    }

    static constexpr size_t pass_count = SorterImpl::pass_count;
};

template<typename... Args>
struct RadixSorter<const std::tuple<Args...> &>
{
    using SorterImpl = TupleRadixSorter<0, sizeof...(Args), const std::tuple<Args...> &>;

    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt buffer_begin, ExtractKey && extract_key)
    {
        return SorterImpl::sort(begin, end, buffer_begin, buffer_begin + (end - begin), extract_key);
    }

    static constexpr size_t pass_count = SorterImpl::pass_count;
};

template<typename T, size_t S>
struct RadixSorter<std::array<T, S>>
{
    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt buffer_begin, ExtractKey && extract_key)
    {
        auto buffer_end = buffer_begin + (end - begin);
        bool which = false;
        for (size_t i = S; i > 0; --i)
        {
            auto extract_i = [&, i = i - 1](auto && o)
            {
                return extract_key(o)[i];
            };
            if (which)
                which = !RadixSorter<T>::sort(buffer_begin, buffer_end, begin, extract_i);
            else
                which = RadixSorter<T>::sort(begin, end, buffer_begin, extract_i);
        }
        return which;
    }

    static constexpr size_t pass_count = RadixSorter<T>::pass_count * S;
};

template<typename T>
struct RadixSorter<const T> : RadixSorter<T>
{
};
template<typename T>
struct RadixSorter<T &> : RadixSorter<const T &>
{
};
template<typename T>
struct RadixSorter<T &&> : RadixSorter<T>
{
};
template<typename T>
struct RadixSorter<const T &> : RadixSorter<T>
{
};
template<typename T>
struct RadixSorter<const T &&> : RadixSorter<T>
{
};
// these structs serve two purposes
// 1. they serve as illustration for how to implement the to_radix_sort_key function
// 2. they help produce better error messages. with these overloads you get the
//    error message "no matching function for call to to_radix_sort(your_type)"
//    without these examples, you'd get the error message "to_radix_sort_key was
//    not declared in this scope" which is a much less useful error message
struct ExampleStructA { int i; };
struct ExampleStructB { float f; };
inline int to_radix_sort_key(ExampleStructA a) { return a.i; }
inline float to_radix_sort_key(ExampleStructB b) { return b.f; }
template<typename T, typename Enable = void>
struct FallbackRadixSorter : RadixSorter<decltype(to_radix_sort_key(std::declval<T>()))>
{
    using base = RadixSorter<decltype(to_radix_sort_key(std::declval<T>()))>;

    template<typename It, typename OutIt, typename ExtractKey>
    static bool sort(It begin, It end, OutIt buffer_begin, ExtractKey && extract_key)
    {
        return base::sort(begin, end, buffer_begin, [&](auto && a) -> decltype(auto)
        {
            return to_radix_sort_key(extract_key(a));
        });
    }
};

template<typename...>
struct nested_void
{
	using type = void;
};

template<typename... Args>
using void_t = typename nested_void<Args...>::type;

template<typename T>
struct has_subscript_operator_impl
{
	template<typename U, typename = decltype(std::declval<U>()[0])>
	static std::true_type test(int);
	template<typename>
	static std::false_type test(...);

	using type = decltype(test<T>(0));
};

template<typename T>
using has_subscript_operator = typename has_subscript_operator_impl<T>::type;


template<typename T>
struct FallbackRadixSorter<T, void_t<decltype(to_unsigned_or_bool(std::declval<T>()))>>
    : RadixSorter<decltype(to_unsigned_or_bool(std::declval<T>()))>
{
};

template<typename T>
struct RadixSorter : FallbackRadixSorter<T>
{
};

template<typename T>
size_t radix_sort_pass_count = RadixSorter<T>::pass_count;

template<typename It, typename Func>
inline void unroll_loop_four_times(It begin, size_t iteration_count, Func && to_call)
{
    size_t loop_count = iteration_count / 4;
    size_t remainder_count = iteration_count - loop_count * 4;
    for (; loop_count > 0; --loop_count)
    {
        to_call(begin);
        ++begin;
        to_call(begin);
        ++begin;
        to_call(begin);
        ++begin;
        to_call(begin);
        ++begin;
    }
    switch(remainder_count)
    {
    case 3:
        to_call(begin);
        ++begin;
    case 2:
        to_call(begin);
        ++begin;
    case 1:
        to_call(begin);
    }
}

template<typename It, typename F>
inline It custom_std_partition(It begin, It end, F && func)
{
    for (;; ++begin)
    {
        if (begin == end)
            return end;
        if (!func(*begin))
            break;
    }
    It it = begin;
    for(++it; it != end; ++it)
    {
        if (!func(*it))
            continue;

        std::iter_swap(begin, it);
        ++begin;
    }
    return begin;
}

struct PartitionInfo
{
    PartitionInfo()
        : count(0)
    {
    }

    union
    {
        size_t count;
        size_t offset;
    };
    size_t next_offset;
};

template<size_t>
struct UnsignedForSize;
template<>
struct UnsignedForSize<1>
{
    typedef uint8_t type;
};
template<>
struct UnsignedForSize<2>
{
    typedef uint16_t type;
};
template<>
struct UnsignedForSize<4>
{
    typedef uint32_t type;
};
template<>
struct UnsignedForSize<8>
{
    typedef uint64_t type;
};
template<typename T>
struct SubKey;
template<size_t Size>
struct SizedSubKey
{
    template<typename T>
    static auto sub_key(T && value, void *)
    {
        return to_unsigned_or_bool(value);
    }

    typedef SubKey<void> next;

    using sub_key_type = typename UnsignedForSize<Size>::type;
};
template<typename T>
struct SubKey<const T> : SubKey<T>
{
};
template<typename T>
struct SubKey<T &> : SubKey<T>
{
};
template<typename T>
struct SubKey<T &&> : SubKey<T>
{
};
template<typename T>
struct SubKey<const T &> : SubKey<T>
{
};
template<typename T>
struct SubKey<const T &&> : SubKey<T>
{
};
template<typename T, typename Enable = void>
struct FallbackSubKey
    : SubKey<decltype(to_radix_sort_key(std::declval<T>()))>
{
    using base = SubKey<decltype(to_radix_sort_key(std::declval<T>()))>;

    template<typename U>
    static decltype(auto) sub_key(U && value, void * data)
    {
        return base::sub_key(to_radix_sort_key(value), data);
    }
};
template<typename T>
struct FallbackSubKey<T, void_t<decltype(to_unsigned_or_bool(std::declval<T>()))>>
    : SubKey<decltype(to_unsigned_or_bool(std::declval<T>()))>
{
};
template<typename T>
struct SubKey : FallbackSubKey<T>
{
};
template<>
struct SubKey<bool>
{
    template<typename T>
    static bool sub_key(T && value, void *)
    {
        return value;
    }

    typedef SubKey<void> next;

    using sub_key_type = bool;
};
template<>
struct SubKey<void>;
template<>
struct SubKey<unsigned char> : SizedSubKey<sizeof(unsigned char)>
{
};
template<>
struct SubKey<unsigned short> : SizedSubKey<sizeof(unsigned short)>
{
};
template<>
struct SubKey<unsigned int> : SizedSubKey<sizeof(unsigned int)>
{
};
template<>
struct SubKey<unsigned long> : SizedSubKey<sizeof(unsigned long)>
{
};
template<>
struct SubKey<unsigned long long> : SizedSubKey<sizeof(unsigned long long)>
{
};
template<typename T>
struct SubKey<T *> : SizedSubKey<sizeof(T *)>
{
};
template<typename F, typename S, typename Current>
struct PairSecondSubKey : Current
{
    static decltype(auto) sub_key(const std::pair<F, S> & value, void * sort_data)
    {
        return Current::sub_key(value.second, sort_data);
    }

    using next = typename std::conditional<std::is_same<SubKey<void>, typename Current::next>::value, SubKey<void>, PairSecondSubKey<F, S, typename Current::next>>::type;
};
template<typename F, typename S, typename Current>
struct PairFirstSubKey : Current
{
    static decltype(auto) sub_key(const std::pair<F, S> & value, void * sort_data)
    {
        return Current::sub_key(value.first, sort_data);
    }

    using next = typename std::conditional<std::is_same<SubKey<void>, typename Current::next>::value, PairSecondSubKey<F, S, SubKey<S>>, PairFirstSubKey<F, S, typename Current::next>>::type;
};
template<typename F, typename S>
struct SubKey<std::pair<F, S>> : PairFirstSubKey<F, S, SubKey<F>>
{
};
template<size_t Index, typename First, typename... More>
struct TypeAt : TypeAt<Index - 1, More..., void>
{
};
template<typename First, typename... More>
struct TypeAt<0, First, More...>
{
    typedef First type;
};

template<size_t Index, typename Current, typename First, typename... More>
struct TupleSubKey;

template<size_t Index, typename Next, typename First, typename... More>
struct NextTupleSubKey
{
    using type = TupleSubKey<Index, Next, First, More...>;
};
template<size_t Index, typename First, typename Second, typename... More>
struct NextTupleSubKey<Index, SubKey<void>, First, Second, More...>
{
    using type = TupleSubKey<Index + 1, SubKey<Second>, Second, More...>;
};
template<size_t Index, typename First>
struct NextTupleSubKey<Index, SubKey<void>, First>
{
    using type = SubKey<void>;
};

template<size_t Index, typename Current, typename First, typename... More>
struct TupleSubKey : Current
{
    template<typename Tuple>
    static decltype(auto) sub_key(const Tuple & value, void * sort_data)
    {
        return Current::sub_key(std::get<Index>(value), sort_data);
    }

    using next = typename NextTupleSubKey<Index, typename Current::next, First, More...>::type;
};
template<size_t Index, typename Current, typename First>
struct TupleSubKey<Index, Current, First> : Current
{
    template<typename Tuple>
    static decltype(auto) sub_key(const Tuple & value, void * sort_data)
    {
        return Current::sub_key(std::get<Index>(value), sort_data);
    }

    using next = typename NextTupleSubKey<Index, typename Current::next, First>::type;
};
template<typename First, typename... More>
struct SubKey<std::tuple<First, More...>> : TupleSubKey<0, SubKey<First>, First, More...>
{
};

struct BaseListSortData
{
    size_t current_index;
    size_t recursion_limit;
    void * next_sort_data;
};
template<typename It, typename ExtractKey>
struct ListSortData : BaseListSortData
{
    void (*next_sort)(It, It, std::ptrdiff_t, ExtractKey &, void *);
};

template<typename CurrentSubKey, typename T>
struct ListElementSubKey : SubKey<typename std::decay<decltype(std::declval<T>()[0])>::type>
{
    using base = SubKey<typename std::decay<decltype(std::declval<T>()[0])>::type>;

    using next = ListElementSubKey;

    template<typename U>
    static decltype(auto) sub_key(U && value, void * sort_data)
    {
        BaseListSortData * list_sort_data = static_cast<BaseListSortData *>(sort_data);
        const T & list = CurrentSubKey::sub_key(value, list_sort_data->next_sort_data);
        return base::sub_key(list[list_sort_data->current_index], list_sort_data->next_sort_data);
    }
};

template<typename T>
struct ListSubKey
{
    using next = SubKey<void>;

    using sub_key_type = T;

    static const T & sub_key(const T & value, void *)
    {
        return value;
    }
};

template<typename T>
struct FallbackSubKey<T, typename std::enable_if<has_subscript_operator<T>::value>::type> : ListSubKey<T>
{
};

template<typename It, typename ExtractKey>
inline void StdSortFallback(It begin, It end, ExtractKey & extract_key)
{
    std::sort(begin, end, [&](auto && l, auto && r){ return extract_key(l) < extract_key(r); });
}

template<std::ptrdiff_t StdSortThreshold, typename It, typename ExtractKey>
inline bool StdSortIfLessThanThreshold(It begin, It end, std::ptrdiff_t num_elements, ExtractKey & extract_key)
{
    if (num_elements <= 1)
        return true;
    if (num_elements >= StdSortThreshold)
        return false;
    StdSortFallback(begin, end, extract_key);
    return true;
}

template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey, typename SubKeyType = typename CurrentSubKey::sub_key_type>
struct InplaceSorter;

template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey, size_t NumBytes, size_t Offset = 0>
struct UnsignedInplaceSorter
{
    static constexpr size_t ShiftAmount = (((NumBytes - 1) - Offset) * 8);
    template<typename T>
    inline static uint8_t current_byte(T && elem, void * sort_data)
    {
        return CurrentSubKey::sub_key(elem, sort_data) >> ShiftAmount;
    }
    template<typename It, typename ExtractKey>
    static void sort(It begin, It end, std::ptrdiff_t num_elements, ExtractKey & extract_key, void (*next_sort)(It, It, std::ptrdiff_t, ExtractKey &, void *), void * sort_data)
    {
        if (num_elements < AmericanFlagSortThreshold)
            american_flag_sort(begin, end, extract_key, next_sort, sort_data);
        else
            ska_byte_sort(begin, end, extract_key, next_sort, sort_data);
    }

    template<typename It, typename ExtractKey>
    static void american_flag_sort(It begin, It end, ExtractKey & extract_key, void (*next_sort)(It, It, std::ptrdiff_t, ExtractKey &, void *), void * sort_data)
    {
        PartitionInfo partitions[256];
        for (It it = begin; it != end; ++it)
        {
            ++partitions[current_byte(extract_key(*it), sort_data)].count;
        }
        size_t total = 0;
        uint8_t remaining_partitions[256];
        int num_partitions = 0;
        for (int i = 0; i < 256; ++i)
        {
            size_t count = partitions[i].count;
            if (!count)
                continue;
            partitions[i].offset = total;
            total += count;
            partitions[i].next_offset = total;
            remaining_partitions[num_partitions] = i;
            ++num_partitions;
        }
        if (num_partitions > 1)
        {
            uint8_t * current_block_ptr = remaining_partitions;
            PartitionInfo * current_block = partitions + *current_block_ptr;
            uint8_t * last_block = remaining_partitions + num_partitions - 1;
            It it = begin;
            It block_end = begin + current_block->next_offset;
            It last_element = end - 1;
            for (;;)
            {
                PartitionInfo * block = partitions + current_byte(extract_key(*it), sort_data);
                if (block == current_block)
                {
                    ++it;
                    if (it == last_element)
                        break;
                    else if (it == block_end)
                    {
                        for (;;)
                        {
                            ++current_block_ptr;
                            if (current_block_ptr == last_block)
                                goto recurse;
                            current_block = partitions + *current_block_ptr;
                            if (current_block->offset != current_block->next_offset)
                                break;
                        }

                        it = begin + current_block->offset;
                        block_end = begin + current_block->next_offset;
                    }
                }
                else
                {
                    size_t offset = block->offset++;
                    std::iter_swap(it, begin + offset);
                }
            }
        }
        recurse:
        if (Offset + 1 != NumBytes || next_sort)
        {
            size_t start_offset = 0;
            It partition_begin = begin;
            for (uint8_t * it = remaining_partitions, * end = remaining_partitions + num_partitions; it != end; ++it)
            {
                size_t end_offset = partitions[*it].next_offset;
                It partition_end = begin + end_offset;
                std::ptrdiff_t num_elements = end_offset - start_offset;
                if (!StdSortIfLessThanThreshold<StdSortThreshold>(partition_begin, partition_end, num_elements, extract_key))
                {
                    UnsignedInplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, NumBytes, Offset + 1>::sort(partition_begin, partition_end, num_elements, extract_key, next_sort, sort_data);
                }
                start_offset = end_offset;
                partition_begin = partition_end;
            }
        }
    }

    template<typename It, typename ExtractKey>
    static void ska_byte_sort(It begin, It end, ExtractKey & extract_key, void (*next_sort)(It, It, std::ptrdiff_t, ExtractKey &, void *), void * sort_data)
    {
        PartitionInfo partitions[256];
        for (It it = begin; it != end; ++it)
        {
            ++partitions[current_byte(extract_key(*it), sort_data)].count;
        }
        uint8_t remaining_partitions[256];
        size_t total = 0;
        int num_partitions = 0;
        for (int i = 0; i < 256; ++i)
        {
            size_t count = partitions[i].count;
            if (count)
            {
                partitions[i].offset = total;
                total += count;
                remaining_partitions[num_partitions] = i;
                ++num_partitions;
            }
            partitions[i].next_offset = total;
        }
        for (uint8_t * last_remaining = remaining_partitions + num_partitions, * end_partition = remaining_partitions + 1; last_remaining > end_partition;)
        {
            last_remaining = custom_std_partition(remaining_partitions, last_remaining, [&](uint8_t partition)
            {
                size_t & begin_offset = partitions[partition].offset;
                size_t & end_offset = partitions[partition].next_offset;
                if (begin_offset == end_offset)
                    return false;

                unroll_loop_four_times(begin + begin_offset, end_offset - begin_offset, [partitions = partitions, begin, &extract_key, sort_data](It it)
                {
                    uint8_t this_partition = current_byte(extract_key(*it), sort_data);
                    size_t offset = partitions[this_partition].offset++;
                    std::iter_swap(it, begin + offset);
                });
                return begin_offset != end_offset;
            });
        }
        if (Offset + 1 != NumBytes || next_sort)
        {
            for (uint8_t * it = remaining_partitions + num_partitions; it != remaining_partitions; --it)
            {
                uint8_t partition = it[-1];
                size_t start_offset = (partition == 0 ? 0 : partitions[partition - 1].next_offset);
                size_t end_offset = partitions[partition].next_offset;
                It partition_begin = begin + start_offset;
                It partition_end = begin + end_offset;
                std::ptrdiff_t num_elements = end_offset - start_offset;
                if (!StdSortIfLessThanThreshold<StdSortThreshold>(partition_begin, partition_end, num_elements, extract_key))
                {
                    UnsignedInplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, NumBytes, Offset + 1>::sort(partition_begin, partition_end, num_elements, extract_key, next_sort, sort_data);
                }
            }
        }
    }
};

template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey, size_t NumBytes>
struct UnsignedInplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, NumBytes, NumBytes>
{
    template<typename It, typename ExtractKey>
    inline static void sort(It begin, It end, std::ptrdiff_t num_elements, ExtractKey & extract_key, void (*next_sort)(It, It, std::ptrdiff_t, ExtractKey &, void *), void * next_sort_data)
    {
        next_sort(begin, end, num_elements, extract_key, next_sort_data);
    }
};

template<typename It, typename ExtractKey, typename ElementKey>
size_t CommonPrefix(It begin, It end, size_t start_index, ExtractKey && extract_key, ElementKey && element_key)
{
    const auto & largest_match_list = extract_key(*begin);
    size_t largest_match = largest_match_list.size();
    if (largest_match == start_index)
        return start_index;
    for (++begin; begin != end; ++begin)
    {
        const auto & current_list = extract_key(*begin);
        size_t current_size = current_list.size();
        if (current_size < largest_match)
        {
            largest_match = current_size;
            if (largest_match == start_index)
                return start_index;
        }
        if (element_key(largest_match_list[start_index]) != element_key(current_list[start_index]))
            return start_index;
        for (size_t i = start_index + 1; i < largest_match; ++i)
        {
            if (element_key(largest_match_list[i]) != element_key(current_list[i]))
            {
                largest_match = i;
                break;
            }
        }
    }
    return largest_match;
}

template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey, typename ListType>
struct ListInplaceSorter
{
    using ElementSubKey = ListElementSubKey<CurrentSubKey, ListType>;
    template<typename It, typename ExtractKey>
    static void sort(It begin, It end, ExtractKey & extract_key, ListSortData<It, ExtractKey> * sort_data)
    {
        size_t current_index = sort_data->current_index;
        void * next_sort_data = sort_data->next_sort_data;
        auto current_key = [&](auto && elem) -> decltype(auto)
        {
            return CurrentSubKey::sub_key(extract_key(elem), next_sort_data);
        };
        auto element_key = [&](auto && elem) -> decltype(auto)
        {
            return ElementSubKey::base::sub_key(elem, sort_data);
        };
        sort_data->current_index = current_index = CommonPrefix(begin, end, current_index, current_key, element_key);
        It end_of_shorter_ones = std::partition(begin, end, [&](auto && elem)
        {
            return current_key(elem).size() <= current_index;
        });
        std::ptrdiff_t num_shorter_ones = end_of_shorter_ones - begin;
        if (sort_data->next_sort && !StdSortIfLessThanThreshold<StdSortThreshold>(begin, end_of_shorter_ones, num_shorter_ones, extract_key))
        {
            sort_data->next_sort(begin, end_of_shorter_ones, num_shorter_ones, extract_key, next_sort_data);
        }
        std::ptrdiff_t num_elements = end - end_of_shorter_ones;
        if (!StdSortIfLessThanThreshold<StdSortThreshold>(end_of_shorter_ones, end, num_elements, extract_key))
        {
            void (*sort_next_element)(It, It, std::ptrdiff_t, ExtractKey &, void *) = static_cast<void (*)(It, It, std::ptrdiff_t, ExtractKey &, void *)>(&sort_from_recursion);
            InplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, ElementSubKey>::sort(end_of_shorter_ones, end, num_elements, extract_key, sort_next_element, sort_data);
        }
    }

    template<typename It, typename ExtractKey>
    static void sort_from_recursion(It begin, It end, std::ptrdiff_t, ExtractKey & extract_key, void * next_sort_data)
    {
        ListSortData<It, ExtractKey> offset = *static_cast<ListSortData<It, ExtractKey> *>(next_sort_data);
        ++offset.current_index;
        --offset.recursion_limit;
        if (offset.recursion_limit == 0)
        {
            StdSortFallback(begin, end, extract_key);
        }
        else
        {
            sort(begin, end, extract_key, &offset);
        }
    }


    template<typename It, typename ExtractKey>
    static void sort(It begin, It end, std::ptrdiff_t, ExtractKey & extract_key, void (*next_sort)(It, It, std::ptrdiff_t, ExtractKey &, void *), void * next_sort_data)
    {
        ListSortData<It, ExtractKey> offset;
        offset.current_index = 0;
        offset.recursion_limit = 16;
        offset.next_sort = next_sort;
        offset.next_sort_data = next_sort_data;
        sort(begin, end, extract_key, &offset);
    }
};

template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey>
struct InplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, bool>
{
    template<typename It, typename ExtractKey>
    static void sort(It begin, It end, std::ptrdiff_t, ExtractKey & extract_key, void (*next_sort)(It, It, std::ptrdiff_t, ExtractKey &, void *), void * sort_data)
    {
        It middle = std::partition(begin, end, [&](auto && a){ return !CurrentSubKey::sub_key(extract_key(a), sort_data); });
        if (next_sort)
        {
            next_sort(begin, middle, middle - begin, extract_key, sort_data);
            next_sort(middle, end, end - middle, extract_key, sort_data);
        }
    }
};

template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey>
struct InplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, uint8_t> : UnsignedInplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, 1>
{
};
template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey>
struct InplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, uint16_t> : UnsignedInplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, 2>
{
};
template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey>
struct InplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, uint32_t> : UnsignedInplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, 4>
{
};
template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey>
struct InplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, uint64_t> : UnsignedInplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, 8>
{
};
template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey, typename SubKeyType, typename Enable = void>
struct FallbackInplaceSorter;

template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey, typename SubKeyType>
struct InplaceSorter : FallbackInplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, SubKeyType>
{
};

template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey, typename SubKeyType>
struct FallbackInplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, SubKeyType, typename std::enable_if<has_subscript_operator<SubKeyType>::value>::type>
	: ListInplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey, SubKeyType>
{
};

template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey>
struct SortStarter;
template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold>
struct SortStarter<StdSortThreshold, AmericanFlagSortThreshold, SubKey<void>>
{
    template<typename It, typename ExtractKey>
    static void sort(It, It, std::ptrdiff_t, ExtractKey &, void *)
    {
    }
};

template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename CurrentSubKey>
struct SortStarter
{
    template<typename It, typename ExtractKey>
    static void sort(It begin, It end, std::ptrdiff_t num_elements, ExtractKey & extract_key, void * next_sort_data = nullptr)
    {
        if (StdSortIfLessThanThreshold<StdSortThreshold>(begin, end, num_elements, extract_key))
            return;

        void (*next_sort)(It, It, std::ptrdiff_t, ExtractKey &, void *) = static_cast<void (*)(It, It, std::ptrdiff_t, ExtractKey &, void *)>(&SortStarter<StdSortThreshold, AmericanFlagSortThreshold, typename CurrentSubKey::next>::sort);
        if (next_sort == static_cast<void (*)(It, It, std::ptrdiff_t, ExtractKey &, void *)>(&SortStarter<StdSortThreshold, AmericanFlagSortThreshold, SubKey<void>>::sort))
            next_sort = nullptr;
        InplaceSorter<StdSortThreshold, AmericanFlagSortThreshold, CurrentSubKey>::sort(begin, end, num_elements, extract_key, next_sort, next_sort_data);
    }
};

template<std::ptrdiff_t StdSortThreshold, std::ptrdiff_t AmericanFlagSortThreshold, typename It, typename ExtractKey>
void inplace_radix_sort(It begin, It end, ExtractKey & extract_key)
{
    using SubKey = SubKey<decltype(extract_key(*begin))>;
    SortStarter<StdSortThreshold, AmericanFlagSortThreshold, SubKey>::sort(begin, end, end - begin, extract_key);
}

struct IdentityFunctor
{
    template<typename T>
    decltype(auto) operator()(T && i) const
    {
        return std::forward<T>(i);
    }
};
}

template<typename It, typename ExtractKey>
static void ska_sort(It begin, It end, ExtractKey && extract_key)
{
    detail::inplace_radix_sort<128, 1024>(begin, end, extract_key);
}

template<typename It>
static void ska_sort(It begin, It end)
{
    ska_sort(begin, end, detail::IdentityFunctor());
}

template<typename It, typename OutIt, typename ExtractKey>
bool ska_sort_copy(It begin, It end, OutIt buffer_begin, ExtractKey && key)
{
    std::ptrdiff_t num_elements = end - begin;
    if (num_elements < 128 || detail::radix_sort_pass_count<typename std::result_of<ExtractKey(decltype(*begin))>::type> >= 8)
    {
        ska_sort(begin, end, key);
        return false;
    }
    else
        return detail::RadixSorter<typename std::result_of<ExtractKey(decltype(*begin))>::type>::sort(begin, end, buffer_begin, key);
}
template<typename It, typename OutIt>
bool ska_sort_copy(It begin, It end, OutIt buffer_begin)
{
    return ska_sort_copy(begin, end, buffer_begin, detail::IdentityFunctor());
}


================================================
FILE: ohmutil/3rdparty/ska/unordered_map.hpp
================================================
//          Copyright Malte Skarupke 2017.
// Distributed under the Boost Software License, Version 1.0.
//    (See http://www.boost.org/LICENSE_1_0.txt)

#pragma once

#include <cstdint>
#include <cstddef>
#include <cmath>
#include <array>
#include <algorithm>
#include <iterator>
#include <utility>
#include <type_traits>
#include "flat_hash_map.hpp"

namespace ska
{

namespace detailv10
{
template<typename T, typename Allocator>
struct sherwood_v10_entry
{
    sherwood_v10_entry()
    {
    }
    ~sherwood_v10_entry()
    {
    }

    using EntryPointer = typename std::allocator_traits<typename std::allocator_traits<Allocator>::template rebind_alloc<sherwood_v10_entry>>::pointer;

    EntryPointer next = nullptr;
    union
    {
        T value;
    };

    static EntryPointer * empty_pointer()
    {
        static EntryPointer result[3] = { EntryPointer(nullptr) + ptrdiff_t(1), nullptr, nullptr };
        return result + 1;
    }
};

using ska::detailv3::functor_storage;
using ska::detailv3::KeyOrValueHasher;
using ska::detailv3::KeyOrValueEquality;
using ska::detailv3::AssignIfTrue;
using ska::detailv3::HashPolicySelector;

template<typename T, typename FindKey, typename ArgumentHash, typename Hasher, typename ArgumentEqual, typename Equal, typename ArgumentAlloc, typename EntryAlloc, typename BucketAllocator>
class sherwood_v10_table : private EntryAlloc, private Hasher, private Equal, private BucketAllocator
{
    using Entry = detailv10::sherwood_v10_entry<T, ArgumentAlloc>;
    using AllocatorTraits = std::allocator_traits<EntryAlloc>;
    using BucketAllocatorTraits = std::allocator_traits<BucketAllocator>;
    using EntryPointer = typename AllocatorTraits::pointer;
    struct convertible_to_iterator;

public:

    using value_type = T;
    using size_type = size_t;
    using difference_type = std::ptrdiff_t;
    using hasher = ArgumentHash;
    using key_equal = ArgumentEqual;
    using allocator_type = EntryAlloc;
    using reference = value_type &;
    using const_reference = const value_type &;
    using pointer = value_type *;
    using const_pointer = const value_type *;

    sherwood_v10_table()
    {
    }
    explicit sherwood_v10_table(size_type bucket_count, const ArgumentHash & hash = ArgumentHash(), const ArgumentEqual & equal = ArgumentEqual(), const ArgumentAlloc & alloc = ArgumentAlloc())
        : EntryAlloc(alloc), Hasher(hash), Equal(equal), BucketAllocator(alloc)
    {
        rehash(bucket_count);
    }
    sherwood_v10_table(size_type bucket_count, const ArgumentAlloc & alloc)
        : sherwood_v10_table(bucket_count, ArgumentHash(), ArgumentEqual(), alloc)
    {
    }
    sherwood_v10_table(size_type bucket_count, const ArgumentHash & hash, const ArgumentAlloc & alloc)
        : sherwood_v10_table(bucket_count, hash, ArgumentEqual(), alloc)
    {
    }
    explicit sherwood_v10_table(const ArgumentAlloc & alloc)
        : EntryAlloc(alloc), BucketAllocator(alloc)
    {
    }
    template<typename It>
    sherwood_v10_table(It first, It last, size_type bucket_count = 0, const ArgumentHash & hash = ArgumentHash(), const ArgumentEqual & equal = ArgumentEqual(), const ArgumentAlloc & alloc = ArgumentAlloc())
        : sherwood_v10_table(bucket_count, hash, equal, alloc)
    {
        insert(first, last);
    }
    template<typename It>
    sherwood_v10_table(It first, It last, size_type bucket_count, const ArgumentAlloc & alloc)
        : sherwood_v10_table(first, last, bucket_count, ArgumentHash(), ArgumentEqual(), alloc)
    {
    }
    template<typename It>
    sherwood_v10_table(It first, It last, size_type bucket_count, const ArgumentHash & hash, const ArgumentAlloc & alloc)
        : sherwood_v10_table(first, last, bucket_count, hash, ArgumentEqual(), alloc)
    {
    }
    sherwood_v10_table(std::initializer_list<T> il, size_type bucket_count = 0, const ArgumentHash & hash = ArgumentHash(), const ArgumentEqual & equal = ArgumentEqual(), const ArgumentAlloc & alloc = ArgumentAlloc())
        : sherwood_v10_table(bucket_count, hash, equal, alloc)
    {
        if (bucket_count == 0)
            reserve(il.size());
        insert(il.begin(), il.end());
    }
    sherwood_v10_table(std::initializer_list<T> il, size_type bucket_count, const ArgumentAlloc & alloc)
        : sherwood_v10_table(il, bucket_count, ArgumentHash(), ArgumentEqual(), alloc)
    {
    }
    sherwood_v10_table(std::initializer_list<T> il, size_type bucket_count, const ArgumentHash & hash, const ArgumentAlloc & alloc)
        : sherwood_v10_table(il, bucket_count, hash, ArgumentEqual(), alloc)
    {
    }
    sherwood_v10_table(const sherwood_v10_table & other)
        : sherwood_v10_table(other, AllocatorTraits::select_on_container_copy_construction(other.get_allocator()))
    {
    }
    sherwood_v10_table(const sherwood_v10_table & other, const ArgumentAlloc & alloc)
        : EntryAlloc(alloc), Hasher(other), Equal(other), BucketAllocator(alloc), _max_load_factor(other._max_load_factor)
    {
        try
        {
            rehash_for_other_container(other);
            insert(other.begin(), other.end());
        }
        catch(...)
        {
            clear();
            deallocate_data();
            throw;
        }
    }
    sherwood_v10_table(sherwood_v10_table && other) noexcept
        : EntryAlloc(std::move(other)), Hasher(std::move(other)), Equal(std::move(other)), BucketAllocator(std::move(other)), _max_load_factor(other._max_load_factor)
    {
        swap_pointers(other);
    }
    sherwood_v10_table(sherwood_v10_table && other, const ArgumentAlloc & alloc) noexcept
        : EntryAlloc(alloc), Hasher(std::move(other)), Equal(std::move(other)), BucketAllocator(alloc), _max_load_factor(other._max_load_factor)
    {
        swap_pointers(other);
    }
    sherwood_v10_table & operator=(const sherwood_v10_table & other)
    {
        if (this == std::addressof(other))
            return *this;

        clear();
        static_assert(AllocatorTraits::propagate_on_container_copy_assignment::value == BucketAllocatorTraits::propagate_on_container_copy_assignment::value, "The allocators have to behave the same way");
        if (AllocatorTraits::propagate_on_container_copy_assignment::value)
        {
            if (static_cast<EntryAlloc &>(*this) != static_cast<const EntryAlloc &>(other) || static_cast<BucketAllocator &>(*this) != static_cast<const BucketAllocator &>(other))
            {
                reset_to_empty_state();
            }
            AssignIfTrue<EntryAlloc, AllocatorTraits::propagate_on_container_copy_assignment::value>()(*this, other);
            AssignIfTrue<BucketAllocator, BucketAllocatorTraits::propagate_on_container_copy_assignment::value>()(*this, other);
        }
        _max_load_factor = other._max_load_factor;
        static_cast<Hasher &>(*this) = other;
        static_cast<Equal &>(*this) = other;
        rehash_for_other_container(other);
        insert(other.begin(), other.end());
        return *this;
    }
    sherwood_v10_table & operator=(sherwood_v10_table && other) noexcept
    {
        static_assert(AllocatorTraits::propagate_on_container_move_assignment::value == BucketAllocatorTraits::propagate_on_container_move_assignment::value, "The allocators have to behave the same way");
        if (this == std::addressof(other))
            return *this;
        else if (AllocatorTraits::propagate_on_container_move_assignment::value)
        {
            clear();
            reset_to_empty_state();
            AssignIfTrue<EntryAlloc, AllocatorTraits::propagate_on_container_move_assignment::value>()(*this, std::move(other));
            AssignIfTrue<BucketAllocator, BucketAllocatorTraits::propagate_on_container_move_assignment::value>()(*this, std::move(other));
            swap_pointers(other);
        }
        else if (static_cast<EntryAlloc &>(*this) == static_cast<EntryAlloc &>(other) && static_cast<BucketAllocator &>(*this) == static_cast<BucketAllocator &>(other))
        {
            swap_pointers(other);
        }
        else
        {
            clear();
            _max_load_factor = other._max_load_factor;
            rehash_for_other_container(other);
            for (T & elem : other)
                emplace(std::move(elem));
            other.clear();
        }
        static_cast<Hasher &>(*this) = std::move(other);
        static_cast<Equal &>(*this) = std::move(other);
        return *this;
    }
    ~sherwood_v10_table()
    {
        clear();
        deallocate_data();
    }

    const allocator_type & get_allocator() const
    {
        return static_cast<const allocator_type &>(*this);
    }
    const ArgumentEqual & key_eq() const
    {
        return static_cast<const ArgumentEqual &>(*this);
    }
    const ArgumentHash & hash_function() const
    {
        return static_cast<const ArgumentHash &>(*this);
    }

    template<typename ValueType>
    struct templated_iterator
    {
        templated_iterator()
        {
        }
        templated_iterator(EntryPointer element, EntryPointer * bucket)
            : current_element(element), current_bucket(bucket)
        {
        }

        EntryPointer current_element = nullptr;
        EntryPointer * current_bucket = nullptr;

        using iterator_category = std::forward_iterator_tag;
        using value_type = ValueType;
        using difference_type = ptrdiff_t;
        using pointer = ValueType *;
        using reference = ValueType &;

        friend bool operator==(const templated_iterator & lhs, const templated_iterator & rhs)
        {
            return lhs.current_element == rhs.current_element;
        }
        friend bool operator!=(const templated_iterator & lhs, const templated_iterator & rhs)
        {
            return !(lhs == rhs);
        }

        templated_iterator & operator++()
        {
            if (!current_element->next)
            {
                do
                {
                    --current_bucket;
                }
                while (!*current_bucket);
                current_element = *current_bucket;
            }
            else
                current_element = current_element->next;
            return *this;
        }
        templated_iterator operator++(int)
        {
            templated_iterator copy(*this);
            ++*this;
            return copy;
        }

        ValueType & operator*() const
        {
            return current_element->value;
        }
        ValueType * operator->() const
        {
            return std::addressof(current_element->value);
        }

        operator templated_iterator<const value_type>() const
        {
            return { current_element, current_bucket };
        }
    };
    using iterator = templated_iterator<value_type>;
    using const_iterator = templated_iterator<const value_type>;

    iterator begin()
    {
        EntryPointer * end = entries - 1;
        for (EntryPointer * it = entries + num_slots_minus_one; it != end; --it)
        {
            if (*it)
                return { *it, it };
        }
        return { *end, end };
    }
    const_iterator begin() const
    {
        return const_cast<sherwood_v10_table *>(this)->begin();
    }
    const_iterator cbegin() const
    {
        return begin();
    }
    iterator end()
    {
        EntryPointer * end = entries - 1;
        return { *end, end };
    }
    const_iterator end() const
    {
        EntryPointer * end = entries - 1;
        return { *end, end };
    }
    const_iterator cend() const
    {
        return end();
    }

    iterator find(const FindKey & key)
    {
        size_t index = hash_policy.index_for_hash(hash_object(key), num_slots_minus_one);
        EntryPointer * bucket = entries + ptrdiff_t(index);
        for (EntryPointer it = *bucket; it; it = it->next)
        {
            if (compares_equal(key, it->value))
                return { it, bucket };
        }
        return end();
    }
    const_iterator find(const FindKey & key) const
    {
        return const_cast<sherwood_v10_table *>(this)->find(key);
    }
    size_t count(const FindKey & key) const
    {
        return find(key) == end() ? 0 : 1;
    }
    std::pair<iterator, iterator> equal_range(const FindKey & key)
    {
        iterator found = find(key);
        if (found == end())
            return { found, found };
        else
            return { found, std::next(found) };
    }
    std::pair<const_iterator, const_iterator> equal_range(const FindKey & key) const
    {
        const_iterator found = find(key);
        if (found == end())
            return { found, found };
        else
            return { found, std::next(found) };
    }

    template<typename Key, typename... Args>
    std::pair<iterator, bool> emplace(Key && key, Args &&... args)
    {
        size_t index = hash_policy.index_for_hash(hash_object(key), num_slots_minus_one);
        EntryPointer * bucket = entries + ptrdiff_t(index);
        for (EntryPointer it = *bucket; it; it = it->next)
        {
            if (compares_equal(key, it->value))
                return { { it, bucket }, false };
        }
        return emplace_new_key(bucket, std::forward<Key>(key), std::forward<Args>(args)...);
    }

    std::pair<iterator, bool> insert(const value_type & value)
    {
        return emplace(value);
    }
    std::pair<iterator, bool> insert(value_type && value)
    {
        return emplace(std::move(value));
    }
    template<typename... Args>
    iterator emplace_hint(const_iterator, Args &&... args)
    {
        return emplace(std::forward<Args>(args)...).first;
    }
    iterator insert(const_iterator, const value_type & value)
    {
        return emplace(value).first;
    }
    iterator insert(const_iterator, value_type && value)
    {
        return emplace(std::move(value)).first;
    }

    template<typename It>
    void insert(It begin, It end)
    {
        for (; begin != end; ++begin)
        {
            emplace(*begin);
        }
    }
    void insert(std::initializer_list<value_type> il)
    {
        insert(il.begin(), il.end());
    }

    void rehash(size_t num_buckets)
    {
        num_buckets = std::max(num_buckets, static_cast<size_t>(std::ceil(num_elements / static_cast<double>(_max_load_factor))));
        if (num_buckets == 0)
        {
            reset_to_empty_state();
            return;
        }
        auto new_prime_index = hash_policy.next_size_over(num_buckets);
        if (num_buckets == bucket_count())
            return;
        EntryPointer * new_buckets(&*BucketAllocatorTraits::allocate(*this, num_buckets + 1));
        EntryPointer * end_it = new_buckets + static_cast<ptrdiff_t>(num_buckets + 1);
        *new_buckets = EntryPointer(nullptr) + ptrdiff_t(1);
        ++new_buckets;
        std::fill(new_buckets, end_it, nullptr);
        std::swap(entries, new_buckets);
        std::swap(num_slots_minus_one, num_buckets);
        --num_slots_minus_one;
        hash_policy.commit(new_prime_index);
        if (!num_buckets)
            return;

        for (EntryPointer * it = new_buckets, * end = it + static_cast<ptrdiff_t>(num_buckets + 1); it != end; ++it)
        {
            for (EntryPointer e = *it; e;)
            {
                EntryPointer next = e->next;
                size_t index = hash_policy.index_for_hash(hash_object(e->value), num_slots_minus_one);
                EntryPointer & new_slot = entries[index];
                e->next = new_slot;
                new_slot = e;
                e = next;
            }
        }
        BucketAllocatorTraits::deallocate(*this, new_buckets - 1, num_buckets + 2);
    }

    void reserve(size_t num_elements)
    {
        if (!num_elements)
            return;
        num_elements = static_cast<size_t>(std::ceil(num_elements / static_cast<double>(_max_load_factor)));
        if (num_elements > bucket_count())
            rehash(num_elements);
    }

    // the return value is a type that can be converted to an iterator
    // the reason for doing this is that it's not free to find the
    // iterator pointing at the next element. if you care about the
    // next iterator, turn the return value into an iterator
    convertible_to_iterator erase(const_iterator to_erase)
    {
        --num_elements;
        AllocatorTraits::destroy(*this, std::addressof(to_erase.current_element->value));
        EntryPointer * previous = to_erase.current_bucket;
        while (*previous != to_erase.current_element)
        {
            previous = &(*previous)->next;
        }
        *previous = to_erase.current_element->next;
        AllocatorTraits::deallocate(*this, to_erase.current_element, 1);
        return { *previous, to_erase.current_bucket };
    }

    convertible_to_iterator erase(const_iterator begin_it, const_iterator end_it)
    {
        while (begin_it.current_bucket != end_it.current_bucket)
        {
            begin_it = erase(begin_it);
        }
        EntryPointer * bucket = begin_it.current_bucket;
        EntryPointer * previous = bucket;
        while (*previous != begin_it.current_element)
            previous = &(*previous)->next;
        while (*previous != end_it.current_element)
        {
            --num_elements;
            EntryPointer entry = *previous;
            AllocatorTraits::destroy(*this, std::addressof(entry->value));
            *previous = entry->next;
            AllocatorTraits::deallocate(*this, entry, 1);
        }
        return { *previous, bucket };
    }

    size_t erase(const FindKey & key)
    {
        auto found = find(key);
        if (found == end())
            return 0;
        else
        {
            erase(found);
            return 1;
        }
    }

    void clear()
    {
        if (!num_slots_minus_one)
            return;
        for (EntryPointer * it = entries, * end = it + static_cast<ptrdiff_t>(num_slots_minus_one + 1); it != end; ++it)
        {
            for (EntryPointer e = *it; e;)
            {
                EntryPointer next = e->next;
                AllocatorTraits::destroy(*this, std::addressof(e->value));
                AllocatorTraits::deallocate(*this, e, 1);
                e = next;
            }
            *it = nullptr;
        }
        num_elements = 0;
    }

    void swap(sherwood_v10_table & other)
    {
        using std::swap;
        swap_pointers(other);
        swap(static_cast<ArgumentHash &>(*this), static_cast<ArgumentHash &>(other));
        swap(static_cast<ArgumentEqual &>(*this), static_cast<ArgumentEqual &>(other));
        if (AllocatorTraits::propagate_on_container_swap::value)
            swap(static_cast<EntryAlloc &>(*this), static_cast<EntryAlloc &>(other));
        if (BucketAllocatorTraits::propagate_on_container_swap::value)
            swap(static_cast<BucketAllocator &>(*this), static_cast<BucketAllocator &>(other));
    }

    size_t size() const
    {
        return num_elements;
    }
    size_t max_size() const
    {
        return (AllocatorTraits::max_size(*this)) / sizeof(Entry);
    }
    size_t bucket_count() const
    {
        return num_slots_minus_one + 1;
    }
    size_type max_bucket_count() const
    {
        return (AllocatorTraits::max_size(*this) - 1) / sizeof(Entry);
    }
    size_t bucket(const FindKey & key) const
    {
        return hash_policy.template index_for_hash<0>(hash_object(key), num_slots_minus_one);
    }
    float load_factor() const
    {
        size_t buckets = bucket_count();
        if (buckets)
            return static_cast<float>(num_elements) / bucket_count();
        else
            return 0;
    }
    void max_load_factor(float value)
    {
        _max_load_factor = value;
    }
    float max_load_factor() const
    {
        return _max_load_factor;
    }

    bool empty() const
    {
        return num_elements == 0;
    }

private:
    EntryPointer * entries = Entry::empty_pointer();
    size_t num_slots_minus_one = 0;
    typename HashPolicySelector<ArgumentHash>::type hash_policy;
    float _max_load_factor = 1.0f;
    size_t num_elements = 0;

    void rehash_for_other_container(const sherwood_v10_table & other)
    {
        reserve(other.size());
    }

    void swap_pointers(sherwood_v10_table & other)
    {
        using std::swap;
        swap(hash_policy, other.hash_policy);
        swap(entries, other.entries);
        swap(num_slots_minus_one, other.num_slots_minus_one);
        swap(num_elements, other.num_elements);
        swap(_max_load_factor, other._max_load_factor);
    }

    template<typename... Args>
    SKA_NOINLINE(std::pair<iterator, bool>) emplace_new_key(EntryPointer * bucket, Args &&... args)
    {
        using std::swap;
        if (is_full())
        {
            grow();
            return emplace(std::forward<Args>(args)...);
        }
        else
        {
            EntryPointer new_entry = AllocatorTraits::allocate(*this, 1);
            try
            {
                AllocatorTraits::construct(*this, std::addressof(new_entry->value), std::forward<Args>(args)...);
            }
            catch(...)
            {
                AllocatorTraits::deallocate(*this, new_entry, 1);
                throw;
            }
            ++num_elements;
            new_entry->next = *bucket;
            *bucket = new_entry;
            return { { new_entry, bucket }, true };
        }
    }

    bool is_full() const
    {
        if (!num_slots_minus_one)
            return true;
        else
            return num_elements + 1 > (num_slots_minus_one + 1) * static_cast<double>(_max_load_factor);
    }

    void grow()
    {
        rehash(std::max(size_t(4), 2 * bucket_count()));
    }

    void deallocate_data()
    {
        if (entries != Entry::empty_pointer())
        {
            BucketAllocatorTraits::deallocate(*this, entries - 1, num_slots_minus_one + 2);
        }
    }

    void reset_to_empty_state()
    {
        deallocate_data();
        entries = Entry::empty_pointer();
        num_slots_minus_one = 0;
        hash_policy.reset();
    }

    template<typename U>
    size_t hash_object(const U & key)
    {
        return static_cast<Hasher &>(*this)(key);
    }
    template<typename U>
    size_t hash_object(const U & key) const
    {
        return static_cast<const Hasher &>(*this)(key);
    }
    template<typename L, typename R>
    bool compares_equal(const L & lhs, const R & rhs)
    {
        return static_cast<Equal &>(*this)(lhs, rhs);
    }

    struct convertible_to_iterator
    {
        EntryPointer element;
        EntryPointer * bucket;

        operator iterator()
        {
            if (element)
                return { element, bucket };
            else
            {
                do
                {
                    --bucket;
                }
                while (!*bucket);
                return { *bucket, bucket };
            }
        }
        operator const_iterator()
        {
            if (element)
                return { element, bucket };
            else
            {
                do
                {
                    --bucket;
                }
                while (!*bucket);
                return { *bucket, bucket };
            }
        }
    };
};
}


template<typename K, typename V, typename H = std::hash<K>, typename E = std::equal_to<K>, typename A = std::allocator<std::pair<K, V> > >
class unordered_map
        : public detailv10::sherwood_v10_table
        <
            std::pair<K, V>,
            K,
            H,
            detailv10::KeyOrValueHasher<K, std::pair<K, V>, H>,
            E,
            detailv10::KeyOrValueEquality<K, std::pair<K, V>, E>,
            A,
            typename std::allocator_traits<A>::template rebind_alloc<detailv10::sherwood_v10_entry<std::pair<K, V>, A>>,
            typename std::allocator_traits<A>::template rebind_alloc<typename std::allocator_traits<A>::template rebind_traits<detailv10::sherwood_v10_entry<std::pair<K, V>, A>>::pointer>
        >
{
    using Table = detailv10::sherwood_v10_table
    <
        std::pair<K, V>,
        K,
        H,
        detailv10::KeyOrValueHasher<K, std::pair<K, V>, H>,
        E,
        detailv10::KeyOrValueEquality<K, std::pair<K, V>, E>,
        A,
        typename std::allocator_traits<A>::template rebind_alloc<detailv10::sherwood_v10_entry<std::pair<K, V>, A>>,
        typename std::allocator_traits<A>::template rebind_alloc<typename std::allocator_traits<A>::template rebind_traits<detailv10::sherwood_v10_entry<std::pair<K, V>, A>>::pointer>
    >;
public:

    using key_type = K;
    using mapped_type = V;

    using Table::Table;
    unordered_map()
    {
    }

    V & operator[](const K & key)
    {
        return emplace(key, convertible_to_value()).first->second;
    }
    V & operator[](K && key)
    {
        return emplace(std::move(key), convertible_to_value()).first->second;
    }
    V & at(const K & key)
    {
        auto found = this->find(key);
        if (found == this->end())
            throw std::out_of_range("Argument passed to at() was not in the map.");
        return found->second;
    }
    const V & at(const K & key) const
    {
        auto found = this->find(key);
        if (found == this->end())
            throw std::out_of_range("Argument passed to at() was not in the map.");
        return found->second;
    }

    using Table::emplace;
    std::pair<typename Table::iterator, bool> emplace()
    {
        return emplace(key_type(), convertible_to_value());
    }

    friend bool operator==(const unordered_map & lhs, const unordered_map & rhs)
    {
        if (lhs.size() != rhs.size())
            return false;
        for (const typename Table::value_type & value : lhs)
        {
            auto found = rhs.find(value.first);
            if (found == rhs.end())
                return false;
            else if (value.second != found->second)
                return false;
        }
        return true;
    }
    friend bool operator!=(const unordered_map & lhs, const unordered_map & rhs)
    {
        return !(lhs == rhs);
    }

private:
    struct convertible_to_value
    {
        operator V() const
        {
            return V();
        }
    };
};

template<typename T, typename H = std::hash<T>, typename E = std::equal_to<T>, typename A = std::allocator<T> >
class unordered_set
        : public detailv10::sherwood_v10_table
        <
            T,
            T,
            H,
            detailv10::functor_storage<size_t, H>,
            E,
            detailv10::functor_storage<bool, E>,
            A,
            typename std::allocator_traits<A>::template rebind_alloc<detailv10::sherwood_v10_entry<T, A>>,
            typename std::allocator_traits<A>::template rebind_alloc<typename std::allocator_traits<A>::template rebind_traits<detailv10::sherwood_v10_entry<T, A>>::pointer>
        >
{
    using Table = detailv10::sherwood_v10_table
    <
        T,
        T,
        H,
        detailv10::functor_storage<size_t, H>,
        E,
        detailv10::functor_storage<bool, E>,
        A,
        typename std::allocator_traits<A>::template rebind_alloc<detailv10::sherwood_v10_entry<T, A>>,
        typename std::allocator_traits<A>::template rebind_alloc<typename std::allocator_traits<A>::template rebind_traits<detailv10::sherwood_v10_entry<T, A>>::pointer>
    >;
public:

    using key_type = T;

    using Table::Table;
    unordered_set()
    {
    }

    template<typename... Args>
    std::pair<typename Table::iterator, bool> emplace(Args &&... args)
    {
        return Table::emplace(T(std::forward<Args>(args)...));
    }
    std::pair<typename Table::iterator, bool> emplace(const key_type & arg)
    {
        return Table::emplace(arg);
    }
    std::pair<typename Table::iterator, bool> emplace(key_type & arg)
    {
        return Table::emplace(arg);
    }
    std::pair<typename Table::iterator, bool> emplace(const key_type && arg)
    {
        return Table::emplace(std::move(arg));
    }
    std::pair<typename Table::iterator, bool> emplace(key_type && arg)
    {
        return Table::emplace(std::move(arg));
    }

    friend bool operator==(const unordered_set & lhs, const unordered_set & rhs)
    {
        if (lhs.size() != rhs.size())
            return false;
        for (const T & value : lhs)
        {
            if (rhs.find(value) == rhs.end())
                return false;
        }
        return true;
    }
    friend bool operator!=(const unordered_set & lhs, const unordered_set & rhs)
    {
        return !(lhs == rhs);
    }
};

} // end namespace ska


================================================
FILE: ohmutil/CMakeLists.txt
================================================

include(GenerateExportHeader)


find_package(Threads)

set(SOURCES
  Colour.cpp
  Colour.h
  p2p.h
  PlyMesh.cpp
  PlyMesh.h
  PlyPointStream.cpp
  PlyPointStream.h
  ProgressMonitor.cpp
  ProgressMonitor.h
  GlmStream.h
  Options.h
  Profile.cpp
  Profile.h
  ProfileMarker.cpp
  ProfileMarker.h
  SafeIO.cpp
  SafeIO.h
  ScopedTimeDisplay.cpp
  ScopedTimeDisplay.h
  VectorHash.h
)

# Split 3rd-party headers so we can include them as system headers and don't need to address clang-tidy issues.
set(PUBLIC_SKA_HEADERS
  3rdparty/ska/bytell_hash_map.hpp
  3rdparty/ska/flat_hash_map.hpp
  3rdparty/ska/ska_sort.hpp
  3rdparty/ska/unordered_map.hpp
)

set(PUBLIC_CXXOPT_HEADERS
  3rdparty/cxxopts/cxxopts.hpp
)

set(PUBLIC_HEADERS
  Colour.h
  GlmStream.h
  Options.h
  p2p.h
  PlyMesh.h
  PlyPointStream.h
  Profile.h
  ProfileMarker.h
  ProgressMonitor.h
  SafeIO.h
  ScopedTimeDisplay.h
  VectorHash.h
  "${CMAKE_CURRENT_BINARY_DIR}/ohmutil/OhmUtilExport.h"
)

add_library(ohmutil ${SOURCES})
clang_tidy_target(ohmutil EXCLUDE_MATCHES ".*ska/\\*\\.hpp")

generate_export_header(ohmutil
      EXPORT_MACRO_NAME ohmutil_API
      EXPORT_FILE_NAME ohmutil/OhmUtilExport.h
      STATIC_DEFINE ohmutil_STATIC)

target_link_libraries(ohmutil PUBLIC glm::glm logutil)

target_include_directories(ohmutil
  PUBLIC
    $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/ohmutil>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
)

target_include_directories(ohmutil SYSTEM
  PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}/3rdparty>
)

install(TARGETS ohmutil EXPORT ${CMAKE_PROJECT_NAME}-config-targets
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/ohmutil
)

install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/ohmutil)
install(FILES ${PUBLIC_SKA_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/ohmutil/ska)
install(FILES ${PUBLIC_CXXOPT_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/ohmutil/cxxopts)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})


================================================
FILE: ohmutil/Colour.cpp
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) 2014 CSIRO
//
#include "Colour.h"

#include <algorithm>
#include <array>
#include <cmath>
#include <limits>

namespace
{
const float kSectorSizeDegrees = 60.0f;
const float kCircleDegrees = 360.0f;
}  // namespace

namespace ohm
{
void colour::hsvToRgb(float &r, float &g, float &b, float h, float s, float v)
{
  // if (s == 0)
  //{
  //  // achromatic (grey)
  //  r = g = b = v;
  //  return;
  //}
  const float h_sector = h / kSectorSizeDegrees;  // sector 0 to 5
  const int sector_index = int(std::min<float>(std::max<float>(0.0f, std::floor(h_sector)), 5.0f));
  const float f = h_sector - float(sector_index);
  const float p = v * (1 - s);
  const float q = v * (1 - s * f);
  const float t = v * (1 - s * (1 - f));

  static const std::array<int, 6> v_index = { 0, 1, 1, 2, 2, 0 };
  static const std::array<int, 6> p_index = { 2, 2, 0, 0, 1, 1 };
  static const std::array<int, 6> q_index = { 3, 0, 3, 1, 3, 2 };
  static const std::array<int, 6> t_index = { 1, 3, 2, 3, 0, 3 };

  std::array<float, 4> rgb = { 0 };
  rgb[v_index[sector_index]] = v;
  rgb[p_index[sector_index]] = p;
  rgb[q_index[sector_index]] = q;
  rgb[t_index[sector_index]] = t;

  // Handle achromatic here by testing s inline.
  r = (s != 0) ? rgb[0] : v;
  g = (s != 0) ? rgb[1] : v;
  b = (s != 0) ? rgb[2] : v;
}


void colour::hsvToRgb(uint8_t &r, uint8_t &g, uint8_t &b, float h, float s, float v)
{
  float rf;
  float gf;
  float bf;
  hsvToRgb(rf, gf, bf, h, s, v);
  r = uint8_t(rf * Colour::kMaxByteF);
  g = uint8_t(gf * Colour::kMaxByteF);
  b = uint8_t(bf * Colour::kMaxByteF);
}


void colour::rgbToHsv(float &h, float &s, float &v, float r, float g, float b)
{
  const float c_min = std::min<float>(r, std::min<float>(g, b));
  const float c_max = std::max<float>(r, std::max<float>(g, b));

  const float delta = c_max - c_min;


  if (!c_max)
  {
    // Black. s = 0, v is undefined.
    s = v = h = 0;
    return;
  }

  v = c_max;
  s = delta / c_max;

  if (r == c_max)
  {
    // Yellow to magenta.
    h = (g - b) / delta;
  }
  else if (g == c_max)
  {
    // Cyan to yellow
    h = 2 + (b - r) / delta;
  }
  else
  {
    // Magenta to cyan
    h = 4 + (r - g) / delta;
  }

  h *= kSectorSizeDegrees;

  h = (h < 0) ? h + kCircleDegrees : h;
  h = (h > kCircleDegrees) ? h - kCircleDegrees : h;
}


void colour::rgbToHsv(float &h, float &s, float &v, uint8_t r, uint8_t g, uint8_t b)
{
  rgbToHsv(h, s, v, float(r) / 255.0f, float(g) / 255.0f, float(b) / 255.0f);
}


Colour Colour::adjust(float factor) const
{
  float h, s, v;
  Colour c;
  ohm::colour::rgbToHsv(h, s, v, rf(), gf(), bf());
  v = std::max(0.0f, std::min(v * factor, 1.0f));
  c.rgba[kA] = this->rgba[kA];
  ohm::colour::hsvToRgb(c.rgba[kR], c.rgba[kG], c.rgba[kB], h, s, v);
  return c;
}

Colour Colour::lerp(const Colour &from, const Colour &to, float factor)
{
  // Convert to hsv space for the lerp.
  float from_h;
  float from_s;
  float from_v;
  colour::rgbToHsv(from_h, from_s, from_v, from.r(), from.g(), from.b());

  float to_h;
  float to_s;
  float to_v;
  colour::rgbToHsv(to_h, to_s, to_v, to.r(), to.g(), to.b());

  const float max_h = kCircleDegrees;
  if (std::abs(to_h - from_h) > 0.5f * max_h)
  {
    if (to_h < from_h)
    {
      to_h += max_h;
    }
    else
    {
      from_h += max_h;
    }
  }

  const float h = std::max(0.0f, std::min(from_h + (to_h - from_h) * factor, max_h));

  // Lerp s and v the easy way.
  const float s = std::max(0.0f, std::min(from_s + (to_s - from_s) * factor, 1.0f));
  const float v = std::max(0.0f, std::min(from_v + (to_v - from_v) * factor, 1.0f));

  Colour c{};
  colour::hsvToRgb(c.rgba[0], c.rgba[1], c.rgba[2], h, s, v);

  // Finally lerp alpha
  const float from_a = from.af();
  const float to_a = to.af();

  const float alpha = std::max(0.0f, std::min(from_a + (to_a - from_a) * factor, 1.0f));

  c.rgba[3] = static_cast<uint8_t>(alpha * std::numeric_limits<uint8_t>::max());

  return c;
}


// Lint(KS): small static allocation should be fine.
// NOLINTNEXTLINE(cert-err58-cpp)
const std::array<Colour, Colour::kPredefinedCount> Colour::kColours =  //
  {
    Colour(220, 220, 220),  //
    Colour(211, 211, 211),  //
    Colour(192, 192, 192),  //
    Colour(169, 169, 169),  //
    Colour(128, 128, 128),  //
    Colour(105, 105, 105),  //
    Colour(119, 136, 153),  //
    Colour(112, 128, 144),  //
    Colour(47, 79, 79),     //
    Colour(0, 0, 0),        //
    Colour(255, 255, 255),  //
    Colour(255, 250, 250),  //
    Colour(240, 255, 240),  //
    Colour(245, 255, 250),  //
    Colour(240, 255, 255),  //
    Colour(240, 248, 255),  //
    Colour(248, 248, 255),  //
    Colour(245, 245, 245),  //
    Colour(255, 245, 238),  //
    Colour(245, 245, 220),  //
    Colour(253, 245, 230),  //
    Colour(255, 250, 240),  //
    Colour(255, 255, 240),  //
    Colour(250, 235, 215),  //
    Colour(250, 240, 230),  //
    Colour(255, 240, 245),  //
    Colour(255, 228, 225),  //
    Colour(255, 192, 203),  //
    Colour(255, 182, 193),  //
    Colour(255, 105, 180),  //
    Colour(255, 20, 147),   //
    Colour(219, 112, 147),  //
    Colour(199, 21, 133),   //
    Colour(255, 160, 122),  //
    Colour(250, 128, 114),  //
    Colour(233, 150, 122),  //
    Colour(240, 128, 128),  //
    Colour(205, 92, 92),    //
    Colour(220, 20, 60),    //
    Colour(178, 34, 34),    //
    Colour(139, 0, 0),      //
    Colour(255, 0, 0),      //
    Colour(255, 69, 0),     //
    Colour(255, 99, 71),    //
    Colour(255, 127, 80),   //
    Colour(255, 140, 0),    //
    Colour(255, 165, 0),    //
    Colour(255, 255, 0),    //
    Colour(255, 255, 224),  //
    Colour(255, 250, 205),  //
    Colour(250, 250, 210),  //
    Colour(255, 239, 213),  //
    Colour(255, 228, 181),  //
    Colour(255, 218, 185),  //
    Colour(238, 232, 170),  //
    Colour(240, 230, 140),  //
    Colour(189, 183, 107),  //
    Colour(255, 215, 0),    //
    Colour(255, 248, 220),  //
    Colour(255, 235, 205),  //
    Colour(255, 228, 196),  //
    Colour(255, 222, 173),  //
    Colour(245, 222, 179),  //
    Colour(222, 184, 135),  //
    Colour(210, 180, 140),  //
    Colour(188, 143, 143),  //
    Colour(244, 164, 96),   //
    Colour(218, 165, 32),   //
    Colour(184, 134, 11),   //
    Colour(205, 133, 63),   //
    Colour(210, 105, 30),   //
    Colour(139, 69, 19),    //
    Colour(160, 82, 45),    //
    Colour(165, 42, 42),    //
    Colour(128, 0, 0),      //
    Colour(85, 107, 47),    //
    Colour(128, 128, 0),    //
    Colour(107, 142, 35),   //
    Colour(154, 205, 50),   //
    Colour(50, 205, 50),    //
    Colour(0, 255, 0),      //
    Colour(124, 252, 0),    //
    Colour(127, 255, 0),    //
    Colour(173, 255, 47),   //
    Colour(0, 255, 127),    //
    Colour(0, 250, 154),    //
    Colour(144, 238, 144),  //
    Colour(152, 251, 152),  //
    Colour(143, 188, 143),  //
    Colour(60, 179, 113),   //
    Colour(46, 139, 87),    //
    Colour(34, 139, 34),    //
    Colour(0, 128, 0),      //
    Colour(0, 100, 0),      //
    Colour(102, 205, 170),  //
    Colour(0, 255, 255),    //
    Colour(0, 255, 255),    //
    Colour(224, 255, 255),  //
    Colour(175, 238, 238),  //
    Colour(127, 255, 212),  //
    Colour(64, 224, 208),   //
    Colour(72, 209, 204),   //
    Colour(0, 206, 209),    //
    Colour(32, 178, 170),   //
    Colour(95, 158, 160),   //
    Colour(0, 139, 139),    //
    Colour(0, 128, 128),    //
    Colour(176, 196, 222),  //
    Colour(176, 224, 230),  //
    Colour(173, 216, 230),  //
    Colour(135, 206, 235),  //
    Colour(135, 206, 250),  //
    Colour(0, 191, 255),    //
    Colour(30, 144, 255),   //
    Colour(100, 149, 237),  //
    Colour(70, 130, 180),   //
    Colour(65, 105, 225),   //
    Colour(0, 0, 255),      //
    Colour(0, 0, 205),      //
    Colour(0, 0, 139),      //
    Colour(0, 0, 128),      //
    Colour(25, 25, 112),    //
    Colour(230, 230, 250),  //
    Colour(216, 191, 216),  //
    Colour(221, 160, 221),  //
    Colour(238, 130, 238),  //
    Colour(218, 112, 214),  //
    Colour(255, 0, 255),    //
    Colour(255, 0, 255),    //
    Colour(186, 85, 211),   //
    Colour(147, 112, 219),  //
    Colour(138, 43, 226),   //
    Colour(148, 0, 211),    //
    Colour(153, 50, 204),   //
    Colour(139, 0, 139),    //
    Colour(128, 0, 128),    //
    Colour(75, 0, 130),     //
    Colour(72, 61, 139),    //
    Colour(106, 90, 205),   //
    Colour(123, 104, 238),  //
  };
}  // namespace ohm


================================================
FILE: ohmutil/Colour.h
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) 2014 CSIRO
//
#ifndef OHMUTIL_COLOUR_H
#define OHMUTIL_COLOUR_H

#include "OhmUtilExport.h"

#include <array>
#include <cinttypes>
#include <limits>

namespace ohm
{
/// Colour conversion utility functions.
namespace colour
{
/// Converts from HSV to RGB colour.
///
/// All floating points values are expected to be in the range [0, 1], except for
/// @p h (see below), or the results are undefined.
///
/// @param[out] r The calculated red colour channel.
/// @param[out] g The calculated green colour channel.
/// @param[out] b The calculated blue colour channel.
/// @param h The colour hue channel [0, 360].
/// @param s The colour saturation channel.
/// @param v The colour value channel.
void ohmutil_API hsvToRgb(float &r, float &g, float &b, float h, float s, float v);

/// Converts from HSV to RGB colour.
///
/// All floating points values are expected to be in the range [0, 1], except for
/// @p h (see below), or the results are undefined.
///
/// @param[out] r The calculated red colour channel.
/// @param[out] g The calculated green colour channel.
/// @param[out] b The calculated blue colour channel.
/// @param h The colour hue channel [0, 360].
/// @param s The colour saturation channel.
/// @param v The colour value channel.
void ohmutil_API hsvToRgb(uint8_t &r, uint8_t &g, uint8_t &b, float h, float s, float v);

/// Converts from RGB to HSV colour.
///
/// All floating points values are expected to be in the range [0, 1], or
/// the results are undefined.
///
/// @param[out] h The calculated hue channel.
/// @param[out] s The calculated saturation channel.
/// @param[out] v The calculated value channel.
/// @param r The red colour channel.
/// @param g The green colour channel.
/// @param b The blue colour channel.
void rgbToHsv(float &h, float &s, float &v, float r, float g, float b);

/// Converts from RGB to HSV colour.
///
/// @param[out] h The calculated hue channel.
/// @param[out] s The calculated saturation channel.
/// @param[out] v The calculated value channel.
/// @param r The red colour channel.
/// @param g The green colour channel.
/// @param b The blue colour channel.
void rgbToHsv(float &h, float &s, float &v, uint8_t r, uint8_t g, uint8_t b);
}  // namespace colour

/// Defines an RGB or RGBA colour value. Each colour channel is represented by an unsigned byte.
struct ohmutil_API Colour
{
  /// The colour channels as indexed in @c rgba.
  enum
  {
    kR = 0,
    kG = 1,
    kB = 2,
    kA = 3
  };
  /// Colour shift values used in conversion to/from 32-bit integer.
  enum : unsigned
  {
    kAShift = 24,
    kRShift = 16,
    kGShift = 8,
    kBShift = 0
  };

  /// Maximum value of a single unsigned byte expressed as a floating point number.
  static constexpr float kMaxByteF = 255.0f;

  /// The rgba colour definition.
  std::array<uint8_t, 4> rgba;

  /// Empty constructor: the colour channels are uninitialised.
  inline Colour() = default;

  /// Initialise a colour with the given channel values.
  /// @param r The red colour channel.
  /// @param g The green colour channel.
  /// @param b The blue colour channel.
  /// @param a The alpha colour channel. Defaults to opaque.
  inline Colour(
    uint8_t r, uint8_t g, uint8_t b,
    uint8_t a = std::numeric_limits<uint8_t>::max()) noexcept  // NOLINT(cppcoreguidelines-pro-type-member-init)
  {
    rgba[kR] = r;
    rgba[kG] = g;
    rgba[kB] = b;
    rgba[kA] = a;
  }

  /// Copy constructor.
  /// @param c The colour to copy.
  inline Colour(const Colour &c) { *this = c; }  // NOLINT(cppcoreguidelines-pro-type-member-init)

  /// Initialise from a 32-bit integer.
  ///
  /// Colour channels are extracted according to the values of
  /// @c AShift, @c RShift, @c GShift and @c BShift.
  /// @param c The integer colour value.
  inline Colour(uint32_t c) noexcept  // NOLINT
  {
    const unsigned byte_mask = 0xffu;
    rgba[kA] = uint8_t((c >> kAShift) & byte_mask);
    rgba[kR] = uint8_t((c >> kRShift) & byte_mask);
    rgba[kG] = uint8_t((c >> kGShift) & byte_mask);
    rgba[kB] = uint8_t((c >> kBShift) & byte_mask);
  }

  /// Initialise a colour with the given floating point channel values.
  /// @param r The red colour channel.
  /// @param g The green colour channel.
  /// @param b The blue colour channel.
  /// @param a The alpha colour channel. Defaults to opaque.
  inline static Colour fromRgbf(float r, float g, float b, float a = 1.0f)
  {
    return Colour(uint8_t(r * kMaxByteF), uint8_t(g * kMaxByteF), uint8_t(b * kMaxByteF), uint8_t(a * kMaxByteF));
  }

  /// Convert to a 32-bit integer colour value.
  ///
  /// Colour channels are extracted according to the values of
  /// @c AShift, @c RShift, @c GShift and @c BShift.
  /// @return The integer colour representation.
  inline explicit operator uint32_t() const noexcept
  {
    uint32_t c = 0;
    c |= unsigned(rgba[kA]) << kAShift;
    c |= unsigned(rgba[kR]) << kRShift;
    c |= unsigned(rgba[kG]) << kGShift;
    c |= unsigned(rgba[kB]) << kBShift;
    return c;
  }

  /// Assignment operator.
  /// @param c The colour to copy.
  /// @return This.
  inline Colour &operator=(const Colour &c) noexcept
  {
    rgba[kR] = c.rgba[kR];
    rgba[kG] = c.rgba[kG];
    rgba[kB] = c.rgba[kB];
    rgba[kA] = c.rgba[kA];
    return *this;
  }

  /// Equality check operator.
  /// @param other The object to compare against.
  /// @return True if the colours are equal.
  inline bool operator==(const Colour &other) const
  {
    return rgba[kR] == other.rgba[kR] && rgba[kG] == other.rgba[kG] && rgba[kB] == other.rgba[kB] &&
           rgba[kA] == other.rgba[kA];
  }

  /// Inequality check operator.
  /// @param other The object to compare against.
  /// @return True if the colours are equal.
  inline bool operator!=(const Colour &other) const { return !operator==(other); }

  /// Access the alpha channel value.
  /// @return The alpha channel.
  inline uint8_t &a() { return rgba[kA]; }

  /// Access the red channel value.
  /// @return The red channel.
  inline uint8_t &r() { return rgba[kR]; }

  /// Access the green channel value.
  /// @return The green channel.
  inline uint8_t &g() { return rgba[kG]; }

  /// Access the 3 channel value.
  /// @return The blue channel.
  inline uint8_t &b() { return rgba[kB]; }

  /// Access the alpha channel value.
  /// @return The alpha channel.
  inline const uint8_t &a() const { return rgba[kA]; }

  /// Access the red channel value.
  /// @return The red channel.
  inline const uint8_t &r() const { return rgba[kR]; }

  /// Access the green channel value.
  /// @return The green channel.
  inline const uint8_t &g() const { return rgba[kG]; }

  /// Access the blue channel value.
  /// @return The blue channel.
  inline const uint8_t &b() const { return rgba[kB]; }

  /// Access red channel as a float.
  /// @return Red channel as a float.
  inline float rf() const { return float(r()) / kMaxByteF; }

  /// Access green channel as a float.
  /// @return Green channel as a float.
  inline float gf() const { return float(g()) / kMaxByteF; }

  /// Access blue channel as a float.
  /// @return Blue channel as a float.
  inline float bf() const { return float(b()) / kMaxByteF; }

  /// Access alpha channel as a float.
  /// @return Alpha channel as a float.
  inline float af() const { return float(a()) / kMaxByteF; }

  /// Set red channel from a float.
  /// @param channel Channel value.
  inline void setRf(float channel) { rgba[kR] = uint8_t(channel * kMaxByteF); }

  /// Set green channel from a float.
  /// @param channel Channel value.
  inline void setGf(float channel) { rgba[kG] = uint8_t(channel * kMaxByteF); }

  /// Set blue channel from a float.
  /// @param channel Channel value.
  inline void setBf(float channel) { rgba[kB] = uint8_t(channel * kMaxByteF); }

  /// Set alpha channel from a float.
  /// @param channel Channel value.
  inline void setAf(float channel) { rgba[kA] = uint8_t(channel * kMaxByteF); }

  /// Lighten or darken a colour by @p factor.
  /// Works in HSV space, multiplying the V value by @p factor and clamping the result [0, 1].
  /// @return The adjusted colour.
  Colour adjust(float factor) const;

  /// Lighten the colour by 1.5
  /// @return A lighter colour.
  inline Colour lighten() const { return adjust(1.5f); }

  /// Darken the colour by 0.5
  /// @return A darker colour.
  inline Colour darken() const { return adjust(0.5f); }

  /// Interpolate from one colour to another.
  /// @param from Colour to start at.
  /// @param to Colour to target at.
  /// @param factor Interpolation factor: [0, 1] or behaviour is undefined.
  /// @return A colour between the given colours.
  static Colour lerp(const Colour &from, const Colour &to, float factor);

  /// An enumeration of predefined colours (web colours).
  enum Names
  {
    // Blacks.
    kGainsboro,
    kLightGrey,
    kSilver,
    kDarkGray,
    kGray,
    kDimGray,
    kLightSlateGray,
    kSlateGray,
    kDarkSlateGray,
    kBlack,

    // Whites
    kWhite,
    kSnow,
    kHoneydew,
    kMintCream,
    kAzure,
    kAliceBlue,
    kGhostWhite,
    kWhiteSmoke,
    kSeashell,
    kBeige,
    kOldLace,
    kFloralWhite,
    kIvory,
    kAntiqueWhite,
    kLinen,
    kLavenderBlush,
    kMistyRose,

    // Pinks
    kPink,
    kLightPink,
    kHotPink,
    kDeepPink,
    kPaleVioletRed,
    kMediumVioletRed,

    // Reds
    kLightSalmon,
    kSalmon,
    kDarkSalmon,
    kLightCoral,
    kIndianRed,
    kCrimson,
    kFireBrick,
    kDarkRed,
    kRed,

    // Oranges
    kOrangeRed,
    kTomato,
    kCoral,
    kDarkOrange,
    kOrange,

    // Yellows
    kYellow,
    kLightYellow,
    kLemonChiffon,
    kLightGoldenrodYellow,
    kPapayaWhip,
    kMoccasin,
    kPeachPuff,
    kPaleGoldenrod,
    kKhaki,
    kDarkKhaki,
    kGold,

    // Browns
    kCornsilk,
    kBlanchedAlmond,
    kBisque,
    kNavajoWhite,
    kWheat,
    kBurlyWood,
    kTan,
    kRosyBrown,
    kSandyBrown,
    kGoldenrod,
    kDarkGoldenrod,
    kPeru,
    kChocolate,
    kSaddleBrown,
    kSienna,
    kBrown,
    kMaroon,

    // Greens
    kDarkOliveGreen,
    kOlive,
    kOliveDrab,
    kYellowGreen,
    kLimeGreen,
    kLime,
    kLawnGreen,
    kChartreuse,
    kGreenYellow,
    kSpringGreen,
    kMediumSpringGreen,
    kLightGreen,
    kPaleGreen,
    kDarkSeaGreen,
    kMediumSeaGreen,
    kSeaGreen,
    kForestGreen,
    kGreen,
    kDarkGreen,

    // Cyans
    kMediumAquamarine,
    kAqua,
    kCyan,
    kLightCyan,
    kPaleTurquoise,
    kAquamarine,
    kTurquoise,
    kMediumTurquoise,
    kDarkTurquoise,
    kLightSeaGreen,
    kCadetBlue,
    kDarkCyan,
    kTeal,

    // Blues
    kLightSteelBlue,
    kPowderBlue,
    kLightBlue,
    kSkyBlue,
    kLightSkyBlue,
    kDeepSkyBlue,
    kDodgerBlue,
    kCornflowerBlue,
    kSteelBlue,
    kRoyalBlue,
    kBlue,
    kMediumBlue,
    kDarkBlue,
    kNavy,
    kMidnightBlue,

    // Purples
    kLavender,
    kThistle,
    kPlum,
    kViolet,
    kOrchid,
    kFuchsia,
    kMagenta,
    kMediumOrchid,
    kMediumPurple,
    kBlueViolet,
    kDarkViolet,
    kDarkOrchid,
    kDarkMagenta,
    kPurple,
    kIndigo,
    kDarkSlateBlue,
    kSlateBlue,
    kMediumSlateBlue,

    kPredefinedCount
  };

  /// Predefined colours (web colours).
  static const std::array<Colour, kPredefinedCount> kColours;
};  // namespace ohm
}  // namespace ohm

#endif  // OHMUTIL_COLOUR_H


================================================
FILE: ohmutil/GlmStream.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMUTIL_GLMSTREAM_H
#define OHMUTIL_GLMSTREAM_H

#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>

#include <array>
#include <iostream>

namespace ohm
{
const int kMaxVectorElements = 32;
}  // namespace ohm

template <typename T>
inline void parseVector(std::istream &in, T *v, int elements)
{
  // Expecting:
  //  <value>,<value>[,...]

  char ch;
  bool brackets = false;
  if (in.peek() == '(')
  {
    brackets = true;
    in >> ch;
  }
  for (int i = 0; i < elements; ++i)
  {
    if (i != 0)
    {
      in >> ch;
      if (ch != ',')
      {
        in.setstate(std::ios::badbit | std::ios::failbit);
      }
    }
    in >> v[i];
  }
  if (brackets)
  {
    in >> ch;
    if (ch != ')')
    {
      in.setstate(std::ios::badbit | std::ios::failbit);
    }
  }
}


inline void parseVector(std::istream &in, char *v, int elements)
{
  std::array<int, ohm::kMaxVectorElements> vv;
  if (elements > int(vv.size()))
  {
    in.setstate(std::ios::badbit);
  }
  parseVector(in, vv.data(), elements);
  for (int i = 0; i < elements; ++i)
  {
    v[i] = char(vv[i]);
  }
}


inline void parseVector(std::istream &in, unsigned char *v, int elements)
{
  std::array<int, ohm::kMaxVectorElements> vv;
  if (elements > int(vv.size()))
  {
    in.setstate(std::ios::badbit);
  }
  parseVector(in, vv.data(), elements);
  for (int i = 0; i < elements; ++i)
  {
    v[i] = char(vv[i]);
  }
}

namespace glm
{
// From GLM 0.9.9.1, numbering starting including a patch number.
// So 0.9.8 was version 98, 0.9.9 was 99 and 0.9.9.1 was 991
// This assumes version 1.0 will be 1000
#if GLM_VERSION < 99
using GlmQualifier = precision;
#else   // GLM_VERSION
using GlmQualifier = qualifier;
#endif  // GLM_VERSION


template <typename T, GlmQualifier Q>
inline std::istream &operator>>(std::istream &in, tvec2<T, Q> &value)
{
  parseVector(in, value_ptr(value), 2);
  return in;
}


template <typename T, GlmQualifier Q>
inline std::istream &operator>>(std::istream &in, tvec3<T, Q> &value)
{
  parseVector(in, value_ptr(value), 3);
  return in;
}


template <typename T, GlmQualifier Q>
inline std::istream &operator>>(std::istream &in, tvec4<T, Q> &value)
{
  parseVector(in, value_ptr(value), 4);
  return in;
}

template <typename T, GlmQualifier Q>
inline std::ostream &operator<<(std::ostream &out, const tvec2<T, Q> &v)
{
  out << '(' << v.x << ',' << v.y << ')';
  return out;
}

template <typename T, GlmQualifier Q>
inline std::ostream &operator<<(std::ostream &out, const tvec3<T, Q> &v)
{
  out << '(' << v.x << ',' << v.y << ',' << v.z << ')';
  return out;
}

template <typename T, GlmQualifier Q>
inline std::ostream &operator<<(std::ostream &out, const tvec4<T, Q> &v)
{
  out << '(' << v.w << ',' << v.x << ',' << v.y << ',' << v.z << ')';
  return out;
}

template <GlmQualifier Q>
inline std::ostream &operator<<(std::ostream &out, const tvec3<char, Q> &v)
{
  out << '(' << int(v.x) << ',' << int(v.y) << ',' << int(v.z) << ')';
  return out;
}

template <GlmQualifier Q>
inline std::ostream &operator<<(std::ostream &out, const tvec3<unsigned char, Q> &v)
{
  out << '(' << int(v.x) << ',' << int(v.y) << ',' << int(v.z) << ')';
  return out;
}

template <GlmQualifier Q>
inline std::ostream &operator<<(std::ostream &out, const tvec4<char, Q> &v)
{
  out << '(' << int(v.w) << ',' << int(v.x) << ',' << int(v.y) << ',' << int(v.z) << ')';
  return out;
}

template <GlmQualifier Q>
inline std::ostream &operator<<(std::ostream &out, const tvec4<unsigned char, Q> &v)
{
  out << '(' << int(v.w) << ',' << int(v.x) << ',' << int(v.y) << ',' << int(v.z) << ')';
  return out;
}
}  // namespace glm

#endif  // OHMUTIL_GLMSTREAM_H


================================================
FILE: ohmutil/OhmUtilConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMUTILCONFIG_H
#define OHMUTILCONFIG_H

#include "OhmUtilExport.h"

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

#include <cmath>

/// Enable experimental parts of GLM, like `glm::length2()` (length squared)
#define GLM_ENABLE_EXPERIMENTAL

#endif  // OHMUTILCONFIG_H


================================================
FILE: ohmutil/Options.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMUTIL_OPTIONS_H
#define OHMUTIL_OPTIONS_H

#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4267)
#endif  // _MSC_VER

#include "GlmStream.h"

#include <cxxopts/cxxopts.hpp>

template <typename T>
inline std::string optStr(const T &value)
{
  std::ostringstream str;
  str << value;
  return str.str();
}

template <typename T>
inline std::shared_ptr<cxxopts::Value> optVal(T &val)
{
  return cxxopts::value(val)->default_value(optStr(val));
}

inline std::shared_ptr<cxxopts::Value> optVal(bool &val)
{
  return cxxopts::value(val)->implicit_value("true")->default_value(val ? "true" : "false");
}

#pragma warning(pop)

#endif  // OHMUTIL_OPTIONS_H


================================================
FILE: ohmutil/PlyMesh.cpp
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) 2014 CSIRO
//
#include "PlyMesh.h"

#include "SafeIO.h"

#include <algorithm>
#include <cstdarg>
#include <cstdio>
#include <iostream>

#ifndef ZU
#if defined(_MSC_VER)
#define ZU "Iu"
#else
#define ZU "zu"
#endif
#endif  // ZU

namespace ohm
{
namespace
{
inline bool isBigEndian()
{
  const union
  {
    uint32_t i;
    char c[4];  // NOLINT(modernize-avoid-c-arrays)
  } check_int = { 0x01020304 };

  return check_int.c[0] == 1;
}
}  // namespace

// File wrapper specialisations.
template <>
class PlyMesh::FileWrapper<FILE>
{
public:
  inline FileWrapper(FILE *file, bool close_file)
    : file_(file)
    , close_file_(close_file)
  {}
  inline ~FileWrapper()
  {
    if (file_ && close_file_)
    {
      fclose(file_);
    }
  }

  inline bool isOpen() const { return file_ != nullptr; }

  void printf(const char *format, ...)
  {
    va_list args;
    va_start(args, format);
    // Why does clang-tidy think args is not initialised?
    vfprintf(file_, format, args);  // NOLINT(clang-analyzer-valist.Uninitialized)
    va_end(args);
  }


  void write(const void *ptr, size_t element_size, size_t element_count = 1u)
  {
    fwrite(ptr, element_size, element_count, file_);
  }

private:
  FILE *file_;
  bool close_file_;
};


template <>
class PlyMesh::FileWrapper<std::ostream>
{
public:
  const unsigned kBufferSize = 1024u;  // Far to big for usage here.

  inline explicit FileWrapper(std::ostream &stream)
    : stream_(&stream)
  {
    buffer_ = new char[kBufferSize];
  }

  inline ~FileWrapper() { delete[] buffer_; }

  inline bool isOpen() const { return stream_ != nullptr; }

  void printf(const char *format, ...)
  {
    int written = 0;
    va_list args;
    va_start(args, format);
    // Why does clang-tidy think args is not initialised?
    written = vsnprintf(buffer_, kBufferSize, format, args);  // NOLINT(clang-analyzer-valist.Uninitialized)
    va_end(args);
    buffer_[std::min<unsigned>(written, kBufferSize)] = '\0';
    *stream_ << buffer_;
  }


  void write(const void *ptr, size_t element_size, size_t element_count = 1u)
  {
    stream_->write(static_cast<const std::ostream::char_type *>(ptr), element_size * element_count);
  }

private:
  std::ostream *stream_;
  char *buffer_;
};


PlyMesh::PlyMesh() = default;


PlyMesh::~PlyMesh() = default;


void PlyMesh::clear()
{
  index_mapper_.reset();
  vertices_.clear();
  edges_.clear();
  triangles_.clear();
  vertex_colours_ = edge_colours_ = face_colours_ = false;
}


unsigned PlyMesh::addVertices(const glm::vec3 *verts, unsigned count, const Colour *colours)
{
  return addVerticesT(verts, count, colours);
}


unsigned PlyMesh::addVertices(const glm::dvec3 *verts, unsigned count, const Colour *colours)
{
  return addVerticesT(verts, count, colours);
}


void PlyMesh::setNormal(unsigned vertex_index, const glm::vec3 &normal)
{
  setNormalT(vertex_index, normal);
}


void PlyMesh::setNormal(unsigned vertex_index, const glm::dvec3 &normal)
{
  setNormalT(vertex_index, normal);
}


void PlyMesh::addEdges(const unsigned *edge_indices, unsigned edge_count, const Colour *colours)
{
  for (unsigned i = 0; i < edge_count; ++i)
  {
    Edge e{};
    e.v[0] = edge_indices[i * 2 + 0];
    e.v[1] = edge_indices[i * 2 + 1];
    if (index_mapper_)
    {
      e.v[0] = (*index_mapper_)[e.v[0]];
      e.v[1] = (*index_mapper_)[e.v[1]];
    }
    e.colour = Colour::kColours[Colour::kWhite];
    if (colours)
    {
      edge_colours_ = true;
      e.colour = colours[i];
    }
    edges_.push_back(e);
  }
}


void PlyMesh::addEdge(const glm::vec3 &v0, const glm::vec3 &v1, const Colour &colour)
{
  addEdgeT(v0, v1, colour);
}


void PlyMesh::addEdge(const glm::dvec3 &v0, const glm::dvec3 &v1, const Colour &colour)
{
  addEdgeT(v0, v1, colour);
}


void PlyMesh::addTriangles(const unsigned *triangle_indices, unsigned triangle_count, const Colour *colours)
{
  for (unsigned i = 0; i < triangle_count; ++i)
  {
    Tri t{};
    t.v[0] = triangle_indices[i * 3 + 0];
    t.v[1] = triangle_indices[i * 3 + 1];
    t.v[2] = triangle_indices[i * 3 + 2];
    if (index_mapper_)
    {
      t.v[0] = (*index_mapper_)[t.v[0]];
      t.v[1] = (*index_mapper_)[t.v[1]];
      t.v[2] = (*index_mapper_)[t.v[2]];
    }
    t.colour = Colour::kColours[Colour::kWhite];
    if (colours)
    {
      face_colours_ = true;
      t.colour = colours[i];
    }
    triangles_.push_back(t);
  }
}


void PlyMesh::addTriangle(const glm::vec3 &v0, const glm::vec3 &v1, const glm::vec3 &v2, const Colour &colour)
{
  addTriangleT(v0, v1, v2, colour);
}


void PlyMesh::addTriangle(const glm::dvec3 &v0, const glm::dvec3 &v1, const glm::dvec3 &v2, const Colour &colour)
{
  addTriangleT(v0, v1, v2, colour);
}


void PlyMesh::addPolygon(const unsigned *indices, unsigned order, const Colour &colour)
{
  if (order == 0)
  {
    return;
  }

  Poly poly{};
  poly.indices_start = polygon_indices_.size();
  poly.order = order;
  poly.colour = colour;

  // Use push_back() for indices, not resize() to allow vector
  // to increase in size in a more expansive way (reduce reallocation).
  for (unsigned i = 0; i < order; ++i)
  {
    polygon_indices_.push_back(indices[i]);
  }

  face_colours_ = face_colours_ || colour != Colour::kColours[Colour::kWhite];

  polygons_.push_back(poly);
}


void PlyMesh::addPolygon(const glm::vec3 *verts, unsigned order, const Colour &colour)
{
  addPolygonT(verts, order, colour);
}


void PlyMesh::addPolygon(const glm::dvec3 *verts, unsigned order, const Colour &colour)
{
  addPolygonT(verts, order, colour);
}


void PlyMesh::addMappedTriangle(const glm::vec3 *verts, const unsigned *vert_ids, const Colour *colour)
{
  addMappedTriangleT(verts, vert_ids, colour);
}


void PlyMesh::addMappedTriangle(const glm::dvec3 *verts, const unsigned *vert_ids, const Colour *colour)
{
  addMappedTriangleT(verts, vert_ids, colour);
}


void PlyMesh::addMappedPolygon(const glm::vec3 *verts, const unsigned *vert_ids, unsigned order, const Colour *colour)
{
  addMappedPolygonT(verts, vert_ids, order, colour);
}


void PlyMesh::addMappedPolygon(const glm::dvec3 *verts, const unsigned *vert_ids, unsigned order, const Colour *colour)
{
  addMappedPolygonT(verts, vert_ids, order, colour);
}


void PlyMesh::addMappedEdge(const glm::vec3 *verts, const unsigned *vert_ids, const Colour *colour)
{
  addMappedEdgeT(verts, vert_ids, colour);
}


void PlyMesh::addMappedEdge(const glm::dvec3 *verts, const unsigned *vert_ids, const Colour *colour)
{
  addMappedEdgeT(verts, vert_ids, colour);
}


void PlyMesh::addComment(const std::string &comment)
{
  comments_.emplace_back(comment);
}


std::string PlyMesh::getComment(unsigned index) const
{
  if (index >= comments_.size())
  {
    return std::string();
  }

  return comments_[index];
}


unsigned PlyMesh::commentCount() const
{
  return unsigned(comments_.size());
}


void PlyMesh::clearComments()
{
  comments_.clear();
}


bool PlyMesh::save(const std::string &out_path, bool binary) const
{
  FILE *cfile = nullptr;
  if (fopen_s(&cfile, out_path.c_str(), "wb"))
  {
    return false;
  }
  FileWrapper<FILE> out(cfile, true);
  return save(out, binary);
}


bool PlyMesh::save(FILE *file, bool binary) const
{
  FileWrapper<FILE> out(file, false);
  return save(out, binary);
}


bool PlyMesh::save(std::ostream &stream, bool binary) const
{
  FileWrapper<std::ostream> out(stream);
  return save(out, binary);
}


template <typename T>
bool PlyMesh::save(FileWrapper<T> &out, bool binary) const
{
  if (!out.isOpen())
  {
    return false;
  }
  bool with_normals = false;

  out.printf("ply\n");
  if (binary)
  {
    if (isBigEndian())
    {
      out.printf("format binary_big_endian 1.0\n");
    }
    else
    {
      out.printf("format binary_little_endian 1.0\n");
    }
  }
  else
  {
    out.printf("format ascii 1.0\n");
  }
  out.printf("comment Exported by ohmutil PlyMesh\n");

  for (const std::string &comment : comments_)
  {
    out.printf("comment %s\n", comment.c_str());
  }

  // Write vertex info.
  out.printf("element vertex %" ZU "\n", vertices_.size());
  out.printf("property float x\n");
  out.printf("property float y\n");
  out.printf("property float z\n");
  if (!normals_.empty())
  {
    with_normals = true;
    out.printf("property float nx\n");
    out.printf("property float ny\n");
    out.printf("property float nz\n");
  }
  if (vertex_colours_)
  {
    out.printf("property uchar red\n");
    out.printf("property uchar green\n");
    out.printf("property uchar blue\n");
  }

  if (!triangles_.empty() || !polygons_.empty())
  {
    out.printf("element face %" ZU "\n", triangles_.size() + polygons_.size());
    out.printf("property list uchar int vertex_indices\n");
    if (face_colours_)
    {
      out.printf("property uchar red\n");
      out.printf("property uchar green\n");
      out.printf("property uchar blue\n");
    }
  }

  if (!edges_.empty())
  {
    out.printf("element edge %" ZU "\n", edges_.size());
    out.printf("property int vertex1\n");
    out.printf("property int vertex2\n");
    if (edge_colours_)
    {
      out.printf("property uchar red\n");
      out.printf("property uchar green\n");
      out.printf("property uchar blue\n");
    }
  }

  out.printf("end_header\n");

  // Write vertices.
  for (size_t i = 0; i < vertices_.size(); ++i)
  {
    const Vertex &v = vertices_[i];
    VertexType n = (with_normals && i < normals_.size()) ? normals_[i] : VertexType(0, 0, 0);
    if (binary)
    {
      out.write(&v.point.x, sizeof(float), 1u);
      out.write(&v.point.y, sizeof(float), 1u);
      out.write(&v.point.z, sizeof(float), 1u);

      if (with_normals)
      {
        out.write(&n.x, sizeof(float), 1u);
        out.write(&n.y, sizeof(float), 1u);
        out.write(&n.z, sizeof(float), 1u);
      }

      if (vertex_colours_)
      {
        out.write(&v.colour.r(), 1u, 1u);
        out.write(&v.colour.g(), 1u, 1u);
        out.write(&v.colour.b(), 1u, 1u);
      }
    }
    else
    {
      out.printf("%f %f %f", v.point.x, v.point.y, v.point.z);
      if (with_normals)
      {
        out.printf(" %f %f %f", n.x, n.y, n.z);
      }
      if (vertex_colours_)
      {
        out.printf(" %d %d %d", unsigned(v.colour.r()), unsigned(v.colour.g()), unsigned(v.colour.b()));
      }
      out.printf("\n");
    }
  }

  // Write triangle faces.
  for (const Tri &t : triangles_)
  {
    if (binary)
    {
      unsigned char vc = 3;
      out.write(&vc, sizeof(vc), 1u);
      out.write(t.v.data(), sizeof(t.v[0]), t.v.size());
      if (face_colours_)
      {
        out.write(&t.colour.r(), 1u, 1u);
        out.write(&t.colour.g(), 1u, 1u);
        out.write(&t.colour.b(), 1u, 1u);
      }
    }
    else
    {
      const unsigned vc = 3;
      out.printf("%u %u %u %u", vc, t.v[0], t.v[1], t.v[2]);
      if (face_colours_)
      {
        out.printf(" %d %d %d", unsigned(t.colour.r()), unsigned(t.colour.g()), unsigned(t.colour.b()));
      }
      out.printf("\n");
    }
  }

  // Write non triangle faces.
  for (const Poly &poly : polygons_)
  {
    if (binary)
    {
      unsigned char vc = poly.order;
      out.write(&vc, sizeof(vc), 1u);
      out.write(&polygon_indices_[poly.indices_start], sizeof(polygon_indices_.front()), poly.order);
      if (face_colours_)
      {
        out.write(&poly.colour.r(), 1u, 1u);
        out.write(&poly.colour.g(), 1u, 1u);
        out.write(&poly.colour.b(), 1u, 1u);
      }
    }
    else
    {
      out.printf("%u", poly.order);
      for (unsigned j = 0; j < poly.order; ++j)
      {
        out.printf(" %u", polygon_indices_[poly.indices_start + j]);
      }
      if (face_colours_)
      {
        out.printf(" %d %d %d", unsigned(poly.colour.r()), unsigned(poly.colour.g()), unsigned(poly.colour.b()));
      }
      out.printf("\n");
    }
  }

  // Write edges/lines.
  for (const Edge &e : edges_)
  {
    if (binary)
    {
      out.write(e.v.data(), sizeof(e.v[0]), e.v.size());
      if (edge_colours_)
      {
        out.write(&e.colour.r(), 1u, 1u);
        out.write(&e.colour.g(), 1u, 1u);
        out.write(&e.colour.b(), 1u, 1u);
      }
    }
    else
    {
      out.printf("%u %u", e.v[0], e.v[1]);
      if (edge_colours_)
      {
        out.printf(" %d %d %d", unsigned(e.colour.r()), unsigned(e.colour.g()), unsigned(e.colour.b()));
      }
      out.printf("\n");
    }
  }

  return true;
}


template bool PlyMesh::save(FileWrapper<FILE> &out, bool binary) const;
template bool PlyMesh::save(FileWrapper<std::ostream> &out, bool binary) const;


template <typename VEC3>
unsigned PlyMesh::addVerticesT(const VEC3 *verts, unsigned count, const Colour *colours)
{
  auto index = unsigned(vertices_.size());
  for (unsigned i = 0; i < count; ++i)
  {
    Vertex v{};
    v.point = VertexType(verts[i]);
    v.colour = Colour::kColours[Colour::kWhite];
    if (colours)
    {
      vertex_colours_ = true;
      v.colour = colours[i];
    }
    vertices_.push_back(v);
  }
  return index;
}

template unsigned PlyMesh::addVerticesT(const glm::vec3 *verts, unsigned count, const Colour *colours);
template unsigned PlyMesh::addVerticesT(const glm::dvec3 *verts, unsigned count, const Colour *colours);


template <typename VEC3>
void PlyMesh::setNormalT(unsigned vertex_index, const VEC3 &normal)
{
  if (normals_.size() <= vertex_index)
  {
    normals_.reserve(std::max(size_t(vertex_index) + 1, vertices_.size()));
    while (normals_.size() <= vertex_index)
    {
      normals_.emplace_back(VertexType(0, 0, 0));
    }
  }

  normals_[vertex_index] = VertexType(normal);
}

template void PlyMesh::setNormalT(unsigned vertex_index, const glm::vec3 &normal);
template void PlyMesh::setNormalT(unsigned vertex_index, const glm::dvec3 &normal);

template <typename VEC3>
void PlyMesh::addEdgeT(const VEC3 &v0, const VEC3 &v1, const Colour &colour)
{
  const auto i0 = unsigned(vertices_.size());
  addVertices(&v0, 1);
  const auto i1 = unsigned(vertices_.size());
  addVertices(&v1, 1);
  addEdge(i0, i1, colour);
}

template void PlyMesh::addEdgeT(const glm::vec3 &v0, const glm::vec3 &v1, const Colour &colour);
template void PlyMesh::addEdgeT(const glm::dvec3 &v0, const glm::dvec3 &v1, const Colour &colour);

template <typename VEC3>
void PlyMesh::addTriangleT(const VEC3 &v0, const VEC3 &v1, const VEC3 &v2, const Colour &colour)
{
  const auto i0 = unsigned(vertices_.size());
  addVertices(&v0, 1);
  const auto i1 = unsigned(vertices_.size());
  addVertices(&v1, 1);
  const auto i2 = unsigned(vertices_.size());
  addVertices(&v2, 1);
  addTriangle(i0, i1, i2, colour);
}

template void PlyMesh::addTriangleT(const glm::vec3 &v0, const glm::vec3 &v1, const glm::vec3 &v2,
                                    const Colour &colour);
template void PlyMesh::addTriangleT(const glm::dvec3 &v0, const glm::dvec3 &v1, const glm::dvec3 &v2,
                                    const Colour &colour);

template <typename VEC3>
void PlyMesh::addPolygonT(const VEC3 *verts, unsigned order, const Colour &colour)
{
  if (!order)
  {
    return;
  }

  Poly poly{};
  poly.indices_start = polygon_indices_.size();
  poly.order = order;
  poly.colour = colour;

  // Use push_back() for indices, not resize() to allow vector
  // to increase in size in a more expansive way (reduce reallocation).
  const auto index_offset = unsigned(vertices_.size());
  addVertices(verts, order);
  for (unsigned i = 0; i < order; ++i)
  {
    polygon_indices_.push_back(index_offset + i);
  }

  face_colours_ = face_colours_ || colour != Colour::kColours[Colour::kWhite];

  polygons_.push_back(poly);
}

template void PlyMesh::addPolygonT(const glm::vec3 *verts, unsigned order, const Colour &colour);
template void PlyMesh::addPolygonT(const glm::dvec3 *verts, unsigned order, const Colour &colour);

template <typename VEC3>
void PlyMesh::addMappedTriangleT(const VEC3 *verts, const unsigned *vert_ids, const Colour *colour)
{
  if (!index_mapper_)
  {
    index_mapper_ = std::make_unique<std::unordered_map<unsigned, unsigned>>();
  }

  Tri tri{};
  tri.colour = (colour) ? *colour : Colour(kDefaultColour);
  face_colours_ = face_colours_ || colour != nullptr;
  for (unsigned i = 0; i < 3; ++i)
  {
    auto search = index_mapper_->find(vert_ids[i]);
    if (search == index_mapper_->end())
    {
      tri.v[i] = uint32_t(vertices_.size());
      addVertex(verts[i]);
    }
    else
    {
      tri.v[i] = search->second;
    }
  }
  triangles_.push_back(tri);
}

template void PlyMesh::addMappedTriangleT(const glm::vec3 *verts, const unsigned *vert_ids, const Colour *colour);
template void PlyMesh::addMappedTriangleT(const glm::dvec3 *verts, const unsigned *vert_ids, const Colour *colour);

template <typename VEC3>
void PlyMesh::addMappedPolygonT(const VEC3 *verts, const unsigned *vert_ids, unsigned order, const Colour *colour)
{
  if (!index_mapper_)
  {
    index_mapper_ = std::make_unique<std::unordered_map<unsigned, unsigned>>();
  }

  Poly poly{};
  poly.indices_start = polygon_indices_.size();
  poly.order = 0;
  poly.colour = (colour) ? *colour : Colour(kDefaultColour);
  face_colours_ = face_colours_ || colour != nullptr;
  for (unsigned i = 0; i < order; ++i)
  {
    auto search = index_mapper_->find(vert_ids[i]);
    if (search == index_mapper_->end())
    {
      polygon_indices_.push_back(uint32_t(vertices_.size()));
      addVertex(verts[i]);
    }
    else
    {
      polygon_indices_.push_back(search->second);
    }
  }
  polygons_.push_back(poly);
}

template void PlyMesh::addMappedPolygonT(const glm::vec3 *verts, const unsigned *vert_ids, unsigned order,
                                         const Colour *colour);
template void PlyMesh::addMappedPolygonT(const glm::dvec3 *verts, const unsigned *vert_ids, unsigned order,
                                         const Colour *colour);

template <typename VEC3>
void PlyMesh::addMappedEdgeT(const VEC3 *verts, const unsigned *vert_ids, const Colour *colour)
{
  if (!index_mapper_)
  {
    index_mapper_ = std::make_unique<std::unordered_map<unsigned, unsigned>>();
  }

  Edge edge{};
  edge.colour = (colour) ? *colour : Colour(kDefaultColour);
  edge_colours_ = edge_colours_ || colour != nullptr;
  for (unsigned i = 0; i < 2; ++i)
  {
    auto search = index_mapper_->find(vert_ids[i]);
    if (search == index_mapper_->end())
    {
      edge.v[i] = uint32_t(vertices_.size());
      addVertex(verts[i]);
    }
    else
    {
      edge.v[i] = search->second;
    }
  }
  edges_.push_back(edge);
}

template void PlyMesh::addMappedEdgeT(const glm::vec3 *verts, const unsigned *vert_ids, const Colour *colour);
template void PlyMesh::addMappedEdgeT(const glm::dvec3 *verts, const unsigned *vert_ids, const Colour *colour);

}  // namespace ohm


================================================
FILE: ohmutil/PlyMesh.h
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) 2014 CSIRO
//
#ifndef OHMUTIL_PLYMESH_H
#define OHMUTIL_PLYMESH_H

#include "OhmUtilExport.h"

#include <glm/glm.hpp>

#include <array>
#include <cstddef>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>

#include "Colour.h"

namespace ohm
{
/// Represents a mesh which can be saved to Ply. Intended as a debugging device.
class ohmutil_API PlyMesh
{
public:
  static const uint32_t kDefaultColour = 0xffffffff;  ///< Default colour (white)
  using VertexType = glm::vec3;                       ///< Vertex typedef.

  /// Constructor.
  PlyMesh();
  /// Typical destructor.
  ~PlyMesh();

  /// Clear the current mesh content. Memory is preserved.
  void clear();

  /// Add an array of vertices to the mesh.
  /// @param verts Array of vertices to add.
  /// @param count Number of vertices to add.
  /// @param colours Optional colours for the vertices. Enables colour export if specified.
  /// @return The index of the first vertex in @p verts after adding.
  unsigned addVertices(const glm::vec3 *verts, unsigned count, const Colour *colours = nullptr);
  /// Add a single vertex to the mesh
  /// @param vert The vertex to add
  /// @return The index of the added vertex.
  inline unsigned addVertex(const glm::vec3 &vert) { return addVertices(&vert, 1, nullptr); }
  /// Add a single coloured vertex to the mesh
  /// @param vert The vertex to add
  /// @param colour Colour value for the vertex.
  /// @return The index of the added vertex.
  inline unsigned addVertex(const glm::vec3 &vert, const Colour &colour) { return addVertices(&vert, 1, &colour); }

  /// @overload
  unsigned addVertices(const glm::dvec3 *verts, unsigned count, const Colour *colours = nullptr);
  /// @overload
  inline unsigned addVertex(const glm::dvec3 &vert) { return addVertices(&vert, 1, nullptr); }
  /// @overload
  inline unsigned addVertex(const glm::dvec3 &vert, const Colour &colour) { return addVertices(&vert, 1, &colour); }

  /// Set the normal for a vertex in the mesh.
  /// @param vertex_index Index of the vertex to add a normal for. Must be in range.
  /// @param normal The vertex normal value.
  void setNormal(unsigned vertex_index, const glm::vec3 &normal);
  /// @overload
  void setNormal(unsigned vertex_index, const glm::dvec3 &normal);

  /// Add edge data to the ply mesh
  /// @param edge_indices Vertex index pairs for the set of edges to add. Expects `edge_count * 2` elements.
  /// Vertex indices must be in range.
  /// @param edge_count Number of edges being added.
  /// @param colours Colour values for the added edges. Expects @p edge_count items or null to not add edge colours.
  void addEdges(const unsigned *edge_indices, unsigned edge_count, const Colour *colours = nullptr);
  /// @overload
  inline void addEdge(unsigned v0, unsigned v1)
  {
    const std::array<unsigned, 2> vi = { v0, v1 };
    return addEdges(vi.data(), 1, nullptr);
  }
  /// Add a single edge to the mesh
  /// @param v0 Index of the first vertex in the edge.
  /// @param v1 Index of the second vertex in the edge.
  /// @param colour Colour value for the edge.
  inline void addEdge(unsigned v0, unsigned v1, const Colour &colour)
  {
    const std::array<unsigned, 2> vi = { v0, v1 };
    return addEdges(vi.data(), 1, &colour);
  }
  /// Add two vertices to the mesh and connect them as a coloured edge
  /// @param v0 First vertex in the edge.
  /// @param v1 Second vertex in the edge
  /// @param colour Colour value for the edge.
  void addEdge(const glm::vec3 &v0, const glm::vec3 &v1, const Colour &colour);
  /// @overload
  void addEdge(const glm::dvec3 &v0, const glm::dvec3 &v1, const Colour &colour);

  /// Add indexed triangles to the mesh
  /// @param triangle_indices Triangle index triples for the triangles to add. Expects `triangle_count * 3` elements.
  /// Vertex indices must be in range.
  /// @param triangle_count Number of triangles being added.
  /// @param colours Colour values for the added triangles. Expects @p triangle_count items or null to not add edge
  /// colours.
  void addTriangles(const unsigned *triangle_indices, unsigned triangle_count, const Colour *colours = nullptr);
  /// @overload
  inline void addTriangle(unsigned v0, unsigned v1, unsigned v2)
  {
    const std::array<unsigned, 3> vi = { v0, v1, v2 };
    return addTriangles(vi.data(), 1, nullptr);
  }
  /// Add a single triangle to the mesh
  /// @param v0 Index of the first vertex in the triangle.
  /// @param v1 Index of the second vertex in the triangle.
  /// @param v2 Index of the third vertex in the triangle.
  /// @param colour Colour value for the triangle.
  inline void addTriangle(unsigned v0, unsigned v1, unsigned v2, const Colour &colour)
  {
    const std::array<unsigned, 3> vi = { v0, v1, v2 };
    return addTriangles(vi.data(), 1, &colour);
  }
  /// Add three vertices to the mesh and connect them as a coloured triangle
  /// @param v0 First vertex in the triangle.
  /// @param v1 Second vertex in the triangle.
  /// @param v2 Third vertex in the triangle.
  /// @param colour Colour value for triangle.
  void addTriangle(const glm::vec3 &v0, const glm::vec3 &v1, const glm::vec3 &v2, const Colour &colour);
  /// @overload
  void addTriangle(const glm::dvec3 &v0, const glm::dvec3 &v1, const glm::dvec3 &v2, const Colour &colour);

  /// Add an indexed polygon to the mesh.
  /// @param indices Vertex indices for the polygon. The expected number of elements at this address is determined by
  /// the value of @p order . Vertex indices must be in range.
  /// @param order The number of vertices in the polygon, therefore the number of elements at @p indices .
  /// @param colour Colour value for the polygon.
  void addPolygon(const unsigned *indices, unsigned order, const Colour &colour);
  /// Add a polygon to the mesh adding new vertices for the polygon at the same time.
  /// @param verts Vertex positions for the polygon. The expected number of elements at this address is determined by
  /// the value of @p order .
  /// @param order The number of vertices in the polygon, therefore the number of elements at @p indices .
  /// @param colour Colour value for the polygon.
  void addPolygon(const glm::vec3 *verts, unsigned order, const Colour &colour);
  /// @overload
  void addPolygon(const glm::dvec3 *verts, unsigned order, const Colour &colour);

  /// Add a triangle with support for vertex mapping.
  ///
  /// This function adds the triangle defined by @p verts, but re-uses existing vertices.
  /// A previously added vertex is used if a vertex with an id matching @p vertIds[n] exists.
  /// In this case the previous value is used and the value for @p vert[n] is ignored.
  ///
  /// This can be used to add parts from another indexed mesh. Triangles and vertices can be added
  /// from the original mesh using this function rather than added vertices and triangles separately.
  /// The @p vertIds will be the indices from the original mesh.
  ///
  /// @param verts An array of three vertex positions.
  /// @param vert_ids An array of three vertex Ids for @p verts.
  /// @param colour An optional colour for the face. Single item only, for the face not the vertices.
  void addMappedTriangle(const glm::vec3 *verts, const unsigned *vert_ids, const Colour *colour = nullptr);
  /// @overload
  void addMappedTriangle(const glm::dvec3 *verts, const unsigned *vert_ids, const Colour *colour = nullptr);

  /// Similar to @c addMappedTriangle() supporting arbitrary order polygons.
  /// @param verts An array of vertex positions. The expected number of elements at this address is determined by
  /// the value of @p order .
  /// @param vert_ids An array of vertex Ids for @p verts.
  /// @param order The number of vertices in the polygon, therefore the number of elements at @p indices .
  /// @param colour An optional colour for the face. Single item only, for the face not the vertices.
  void addMappedPolygon(const glm::vec3 *verts, const unsigned *vert_ids, unsigned order, const Colour *colour);
  /// @overload
  void addMappedPolygon(const glm::dvec3 *verts, const unsigned *vert_ids, unsigned order, const Colour *colour);

  /// Add an edge with support for vertex mapping.
  ///
  /// This function behaves much as @c addMappedTriangle(), but for edges.
  /// @param verts An array of two vertex positions.
  /// @param vert_ids An array of two vertex Ids for @p verts.
  /// @param colour An optional colour for the edge. Single item only, for the edge not the vertices.
  void addMappedEdge(const glm::vec3 *verts, const unsigned *vert_ids, const Colour *colour = nullptr);
  /// @overload
  inline void addMappedEdge(const glm::vec3 *verts, const unsigned *vert_ids, const Colour &colour)
  {
    return addMappedEdge(verts, vert_ids, &colour);
  }
  /// @overload
  void addMappedEdge(const glm::dvec3 *verts, const unsigned *vert_ids, const Colour *colour = nullptr);
  /// @overload
  inline void addMappedEdge(const glm::dvec3 *verts, const unsigned *vert_ids, const Colour &colour)
  {
    return addMappedEdge(verts, vert_ids, &colour);
  }

  /// Add a comment to the PLY file. Comment will be written on calling @c save() .
  /// @param comment The comment string.
  void addComment(const std::string &comment);
  /// Get a previously added comment string.
  /// @param index Index of the comment string. Must be in range `[0, commentCount())`
  /// @return The requested comment string.
  std::string getComment(unsigned index) const;
  /// Query the number of comment strings which have been added using @c addComment() .
  /// @return The number of added comment strings.
  unsigned commentCount() const;
  /// Remove all added comments.
  void clearComments();

  /// Save the PLY mesh to the given @p out_path .
  /// @param out_path File path specifying where to save. String should include the `.ply` extension.
  /// @param binary Save in binary PLY format? False for ASCII.
  /// @return True on success.
  bool save(const std::string &out_path, bool binary) const;
  /// Save the PLY mesh using a @c FILE object.
  /// @param outfile @c FILE object to save to.
  /// @param binary Save in binary PLY format? False for ASCII.
  /// @return True on success.
  bool save(FILE *outfile, bool binary) const;
  /// Save the PLY mesh to the given @c stream .
  /// @param stream The output stream to save to.
  /// @param binary Save in binary PLY format? False for ASCII.
  /// @return True on success.
  bool save(std::ostream &stream, bool binary) const;

  /// API adaptor for either @c std::ostream or @c FILE .
  ///
  /// Specialisations are internal. Declaration is here to support the declarations below.
  template <typename T>
  class FileWrapper
  {
  public:
    /// Check if the file open?
    /// @return True if open.
    inline bool isOpen() const { return false; }
    /// Write text to the file using @c printf() style formatting.
    /// @param format @c printf format string
    inline void printf(const char *format, ...) = delete;
    /// Write binary data to the file.
    /// @param ptr Pointer to the data to write.
    /// @param element_size Size of a single item at @p ptr
    /// @param element_count Number of elements at @p ptr to write.
    inline void write(const void *ptr, size_t element_size, size_t element_count) = delete;
  };

private:
  template <typename T>
  bool save(FileWrapper<T> &out, bool binary) const;

  template <typename VEC3>
  unsigned addVerticesT(const VEC3 *verts, unsigned count, const Colour *colours);

  template <typename VEC3>
  void setNormalT(unsigned vertex_index, const VEC3 &normal);

  template <typename VEC3>
  void addEdgeT(const VEC3 &v0, const VEC3 &v1, const Colour &colour);

  template <typename VEC3>
  void addTriangleT(const VEC3 &v0, const VEC3 &v1, const VEC3 &v2, const Colour &colour);

  template <typename VEC3>
  void addPolygonT(const VEC3 *verts, unsigned order, const Colour &colour);

  template <typename VEC3>
  void addMappedTriangleT(const VEC3 *verts, const unsigned *vert_ids, const Colour *colour = nullptr);
  template <typename VEC3>
  void addMappedPolygonT(const VEC3 *verts, const unsigned *vert_ids, unsigned order, const Colour *colour);
  template <typename VEC3>
  void addMappedEdgeT(const VEC3 *verts, const unsigned *vert_ids, const Colour *colour = nullptr);

  struct Vertex  // NOLINT(cppcoreguidelines-pro-type-member-init)
  {
    VertexType point;
    Colour colour;
  };

  struct Edge  // NOLINT(cppcoreguidelines-pro-type-member-init)
  {
    std::array<uint32_t, 2> v;
    Colour colour;
  };

  struct Tri  // NOLINT(cppcoreguidelines-pro-type-member-init)
  {
    std::array<uint32_t, 3> v;
    Colour colour;
  };

  struct Poly  // NOLINT(cppcoreguidelines-pro-type-member-init)
  {
    size_t indices_start;  ///< Index into _polygonIndices for first index.
    unsigned order;        ///< Number of indices in the polygon (and in _polygonIndices).
    Colour colour;
  };

  std::vector<Vertex> vertices_;
  std::vector<VertexType> normals_;
  std::vector<Edge> edges_;
  std::vector<Tri> triangles_;
  std::vector<Poly> polygons_;
  std::vector<uint32_t> polygon_indices_;  ///< Stores polygon indices.
  std::vector<std::string> comments_;      ///< List of comments to add to the file.
  bool vertex_colours_ = false;
  bool edge_colours_ = false;
  bool face_colours_ = false;

  std::unique_ptr<std::unordered_map<unsigned, unsigned>> index_mapper_;
};

}  // namespace ohm

#endif  // OHMUTIL_PLYMESH_H


================================================
FILE: ohmutil/PlyPointStream.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "PlyPointStream.h"

#include <glm/vec3.hpp>

#include <array>
#include <iomanip>
#include <iostream>
#include <limits>
#include <stdexcept>

namespace ohm
{
namespace
{
template <typename T>
struct PlyTypeOf
{
};

#define PLYTYPEOF(type_decl, enum_value)                 \
  template <>                                            \
  struct PlyTypeOf<type_decl>                            \
  {                                                      \
    static const PlyPointStream::Type type = enum_value; \
  }

PLYTYPEOF(int8_t, PlyPointStream::Type::kInt8);
PLYTYPEOF(uint8_t, PlyPointStream::Type::kUInt8);
PLYTYPEOF(int16_t, PlyPointStream::Type::kInt16);
PLYTYPEOF(uint16_t, PlyPointStream::Type::kUInt16);
PLYTYPEOF(int32_t, PlyPointStream::Type::kInt32);
PLYTYPEOF(uint32_t, PlyPointStream::Type::kUInt32);
PLYTYPEOF(float, PlyPointStream::Type::kFloat32);
PLYTYPEOF(double, PlyPointStream::Type::kFloat64);


bool isBigEndian()
{
  union
  {
    uint32_t i;
    char c[sizeof(uint32_t)];  // NOLINT(modernize-avoid-c-arrays)
  } bint = { 0x01020304 };     // NOLINT(readability-magic-numbers)

  return bint.c[0] == 1;
}


/// Helper for use with writing a PLY element count to an @c ostream .
///
/// Usage:
/// ```cpp
/// std::cout << "element vertex " << ElementCount(point_count) << '\n';
/// ```
///
/// This will prefix the output value with '0' to maximum width of the @c ElementCount::value . This can be used
/// to write a place holder value at the start of the stream, then to return and write the correct value.
class ElementCount
{
public:
  /// Constructor
  /// @param count The value for the element count.
  explicit ElementCount(uint64_t count = 0)
    : value(count)
  {}

  uint64_t value = 0;  ///< The element count value to write.
};


/// Streaming operator for an @c ElementCount .
///
/// This writes the @c ElementCount::value padded with leading zeros and can be used to support writing a placeholder
/// value, then later returning the stream to write the corrected value.
/// @param out The output stream to write.
/// @param count The @c ElementCount to write.
/// @return @c out
std::ostream &operator<<(std::ostream &out, const ElementCount &count)
{
  auto restore_fill = out.fill();
  auto restore_width = out.width();
  out << std::setfill('0') << std::setw(std::numeric_limits<decltype(count.value)>::digits10) << count.value;
  out.fill(restore_fill);
  out.width(restore_width);
  return out;
}
}  // namespace

PlyPointStream::PlyPointStream() = default;

PlyPointStream::PlyPointStream(const std::vector<Property> &properties)
{
  properties_ = properties;
  values_.resize(properties_.size());
}


PlyPointStream::PlyPointStream(const std::vector<Property> &properties, std::ostream &out)
{
  properties_ = properties;
  values_.resize(properties_.size());
  open(out);
}


PlyPointStream::PlyPointStream(PlyPointStream &&other) noexcept
  : out_(std::exchange(other.out_, nullptr))
  , properties_(std::move(other.properties_))
  , values_(std::move(other.values_))
  , point_count_(std::exchange(other.point_count_, 0))
  , point_count_pos_(std::exchange(other.point_count_pos_, -1))
{}


PlyPointStream::~PlyPointStream()
{
  close();
}


PlyPointStream &PlyPointStream::operator=(PlyPointStream &&other) noexcept
{
  out_ = std::exchange(other.out_, nullptr);
  properties_ = std::move(other.properties_);
  values_ = std::move(other.values_);
  point_count_ = std::exchange(other.point_count_, 0);
  point_count_pos_ = std::exchange(other.point_count_pos_, -1);
  return *this;
}


bool PlyPointStream::setProperties(const std::vector<Property> &properties)
{
  if (isOpen())
  {
    return false;
  }

  properties_ = properties;
  values_.resize(properties_.size());

  return true;
}


void PlyPointStream::open(std::ostream &out)
{
  close();
  out_ = &out;
  point_count_pos_ = -1;
  point_count_pos_ = 0;
  writeHeader();
}


bool PlyPointStream::close()
{
  bool ok = false;
  if (isOpen())
  {
    ok = finalisePointCount();
    out_ = nullptr;
  }
  return ok;
}


bool PlyPointStream::isOpen() const
{
  return out_ != nullptr;
}


bool PlyPointStream::setPointPosition(const glm::dvec3 &pos)
{
  return setProperty("x", pos.x) && setProperty("y", pos.y) && setProperty("z", pos.z);
}


bool PlyPointStream::setPointTimestamp(double time)
{
  return setProperty("time", time);
}


bool PlyPointStream::setPointNormal(const glm::dvec3 &normal)
{
  return setProperty("nx", normal.x) && setProperty("ny", normal.y) && setProperty("nz", normal.z);
}


bool PlyPointStream::setProperty(const std::string &name, int8_t value)
{
  return setPropertyT(name, value);
}


bool PlyPointStream::setProperty(const std::string &name, uint8_t value)
{
  return setPropertyT(name, value);
}


bool PlyPointStream::setProperty(const std::string &name, int16_t value)
{
  return setPropertyT(name, value);
}


bool PlyPointStream::setProperty(const std::string &name, uint16_t value)
{
  return setPropertyT(name, value);
}


bool PlyPointStream::setProperty(const std::string &name, int32_t value)
{
  return setPropertyT(name, value);
}


bool PlyPointStream::setProperty(const std::string &name, uint32_t value)
{
  return setPropertyT(name, value);
}


bool PlyPointStream::setProperty(const std::string &name, float value)
{
  return setPropertyT(name, value);
}


bool PlyPointStream::setProperty(const std::string &name, double value)
{
  return setPropertyT(name, value);
}


void PlyPointStream::writePoint()
{
  for (size_t i = 0; i < properties_.size(); ++i)
  {
    const Value &value = values_[i];
    switch (properties_[i].type)
    {
    case Type::kInt8:
      out_->write(reinterpret_cast<const char *>(&value.i8), sizeof(value.i8));
      break;
    case Type::kUInt8:
      out_->write(reinterpret_cast<const char *>(&value.u8), sizeof(value.u8));
      break;
    case Type::kInt16:
      out_->write(reinterpret_cast<const char *>(&value.i16), sizeof(value.i16));
      break;
    case Type::kUInt16:
      out_->write(reinterpret_cast<const char *>(&value.u16), sizeof(value.u16));
      break;
    case Type::kInt32:
      out_->write(reinterpret_cast<const char *>(&value.i32), sizeof(value.i32));
      break;
    case Type::kUInt32:
      out_->write(reinterpret_cast<const char *>(&value.u32), sizeof(value.u32));
      break;
    case Type::kFloat32:
      out_->write(reinterpret_cast<const char *>(&value.f32), sizeof(value.f32));
      break;
    case Type::kFloat64:
      out_->write(reinterpret_cast<const char *>(&value.f64), sizeof(value.f64));
      break;
    default:
      throw std::runtime_error("Unexpected data type");
    }
  }
  ++point_count_;
}


std::string PlyPointStream::typeName(Type type)
{
  // Lint(KS): tried using an enum value for the array size, but that didn't work.
  // NOLINTNEXTLINE(readability-magic-numbers)
  static std::array<const std::string, 9> names =  //
    {
      "null",    //
      "char",    //
      "uchar",   //
      "short",   //
      "ushort",  //
      "int",     //
      "uint",    //
      "float",   //
      "double"   //
    };

  if (unsigned(type) < names.size())
  {
    return names[unsigned(type)];
  }

  return std::string();
}


template <typename T>
bool PlyPointStream::setPropertyT(const std::string &name, T value)
{
  for (size_t i = 0; i < properties_.size(); ++i)
  {
    if (name == properties_[i].name)
    {
      if (PlyTypeOf<T>::type == properties_[i].type)
      {
        setValue(&values_[i], value);
        return true;
      }
      return false;
    }
  }

  return false;
}


void PlyPointStream::setValue(Value *dst, int8_t value)
{
  dst->i8 = value;
}


void PlyPointStream::setValue(Value *dst, uint8_t value)
{
  dst->u8 = value;
}


void PlyPointStream::setValue(Value *dst, int16_t value)
{
  dst->i16 = value;
}


void PlyPointStream::setValue(Value *dst, uint16_t value)
{
  dst->u16 = value;
}


void PlyPointStream::setValue(Value *dst, int32_t value)
{
  dst->i32 = value;
}


void PlyPointStream::setValue(Value *dst, uint32_t value)
{
  dst->u32 = value;
}


void PlyPointStream::setValue(Value *dst, float value)
{
  dst->f32 = value;
}


void PlyPointStream::setValue(Value *dst, double value)
{
  dst->f64 = value;
}


void PlyPointStream::writeHeader()
{
  std::ostream &out = *out_;
  /// Write the ply header fields.
  out << "ply\n";
  if (isBigEndian())
  {
    out << "format binary_big_endian 1.0\n";
  }
  else
  {
    out << "format binary_little_endian 1.0\n";
  }
  out << "comment Generated by ohmutil\n";
  // Write the initial point count as 0, but we record where to patch later. We add general padding plus enough for
  // the padding comment.
  out << "element vertex " << std::flush;
  point_count_pos_ = out.tellp();
  out << ElementCount() << '\n';
  // Write property details.
  for (const auto &property : properties_)
  {
    out << "property " << typeName(property.type) << " " << property.name << "\n";
  }

  out << "end_header" << std::endl;
}


bool PlyPointStream::finalisePointCount()
{
  if (out_ && point_count_pos_ >= 0)
  {
    auto cur_pos = out_->tellp();
    out_->flush();
    out_->seekp(point_count_pos_);
    if (out_->tellp() != point_count_pos_)
    {
      return false;
    }
    *out_ << ElementCount(point_count_) << '\n';
    // Seek back to where we were before finalising the point count.
    out_->seekp(cur_pos);
    return true;
  }
  return false;
}
}  // namespace ohm


================================================
FILE: ohmutil/PlyPointStream.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMUTILPLYPOINTSTREAM_H
#define OHMUTILPLYPOINTSTREAM_H

#include "OhmUtilExport.h"

#include <glm/fwd.hpp>

#include <cinttypes>
#include <ostream>
#include <string>
#include <vector>

namespace ohm
{
/// A utility class for writing out a point cloud to PLY format. The cloud is written in a progressive, streaming
/// fashion so has low memory overhead and can handle large clouds. This relies on using a @c std::ostream which
/// supports seeking in order to patch the number of points in the correct location on completion.
///
/// This only supports writing point clouds and does not support any polygonal data.
///
/// Usage:
/// - Open a file output stream.
/// - Create a @c PlyPointStream with the desired properties and the file stream
/// - For each point
///   - call @c setPointPosition() and additional @c setProperty() functions as required
///   - call @c writePoint()
/// - call @c close()
///
/// @code
/// void writePly(const std::string &filename, const std::vector<glm::dvec3> &points)
/// {
///   std::ofstream out(filename.c_str());
///   using Property = ohm::PlyPointStream::Property;
///   using Type = ohm::PlyPointStream::Type;
///   ohm::PlyPointStream ply({ Property{ "x", Type::kFloat64 }, Property{ "y", Type::kFloat64 },
///                             Property{ "z", Type::kFloat64 },
///                           out);
///   for (const auto &point : points)
///   {
///     ply.setPointPosition(point);
///     ply.writePoint();
///   }
///   ply.close();
/// }
/// @endcode
class ohmutil_API PlyPointStream
{
public:
  /// Defines the available ply property types.
  enum class ohmutil_API Type : unsigned
  {
    kNull,     ///< No type set
    kInt8,     ///< Signed byte
    kUInt8,    ///< Unsigned byte
    kInt16,    ///< `int16_t`
    kUInt16,   ///< `uint16_t`
    kInt32,    ///< `int32_t`
    kUInt32,   ///< `uint32_t`
    kFloat32,  ///< `float`
    kFloat64,  ///< `double`
    kCount     ///< Number of supported types.
  };

  /// Represents a point property in the ply file. At the very least, "x", "y", "z" properties of type @c kFloat64
  /// are expected.
  struct ohmutil_API Property
  {
    std::string name;  ///< Name of the property.
    Type type;         ///< Data type for the property
  };

  /// Default constructor. Needs @c setProperties() to be called before calling @c open().
  PlyPointStream();
  /// Create a stream with the given properties. @c open() to be called later.
  /// @param properties The point properties to write. Cannot be changed after construction.
  explicit PlyPointStream(const std::vector<Property> &properties);
  /// Create a stream with the given properties writing to the given stream. The header is written immediately.
  /// @param properties The point properties to write. Cannot be changed after construction.
  /// @param out The output stream to write to. Must be seekable. Must outlive this object.
  PlyPointStream(const std::vector<Property> &properties, std::ostream &out);
  /// Move constructor
  PlyPointStream(PlyPointStream &&other) noexcept;
  /// Destructor. Ensures the point count is finalised.
  ~PlyPointStream();

  /// Move assignment operator. Do not call while @c isOpen()
  /// @param other The object to move.
  /// @return `*this`
  PlyPointStream &operator=(PlyPointStream &&other) noexcept;

  /// Set the properties. Fails if called after @c open() .
  /// @param properties The properties list.
  /// @return True on success.
  bool setProperties(const std::vector<Property> &properties);
  /// Query the list of properties to export.
  /// @return The Ply properties list.
  inline const std::vector<Property> &properties() const { return properties_; }

  /// Open ply writing with the given stream. Ensures any current stream is first closed.
  /// @param out The output stream to write to. Must be seekable. Must outlive this object.
  void open(std::ostream &out);
  /// Close the current stream (if open)
  /// @return True if open and the point count is successfully finalised (using seek).
  bool close();

  /// Is the object currently open with an output stream?
  bool isOpen() const;

  /// Query the number of points written.
  /// @return The nubmer of points written.
  inline uint64_t pointCount() const { return point_count_; }

  /// Set the position property values "x", "y", "z" for the current point.
  /// @param pos The position values to write.
  /// @return True if the properties exist and are of the correct type.
  bool setPointPosition(const glm::dvec3 &pos);
  /// Set the "time" value for the current point.
  /// @param time The time value to write.
  /// @return True if the property exists and is of the correct type.
  bool setPointTimestamp(double time);
  /// Set the position property values "nx", "ny", "nz" for the current point.
  /// @param normal The normal values to write.
  /// @return True if the properties exist and are of the correct type.
  bool setPointNormal(const glm::dvec3 &normal);

  /// Set the value for the property @p name for the current point.
  /// @param name The property name.
  /// @param value Value to write - must match the expected type.
  /// @return True if the property exists and is of the correct type.
  bool setProperty(const std::string &name, int8_t value);
  /// @overload
  bool setProperty(const std::string &name, uint8_t value);
  /// @overload
  bool setProperty(const std::string &name, int16_t value);
  /// @overload
  bool setProperty(const std::string &name, uint16_t value);
  /// @overload
  bool setProperty(const std::string &name, int32_t value);
  /// @overload
  bool setProperty(const std::string &name, uint32_t value);
  /// @overload
  bool setProperty(const std::string &name, float value);
  /// @overload
  bool setProperty(const std::string &name, double value);

  /// Write the current collected point data. Values which have not been set will retain their previous value.
  void writePoint();

  /// Query the ply string name for @p type . This is written to the ply file as the property type.
  /// @param type The type to query.
  /// @return The ply type name for @p type
  static std::string typeName(Type type);

private:
  union Value
  {
    int8_t i8;
    uint8_t u8;
    int16_t i16;
    uint16_t u16;
    int32_t i32;
    uint32_t u32;
    float f32;
    double f64 = 0;
  };

  /// Internal helper for setting the a property value. Checks type equality.
  /// @param name The property name.
  /// @param value Value to write - must match the expected type.
  /// @return True if the property exists and is of the correct type.
  template <typename T>
  bool setPropertyT(const std::string &name, T value);

  /// Internal helper for setting the a property value after validation.
  /// @param dst The value to write to.
  /// @param value Value to write.
  static void setValue(Value *dst, int8_t value);
  /// @overload
  static void setValue(Value *dst, uint8_t value);
  /// @overload
  static void setValue(Value *dst, int16_t value);
  /// @overload
  static void setValue(Value *dst, uint16_t value);
  /// @overload
  static void setValue(Value *dst, int32_t value);
  /// @overload
  static void setValue(Value *dst, uint32_t value);
  /// @overload
  static void setValue(Value *dst, float value);
  /// @overload
  static void setValue(Value *dst, double value);

  /// Write the ply header with a placeholder point count of 0.
  void writeHeader();

  /// Rewind the stream to the point count and write the correct value. Additional padding will be consumed with a
  /// ply comment.
  /// @return True on success, false on failure to rewind, padding size overflow or when not open.
  bool finalisePointCount();

  std::ostream *out_{ nullptr };
  std::vector<Property> properties_;
  std::vector<Value> values_;
  uint64_t point_count_ = 0;
  std::ostream::pos_type point_count_pos_ = -1;
};
}  // namespace ohm

#endif  // OHMUTILPLYPOINTSTREAM_H


================================================
FILE: ohmutil/Profile.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "Profile.h"

#include "ska/bytell_hash_map.hpp"  // NOLINT

#include <atomic>
#include <cinttypes>
#include <cstring>
#include <iostream>
#include <mutex>
#include <thread>
#include <vector>

#include <logutil/LogUtil.h>

#ifdef OHM_FEATURE_THREADS
#include <tbb/tbb_thread.h>
#endif  // OHM_FEATURE_THREADS

namespace ohm
{
struct ProfileRecord
{
  const char *name;
  const char *parent_name;
  ProfileClock::duration total_time;
  ProfileClock::duration recent;
  ProfileClock::duration max_time;
  unsigned marker_count;
};

struct ProfileScope  // NOLINT(cppcoreguidelines-pro-type-member-init)
{
  const char *name;
  ProfileClock::time_point start_time;
  ProfileRecord *record;

  inline ProfileScope() = default;  // NOLINT(cppcoreguidelines-pro-type-member-init)
  inline explicit ProfileScope(const char *name)
    : name(name)
    , start_time(ProfileClock::now())
    , record(nullptr)
  {}

  inline ProfileScope(const char *name, const ProfileClock::time_point &start_time)
    : name(name)
    , start_time(start_time)
    , record(nullptr)
  {}

  inline ProfileScope(const char *name, ProfileClock::time_point &&start_time)
    : name(name)
    , start_time(start_time)
    , record(nullptr)
  {}

  inline ProfileScope(const ProfileScope &other) = default;

  inline ProfileScope(ProfileScope &&other) noexcept
    : name(other.name)
    , start_time(other.start_time)
    , record(other.record)
  {}
};

struct ThreadRecords
{
  ska::bytell_hash_map<std::string, ProfileRecord *> records;
  std::vector<ProfileScope> marker_stack;

  ~ThreadRecords()
  {
    for (auto &&iter : records)
    {
      delete iter.second;
    }
  }
};

#ifdef OHM_FEATURE_THREADS
void compareThreadIds(std::thread::id stdId, tbb::internal::tbb_thread_v3::id tbbId)
{
  static bool once = true;
  if (once && *(unsigned *)&stdId != *(unsigned *)&tbbId)
  {
    once = false;
  }
}
#endif  // OHM_FEATURE_THREADS

struct ProfileDetail
{
  std::mutex mutex;
  std::vector<std::pair<std::thread::id, ThreadRecords>> thread_records;
  std::atomic_bool reported;
  std::atomic_bool suppress_report;

  inline ProfileDetail()
    : reported(true)
    , suppress_report(false)
  {}


  inline ThreadRecords &getCurrentThreadRecords()
  {
    std::unique_lock<std::mutex> guard(mutex);
#ifdef OHM_FEATURE_THREADS
    // compareThreadIds(std::this_thread::get_id(), tbb::this_tbb_thread::get_id());
#endif  // OHM_FEATURE_THREADS
    for (auto &search : thread_records)
    {
      if (search.first == std::this_thread::get_id())
      {
        return search.second;
      }
    }

    thread_records.emplace_back(std::make_pair(std::this_thread::get_id(), ThreadRecords()));
    return thread_records.back().second;
  }
};


void showReport(std::ostream &o, const ProfileRecord &record, const ThreadRecords &thread_records, int level = 0)
{
  std::string indent(level * 2, ' ');
  const auto average_time = (record.marker_count) ? record.total_time / record.marker_count : ProfileClock::duration(0);
  o << indent << record.name << " cur: " << record.recent << " avg: " << average_time << " max: " << record.max_time
    << " total: " << record.total_time << " / " << record.marker_count << " calls\n";

  // Recurse on children.
  for (auto &&entry : thread_records.records)
  {
    if (entry.second->parent_name && strcmp(record.name, entry.second->parent_name) == 0 &&
        strcmp(record.name, entry.second->name) != 0)
    {
      if (strcmp(record.name, entry.second->name) != 0)
      {
        showReport(o, *entry.second, thread_records, level + 1);
      }
      else
      {
        o << indent << "*** self reference error ***\n";
      }
    }
  }
}


void showReport(std::ostream &o, const std::thread::id &thread_id, const ThreadRecords &records)
{
  o << "thread " << thread_id << '\n';
  for (auto &&record : records.records)
  {
    if (record.second->parent_name == nullptr)
    {
      // Only display root items, but do so by traversing the children.
      showReport(o, *record.second, records, 1);
    }
  }
}


Profile::Profile()
  : imp_(std::make_unique<ProfileDetail>())
{}


Profile::~Profile()
{
  report();
}


Profile &Profile::instance()
{
  static Profile s_instance;
  return s_instance;
}


bool Profile::push(const char *name)
{
  imp_->reported = false;
  ThreadRecords &records = imp_->getCurrentThreadRecords();
  if (!records.marker_stack.empty())
  {
    if (strcmp(records.marker_stack.back().name, name) == 0)
    {
      // Self reference.
      // std::cout << "selfref\n";
      return false;
    }
  }
  records.marker_stack.emplace_back(ProfileScope(name));
  return true;
}


void Profile::pop()
{
  ThreadRecords &records = imp_->getCurrentThreadRecords();
  if (records.marker_stack.empty())
  {
    return;
  }

  imp_->reported = false;
  const ProfileScope popped_scope = records.marker_stack.back();
  const auto elapsed = ProfileClock::now() - popped_scope.start_time;
  records.marker_stack.pop_back();

  // Key on the parent scope plus the popped scope.
  const char *parent_name = (!records.marker_stack.empty()) ? records.marker_stack.back().name : "";
  std::string key = std::string(parent_name) + popped_scope.name;
  const auto existing_record = records.records.find(key);
  ProfileRecord *record = nullptr;
  if (existing_record != records.records.end())
  {
    record = existing_record->second;
  }
  else
  {
    const ProfileClock::duration zero_duration(0);
    record = new ProfileRecord{ popped_scope.name, nullptr, zero_duration, zero_duration, zero_duration, 0u };
    if (!records.marker_stack.empty())
    {
      record->parent_name = parent_name;
    }
    records.records.insert(std::make_pair(key, record));
  }

  record->total_time += elapsed;
  record->recent = elapsed;
  record->max_time = std::max(record->max_time, record->recent);
  ++record->marker_count;
}


void Profile::report(std::ostream *optr)
{
  if (!imp_->reported && !imp_->suppress_report)
  {
    std::ostream &out = (optr) ? *optr : std::cout;
    std::unique_lock<std::mutex> guard(imp_->mutex);

    out << "----------------------------------------\n";
    out << "Profile report\n";
    out << "----------------------------------------\n";

    for (auto &&records : imp_->thread_records)
    {
      showReport(out, records.first, records.second);
    }

    out << "----------------------------------------\n";
    out << "----------------------------------------\n";

    imp_->reported = true;
  }
}


void Profile::suppressReport(bool suppress)
{
  imp_->suppress_report = suppress;
}


bool Profile::reportSupressed() const
{
  return imp_->suppress_report;
}
}  // namespace ohm


================================================
FILE: ohmutil/Profile.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMUTIL_PROFILE_H
#define OHMUTIL_PROFILE_H

#include "OhmUtilExport.h"

#include "ProfileMarker.h"

#include <chrono>
#include <iosfwd>
#include <memory>

namespace ohm
{
using ProfileClock = std::chrono::high_resolution_clock;
struct ProfileDetail;

/// A generalised instrumented profiling API.
///
/// This class provides the means for adding profiling markers to track how long code takes to execute via a
/// singleton instance. Except for reporting, the class should not be used directly, instead using @c ProfileMarker,
/// or the @c PROFILE(), @c PROFILE_END() and @c PROFILE_RESTART() macros (preferred).
///
/// General usage is to add a @c PROFILE() marker at the start of the scope which is to be profiled;
/// equivalent to declaring a local @c ProfileMarker variable. This pushes a named marker onto the profiling stack
/// via @c push(). When the @c ProfileMarker goes out of scope, it calls @c pop(), which removes the marker
/// and adds the time spend within the named scope to the internal database.
///
/// Markers may be nested, where one @c ProfileMarker scope contained another with the outer scope including the
/// time spend in the inner scope. The profiling system maintains a named scope stack per thread via @c push() and
/// @c pop(). This is threadsafe with each call implicitly resolving the active @c std::thread. Scope naming is
/// relative to the parent scope, so two scopes may have the same name with different parents (and/or within different
/// threads) and will be individually tracked.
///
/// On program exit, the profile report is automatically shown so long as @c Profile::instance() has been called at
/// least once (generally from the @c ProfileMarker constructor and destructor). The report may be shown sooner, or
/// logged to @c std::ostream by explicitly calling @c report(). The report logs the total and average time spend in
/// each scope in a hierarchical fashion.
///
/// The report is printed in the following format:
/// @code{.unparsed}
/// ----------------------------------------
///  Profile report
/// ----------------------------------------
/// thread <id>
///   <scope> avg: <average-time> total: <total-time> / <call-count> calls
///     <scope> avg: <average-time> total: <total-time> / <call-count> calls
///       <scope> avg: <average-time> total: <total-time> / <call-count> calls
///     <scope> avg: <average-time> total: <total-time> / <call-count> calls
///     <scope> avg: <average-time> total: <total-time> / <call-count> calls
/// thread <id>
///   <scope> avg: <average-time> total: <total-time> / <call-count> calls
/// ----------------------------------------
/// ----------------------------------------
/// @endcode
class ohmutil_API Profile
{
public:
  /// Profile system constructor.
  Profile();
  /// @c report() and cleanup the profile system.
  ~Profile();

  /// Access the singleton profile instance. This is what @c ProfileMarker uses by default.
  /// @return A singleton instance of the profile system.
  static Profile &instance();

  /// Push a named scope onto the profile stack. Timing for this name is tracked until @c pop() is called and
  /// this name is at the top of the stack.
  ///
  /// @par String Assumption
  /// The assumption is made that the memory for @p name remains valid throughout the lifetime of the program.
  /// This is most easily achieved by using string literals.
  ///
  /// @param name Name of the scope to profile.
  /// @return True on success. False indicates a stack error and the marker has not been pushed (e.g., self reference)
  bool push(const char *name);
  /// Pop the last scope given to @c push() and record the time spent.
  void pop();

  /// Print a report of the current profiling time spent.
  /// The profile is printed to @p optr if provided or @c std::cout otherwise.
  ///
  /// Prevent further reports unless new timing information is given (@c push() or @c pop() called).
  ///
  /// @param optr The output stream to print to.
  void report(std::ostream *optr = nullptr);
  /// @overload
  inline void report(std::ostream &optr) { report(&optr); }

  /// Suppresses @c report() printing.
  void suppressReport(bool suppress);

  /// Checks if reporting has been suppressed.
  /// @return True when reporting has been suppressed.
  bool reportSupressed() const;

private:
  std::unique_ptr<ProfileDetail> imp_;
};
}  // namespace ohm

/// @def PROFILE(name)
/// Pushes a @c ProfileMarker to the default @c Profile using the given @p name.
///
/// Ignored when @c PROFILING is not defined or zero.
/// @param name The profile marker name. Must contain only characters valid for an identifier.
/// @see Profile

/// @def PROFILE_END(name)
/// Pops the named @c ProfileMarker from the default Profile. The @p name must match that of a previous @c PROFILE()
/// statement within the current scope.
///
/// Ignored when @c PROFILING is not defined or zero.
/// @param name The profile marker name. Must contain only characters valid for an identifier.
/// @see Profile

/// @def PROFILE_RESTART(name)
/// Re-pushes the named @c ProfileMarker to the default Profile. The @p name must match that of a previous @c PROFILE()
/// statement within the current scope.
///
/// Ignored when @c PROFILING is not defined or zero.
/// @param name The profile marker name. Must contain only characters valid for an identifier.
/// @see Profile

#if PROFILING

#define PROFILE(name) ohm::ProfileMarker __profile##name(#name);
#define PROFILE_IF(name, cond) ohm::ProfileMarker __profile##name(#name, cond);
#define PROFILE_END(name) __profile##name.end();
#define PROFILE_RESTART(name) __profile##name.restart();
#define PROFILE_RESTART_IF(name, cond) __profile##name.restart(cond);

#define PROFILE2(name, prof) ohm::ProfileMarker __profile##name(#name, prof);
#define PROFILE_IF2(name, cond) ohm::ProfileMarker __profile##name(#name, prof, cond);

#else  // PROFILING

#define PROFILE(name)
#define PROFILE_IF(name)
#define PROFILE_END(name)
#define PROFILE_RESTART(name)
#define PROFILE_RESTART_IF(name)

#endif  // PROFILING

#endif  // OHMUTIL_PROFILE_H


================================================
FILE: ohmutil/ProfileMarker.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "ProfileMarker.h"

#include "Profile.h"

namespace ohm
{
ProfileMarker::ProfileMarker(const char *name, bool activate)
  : ProfileMarker(name, nullptr, activate)
{}


ProfileMarker::ProfileMarker(const char *name, Profile *profile, bool activate)
  : name_(name)
  , profile_(profile)
  , active_(false)
{
  restart(activate);
}


ProfileMarker::~ProfileMarker()
{
  end();
}


void ProfileMarker::end()
{
  if (active_)
  {
    // Pop first so this marker isn't a parent of itself.
    Profile *profile = (profile_ == nullptr) ? &Profile::instance() : profile_;
    profile->pop();
    active_ = false;
  }
}


void ProfileMarker::restart(bool activate)
{
  if (!active_ && activate)
  {
    Profile *profile = (profile_ == nullptr) ? &Profile::instance() : profile_;
    active_ = profile->push(name_);
  }
}
}  // namespace ohm


================================================
FILE: ohmutil/ProfileMarker.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMUTIL_PROFILEMARKER_H
#define OHMUTIL_PROFILEMARKER_H

#include "OhmUtilExport.h"

namespace ohm
{
class Profile;

/// A utility class for profiling a scope.
///
/// The marker pushes itself onto the current profiling stack via @c Profile::push() on creation and
/// pops itself on destruction. Push and pop operations may be explicitly controlled by using @c end() - pop - and
/// @c restart() - push. These are ignored if the marker is not in the appropriate state.
///
/// @see Profile
class ohmutil_API ProfileMarker
{
public:
  /// Create a marker with the given name using the @c Profile::instance().
  ///
  /// @par String Assumption
  /// The assumption is made that the memory for @p name remains valid throughout the lifetime of the program.
  /// This is most easily achieved by using string literals.
  ///
  /// @param name Name of the scope to profile.
  /// @param activate Only start profiling if this is true. Useful for conditional profiling.
  explicit ProfileMarker(const char *name, bool activate = true);
  /// Create a marker with the given name targetting the given @p profile.
  ///
  /// @par String Assumption
  /// The assumption is made that the memory for @p name remains valid throughout the lifetime of the program.
  /// This is most easily achieved by using string literals.
  ///
  /// @param name Name of the scope to profile.
  /// @param profile The profile system to add the marker to.
  /// @param activate Only start profiling if this is true. Useful for conditional profiling.
  ProfileMarker(const char *name, Profile *profile, bool activate = true);

  /// Pops the marker from the profile stack so long as @c end() has not been called.
  ~ProfileMarker();

  /// Explicitly pops the marker from the profile stack. The destructor will not pop it again unless @c restart()
  /// is called.
  void end();

  /// Pushes the marker onto the stack so long as @c end() has been called.
  /// @param activate Only restart if this is true. Useful for conditional profiling.
  void restart(bool activate = true);

private:
  const char *name_;
  Profile *profile_;
  bool active_;
};
}  // namespace ohm

#endif  // OHMUTIL_PROFILEMARKER_H


================================================
FILE: ohmutil/ProgressMonitor.cpp
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) 2014 CSIRO
//
#include "ProgressMonitor.h"

#include <chrono>


ProgressMonitor::ProgressMonitor(unsigned update_frequency)
  : progress_(0)
  , total_progress_(0)
  , pass_(0)
  , total_passes_(0)
  , quit_(false)
  , pause_(false)
  , paused_(false)
  , displayed_(false)
  , update_frequency_(update_frequency)
{
  clearDisplayFunction();
}


ProgressMonitor::ProgressMonitor(const DisplayFunction &display_func, unsigned update_frequency)
  : ProgressMonitor(update_frequency)
{
  display_func_ = display_func;
  update_frequency_ = update_frequency;
}


ProgressMonitor::~ProgressMonitor()
{
  joinThread();
}


void ProgressMonitor::clearDisplayFunction()
{
  display_func_ = [](const Progress & /*progress*/, bool) {};
}


void ProgressMonitor::startThread(bool paused)
{
  if (thread_)
  {
    return;
  }

  paused_.store(paused);
  thread_ = std::make_unique<std::thread>(std::thread([this]() { this->entry(); }));
}


void ProgressMonitor::joinThread()
{
  quit();
  if (thread_)
  {
    thread_->join();
    thread_.reset(nullptr);
  }
}


void ProgressMonitor::pause()
{
  if (!pause_)
  {
    pause_.store(true);
    while (thread_ && !paused_)
    {
      std::this_thread::yield();
    }
  }
}


void ProgressMonitor::unpause()
{
  if (pause_)
  {
    pause_.store(false);
  }
}


void ProgressMonitor::incrementProgress()
{
  ++progress_;
}


void ProgressMonitor::incrementProgressBy(uint64_t increment)
{
  progress_ += increment;
}


void ProgressMonitor::updateProgress(uint64_t progress)
{
  progress_ = progress;
}


void ProgressMonitor::beginProgress(unsigned pass, const Info &info, bool unpause)
{
  info_ = info.info;
  progress_ = 0;
  pass_ = pass;
  total_progress_ = info.total;
  total_passes_ = info.total_passes;
  displayed_ = false;
  if (unpause)
  {
    this->unpause();
  }
}


void ProgressMonitor::endProgress()
{
  pause();

  if (progress_ != 0u && display_func_)
  {
    Progress prog;
    prog.info.info = info_;
    prog.info.total_passes = total_passes_;
    prog.info.total = total_progress_;
    prog.pass = pass_;
    prog.progress = progress_;
    displayed_ = true;
    display_func_(prog, true);
  }
}


void ProgressMonitor::entry()
{
  const unsigned sleep_time_ms = 1000 / update_frequency_;
  const std::chrono::milliseconds sleep_duration(sleep_time_ms);
  unsigned last_pass = 0u;
  uint64_t last_progress = 0u;
  while (!quit_)
  {
    if (!pause_)
    {
      unsigned pass = pass_;
      bool displayed = displayed_;
      uint64_t progress = progress_;
      if (!displayed && (progress || total_progress_) || progress && progress != last_progress || pass != last_pass)
      {
        displayed_ = true;
        Progress prog;
        prog.info.info = info_;
        prog.info.total_passes = total_passes_;
        prog.info.total = total_progress_;
        prog.pass = pass;
        prog.progress = progress;
        display_func_(prog, false);
      }
      last_progress = progress;
      last_pass = pass;
    }
    paused_.store(pause_.load());
    std::this_thread::sleep_for(sleep_duration);
  }

  // std::cout << std::endl;
}


================================================
FILE: ohmutil/ProgressMonitor.h
================================================
//
// @author Kazys Stepanas
//
// Copyright (c) 2014 CSIRO
//
#ifndef OHMUTIL_PROGRESSMONITOR_H
#define OHMUTIL_PROGRESSMONITOR_H

#include "OhmUtilExport.h"

#include <atomic>
#include <cstdint>
#include <functional>
#include <string>
#include <thread>
#include <utility>

/// A utility class for monitoring the progress of a point cloud operations.
///
/// The progress monitor runs in a thread and periodically calls the bound
/// @c displayProgress(unsigned) function from this thread.
///
/// Intended usage is as follows:
/// - Create the progress monitor.
/// - Bind the display function.
/// - Periodically call @c incrementProgress() from processing threads to update progress.
///   This is thread safe and uses atomic operations.
/// Call @c quit() once complete and optionally @c join() once done processing.
class ohmutil_API ProgressMonitor
{
public:
  /// Structure used to provide information about the current activity.
  struct Info
  {
    std::string info;           ///< String label for the current activity.
    unsigned total_passes = 0;  ///< Total number of passes to target. Zero => unknown or irrelevant.
    uint64_t total = 0;         ///< Total number of items to process in each pass. Zero => unknown.

    /// Default constructor.
    Info() {}  // NOLINT(modernize-use-equals-default) Lint(KS): gcc failed to deduce the default code before use

    /// Copy constructor
    /// @param other Object to copy.
    Info(const Info &other) = default;

    /// Constructor specifying the total number of items to process.
    /// @param total The number of items to process.
    explicit Info(uint64_t total)
      : total(total)
    {}
    /// Constructor specifying the number of passes to run and the total number of items to process.
    /// @param passes The number of passes to run.
    /// @param total The number of items to process.
    Info(unsigned passes, uint64_t total)
      : total_passes(passes)
      , total(total)
    {}
    /// Constructor specifying the label and total number of items to process.
    /// @param info Label for the current activity.
    /// @param total The number of items to process.
    Info(std::string info, uint64_t total)
      : info(std::move(info))
      , total(total)
    {}
    /// Constructor specifying the label, number of passes and total number of items to process.
    /// @param info Label for the current activity.
    /// @param passes The number of passes to run.
    /// @param total The number of items to process.
    Info(std::string info, unsigned passes, uint64_t total)
      : info(std::move(info))
      , total_passes(passes)
      , total(total)
    {}
  };

  /// Current progress information.
  struct Progress
  {
    Info info;              ///< Activity details
    unsigned pass = 0;      ///< Number of completed passes
    uint64_t progress = 0;  ///< Number of completed items in the current pass.
  };

  /// Progress callback function.
  using DisplayFunction = std::function<void(const Progress &, bool final)>;

  /// Default frequency to call the @c displayFunction() at.
  static const int kDefaultUpdateFrequency = 10;

  /// Constructor.
  /// @param update_frequency Number of calls to make to the @c displayFunction() every second.
  explicit ProgressMonitor(unsigned update_frequency = kDefaultUpdateFrequency);
  /// Constructor.
  /// @param display_func Number of calls to make to the @p display_func every second.
  /// @param update_frequency Number of calls to make to the @c displayFunction() every second.
  explicit ProgressMonitor(const DisplayFunction &display_func, unsigned update_frequency = kDefaultUpdateFrequency);
  /// Destructor - stops and joins the thread.
  ~ProgressMonitor();

  /// Set the display function to call on every cycle. This is called from the background thread.
  /// @param display_func Display function callback.
  void setDisplayFunction(const DisplayFunction &display_func);
  /// Clear the @c displayFunction() .
  void clearDisplayFunction();
  /// Query the function called on every cycle.
  /// @return The display function callback.
  const DisplayFunction &displayFunction() const;

  /// Start the background display thread.
  /// @param paused True to start in a paused state.
  void startThread(bool paused = false);
  /// Requests a quit then joins the background thread.
  void joinThread();

  /// Pause displaying the progress value. The thread continues to run.
  /// Blocks until pausing is certain to allow display output to finish.
  /// This may wait up to approximately the update frequency interval.
  void pause();

  /// Unpauses display. This does not block.
  void unpause();

  /// Request the background thread quit.
  inline void quit() { quit_.store(true); }

  /// Returns true if the display function has been called since the first call to
  /// @c startThread() or the last call to @c beginProgress().
  /// @return @c true if the display function has been invoked.
  inline bool hasDisplayed() const { return bool(displayed_); }

  /// Increment progress in the current pass by 1. Threadsafe.
  void incrementProgress();
  /// Increment progress in the current pass by @c increment. Threadsafe.
  /// @param increment The progress increment adjustment.
  void incrementProgressBy(uint64_t increment);

  /// Updates progress to the given value.
  /// @param progress The new progress value to set.
  void updateProgress(uint64_t progress);

  /// Begins progress reporting, resetting the current progress value to zero. Also sets the
  /// total progress, defaulting to zero (unknown). Optionally unpauses the progress thread.
  /// @param info Details about the progress to report.
  /// @param unpause @c true to resume the progress thread.
  inline void beginProgress(const Info &info = Info(), bool unpause = true) { beginProgress(0, info, unpause); }
  /// Begin a new pass.
  /// @param pass The starting pass number.
  /// @param info Details about the progress to report.
  /// @param unpause @c true to resume the progress thread.
  void beginProgress(unsigned pass, const Info &info = Info(), bool unpause = true);

  /// Ends reporting of the current progress. The display is first paused by
  /// invoking @c pause(), then if the total is non-zero the display function
  /// is invoked with the progress being equal to the total.
  void endProgress();

protected:
  /// Thread entry point.
  void entry();

private:
  std::string info_;
  std::atomic<uint64_t> progress_;
  std::atomic<uint64_t> total_progress_;
  std::atomic_uint pass_;
  std::atomic_uint total_passes_;
  std::atomic_bool quit_;
  std::atomic_bool pause_;
  std::atomic_bool paused_;
  std::atomic_bool displayed_;
  DisplayFunction display_func_;
  unsigned update_frequency_;
  std::unique_ptr<std::thread> thread_;
};


inline void ProgressMonitor::setDisplayFunction(const DisplayFunction &display_func)
{
  display_func_ = display_func;
}


inline const ProgressMonitor::DisplayFunction &ProgressMonitor::displayFunction() const
{
  return display_func_;
}

#endif  // OHMUTIL_PROGRESSMONITOR_H


================================================
FILE: ohmutil/SafeIO.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "SafeIO.h"

#include <cstdio>

#if !defined(_MSC_VER)

int fopen_s(FILE **file, const char *name, const char *mode)  // NOLINT
{
  *file = fopen(name, mode);
  if (*file)
  {
    return 0;
  }
  return 1;
}

#else  // !defined(_MSC_VER)

namespace
{
int avoid_no_symbols_link_warning = 0;
}

#endif  // !defined(_MSC_VER)


================================================
FILE: ohmutil/SafeIO.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMUTIL_SAFEIO_H
#define OHMUTIL_SAFEIO_H

#include "OhmUtilExport.h"

#include <cstdio>

#if !defined(_MSC_VER)

#define sprintf_s snprintf
#define sscanf_s sscanf

int ohmutil_API fopen_s(FILE **file, const char *name, const char *mode);  // NOLINT

#endif  // !defined(_MSC_VER)

#endif  // OHMUTIL_SAFEIO_H


================================================
FILE: ohmutil/ScopedTimeDisplay.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "ScopedTimeDisplay.h"

#include <logutil/LogUtil.h>

#include <chrono>
#include <iostream>
#include <sstream>
#include <string>

namespace ohm
{
using Clock = std::chrono::high_resolution_clock;

struct ScopedTimeDisplayDetail
{
  Clock::time_point start_time;
  Clock::time_point end_time;
  ScopedTimeDisplay::CustomLogger logger;
  std::string msg_prefix;
  std::ostream *out = nullptr;
  bool enabled = true;
};

ScopedTimeDisplay::ScopedTimeDisplay(const char *msg_prefix)
  : imp_(std::make_unique<ScopedTimeDisplayDetail>())
{
  imp_->msg_prefix = msg_prefix;
  imp_->out = &std::cout;
  imp_->start_time = Clock::now();
}


ScopedTimeDisplay::ScopedTimeDisplay(const char *msg_prefix, std::ostream &out)
  : ScopedTimeDisplay(msg_prefix)
{
  imp_->out = &out;
}


ScopedTimeDisplay::ScopedTimeDisplay(const char *msg_prefix, const CustomLogger &logger)
  : ScopedTimeDisplay(msg_prefix)
{
  imp_->logger = logger;
}


ScopedTimeDisplay::~ScopedTimeDisplay()
{
  finish();
}

const char *ScopedTimeDisplay::messagePrefix() const
{
  return imp_ ? imp_->msg_prefix.c_str() : nullptr;
}


std::ostream *ScopedTimeDisplay::out() const
{
  return imp_ ? imp_->out : nullptr;
}


const ScopedTimeDisplay::CustomLogger &ScopedTimeDisplay::customLogger() const
{
  static ScopedTimeDisplay::CustomLogger s_empty;
  return imp_ ? imp_->logger : s_empty;
}


void ScopedTimeDisplay::disable()
{
  if (imp_)
  {
    imp_->enabled = false;
  }
}


void ScopedTimeDisplay::finish()
{
  if (imp_)
  {
    if (imp_->enabled)
    {
      imp_->end_time = Clock::now();
      auto duration = imp_->end_time - imp_->start_time;
      if (imp_->logger)
      {
        std::ostringstream ostr;
        ostr << imp_->msg_prefix << ": " << duration << std::endl;
        imp_->logger(ostr.str().c_str());
      }
      else if (imp_->out)
      {
        (*imp_->out) << imp_->msg_prefix << ": " << duration << std::endl;
        ;
      }

      imp_->enabled = false;
    }
  }
}
}  // namespace ohm


================================================
FILE: ohmutil/ScopedTimeDisplay.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMUTIL_SCOPEDTIMEDISPLAY_H
#define OHMUTIL_SCOPEDTIMEDISPLAY_H

#include "OhmUtilExport.h"

#include <functional>
#include <iosfwd>
#include <memory>

namespace ohm
{
struct ScopedTimeDisplayDetail;

/// A scope based timer. Displays the time taken in the current scope on destruction.
///
/// Simple usage:
/// @code
/// int main()
/// {
///   ScopedTimeDisplay timer("Execution time");
///
///   // Explicit logging call because this example will have the logger and logExecTime go out of scope too.
///   timer.finish();
/// }
///
/// Usage:
/// @code
/// int main()
/// {
///   Logger logger; // Custom logging system.
///   const auto logExecTime = [&logger] (const char *timingString)
///   {
///     logger.info(timingString);
///   }
///   ScopedTimeDisplay timer("Execution time", logExecTime);
///
///   // Explicit logging call because this example will have the logger and logExecTime go out of scope too.
///   timer.finish();
/// }
/// @endcode
///
/// The default output format is: <tt>&lt;messagePrefix&gt;: &lt;time&gt;\\n</tt>.
class ohmutil_API ScopedTimeDisplay
{
public:
  /// Custom logging function signature.
  using CustomLogger = std::function<void(const char *)>;

  /// Create time display with the given message prefix.
  /// @param msg_prefix Output message prefix.
  explicit ScopedTimeDisplay(const char *msg_prefix);
  /// Create time display with the given message prefix, logging to the given output stream.
  /// @param msg_prefix Output message prefix.
  /// @param out Output stream to log to. Must remain valid until after logging.
  ScopedTimeDisplay(const char *msg_prefix, std::ostream &out);
  /// Create time display with the given message prefix and custom logging.
  /// @param msg_prefix Output message prefix.
  /// @param logger Custom log handler. Called with the formatted output message.
  ScopedTimeDisplay(const char *msg_prefix, const CustomLogger &logger);
  /// Destructor. Calls @c finish() to display time message.
  ~ScopedTimeDisplay();

  /// Prefix for display message.
  /// @return Message prefix.
  const char *messagePrefix() const;
  /// Output stream to log to.
  /// @return Output stream. Null if using @c customLogger().
  std::ostream *out() const;
  /// Custom logging handler.
  /// @return Custom logging function to call.
  const CustomLogger &customLogger() const;

  /// Disable logging. Prevents @c finish() from having any effect.
  void disable();

  /// Finish timing and display the message. Can only ever display once and only if @c disable() has
  /// not been called.
  void finish();

private:
  std::unique_ptr<ohm::ScopedTimeDisplayDetail> imp_;
};
}  // namespace ohm

#endif  // OHMUTIL_SCOPEDTIMEDISPLAY_H


================================================
FILE: ohmutil/VectorHash.h
================================================
/*
License from the NVIDIA source on which this code is based:
Copyright (c) 2009-2011, NVIDIA Corporation
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of NVIDIA Corporation nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#ifndef OHMUTIL_VECTORHASH_H
#define OHMUTIL_VECTORHASH_H

#include <cinttypes>
#include <cmath>
#include <unordered_map>

/// Magic number for vector hashing.
#define VHASH_MAGIC (0x9e3779b9u)

// By Bob Jenkins, 1996. bob_jenkins@burtleburtle.net.
#define VHASH_JENKINS_MIX(a, b, c) \
  (a) -= (b);                      \
  (a) -= (c);                      \
  (a) ^= ((c) >> 13);              \
  (b) -= (c);                      \
  (b) -= (a);                      \
  (b) ^= ((a) << 8);               \
  (c) -= (a);                      \
  (c) -= (b);                      \
  (c) ^= ((b) >> 13);              \
  (a) -= (b);                      \
  (a) -= (c);                      \
  (a) ^= ((c) >> 12);              \
  (b) -= (c);                      \
  (b) -= (a);                      \
  (b) ^= ((a) << 16);              \
  (c) -= (a);                      \
  (c) -= (b);                      \
  (c) ^= ((b) >> 5);               \
  (a) -= (b);                      \
  (a) -= (c);                      \
  (a) ^= ((c) >> 3);               \
  (b) -= (c);                      \
  (b) -= (a);                      \
  (b) ^= ((a) << 10);              \
  (c) -= (a);                      \
  (c) -= (b);                      \
  (c) ^= ((b) >> 15);

/// Contains functions for hashing vector3/vector4 style vertices for vertex hash maps.
///
/// This hash technique was taken from NVIDIA open source code.
/// Specifically the code for the paper "Efficient Sparse Voxel Octrees"
namespace vhash
{
/// Generate a hash for 2 to 3 components.
/// @param a First component.
/// @param b Second component.
/// @param c Third component.
inline uint32_t hashBits(uint32_t a, uint32_t b = VHASH_MAGIC, uint32_t c = 0)
{
  c += VHASH_MAGIC;
  VHASH_JENKINS_MIX(a, b, c);  // NOLINT(hicpp-signed-bitwise)
  return c;
}


/// Generate a hash for 4 to 6 components.
/// @param a First component.
/// @param b Second component.
/// @param c Third component.
/// @param d Fourth component.
/// @param e Fifth component.
/// @param f Sixth component.
inline uint32_t hashBits(uint32_t a, uint32_t b, uint32_t c, uint32_t d, uint32_t e = 0, uint32_t f = 0)
{
  c += VHASH_MAGIC;
  VHASH_JENKINS_MIX(a, b, c);  // NOLINT(hicpp-signed-bitwise)
  a += d;
  b += e;
  c += f;
  VHASH_JENKINS_MIX(a, b, c);  // NOLINT(hicpp-signed-bitwise)
  return c;
}


#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
#endif  // __GNUC__

/// Generate a hash code for a 3-component vertex.
/// @param x A vector coordinate.
/// @param y A vector coordinate.
/// @param z A vector coordinate.
inline uint32_t hash(int32_t x, int32_t y, int32_t z)
{
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  return hashBits(*reinterpret_cast<const uint32_t *>(&x), *reinterpret_cast<const uint32_t *>(&y),
                  *reinterpret_cast<const uint32_t *>(&z));  // NOLINT
}


/// Generate a hash code for a 3-component vertex.
/// @param x A vector coordinate.
/// @param y A vector coordinate.
/// @param z A vector coordinate.
inline uint32_t hash(float x, float y, float z)
{
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  return hashBits(*reinterpret_cast<const uint32_t *>(&x), *reinterpret_cast<const uint32_t *>(&y),
                  *reinterpret_cast<const uint32_t *>(&z));  // NOLINT
}


/// Generate a hash code for a 4-component vertex.
/// @param x A vector coordinate.
/// @param y A vector coordinate.
/// @param z A vector coordinate.
/// @param w A vector coordinate.
inline uint32_t hash(float x, float y, float z, float w)
{
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  return hashBits(*reinterpret_cast<const uint32_t *>(&x), *reinterpret_cast<const uint32_t *>(&y),
                  *reinterpret_cast<const uint32_t *>(&z), *reinterpret_cast<const uint32_t *>(&w));  // NOLINT
}

#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif  // __GNUC__
}  // namespace vhash


/// Hash structure for use with standard library maps.
/// @tparam The vector3 type. Must support x, y, z members (not functions).
template <class T>
class Vector3Hash
{
public:
  /// Operator to convert the vector @p p to its hash code.
  /// @param p A vector3 object.
  /// @return The 32-bit hash code for @p p.
  inline size_t operator()(const T &p) const { return vhash::hash(p.x, p.y, p.z); }
};

#endif  // OHMUTIL_VECTORHASH_H


================================================
FILE: ohmutil/p2p.h
================================================
//
// author Kazys Stepanas
//
#ifndef OHMUTIL_P2P_H
#define OHMUTIL_P2P_H

#include <glm/glm.hpp>

#ifdef TES_ENABLE
#include "3esvector3.h"

#include <cstddef>

inline tes::Vector3f p2p(const glm::vec3 &p)
{
  return tes::Vector3f(p.x, p.y, p.z);
}

inline glm::vec3 p2p(const tes::Vector3f &p)
{
  return glm::vec3(p.x, p.y, p.z);
}

inline const tes::Vector3f *p2pArray(const glm::vec3 *points)
{
  static_assert(sizeof(tes::Vector3f) == sizeof(glm::vec3), "tes::Vector3f size does not match glm::vec3 size.");
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  return reinterpret_cast<const tes::Vector3f *>(points);
}

inline const glm::vec3 *p2pArray(const tes::Vector3f *points)
{
  static_assert(sizeof(tes::Vector3f) == sizeof(glm::vec3), "tes::Vector3f size does not match glm::vec3 size.");
  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
  return reinterpret_cast<const glm::vec3 *>(points);
}

#endif  // TES_ENABLE

#endif  // OHMUTIL_P2P_H


================================================
FILE: package.xml
================================================
<?xml version="1.0"?>
<package format="2">
    <name>ohm</name>
    <version>0.5.0</version>
    <description>Occupancy Homogeneous Map libraries</description>
    <maintainer email="kazys.stepanas@csiro.au">Kazys Stepanas</maintainer>
    <license>Modified MIT</license>

    <depend>libglew-dev</depend>
    <depend>libglfw3-dev</depend>
    <depend>libglm-dev</depend>
    <depend>ocl-icd-opencl-dev</depend>

    <export>
        <build_type>cmake</build_type>
    </export>
</package>



================================================
FILE: readme.md
================================================
# Occupancy Homogeneous Map

The ohm library is a probabilistic voxel occupancy map supporting fast GPU based population and operations and normal distribution transform semantics. The ohm library defines an occupancy map consisting of regions or chunks of homogeneous voxels, arranged in contiguous memory blocks. This homogeneous voxel layout, rather than an octree layout, supports fast GPU based map population using OpenCL and CUDA.

## Building

### Prerequisites and Requirements

The ohm library supports both OpenCL and CUDA GPU processing. OpenCL development focuses on GCC and Visual Studio running on an Intel OpenCL GPU device. AMD and NVIDIA GPUs have been tested and should also function. The CUDA implementation has been built for compute 5.0 and 6.0. Other architectures may work, but must be specifically tested.

Building ohm requires:

- C++14 compatible compiler such as:
  - GCC 5.7
  - Visual Studio 15 2017
  - APPLE LLVM 9.1
- For OpenCL
  - An OpenCL 1.2 or 2.0 SDK. 1.2 must be used if running on NVIDIA hardware.
  - OpenCL 1.2 runtime. OpenCL 2.x also supported (command line selectable)
- For CUDA
  - CUDA 10

The following 3rd-party libraries are required to build ohm:

- Either an [OpenCL SDK](./OpenCL.md) or the [CUDA](https://developer.nvidia.com/cuda-zone) SDK
- [ZLib](https://www.zlib.net/) for serialisation compression.
- [CMake](https://cmake.org/) for project set up
- [OpenGL Mathematics (GLM)](https://glm.g-truc.net/) for 3D data types.

[Googletest](https://github.com/google/googletest) is also used when building unit tests, however, this is downloaded as part of the build.

Additional, the following 3rd-party libraries may optionally be used:

| Library                                                                     | Feature Usage                                                                           |
| --------------------------------------------------------------------------- | --------------------------------------------------------------------------------------- |
| [3rd Eye Scene](https://github.com/data61/3rdEyeScene)                      | For debug visualisation of map generation.                                              |
| [Doxygen](http://www.doxygen.nl/)                                           | For generation of API documentation.                                                    |
| [Eigen3](http://eigen.tuxfamily.org/index.php)                              | Used in small amounts in some tests and as a faster option for some geometry operations |
| [Intel Threading Building Blocks](https://www.threadingbuildingblocks.org/) | Multi-threaded CPU operations.                                                          |
| [GLEW](http://glew.sourceforge.net/)                                        | For HeightmapImage in ohmheightmaputil                                                  |
| [GLFW](https://www.glfw.org/)                                               | For HeightmapImage in ohmheightmaputil                                                  |
| [libpng](http://www.libpng.org/)                                            | To convert heightmap to image using utils/ohmhm2img                                     |
| [PDAL](https://pdal.io/)                                                    | Load point various point cloud formats for ohmpop.                                      |

While efforts are made to ensure components remain optional, certain configurations may be incompatible.

#### Ubuntu apt Packages

On Ubuntu, the required packages may be installed using the following command:

```bash
sudo apt install cmake zlib1g-dev libglm-dev googletest
```

Setup of OpenCL requires mode detailed [instructions (link)](./OpenCL.md).

For CUDA setup instructions, visit [NVIDIA CUDA Zone](https://developer.nvidia.com/cuda-zone).

Additional, recommended packages can be installed using:

```bash
sudo apt install libtbb-dev libpdal-dev doxygen
```

Optional packages for heightmap generation and image conversion:

```bash
sudo apt install libglew-dev libglfw3-dev libpng-dev libeigen3-dev
```

### Build Instructions

1. Download source code.
2. Install pre-requisites.
3. Configure the project.
    - Create a `build` subdirectory and change into this subdirectory.
    - Execute `cmake ..`
        - For Visual Studio configuration, include the option `-G Visual Studio 15 2017 Win64`
    - Build the project
        - For Visual Studio, open the solution file and build.
        - For make based platforms, run `make -j`

### Building with vcpkg

Ohm supports building with in [vcpkg manifest mode](https://vcpkg.readthedocs.io/en/latest/users/manifests/). To build using vcpkg, first follow the [vcpkg install instructions](https://vcpkg.io/en/getting-started.html) then build with the vcpkg toolchain enabled.

Building in manifest mode obviates the need to download the dependencies, except for the CUDA SDK on Windows. To build with vcpkg, adjust the cmake command line to enable vcpkg manifest mode when configuring the project;

```bash
# Run from the source directory
# Note we must run CMake from the source directory and use -B and -S to specify
# the build and source directories respectively. This allows vcpkg to find the
# vcpkg.json manifest file.
mkdir build
cmake -B build -S . -DCMAKE_BUILD_TYPE=Release -DCMAKE_TOOLCHAIN_FILE=<vcpkg_path>/scripts/buildsystem/vcpkg.cmake -DVCPKG_MANIFEST_FEATURES=<features>
cmake --build build --target all --
```

> It's recommended to add `-G Ninja` as this builds much faster than other build systems.

Or for a multi config generator such as Visual Studio or `Ninja Multi-Config`;

```bash
mkdir build
cmake -B build -S . -G "Ninja Multi-Config" -DCMAKE_TOOLCHAIN_FILE=<vcpkg_path>/scripts/buildsystem/vcpkg.cmake -DVCPKG_MANIFEST_FEATURES=<features>
cmake --build build --config Release --target all --
```

The `VCPKG_MANIFEST_FEATURES` specifies the features to enable for ohm, where they differ from the default features. This is a semicolon separated list of features choosing from the items listed below. It is generally expected that either `cuda` or `opencl` are listed, or both.

| Feature           | Description                          |
| ----------------- | ------------------------------------ |
| `cuda`            | Build with CUDA GPU acceleration.    |
| `eigen` \*        | Enable Eigen support (private)       |
| `heightmap` \*    | Enable the heightmap library         |
| `heightmap-image` | Enable heightmap to image conversion |
| `opencl`          | Enable OpenCL acceleration.          |
| `pdal`            | Enable PDAL point cloud loader. \*\* |
| `threads` \*      | Enable TBB threading (limited).      |
| `test`            | Build the ohm unit tests.            |

\* This feature is enabled by default.
\*\* Warning: enabling PDAL supports takes a long time for initial configuration while it builds the GDAL dependency. This can take on the order of several hours. Also note it may be necessary to set the `LD_LIBRARY_PATH` to include the directly where the pdal shared libraries are placed by vcpkg.

## Notable Known Issues

- OpenCL compatibility with certain devices may vary.
- OpenCL performance on various devices may vary especially with memory transfer rates.
- Using the OpenCL 2.x SDK and selecting an NVIDIA GPU will result in runtime crashes.
- When installing, OHM_EMBED_GPU_CODE must be defined in order to run ohmocl; otherwise OpenCL source is not found.
- When building from VSCode on Windows and using the Ninja generation, `nvcc` may fail reporting "command too long". This appears to be caused by a long path name. Changing to an "unspecified" CMake Kit or launching VSCode with a shorter PATH variable may help.

## Resolving OpenCL SDK With Multiple Options

There are some pitfalls in trying to resolve an appropriate OpenCL SDK when multiple SDK options are installed. Most notably selecting between the Intel SDK over the NVIDIA SDK can be problematic. The best way to ensure the correct option is selected is to ensure the desired SDK prefix path is set in the PATH environment variable as the before other SDK paths. The prefix path, is essentially the path containing the `include` and `lib` directories.

While this can be changed by explicitly changing `OpenCL_INCLUDE_DIR` and `OpenCL_LIBRARY` in the CMake cache (using `ccmake` or `cmake-gui`) this can still have issues with selecting a particular API version. This is because the available API versions are determined when the OpenCL header is first found and will not be updated if the paths are explicitly changed. To combat this, edit the `CMakeCache.txt` file and remove all `OPENCL_VERSION_X_X` entries.

## Further documentation

- [OHM Glossary](./docs/docglossary.md)
- [Occupancy map usage](./docs/docusage.md)
- [OHM utilities](./docs/docutils.md)
- [GPU algorithm overview](./docs/gpu/docgpualgorithm.md)
- [GPU technical details and performance tips](./docs/gpu/docgpudetail.md)
- [Built in voxel layers](./docs/docvoxellayers.md)

## Citation

Please use the following citation if you use OHM in your research.

```txt
@ARTICLE{9849048,
  author={Stepanas, Kazys and Williams, Jason and Hern&#x00E1;ndez, Emili and Ruetz, Fabio and Hines, Thomas},
  journal={IEEE Robotics and Automation Letters}, 
  title={OHM: GPU Based Occupancy Map Generation}, 
  year={2022},
  volume={},
  number={},
  pages={1-8},
  doi={10.1109/LRA.2022.3196145}}
```


================================================
FILE: scripts/ohm-parse-timing.py
================================================
#!/usr/bin/python3

import glob
import os.path
import re

content_res_expr = re.compile(
    r'^Map resolution: ([0-9\.]+)$', re.MULTILINE)
content_mean_expr = re.compile(
    r'^Voxel mean position: (on|off)$', re.MULTILINE)
content_ndt_expr = re.compile(
    r'^NDT map enabled:$', re.MULTILINE)
content_seg_len_expr = re.compile(
    r'^Gpu max ray segment: ([0-9]+)B?$', re.MULTILINE)
content_time_expr = re.compile(
    r'^Total processing time: ([0-9\.]+)s$', re.MULTILINE)
filename_info = re.compile(
    r'(cpu|cuda|ocl)(-(intel|nvidia))?(-(mean|ndt|occ))?(-r([0-9]+)-s([0-9]+))?.*\.txt')


def get_info_item(info, expr, group, default=None):
    first = True
    duplicates = False
    for item in expr.finditer(info):
        if first:
            return item.group(group)
        else:
            duplicates = True
            break
        first = False
    if not duplicates:
        if default is not None:
            return default
        raise RuntimeError('Unable to find content for expression', expr)
    raise RuntimeError('Multiple content items for expression', expr)


def pull_timing(file_path, table):
    with open(file_path, 'r') as data_file:
        try:
            filename = os.path.basename(file_path)
            content = data_file.read()
            # resolution = float(get_info_item(content, content_res_expr, 1))
            mean = get_info_item(content, content_mean_expr, 1)
            mean = True if mean == 'on' else False
            ndt = get_info_item(content, content_ndt_expr, 0, False)
            ndt = True if isinstance(ndt, str) else False
            segment_length = int(get_info_item(
                content, content_seg_len_expr, 1, default=0))
            timing = float(get_info_item(content, content_time_expr, 1))
            run_type = get_info_item(filename, filename_info, 1)
            ocl_gpu_type = get_info_item(filename, filename_info, 3, '')

            occupancy_type = 'occ'
            if mean:
                occupancy_type = 'mean'
            if ndt:
                occupancy_type = 'ndt'

            if ocl_gpu_type:
                run_type = '{} {}'.format(run_type, ocl_gpu_type)

            if segment_length:
                run_type = '{} s{:02d}m'.format(run_type, segment_length)

            if run_type not in table:
                table[run_type] = {}

            table[run_type][occupancy_type] = timing
        except RuntimeError:
            print('Skipping {} - failed extraction'.format(file_path))


if __name__ == '__main__':
    # with open('timings.csv', 'w') as out:

    table = {}
    data_files = glob.glob('*.txt')

    for data_file in data_files:
        # print(data_file)
        pull_timing(data_file, table)

    delimit = ','
    with open('timings.csv', 'w') as out:
        def write_result(out, name, data_line):
            if name in data_line:
                out.write(str(data_line[name]))
            else:
                out.write('')

        out.write('Run Type' + delimit + 'occ' + delimit + 'mean' + delimit + 'ndt\n')
        keys = list(table.keys())
        keys.sort()
        for run_type in keys:
            run_data = table[run_type]
            out.write(run_type)
            out.write(delimit)
            write_result(out, 'occ', run_data)
            out.write(delimit)
            write_result(out, 'mean', run_data)
            out.write(delimit)
            write_result(out, 'ndt', run_data)
            out.write('\n')


================================================
FILE: scripts/ohm-timing-run.py
================================================
#!/usr/bin/python3
import argparse
import copy
import subprocess


def parse_args():
    parser = argparse.ArgumentParser(description='ohm timing util.')
    parser.add_argument('--cloud', type=str, help='Point cloud file')
    parser.add_argument('--traj', type=str, help='Trajectory file')
    parser.add_argument('--compute', type=str, nargs='+', default=['cpu'], choices=['cpu', 'cuda', 'ocl', 'oct'],
                        help='Compute type(s) to use for the run')
    parser.add_argument('--occ', type=str, nargs='+', default=['occ'], choices=['mean', 'occ', 'ndt', 'ndt-tm'],
                        help='Occupancy type(s) to use for the run')
    parser.add_argument('--resolution', type=float, default=0.1, help='Map resolution')
    parser.add_argument('--ocl-vendor', type=str, nargs='+',
                        default=['none'], help='OpenCL accelerator vendor type(s).')
    parser.add_argument('--gpu-segment-min', type=float, default=0, help='Minimum GPU ray segment to try')
    parser.add_argument('--gpu-segment-max', type=float, default=0, help='Maximum GPU ray segment to try')
    parser.add_argument('--gpu-segment-step', type=float, default=5.0, help='GPU ray segment increment')
    parser.add_argument('--dry-run', action='store_true', help='Show command lines, but do not execute')

    args = parser.parse_args()
    return args


class RunDef:
    def __init__(self):
        self.cloud = None
        self.traj = None
        self.compute = 'cpu'
        self.occupancy = 'occ'
        self.resolution = 0.1
        self.ocl_vendor = None
        self.gpu_segment_length = 0
        self.dry_run = False

    def execute(self):
        program_name = 'ohmpop{}'.format(self.compute) if self.compute != 'oct' else 'octopop'
        out_name = self.compute
        if self.compute == 'ocl' and self.ocl_vendor is not None:
            out_name += '-{}'.format(self.ocl_vendor)
        out_name += '-{}-r{}cm'.format(self.occupancy, int(self.resolution * 100))
        if self.gpu_segment_length > 0:
            out_name += '-s{}m'.format(int(self.gpu_segment_length))

        args = [program_name, self.cloud, self.traj, out_name, '--save-info', '--preload', '--serialise=false']
        args.append('--resolution={}'.format(self.resolution))

        if self.compute == 'ocl' and self.ocl_vendor:
            args.append('--vendor={}'.format(self.ocl_vendor))

        if self.gpu_segment_length > 0:
            args.append('--gpu-ray-segment-length={}'.format(self.gpu_segment_length))

        if self.occupancy == 'mean':
            args.append('--voxel-mean')
        elif self.occupancy == 'ndt':
            args.append('--ndt')
        elif self.occupancy == 'ndt-tm':
            args.append('--ndt=tm')

        if not self.dry_run:
            proc = subprocess.Popen(' '.join(args), shell=True)
            proc.wait()
        else:
            print(' '.join(args))


def float_range(start, stop, step):
    while start < stop:
        yield float(start)
        start += float(step)


if __name__ == '__main__':
    args = parse_args()

    # Build run list
    runs = []

    for compute in args.compute:
        run = RunDef()
        run.cloud = args.cloud
        run.traj = args.traj
        run.resolution = args.resolution
        run.dry_run = args.dry_run
        run.gpu_segment_length = 0

        run.compute = compute

        if compute == 'oct':
            run.occupancy = 'occ'
            runs.append(copy.copy(run))
            break

        for occ in args.occ:
            run.occupancy = occ
            if compute == 'cpu' or compute == 'oct':
                runs.append(copy.copy(run))
            else:
                # GPU based run. Need to add the segment lengths
                segments = list(float_range(args.gpu_segment_min, args.gpu_segment_max +
                                            args.gpu_segment_step * 0.5, args.gpu_segment_step))
                if len(segments) == 0:
                    segments = [0]
                ocl_vendors = args.ocl_vendor if compute == 'ocl' else ['none']
                for ocl_vendor in ocl_vendors:
                    run.ocl_vendor = ocl_vendor if ocl_vendor != 'none' else None
                    for segment in segments:
                        run.gpu_segment_length = segment
                        runs.append(copy.copy(run))

    # execute the runs
    for run in runs:
        run.execute()
    print(len(runs), 'items completed')


================================================
FILE: scripts/setup.cfg
================================================
[pep8]
max-line-length = 120

[flake8]
max-line-length = 120


================================================
FILE: slamio/CMakeLists.txt
================================================
include(GenerateExportHeader)



set(SOURCES
  rply/rply.c
  rply/rply.h
  rply/rplyfile.h
  DataChannel.h
  PointCloudReader.cpp
  PointCloudReader.h
  PointCloudReaderPly.cpp
  PointCloudReaderPly.h
  PointCloudReaderTraj.cpp
  PointCloudReaderTraj.h
  PointCloudReaderXyz.cpp
  PointCloudReaderXyz.h
  Points.cpp
  Points.h
  SlamCloudLoader.cpp
  SlamCloudLoader.h
  SlamIO.cpp
  SlamIO.h
  "${CMAKE_CURRENT_BINARY_DIR}/slamio/SlamIOConfig.h"
  "${CMAKE_CURRENT_BINARY_DIR}/slamio/SlamIOExport.h"
)

set(SLAMIO_HAVE_PDAL 0)
set(SLAMIO_HAVE_PDAL_STREAMS 0)
if(OHM_FEATURE_PDAL)
  find_package(PDAL REQUIRED)
  set(SLAMIO_HAVE_PDAL 1)
  list(APPEND SOURCES
    PointCloudReaderPdal.cpp
    PointCloudReaderPdal.h
  )
  # Need to check PDAL version. We streaming requires PDAL version 1.7+
  if(PDAL_VERSION VERSION_GREATER_EQUAL 1.7)
    set(SLAMIO_HAVE_PDAL_STREAMS 1)
    list(APPEND SOURCES
      pdal/PointStream.cpp
      pdal/PointStream.h
      PointCloudReaderPdal.cpp
      PointCloudReaderPdal.h
    )
  endif(PDAL_VERSION VERSION_GREATER_EQUAL 1.7)
endif(OHM_FEATURE_PDAL)

configure_file(SlamIOConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/slamio/SlamIOConfig.h")

set(PUBLIC_HEADERS
  DataChannel.h
  PointCloudReader.h
  Points.h
  SlamCloudLoader.h
  SlamIO.h
  "${CMAKE_CURRENT_BINARY_DIR}/slamio/SlamIOConfig.h"
  "${CMAKE_CURRENT_BINARY_DIR}/slamio/SlamIOExport.h"
)

add_library(slamio ${SOURCES})
# More things pointing to the need for a refactor here
if(MSVC)
  target_compile_definitions(slamio PUBLIC NOMINMAX _USE_MATH_DEFINES)
endif(MSVC)

clang_tidy_target(slamio)

target_link_libraries(slamio PUBLIC ohmutil glm::glm)

target_include_directories(slamio
  PUBLIC
    $<INSTALL_INTERFACE:${OHM_PREFIX_INCLUDE}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/slamio>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
)

generate_export_header(slamio
      EXPORT_MACRO_NAME slamio_API
      EXPORT_FILE_NAME slamio/SlamIOExport.h
      STATIC_DEFINE slamio_STATIC)

if(OHM_FEATURE_PDAL)
  if(BUILD_SHARED)
    # Linking PDAL into a shared library, we don't need to propagate downstream linking.
    target_link_libraries(slamio PRIVATE $<BUILD_INTERFACE:${PDAL_LIBRARIES}>)
  else(BUILD_SHARED)
    # Linking PDAL into a static library, we need to propagate downstream linking.
    target_link_libraries(slamio PRIVATE ${PDAL_LIBRARIES})
  endif(BUILD_SHARED)
endif(OHM_FEATURE_PDAL)

install(TARGETS slamio EXPORT ${CMAKE_PROJECT_NAME}-config-targets
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION ${OHM_PREFIX_INCLUDE}/slamio
)

install(FILES ${PUBLIC_HEADERS} DESTINATION ${OHM_PREFIX_INCLUDE}/slamio)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})


================================================
FILE: slamio/DataChannel.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef SLAMIO_DATACHANNEL_H_
#define SLAMIO_DATACHANNEL_H_

#include "SlamIOConfig.h"

namespace slamio
{
enum class DataChannel : unsigned;
}

slamio::DataChannel operator~(slamio::DataChannel a);
slamio::DataChannel operator|(slamio::DataChannel a, slamio::DataChannel b);
slamio::DataChannel operator|=(slamio::DataChannel &a, slamio::DataChannel b);
slamio::DataChannel operator&(slamio::DataChannel a, slamio::DataChannel b);
slamio::DataChannel operator&=(slamio::DataChannel &a, slamio::DataChannel b);

namespace slamio
{
enum class DataChannel : unsigned
{
  None = 0u,
  Time = (1u << 0u),
  Position = (1u << 1u),
  Normal = (1u << 2u),
  ColourRgb = (1u << 3u),
  ColourAlpha = (1u << 4u),
  Intensity = (1u << 5u),
  ReturnNumber = (1u << 6u),

  Colour = ColourRgb | ColourAlpha
};
}

inline slamio::DataChannel operator~(slamio::DataChannel a)
{
  return slamio::DataChannel(~unsigned(a));
}

inline slamio::DataChannel operator|(slamio::DataChannel a, slamio::DataChannel b)
{
  return slamio::DataChannel(unsigned(a) | unsigned(b));
}

inline slamio::DataChannel operator|=(slamio::DataChannel &a, slamio::DataChannel b)
{
  a = a | b;
  return a;
}

inline slamio::DataChannel operator&(slamio::DataChannel a, slamio::DataChannel b)
{
  return slamio::DataChannel(unsigned(a) & unsigned(b));
}

inline slamio::DataChannel operator&=(slamio::DataChannel &a, slamio::DataChannel b)
{
  a = a & b;
  return a;
}

#endif  // SLAMIO_DATACHANNEL_H_


================================================
FILE: slamio/PointCloudReader.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "PointCloudReader.h"

#include <array>

namespace slamio
{
const char *const *timeFieldNames(size_t &count)
{
  static const std::array<const char *, 8> names =  //
    {
      //
      "gps_time",       //
      "gpstime",        //
      "internal_time",  //
      "internaltime",   //
      "offset_time",    //
      "offsettime",     //
      "timestamp",      //
      "time"            //
    };
  count = names.size();
  return names.data();
}

const char *const *returnNumberFieldNames(size_t &count)
{
  {
    static const std::array<const char *, 4> names =  //
      {
        //
        "returnnumber",   //
        "return_number",  //
        "returnnum",      //
        "return_num",     //
      };
    count = names.size();
    return names.data();
  }
}

PointCloudReader::~PointCloudReader() = default;
}  // namespace slamio


================================================
FILE: slamio/PointCloudReader.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef SLAMIO_POINTCLOUDREADER_H_
#define SLAMIO_POINTCLOUDREADER_H_

#include "SlamIOConfig.h"

#include "DataChannel.h"
#include "Points.h"

#include <memory>

namespace slamio
{
/// Query the list of standard time field names to search for in input data.
///
/// This returns a pointer to an array of C strings with contains elements equal to @p count .
/// Ownership of the strings is retained by the function (no need to release).
///
/// The list of time fields to search for is (in order):
/// - gps_time
/// - gpstime
/// - internal_time
/// - internaltime
/// - offset_time
/// - offsettime
/// - timestamp
/// - time
///
/// Any search using these values should be case insensitive.
const char slamio_API *const *timeFieldNames(size_t &count);

/// Query the list of standard return number names to search for in input data.
///
/// This returns a pointer to an array of C strings with contains elements equal to @p count .
/// Ownership of the strings is retained by the function (no need to release).
///
/// The list of time fields to search for is (in order):
/// - returnnumber
/// - return_number
/// - returnnum
/// - return_num
///
/// Any search using these values should be case insensitive.
const char slamio_API *const *returnNumberFieldNames(size_t &count);

/// Abstract interface for a point cloud loader. These can be created from functions in @c SlamIO.h
///
/// Typical usage:
/// - Create a @c PointCloudReader via functions in @c SlamIO.h
/// - (optional) Set the @c setDesiredChannels() to flag which data channels to load. Load all present if not set.
/// - Call @c open()
/// - Iteratively call @c readNext() and/or @c readChunk()
/// - @c close() when done (or destroy)
class slamio_API PointCloudReader
{
public:
  /// Virtual, empty destructor.
  virtual ~PointCloudReader();

  /// Query the available data channels (flags).
  virtual DataChannel availableChannels() const = 0;
  /// Query the desired data channels (flags). Maybe @c DataChannel::None to use all available.
  virtual DataChannel desiredChannels() const = 0;
  /// Set the desired data channels (flags). Maybe @c DataChannel::None to use all available.
  /// @param channels The desired channel flags.
  virtual void setDesiredChannels(DataChannel channels) = 0;
  /// Add a desired data channel.
  /// @param channel The data channel to add.
  inline void addDesiredChannel(DataChannel channel) { setDesiredChannels(desiredChannels() | channel); }
  /// Remove a desired data channel.
  /// @param channel The data channel to remove.
  inline void removeDesiredChannel(DataChannel channel) { setDesiredChannels(desiredChannels() & ~channel); }

  /// Query if @c open() has been successfully called.
  /// @return True if open.
  virtual bool isOpen() = 0;
  /// Attempt to open @p filename.
  /// @param filename The file name/path to open.
  /// @return True on success.
  virtual bool open(const char *filename) = 0;
  /// Close the currently open file. Safe to call when not open.
  virtual void close() = 0;

  /// Is data loaded progressively or via a streaming thread? Non-streaming readers may block on calling @c open() .
  /// @return True if streaming load.
  virtual bool streaming() const = 0;

  /// Return the number of points in the cloud. Only valid after calling @c open() .
  /// May be zero if unknown until loading has completed.
  /// @return The number of points in the open cloud file if known, zero if unknown.
  virtual uint64_t pointCount() const = 0;

  /// Read the next data point.
  /// @param point The structure to load into.
  /// @return True if a point has been successfully loaded into @p point . False when there are no more data to load.
  virtual bool readNext(CloudPoint &point) = 0;
  /// Read multiple points
  /// @param points Address to load points into. Must have sufficient space to store @c count elements.
  /// @param count The number of points to try load into @p points .
  /// @return The number of points successfully loaded. Will be less than @c count once the end of file is reached.
  virtual uint64_t readChunk(CloudPoint *points, uint64_t count) = 0;
};

/// @c PointCloudReader pointer typedef.
using PointCloudReaderPtr = std::shared_ptr<PointCloudReader>;
}  // namespace slamio

#endif  // SLAMIO_POINTCLOUDREADER_H_


================================================
FILE: slamio/PointCloudReaderPdal.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "PointCloudReaderPdal.h"

#if SLAMIO_HAVE_PDAL_STREAMS
#ifdef _MSC_VER
#pragma warning(push)
// Disable warning about inheritance via dominance from pdal code.
#pragma warning(disable : 4250)
#endif  // _MSC_VER
// Internal stream support.
#include "pdal/PointStream.h"
#endif  // SLAMIO_HAVE_PDAL_STREAMS

#include <pdal/Options.hpp>
#include <pdal/PointView.hpp>
#include <pdal/Reader.hpp>
#if SLAMIO_HAVE_PDAL_STREAMS
#include <pdal/io/BpfReader.hpp>
#include <pdal/io/LasReader.hpp>
#endif  // SLAMIO_HAVE_PDAL_STREAMS

namespace
{
const size_t kCloudStreamBufferSize = 10000u;

std::string getFileExtension(const std::string &file)
{
  const size_t last_dot = file.find_last_of('.');
  if (last_dot != std::string::npos)
  {
    return file.substr(last_dot + 1);
  }

  return "";
}

slamio::PointCloudReaderPdal::PdalReaderPtr createReader(pdal::StageFactory &factory, const std::string &file_name)
{
  const std::string ext = getFileExtension(file_name);
  std::string reader_type;
  pdal::Options options;

  reader_type = ext;

  if (ext == "laz")
  {
    reader_type = "las";
    options.add("compression", "EITHER");
  }
  else if (ext == "xyz")
  {
    reader_type = "text";
  }

  reader_type = "readers." + reader_type;
  std::shared_ptr<pdal::Stage> reader(factory.createStage(reader_type),  //
                                      [&factory](pdal::Stage *stage) { factory.destroyStage(stage); });

  if (!reader)
  {
    std::cerr << "PDAL reader for " << reader_type << " not available" << std::endl;
    return nullptr;
  }

#if SLAMIO_HAVE_PDAL_STREAMS
  if (!reader->pipelineStreamable())
  {
    std::cout << "PDAL reader for " << reader_type << " does not support streaming" << std::endl;
    return nullptr;
  }

  auto streamable_reader = std::dynamic_pointer_cast<pdal::Streamable>(reader);
#endif  // SLAMIO_HAVE_PDAL_STREAMS

  if (reader)
  {
    options.add("filename", file_name);
    reader->setOptions(options);
  }

#if SLAMIO_HAVE_PDAL_STREAMS
  return streamable_reader;
#else   // SLAMIO_HAVE_PDAL_STREAMS
  return reader;
#endif  // SLAMIO_HAVE_PDAL_STREAMS
}

#if SLAMIO_HAVE_PDAL_STREAMS
template <typename T, typename Func>
bool tryPdalPointCount(std::shared_ptr<pdal::Streamable> reader, Func func, pdal::point_count_t &point_count)
{
  if (T *r = dynamic_cast<T *>(reader.get()))
  {
    const auto count = (r->*func)();
    if (count < std::numeric_limits<decltype(count)>::max())
    {
      point_count = count;
      return true;
    }
  }
  return false;
}


pdal::point_count_t pdalPointCount(std::shared_ptr<pdal::Streamable> reader)
{
  // Doesn't seem to be a consistent way to read the point count. `Reader::count()` didn't work with las, but casting
  // and calling `LasReader::getNumPoints()` does.

  pdal::point_count_t point_count = 0;
  if (tryPdalPointCount<pdal::BpfReader>(reader, &pdal::BpfReader::numPoints, point_count))
  {
    return point_count;
  }

  if (tryPdalPointCount<pdal::LasReader>(reader, &pdal::LasReader::getNumPoints, point_count))
  {
    return point_count;
  }

  if (tryPdalPointCount<pdal::Reader>(reader, &pdal::Reader::count, point_count))
  {
    return point_count;
  }

  return point_count;
}
#else   // SLAMIO_HAVE_PDAL_STREAMS
pdal::Dimension::Id findField(pdal::PointTable &point_table, const std::vector<pdal::Dimension::Id> ids,
                              const std::vector<std::string> &names, pdal::Dimension::Type *dim_type = nullptr)
{
  pdal::Dimension::Id field_id = pdal::Dimension::Id::Unknown;
  for (const auto &id : ids)
  {
    if (point_table.layout()->hasDim(id))
    {
      field_id = id;
      break;
    }
  }

  for (const auto &name : names)
  {
    const auto id = point_table.layout()->findDim(name);
    if (id != pdal::Dimension::Id::Unknown)
    {
      field_id = id;
      break;
    }
  }

  if (dim_type && field_id != pdal::Dimension::Id::Unknown)
  {
    *dim_type = point_table.layout()->dimType(field_id);
  }

  return field_id;
}

float colourScaleForType(pdal::Dimension::Type dim_type)
{
  switch (dim_type)
  {
  case pdal::Dimension::Type::Unsigned8:
    return 1.0f / 255.0f;
  case pdal::Dimension::Type::Unsigned16:
    return 1.0f / float(std::numeric_limits<uint16_t>::max());
  case pdal::Dimension::Type::Unsigned32:
    return 1.0f / float(std::numeric_limits<uint32_t>::max());
  default:
    break;
  }

  return 1.0f;
}
#endif  // SLAMIO_HAVE_PDAL_STREAMS
}  // namespace

namespace slamio
{
PointCloudReaderPdal::PointCloudReaderPdal()
  : pdal_factory_(std::make_unique<pdal::StageFactory>())
{}

PointCloudReaderPdal::~PointCloudReaderPdal()
{
  close();
}

DataChannel PointCloudReaderPdal::availableChannels() const
{
  return available_channels_;
}
DataChannel PointCloudReaderPdal::desiredChannels() const
{
  return desired_channels_;
}
void PointCloudReaderPdal::setDesiredChannels(DataChannel channels)
{
  desired_channels_ = channels;
}

bool PointCloudReaderPdal::isOpen()
{
  return cloud_reader_ != nullptr;
}

bool PointCloudReaderPdal::open(const char *filename)
{
  // Get the reader name for the file being loaded.
  cloud_reader_ = createReader(*pdal_factory_, filename);
  if (!cloud_reader_)
  {
    close();
    return false;
  }

#if SLAMIO_HAVE_PDAL_STREAMS
  const DataChannel required =
    (desired_channels_ == DataChannel::None) ? (DataChannel::Position | DataChannel::Time) : desired_channels_;

  point_stream_ = std::make_unique<PointStream>(kCloudStreamBufferSize, required);
  cloud_reader_->prepare(*point_stream_);

  point_stream_->finalize();
  if (!point_stream_->isValid())
  {
    std::cerr << "Unable to resolve time field in " << filename << std::endl;
    std::cerr << "Require point X, Y, Z and time fields" << std::endl;
    close();
    return false;
  }

  // Determine available data channels.
  available_channels_ |= DataChannel::Position;
  available_channels_ |= (point_stream_->hasTimestamp()) ? DataChannel::Time : DataChannel::None;
  available_channels_ |= (point_stream_->hasNormals()) ? DataChannel::Normal : DataChannel::None;
  available_channels_ |= (point_stream_->hasColourRgb()) ? DataChannel::ColourRgb : DataChannel::None;
  available_channels_ |= (point_stream_->hasColourAlpha()) ? DataChannel::ColourAlpha : DataChannel::None;
  available_channels_ |= (point_stream_->hasIntensity()) ? DataChannel::Intensity : DataChannel::None;

  point_count_ = pdalPointCount(cloud_reader_);
#else   // SLAMIO_HAVE_PDAL_STREAMS
  point_table_ = std::make_unique<pdal::PointTable>();
  cloud_reader_->prepare(*point_table_);

  available_channels_ = DataChannel::None;

  std::vector<std::string> time_fields;
  size_t field_name_count = 0;
  const auto *time_field_names = timeFieldNames(field_name_count);
  time_fields.resize(field_name_count);
  std::copy(time_field_names, time_field_names + field_name_count, time_fields.begin());

  fields_.time = findField(
    *point_table_, { pdal::Dimension::Id::GpsTime, pdal::Dimension::Id::InternalTime, pdal::Dimension::Id::OffsetTime },
    time_fields);

  if (fields_.time != pdal::Dimension::Id::Unknown)
  {
    available_channels_ |= DataChannel::Time;
  }

  if (findField(*point_table_, { pdal::Dimension::Id::X }, {}) != pdal::Dimension::Id::Unknown &&
      findField(*point_table_, { pdal::Dimension::Id::Y }, {}) != pdal::Dimension::Id::Unknown &&
      findField(*point_table_, { pdal::Dimension::Id::Z }, {}) != pdal::Dimension::Id::Unknown)
  {
    available_channels_ |= DataChannel::Position;
  }

  if (findField(*point_table_, { pdal::Dimension::Id::NormalX }, {}) != pdal::Dimension::Id::Unknown &&
      findField(*point_table_, { pdal::Dimension::Id::NormalY }, {}) != pdal::Dimension::Id::Unknown &&
      findField(*point_table_, { pdal::Dimension::Id::NormalZ }, {}) != pdal::Dimension::Id::Unknown)
  {
    available_channels_ |= DataChannel::Normal;
  }

  pdal::Dimension::Type rgba_types[4] = { pdal::Dimension::Type::None };
  if (findField(*point_table_, { pdal::Dimension::Id::Red }, {}, &rgba_types[0]) != pdal::Dimension::Id::Unknown &&
      findField(*point_table_, { pdal::Dimension::Id::Green }, {}, &rgba_types[1]) != pdal::Dimension::Id::Unknown &&
      findField(*point_table_, { pdal::Dimension::Id::Blue }, {}, &rgba_types[2]) != pdal::Dimension::Id::Unknown)
  {
    if (rgba_types[0] == rgba_types[1] && rgba_types[0] == rgba_types[1])
    {
      available_channels_ |= DataChannel::ColourRgb;
      fields_.rgb_scale = colourScaleForType(rgba_types[0]);
    }
  }

  if (findField(*point_table_, { pdal::Dimension::Id::Alpha }, {}, &rgba_types[3]) != pdal::Dimension::Id::Unknown)
  {
    available_channels_ |= DataChannel::ColourAlpha;
    fields_.alpha_scale = colourScaleForType(rgba_types[3]);
  }

  if (findField(*point_table_, { pdal::Dimension::Id::Intensity }, {}) != pdal::Dimension::Id::Unknown)
  {
    available_channels_ |= DataChannel::Intensity;
  }

  std::vector<std::string> return_number_fields;
  field_name_count = 0;
  const auto *return_number_field_names = returnNumberFieldNames(field_name_count);
  return_number_fields.resize(field_name_count);
  std::copy(return_number_field_names, return_number_field_names + field_name_count, return_number_fields.begin());

  if (findField(*point_table_, { pdal::Dimension::Id::ReturnNumber }, return_number_fields) !=
      pdal::Dimension::Id::Unknown)
  {
    available_channels_ |= DataChannel::Intensity;
  }

  pdal::PointViewSet point_sets = cloud_reader_->execute(*point_table_);
  samples_view_ = *point_sets.begin();

  point_count_ = samples_view_->size();
#endif  // SLAMIO_HAVE_PDAL_STREAMS

  // Set the desired channels if not set yet.
  if (desired_channels_ == DataChannel::None)
  {
    desired_channels_ = available_channels_;
  }

#if SLAMIO_HAVE_PDAL_STREAMS
  sample_thread_ = std::thread([this]() {  //
    cloud_reader_->execute(*point_stream_);
    point_stream_->markLoadComplete();
  });
#endif  // SLAMIO_HAVE_PDAL_STREAMS

  return true;
}

void PointCloudReaderPdal::close()
{
#if SLAMIO_HAVE_PDAL_STREAMS
  if (point_stream_)
  {
    point_stream_->abort();
  }
#else   // SLAMIO_HAVE_PDAL_STREAMS
  samples_view_ = nullptr;
  point_table_ = nullptr;
  samples_view_index_ = 0;
  fields_ = PointFields{};
#endif  // SLAMIO_HAVE_PDAL_STREAMS
  if (cloud_reader_)
  {
    if (sample_thread_.joinable())
    {
      sample_thread_.join();
    }
  }
  cloud_reader_ = nullptr;
  point_count_ = 0;
}


bool PointCloudReaderPdal::streaming() const
{
#if SLAMIO_HAVE_PDAL_STREAMS
  return true;
#else   // SLAMIO_HAVE_PDAL_STREAMS
  return false;
#endif  // SLAMIO_HAVE_PDAL_STREAMS
}


uint64_t PointCloudReaderPdal::pointCount() const
{
  return point_count_;
}


bool PointCloudReaderPdal::readNext(CloudPoint &point)
{
#if SLAMIO_HAVE_PDAL_STREAMS
  // FIXME: Should really use a condition variable in nextPoint() rather than busy wait.
  bool have_read = point_stream_->nextPoint(point);
  while (!point_stream_->done() && !have_read)
  {
    std::this_thread::yield();
    have_read = point_stream_->nextPoint(point);
  }

  return have_read;
#else   // SLAMIO_HAVE_PDAL_STREAMS
  if (samples_view_index_ < point_count_)
  {
    if ((available_channels_ & DataChannel::Time) != DataChannel::None)
    {
      point.timestamp = samples_view_->getFieldAs<double>(fields_.time, samples_view_index_);
    }
    else
    {
      point.timestamp = 0;
    }
    point.position.x = samples_view_->getFieldAs<double>(pdal::Dimension::Id::X, samples_view_index_);
    point.position.y = samples_view_->getFieldAs<double>(pdal::Dimension::Id::Y, samples_view_index_);
    point.position.z = samples_view_->getFieldAs<double>(pdal::Dimension::Id::Z, samples_view_index_);

    if ((available_channels_ & DataChannel::Normal) != DataChannel::None)
    {
      point.normal.x = samples_view_->getFieldAs<double>(pdal::Dimension::Id::NormalX, samples_view_index_);
      point.normal.y = samples_view_->getFieldAs<double>(pdal::Dimension::Id::NormalY, samples_view_index_);
      point.normal.z = samples_view_->getFieldAs<double>(pdal::Dimension::Id::NormalZ, samples_view_index_);
    }

    if ((available_channels_ & DataChannel::ColourRgb) != DataChannel::None)
    {
      point.colour[0] =
        samples_view_->getFieldAs<float>(pdal::Dimension::Id::Red, samples_view_index_) * fields_.rgb_scale;
      point.colour[1] =
        samples_view_->getFieldAs<float>(pdal::Dimension::Id::Green, samples_view_index_) * fields_.rgb_scale;
      point.colour[2] =
        samples_view_->getFieldAs<float>(pdal::Dimension::Id::Blue, samples_view_index_) * fields_.rgb_scale;
    }
    if ((available_channels_ & DataChannel::ColourAlpha) != DataChannel::None)
    {
      point.colour[3] =
        samples_view_->getFieldAs<float>(pdal::Dimension::Id::Alpha, samples_view_index_) * fields_.alpha_scale;
    }

    if ((available_channels_ & DataChannel::Intensity) != DataChannel::None)
    {
      point.intensity = samples_view_->getFieldAs<float>(pdal::Dimension::Id::Intensity, samples_view_index_);
    }

    ++samples_view_index_;
    return true;
  }
  return false;
#endif  // SLAMIO_HAVE_PDAL_STREAMS
}


uint64_t PointCloudReaderPdal::readChunk(CloudPoint *point, uint64_t count)
{
  uint64_t read_count = 0;

  for (uint64_t i = 0; i < count; ++i)
  {
    if (readNext(point[i]))
    {
      ++read_count;
    }
    else
    {
      break;
    }
  }

  return read_count;
}
}  // namespace slamio

#if SLAMIO_HAVE_PDAL_STREAMS
#ifdef _MSC_VER
#pragma warning(pop)
#endif  // _MSC_VER
#endif  // SLAMIO_HAVE_PDAL_STREAMS


================================================
FILE: slamio/PointCloudReaderPdal.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef SLAMIO_POINTCLOUDREADERPDAL_H_
#define SLAMIO_POINTCLOUDREADERPDAL_H_

#include "slamio/SlamIOConfig.h"

#include "PointCloudReader.h"

#include <pdal/Reader.hpp>
#include <pdal/StageFactory.hpp>

#include <memory>
#include <thread>

namespace slamio
{
class PointStream;

/// A point cloud loader which uses PDAL to load files. Requires PDAL 1.7 as it aims to support streaming via a
/// background thread.
///
/// Supports reading all file types the installed PDAL library supports.
///
/// Note: streaming cannot be interrupted once started. This means that the program cannot exit until file loading
/// has completed.
class slamio_API PointCloudReaderPdal : public PointCloudReader
{
public:
  PointCloudReaderPdal();
  ~PointCloudReaderPdal();

  DataChannel availableChannels() const override;
  DataChannel desiredChannels() const override;
  void setDesiredChannels(DataChannel channels) override;

  bool isOpen() override;
  bool open(const char *filename) override;
  void close() override;

  bool streaming() const override;

  uint64_t pointCount() const override;
  bool readNext(CloudPoint &point) override;
  uint64_t readChunk(CloudPoint *point, uint64_t count) override;

#if SLAMIO_HAVE_PDAL_STREAMS
  /// PDAL reader typedef
  using PdalReaderPtr = std::shared_ptr<pdal::Streamable>;
#else   // SLAMIO_HAVE_PDAL_STREAMS
  /// PDAL reader typedef
  using PdalReaderPtr = std::shared_ptr<pdal::Stage>;
#endif  // SLAMIO_HAVE_PDAL_STREAMS

private:
#if !SLAMIO_HAVE_PDAL_STREAMS
  struct PointFields
  {
    pdal::Dimension::Id time = pdal::Dimension::Id::Unknown;
    float rgb_scale = 1.0f;
    float alpha_scale = 1.0f;
  };
#endif  // SLAMIO_HAVE_PDAL_STREAMS

  std::unique_ptr<pdal::StageFactory> pdal_factory_;
  PdalReaderPtr cloud_reader_;
#if SLAMIO_HAVE_PDAL_STREAMS
  std::unique_ptr<PointStream> point_stream_;
#else   // SLAMIO_HAVE_PDAL_STREAMS
  std::unique_ptr<pdal::PointTable> point_table_;
  pdal::PointViewPtr samples_view_;
  uint64_t samples_view_index_ = 0;
  PointFields fields_;
#endif  // SLAMIO_HAVE_PDAL_STREAMS
  pdal::point_count_t point_count_ = 0;
  DataChannel available_channels_ = DataChannel::None;
  DataChannel desired_channels_ = DataChannel::None;
  std::thread sample_thread_;
};
}  // namespace slamio

#endif  // SLAMIO_POINTCLOUDREADERPDAL_H_


================================================
FILE: slamio/PointCloudReaderPly.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "PointCloudReaderPly.h"

#include "rply/rply.h"
#include "rply/rplyfile.h"

#include <algorithm>
#include <cctype>
#include <iostream>
#include <memory>

namespace
{
void onPlyError(p_ply ply, const char *message)
{
  (void)ply;
  std::cerr << message << std::endl;
}
}  // namespace

namespace slamio
{
struct RPlyHandle
{
  std::unique_ptr<FILE, int (*)(FILE *)> file;
  std::unique_ptr<t_ply_, int (*)(p_ply)> ply;
  p_ply_element vertex_element = nullptr;
  p_ply_argument ply_argument = nullptr;

  RPlyHandle()
    : file(nullptr, &fclose)
    , ply(nullptr, &ply_close)
  {}

  ~RPlyHandle() { close(); }

  bool open(const char *filename)
  {
    close();
    file = std::unique_ptr<FILE, int (*)(FILE *)>(fopen(filename, "rb"), &fclose);
    if (!file)
    {
      close();
      return false;
    }
    ply = std::unique_ptr<t_ply_, int (*)(p_ply)>(ply_open_from_file(file.get(), &onPlyError, 0, nullptr), &ply_close);
    if (!ply)
    {
      close();
      return false;
    }
    return true;
  }

  void close()
  {
    ply.release();
    file.release();
    vertex_element = nullptr;
  }
};
}  // namespace slamio

namespace
{
/// General vertex property callback.
int vertexProperty(p_ply_argument argument)
{
  long datum_id = 0;
  slamio::PointCloudReaderPly::ReadSampleData *read_data = nullptr;
  ply_get_argument_user_data(argument, (void **)&read_data, &datum_id);
  read_data->properties[datum_id] = ply_get_argument_value(argument) * read_data->scale_factor[datum_id];
  return 1;
}

/// First vertex property callback.
int vertexPropertyFirst(p_ply_argument argument)
{
  long datum_id = 0;
  slamio::PointCloudReaderPly::ReadSampleData *read_data = nullptr;
  ply_get_argument_user_data(argument, (void **)&read_data, &datum_id);
  std::fill(read_data->properties.begin(), read_data->properties.end(), 0.0);
  read_data->properties[datum_id] = ply_get_argument_value(argument) * read_data->scale_factor[datum_id];
  return 1;
}

/// Last vertex property callback.
int vertexPropertyFinalise(p_ply_argument argument)
{
  // Read laste property
  int result = vertexProperty(argument);
  long datum_index = 0;
  slamio::PointCloudReaderPly::ReadSampleData *read_data = nullptr;
  ply_get_argument_user_data(argument, (void **)&read_data, &datum_index);

  // Vertex done.
  const bool have_alpha =
    (read_data->have_property_flags & (1 << unsigned(slamio::PointCloudReaderPly::PlyProperty::kA))) != 0;

  read_data->sample = {};
  read_data->sample.timestamp = read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kTimestamp)];
  read_data->sample.position.x = read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kX)];
  read_data->sample.position.y = read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kY)];
  read_data->sample.position.z = read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kZ)];
  read_data->sample.normal.x = read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kNX)];
  read_data->sample.normal.y = read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kNY)];
  read_data->sample.normal.z = read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kNZ)];
  read_data->sample.colour.r = float(read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kR)]);
  read_data->sample.colour.g = float(read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kG)]);
  read_data->sample.colour.b = float(read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kB)]);
  read_data->sample.colour.a =
    have_alpha ? float(read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kA)]) : 1.0f;
  read_data->sample.intensity =
    float(read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kIntensity)]);
  read_data->sample.return_number = uint8_t(std::round(
    std::min<double>(read_data->properties[unsigned(slamio::PointCloudReaderPly::PlyProperty::kReturnNumber)] *
                       std::numeric_limits<uint8_t>::max(),
                     std::numeric_limits<uint8_t>::max())));

  return result;
}


bool isOneOf(const std::string &value, const std::vector<std::string> &value_set)
{
  return std::find(value_set.begin(), value_set.end(), value) != value_set.end();
}


bool isOneOf(const std::string &value, const std::initializer_list<std::string> &value_set)
{
  for (const auto &test_value : value_set)
  {
    if (value == test_value)
    {
      return true;
    }
  }
  return false;
}

bool isOneOf(const std::string &value, const std::string &test_value)
{
  return value == test_value;
}

bool haveProperty(unsigned flags, slamio::PointCloudReaderPly::PlyProperty property)
{
  return (flags & (1u << unsigned(property))) != 0u;
}

double scaleFactorForType(e_ply_type type)
{
  switch (type)
  {
  case PLY_INT8:
    return 1.0 / double(std::numeric_limits<int8_t>::max());
  case PLY_UINT8:
    return 1.0 / double(std::numeric_limits<uint8_t>::max());
  case PLY_INT16:
    return 1.0 / double(std::numeric_limits<int16_t>::max());
  case PLY_UINT16:
    return 1.0 / double(std::numeric_limits<uint16_t>::max());
  case PLY_INT32:
    return 1.0 / double(std::numeric_limits<int32_t>::max());
  case PLY_UIN32:
    return 1.0 / double(std::numeric_limits<uint32_t>::max());
  case PLY_CHAR:
    return 1.0 / double(std::numeric_limits<int8_t>::max());
  case PLY_UCHAR:
    return 1.0 / double(std::numeric_limits<uint8_t>::max());
  case PLY_SHORT:
    return 1.0 / double(std::numeric_limits<int16_t>::max());
  case PLY_USHORT:
    return 1.0 / double(std::numeric_limits<uint16_t>::max());
  case PLY_INT:
    return 1.0 / double(std::numeric_limits<int32_t>::max());
  case PLY_UINT:
    return 1.0 / double(std::numeric_limits<uint32_t>::max());
  default:
    break;
  }
  return 1.0;
}
}  // namespace

namespace slamio
{
PointCloudReaderPly::PointCloudReaderPly()
  : ply_handle_(std::make_unique<RPlyHandle>())
{}

PointCloudReaderPly::~PointCloudReaderPly()
{
  close();
}

DataChannel PointCloudReaderPly::availableChannels() const
{
  return available_channels_;
}

DataChannel PointCloudReaderPly::desiredChannels() const
{
  return desired_channels_;
}

void PointCloudReaderPly::setDesiredChannels(DataChannel channels)
{
  desired_channels_ = channels;
}

bool PointCloudReaderPly::isOpen()
{
  return ply_handle_->ply.get() != nullptr;
}

bool PointCloudReaderPly::open(const char *filename)
{
  close();

  if (!ply_handle_->open(filename))
  {
    close();
    return false;
  }

  if (!ply_read_header(ply_handle_->ply.get()))
  {
    std::cerr << "Failed to read header for " << filename << std::endl;
    return false;
  }

  readHeader();

  return true;
}

void PointCloudReaderPly::close()
{
  ply_handle_->close();
  read_sample_.sample = {};
  point_count_ = 0;
  available_channels_ = DataChannel::None;
}

bool PointCloudReaderPly::streaming() const
{
  return true;
}

uint64_t PointCloudReaderPly::pointCount() const
{
  return point_count_;
}

bool PointCloudReaderPly::readNext(CloudPoint &point)
{
  if (next_point_index_ < long(point_count_))
  {
    if (ply_read_next_instance(ply_handle_->ply.get(), ply_handle_->vertex_element, ply_handle_->ply_argument,
                               &next_point_index_))
    {
      point = read_sample_.sample;
      return true;
    }
  }

  return false;
}

uint64_t PointCloudReaderPly::readChunk(CloudPoint *point, uint64_t count)
{
  uint64_t read_count = 0;

  for (uint64_t i = 0; i < count; ++i)
  {
    if (readNext(point[i]))
    {
      ++read_count;
    }
    else
    {
      break;
    }
  }

  return read_count;
}

bool PointCloudReaderPly::readHeader()
{
  if (!ply_handle_->ply)
  {
    return false;
  }

  std::fill(read_sample_.properties.begin(), read_sample_.properties.end(), 0.0);
  std::fill(read_sample_.scale_factor.begin(), read_sample_.scale_factor.end(), 1.0);

  p_ply ply = ply_handle_->ply.get();
  p_ply_element element = nullptr;
  std::string last_vertex_property_name;
  PlyProperty last_vertex_property_id{};

  std::vector<std::string> time_fields;
  size_t field_name_count = 0;
  const auto *time_field_names = timeFieldNames(field_name_count);
  time_fields.resize(field_name_count);
  std::copy(time_field_names, time_field_names + field_name_count, time_fields.begin());

  std::vector<std::string> return_number_fields;
  field_name_count = 0;
  const auto *return_number_field_names = returnNumberFieldNames(field_name_count);
  return_number_fields.resize(field_name_count);
  std::copy(return_number_field_names, return_number_field_names + field_name_count, return_number_fields.begin());

  // iterate over all elements in input file
  unsigned element_count = 0;
  unsigned vertex_element_index = 0;
  while ((element = ply_get_next_element(ply, element)))
  {
    p_ply_property property = nullptr;
    long instances = 0;  // This will unfortunately have a different width on Windows vs Linux.
    const char *element_name;
    ply_get_element_info(element, &element_name, &instances);
    if (strcmp(element_name, "vertex") == 0)
    {
      p_ply_read_cb property_callback = &vertexPropertyFirst;
      vertex_element_index = element_count;
      ply_handle_->vertex_element = element;
      point_count_ = uint64_t(instances);
      // Resolve point data and types.
      const char *property_name{};
      e_ply_type type{};
      e_ply_type length_type{};
      e_ply_type value_type{};
      while ((property = ply_get_next_property(element, property)))
      {
        ply_get_property_info(property, &property_name, &type, &length_type, &value_type);
        std::string property_name_lower = property_name;
        std::transform(property_name_lower.begin(), property_name_lower.end(), property_name_lower.begin(),
                       [](const unsigned char ch) { return std::tolower(ch); });
        if (isOneOf(property_name_lower, time_fields) && (desired_channels_ & DataChannel::Time) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_,
                          long(PlyProperty::kTimestamp));
          last_vertex_property_id = PlyProperty::kTimestamp;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = 1.0;
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, "x") && (desired_channels_ & DataChannel::Position) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_, long(PlyProperty::kX));
          last_vertex_property_id = PlyProperty::kX;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = 1.0;
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, "y") && (desired_channels_ & DataChannel::Position) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_, long(PlyProperty::kY));
          last_vertex_property_id = PlyProperty::kY;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = 1.0;
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, "z") && (desired_channels_ & DataChannel::Position) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_, long(PlyProperty::kZ));
          last_vertex_property_id = PlyProperty::kZ;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = 1.0;
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, { "nx", "normal_x" }) &&
                 (desired_channels_ & DataChannel::Normal) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_, long(PlyProperty::kNX));
          last_vertex_property_id = PlyProperty::kNX;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = 1.0;
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, { "ny", "normal_y" }) &&
                 (desired_channels_ & DataChannel::Normal) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_, long(PlyProperty::kNY));
          last_vertex_property_id = PlyProperty::kNY;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = 1.0;
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, { "nz", "normal_z" }) &&
                 (desired_channels_ & DataChannel::Normal) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_, long(PlyProperty::kNZ));
          last_vertex_property_id = PlyProperty::kNZ;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = 1.0;
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, { "red", "r" }) &&
                 (desired_channels_ & DataChannel::ColourRgb) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_, long(PlyProperty::kR));
          last_vertex_property_id = PlyProperty::kR;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = scaleFactorForType(type);
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, { "green", "g" }) &&
                 (desired_channels_ & DataChannel::ColourRgb) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_, long(PlyProperty::kG));
          last_vertex_property_id = PlyProperty::kG;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = scaleFactorForType(type);
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, { "blue", "b" }) &&
                 (desired_channels_ & DataChannel::ColourRgb) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_, long(PlyProperty::kB));
          last_vertex_property_id = PlyProperty::kB;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = scaleFactorForType(type);
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, { "alpha", "a" }) &&
                 (desired_channels_ & DataChannel::ColourAlpha) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_, long(PlyProperty::kA));
          last_vertex_property_id = PlyProperty::kA;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = scaleFactorForType(type);
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, "intensity") &&
                 (desired_channels_ & DataChannel::Intensity) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_,
                          long(PlyProperty::kIntensity));
          last_vertex_property_id = PlyProperty::kIntensity;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = scaleFactorForType(type);
          property_callback = &vertexProperty;
        }
        else if (isOneOf(property_name_lower, return_number_fields) &&
                 (desired_channels_ & DataChannel::ReturnNumber) != DataChannel::None)
        {
          ply_set_read_cb(ply, element_name, property_name, property_callback, &read_sample_,
                          long(PlyProperty::kReturnNumber));
          last_vertex_property_id = PlyProperty::kReturnNumber;
          last_vertex_property_name = property_name;
          read_sample_.scale_factor[unsigned(last_vertex_property_id)] = scaleFactorForType(type);
          property_callback = &vertexProperty;
        }

        read_sample_.have_property_flags |= (1u << unsigned(last_vertex_property_id));
      }
    }
    // else // not interested for point data
    ++element_count;
  }

  // Change the callback for the last read vertex property to finalise the point.
  ply_set_read_cb(ply, "vertex", last_vertex_property_name.c_str(), vertexPropertyFinalise, &read_sample_,
                  long(last_vertex_property_id));

  // Prime for reading vertex data.
  ply_handle_->ply_argument = ply_get_argument(ply);
  // Skip to the vertex element
  for (unsigned i = 0; i < vertex_element_index; ++i)
  {
    p_ply_element element = ply_get_element(ply, long(i));
    if (!ply_set_argument_element(ply, ply_handle_->ply_argument, element))
    {
      return false;
    }
    if (!ply_read_element(ply, element, ply_handle_->ply_argument))
    {
      return false;
    }
  }

  if (!ply_set_argument_element(ply, ply_handle_->ply_argument, ply_handle_->vertex_element))
  {
    return false;
  }

  // Confirm which data values are available
  const unsigned property_flags = read_sample_.have_property_flags;
  if (haveProperty(property_flags, PlyProperty::kTimestamp))
  {
    available_channels_ |= DataChannel::Time;
  }
  if (haveProperty(property_flags, PlyProperty::kX) && haveProperty(property_flags, PlyProperty::kY) &&
      haveProperty(property_flags, PlyProperty::kZ))
  {
    available_channels_ |= DataChannel::Position;
  }
  if (haveProperty(property_flags, PlyProperty::kNX) && haveProperty(property_flags, PlyProperty::kNY) &&
      haveProperty(property_flags, PlyProperty::kNZ))
  {
    available_channels_ |= DataChannel::Normal;
  }
  if (haveProperty(property_flags, PlyProperty::kR) && haveProperty(property_flags, PlyProperty::kG) &&
      haveProperty(property_flags, PlyProperty::kB))
  {
    available_channels_ |= DataChannel::ColourRgb;
  }
  if (haveProperty(property_flags, PlyProperty::kA))
  {
    available_channels_ |= DataChannel::ColourAlpha;
  }
  if (haveProperty(property_flags, PlyProperty::kIntensity))
  {
    available_channels_ |= DataChannel::Intensity;
  }
  if (haveProperty(property_flags, PlyProperty::kReturnNumber))
  {
    available_channels_ |= DataChannel::ReturnNumber;
  }

  return true;
}
}  // namespace slamio


================================================
FILE: slamio/PointCloudReaderPly.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef SLAMIO_POINTCLOUDREADERPLY_H_
#define SLAMIO_POINTCLOUDREADERPLY_H_

#include "SlamIOConfig.h"

#include "PointCloudReader.h"

#include <array>
#include <memory>
#include <string>
#include <vector>

namespace slamio
{
struct RPlyHandle;

/// A PLY point cloud loading using miniply.
class slamio_API PointCloudReaderPly : public PointCloudReader
{
public:
  /// Set of relevant PLY point properties to try read.
  enum class PlyProperty : unsigned
  {
    kTimestamp,     ///< Point timestamp.
    kX,             ///< X position coordinate.
    kY,             ///< Y position coordinate.
    kZ,             ///< Z position coordinate.
    kNX,            ///< Point normal vector X component.
    kNY,            ///< Point normal vector Y component.
    kNZ,            ///< Point normal vector X component.
    kR,             ///< Point cloud red channel.
    kG,             ///< Point cloud green channel.
    kB,             ///< Point cloud blue channel.
    kA,             ///< Point cloud alpha channel (optional).
    kIntensity,     ///< Point intensity channel.
    kReturnNumber,  ///< Point return number.
    kCount          ///< Number of items in the @c PlyProperty enumeration.
  };

  /// User data structure while reading from rply library.
  struct slamio_API ReadSampleData
  {
    /// Next point property values.
    std::array<double, unsigned(PlyProperty::kCount)> properties;
    /// Scale factor to apply when loading properties for type conversion. E.g., convert uint16 colour to range [0, 1].
    std::array<double, unsigned(PlyProperty::kCount)> scale_factor;
    /// Flags indicating which proeprties we have
    unsigned have_property_flags = 0;
    /// Last loaded sample point.
    CloudPoint sample;
  };

  PointCloudReaderPly();
  ~PointCloudReaderPly();

  DataChannel availableChannels() const override;
  DataChannel desiredChannels() const override;
  void setDesiredChannels(DataChannel channels) override;

  bool isOpen() override;
  bool open(const char *filename) override;
  void close() override;

  bool streaming() const override;

  uint64_t pointCount() const override;
  bool readNext(CloudPoint &point) override;
  uint64_t readChunk(CloudPoint *point, uint64_t count) override;

private:
  bool readHeader();

  uint64_t point_count_ = 0;
  long next_point_index_ = 0;
  ReadSampleData read_sample_;
  DataChannel available_channels_ = DataChannel::None;
  DataChannel desired_channels_ = DataChannel::Time | DataChannel::Position;
  std::unique_ptr<RPlyHandle> ply_handle_;
};
}  // namespace slamio

#endif  // SLAMIO_POINTCLOUDREADERPLY_H_


================================================
FILE: slamio/PointCloudReaderTraj.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "PointCloudReaderTraj.h"

namespace slamio
{
PointCloudReaderTraj::PointCloudReaderTraj() = default;
PointCloudReaderTraj::~PointCloudReaderTraj()
{
  close();
}


DataChannel PointCloudReaderTraj::availableChannels() const
{
  return DataChannel::Position | DataChannel::Time;
}

DataChannel PointCloudReaderTraj::desiredChannels() const
{
  return desired_channels_;
}

void PointCloudReaderTraj::setDesiredChannels(DataChannel channels)
{
  desired_channels_ = channels;
}

bool PointCloudReaderTraj::isOpen()
{
  return file_in_.is_open();
}

bool PointCloudReaderTraj::open(const char *filename)
{
  close();
  file_in_.open(filename, std::ios::binary);

  if (!file_in_.is_open())
  {
    close();
    return false;
  }

  // Try read the first data line. It may be valid or it may be headings.
  CloudPoint point;
  auto reset_pos = file_in_.tellg();
  if (!readNext(point))
  {
    reset_pos = file_in_.tellg();
    if (!readNext(point))
    {
      // Data not ok.
      close();
      return false;
    }
    else
    {
      // Second line ok.
    }
  }
  else
  {
    // First line is ok.
  }

  // Reset to the first valid data line.
  file_in_.seekg(reset_pos);

  if (desired_channels_ == DataChannel::None)
  {
    desired_channels_ = DataChannel::Position | DataChannel::Time;
  }

  return true;
}

void PointCloudReaderTraj::close()
{
  file_in_.close();
  desired_channels_ = DataChannel::None;
}

bool PointCloudReaderTraj::streaming() const
{
  return true;
}

uint64_t PointCloudReaderTraj::pointCount() const
{
  return 0;  // Not known.
}

bool PointCloudReaderTraj::readNext(CloudPoint &point)
{
  if (!eof_)
  {
    if (!std::getline(file_in_, data_line_))
    {
      // End of file.
      eof_ = true;
      return false;
    }

    // sscanf is far faster than using stream operators.
#ifdef _MSC_VER
    if (sscanf_s(data_line_.c_str(), "%lg %lg %lg %lg", &point.timestamp, &point.position.x, &point.position.y,
                 &point.position.z) == 4)
#else   // _MSC_VER
    if (sscanf(data_line_.c_str(), "%lg %lg %lg %lg", &point.timestamp, &point.position.x, &point.position.y,
               &point.position.z) == 4)
#endif  // _MSC_VER
    {
      return true;
    }
  }

  return false;
}

uint64_t PointCloudReaderTraj::readChunk(CloudPoint *point, uint64_t count)
{
  uint64_t read_count = 0;

  for (uint64_t i = 0; i < count; ++i)
  {
    if (readNext(point[i]))
    {
      ++read_count;
    }
    else
    {
      break;
    }
  }

  return read_count;
}
}  // namespace slamio


================================================
FILE: slamio/PointCloudReaderTraj.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef SLAMIO_POINTCLOUDREADERTRAJ_H_
#define SLAMIO_POINTCLOUDREADERTRAJ_H_

#include "SlamIOConfig.h"

#include "PointCloudReader.h"

#include <fstream>
#include <memory>
#include <string>

namespace miniply
{
class PLYReader;
}

namespace slamio
{
/// Text file trajectory reader.
///
/// Text file format assumptions:
/// - First like may or may not be a headings line. Ignored on failure to read as numeric values.
/// - One data item per line.
/// - Data line format is : `time x y z <additional_fields>`
/// - Data in a line are space delimited
/// - @c `<additional_fields>` are ignored
class slamio_API PointCloudReaderTraj : public PointCloudReader
{
public:
  PointCloudReaderTraj();
  ~PointCloudReaderTraj();

  DataChannel availableChannels() const override;
  DataChannel desiredChannels() const override;
  void setDesiredChannels(DataChannel channels) override;

  bool isOpen() override;
  bool open(const char *filename) override;
  void close() override;

  bool streaming() const override;

  uint64_t pointCount() const override;
  bool readNext(CloudPoint &point) override;
  uint64_t readChunk(CloudPoint *point, uint64_t count) override;

private:
  using FilePtr = std::unique_ptr<FILE, int (*)(FILE *)>;

  std::ifstream file_in_;
  std::string data_line_;
  bool eof_ = false;
  DataChannel desired_channels_ = DataChannel::Position | DataChannel::Time;
};
}  // namespace slamio

#endif  // SLAMIO_POINTCLOUDREADERTRAJ_H_


================================================
FILE: slamio/PointCloudReaderXyz.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "PointCloudReaderXyz.h"

#include <algorithm>
#include <cctype>
#include <sstream>

namespace slamio
{
PointCloudReaderXyz::PointCloudReaderXyz() = default;
PointCloudReaderXyz::~PointCloudReaderXyz()
{
  close();
}


DataChannel PointCloudReaderXyz::availableChannels() const
{
  return available_channels_;
}

DataChannel PointCloudReaderXyz::desiredChannels() const
{
  return desired_channels_;
}

void PointCloudReaderXyz::setDesiredChannels(DataChannel channels)
{
  desired_channels_ = channels;
}

bool PointCloudReaderXyz::isOpen()
{
  return file_in_.is_open();
}

bool PointCloudReaderXyz::open(const char *filename)
{
  close();
  file_in_.open(filename, std::ios::binary);

  if (!file_in_.is_open())
  {
    close();
    return false;
  }

  // Try read the first data line. Looking to decode headings.
  if (!readHeadings())
  {
    // No headings available.
    close();
    return false;
  }

  if (desired_channels_ == DataChannel::None)
  {
    desired_channels_ = DataChannel::Position | DataChannel::Time;
  }

  return true;
}

void PointCloudReaderXyz::close()
{
  file_in_.close();
  available_channels_ = DataChannel::None;
}

bool PointCloudReaderXyz::streaming() const
{
  return true;
}

uint64_t PointCloudReaderXyz::pointCount() const
{
  return 0;  // Not known.
}

bool PointCloudReaderXyz::readNext(CloudPoint &point)
{
  if (!eof_)
  {
    if (!std::getline(file_in_, data_line_))
    {
      // End of file.
      eof_ = true;
      return false;
    }

    // sscanf is far faster than using stream operators, but the unknown number of data items on a line mandate streams
    std::istringstream istr(data_line_);

    for (auto &value : values_buffer_)
    {
      istr >> value;
    }

    if (!istr.fail())
    {
      point.timestamp = values_buffer_[time_index_];
      point.position.x = values_buffer_[x_index_];
      point.position.y = values_buffer_[y_index_];
      point.position.z = values_buffer_[z_index_];
      return true;
    }
  }

  return false;
}

uint64_t PointCloudReaderXyz::readChunk(CloudPoint *point, uint64_t count)
{
  uint64_t read_count = 0;

  for (uint64_t i = 0; i < count; ++i)
  {
    if (readNext(point[i]))
    {
      ++read_count;
    }
    else
    {
      break;
    }
  }

  return read_count;
}


namespace
{
const size_t heading_not_found = ~size_t(0u);


size_t headingIndex(const std::string &name, const std::vector<std::string> &headings)
{
  for (size_t i = 0; i < headings.size(); ++i)
  {
    if (name == headings[i])
    {
      return i;
    }
  }

  return heading_not_found;
}


size_t headingIndex(const std::vector<std::string> &names, const std::vector<std::string> &headings)
{
  for (const auto &name : names)
  {
    const size_t name_index = headingIndex(name, headings);
    if (name_index != heading_not_found)
    {
      return name_index;
    }
  }
  return heading_not_found;
}
}  // namespace


bool PointCloudReaderXyz::readHeadings()
{
  if (!eof_)
  {
    if (!std::getline(file_in_, data_line_))
    {
      // End of file.
      eof_ = true;
      return false;
    }
  }

  // Parse headings.
  std::istringstream istr(data_line_);
  std::vector<std::string> headings;
  std::string token;
  while (!istr.fail())
  {
    istr >> token;
    if (!istr.fail())
    {
      // Convert to lower.
      std::transform(token.begin(), token.end(), token.begin(), tolower);
      headings.emplace_back(token);
    }
  }

  // Resolve heading data items.
  std::vector<std::string> time_fields;
  size_t field_name_count = 0;
  const auto *time_field_names = timeFieldNames(field_name_count);
  time_fields.resize(field_name_count);
  std::copy(time_field_names, time_field_names + field_name_count, time_fields.begin());
  time_index_ = headingIndex(time_fields, headings);

  x_index_ = headingIndex("x", headings);
  y_index_ = headingIndex("y", headings);
  z_index_ = headingIndex("z", headings);

  nx_index_ = headingIndex("nx", headings);
  ny_index_ = headingIndex("ny", headings);
  nz_index_ = headingIndex("nz", headings);

  intensity_index_ = headingIndex("intensity", headings);

  field_name_count = 0;
  std::vector<std::string> return_number_fields;
  const auto *return_number_field_names = returnNumberFieldNames(field_name_count);
  return_number_fields.resize(field_name_count);
  std::copy(return_number_field_names, return_number_field_names + field_name_count, return_number_fields.begin());
  return_number_index_ = headingIndex(return_number_fields, headings);

  available_channels_ = DataChannel::None;
  size_t required_values_count = 0;
  if (time_index_ != heading_not_found)
  {
    available_channels_ |= DataChannel::Time;
    required_values_count = std::max(required_values_count, time_index_ + 1);
  }
  if (x_index_ != heading_not_found && y_index_ != heading_not_found && z_index_ != heading_not_found)
  {
    available_channels_ |= DataChannel::Position;
    required_values_count = std::max(required_values_count, x_index_ + 1);
    required_values_count = std::max(required_values_count, y_index_ + 1);
    required_values_count = std::max(required_values_count, z_index_ + 1);
  }
  if (nx_index_ != heading_not_found && ny_index_ != heading_not_found && nz_index_ != heading_not_found)
  {
    available_channels_ |= DataChannel::Normal;
    required_values_count = std::max(required_values_count, nx_index_ + 1);
    required_values_count = std::max(required_values_count, ny_index_ + 1);
    required_values_count = std::max(required_values_count, nz_index_ + 1);
  }

  if (intensity_index_ != heading_not_found)
  {
    available_channels_ |= DataChannel::Intensity;
    required_values_count = std::max(required_values_count, intensity_index_ + 1);
  }

  if (return_number_index_ != heading_not_found)
  {
    available_channels_ |= DataChannel::ReturnNumber;
    required_values_count = std::max(required_values_count, return_number_index_ + 1);
  }

  values_buffer_.resize(required_values_count);

  const DataChannel required_channels = DataChannel::Position | DataChannel::Time;
  return (available_channels_ & required_channels) == required_channels;
}
}  // namespace slamio


================================================
FILE: slamio/PointCloudReaderXyz.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef SLAMIO_POINTCLOUDREADERXYZ_H_
#define SLAMIO_POINTCLOUDREADERXYZ_H_

#include "SlamIOConfig.h"

#include "PointCloudReader.h"

#include <fstream>
#include <memory>
#include <string>
#include <vector>

namespace miniply
{
class PLYReader;
}

namespace slamio
{
/// XYZ point cloud text file reader.
///
/// Text file format assumptions:
/// - First line is a headings line.
/// - First line specifies the available @c DataChannel items.
/// - One data item per line.
/// - Only interested in the following @c DataChannel items:
///   - @c DataChannel::Time (required)
///   - @c DataChannel::Position (required)
///   - @c DataChannel::Normals (optional)
/// - @c DataChannel::Time is derived from the following headings in preferential order (case insensitive):
///   - gps_time
///   - gpstime
///   - internal_time
///   - internaltime
///   - offset_time
///   - offsettime
///   - timestamp
///   - time
/// - Data in a line are space delimited
class slamio_API PointCloudReaderXyz : public PointCloudReader
{
public:
  PointCloudReaderXyz();
  ~PointCloudReaderXyz();

  DataChannel availableChannels() const override;
  DataChannel desiredChannels() const override;
  void setDesiredChannels(DataChannel channels) override;

  bool isOpen() override;
  bool open(const char *filename) override;
  void close() override;

  bool streaming() const override;

  uint64_t pointCount() const override;
  bool readNext(CloudPoint &point) override;
  uint64_t readChunk(CloudPoint *point, uint64_t count) override;

private:
  using FilePtr = std::unique_ptr<FILE, int (*)(FILE *)>;

  bool readHeadings();

  std::ifstream file_in_;
  std::string data_line_;
  bool eof_ = false;
  DataChannel desired_channels_ = DataChannel::Position | DataChannel::Time;
  DataChannel available_channels_ = DataChannel::None;
  std::vector<double> values_buffer_;
  size_t time_index_ = ~0u;
  size_t x_index_ = ~0u;
  size_t y_index_ = ~0u;
  size_t z_index_ = ~0u;
  size_t nx_index_ = ~0u;
  size_t ny_index_ = ~0u;
  size_t nz_index_ = ~0u;
  size_t intensity_index_ = ~0u;
  size_t return_number_index_ = ~0u;
};
}  // namespace slamio

#endif  // SLAMIO_POINTCLOUDREADERXYZ_H_


================================================
FILE: slamio/Points.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "Points.h"

#include <type_traits>

#define SLAMIO_CHECK_ALIGNMENT(member_c, member_s)                                 \
  static_assert(offsetof(CloudPoint, member_c) == offsetof(SamplePoint, member_s), \
                "CloudPoint/SamplePoint misalignment: " #member_c "/" #member_s)

namespace slamio
{
// Validate assumptions about the CloudPoint/SamplePoint relationship.
static_assert(sizeof(CloudPoint) == sizeof(SamplePoint), "CloudPoint/SamplePoint sizes differ");
// Assert we have POD types so we can use offsetof()
static_assert(std::is_pod<CloudPoint>(), "CloudPoint is not a POD type.");
static_assert(std::is_pod<SamplePoint>(), "SamplePoint is not a POD type.");
SLAMIO_CHECK_ALIGNMENT(timestamp, timestamp);
SLAMIO_CHECK_ALIGNMENT(position, sample);
SLAMIO_CHECK_ALIGNMENT(normal, origin);
SLAMIO_CHECK_ALIGNMENT(colour, colour);
SLAMIO_CHECK_ALIGNMENT(intensity, intensity);
}  // namespace slamio


================================================
FILE: slamio/Points.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef SLAMIO_POINTS_H_
#define SLAMIO_POINTS_H_

#include "SlamIOConfig.h"

#include <glm/vec3.hpp>
#include <glm/vec4.hpp>

#include <cstring>

namespace slamio
{
/// Extracted point cloud sample.
struct CloudPoint
{
  double timestamp;     ///< Point timestamp (if available)
  glm::dvec3 position;  ///< Sample position (if available)
  glm::dvec3 normal;    ///< Sample normal value. May be a ray when loading rayclouds or with trajectory (if available)
  glm::vec4 colour;     ///< Point colour (if available)
  float intensity;      ///< Point intensity value (if available)
  uint8_t return_number;  ///< Zero based, sample return number (if available). Primary return is 0, second 1, etc.
};

/// SLAM sample point. This is very similar to a point cloud sample, but has an @p origin point rather than a point
/// cloud @p normal . In practice, clouds loaded by this library expect that point cloud normals will already be origin
/// points rather than normals.
struct SamplePoint
{
  double timestamp;       ///< Sample timestamp (if available)
  glm::dvec3 sample;      ///< Sample position
  glm::dvec3 origin;      ///< Point from which the same was generated (sensor position, if available)
  glm::vec4 colour;       ///< Sample colour (if available)
  float intensity;        ///< Sample intensity (if available)
  uint8_t return_number;  ///< Zero based, sample return number (if available). Primary return is 0, second 1, etc.
};

inline void c2sPt(SamplePoint &sample, const CloudPoint &cloud)
{
  memcpy(&sample, &cloud, sizeof(sample));
}

inline SamplePoint c2sPt(const CloudPoint &cloud)
{
  SamplePoint sample;
  c2sPt(sample, cloud);
  return sample;
}

inline void s2cPt(CloudPoint &cloud, const SamplePoint &sample)
{
  static_assert(sizeof(cloud) == sizeof(sample), "Cloud/sample point size mismatch. Cannot use memcpy().");
  memcpy(&cloud, &sample, sizeof(sample));
}

inline CloudPoint s2cPt(const SamplePoint &sample)
{
  CloudPoint cloud;
  s2cPt(cloud, sample);
  return cloud;
}
}  // namespace slamio

#endif  // SLAMIO_POINTS_H_


================================================
FILE: slamio/SlamCloudLoader.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "SlamCloudLoader.h"

#include "PointCloudReader.h"
#include "SlamIO.h"

#include <chrono>
#include <fstream>
#include <functional>
#include <iostream>
#include <sstream>
#include <string>
#include <thread>
#include <vector>

namespace
{
using Clock = std::chrono::high_resolution_clock;
}  // namespace

namespace slamio
{
struct SlamCloudLoaderDetail
{
  PointCloudReaderPtr sample_reader;
  PointCloudReaderPtr trajectory_reader;

  CloudPoint trajectory_buffer[2] = {};
  glm::dvec3 trajectory_to_sensor_offset{};

  SamplePoint next_sample;
  uint64_t read_count = 0;
  uint64_t preload_index = 0;

  Clock::time_point first_sample_read_time;
  double first_sample_timestamp = -1.0;
  bool ray_cloud = false;
  bool real_time_mode = false;
  bool infer_return_number = false;
  bool allow_return_number_inference = false;

  std::vector<SamplePoint> preload_samples;

  SlamCloudLoader::Log error_log;
};

template <typename T>
void error(std::ostream &out, const T &msg)
{
  out << msg << std::endl;
}

template <typename T, typename... Args>
void error(std::ostream &out, const T &msg, Args... args)
{
  out << msg;
  error(out, args...);
}

template <typename T>
void error(SlamCloudLoader::Log log, const T &msg)
{
  if (log)
  {
    std::ostringstream out;
    error(out, msg);
    log(out.str().c_str());
  }
}

template <typename T, typename... Args>
void error(SlamCloudLoader::Log log, const T &msg, Args... args)
{
  if (log)
  {
    std::ostringstream out;
    error(out, msg, args...);
    log(out.str().c_str());
  }
}

SlamCloudLoader::SlamCloudLoader(bool real_time_mode)
  : imp_(std::make_unique<SlamCloudLoaderDetail>())
{
  imp_->real_time_mode = real_time_mode;
}


SlamCloudLoader::~SlamCloudLoader()
{
  close();
}


void SlamCloudLoader::setErrorLog(Log error_log)
{
  imp_->error_log = error_log;
}


SlamCloudLoader::Log SlamCloudLoader::errorLog() const
{
  return imp_->error_log;
}


void SlamCloudLoader::setSensorOffset(const glm::dvec3 &offset)
{
  imp_->trajectory_to_sensor_offset = offset;
}


glm::dvec3 SlamCloudLoader::sensorOffset() const
{
  return imp_->trajectory_to_sensor_offset;
}


void SlamCloudLoader::enableReturnNumberInference(bool enable)
{
  imp_->allow_return_number_inference = enable;
}


bool SlamCloudLoader::returnNumberInference() const
{
  return imp_->allow_return_number_inference;
}


bool SlamCloudLoader::openWithTrajectory(const char *sample_file_path, const char *trajectory_file_path)
{
  return open(sample_file_path, trajectory_file_path, false);
}


bool SlamCloudLoader::openPointCloud(const char *sample_file_path)
{
  return open(sample_file_path, nullptr, false);
}


bool SlamCloudLoader::openRayCloud(const char *sample_file_path)
{
  return open(sample_file_path, nullptr, true);
}


void SlamCloudLoader::close()
{
  imp_->sample_reader = nullptr;
  imp_->trajectory_reader = nullptr;
  imp_->read_count = 0;
  imp_->preload_index = 0;
  imp_->first_sample_timestamp = -1.0;
  imp_->ray_cloud = false;
  imp_->preload_samples = std::vector<SamplePoint>();
}


size_t SlamCloudLoader::numberOfPoints() const
{
  return imp_->sample_reader ? imp_->sample_reader->pointCount() : 0;
}


bool SlamCloudLoader::realTimeMode() const
{
  return imp_->real_time_mode;
}


bool SlamCloudLoader::sampleFileIsOpen() const
{
  return imp_->sample_reader != nullptr;
}


bool SlamCloudLoader::trajectoryFileIsOpen() const
{
  return imp_->trajectory_reader != nullptr;
}


bool SlamCloudLoader::hasTimestamp() const
{
  return imp_->sample_reader && (imp_->sample_reader->availableChannels() & DataChannel::Time) != DataChannel::None;
}


bool SlamCloudLoader::hasOrigin() const
{
  return trajectoryFileIsOpen();
}


bool SlamCloudLoader::hasIntensity() const
{
  return imp_->sample_reader &&
         (imp_->sample_reader->availableChannels() & DataChannel::Intensity) != DataChannel::None;
}


bool SlamCloudLoader::hasColour() const
{
  return imp_->sample_reader &&
         (imp_->sample_reader->availableChannels() & DataChannel::ColourRgb) == DataChannel::ColourRgb;
}


bool SlamCloudLoader::hasReturnNumber() const
{
  return imp_->sample_reader &&
         (imp_->sample_reader->availableChannels() & DataChannel::ReturnNumber) == DataChannel::ReturnNumber;
}


void SlamCloudLoader::preload(size_t point_count)
{
  if (!imp_->sample_reader)
  {
    return;
  }

  if (point_count && numberOfPoints())
  {
    point_count = std::min(point_count, numberOfPoints());
  }

  const bool load_all = point_count == 0 || (numberOfPoints() && point_count >= numberOfPoints());

  std::vector<SamplePoint> preload_data;
  // Cache the read count. It will increment as we preload, but need it to remain unchanged on return.
  uint64_t initial_read_count = imp_->read_count;
  preload_data.reserve(point_count);

  SamplePoint sample{};
  while (nextSample(sample) && (preload_data.size() < point_count || load_all))
  {
    preload_data.emplace_back(sample);
  }

  std::swap(imp_->preload_samples, preload_data);
  imp_->preload_index = 0;
  imp_->read_count = initial_read_count;
}


bool SlamCloudLoader::nextSample(SamplePoint &sample)
{
  if (loadPoint())
  {
    ++imp_->read_count;

    // Read next sample.
    sample = imp_->next_sample;

    // If in real time mode, sleep until we should deliver this sample.
    if (imp_->real_time_mode && imp_->first_sample_timestamp >= 0)
    {
      const double sample_relative_time = sample.timestamp - imp_->first_sample_timestamp;
      if (sample_relative_time > 0)
      {
        auto uptime = Clock::now() - imp_->first_sample_read_time;
        const double sleep_time =
          sample_relative_time - std::chrono::duration_cast<std::chrono::duration<double>>(uptime).count();
        if (sleep_time > 0)
        {
          std::this_thread::sleep_for(std::chrono::duration<double>(sleep_time));
        }
      }
    }
    return true;
  }
  return false;
}


bool SlamCloudLoader::open(const char *sample_file_path, const char *trajectory_file_path, bool ray_cloud)
{
  close();

  imp_->sample_reader = slamio::createCloudReaderFromFilename(sample_file_path);

  if (!ray_cloud)
  {
    if (trajectory_file_path && trajectory_file_path[0])
    {
      imp_->trajectory_reader = slamio::createCloudReaderFromFilename(trajectory_file_path);

      if (!imp_->trajectory_reader)
      {
        error(imp_->error_log, "Unsupported extension for trajectory file ", trajectory_file_path);
        close();
        return false;
      }
    }
  }

  if (!imp_->sample_reader)
  {
    error(imp_->error_log, "Unsupported extension for point cloud file ", sample_file_path);
    close();
    return false;
  }

  DataChannel required_channels = DataChannel::Position;
  if (ray_cloud)
  {
    // Normals required for a ray cloud.
    required_channels |= DataChannel::Normal;
  }

  if (imp_->trajectory_reader)
  {
    const DataChannel required_trajectory_channels = DataChannel::Time | DataChannel::Position;
    imp_->trajectory_reader->setDesiredChannels(required_trajectory_channels);

    if (!imp_->trajectory_reader->open(trajectory_file_path))
    {
      error(imp_->error_log, "Failed to open trajectory file ", trajectory_file_path);
      close();
      return false;
    }

    if (imp_->trajectory_reader->readChunk(imp_->trajectory_buffer, 2) != 2)
    {
      std::cerr << "Failed to read data from trajectory file " << trajectory_file_path << std::endl;
      error(imp_->error_log, "Failed to read data from trajectory file ", trajectory_file_path);
      close();
      return false;
    }

    if ((required_trajectory_channels & imp_->trajectory_reader->availableChannels()) != required_trajectory_channels)
    {
      error(imp_->error_log, "Unable to load required data channels from trajectory from file ", trajectory_file_path);
      close();
      return false;
    }

    // Need time to correlate with trajectory.
    required_channels |= DataChannel::Time;
  }

  // Set desired channels to include the required channels and ones we could additionally use.
  imp_->sample_reader->setDesiredChannels(required_channels | DataChannel::Colour | DataChannel::Intensity |
                                          DataChannel::ReturnNumber);
  if (!imp_->sample_reader->open(sample_file_path))
  {
    error(imp_->error_log, "Unable to open point cloud ", sample_file_path);
    close();
    return false;
  }

  if (!imp_->trajectory_reader)
  {
    if (ray_cloud)
    {
      // Ray cloud expects normals.
      required_channels |= DataChannel::Normal;
    }
  }

  // Check for required channels.
  if ((required_channels & imp_->sample_reader->availableChannels()) != required_channels)
  {
    error(imp_->error_log, "Unable to load required data channels from point cloud ", sample_file_path);
    close();
    return false;
  }

  imp_->infer_return_number = imp_->allow_return_number_inference && (imp_->sample_reader->availableChannels() &
                                                                      DataChannel::ReturnNumber) == DataChannel::None;

  imp_->ray_cloud = ray_cloud;
  return true;
}

bool SlamCloudLoader::loadPoint()
{
  if (imp_->preload_index < imp_->preload_samples.size())
  {
    imp_->next_sample = imp_->preload_samples[imp_->preload_index++];
    return true;
  }

  if (!imp_->preload_samples.empty())
  {
    // Preload done. Release the memory for preload_samples
    imp_->preload_samples.shrink_to_fit();
    imp_->preload_index = 0;
  }

  CloudPoint point{};
  if (imp_->sample_reader && imp_->sample_reader->readNext(point))
  {
    SamplePoint sample = c2sPt(point);

    const bool is_first_sample = imp_->first_sample_timestamp < 0;
    bool is_secondary_return = !is_first_sample && point.return_number > 0;

    // Infer secondary returns if the return number is not available.
    // Only if this is not the first point sample.
    if (!is_first_sample && imp_->infer_return_number)
    {
      // We assume secondary returns can occur when sequential points have exactly the same timestamp.
      if (sample.timestamp == imp_->next_sample.timestamp)
      {
        sample.return_number = 1u;
        is_secondary_return = true;
        // std::cout << "infer dual return\n" << std::flush;
      }
    }

    if (imp_->ray_cloud)
    {
      // Loading a ray cloud. The normal is the vector from sample back to sensor.
      sample.origin = point.position + point.normal;
    }
    else
    {
      if (!is_secondary_return)
      {
        sampleTrajectory(sample.origin, sample.sample, sample.timestamp);
      }
      else
      {
        // Use previous sample point as the sample origin for this one.
        sample.origin = imp_->next_sample.sample;
      }
    }
    imp_->next_sample = sample;

    if (is_first_sample)
    {
      imp_->first_sample_timestamp = sample.timestamp;
      imp_->first_sample_read_time = Clock::now();
    }
    return true;
  }
  return false;
}


bool SlamCloudLoader::sampleTrajectory(glm::dvec3 &position, const glm::dvec3 &sample, double timestamp)
{
  if (imp_->trajectory_reader)
  {
    CloudPoint traj_point;

    while (timestamp > imp_->trajectory_buffer[1].timestamp && imp_->trajectory_reader->readNext(traj_point))
    {
      imp_->trajectory_buffer[0] = imp_->trajectory_buffer[1];
      imp_->trajectory_buffer[1] = traj_point;
    }

    if (imp_->trajectory_buffer[0].timestamp <= timestamp && timestamp <= imp_->trajectory_buffer[1].timestamp &&
        imp_->trajectory_buffer[0].timestamp != imp_->trajectory_buffer[1].timestamp)
    {
      const double lerp = (timestamp - imp_->trajectory_buffer[0].timestamp) /
                          (imp_->trajectory_buffer[1].timestamp - imp_->trajectory_buffer[0].timestamp);
      position = imp_->trajectory_buffer[0].position +
                 lerp * (imp_->trajectory_buffer[1].position - imp_->trajectory_buffer[0].position);
      position += imp_->trajectory_to_sensor_offset;
      return true;
    }
  }
  else
  {
    position = sample;
  }
  return false;
}
}  // namespace slamio


================================================
FILE: slamio/SlamCloudLoader.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef SLAMIO_SLAMCLOUDLOADER_H_
#define SLAMIO_SLAMCLOUDLOADER_H_

#include "SlamIOExport.h"

#include "Points.h"

#include <functional>
#include <memory>

namespace slamio
{
struct SlamCloudLoaderDetail;

/// A utility class for loading a point cloud with a trajectory.
///
/// This class provides support for loading point cloud based sample files and trajectories. Minimum supported formats
/// are PLY point cloud loading (via miniply) and PLY or text file trajectory loading (see @c PointCloudReaderTraj ).
/// Building with @c OHM_FEATURE_PDAL enables PDAL support for other point cloud data types, with PLY loading unchanged.
/// PDAL version 1.7+ supports streaming loading.
///
/// The loader may be opened in one of three ways:
/// -# @c openWithTrajectory() to load separate point cloud and trajectories.
/// -# @c openPointCloud() to load data samples with no trajectory.
/// -# @c openRayCloud() to open a self contained ray cloud.
///
/// Using @c openWithTrajetory() , the loader combines an input point cloud with a trajectory file where the trajectory
/// file identifies the sensor trajectory. The timestamps in each file are used to used to match sample and sensor
/// pairs, thus both files must contain correlated timestamps. The @c SamplePoint::origin values are reported as
/// @c (0,0,0) when timestamps cannot be matched.
///
/// Using @c openPointCloud() opens a point cloud without trajectory. The @c SamplePoint::origin values are reported at
/// the same location as the @c SamplePoint::sample positions.
///
/// Using @c openRayCloud() opens only a point cloud which must have per point normal channels available. The normals
/// are treated as a non-normalised vector pointing from the sample point back to the sensor location at the time of
/// sampling. That is, `SamplePoint::origin = SamplePoint::sample + normal`. See
/// [RayCloudTools](https://github.com/csiro-robotics/raycloudtools) for more on ray clouds.
///
/// Typical usage:
///
/// @code
/// void readSlamCloud(const char *sample_file, const char *trajectory_file, std::vector<slamio::SamplePoint> &samples)
/// {
///   slamio::SlamCloudLoader reader;
///   // (optional) set error logging hook.
///   reader.setErrorLog([](const char *msg) { std::cerr << msg << std::flush; });
///   if (!reader.openWithTrajectory(sample_file, trajectory_file))
///   {
///     return;
///   }
///
///   // Note: numberOfPoints() may be unavailable (zero).
///   samples.reserve(reader.numberOfPoints());
///
///   slamio::SamplePoint sample{};
///   while (reader.nextSample(sample))
///   {
///     samples.emplace_back(sample);
///   }
/// }
/// @endcode
class slamio_API SlamCloudLoader
{
public:
  /// Logging function hook.
  using Log = std::function<void(const char *)>;

  /// Create a SLAM cloud loader.
  /// @param real_time_mode True to throttle point loading to simulate real time data acquisition.
  explicit SlamCloudLoader(bool real_time_mode = false);

  /// Set the error logging function.
  /// @param error_log Error loging function. May be empty to clear.
  void setErrorLog(Log error_log);
  /// Get the error logging function.
  Log errorLog() const;

  /// Destructor.
  ~SlamCloudLoader();

  /// Set the fixed offset between the trajectory point to the sensor frame. This is added to all trajectory points.
  /// @param offset The trajectory to sensor offset.
  void setSensorOffset(const glm::dvec3 &offset);

  /// Get the fixed offset between the trajectory point to the sensor frame.
  glm::dvec3 sensorOffset() const;

  /// Enable or disable infering the return number. The return number may be inferred when the two sequential samples
  /// have precisely the same timestamp. This assumption is only valid for some data sets. This inference is only
  /// performed when there is no explicit @c return_number point attribute.
  ///
  /// Must be enabled before calling @c open() as this has no effect on an open data stream.
  ///
  /// @param enable True to allow infering the return number.
  void enableReturnNumberInference(bool enable);

  /// Check if return number inference is enabled. See @c enableReturnNumberInference() .
  bool returnNumberInference() const;

  /// Open the given point cloud and trajectory file pair. Both file must be valid. The @p sample_file_path must be a
  /// point cloud file, while @p trajectory_file_path can be either a point cloud file or a text trajectory.
  ///
  /// A text trajectory file contains:
  /// - A headings line (optional)
  /// - One sample per line formatted: `time x y z [additional_fields]`
  ///
  /// All values are floating point values (double precision supported) and whitespace separated. Any
  /// @c additional_fields are ignored.
  ///
  /// @c SamplePoint values are generated from the @p sample_file_path point cloud and timestamps are correlated against
  /// the @p trajectory_file_path to interpolate a sensor position at that time. The @c sensorOffset() is added before
  /// reporting the combined @c CloudSample via @c nextSample()
  ///
  /// @param sample_file_path Point cloud file name.
  /// @param trajectory_file_path Point cloud or trajectory file name.
  /// @return True on successfully opening both files.
  bool openWithTrajectory(const char *sample_file_path, const char *trajectory_file_path);

  /// Open the given point cloud file. This generates @c CloudSample values which have a fixed, zero @p origin value.
  /// @param sample_file_path Point cloud file name.
  /// @return True on successfully opening the point cloud.
  bool openPointCloud(const char *sample_file_path);

  /// Open the given ray cloud file. A ray cloud is a point cloud file where the normals channel is used to represent
  /// a vector from the position back to the ray origin.
  ///
  /// For more information on ray clouds see:
  /// - [RayCloudTools source repository](https://github.com/csiro-robotics/raycloudtools)
  /// - [RayCloudTools paper](https://ieeexplore.ieee.org/abstract/document/9444433)
  ///
  /// @param sample_file_path Ray cloud file name.
  /// @return True on successfully opening the ray cloud.
  bool openRayCloud(const char *sample_file_path);

  /// Close the current input files.
  void close();

  /// Query the number of points. May be zero as some readers do not report the total point count.
  size_t numberOfPoints() const;

  /// Running in real time mode with points given at a rate determined by the the point cloud timestamps?
  bool realTimeMode() const;

  /// Do we have a point cloud?
  bool sampleFileIsOpen() const;
  /// Do we have a trajectory?
  bool trajectoryFileIsOpen() const;

  /// True if the input data has a timestamp channel.
  bool hasTimestamp() const;

  /// True if the input data has origin points for the sensor samples.
  bool hasOrigin() const;

  /// True if the input data has an intensity channel.
  bool hasIntensity() const;

  /// True if the input data has colour channels.
  bool hasColour() const;

  /// True if the input data has lidar point return numbers.
  bool hasReturnNumber() const;

  /// Attempt to preload the given number of points. Use zero to preload all. This does nothing with PDAL 1.6 as
  /// streaming is not supported in that version. PDAL 1.8 allow streaming.
  void preload(size_t point_count = 0);

  /// Get the next point, sensor position and timestamp.
  bool nextSample(SamplePoint &sample);

private:
  bool open(const char *sample_file_path, const char *trajectory_file_path, bool ray_cloud);

  bool loadPoint();

  /// Sample the trajectory at the given timestamp.
  ///
  /// This reads the trajectory to the segment which covers @p timestamp and
  /// linearly interpolates a @p position at this time.
  ///
  /// @param[out] position Set to the trajectory position on success.
  /// @param timestamp The desired sample time.
  /// @return True on success, false when @p timestamp is out of range.
  bool sampleTrajectory(glm::dvec3 &position, const glm::dvec3 &sample, double timestamp);

  std::unique_ptr<SlamCloudLoaderDetail> imp_;
};
}  // namespace slamio

#endif  // SLAMIO_SLAMCLOUDLOADER_H_


================================================
FILE: slamio/SlamIO.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "SlamIO.h"

#include "PointCloudReaderPly.h"
#include "PointCloudReaderTraj.h"
#include "PointCloudReaderXyz.h"

#if SLAMIO_HAVE_PDAL
#include "PointCloudReaderPdal.h"
#endif  // SLAMIO_HAVE_PDAL

#include <string>

namespace
{
std::string getFileExtension(const std::string &file)
{
  const size_t last_dot = file.find_last_of(".");
  if (last_dot != std::string::npos)
  {
    return file.substr(last_dot + 1);
  }

  return "";
}
}  // namespace

namespace slamio
{
PointCloudReaderPtr createCloudReader(const char *extension_c)
{
  PointCloudReaderPtr reader;

  std::string extension;
  if (extension_c && extension_c[0] == '.')
  {
    // Skip `.` character.
    extension = extension_c + 1;
  }
  else
  {
    extension = extension_c;
  }

  if (extension == "ply")
  {
    reader = std::make_shared<PointCloudReaderPly>();
  }
  else if (extension == "txt")
  {
    reader = std::make_shared<PointCloudReaderTraj>();
  }
  else if (extension == "xyz")
  {
    reader = std::make_shared<PointCloudReaderXyz>();
  }
#if SLAMIO_HAVE_PDAL
  else
  {
    reader = std::make_shared<PointCloudReaderPdal>();
  }
#endif  // SLAMIO_HAVE_PDAL

  return reader;
}

PointCloudReaderPtr createCloudReaderFromFilename(const char *filename)
{
  const auto extension = getFileExtension(filename);
  return createCloudReader(extension.c_str());
}
}  // namespace slamio


================================================
FILE: slamio/SlamIO.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef SLAMIO_SLAMIO_H_
#define SLAMIO_SLAMIO_H_

#include "SlamIOConfig.h"

#include <memory>

namespace slamio
{
class PointCloudReader;
using PointCloudReaderPtr = std::shared_ptr<PointCloudReader>;

/// Create a @c PointCloudReader from the given filename @p extension . The appropriate reader is created based on the
/// extension.
/// @note This does not call @c PointCloudReader::open() .
/// @param extension The file type extension. No leading `.` is required but may be provided.
/// @return The appropriate reader or a @c nullptr for an unsupported extension.
PointCloudReaderPtr slamio_API createCloudReader(const char *extension);

/// Create a @c PointCloudReader from the given @p filename . The appropriate reader is created based on the extension.
/// @note This does not call @c PointCloudReader::open() .
/// @param filename The point cloud file to read.
/// @return The appropriate reader or a @c nullptr for an unsupported extension.
PointCloudReaderPtr slamio_API createCloudReaderFromFilename(const char *filename);
}  // namespace slamio

#endif  // SLAMIO_SLAMIO_H_


================================================
FILE: slamio/SlamIOConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef SLAMIO_SLAMIOCONFIG_H_
#define SLAMIO_SLAMIOCONFIG_H_

#include "SlamIOExport.h"

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

#include <cmath>

#include <memory>

/// Enable experimental parts of GLM, like `glm::length2()` (length squared)
#define GLM_ENABLE_EXPERIMENTAL

// clang-format on

#cmakedefine01 SLAMIO_HAVE_PDAL
#cmakedefine01 SLAMIO_HAVE_PDAL_STREAMS

#endif  // SLAMIO_SLAMIOCONFIG_H_


================================================
FILE: slamio/pdal/PointStream.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "PointStream.h"

namespace slamio
{
PointStream::PointStream(size_t buffer_capacity, DataChannel require)
  : pdal::StreamPointTable(layout_, buffer_capacity)
  , required_channels_(require)
{
  // Register for the data we are interested in
  buffers_[0].reserve(buffer_capacity);
  buffers_[1].reserve(buffer_capacity);
  // Start with flipping allowed.
  flip_wait_.notify_one();
}

void PointStream::finalize()
{
  if (!layout_.finalized())
  {
    // Validate the dimensions.
    available_channels_ = DataChannel::None;

    bool have_dim = true;
    for (int i = 0; i < 3; ++i)
    {
      const auto *pos_dim = layout_.dimDetail(pdal::Dimension::Id(i + int(pdal::Dimension::Id::X)));
      if (pos_dim)
      {
        position_channel_types_[i] = pos_dim->type();
        have_dim = have_dim && (position_channel_types_[i] == pdal::Dimension::Type::Double ||
                                position_channel_types_[i] == pdal::Dimension::Type::Float);
      }
      else
      {
        have_dim = false;
      }
    }
    available_channels_ |= (have_dim) ? DataChannel::Position : DataChannel::None;

    have_dim = true;
    for (int i = 0; i < 3; ++i)
    {
      const auto *pos_dim = layout_.dimDetail(pdal::Dimension::Id(i + int(pdal::Dimension::Id::NormalX)));
      if (pos_dim)
      {
        normal_channel_types_[i] = pos_dim->type();
        have_dim = have_dim && (normal_channel_types_[i] == pdal::Dimension::Type::Double ||
                                normal_channel_types_[i] == pdal::Dimension::Type::Float);
      }
      else
      {
        have_dim = false;
      }
    }
    available_channels_ |= (have_dim) ? DataChannel::Normal : DataChannel::None;

    have_dim = true;
    for (int i = 0; i < 3; ++i)
    {
      const auto *colour_dim = layout_.dimDetail(pdal::Dimension::Id(i + int(pdal::Dimension::Id::Red)));
      if (colour_dim)
      {
        colour_channel_types_[i] = colour_dim->type();
        have_dim = have_dim && (colour_channel_types_[i] == pdal::Dimension::Type::Unsigned8 ||
                                colour_channel_types_[i] == pdal::Dimension::Type::Unsigned16 ||
                                colour_channel_types_[i] == pdal::Dimension::Type::Unsigned32);
      }
      else
      {
        have_dim = false;
      }
    }
    available_channels_ |= (have_dim) ? DataChannel::ColourRgb : DataChannel::None;

    const auto *alpha_dim = layout_.dimDetail(pdal::Dimension::Id::Alpha);
    if (alpha_dim)
    {
      intensity_channel_type_ = alpha_dim->type();
      if (intensity_channel_type_ == pdal::Dimension::Type::Unsigned8 ||
          intensity_channel_type_ == pdal::Dimension::Type::Unsigned16 ||
          intensity_channel_type_ == pdal::Dimension::Type::Unsigned32)
      {
        available_channels_ |= DataChannel::ColourAlpha;
      }
    }

    const auto *intensity_dim = layout_.dimDetail(pdal::Dimension::Id::Intensity);
    if (intensity_dim)
    {
      intensity_channel_type_ = intensity_dim->type();
      if (intensity_channel_type_ == pdal::Dimension::Type::Unsigned8 ||
          intensity_channel_type_ == pdal::Dimension::Type::Unsigned16 ||
          intensity_channel_type_ == pdal::Dimension::Type::Unsigned32 ||
          intensity_channel_type_ == pdal::Dimension::Type::Double ||
          intensity_channel_type_ == pdal::Dimension::Type::Float)
      {
        available_channels_ |= DataChannel::Intensity;
      }
    }

    // Resolve time dimension.
    time_dimension_ = pdal::Dimension::Id::Unknown;
    // First try resolve by Dimension ID
    const std::array<pdal::Dimension::Id, 3> time_ids = { pdal::Dimension::Id::GpsTime,
                                                          pdal::Dimension::Id::InternalTime,
                                                          pdal::Dimension::Id::OffsetTime };
    for (const auto &time_dim : time_ids)
    {
      if (layout_.hasDim(time_dim))
      {
        time_dimension_ = time_dim;
        break;
      }
    }

    // Not found. Try resolve by name.
    if (time_dimension_ == pdal::Dimension::Id::Unknown)
    {
      const std::array<const std::string, 2> time_dim_names = { "time", "timestamp" };
      for (const auto &time_name : time_dim_names)
      {
        time_dimension_ = layout_.findDim(time_name);
        if (time_dimension_ != pdal::Dimension::Id::Unknown)
        {
          break;
        }
      }
    }

    if (time_dimension_ != pdal::Dimension::Id::Unknown)
    {
      available_channels_ |= DataChannel::Time;
      time_channel_type_ = layout_.dimDetail(time_dimension_)->type();
    }

    valid_dimensions_ = (available_channels_ & required_channels_) == required_channels_;

    layout_.finalize();
    pdal::StreamPointTable::finalize();
  }
}

bool PointStream::nextPoint(CloudPoint &point)
{
  std::unique_lock<std::mutex> guard(buffer_mutex_);
  const int read_buffer = 1 - write_index_;
  const unsigned read_index = next_read_;
  // Ensure colour pointer is valid, pointing to a stack variable if null.
  bool have_read = false;
  if (next_read_ < buffers_[read_buffer].size())
  {
    point = buffers_[read_buffer][read_index];
    ++next_read_;
    have_read = true;
  }
  guard.unlock();
  flip_wait_.notify_one();
  return have_read;
}

void PointStream::setFieldInternal(pdal::Dimension::Id dim, pdal::PointId idx, const void *val)
{
  if (!abort_)
  {
    auto &point_buffer = buffers_[write_index_];
    while (point_buffer.size() <= idx)
    {
      point_buffer.emplace_back(CloudPoint{});
    }

    auto &sample = point_buffer[idx];

    switch (dim)
    {
    case pdal::Dimension::Id::X:
    case pdal::Dimension::Id::Y:
    case pdal::Dimension::Id::Z: {
      const int pos_index = int(dim) - int(pdal::Dimension::Id::X);
      if (position_channel_types_[pos_index] == pdal::Dimension::Type::Double)
      {
        sample.position[pos_index] = *static_cast<const double *>(val);
      }
      else
      {
        sample.position[pos_index] = double(*static_cast<const float *>(val));
      }
    }
    break;
    case pdal::Dimension::Id::NormalX:
    case pdal::Dimension::Id::NormalY:
    case pdal::Dimension::Id::NormalZ: {
      const int pos_index = int(dim) - int(pdal::Dimension::Id::NormalX);
      if (normal_channel_types_[pos_index] == pdal::Dimension::Type::Double)
      {
        sample.normal[pos_index] = *static_cast<const double *>(val);
      }
      else
      {
        sample.normal[pos_index] = double(*static_cast<const float *>(val));
      }
    }
    break;
    case pdal::Dimension::Id::Red:
    case pdal::Dimension::Id::Green:
    case pdal::Dimension::Id::Blue: {
      const int colour_index = int(dim) - int(pdal::Dimension::Id::Red);
      switch (colour_channel_types_[colour_index])
      {
      case pdal::Dimension::Type::Signed8:  // Ignore sign
      case pdal::Dimension::Type::Unsigned8:
        sample.colour[colour_index] = *static_cast<const uint8_t *>(val);
        break;
      case pdal::Dimension::Type::Signed16:  // Ignore sign
      case pdal::Dimension::Type::Unsigned16:
        sample.colour[colour_index] =
          uint8_t(255.0 * double(*static_cast<const uint16_t *>(val)) / double(std::numeric_limits<uint16_t>::max()));
        break;
      case pdal::Dimension::Type::Signed32:  // Ignore sign
      case pdal::Dimension::Type::Unsigned32:
        sample.colour[colour_index] =
          uint8_t(255.0 * double(*static_cast<const uint32_t *>(val)) / double(std::numeric_limits<uint32_t>::max()));
        break;
      default:
        break;
      }
    }
    break;
    case pdal::Dimension::Id::Intensity:
      switch (intensity_channel_type_)
      {
      case pdal::Dimension::Type::Unsigned8:
        sample.intensity = float(*static_cast<const uint8_t *>(val));
        break;
      case pdal::Dimension::Type::Unsigned16:
        sample.intensity = float(*static_cast<const uint16_t *>(val));
        break;
      case pdal::Dimension::Type::Unsigned32:
        sample.intensity = float(*static_cast<const uint32_t *>(val));
        break;
      case pdal::Dimension::Type::Float:
        sample.intensity = *static_cast<const float *>(val);
        break;
      case pdal::Dimension::Type::Double:
        sample.intensity = float(*static_cast<const double *>(val));
        break;
      default:
        break;
      }
      break;
    default:
      if (dim == time_dimension_)
      {
        if (time_channel_type_ == pdal::Dimension::Type::Double)
        {
          sample.timestamp = *static_cast<const double *>(val);
        }
        else
        {
          sample.timestamp = double(*static_cast<const float *>(val));
        }
      }
      break;
    }
  }
}

/// Called whenever the buffer capacity is filled before starting on the next block.
void PointStream::reset()
{
  if (!abort_)
  {
    std::unique_lock<std::mutex> guard(buffer_mutex_);
    const int read_buffer = 1 - write_index_;
    flip_wait_.wait(guard, [this, read_buffer]() { return abort_ || next_read_ >= buffers_[read_buffer].size(); });
    write_index_ = read_buffer;
    buffers_[read_buffer].clear();
    next_read_ = 0;
    have_data_ = true;
  }
}
}  // namespace slamio


================================================
FILE: slamio/pdal/PointStream.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef SLAMIO_PDAL_POINTSTREAM_H_
#define SLAMIO_PDAL_POINTSTREAM_H_

#include "slamio/SlamIOConfig.h"

#include "slamio/DataChannel.h"
#include "slamio/Points.h"

#include <pdal/Dimension.hpp>
#include <pdal/PointTable.hpp>

#include <array>
#include <atomic>
#include <condition_variable>
#include <mutex>

namespace slamio
{
class PointStream : public pdal::StreamPointTable
{
public:
  explicit PointStream(size_t buffer_capacity, DataChannel require = DataChannel::Position | DataChannel::Time);

  /// Called when execute() is started.  Typically used to set buffer size
  /// when all dimensions are known.
  void finalize() override;

  bool nextPoint(CloudPoint &sample);

  inline bool isValid() const { return valid_dimensions_; }

  /// True once we have data available for reading.
  inline bool haveData() const { return have_data_; }

  /// True once loading has been completed (@c markLoadComplete() called) and the read index is at the end of the
  /// read buffer.
  inline bool done()
  {
    if (loading_complete_)
    {
      std::unique_lock<std::mutex> guard(buffer_mutex_);
      return next_read_ >= buffers_[1 - write_index_].size();
    }
    return false;
  }

  /// Mark for abort. No more data points are stored.
  inline void abort()
  {
    abort_ = true;
    flip_wait_.notify_all();
  }

  /// Mark loading as done: only to be called from the loading thread.
  inline void markLoadComplete() { loading_complete_ = true; }

  inline bool hasTimestamp() const { return (available_channels_ & DataChannel::Time) != DataChannel::None; }
  inline bool hasNormals() const { return (available_channels_ & DataChannel::Normal) != DataChannel::None; }
  inline bool hasColourRgb() const { return (available_channels_ & DataChannel::ColourRgb) != DataChannel::None; }
  inline bool hasColourAlpha() const { return (available_channels_ & DataChannel::ColourAlpha) != DataChannel::None; }
  inline bool hasIntensity() const { return (available_channels_ & DataChannel::Intensity) != DataChannel::None; }

protected:
  // Not supported
  inline char *getPoint(pdal::PointId /* idx */) override { return nullptr; }

  void setFieldInternal(pdal::Dimension::Id dim, pdal::PointId idx, const void *val) override;

  /// Called whenever the buffer capacity is filled before starting on the next block.
  void reset() override;

private:
  // Double buffer to allow background thread streaming.
  // Use w channel for timetstamp
  std::array<std::vector<CloudPoint>, 2> buffers_;
  std::array<pdal::Dimension::Type, 3> position_channel_types_{};
  std::array<pdal::Dimension::Type, 3> normal_channel_types_{};
  std::array<pdal::Dimension::Type, 3> colour_channel_types_{};
  pdal::Dimension::Id time_dimension_{ pdal::Dimension::Id::Unknown };
  pdal::Dimension::Type time_channel_type_{};
  pdal::Dimension::Type intensity_channel_type_{};
  std::atomic_uint next_read_{ 0 };
  std::atomic_int write_index_{ 0 };
  pdal::PointLayout layout_;
  std::mutex buffer_mutex_;
  std::condition_variable flip_wait_;
  std::atomic_bool have_data_{ false };
  std::atomic_bool loading_complete_{ false };
  std::atomic_bool abort_{ false };
  std::atomic_bool valid_dimensions_{ false };
  DataChannel available_channels_ = DataChannel::None;
  DataChannel required_channels_ = DataChannel::Position | DataChannel::Time;
};
}  // namespace slamio

#endif  // SLAMIO_PDAL_POINTSTREAM_H_


================================================
FILE: slamio/rply/LICENSE
================================================
RPly 1.1.4 license
Copyright  2003-2015 Diego Nehab.

Permission is hereby granted, free of charge, to any person obtaining a
copy of this software and associated documentation files (the "Software"),
to deal in the Software without restriction, including without limitation
the rights to use, copy, modify, merge, publish, distribute, sublicense,
and/or sell copies of the Software, and to permit persons to whom the
Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.


================================================
FILE: slamio/rply/rply.c
================================================
/* ----------------------------------------------------------------------
 * RPly library, read/write PLY files
 * Diego Nehab, IMPA
 * http://www.impa.br/~diego/software/rply
 *
 * This library is distributed under the MIT License. See notice
 * at the end of this file.
 * ---------------------------------------------------------------------- */
#include <stdio.h>
#include <ctype.h>
#include <assert.h>
#include <string.h>
#include <limits.h>
#include <float.h>
#include <stdarg.h>
#include <stdlib.h>
#include <stddef.h>

#include "rply.h"
#include "rplyfile.h"

/* ----------------------------------------------------------------------
 * Make sure we get our integer types right
 * ---------------------------------------------------------------------- */
#if defined(_MSC_VER) && (_MSC_VER < 1600)
/* C99 stdint.h only supported in MSVC++ 10.0 and up */
typedef __int8 t_ply_int8;
typedef __int16 t_ply_int16;
typedef __int32 t_ply_int32;
typedef unsigned __int8 t_ply_uint8;
typedef unsigned __int16 t_ply_uint16;
typedef unsigned __int32 t_ply_uint32;
#define PLY_INT8_MAX (127)
#define PLY_INT8_MIN (-PLY_INT8_MAX-1)
#define PLY_INT16_MAX (32767)
#define PLY_INT16_MIN (-PLY_INT16_MAX-1)
#define PLY_INT32_MAX (2147483647)
#define PLY_INT32_MIN (-PLY_INT32_MAX-1)
#define PLY_UINT8_MAX (255)
#define PLY_UINT16_MAX (65535)
#define PLY_UINT32_MAX  (4294967295)
#else
#include <stdint.h>
typedef int8_t t_ply_int8;
typedef int16_t t_ply_int16;
typedef int32_t t_ply_int32;
typedef uint8_t t_ply_uint8;
typedef uint16_t t_ply_uint16;
typedef uint32_t t_ply_uint32;
#define PLY_INT8_MIN INT8_MIN
#define PLY_INT8_MAX INT8_MAX
#define PLY_INT16_MIN INT16_MIN
#define PLY_INT16_MAX INT16_MAX
#define PLY_INT32_MIN INT32_MIN
#define PLY_INT32_MAX INT32_MAX
#define PLY_UINT8_MAX UINT8_MAX
#define PLY_UINT16_MAX UINT16_MAX
#define PLY_UINT32_MAX UINT32_MAX
#endif

/* ----------------------------------------------------------------------
 * Constants
 * ---------------------------------------------------------------------- */
#define WORDSIZE 256
#define LINESIZE 1024
#define BUFFERSIZE (8*1024)

typedef enum e_ply_io_mode_ {
    PLY_READ,
    PLY_WRITE
} e_ply_io_mode;

static const char *const ply_storage_mode_list[] = {
    "binary_big_endian", "binary_little_endian", "ascii", NULL
};     /* order matches e_ply_storage_mode enum */

static const char *const ply_type_list[] = {
    "int8", "uint8", "int16", "uint16",
    "int32", "uint32", "float32", "float64",
    "char", "uchar", "short", "ushort",
    "int", "uint", "float", "double",
    "list", NULL
};     /* order matches e_ply_type enum */

/* ----------------------------------------------------------------------
 * Property reading callback argument
 *
 * element: name of element being processed
 * property: name of property being processed
 * nelements: number of elements of this kind in file
 * instance_index: index current element of this kind being processed
 * length: number of values in current list (or 1 for scalars)
 * value_index: index of current value int this list (or 0 for scalars)
 * value: value of property
 * pdata/idata: user data defined with ply_set_cb
 *
 * Returns handle to PLY file if succesful, NULL otherwise.
 * ---------------------------------------------------------------------- */
typedef struct t_ply_argument_ {
    p_ply_element element;
    long instance_index;
    p_ply_property property;
    long length, value_index;
    double value;
    void *pdata;
    long idata;
} t_ply_argument;

/* ----------------------------------------------------------------------
 * Property information
 *
 * name: name of this property
 * type: type of this property (list or type of scalar value)
 * length_type, value_type: type of list property count and values
 * read_cb: function to be called when this property is called
 *
 * Returns 1 if should continue processing file, 0 if should abort.
 * ---------------------------------------------------------------------- */
typedef struct t_ply_property_ {
    char name[WORDSIZE];
    e_ply_type type, value_type, length_type;
    p_ply_read_cb read_cb;
    void *pdata;
    long idata;
} t_ply_property;

/* ----------------------------------------------------------------------
 * Element information
 *
 * name: name of this property
 * ninstances: number of elements of this type in file
 * property: property descriptions for this element
 * nproperty: number of properties in this element
 *
 * Returns 1 if should continue processing file, 0 if should abort.
 * ---------------------------------------------------------------------- */
typedef struct t_ply_element_ {
    char name[WORDSIZE];
    long ninstances;
    p_ply_property property;
    long nproperties;
} t_ply_element;

/* ----------------------------------------------------------------------
 * Input/output driver
 *
 * Depending on file mode, different functions are used to read/write
 * property fields. The drivers make it transparent to read/write in ascii,
 * big endian or little endian cases.
 * ---------------------------------------------------------------------- */
typedef int (*p_ply_ihandler)(p_ply ply, double *value);
typedef int (*p_ply_ichunk)(p_ply ply, void *anydata, size_t size);
typedef struct t_ply_idriver_ {
    p_ply_ihandler ihandler[16];
    p_ply_ichunk ichunk;
    const char *name;
} t_ply_idriver;
typedef t_ply_idriver *p_ply_idriver;

typedef int (*p_ply_ohandler)(p_ply ply, double value);
typedef int (*p_ply_ochunk)(p_ply ply, void *anydata, size_t size);
typedef struct t_ply_odriver_ {
    p_ply_ohandler ohandler[16];
    p_ply_ochunk ochunk;
    const char *name;
} t_ply_odriver;
typedef t_ply_odriver *p_ply_odriver;

/* ----------------------------------------------------------------------
 * Ply file handle.
 *
 * io_mode: read or write (from e_ply_io_mode)
 * storage_mode: mode of file associated with handle (from e_ply_storage_mode)
 * element: elements description for this file
 * nelement: number of different elements in file
 * comment: comments for this file
 * ncomments: number of comments in file
 * obj_info: obj_info items for this file
 * nobj_infos: number of obj_info items in file
 * fp: file pointer associated with ply file
 * rn: skip extra char after end_header?
 * buffer: last word/chunck of data read from ply file
 * buffer_first, buffer_last: interval of untouched good data in buffer
 * buffer_token: start of parsed token (line or word) in buffer
 * idriver, odriver: input driver used to get property fields from file
 * argument: storage space for callback arguments
 * welement, wproperty: element/property type being written
 * winstance_index: index of instance of current element being written
 * wvalue_index: index of list property value being written
 * wlength: number of values in list property being written
 * error_cb: error callback
 * pdata/idata: user data defined with ply_open/ply_create
 * ---------------------------------------------------------------------- */
typedef struct t_ply_ {
    e_ply_io_mode io_mode;
    e_ply_storage_mode storage_mode;
    p_ply_element element;
    long nelements;
    char *comment;
    long ncomments;
    char *obj_info;
    long nobj_infos;
    FILE *fp;
    int own_fp;
    int rn;
    char buffer[BUFFERSIZE];
    size_t buffer_first, buffer_token, buffer_last;
    p_ply_idriver idriver;
    p_ply_odriver odriver;
    t_ply_argument argument;
    long welement, wproperty;
    long winstance_index, wvalue_index, wlength;
    p_ply_error_cb error_cb;
    void *pdata;
    long idata;
} t_ply;

/* ----------------------------------------------------------------------
 * I/O functions and drivers
 * ---------------------------------------------------------------------- */
static t_ply_idriver ply_idriver_ascii;
static t_ply_idriver ply_idriver_binary;
static t_ply_idriver ply_idriver_binary_reverse;
static t_ply_odriver ply_odriver_ascii;
static t_ply_odriver ply_odriver_binary;
static t_ply_odriver ply_odriver_binary_reverse;

static int ply_read_word(p_ply ply);
static int ply_check_word(p_ply ply);
static void ply_finish_word(p_ply ply, size_t size);
static int ply_read_line(p_ply ply);
static int ply_check_line(p_ply ply);
static int ply_read_chunk(p_ply ply, void *anybuffer, size_t size);
static int ply_read_chunk_reverse(p_ply ply, void *anybuffer, size_t size);
static int ply_write_chunk(p_ply ply, void *anybuffer, size_t size);
static int ply_write_chunk_reverse(p_ply ply, void *anybuffer, size_t size);
static void ply_reverse(void *anydata, size_t size);

/* ----------------------------------------------------------------------
 * String functions
 * ---------------------------------------------------------------------- */
static int ply_find_string(const char *item, const char* const list[]);
static p_ply_element ply_find_element(p_ply ply, const char *name);
static p_ply_property ply_find_property(p_ply_element element,
        const char *name);

/* ----------------------------------------------------------------------
 * Header parsing
 * ---------------------------------------------------------------------- */
static int ply_read_header_magic(p_ply ply);
static int ply_read_header_format(p_ply ply);
static int ply_read_header_comment(p_ply ply);
static int ply_read_header_obj_info(p_ply ply);
static int ply_read_header_property(p_ply ply);
static int ply_read_header_element(p_ply ply);

/* ----------------------------------------------------------------------
 * Error handling
 * ---------------------------------------------------------------------- */
static void ply_error_cb(p_ply ply, const char *message);
static void ply_ferror(p_ply ply, const char *fmt, ...);

/* ----------------------------------------------------------------------
 * Memory allocation and initialization
 * ---------------------------------------------------------------------- */
static void ply_init(p_ply ply);
static void ply_element_init(p_ply_element element);
static void ply_property_init(p_ply_property property);
static p_ply ply_alloc(void);
static p_ply_element ply_grow_element(p_ply ply);
static p_ply_property ply_grow_property(p_ply ply, p_ply_element element);
static void *ply_grow_array(p_ply ply, void **pointer, long *nmemb, long size);

/* ----------------------------------------------------------------------
 * Special functions
 * ---------------------------------------------------------------------- */
static e_ply_storage_mode ply_arch_endian(void);
static int ply_type_check(void);

/* ----------------------------------------------------------------------
 * Auxiliary read functions
 * ---------------------------------------------------------------------- */
// static int ply_read_element(p_ply ply, p_ply_element element,
//         p_ply_argument argument);
static int ply_read_property(p_ply ply, p_ply_element element,
        p_ply_property property, p_ply_argument argument);
static int ply_read_list_property(p_ply ply, p_ply_element element,
        p_ply_property property, p_ply_argument argument);
static int ply_read_scalar_property(p_ply ply, p_ply_element element,
        p_ply_property property, p_ply_argument argument);

/* ----------------------------------------------------------------------
 * Buffer support functions
 * ---------------------------------------------------------------------- */
/* pointers to tokenized word and line in buffer */
#define BWORD(p) (p->buffer + p->buffer_token)
#define BLINE(p) (p->buffer + p->buffer_token)

/* pointer to start of untouched bytes in buffer */
#define BFIRST(p) (p->buffer + p->buffer_first)

/* number of bytes untouched in buffer */
#define BSIZE(p) (p->buffer_last - p->buffer_first)

/* consumes data from buffer */
#define BSKIP(p, s) (p->buffer_first += s)

/* refills the buffer */
static int BREFILL(p_ply ply) {
    /* move untouched data to beginning of buffer */
    size_t size = BSIZE(ply);
    memmove(ply->buffer, BFIRST(ply), size);
    ply->buffer_last = size;
    ply->buffer_first = ply->buffer_token = 0;
    /* fill remaining with new data */
    size = fread(ply->buffer+size, 1, BUFFERSIZE-size-1, ply->fp);
    /* increase size to account for new data */
    ply->buffer_last += size;
    /* place sentinel so we can use str* functions with buffer */
    ply->buffer[ply->buffer_last] = '\0';
    /* check if read failed */
    return size > 0;
}

/* We don't care about end-of-line, generally, because we
 * separate words by any white-space character.
 * Unfortunately, in binary mode, right after 'end_header',
 * we have to know *exactly* how many characters to skip */
/* We use the end-of-line marker after the 'ply' magic
 * number to figure out what to do */
static int ply_read_header_magic(p_ply ply) {
    char *magic = ply->buffer;
    if (!BREFILL(ply)) {
        ply->error_cb(ply, "Unable to read magic number from file");
        return 0;
    }
    /* check if it is ply */
    if (magic[0] != 'p' || magic[1] != 'l' || magic[2] != 'y'
            || !isspace(magic[3])) {
        ply->error_cb(ply, "Wrong magic number. Expected 'ply'");
        return 0;
    }
    /* figure out if we have to skip the extra character
     * after header when we reach the binary part of file */
    ply->rn = magic[3] == '\r' && magic[4] == '\n';
    BSKIP(ply, 3);
    return 1;
}

/* ----------------------------------------------------------------------
 * Exported functions
 * ---------------------------------------------------------------------- */
/* ----------------------------------------------------------------------
 * Read support functions
 * ---------------------------------------------------------------------- */
p_ply ply_open(const char *name, p_ply_error_cb error_cb,
        long idata, void *pdata) {
    FILE *fp;
    p_ply ply;
    if (error_cb == NULL) error_cb = ply_error_cb;
    assert(name);
    fp = fopen(name, "rb");
    if (!fp) {
        error_cb(NULL, "Unable to open file");
        return NULL;
    }
    ply = ply_open_from_file(fp, error_cb, idata, pdata);
    if (ply) ply->own_fp = 1;
    else fclose(fp);
    return ply;
}

p_ply ply_open_from_file(FILE *fp, p_ply_error_cb error_cb,
        long idata, void *pdata) {
    p_ply ply;
    ply = NULL;
    if (error_cb == NULL) error_cb = ply_error_cb;
    assert(fp);
    if (!ply_type_check()) {
        error_cb(ply, "Incompatible type system");
        return NULL;
    }
    ply = ply_alloc();
    if (!ply) {
        error_cb(NULL, "Out of memory");
        return NULL;
    }
    ply->idata = idata;
    ply->pdata = pdata;
    ply->io_mode = PLY_READ;
    ply->error_cb = error_cb;
    ply->fp = fp;
    ply->own_fp = 0;
    return ply;
}

int ply_read_header(p_ply ply) {
    assert(ply && ply->fp && ply->io_mode == PLY_READ);
    if (!ply_read_header_magic(ply)) return 0;
    if (!ply_read_word(ply)) return 0;
    /* parse file format */
    if (!ply_read_header_format(ply)) {
        ply_ferror(ply, "Invalid file format");
        return 0;
    }
    /* parse elements, comments or obj_infos until the end of header */
    while (strcmp(BWORD(ply), "end_header")) {
        if (!ply_read_header_comment(ply) &&
                !ply_read_header_element(ply) &&
                !ply_read_header_obj_info(ply)) {
            ply_ferror(ply, "Unexpected token '%s'", BWORD(ply));
            return 0;
        }
    }
    /* skip extra character? */
    if (ply->rn) {
        if (BSIZE(ply) < 1 && !BREFILL(ply)) {
            ply_ferror(ply, "Unexpected end of file");
            return 0;
        }
        BSKIP(ply, 1);
    }
    return 1;
}

long ply_set_read_cb(p_ply ply, const char *element_name,
        const char* property_name, p_ply_read_cb read_cb,
        void *pdata, long idata) {
    p_ply_element element = NULL;
    p_ply_property property = NULL;
    assert(ply && element_name && property_name);
    element = ply_find_element(ply, element_name);
    if (!element) return 0;
    property = ply_find_property(element, property_name);
    if (!property) return 0;
    property->read_cb = read_cb;
    property->pdata = pdata;
    property->idata = idata;
    return (int) element->ninstances;
}

int ply_read(p_ply ply) {
    long i;
    p_ply_argument argument;
    assert(ply && ply->fp && ply->io_mode == PLY_READ);
    argument = &ply->argument;
    /* for each element type */
    for (i = 0; i < ply->nelements; i++) {
        p_ply_element element = &ply->element[i];
        argument->element = element;
        if (!ply_read_element(ply, element, argument))
            return 0;
    }
    return 1;
}

/* ----------------------------------------------------------------------
 * Write support functions
 * ---------------------------------------------------------------------- */
p_ply ply_create(const char *name, e_ply_storage_mode storage_mode,
        p_ply_error_cb error_cb, long idata, void *pdata) {
    p_ply ply = NULL;
    FILE *fp = NULL;
    assert(name && storage_mode <= PLY_DEFAULT);
    if (error_cb == NULL) error_cb = ply_error_cb;
    fp = fopen(name, "wb");
    if (!fp) {
        error_cb(ply, "Unable to create file");
        return NULL;
    }
    ply = ply_create_to_file(fp, storage_mode, error_cb, idata, pdata);
    if (ply) ply->own_fp = 1;
    else fclose(fp);
    return ply;
}

p_ply ply_create_to_file(FILE *fp, e_ply_storage_mode storage_mode,
        p_ply_error_cb error_cb, long idata, void *pdata) {
    p_ply ply;
    ply = NULL;
    assert(fp && storage_mode <= PLY_DEFAULT);
    if (!ply_type_check()) {
        error_cb(ply, "Incompatible type system");
        return NULL;
    }
    ply = ply_alloc();
    if (!ply) {
        error_cb(NULL, "Out of memory");
        return NULL;
    }
    ply->idata = idata;
    ply->pdata = pdata;
    ply->io_mode = PLY_WRITE;
    if (storage_mode == PLY_DEFAULT) storage_mode = ply_arch_endian();
    if (storage_mode == PLY_ASCII) ply->odriver = &ply_odriver_ascii;
    else if (storage_mode == ply_arch_endian())
        ply->odriver = &ply_odriver_binary;
    else ply->odriver = &ply_odriver_binary_reverse;
    ply->storage_mode = storage_mode;
    ply->fp = fp;
    ply->own_fp = 0;
    ply->error_cb = error_cb;
    return ply;
}


int ply_add_element(p_ply ply, const char *name, long ninstances) {
    p_ply_element element = NULL;
    assert(ply && ply->fp && ply->io_mode == PLY_WRITE);
    assert(name && strlen(name) < WORDSIZE && ninstances >= 0);
    if (strlen(name) >= WORDSIZE || ninstances < 0) {
        ply_ferror(ply, "Invalid arguments");
        return 0;
    }
    element = ply_grow_element(ply);
    if (!element) return 0;
    strcpy(element->name, name);
    element->ninstances = ninstances;
    return 1;
}

int ply_add_scalar_property(p_ply ply, const char *name, e_ply_type type) {
    p_ply_element element = NULL;
    p_ply_property property = NULL;
    assert(ply && ply->fp && ply->io_mode == PLY_WRITE);
    assert(name && strlen(name) < WORDSIZE);
    assert(type < PLY_LIST);
    if (strlen(name) >= WORDSIZE || type >= PLY_LIST) {
        ply_ferror(ply, "Invalid arguments");
        return 0;
    }
    element = &ply->element[ply->nelements-1];
    property = ply_grow_property(ply, element);
    if (!property) return 0;
    strcpy(property->name, name);
    property->type = type;
    return 1;
}

int ply_add_list_property(p_ply ply, const char *name,
        e_ply_type length_type, e_ply_type value_type) {
    p_ply_element element = NULL;
    p_ply_property property = NULL;
    assert(ply && ply->fp && ply->io_mode == PLY_WRITE);
    assert(name && strlen(name) < WORDSIZE);
    if (strlen(name) >= WORDSIZE) {
        ply_ferror(ply, "Invalid arguments");
        return 0;
    }
    assert(length_type < PLY_LIST);
    assert(value_type < PLY_LIST);
    if (length_type >= PLY_LIST || value_type >= PLY_LIST) {
        ply_ferror(ply, "Invalid arguments");
        return 0;
    }
    element = &ply->element[ply->nelements-1];
    property = ply_grow_property(ply, element);
    if (!property) return 0;
    strcpy(property->name, name);
    property->type = PLY_LIST;
    property->length_type = length_type;
    property->value_type = value_type;
    return 1;
}

int ply_add_property(p_ply ply, const char *name, e_ply_type type,
        e_ply_type length_type, e_ply_type value_type) {
    if (type == PLY_LIST)
        return ply_add_list_property(ply, name, length_type, value_type);
    else
        return ply_add_scalar_property(ply, name, type);
}

int ply_add_comment(p_ply ply, const char *comment) {
    char *new_comment = NULL;
    assert(ply && comment && strlen(comment) < LINESIZE);
    if (!comment || strlen(comment) >= LINESIZE) {
        ply_ferror(ply, "Invalid arguments");
        return 0;
    }
    new_comment = (char *) ply_grow_array(ply, (void **) &ply->comment,
            &ply->ncomments, LINESIZE);
    if (!new_comment) return 0;
    strcpy(new_comment, comment);
    return 1;
}

int ply_add_obj_info(p_ply ply, const char *obj_info) {
    char *new_obj_info = NULL;
    assert(ply && obj_info && strlen(obj_info) < LINESIZE);
    if (!obj_info || strlen(obj_info) >= LINESIZE) {
        ply_ferror(ply, "Invalid arguments");
        return 0;
    }
    new_obj_info = (char *) ply_grow_array(ply, (void **) &ply->obj_info,
            &ply->nobj_infos, LINESIZE);
    if (!new_obj_info) return 0;
    strcpy(new_obj_info, obj_info);
    return 1;
}

int ply_write_header(p_ply ply) {
    long i, j;
    assert(ply && ply->fp && ply->io_mode == PLY_WRITE);
    assert(ply->element || ply->nelements == 0);
    assert(!ply->element || ply->nelements > 0);
    if (fprintf(ply->fp, "ply\nformat %s 1.0\n",
                ply_storage_mode_list[ply->storage_mode]) <= 0) goto error;
    for (i = 0; i < ply->ncomments; i++)
        if (fprintf(ply->fp, "comment %s\n", ply->comment + LINESIZE*i) <= 0)
            goto error;
    for (i = 0; i < ply->nobj_infos; i++)
        if (fprintf(ply->fp, "obj_info %s\n", ply->obj_info + LINESIZE*i) <= 0)
            goto error;
    for (i = 0; i < ply->nelements; i++) {
        p_ply_element element = &ply->element[i];
        assert(element->property || element->nproperties == 0);
        assert(!element->property || element->nproperties > 0);
        if (fprintf(ply->fp, "element %s %ld\n", element->name,
                    element->ninstances) <= 0) goto error;
        for (j = 0; j < element->nproperties; j++) {
            p_ply_property property = &element->property[j];
            if (property->type == PLY_LIST) {
                if (fprintf(ply->fp, "property list %s %s %s\n",
                            ply_type_list[property->length_type],
                            ply_type_list[property->value_type],
                            property->name) <= 0) goto error;
            } else {
                if (fprintf(ply->fp, "property %s %s\n",
                            ply_type_list[property->type],
                            property->name) <= 0) goto error;
            }
        }
    }
    return fprintf(ply->fp, "end_header\n") > 0;
error:
    ply_ferror(ply, "Error writing to file");
    return 0;
}

int ply_write(p_ply ply, double value) {
    p_ply_element element = NULL;
    p_ply_property property = NULL;
    int type = -1;
    int breakafter = 0;
    int spaceafter = 1;
    if (ply->welement > ply->nelements) return 0;
    element = &ply->element[ply->welement];
    if (ply->wproperty > element->nproperties) return 0;
    property = &element->property[ply->wproperty];
    if (property->type == PLY_LIST) {
        if (ply->wvalue_index == 0) {
            type = property->length_type;
            ply->wlength = (long) value;
        } else type = property->value_type;
    } else {
        type = property->type;
        ply->wlength = 0;
    }
    if (!ply->odriver->ohandler[type](ply, value)) {
        ply_ferror(ply, "Failed writing %s of %s %d (%s: %s)",
                    property->name, element->name,
                    ply->winstance_index,
                    ply->odriver->name, ply_type_list[type]);
        return 0;
    }
    ply->wvalue_index++;
    if (ply->wvalue_index > ply->wlength) {
        ply->wvalue_index = 0;
        ply->wproperty++;
    }
    if (ply->wproperty >= element->nproperties) {
        ply->wproperty = 0;
        ply->winstance_index++;
        breakafter = 1;
        spaceafter = 0;
    }
    if (ply->winstance_index >= element->ninstances) {
        ply->winstance_index = 0;
        do {
            ply->welement++;
            element = &ply->element[ply->welement];
        } while (ply->welement < ply->nelements && !element->ninstances);
    }
    if (ply->storage_mode == PLY_ASCII) {
        return (!spaceafter || putc(' ', ply->fp) > 0) &&
               (!breakafter || putc('\n', ply->fp) > 0);
    } else {
        return 1;
    }
}

int ply_close(p_ply ply) {
    long i;
    assert(ply && ply->fp);
    assert(ply->element || ply->nelements == 0);
    assert(!ply->element || ply->nelements > 0);
    /* write last chunk to file */
    if (ply->io_mode == PLY_WRITE &&
      fwrite(ply->buffer, 1, ply->buffer_last, ply->fp) < ply->buffer_last) {
        ply_ferror(ply, "Error closing up");
        return 0;
    }
    if (ply->own_fp) fclose(ply->fp);
    /* free all memory used by handle */
    if (ply->element) {
        for (i = 0; i < ply->nelements; i++) {
            p_ply_element element = &ply->element[i];
            if (element->property) free(element->property);
        }
        free(ply->element);
    }
    if (ply->obj_info) free(ply->obj_info);
    if (ply->comment) free(ply->comment);
    free(ply);
    return 1;
}

/* ----------------------------------------------------------------------
 * Query support functions
 * ---------------------------------------------------------------------- */
p_ply_element ply_get_next_element(p_ply ply,
        p_ply_element last) {
    assert(ply);
    if (!last) return ply->element;
    last++;
    if (last < ply->element + ply->nelements) return last;
    else return NULL;
}

int ply_get_element_info(p_ply_element element, const char** name,
        long *ninstances) {
    assert(element);
    if (name) *name = element->name;
    if (ninstances) *ninstances = (long) element->ninstances;
    return 1;
}

p_ply_property ply_get_next_property(p_ply_element element,
        p_ply_property last) {
    assert(element);
    if (!last) return element->property;
    last++;
    if (last < element->property + element->nproperties) return last;
    else return NULL;
}

int ply_get_property_info(p_ply_property property, const char** name,
        e_ply_type *type, e_ply_type *length_type, e_ply_type *value_type) {
    assert(property);
    if (name) *name = property->name;
    if (type) *type = property->type;
    if (length_type) *length_type = property->length_type;
    if (value_type) *value_type = property->value_type;
    return 1;

}

const char *ply_get_next_comment(p_ply ply, const char *last) {
    assert(ply);
    if (!last) return ply->comment;
    last += LINESIZE;
    if (last < ply->comment + LINESIZE*ply->ncomments) return last;
    else return NULL;
}

const char *ply_get_next_obj_info(p_ply ply, const char *last) {
    assert(ply);
    if (!last) return ply->obj_info;
    last += LINESIZE;
    if (last < ply->obj_info + LINESIZE*ply->nobj_infos) return last;
    else return NULL;
}

/* ----------------------------------------------------------------------
 * Callback argument support functions
 * ---------------------------------------------------------------------- */
int ply_get_argument_element(p_ply_argument argument,
        p_ply_element *element, long *instance_index) {
    assert(argument);
    if (!argument) return 0;
    if (element) *element = argument->element;
    if (instance_index) *instance_index = argument->instance_index;
    return 1;
}

int ply_get_argument_property(p_ply_argument argument,
        p_ply_property *property, long *length, long *value_index) {
    assert(argument);
    if (!argument) return 0;
    if (property) *property = argument->property;
    if (length) *length = argument->length;
    if (value_index) *value_index = argument->value_index;
    return 1;
}

int ply_get_argument_user_data(p_ply_argument argument, void **pdata,
        long *idata) {
    assert(argument);
    if (!argument) return 0;
    if (pdata) *pdata = argument->pdata;
    if (idata) *idata = argument->idata;
    return 1;
}

double ply_get_argument_value(p_ply_argument argument) {
    assert(argument);
    if (!argument) return 0.0;
    return argument->value;
}

int ply_get_ply_user_data(p_ply ply, void **pdata, long *idata) {
    assert(ply);
    if (!ply) return 0;
    if (pdata) *pdata = ply->pdata;
    if (idata) *idata = ply->idata;
    return 1;
}

/* ----------------------------------------------------------------------
 * Internal functions
 * ---------------------------------------------------------------------- */
static int ply_read_list_property(p_ply ply, p_ply_element element,
        p_ply_property property, p_ply_argument argument) {
    int l;
    p_ply_read_cb read_cb = property->read_cb;
    p_ply_ihandler *driver = ply->idriver->ihandler;
    /* get list length */
    p_ply_ihandler handler = driver[property->length_type];
    double length;
    if (!handler(ply, &length)) {
        ply_ferror(ply, "Error reading '%s' of '%s' number %d",
                property->name, element->name, argument->instance_index);
        return 0;
    }
    /* invoke callback to pass length in value field */
    argument->length = (long) length;
    argument->value_index = -1;
    argument->value = length;
    if (read_cb && !read_cb(argument)) {
        ply_ferror(ply, "Aborted by user");
        return 0;
    }
    /* read list values */
    handler = driver[property->value_type];
    /* for each value in list */
    for (l = 0; l < (long) length; l++) {
        /* read value from file */
        argument->value_index = l;
        if (!handler(ply, &argument->value)) {
            ply_ferror(ply, "Error reading value number %d of '%s' of "
                    "'%s' number %d", l+1, property->name,
                    element->name, argument->instance_index);
            return 0;
        }
        /* invoke callback to pass value */
        if (read_cb && !read_cb(argument)) {
            ply_ferror(ply, "Aborted by user");
            return 0;
        }
    }
    return 1;
}

static int ply_read_scalar_property(p_ply ply, p_ply_element element,
        p_ply_property property, p_ply_argument argument) {
    p_ply_read_cb read_cb = property->read_cb;
    p_ply_ihandler *driver = ply->idriver->ihandler;
    p_ply_ihandler handler = driver[property->type];
    argument->length = 1;
    argument->value_index = 0;
    if (!handler(ply, &argument->value)) {
        ply_ferror(ply, "Error reading '%s' of '%s' number %d",
                property->name, element->name, argument->instance_index);
        return 0;
    }
    if (read_cb && !read_cb(argument)) {
        ply_ferror(ply, "Aborted by user");
        return 0;
    }
    return 1;
}

static int ply_read_property(p_ply ply, p_ply_element element,
        p_ply_property property, p_ply_argument argument) {
    if (property->type == PLY_LIST)
        return ply_read_list_property(ply, element, property, argument);
    else
        return ply_read_scalar_property(ply, element, property, argument);
}

/*static*/ int ply_read_element(p_ply ply, p_ply_element element,
        p_ply_argument argument) {
    long j, k;
    /* for each element of this type */
    for (j = 0; j < element->ninstances; j++) {
        argument->instance_index = j;
        /* for each property */
        for (k = 0; k < element->nproperties; k++) {
            p_ply_property property = &element->property[k];
            argument->property = property;
            argument->pdata = property->pdata;
            argument->idata = property->idata;
            if (!ply_read_property(ply, element, property, argument))
                return 0;
        }
    }
    return 1;
}

static int ply_find_string(const char *item, const char* const list[]) {
    int i;
    assert(item && list);
    for (i = 0; list[i]; i++)
        if (!strcmp(list[i], item)) return i;
    return -1;
}

static p_ply_element ply_find_element(p_ply ply, const char *name) {
    p_ply_element element;
    int i, nelements;
    assert(ply && name);
    element = ply->element;
    nelements = ply->nelements;
    assert(element || nelements == 0);
    assert(!element || nelements > 0);
    for (i = 0; i < nelements; i++)
        if (!strcmp(element[i].name, name)) return &element[i];
    return NULL;
}

static p_ply_property ply_find_property(p_ply_element element,
        const char *name) {
    p_ply_property property;
    int i, nproperties;
    assert(element && name);
    property = element->property;
    nproperties = element->nproperties;
    assert(property || nproperties == 0);
    assert(!property || nproperties > 0);
    for (i = 0; i < nproperties; i++)
        if (!strcmp(property[i].name, name)) return &property[i];
    return NULL;
}

static int ply_check_word(p_ply ply) {
    size_t size = strlen(BWORD(ply));
    if (size >= WORDSIZE) {
        ply_ferror(ply, "Word too long");
        return 0;
    }
    else if (size == 0)
    {
      ply_ferror(ply, "Unexpected end of file");
      return 0;
    }
    return 1;
}

static int ply_read_word(p_ply ply) {
    size_t t = 0;
    assert(ply && ply->fp && ply->io_mode == PLY_READ);
    /* skip leading blanks */
    while (1) {
        t = strspn(BFIRST(ply), " \n\r\t");
        /* check if all buffer was made of blanks */
        if (t >= BSIZE(ply)) {
            if (!BREFILL(ply)) {
                ply_ferror(ply, "Unexpected end of file");
                return 0;
            }
        }
        else
          break;
    }
    BSKIP(ply, t);
    /* look for a space after the current word */
    t = strcspn(BFIRST(ply), " \n\r\t");
    /* if we didn't reach the end of the buffer, we are done */
    if (t < BSIZE(ply)) {
        ply_finish_word(ply, t);
        return ply_check_word(ply);
    }
    /* otherwise, try to refill buffer */
    if (!BREFILL(ply)) {
        /* if we reached the end of file, try to do with what we have */
        ply_finish_word(ply, t);
        return ply_check_word(ply);
        /* ply_ferror(ply, "Unexpected end of file"); */
        /* return 0; */
    }
    /* keep looking from where we left */
    t += strcspn(BFIRST(ply) + t, " \n\r\t");
    /* check if the token is too large for our buffer */
    if (t >= BSIZE(ply)) {
        ply_ferror(ply, "Token too large");
        return 0;
    }
    /* we are done */
    ply_finish_word(ply, t);
    return ply_check_word(ply);
}

static void ply_finish_word(p_ply ply, size_t size) {
    ply->buffer_token = ply->buffer_first;
    BSKIP(ply, size);
    *BFIRST(ply) = '\0';
    BSKIP(ply, 1);
}

static int ply_check_line(p_ply ply) {
    if (strlen(BLINE(ply)) >= LINESIZE) {
        ply_ferror(ply, "Line too long");
        return 0;
    }
    return 1;
}

static int ply_read_line(p_ply ply) {
    const char *end = NULL;
    assert(ply && ply->fp && ply->io_mode == PLY_READ);
    /* look for a end of line */
    end = strchr(BFIRST(ply), '\n');
    /* if we didn't reach the end of the buffer, we are done */
    if (end) {
        ply->buffer_token = ply->buffer_first;
        BSKIP(ply, end - BFIRST(ply));
        *BFIRST(ply) = '\0';
        BSKIP(ply, 1);
        return ply_check_line(ply);
    }
    else
    {
      end = ply->buffer + BSIZE(ply);
      /* otherwise, try to refill buffer */
      if (!BREFILL(ply))
      {
        ply_ferror(ply, "Unexpected end of file");
        return 0;
      }
    }
    /* keep looking from where we left */
    end = strchr(end, '\n');
    /* check if the token is too large for our buffer */
    if (!end) {
        ply_ferror(ply, "Token too large");
        return 0;
    }
    /* we are done */
    ply->buffer_token = ply->buffer_first;
    BSKIP(ply, end - BFIRST(ply));
    *BFIRST(ply) = '\0';
    BSKIP(ply, 1);
    return ply_check_line(ply);
}

static int ply_read_chunk(p_ply ply, void *anybuffer, size_t size) {
    char *buffer = (char *) anybuffer;
    size_t i = 0;
    assert(ply && ply->fp && ply->io_mode == PLY_READ);
    assert(ply->buffer_first <= ply->buffer_last);
    while (i < size) {
        if (ply->buffer_first < ply->buffer_last) {
            buffer[i] = ply->buffer[ply->buffer_first];
            ply->buffer_first++;
            i++;
        }
        else
        {
          ply->buffer_first = 0;
          ply->buffer_last = fread(ply->buffer, 1, BUFFERSIZE, ply->fp);
          if (ply->buffer_last <= 0)
            return 0;
        }
    }
    return 1;
}

static int ply_write_chunk(p_ply ply, void *anybuffer, size_t size) {
    char *buffer = (char *) anybuffer;
    size_t i = 0;
    assert(ply && ply->fp && ply->io_mode == PLY_WRITE);
    assert(ply->buffer_last <= BUFFERSIZE);
    while (i < size) {
        if (ply->buffer_last < BUFFERSIZE) {
            ply->buffer[ply->buffer_last] = buffer[i];
            ply->buffer_last++;
            i++;
        }
        else
        {
          ply->buffer_last = 0;
          if (fwrite(ply->buffer, 1, BUFFERSIZE, ply->fp) < BUFFERSIZE)
            return 0;
        }
    }
    return 1;
}

static int ply_write_chunk_reverse(p_ply ply, void *anybuffer, size_t size) {
    int ret = 0;
    ply_reverse(anybuffer, size);
    ret = ply_write_chunk(ply, anybuffer, size);
    ply_reverse(anybuffer, size);
    return ret;
}

static int ply_read_chunk_reverse(p_ply ply, void *anybuffer, size_t size) {
    if (!ply_read_chunk(ply, anybuffer, size)) return 0;
    ply_reverse(anybuffer, size);
    return 1;
}

static void ply_reverse(void *anydata, size_t size) {
    char *data = (char *) anydata;
    char temp;
    size_t i;
    for (i = 0; i < size/2; i++) {
        temp = data[i];
        data[i] = data[size-i-1];
        data[size-i-1] = temp;
    }
}

static void ply_init(p_ply ply) {
    ply->element = NULL;
    ply->nelements = 0;
    ply->comment = NULL;
    ply->ncomments = 0;
    ply->obj_info = NULL;
    ply->nobj_infos = 0;
    ply->idriver = NULL;
    ply->odriver = NULL;
    ply->buffer[0] = '\0';
    ply->buffer_first = ply->buffer_last = ply->buffer_token = 0;
    ply->welement = 0;
    ply->wproperty = 0;
    ply->winstance_index = 0;
    ply->wlength = 0;
    ply->wvalue_index = 0;
}

static void ply_element_init(p_ply_element element) {
    element->name[0] = '\0';
    element->ninstances = 0;
    element->property = NULL;
    element->nproperties = 0;
}

static void ply_property_init(p_ply_property property) {
    property->name[0] = '\0';
    property->type = -1;
    property->length_type = -1;
    property->value_type = -1;
    property->read_cb = (p_ply_read_cb) NULL;
    property->pdata = NULL;
    property->idata = 0;
}

static p_ply ply_alloc(void) {
    p_ply ply = (p_ply) calloc(1, sizeof(t_ply));
    if (!ply) return NULL;
    ply_init(ply);
    return ply;
}

static void *ply_grow_array(p_ply ply, void **pointer,
        long *nmemb, long size) {
    void *temp = *pointer;
    long count = *nmemb + 1;
    if (!temp) temp = malloc(count*size);
    else temp = realloc(temp, count*size);
    if (!temp) {
        ply_ferror(ply, "Out of memory");
        return NULL;
    }
    *pointer = temp;
    *nmemb = count;
    return (char *) temp + (count-1) * size;
}

static p_ply_element ply_grow_element(p_ply ply) {
    p_ply_element element = NULL;
    assert(ply);
    assert(ply->element || ply->nelements == 0);
    assert(!ply->element || ply->nelements > 0);
    element = (p_ply_element) ply_grow_array(ply, (void **) &ply->element,
            &ply->nelements, sizeof(t_ply_element));
    if (!element) return NULL;
    ply_element_init(element);
    return element;
}

static p_ply_property ply_grow_property(p_ply ply, p_ply_element element) {
    p_ply_property property = NULL;
    assert(ply);
    assert(element);
    assert(element->property || element->nproperties == 0);
    assert(!element->property || element->nproperties > 0);
    property = (p_ply_property) ply_grow_array(ply,
            (void **) &element->property,
            &element->nproperties, sizeof(t_ply_property));
    if (!property) return NULL;
    ply_property_init(property);
    return property;
}

static int ply_read_header_format(p_ply ply) {
    assert(ply && ply->fp && ply->io_mode == PLY_READ);
    if (strcmp(BWORD(ply), "format")) return 0;
    if (!ply_read_word(ply)) return 0;
    ply->storage_mode = ply_find_string(BWORD(ply), ply_storage_mode_list);
    if (ply->storage_mode == (e_ply_storage_mode) (-1)) return 0;
    if (ply->storage_mode == PLY_ASCII) ply->idriver = &ply_idriver_ascii;
    else if (ply->storage_mode == ply_arch_endian())
        ply->idriver = &ply_idriver_binary;
    else ply->idriver = &ply_idriver_binary_reverse;
    if (!ply_read_word(ply)) return 0;
    if (strcmp(BWORD(ply), "1.0")) return 0;
    if (!ply_read_word(ply)) return 0;
    return 1;
}

static int ply_read_header_comment(p_ply ply) {
    assert(ply && ply->fp && ply->io_mode == PLY_READ);
    if (strcmp(BWORD(ply), "comment")) return 0;
    if (!ply_read_line(ply)) return 0;
    if (!ply_add_comment(ply, BLINE(ply))) return 0;
    if (!ply_read_word(ply)) return 0;
    return 1;
}

static int ply_read_header_obj_info(p_ply ply) {
    assert(ply && ply->fp && ply->io_mode == PLY_READ);
    if (strcmp(BWORD(ply), "obj_info")) return 0;
    if (!ply_read_line(ply)) return 0;
    if (!ply_add_obj_info(ply, BLINE(ply))) return 0;
    if (!ply_read_word(ply)) return 0;
    return 1;
}

static int ply_read_header_property(p_ply ply) {
    p_ply_element element = NULL;
    p_ply_property property = NULL;
    /* make sure it is a property */
    if (strcmp(BWORD(ply), "property")) return 0;
    element = &ply->element[ply->nelements-1];
    property = ply_grow_property(ply, element);
    if (!property) return 0;
    /* get property type */
    if (!ply_read_word(ply)) return 0;
    property->type = ply_find_string(BWORD(ply), ply_type_list);
    if (property->type == (e_ply_type) (-1)) return 0;
    if (property->type == PLY_LIST) {
        /* if it's a list, we need the base types */
        if (!ply_read_word(ply)) return 0;
        property->length_type = ply_find_string(BWORD(ply), ply_type_list);
        if (property->length_type == (e_ply_type) (-1)) return 0;
        if (!ply_read_word(ply)) return 0;
        property->value_type = ply_find_string(BWORD(ply), ply_type_list);
        if (property->value_type == (e_ply_type) (-1)) return 0;
    }
    /* get property name */
    if (!ply_read_word(ply)) return 0;
    strcpy(property->name, BWORD(ply));
    if (!ply_read_word(ply)) return 0;
    return 1;
}

static int ply_read_header_element(p_ply ply) {
    p_ply_element element = NULL;
    long dummy;
    assert(ply && ply->fp && ply->io_mode == PLY_READ);
    if (strcmp(BWORD(ply), "element")) return 0;
    /* allocate room for new element */
    element = ply_grow_element(ply);
    if (!element) return 0;
    /* get element name */
    if (!ply_read_word(ply)) return 0;
    strcpy(element->name, BWORD(ply));
    /* get number of elements of this type */
    if (!ply_read_word(ply)) return 0;
    if (sscanf(BWORD(ply), "%ld", &dummy) != 1) {
        ply_ferror(ply, "Expected number got '%s'", BWORD(ply));
        return 0;
    }
    element->ninstances = dummy;
    /* get all properties for this element */
    if (!ply_read_word(ply)) return 0;
    while (ply_read_header_property(ply) ||
        ply_read_header_comment(ply) || ply_read_header_obj_info(ply))
        /* do nothing */;
    return 1;
}

static void ply_error_cb(p_ply ply, const char *message) {
    (void) ply;
    fprintf(stderr, "RPly: %s\n", message);
}

static void ply_ferror(p_ply ply, const char *fmt, ...) {
    char buffer[1024];
    va_list ap;
    va_start(ap, fmt);
    vsprintf(buffer, fmt, ap);
    va_end(ap);
    ply->error_cb(ply, buffer);
}

static e_ply_storage_mode ply_arch_endian(void) {
    unsigned long i = 1;
    unsigned char *s = (unsigned char *) &i;
    if (*s == 1) return PLY_LITTLE_ENDIAN;
    else return PLY_BIG_ENDIAN;
}

static int ply_type_check(void) {
    assert(sizeof(t_ply_int8) == 1);
    assert(sizeof(t_ply_uint8) == 1);
    assert(sizeof(t_ply_int16) == 2);
    assert(sizeof(t_ply_uint16) == 2);
    assert(sizeof(t_ply_int32) == 4);
    assert(sizeof(t_ply_uint32) == 4);
    assert(sizeof(float) == 4);
    assert(sizeof(double) == 8);
    if (sizeof(t_ply_int8) != 1) return 0;
    if (sizeof(t_ply_uint8) != 1) return 0;
    if (sizeof(t_ply_int16) != 2) return 0;
    if (sizeof(t_ply_uint16) != 2) return 0;
    if (sizeof(t_ply_int32) != 4) return 0;
    if (sizeof(t_ply_uint32) != 4) return 0;
    if (sizeof(float) != 4) return 0;
    if (sizeof(double) != 8) return 0;
    return 1;
}

/* ----------------------------------------------------------------------
 * Output handlers
 * ---------------------------------------------------------------------- */
static int oascii_int8(p_ply ply, double value) {
    if (value > PLY_INT8_MAX || value < PLY_INT8_MIN) return 0;
    return fprintf(ply->fp, "%d", (t_ply_int8) value) > 0;
}

static int oascii_uint8(p_ply ply, double value) {
    if (value > PLY_UINT8_MAX || value < 0) return 0;
    return fprintf(ply->fp, "%d", (t_ply_uint8) value) > 0;
}

static int oascii_int16(p_ply ply, double value) {
    if (value > PLY_INT16_MAX || value < PLY_INT16_MIN) return 0;
    return fprintf(ply->fp, "%d", (t_ply_int16) value) > 0;
}

static int oascii_uint16(p_ply ply, double value) {
    if (value > PLY_UINT16_MAX || value < 0) return 0;
    return fprintf(ply->fp, "%d", (t_ply_uint16) value) > 0;
}

static int oascii_int32(p_ply ply, double value) {
    if (value > PLY_INT32_MAX || value < PLY_INT32_MIN) return 0;
    return fprintf(ply->fp, "%d", (t_ply_int32) value) > 0;
}

static int oascii_uint32(p_ply ply, double value) {
    if (value > PLY_UINT32_MAX || value < 0) return 0;
    return fprintf(ply->fp, "%d", (t_ply_uint32) value) > 0;
}

static int oascii_float32(p_ply ply, double value) {
    if (value < -FLT_MAX || value > FLT_MAX) return 0;
    return fprintf(ply->fp, "%g", (float) value) > 0;
}

static int oascii_float64(p_ply ply, double value) {
    if (value < -DBL_MAX || value > DBL_MAX) return 0;
    return fprintf(ply->fp, "%g", value) > 0;
}

static int obinary_int8(p_ply ply, double value) {
    t_ply_int8 int8 = (t_ply_int8) value;
    if (value > PLY_INT8_MAX || value < PLY_INT8_MIN) return 0;
    return ply->odriver->ochunk(ply, &int8, sizeof(int8));
}

static int obinary_uint8(p_ply ply, double value) {
    t_ply_uint8 uint8 = (t_ply_uint8) value;
    if (value > PLY_UINT8_MAX || value < 0) return 0;
    return ply->odriver->ochunk(ply, &uint8, sizeof(uint8));
}

static int obinary_int16(p_ply ply, double value) {
    t_ply_int16 int16 = (t_ply_int16) value;
    if (value > PLY_INT16_MAX || value < PLY_INT16_MIN) return 0;
    return ply->odriver->ochunk(ply, &int16, sizeof(int16));
}

static int obinary_uint16(p_ply ply, double value) {
    t_ply_uint16 uint16 = (t_ply_uint16) value;
    if (value > PLY_UINT16_MAX || value < 0) return 0;
    return ply->odriver->ochunk(ply, &uint16, sizeof(uint16));
}

static int obinary_int32(p_ply ply, double value) {
    t_ply_int32 int32 = (t_ply_int32) value;
    if (value > PLY_INT32_MAX || value < PLY_INT32_MIN) return 0;
    return ply->odriver->ochunk(ply, &int32, sizeof(int32));
}

static int obinary_uint32(p_ply ply, double value) {
    t_ply_uint32 uint32 = (t_ply_uint32) value;
    if (value > PLY_UINT32_MAX || value < 0) return 0;
    return ply->odriver->ochunk(ply, &uint32, sizeof(uint32));
}

static int obinary_float32(p_ply ply, double value) {
    float float32 = (float) value;
    if (value > FLT_MAX || value < -FLT_MAX) return 0;
    return ply->odriver->ochunk(ply, &float32, sizeof(float32));
}

static int obinary_float64(p_ply ply, double value) {
    return ply->odriver->ochunk(ply, &value, sizeof(value));
}

/* ----------------------------------------------------------------------
 * Input  handlers
 * ---------------------------------------------------------------------- */
static int iascii_int8(p_ply ply, double *value) {
    char *end;
    if (!ply_read_word(ply)) return 0;
    *value = strtol(BWORD(ply), &end, 10);
    if (*end || *value > PLY_INT8_MAX || *value < PLY_INT8_MIN) return 0;
    return 1;
}

static int iascii_uint8(p_ply ply, double *value) {
    char *end;
    if (!ply_read_word(ply)) return 0;
    *value = strtol(BWORD(ply), &end, 10);
    if (*end || *value > PLY_UINT8_MAX || *value < 0) return 0;
    return 1;
}

static int iascii_int16(p_ply ply, double *value) {
    char *end;
    if (!ply_read_word(ply)) return 0;
    *value = strtol(BWORD(ply), &end, 10);
    if (*end || *value > PLY_INT16_MAX || *value < PLY_INT16_MIN) return 0;
    return 1;
}

static int iascii_uint16(p_ply ply, double *value) {
    char *end;
    if (!ply_read_word(ply)) return 0;
    *value = strtol(BWORD(ply), &end, 10);
    if (*end || *value > PLY_UINT16_MAX || *value < 0) return 0;
    return 1;
}

static int iascii_int32(p_ply ply, double *value) {
    char *end;
    if (!ply_read_word(ply)) return 0;
    *value = strtol(BWORD(ply), &end, 10);
    if (*end || *value > PLY_INT32_MAX || *value < PLY_INT32_MIN) return 0;
    return 1;
}

static int iascii_uint32(p_ply ply, double *value) {
    char *end;
    if (!ply_read_word(ply)) return 0;
    *value = strtol(BWORD(ply), &end, 10);
    if (*end || *value > PLY_UINT32_MAX || *value < 0) return 0;
    return 1;
}

static int iascii_float32(p_ply ply, double *value) {
    char *end;
    if (!ply_read_word(ply)) return 0;
    *value = strtod(BWORD(ply), &end);
    if (*end || *value < -FLT_MAX || *value > FLT_MAX) return 0;
    return 1;
}

static int iascii_float64(p_ply ply, double *value) {
    char *end;
    if (!ply_read_word(ply)) return 0;
    *value = strtod(BWORD(ply), &end);
    if (*end || *value < -DBL_MAX || *value > DBL_MAX) return 0;
    return 1;
}

static int ibinary_int8(p_ply ply, double *value) {
    t_ply_int8 int8;
    if (!ply->idriver->ichunk(ply, &int8, 1)) return 0;
    *value = int8;
    return 1;
}

static int ibinary_uint8(p_ply ply, double *value) {
    t_ply_uint8 uint8;
    if (!ply->idriver->ichunk(ply, &uint8, 1)) return 0;
    *value = uint8;
    return 1;
}

static int ibinary_int16(p_ply ply, double *value) {
    t_ply_int16 int16;
    if (!ply->idriver->ichunk(ply, &int16, sizeof(int16))) return 0;
    *value = int16;
    return 1;
}

static int ibinary_uint16(p_ply ply, double *value) {
    t_ply_uint16 uint16;
    if (!ply->idriver->ichunk(ply, &uint16, sizeof(uint16))) return 0;
    *value = uint16;
    return 1;
}

static int ibinary_int32(p_ply ply, double *value) {
    t_ply_int32 int32;
    if (!ply->idriver->ichunk(ply, &int32, sizeof(int32))) return 0;
    *value = int32;
    return 1;
}

static int ibinary_uint32(p_ply ply, double *value) {
    t_ply_uint32 uint32;
    if (!ply->idriver->ichunk(ply, &uint32, sizeof(uint32))) return 0;
    *value = uint32;
    return 1;
}

static int ibinary_float32(p_ply ply, double *value) {
    float float32;
    if (!ply->idriver->ichunk(ply, &float32, sizeof(float32))) return 0;
    *value = float32;
    return 1;
}

static int ibinary_float64(p_ply ply, double *value) {
    return ply->idriver->ichunk(ply, value, sizeof(double));
}

/* ----------------------------------------------------------------------
 * Constants
 * ---------------------------------------------------------------------- */
static t_ply_idriver ply_idriver_ascii = {
    {   iascii_int8, iascii_uint8, iascii_int16, iascii_uint16,
        iascii_int32, iascii_uint32, iascii_float32, iascii_float64,
        iascii_int8, iascii_uint8, iascii_int16, iascii_uint16,
        iascii_int32, iascii_uint32, iascii_float32, iascii_float64
    }, /* order matches e_ply_type enum */
    NULL,
    "ascii input"
};

static t_ply_idriver ply_idriver_binary = {
    {   ibinary_int8, ibinary_uint8, ibinary_int16, ibinary_uint16,
        ibinary_int32, ibinary_uint32, ibinary_float32, ibinary_float64,
        ibinary_int8, ibinary_uint8, ibinary_int16, ibinary_uint16,
        ibinary_int32, ibinary_uint32, ibinary_float32, ibinary_float64
    }, /* order matches e_ply_type enum */
    ply_read_chunk,
    "binary input"
};

static t_ply_idriver ply_idriver_binary_reverse = {
    {   ibinary_int8, ibinary_uint8, ibinary_int16, ibinary_uint16,
        ibinary_int32, ibinary_uint32, ibinary_float32, ibinary_float64,
        ibinary_int8, ibinary_uint8, ibinary_int16, ibinary_uint16,
        ibinary_int32, ibinary_uint32, ibinary_float32, ibinary_float64
    }, /* order matches e_ply_type enum */
    ply_read_chunk_reverse,
    "reverse binary input"
};

static t_ply_odriver ply_odriver_ascii = {
    {   oascii_int8, oascii_uint8, oascii_int16, oascii_uint16,
        oascii_int32, oascii_uint32, oascii_float32, oascii_float64,
        oascii_int8, oascii_uint8, oascii_int16, oascii_uint16,
        oascii_int32, oascii_uint32, oascii_float32, oascii_float64
    }, /* order matches e_ply_type enum */
    NULL,
    "ascii output"
};

static t_ply_odriver ply_odriver_binary = {
    {   obinary_int8, obinary_uint8, obinary_int16, obinary_uint16,
        obinary_int32, obinary_uint32, obinary_float32, obinary_float64,
        obinary_int8, obinary_uint8, obinary_int16, obinary_uint16,
        obinary_int32, obinary_uint32, obinary_float32, obinary_float64
    }, /* order matches e_ply_type enum */
    ply_write_chunk,
    "binary output"
};

static t_ply_odriver ply_odriver_binary_reverse = {
    {   obinary_int8, obinary_uint8, obinary_int16, obinary_uint16,
        obinary_int32, obinary_uint32, obinary_float32, obinary_float64,
        obinary_int8, obinary_uint8, obinary_int16, obinary_uint16,
        obinary_int32, obinary_uint32, obinary_float32, obinary_float64
    }, /* order matches e_ply_type enum */
    ply_write_chunk_reverse,
    "reverse binary output"
};

/* ----------------------------------------------------------------------
 * Extension functions - not part of the standard rply API
 *---------------------------------------------------------------------- */

p_ply_element ply_get_element(p_ply ply, long index)
{
    if (index < ply->nelements)
    {
        return &ply->element[index];
    }
    return NULL;
}

p_ply_argument ply_get_argument(p_ply ply)
{
    return &ply->argument;
}

int ply_set_argument_element(p_ply ply, p_ply_argument argument, p_ply_element element)
{
    (void)ply;
    argument->element = element;
    return 1;
}

int ply_read_next_instance(p_ply ply, p_ply_element element, p_ply_argument argument, long *cursor) {
    long k;
    if (*cursor < element->ninstances) {
      argument->instance_index = *cursor;
      /* for each property */
      for (k = 0; k < element->nproperties; k++)
      {
        p_ply_property property = &element->property[k];
        argument->property = property;
        argument->pdata = property->pdata;
        argument->idata = property->idata;
        if (!ply_read_property(ply, element, property, argument))
          return 0;
      }
      ++(*cursor);
    }
    return 1;
}


/* ----------------------------------------------------------------------
 * Copyright (C) 2003-2015 Diego Nehab.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 * ---------------------------------------------------------------------- */


================================================
FILE: slamio/rply/rply.h
================================================
#ifndef RPLY_H
#define RPLY_H
/* ----------------------------------------------------------------------
 * RPly library, read/write PLY files
 * Diego Nehab, IMPA
 * http://www.impa.br/~diego/software/rply
 *
 * This library is distributed under the MIT License. See notice
 * at the end of this file.
 * ---------------------------------------------------------------------- */

#ifdef __cplusplus
extern "C" {
#endif

#define RPLY_VERSION   "RPly 1.1.4"
#define RPLY_COPYRIGHT "Copyright (C) 2003-2015 Diego Nehab"
#define RPLY_AUTHORS   "Diego Nehab"

/* ----------------------------------------------------------------------
 * Types
 * ---------------------------------------------------------------------- */
/* structures are opaque */
typedef struct t_ply_ *p_ply;
typedef struct t_ply_element_ *p_ply_element;
typedef struct t_ply_property_ *p_ply_property;
typedef struct t_ply_argument_ *p_ply_argument;
typedef struct t_ply_read_index_ t_ply_read_index;

/* ply format mode type */
typedef enum e_ply_storage_mode_ {
    PLY_BIG_ENDIAN,
    PLY_LITTLE_ENDIAN,
    PLY_ASCII,
    PLY_DEFAULT      /* has to be the last in enum */
} e_ply_storage_mode; /* order matches ply_storage_mode_list */

/* ply data type */
typedef enum e_ply_type {
    PLY_INT8, PLY_UINT8, PLY_INT16, PLY_UINT16,
    PLY_INT32, PLY_UIN32, PLY_FLOAT32, PLY_FLOAT64,
    PLY_CHAR, PLY_UCHAR, PLY_SHORT, PLY_USHORT,
    PLY_INT, PLY_UINT, PLY_FLOAT, PLY_DOUBLE,
    PLY_LIST    /* has to be the last in enum */
} e_ply_type;   /* order matches ply_type_list */

/* ----------------------------------------------------------------------
 * Error callback prototype
 *
 * message: error message
 * ply: handle returned by ply_open or ply_create
 * ---------------------------------------------------------------------- */
typedef void (*p_ply_error_cb)(p_ply ply, const char *message);

/* ----------------------------------------------------------------------
 * Gets user data from within an error callback
 *
 * ply: handle returned by ply_open or ply_create
 * idata,pdata: contextual information set in ply_open or ply_create
 * ---------------------------------------------------------------------- */
int ply_get_ply_user_data(p_ply ply, void **pdata, long *idata);

/* ----------------------------------------------------------------------
 * Opens a PLY file for reading (fails if file is not a PLY file)
 *
 * name: file name
 * error_cb: error callback function
 * idata,pdata: contextual information available to users
 *
 * Returns 1 if successful, 0 otherwise
 * ---------------------------------------------------------------------- */
p_ply ply_open(const char *name, p_ply_error_cb error_cb, long idata,
        void *pdata);

/* ----------------------------------------------------------------------
 * Reads and parses the header of a PLY file returned by ply_open
 *
 * ply: handle returned by ply_open
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_read_header(p_ply ply);

/* ----------------------------------------------------------------------
 * Property reading callback prototype
 *
 * argument: parameters for property being processed when callback is called
 *
 * Returns 1 if should continue processing file, 0 if should abort.
 * ---------------------------------------------------------------------- */
typedef int (*p_ply_read_cb)(p_ply_argument argument);

/* ----------------------------------------------------------------------
 * Sets up callbacks for property reading after header was parsed
 *
 * ply: handle returned by ply_open
 * element_name: element where property is
 * property_name: property to associate element with
 * read_cb: function to be called for each property value
 * pdata/idata: user data that will be passed to callback
 *
 * Returns 0 if no element or no property in element, returns the
 * number of element instances otherwise.
 * ---------------------------------------------------------------------- */
long ply_set_read_cb(p_ply ply, const char *element_name,
        const char *property_name, p_ply_read_cb read_cb,
        void *pdata, long idata);

/* ----------------------------------------------------------------------
 * Returns information about the element originating a callback
 *
 * argument: handle to argument
 * element: receives a the element handle (if non-null)
 * instance_index: receives the index of the current element instance
 *     (if non-null)
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_get_argument_element(p_ply_argument argument,
        p_ply_element *element, long *instance_index);

/* ----------------------------------------------------------------------
 * Returns information about the property originating a callback
 *
 * argument: handle to argument
 * property: receives the property handle (if non-null)
 * length: receives the number of values in this property (if non-null)
 * value_index: receives the index of current property value (if non-null)
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_get_argument_property(p_ply_argument argument,
        p_ply_property *property, long *length, long *value_index);

/* ----------------------------------------------------------------------
 * Returns user data associated with callback
 *
 * pdata: receives a copy of user custom data pointer (if non-null)
 * idata: receives a copy of user custom data integer (if non-null)
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_get_argument_user_data(p_ply_argument argument, void **pdata,
        long *idata);

/* ----------------------------------------------------------------------
 * Returns the value associated with a callback
 *
 * argument: handle to argument
 *
 * Returns the current data item
 * ---------------------------------------------------------------------- */
double ply_get_argument_value(p_ply_argument argument);

/* ----------------------------------------------------------------------
 * Reads all elements and properties calling the callbacks defined with
 * calls to ply_set_read_cb
 *
 * ply: handle returned by ply_open
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_read(p_ply ply);

/* ----------------------------------------------------------------------
 * Iterates over all elements by returning the next element.
 * Call with NULL to return handle to first element.
 *
 * ply: handle returned by ply_open
 * last: handle of last element returned (NULL for first element)
 *
 * Returns element if successfull or NULL if no more elements
 * ---------------------------------------------------------------------- */
p_ply_element ply_get_next_element(p_ply ply, p_ply_element last);

/* ----------------------------------------------------------------------
 * Iterates over all comments by returning the next comment.
 * Call with NULL to return pointer to first comment.
 *
 * ply: handle returned by ply_open
 * last: pointer to last comment returned (NULL for first comment)
 *
 * Returns comment if successfull or NULL if no more comments
 * ---------------------------------------------------------------------- */
const char *ply_get_next_comment(p_ply ply, const char *last);

/* ----------------------------------------------------------------------
 * Iterates over all obj_infos by returning the next obj_info.
 * Call with NULL to return pointer to first obj_info.
 *
 * ply: handle returned by ply_open
 * last: pointer to last obj_info returned (NULL for first obj_info)
 *
 * Returns obj_info if successfull or NULL if no more obj_infos
 * ---------------------------------------------------------------------- */
const char *ply_get_next_obj_info(p_ply ply, const char *last);

/* ----------------------------------------------------------------------
 * Returns information about an element
 *
 * element: element of interest
 * name: receives a pointer to internal copy of element name (if non-null)
 * ninstances: receives the number of instances of this element (if non-null)
 *
 * Returns 1 if successfull or 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_get_element_info(p_ply_element element, const char** name,
        long *ninstances);

/* ----------------------------------------------------------------------
 * Iterates over all properties by returning the next property.
 * Call with NULL to return handle to first property.
 *
 * element: handle of element with the properties of interest
 * last: handle of last property returned (NULL for first property)
 *
 * Returns element if successfull or NULL if no more properties
 * ---------------------------------------------------------------------- */
p_ply_property ply_get_next_property(p_ply_element element,
        p_ply_property last);

/* ----------------------------------------------------------------------
 * Returns information about a property
 *
 * property: handle to property of interest
 * name: receives a pointer to internal copy of property name (if non-null)
 * type: receives the property type (if non-null)
 * length_type: for list properties, receives the scalar type of
 *     the length field (if non-null)
 * value_type: for list properties, receives the scalar type of the value
 *     fields  (if non-null)
 *
 * Returns 1 if successfull or 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_get_property_info(p_ply_property property, const char** name,
        e_ply_type *type, e_ply_type *length_type, e_ply_type *value_type);

/* ----------------------------------------------------------------------
 * Creates new PLY file
 *
 * name: file name
 * storage_mode: file format mode
 * error_cb: error callback function
 * idata,pdata: contextual information available to users
 *
 * Returns handle to PLY file if successfull, NULL otherwise
 * ---------------------------------------------------------------------- */
p_ply ply_create(const char *name, e_ply_storage_mode storage_mode,
        p_ply_error_cb error_cb, long idata, void *pdata);

/* ----------------------------------------------------------------------
 * Adds a new element to the PLY file created by ply_create
 *
 * ply: handle returned by ply_create
 * name: name of new element
 * ninstances: number of element of this time in file
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_add_element(p_ply ply, const char *name, long ninstances);

/* ----------------------------------------------------------------------
 * Adds a new property to the last element added by ply_add_element
 *
 * ply: handle returned by ply_create
 * name: name of new property
 * type: property type
 * length_type: scalar type of length field of a list property
 * value_type: scalar type of value fields of a list property
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_add_property(p_ply ply, const char *name, e_ply_type type,
        e_ply_type length_type, e_ply_type value_type);

/* ----------------------------------------------------------------------
 * Adds a new list property to the last element added by ply_add_element
 *
 * ply: handle returned by ply_create
 * name: name of new property
 * length_type: scalar type of length field of a list property
 * value_type: scalar type of value fields of a list property
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_add_list_property(p_ply ply, const char *name,
        e_ply_type length_type, e_ply_type value_type);

/* ----------------------------------------------------------------------
 * Adds a new property to the last element added by ply_add_element
 *
 * ply: handle returned by ply_create
 * name: name of new property
 * type: property type
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_add_scalar_property(p_ply ply, const char *name, e_ply_type type);

/* ----------------------------------------------------------------------
 * Adds a new comment item
 *
 * ply: handle returned by ply_create
 * comment: pointer to string with comment text
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_add_comment(p_ply ply, const char *comment);

/* ----------------------------------------------------------------------
 * Adds a new obj_info item
 *
 * ply: handle returned by ply_create
 * comment: pointer to string with obj_info data
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_add_obj_info(p_ply ply, const char *obj_info);

/* ----------------------------------------------------------------------
 * Writes the PLY file header after all element and properties have been
 * defined by calls to ply_add_element and ply_add_property
 *
 * ply: handle returned by ply_create
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_write_header(p_ply ply);

/* ----------------------------------------------------------------------
 * Writes one property value, in the order they should be written to the
 * file. For each element type, write all elements of that type in order.
 * For each element, write all its properties in order. For scalar
 * properties, just write the value. For list properties, write the length
 * and then each of the values.
 *
 * ply: handle returned by ply_create
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_write(p_ply ply, double value);

/* ----------------------------------------------------------------------
 * Closes a PLY file handle. Releases all memory used by handle
 *
 * ply: handle to be closed.
 *
 * Returns 1 if successfull, 0 otherwise
 * ---------------------------------------------------------------------- */
int ply_close(p_ply ply);


/* ----------------------------------------------------------------------
 * Extension functions - not part of the standard rply API
 *---------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 * Read the given element data. Actually an internal API function exposed
 * for incremental point data reading.
 *---------------------------------------------------------------------- */
int ply_read_element(p_ply ply, p_ply_element element,
        p_ply_argument argument);

/* ----------------------------------------------------------------------
 * Get an element by index
 *---------------------------------------------------------------------- */
p_ply_element ply_get_element(p_ply ply, long index);

/* ----------------------------------------------------------------------
 * Get the ply argument handle.
 *---------------------------------------------------------------------- */
p_ply_argument ply_get_argument(p_ply ply);

/* ----------------------------------------------------------------------
 * Set the elment pointer for an argument.
 *---------------------------------------------------------------------- */
int ply_set_argument_element(p_ply ply, p_ply_argument argument, p_ply_element element);

/* ----------------------------------------------------------------------
 * Read the next instance from an element, incrementing the cursor value.
 *---------------------------------------------------------------------- */
int ply_read_next_instance(p_ply ply, p_ply_element element, p_ply_argument argument, long *cursor);

#ifdef __cplusplus
}
#endif

#endif /* RPLY_H */

/* ----------------------------------------------------------------------
 * Copyright (C) 2003-2015 Diego Nehab. All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 * ---------------------------------------------------------------------- */


================================================
FILE: slamio/rply/rplyfile.h
================================================
#ifndef RPLY_FILE_H
#define RPLY_FILE_H
/* ----------------------------------------------------------------------
 * RPly library, read/write PLY files
 * Diego Nehab, IMPA
 * http://www.impa.br/~diego/software/rply
 *
 * This library is distributed under the MIT License. See notice
 * at the end of this file.
 * ---------------------------------------------------------------------- */

#ifdef __cplusplus
extern "C" {
#endif

/* ----------------------------------------------------------------------
 * Opens a PLY file for reading (fails if file is not a PLY file)
 *
 * file_pointer: FILE * to file open for reading
 * error_cb: error callback function
 * idata,pdata: contextual information available to users
 *
 * Returns 1 if successful, 0 otherwise
 * ---------------------------------------------------------------------- */
p_ply ply_open_from_file(FILE *file_pointer, p_ply_error_cb error_cb,
    long idata, void *pdata);

/* ----------------------------------------------------------------------
 * Creates new PLY file
 *
 * file_pointer: FILE * to a file open for writing
 * storage_mode: file format mode
 * error_cb: error callback function
 * idata,pdata: contextual information available to users
 *
 * Returns handle to PLY file if successfull, NULL otherwise
 * ---------------------------------------------------------------------- */
p_ply ply_create_to_file(FILE *file_pointer, e_ply_storage_mode storage_mode,
        p_ply_error_cb error_cb, long idata, void *pdata);

#ifdef __cplusplus
}
#endif

#endif /* RPLY_FILE_H */

/* ----------------------------------------------------------------------
 * Copyright (C) 2003-2015 Diego Nehab. All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 * ---------------------------------------------------------------------- */


================================================
FILE: tests/CMakeLists.txt
================================================
# Build GTest
include(OhmGTest)

enable_testing()
add_subdirectory(data)
add_subdirectory(gputiltest)
add_subdirectory(ohmtestcommon)
add_subdirectory(ohmtest)
add_subdirectory(ohmtestgpu)
add_subdirectory(ohmtestheightmap)
add_subdirectory(slamiotest)


================================================
FILE: tests/data/CMakeLists.txt
================================================
# Target for marshalling test data
# NOTE: only add very small data sets (KiB, no MiB)!

set(DATA_FILES
  test-map.0.1.0.ohm
  test-map.0.2.0.ohm
  test-map.0.3.0.ohm
  test-map.0.4.0.ohm
  test-map.0.ohm
)

add_custom_target(marshal_test_data SOURCES ${DATA_FILES})

foreach(DATA_FILE ${DATA_FILES})
  add_custom_command(
    OUTPUT "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/${DATA_FILE}"
    COMMAND "${CMAKE_COMMAND}" -E copy_if_different "${CMAKE_CURRENT_LIST_DIR}/${DATA_FILE}" "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/${DATA_FILE}"
    MAIN_DEPENDENCY "${DATA_FILE}"
    COMMENT "Copying ${DATA_FILE}"
  )
endforeach(DATA_FILE ${DATA_FILES})

set_target_properties(marshal_test_data PROPERTIES FOLDER tests)


================================================
FILE: tests/data/readme.md
================================================
This directory holds test data for unit tests. These are marshalled to the binary directory on build.

*Note:* Only to be used for very small data sets. Larger data sets should not be in Git this way.


================================================
FILE: tests/gputiltest/CMakeLists.txt
================================================
include(TextFileResource)

set(SOURCES
  GpuBufferTest.cpp
  GpuDeviceTest.cpp
  KernelTest.cpp
  TestMain.cpp
)

set(GPU_SOURCES
  matrix.cl
)

function(_gputiltest_setup_target TARGET_NAME GPUTIL_LIBRARY)
  leak_track_target_enable(${TARGET_NAME} CONDITION OHM_LEAK_TRACK)
  set_target_properties(${TARGET_NAME} PROPERTIES FOLDER tests)
  if(MSVC)
    set_target_properties(${TARGET_NAME} PROPERTIES DEBUG_POSTFIX "d")
  endif(MSVC)

  target_include_directories(${TARGET_NAME}
    PUBLIC
      $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
      $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
  )

  target_include_directories(${TARGET_NAME}
    SYSTEM PRIVATE
      "${GTEST_INCLUDE_DIRS}"
      $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}>
  )

  target_link_libraries(${TARGET_NAME} PUBLIC ${GPUTIL_LIBRARY} ${GTEST_LIBRARIES} ${GTEST_MAIN_LIBRARIES} logutil)

  add_test(NAME ${TARGET_NAME} COMMAND ${TARGET_NAME} --gtest_output=xml:test-reports/)

  install(TARGETS ${TARGET_NAME} DESTINATION bin)
endfunction(_gputiltest_setup_target)

if(OHM_FEATURE_OPENCL)
  get_target_property(GPUTIL_DEVICE_INCLUDE_DIRS gputilocl SOURCE_DIR)
  # Embedding GPU code. Generate source file resources.
  # Only need to embed for OpenCL.
  foreach(CLSOURCE ${GPU_SOURCES})
    get_filename_component(CLSOURCENAME "${CLSOURCE}" NAME)
    get_filename_component(CLSOURCENAME_WE "${CLSOURCE}" NAME_WE)
    text_file_resource("${CLSOURCE}" "${CLSOURCENAME_WE}Code"
      TYPE opencl
      # ECHO
      PATHS "${CMAKE_CURRENT_LIST_DIR}" "${GPUTIL_DEVICE_INCLUDE_DIRS}"
      # Append to the SOURCES list.
      FILELIST SOURCES
    )
  endforeach(CLSOURCE)

  add_executable(gputiltestocl ${SOURCES} ${GPU_SOURCES})
  _gputiltest_setup_target(gputiltestocl gputilocl)
  # Required to run NVIDIA OpenCL
  leak_track_default_options(gputiltestocl CONDITION OHM_LEAK_TRACK ${OHM_ASAN_OPTIONS_CUDA})
  leak_track_suppress(gputiltestocl CONDITION OHM_LEAK_TRACK
    ${OHM_LEAK_SUPPRESS_OCL}
  )
endif(OHM_FEATURE_OPENCL)

if(OHM_FEATURE_CUDA)
  list(APPEND GPU_SOURCES
    cuda/matrix_kernel.cu
  )
  if(OHM_USE_DEPRECATED_CMAKE_CUDA)
    cuda_add_executable(gputiltestcuda ${SOURCES} ${GPU_SOURCES})
  else(OHM_USE_DEPRECATED_CMAKE_CUDA)
    add_executable(gputiltestcuda ${SOURCES} ${GPU_SOURCES})
  endif(OHM_USE_DEPRECATED_CMAKE_CUDA)
  _gputiltest_setup_target(gputiltestcuda gputilcuda)
  leak_track_default_options(gputiltestcuda CONDITION OHM_LEAK_TRACK ${OHM_ASAN_OPTIONS_CUDA})
  leak_track_suppress(gputiltestcuda CONDITION OHM_LEAK_TRACK
    ${OHM_LEAK_SUPPRESS_CUDA}
  )
endif(OHM_FEATURE_CUDA)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})


================================================
FILE: tests/gputiltest/GpuBufferTest.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

// #include <gputil/cl/gpuEventDetail.h>
#include <gputil/gpuBuffer.h>
#include <gputil/gpuDevice.h>
#include <gputil/gpuEvent.h>
#include <gputil/gpuPinnedBuffer.h>
#include <gputil/gpuQueue.h>

#include <logutil/LogUtil.h>

extern gputil::Device g_gpu;
namespace gpubuffertest
{
typedef std::chrono::high_resolution_clock TimingClock;

TEST(GpuBuffer, MemRate)
{
  // We are going to test the memory transfer rates between host and GPU memory comparing:
  // - Unmapped vs. unmapped transfer.
  // - Synchronous vs. asynchronous transfer.
  // - Many small buffers vs. one large buffer with offsets.
  const size_t small_buffer_size = 256 * logutil::kKibiSize;
  size_t small_buffer_count = 5000;
  size_t large_buffer_size = 0;

  gputil::Device &gpu = g_gpu;
  size_t predicted_mem = small_buffer_size * small_buffer_count * 2;
  // Allow some additional overhead, dividing the device memory by 2.
  while (predicted_mem >= g_gpu.deviceMemory() / 2 ||
         small_buffer_count * small_buffer_size > g_gpu.maxAllocationSize())
  {
    small_buffer_count /= 2;
    predicted_mem = small_buffer_size * small_buffer_count * 2;
  }
  large_buffer_size = small_buffer_size * small_buffer_count;

  ASSERT_LE(large_buffer_size, g_gpu.maxAllocationSize());

  std::vector<uint8_t> host_buffer(small_buffer_size);
  std ::vector<uint8_t> test_buffer(small_buffer_size);

  //--------------------------------------------------------------------------
  std::cout << "Running timing test on GPU buffer usage." << std::endl;
  std::cout << small_buffer_count << " buffers of size " << logutil::Bytes(small_buffer_size) << ", total "
            << logutil::Bytes(small_buffer_count * small_buffer_size) << std::endl;
  std::cout << "1 buffer of size " << logutil::Bytes(large_buffer_size) << std::endl;

  for (size_t i = 0; i < host_buffer.size(); ++i)
  {
    host_buffer[i] = uint8_t(i % 256);
  }
  std::cout << '\n';
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Allocating host accessible buffers." << std::endl;
  gputil::Queue queue = gpu.createQueue();
  gputil::Event large_buffer_event;
  std::vector<gputil::Event> large_buffer_events(small_buffer_count);
  std::vector<gputil::Event> small_buffer_events(small_buffer_count);

  auto alloc_start = TimingClock::now();
  gputil::Buffer large_buffer(gpu, large_buffer_size, gputil::kBfReadWriteHost);
  std::vector<gputil::Buffer> small_buffers(small_buffer_count);

  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    small_buffers[i] = gputil::Buffer(gpu, small_buffer_size, gputil::kBfReadWriteHost);
  }
  auto alloc_end = TimingClock::now();
  std::cout << (alloc_end - alloc_start) << std::endl;
  std::cout << '\n';
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Mapped copy to large buffer in chunks." << std::endl;
  const auto mapped_up1_start = TimingClock::now();
  gputil::PinnedBuffer buffer(large_buffer, gputil::kPinWrite);
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    buffer.write(host_buffer.data(), small_buffer_size, small_buffer_size * i);
  }
  buffer.unpin(&queue, nullptr, &large_buffer_event);
  queue.flush();
  const auto mapped_up1_queued = TimingClock::now();
  large_buffer_event.wait();
  const auto mapped_up1_end = TimingClock::now();
  std::cout << mapped_up1_end - mapped_up1_start << " total => " << mapped_up1_queued - mapped_up1_start << " queue + "
            << mapped_up1_end - mapped_up1_queued << " wait." << std::endl;
  std::cout << '\n';
  queue.finish();
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Mapped copy to small buffers." << std::endl;
  const auto mapped_up2_start = TimingClock::now();
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    gputil::PinnedBuffer buffer(small_buffers[i], gputil::kPinWrite);
    buffer.write(host_buffer.data(), small_buffer_size);
    buffer.unpin(&queue, nullptr, &small_buffer_events[i]);
  }
  queue.flush();
  const auto mapped_up2_queued = TimingClock::now();
  gputil::Event::wait(small_buffer_events.data(), small_buffer_count);
  const auto mapped_up2_end = TimingClock::now();
  std::cout << mapped_up2_end - mapped_up2_start << " total => " << mapped_up2_queued - mapped_up2_start << " queue + "
            << mapped_up2_end - mapped_up2_queued << " wait." << std::endl;
  std::cout << '\n';
  queue.finish();
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Mapped download large buffer." << std::endl;
  const auto mapped_down1_start = TimingClock::now();
  buffer = gputil::PinnedBuffer(large_buffer, gputil::kPinRead);
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    buffer.read(test_buffer.data(), small_buffer_size, small_buffer_size * i);
  }
  buffer.unpin(&queue, nullptr, &large_buffer_event);
  queue.flush();
  const auto mapped_down1_queued = TimingClock::now();
  large_buffer_event.wait();
  const auto mapped_down1_end = TimingClock::now();

  const size_t failure_limit = 32;
  size_t failure_count = 0;
  for (size_t i = 0; i < test_buffer.size(); ++i)
  {
    EXPECT_EQ(test_buffer[i], host_buffer[i]);
    if (test_buffer[i] != host_buffer[i])
    {
      ++failure_count;
      if (failure_count >= failure_limit)
      {
        std::cerr << "Aborting memory validation after " << failure_limit << " failures" << std::endl;
        break;
      }
    }
  }

  std::cout << mapped_down1_end - mapped_down1_start << " total => " << mapped_down1_queued - mapped_down1_start
            << " queue + " << mapped_down1_end - mapped_down1_queued << " wait." << std::endl;
  std::cout << '\n';
  queue.finish();
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Releasing events." << std::endl;
  auto release_start = TimingClock::now();
  large_buffer_event.release();
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    large_buffer_events[i].release();
    small_buffer_events[i].release();
  }
  auto release_end = TimingClock::now();
  std::cout << release_end - release_start << std::endl;
  std::cout << '\n';
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Mapped download small buffers." << std::endl;
  const auto mapped_down2_start = TimingClock::now();
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    gputil::PinnedBuffer buffer(small_buffers[i], gputil::kPinRead);
    buffer.read(test_buffer.data(), small_buffer_size);
    buffer.unpin(&queue, nullptr, &small_buffer_events[i]);
  }
  queue.flush();
  const auto mapped_down2_queued = TimingClock::now();
  gputil::Event::wait(small_buffer_events.data(), small_buffer_count);
  const auto mapped_down2_end = TimingClock::now();

  failure_count = 0;
  for (size_t i = 0; i < test_buffer.size(); ++i)
  {
    EXPECT_EQ(test_buffer[i], host_buffer[i]);
    if (test_buffer[i] != host_buffer[i])
    {
      ++failure_count;
      if (failure_count >= failure_limit)
      {
        std::cerr << "Aborting memory validation after " << failure_limit << " failures" << std::endl;
        break;
      }
    }
  }

  std::cout << mapped_down2_end - mapped_down2_start << " total => " << mapped_down2_queued - mapped_down2_start
            << " queue + " << mapped_down2_end - mapped_down2_queued << " wait." << std::endl;
  std::cout << '\n';
  queue.finish();
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Releasing buffers and events." << std::endl;
  release_start = TimingClock::now();
  large_buffer.release();
  large_buffer_event.release();
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    small_buffers[i].release();
    large_buffer_events[i].release();
    small_buffer_events[i].release();
    std::cout << "\r" << i + 1 << " / " << small_buffer_count << std::flush;
  }
  std::cout << '\n';
  release_end = TimingClock::now();
  std::cout << release_end - release_start << std::endl;
  std::cout << '\n';

  //--------------------------------------------------------------------------
  std::cout << "Allocating device only buffers." << std::endl;
  alloc_start = TimingClock::now();
  large_buffer = gputil::Buffer(gpu, large_buffer_size, gputil::kBfReadWrite);

  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    small_buffers[i] = gputil::Buffer(gpu, small_buffer_size, gputil::kBfReadWrite);
  }
  alloc_end = TimingClock::now();
  std::cout << (alloc_end - alloc_start) << std::endl;
  std::cout << '\n';
  queue.finish();
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Queued copy to large buffer in chunks." << std::endl;
  const auto queued_up1_start = TimingClock::now();
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    large_buffer.write(host_buffer.data(), small_buffer_size, small_buffer_size * i, &queue, nullptr,
                       &large_buffer_events[i]);
  }
  queue.flush();
  const auto queued_up1_queued = TimingClock::now();
  gputil::Event::wait(large_buffer_events.data(), small_buffer_count);
  queue.finish();
  const auto queued_up1_end = TimingClock::now();
  std::cout << queued_up1_end - queued_up1_start << " total => " << queued_up1_queued - queued_up1_start << " queue + "
            << queued_up1_end - queued_up1_queued << " wait." << std::endl;
  std::cout << '\n';
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Queued copy to small buffers." << std::endl;
  const auto queued_up2_start = TimingClock::now();
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    small_buffers[i].write(host_buffer.data(), small_buffer_size, 0, &queue, nullptr, &small_buffer_events[i]);
  }
  queue.flush();
  const auto queued_up2_queued = TimingClock::now();
  gputil::Event::wait(small_buffer_events.data(), small_buffer_count);
  const auto queued_up2_end = TimingClock::now();
  std::cout << queued_up2_end - queued_up2_start << " total => " << queued_up2_queued - queued_up2_start << " queue + "
            << queued_up2_end - queued_up2_queued << " wait." << std::endl;
  std::cout << '\n';
  queue.finish();
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Releasing events." << std::endl;
  release_start = TimingClock::now();
  large_buffer_event.release();
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    large_buffer_events[i].release();
    small_buffer_events[i].release();
  }
  release_end = TimingClock::now();
  std::cout << release_end - release_start << std::endl;
  std::cout << '\n';
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Queued download large buffer." << std::endl;
  const auto queued_down1_start = TimingClock::now();
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    large_buffer.read(test_buffer.data(), small_buffer_size, small_buffer_size * i, &queue, nullptr,
                      &large_buffer_events[i]);
  }
  queue.flush();
  const auto queued_down1_queued = TimingClock::now();
  gputil::Event::wait(large_buffer_events.data(), small_buffer_count);
  const auto queued_down1_end = TimingClock::now();

  failure_count = 0;
  for (size_t i = 0; i < test_buffer.size(); ++i)
  {
    EXPECT_EQ(test_buffer[i], host_buffer[i]);
    if (test_buffer[i] != host_buffer[i])
    {
      ++failure_count;
      if (failure_count >= failure_limit)
      {
        std::cerr << "Aborting memory validation after " << failure_limit << " failures" << std::endl;
        break;
      }
    }
  }

  std::cout << queued_down1_end - queued_down1_start << " total => " << queued_down1_queued - queued_down1_start
            << " queue + " << queued_down1_end - queued_down1_queued << " wait." << std::endl;
  std::cout << '\n';
  queue.finish();
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Queued download small buffers." << std::endl;
  const auto queued_down2_start = TimingClock::now();
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    small_buffers[i].read(test_buffer.data(), small_buffer_size, 0, &queue, nullptr, &small_buffer_events[i]);
  }
  queue.flush();
  const auto queued_down2_queued = TimingClock::now();
  gputil::Event::wait(small_buffer_events.data(), small_buffer_count);
  const auto queued_down2_end = TimingClock::now();

  failure_count = 0u;
  for (size_t i = 0; i < test_buffer.size(); ++i)
  {
    EXPECT_EQ(test_buffer[i], host_buffer[i]);
    if (test_buffer[i] != host_buffer[i])
    {
      ++failure_count;
      if (failure_count >= failure_limit)
      {
        std::cerr << "Aborting memory validation after " << failure_limit << " failures" << std::endl;
        break;
      }
    }
  }

  std::cout << queued_down2_end - queued_down2_start << " total => " << queued_down2_queued - queued_down2_start
            << " queue + " << queued_down2_end - queued_down2_queued << " wait." << std::endl;
  std::cout << '\n';
  queue.finish();
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  std::cout << "Releasing buffers and events." << std::endl;
  release_start = TimingClock::now();
  large_buffer.release();
  large_buffer_event.release();
  for (size_t i = 0; i < small_buffer_count; ++i)
  {
    small_buffers[i].release();
    large_buffer_events[i].release();
    small_buffer_events[i].release();
    std::cout << "\r" << i + 1 << " / " << small_buffer_count << std::flush;
  }
  std::cout << '\n';
  release_end = TimingClock::now();
  std::cout << release_end - release_start << std::endl;
  std::cout << '\n';
  queue.finish();
  //--------------------------------------------------------------------------

  //--------------------------------------------------------------------------
  //--------------------------------------------------------------------------
  const auto log_timing = [large_buffer_size](const char *name, const TimingClock::duration &up,
                                              const TimingClock::duration &down, int width) {
    std::ostringstream sstr;
    size_t bytes_per_second;
    int w;
    auto whitespace = [](int count) {
      while (count-- > 0)
      {
        std::cout << ' ';
      }
    };

    sstr << name << std::flush;
    w = int(sstr.str().size());
    std::cout << sstr.str();
    whitespace(width - w);
    sstr.str(std::string());

    sstr << up << std::flush;
    w = int(sstr.str().size());
    std::cout << sstr.str();
    whitespace(width - w);
    sstr.str(std::string());

    bytes_per_second =
      size_t(large_buffer_size / std::chrono::duration_cast<std::chrono::duration<double>>(up).count() + 0.5);
    sstr << logutil::Bytes(bytes_per_second) << "/s" << std::flush;
    w = int(sstr.str().size());
    std::cout << sstr.str();
    whitespace(width - w);
    sstr.str(std::string());

    sstr << down << std::flush;
    w = int(sstr.str().size());
    std::cout << sstr.str();
    whitespace(width - w);
    sstr.str(std::string());

    bytes_per_second =
      size_t(large_buffer_size / std::chrono::duration_cast<std::chrono::duration<double>>(down).count() + 0.5);
    sstr << logutil::Bytes(bytes_per_second) << "/s" << std::flush;
    // w = (int)sstr.str().size();
    std::cout << sstr.str();
    // whitespace(width - w);
    // sstr.str(std::string());

    std::cout << std::endl;
  };

  std::cout << "Summary\n"
            << "Type          Upload        Up Rate       Download      Down Rate" << std::endl;
  log_timing("Map large", (mapped_up1_end - mapped_up1_start), (mapped_down1_end - mapped_down1_start), 14);
  log_timing("Map small", (mapped_up2_end - mapped_up2_start), (mapped_down2_end - mapped_down2_start), 14);
  log_timing("Queue large", (queued_up1_end - queued_up1_start), (queued_down1_end - queued_down1_start), 14);
  log_timing("Queue small", (queued_up2_end - queued_up2_start), (queued_down2_end - queued_down2_start), 14);
}


TEST(GpuBuffer, Ref)
{
  // #if GPUTIL_TYPE == GPUTIL_OPENCL
  //     gputil::Event event;
  //     gputil::Device &gpu = g_gpu;
  //     gputil::Buffer buffer(gpu, 64 * 1024u, gputil::kBfReadWriteHost);
  //     gputil::Queue queue = gpu.createQueue();
  //     std::vector<uint8_t> host_buffer(buffer.size());

  //     for (size_t i = 0; i < host_buffer.size(); ++i)
  //     {
  //       host_buffer[i] = uint8_t(i % 256);
  //     }

  //     buffer.write(host_buffer.data(), host_buffer.size(), 0, &queue, nullptr, &event);

  //     cl_uint ref_count = 0;
  //     const cl_event event_ocl = event.detail()->event;

  //     clGetEventInfo(event_ocl, CL_EVENT_REFERENCE_COUNT, sizeof(ref_count), &ref_count, nullptr);
  //     std::cout << ref_count << " : queued" << std::endl;
  //     // Expected references : event, OpenCL(?)
  //     EXPECT_GE(ref_count, 1u);
  //     clRetainEvent(event_ocl);
  //     clGetEventInfo(event_ocl, CL_EVENT_REFERENCE_COUNT, sizeof(ref_count), &ref_count, nullptr);
  //     std::cout << ref_count << " : +ref" << std::endl;
  //     // Expected references : event, eventOcl + OpenCL(?)
  //     EXPECT_GE(ref_count, 2u);
  //     queue.finish();
  //     clGetEventInfo(event_ocl, CL_EVENT_REFERENCE_COUNT, sizeof(ref_count), &ref_count, nullptr);
  //     std::cout << ref_count << " : finish" << std::endl;
  //     // Expected references : event, eventOcl
  //     EXPECT_EQ(ref_count, 2u);
  //     event.release();
  //     clGetEventInfo(event_ocl, CL_EVENT_REFERENCE_COUNT, sizeof(ref_count), &ref_count, nullptr);
  //     std::cout << ref_count << " : release" << std::endl;
  //     // Expected references : eventOcl
  //     EXPECT_EQ(ref_count, 1u);

  //     clReleaseEvent(event_ocl);
  // #endif  // GPUTIL_TYPE == GPUTIL_OPENCL
}

TEST(GpuBuffer, ReadWriteCopy)
{
  // Basic buffer read/write with offsets.
  gputil::Device &gpu = g_gpu;
  const std::string buffer_ref = "The quick brown fox jumps over the lazy dog.";
  std::string buffer_read;

  buffer_read.resize(buffer_ref.size());

  // Allocate a larger buffer to allow writing with offset.
  gputil::Buffer buffer(gpu, buffer_ref.size() * 2);
  buffer.write(buffer_ref.data(), buffer_ref.size());
  buffer.read(&buffer_read.front(), buffer_read.size());

  std::cout << "Write/read: " << buffer_read << std::endl;
  EXPECT_EQ(buffer_read, buffer_ref);

  // Write with offset. Target result is: "The The quick..."
  const unsigned offset = 4;
  buffer.write(buffer_ref.data(), buffer_ref.size(), offset);
  buffer.read(&buffer_read.front(), buffer_read.size(), offset);

  std::cout << "Write + offset/read + offset: " << buffer_read << std::endl;
  EXPECT_EQ(buffer_read, buffer_ref);

  // Read at byte zero.
  const std::string offset_ref = "The The quick brown fox jumps over the lazy dog.";
  buffer_read.resize(buffer_ref.size() + offset);
  buffer.read(&buffer_read.front(), buffer_read.size());

  std::cout << "Write + offset/read: " << buffer_read << std::endl;
  EXPECT_EQ(buffer_read, offset_ref);

  // Reset to the original string.
  buffer.write(buffer_ref.data(), buffer_ref.size());

  // Now copy into another buffer.
  gputil::Buffer buffer2(gpu, buffer_ref.size() * 2);
  gputil::copyBuffer(buffer2, buffer);

  buffer_read.resize(buffer_ref.size());
  buffer2.read(&buffer_read.front(), buffer_read.size());

  // Validate the result.
  std::cout << "Copy: " << buffer_read << std::endl;
  EXPECT_EQ(buffer_read, buffer_ref);
}

TEST(GpuBuffer, Pinned)
{
  // Pinned buffer usage.
  gputil::Device &gpu = g_gpu;
  const std::string buffer_ref = "The quick brown fox jumps over the lazy dog.";
  std::string buffer_read;

  buffer_read.resize(buffer_ref.size());

  // Allocate a larger buffer to allow writing with offset.
  gputil::Buffer buffer(gpu, buffer_ref.size() * 2, gputil::kBfReadWriteHost);

  {
    // Use one buffer for read and write to validate RValue operators.
    gputil::PinnedBuffer pin;

    // Pin and write.
    pin = gputil::PinnedBuffer(buffer, gputil::kPinWrite);
    ASSERT_TRUE(pin.isPinned());
    pin.write(buffer_ref.data(), buffer_ref.size());
    pin.unpin();
    ASSERT_FALSE(pin.isPinned());

    // Pin and read.
    pin = gputil::PinnedBuffer(buffer, gputil::kPinRead);
    ASSERT_TRUE(pin.isPinned());
    pin.read(&buffer_read.front(), buffer_read.size());
    pin.unpin();
    ASSERT_FALSE(pin.isPinned());

    std::cout << "Write/read: " << buffer_read << std::endl;
    EXPECT_EQ(buffer_read, buffer_ref);
  }

  // Now use separate read/write pins.

  // Write with offset. Target result is: "The The quick..."
  const unsigned offset = 4;
  gputil::PinnedBuffer writePin(buffer, gputil::kPinWrite);
  ASSERT_TRUE(writePin.isPinned());
  writePin.write(buffer_ref.data(), buffer_ref.size(), offset);
  writePin.unpin();
  ASSERT_FALSE(writePin.isPinned());

  gputil::PinnedBuffer readPin(buffer, gputil::kPinRead);
  ASSERT_TRUE(readPin.isPinned());
  readPin.read(&buffer_read.front(), buffer_read.size(), offset);
  readPin.unpin();
  ASSERT_FALSE(readPin.isPinned());

  std::cout << "Write + offset/read + offset: " << buffer_read << std::endl;
  EXPECT_EQ(buffer_read, buffer_ref);

  // Read at byte zero.
  const std::string offset_ref = "The The quick brown fox jumps over the lazy dog.";

  readPin.pin();
  ASSERT_TRUE(readPin.isPinned());
  buffer_read.resize(buffer_ref.size() + offset);
  readPin.read(&buffer_read.front(), buffer_read.size());
  readPin.unpin();
  ASSERT_FALSE(readPin.isPinned());

  std::cout << "Write + offset/read: " << buffer_read << std::endl;
  EXPECT_EQ(buffer_read, offset_ref);
}

// Ensure we are not retaining buffer memory.
TEST(GpuBuffer, Allocation)
{
  gputil::Device &gpu = g_gpu;
  uint64_t alloc_size = gpu.maxAllocationSize();
  // Limit to 2 GiB.
  alloc_size = std::min<size_t>(alloc_size, 2u * 1024u * 1024u * 1024u);

  std::cout << "Max allocation size: ";
  std::cout << logutil::Bytes(alloc_size) << std::endl;
  alloc_size /= 4;
  std::cout << "Using allocation size: ";
  std::cout << logutil::Bytes(alloc_size) << std::endl;

  // Allocate and release large amounts of memory. Ensure we are releasing it correctly.
  // This should raise an exception if we are not correctly releasing memory.
  const int iter_count = 20;
  const int fill_value = 42;
  for (int i = 0; i < 20; ++i)
  {
    gputil::Buffer mem_buffer(gpu, alloc_size);
    mem_buffer.fill(&fill_value, sizeof(fill_value));
    std::cout << i + 1 << " / " << iter_count << std::endl;
  }
}
}  // namespace gpubuffertest


================================================
FILE: tests/gputiltest/GpuDeviceTest.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include <gputil/gpuDevice.h>
#include <gputil/gpuQueue.h>

#include <chrono>

extern gputil::Device g_gpu;

namespace gpudevicetest
{
TEST(GpuDevice, Enumerate)
{
  std::vector<gputil::DeviceInfo> device_infos;
  gputil::Device::enumerateDevices(device_infos);
  ASSERT_GT(device_infos.size(), 0u);

  // Select and create the first device.
  const gputil::DeviceInfo &device_info = device_infos[0];
  gputil::Device device(device_infos[0]);
  ASSERT_TRUE(device.isValid());

  EXPECT_EQ(device.info(), device_infos[0]);

  // Select the first device by simulated command line arguments.
  std::vector<std::string> args;
  std::string arg;
  switch (device_info.type)
  {
  case gputil::kDeviceCpu:
    arg = "cpu";
    break;
  case gputil::kDeviceGpu:
    arg = "gpu";
    break;
  default:
    arg = "any";
    break;
  }
  args.push_back("--accel=" + arg);

  std::ostringstream arg_stream;
  arg_stream << "--clver=" << device_info.version.major << '.' << device_info.version.minor;
  args.push_back(arg_stream.str());
  args.push_back("--device=\"" + device_info.name + "\"");
  args.push_back("--platform=\"" + device_info.platform + "\"");

  int argc = int(args.size());
  std::vector<const char *> argv;
  for (std::string &a : args)
  {
    argv.push_back(a.c_str());
  }
  device = gputil::Device();
  EXPECT_FALSE(device.isValid());
  device.select(argc, argv.data());
  ASSERT_TRUE(device.isValid());
  const gputil::DeviceInfo &info = device.info();
  EXPECT_EQ(info, device_info);
}
}  // namespace gpudevicetest


================================================
FILE: tests/gputiltest/KernelTest.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include <gputil/gpuApiException.h>
#include <gputil/gpuBuffer.h>
#include <gputil/gpuDevice.h>
#include <gputil/gpuKernel.h>
#include <gputil/gpuProgram.h>
#include <gputil/gpuQueue.h>

#include <chrono>

#if GPUTIL_TYPE == GPUTIL_OPENCL
#include "matrixResource.h"
#endif  // GPUTIL_TYPE == GPUTIL_OPENCL

extern gputil::Device g_gpu;

#if GPUTIL_TYPE == GPUTIL_CUDA
GPUTIL_CUDA_DECLARE_KERNEL(matrixMultiply);
#endif  // GPUTIL_TYPE == GPUTIL_CUDA

namespace gpukerneltest
{
const unsigned kN = 32;
extern const float kMatrixA[kN * kN];
extern const float kMatrixB[kN * kN];

/// Log a @c std::chrono::clock::duration to an output stream.
///
/// The resulting string displays in the smallest possible unit to show three three
/// decimal places with display units ranging from seconds to nanoseconds. The table below
/// shows some example times.
///
/// Time(s)     | Display
/// ----------- | --------
/// 0.000000018 | 18ns
/// 0.000029318 | 29.318us
/// 0.0295939   | 29.593ms
/// 0.93        | 930ms
/// 15.023      | 15.023s
/// 15.000025   | 15.000s
///
/// Note that times are truncated, not rounded.
///
/// @tparam D The duration type of the form @c std::chrono::clock::duration.
/// @param out The output stream to log to.
/// @param duration The duration to convert to string.
template <typename D>
inline void logDuration(std::ostream &out, const D &duration)
{
  const bool negative = std::chrono::duration_cast<std::chrono::nanoseconds>(duration).count() < 0;
  const char *sign = (!negative) ? "" : "-";
  D abs_duration = (!negative) ? duration : duration * -1;
  auto s = std::chrono::duration_cast<std::chrono::seconds>(abs_duration).count();
  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(abs_duration).count();
  ms = ms % 1000;

  if (s)
  {
    out << sign << s << "." << std::setw(3) << std::setfill('0') << ms << "s";
  }
  else
  {
    auto us = std::chrono::duration_cast<std::chrono::microseconds>(abs_duration).count();
    us = us % 1000;

    if (ms)
    {
      out << sign << ms << "." << std::setw(3) << std::setfill('0') << us << "ms";
    }
    else
    {
      auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(abs_duration).count();
      ns = ns % 1000;

      if (us)
      {
        out << sign << us << "." << std::setw(3) << std::setfill('0') << ns << "us";
      }
      else
      {
        out << sign << ns << "ns";
      }
    }
  }
}

template <typename T, typename R>
inline std::ostream &operator<<(std::ostream &out, const std::chrono::duration<T, R> &d)
{
  logDuration(out, d);
  return out;
}
using TimingClock = std::chrono::high_resolution_clock;

TEST(GpuKernel, Simple)
{
  int err = 0;
  gputil::Program program(g_gpu, "test-program");

  err = GPUTIL_BUILD_FROM_SOURCE(program, matrixCode, matrixCode_length, gputil::BuildArgs{});

  ASSERT_EQ(err, 0) << gputil::ApiException::errorCodeString(err);

  gputil::Kernel kernel = GPUTIL_MAKE_KERNEL(program, matrixMultiply);
  ASSERT_TRUE(kernel.isValid());

  kernel.addLocal([](size_t group_size) { return group_size * sizeof(float); });

  kernel.calculateOptimalWorkGroupSize();

  gputil::Queue queue = g_gpu.defaultQueue();

  gputil::Buffer matrix_a(g_gpu, sizeof(kMatrixA), gputil::kBfRead);
  gputil::Buffer matrix_b(g_gpu, sizeof(kMatrixB), gputil::kBfRead);
  gputil::Buffer matrix_out(g_gpu, sizeof(float) * kN, gputil::kBfWrite);

  const auto gpu_start = TimingClock::now();
  size_t copy_bytes = 0;
  copy_bytes = matrix_a.write(kMatrixA, sizeof(kMatrixA));
  ASSERT_EQ(copy_bytes, sizeof(kMatrixA));
  copy_bytes = matrix_b.write(kMatrixB, sizeof(kMatrixB));
  ASSERT_EQ(copy_bytes, sizeof(kMatrixB));
  err = kernel(gputil::Dim3(kN), gputil::Dim3(kN), &queue, gputil::BufferArg<float>(matrix_out),
               gputil::BufferArg<float>(matrix_a), gputil::BufferArg<float>(matrix_b), kN);
  ASSERT_EQ(err, 0) << gputil::ApiException::errorCodeString(err);
  queue.finish();

  // Copy results from GPU.
  std::vector<float> gpu_results(kN);
  copy_bytes = matrix_out.read(gpu_results.data(), sizeof(*gpu_results.data()) * gpu_results.size());
  ASSERT_EQ(copy_bytes, sizeof(*gpu_results.data()) * gpu_results.size());
  const auto gpu_end = TimingClock::now();
  std::cout << "GPU time: " << (gpu_end - gpu_start) << std::endl;

  // Calculate in CPU.
  std::vector<float> cpu_results(kN);

  const auto cpu_start = TimingClock::now();
  for (unsigned i = 0; i < kN; ++i)
  {
    float &out = cpu_results[i];
    out = 0;
    for (unsigned j = 0; j < kN; ++j)
    {
      out += kMatrixA[i * kN + j] * kMatrixB[j * kN + i];
    }
  }
  const auto cpu_end = TimingClock::now();
  std::cout << "CPU time: " << (cpu_end - cpu_start) << std::endl;

  // Validate results.
  for (size_t i = 0; i < gpu_results.size(); ++i)
  {
    EXPECT_NEAR(gpu_results[i], cpu_results[i], 1e-3f);
  }
}

TEST(GpuKernel, NullArg)
{
  int err = 0;
  gputil::Program program(g_gpu, "test-program");

  err = GPUTIL_BUILD_FROM_SOURCE(program, matrixCode, matrixCode_length, gputil::BuildArgs{});

  ASSERT_EQ(err, 0) << gputil::ApiException::errorCodeString(err);

  gputil::Kernel kernel = GPUTIL_MAKE_KERNEL(program, matrixMultiply);
  ASSERT_TRUE(kernel.isValid());

  kernel.addLocal([](size_t group_size) { return group_size * sizeof(float); });

  kernel.calculateOptimalWorkGroupSize();

  gputil::Queue queue = g_gpu.defaultQueue();

  err = kernel(gputil::Dim3(kN), gputil::Dim3(kN), &queue, gputil::BufferArg<float>(nullptr),
               gputil::BufferArg<float>(nullptr), gputil::BufferArg<float>(nullptr), kN);
  ASSERT_EQ(err, 0) << gputil::ApiException::errorCodeString(err);
}

const float kMatrixA[kN * kN] =  //
  { -5.36024f, 3.03301f,  7.89674f,  -6.46361f, 2.37263f,  6.38046f,  -4.85497f, 4.01736f,  -5.83961f, -0.30897f,
    2.34736f,  9.19266f,  -4.81157f, 5.12527f,  -6.22901f, 4.46971f,  3.42575f,  8.55879f,  5.77094f,  4.38269f,
    -1.98192f, -3.92955f, -7.52568f, 5.00008f,  -2.03231f, -0.67967f, -5.99790f, -9.10126f, 6.17714f,  -1.57793f,
    -9.70259f, -1.88551f, -4.55362f, 6.07408f,  -1.24905f, -2.56581f, -2.32887f, 0.18148f,  2.41612f,  -7.29838f,
    -1.10756f, 9.76593f,  -3.10208f, 5.38991f,  -3.62616f, -3.80617f, 8.14205f,  5.79305f,  1.71217f,  4.25061f,
    -6.16396f, -0.55964f, 7.84027f,  6.69293f,  -1.74443f, 0.32048f,  2.04413f,  2.42060f,  -2.69034f, -1.81077f,
    -7.80228f, -3.67605f, -7.90027f, -1.35433f, -5.90841f, 6.49489f,  -5.28090f, 4.95714f,  -9.37658f, -4.61623f,
    6.33320f,  9.05788f,  7.74236f,  -0.84028f, -6.64415f, 1.74848f,  5.77861f,  -3.17352f, 1.21083f,  5.08367f,
    -7.72516f, 6.42745f,  -9.83351f, -9.70465f, -3.54772f, -7.37416f, -2.08868f, 3.56927f,  -3.43333f, -3.02251f,
    7.18175f,  -8.15131f, -6.03459f, -1.73443f, -0.91152f, -3.75740f, 6.19633f,  4.14575f,  -5.37607f, -7.36705f,
    8.89244f,  1.39289f,  1.53383f,  1.12498f,  -5.87719f, -4.37070f, -6.50984f, 3.57958f,  5.40322f,  -4.65746f,
    -5.70183f, -7.44602f, 7.56560f,  -6.92505f, 6.06597f,  -8.85882f, 8.20909f,  -8.84451f, -7.55648f, -4.73150f,
    -0.96385f, 0.38258f,  -3.10631f, 9.93032f,  -7.06206f, 4.61396f,  -6.19065f, 8.91965f,  -3.56298f, 0.67778f,
    7.93350f,  9.53591f,  5.65004f,  2.51638f,  -9.49472f, -8.75178f, 4.07173f,  -4.84450f, -1.54252f, -0.56567f,
    7.19198f,  -2.53427f, -4.16850f, 1.47983f,  -5.05385f, 4.89241f,  -6.33212f, -0.49553f, 8.43587f,  -4.96575f,
    -6.58216f, -9.68587f, -1.80548f, 8.79417f,  6.98266f,  4.28916f,  0.70989f,  1.61469f,  -9.07296f, -1.58392f,
    -7.42654f, -2.39975f, -2.34463f, -6.53747f, -1.00427f, -8.92843f, 2.95637f,  3.31480f,  -9.64551f, -8.64690f,
    -3.58853f, 4.24319f,  7.50260f,  -3.61401f, -9.91683f, -4.56730f, -6.58313f, -8.46006f, -6.66975f, 0.09210f,
    -8.16130f, 2.19071f,  -7.38338f, -7.32992f, 1.43260f,  4.27257f,  -6.88868f, -4.32963f, -7.59036f, 1.21713f,
    6.43214f,  -8.95334f, -1.72864f, -1.03092f, 5.44935f,  0.31199f,  -8.90184f, 4.49388f,  7.20876f,  7.47423f,
    -2.83166f, 5.43556f,  9.89562f,  -1.17701f, 2.41719f,  0.93132f,  -2.64796f, 6.97072f,  6.98565f,  -1.43908f,
    9.49609f,  -8.53626f, 5.72548f,  0.14453f,  3.07307f,  1.92560f,  2.96141f,  7.39365f,  5.07517f,  -3.35717f,
    -9.99346f, -8.71735f, 4.27034f,  4.41887f,  -5.94145f, 6.74185f,  4.45710f,  0.85286f,  6.23896f,  -1.69507f,
    -0.28377f, 2.36344f,  7.08174f,  3.88279f,  -0.17452f, -3.92190f, 4.72935f,  -6.47502f, -7.46027f, -7.74994f,
    2.08494f,  -7.89875f, 9.70752f,  -0.47615f, -2.94333f, -8.64008f, -8.43222f, 1.79943f,  -0.72707f, -5.05841f,
    8.19380f,  -9.77925f, -7.57259f, -9.98601f, 5.58037f,  9.71955f,  -9.63579f, -6.12072f, 7.87742f,  -3.50368f,
    2.67012f,  -9.73830f, -3.51469f, 2.88600f,  1.19969f,  5.83671f,  8.51835f,  3.37408f,  5.75521f,  1.05292f,
    8.05218f,  -6.35638f, -9.35331f, -6.83586f, -9.18902f, 7.46615f,  4.29568f,  -5.33234f, 7.68480f,  1.73829f,
    8.37063f,  0.37742f,  -7.88483f, 6.52919f,  2.11953f,  -6.10367f, 4.85482f,  -3.29317f, -8.08474f, -8.98830f,
    4.65221f,  -1.64774f, -6.80234f, 3.86627f,  9.08059f,  3.37334f,  1.56074f,  -7.22893f, 0.11437f,  -6.27332f,
    8.09680f,  -2.57089f, 1.01584f,  -6.95102f, -1.32714f, -2.38946f, -1.19276f, -7.92486f, -4.55988f, 9.77793f,
    -4.71558f, 2.69454f,  -3.19649f, 8.08590f,  2.44223f,  7.55874f,  -2.88437f, 6.44491f,  -8.84439f, 9.65934f,
    -2.88316f, 6.63873f,  6.22111f,  -0.94517f, 1.12322f,  1.39204f,  -1.52287f, 5.85185f,  0.40659f,  -2.79680f,
    -8.36758f, -8.92702f, -4.47743f, -1.12899f, 2.93885f,  4.55356f,  7.32685f,  3.61080f,  -3.09299f, 3.34278f,
    7.43658f,  -8.00631f, 3.95550f,  3.51196f,  7.91253f,  5.53583f,  7.71750f,  6.74937f,  6.30989f,  -9.09123f,
    -0.55358f, -1.26978f, 6.43639f,  -0.07554f, -1.55975f, 7.11895f,  -6.25526f, -5.50645f, 6.79627f,  -3.74662f,
    6.33883f,  9.51613f,  8.90242f,  4.62723f,  -5.18018f, 8.09495f,  -5.46958f, -5.39303f, -7.75632f, -6.12000f,
    3.55112f,  7.44182f,  -9.13721f, 2.42484f,  7.64426f,  0.46464f,  -2.24053f, 4.31547f,  -8.39008f, 4.22868f,
    -8.98346f, -0.08195f, -2.41010f, 3.90651f,  -8.87544f, 1.98014f,  5.76648f,  7.06711f,  7.60926f,  0.86063f,
    1.39293f,  -3.60422f, 0.49759f,  8.38953f,  -6.78263f, -7.37406f, 3.63870f,  7.58595f,  -0.20697f, -8.83972f,
    0.26591f,  -5.29550f, -6.75722f, 4.18623f,  -6.89695f, -0.03029f, -1.04303f, 3.63271f,  -6.79129f, -7.75546f,
    6.29580f,  -9.10068f, 8.01421f,  -7.27066f, -6.83203f, 9.06481f,  -3.64834f, -0.86170f, -8.12355f, -3.06568f,
    1.20411f,  3.69757f,  -6.66817f, -2.82572f, 8.17502f,  3.21200f,  -3.59027f, -4.94095f, -1.46972f, -1.07293f,
    -6.52541f, 0.08928f,  1.53614f,  7.96836f,  -3.74326f, -3.75225f, -3.38809f, 2.67324f,  -5.92884f, 1.06186f,
    -2.23427f, 2.54998f,  4.38539f,  -4.85867f, 9.55938f,  -3.87439f, -1.59161f, 6.58846f,  -4.76559f, -3.40654f,
    -0.57622f, -5.13035f, -2.22436f, -1.33697f, -3.96378f, 0.05732f,  3.21759f,  -5.40274f, 1.15436f,  3.44465f,
    -0.73830f, 7.67949f,  9.65987f,  0.31785f,  8.12037f,  -8.01185f, 7.60512f,  -5.86755f, 8.77293f,  -6.75717f,
    -2.89329f, -5.79838f, -7.94096f, -1.10030f, -8.30164f, -4.31436f, 4.81738f,  -4.44737f, 0.23332f,  7.57553f,
    3.65214f,  1.03828f,  -4.95143f, -5.18061f, 6.88024f,  -1.84921f, -0.49890f, 9.95765f,  6.68037f,  2.39398f,
    -1.63835f, 0.77123f,  -1.98306f, 4.92871f,  -3.54779f, 8.01561f,  5.38072f,  -1.35251f, -5.72469f, 4.63131f,
    6.28110f,  -2.05917f, 5.05418f,  -1.96820f, 0.26304f,  -3.35312f, -8.72488f, -4.31363f, 2.09205f,  1.34882f,
    -8.02546f, -0.89581f, -6.26459f, -3.93678f, 1.68485f,  -9.94318f, 9.65893f,  -2.57243f, 0.31969f,  -8.32212f,
    3.05471f,  -0.73482f, -2.51721f, -3.15348f, -8.68139f, 0.04443f,  -4.44787f, 4.30533f,  1.07191f,  -9.43039f,
    -7.51728f, -3.40529f, -8.44354f, -6.64094f, -6.72808f, 6.99461f,  3.27563f,  7.80686f,  8.33628f,  -1.53385f,
    5.04029f,  -8.32624f, 4.43213f,  1.22868f,  -2.93367f, -1.88713f, -5.11814f, -2.45039f, 0.51051f,  -6.97197f,
    -1.15625f, -7.82870f, -2.22295f, 8.50125f,  -1.41646f, 7.43619f,  4.90238f,  6.69840f,  -6.78077f, -2.19466f,
    -2.07207f, 1.12748f,  -8.75182f, -8.09828f, 8.30520f,  -8.90648f, -7.00372f, 1.30008f,  -7.46223f, -5.07510f,
    4.41644f,  -3.87758f, -5.16742f, -5.74961f, -5.13970f, -4.48847f, 2.57534f,  -7.58444f, -6.22609f, -7.25655f,
    5.92211f,  -5.16910f, -2.96338f, 0.02568f,  -0.17946f, 3.99880f,  -5.41442f, 7.44778f,  -7.06188f, 2.98760f,
    3.37074f,  1.51384f,  5.91171f,  8.52166f,  -1.82390f, 4.18569f,  -1.64458f, -6.09212f, 4.97994f,  3.00369f,
    -2.73404f, 4.76250f,  2.59263f,  -5.31223f, 2.18863f,  2.61276f,  8.79504f,  2.70871f,  9.23223f,  -4.30108f,
    1.52192f,  -7.93707f, -7.49678f, 2.14144f,  6.98578f,  -9.50332f, 5.69072f,  4.46349f,  3.46314f,  -5.13240f,
    -9.64027f, 5.65795f,  8.53658f,  1.14730f,  6.73145f,  -6.27883f, -5.73621f, 9.34599f,  0.48297f,  -3.98020f,
    5.93749f,  9.03804f,  -7.68380f, 2.96965f,  3.48118f,  -5.21574f, -2.72929f, 6.84835f,  5.74761f,  -3.31882f,
    1.19517f,  9.46701f,  6.26552f,  -4.95271f, 9.61201f,  9.67824f,  6.49148f,  4.01508f,  0.74510f,  -9.28003f,
    4.88930f,  6.57170f,  8.22741f,  6.64313f,  7.29528f,  2.41739f,  -1.84803f, -8.01405f, -3.60924f, 2.71333f,
    -9.01155f, -4.14871f, -6.24235f, 5.38007f,  -2.16414f, 5.85224f,  -0.57668f, 7.28138f,  6.31025f,  2.66814f,
    6.15197f,  -0.70156f, -2.19628f, 4.90916f,  1.35013f,  2.50890f,  -4.20175f, -3.60368f, -8.59165f, -1.10329f,
    -7.19165f, -4.87665f, 6.73913f,  1.10513f,  5.73071f,  8.86371f,  4.79894f,  2.93544f,  2.91481f,  7.39946f,
    5.68901f,  -2.34821f, -5.85872f, -7.25887f, 6.46002f,  6.08832f,  5.65362f,  9.87776f,  5.09161f,  -0.01409f,
    -4.96815f, -5.66504f, 7.71045f,  -3.27393f, -4.29291f, -1.78235f, -7.61528f, 2.95097f,  -5.73902f, -8.70765f,
    2.39502f,  7.04027f,  -4.73373f, 1.09950f,  9.62861f,  9.98262f,  4.93704f,  -6.33815f, -5.94339f, -9.57139f,
    -6.03906f, -5.27811f, -5.01243f, 3.01877f,  8.96819f,  7.47303f,  -2.31526f, -5.66858f, -2.71667f, 1.72024f,
    0.11318f,  -2.70216f, 6.07467f,  9.78337f,  -4.72329f, -4.02421f, -0.18395f, 5.27181f,  3.96694f,  -7.48053f,
    -6.92074f, -3.42988f, -2.40490f, 9.77333f,  -0.30730f, 4.37769f,  2.63065f,  0.53899f,  -3.11768f, 5.71806f,
    8.91440f,  8.94023f,  8.02451f,  5.28794f,  0.70042f,  -4.69685f, -8.20483f, -2.71358f, 8.24728f,  -9.17618f,
    0.20763f,  -6.70992f, -4.39054f, 0.62377f,  -6.60567f, 1.77831f,  -7.72461f, -7.43482f, -8.09086f, -4.98254f,
    7.44223f,  3.77624f,  9.52371f,  9.37664f,  -9.34868f, 3.35080f,  -6.27862f, -8.37104f, -8.24965f, -1.84965f,
    -8.86093f, 0.18754f,  1.22750f,  4.23103f,  -1.45596f, -3.03658f, -7.51905f, -1.39812f, 2.69410f,  -3.25497f,
    1.77429f,  -7.11159f, -4.00556f, 4.15675f,  -5.66598f, -0.44158f, -9.76796f, 9.54147f,  -9.86836f, -4.84623f,
    -1.09882f, -9.89516f, 5.35354f,  9.58199f,  -4.00881f, 9.91393f,  -5.05084f, 4.24625f,  -7.21973f, -1.75442f,
    4.37824f,  -8.95964f, -6.88128f, -5.19692f, 3.27883f,  -0.78038f, 8.13298f,  6.63811f,  -0.86566f, 6.81223f,
    -9.96513f, -9.50483f, -1.65841f, -3.17166f, 4.88875f,  -8.84550f, 4.35808f,  -5.93102f, 7.72902f,  -4.47840f,
    0.94915f,  8.90513f,  -2.34326f, -5.83943f, 0.80032f,  9.48064f,  -8.88043f, -8.53178f, 8.49664f,  -2.25810f,
    4.40557f,  -5.06630f, -1.88566f, 3.45469f,  2.00550f,  -8.32170f, 6.64334f,  9.36825f,  -6.39497f, -9.24162f,
    5.66811f,  8.15070f,  -9.85093f, 7.74863f,  -3.68800f, 7.97893f,  -4.50936f, -3.12566f, -6.79998f, 1.17077f,
    -1.42418f, 6.95268f,  -8.15174f, 6.24649f,  -7.90176f, -7.18254f, -0.01591f, 1.21140f,  4.71176f,  -7.12009f,
    -1.44458f, -7.25360f, 9.59549f,  3.24089f,  1.61594f,  4.44725f,  -7.77722f, -9.66682f, -9.12136f, 5.34722f,
    -7.76316f, -2.12404f, 6.59313f,  -8.92211f, 1.71711f,  -3.43437f, -9.18456f, 7.97844f,  8.35414f,  -3.04156f,
    5.70274f,  3.46606f,  0.59527f,  5.55289f,  6.19152f,  -1.46750f, -5.02463f, 9.02596f,  -4.21127f, 3.32164f,
    -8.33186f, 4.49628f,  0.28004f,  2.55836f,  6.85301f,  -4.94816f, -9.18357f, 1.71467f,  -4.22061f, 5.04427f,
    2.10975f,  2.49635f,  6.53246f,  2.67699f,  5.66162f,  1.25500f,  -0.36122f, 9.59258f,  5.01117f,  -9.81135f,
    7.09733f,  1.14298f,  -7.04834f, 0.16270f,  -3.23290f, -8.00067f, -1.88708f, -3.64277f, -5.08015f, -6.67229f,
    -7.38615f, -6.44324f, -1.64319f, -2.12077f, -9.58402f, -4.62701f, 6.63675f,  3.77680f,  -2.78879f, -3.81928f,
    2.92034f,  -5.45090f, -7.53028f, 7.72819f,  -8.86762f, 6.86855f,  7.02809f,  -4.87721f, 1.48462f,  0.20060f,
    8.67957f,  -2.56093f, -6.42348f, 0.37000f,  9.35188f,  9.18333f,  -9.44512f, -0.96261f, -3.07814f, 7.24771f,
    -9.80699f, -5.89575f, 5.21080f,  0.42694f,  -8.56844f, 4.09713f,  8.89651f,  -5.50764f, 0.76284f,  7.66084f,
    -6.09291f, -6.84949f, 8.99053f,  -2.49485f, 8.34431f,  3.49296f,  9.39794f,  8.03067f,  -0.34257f, 4.59870f,
    -8.20489f, -2.41027f, -5.17759f, 4.23562f,  5.27438f,  5.16272f,  -1.55350f, -1.10750f, -0.26884f, -7.34586f,
    -3.73046f, 4.65437f,  -9.48321f, -4.41696f, 2.21830f,  2.05298f,  -9.80857f, 1.29349f,  -3.08281f, 5.69946f,
    -0.37416f, -3.32929f, -1.89301f, 5.33941f,  8.28091f,  1.60708f,  -4.06870f, 5.07272f,  7.90258f,  -4.51025f,
    1.08962f,  -6.81522f, -2.66204f, 3.71443f,  0.81955f,  4.86542f,  -6.39781f, 1.47674f,  0.07887f,  -4.24451f,
    2.11837f,  1.66433f,  5.47027f,  8.70229f };

const float kMatrixB[kN * kN] =  //
  { 9.72021f,  6.49670f,  -7.54882f, -8.00339f, 9.95481f,  4.63599f,  4.20078f,  -4.27181f, 0.30477f,  7.84052f,
    2.90157f,  -6.98213f, 7.47744f,  -7.89820f, -7.65084f, 2.48255f,  1.32322f,  -6.52176f, 4.50639f,  -3.53208f,
    -5.74609f, 3.93423f,  2.71765f,  -8.25343f, -0.35546f, 0.94940f,  -9.00612f, 0.58083f,  5.97516f,  6.82584f,
    1.93767f,  5.77743f,  -6.12759f, -9.13520f, 4.31707f,  7.22972f,  -7.86060f, 3.07278f,  -1.19097f, -0.84330f,
    -3.17867f, 6.15698f,  -6.53906f, 4.97187f,  -8.34039f, 5.87001f,  -1.54037f, -7.58075f, -9.56430f, 6.75385f,
    5.76880f,  2.87913f,  -4.01859f, -1.37107f, -9.25824f, 7.36066f,  -8.28706f, -4.43314f, 2.24932f,  2.66087f,
    -3.18037f, -8.11990f, 2.24388f,  3.49329f,  7.75417f,  -9.57024f, 1.95602f,  -1.52300f, -4.80017f, 9.40973f,
    -9.52204f, -2.72032f, 3.66919f,  1.47221f,  -8.97749f, 7.80755f,  -3.92024f, 3.61067f,  9.26715f,  -6.90612f,
    -0.61920f, -7.27325f, 1.82546f,  3.50142f,  5.60867f,  -4.10997f, -3.31390f, -7.62985f, 0.44593f,  -2.09340f,
    5.86013f,  -2.96442f, -8.30453f, -3.08324f, 1.55652f,  3.80774f,  5.96238f,  9.92005f,  -3.10870f, 1.66077f,
    2.70532f,  6.21491f,  3.05901f,  -7.99205f, -9.86389f, -0.51518f, -5.36827f, -2.75489f, 3.29030f,  -4.18169f,
    7.43399f,  -6.65926f, -6.55035f, -5.33037f, -6.13122f, 5.14048f,  -5.20897f, -4.77415f, 0.73274f,  6.44297f,
    3.99113f,  6.58172f,  -0.87100f, 2.19202f,  4.37717f,  4.37294f,  -3.53482f, 5.26908f,  -7.14042f, -0.19742f,
    5.79998f,  -7.74966f, 9.06204f,  0.59119f,  5.91872f,  -1.26244f, -9.30614f, 1.60701f,  -9.77323f, 7.84483f,
    -0.49254f, 1.09524f,  -2.61935f, -1.54054f, 3.64331f,  9.23285f,  4.14123f,  6.48069f,  -2.40550f, -7.77255f,
    5.96298f,  -8.09423f, -5.23870f, -3.21136f, -8.94306f, -3.34556f, 8.70678f,  -3.15603f, 2.92980f,  1.83122f,
    5.69786f,  9.91403f,  -3.72939f, -5.29828f, 7.88044f,  5.02729f,  8.64174f,  -4.67439f, -3.60892f, -0.31501f,
    5.45154f,  4.44833f,  -7.38359f, -2.57008f, 5.05439f,  6.14216f,  8.33218f,  -6.91002f, -5.85057f, 9.38616f,
    8.51808f,  -1.40023f, 5.30463f,  6.06921f,  9.31414f,  -5.25626f, 0.50421f,  9.37392f,  6.20538f,  -5.25048f,
    9.28179f,  7.46624f,  -4.29274f, -3.99396f, -1.70260f, -1.41805f, -2.99618f, -4.14984f, -4.43649f, 0.22653f,
    -3.85346f, -6.16905f, -1.03679f, 9.48265f,  6.99228f,  4.37460f,  0.88836f,  -8.59612f, -0.10017f, 8.61519f,
    1.44506f,  -1.95708f, 0.61289f,  -2.03448f, -4.29022f, 3.46706f,  3.58971f,  -2.08406f, -0.12826f, -6.55930f,
    -5.53889f, 5.33895f,  4.95526f,  -6.79917f, 4.64243f,  -0.96457f, -8.80923f, 7.97795f,  2.52293f,  5.33506f,
    0.93494f,  -4.64497f, -9.66885f, 6.96163f,  -5.15156f, -9.57337f, 2.04115f,  4.53541f,  -7.96206f, 4.67903f,
    -0.95967f, -3.80926f, 9.72102f,  6.95524f,  8.96244f,  -1.85067f, 8.34267f,  9.20739f,  -2.97400f, 0.50522f,
    4.45701f,  -2.45030f, -9.37817f, 0.51074f,  -0.64578f, -1.08584f, 0.93652f,  9.04023f,  -0.38636f, -0.92356f,
    5.74699f,  1.76563f,  8.96288f,  5.37908f,  1.03215f,  -3.62605f, -1.49537f, -9.85393f, -1.15119f, 3.10760f,
    -1.09071f, -0.44696f, 4.50947f,  1.11015f,  -6.82136f, -2.81877f, -8.25014f, -1.69993f, 3.39525f,  6.94262f,
    9.30892f,  -0.91876f, -7.59360f, -3.07290f, -6.58888f, -8.53159f, -5.13604f, 4.48454f,  4.35136f,  9.60140f,
    -4.84089f, 4.81833f,  3.76677f,  5.70460f,  -9.98024f, 4.96289f,  0.57483f,  -6.88775f, 9.53015f,  -8.31271f,
    -3.50710f, -8.36483f, -7.66609f, -9.78223f, -7.69138f, -6.74942f, 9.82744f,  -4.79658f, -5.08425f, 0.58182f,
    -9.45953f, 2.42277f,  -8.02090f, -2.17367f, -2.36670f, -5.69205f, 6.37509f,  9.93978f,  7.07821f,  0.04397f,
    4.21192f,  2.26467f,  1.84011f,  8.99113f,  4.24749f,  0.21837f,  -1.81613f, -9.64470f, 2.29366f,  -0.08927f,
    -2.86600f, -3.90374f, 2.29998f,  -7.22178f, 2.46880f,  -2.22964f, 8.15552f,  7.34957f,  8.34927f,  9.99332f,
    0.90497f,  -8.41086f, -3.36952f, 0.03720f,  -4.21072f, 4.74920f,  1.94162f,  3.85573f,  -3.48802f, 4.34596f,
    -1.90485f, -0.13649f, 9.01643f,  -6.86455f, -5.64406f, 0.56684f,  -7.40443f, 3.03110f,  3.16201f,  -0.50511f,
    -1.95300f, 1.86506f,  -2.57450f, 8.32579f,  -8.20120f, -1.86530f, 9.84893f,  8.08739f,  7.01831f,  2.31716f,
    3.77998f,  3.89498f,  6.77104f,  -9.78017f, -9.93904f, 8.96840f,  -4.54495f, -4.99620f, 2.91460f,  -7.72258f,
    3.79553f,  -1.70769f, 3.52195f,  -7.85207f, 4.08745f,  4.18976f,  -2.19477f, 4.42266f,  6.44037f,  0.03779f,
    -6.08180f, 6.59765f,  2.98794f,  8.87748f,  2.37885f,  -9.15098f, -3.68875f, -6.28752f, -3.12921f, 2.56893f,
    0.04527f,  7.34534f,  6.84835f,  4.55525f,  2.56147f,  0.32295f,  4.09264f,  -9.77629f, -2.20164f, 8.98122f,
    -2.37686f, 3.67821f,  -8.71226f, -3.63423f, -3.95822f, -5.07941f, -6.28217f, 5.45521f,  8.29136f,  -3.95979f,
    5.34883f,  8.49710f,  -9.45647f, 3.52080f,  -6.85006f, -2.51907f, -2.27403f, -0.96760f, 1.38505f,  7.14016f,
    -5.29428f, 2.03995f,  8.33777f,  -1.08866f, 9.93598f,  -2.34727f, 4.51508f,  9.88285f,  -6.72229f, -5.94667f,
    8.92935f,  7.51640f,  6.30818f,  -3.17546f, 4.99993f,  -2.77358f, 5.52634f,  7.15288f,  -1.37215f, 4.91670f,
    -8.88527f, 1.98117f,  -6.50936f, -3.61517f, -7.12799f, 1.12608f,  9.85108f,  -7.29680f, 9.47304f,  -0.33523f,
    -9.91071f, 7.42134f,  -8.52789f, -7.01830f, -1.47236f, 2.82212f,  7.18804f,  5.02125f,  5.50058f,  -6.17321f,
    6.48908f,  -9.92950f, -6.71491f, 4.40735f,  7.97303f,  -4.97025f, -5.60849f, 1.59477f,  -1.81922f, 3.71182f,
    -1.09127f, -7.62416f, 3.53435f,  -9.64159f, 1.32484f,  7.28214f,  0.27252f,  8.80178f,  3.66838f,  3.98759f,
    -2.02252f, 4.07606f,  8.82064f,  8.81843f,  3.26974f,  2.14770f,  -4.84002f, 6.72732f,  9.31378f,  -8.35054f,
    -6.75543f, 0.04947f,  0.32170f,  -7.36081f, 8.97718f,  -3.82033f, 7.50091f,  9.80347f,  -9.85488f, -3.27167f,
    0.20347f,  8.70520f,  -7.09139f, -8.08796f, -4.49265f, -6.69558f, -9.82386f, -9.14083f, -4.98351f, -8.62950f,
    5.41930f,  -0.34157f, -6.57185f, 2.84796f,  3.53601f,  -8.92027f, -0.40482f, 4.10119f,  9.76288f,  5.11375f,
    3.81444f,  -8.32347f, 6.88764f,  6.94036f,  -6.72126f, -6.71859f, 6.90051f,  -0.63200f, 2.23868f,  1.76648f,
    2.33194f,  5.19969f,  -2.25654f, -2.86422f, 4.06623f,  -2.14925f, -4.11815f, 0.00345f,  7.41533f,  -6.53569f,
    0.81220f,  9.68861f,  -0.28606f, 9.41337f,  -1.86321f, 2.28227f,  -1.06735f, -4.68472f, -1.37232f, -3.14666f,
    -8.96117f, -3.28558f, -9.98905f, 4.53115f,  -6.46004f, 8.31572f,  -1.53614f, 7.73957f,  6.71808f,  -2.61464f,
    -9.50774f, -0.10605f, 2.83820f,  -6.07639f, -8.05567f, 7.19532f,  6.72458f,  -8.00312f, 7.50167f,  9.14375f,
    0.77346f,  3.22835f,  -6.08578f, 0.31340f,  -7.80426f, -1.62902f, 8.78675f,  5.50377f,  -6.59283f, -3.05217f,
    -6.94200f, 8.81406f,  -0.25877f, -9.58286f, -7.76065f, -7.84144f, 4.98209f,  3.28529f,  6.34059f,  8.59927f,
    3.24917f,  -7.25361f, 2.25505f,  2.97473f,  0.47787f,  -2.50142f, 7.04716f,  8.43826f,  1.77323f,  -6.62130f,
    6.73207f,  3.35351f,  -2.59137f, 5.57984f,  3.54933f,  -0.94410f, -4.35811f, 2.79520f,  -3.10291f, -2.70173f,
    7.30030f,  4.61770f,  -5.70417f, -6.84566f, -0.25059f, 0.10636f,  9.64547f,  -6.82760f, 2.37075f,  -7.82050f,
    -2.94639f, 5.89824f,  4.98369f,  0.12572f,  -2.39117f, 3.23328f,  -8.50044f, -7.46540f, 1.74079f,  -4.88408f,
    8.08733f,  -5.25252f, -6.14164f, -1.86469f, 1.62247f,  -4.80371f, 1.11036f,  4.95736f,  -2.01481f, -4.29467f,
    -6.41822f, 0.85665f,  9.62229f,  0.08958f,  9.63046f,  -7.88349f, 7.33600f,  -4.94165f, 4.75199f,  0.38527f,
    6.51710f,  -1.57478f, -7.06609f, -8.11342f, 7.98754f,  2.23761f,  -9.37049f, -3.11926f, 7.84700f,  0.26796f,
    -0.86136f, -6.92978f, -4.75270f, -1.07718f, 0.19839f,  -7.83948f, 5.21860f,  -5.59296f, 8.28023f,  7.97795f,
    -8.78858f, 2.62840f,  0.75065f,  4.95771f,  3.76687f,  -0.05831f, 6.48141f,  -0.92250f, -9.68367f, -9.94385f,
    -2.10315f, -6.87092f, 2.18411f,  1.68643f,  -1.18880f, 3.82465f,  -6.41588f, -4.70454f, 6.11489f,  -1.35429f,
    8.46660f,  -0.48650f, -3.45996f, 4.07882f,  6.81541f,  9.17308f,  -1.97050f, -5.76110f, 1.74471f,  1.27162f,
    0.37992f,  -3.93265f, 3.43787f,  5.80283f,  0.95588f,  4.29761f,  -6.69165f, 2.87665f,  -5.62054f, -6.31030f,
    -0.72317f, 2.72721f,  -6.34081f, -8.53630f, 7.06262f,  0.87856f,  7.72985f,  -3.86377f, -6.96312f, -1.58210f,
    -8.50367f, 8.34352f,  6.58261f,  -0.78997f, -4.05883f, -6.57532f, -1.53368f, -5.81707f, 8.21309f,  -2.28107f,
    -1.07931f, 6.87076f,  -5.64831f, 9.03000f,  5.74198f,  3.93601f,  5.72864f,  -4.49742f, 1.34126f,  -4.51461f,
    0.69773f,  4.08540f,  -0.47407f, 9.33771f,  -9.12955f, -0.41582f, 1.76603f,  -9.95335f, -2.19463f, -8.03589f,
    -5.28637f, 5.54113f,  0.66964f,  8.06367f,  3.42697f,  2.77308f,  -8.86805f, 6.72570f,  -2.39519f, 3.34673f,
    -2.54326f, -1.54055f, 5.62776f,  4.26711f,  -9.73248f, 5.78107f,  6.63174f,  -7.02314f, 2.96437f,  8.08580f,
    7.75526f,  -7.81702f, 9.53008f,  4.75763f,  2.66545f,  9.14802f,  -8.81257f, -5.60602f, -9.04797f, 5.89291f,
    -5.27688f, -9.09167f, -2.31738f, -3.26110f, -5.47175f, 4.52934f,  -7.04145f, -1.39708f, 0.76648f,  -8.86058f,
    6.67461f,  -6.77147f, 3.83488f,  7.93292f,  -8.44415f, -5.42389f, -1.18755f, 7.32417f,  9.59265f,  -7.95509f,
    5.62093f,  7.12622f,  5.64040f,  9.70391f,  7.89931f,  3.16333f,  0.09640f,  -6.19396f, -6.02146f, 5.45094f,
    -9.28257f, 1.30429f,  7.99676f,  1.18445f,  -4.00784f, -1.67790f, -3.01441f, 8.26108f,  0.79506f,  -1.41668f,
    -3.27432f, -6.23652f, 6.84399f,  4.54946f,  -8.84018f, -4.91627f, -6.75428f, 5.16635f,  2.13818f,  -2.12039f,
    8.50615f,  -0.48173f, -0.82237f, -3.47784f, -3.76297f, 6.28055f,  -2.94092f, -4.43520f, -6.33660f, -4.74115f,
    -4.94868f, 1.76024f,  -4.03274f, 7.67758f,  1.45710f,  8.71992f,  -1.17401f, 0.94716f,  -0.40519f, 2.31730f,
    7.70648f,  -9.08301f, -2.60225f, 3.06807f,  8.55542f,  8.51581f,  -6.03861f, 5.94079f,  -4.35557f, -0.50496f,
    -7.69884f, 1.28922f,  6.31896f,  -9.48662f, -7.08348f, -3.46687f, -7.50693f, -0.58527f, -2.44543f, 4.11910f,
    -7.57746f, 7.25402f,  -9.21561f, 9.73029f,  2.24737f,  -2.02344f, 6.47679f,  -6.02309f, -9.56763f, 6.85342f,
    2.48227f,  7.14803f,  9.90175f,  -3.56324f, -3.12123f, -7.39850f, -0.33606f, 7.16785f,  3.89968f,  -6.50857f,
    -3.56363f, -5.90069f, 6.72724f,  -1.14711f, -4.38121f, -3.80071f, -5.63624f, -3.85042f, 5.21815f,  -2.09133f,
    -3.02341f, 9.35707f,  -3.41354f, -5.53424f, 6.98790f,  8.44789f,  0.25365f,  7.98675f,  -9.72488f, 1.77071f,
    3.02028f,  2.25621f,  -9.62384f, -5.26989f, 9.18194f,  -9.40513f, -5.42840f, -5.80036f, 3.42547f,  6.38808f,
    -7.05080f, 1.16107f,  3.64027f,  -9.01784f, -4.97042f, -2.22902f, -0.96993f, 4.11411f,  -1.50448f, 9.37264f,
    -7.45777f, -2.76789f, -0.51130f, -0.64516f, -6.09574f, 7.19147f,  6.34692f,  -1.67457f, -4.69518f, -8.90410f,
    -5.24984f, 2.68163f,  1.49346f,  6.63707f,  4.64022f,  3.38275f,  9.58110f,  6.09008f,  0.75057f,  -0.43655f,
    -9.55913f, 6.07248f,  5.68667f,  -1.39139f, -8.26138f, -0.59398f, -5.68159f, -6.57423f, -6.83017f, 8.95342f,
    -3.55264f, -5.18688f, 4.03319f,  9.11997f,  3.20559f,  8.87284f,  0.23848f,  -9.19120f, 1.44016f,  -0.54968f,
    -8.31378f, 0.80618f,  3.63805f,  -2.99200f, 4.42686f,  5.58783f,  1.59461f,  -3.27937f, -4.83772f, -4.13061f,
    -8.75030f, -8.99610f, 9.12702f,  -3.83355f, -0.04116f, 5.80286f,  9.80364f,  -5.51030f, -5.28546f, 8.44179f,
    -5.61439f, -8.22297f, 5.64440f,  -7.04520f, 0.10237f,  -0.60731f, -8.78457f, 2.47468f,  8.79759f,  9.10771f,
    -2.59082f, -2.69320f, 7.91567f,  7.88814f,  1.50752f,  5.77965f,  1.63419f,  -9.32001f, -2.35408f, 2.25639f,
    -9.24492f, 2.41877f,  6.47009f,  0.73462f };
}  // namespace gpukerneltest


================================================
FILE: tests/gputiltest/TestMain.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gputil/gpuDevice.h>

#include <gtest/gtest.h>

gputil::Device g_gpu;

int main(int argc, char **argv)
{
  g_gpu.select(argc, argv);
  if (!g_gpu.isValid())
  {
    std::cerr << "GPU initialisation failed." << std::endl;
    return -1;
  }
  ::testing::InitGoogleTest(&argc, argv);
  std::cout << g_gpu.description() << std::endl;
  return RUN_ALL_TESTS();
}


================================================
FILE: tests/gputiltest/cuda/matrix_kernel.cu
================================================
#include <gputil/cuda/cutil_importcl.h>
#include <gputil/cuda/cutil_math.h>

#include "matrix.cl"

GPUTIL_CUDA_DEFINE_KERNEL(matrixMultiply);


================================================
FILE: tests/gputiltest/matrix.cl
================================================
#include "gpu_ext.h"  // Must be first

__kernel void matrixMultiply(__global float *out, __global float *a, __global float *b,
                             unsigned n LOCAL_ARG(float *, work))
{
  LOCAL_MEM_ENABLE();
  LOCAL_VAR(float *, work, LM_PER_THREAD(sizeof(float)));

  if (get_global_id(0) >= n)
  {
    return;
  }

  // Handled null for testing passing null arguments
  if (!a || !b || !out)
  {
    return;
  }

  for (unsigned i = 0; i < n; ++i)
  {
    work[get_local_id(0)] = a[i * n + get_local_id(0)] * b[get_local_id(0) * n + i];
    barrier(CLK_LOCAL_MEM_FENCE);

    if (get_local_id(0) == 0)
    {
      float r = 0;
      for (unsigned j = 0; j < n; ++j)
      {
        r += work[j];
      }
      out[i] = r;
    }
  }
}


================================================
FILE: tests/ohmtest/CMakeLists.txt
================================================
# Setup of GTEST changed at CMake 3.5.
cmake_minimum_required(VERSION 3.5)



# Eigen required to support some tests - NDT in particular
find_package(Eigen3 QUIET)

configure_file(OhmTestConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/OhmTestConfig.h")

set(SOURCES
  CompressionTests.cpp
  CopyTests.cpp
  IncidentsTests.cpp
  KeyTests.cpp
  LayoutTests.cpp
  LineQueryTests.cpp
  LineWalkTests.cpp
  MapTests.cpp
  MathsTests.cpp
  OhmTestConfig.in.h
  SerialisationTests.cpp
  VoxelMeanTests.cpp
  RaysQueryTests.cpp
  RayPatternTests.cpp
  RayValidation.cpp
  RayValidation.h
  SecondarySampleTests.cpp
  TestMain.cpp
  TouchTimeTests.cpp
  TraversalTests.cpp
  TsdfTests.cpp
  "${CMAKE_CURRENT_BINARY_DIR}/OhmTestConfig.h"
)

if(Eigen3_FOUND)
  message(STATUS "Eigen3 found. Adding NDT tests.")
  list(APPEND SOURCES NdtTests.cpp)
endif(Eigen3_FOUND)

add_executable(ohmtest ${SOURCES})
leak_track_target_enable(ohmtest CONDITION OHM_LEAK_TRACK)
leak_track_suppress(ohmtest CONDITION OHM_LEAK_TRACK
  ${OHM_LEAK_SUPPRESS_TBB}
)

set_target_properties(ohmtest PROPERTIES FOLDER tests)
if(MSVC)
  set_target_properties(ohmtest PROPERTIES DEBUG_POSTFIX "d")
endif(MSVC)

target_include_directories(ohmtest
  PRIVATE
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
)

target_include_directories(ohmtest
  SYSTEM PRIVATE
    "${GTEST_INCLUDE_DIRS}"
    $<$<NOT:$<BOOL:${OHM_TES_DEBUG}>>:${3ES_INCLUDE_DIRS}>
)

target_link_libraries(ohmtest PRIVATE ohmtestcommon ohmtools ohm ohmutil)

target_link_libraries(ohmtest
  PRIVATE
    ${GTEST_LIBRARIES}
    ${GTEST_MAIN_LIBRARIES}
    glm::glm
    $<BUILD_INTERFACE:$<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>>
    $<BUILD_INTERFACE:$<$<BOOL:${OHM_FEATURE_EIGEN}>:Eigen3::Eigen>>
)

add_test(NAME ohmtest COMMAND ohmtest --gtest_output=xml:test-reports/)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

# install(TARGETS ohmtest DESTINATION bin)


================================================
FILE: tests/ohmtest/CompressionTests.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <gtest/gtest.h>

#include <ohm/DefaultLayer.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/OccupancyMap.h>
#include <ohm/VoxelBlock.h>
#include <ohm/VoxelBlockCompressionQueue.h>

#include <logutil/LogUtil.h>

#include <chrono>

namespace
{
using Clock = std::chrono::high_resolution_clock;
}

TEST(Compression, Simple)
{
  ohm::VoxelBlockCompressionQueue compressor(true);  // Instantiate in test mode
  // Create a map in order to use the layout. DO NOT SET kCompressed. That would start a new compression object.
  ohm::OccupancyMap map(1.0, ohm::MapFlag::kNone);
  std::vector<ohm::VoxelBlock::Ptr> blocks;
  std::vector<uint8_t> compression_buffer;

  const size_t block_count = 10;
  // Create a set of blocks which we can register with the compression object.
  const ohm::MapLayer &layer = map.layout().layer(map.layout().occupancyLayer());
  const size_t layer_mem_size = layer.layerByteSize(map.regionVoxelDimensions());
  size_t uncompressed_size = layer_mem_size * block_count;
  for (size_t i = 0; i < block_count; ++i)
  {
    blocks.emplace_back();
    blocks[i].reset(new ohm::VoxelBlock(map.detail(), layer));
    compressor.push(blocks[i].get());
  }
  // Set the high water mark above the current allocation size.
  compressor.setHighTide((block_count + 1) * layer_mem_size);
  compressor.__tick(compression_buffer);

  // No compression should have occurred.
  std::cout << "allocated: " << logutil::Bytes(compressor.estimatedAllocationSize()) << std::endl;
  EXPECT_EQ(compressor.estimatedAllocationSize(), uncompressed_size);

  // Now lock all the buffers and set a zero high/low water mark. Everything should stay allocated.
  for (auto &block : blocks)
  {
    block->retain();
  }
  compressor.setHighTide(0);
  compressor.setLowTide(0);
  compressor.__tick(compression_buffer);
  EXPECT_EQ(compressor.estimatedAllocationSize(), uncompressed_size);

  // Now unlock and compress everything.
  for (auto &block : blocks)
  {
    block->release();
  }
  compressor.__tick(compression_buffer);
  const size_t compressed_size = compressor.estimatedAllocationSize();
  // All should compress the same.
  const size_t single_block_compressed_size = compressed_size / block_count;
  std::cout << "compressed: " << logutil::Bytes(compressed_size) << std::endl;
  std::cout << "single-block-compressed: " << logutil::Bytes(single_block_compressed_size) << std::endl;
  EXPECT_LT(compressor.estimatedAllocationSize(), uncompressed_size);
  for (auto &block : blocks)
  {
    EXPECT_FALSE((block->flags() & ohm::VoxelBlock::kFUncompressed));
    EXPECT_FALSE((block->flags() & ohm::VoxelBlock::kFLocked));
  }

  // Relock everything.
  for (auto &block : blocks)
  {
    block->retain();
    EXPECT_TRUE((block->flags() & ohm::VoxelBlock::kFUncompressed));
  }
  // Expect everything uncompressed again. Note we only run the tick to update the allocation size.
  compressor.__tick(compression_buffer);
  EXPECT_EQ(compressor.estimatedAllocationSize(), uncompressed_size);

  // Now leave the high water mark at zero, but raise the low water mark to keep everything allocated. We'll then lower
  // the low water mark and expect one block at a time to be released.
  // This may not scale with increased block_count as compressed size isn't zero.
  compressor.setLowTide(uncompressed_size + 1);
  for (auto &block : blocks)
  {
    block->release();
    EXPECT_TRUE((block->flags() & ohm::VoxelBlock::kFUncompressed));
  }
  compressor.__tick(compression_buffer);
  EXPECT_EQ(compressor.estimatedAllocationSize(), uncompressed_size);

  auto count_uncompressed_blocks = [](const std::vector<ohm::VoxelBlock::Ptr> &blocks) -> size_t {
    size_t count = 0;
    for (const auto &block : blocks)
    {
      if (block->flags() & ohm::VoxelBlock::kFUncompressed)
      {
        ++count;
      }
    }
    return count;
  };

  for (size_t i = block_count; i > 0; --i)
  {
    // Lower water mark.
    compressor.setLowTide(layer_mem_size * i);
    compressor.__tick(compression_buffer);
    const size_t uncompressed_count = count_uncompressed_blocks(blocks);
    EXPECT_EQ(uncompressed_count, i - 1);
    EXPECT_LT(compressor.estimatedAllocationSize(), layer_mem_size * i);
    EXPECT_GE(compressor.estimatedAllocationSize(), layer_mem_size * uncompressed_count);
  }

  // Ensure the blocks are releases.
  blocks.clear();
  compressor.__tick(compression_buffer);
}


TEST(Compression, HighLoad)
{
  ohm::VoxelBlockCompressionQueue compressor(true);  // Instantiate in test mode
  ohm::MapLayout layout;
  ohm::addCovariance(layout);
  // Create a map in order to use the layout. DO NOT SET kCompressed. That would start a new compression object.
  ohm::OccupancyMap map(1.0, ohm::MapFlag::kNone, layout);
  // Ensure the covariance layer is present.
  std::vector<ohm::VoxelBlock::Ptr> blocks;
  std::vector<uint8_t> compression_buffer;

  const size_t block_count = 5000;
  // Create a set of blocks which we can register with the compression object.
  ASSERT_NE(map.layout().covarianceLayer(), -1);
  const ohm::MapLayer &layer = map.layout().layer(map.layout().covarianceLayer());
  const size_t layer_mem_size = layer.layerByteSize(map.regionVoxelDimensions());
  size_t uncompressed_size = layer_mem_size * block_count;
  for (size_t i = 0; i < block_count; ++i)
  {
    blocks.emplace_back();
    blocks[i].reset(new ohm::VoxelBlock(map.detail(), layer));
    compressor.push(blocks[i].get());
  }
  // Set the high water mark above the current allocation size.
  compressor.setHighTide((block_count + 1) * layer_mem_size);
  auto start = Clock::now();
  compressor.__tick(compression_buffer);
  auto end = Clock::now();

  std::cout << "Initial tick: " << (end - start) << std::endl;

  // No compression should have occurred.
  std::cout << "allocated: " << logutil::Bytes(compressor.estimatedAllocationSize()) << std::endl;
  EXPECT_EQ(compressor.estimatedAllocationSize(), uncompressed_size);

  // Set a zero tolerance low water mark and time the tick.
  compressor.setHighTide(0);
  compressor.setLowTide(0);
  start = Clock::now();
  compressor.__tick(compression_buffer);
  end = Clock::now();
  EXPECT_LT(compressor.estimatedAllocationSize(), uncompressed_size);
  std::cout << "Compression tick: " << (end - start) << std::endl;

  // Ensure the blocks are releases.
  blocks.clear();
  start = Clock::now();
  compressor.__tick(compression_buffer);
  end = Clock::now();
  EXPECT_EQ(compressor.estimatedAllocationSize(), 0);
  std::cout << "Release tick: " << (end - start) << std::endl;
}


================================================
FILE: tests/ohmtest/CopyTests.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <ohm/Aabb.h>
#include <ohm/CopyUtil.h>
#include <ohm/Key.h>
#include <ohm/LineQuery.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/VoxelData.h>

#include <ohmtools/OhmCloud.h>
#include <ohmtools/OhmGen.h>

#include <logutil/LogUtil.h>

#include <ohmutil/Profile.h>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <random>

#include <gtest/gtest.h>

#include "ohmtestcommon/OhmTestUtil.h"

namespace maptests
{
TEST(Copy, Clone)
{
  ohm::OccupancyMap map(0.25);

  // Generate occupancy.
  const double box_size = 5.0;
  ohmgen::boxRoom(map, glm::dvec3(-box_size), glm::dvec3(box_size));

  // Clone the map
  const std::unique_ptr<ohm::OccupancyMap> map_copy(map.clone());

  // Compare maps.
  ohmtestutil::compareMaps(*map_copy, map, ohmtestutil::kCfCompareExtended);
}


TEST(Copy, CloneSubmap)
{
  const glm::dvec3 clone_min(0);
  const glm::dvec3 clone_max(4.5);
  ohm::OccupancyMap map(0.2);

  // Generate occupancy.
  const double box_size = 5.0;
  ohmgen::boxRoom(map, glm::dvec3(-box_size), glm::dvec3(box_size));

  // Clone the map
  const std::unique_ptr<ohm::OccupancyMap> map_copy(map.clone(clone_min, clone_max));

  // Compare maps.
  ohmtestutil::compareMaps(*map_copy, map, clone_min, clone_max, ohmtestutil::kCfCompareExtended);
}


TEST(Copy, Copy)
{
  // Test copying utilities.
  ohm::OccupancyMap map(0.2);
  ohm::OccupancyMap dst_map(map.resolution());

  // Generate occupancy.
  const double box_size = 5.0;
  ohmgen::boxRoom(map, glm::dvec3(-box_size), glm::dvec3(box_size));

  // Validate we can't copy into a map of differing resolution...
  EXPECT_FALSE(ohm::canCopy(ohm::OccupancyMap(map.resolution() * 2), map));
  // or of different region sizes...
  EXPECT_FALSE(ohm::canCopy(ohm::OccupancyMap(map.resolution(), glm::u8vec3(1, 1, 1)), map));
  // or at different origins
  dst_map.setOrigin(glm::dvec3(1, 2, 3));
  EXPECT_FALSE(ohm::canCopy(dst_map, map));

  // Rest dst_map origin for copy test.
  dst_map.setOrigin(map.origin());
  EXPECT_TRUE(ohm::canCopy(dst_map, map));

  // Copy and compare with no filter.
  EXPECT_TRUE(ohm::copyMap(dst_map, map));

  // Compare maps.
  ohmtestutil::compareMaps(dst_map, map, ohmtestutil::kCfCompareExtended);
}


TEST(Copy, CopySubmap)
{
  // Test copying utilities.
  const glm::dvec3 copy_min(0);
  const glm::dvec3 copy_max(4.5);
  ohm::OccupancyMap map(0.2);
  ohm::OccupancyMap dst_map(map.resolution());

  // Generate occupancy.
  const double box_size = 5.0;
  ohmgen::boxRoom(map, glm::dvec3(-box_size), glm::dvec3(box_size));

  // Copy and compare with no filter.
  EXPECT_TRUE(ohm::copyMap(dst_map, map, ohm::copyFilterExtents(copy_min, copy_max)));
  // Compare maps.
  ohmtestutil::compareMaps(dst_map, map, copy_min, copy_max, ohmtestutil::kCfCompareExtended);
}

TEST(Copy, CopyUpdated)
{
  // Test copying only touched voxels.
  // Test copying utilities.
  const glm::dvec3 copy_min(0);
  const glm::dvec3 copy_max(4.5);
  ohm::OccupancyMap map(0.2);
  ohm::OccupancyMap dst_map(map.resolution());

  // Generate occupancy.
  const double box_size = 5.0;
  ohmgen::boxRoom(map, glm::dvec3(-box_size), glm::dvec3(box_size));

  // Get the map stamp now.
  auto stamp = map.stamp();

  // Invoke the copy. Expect nothing to be copied.
  EXPECT_TRUE(ohm::copyMap(dst_map, map, ohm::copyFilterStamp(stamp)));
  EXPECT_EQ(dst_map.regionCount(), 0);

  // Make some updates to a single region.
  ohm::Key key(0, 0, 0, 0, 0, 0);
  ohm::Voxel<float> occupancy(&map, map.layout().occupancyLayer());
  ASSERT_TRUE(occupancy.isLayerValid());

  for (uint8_t z = 0; z < map.regionVoxelDimensions().z; ++z)
  {
    for (uint8_t y = 0; y < map.regionVoxelDimensions().y; ++y)
    {
      for (uint8_t x = 0; x < map.regionVoxelDimensions().x; ++x)
      {
        key.setLocalKey(glm::u8vec3(x, y, z));
        occupancy.setKey(key);
        ASSERT_TRUE(occupancy.isValid());
        ohm::integrateHit(occupancy);
      }
    }
  }

  // To finalise the map stamping changes, we need to change regions or invalid the Voxel object.
  occupancy.commit();

  // Copy only updated regions.
  EXPECT_TRUE(ohm::copyMap(dst_map, map, ohm::copyFilterStamp(stamp)));

  // Validate the target map only has the region we've updated
  EXPECT_EQ(dst_map.regionCount(), 1);
  EXPECT_NE(dst_map.region(key.regionKey(), false), nullptr);

  // Validate the region matches.
  // Lock the occupancy layers.
  ohm::Voxel<const float> src_occupancy(&map, map.layout().occupancyLayer(), key);
  ASSERT_TRUE(src_occupancy.isValid());
  ohm::Voxel<const float> dst_occupancy(&dst_map, dst_map.layout().occupancyLayer(), key);
  ASSERT_TRUE(dst_occupancy.isValid());

  // Compare voxels in the region.
  for (uint8_t z = 0; z < map.regionVoxelDimensions().z; ++z)
  {
    for (uint8_t y = 0; y < map.regionVoxelDimensions().y; ++y)
    {
      for (uint8_t x = 0; x < map.regionVoxelDimensions().x; ++x)
      {
        key.setLocalKey(glm::u8vec3(x, y, z));
        src_occupancy.setKey(key);
        dst_occupancy.setKey(key);
        ASSERT_TRUE(src_occupancy.isValid());
        ASSERT_TRUE(dst_occupancy.isValid());
        EXPECT_EQ(dst_occupancy.data(), src_occupancy.data());
      }
    }
  }
}

TEST(Copy, LayerFilter)
{
  // Test copying only selected layers.
  ohm::MapFlag map_flags = ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal | ohm::MapFlag::kTouchTime;
  ohm::OccupancyMap src_map(0.1, map_flags);

  ohm::Voxel<float> src_occupancy(&src_map, src_map.layout().occupancyLayer());
  ohm::Voxel<ohm::VoxelMean> src_mean(&src_map, src_map.layout().meanLayer());
  ohm::Voxel<float> src_traversal(&src_map, src_map.layout().traversalLayer());
  ohm::Voxel<float> src_touch_time(&src_map, src_map.layout().layerIndex(ohm::default_layer::touchTimeLayerName()));

  ASSERT_TRUE(src_occupancy.isLayerValid());
  ASSERT_TRUE(src_mean.isLayerValid());
  ASSERT_TRUE(src_traversal.isLayerValid());
  ASSERT_TRUE(src_touch_time.isLayerValid());

  const float occupancy_value = -1.0f;
  const ohm::VoxelMean mean_value = { 1, 2 };
  const float traversal_value = 1.0f;
  const uint32_t touch_time_value = 42u;

  // Populate a single chunk with known values.
  ohm::Key key(0, 0, 0, 0, 0, 0);
  for (int z = 0; z < src_map.regionVoxelDimensions().z; ++z)
  {
    key.setLocalAxis(2, z);
    for (int y = 0; y < src_map.regionVoxelDimensions().y; ++y)
    {
      key.setLocalAxis(1, y);
      for (int x = 0; x < src_map.regionVoxelDimensions().x; ++x)
      {
        key.setLocalAxis(0, x);
        ohm::setVoxelKey(key, src_occupancy, src_mean, src_traversal, src_touch_time);
        ASSERT_TRUE(src_occupancy.isValid());
        ASSERT_TRUE(src_mean.isValid());
        ASSERT_TRUE(src_traversal.isValid());
        ASSERT_TRUE(src_touch_time.isValid());

        src_occupancy.write(occupancy_value);
        src_mean.write(mean_value);
        src_traversal.write(traversal_value);
        src_touch_time.write(touch_time_value);
      }
    }
  }
  src_occupancy.commit();
  src_mean.commit();
  src_traversal.commit();
  src_touch_time.commit();

  // Create a map to copy into omitting the TouchTime layer.
  map_flags &= ~ohm::MapFlag::kTouchTime;
  ohm::OccupancyMap dst_map(src_map.resolution(), map_flags);

  // Copy all layers except occupancy.
  std::vector<std::string> copy_layers = { ohm::default_layer::meanLayerName(),
                                           ohm::default_layer::traversalLayerName(),
                                           ohm::default_layer::touchTimeLayerName() };

  ohm::copyMap(dst_map, src_map, {}, ohm::copyLayersFilter(copy_layers));

  // Validate all voxels match except occupancy values.
  ohm::Voxel<const float> dst_occupancy(&dst_map, dst_map.layout().occupancyLayer());
  ohm::Voxel<const ohm::VoxelMean> dst_mean(&dst_map, dst_map.layout().meanLayer());
  ohm::Voxel<const float> dst_traversal(&dst_map, dst_map.layout().traversalLayer());
  // Not using touch time in the dst_map.
  ASSERT_EQ(dst_map.layout().layerIndex(ohm::default_layer::touchTimeLayerName()), -1);

  ASSERT_TRUE(dst_occupancy.isLayerValid());
  ASSERT_TRUE(dst_mean.isLayerValid());
  ASSERT_TRUE(dst_traversal.isLayerValid());

  // Now compare values.
  key = ohm::Key(0, 0, 0, 0, 0, 0);
  for (int z = 0; z < dst_map.regionVoxelDimensions().z; ++z)
  {
    key.setLocalAxis(2, z);
    for (int y = 0; y < dst_map.regionVoxelDimensions().y; ++y)
    {
      key.setLocalAxis(1, y);
      for (int x = 0; x < dst_map.regionVoxelDimensions().x; ++x)
      {
        key.setLocalAxis(0, x);
        ohm::setVoxelKey(key, dst_occupancy, dst_mean, dst_traversal);
        ASSERT_TRUE(dst_occupancy.isValid());
        ASSERT_TRUE(dst_mean.isValid());
        ASSERT_TRUE(dst_traversal.isValid());

        ASSERT_NE(dst_occupancy.data(), occupancy_value);
        ASSERT_EQ(dst_occupancy.data(), ohm::unobservedOccupancyValue());
        ASSERT_EQ(dst_mean.data().coord, mean_value.coord);
        ASSERT_EQ(dst_mean.data().count, mean_value.count);
        ASSERT_EQ(dst_traversal.data(), traversal_value);
      }
    }
  }
  dst_occupancy.reset();
  dst_mean.reset();
  dst_traversal.reset();

  // Clear the map and copy again using the negated filter.
  dst_map.clear();
  ohm::copyMap(dst_map, src_map, {}, ohm::copyFilterNegate(ohm::copyLayersFilter(copy_layers)));

  dst_occupancy = ohm::Voxel<const float>(&dst_map, dst_map.layout().occupancyLayer());
  dst_mean = ohm::Voxel<const ohm::VoxelMean>(&dst_map, dst_map.layout().meanLayer());
  dst_traversal = ohm::Voxel<const float>(&dst_map, dst_map.layout().traversalLayer());
  ASSERT_TRUE(dst_occupancy.isLayerValid());
  ASSERT_TRUE(dst_mean.isLayerValid());
  ASSERT_TRUE(dst_traversal.isLayerValid());

  // Compare again.
  key = ohm::Key(0, 0, 0, 0, 0, 0);
  for (int z = 0; z < dst_map.regionVoxelDimensions().z; ++z)
  {
    key.setLocalAxis(2, z);
    for (int y = 0; y < dst_map.regionVoxelDimensions().y; ++y)
    {
      key.setLocalAxis(1, y);
      for (int x = 0; x < dst_map.regionVoxelDimensions().x; ++x)
      {
        key.setLocalAxis(0, x);
        ohm::setVoxelKey(key, dst_occupancy, dst_mean, dst_traversal);
        ASSERT_TRUE(dst_occupancy.isValid());
        ASSERT_TRUE(dst_mean.isValid());
        ASSERT_TRUE(dst_traversal.isValid());

        ASSERT_EQ(dst_occupancy.data(), occupancy_value);
        ASSERT_NE(dst_mean.data().coord, mean_value.coord);
        ASSERT_NE(dst_mean.data().count, mean_value.count);
        ASSERT_EQ(dst_mean.data().coord, 0);
        ASSERT_EQ(dst_mean.data().count, 0);
        ASSERT_NE(dst_traversal.data(), traversal_value);
        ASSERT_EQ(dst_traversal.data(), 0);
      }
    }
  }
}
}  // namespace maptests


================================================
FILE: tests/ohmtest/IncidentsTests.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <gtest/gtest.h>

#include <ohm/DefaultLayer.h>
#include <ohm/Key.h>
#include <ohm/NdtMap.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapperNdt.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/Voxel.h>
#include <ohm/VoxelIncident.h>
#include <ohm/VoxelMean.h>

#include <glm/gtx/norm.hpp>
#include <glm/vec3.hpp>

#include <random>

namespace incidents
{
void testIncidentNormals(ohm::OccupancyMap &map, ohm::RayMapper &mapper)
{
  const unsigned iterations = 10;
  const unsigned ray_count = 1000u;
  std::vector<glm::dvec3> rays;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-1.0, 1.0);

  // Set the map origin to avoid (0, 0, 0) being on a voxel boundary.
  map.setOrigin(glm::dvec3(-0.5 * map.resolution()));

  ASSERT_TRUE(map.incidentNormalEnabled());
  ASSERT_TRUE(map.voxelMeanEnabled());

  rays.reserve(2 * ray_count);
  for (unsigned i = 0; i < iterations; ++i)
  {
    unsigned expected_packed = 0;
    rays.clear();
    for (unsigned r = 0; r < ray_count; ++r)
    {
      // Sample is at the origin. We'll build random rays around that.
      glm::dvec3 origin;
      do
      {
        origin = glm::dvec3(uniform(rng), uniform(rng), uniform(rng));
      } while (glm::length2(origin) < 1e-6);  // Make sure it's not degenerate.
      // Normalise the origin ray, then expand it out to be larger than a single voxel.
      origin = glm::normalize(origin);
      origin *= map.resolution() * 3;
      rays.emplace_back(origin);
      rays.emplace_back(glm::dvec3(0));

      // Do the same calculation made by the mapper, but without encoding.
      expected_packed = ohm::updateIncidentNormal(expected_packed, origin, r);
    }
    // Now use the ray mapper
    mapper.integrateRays(rays.data(), rays.size());

    // Check the result.
    ohm::Voxel<uint32_t> incident_voxel(&map, map.layout().layerIndex(ohm::default_layer::incidentNormalLayerName()));
    ASSERT_TRUE(incident_voxel.isLayerValid());
    incident_voxel.setKey(map.voxelKey(glm::dvec3(0)));
    ASSERT_TRUE(incident_voxel.isValid());

    EXPECT_EQ(incident_voxel.data(), expected_packed);

    // Clear the incident normal
    incident_voxel.write(0);
    EXPECT_EQ(incident_voxel.data(), 0);

    // Also clear the voxel mean as we use the sample count from there.
    ohm::Voxel<ohm::VoxelMean> mean_voxel(&map, map.layout().meanLayer());
    ASSERT_TRUE(mean_voxel.isLayerValid());
    mean_voxel.setKey(map.voxelKey(glm::dvec3(0)));
    ASSERT_TRUE(mean_voxel.isValid());

    mean_voxel.write(ohm::VoxelMean{});
    EXPECT_EQ(mean_voxel.data().count, 0);
  }
}

TEST(Incident, WithOccupancy)
{
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kIncidentNormal);
  ohm::RayMapperOccupancy mapper(&map);
  testIncidentNormals(map, mapper);
}

TEST(Incident, WithNdt)
{
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kIncidentNormal);
  ohm::NdtMap ndt_map(&map, true);
  ohm::RayMapperNdt mapper(&ndt_map);
  testIncidentNormals(map, mapper);
}
}  // namespace incidents


================================================
FILE: tests/ohmtest/KeyTests.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <ohm/Key.h>
#include <ohm/MapChunk.h>
#include <ohm/MapCoord.h>
#include <ohm/OccupancyMap.h>

#include <cstdio>
#include <vector>

#include <gtest/gtest.h>

// TODO: Convert from legacy test layout to use gtest macros and features.

using namespace ohm;

namespace keytests
{
struct SampleInfo
{
  glm::dvec3 point;
  ohm::Key expected_key;
};

static const float kEpsilon = 0.1f;

void addSample(std::vector<SampleInfo> &samples, ohm::OccupancyMap &map, const glm::dvec3 p)
{
  SampleInfo sample;

  sample.point = p;
  sample.expected_key = map.voxelKey(p);

  samples.push_back(sample);
}

void generateInternalSamples(std::vector<SampleInfo> &samples, OccupancyMap &map)
{
  Key origin_key;
  Key key;
  glm::dvec3 region_spatial_dim;
  glm::dvec3 region_min, region_max;

  // We'll use the central region as a base line and generate keys wholy within the bounds of this reagion.
  origin_key = map.voxelKey(map.origin());
  region_spatial_dim = map.regionSpatialResolution();

  region_min = map.origin() - 0.5 * region_spatial_dim;
  region_max = map.origin() + 0.5 * region_spatial_dim;

  // For internal samples we start by adding points at the corners of the region. Note the
  // maximal corners should be for the next region so we remove epsilon.
  addSample(samples, map, glm::vec3(region_min.x, region_min.y, region_min.z));
  addSample(samples, map, glm::vec3(region_max.x - kEpsilon, region_min.y, region_min.z));
  addSample(samples, map, glm::vec3(region_min.x, region_max.y - kEpsilon, region_min.z));
  addSample(samples, map, glm::vec3(region_max.x - kEpsilon, region_max.y - kEpsilon, region_min.z));
  addSample(samples, map, glm::vec3(region_min.x, region_min.y, region_max.z - kEpsilon));
  addSample(samples, map, glm::vec3(region_max.x - kEpsilon, region_min.y, region_max.z - kEpsilon));
  addSample(samples, map, glm::vec3(region_min.x, region_max.y - kEpsilon, region_max.z - kEpsilon));
  addSample(samples, map, glm::vec3(region_max.x - kEpsilon, region_max.y - kEpsilon, region_max.z - kEpsilon));

  // Add the map origin.
  addSample(samples, map, map.origin());

  key.setRegionKey(origin_key.regionKey());
  // Now add the centre of each voxel within the region.
  for (int z = 0; z < map.regionVoxelDimensions().z; ++z)
  {
    key.setLocalAxis(2, z);
    for (int y = 0; y < map.regionVoxelDimensions().y; ++y)
    {
      key.setLocalAxis(1, y);
      for (int x = 0; x < map.regionVoxelDimensions().x; ++x)
      {
        key.setLocalAxis(0, x);
        addSample(samples, map, map.voxelCentreGlobal(key));
      }
    }
  }
}

void generateExternalSamples(std::vector<SampleInfo> &samples, OccupancyMap &map)
{
  glm::dvec3 region_spatial_dim;
  glm::dvec3 region_min, region_max;

  // We'll use the central region as a base line and generate keys wholy within the bounds of this reagion.
  region_spatial_dim = map.regionSpatialResolution();

  region_min = map.origin() - 0.5 * region_spatial_dim;
  region_max = map.origin() + 0.5 * region_spatial_dim;

  // For external samples we start with the maximal corners (which should lie in adjacent
  // regions), and the minimal corners with an epsilon value shift.
  addSample(samples, map, glm::vec3(region_min.x - kEpsilon, region_min.y - kEpsilon, region_min.z - kEpsilon));
  addSample(samples, map, glm::vec3(region_max.x, region_min.y - kEpsilon, region_min.z - kEpsilon));
  addSample(samples, map, glm::vec3(region_min.x - kEpsilon, region_max.y, region_min.z - kEpsilon));
  addSample(samples, map, glm::vec3(region_max.x, region_max.y, region_min.z - kEpsilon));
  addSample(samples, map, glm::vec3(region_min.x - kEpsilon, region_min.y - kEpsilon, region_max.z));
  addSample(samples, map, glm::vec3(region_max.x, region_min.y - kEpsilon, region_max.z));
  addSample(samples, map, glm::vec3(region_min.x - kEpsilon, region_max.y, region_max.z));
  addSample(samples, map, glm::vec3(region_max.x, region_max.y, region_max.z));
}

bool testSamples(const std::vector<SampleInfo> &samples, OccupancyMap &map, const Key &reference_key,
                 bool expect_region_match)
{
  Key key;
  glm::dvec3 coord;
  glm::dvec3 separation;
  bool tests_ok = true;
  enum FailureFlag
  {
    kFfZero = 0,
    kFfRegion = (1 << 0),
    kFfLocal = (1 << 1),
    kFfCoord = (1 << 2),
    kFfExpectedKey = (1 << 3)
  };
  unsigned failures;

  // Validate the internal samples.
  // Don't use range based loop so I can adjust 'i' in the debugger and retest failures.
  for (size_t i = 0; i < samples.size(); ++i)
  {
    const SampleInfo &sample = samples[i];
    failures = 0u;
    key = map.voxelKey(sample.point);
    coord = map.voxelCentreGlobal(key);
    separation = sample.point - coord;

    // Validate results.
    // 1. Region check.
    if (expect_region_match && key.regionKey() != reference_key.regionKey() ||
        !expect_region_match && key.regionKey() == reference_key.regionKey())
    {
      // Region mismatch.
      failures |= kFfRegion;
    }

    if (key.localKey().x >= map.regionVoxelDimensions().x || key.localKey().y >= map.regionVoxelDimensions().y ||
        key.localKey().z >= map.regionVoxelDimensions().z)
    {
      failures |= kFfLocal;
    }

    if (separation.x < -0.5 * map.resolution() || separation.x >= 0.5 * map.resolution() ||
        separation.y < -0.5 * map.resolution() || separation.y >= 0.5 * map.resolution() ||
        separation.z < -0.5 * map.resolution() || separation.z >= 0.5 * map.resolution())
    {
      failures |= kFfCoord;
    }

    if (key != sample.expected_key)
    {
      failures |= kFfExpectedKey;
    }

    if (failures)
    {
      tests_ok = false;

      fprintf(stderr, "Sample/key failure: (%g %g %g) : [(%d %d %d) (%u %u %u)]]n", sample.point.x, sample.point.y,
              sample.point.z, int(sample.expected_key.localKey().x), int(sample.expected_key.localKey().y),
              int(sample.expected_key.localKey().z), sample.expected_key.regionKey().x,
              sample.expected_key.regionKey().y, sample.expected_key.regionKey().z);

      if (failures & kFfRegion)
      {
        fprintf(stderr, "  Incorrect region: (%u %u %u)\n", key.regionKey().x, key.regionKey().y, key.regionKey().z);
      }

      if (failures & kFfLocal)
      {
        fprintf(stderr, "  Invalid local index: (%i %i %i) Max (%i %i %i)\n", int(key.localKey().x),
                int(key.localKey().y), int(key.localKey().z), int(map.regionVoxelDimensions().x),
                int(map.regionVoxelDimensions().y), int(map.regionVoxelDimensions().z));
      }

      if (failures & kFfCoord)
      {
        fprintf(stderr, "  Voxel centre too far from sample (%g): S:(%g %g %g) V:(%g %g %g) D:(%g %g %g)\n",
                map.resolution(), sample.point.x, sample.point.y, sample.point.z, coord.x, coord.y, coord.z,
                separation.x, separation.y, separation.z);
      }

      if (failures & kFfExpectedKey)
      {
        fprintf(stderr, "  Unexpected key: [(%d %d %d) (%u %u %u)]]\n", int(key.localKey().x), int(key.localKey().y),
                int(key.localKey().z), key.regionKey().x, key.regionKey().y, key.regionKey().z);
      }
    }
  }

  return tests_ok;
}

TEST(Keys, Indexing)
{
  // Define a (dummy) map to work with.
  OccupancyMap map(0.3f);
  Key origin_key;
  std::vector<SampleInfo> internal_samples;
  std::vector<SampleInfo> external_samples;

  // We'll use the central region as a base line and generate keys wholy within the bounds of this reagion.
  origin_key = map.voxelKey(map.origin());

  // Now generate a list of points which are expected to line inside and a list of points
  // expected to lie outside the central region.
  generateInternalSamples(internal_samples, map);
  generateExternalSamples(external_samples, map);

  EXPECT_TRUE(testSamples(internal_samples, map, origin_key, true));
  EXPECT_TRUE(testSamples(external_samples, map, origin_key, false));
}

// Test conversion from voxel key to centre and back.
TEST(Keys, Conversion)
{
  const double resolution = 0.25;
  const glm::u8vec3 region_size(16);
  OccupancyMap map(resolution, region_size);
  Key key, test_key;
  glm::dvec3 v;

  // Create a set of rays which will densely populate a single region.
  for (int z = 0; z < region_size.z; ++z)
  {
    key.setLocalAxis(2, z);
    for (int y = 0; y < region_size.y; ++y)
    {
      key.setLocalAxis(1, y);
      for (int x = 0; x < region_size.x; ++x)
      {
        key.setLocalAxis(0, x);
        v = map.voxelCentreGlobal(key);
        test_key = map.voxelKey(v);
        EXPECT_EQ(key, test_key);
      }
    }
  }
}

template <typename REAL>
void quantisationTest(const REAL coord, const int region_size, const REAL voxel_resolution)
{
  int r = ohm::pointToRegionCoord(coord, region_size * voxel_resolution);
  REAL rmin = (ohm::regionCentreCoord(r, region_size * voxel_resolution) - REAL(0.5) * region_size * voxel_resolution);
  int v = ohm::pointToRegionVoxel(coord - rmin, voxel_resolution, region_size * voxel_resolution);
  EXPECT_LT(v, region_size);
}

// Test conversion from voxel key to centre and back.
TEST(Keys, Quantisation)
{
  const int region_size = 32;
  const double resolution = 0.4;
  const float resolution_f = float(resolution);

  // Attempting to quantise a coordinate at the upper boundary of region -1 would generate a bad voxel coordinate.
  // This is testing that this case is fixed.
  {
    // const float badValue = -6.4000005722045898f;
    const float epsilon = 5e-7f;
    const float bad_value = region_size * resolution_f * -0.5f - epsilon;
    quantisationTest(bad_value, region_size, resolution_f);
  }

  {
    const double epsilon = 1e-15;
    const double bad_value = region_size * resolution * -0.5 - epsilon;
    quantisationTest(bad_value, region_size, resolution);
  }
}
}  // namespace keytests


================================================
FILE: tests/ohmtest/LayoutTests.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <ohm/DefaultLayer.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/OccupancyMap.h>
#include <ohm/VoxelLayout.h>
#include <ohm/VoxelMean.h>

#include <logutil/LogUtil.h>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <random>

#include <gtest/gtest.h>
#include "ohmtestcommon/OhmTestUtil.h"

using namespace ohm;

TEST(Layout, Basic)
{
  OccupancyMap map(1.0);

  const MapLayout &layout = map.layout();

  EXPECT_EQ(layout.layerCount(), 1);
  const MapLayer *occupancy_layer = layout.layer(default_layer::occupancyLayerName());
  ASSERT_NE(occupancy_layer, nullptr);

  EXPECT_EQ(occupancy_layer->layerIndex(), 0);

  EXPECT_EQ(occupancy_layer->voxelByteSize(), sizeof(float));

  VoxelLayoutConst occupancy_voxel = occupancy_layer->voxelLayout();

  EXPECT_EQ(occupancy_voxel.memberCount(), 1);
  EXPECT_STREQ(occupancy_voxel.memberName(0), default_layer::occupancyLayerName());
  EXPECT_EQ(occupancy_voxel.memberOffset(0), 0);
  EXPECT_EQ(occupancy_voxel.memberSize(0), sizeof(float));
}


TEST(Layout, DefaultLayers)
{
  OccupancyMap map(1.0);

  MapLayout modified_layout = map.layout();

  EXPECT_EQ(modified_layout.layerCount(), 1);
  EXPECT_EQ(modified_layout.occupancyLayer(), 0);
  EXPECT_EQ(modified_layout.meanLayer(), -1);
  EXPECT_EQ(modified_layout.clearanceLayer(), -1);
  EXPECT_EQ(modified_layout.layer(default_layer::covarianceLayerName()), nullptr);

  addVoxelMean(modified_layout);
  EXPECT_EQ(modified_layout.layerCount(), 2);
  EXPECT_EQ(modified_layout.occupancyLayer(), 0);
  EXPECT_EQ(modified_layout.meanLayer(), 1);
  EXPECT_EQ(modified_layout.clearanceLayer(), -1);
  EXPECT_EQ(modified_layout.layer(default_layer::covarianceLayerName()), nullptr);

  addClearance(modified_layout);
  EXPECT_EQ(modified_layout.layerCount(), 3);
  EXPECT_EQ(modified_layout.occupancyLayer(), 0);
  EXPECT_EQ(modified_layout.meanLayer(), 1);
  EXPECT_EQ(modified_layout.clearanceLayer(), 2);
  EXPECT_EQ(modified_layout.layer(default_layer::covarianceLayerName()), nullptr);

  addCovariance(modified_layout);
  EXPECT_EQ(modified_layout.layerCount(), 4);
  EXPECT_EQ(modified_layout.occupancyLayer(), 0);
  EXPECT_EQ(modified_layout.meanLayer(), 1);
  EXPECT_EQ(modified_layout.clearanceLayer(), 2);
  EXPECT_NE(modified_layout.layer(default_layer::covarianceLayerName()), nullptr);
  EXPECT_EQ(modified_layout.layer(default_layer::covarianceLayerName())->layerIndex(), 3);

  // Update the map.
  map.updateLayout(modified_layout);

  // Get the updated layout.
  const MapLayout &layout = map.layout();
  const MapLayer *occupancy_layer = layout.layer(default_layer::occupancyLayerName());
  const MapLayer *mean_layer = layout.layer(default_layer::meanLayerName());
  const MapLayer *clearance_layer = layout.layer(default_layer::clearanceLayerName());
  const MapLayer *covariance_layer = layout.layer(default_layer::covarianceLayerName());
  ASSERT_NE(occupancy_layer, nullptr);
  ASSERT_NE(mean_layer, nullptr);
  ASSERT_NE(clearance_layer, nullptr);

  EXPECT_EQ(occupancy_layer->layerIndex(), 0);
  EXPECT_EQ(mean_layer->layerIndex(), 1);
  EXPECT_EQ(clearance_layer->layerIndex(), 2);

  EXPECT_EQ(occupancy_layer->voxelByteSize(), sizeof(float));
  EXPECT_EQ(mean_layer->voxelByteSize(), sizeof(VoxelMean));
  EXPECT_EQ(clearance_layer->voxelByteSize(), sizeof(float));

  VoxelLayoutConst occupancy_voxel = occupancy_layer->voxelLayout();
  VoxelLayoutConst mean_voxel = mean_layer->voxelLayout();
  VoxelLayoutConst clearance_voxel = clearance_layer->voxelLayout();
  VoxelLayoutConst covariance_voxel = covariance_layer->voxelLayout();

  EXPECT_EQ(occupancy_voxel.memberCount(), 1);
  EXPECT_STREQ(occupancy_voxel.memberName(0), default_layer::occupancyLayerName());
  EXPECT_EQ(occupancy_voxel.memberOffset(0), 0);
  EXPECT_EQ(occupancy_voxel.memberSize(0), sizeof(float));

  EXPECT_EQ(mean_voxel.memberCount(), 2);
  EXPECT_STREQ(mean_voxel.memberName(0), "coord");
  EXPECT_STREQ(mean_voxel.memberName(1), "count");
  EXPECT_EQ(mean_voxel.memberOffset(0), 0);
  EXPECT_EQ(mean_voxel.memberSize(0), sizeof(uint32_t));
  EXPECT_EQ(mean_voxel.memberOffset(1), sizeof(uint32_t));
  EXPECT_EQ(mean_voxel.memberSize(1), sizeof(uint32_t));

  EXPECT_EQ(clearance_voxel.memberCount(), 1);
  EXPECT_STREQ(clearance_voxel.memberName(0), default_layer::clearanceLayerName());
  EXPECT_EQ(clearance_voxel.memberOffset(0), 0);
  EXPECT_EQ(clearance_voxel.memberSize(0), sizeof(float));

  EXPECT_EQ(covariance_voxel.memberCount(), 6);
  for (unsigned i = 0; i < 6; ++i)
  {
    EXPECT_EQ(covariance_voxel.memberOffset(i), i * sizeof(float));
    EXPECT_EQ(covariance_voxel.memberSize(i), sizeof(float));
  }
}


TEST(Layout, Filter)
{
  OccupancyMap map(1.0);

  MapLayout layout = map.layout();

  addClearance(layout);

  EXPECT_EQ(layout.layerCount(), 2);
  const MapLayer *occupancy_layer = layout.layer(default_layer::occupancyLayerName());
  ASSERT_NE(occupancy_layer, nullptr);

  // Remove the occupancy layer.
  layout.filterLayers({ default_layer::clearanceLayerName() });

  EXPECT_EQ(layout.layerCount(), 1);
  occupancy_layer = layout.layer(default_layer::occupancyLayerName());
  const MapLayer *clearance_layer = layout.layer(default_layer::clearanceLayerName());
  ASSERT_EQ(occupancy_layer, nullptr);
  ASSERT_NE(clearance_layer, nullptr);

  EXPECT_EQ(clearance_layer->layerIndex(), 0);

  EXPECT_EQ(clearance_layer->voxelByteSize(), sizeof(float));

  VoxelLayoutConst clearance_voxel = clearance_layer->voxelLayout();

  EXPECT_EQ(clearance_voxel.memberCount(), 1);
  EXPECT_STREQ(clearance_voxel.memberName(0), default_layer::clearanceLayerName());
  EXPECT_EQ(clearance_voxel.memberOffset(0), 0);
  EXPECT_EQ(clearance_voxel.memberSize(0), sizeof(float));

  layout.clear();
  EXPECT_EQ(layout.layerCount(), 0);
}


TEST(Layout, Structure)
{
  // Structure with variable packing and alignment.
  // We are simulating the following structure
  // struct LayoutTestStruct
  // {
  //   uint8_t m1_0;
  //   uint8_t m1_1;
  //   uint16_t m2_3;
  //   uint16_t m2_4;
  //   uint32_t m4_5;
  //   uint64_t m8_6;
  //   uint8_t m1_7;
  // };

  struct MemberInfo
  {
    const char *name;
    ohm::DataType::Type type;
    unsigned expected_offset;
    unsigned expected_cumulative_size;
  };

  static const MemberInfo members[] =  //
    {
      { "0", ohm::DataType::kUInt8, 0, 4 },     //
      { "1", ohm::DataType::kUInt8, 1, 4 },     //
      { "2", ohm::DataType::kUInt16, 2, 4 },    //
      { "3", ohm::DataType::kUInt16, 4, 8 },    //
      { "4", ohm::DataType::kUInt32, 8, 16 },   //
      { "5", ohm::DataType::kUInt64, 16, 24 },  //
      { "6", ohm::DataType::kUInt8, 24, 32 },   //
    };

  MapLayout layout;
  MapLayer *layer = layout.addLayer("structured");
  VoxelLayout voxel = layer->voxelLayout();

  size_t clear_value = 0u;

  size_t i = 0;
  for (const MemberInfo &member : members)
  {
    std::cout << "member " << member.name << std::endl;
    voxel.addMember(member.name, member.type, clear_value);
    EXPECT_EQ(voxel.memberOffset(i), member.expected_offset);
    EXPECT_EQ(voxel.voxelByteSize(), member.expected_cumulative_size);
    ++i;
  }
}


================================================
FILE: tests/ohmtest/LineQueryTests.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <ohm/Key.h>
#include <ohm/KeyList.h>
#include <ohm/LineQuery.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyType.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/VoxelData.h>

#include <logutil/LogUtil.h>

#include <ohmtools/OhmCloud.h>
#include <ohmtools/OhmGen.h>
#include <ohmutil/Profile.h>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <random>

#include <gtest/gtest.h>

using namespace ohm;

namespace linequerytests
{
typedef std::chrono::high_resolution_clock TimingClock;

void sparseMap(OccupancyMap &map, bool add_hit = true)
{
  ohmgen::fillMapWithEmptySpace(map, -64, -64, -64, 63, 63, 63);
  Key key = map.voxelKey(glm::dvec3(0.5 * map.resolution()));
  Voxel<float> voxel(&map, map.layout().occupancyLayer(), key);
  ASSERT_TRUE(voxel.isValid());
  if (add_hit)
  {
    for (int i = 0; i < 1; ++i)
    {
      integrateHit(voxel);
    }
    float occupancy;
    voxel.read(&occupancy);
    EXPECT_TRUE(occupancy >= 0);
  }
}

void lineQueryTest(OccupancyMap &map)
{
  const double map_res = map.resolution();
  LineQuery query(map, glm::dvec3(0) - glm::dvec3(map_res), glm::dvec3(0), 2.0f,
                  LineQuery::kDefaultFlags | kQfNearestResult);
  query.setStartPoint(glm::dvec3(-2, 0, 0));
  query.setEndPoint(glm::dvec3(2, 0, 0));

  const bool exec_ok = query.execute();
  ASSERT_TRUE(exec_ok);

  ASSERT_GT(query.numberOfResults(), 0);
  EXPECT_EQ(query.numberOfResults(), 1);

  std::cout << "Nearest result: " << query.intersectedVoxels()[0] << " @ " << query.ranges()[0] << std::endl;

  EXPECT_EQ(query.intersectedVoxels()[0], map.voxelKey(glm::dvec3(0)))
    << query.intersectedVoxels()[0] << " != " << map.voxelKey(glm::dvec3(0));
  EXPECT_EQ(query.ranges()[0], 0);

  std::string file_name("line-query-");
  file_name += "cpu";
  file_name += ".ohm";
  ohm::save(file_name.c_str(), map);
}

TEST(LineQuery, Cpu)
{
  OccupancyMap map(0.1);
  sparseMap(map);
  lineQueryTest(map);
}
}  // namespace linequerytests


================================================
FILE: tests/ohmtest/LineWalkTests.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <ohm/Aabb.h>
#include <ohm/Key.h>
#include <ohm/KeyList.h>
#include <ohm/LineWalk.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyType.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/Trace.h>
#include <ohm/VoxelData.h>

#include <ohm/CalculateSegmentKeys.h>
#include <ohm/OccupancyUtil.h>

#include <logutil/LogUtil.h>
#include <ohmtools/OhmCloud.h>
#include <ohmtools/OhmGen.h>
#include <ohmutil/GlmStream.h>
#include <ohmutil/Profile.h>

#include <3esservermacros.h>
#ifdef TES_ENABLE
#include <shapes/3esmutablemesh.h>
#endif  // TES_ENABLE

#include <chrono>
#include <iomanip>
#include <iostream>
#include <random>

#include <gtest/gtest.h>

using namespace ohm;

namespace linewalktests
{
void testWalk(const ohm::OccupancyMap &map, const glm::dvec3 &start_point, const glm::dvec3 &end_point,
              bool include_end_point, bool log = false)
{
  // Validate the following conditions:
  // - First voxel is the start key.
  // - First voxel contains the start point.
  // - Each step is orthogonal to the last voxel.
  // - Each step is closer to the goal.
  // - End voxel is the end key.
  // - End voxel is contains the end point.

  ohm::Key start_key = map.voxelKey(start_point);
  ohm::Key end_key = map.voxelKey(end_point);

  TES_SERVER_UPDATE(ohm::g_tes, 0.0f);
  TES_LINE(ohm::g_tes, TES_COLOUR(Yellow), tes::Id(1), tes::Vector3d(glm::value_ptr(start_point)),
           tes::Vector3d(glm::value_ptr(end_point)));

  TES_STMT(unsigned id_offset = 0);
  TES_BOX_W(g_tes, TES_COLOUR(Green), tes::Id(++id_offset),
            tes::Spherical(tes::Vector3d(glm::value_ptr(map.voxelCentreGlobal(start_key))), 0.9 * map.resolution()));
  TES_BOX_W(g_tes, TES_COLOUR(Orange), tes::Id(++id_offset),
            tes::Spherical(tes::Vector3d(glm::value_ptr(map.voxelCentreGlobal(end_key))), 0.9 * map.resolution()));

  if (log)
  {
    std::cout << "walk: " << start_key << " -> " << end_key << std::endl;
  }

  std::string fail_info;
  {
    std::ostringstream str;
    str << "trace: " << start_point << " -> " << end_point << (include_end_point ? " include end" : " exclude end")
        << std::flush;
    fail_info = str.str();
  }

  bool is_first = true;
  ohm::Key last_key(nullptr);
  double last_distance = -1.0;
  unsigned voxel_count = 0;
  bool abort = false;

  const auto &visit_function = [&](const ohm::Key &current_key, double enter_range, double exit_range) -> bool  //
  {
    const glm::ivec3 key_delta = map.rangeBetween(last_key, current_key);
    const glm::ivec3 key_delta_to_end = map.rangeBetween(current_key, end_key);

    const glm::dvec3 voxel_pos = map.voxelCentreGlobal(current_key);

    if (log)
    {
      std::cout << voxel_count << ": " << current_key;
      if (!is_first)
      {
        std::cout << " step ";
        for (int j = 0; j < 3; ++j)
        {
          if (key_delta[j])
          {
            std::cout << j;
          }
        }
      }
      else
      {
        std::cout << "       ";
      }
      std::cout << " enter: " << enter_range << " exit: " << exit_range;
      std::cout << " to end " << key_delta_to_end;
      std::cout << std::endl;
    }
    if (!is_first)
    {
      EXPECT_NE(current_key, start_key) << fail_info;
      // Workout the delta.
      // Validate we have made one, orthogonal voxel step.
      EXPECT_NEAR(glm::length(glm::dvec3(key_delta)), 1.0, 1e-6) << fail_info;

      const double distance_to_end = glm::length(glm::dvec3(key_delta_to_end));
      // Valiate we are closer.
      EXPECT_LT(distance_to_end, last_distance) << fail_info;
      abort = distance_to_end >= last_distance;  // Prevent an infinite loop when it's not working.
      last_distance = distance_to_end;
    }
    else
    {
      const glm::dvec3 expected_point = start_point;
      EXPECT_EQ(current_key, start_key) << fail_info;
      EXPECT_EQ(current_key, map.voxelKey(expected_point)) << fail_info;
      last_distance = glm::length(glm::dvec3(key_delta_to_end));
      is_first = false;
    }

    // Make sure that the ray actually intersects the reported voxel. We use an epsilon to account for floating point
    // error near voxel boundaries.
    const double aabb_padding = 1e-3;
    const Aabb voxel_box(voxel_pos - 0.5 * glm::dvec3(map.resolution() + aabb_padding),
                         voxel_pos + 0.5 * glm::dvec3(map.resolution() + aabb_padding));
    const bool ray_intersect = voxel_box.rayIntersect(start_point, glm::normalize(end_point - start_point));
    if (!ray_intersect)
    {
      EXPECT_TRUE(ray_intersect) << fail_info;
    }

    TES_BOX_W(
      g_tes,
      (current_key == end_key) ?
        TES_COLOUR(Orange) :
        ((current_key == start_key) ? TES_COLOUR(Green) : (ray_intersect ? TES_COLOUR(White) : TES_COLOUR(Red))),
      tes::Id(voxel_count + 1 + id_offset), tes::Spherical(tes::Vector3d(glm::value_ptr(voxel_pos)), map.resolution()));
    std::ostringstream str;
    str << voxel_count << std::flush;
    TES_TEXT2D_SCREEN(g_tes, TES_COLOUR(Yellow), str.str().c_str(), tes::Id(),
                      tes::Spherical(tes::Vector3d(0.1, 0.1, 0)));

    last_key = current_key;
    ++voxel_count;
    TES_SERVER_UPDATE(ohm::g_tes, 0.0f);

    return !abort;
  };

  // Walk the ray.
  walkSegmentKeys(LineWalkContext(map, visit_function), start_point, end_point,
                  (!include_end_point ? kExcludeEndVoxel : 0u));

  const ohm::Key expected_key = end_key;
  // Validate last key.
  if (include_end_point)
  {
    EXPECT_EQ(last_key, expected_key) << fail_info;
    EXPECT_EQ(last_key, end_key) << fail_info;
  }
  else if (start_key != end_key)
  {
    // Ensure we are orthogonal to the end voxel.
    const glm::ivec3 key_delta_to_end = map.rangeBetween(last_key, end_key);
    const double distance_to_end = glm::length(glm::dvec3(key_delta_to_end));
    EXPECT_NEAR(distance_to_end, 1.0, 1e-6) << fail_info;
  }
  else
  {
    // Should not have made any iterations. last_distance should be < 0
    EXPECT_LT(last_distance, 0.0) << fail_info;
    EXPECT_EQ(last_key, ohm::Key(nullptr)) << fail_info;
  }
  TES_LINES_END(ohm::g_tes, tes::Id(1));
#ifdef TES_ENABLE
  for (unsigned i = 0; i < voxel_count + id_offset; ++i)
  {
    TES_BOX_END(ohm::g_tes, tes::Id(i + 1));
  }
#endif  // TES_ENABLE
}


TEST(LineWalk, Random)
{
  const unsigned test_count = 1000;
  ohm::Trace tes_trace("line_walk_random.3es", true);
  glm::dvec3 start;
  glm::dvec3 end;
  OccupancyMap map(0.1);
  std::vector<glm::dvec3> rays;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-1.0, 1.0);
  bool log = false;

  const auto restore_precision = std::cout.precision();
  std::cout << std::setprecision(std::numeric_limits<double>::max_digits10);

  // Try with different voxel offsets.
  const std::array<glm::dvec3, 2> map_origins = { glm::dvec3(0.0), glm::dvec3(0.5 * map.resolution()) };
  for (unsigned i = 0; i < test_count; ++i)
  {
    for (const auto &origin : map_origins)
    {
      map.setOrigin(origin);
      start = glm::dvec3(uniform(rng), uniform(rng), uniform(rng));
      end = glm::dvec3(uniform(rng), uniform(rng), uniform(rng));
      // std::cout << start << " -> " << end << std::endl;
      testWalk(map, start, end, true, log);
      testWalk(map, start, end, false, log);
    }
  }
  std::cout << std::setprecision(restore_precision);
}


TEST(LineWalk, Walk)
{
  ohm::Trace tes_trace("line_walk.3es", true);
  const int scale_iterations = 10;  // Test at different ranges.
  glm::dvec3 start(0.0, 0, 0);
  OccupancyMap map(0.1);
  bool log = false;

  const auto restore_precision = std::cout.precision();
  std::cout << std::setprecision(std::numeric_limits<double>::max_digits10);

#if 0
  const glm::dvec3 end(0, 0, 0);
  log = true;
  start = glm::dvec3(-1, 0, 1);
  // map.setOrigin(glm::dvec3(0.05));
  std::cout << "include end" << std::endl;
  testWalk(map, start, end, true, log);
  std::cout << "exclude end" << std::endl;
  testWalk(map, start, end, false, log);
  return;
#endif  // #

  // Try with different voxel offsets.
  const std::array<glm::dvec3, 2> map_origins = { glm::dvec3(0.0), glm::dvec3(0.5 * map.resolution()) };
  for (const auto &origin : map_origins)
  {
    map.setOrigin(origin);
    // Test at different voxel scales.
    for (int s = 1; s <= scale_iterations; ++s)
    {
      const double scale = double(s) / double(scale_iterations);
      std::cout << "Scale: " << s << std::endl;
      for (int z = -1; z <= 1; ++z)
      {
        for (int y = -1; y <= 1; ++y)
        {
          for (int x = -1; x <= 1; ++x)
          {
            const auto step_coord = glm::ivec3(x, y, z);
            glm::dvec3 end = glm::dvec3(step_coord);
            // Make sure we do not scale at s == 100 so as not to introduce new floating point errors.
            // We are testing exact voxel boundary walking and we don't want to perturb that.
            if (s != scale_iterations)
            {
              end *= scale;
            }
            // std::cout << start << " -> " << end << " << include end " << std::endl;
            testWalk(map, start, end, true, log);
            // std::cout << start << " -> " << end << " exclude end" << std::endl;
            testWalk(map, start, end, false, log);
          }
        }
      }
    }
  }
  std::cout << std::setprecision(restore_precision);
}
}  // namespace linewalktests


================================================
FILE: tests/ohmtest/MapTests.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <ohm/Aabb.h>
#include <ohm/Key.h>
#include <ohm/LineQuery.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/VoxelData.h>

#include <ohmtools/OhmCloud.h>
#include <ohmtools/OhmGen.h>

#include <logutil/LogUtil.h>

#include <ohmutil/Profile.h>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <random>

#include <gtest/gtest.h>
#include "ohmtestcommon/OhmTestUtil.h"

using namespace ohm;

namespace maptests
{
TEST(Map, Hit)
{
  OccupancyMap map(0.25);
  Key key(0, 0, 0, 0, 0, 0);

  {
    Voxel<float> voxel_write(&map, map.layout().occupancyLayer());
    voxel_write.setKey(key);
    ASSERT_TRUE(voxel_write.isValid());
    integrateHit(voxel_write);
  }

  {
    Voxel<const float> voxel_read(&map, map.layout().occupancyLayer(), key);
    ASSERT_TRUE(voxel_read.isValid());
    EXPECT_TRUE(isOccupied(voxel_read));

    float voxel_value;
    voxel_read.read(&voxel_value);
    EXPECT_EQ(voxel_value, map.hitValue());
  }
}


TEST(Map, Miss)
{
  OccupancyMap map(0.25);
  Key key(0, 0, 0, 0, 0, 0);

  {
    Voxel<float> voxel_write(&map, map.layout().occupancyLayer());
    voxel_write.setKey(key);
    ASSERT_TRUE(voxel_write.isValid());
    integrateMiss(voxel_write);
  }

  {
    Voxel<const float> voxel_read(&map, map.layout().occupancyLayer(), key);
    ASSERT_TRUE(voxel_read.isValid());
    EXPECT_TRUE(isFree(voxel_read));

    float voxel_value;
    voxel_read.read(&voxel_value);
    EXPECT_EQ(voxel_value, map.missValue());
  }
}


TEST(Map, ClipBox)
{
  // Test clipping of rays to an Aabb on insert.
  const double resolution = 0.2;
  const uint8_t region_size = 32u;
  OccupancyMap map(resolution, glm::u8vec3(region_size));

  Aabb clip_box(glm::dvec3(-1.0), glm::dvec3(2.0));
  std::vector<glm::dvec3> rays;

  // Start with rays which pass through the box.
  rays.push_back(glm::dvec3(-2, 0, 0));
  rays.push_back(glm::dvec3(3, 0, 0));

  rays.push_back(glm::dvec3(0, -2, 0));
  rays.push_back(glm::dvec3(0, 3, 0));

  rays.push_back(glm::dvec3(0, 0, 3));
  rays.push_back(glm::dvec3(0, 0, -2));

  map.setRayFilter([&clip_box](glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags) {
    return clipBounded(start, end, filter_flags, clip_box);
  });

  RayMapperOccupancy(&map).integrateRays(rays.data(), rays.size());

  // Validate the map contains no occupied points; only free and unknown.
  const glm::dvec3 voxel_half_extents(0.5 * map.resolution());
  bool touched = false;
  {
    Voxel<const float> voxel(&map, map.layout().occupancyLayer());
    for (auto iter = map.begin(); iter != map.end(); ++iter)
    {
      voxel.setKey(iter);
      touched = true;

      if (voxel.isValid())
      {
        float voxel_occupancy;
        voxel.read(&voxel_occupancy);
        if (!isUnobserved(voxel_occupancy))
        {
          EXPECT_LT(voxel_occupancy, map.occupancyThresholdValue());
          EXPECT_FALSE(isOccupied(voxel));

          // Voxel should also be with in the bounds of the Aabb. Check this.
          const Aabb voxel_box(map.voxelCentreGlobal(*iter) - voxel_half_extents,
                               map.voxelCentreGlobal(*iter) + voxel_half_extents);
          EXPECT_TRUE(clip_box.overlaps(voxel_box)) << "Voxel box does not overlap extents";
        }
      }
    }
  }

  EXPECT_TRUE(touched);

  // Reset the map. This also tests that resetting a GPU map works.
  map.clear();

  rays.clear();

  // Now rays which enter the box, ending at the origin.
  // Start with rays which pass through the box.
  rays.push_back(glm::dvec3(-2, 0, 0));
  rays.push_back(glm::dvec3(0, 0, 0));

  rays.push_back(glm::dvec3(0, -2, 0));
  rays.push_back(glm::dvec3(0, 0, 0));

  rays.push_back(glm::dvec3(0, 0, 3));
  rays.push_back(glm::dvec3(0, 0, 0));

  RayMapperOccupancy(&map).integrateRays(rays.data(), rays.size());

  // Validate the map contains no occupied points; only free and unknown.
  const Key target_key = map.voxelKey(glm::dvec3(0));
  float voxel_occupancy;
  touched = false;
  {
    Voxel<const float> voxel(&map, map.layout().occupancyLayer());
    for (auto iter = map.begin(); iter != map.end(); ++iter)
    {
      voxel.setKey(iter);
      touched = true;

      if (voxel.isValid())
      {
        voxel.read(&voxel_occupancy);
        if (voxel.key() != target_key)
        {
          if (!isUnobserved(voxel))
          {
            EXPECT_LT(voxel_occupancy, map.occupancyThresholdValue());
          }
          EXPECT_FALSE(isOccupied(voxel));
        }
        else
        {
          EXPECT_GE(voxel_occupancy, map.occupancyThresholdValue());
          EXPECT_TRUE(isOccupied(voxel));
        }

        // Touched voxels should also be with in the bounds of the Aabb. Check this.
        if (!isUnobserved(voxel))
        {
          const Aabb voxel_box(map.voxelCentreGlobal(*iter) - voxel_half_extents,
                               map.voxelCentreGlobal(*iter) + voxel_half_extents);
          EXPECT_TRUE(clip_box.overlaps(voxel_box)) << "Voxel box does not overlap extents";
        }
      }
    }
  }

  EXPECT_TRUE(touched);
}
}  // namespace maptests


================================================
FILE: tests/ohmtest/MathsTests.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <gtest/gtest.h>

#include <ohm/Aabb.h>

#include <ohmutil/GlmStream.h>

#include <sstream>

using namespace ohm;

inline std::ostream &operator<<(std::ostream &out, const ohm::Aabb &aabb)
{
  out << "[ (" << aabb.minExtents().x << ',' << aabb.minExtents().y << ',' << aabb.minExtents().z << ") ("
      << aabb.maxExtents().x << ',' << aabb.maxExtents().y << ',' << aabb.maxExtents().z << ") ]";
  return out;
}


namespace
{
void testOverlap(const glm::ivec3 &axis_range, const Aabb &reference_box, const Aabb &initial_box, bool overlap_test)
{
  char sign_char[3];
  int axis[3];
  Aabb test_box;

  const double move_scale = (overlap_test) ? 1.0 : 2.0;
  glm::dvec3 move;

  for (int k = 0; k < axis_range[2]; ++k)
  {
    move = glm::dvec3(0);

    sign_char[2] = ' ';
    axis[2] = -1;
    if (axis_range[2] > 1)
    {
      axis[2] = k % 3;
      if (k < 3)
      {
        move[axis[2]] = move_scale * reference_box.minExtents()[axis[2]];
        sign_char[2] = '-';
      }
      else
      {
        move[axis[2]] = move_scale * reference_box.maxExtents()[axis[2]];
        sign_char[2] = '+';
      }
    }

    for (int j = 0; j < axis_range[1]; ++j)
    {
      sign_char[1] = ' ';
      axis[1] = -1;
      if (axis_range[1] > 1)
      {
        if (j == k)
        {
          // Do not do duplicate parings.
          continue;
        }

        axis[1] = j % 3;
        if (j < 3)
        {
          move[axis[1]] = move_scale * reference_box.minExtents()[axis[1]];
          sign_char[1] = '-';
        }
        else
        {
          move[axis[1]] = move_scale * reference_box.maxExtents()[axis[1]];
          sign_char[1] = '+';
        }
      }

      for (int i = 0; i < axis_range[0]; ++i)
      {
        if (axis_range[2] && i == k || axis_range[1] && i == j)
        {
          // Don't pair the same axes.
          continue;
        }

        axis[0] = i % 3;
        if (i < 3)
        {
          move[axis[0]] = move_scale * reference_box.minExtents()[axis[0]];
          sign_char[0] = '-';
        }
        else
        {
          move[axis[0]] = move_scale * reference_box.maxExtents()[axis[0]];
          sign_char[0] = '+';
        }

        test_box = initial_box + move;

        std::ostringstream msg;
        if (overlap_test)
        {
          msg << "overlap ";
        }
        else
        {
          msg << "exclude ";
        }

        msg << sign_char[0] << '[' << axis[0] << ']';
        if (axis[1] >= 0)
        {
          msg << sign_char[1] << '[' << axis[1] << ']';
        }
        if (axis[2] >= 0)
        {
          msg << sign_char[2] << '[' << axis[2] << ']';
        }


        if (overlap_test)
        {
          EXPECT_TRUE(reference_box.overlaps(test_box)) << msg.str();
          EXPECT_TRUE(test_box.overlaps(reference_box)) << msg.str();
        }
        else
        {
          // Expect exclusion.
          EXPECT_FALSE(reference_box.overlaps(test_box)) << msg.str();
          EXPECT_FALSE(test_box.overlaps(reference_box)) << msg.str();
        }
      }
    }
  }
}
}  // namespace


TEST(Maths, AabbOverlap)
{
  const Aabb reference_box(glm::dvec3(-1.1, -2.2, -3.3), glm::dvec3(4.4, 5.5, 6.6));
  const Aabb initial_box(glm::dvec3(-1.0), glm::dvec3(1.0));
  Aabb test_box;

  test_box = initial_box;
  EXPECT_TRUE(reference_box.overlaps(test_box)) << "inside";
  EXPECT_TRUE(test_box.overlaps(reference_box)) << "inside";

  // Move the test box so it overlaps each axis
  // Single axis overlap.
  testOverlap(glm::ivec3(6, 0, 0), reference_box, initial_box, true);
  // Two axis overlap
  testOverlap(glm::ivec3(6, 6, 0), reference_box, initial_box, true);
  // Three axis overlap
  testOverlap(glm::ivec3(6, 6, 6), reference_box, initial_box, true);

  // Move so we don't have an overlap.
  // Single axis overlap.
  testOverlap(glm::ivec3(6, 0, 0), reference_box, initial_box, false);
  // Two axis overlap
  testOverlap(glm::ivec3(6, 6, 0), reference_box, initial_box, false);
  // Three axis overlap
  testOverlap(glm::ivec3(6, 6, 6), reference_box, initial_box, false);
}


TEST(Maths, AabbClip)
{
  const Aabb reference_box(glm::dvec3(-1.1, -2.2, -3.3), glm::dvec3(4.4, 5.5, 6.6));

  std::vector<glm::dvec3> lines;

  // Setup some lines entirely within the box .
  lines.push_back(reference_box.minExtents() + glm::dvec3(1, 1, 1));
  lines.push_back(reference_box.maxExtents() - glm::dvec3(1, 1, 1));

  lines.push_back(reference_box.maxExtents() - glm::dvec3(1, 1, 1));
  lines.push_back(reference_box.minExtents() + glm::dvec3(1, 1, 1));

  unsigned clip_flags = 0;
  bool line_intersect = false;
  glm::dvec3 start, end, dir1, dir2;
  for (size_t i = 0; i < lines.size(); i += 2)
  {
    start = lines[i];
    end = lines[i + 1];
    // Cache initial direction.
    dir1 = glm::normalize(end - start);

    EXPECT_TRUE(reference_box.contains(start));
    EXPECT_TRUE(reference_box.contains(end));

    line_intersect = reference_box.clipLine(start, end, &clip_flags);
    EXPECT_EQ(clip_flags, 0);
    EXPECT_FALSE(line_intersect);

    // Clipped direction.
    dir2 = glm::normalize(end - start);

    EXPECT_EQ(start, lines[i]);
    EXPECT_EQ(end, lines[i + 1]);

    EXPECT_TRUE(reference_box.contains(start)) << "no-clip " << i;
    EXPECT_TRUE(reference_box.contains(end)) << "no-clip " << i;

    // Validate direction.
    EXPECT_FALSE(glm::any(glm::greaterThan(glm::abs(dir1 - dir2), glm::dvec3(1e-9))));
  }

  lines.clear();
  // Add lines with end points outside the box.
  for (int i = 0; i < 8; ++i)
  {
    lines.push_back(glm::dvec3(0));
    lines.push_back(reference_box.corner(i) * 2.5);
  }

  for (size_t i = 0; i < lines.size(); i += 2)
  {
    // Iterate twice: once with clamping, once with an epsilon
    bool clamp = false;
    double epsilon = 1e-6;
    for (int j = 0; j < 2; ++j)
    {
      start = lines[i];
      end = lines[i + 1];
      // Cache initial direction.
      dir1 = glm::normalize(end - start);

      EXPECT_TRUE(reference_box.contains(start));
      EXPECT_FALSE(reference_box.contains(end));

      line_intersect = reference_box.clipLine(start, end, &clip_flags, clamp);
      EXPECT_EQ(clip_flags, Aabb::kClippedEnd);
      EXPECT_TRUE(line_intersect);

      // Clipped direction.
      dir2 = glm::normalize(end - start);

      EXPECT_EQ(start, lines[i]);
      EXPECT_NE(end, lines[i + 1]);

      EXPECT_TRUE(reference_box.contains(start, epsilon)) << "clip-end " << i;
      EXPECT_TRUE(reference_box.contains(end, epsilon)) << "clip-end " << i;

      // Validate direction.
      EXPECT_FALSE(glm::any(glm::greaterThan(glm::abs(dir1 - dir2), glm::dvec3(1e-9))));

      clamp = true;
      epsilon = 0;
    }
  }

  lines.clear();
  // Add lines with start points outside the box.
  for (int i = 0; i < 8; ++i)
  {
    lines.push_back(reference_box.corner(i) * 2.5);
    lines.push_back(glm::dvec3(0));
  }

  for (size_t i = 0; i < lines.size(); i += 2)
  {
    start = lines[i];
    end = lines[i + 1];
    // Cache initial direction.
    dir1 = glm::normalize(end - start);

    EXPECT_FALSE(reference_box.contains(start));
    EXPECT_TRUE(reference_box.contains(end));

    line_intersect = reference_box.clipLine(start, end, &clip_flags);
    EXPECT_EQ(clip_flags, Aabb::kClippedStart);
    EXPECT_TRUE(line_intersect);

    // Clipped direction.
    dir2 = glm::normalize(end - start);

    EXPECT_NE(start, lines[i]);
    EXPECT_EQ(end, lines[i + 1]);

    EXPECT_TRUE(reference_box.contains(start)) << "clip-start " << i;
    EXPECT_TRUE(reference_box.contains(end)) << "clip-start " << i;

    // Validate direction.
    EXPECT_FALSE(glm::any(glm::greaterThan(glm::abs(dir1 - dir2), glm::dvec3(1e-9))));
  }

  lines.clear();
  // Add lines with both points outside the box but intersecting the box.
  lines.push_back(reference_box.corner(Aabb::kCornerLll) * 2.5);
  lines.push_back(reference_box.corner(Aabb::kCornerUuu) * 2.5);

  lines.push_back(reference_box.corner(Aabb::kCornerUll) * 2.5);
  lines.push_back(reference_box.corner(Aabb::kCornerLuu) * 2.5);

  lines.push_back(reference_box.corner(Aabb::kCornerLul) * 2.5);
  lines.push_back(reference_box.corner(Aabb::kCornerUlu) * 2.5);

  lines.push_back(reference_box.corner(Aabb::kCornerUul) * 2.5);
  lines.push_back(reference_box.corner(Aabb::kCornerLlu) * 2.5);

  for (size_t i = 0; i < lines.size(); i += 2)
  {
    start = lines[i];
    end = lines[i + 1];
    // Cache initial direction.
    dir1 = glm::normalize(end - start);

    EXPECT_FALSE(reference_box.contains(start));
    EXPECT_FALSE(reference_box.contains(end));

    line_intersect = reference_box.clipLine(start, end, &clip_flags);
    EXPECT_EQ(clip_flags, Aabb::kClippedStart | Aabb::kClippedEnd);
    EXPECT_TRUE(line_intersect);

    // Clipped direction.
    dir2 = glm::normalize(end - start);

    EXPECT_NE(start, lines[i]);
    EXPECT_NE(end, lines[i + 1]);

    EXPECT_TRUE(reference_box.contains(start, 1e-9)) << "clip-both " << i;
    EXPECT_TRUE(reference_box.contains(end, 1e-9)) << "clip-both " << i;

    // Validate direction.
    EXPECT_FALSE(glm::any(glm::greaterThan(glm::abs(dir1 - dir2), glm::dvec3(1e-9))));
  }

  lines.clear();
  // Add lines which do not intersect the box.
  lines.push_back(reference_box.corner(Aabb::kCornerLll) * 2.5);
  lines.push_back(reference_box.corner(Aabb::kCornerLlu) * 2.5);

  lines.push_back(reference_box.corner(Aabb::kCornerLll) * 2.5);
  lines.push_back(reference_box.corner(Aabb::kCornerLll) * 5.0);

  lines.push_back(reference_box.corner(Aabb::kCornerLll) * 5.0);
  lines.push_back(reference_box.corner(Aabb::kCornerLll) * 2.5);

  for (size_t i = 0; i < lines.size(); i += 2)
  {
    start = lines[i];
    end = lines[i + 1];

    EXPECT_FALSE(reference_box.contains(start));
    EXPECT_FALSE(reference_box.contains(end));

    line_intersect = reference_box.clipLine(start, end, &clip_flags);
    EXPECT_EQ(clip_flags, 0);
    EXPECT_FALSE(line_intersect);

    EXPECT_EQ(start, lines[i]);
    EXPECT_EQ(end, lines[i + 1]);

    EXPECT_FALSE(reference_box.contains(start));
    EXPECT_FALSE(reference_box.contains(end));
  }

  lines.clear();
  // Cast lines from inside the box along axes.
  lines.push_back(glm::dvec3(0.0));
  lines.push_back(glm::dvec3(10.0, 0, 0));
  lines.push_back(glm::dvec3(0.0));
  lines.push_back(glm::dvec3(-10.0, 0, 0));
  lines.push_back(glm::dvec3(0.0));
  lines.push_back(glm::dvec3(0, 10.0, 0));
  lines.push_back(glm::dvec3(0.0));
  lines.push_back(glm::dvec3(0, -10.0, 0));
  lines.push_back(glm::dvec3(0.0));
  lines.push_back(glm::dvec3(0, 0, 10.0));
  lines.push_back(glm::dvec3(0.0));
  lines.push_back(glm::dvec3(0, 0, -10.0));


  for (size_t i = 0; i < lines.size(); i += 2)
  {
    start = lines[i];
    end = lines[i + 1];

    EXPECT_TRUE(reference_box.contains(start));
    EXPECT_FALSE(reference_box.contains(end));

    line_intersect = reference_box.clipLine(start, end, &clip_flags, true);
    EXPECT_EQ(clip_flags, Aabb::kClippedEnd);
    EXPECT_TRUE(line_intersect);

    EXPECT_EQ(start, lines[i]);
    EXPECT_NE(end, lines[i + 1]);

    EXPECT_TRUE(reference_box.contains(start));
    EXPECT_TRUE(reference_box.contains(end));
  }
}


================================================
FILE: tests/ohmtest/NdtTests.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas, Jason Williams
#include "OhmTestConfig.h"

#include <ohm/Key.h>
#include <ohm/NdtMap.h>
#include <ohm/OccupancyMap.h>
#include <ohm/Trace.h>
#include <ohm/VoxelData.h>

#include <gtest/gtest.h>
#include "ohmtestcommon/CovarianceTestUtil.h"
#include "ohmtestcommon/OhmTestUtil.h"

#include <chrono>
#include <random>
#include <unordered_map>

#include <glm/gtc/matrix_access.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/norm.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

#include <3esservermacros.h>

#include <Eigen/Dense>

using namespace ohm;
using namespace ohmtestutil;

namespace ndttests
{
/// Default tolerance for checking NDT probability changes.
///
/// We use random generators with fixed seeds to create our ray set for each run. This is deterministic for a given
/// platform, but not beteen platforms as the random generator code may differ. We set a tight, default tolerance,
/// but may tune larger values for specific cases.
const float default_ndt_tolerance = 0.001f;

/// Helper for testing covariance update via @p NdtMap .
///
/// We add each sample to the map as if the sensor were at the origin. The changes to the target voxels are
/// compared against a reference code implementation. Samples may affect any number of voxels.
///
/// @param samples The set of sample points to integrate into the map.
/// @param voxel_resolution The voxel size for the @c OccupancyMap
void testNdtHits(const std::vector<glm::dvec3> &samples, const std::vector<glm::dvec3> &sensors, double resolution)
{
  std::unordered_map<ohm::Key, CovTestVoxel> reference_voxels;

  ohm::OccupancyMap map(resolution, ohm::MapFlag::kVoxelMean);
  ohm::NdtMap ndt(&map, true);

  // Simulate a sensor at the origin. Not used.
  const glm::dvec3 sensor(0.0);

  ohm::Voxel<const ohm::VoxelMean> mean(&map, map.layout().meanLayer());
  ohm::Voxel<const ohm::CovarianceVoxel> covariance(&map, map.layout().covarianceLayer());
  ASSERT_TRUE(mean.isLayerValid());
  ASSERT_TRUE(covariance.isLayerValid());

  // Build a a set of samples into the NDT map and into a reference data set.
  for (size_t i = 0; i < samples.size(); ++i)
  {
    const glm::dvec3 &sample = samples[i];
    const glm::dvec3 &sensor = sensors[i];

    ohm::Key key = map.voxelKey(sample);

    ohm::integrateNdtHit(ndt, key, sensor, sample);
    setVoxelKey(key, mean, covariance);

    ASSERT_TRUE(mean.isValid());
    ASSERT_TRUE(covariance.isValid());
    ohm::CovarianceVoxel cov_voxel;
    ohm::VoxelMean voxel_mean;
    covariance.read(&cov_voxel);
    mean.read(&voxel_mean);

    // Update the reference voxel as well.
    auto ref = reference_voxels.find(key);
    if (ref == reference_voxels.end())
    {
      // New voxel. Initialise.
      CovTestVoxel ref_voxel;
      initialiseTestVoxel(&ref_voxel, float(ndt.map().resolution()));
      ref = reference_voxels.insert(std::make_pair(key, ref_voxel)).first;
    }

    // Update the reference algorithm.
    updateHit(&ref->second, sample);

    // Progressive validation
    EXPECT_TRUE(validate(positionSafe(mean), voxel_mean.count, cov_voxel, ref->second));
  }

  // Finalise validation
  // Reset the voxel references.
  mean = ohm::Voxel<const ohm::VoxelMean>(&map, map.layout().meanLayer());
  covariance = ohm::Voxel<const ohm::CovarianceVoxel>(&map, map.layout().covarianceLayer());
  for (const auto &ref : reference_voxels)
  {
    // Lookup the target voxel.
    setVoxelKey(ref.first, mean, covariance);
    ASSERT_TRUE(mean.isValid());
    ASSERT_TRUE(covariance.isValid());
    ohm::CovarianceVoxel cov_voxel;
    ohm::VoxelMean voxel_mean;
    covariance.read(&cov_voxel);
    mean.read(&voxel_mean);
    EXPECT_TRUE(validate(positionSafe(mean), voxel_mean.count, cov_voxel, ref.second));
  }

  mean.reset();
  covariance.reset();
}

/// Helper for testing NDT based voxel miss logic.
///
/// We first build an @c NdtMap with the given set of @p samples . While we allow for these to be traced from
/// @p sensor only modify the voxel for each sample rather than integrating the full ray into the map. We assume that
/// all @p samples fall with in the same voxel.
///
/// Once the sample voxel is populated, we trace all the @p test_rays through that voxel and compare the *change* in
/// probability against the @p expected_prob_and_tolerance .
///
/// @param sensor The simulated sensor position for all the @p samples .
/// @param samples The set of sample points to integrate into the map. These should all fall in the same voxel.
/// @param voxel_resolution The voxel size for the @c OccupancyMap
/// @param sensor_noise Sensor noise value for the @c NdtMap
/// @param map_origin Origin for the @p OccupancyMap
/// @param test_rays Set of rays to test against the sample voxel. These are expected to all pass through, but not
///   end in the sample voxel. There are two elements per ray in this array making ray origin, end pairs.
/// @param expected_prob_and_tolerance Expected probability adjustments for each of the @p test_rays . This array has
///   half the elements of @p test_rays .
void testNdtMiss(const glm::dvec3 &sensor, const std::vector<glm::dvec3> samples, double voxel_resolution,
                 float sensor_noise, const glm::dvec3 &map_origin, const std::vector<glm::dvec3> &test_rays,
                 const std::vector<std::pair<float, float>> &expected_prob_and_tolerance)
{
  (void)sensor;  // Only used for 3es debugging.
  ohm::OccupancyMap map(voxel_resolution, ohm::MapFlag::kVoxelMean);
  ohm::NdtMap ndt(&map, true);

  map.setOrigin(map_origin);
  // Set explicit probabilities
  map.setHitProbability(0.55f);
  map.setMissProbability(0.45f);
  ndt.setAdaptationRate(1.0f);
  ndt.setSensorNoise(sensor_noise);

  // Integrate all the samples into the map to build voxel covariance and mean. We expect all samples to fall in one
  // voxel and test rays to pass through this voxel. While this assumption is not enforced by the integrateHit() calls
  // below, but we do cache the corresponding voxel key in target_key.
  ohm::Key target_key;
  TES_STMT(std::vector<tes::Vector3d> lines);
  for (size_t i = 0; i < samples.size(); ++i)
  {
    const glm::dvec3 &sample = samples[i];

    ohm::Key key = map.voxelKey(sample);
    target_key = key;
    ohm::integrateNdtHit(ndt, key, sensor, sample);
    TES_LINE(ohm::g_tes, TES_COLOUR(Yellow), tes::Id(), glm::value_ptr(sensor), glm::value_ptr(sample));
    TES_STMT(lines.emplace_back(tes::Vector3d(glm::value_ptr(sensor))));
    TES_STMT(lines.emplace_back(tes::Vector3d(glm::value_ptr(sample))));
  }
  TES_LINES(ohm::g_tes, TES_COLOUR(Yellow), tes::Id(), tes::DataBuffer(lines));
  TES_SERVER_UPDATE(ohm::g_tes, 0.0f);

  // Fetch the target_voxel in which we expect all samples to fall.
  Voxel<float> occupancy(&map, map.layout().occupancyLayer(), target_key);
  ASSERT_TRUE(occupancy.isValid());
  float initial_value;
  occupancy.read(&initial_value);
  ndt.setTrace(true);  // For 3es debugging

  // Now trace all the test_rays through the target_voxel. For each we will restore the initial voxel value, so
  // each test is independent. We then integrate the ray miss/passthrough and validate the probability adjustment
  // against the expected result.
  for (size_t i = 0; i < test_rays.size(); i += 2)
  {
    occupancy.write(initial_value);
    TES_LINE(ohm::g_tes, TES_COLOUR(Cornsilk), tes::Id(&sensor), glm::value_ptr(test_rays[i]),
             glm::value_ptr(test_rays[i + 1]));
    TES_SERVER_UPDATE(ohm::g_tes, 0.0f);
    ohm::integrateNdtMiss(ndt, target_key, test_rays[i], test_rays[i + 1]);
    TES_LINES_END(ohm::g_tes, tes::Id(&sensor));
    // Calculate the value adjustment.
    float adjusted_value;
    occupancy.read(&adjusted_value);
    float value_adjustment = adjusted_value - initial_value;
    // Convert to probability.
    float ray_probability = ohm::valueToProbability(value_adjustment);
    // Validate
    // Note: there is some variance here across platforms as the random generators differ even though we use
    // a fixed seed.
    EXPECT_NEAR(ray_probability, expected_prob_and_tolerance[i / 2].first, expected_prob_and_tolerance[i / 2].second);
  }
  occupancy.reset();
}

TEST(Ndt, Hit)
{
  // Use a fixed seed for test repeatability.
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::normal_distribution<double> gaussian(0.0, 1.0);
  // Generate points within a 2m cube.
  std::uniform_real_distribution<double> uniform(0.01, 1.99);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);
  std::vector<glm::dvec3> sensors;
  sensors.reserve(sample_count);

  // First generate a sample surface to target. We have to make sure this falls within a single voxel.
  glm::dvec3 mean(0);
  glm::dmat3 cov(0);

  // Sensor position from which measurements were taken
  glm::dvec3 sensor(0);

  double num_pt = 0;
  while (num_pt < 4)
  {
    // glm::dvec3 pt = 0.5 * randomVector3();
    glm::dvec3 pt;

    pt[0] = uniform(rng);
    pt[1] = uniform(rng);
    pt[2] = uniform(rng);

    const glm::dvec3 diff = pt - mean;
    const double one_on_num_pt_plus_one = 1.0 / (num_pt + 1.0);
    mean = (num_pt * mean + pt) * one_on_num_pt_plus_one;
    cov = (num_pt * cov + glm::dot((num_pt * one_on_num_pt_plus_one) * diff, diff)) * one_on_num_pt_plus_one;
    num_pt += 1.0;
  }

  {
    // TODO: this is the only part we need Eigen for. Find an alternative Eigen decomposition solution.
    Eigen::Matrix3d cov_eigen;
    cov_eigen << glm::row(cov, 0)[0], glm::row(cov, 0)[1], glm::row(cov, 0)[2],  //
      glm::row(cov, 1)[0], glm::row(cov, 1)[1], glm::row(cov, 1)[1],             //
      glm::row(cov, 2)[0], glm::row(cov, 2)[1], glm::row(cov, 2)[1];
    Eigen::LDLT<Eigen::Matrix3d, Eigen::Lower> cov_d(cov_eigen);
    const Eigen::Vector3d &vD = cov_d.vectorD();

    const Eigen::Vector3d mean_eigen(mean.x, mean.y, mean.z);
    for (size_t i = 0; i < sample_count; ++i)
    {
      Eigen::Vector3d pt;
      for (size_t i = 0; i < 3; ++i)
      {
        pt[i] = sqrt(vD[i]) * gaussian(rng);
      }

      pt = mean_eigen + cov_d.matrixL() * pt;
      samples.emplace_back(glm::dvec3(pt.x(), pt.y(), pt.z()));
      sensors.push_back(sensor);
    }
  }

  testNdtHits(samples, sensors, 2.0);
}


TEST(Ndt, MissPlanar)
{
  ohm::Trace trace("ndt-miss-planar.3es");

  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  std::uniform_real_distribution<double> uniform(0.01, 1.99);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(1, 1, 5);

  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    sample.x = uniform(rng);
    sample.y = uniform(rng);
    sample.z = 1.0;
    // sample.z += 0.025 * uniform(rng);
    samples.emplace_back(sample);
  }

  // Setup to trace a ray through the voxel, but not ending in the voxel.
  std::vector<std::pair<float, float>> expected_prob_and_tolerance;
  std::vector<glm::dvec3> rays;
  // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(1, 1, -5));
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Ray parallel to the voxel ellipsoid - expected to miss.
  rays.emplace_back(glm::dvec3(-5, 1, 0.25));
  rays.emplace_back(glm::dvec3(5, 1, 0.25));
  expected_prob_and_tolerance.emplace_back(0.5f, default_ndt_tolerance);
  // Ray parallel to the voxel ellipsoid, but near the centre.
  // Note: we build a perfect plane above in the centre of the voxel. However, the quantisation of the mean
  // can offset the plane. Running exactly on the plane on which we generated the points will not result in the same
  // effect in testing. We must offset the ray a little to ensure we are a bit off the plane either when using
  // voxel mean quantisation or not.
  rays.emplace_back(glm::dvec3(1, 5, 1.01));
  rays.emplace_back(glm::dvec3(1, -5, 1.01));
  expected_prob_and_tolerance.emplace_back(0.5f, default_ndt_tolerance);
  // Ray running across the voxel, down towards the ellipsoid, but not crossing. This simulates rays running near
  // parallel a ground plane as it approaches the sample position.
  rays.emplace_back(glm::dvec3(-5, 1, 2));
  rays.emplace_back(glm::dvec3(5, 1, 1));
  expected_prob_and_tolerance.emplace_back(0.5f, default_ndt_tolerance);
  // Ray running across the voxel, and through the ellipsoid.
  rays.emplace_back(glm::dvec3(-5, 1, 2));
  rays.emplace_back(glm::dvec3(5, 1, 0.5));
  expected_prob_and_tolerance.emplace_back(0.23f, 0.02f);

  testNdtMiss(sensor, samples, voxel_resolution, 0.05f, glm::dvec3(0), rays, expected_prob_and_tolerance);
}


TEST(Ndt, MissCylindrical)
{
  ohm::Trace trace("ndt-miss-cylindrical.3es");

  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  const float sensor_noise = 0.05f;

  const double cylinder_radius = 0.3;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-0.99, 0.99);
  std::uniform_real_distribution<double> uniform_radius(cylinder_radius - sensor_noise, cylinder_radius + sensor_noise);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(0, 0, 5);

  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    // Create a random sample in the voxel
    sample.x = uniform(rng);
    sample.y = uniform(rng);
    sample.z = uniform(rng);
    // Convert into a cylinder.
    // Normalise in X/Y, create a random radius (to the limit) and scale by this radius.
    double length_xy = std::sqrt(sample.x * sample.x + sample.y * sample.y);
    if (length_xy > 1e-6)
    {
      const double radius = uniform_radius(rng);
      sample.x = radius * sample.x / length_xy;
      sample.y = radius * sample.y / length_xy;
    }
    samples.emplace_back(sample);
  }

  // Setup rays through the voxel, but not ending in the voxel.
  std::vector<std::pair<float, float>> expected_prob_and_tolerance;
  std::vector<glm::dvec3> rays;
  // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(0, 0, -5));
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Ray running parallel to the cylinder near the edge. Should be a near hit.
  rays.emplace_back(glm::dvec3(cylinder_radius, cylinder_radius, 5));
  rays.emplace_back(glm::dvec3(cylinder_radius, cylinder_radius, -5));
  expected_prob_and_tolerance.emplace_back(0.425f, 0.01f);
  // Ray running parallel to the cylinder, but should miss.
  rays.emplace_back(glm::dvec3(1.5 * cylinder_radius, 1.5 * cylinder_radius, -5));
  rays.emplace_back(glm::dvec3(2.0 * cylinder_radius, 2.0 * cylinder_radius, 5));
  expected_prob_and_tolerance.emplace_back(0.499f, default_ndt_tolerance);
  // Ray across the middle of the cylinder.
  rays.emplace_back(glm::dvec3(2, -cylinder_radius, 0));
  rays.emplace_back(glm::dvec3(-2, -cylinder_radius, 0));
  expected_prob_and_tolerance.emplace_back(0.312f, 0.005f);
  // Ray across the end (top) of the cylinder.
  rays.emplace_back(glm::dvec3(2, -cylinder_radius, 0.85));
  rays.emplace_back(glm::dvec3(-2, -cylinder_radius, 0.85));
  expected_prob_and_tolerance.emplace_back(0.436f, 0.003f);
  // Ray across the voxel, missing the cylinder (top) of the cylinder.
  rays.emplace_back(glm::dvec3(2, 2.0 * cylinder_radius, 0.85));
  rays.emplace_back(glm::dvec3(-2, 2.0 * cylinder_radius, 0.85));
  expected_prob_and_tolerance.emplace_back(0.497f, default_ndt_tolerance);
  testNdtMiss(sensor, samples, voxel_resolution, sensor_noise, glm::dvec3(-0.5 * voxel_resolution), rays,
              expected_prob_and_tolerance);
}


TEST(Ndt, MissSpherical)
{
  ohm::Trace trace("ndt-miss-spherical.3es");

  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  const float sensor_noise = 0.05f;

  const double radius = 0.3;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-0.99, 0.99);
  std::uniform_real_distribution<double> uniform_radius(radius - sensor_noise, radius + sensor_noise);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(0, 0, 5);

  // Setup rays through the voxel, but not ending in the voxel.
  std::vector<std::pair<float, float>> expected_prob_and_tolerance;
  std::vector<glm::dvec3> rays;
  // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(0, 0, -5));
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Edge of the sphere..
  rays.emplace_back(glm::dvec3(radius, radius, 5));
  rays.emplace_back(glm::dvec3(radius, radius, -5));
  expected_prob_and_tolerance.emplace_back(0.469f, 0.006f);
  // Near the edge of the sphere, but should miss.
  rays.emplace_back(glm::dvec3(1.5 * radius, 1.5 * radius, -5));
  rays.emplace_back(glm::dvec3(2.0 * radius, 2.0 * radius, 5));
  expected_prob_and_tolerance.emplace_back(0.5f, default_ndt_tolerance);
  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    double sample_len2 = 0;
    // Keep trying while we have a near zero vector.
    while (sample_len2 < 1e-6)
    {
      // Create a random sample in the voxel
      sample.x = uniform(rng);
      sample.y = uniform(rng);
      sample.z = uniform(rng);
      sample_len2 = glm::length2(sample);
    }
    sample = glm::normalize(sample);
    sample *= uniform_radius(rng);
    samples.emplace_back(sample);
  }

  testNdtMiss(sensor, samples, voxel_resolution, sensor_noise, glm::dvec3(-0.5 * voxel_resolution), rays,
              expected_prob_and_tolerance);
}
}  // namespace ndttests


================================================
FILE: tests/ohmtest/OhmConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMCONFIG_H
#define OHMCONFIG_H

#include "OhmExport.h"

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

#include <cmath>

// clang-format off

// Enable various validation tests throughout this library.
#cmakedefine OHM_VALIDATION
#cmakedefine OHM_FEATURE_THREADS
#cmakedefine OHM_PROFILE
#cmakedefine OHM_EMBED_GPU_CODE

#ifdef OHM_PROFILE
#define PROFILING 1
#endif  // OHM_PROFILE

#cmakedefine TES_ENABLE
#ifdef TES_ENABLE
namespace tes
{
  class Server;
}
namespace ohm
{
  /// Debug visualisation server pointer. Must be set by the executable to enable its use in this library.
  /// That is, this is considered a borrowed pointer in this library.
  extern tes::Server *g_tes;  // Symbol defined in occupancyutil.cpp.
}  // namespace ohm
#endif  // TES_ENABLE

#include <memory>

// clang-format on

#endif  // OHMCONFIG_H


================================================
FILE: tests/ohmtest/OhmTestConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMTESTCONFIG_H
#define OHMTESTCONFIG_H

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

#include <cmath>
#include <memory>

#cmakedefine OHM_WITH_PNG

#endif  // OHMTESTCONFIG_H


================================================
FILE: tests/ohmtest/RayPatternTests.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include "RayValidation.h"

#include <ohm/OccupancyMap.h>
#include <ohm/RayPatternConical.h>
#include <ohm/VoxelData.h>

#include <ohmtestcommon/RayPatternTestUtil.h>

#include <3esservermacros.h>

#include <array>
#include <cstdio>
#include <vector>

#include <glm/ext.hpp>

#include <gtest/gtest.h>

#ifdef OHM_WITH_PNG
#include <png.h>
#endif  // OHM_WITH_PNG

using namespace ohm;

namespace raypattern
{
#ifdef OHM_WITH_PNG
bool savePng(const char *filename, const std::vector<uint8_t> &raw, unsigned w, unsigned h)
{
  png_image image;

  // Initialize the 'png_image' structure.
  memset(&image, 0, sizeof(image));
  image.version = PNG_IMAGE_VERSION;
  image.width = int(w);
  image.height = int(h);
  image.flags = 0;
  image.colormap_entries = 0;

  int row_stride = w;
  image.format = PNG_FORMAT_GRAY;
  row_stride = -int(w);

  // Negative row stride to flip the image.
  if (png_image_write_to_file(&image, filename, false,  // convert_to_8bit,
                              raw.data(),
                              row_stride,  // row_stride
                              nullptr      // colormap
                              ))
  {
    return true;
  }

  return false;
}
#endif  // OHM_WITH_PNG

// Debug function: log the image for generating comparison data. This essentially makes a regression test, but with
// visual confirmation of the image from savePng().
void logImage(const std::vector<uint8_t> &image, unsigned width, unsigned height)
{
  std::cout << "const size_t image_size = " << width * height << ";\n";
  std::cout << "const uint8_t image[" << width * height << "] = //\n";
  std::cout << "{ //";
  const auto restore_width = std::cout.width();
  const auto restore_fill = std::cout.fill();
  const size_t bytes_per_line = 20;
  for (size_t i = 0; i < image.size(); ++i)
  {
    if (i != 0)
    {
      std::cout << ',';
    }
    if (i % bytes_per_line == 0)
    {
      std::cout << "\n  ";
    }
    std::cout << "0x" << std::setfill('0') << std::setw(2) << std::hex << unsigned(image[i]);
  }
  std::cout.width(restore_width);
  std::cout.fill(restore_fill);
  std::cout << "\n};\n";
  std::cout << std::endl;
}

void validateImage(const std::vector<uint8_t> &image, unsigned width, unsigned height, const uint8_t *reference,
                   size_t reference_element_count)
{
  ASSERT_EQ(width * height, reference_element_count);
  ASSERT_EQ(image.size(), reference_element_count);

  for (size_t i = 0; i < reference_element_count; ++i)
  {
    ASSERT_EQ(image[i], reference[i]);
  }
}

TEST(RayPattern, Conical)
{
  const double angular_resolution = glm::radians(5.0);
  RayPatternConical pattern(glm::dvec3(0.7, 0.7, 0), glm::radians(65.0), 5.0, angular_resolution);

  // To test the ray pattern, we will convert the rays to a spherical image and validate we have a circle where we
  // expect.
  std::vector<uint8_t> spherical_image;
  const unsigned image_height = unsigned(std::ceil(M_PI / angular_resolution));
  const unsigned image_width = unsigned(std::ceil(2.0 * M_PI / angular_resolution));
  spherical_image.resize(image_height * image_width);

  // Clear the image.
  std::fill(spherical_image.begin(), spherical_image.end(), uint8_t(0));

  // Access the ray pattern and quantise in polar space.
  const size_t ray_count = pattern.rayCount();
  const glm::dvec3 *ray_end = pattern.rayPoints() + 1;
  for (size_t i = 0; i < ray_count; ++i, ray_end += 2)
  {
    glm::dvec2 ray_xy(ray_end->x, ray_end->y);
    const double scale_2d = glm::length(ray_xy);
    ray_xy *= (scale_2d) ? 1.0 / scale_2d : 0.0;
    double azimuth = std::atan2(ray_xy.y, ray_xy.x);
    double elevation = std::atan2(ray_end->z, scale_2d);

    // Ensure azimuth is in the range [0, 2pi]
    if (azimuth < 0)
    {
      azimuth += 2.0 * M_PI;
    }

    // Quantise.
    int azimuth_bucket = int(azimuth / angular_resolution);
    // Ensure azimuth is in the range [0, pi]
    int elevation_bucket = int((elevation + 0.5 * M_PI) / angular_resolution);

    ASSERT_GE(azimuth_bucket, 0);
    ASSERT_LT(azimuth_bucket, int(image_width));
    ASSERT_GE(elevation_bucket, 0);
    ASSERT_LT(elevation_bucket, int(image_height));

    const unsigned pixel_index = unsigned(azimuth_bucket) + unsigned(elevation_bucket) * image_width;
    ASSERT_LT(pixel_index, unsigned(spherical_image.size()));
    spherical_image[pixel_index] = uint8_t(255u);
  }

#ifdef OHM_WITH_PNG
  savePng("ray-conical.png", spherical_image, image_width, image_height);
#endif  // OHM_WITH_PNG

  // logImage(spherical_image, image_width, image_height);

  validateImage(spherical_image, image_width, image_height, ohm::ray::conical_ray_image,
                ohm::ray::conical_ray_image_size);
}

TEST(RayPattern, Clearing)
{
  // Build a map of a solid line of voxels.
  const unsigned voxel_count = 20;
  ohm::OccupancyMap map;

  // Ensure a single miss erases a single hit.
  map.setHitProbability(0.51f);
  map.setMissProbability(0.0f);

  ohm::Key key(0, 0, 0, 0, 0, 0);
  {
    ohm::Voxel<float> voxel_write(&map, map.layout().occupancyLayer());
    ASSERT_TRUE(voxel_write.isLayerValid());
    for (unsigned i = 0; i < voxel_count; ++i)
    {
      voxel_write.setKey(key);
      ASSERT_TRUE(voxel_write.isValid());
      ohm::integrateHit(voxel_write);
      ASSERT_TRUE(isOccupied(voxel_write));
      map.moveKey(key, 1, 0, 0);
    }
  }

  // Now create a clearing pattern of a single ray large enough to erase all the voxels.
  // We build the line along Y and rotate it to X with a quaternion.
  RayPattern line_pattern;
  line_pattern.addPoint(glm::dvec3(0, map.resolution() * voxel_count, 0));
  ClearingPattern clearing(&line_pattern, false);

  // Set the key to the voxel we want to check.
  key = ohm::Key(0, 0, 0, 0, 0, 0);
  // Translate the ray pattern to start at the centre of the voxel at key.
  const glm::dvec3 pattern_translate = map.voxelCentreGlobal(key);
  // Setup the quaternion to rotate from Y to X axis.
  const glm::dquat rotation = glm::angleAxis(-0.5 * M_PI, glm::dvec3(0, 0, 1));
  ohm::Voxel<const float> voxel_read(&map, map.layout().occupancyLayer());
  ASSERT_TRUE(voxel_read.isLayerValid());
  for (unsigned i = 0; i < voxel_count; ++i)
  {
    // Validate we have an occupied voxel at key.
    voxel_read.setKey(key);
    ASSERT_TRUE(isOccupied(voxel_read));

    // Apply the pattern.
    clearing.apply(&map, pattern_translate, rotation);

    // Validate we have removed a voxel.
    ASSERT_TRUE(!isOccupied(voxel_read));

    // Next key.
    map.moveKey(key, 1, 0, 0);
  }
  voxel_read.reset();
}

TEST(RayPattern, Exclude)
{
  // First build a simple map with three voxels of interest along X: { unobserved, free, occupied, occupied }
  ohm::OccupancyMap map(1.0);
  ohmtestutil::RayPatternExcludeTest(map, map);
}
}  // namespace raypattern


================================================
FILE: tests/ohmtest/RayValidation.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "RayValidation.h"

namespace ohm
{
namespace ray
{
const size_t conical_ray_image_size = 2592;
const uint8_t conical_ray_image[conical_ray_image_size] =  //
  {                                                        //
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff,
    0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff,
    0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
    0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff,
    0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
    0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
  };
}  // namespace ray
}  // namespace ohm


================================================
FILE: tests/ohmtest/RayValidation.h
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef RAYVALIDATION_H
#define RAYVALIDATION_H

#include "OhmTestConfig.h"

namespace ohm
{
namespace ray
{
extern const size_t conical_ray_image_size;
extern const uint8_t conical_ray_image[];
}  // namespace ray
}  // namespace ohm

#endif  // RAYVALIDATION_H


================================================
FILE: tests/ohmtest/RaysQueryTests.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <ohm/Aabb.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyType.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/RaysQuery.h>
#include <ohm/VoxelOccupancy.h>

#include <gtest/gtest.h>

namespace raysquerytests
{
TEST(RaysQuery, Cpu)
{
  const double base_scale = 10.0;
  const std::array<double, 3> query_scale = { 1.2, 1.2, 0.6 };
  const double resolution = 0.1;
  const std::vector<glm::dvec3> rays =  //
    {
      glm::dvec3(0.0), glm::dvec3(base_scale, 0, 0),                      //
      glm::dvec3(0.0), glm::dvec3(0, base_scale, 0),                      //
      glm::dvec3(0.0), glm::dvec3(0, 0, base_scale),                      //
      glm::dvec3(0.0), glm::dvec3(-base_scale, 0, 0),                     //
      glm::dvec3(0.0), glm::dvec3(0, -base_scale, 0),                     //
      glm::dvec3(0.0), glm::dvec3(0, 0, -base_scale),                     //
      glm::dvec3(0.0), glm::dvec3(base_scale, base_scale, 0),             //
      glm::dvec3(0.0), glm::dvec3(0, base_scale, base_scale),             //
      glm::dvec3(0.0), glm::dvec3(base_scale, 0, base_scale),             //
      glm::dvec3(0.0), glm::dvec3(-base_scale, -base_scale, 0),           //
      glm::dvec3(0.0), glm::dvec3(0, -base_scale, -base_scale),           //
      glm::dvec3(0.0), glm::dvec3(-base_scale, 0, -base_scale),           //
      glm::dvec3(0.0), glm::dvec3(base_scale, base_scale, base_scale),    //
      glm::dvec3(0.0), glm::dvec3(-base_scale, -base_scale, -base_scale)  //
    };

  // Build the map.
  ohm::OccupancyMap map(resolution);
  ohm::RayMapperOccupancy mapper(&map);

  // First add just the samples into the map, keeping the rest unobserved.
  for (size_t i = 1; i < rays.size(); i += 2)
  {
    ohm::integrateHit(map, map.voxelKey(rays[i]));
  }

  // Construct the the new RaysQuery object now so we can test the reset as well.
  ohm::RaysQuery query;
  query.setMap(&map);
  query.setVolumeCoefficient(1.0f);

  const std::array<ohm::OccupancyType, 3> expected_terminal_type = { ohm::OccupancyType::kOccupied,
                                                                     ohm::OccupancyType::kOccupied,
                                                                     ohm::OccupancyType::kFree };

  for (size_t iteration = 0; iteration < query_scale.size(); ++iteration)
  {
    // Scale the rays for lookup.
    for (size_t i = 0; i < rays.size(); i += 2)
    {
      query.addRay(rays[i], rays[i + 1] * query_scale[iteration]);
    }

    // Make the query.
    query.execute();

    // Compare results.
    ASSERT_EQ(query.numberOfResults(), rays.size() / 2);
    const double *ranges = query.ranges();
    const double *unobserved_volumes = query.unobservedVolumes();
    const ohm::OccupancyType *terminal_types = query.terminalOccupancyTypes();
    for (size_t i = 0; i < query.numberOfResults(); ++i)
    {
      // First iteration has the whole ray length unobserved. Second has zero.
      if (iteration > 0)
      {
        EXPECT_EQ(unobserved_volumes[i], 0.0) << "iteration " << i;
      }
      else
      {
        // TODO: contrive a reasonable comparative calculation for the unobserved_volume results.
        EXPECT_GT(unobserved_volumes[i], 0.0) << "iteration " << i;
      }

      double ray_length = 0;
      if (iteration < 2)
      {
        // We should reach the last voxel of each input ray on each query.
        const glm::dvec3 ray_delta = rays[i * 2 + 1] - rays[i * 2 + 0];
        ray_length = glm::length(ray_delta);

        // Remove clip the ray for occupied voxels.
        if (terminal_types[i] == ohm::OccupancyType::kOccupied)
        {
          const glm::dvec3 voxel_centre = map.voxelCentreGlobal(map.voxelKey(rays[i * 2 + 1]));
          ohm::Aabb voxel(voxel_centre - 0.5 * glm::dvec3(map.resolution()),
                          voxel_centre + 0.5 * glm::dvec3(map.resolution()));
          // Intersect the ray with the end voxel to work out the entry time and deduct from the expected length.
          std::array<double, 2> hit_times;
          if (voxel.rayIntersect(rays[i * 2 + 0], glm::normalize(rays[i * 2 + 1] - rays[i * 2 + 0]), &hit_times))
          {
            ray_length = hit_times[0];
          }
        }
      }
      else
      {
        // We should reach the last voxel of each query ray on each query.
        const glm::dvec3 ray_delta = query.rays()[i * 2 + 1] - query.rays()[i * 2 + 0];
        ray_length = glm::length(ray_delta);
      }

      EXPECT_NEAR(ranges[i], ray_length, 1e-5) << "iteration " << iteration << " ray " << i;
      EXPECT_EQ(terminal_types[i], expected_terminal_type[iteration]) << "iteration " << iteration << " ray " << i;
    }


    // Now do a full ray integration and redo the comparison.
    if (iteration == 0)
    {
      mapper.integrateRays(rays.data(), rays.size());
    }

    // Hard reset to clear the rays
    query.reset(true);
  }
}
}  // namespace raysquerytests


================================================
FILE: tests/ohmtest/SecondarySampleTests.cpp
================================================
// Copyright (c) 2022
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <ohm/Key.h>
#include <ohm/LineWalk.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/RayMapperSecondarySample.h>
#include <ohm/VoxelData.h>

#include <sstream>
#include <vector>

#include <gtest/gtest.h>

#include "ohmtestcommon/OhmTestUtil.h"

namespace sampletests
{
TEST(SecondarySample, ExcludeOrigin)
{
  std::ostringstream errors;

  // Test the behaviour of the ray flag kRfExcludeOrigin, fundamental to treating dual returns correctly.
  const std::vector<glm::dvec3> rays = { glm::dvec3(0, 0, 0), glm::dvec3(1, 0, 0) };
  ohm::OccupancyMap map(0.1);

  // Offset the map so that (0, 0, 0) is a voxel centre.
  map.setOrigin(glm::dvec3(0.5 * map.resolution()));

  ohm::RayMapperOccupancy occupancy_mapper(&map);
  occupancy_mapper.integrateRays(rays.data(), rays.size(), nullptr, nullptr, ohm::kRfExcludeOrigin);

  ohm::Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  ASSERT_TRUE(occupancy.isLayerValid());
  const auto visit_function = [&errors, &map, &occupancy, &rays](const ohm::Key &voxel_key, double enter_range,
                                                                 double exit_range) -> bool {
    (void)enter_range;
    (void)exit_range;

    occupancy.setKey(voxel_key);
    if (!occupancy.isValid())
    {
      errors << "occupancy voxel not valid\n";
      return false;
    }

    // Ensure voxel at (0, 0, 0) is still unobserved.
    float expected_occupancy = {};
    if (voxel_key == map.voxelKey(rays[0]))
    {
      // First voxel in the ray.
      expected_occupancy = ohm::unobservedOccupancyValue();
    }
    else if (voxel_key == map.voxelKey(rays[1]))
    {
      // Last voxel in ray.
      expected_occupancy = map.hitValue();
    }
    else
    {
      // Intermediary voxel.
      expected_occupancy = map.missValue();
    }

    if (occupancy.data() != expected_occupancy)
    {
      errors << "occupancy mismatch @ " << voxel_key << '\n';
      errors << "  expected: " << expected_occupancy << '\n';
      errors << "  actual: " << occupancy.data() << '\n';
    }

    return true;
  };

  ohm::walkSegmentKeys(ohm::LineWalkContext(map, visit_function), rays[0], rays[1]);

  errors.flush();
  if (!errors.str().empty())
  {
    FAIL() << errors.str();
  }
}

TEST(SecondarySample, DualReturnRay)
{
  // Simulate a lidar dual return scenario.
  std::ostringstream errors;
  // Test the behaviour of the ray flag kRfExcludeOrigin, fundamental to treating dual returns correctly.
  ohm::OccupancyMap map(0.1);

  // Offset the map so that (0, 0, 0) is a voxel centre.
  map.setOrigin(glm::dvec3(0.5 * map.resolution()));

  // Build rays. First ray is the "first return". The second ray is a "dual return" which starts from the sample point
  // of the first ray.
  const std::vector<glm::dvec3> rays =           //
    {                                            //
      glm::dvec3(0, 0, 0), glm::dvec3(1, 0, 0),  //
      glm::dvec3(1, 0, 0), glm::dvec3(2, 0, 0)
    };

  // Add the samples to map, excluding the origin. We lose something here for the first ray as the origin voxel is not
  // cleared, but we also do not try clear the origin of the second return.
  ohm::RayMapperOccupancy occupancy_mapper(&map);
  occupancy_mapper.integrateRays(rays.data(), rays.size(), nullptr, nullptr, ohm::kRfExcludeOrigin);
  ohm::Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  ASSERT_TRUE(occupancy.isLayerValid());
  const auto visit_function = [&errors, &map, &occupancy, &rays](const ohm::Key &voxel_key, double enter_range,
                                                                 double exit_range) -> bool {
    (void)enter_range;
    (void)exit_range;

    occupancy.setKey(voxel_key);
    if (!occupancy.isValid())
    {
      errors << "occupancy voxel not valid\n";
      return false;
    }

    // Ensure voxel at (0, 0, 0) is still unobserved.
    float expected_occupancy = {};
    if (voxel_key == map.voxelKey(rays[0]))
    {
      // First voxel in the ray.
      expected_occupancy = ohm::unobservedOccupancyValue();
    }
    else if (voxel_key == map.voxelKey(rays[1]) || voxel_key == map.voxelKey(rays[3]))
    {
      // One of the return samples.
      expected_occupancy = map.hitValue();
    }
    else
    {
      // Intermediary voxel.
      expected_occupancy = map.missValue();
    }

    if (occupancy.data() != expected_occupancy)
    {
      errors << "occupancy mismatch @ " << voxel_key << '\n';
      errors << "  expected: " << expected_occupancy << '\n';
      errors << "  actual: " << occupancy.data() << '\n';
    }

    return true;
  };

  // Walk the entire ray from "sensor" to "second return".
  ohm::walkSegmentKeys(ohm::LineWalkContext(map, visit_function), rays[0], rays[3]);

  errors.flush();
  if (!errors.str().empty())
  {
    FAIL() << errors.str();
  }
}

TEST(SecondarySample, Stats)
{
  // Validate the stats collected for secondary samples such as for lidar dual returns.
  ohm::OccupancyMap map(0.1, ohm::MapFlag::kSecondarySample);

  // Offset the map so that (0, 0, 0) is a voxel centre.
  map.setOrigin(glm::dvec3(0.5 * map.resolution()));

  // Build a set of "secondary returns". We treat the origin as the first return, and progressively shift the first
  // sample along voxel centres for voxels along the line (0, 0, 0) -> (1, 0, 0). The secondary sample is always at
  // (1, 0, 0).
  std::vector<glm::dvec3> rays;

  ohm::Key key(0, 0, 0, 0, 0, 0);
  const int sample_count = 10;
  for (int i = 0; i < sample_count; ++i)
  {
    rays.emplace_back(glm::dvec3(i * map.resolution(), 0, 0));
    rays.emplace_back(glm::dvec3(1, 0, 0));
  }

  // Work out our expected mean distance from first to second sample and expected standard deviation.
  double expected_mean = 0;
  double expected_std_dev = 0;
  for (size_t i = 0; i < rays.size(); i += 2)
  {
    const double distance = glm::length(rays[i + 1] - rays[i]);
    // Calculate total distance
    expected_mean += distance;
  }
  // Finalise mean distance.
  expected_mean /= sample_count;

  // Calculate variance.
  for (size_t i = 0; i < rays.size(); i += 2)
  {
    const double distance = glm::length(rays[i + 1] - rays[i]);
    const double deviation = distance - expected_mean;
    expected_std_dev += deviation * deviation;
  }

  // Finalise the standard deviation
  expected_std_dev = std::sqrt(expected_std_dev / sample_count);

  // Use the secondary sample mapper.
  ohm::RayMapperSecondarySample sample_mapper(&map);
  ASSERT_TRUE(sample_mapper.valid());
  sample_mapper.integrateRays(rays.data(), rays.size());

  // Check the sample voxel stats.
  ohm::Voxel<const ohm::VoxelSecondarySample> secondary_sample_voxel(
    &map, map.layout().layerIndex(ohm::default_layer::secondarySamplesLayerName()), map.voxelKey(rays[1]));
  ASSERT_TRUE(secondary_sample_voxel.isValid());
  const ohm::VoxelSecondarySample secondary_sample = secondary_sample_voxel.data();

  const double actual_mean = secondarySampleRangeMean(secondary_sample);
  const double actual_std_dev = secondarySampleRangeStdDev(secondary_sample);

  EXPECT_EQ(secondary_sample.count, sample_count);
  EXPECT_NEAR(actual_mean, expected_mean, 1e-2f);
  EXPECT_NEAR(actual_std_dev, expected_std_dev, 1e-2f);
}
}  // namespace sampletests


================================================
FILE: tests/ohmtest/SerialisationTests.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include "ohmtestcommon/OhmTestUtil.h"
#include "ohmtestcommon/WalkSegmentKeysLegacy.h"

#include <ohm/CalculateSegmentKeys.h>
#include <ohm/Key.h>
#include <ohm/KeyList.h>
#include <ohm/LineQuery.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/VoxelOccupancy.h>

#include <ohmtools/OhmCloud.h>
#include <ohmtools/OhmGen.h>

#include <logutil/LogUtil.h>
#include <ohmutil/Profile.h>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <memory>
#include <random>

#include <gtest/gtest.h>

using namespace ohm;

namespace serialisationtests
{
class ProgressDisplay : public SerialiseProgress
{
public:
  unsigned target() const { return target_; }

  void setTarget(unsigned target) { target_ = target; }

  unsigned progress() const { return progress_; }

  void setProgress(unsigned progress) { progress_ = progress; }

  bool quit() const override { return false; }

  void setTargetProgress(unsigned target) override { target_ = target; }
  void incrementProgress(unsigned inc = 1u) override
  {
    const unsigned cur_decimation = 100 * progress_ / target_;
    progress_ += inc;
    const unsigned decimation = 100 * progress_ / target_;
    if (decimation > cur_decimation)
    {
      std::cout << "\r" << progress_ << "/" << target_ << std::flush;
    }
  }

  void reset() { target_ = progress_ = 0; }

private:
  unsigned target_ = 0;
  unsigned progress_ = 0;
};


TEST(Serialisation, Basic)
{
  const char *map_name = "test-map.ohm";
  // Profile profile;
  int error_code = 0;
  const double boundary_distance = 2.5;
  OccupancyMap save_map(0.25);
  OccupancyMap load_map(1);  // Initialise at the wrong resolution. Will be fixed on load.

  // Build a cloud with real samples around a cubic boundary. Does not cover every voxel in the boundary.
  ohmgen::boxRoom(save_map, glm::dvec3(-boundary_distance), glm::dvec3(boundary_distance));

  ProgressDisplay progress;
  std::cout << "Saving" << std::endl;
  error_code = save(map_name, save_map, &progress);
  std::cout << std::endl;
  ASSERT_EQ(error_code, 0);

  std::cout << "Validate header" << std::endl;
  MapVersion version;
  error_code = loadHeader(map_name, load_map, &version);
  ASSERT_EQ(error_code, 0);
  ASSERT_EQ(version.major, kCurrentVersion.major);
  ASSERT_EQ(version.minor, kCurrentVersion.minor);
  ASSERT_EQ(version.patch, kCurrentVersion.patch);

  progress.reset();
  std::cout << "Loading" << std::endl;
  error_code = load(map_name, load_map, &progress);
  std::cout << std::endl;
  ASSERT_EQ(error_code, 0);

  ohmtestutil::compareMaps(load_map, save_map, ohmtestutil::kCfCompareExtended);
}


// Legacy code used to generate the test map for Serialisation.Upgrade tests.
void cubicRoomLegacy(OccupancyMap &map, float boundary_range, int voxel_step)
{
  int extents = int(boundary_range / map.resolution());

  // Set legacy probability values.
  map.setHitProbability(0.7f);
  map.setMissProbability(0.4f);

  const auto build_walls = [&map, extents, voxel_step](int a0, int a1, int a2) {
    const double map_res = map.resolution();
    KeyList ray;
    glm::dvec3 point;
    glm::dvec3 origin = map.origin();
    for (int i = -extents + 1; i <= extents; i += voxel_step)
    {
      for (int j = -extents + 1; j <= extents; j += voxel_step)
      {
        for (int k = 0; k < 2; ++k)
        {
          point = map.origin();
          point[a0] = i * map_res;
          point[a1] = j * map_res;
          point[a2] = (k == 0 ? 1.0 : -1.0) * extents * map_res;
          calculateSegmentKeysLegacy(ray, map, origin, point, false);
          for (auto key : ray)
          {
            ohm::integrateMiss(map, key);
          }
          ohm::integrateHit(map, map.voxelKey(point));
        }
      }
    }
  };

  build_walls(0, 1, 2);
  build_walls(1, 2, 0);
  build_walls(0, 2, 1);
}


/// Test loading older version data files.
///
/// A number of voxel occupancy comparison failures are allowed ( @p allowed_occupancy_mismatches ) to deal with legacy
/// map file issues. Specifically, a maths change in LineWalk.h introduced floating point differences in the line
/// traversal despite being mathematically equivalent. This resulted in a mismatch in the test_map and the loaded map.
/// We expect exactly 222 voxels to have different occupancy values. This only affects map version zero which was not
/// able to be replicated using the new maths.
///
/// @param map_file File to load.
/// @param expected_version Expected version of the loaded file.
/// @param allowed_occupancy_mismatches Number of allowed voxel occupancy comparison failures.
void upgradeTest(const std::string map_file, const ohm::MapVersion &expected_version,
                 unsigned allowed_occupancy_mismatches = 0)
{
  // Profile profile;
  int error_code = 0;
  const float boundary_distance = 5.0f;
  OccupancyMap test_map(0.25);
  OccupancyMap load_map(1);  // Initialise at the wrong resolution. Will be fixed on load.

  // Build a cloud with real samples around a cubic boundary. Does not cover every voxel in the boundary.
  // Need to use the same code with which test-map.0.ohm was generated.
  cubicRoomLegacy(test_map, boundary_distance, 3);

  std::cout << "Validate header" << std::endl;
  MapVersion version;
  error_code = loadHeader(map_file.c_str(), load_map, &version);
  ASSERT_EQ(error_code, 0);
  ASSERT_EQ(version.major, expected_version.major);
  ASSERT_EQ(version.minor, expected_version.minor);
  ASSERT_EQ(version.patch, expected_version.patch);

  std::cout << "Loading" << std::endl;
  error_code = load(map_file.c_str(), load_map);
  std::cout << std::endl;
  ASSERT_EQ(error_code, 0);

  ohmtestutil::compareMaps(load_map, test_map, ohmtestutil::kCfDefault, allowed_occupancy_mismatches);
}

TEST(Serialisation, UpgradeV0)
{
  // Test loading older version data files.
  const std::string map_name = std::string(ohmtestutil::applicationDir()) + "test-map.0.ohm";
  // A maths change in LineWalk.h introduced floating point differences in the line traversal despite being
  // mathematically equivalent. This resulted in a mismatch in the test_map and the loaded map. We expect exactly 222
  // voxels to have different occupancy values.
  //
  // All the other map versions from 0.1.0 to 0.4.x have been generated with the same maths change applied to their
  // generation.
  const unsigned k_allowed_occupancy_mismatches = 222;
  upgradeTest(map_name, ohm::MapVersion(0, 0, 0), k_allowed_occupancy_mismatches);
}


TEST(Serialisation, UpgradeV0_1_0)
{
  // Test loading older version data files.
  const std::string map_name = std::string(ohmtestutil::applicationDir()) + "test-map.0.1.0.ohm";
  upgradeTest(map_name, ohm::MapVersion(0, 1, 0));
}


TEST(Serialisation, UpgradeV0_2_0)
{
  // Test loading older version data files.
  const std::string map_name = std::string(ohmtestutil::applicationDir()) + "test-map.0.2.0.ohm";
  upgradeTest(map_name, ohm::MapVersion(0, 2, 0));
}


TEST(Serialisation, UpgradeV0_3_0)
{
  // Test loading older version data files.
  const std::string map_name = std::string(ohmtestutil::applicationDir()) + "test-map.0.3.0.ohm";
  OccupancyMap load_map(1);  // Initialise at the wrong resolution. Will be fixed on load.

  MapVersion version;
  int error_code = loadHeader(map_name.c_str(), load_map, &version);
  std::cout << "Note: upgrade from 0.3.x is deprecated and will fail. Validating deprecation return code." << std::endl;
  ASSERT_EQ(error_code, ohm::kSeDeprecatedVersion);
  ASSERT_EQ(version.major, 0);
  ASSERT_EQ(version.minor, 3);
  ASSERT_EQ(version.patch, 0);
}


TEST(Serialisation, UpgradeV0_4_0)
{
  // Test loading older version data files.
  const std::string map_name = std::string(ohmtestutil::applicationDir()) + "test-map.0.4.0.ohm";

  // voxels to have different occupancy values.
  upgradeTest(map_name, ohm::MapVersion(0, 4, 0));
}
}  // namespace serialisationtests


================================================
FILE: tests/ohmtest/TestMain.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <gtest/gtest.h>

#include "ohmtestcommon/OhmTestUtil.h"

int main(int argc, char **argv)
{
  ohmtestutil::setApplicationPath(argv[0]);

  ::testing::InitGoogleTest(&argc, argv);
  int err = RUN_ALL_TESTS();
  return err;
}


================================================
FILE: tests/ohmtest/TouchTimeTests.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <gtest/gtest.h>

#include <ohm/DefaultLayer.h>
#include <ohm/Key.h>
#include <ohm/NdtMap.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapperNdt.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/Voxel.h>
#include <ohm/VoxelTouchTime.h>

#include <glm/gtx/norm.hpp>
#include <glm/vec3.hpp>

#include <random>

namespace touchtime
{
void testTouchTime(ohm::OccupancyMap &map, ohm::RayMapper &mapper)
{
  const unsigned iterations = 10;
  const unsigned ray_count = 1000u;
  const double time_base = 1000.0;
  const double time_step = 0.5;
  std::vector<glm::dvec3> rays;
  std::vector<double> timestamps;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-1.0, 1.0);

  // Set the map origin to avoid (0, 0, 0) being on a voxel boundary.
  map.setOrigin(glm::dvec3(-0.5 * map.resolution()));

  ASSERT_TRUE(map.touchTimeEnabled());

  rays.reserve(2 * ray_count);
  for (unsigned i = 0; i < iterations; ++i)
  {
    rays.clear();
    timestamps.clear();
    for (unsigned r = 0; r < ray_count; ++r)
    {
      // Sample is at the origin. We'll build random rays around that.
      glm::dvec3 origin;
      do
      {
        origin = glm::dvec3(uniform(rng), uniform(rng), uniform(rng));
      } while (glm::length2(origin) < 1e-6);  // Make sure it's not degenerate.
      // Normalise the origin ray, then expand it out to be larger than a single voxel.
      origin = glm::normalize(origin);
      origin *= map.resolution() * 3;
      rays.emplace_back(origin);
      rays.emplace_back(glm::dvec3(0));

      timestamps.emplace_back(time_base + r * time_step);
    }
    // Now use the ray mapper
    mapper.integrateRays(rays.data(), rays.size(), nullptr, timestamps.data(), ohm::kRfDefault);

    // Check the result.
    ohm::Voxel<uint32_t> time_voxel(&map, map.layout().layerIndex(ohm::default_layer::touchTimeLayerName()));
    ASSERT_TRUE(time_voxel.isLayerValid());
    time_voxel.setKey(map.voxelKey(glm::dvec3(0)));
    ASSERT_TRUE(time_voxel.isValid());

    const double extracted_time = ohm::decodeVoxelTouchTime(map.firstRayTime(), time_voxel.data());
    EXPECT_EQ(extracted_time, timestamps.back());

    time_voxel.write(0u);
    EXPECT_EQ(time_voxel.data(), 0);
  }
}

TEST(TouchTime, WithOccupancy)
{
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kTouchTime);
  ohm::RayMapperOccupancy mapper(&map);
  testTouchTime(map, mapper);
}

TEST(TouchTime, WithNdt)
{
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kTouchTime);
  ohm::NdtMap ndt_map(&map, true);
  ohm::RayMapperNdt mapper(&ndt_map);
  testTouchTime(map, mapper);
}
}  // namespace touchtime


================================================
FILE: tests/ohmtest/TraversalTests.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <gtest/gtest.h>

#include <ohm/Key.h>
#include <ohm/NdtMap.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapperNdt.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/Voxel.h>

#include <ohmtestcommon/TraversalTest.h>

#include <glm/vec3.hpp>

namespace traversal
{
TEST(Traversal, Through)
{
  // For this test, we want to ensure that the traversal accumulates correctly. To do that we select a set of rays
  // which pass through a voxel perpendicular to various faces of that voxel. In this way we know that the traversal
  // should accumulate at approximately the voxel resolution per ray.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal);
  ohm::RayMapperOccupancy mapper(&map);
  ohmtestcommon::traversal::testThrough(map, mapper);
}

TEST(Traversal, ThroughNdt)
{
  // As Through, but using the NDT mapper.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal);
  ohm::NdtMap ndt(&map, true);
  ohm::RayMapperNdt mapper(&ndt);
  ohmtestcommon::traversal::testThrough(map, mapper);
}

TEST(Traversal, Into)
{
  // For this test, we want to ensure that the traversal accumulates correctly in the final voxel. To do that we select
  // a set of rays which end in a voxel a voxel entering perpendicular to various faces of that voxel. In this way we
  // know that the traversal should accumulate at approximately half the voxel resolution per ray.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal);
  ohm::RayMapperOccupancy mapper(&map);
  ohmtestcommon::traversal::testInto(map, mapper);
}

TEST(Traversal, IntoNdt)
{
  // As Into, but using the NDT mapper.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal);
  ohm::NdtMap ndt(&map, true);
  ohm::RayMapperNdt mapper(&ndt);
  ohmtestcommon::traversal::testInto(map, mapper);
}
}  // namespace traversal


================================================
FILE: tests/ohmtest/TsdfTests.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <gtest/gtest.h>

#include <ohm/LineWalk.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/RayMapperTsdf.h>
#include <ohm/VoxelData.h>

namespace tsdf
{
TEST(Tsdf, Basic)
{
  // Really not sure what sort of test to do here, so we'll keep it really simple.
  // For each ray we'll start with a clear map and populate the ray. They we'll check each voxel has the same
  // value as ohm::computeDistance() reports.
  const double resolution = 0.1;
  const glm::u8vec3 region_size(32);

  const std::vector<glm::dvec3> rays = {
    glm::dvec3(0.0), glm::dvec3(1.0, 0.0, 0.0),  glm::dvec3(0.0), glm::dvec3(-1.0, 0.0, 0.0),
    glm::dvec3(0.0), glm::dvec3(1.0, 1.0, 0.0),  glm::dvec3(0.0), glm::dvec3(-1.0, 1.0, 0.0),
    glm::dvec3(0.0), glm::dvec3(1.0, 0.0, 0.0),  glm::dvec3(0.0), glm::dvec3(1.0, -1.0, 0.0),
    glm::dvec3(0.0), glm::dvec3(-1.0, 0.0, 1.0), glm::dvec3(0.0), glm::dvec3(1.0, 1.0, 1.0),
    glm::dvec3(0.0), glm::dvec3(-1.0, 1.0, 1.0), glm::dvec3(0.0), glm::dvec3(1.0, 0.0, 1.0),
    glm::dvec3(0.0), glm::dvec3(1.0, -1.0, 1.0), glm::dvec3(0.0), glm::dvec3(-1.0, 0.0, 1.0),
    glm::dvec3(0.0), glm::dvec3(1.0, 1.0, -1.0), glm::dvec3(0.0), glm::dvec3(-1.0, 1.0, -1.0),
    glm::dvec3(0.0), glm::dvec3(1.0, 0.0, -1.0), glm::dvec3(0.0), glm::dvec3(1.0, -1.0, -1.0),
  };

  // Test core voxel mean positioning
  ohm::OccupancyMap map(resolution, region_size, ohm::MapFlag::kTsdf);
  ohm::RayMapperTsdf tsdf_mapper(&map);

  // Offset the map origin to trace between voxel centres.
  map.setOrigin(glm::dvec3(-0.5 * map.resolution()));
  // Set large truncation distance so our values matche computeDistance() results.
  tsdf_mapper.setDefaultTruncationDistance(10.0);

  for (size_t i = 0; i < rays.size(); i += 2)
  {
    map.clear();
    // Must initialise the voxel reference after the map clear or it may become invalid.
    ohm::Voxel<const ohm::VoxelTsdf> tsdf(&map, map.layout().layerIndex(ohm::default_layer::tsdfLayerName()));
    ASSERT_TRUE(tsdf.isLayerValid()) << "ray index " << i / 2u;
    // Trace ray.
    tsdf_mapper.integrateRays(&rays[i], 2, nullptr, nullptr, 0);

    const auto visit_func = [&](const ohm::Key &key, double enter_range, double exit_range) -> bool  //
    {
      (void)enter_range;
      (void)exit_range;
      ohm::setVoxelKey(key, tsdf);
      EXPECT_TRUE(tsdf.isValid()) << "ray index " << i / 2u;
      if (!tsdf.isValid())
      {
        return false;
      }
      const ohm::VoxelTsdf tsdf_data = tsdf.data();
      EXPECT_NEAR(tsdf_data.distance, ohm::computeDistance(rays[i], rays[i + 1], map.voxelCentreGlobal(key)), 1e-6)
        << "ray index " << i / 2u;
      return true;
    };

    ohm::walkSegmentKeys(ohm::LineWalkContext(map, visit_func), rays[i], rays[i + 1]);
  }
}

TEST(Tsdf, Truncation)
{
  // Truncation test: ensure that the reported distances are truncated to tsdf_mapper.setDefaultTruncationDistance()
  const double resolution = 0.1;
  const glm::u8vec3 region_size(32);

  const std::vector<glm::dvec3> rays = {
    glm::dvec3(0.0), glm::dvec3(1.0, 0.0, 0.0),  glm::dvec3(0.0), glm::dvec3(-1.0, 0.0, 0.0),
    glm::dvec3(0.0), glm::dvec3(1.0, 1.0, 0.0),  glm::dvec3(0.0), glm::dvec3(-1.0, 1.0, 0.0),
    glm::dvec3(0.0), glm::dvec3(1.0, 0.0, 0.0),  glm::dvec3(0.0), glm::dvec3(1.0, -1.0, 0.0),
    glm::dvec3(0.0), glm::dvec3(-1.0, 0.0, 1.0), glm::dvec3(0.0), glm::dvec3(1.0, 1.0, 1.0),
    glm::dvec3(0.0), glm::dvec3(-1.0, 1.0, 1.0), glm::dvec3(0.0), glm::dvec3(1.0, 0.0, 1.0),
    glm::dvec3(0.0), glm::dvec3(1.0, -1.0, 1.0), glm::dvec3(0.0), glm::dvec3(-1.0, 0.0, 1.0),
    glm::dvec3(0.0), glm::dvec3(1.0, 1.0, -1.0), glm::dvec3(0.0), glm::dvec3(-1.0, 1.0, -1.0),
    glm::dvec3(0.0), glm::dvec3(1.0, 0.0, -1.0), glm::dvec3(0.0), glm::dvec3(1.0, -1.0, -1.0),
  };

  // Test core voxel mean positioning
  ohm::OccupancyMap map(resolution, region_size, ohm::MapFlag::kTsdf);
  ohm::RayMapperTsdf tsdf_mapper(&map);

  // Offset the map origin to trace between voxel centres.
  map.setOrigin(glm::dvec3(-0.5 * map.resolution()));
  // Set a small truncation distance. We expect voxels to quickly reach the truncation distance.
  tsdf_mapper.setDefaultTruncationDistance(float(map.resolution()));

  for (size_t i = 0; i < rays.size(); i += 2)
  {
    map.clear();

    // We integrate ray twice, ensuring the distance stays truncated.
    for (int j = 0; j < 2; ++j)
    {
      // Must initialise the voxel reference after the map clear or it may become invalid.
      ohm::Voxel<const ohm::VoxelTsdf> tsdf(&map, map.layout().layerIndex(ohm::default_layer::tsdfLayerName()));
      ASSERT_TRUE(tsdf.isLayerValid()) << "ray index " << i / 2u;

      // Trace ray.
      tsdf_mapper.integrateRays(&rays[i], 2, nullptr, nullptr, 0);

      const auto visit_func = [&](const ohm::Key &key, double enter_range, double exit_range) -> bool  //
      {
        (void)enter_range;
        (void)exit_range;
        ohm::setVoxelKey(key, tsdf);
        EXPECT_TRUE(tsdf.isValid()) << "ray index " << i / 2u;
        if (!tsdf.isValid())
        {
          return false;
        }
        const ohm::VoxelTsdf tsdf_data = tsdf.data();
        EXPECT_NEAR(tsdf_data.distance,
                    std::min(tsdf_mapper.defaultTruncationDistance(),
                             ohm::computeDistance(rays[i], rays[i + 1], map.voxelCentreGlobal(key))),
                    1e-6)
          << "ray index " << i / 2u;
        return true;
      };

      ohm::walkSegmentKeys(ohm::LineWalkContext(map, visit_func), rays[i], rays[i + 1]);
    }
  }
}
}  // namespace tsdf


================================================
FILE: tests/ohmtest/VoxelMeanTests.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestConfig.h"

#include <gtest/gtest.h>

#include <ohm/Aabb.h>
#include <ohm/KeyList.h>
#include <ohm/MapChunk.h>
#include <ohm/MapProbability.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/VoxelData.h>

#include <ohmtools/OhmCloud.h>

#include <logutil/LogUtil.h>

#include <ohmutil/GlmStream.h>

#include <chrono>
#include <fstream>
#include <iostream>
#include <random>

using namespace ohm;

namespace voxelmean
{
typedef std::chrono::high_resolution_clock TimingClock;

struct VoxelMeanResult
{
  glm::dvec3 expected_position;
  glm::dvec3 reported_position;
  glm::dvec3 voxel_centre;
};

void printVoxelPositionResults(const std::vector<VoxelMeanResult> &mean_results, bool common_voxel_centre,
                               double map_resolution)
{
  if (mean_results.empty())
  {
    return;
  }

  std::cout << std::setfill(' ');

  // Error checking first, then pretty reporting.
  for (const VoxelMeanResult &result : mean_results)
  {
    EXPECT_NEAR(result.expected_position.x, result.reported_position.x, map_resolution / 1e3);
    EXPECT_NEAR(result.expected_position.y, result.reported_position.y, map_resolution / 1e3);
    EXPECT_NEAR(result.expected_position.z, result.reported_position.z, map_resolution / 1e3);
  }

  if (common_voxel_centre)
  {
    std::cout << "Voxel centre: " << mean_results.front().voxel_centre << std::endl;
  }

  glm::dvec3 pos_error;
  const int col = 30;
  std::ostringstream s;
  std::cout << std::left;
  std::cout << std::setw(col) << "Input position" << std::setw(col) << "Voxel mean";
  if (!common_voxel_centre)
  {
    std::cout << std::setw(col) << "Centre";
  }
  std::cout << std::setw(col) << "error" << '\n';

  for (const VoxelMeanResult &result : mean_results)
  {
    pos_error = result.expected_position - result.reported_position;

    s << result.expected_position;
    std::cout << std::setw(col) << s.str();
    s.str(std::string());
    s << result.reported_position;
    std::cout << std::setw(col) << s.str();
    s.str(std::string());
    if (!common_voxel_centre)
    {
      s << result.voxel_centre;
      std::cout << std::setw(col) << s.str();
      s.str(std::string());
    }
    s << pos_error;
    std::cout << std::setw(col) << s.str();
    s.str(std::string());
    std::cout << std::endl;
  }
}

TEST(VoxelMean, Basic)
{
  const double resolution = 0.5;
  const glm::u8vec3 region_size(32);

  // Test core voxel mean positioning
  OccupancyMap map(resolution, region_size, MapFlag::kVoxelMean);

  Voxel<ohm::VoxelMean> mean(&map, map.layout().meanLayer(), map.voxelKey(glm::dvec3(0.5 * resolution)));
  ASSERT_TRUE(mean.isLayerValid());
  ASSERT_TRUE(mean.isValid());

  static const glm::dvec3 positions[] =  //
    {
      //
      glm::dvec3(0.0),   //
      glm::dvec3(0.05),  //
      glm::dvec3(0.15),  //
      glm::dvec3(0.20),  //
      glm::dvec3(0.25),  //
      glm::dvec3(0.30),  //
      glm::dvec3(0.35),  //
      glm::dvec3(0.40),  //
      glm::dvec3(0.45),  //
      glm::dvec3(0.50),  //
    };

  std::vector<VoxelMeanResult> results;

  for (unsigned i = 0; i < sizeof(positions) / sizeof(positions[0]); ++i)
  {
    VoxelMeanResult sub_vox;

    setPositionSafe(mean, positions[i], 1);

    sub_vox.expected_position = positions[i];
    sub_vox.reported_position = positionSafe(mean);
    sub_vox.voxel_centre = map.voxelCentreGlobal(mean.key());

    results.push_back(sub_vox);
  }
  mean.reset();

  printVoxelPositionResults(results, true, map.resolution());
}

TEST(VoxelMean, LayoutToggle)
{
  // Test enabling and disabling voxel mean layout.
  const double resolution = 0.5;
  const glm::u8vec3 region_size(32);

  // Test core voxel mean positioning
  OccupancyMap map(resolution, region_size);

  // First integrate without voxel mean positioning
  glm::dvec3 sample_pos = glm::dvec3(1.1);
  const Key key(map.voxelKey(sample_pos));
  integrateHit(map, key);

  Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  Voxel<ohm::VoxelMean> mean(&map, map.layout().meanLayer());
  ASSERT_TRUE(occupancy.isLayerValid());
  ASSERT_FALSE(mean.isLayerValid());

  setVoxelKey(map.voxelKey(sample_pos), mean, occupancy);
  ASSERT_TRUE(occupancy.isValid());
  ASSERT_FALSE(mean.isValid());

  glm::dvec3 voxel_centre = map.voxelCentreGlobal(occupancy.key());
  glm::dvec3 voxel_pos = positionSafe(mean);

  EXPECT_EQ(voxel_centre.x, voxel_pos.x);
  EXPECT_EQ(voxel_centre.y, voxel_pos.y);
  EXPECT_EQ(voxel_centre.z, voxel_pos.z);
  occupancy.reset();
  mean.reset();

  // Now enable voxel mean positioning.
  // voxel becomes invalid.
  // Cache the current layout before voxel mean. We'll restore this layout later.
  MapLayout cached_layout(map.layout());

  map.addVoxelMeanLayer();

  occupancy = Voxel<const float>(&map, map.layout().occupancyLayer(), key);
  mean = Voxel<ohm::VoxelMean>(&map, map.layout().meanLayer(), key);
  ASSERT_TRUE(occupancy.isValid());
  ASSERT_TRUE(mean.isValid());

  // Set the voxel position.
  setPositionSafe(mean, sample_pos);

  // Position should no longer match the voxel centre.
  voxel_pos = positionSafe(mean);
  EXPECT_NE(voxel_centre.x, voxel_pos.x);
  EXPECT_NE(voxel_centre.y, voxel_pos.y);
  EXPECT_NE(voxel_centre.z, voxel_pos.z);

  EXPECT_NEAR(voxel_pos.x, sample_pos.x, resolution / 1000.0);
  EXPECT_NEAR(voxel_pos.y, sample_pos.y, resolution / 1000.0);
  EXPECT_NEAR(voxel_pos.z, sample_pos.z, resolution / 1000.0);
  occupancy.reset();
  mean.reset();

  // Now remove voxel mean positioning.
  map.updateLayout(cached_layout);
  occupancy = Voxel<const float>(&map, map.layout().occupancyLayer(), key);
  mean = Voxel<ohm::VoxelMean>(&map, map.layout().meanLayer(), key);
  ASSERT_TRUE(occupancy.isValid());
  ASSERT_FALSE(mean.isValid());

  // Expect occupancy to be unchanged.
  EXPECT_TRUE(isOccupied(occupancy));

  // Expect the position to match the voxel centre.
  voxel_pos = positionSafe(mean);
  EXPECT_EQ(voxel_centre.x, voxel_pos.x);
  EXPECT_EQ(voxel_centre.y, voxel_pos.y);
  EXPECT_EQ(voxel_centre.z, voxel_pos.z);
  occupancy.reset();
  mean.reset();
}

TEST(VoxelMean, Cpu)
{
  const double resolution = 0.5;
  const glm::u8vec3 region_size(32);

  // Make a ray.
  std::vector<glm::dvec3> rays;
  rays.emplace_back(glm::dvec3(0));
  rays.emplace_back(glm::dvec3(1.1));
  rays.emplace_back(glm::dvec3(0));
  rays.emplace_back(glm::dvec3(-2.4));
  rays.emplace_back(glm::dvec3(0));
  rays.emplace_back(glm::dvec3(1, -2.2, -3.3));
  rays.emplace_back(glm::dvec3(0));
  rays.emplace_back(glm::dvec3(2.5, -1.6, -3.38));

  // Test basic map populate using GPU and ensure it matches CPU (close enough).
  OccupancyMap map(resolution, region_size, MapFlag::kVoxelMean);
  RayMapperOccupancy(&map).integrateRays(rays.data(), unsigned(rays.size()));

  std::vector<VoxelMeanResult> results;
  Voxel<ohm::VoxelMean> voxel(&map, map.layout().meanLayer());
  for (size_t i = 1; i < rays.size(); i += 2)
  {
    voxel.setKey(map.voxelKey(rays[i]));
    if (voxel.isValid())
    {
      VoxelMeanResult sub_vox;

      sub_vox.expected_position = rays[i];
      sub_vox.reported_position = positionSafe(voxel);
      sub_vox.voxel_centre = map.voxelCentreGlobal(voxel.key());

      results.push_back(sub_vox);
    }
  }
  voxel.reset();

  printVoxelPositionResults(results, false, map.resolution());
}
}  // namespace voxelmean


================================================
FILE: tests/ohmtestcommon/CMakeLists.txt
================================================
# Setup of GTEST changed at CMake 3.5.
cmake_minimum_required(VERSION 3.5)



# Eigen required to support some tests - NDT in particular
find_package(Eigen3 QUIET)

set(SOURCES
  OhmTestUtil.cpp
  OhmTestUtil.h
  RayPatternTestUtil.h
  TraversalTest.cpp
  TraversalTest.h
  WalkSegmentKeysLegacy.cpp
  WalkSegmentKeysLegacy.h
)

if(Eigen3_FOUND)
  message(STATUS "Eigen3 found. Adding CovarianceTestUtil.")
  list(APPEND SOURCES
    CovarianceTestUtil.cpp
    CovarianceTestUtil.h
  )
endif(Eigen3_FOUND)

add_library(ohmtestcommon STATIC ${SOURCES})

set_target_properties(ohmtestcommon PROPERTIES FOLDER tests)

target_include_directories(ohmtestcommon
  PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/tests>
)

target_link_libraries(ohmtestcommon PUBLIC ohmtools ohm ohmutil)

target_link_libraries(ohmtestcommon
  PRIVATE
    ${GTEST_LIBRARIES}
    $<BUILD_INTERFACE:$<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>>
    $<BUILD_INTERFACE:$<$<BOOL:${OHM_FEATURE_EIGEN}>:Eigen3::Eigen>>
)

add_dependencies(ohmtestcommon marshal_test_data)

target_include_directories(ohmtestcommon
  SYSTEM PRIVATE
    "${GTEST_INCLUDE_DIRS}"
    $<$<NOT:$<BOOL:${OHM_TES_DEBUG}>>:${3ES_INCLUDE_DIRS}>
  )

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

# install(TARGETS ohmtest DESTINATION bin)


================================================
FILE: tests/ohmtestcommon/CovarianceTestUtil.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "CovarianceTestUtil.h"

#include <algorithm>
#include <vector>

// The reference code below was provided by Jason Williams as a guide to implementing the NDT map epsecially with
// consideration to using a packed covariance matrix. We use this code to validate the NDT results.
// Note: Jason's code was written using Eigen for mathematical operations which we've converted to glm. The ohm
// library prefers glm as it is a much ligher weight library and we do not require Eigen's advanced functionality.

namespace ohmtestutil
{
void initialiseTestVoxel(CovTestVoxel *ref_voxel, float voxel_resolution)
{
  initialiseCovariance(ref_voxel, voxel_resolution);
  ref_voxel->mean[0] = ref_voxel->mean[1] = ref_voxel->mean[2] = 0;
  ref_voxel->point_count = 0;
}

// dot product of j-th and k-th columns of A
// A is (4,3), assumed to be packed as follows, where z is non-represented zero
// 0 1 3
// z 2 4
// z z 5
// 6 7 8
double packed_dot(const double *A, const size_t j, const size_t k)
{
  const static size_t col_first_el[] = { 0, 1, 3 };
  double d = A[6 + k] * A[6 + j];
  const size_t indj = col_first_el[j], indk = col_first_el[k], m = std::min(j, k);
  for (size_t i = 0; i <= m; ++i)
  {
    d += A[indj + i] * A[indk + i];
  }
  return d;
}

void updateHit(CovTestVoxel *cov, const glm::dvec3 &sample)
{
  const double num_pt = double(cov->point_count);
  const double one_on_num_pt_plus_one = 1.0 / (num_pt + 1.0);
  glm::dvec3 mean(cov->mean[0], cov->mean[1], cov->mean[2]);
  const glm::dvec3 diff = sample - mean;
  const double sc_1 = num_pt ? std::sqrt(num_pt * one_on_num_pt_plus_one) : 1;
  const double sc_2 = one_on_num_pt_plus_one * std::sqrt(num_pt);
  std::vector<double> A(9);
  for (unsigned i = 0; i < 6; ++i)
  {
    A[i] = sc_1 * cov->trianglar_covariance[i];
  }
  for (unsigned i = 0; i < 3; ++i)
  {
    A[i + 6] = sc_2 * diff[i];
  }
  for (unsigned k = 0; k < 3; ++k)
  {
    const unsigned ind1 = (k * (k + 3)) >> 1,  // packed index of (k,k) term
      indk = ind1 - k;                         // packed index of (1,k)
    const double ak = std::sqrt(packed_dot(&A[0], k, k));
    cov->trianglar_covariance[ind1] = float(ak);
    if (ak > 0.0)
    {
      const double aki = 1.0 / ak;
      for (unsigned j = k + 1; j < 3; ++j)
      {
        const unsigned indj = (j * (j + 1)) >> 1, indkj = indj + k;
        double c = packed_dot(&A[0], j, k) * aki;
        cov->trianglar_covariance[indkj] = float(c);
        c *= aki;
        A[j + 6] -= c * A[k + 6];
        for (unsigned l = 0; l <= k; ++l)
        {
          A[indj + l] -= c * A[indk + l];
        }
      }
    }
  }

  mean = (num_pt * mean + sample) * one_on_num_pt_plus_one;
  cov->mean[0] = mean[0];
  cov->mean[1] = mean[1];
  cov->mean[2] = mean[2];
  ++cov->point_count;
}

bool validate(const glm::dvec3 &mean, unsigned point_count, const ohm::CovarianceVoxel &cov, const CovTestVoxel &ref)
{
  // Quantisation in the mean storage create more signficant absolute errors in the covariance and mean.
  const double epsilon_cov = 1e-2;
  const double epsilon_mean = 1e-1;
  for (int i = 0; i < 6; ++i)
  {
    if (std::abs(cov.trianglar_covariance[i] - ref.trianglar_covariance[i]) > epsilon_cov)
    {
      return false;
    }
  }

  if (point_count != ref.point_count)
  {
    return false;
  }

  const glm::dvec3 ref_mean(ref.mean[0], ref.mean[1], ref.mean[2]);
  const double mean_diff = glm::length(mean - ref_mean);
  if (std::abs(mean_diff) > epsilon_mean)
  {
    return false;
  }

  return true;
}
}  // namespace ohmtestutil


================================================
FILE: tests/ohmtestcommon/CovarianceTestUtil.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef CovarianceTestUtil_H
#define CovarianceTestUtil_H

#include <ohm/OhmConfig.h>

// Required before CovarianceVoxel.h
#include <glm/gtx/norm.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

#include <ohm/CovarianceVoxel.h>

namespace ohmtestutil
{
/// Ndt reference data.
struct CovTestVoxel : public ohm::CovarianceVoxel
{
  double mean[3];
  unsigned point_count;
};

void initialiseTestVoxel(CovTestVoxel *ref_voxel, float sensor_noise);
double packed_dot(const double *A, const size_t j, const size_t k);
void updateHit(CovTestVoxel *cov, const glm::dvec3 &sample);
bool validate(const glm::dvec3 &mean, unsigned point_count, const ohm::CovarianceVoxel &cov, const CovTestVoxel &ref);
}  // namespace ohmtestutil

#endif  // CovarianceTestUtil_H


================================================
FILE: tests/ohmtestcommon/OhmTestUtil.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "OhmTestUtil.h"

#include <ohm/Key.h>
#include <ohm/MapChunk.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/OccupancyMap.h>
#include <ohm/VoxelData.h>

#include <gtest/gtest.h>

#include <stdio.h> /* defines FILENAME_MAX */
#ifdef WIN32
#include <direct.h>
#define GetCurrentDir _getcwd
#else
#include <unistd.h>
#define GetCurrentDir getcwd
#endif

using namespace ohm;

namespace ohmtestutil
{
std::string app_dir;

const char *applicationDir()
{
  return (!app_dir.empty()) ? app_dir.c_str() : "";
}

void setApplicationPath(const char *path)
{
  app_dir = path;
  // Strip the executable name
  auto lastSeparator = app_dir.find_last_of("/\\");
  if (lastSeparator != std::string::npos)
  {
    app_dir = app_dir.substr(0, lastSeparator + 1);
  }
  else
  {
    // Current dir.
    app_dir.clear();
  }
}

bool compareLayout(const OccupancyMap &map, const OccupancyMap &reference_map)
{
  const MapLayout &layout = map.layout();
  const MapLayout &ref_layout = reference_map.layout();
  bool layout_matches = layout.layerCount() == ref_layout.layerCount();
  EXPECT_EQ(layout.layerCount(), ref_layout.layerCount());

  for (unsigned i = 0; layout_matches && i < layout.layerCount(); ++i)
  {
    const MapLayer &layer = layout.layer(i);
    const MapLayer &ref_layer = ref_layout.layer(i);

    layout_matches = layout_matches && strcmp(layer.name(), ref_layer.name()) == 0;
    EXPECT_STREQ(layer.name(), ref_layer.name());
    layout_matches = layout_matches && layer.layerIndex() == ref_layer.layerIndex();
    EXPECT_EQ(layer.layerIndex(), ref_layer.layerIndex());
    layout_matches = layout_matches && layer.subsampling() == ref_layer.subsampling();
    EXPECT_EQ(layer.subsampling(), ref_layer.subsampling());
    layout_matches = layout_matches && layer.flags() == ref_layer.flags();
    EXPECT_EQ(layer.flags(), ref_layer.flags());
    layout_matches = layout_matches && layer.dimensions(map.regionVoxelDimensions()) ==
                                         ref_layer.dimensions(reference_map.regionVoxelDimensions());
    EXPECT_EQ(layer.dimensions(map.regionVoxelDimensions()),
              ref_layer.dimensions(reference_map.regionVoxelDimensions()));
    layout_matches = layout_matches && layer.layerByteSize(map.regionVoxelDimensions()) ==
                                         ref_layer.layerByteSize(reference_map.regionVoxelDimensions());
    EXPECT_EQ(layer.layerByteSize(map.regionVoxelDimensions()),
              ref_layer.layerByteSize(reference_map.regionVoxelDimensions()));

    VoxelLayoutConst voxel_layout = layer.voxelLayout();
    VoxelLayoutConst ref_voxel_layout = ref_layer.voxelLayout();

    layout_matches = layout_matches && voxel_layout.memberCount() == ref_voxel_layout.memberCount();
    EXPECT_EQ(voxel_layout.memberCount(), ref_voxel_layout.memberCount());
    layout_matches = layout_matches && voxel_layout.voxelByteSize() == ref_voxel_layout.voxelByteSize();
    EXPECT_EQ(voxel_layout.voxelByteSize(), ref_voxel_layout.voxelByteSize());

    for (unsigned v = 0; layout_matches && v < voxel_layout.memberCount(); ++v)
    {
      layout_matches = layout_matches && strcmp(voxel_layout.memberName(v), ref_voxel_layout.memberName(v)) == 0;
      EXPECT_STREQ(voxel_layout.memberName(v), ref_voxel_layout.memberName(v));
      layout_matches = layout_matches && voxel_layout.memberType(v) == ref_voxel_layout.memberType(v);
      EXPECT_EQ(voxel_layout.memberType(v), ref_voxel_layout.memberType(v));
      layout_matches = layout_matches && voxel_layout.memberClearValue(v) == ref_voxel_layout.memberClearValue(v);
      EXPECT_EQ(voxel_layout.memberClearValue(v), ref_voxel_layout.memberClearValue(v));
    }
  }

  return layout_matches;
}


void compareMaps(const OccupancyMap &map, const OccupancyMap &reference_map, unsigned compare_flags,
                 unsigned allowed_occupancy_mismatch_count)
{
  compareMaps(map, reference_map, glm::dvec3(-std::numeric_limits<double>::infinity()),
              glm::dvec3(std::numeric_limits<double>::infinity()), compare_flags, allowed_occupancy_mismatch_count);
}


void compareMaps(const OccupancyMap &map, const OccupancyMap &reference_map, const glm::dvec3 &min_ext,
                 const glm::dvec3 &max_ext, unsigned compare_flags, unsigned allowed_occupancy_mismatch_count)
{
  const bool full_extents = glm::all(glm::equal(min_ext, glm::dvec3(-std::numeric_limits<double>::infinity()))) &&
                            glm::all(glm::equal(max_ext, glm::dvec3(std::numeric_limits<double>::infinity())));

  if (compare_flags & kCfGeneral)
  {
    EXPECT_EQ(map.resolution(), reference_map.resolution());
    EXPECT_EQ(map.regionSpatialResolution(), reference_map.regionSpatialResolution());
    EXPECT_EQ(map.regionVoxelDimensions(), reference_map.regionVoxelDimensions());
    EXPECT_EQ(map.regionVoxelVolume(), reference_map.regionVoxelVolume());
    EXPECT_EQ(map.origin(), reference_map.origin());
    if (full_extents)
    {
      EXPECT_EQ(map.regionCount(), reference_map.regionCount());
    }
    EXPECT_EQ(map.hitValue(), reference_map.hitValue());
    EXPECT_EQ(map.missValue(), reference_map.missValue());
    EXPECT_EQ(map.occupancyThresholdValue(), reference_map.occupancyThresholdValue());
  }

  // Layout
  // Start with layout not compared. Important so later we can assume layout is OK.
  bool layout_matches = false;
  if (compare_flags & kCfLayout)
  {
    layout_matches = compareLayout(map, reference_map);
    EXPECT_TRUE(layout_matches);
  }

  // Chunks
  if (compare_flags & kCfChunksGeneral)
  {
    std::vector<const MapChunk *> ref_chunks;
    reference_map.enumerateRegions(ref_chunks);

    for (const MapChunk *ref_chunk : ref_chunks)
    {
      const MapChunk *chunk = map.region(ref_chunk->region.coord);

      if (full_extents || ref_chunk->overlapsExtents(min_ext, max_ext))
      {
        EXPECT_TRUE(chunk != nullptr);
      }

      if (!chunk)
      {
        continue;
      }

      EXPECT_EQ(chunk->region.centre, ref_chunk->region.centre);
      EXPECT_EQ(chunk->region.coord, ref_chunk->region.coord);
      // First valid index should be the same when occupancy and general map values are expected to match because it is
      // derived from the occupancy.
      if (compare_flags & (kCfGeneral | kCfOccupancy))
      {
        EXPECT_EQ(chunk->first_valid_index, ref_chunk->first_valid_index);
      }

      if (compare_flags & kCfChunksFine)
      {
        EXPECT_EQ(chunk->touched_time, ref_chunk->touched_time);
        EXPECT_EQ(chunk->dirty_stamp, ref_chunk->dirty_stamp);
        for (unsigned i = 0; i < chunk->layout().layerCount(); ++i)
        {
          EXPECT_EQ(chunk->touched_stamps[i], ref_chunk->touched_stamps[i]);
        }
        EXPECT_EQ(chunk->flags, ref_chunk->flags);
      }
    }
  }

  // Voxels
  unsigned occupancy_failures = 0;
  if (compare_flags & (kCfOccupancy | kCfClearance))
  {
    bool have_valid_clearance = false;
    Voxel<const float> map_occupancy(&map, map.layout().occupancyLayer());
    Voxel<const float> map_clearance(&map, map.layout().clearanceLayer());
    Voxel<const float> ref_occupancy(&reference_map, reference_map.layout().occupancyLayer());
    Voxel<const float> ref_clearance(&reference_map, reference_map.layout().clearanceLayer());
    // Allow for some tolerance due to GPU non-determinism.
    // TODO(KS): This started failing on a Quadro RTX 5000 on OpenCL 1.2 about 50% of the time, Ubuntu 18.04 with
    // nvidia 460 drivers. The variance was often near zero (hidden by the displayed presision), but would
    // reliably come out at ~0.35 absolute error with 0.1 voxel resolution. This may be indicative of more than
    // just GPU non-determinism and may hide a real bug. As such this tolerance may be too large.
    const auto clearance_tolerance = float(0.5 * map.resolution());

    ASSERT_EQ(map_occupancy.isLayerValid(), ref_occupancy.isLayerValid());
    if (compare_flags & kCfClearance)
    {
      ASSERT_EQ(map_clearance.isLayerValid(), ref_clearance.isLayerValid());
    }

    for (auto iter = map.begin(); iter != map.end(); ++iter)
    {
      map_clearance.setKey(map_occupancy.setKey(iter));
      ref_clearance.setKey(ref_occupancy.setKey(iter.key()));

      ASSERT_TRUE(map_occupancy.isValid());

      if (full_extents || map_occupancy.chunk()->overlapsExtents(min_ext, max_ext))
      {
        ASSERT_TRUE(ref_occupancy.isValid());
      }

      if (!ref_occupancy.isValid())
      {
        continue;
      }

      ASSERT_EQ(ref_clearance.key(), map_occupancy.key());
      if (compare_flags & kCfOccupancy)
      {
        float ref_value, map_value;

        if (ref_occupancy.read(&ref_value) != map_occupancy.read(&map_value))
        {
          ++occupancy_failures;
        }
        // Use >, not >= as we'll have just incremented the failure above.
        if (occupancy_failures > allowed_occupancy_mismatch_count)
        {
          EXPECT_EQ(ref_occupancy.read(&ref_value), map_occupancy.read(&map_value));
        }
      }

      if (compare_flags & kCfClearance)
      {
        if (map_clearance.isValid())
        {
          ASSERT_TRUE(ref_clearance.isValid());
          float ref_clearance_value, map_clearance_value;
          ref_clearance.read(&ref_clearance_value);
          map_clearance.read(&map_clearance_value);
          EXPECT_NEAR(ref_clearance_value, map_clearance_value, clearance_tolerance);
          have_valid_clearance = have_valid_clearance || ref_clearance_value >= 0;
        }
      }
    }

    if (compare_flags & kCfExpectClearance)
    {
      EXPECT_TRUE(have_valid_clearance);
    }
  }
}
}  // namespace ohmtestutil


================================================
FILE: tests/ohmtestcommon/OhmTestUtil.h
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#ifndef OHMTESTUTIL_H
#define OHMTESTUTIL_H

#include <glm/fwd.hpp>

namespace ohm
{
class OccupancyMap;
}

namespace ohmtestutil
{
enum CompareFlag
{
  kCfGeneral = (1 << 0),
  kCfChunksGeneral = (1 << 1),
  /// Fine detail comparison of chunks. Requires @c CF_ChunksGeneral.
  kCfChunksFine = (1 << 2),
  kCfLayout = (1 << 3),
  kCfOccupancy = (1 << 4),
  kCfClearance = (1 << 5),
  kCfExpectClearance = (1 << 6),

  kCfDefault = kCfGeneral | kCfChunksGeneral | kCfOccupancy,
  kCfCompareExtended = kCfGeneral | kCfChunksGeneral | kCfLayout | kCfOccupancy | kCfClearance,
  kCfCompareFineDetail = kCfGeneral | kCfChunksGeneral | kCfChunksFine | kCfLayout | kCfOccupancy | kCfClearance
};

const char *applicationDir();
void setApplicationPath(const char *path);

bool compareLayout(const ohm::OccupancyMap &map, const ohm::OccupancyMap &reference_map);

void compareMaps(const ohm::OccupancyMap &map, const ohm::OccupancyMap &reference_map,
                 unsigned compare_flags = kCfDefault, unsigned allowed_occupancy_mismatch_count = 0);

void compareMaps(const ohm::OccupancyMap &map, const ohm::OccupancyMap &reference_map, const glm::dvec3 &min_ext,
                 const glm::dvec3 &max_ext, unsigned compare_flags = kCfDefault,
                 unsigned allowed_occupancy_mismatch_count = 0);
}  // namespace ohmtestutil

#endif  // OHMTESTUTIL_H


================================================
FILE: tests/ohmtestcommon/RayPatternTestUtil.h
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include <ohm/OccupancyMap.h>
#include <ohm/RayPatternConical.h>
#include <ohm/VoxelData.h>

#include <ohm/ClearingPattern.h>

#include <array>
#include <cstddef>
#include <functional>

#include <gtest/gtest.h>

namespace ohmtestutil
{
/// Template function for testing the RayPattern integration for use with either a CPU or GPU map.
///
/// This test creates a series of four voxels of varying types then applys a single ray ClearingPattern using various
/// exclusion flags, and checks the results.
///
/// @param map The @c OccupancyMap being updated.
/// @param ray_map The map object to apply the @c ClearingPattern to. This is the @c OccupancyMap when updating using
/// CPU and the @c GpuMap when using GPU.
/// @param pre_validation_function Function to call before validation. For GPU, this should sync to CPU.
/// @param test_start_index Debug parameter used to choose the first test index to run.
/// @param test_end_index Debug parameter used to choose the last test index to run - should be set to 1 more than the
///   last test index to be run.
template <typename RayMap>
void RayPatternExcludeTest(ohm::OccupancyMap &map, RayMap &ray_map,
                           std::function<void()> pre_validation_func = std::function<void()>(),
                           size_t test_start_index = 0, size_t test_end_index = ~0u)
{
  // Setup the map. Must work with a 1.0m resolution for convenience.
  ASSERT_EQ(map.resolution(), 1.0);
  // Set the miss value to be greater magnitude than the hit value. This ensures a single clear ray will erase a hit.
  // Note the negative sign is required for a probability decrease.
  map.setMissValue(-1.1f * map.hitValue());
  // Offset by half a voxel so that integer coordinates approximately index voxel centres.
  map.setOrigin(glm::dvec3(0.5 * map.resolution()));
  // Disable voxel saturation
  map.setSaturateAtMinValue(false);
  map.setSaturateAtMaxValue(false);
  // Remove voxel value limits
  map.setMinVoxelValue(std::numeric_limits<float>::lowest());
  map.setMaxVoxelValue(std::numeric_limits<float>::max());

  // Configure a function object to reset the voxel values of interest.
  ohm::Voxel<float> occupancy(&map, map.layout().occupancyLayer());
  ASSERT_TRUE(occupancy.isLayerValid());

  // function object to call to reset the map state to:
  // - unobserved @ (0, 0, 0)
  // - free @ (1, 0, 0)
  // - occupied @ (2, 0, 0)
  const auto reset_map_func = [&map, &occupancy]  //
  {
    occupancy.setKey(map.voxelKey(glm::dvec3(0, 0, 0)));
    ASSERT_TRUE(occupancy.isValid());
    occupancy.write(ohm::unobservedOccupancyValue());

    occupancy.setKey(map.voxelKey(glm::dvec3(1, 0, 0)));
    ASSERT_TRUE(occupancy.isValid());
    occupancy.write(map.missValue());

    occupancy.setKey(map.voxelKey(glm::dvec3(2, 0, 0)));
    ASSERT_TRUE(occupancy.isValid());
    occupancy.write(map.hitValue());

    occupancy.setKey(map.voxelKey(glm::dvec3(3, 0, 0)));
    ASSERT_TRUE(occupancy.isValid());
    occupancy.write(map.hitValue());

    // Ensure the last write operation is committed. Particularly important when this test is used on a GpuMap as it
    // ensures the GpuCache recognises the CPU based change.
    occupancy.commit();
  };

  // Build a single ray pattern.
  ohm::RayPattern rays;
  rays.addRay(glm::dvec3(0, 0, 0), glm::dvec3(10, 0, 0));
  ohm::ClearingPattern pattern(&rays, false);

  // Setup the various test cases.
  struct TestData
  {
    /// Context used for error display.
    const char *context;
    /// @c RayFlag values to use when applying the pattern.
    unsigned ray_flags;
    /// Expected occupancy values along the ray.
    std::array<float, 4> expected_values;
  };

  const std::array<TestData, 5> tests =  //
    {                                    //
      TestData{
        "default flags",
        ohm::ClearingPattern::kDefaultRayFlags,
        { ohm::unobservedOccupancyValue(), map.missValue(), map.hitValue() + map.missValue(), map.hitValue() } },
      TestData{ "continue past occupied",
                ohm::ClearingPattern::kDefaultRayFlags & ~ohm::kRfStopOnFirstOccupied,
                { ohm::unobservedOccupancyValue(), map.missValue(), map.hitValue() + map.missValue(),
                  map.hitValue() + map.missValue() } },
      TestData{ "exclude unobserved",
                ohm::kRfEndPointAsFree | ohm::kRfExcludeUnobserved,
                { ohm::unobservedOccupancyValue(), 2.0f * map.missValue(), map.hitValue() + map.missValue(),
                  map.hitValue() + map.missValue() } },
      TestData{
        "exclude free",
        ohm::kRfEndPointAsFree | ohm::kRfExcludeFree,
        { map.missValue(), map.missValue(), map.hitValue() + map.missValue(), map.hitValue() + map.missValue() } },
      TestData{ "exclude occupied",
                ohm::kRfEndPointAsFree | ohm::kRfExcludeOccupied,
                { map.missValue(), 2.0f * map.missValue(), map.hitValue(), map.hitValue() } }
    };

  // Run the test data sets
  for (size_t i = std::max<size_t>(0u, test_start_index); i < std::min<size_t>(tests.size(), test_end_index); ++i)
  {
    const TestData &test = tests[i];
    // Reset the map.
    reset_map_func();
    // Set the ray flags.
    pattern.setRayFlags(test.ray_flags);
    // Apply the pattern.
    pattern.apply(&ray_map, glm::dmat4(1.0));
    if (pre_validation_func)
    {
      pre_validation_func();
    }
    // Check the results.
    for (size_t i = 0; i < test.expected_values.size(); ++i)
    {
      // Retrieve the corresponding voxel.
      occupancy.setKey(map.voxelKey(glm::dvec3(double(i), 0, 0)));
      ASSERT_TRUE(occupancy.isValid()) << test.context << " voxel " << i;
      if (occupancy.data() != ohm::unobservedOccupancyValue() ||
          test.expected_values[i] != ohm::unobservedOccupancyValue())
      {
        EXPECT_NEAR(occupancy.data(), test.expected_values[i], 1e-3f) << test.context << " voxel " << i;
      }
      else
      {
        // Handle unobserved == infinity
        EXPECT_EQ(occupancy.data(), test.expected_values[i]);
      }
    }
  }
}
}  // namespace ohmtestutil

================================================
FILE: tests/ohmtestcommon/TraversalTest.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "TraversalTest.h"

#include <gtest/gtest.h>

#include <ohm/Key.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapper.h>
#include <ohm/Voxel.h>

#include <glm/vec3.hpp>


namespace ohmtestcommon
{
namespace traversal
{
void testInto(ohm::OccupancyMap &map, ohm::RayMapper &mapper, std::function<void()> pre_validation)
{
  ASSERT_GT(map.layout().traversalLayer(), 0);

  // Offset the map so that we can cast rays through the origin without incurring floating point ambituity on the target
  // voxel.
  map.setOrigin(glm::dvec3(-0.5f * map.resolution()));

  // Create the rays we are to cast.
  const std::vector<glm::dvec3> rays = {
    // Orthogonal rays.
    glm::dvec3(-1, 0, 0), glm::dvec3(0, 0, 0), glm::dvec3(0, -1, 0), glm::dvec3(0, 0, 0), glm::dvec3(0, 0, -1),
    glm::dvec3(0, 0, 0), glm::dvec3(1, 0, 0), glm::dvec3(0, 0, 0), glm::dvec3(0, 1, 0), glm::dvec3(0, 0, 0),
    glm::dvec3(0, 0, 1), glm::dvec3(0, 0, 0),
    // 2D diagonals.
    glm::dvec3(-1, -1, 0), glm::dvec3(0, 0, 0), glm::dvec3(1, -1, 0), glm::dvec3(0, 0, 0), glm::dvec3(-1, 1, 0),
    glm::dvec3(0, 0, 0), glm::dvec3(1, 1, 0), glm::dvec3(0, 0, 0), glm::dvec3(-1, 0, -1), glm::dvec3(0, 0, 0),
    glm::dvec3(1, 0, -1), glm::dvec3(0, 0, 0), glm::dvec3(-1, 0, 1), glm::dvec3(0, 0, 0), glm::dvec3(1, 0, 1),
    glm::dvec3(0, 0, 0), glm::dvec3(0, -1, -1), glm::dvec3(0, 0, 0), glm::dvec3(0, 1, -1), glm::dvec3(0, 0, 0),
    glm::dvec3(0, -1, 1), glm::dvec3(0, 0, 0), glm::dvec3(0, 1, 1), glm::dvec3(0, 0, 0),
    // 3D diagonals
    glm::dvec3(-1, -1, -1), glm::dvec3(0, 0, 0), glm::dvec3(1, -1, -1), glm::dvec3(0, 0, 0), glm::dvec3(-1, 1, -1),
    glm::dvec3(0, 0, 0), glm::dvec3(1, 1, -1), glm::dvec3(0, 0, 0), glm::dvec3(-1, -1, 1), glm::dvec3(0, 0, 0),
    glm::dvec3(1, -1, 1), glm::dvec3(0, 0, 0), glm::dvec3(-1, 1, 1), glm::dvec3(0, 0, 0), glm::dvec3(1, 1, 1),
    glm::dvec3(0, 0, 0)
  };
  const float expected_orthogonal = float(0.5 * map.resolution());
  const float expected_diagonal2d =
    float(0.5 * std::sqrt(map.resolution() * map.resolution() + map.resolution() * map.resolution()));
  const float expected_diagonal3d =
    float(0.5 * std::sqrt(map.resolution() * map.resolution() + map.resolution() * map.resolution() +
                          map.resolution() * map.resolution()));
  const std::vector<float> expected_rate = {
    // Orthogonal
    expected_orthogonal, expected_orthogonal, expected_orthogonal, expected_orthogonal, expected_orthogonal,
    expected_orthogonal,
    // Diagonal 2D
    expected_diagonal2d, expected_diagonal2d, expected_diagonal2d, expected_diagonal2d, expected_diagonal2d,
    expected_diagonal2d, expected_diagonal2d, expected_diagonal2d, expected_diagonal2d, expected_diagonal2d,
    expected_diagonal2d, expected_diagonal2d,
    // Diagonal 3D
    expected_diagonal3d, expected_diagonal3d, expected_diagonal3d, expected_diagonal3d, expected_diagonal3d,
    expected_diagonal3d, expected_diagonal3d, expected_diagonal3d
  };

  ASSERT_EQ(rays.size(), expected_rate.size() * 2) << "Test data misalignment";

  ohm::Voxel<const float> traversal_voxel(&map, map.layout().traversalLayer());
  ASSERT_TRUE(traversal_voxel.isLayerValid());

  // Integrate and test one ray at a time.
  float expected_traversal = 0;
  for (size_t i = 0; i < rays.size(); i += 2)
  {
    mapper.integrateRays(&rays[i], 2);
    if (pre_validation)
    {
      pre_validation();
    }
    expected_traversal += expected_rate[i / 2];
    const auto key = map.voxelKey(glm::dvec3(0));
    traversal_voxel.setKey(key);
    ASSERT_TRUE(traversal_voxel.isValid());
    EXPECT_NEAR(traversal_voxel.data(), expected_traversal, 1e-3f);
  }
}

void testThrough(ohm::OccupancyMap &map, ohm::RayMapper &mapper, std::function<void()> pre_validation)
{
  ASSERT_GT(map.layout().traversalLayer(), 0);

  // Offset the map so that we can cast rays through the origin without incurring floating point ambituity on the target
  // voxel.
  map.setOrigin(glm::dvec3(-0.5f * map.resolution()));

  // Create the rays we are to cast.
  const std::vector<glm::dvec3> rays = {
    // Orthogonal rays.
    glm::dvec3(-1, 0, 0),
    glm::dvec3(1, 0, 0),
    glm::dvec3(0, -1, 0),
    glm::dvec3(0, 1, 0),
    glm::dvec3(0, 0, -1),
    glm::dvec3(0, 0, 1),
    // 2D diagonals.
    glm::dvec3(-1, -1, 0),
    glm::dvec3(1, 1, 0),
    glm::dvec3(1, -1, 0),
    glm::dvec3(-1, 1, 0),
    glm::dvec3(-1, 1, 0),
    glm::dvec3(1, -1, 0),
    glm::dvec3(1, 1, 0),
    glm::dvec3(-1, -1, 0),
    glm::dvec3(-1, 0, -1),
    glm::dvec3(1, 0, 1),
    glm::dvec3(1, 0, -1),
    glm::dvec3(-1, 0, 1),
    glm::dvec3(-1, 0, 1),
    glm::dvec3(1, 0, -1),
    glm::dvec3(1, 0, 1),
    glm::dvec3(-1, 0, -1),
    glm::dvec3(0, -1, -1),
    glm::dvec3(0, 1, 1),
    glm::dvec3(0, 1, -1),
    glm::dvec3(0, -1, 1),
    glm::dvec3(0, -1, 1),
    glm::dvec3(0, 1, -1),
    glm::dvec3(0, 1, 1),
    glm::dvec3(0, -1, -1),
    // 3D diagonals
    glm::dvec3(-1, -1, -1),
    glm::dvec3(1, 1, 1),
    glm::dvec3(1, -1, -1),
    glm::dvec3(-1, 1, 1),
    glm::dvec3(-1, 1, -1),
    glm::dvec3(1, -1, 1),
    glm::dvec3(1, 1, -1),
    glm::dvec3(-1, -1, 1),
    glm::dvec3(-1, -1, 1),
    glm::dvec3(1, 1, -1),
    glm::dvec3(1, -1, 1),
    glm::dvec3(-1, 1, -1),
    glm::dvec3(-1, 1, 1),
    glm::dvec3(1, -1, -1),
    glm::dvec3(1, 1, 1),
    glm::dvec3(-1, -1, -1),
  };

  const float expected_orthogonal = float(map.resolution());
  const float expected_diagonal2d =
    float(std::sqrt(map.resolution() * map.resolution() + map.resolution() * map.resolution()));
  const float expected_diagonal3d = float(std::sqrt(
    map.resolution() * map.resolution() + map.resolution() * map.resolution() + map.resolution() * map.resolution()));
  const std::vector<float> expected_rate =  //
    {                                       //
      // Orthogonal
      expected_orthogonal, expected_orthogonal, expected_orthogonal,
      // Diagonal 2D
      expected_diagonal2d, expected_diagonal2d, expected_diagonal2d, expected_diagonal2d, expected_diagonal2d,
      expected_diagonal2d, expected_diagonal2d, expected_diagonal2d, expected_diagonal2d, expected_diagonal2d,
      expected_diagonal2d, expected_diagonal2d,
      // Diagonal 3D
      expected_diagonal3d, expected_diagonal3d, expected_diagonal3d, expected_diagonal3d, expected_diagonal3d,
      expected_diagonal3d, expected_diagonal3d, expected_diagonal3d
    };

  ASSERT_EQ(rays.size(), expected_rate.size() * 2) << "Test data misalignment";

  ohm::Voxel<const float> traversal_voxel(&map, map.layout().traversalLayer());
  ASSERT_TRUE(traversal_voxel.isLayerValid());

  // Integrate and test one ray at a time.
  float expected_traversal = 0;
  for (size_t i = 0; i < rays.size(); i += 2)
  {
    mapper.integrateRays(&rays[i], 2);
    if (pre_validation)
    {
      pre_validation();
    }
    expected_traversal += expected_rate[i / 2];
    const auto key = map.voxelKey(glm::dvec3(0));
    traversal_voxel.setKey(key);
    ASSERT_TRUE(traversal_voxel.isValid());
    EXPECT_NEAR(traversal_voxel.data(), expected_traversal, 1e-3f);
  }
}
}  // namespace traversal
}  // namespace ohmtestcommon


================================================
FILE: tests/ohmtestcommon/TraversalTest.h
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <ohm/OhmConfig.h>

#include <functional>

namespace ohm
{
class OccupancyMap;
class RayMapper;
}  // namespace ohm

namespace ohmtestcommon
{
namespace traversal
{
/// Test ray trace accumulation for rays traced into a target voxel.
/// @param map The map object to trace into. The map should be empty/clear.
/// @param mapper The ray tracing algorithm.
/// @param pre_validation Function to call before performing validation. For example, allows GPU to CPU sync.
void testInto(ohm::OccupancyMap &map, ohm::RayMapper &mapper, std::function<void()> pre_validation = {});
/// Test ray trace accumulation for rays traced through a target voxel.
/// @param map The map object to trace into. The map should be empty/clear.
/// @param mapper The ray tracing algorithm.
/// @param pre_validation Function to call before performing validation. For example, allows GPU to CPU sync.
void testThrough(ohm::OccupancyMap &map, ohm::RayMapper &mapper, std::function<void()> pre_validation = {});
}  // namespace traversal
}  // namespace ohmtestcommon


================================================
FILE: tests/ohmtestcommon/WalkSegmentKeysLegacy.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include "WalkSegmentKeysLegacy.h"

#include <ohm/Key.h>
#include <ohm/OccupancyMap.h>

namespace ohm
{
size_t walkSegmentKeysLegacy(WalkSegmentFunc walk_func, const glm::dvec3 &start_point, const glm::dvec3 &end_point,
                             bool include_end_point, const ohm::OccupancyMap &map,
                             double length_epsilon)  // NOLINT(readability-magic-numbers)
{
  // see "A Faster Voxel Traversal Algorithm for Ray Tracing" by Amanatides & Woo
  Key start_point_key = map.voxelKey(start_point);
  Key end_point_key = map.voxelKey(end_point);

  if (start_point_key.isNull() || end_point_key.isNull())
  {
    return 0;
  }

  glm::dvec3 direction = glm::dvec3(end_point - start_point);
  const double length = std::sqrt(glm::dot(direction, direction));
  const glm::dvec3 voxel = map.voxelCentreGlobal(start_point_key);

  // Very small segments which straddle a voxel boundary can be problematic. We want to avoid
  // inverting a very small number, but be robust enough to handle the situation.
  // To that end, we skip normalising the direction for directions below a tenth of the voxel.
  // Then we will exit either with start/end voxels being the same, or we will step from start
  // to end in one go.
  const bool valid_length = length >= length_epsilon;
  if (valid_length)
  {
    direction *= 1.0 / length;
  }

  if (start_point_key == end_point_key)
  {
    if (include_end_point)
    {
      walk_func(end_point_key, 0.0, length);
    }
    return 1;
  }

  if (!valid_length)
  {
    // Start/end points are in different, but adjacent voxels. Prevent issues with the loop by
    // early out.
    const int axis =
      start_point_key.axisMatches(0, end_point_key) ? (start_point_key.axisMatches(1, end_point_key) ? 2 : 1) : 0;
    const double sign_direction = (direction[axis] > 0) ? 1 : -1;
    const glm::dvec3 voxel = map.voxelCentreGlobal(start_point_key);
    const double next_voxel_border =
      voxel[axis] + sign_direction * 0.5 * map.resolution();  // NOLINT(readability-magic-numbers)
    const double first_voxel_length =
      std::abs((next_voxel_border - start_point[axis]) / (end_point[axis] - start_point[axis])) * length;
    if (walk_func(start_point_key, 0.0, first_voxel_length))
    {
      if (include_end_point)
      {
        walk_func(end_point_key, first_voxel_length, length);
        return 2;
      }
    }
    return 1;
  }

  std::array<int, 3> step = { 0, 0, 0 };
  std::array<double, 3> time_max;
  std::array<double, 3> time_delta;
  std::array<double, 3> time_limit;
  // Correction offset for time_limit for when we are tracing from the exact bottom corner of a voxel to another exact
  // corner in the positive direction.
  glm::dvec3 time_limit_offset = glm::dvec3(0.0);
  double next_voxel_border;
  double direction_axis_inv;
  size_t added = 0;
  Key current_key = start_point_key;
  double time_current = 0.0;

  // Compute step direction, increments and maximums along each axis.
  for (unsigned i = 0; i < 3; ++i)
  {
    if (direction[i] != 0)
    {
      direction_axis_inv = 1.0 / direction[i];
      step[i] = (direction[i] > 0) ? 1 : -1;
      // Time delta is the ray time between voxel boundaries calculated for each axis.
      time_delta[i] = map.resolution() * std::abs(direction_axis_inv);
      // Calculate the distance from the origin to the nearest voxel edge for this axis.
      next_voxel_border = voxel[i] + step[i] * 0.5 * map.resolution();  // NOLINT(readability-magic-numbers)
      time_max[i] = (next_voxel_border - start_point[i]) * direction_axis_inv;
      // Set the distance limit
      // original...
      // time_limit[i] = std::abs((end_point[i] - start_point[i]) * direction_axis_inv);
      // which is equivalent to...
      time_limit[i] = length;
      // Add an effective epsilon to handle traversing exact voxel boundaries.
      // time_limit[i] += 0.5 * time_delta[i];
      if (std::abs(time_max[i] - glm::length(glm::dvec3(map.resolution()))) < 1e-6)
      {
        time_limit_offset[i] = step[i] * 0.5 * map.resolution();
      }
    }
    else
    {
      time_max[i] = time_delta[i] = std::numeric_limits<double>::max();
      time_limit[i] = 0;
    }
  }

  for (int i = 0; i < 3; ++i)
  {
    time_limit[i] += time_limit_offset[i];
  }


  int axis = 0;
  bool limit_reached = false;
  bool user_exit = false;
  while (!limit_reached && !user_exit && current_key != end_point_key)
  {
    axis = (time_max[0] < time_max[2]) ? ((time_max[0] < time_max[1]) ? 0 : 1) : ((time_max[1] < time_max[2]) ? 1 : 2);
    // Strictly speaking std::abs() is unnecessary here. However, from memory there were instances where it could be
    // negative in practice (floating point error). Possibly in the zero case (positive and negative zeros).
    limit_reached = std::abs(time_max[axis]) > time_limit[axis];
    const double new_time_current = limit_reached ? time_limit[axis] : time_max[axis];

    user_exit = !walk_func(current_key, time_current, new_time_current);

    time_max[axis] += time_delta[axis];
    time_current = new_time_current;
    ++added;
    map.stepKey(current_key, axis, step[axis]);
  }

  if (!user_exit && include_end_point)
  {
    walk_func(end_point_key, time_current, length);
    ++added;
  }

  assert(added);
  return added;
}
}  // namespace ohm


================================================
FILE: tests/ohmtestcommon/WalkSegmentKeysLegacy.h
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#ifndef OHMTESTCOMMON_WALKSEGMENTKEYSLEGACY_H
#define OHMTESTCOMMON_WALKSEGMENTKEYSLEGACY_H

#include <ohm/KeyList.h>

#include <glm/vec3.hpp>

#include <functional>

namespace ohm
{
class Key;
class OccupancyMap;

/// Function signature for the visit function called from @c walkSegmentKeys().
/// @param key The key of the current voxel being visited.
/// @param enter_range Range at which the voxel is entered. detail required
/// @param exit_range Range at which the voxel is entered. detail required
/// @return True to keep walking the voxels along the ray, false to abort walking the ray.
using WalkSegmentFunc = std::function<bool(const Key &, double, double)>;

/// The legacy implementation of line walking in ohm for use in tests which compare with legacy generated data.
///
/// The @p walk_func is invoked for each voxel in the line segment connecting @p start_point to @p end_point .
///
/// Based on J. Amanatides and A. Woo, "A fast voxel traversal algorithm for raytracing," 1987.
///
/// @param walk_func The callable object to invoke for each traversed voxel key.
/// @param start_point The start of the line in 3D space.
/// @param end_point The end of the line in 3D space.
/// @param include_end_point Should be @c true if @p walkFunc should be called for the voxel containing
///     @c endPoint, when it does not lie in the same voxel as @p startPoint.
/// @param map The occupancy map to walk in.
/// @param length_epsilon Small length at which a segment is considered degenerate.
/// @return The number of voxels traversed. This includes @p endPoint when @p includeEndPoint is true.
size_t walkSegmentKeysLegacy(WalkSegmentFunc walk_func, const glm::dvec3 &start_point, const glm::dvec3 &end_point,
                             bool include_end_point, const ohm::OccupancyMap &map,
                             double length_epsilon = 1e-6);  // NOLINT(readability-magic-numbers)

/// Function which calculates segment keys using legacy line walking.
inline size_t calculateSegmentKeysLegacy(KeyList &keys, const OccupancyMap &map, const glm::dvec3 &start_point,
                                         const glm::dvec3 &end_point, bool include_end_point)
{
  keys.clear();
  return ohm::walkSegmentKeysLegacy(
    [&keys](const Key &key, double, double) {
      keys.add(key);
      return true;
    },
    start_point, end_point, include_end_point, map);
}

}  // namespace ohm

#endif  // OHMTESTCOMMON_WALKSEGMENTKEYSLEGACY_H


================================================
FILE: tests/ohmtestgpu/CMakeLists.txt
================================================
# Setup of GTEST changed at CMake 3.5.
cmake_minimum_required(VERSION 3.5)



# Eigen required to support some tests - NDT in particular
find_package(Eigen3 QUIET)

set(SOURCES
  GpuCopyTests.cpp
  GpuIncidentsTests.cpp
  GpuLineKeysTests.cpp
  GpuLineQueryTests.cpp
  GpuMapperTests.cpp
  GpuMapTest.cpp
  GpuRangesTests.cpp
  GpuRayPatternTests.cpp
  GpuRaysQueryTests.cpp
  GpuSerialisationTests.cpp
  GpuTestMain.cpp
  GpuTests.cpp
  GpuTouchTimeTests.cpp
  GpuTraversalTests.cpp
  GpuTsdfTests.cpp
  GpuVoxelMeanTests.cpp
)

if(Eigen3_FOUND)
  message(STATUS "Eigen3 found. Adding NDT tests.")
  list(APPEND SOURCES GpuNdtTests.cpp)
endif(Eigen3_FOUND)

function(_ohmtestgpu_setup GPU)
  add_executable(ohmtest${GPU} ${SOURCES})
  leak_track_target_enable(ohmtest${GPU} CONDITION OHM_LEAK_TRACK)

  set_target_properties(ohmtest${GPU} PROPERTIES FOLDER tests)
  if(MSVC)
    set_target_properties(ohmtest${GPU} PROPERTIES DEBUG_POSTFIX "d")
  endif(MSVC)

  target_include_directories(ohmtest${GPU}
    PUBLIC
      $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
      $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
  )

  target_link_libraries(ohmtest${GPU} PUBLIC ohmtestcommon ohmtools ohm${GPU} gputil${GPU} ohmutil)

  target_link_libraries(ohmtest${GPU}
    PRIVATE
      ${GTEST_LIBRARIES}
      ${GTEST_MAIN_LIBRARIES}
      glm::glm
      $<BUILD_INTERFACE:$<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>>
      $<BUILD_INTERFACE:$<$<BOOL:${OHM_FEATURE_EIGEN}>:Eigen3::Eigen>>
  )


  target_include_directories(ohmtest${GPU}
      SYSTEM PRIVATE
        "${GTEST_INCLUDE_DIRS}"
        $<$<NOT:$<BOOL:${OHM_TES_DEBUG}>>:${3ES_INCLUDE_DIRS}>
    )

  add_test(NAME ohmtest${GPU} COMMAND ohmtest${GPU} --gtest_output=xml:test-reports/)
endfunction(_ohmtestgpu_setup)

if(OHM_FEATURE_OPENCL)
  _ohmtestgpu_setup(ocl)
  # Required to run NVIDIA OpenCL
  leak_track_default_options(ohmtestocl CONDITION OHM_LEAK_TRACK ${OHM_ASAN_OPTIONS_CUDA})
  leak_track_suppress(ohmtestocl CONDITION OHM_LEAK_TRACK
    ${OHM_LEAK_SUPPRESS_OCL}
    ${OHM_LEAK_SUPPRESS_TBB}
  )
endif(OHM_FEATURE_OPENCL)
if(OHM_FEATURE_CUDA)
  _ohmtestgpu_setup(cuda)
  leak_track_default_options(ohmtestcuda CONDITION OHM_LEAK_TRACK ${OHM_ASAN_OPTIONS_CUDA})
  leak_track_suppress(ohmtestcuda CONDITION OHM_LEAK_TRACK
    ${OHM_LEAK_SUPPRESS_CUDA}
    ${OHM_LEAK_SUPPRESS_TBB}
  )
endif(OHM_FEATURE_CUDA)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

# install(TARGETS ohmtest DESTINATION bin)


================================================
FILE: tests/ohmtestgpu/GpuCopyTests.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <ohm/Aabb.h>
#include <ohm/CopyUtil.h>
#include <ohm/Key.h>
#include <ohm/LineQuery.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/VoxelData.h>

#include <ohmgpu/GpuMap.h>

#include <ohmtools/OhmCloud.h>
#include <ohmtools/OhmGen.h>

#include <logutil/LogUtil.h>
#include <ohmutil/Profile.h>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <random>

#include <gtest/gtest.h>
#include "ohmtestcommon/OhmTestUtil.h"

using namespace ohm;

namespace maptests
{
TEST(Copy, CopyFromGpu)
{
  // Test copying from GPU memory.
  const double map_extents = 10.0;
  const double noise = 1.0;
  const unsigned ray_count = 1024 * 128;
  OccupancyMap map(0.2);
  OccupancyMap dst_map(map.resolution());
  {
    GpuMap gpu_map(&map, true);

    // Make some rays.
    std::mt19937 rand_engine;
    std::uniform_real_distribution<double> unit_rand(-1, 1);
    std::uniform_real_distribution<double> length_rand(map_extents - noise, map_extents);
    std::vector<glm::dvec3> rays;

    // Build rays to create a generally spherical shell.
    while (rays.size() < ray_count * 2)
    {
      rays.push_back(glm::dvec3(0.05));
      glm::dvec3 ray_end(unit_rand(rand_engine), unit_rand(rand_engine), unit_rand(rand_engine));
      ray_end = glm::normalize(ray_end);
      ray_end *= length_rand(rand_engine);
      rays.push_back(ray_end);
    }

    gpu_map.integrateRays(rays.data(), rays.size());

    // Do not sync back to CPU. Copy directly from GPU.
    EXPECT_TRUE(copyMap(dst_map, map));
    // Compare one region ensuring it *does not* match.
    const MapChunk *src_region = map.region(map.voxelKey(rays[1]).regionKey(), false);
    ASSERT_NE(src_region, nullptr);

    // Find the corresponding chunk in the copied map.
    const MapChunk *dst_region = dst_map.region(src_region->region.coord, false);
    ASSERT_NE(dst_region, nullptr);

    // Lock the occupancy layers.
    Voxel<const float> src_occupancy(&map, map.layout().occupancyLayer(), Key(src_region->region.coord, 0, 0, 0));
    ASSERT_TRUE(src_occupancy.isValid());
    Voxel<const float> dst_occupancy(&dst_map, dst_map.layout().occupancyLayer(), src_occupancy.key());
    ASSERT_TRUE(dst_occupancy.isValid());

    // Compare voxels in the region.
    size_t observed_voxel_count = 0;
    for (uint8_t z = 0; z < map.regionVoxelDimensions().z; ++z)
    {
      for (uint8_t y = 0; y < map.regionVoxelDimensions().y; ++y)
      {
        for (uint8_t x = 0; x < map.regionVoxelDimensions().x; ++x)
        {
          src_occupancy.setKey(Key(src_region->region.coord, x, y, z));
          dst_occupancy.setKey(src_occupancy.key());
          ASSERT_TRUE(src_occupancy.isValid());
          ASSERT_TRUE(dst_occupancy.isValid());

          // Observed voxels in dst_map should not match those in map.
          if (dst_occupancy.data() != unobservedOccupancyValue())
          {
            EXPECT_NE(dst_occupancy.data(), src_occupancy.data());
            ++observed_voxel_count;
          }
        }
      }
    }

    // We expect to have observed something. Otherwise we'd pass a failure to copy at all.
    EXPECT_GT(observed_voxel_count, 0);
    std::cout << "Compared " << observed_voxel_count << " voxels" << std::endl;

    // Now sync the GPU map back to CPU and compare the whole map - they should match.
    gpu_map.syncVoxels();
    ohmtestutil::compareMaps(dst_map, map, ohmtestutil::kCfCompareExtended);
  }
}

TEST(Copy, CopySubmapFromGpu)
{
  // Test copying from GPU memory.
  const double map_extents = 10.0;
  const double noise = 1.0;
  const unsigned ray_count = 1024 * 128;
  const glm::dvec3 copy_min(0);
  const glm::dvec3 copy_max(4.5);
  OccupancyMap map(0.2);
  OccupancyMap dst_map(map.resolution());
  {
    GpuMap gpu_map(&map, true);

    // Make some rays.
    std::mt19937 rand_engine;
    std::uniform_real_distribution<double> unit_rand(-1, 1);
    std::uniform_real_distribution<double> length_rand(map_extents - noise, map_extents);
    std::vector<glm::dvec3> rays;

    // Build rays to create a generally spherical shell.
    while (rays.size() < ray_count * 2)
    {
      rays.push_back(glm::dvec3(0.05));
      glm::dvec3 ray_end(unit_rand(rand_engine), unit_rand(rand_engine), unit_rand(rand_engine));
      ray_end = glm::normalize(ray_end);
      ray_end *= length_rand(rand_engine);
      rays.push_back(ray_end);
    }

    gpu_map.integrateRays(rays.data(), rays.size());

    // Do not sync back to CPU. Copy directly from GPU.
    EXPECT_TRUE(copyMap(dst_map, map, copyFilterExtents(copy_min, copy_max)));

    // Now sync the GPU map back to CPU and compare the whole map - they should match.
    gpu_map.syncVoxels();
    ohmtestutil::compareMaps(dst_map, map, copy_min, copy_max, ohmtestutil::kCfCompareExtended);
  }
}

TEST(Copy, CopyUpdated)
{
  // Test copying from GPU memory.
  const double map_extents = 10.0;
  const double noise = 1.0;
  const unsigned ray_count = 1024 * 128;
  const glm::dvec3 copy_min(0);
  const glm::dvec3 copy_max(4.5);
  OccupancyMap map(0.2);
  OccupancyMap dst_map(map.resolution());
  {
    GpuMap gpu_map(&map, true);

    // Make some rays.
    std::mt19937 rand_engine;
    std::uniform_real_distribution<double> unit_rand(-1, 1);
    std::uniform_real_distribution<double> length_rand(map_extents - noise, map_extents);
    std::vector<glm::dvec3> rays;

    // Build rays to create a generally spherical shell.
    while (rays.size() < ray_count * 2)
    {
      rays.push_back(glm::dvec3(0.05));
      glm::dvec3 ray_end(unit_rand(rand_engine), unit_rand(rand_engine), unit_rand(rand_engine));
      ray_end = glm::normalize(ray_end);
      ray_end *= length_rand(rand_engine);
      rays.push_back(ray_end);
    }

    gpu_map.integrateRays(rays.data(), rays.size());

    // Get the map stamp now.
    auto stamp = map.stamp();

    // Invoke the copy. Expect nothing to be copied.
    EXPECT_TRUE(ohm::copyMap(dst_map, map, ohm::copyFilterStamp(stamp)));
    EXPECT_EQ(dst_map.regionCount(), 0);

    // Make some updates to a single region.
    Key key(0, 0, 0, 0, 0, 0);

    rays.clear();

    for (uint8_t z = 0; z < map.regionVoxelDimensions().z; ++z)
    {
      for (uint8_t y = 0; y < map.regionVoxelDimensions().y; ++y)
      {
        for (uint8_t x = 0; x < map.regionVoxelDimensions().x; ++x)
        {
          key.setLocalKey(glm::u8vec3(x, y, z));
          // Make a ray which goes nowhere, but update a single voxel.
          rays.emplace_back(map.voxelCentreGlobal(key));
          rays.emplace_back(map.voxelCentreGlobal(key));
        }
      }
    }

    // Integrate the blatting of the region.
    gpu_map.integrateRays(rays.data(), rays.size());

    // Copy only updated regions.
    EXPECT_TRUE(ohm::copyMap(dst_map, map, ohm::copyFilterStamp(stamp)));

    // Validate the target map only has the region we've updated
    EXPECT_EQ(dst_map.regionCount(), 1);
    EXPECT_NE(dst_map.region(key.regionKey(), false), nullptr);

    // Sync to CPU for comparison.
    gpu_map.syncVoxels();
    std::cout << "Stamp @ 3: " << map.stamp() << std::endl;

    // Validate the region matches.
    // Lock the occupancy layers.
    Voxel<const float> src_occupancy(&map, map.layout().occupancyLayer(), key);
    ASSERT_TRUE(src_occupancy.isValid());
    Voxel<const float> dst_occupancy(&dst_map, dst_map.layout().occupancyLayer(), key);
    ASSERT_TRUE(dst_occupancy.isValid());

    // Compare voxels in the region.
    for (uint8_t z = 0; z < map.regionVoxelDimensions().z; ++z)
    {
      for (uint8_t y = 0; y < map.regionVoxelDimensions().y; ++y)
      {
        for (uint8_t x = 0; x < map.regionVoxelDimensions().x; ++x)
        {
          key.setLocalKey(glm::u8vec3(x, y, z));
          src_occupancy.setKey(key);
          dst_occupancy.setKey(key);
          ASSERT_TRUE(src_occupancy.isValid());
          ASSERT_TRUE(dst_occupancy.isValid());
          EXPECT_EQ(dst_occupancy.data(), src_occupancy.data());
        }
      }
    }
  }
}
}  // namespace maptests


================================================
FILE: tests/ohmtestgpu/GpuIncidentsTests.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include <ohm/DefaultLayer.h>
#include <ohm/Key.h>
#include <ohm/NdtMap.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapperNdt.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/Voxel.h>
#include <ohm/VoxelIncident.h>
#include <ohm/VoxelMean.h>

#include <ohmgpu/GpuMap.h>
#include <ohmgpu/GpuNdtMap.h>
#include <ohmgpu/RayItem.h>

#include <glm/gtx/norm.hpp>
#include <glm/vec3.hpp>

#include <algorithm>
#include <random>

namespace incidents
{
void testIncidentNormals(ohm::OccupancyMap &map, ohm::GpuMap &mapper, bool single_ray_batches)
{
  const unsigned iterations = 10;
  const unsigned ray_count = 1000u;
  std::vector<ohm::RayItem> ray_items;
  std::vector<glm::dvec3> rays;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-1.0, 1.0);

  // Set the map origin to avoid (0, 0, 0) being on a voxel boundary.
  map.setOrigin(glm::dvec3(-0.5 * map.resolution()));

  ASSERT_TRUE(map.incidentNormalEnabled());
  ASSERT_TRUE(map.voxelMeanEnabled());

  ray_items.reserve(ray_count);
  rays.reserve(2 * ray_count);
  for (unsigned i = 0; i < iterations; ++i)
  {
    unsigned expected_packed = 0;
    ray_items.clear();
    rays.clear();

    // We build into RayItem entries to ensure we sort into the same order as the GPU will and can process in that
    // order.
    for (unsigned r = 0; r < ray_count; ++r)
    {
      // Sample is at the origin. We'll build random rays around that.
      ohm::RayItem item{};
      item.sample = glm::dvec3(0.0);
      do
      {
        item.origin = glm::dvec3(uniform(rng), uniform(rng), uniform(rng));
      } while (glm::length2(item.origin) < 1e-6);  // Make sure it's not degenerate.
      // Normalise the origin ray, then expand it out to be larger than a single voxel.
      item.origin = glm::normalize(item.origin);
      item.origin *= map.resolution() * 3;
      item.origin_key = map.voxelKey(item.origin);
      item.origin_key = map.voxelKey(item.origin);
      item.sample_key = map.voxelKey(item.sample);
      ray_items.push_back(item);
    }

    // Now sort the ray items.
    std::sort(ray_items.begin(), ray_items.end());

    // Migrate into rays to use mapper API and make the expected calculations.
    for (unsigned r = 0; r < unsigned(ray_items.size()); ++r)
    {
      // Do the same calculation made by the mapper, but without encoding.
      const auto &item = ray_items[r];
      rays.emplace_back(item.origin);
      rays.emplace_back(item.sample);
      expected_packed = ohm::updateIncidentNormal(expected_packed, glm::vec3(item.origin) - glm::vec3(item.sample), r);
    }
    // Now use the ray mapper
    if (single_ray_batches)
    {
      // Need to add the rays one at a time because it's heavily affected by order (non deterministic)
      for (unsigned r = 0; r < ray_count; ++r)
      {
        mapper.integrateRays(rays.data() + r * 2, 2);
      }
    }
    else
    {
      mapper.integrateRays(rays.data(), ray_count * 2);
    }
    mapper.syncVoxels();

    // Check the result.
    ohm::Voxel<uint32_t> incident_voxel(&map, map.layout().layerIndex(ohm::default_layer::incidentNormalLayerName()));
    ASSERT_TRUE(incident_voxel.isLayerValid());
    incident_voxel.setKey(map.voxelKey(glm::dvec3(0)));
    ASSERT_TRUE(incident_voxel.isValid());

    // Convert the vectors for comparison - CPU works in double while GPU is float so we expect some error.
    const glm::vec3 mapped_incident = ohm::decodeNormal(incident_voxel.data());
    const glm::vec3 expected_incident = ohm::decodeNormal(expected_packed);
    const glm::vec3 diff_incident = expected_incident - mapped_incident;
    EXPECT_NEAR(glm::length(diff_incident), 0, 1e-2f);

    // Clear the incident normal
    incident_voxel.write(0);
    EXPECT_EQ(incident_voxel.data(), 0);

    // Also clear the voxel mean as we use the sample count from there.
    ohm::Voxel<ohm::VoxelMean> mean_voxel(&map, map.layout().meanLayer());
    ASSERT_TRUE(mean_voxel.isLayerValid());
    mean_voxel.setKey(map.voxelKey(glm::dvec3(0)));
    ASSERT_TRUE(mean_voxel.isValid());

    mean_voxel.write(ohm::VoxelMean{});
    EXPECT_EQ(mean_voxel.data().count, 0);
  }
}

TEST(Incident, WithOccupancy)
{
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kIncidentNormal);
  ohm::GpuMap mapper(&map, true);
  // Must do one ray at a time because of non-determinism.
  testIncidentNormals(map, mapper, true);
}

TEST(Incident, WithNdt)
{
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kIncidentNormal);
  ohm::GpuNdtMap mapper(&map, true);
  // NDT can batch rays because sample update is sequential.
  testIncidentNormals(map, mapper, false);
  // testIncidentNormals(map, mapper, true);
}
}  // namespace incidents


================================================
FILE: tests/ohmtestgpu/GpuLineKeysTests.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

#include <ohm/CalculateSegmentKeys.h>
#include <ohm/Key.h>
#include <ohm/KeyList.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohmgpu/LineKeysQueryGpu.h>
// #include <ohm/OccupancyType.h>

#include <logutil/LogUtil.h>

#include <ohmtools/OhmGen.h>
#include <ohmutil/GlmStream.h>

#include <chrono>
#include <random>

#include <gtest/gtest.h>

namespace linekeys
{
typedef std::chrono::high_resolution_clock TimingClock;

void compareResults(const ohm::LineKeysQuery &query, const ohm::LineKeysQuery &reference)
{
  EXPECT_EQ(query.numberOfResults(), reference.numberOfResults());
  const size_t result_count = std::min(query.numberOfResults(), reference.numberOfResults());

  const ohm::OccupancyMap &map = *query.map();
  ohm::Key key, key_ref;
  size_t index_offset, index_offset_ref;
  size_t key_count, key_count_ref;
  size_t compare_key_count;
  bool line_ok;
  const int allow_voxel_deviation = 2;
  for (size_t r = 0; r < result_count; ++r)
  {
    line_ok = true;
    index_offset = query.resultIndices()[r];
    key_count = query.resultCounts()[r];
    index_offset_ref = reference.resultIndices()[r];
    key_count_ref = reference.resultCounts()[r];

    // Result count may differ by one for floating point error.
    if (key_count >= key_count_ref)
    {
      EXPECT_LE(key_count - key_count_ref, 1);
      line_ok = key_count - key_count_ref <= 1;
    }
    else
    {
      EXPECT_LE(key_count_ref - key_count, 1);
      line_ok = key_count_ref - key_count <= 1;
    }

    compare_key_count = std::min(key_count, key_count_ref);

    // First and last keys must match exactly (start and end voxels).
    if (compare_key_count)
    {
      EXPECT_EQ(query.intersectedVoxels()[index_offset], reference.intersectedVoxels()[index_offset_ref]);
      EXPECT_EQ(query.intersectedVoxels()[index_offset + key_count - 1],
                reference.intersectedVoxels()[index_offset_ref + key_count_ref - 1]);
    }

    // Start at index 1. Already checked the start voxel.
    for (size_t k = 1; k < compare_key_count; ++k)
    {
      bool keys_ok = true;
      // The comparison of the voxels must account for differences in precision on CPU (double) and GPU (single).
      // We allow line keys to diverge by one voxel.
      key = query.intersectedVoxels()[index_offset + k];
      key_ref = reference.intersectedVoxels()[index_offset_ref + k];

      if (key != key_ref)
      {
        // Keys are not equal. Ensure we are only one voxel off.
        if (key.regionKey() == key_ref.regionKey())
        {
          // In the same region. Ensure we are only one voxel off.
          for (int a = 0; a < 3; ++a)
          {
            if (key.localKey()[a] != key_ref.localKey()[a])
            {
              // Divergent axis. We expect only a single voxel difference.
              const bool allowed_voxel_shift =
                std::abs(key.localKey()[a] - key_ref.localKey()[a]) <= allow_voxel_deviation;
              keys_ok = keys_ok && allowed_voxel_shift;
              EXPECT_LE(std::abs(key.localKey()[a] - key_ref.localKey()[a]), allow_voxel_deviation);
            }
          }
        }
        else
        {
          // Region change.
          for (int a = 0; a < 3; ++a)
          {
            if (key.regionKey()[a] != key_ref.regionKey()[a])
            {
              // Divergent axis. We expect only a single voxel difference.
              const bool single_region_shift = std::abs(key.regionKey()[a] - key_ref.regionKey()[a]) <= 1;
              keys_ok = keys_ok && single_region_shift;
              EXPECT_LE(std::abs(key.regionKey()[a] - key_ref.regionKey()[a]), 1);
            }
          }

          if (keys_ok)
          {
            // Check the local key change.
            for (int a = 0; a < 3; ++a)
            {
              if (key.localKey()[a] != key_ref.localKey()[a])
              {
                // Divergent axis. We expect one voxel to be zero and the other to be the maximum of
                // the next region.
                bool allowed_voxel_shift =
                  key.localKey()[a] == 0 &&
                    key_ref.localKey()[a] == map.regionVoxelDimensions()[a] - allow_voxel_deviation ||
                  key_ref.localKey()[a] == 0 &&
                    key.localKey()[a] == map.regionVoxelDimensions()[a] - allow_voxel_deviation ||
                  std::abs(key.regionKey()[a] - key_ref.regionKey()[a]) <= allow_voxel_deviation;
                EXPECT_TRUE(allowed_voxel_shift);
                if (!allowed_voxel_shift)
                {
                  std::cout << "a: " << a << std::endl;
                  // EXPECT_EQ((int)key.localKey()[a], 0);
                  // EXPECT_EQ((int)keyB.localKey()[a], 0);
                  // EXPECT_EQ((int)keyB.localKey()[a], map.regionVoxelDimensions()[a] - 1);
                  // EXPECT_EQ((int)key.localKey()[a], map.regionVoxelDimensions()[a] - 1);
                }
                keys_ok = keys_ok && allowed_voxel_shift;
              }
            }
          }
        }

        if (!keys_ok)
        {
          std::cout << "More than one voxel difference : " << key << " vs. " << key_ref << std::endl;
          line_ok = false;
        }
      }
    }

    if (!line_ok)
    {
      std::cout << std::setprecision(20);
      std::cout << "Failed with line [" << r << "]: " << (query.rays()[r * 2 + 0]) << " to "
                << (query.rays()[r * 2 + 1]) << std::endl;
      EXPECT_TRUE(line_ok);
    }
  }
}


void dumpLines(const ohm::LineKeysQuery &query)
{
  const size_t result_count = query.numberOfResults();
  size_t index_offset;
  size_t key_count;
  ohm::Key key;

  for (size_t r = 0; r < result_count; ++r)
  {
    index_offset = query.resultIndices()[r];
    key_count = query.resultCounts()[r];

    std::cout << r << " line length " << key_count << '\n';
    for (size_t k = 0; k < key_count; ++k)
    {
      key = query.intersectedVoxels()[index_offset + k];
      std::cout << "  " << k << ": " << key << '\n';
    }
  }
}


TEST(LineKeys, QueryGpu)
{
  const double query_half_extents = 10.0;
  const int query_count = 10000;
  const int iteration_count = 1;
  ohm::OccupancyMap map(0.25);

  std::mt19937 rand_engine;
  std::uniform_real_distribution<double> rand(-query_half_extents, query_half_extents);
  std::vector<glm::dvec3> line_points;

  // No need to populate the map. We are just calculating ray keys.

  // Full GPU evaluation query.
  ohm::LineKeysQueryGpu gpu_query(ohm::kQfGpuEvaluate | ohm::kQfNoCache);
  // Allow using cached GPU results query.
  ohm::LineKeysQueryGpu gpu_query2(ohm::kQfGpuEvaluate);
  // CPU query.
  ohm::LineKeysQuery cpu_query;

  gpu_query.setMap(&map);
  gpu_query2.setMap(&map);
  cpu_query.setMap(&map);

#if 1
  for (int i = 0; i < query_count; ++i)
  {
    line_points.push_back(glm::dvec3(rand(rand_engine), rand(rand_engine), rand(rand_engine)));
    line_points.push_back(glm::dvec3(rand(rand_engine), rand(rand_engine), rand(rand_engine)));
  }
#else   // #
  // The following line highlights a deviation between CPU and GPU algorithms. If you push these points
  // as dvec3, then there is a discrepancy between the CPU/GPU results.
  line_points.push_back(glm::vec3(8.249521446748637743, 1.6640723158759520572, 15.084516018081700395));
  line_points.push_back(glm::vec3(11.186323857973910378, -14.315011276937029905, 6.2049214437162198976));
#endif  // #
  gpu_query.setRays(line_points.data(), line_points.size());
  gpu_query2.setRays(line_points.data(), line_points.size());
  cpu_query.setRays(line_points.data(), line_points.size());

  // Run the initial queries now so we don't measure initialisation and memory allocation and overheads.
  gpu_query.execute();
  gpu_query2.execute();
  cpu_query.execute();

  ohm::KeyList keys;
  const size_t line_count = line_points.size() / 2;
  for (int i = 0; i < iteration_count; ++i)
  {
    std::cout << "Calculating " << line_points.size() / 2 << " line segments.\n";
    std::cout << "GPU  " << std::flush;
    const auto gpu_start = TimingClock::now();
    bool gpu_exec_ok = gpu_query.execute();
    const auto gpu_end = TimingClock::now();
    const auto gpu_duration = gpu_end - gpu_start;
    const auto gpu_per_line = gpu_duration / line_count;
    std::cout << gpu_duration << " " << gpu_per_line << " per line" << std::endl;
    EXPECT_TRUE(gpu_exec_ok);

    std::cout << "Calculating " << line_points.size() / 2 << " line segments.\n";
    std::cout << "GPU (cached)  " << std::flush;
    const auto gpu_start2 = TimingClock::now();
    bool gpu_exec_ok2 = gpu_query2.execute();
    const auto gpu_end2 = TimingClock::now();
    const auto gpu_duration2 = gpu_end2 - gpu_start2;
    const auto gpu_per_line2 = gpu_duration / line_count;
    std::cout << gpu_duration2 << " " << gpu_per_line2 << " per line" << std::endl;
    EXPECT_TRUE(gpu_exec_ok2);

    std::cout << "CPU  " << std::flush;
    auto cpu_start = TimingClock::now();
    bool cpu_exec_ok = cpu_query.execute();
    auto cpu_end = TimingClock::now();
    auto cpu_duration = cpu_end - cpu_start;
    auto cpu_per_line = cpu_duration / line_count;
    std::cout << cpu_duration << " " << cpu_per_line << " per line" << std::endl;
    EXPECT_TRUE(cpu_exec_ok);

    std::cout << "CPU2 " << std::flush;
    cpu_start = TimingClock::now();
    for (size_t j = 0; j < line_count; ++j)
    {
      calculateSegmentKeys(keys, map, line_points[j * 2 + 0], line_points[j * 2 + 1], true);
    }
    cpu_end = TimingClock::now();
    cpu_duration = cpu_end - cpu_start;
    cpu_per_line = cpu_duration / line_count;
    std::cout << cpu_duration << " " << cpu_per_line << " per line" << std::endl;

    // std::cout << "GPU:\n";
    // dumpLines(gpuQuery);
    // std::cout << "CPU:\n";
    // dumpLines(cpuQuery);
    compareResults(gpu_query, cpu_query);
    // Compare cached GPU query against the original.
    compareResults(gpu_query, gpu_query2);
  }
}
}  // namespace linekeys


================================================
FILE: tests/ohmtestgpu/GpuLineQueryTests.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <ohm/Key.h>
#include <ohm/KeyList.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyType.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/VoxelData.h>
#include <ohmgpu/ClearanceProcess.h>
#include <ohmgpu/LineQueryGpu.h>
#include <ohmgpu/OhmGpu.h>

#include <logutil/LogUtil.h>
#include <ohmtools/OhmCloud.h>
#include <ohmtools/OhmGen.h>
#include <ohmutil/GlmStream.h>
#include <ohmutil/Profile.h>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <random>

#include <gtest/gtest.h>

using namespace ohm;

namespace linequerytests
{
typedef std::chrono::high_resolution_clock TimingClock;

void sparseMap(OccupancyMap &map, bool add_hit = true)
{
  ohmgen::fillMapWithEmptySpace(map, -64, -64, -64, 63, 63, 63);
  Key key = map.voxelKey(glm::dvec3(0.5 * map.resolution()));
  Voxel<float> voxel(&map, map.layout().occupancyLayer());
  ASSERT_TRUE(voxel.isLayerValid());
  if (add_hit)
  {
    voxel.setKey(key);
    for (int i = 0; i < 1; ++i)
    {
      integrateHit(voxel);
    }
    float occupancy;
    voxel.read(&occupancy);
    EXPECT_TRUE(occupancy >= 0);
  }
}

void lineQueryTest(OccupancyMap &map, bool gpu)
{
  const double map_res = map.resolution();
  LineQueryGpu query(map, glm::dvec3(0) - glm::dvec3(map_res), glm::dvec3(0), 2.0f,
                     LineQuery::kDefaultFlags | kQfNearestResult);
  query.setStartPoint(glm::dvec3(-2, 0, 0));
  query.setEndPoint(glm::dvec3(2, 0, 0));
  if (gpu)
  {
    query.setQueryFlags(kQfNearestResult | kQfGpuEvaluate);
    query.setQueryFlags(kQfNearestResult | kQfGpuEvaluate);
  }

  const bool exec_ok = query.execute();
  ASSERT_TRUE(exec_ok);

  ASSERT_GT(query.numberOfResults(), 0);
  EXPECT_EQ(query.numberOfResults(), 1);

  std::cout << "Nearest result: " << query.intersectedVoxels()[0] << " @ " << query.ranges()[0] << std::endl;

  EXPECT_EQ(query.intersectedVoxels()[0], map.voxelKey(glm::dvec3(0)))
    << query.intersectedVoxels()[0] << " != " << map.voxelKey(glm::dvec3(0));
  EXPECT_EQ(query.ranges()[0], 0);

  std::string file_name("line-query-");
  file_name += (gpu) ? "gpu" : "cpu";
  file_name += ".ohm";
  ohm::save(file_name.c_str(), map);
}

TEST(LineQuery, Gpu)
{
  OccupancyMap map(0.1);
  sparseMap(map);
  lineQueryTest(map, true);
}

TEST(LineQuery, CpuVsGpuSimple)
{
  OccupancyMap map(0.1);
  sparseMap(map);

  const double map_res = map.resolution();
  LineQuery cpu_query(map, glm::dvec3(0) - glm::dvec3(map_res), glm::dvec3(0), 2.0f);
  cpu_query.setStartPoint(glm::dvec3(-5, 0, 0));
  cpu_query.setEndPoint(glm::dvec3(5, 0, 0));

  LineQueryGpu gpu_query(map, glm::dvec3(0) - glm::dvec3(map_res), glm::dvec3(0), 2.0f, kQfGpuEvaluate);
  gpu_query.setStartPoint(cpu_query.startPoint());
  gpu_query.setEndPoint(cpu_query.endPoint());

  cpu_query.execute();
  gpu_query.execute();

  EXPECT_EQ(cpu_query.numberOfResults(), gpu_query.numberOfResults());
  const size_t result_count = std::min(cpu_query.numberOfResults(), gpu_query.numberOfResults());
  for (size_t i = 0; i < result_count; ++i)
  {
    EXPECT_NEAR(cpu_query.ranges()[i], gpu_query.ranges()[i], 1e-6f);
  }
}

TEST(LineQuery, CpuVsGpu)
{
  Profile profile;
  const float boundary_distance = 5.0f;
  OccupancyMap map(0.1);

  // Build a cloud with real samples around a cubic boundary. Does not cover every voxel in the boundary.
  ohmgen::boxRoom(map, glm::dvec3(-boundary_distance), glm::dvec3(boundary_distance), 3);

  // Run the query and compare CPU/GPU results.
  LineQuery cpu_query(map, glm::dvec3(0), glm::dvec3(0), 2.0f);
  LineQueryGpu gpu_query(map, glm::dvec3(0), glm::dvec3(0), 2.0f, kQfGpuEvaluate);
  std::mt19937 rand_engine;
  std::uniform_real_distribution<double> rand(-1.1 * boundary_distance, 1.1 * boundary_distance);

  // Generate line set to test.
  std::vector<glm::dvec3> line_points;
  // Seed the query with a known line(s).
  line_points.push_back(glm::dvec3(-0.5f * boundary_distance, -0.25f * boundary_distance, 0.25f * boundary_distance));
  line_points.push_back(glm::dvec3(1.2f * boundary_distance));
  // line_points.push_back(glm::dvec3(-1.2f * boundaryDistance, 0, 0));
  // line_points.push_back(glm::dvec3(1.2f * boundaryDistance, 0, 0));

  const int query_iterations = 50;
  // const int query_iterations = 1;
  // Create new random line query points to meet the hard coded queryIterations.
  while (line_points.size() < query_iterations * 2)
  {
    line_points.push_back(glm::dvec3(rand(rand_engine), rand(rand_engine), rand(rand_engine)));
  }

  // Save data for debugging (optional)
  bool save_clouds = true;
  if (save_clouds)
  {
    ohmtools::saveCloud("boundary.ply", map);
  }

#if 0
    // Precalculate clearance values for the GPU map (using GPU).
    ClearanceProcess clearances(gpuQuery.searchRadius(), kQfGpuEvaluate);
    // Calculate for the whole map.
    std::cout << "Clearance Map: " << std::flush;
    auto clearanceStartTime = timing_clock::now();
    ProfileMarker gpuClearance("GPU Clearance", &profile);
    clearances.update(map, std::numeric_limits<float>::infinity());
    gpuClearance.end();
    auto clearanceElapsed = timing_clock::now() - clearanceStartTime;
    std::cout << clearanceElapsed << std::endl;
#endif  // #

  std::cout << "Compare results from " << query_iterations << " line queries." << std::endl;
  for (int i = 0; i < query_iterations; ++i)
  {
    // Setup query.
    cpu_query.setStartPoint(line_points[i * 2 + 0]);
    cpu_query.setEndPoint(line_points[i * 2 + 1]);
    gpu_query.setStartPoint(cpu_query.startPoint());
    gpu_query.setEndPoint(cpu_query.endPoint());

    std::cout << std::setprecision(4) << "Line: (" << cpu_query.startPoint().x << ", " << cpu_query.startPoint().y
              << ", " << cpu_query.startPoint().z << ")->(" << cpu_query.endPoint().x << ", " << cpu_query.endPoint().y
              << ", " << cpu_query.endPoint().z << ")" << std::endl;

    // Run CPU query
    std::cout << "CPU: " << std::flush;
    auto start_time = TimingClock::now();
    ProfileMarker mark_cpu("CPU", &profile);
    cpu_query.execute();
    mark_cpu.end();
    auto exec_time = TimingClock::now() - start_time;
    std::cout << exec_time << std::endl;

    if (save_clouds)
    {
      ohmtools::saveQueryCloud("boundary-cpu-line.ply", map, cpu_query, cpu_query.searchRadius());
    }

    // Run GPU query.
    std::cout << "GPU: " << std::flush;

    start_time = TimingClock::now();
    ProfileMarker mark_gpu("GPU", &profile);
    gpu_query.execute();
    mark_gpu.end();
    exec_time = TimingClock::now() - start_time;
    std::cout << exec_time << std::endl;

    if (save_clouds)
    {
      ohmtools::saveQueryCloud("boundary-gpu-line.ply", map, gpu_query, gpu_query.searchRadius());
      glm::dvec3 min_ext = gpu_query.startPoint();
      glm::dvec3 max_ext = min_ext;
      min_ext.x = std::min(min_ext.x, gpu_query.endPoint().x);
      min_ext.y = std::min(min_ext.y, gpu_query.endPoint().y);
      min_ext.z = std::min(min_ext.z, gpu_query.endPoint().z);
      max_ext.x = std::max(max_ext.x, gpu_query.endPoint().x);
      max_ext.y = std::max(max_ext.y, gpu_query.endPoint().y);
      max_ext.z = std::max(max_ext.z, gpu_query.endPoint().z);
      ohmtools::saveClearanceCloud("boundary-gpu-region.ply", map, min_ext, max_ext, gpu_query.searchRadius());
    }

    // Compare results.
    start_time = TimingClock::now();
    EXPECT_EQ(cpu_query.numberOfResults(), gpu_query.numberOfResults());
    const size_t result_count = std::min(cpu_query.numberOfResults(), gpu_query.numberOfResults());
    for (size_t i = 0; i < result_count; ++i)
    {
      EXPECT_EQ(cpu_query.intersectedVoxels()[i], gpu_query.intersectedVoxels()[i]);
      EXPECT_NEAR(cpu_query.ranges()[i], gpu_query.ranges()[i], float(0.5 * map.resolution()));
      if (std::abs(cpu_query.ranges()[i] - gpu_query.ranges()[i]) >= float(0.5 * map.resolution()))
      {
        auto voxel = gpu_query.intersectedVoxels()[i];
        std::cout << "@ voxel : " << voxel << ' ' << map.voxelCentreGlobal(voxel) << std::endl;
      }
    }
    exec_time = TimingClock::now() - start_time;
    std::cout << "Compared results in " << exec_time << std::endl;
  }
}

TEST(LineQuery, CpuVsGpuPerf)
{
  Profile profile;
  OccupancyMap map(0.1);
  sparseMap(map);

  // For this test we make repeated iterations to ensure that GPU ends up being faster when it can precalculate
  // and cache results.
  // In this test, we also compare the results to ensure they are the same.
  const int iterations = 50;
  TimingClock::time_point start_time_cpu, end_time_cpu, start_time_gpu, end_time_gpu;
  LineQueryGpu query(map, glm::dvec3(0), glm::dvec3(0), 2.0f);
  std::vector<Key> cpu_keys;
  std::vector<double> cpu_ranges;

  query.setStartPoint(glm::dvec3(-5, 0, 0));
  query.setEndPoint(glm::dvec3(5, 0, 0));
  query.setQueryFlags(LineQuery::kDefaultFlags | kQfNearestResult);

  std::cout << "Execute " << iterations << " query iterations." << std::endl;

  // CPU execution.
  std::cout << "CPU " << std::flush;
  start_time_cpu = TimingClock::now();
  for (int i = 0; i < iterations; ++i)
  {
    ProfileMarker mark_cpu("CPU", &profile);
    query.execute();
  }
  end_time_cpu = TimingClock::now();

  auto cpu_total_duration = end_time_cpu - start_time_cpu;
  auto cpu_average_duration = cpu_total_duration / iterations;

  std::cout << cpu_total_duration << " average completion " << cpu_average_duration << std::endl;

  // Collect CPU results.
  cpu_keys.resize(query.numberOfResults());
  cpu_ranges.resize(query.numberOfResults());
  for (size_t i = 0; i < query.numberOfResults(); ++i)
  {
    cpu_keys[i] = query.intersectedVoxels()[i];
    cpu_ranges[i] = query.ranges()[i];
  }

  // Touch the map so that the GPU query won't use values from the last iteration.
  map.touch();

  // GPU execution.
  query.reset();  // Hard reset.
  query.setQueryFlags(kQfNearestResult | ohm::kQfGpuEvaluate);

  std::cout << "GPU " << std::flush;
  start_time_gpu = TimingClock::now();
  for (int i = 0; i < iterations; ++i)
  {
    ProfileMarker mark_gpu("GPU", &profile);
    query.execute();
  }
  end_time_gpu = TimingClock::now();

  auto gpu_total_duration = end_time_gpu - start_time_gpu;
  auto gpu_average_duration = gpu_total_duration / iterations;
  std::cout << gpu_total_duration << " average completion " << gpu_average_duration << std::endl;

  // Compare results.
  EXPECT_EQ(cpu_keys.size(), query.numberOfResults());
  const size_t result_count = std::min(cpu_keys.size(), query.numberOfResults());
  // GPU has algorithmic differences which result in slightly different ranges at time.
  // Allow a tolerance for range comparison.
  const float ranges_tolerance = 0.5f * float(map.resolution());
  for (size_t i = 0; i < result_count; ++i)
  {
    EXPECT_EQ(cpu_keys[i], query.intersectedVoxels()[i]);
    if (std::signbit(cpu_ranges[i]) != std::signbit(query.ranges()[i]))
    {
      std::cerr << "Range sign mismatch for " << query.intersectedVoxels()[i] << std::endl;
      std::cerr << "CPU: " << cpu_ranges[i] << " GPU: " << query.ranges()[i] << std::endl;
    }
    EXPECT_NEAR(cpu_ranges[i], query.ranges()[i], ranges_tolerance);
  }

  // Validate that the GPU queries are faster (only with repeated iterations though).

  // Average out the timing to per iteration time.

  EXPECT_LT(gpu_total_duration.count(), cpu_total_duration.count());
  EXPECT_LT(gpu_average_duration.count(), cpu_average_duration.count());
}
}  // namespace linequerytests


================================================
FILE: tests/ohmtestgpu/GpuMapTest.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include <ohm/Aabb.h>
#include <ohm/KeyList.h>
#include <ohm/MapChunk.h>
#include <ohm/MapProbability.h>
#include <ohm/MapSerialise.h>
#include <ohm/NdtMap.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/RayMapperNdt.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/VoxelData.h>
#include <ohmgpu/GpuCache.h>
#include <ohmgpu/GpuMap.h>
#include <ohmgpu/GpuNdtMap.h>

#include <logutil/LogUtil.h>
#include <ohmtools/OhmCloud.h>

#include <chrono>
#include <fstream>
#include <future>
#include <iostream>
#include <random>
#include <thread>

// Assertion courtesy of https://github.com/google/googletest/issues/348
#define ASSERT_DURATION_LE(secs, stmt)                                                               \
  {                                                                                                  \
    std::promise<bool> completed;                                                                    \
    auto stmt_future = completed.get_future();                                                       \
    std::thread(                                                                                     \
      [&](std::promise<bool> &completed) {                                                           \
        stmt;                                                                                        \
        completed.set_value(true);                                                                   \
      },                                                                                             \
      std::ref(completed))                                                                           \
      .detach();                                                                                     \
    if (stmt_future.wait_for(std::chrono::seconds(secs)) == std::future_status::timeout)             \
      GTEST_FATAL_FAILURE_("       timed out (> " #secs " seconds). Check code for infinite loops"); \
  }

using namespace ohm;

namespace gpumap
{
typedef std::chrono::high_resolution_clock TimingClock;

typedef std::function<void(OccupancyMap &, GpuMap &)> PostGpuMapTestFunc;

struct GpuMapTestParams
{
  double resolution = 0.25;
  double ray_segment_length = 0;
  unsigned batch_size = 0u;
  size_t gpu_mem_size = 0u;
  glm::u8vec3 region_size = glm::u8vec3(32);
  bool voxel_means = false;
  bool ndt = false;
};

void gpuMapTest(GpuMapTestParams params, const std::vector<glm::dvec3> &rays, const PostGpuMapTestFunc &post_populate,
                const char *save_prefix = nullptr)
{
  // Test basic map populate using GPU and ensure it matches CPU (close enough).
  OccupancyMap cpu_map(params.resolution, params.region_size,
                       (params.voxel_means | params.ndt) ? MapFlag::kVoxelMean : MapFlag::kNone);
  OccupancyMap gpu_map(params.resolution, params.region_size,
                       (params.voxel_means | params.ndt) ? MapFlag::kVoxelMean : MapFlag::kNone);
  std::unique_ptr<NdtMap> cpu_ndt_map;
  std::unique_ptr<RayMapper> cpu_ray_mapper;

  // Setup CPU RayMapper
  if (params.ndt)
  {
    cpu_ndt_map = std::make_unique<NdtMap>(&cpu_map, true);
    cpu_ray_mapper = std::make_unique<RayMapperNdt>(cpu_ndt_map.get());
  }
  else
  {
    cpu_ray_mapper = std::make_unique<RayMapperOccupancy>(&cpu_map);
  }

  std::unique_ptr<GpuMap> gpu_wrap((!params.ndt) ?
                                     new GpuMap(&gpu_map, true, params.batch_size, params.gpu_mem_size) :
                                     new GpuNdtMap(&gpu_map, true, params.batch_size, params.gpu_mem_size));

  gpu_wrap->setRaySegmentLength(params.ray_segment_length);

  ASSERT_TRUE(gpu_wrap->gpuOk());

  if (!params.batch_size)
  {
    params.batch_size = unsigned(rays.size() / 2);
  }

  std::cout << "Integrating " << rays.size() / 2 << " rays into each map.\n";

#if 0
    // Output to CSV for Intel Code Builder.
    {
      std::ofstream out("rays.csv");
      glm::dvec3 p = rays[0];
      out << std::setprecision(20);
      for (size_t i = 0; i < rays.size(); ++i)
      {
        p = rays[i];
        out << p.x << ',' << p.y << ',' << p.z;
        if (i + 1 < rays.size())
        {
          out << ',';
        }
        out << '\n';
      }
    }

    {
      std::ofstream out("voxels.csv");

      const MapChunk *chunk = cpuMap.region(glm::i16vec3(0, 0, 0), true);
      const uint8_t *voxelMem = (const uint8_t *)chunk->occupancy;

      for (size_t i = 0; i < cpuMap.regionVoxelVolume(); ++i)
      {
        for (size_t j = 0; j < sizeof(*chunk->occupancy); ++j)
        {
          out << int(voxelMem[j]);

          if (j + 1 < sizeof(*chunk->occupancy) || i + 1 < cpuMap.regionVoxelVolume())
          {
            out << ',' << '\n';
          }
        }
      }
      out << '\n';
    }
    return;
#endif  // #

  std::cout << "GPU " << std::flush;
  const auto gpu_start = TimingClock::now();
  for (size_t i = 0; i < rays.size(); i += params.batch_size * 2)
  {
    const unsigned point_count = unsigned(std::min<size_t>(params.batch_size * 2, rays.size() - i));
    gpu_wrap->integrateRays(rays.data() + i, point_count);
  }
  const auto gpu_queued = TimingClock::now();
  std::cout << gpu_queued - gpu_start << '\n';

  std::cout << "GPU sync: " << std::flush;
  gpu_wrap->syncVoxels();
  const auto gpu_end = TimingClock::now();
  std::cout << (gpu_end - gpu_queued) << '\n';
  std::cout << "Per ray: " << (gpu_end - gpu_start) / (rays.size() / 2);
  std::cout << " queue: " << (gpu_queued - gpu_start) / (rays.size() / 2);
  std::cout << std::endl;

  std::cout << "CPU " << std::flush;
  const auto cpu_start = TimingClock::now();
  cpu_ray_mapper->integrateRays(rays.data(), unsigned(rays.size()));
  const auto cpu_end = TimingClock::now();
  const auto cpu_elapsed = cpu_end - cpu_start;
  std::cout << cpu_elapsed << ' ';
  std::cout << cpu_elapsed / (rays.size() / 2) << " per ray\n";

  if (post_populate)
  {
    post_populate(cpu_map, *gpu_wrap);
  }

  if (save_prefix)
  {
    const auto save_start = TimingClock::now();
    std::cout << "Saving clouds " << std::flush;

    std::string filename;
    filename = save_prefix;
    filename += "cloud-gpu.ply";
    ohmtools::saveCloud(filename.c_str(), gpu_map);
    filename = save_prefix;
    filename += "cloud-cpu.ply";
    ohmtools::saveCloud(filename.c_str(), cpu_map);
    filename = save_prefix;
    filename += "gpu.ohm";
    ohm::save(filename.c_str(), gpu_map);
    filename = save_prefix;
    filename += "cpu.ohm";
    ohm::save(filename.c_str(), cpu_map);

    const auto save_end = TimingClock::now();
    const auto save_elapsed = save_end - save_start;
    std::cout << save_elapsed << std::endl;
  }

  // Control release order.
  cpu_ray_mapper.reset(nullptr);
  cpu_ndt_map.reset(nullptr);
  gpu_wrap.reset(nullptr);
}

void compareMaps(const OccupancyMap &reference_map, const OccupancyMap &test_map)
{
  const auto compare_start = TimingClock::now();
  std::cout << "Compare maps " << std::flush;
  // We need to allow for some discrepancies as the GPU map is non-deterministic.
  const float allowed_failure_ratio = 0.01f;

  // Note: this test may be too prescriptive.
  // Iterate the CPU map and lookup the GPU map.
  unsigned failures = 0;
  unsigned processed = 0;
  unsigned logged_failures = 0;
  float expect, actual;

  glm::dvec3 ref_pos, test_pos;

  const auto should_report_failure = [&failures, &processed, &logged_failures, &allowed_failure_ratio]() {
    return float(failures) / float(processed) > allowed_failure_ratio && logged_failures < 100;
  };

  ohm::Voxel<const float> ref_occupancy(&reference_map, reference_map.layout().occupancyLayer());
  ohm::Voxel<const ohm::VoxelMean> ref_mean(&reference_map, reference_map.layout().meanLayer());
  ohm::Voxel<const float> test_occupancy(&test_map, test_map.layout().occupancyLayer());
  ohm::Voxel<const ohm::VoxelMean> test_mean(&test_map, test_map.layout().meanLayer());
  ASSERT_TRUE(ref_occupancy.isLayerValid());
  ASSERT_TRUE(test_occupancy.isLayerValid());
  ASSERT_EQ(test_mean.isLayerValid(), ref_mean.isLayerValid());
  for (auto iter = reference_map.begin(); iter != reference_map.end(); ++iter)
  {
    ohm::setVoxelKey(iter, ref_occupancy, ref_mean);
    if (!ohm::isUnobservedOrNull(ref_occupancy))
    {
      ++processed;
      // Note: we must deference the iterator and use the key value only because the iterator is from a difference
      // map.
      ohm::setVoxelKey(*iter, test_occupancy, test_mean);

      if (test_occupancy.isValid())
      {
        bool ok = true;
        ref_occupancy.read(&expect);
        test_occupancy.read(&actual);

        if (std::abs(expect - actual) >= reference_map.hitValue() * 0.5f)
        {
          ok = false;

          if (should_report_failure())
          {
            EXPECT_NEAR(expect, actual, reference_map.hitValue() * 0.5f);
            ++logged_failures;
          }
        }

        ref_pos = ohm::positionSafe(ref_mean);
        test_pos = ohm::positionSafe(test_mean);

        // Ensure we aren't just getting the default code path all the time.
        if (ref_mean.isValid())
        {
          const glm::dvec3 pos = ohm::positionUnsafe(ref_mean);
          EXPECT_NEAR(ref_pos.x, pos.x, 1e-4f);
          EXPECT_NEAR(ref_pos.y, pos.y, 1e-4f);
          EXPECT_NEAR(ref_pos.z, pos.z, 1e-4f);
        }

        if (glm::any(glm::greaterThan(glm::abs(ref_pos - test_pos), glm::dvec3(1e-5))))
        {
          ok = false;
          if (should_report_failure())
          {
            EXPECT_NEAR(ref_pos.x, test_pos.x, 1e-5);
            EXPECT_NEAR(ref_pos.y, test_pos.y, 1e-5);
            EXPECT_NEAR(ref_pos.z, test_pos.z, 1e-5);
          }
        }

        if (!ok)
        {
          ++failures;
        }
      }
      else
      {
        ++failures;
        if (should_report_failure())
        {
          // Revalidate the voxel for reporting purposes only. We know it's false here.
          EXPECT_TRUE(test_occupancy.isValid());
          ++logged_failures;
        }
      }
    }
  }

  if (processed)
  {
    EXPECT_LT(float(failures) / float(processed), allowed_failure_ratio);
  }

  const auto compare_end = TimingClock::now();
  const auto compare_elapsed = compare_end - compare_start;
  std::cout << compare_elapsed << std::endl;
}

void compareCpuGpuMaps(const OccupancyMap &reference_map, const GpuMap &test_map)
{
  return compareMaps(reference_map, test_map.map());
}

TEST(GpuMap, PopulateTiny)
{
  GpuMapTestParams params;
  params.batch_size = 2;

  // Make a ray.
  std::vector<glm::dvec3> rays;
  rays.emplace_back(glm::dvec3(0.3));
  rays.emplace_back(glm::dvec3(1.1));

  rays.emplace_back(glm::dvec3(-5.0));
  rays.emplace_back(glm::dvec3(0.3));

  gpuMapTest(params, rays, compareCpuGpuMaps, "tiny");
}

TEST(GpuMap, PopulateSmall)
{
  const double map_extents = 50.0;
  const unsigned ray_count = 64;
  GpuMapTestParams params;
  params.batch_size = 32;

  // Make some rays.
  std::mt19937 rand_engine;
  std::uniform_real_distribution<double> rand(-map_extents, map_extents);
  std::vector<glm::dvec3> rays;

  while (rays.size() < ray_count * 2)
  {
    rays.emplace_back(glm::dvec3(0.05));
    rays.emplace_back(glm::dvec3(rand(rand_engine), rand(rand_engine), rand(rand_engine)));
  }

  gpuMapTest(params, rays, compareCpuGpuMaps, "small");
}

TEST(GpuMap, PopulateLarge)
{
  const double map_extents = 25.0;
  const unsigned ray_count = 1024 * 128;

  GpuMapTestParams params;
  params.batch_size = 2 * 1024u;

  // Make some rays.
  std::mt19937 rand_engine;
  std::uniform_real_distribution<double> rand(-map_extents, map_extents);
  std::vector<glm::dvec3> rays;

  while (rays.size() < ray_count * 2)
  {
    rays.emplace_back(glm::dvec3(0.05));
    rays.emplace_back(glm::dvec3(rand(rand_engine), rand(rand_engine), rand(rand_engine)));
  }

  gpuMapTest(params, rays, compareCpuGpuMaps, "large");
}

TEST(GpuMap, PopulateSmallCache)
{
  const double map_extents = 50.0;
  const unsigned ray_count = 1024 * 8;

  GpuMapTestParams params;
  params.batch_size = 1024 * 2;
  params.gpu_mem_size = 256u * 1024u * 1024;

  // Make some rays.
  std::mt19937 rand_engine;
  std::uniform_real_distribution<double> rand(-map_extents, map_extents);
  std::vector<glm::dvec3> rays;

  while (rays.size() < ray_count * 2)
  {
    rays.emplace_back(glm::dvec3(0.05));
    rays.emplace_back(glm::dvec3(rand(rand_engine), rand(rand_engine), rand(rand_engine)));
  }

  // Small cache: 256MiB.
  gpuMapTest(params, rays, PostGpuMapTestFunc(), "small-cache-");
}

TEST(GpuMap, PopulateMultiple)
{
  // Test having multiple GPU maps operating at once to ensure we don't get any GPU management issues.
  const double map_extents = 50.0;
  const double resolution = 0.25;
  const unsigned ray_count = 1024 * 8;
  const unsigned batch_size = 1024 * 2;  // Must be even
  const size_t target_gpu_cache_size = GpuCache::kMiB * 200;
  const glm::u8vec3 region_size(32);
  // Make some rays.
  std::mt19937 rand_engine;
  std::uniform_real_distribution<double> rand(-map_extents, map_extents);
  std::vector<glm::dvec3> rays;

  while (rays.size() < ray_count * 2)
  {
    rays.emplace_back(glm::dvec3(0.05));
    rays.emplace_back(glm::dvec3(rand(rand_engine), rand(rand_engine), rand(rand_engine)));
  }

  // Two simultaneous, maps with the same scope.
  OccupancyMap map1(resolution, region_size);
  GpuMap gpu_map1(&map1, true, batch_size, target_gpu_cache_size);  // Borrow pointer.
  OccupancyMap map2(resolution, region_size);
  GpuMap gpu_map2(&map2, true, batch_size, target_gpu_cache_size);  // Borrow pointer.

  // Third map with transient GpuMap wrapper.
  OccupancyMap map3(resolution, region_size);

  for (unsigned i = 0; i < rays.size(); i += batch_size)
  {
    std::cout << "\r" << i << " / " << rays.size() << std::flush;

    const unsigned remaining = unsigned(rays.size() - i);
    const unsigned current_batch_size = std::min(batch_size, remaining);
    gpu_map1.integrateRays(rays.data() + i, current_batch_size);
    gpu_map2.integrateRays(rays.data() + i, current_batch_size);

    GpuMap gpu_map3(&map3, true, batch_size, target_gpu_cache_size);  // Borrow pointer.
    gpu_map3.integrateRays(rays.data() + i, current_batch_size);
    gpu_map3.syncVoxels();

    // Forth, transient map.
    OccupancyMap map4(resolution, region_size);
    // std::cout << "\n" << map4.origin() << std::endl;
    GpuMap gpu_map4(&map4, true, batch_size, target_gpu_cache_size);  // Borrow pointer.
    gpu_map4.integrateRays(rays.data() + i, current_batch_size);
    gpu_map4.syncVoxels();
  }
  std::cout << "\r" << rays.size() << " / " << rays.size() << std::endl;

  gpu_map1.syncVoxels();
  gpu_map2.syncVoxels();

  std::cout << "Comparing maps" << std::endl;
  compareMaps(map1, map2);
  compareMaps(map1, map3);
}

TEST(GpuMap, PopulateSegmented)
{
  // Populate a map with long rays being segmented into multiple, smaller parts.

  // For this we'll use 100 rays, 100m long each at 25cm and set the segment length to 15m.
  // We'll also populate a control map (no segmentation) for validation and performance comparison.

  const double ray_length = 50.0;
  const unsigned ray_count = 100;

  GpuMapTestParams params;
  params.ray_segment_length = 15.0;
  params.batch_size = ray_count;
  params.voxel_means = true;

  std::vector<glm::dvec3> rays;
  std::mt19937 re;
  re.seed(5489ul);
  std::uniform_real_distribution<double> rand(0, 1.0);

  for (unsigned i = 0; i < ray_count; ++i)
  {
    // Start all rays at the origin.
    rays.emplace_back(glm::dvec3(0.0));
    glm::dvec3 sample(rand(re), rand(re), rand(re));  // Random ~unit vector.
    sample = glm::normalize(sample) * ray_length;
    rays.emplace_back(sample);
  }

  // Now run the segmented, and unsegmented tests the two gpu maps.
  gpuMapTest(params, rays, compareCpuGpuMaps, "segmented");
}

TEST(GpuMap, PopulateSegmentedNdt)
{
  // Populate a map with long rays being segmented into multiple, smaller parts.

  // For this we'll use 100 rays, 100m long each at 25cm and set the segment length to 15m.
  // We'll also populate a control map (no segmentation) for validation and performance comparison.

  const double ray_length = 50.0;
  const unsigned ray_count = 100;

  GpuMapTestParams params;
  params.ray_segment_length = 15.0;
  params.batch_size = ray_count;
  params.ndt = true;

  std::vector<glm::dvec3> rays;
  std::mt19937 re;
  re.seed(5489ul);
  std::uniform_real_distribution<double> rand(0, 1.0);

  for (unsigned i = 0; i < ray_count; ++i)
  {
    // Start all rays at the origin.
    rays.emplace_back(glm::dvec3(0.0));
    glm::dvec3 sample(rand(re), rand(re), rand(re));  // Random ~unit vector.
    sample = glm::normalize(sample) * ray_length;
    rays.emplace_back(sample);
  }

  // Now run the segmented, and unsegmented tests the two gpu maps.
  gpuMapTest(params, rays, compareCpuGpuMaps, "segmented-ndt");
}

TEST(GpuMap, Compare)
{
  const double resolution = 0.25;

  GpuMapTestParams params;
  params.region_size = glm::u8vec3(16);

  std::vector<glm::dvec3> rays;

  // Create a map for generating voxel centres.
  OccupancyMap grid_map(resolution, params.region_size);
  Key key(glm::i16vec3(0), 0, 0, 0);
  glm::dvec3 v;
  // Create a set of rays which will densely populate a single region.
  for (int z = 0; z < params.region_size.z; ++z)
  {
    key.setLocalAxis(2, z);
    for (int y = 0; y < params.region_size.y; ++y)
    {
      key.setLocalAxis(1, y);
      for (int x = 0; x < params.region_size.x; ++x)
      {
        key.setLocalAxis(0, x);
        v = grid_map.voxelCentreGlobal(key);
        // Create a ray starting and ending in the selected voxel.
        rays.emplace_back(v);
        rays.emplace_back(v);
      }
    }
  }

  const auto compare_results = [params](OccupancyMap &cpu_map, OccupancyMap &gpu_map) {
    Key key(glm::i16vec3(0), 0, 0, 0);
    ohm::Voxel<const float> cpu_voxel(&cpu_map, cpu_map.layout().occupancyLayer());
    ohm::Voxel<const float> gpu_voxel(&gpu_map, gpu_map.layout().occupancyLayer());
    ASSERT_TRUE(cpu_voxel.isLayerValid());
    ASSERT_TRUE(gpu_voxel.isLayerValid());
    // Walk the region pulling a voxel from both maps and comparing.
    for (int z = 0; z < params.region_size.z; ++z)
    {
      key.setLocalAxis(2, z);
      for (int y = 0; y < params.region_size.y; ++y)
      {
        key.setLocalAxis(1, y);
        for (int x = 0; x < params.region_size.x; ++x)
        {
          key.setLocalAxis(0, x);
          cpu_voxel.setKey(key);
          gpu_voxel.setKey(key);

          ASSERT_TRUE(cpu_voxel.isValid());
          ASSERT_TRUE(gpu_voxel.isValid());

          float cpu_occupancy, gpu_occupancy;
          cpu_voxel.read(&cpu_occupancy);
          gpu_voxel.read(&gpu_occupancy);

          EXPECT_EQ(cpu_occupancy, gpu_occupancy);

          if (cpu_occupancy != gpu_occupancy)
          {
            std::cout << "Voxel error: " << key << '\n';
          }
        }
      }
    }
  };

  const auto compare_and_clear = [params, compare_results](OccupancyMap &cpu_map, GpuMap &gpu_map) {
    compare_results(cpu_map, gpu_map.map());

    // Now we will try clear all the voxels from the bottom slice, except for those at max Y in the region.
    // To help, we adjust the miss value to greater than the hit probability and then some.
    cpu_map.setMissProbability(valueToProbability(-cpu_map.hitValue() + cpu_map.missValue()));
    gpu_map.map().setMissProbability(valueToProbability(-gpu_map.map().hitValue() + gpu_map.map().missValue()));

    // Build the clearing rays.
    std::vector<glm::dvec3> clear_rays;
    Key from_key(glm::i16vec3(0), 0, 0, 0);
    Key to_key(glm::i16vec3(0), 0, params.region_size.y - 1, 0);
    glm::dvec3 from, to;

    for (int x = 0; x < params.region_size.x; ++x)
    {
      from_key.setLocalAxis(0, x);
      to_key.setLocalAxis(0, x);

      from = cpu_map.voxelCentreGlobal(from_key);
      to = cpu_map.voxelCentreGlobal(to_key);

      clear_rays.emplace_back(from);
      clear_rays.emplace_back(to);
    }

    // Add the rays.
    gpu_map.integrateRays(clear_rays.data(), unsigned(clear_rays.size()));
    // dumpKeys = true;
    cpu_map.integrateRays(clear_rays.data(), unsigned(clear_rays.size()));
    gpu_map.syncVoxels();

    compare_results(cpu_map, gpu_map.map());
  };

  // gpuMapTest(params, rays, compareResults, "grid-");
  gpuMapTest(params, rays, compare_and_clear, "grid-");
}


TEST(GpuMap, ClipBox)
{
  // Test clipping of rays to an Aabb on insert.
  const double resolution = 0.2;
  const unsigned batch_size = 2 * 1024u;
  const uint8_t region_size = 32u;
  OccupancyMap gpu_map(resolution, glm::u8vec3(region_size));
  GpuMap gpu_wrap(&gpu_map, true, unsigned(batch_size * 2));  // Borrow pointer.

  Aabb clip_box(glm::dvec3(-1.0), glm::dvec3(2.0));

  const auto clip_filter = [&clip_box](glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags) {
    return clipBounded(start, end, filter_flags, clip_box);
  };
  gpu_wrap.setRayFilter(clip_filter);

  std::vector<glm::dvec3> rays;

  // Start with rays which pass through the box.
  rays.push_back(glm::dvec3(-2, 0, 0));
  rays.push_back(glm::dvec3(3, 0, 0));

  rays.push_back(glm::dvec3(0, -2, 0));
  rays.push_back(glm::dvec3(0, 3, 0));

  rays.push_back(glm::dvec3(0, 0, 3));
  rays.push_back(glm::dvec3(0, 0, -2));

  gpu_wrap.integrateRays(rays.data(), unsigned(rays.size()));
  gpu_wrap.syncVoxels();

  // Validate the map contains no occupied points; only free and unknown.
  const glm::dvec3 voxel_half_extents(0.5 * gpu_map.resolution());
  bool touched = false;
  ohm::Voxel<const float> voxel(&gpu_map, gpu_map.layout().occupancyLayer());
  ASSERT_TRUE(voxel.isLayerValid());
  for (auto iter = gpu_map.begin(); iter != gpu_map.end(); ++iter)
  {
    voxel.setKey(iter);
    touched = true;

    if (voxel.isValid())
    {
      float occupancy;
      voxel.read(&occupancy);
      if (!ohm::isUnobserved(occupancy))
      {
        EXPECT_LT(occupancy, gpu_map.occupancyThresholdValue());
        EXPECT_FALSE(ohm::isOccupied(voxel));

        // Voxel should also be with in the bounds of the Aabb. Check this.
        const Aabb voxel_box(gpu_map.voxelCentreGlobal(voxel.key()) - voxel_half_extents,
                             gpu_map.voxelCentreGlobal(voxel.key()) + voxel_half_extents);
        EXPECT_TRUE(clip_box.overlaps(voxel_box)) << "Voxel box does not overlap extents";
      }
    }
  }
  voxel.reset();

  EXPECT_TRUE(touched);

  // Reset the map. This also tests that resetting a GPU map works.
  gpu_map.clear();

  // Now rays which enter the box, ending at the origin.
  // Start with rays which pass through the box.
  rays.push_back(glm::dvec3(-2, 0, 0));
  rays.push_back(glm::dvec3(0, 0, 0));

  rays.push_back(glm::dvec3(0, -2, 0));
  rays.push_back(glm::dvec3(0, 0, 0));

  rays.push_back(glm::dvec3(0, 0, 3));
  rays.push_back(glm::dvec3(0, 0, 0));

  gpu_wrap.integrateRays(rays.data(), unsigned(rays.size()));
  gpu_wrap.syncVoxels();

  // Validate the map contains an occupied point at the origin voxel.
  const Key target_key = gpu_map.voxelKey(glm::dvec3(0));
  touched = false;
  voxel = ohm::Voxel<const float>(&gpu_map, gpu_map.layout().occupancyLayer());
  ASSERT_TRUE(voxel.isLayerValid());
  for (auto iter = gpu_map.begin(); iter != gpu_map.end(); ++iter)
  {
    voxel.setKey(iter);
    touched = true;

    if (voxel.isValid())
    {
      float occupancy;
      voxel.read(&occupancy);
      if (voxel.key() != target_key)
      {
        if (!ohm::isUnobserved(voxel))
        {
          EXPECT_LT(occupancy, gpu_map.occupancyThresholdValue());
        }
        EXPECT_FALSE(ohm::isOccupied(voxel));
      }
      else
      {
        EXPECT_GE(occupancy, gpu_map.occupancyThresholdValue());
        EXPECT_TRUE(ohm::isOccupied(voxel));
      }

      // Touched voxels should also be with in the bounds of the Aabb. Check this.
      if (!ohm::isUnobserved(voxel))
      {
        const Aabb voxel_box(gpu_map.voxelCentreGlobal(voxel.key()) - voxel_half_extents,
                             gpu_map.voxelCentreGlobal(voxel.key()) + voxel_half_extents);
        EXPECT_TRUE(clip_box.overlaps(voxel_box)) << "Voxel box does not overlap extents";
      }
    }
  }
  voxel.reset();

  EXPECT_TRUE(touched);
}


TEST(GpuMap, ClipBoxCompare)
{
  // Test clipping of rays to an Aabb on insert.
  const double resolution = 0.2;
  const unsigned batch_size = 2 * 1024u;
  const uint8_t region_size = 32u;
  OccupancyMap cpu_map(resolution, glm::u8vec3(region_size));
  OccupancyMap gpu_map(resolution, glm::u8vec3(region_size));
  GpuMap gpu_wrap(&gpu_map, true, unsigned(batch_size * 2));  // Borrow pointer.

  Aabb clip_box(glm::dvec3(-1.0), glm::dvec3(2.0));

  const auto clip_filter = [&clip_box](glm::dvec3 *start, glm::dvec3 *end, unsigned *filter_flags) {
    return clipBounded(start, end, filter_flags, clip_box);
  };

  cpu_map.setRayFilter(clip_filter);
  gpu_wrap.setRayFilter(clip_filter);

  std::vector<glm::dvec3> rays;

  // Compare GPU/CPU map clipping results.
  // Start with rays which pass through the box.
  rays.push_back(glm::dvec3(-2, 0, 0));
  rays.push_back(glm::dvec3(0, 0, 0));

  rays.push_back(glm::dvec3(0, -2, 0));
  rays.push_back(glm::dvec3(0, 0, 0));

  rays.push_back(glm::dvec3(0, 0, 3));
  rays.push_back(glm::dvec3(0, 0, 0));

  cpu_map.integrateRays(rays.data(), unsigned(rays.size()));
  gpu_wrap.integrateRays(rays.data(), unsigned(rays.size()));
  gpu_wrap.syncVoxels();

  compareMaps(cpu_map, gpu_map);
}

TEST(GpuMap, VoxelMean)
{
  // Populate with voxel means
  const double map_extents = 50.0;
  const unsigned ray_count = 64;

  GpuMapTestParams params;
  params.batch_size = 32;
  params.voxel_means = true;

  // Make some rays.
  std::mt19937 rand_engine;
  std::uniform_real_distribution<double> rand(-map_extents, map_extents);
  std::vector<glm::dvec3> rays;

  while (rays.size() < ray_count * 2)
  {
    rays.emplace_back(glm::dvec3(0.05));
    rays.emplace_back(glm::dvec3(rand(rand_engine), rand(rand_engine), rand(rand_engine)));
  }

  gpuMapTest(params, rays, compareCpuGpuMaps, "voxelmean");
}

TEST(GpuMap, CheckBadRays)
{
  GpuMapTestParams params;
  params.resolution = 0.1;
  params.batch_size = 32;
  params.voxel_means = true;

  // Rays which have been known to cause infinite loops.
  std::vector<glm::dvec3> rays =  //
    {                             //
      // Infinite loop walking regions before integrating into the map.
      // Basically it's rays under the length epsilon which are in different voxels.
      glm::dvec3{ -2.699077907025583, -1.5999031032475868, 1.0755428728082643 },
      glm::dvec3{ -2.6998157732186034, -1.6000298354709896, 1.0756803244026165 }
    };


  ASSERT_DURATION_LE(5, gpuMapTest(params, rays, compareCpuGpuMaps, "bad-rays"));
}
}  // namespace gpumap


================================================
FILE: tests/ohmtestgpu/GpuMapperTests.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "ohmtestcommon/OhmTestUtil.h"

#include <ohm/DefaultLayer.h>
#include <ohm/Key.h>
#include <ohm/KeyList.h>
#include <ohm/MapSerialise.h>
#include <ohm/Mapper.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyType.h>
#include <ohm/OccupancyUtil.h>
#include <ohmgpu/ClearanceProcess.h>
#include <ohmgpu/GpuCache.h>
#include <ohmgpu/GpuMap.h>
#include <ohmgpu/OhmGpu.h>

#include <ohmtools/OhmCloud.h>
#include <ohmtools/OhmGen.h>

#include <logutil/LogUtil.h>

#include <ohmutil/Profile.h>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <random>

#include <gtest/gtest.h>

using namespace ohm;

namespace mappertests
{
// NOTE: This test has been broken for some time. This has been hidden by the fact that the validation has been using
// exactly the same, incorrect results to compare against.
TEST(Mapper, Clearance)
{
  // This is a very rudimentary test for the ClearanceProcess. We build a map, generate the clearance values, clone
  // the map and generate it's clearance values and ensure the results match.
  // This test is only partly maintained and the ClearanceProcess is an experimental feature.

  Profile profile;
  const double map_extents = 10.0;
  const double noise = 1.0;
  const double resolution = 0.25;
  const unsigned ray_count = 1024 * 128;
  const unsigned batch_size = 1024 * 2;
  const glm::u8vec3 region_size(32);
  const float clearance_range = 3.0f;
  // const unsigned clearance_flags = kQfGpuEvaluate | kQfUnknownAsOccupied;
  const unsigned clearance_flags = kQfGpuEvaluate;

  // Make some rays.
  std::mt19937 rand_engine;
  std::uniform_real_distribution<double> unit_rand(-1, 1);
  std::uniform_real_distribution<double> length_rand(map_extents - noise, map_extents);
  std::vector<glm::dvec3> rays;

  // Build rays to create a generally spherical shell.
  while (rays.size() < ray_count * 2)
  {
    rays.push_back(glm::dvec3(0.05));
    glm::dvec3 ray_end(unit_rand(rand_engine), unit_rand(rand_engine), unit_rand(rand_engine));
    ray_end = glm::normalize(ray_end);
    ray_end *= length_rand(rand_engine);
    rays.push_back(ray_end);
  }

  // Setup the map and mapper
  // Test basic map populate using GPU and ensure it matches CPU (close enough).
  OccupancyMap map(resolution, region_size, MapFlag::kNone);
  Mapper mapper(&map);

  ClearanceProcess *clearance_process = new ClearanceProcess(clearance_range, clearance_flags);
  mapper.addProcess(clearance_process);
  clearance_process->ensureClearanceLayer(map);
  // clearance_process = nullptr;  // Ownership lost.

  // Insantiate the GPU map after the clearance layer has been created. Saves resetting the GPU cache.
  GpuMap gpu_map(&map, true, unsigned(batch_size * 2));  // Borrow pointer.

  ASSERT_TRUE(gpu_map.gpuOk());

  std::cout << "Integrating " << rays.size() / 2 << " rays.\n";

  ProfileMarker populate_marker("populate", false);
  ProfileMarker mapper_marker("mapper", false);
  for (size_t i = 0; i < rays.size(); i += batch_size * 2)
  {
    populate_marker.restart();
    const unsigned point_count = unsigned(std::min<size_t>(batch_size * 2, rays.size() - i));
    gpu_map.integrateRays(rays.data() + i, point_count);
    populate_marker.end();
    mapper_marker.restart();
    // FIXME(KS): As part of this tests lack of maintenance, I've commented the update here in favour of a full update
    // later. This update introduces some non-determinism and error in the final values.
    // // Partial update unless this is the last update. Full update for the last update (below).
    // mapper.update(0.01);
    mapper_marker.end();
  }
  populate_marker.end();

  {
    std::cout << "Finalising clearance" << std::endl;
    gpu_map.syncVoxels();
    ProfileMarker mapper_finalise_marker("mapper-finalise");
    mapper.update(0.0);
  }
  {
    ProfileMarker sync_marker("sync");
    std::cout << "Syncing occupancy" << std::endl;
    gpu_map.syncVoxels();
  }

  // Clone the map and calculate clearance values accross the whole map for verification.
  std::cout << "Cloning map" << std::endl;
  std::unique_ptr<OccupancyMap> clone_map(map.clone());
  {
    std::cout << "Calculating cloned clearance" << std::endl;
    ClearanceProcess clone_clearance(clearance_range, clearance_flags);
    ProfileMarker clearance_only_marker("clearanceOnly");
    // Update via explicit extents to force recalculation.
    glm::dvec3 min_ext{};
    glm::dvec3 max_ext{};
    clone_map->calculateExtents(&min_ext, &max_ext);
    clone_clearance.calculateForExtents(*clone_map, min_ext, max_ext);
  }

  std::cout << "Saving maps" << std::endl;
  ohm::save("mapper-map.ohm", map);
  ohm::save("mapper-clone.ohm", *clone_map);
  ohmtools::saveCloud("mapper-map.ply", map);
  ohmtools::saveCloud("mapper-clone.ply", *clone_map);
  ohmtools::saveClearanceCloud("mapper-map-clearance.ply", map, glm::dvec3(-10), glm::dvec3(10), clearance_range);
  ohmtools::saveClearanceCloud("mapper-clone-clearance.ply", *clone_map, glm::dvec3(-10), glm::dvec3(10),
                               clearance_range);

  // Compare maps.
  std::cout << "Comparing maps" << std::endl;
  ohmtestutil::compareMaps(map, *clone_map, ohmtestutil::kCfCompareExtended | ohmtestutil::kCfExpectClearance);
}
}  // namespace mappertests


================================================
FILE: tests/ohmtestgpu/GpuNdtTests.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas, Jason Williams
#include <ohm/DefaultLayer.h>
#include <ohm/Key.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/NdtMap.h>
#include <ohm/OccupancyMap.h>
#include <ohm/Trace.h>
#include <ohm/VoxelData.h>

#include <ohmgpu/GpuCache.h>
#include <ohmgpu/GpuNdtMap.h>

#include <gtest/gtest.h>
#include "ohmtestcommon/CovarianceTestUtil.h"
#include "ohmtestcommon/OhmTestUtil.h"

#include <chrono>
#include <random>
#include <unordered_map>

#include <glm/gtc/matrix_access.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/norm.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

// Must come after glm includes due to usage on GPU.
#include <ohm/CovarianceVoxel.h>

#include <3esservermacros.h>

#include <Eigen/Dense>

using namespace ohm;
using namespace ohmtestutil;

namespace ndttests
{
void testNdtHits(const std::vector<glm::dvec3> &samples, double resolution)
{
  std::unordered_map<ohm::Key, CovTestVoxel> reference_voxels;

  ohm::OccupancyMap map(resolution, ohm::MapFlag::kVoxelMean);
  ohm::GpuNdtMap ndt(&map, true, ohm::NdtMode::kTraversability);

  // Simulate a sensor at the origin. Not used.
  const glm::dvec3 sensor(0.0);

  // Queue in rays.
  std::vector<glm::dvec3> rays(samples.size() * 2);
  std::vector<float> intensities(samples.size(), 0.0f);
  for (size_t i = 0; i < samples.size(); ++i)
  {
    rays[i * 2 + 0] = sensor;
    rays[i * 2 + 1] = samples[i];

    // Update reference voxel
    ohm::Key key = map.voxelKey(samples[i]);
    auto ref = reference_voxels.find(key);
    if (ref == reference_voxels.end())
    {
      // New voxel. Initialise.
      CovTestVoxel ref_voxel;
      initialiseTestVoxel(&ref_voxel, float(ndt.map().resolution()));
      ref = reference_voxels.insert(std::make_pair(key, ref_voxel)).first;
    }

    // Update the reference algorithm.
    updateHit(&ref->second, samples[i]);
  }

  ndt.integrateRays(rays.data(), rays.size(), intensities.data(), nullptr, ohm::kRfExcludeRay);
  ndt.syncVoxels();

  // Validate
  int covariance_layer_index = -1;
  if (const ohm::MapLayer *layer = map.layout().layer(ohm::default_layer::covarianceLayerName()))
  {
    covariance_layer_index = layer->layerIndex();
  }

  ASSERT_GE(covariance_layer_index, 0);

  ohm::Voxel<const ohm::VoxelMean> mean(&map, map.layout().meanLayer());
  ohm::Voxel<const ohm::CovarianceVoxel> covariance(&map, map.layout().covarianceLayer());
  for (const auto &ref : reference_voxels)
  {
    ohm::setVoxelKey(ref.first, mean, covariance);
    ASSERT_TRUE(mean.isValid());
    ASSERT_TRUE(covariance.isValid());
    // Lookup the target voxel.
    ohm::VoxelMean voxel_mean;
    ohm::CovarianceVoxel cov_voxel;
    mean.read(&voxel_mean);
    covariance.read(&cov_voxel);
    EXPECT_TRUE(validate(ohm::positionSafe(mean), voxel_mean.count, cov_voxel, ref.second));
  }
}

void testNdtMiss(const glm::dvec3 &sensor, const std::vector<glm::dvec3> samples, double voxel_resolution,
                 float sensor_noise, const glm::dvec3 &map_origin, const std::vector<glm::dvec3> test_rays)
{
  (void)sensor;
  ohm::OccupancyMap map_cpu(voxel_resolution, ohm::MapFlag::kVoxelMean);
  ohm::NdtMap ndt_cpu(&map_cpu, true);

  map_cpu.setOrigin(map_origin);
  map_cpu.setMissProbability(0.45f);
  ndt_cpu.setSensorNoise(sensor_noise);

  // First process the samples
  ohm::Key target_key;
  for (size_t i = 0; i < samples.size(); ++i)
  {
    const glm::dvec3 &sample = samples[i];

    ohm::Key key = map_cpu.voxelKey(sample);
    target_key = key;
    ohm::integrateNdtHit(ndt_cpu, key, sensor, sample);
  }

  // Clone the map for use in GPU.
  ohm::GpuNdtMap ndt_gpu(map_cpu.clone(), false);
  // Copy parameterisation.
  ndt_gpu.setSensorNoise(ndt_cpu.sensorNoise());

  // Validate that our map objects differ.
  EXPECT_NE(&map_cpu, &ndt_gpu.map());

  // Fire rays to punch through the target voxel, and compare the delta one at a time.
  // In between we reset the probability value of the target voxel.
  ohm::Voxel<float> target_voxel_cpu(&map_cpu, map_cpu.layout().occupancyLayer(), target_key);
  ASSERT_TRUE(target_voxel_cpu.isValid());
  float initial_value;
  target_voxel_cpu.read(&initial_value);

  for (size_t i = 0; i < test_rays.size(); i += 2)
  {
    // Start the update in GPU (one ray -> inefficient).
    ndt_gpu.integrateRays(test_rays.data() + i, 2);

    ohm::integrateNdtMiss(ndt_cpu, target_key, test_rays[i], test_rays[i + 1]);

    // Sync from GPU.
    ndt_gpu.syncVoxels();
    // We will be updating on CPU. Invalidate the GPU cache.
    ndt_gpu.gpuCache()->clear();

    // Read the voxel value from GPU update.
    ohm::Voxel<float> target_voxel_gpu(&ndt_gpu.map(), ndt_gpu.map().layout().occupancyLayer(), target_key);
    ASSERT_TRUE(target_voxel_gpu.isValid());
    float ndt_cpu_value;
    target_voxel_cpu.read(&ndt_cpu_value);
    float ndt_gpu_value;
    target_voxel_gpu.read(&ndt_gpu_value);

    EXPECT_NEAR(ndt_gpu_value, ndt_cpu_value, 1e-4f);

    // Restore both voxel values.
    target_voxel_cpu.write(initial_value);
    target_voxel_gpu.write(initial_value);
  }
}


TEST(Ndt, Hit)
{
  // Use a fixed seed for test repeatability.
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::normal_distribution<double> gaussian(0.0, 1.0);
  // Generate points within a 2m cube.
  std::uniform_real_distribution<double> uniform(0.01, 1.99);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // First generate a sample surface to target. We have to make sure this falls within a single voxel.
  glm::dvec3 mean(0);
  glm::dmat3 cov(0);

  double num_pt = 0;
  while (num_pt < 4)
  {
    // glm::dvec3 pt = 0.5 * randomVector3();
    glm::dvec3 pt;

    pt[0] = uniform(rng);
    pt[1] = uniform(rng);
    pt[2] = uniform(rng);

    const glm::dvec3 diff = pt - mean;
    const double one_on_num_pt_plus_one = 1.0 / (num_pt + 1.0);
    mean = (num_pt * mean + pt) * one_on_num_pt_plus_one;
    cov = (num_pt * cov + glm::dot((num_pt * one_on_num_pt_plus_one) * diff, diff)) * one_on_num_pt_plus_one;
    num_pt += 1.0;
  }

  {
    // TODO: this is the only part we need Eigen for. Find an alternative Eigen decomposition solution.
    Eigen::Matrix3d cov_eigen;
    cov_eigen << glm::row(cov, 0)[0], glm::row(cov, 0)[1], glm::row(cov, 0)[2],  //
      glm::row(cov, 1)[0], glm::row(cov, 1)[1], glm::row(cov, 1)[1],             //
      glm::row(cov, 2)[0], glm::row(cov, 2)[1], glm::row(cov, 2)[1];
    Eigen::LDLT<Eigen::Matrix3d, Eigen::Lower> cov_d(cov_eigen);
    const Eigen::Vector3d &vD = cov_d.vectorD();

    const Eigen::Vector3d mean_eigen(mean.x, mean.y, mean.z);
    for (size_t i = 0; i < sample_count; ++i)
    {
      Eigen::Vector3d pt;
      for (size_t i = 0; i < 3; ++i)
      {
        pt[i] = sqrt(vD[i]) * gaussian(rng);
      }

      pt = mean_eigen + cov_d.matrixL() * pt;
      samples.emplace_back(glm::dvec3(pt.x(), pt.y(), pt.z()));
    }
  }

  testNdtHits(samples, 2.0);
}


// For the miss tests, we setup a GPU and a CPU based NDT map, populate one voxel and compare the miss results on one
// focus voxel. On CPU we only interate misses for that voxel, for GPU we do the full test rays and compare the
// results on the focus voxel.

TEST(Ndt, MissPlanar)
{
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  std::uniform_real_distribution<double> uniform(0.01, 1.99);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(1, 1, 5);
  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    sample.x = uniform(rng);
    sample.y = uniform(rng);
    sample.z = 1.0;
    // sample.z += 0.025 * uniform(rng);
    samples.emplace_back(sample);
  }

  // Build a set of rays to punch through the target voxel.
  std::vector<glm::dvec3> rays;
  // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(1, 1, -5));
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  // Ray parallel to the voxel ellipsoid - expected to miss.
  rays.emplace_back(glm::dvec3(-5, 1, 0.25));
  rays.emplace_back(glm::dvec3(5, 1, 0.25));
  // Ray parallel to the voxel ellipsoid, but near the centre.
  // Note: we build a perfect plane above in the centre of the voxel. However, the quantisation of the mean
  // can offset the plane. Running exactly on the plane on which we generated the points will not result in the same
  // effect in testing. We must offset the ray a little to ensure we are a bit off the plane either when using
  // voxel mean quantisation or not.
  rays.emplace_back(glm::dvec3(1, 5, 1.01));
  rays.emplace_back(glm::dvec3(1, -5, 1.01));
  // Ray running across the voxel, down towards the ellipsoid, but not crossing. This simulates rays running near
  // parallel a ground plane as it approaches the sample position.
  rays.emplace_back(glm::dvec3(-5, 1, 2));
  rays.emplace_back(glm::dvec3(5, 1, 1));
  // Ray running across the voxel, and through the ellipsoid.
  rays.emplace_back(glm::dvec3(-5, 1, 2));
  rays.emplace_back(glm::dvec3(5, 1, 0.5));

  testNdtMiss(sensor, samples, voxel_resolution, 0.05f, glm::dvec3(0), rays);
}


TEST(Ndt, MissCylindrical)
{
  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  const float sensor_noise = 0.05f;
  const glm::dvec3 map_origin(-0.5 * voxel_resolution);

  const double cylinder_radius = 0.3;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-0.99, 0.99);
  std::uniform_real_distribution<double> uniform_radius(cylinder_radius - sensor_noise, cylinder_radius + sensor_noise);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(0, 0, 5);

  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    // Create a random sample in the voxel
    sample.x = uniform(rng);
    sample.y = uniform(rng);
    sample.z = uniform(rng);
    // Convert into a cylinder.
    // Normalise in X/Y, create a random radius (to the limit) and scale by this radius.
    double length_xy = std::sqrt(sample.x * sample.x + sample.y * sample.y);
    if (length_xy > 1e-6)
    {
      const double radius = uniform_radius(rng);
      sample.x = radius * sample.x / length_xy;
      sample.y = radius * sample.y / length_xy;
    }
    samples.emplace_back(sample);
  }

  // Build a set of rays to punch through the target voxel.
  std::vector<glm::dvec3> rays;
  // // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(0, 0, -5));
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  // Ray running parallel to the cylinder near the edge. Should be a near hit.
  rays.emplace_back(glm::dvec3(cylinder_radius, cylinder_radius, 5));
  rays.emplace_back(glm::dvec3(cylinder_radius, cylinder_radius, -5));
  // Ray running parallel to the cylinder, but should miss.
  rays.emplace_back(glm::dvec3(1.5 * cylinder_radius, 1.5 * cylinder_radius, -5));
  rays.emplace_back(glm::dvec3(2.0 * cylinder_radius, 2.0 * cylinder_radius, 5));
  // Ray across the middle of the cylinder.
  rays.emplace_back(glm::dvec3(2, -cylinder_radius, 0));
  rays.emplace_back(glm::dvec3(-2, -cylinder_radius, 0));
  // Ray across the end (top) of the cylinder.
  rays.emplace_back(glm::dvec3(2, -cylinder_radius, 0.85));
  rays.emplace_back(glm::dvec3(-2, -cylinder_radius, 0.85));
  // Ray across the voxel, missing the cylinder (top) of the cylinder.
  rays.emplace_back(glm::dvec3(2, 2.0 * cylinder_radius, 0.85));
  rays.emplace_back(glm::dvec3(-2, 2.0 * cylinder_radius, 0.85));

  testNdtMiss(sensor, samples, voxel_resolution, sensor_noise, map_origin, rays);
}


TEST(Ndt, MissSpherical)
{
  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  const float sensor_noise = 0.05f;
  const glm::dvec3 map_origin(-0.5 * voxel_resolution);

  const double radius = 0.3;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-0.99, 0.99);
  std::uniform_real_distribution<double> uniform_radius(radius - sensor_noise, radius + sensor_noise);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(0, 0, 5);
  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    double sample_len2 = 0;
    // Keep trying while we have a near zero vector.
    while (sample_len2 < 1e-6)
    {
      // Create a random sample in the voxel
      sample.x = uniform(rng);
      sample.y = uniform(rng);
      sample.z = uniform(rng);
      sample_len2 = glm::length2(sample);
    }
    sample = glm::normalize(sample);
    sample *= uniform_radius(rng);
    samples.emplace_back(sample);
  }

  // Now trace a ray through the voxel, but not ending in the voxel.
  std::vector<glm::dvec3> rays;
  // // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(0, 0, -5));
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  // Edge of the sphere..
  rays.emplace_back(glm::dvec3(radius, radius, 5));
  rays.emplace_back(glm::dvec3(radius, radius, -5));
  // Near the edge of the sphere, but should miss.
  rays.emplace_back(glm::dvec3(1.5 * radius, 1.5 * radius, -5));
  rays.emplace_back(glm::dvec3(2.0 * radius, 2.0 * radius, 5));

  testNdtMiss(sensor, samples, voxel_resolution, sensor_noise, map_origin, rays);
}
}  // namespace ndttests


================================================
FILE: tests/ohmtestgpu/GpuRangesTests.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include <ohm/MapChunk.h>
#include <ohm/MapLayout.h>
#include <ohm/MapProbability.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/QueryFlag.h>
#include <ohm/VoxelData.h>
#include <ohmgpu/ClearanceProcess.h>
#include <ohmgpu/GpuMap.h>

#include <ohmtools/OhmCloud.h>
#include <ohmtools/OhmGen.h>

#include <logutil/LogUtil.h>

#include <ohmutil/PlyMesh.h>
#include <ohmutil/Profile.h>

#include <chrono>
#include <fstream>
#include <iostream>
#include <random>

using namespace ohm;

namespace ranges
{
typedef std::chrono::high_resolution_clock TimingClock;


void saveClearanceCloud(const char *file_name, const OccupancyMap &map, const glm::i16vec3 &region, float search_radius)
{
  PlyMesh ply;
  Key key(region, 0, 0, 0);
  glm::vec3 voxel_pos;
  // auto region_size = map.regionVoxelDimensions();
  // std::cout << "regionSize: " << regionSize << std::endl;

  Voxel<const float> clearance(&map, map.layout().clearanceLayer(), key);
  if (clearance.isValid())
  {
    do
    {
      uint8_t r = 255;
      uint8_t g = 128;
      uint8_t b = 0;
      float range_value;
      clearance.read(&range_value);
      if (range_value < 0)
      {
        range_value = search_radius;
      }
      r = uint8_t(255 * std::max(0.0f, (search_radius - range_value) / search_radius));
      voxel_pos = map.voxelCentreGlobal(clearance.key());
      ply.addVertex(voxel_pos, Colour(r, g, b));
    } while (clearance.nextInRegion());
  }

  ply.save(file_name, true);
}


void showVoxel(ohm::OccupancyMap &map, const ohm::Key &key, float expected_range = FLT_MAX)
{
  Voxel<const float> occupancy(&map, map.layout().occupancyLayer(), key);
  Voxel<const float> clearance(&map, map.layout().clearanceLayer(), key);
  float occupancy_value = 0, clearance_value = 0;
  if (occupancy.isValid())
  {
    occupancy.read(&occupancy_value);
  }
  if (occupancy.isValid())
  {
    clearance.read(&clearance_value);
  }

  if (occupancy.isValid() && clearance.isValid())
  {
    std::cout << occupancy.key() << " " << occupancy_value << ": " << clearance_value << '\n';
  }
  else
  {
    std::cout << key << " invalid\n";
  }

  if (expected_range != FLT_MAX)
  {
    EXPECT_TRUE(clearance.isValid());
    if (clearance.isValid())
    {
      EXPECT_NEAR(clearance_value, expected_range, 1e-3f);
    }
  }
}


TEST(Ranges, Simple)
{
  const double resolution = 1.0;
  const glm::u8vec3 region_size(32);
  // Deliberately require search padding of more than one region to validate GPU memory alignment.
  const float search_range = float(resolution) * float(region_size.x);
  OccupancyMap map(resolution, region_size);

  std::cout << "Populate map: " << std::flush;
  auto start_time = TimingClock::now();

  // Build a map in which we clear all voxels except the one at key (0, 0, 0 : 0, 0, 0).
  // We clear region (0, 0, 0) and make sure we don't query with the flag kQfUnknownAsOccupied.
  ohmgen::fillMapWithEmptySpace(map, 0, 0, 0, region_size.x, region_size.y, region_size.z);
  {
    ohm::Voxel<float> occupied_voxel(&map, map.layout().occupancyLayer(), Key(0, 0, 0, 0, 0, 0));
    ASSERT_TRUE(occupied_voxel.isValid());
    occupied_voxel.write(map.occupancyThresholdValue());
  }

  auto end_time = TimingClock::now();
  std::cout << (end_time - start_time) << std::endl;

  std::cout << "Obstacle query: " << std::flush;
  start_time = TimingClock::now();
  // No need to sync the map yet.
  Key key(0, 0, 0, 0, 0, 0);

  ClearanceProcess clearance_process(search_range, kQfGpuEvaluate);
  clearance_process.calculateForExtents(map, map.voxelCentreGlobal(key), map.voxelCentreGlobal(key));
  end_time = TimingClock::now();
  std::cout << (end_time - start_time) << std::endl;

  // Now check obstacle ranges in the map.
  std::cout << "Post query\n";
  key = Key(0, 0, 0, 0, 0, 0);
  showVoxel(map, key, 0.0f);
  map.stepKey(key, 0, 1);
  showVoxel(map, key, 1.0f);
  map.stepKey(key, 0, 1);
  showVoxel(map, key, 2.0f);

  saveClearanceCloud("ranges-simple-region.ply", map, glm::i16vec3(0, 0, 0), clearance_process.searchRadius());
  ohmtools::saveCloud("ranges-simple-cloud.ply", map);

  key = Key(0, 0, 0, 0, 0, 0);
  Voxel<const float> clearance(&map, map.layout().clearanceLayer(), key);
  ASSERT_TRUE(clearance.isValid());
  const unsigned max_failures = 20;
  unsigned failure_count = 0;
  do
  {
    ASSERT_TRUE(clearance.isValid());
    const float epsilon = 1e-3f;
    const float dist_to_region_origin = glm::length(glm::vec3(clearance.key().localKey()));
    const float expected_range =
      (dist_to_region_origin <= clearance_process.searchRadius()) ? dist_to_region_origin : -1.0f;
    float clearance_value;
    clearance.read(&clearance_value);
    if (std::abs(expected_range - clearance_value) > epsilon)
    {
      std::cout << "Fail: " << clearance.key() << " to RO: " << dist_to_region_origin << " expect: " << expected_range
                << " actual: " << clearance_value << '\n';
      ++failure_count;
    }
    EXPECT_NEAR(expected_range, clearance_value, epsilon);
  } while (clearance.nextInRegion() && failure_count < max_failures);

  if (failure_count == max_failures)
  {
    FAIL() << "Too many failure";
  }
  clearance.reset();
}


TEST(Ranges, Serialised)
{
  // Validate that if we save and reload a map, we can get expected clearance values back.
  // This test came about due to a bug where MapChunk::touched_stamps was not saved. This meant that on load, all
  // MapChunk::touched_stamps would be set back to zero and match when the LineQuery checked the need to recalculate
  // clearance values (on for GPU). No clearance values would be calculated as a result.
  const double resolution = 1.0;
  const glm::u8vec3 region_size(32);
  const char *map_file = "ranges-serialise.ohm";

  // Deliberately require search padding of more than one region to validate GPU memory alignment.
  const float search_range = float(resolution) * float(region_size.x);
  OccupancyMap map(resolution, region_size);

  std::cout << "Populate map: " << std::flush;
  auto start_time = TimingClock::now();

  // Build a map in which we clear all voxels except the one at key (0, 0, 0 : 0, 0, 0).
  // We clear region (0, 0, 0) and make sure we don't query with the flag kQfUnknownAsOccupied.
  ohmgen::fillMapWithEmptySpace(map, 0, 0, 0, region_size.x, region_size.y, region_size.z);
  {
    ohm::Voxel<float> occupied_voxel(&map, map.layout().occupancyLayer(), Key(0, 0, 0, 0, 0, 0));
    ASSERT_TRUE(occupied_voxel.isValid());
    occupied_voxel.write(map.occupancyThresholdValue());
  }

  auto end_time = TimingClock::now();
  std::cout << (end_time - start_time) << std::endl;

  // Save and load the map.
  std::cout << "Save map file " << map_file << std::endl;
  ohm::save(map_file, map);

  // Clear the map.
  map.clear();

  // And load what we saved.
  std::cout << "Load map file " << map_file << std::endl;
  ohm::load(map_file, map);

  // Perform a GPU range query.
  std::cout << "Query cycle 1: " << std::flush;
  start_time = TimingClock::now();
  // No need to sync the map yet.
  Key key(0, 0, 0, 0, 0, 0);

  std::unique_ptr<ClearanceProcess> clearance_process(new ClearanceProcess(search_range, kQfGpuEvaluate));
  clearance_process->calculateForExtents(map, map.voxelCentreGlobal(key), map.voxelCentreGlobal(key), false);
  end_time = TimingClock::now();
  std::cout << (end_time - start_time) << std::endl;

  // Now check obstacle ranges in the map.
  key = Key(0, 0, 0, 0, 0, 0);
  showVoxel(map, key, 0.0f);
  map.stepKey(key, 0, 1);
  showVoxel(map, key, 1.0f);
  map.stepKey(key, 0, 1);
  showVoxel(map, key, 2.0f);

  saveClearanceCloud("ranges-serialise-region.ply", map, glm::i16vec3(0, 0, 0), clearance_process->searchRadius());
  ohmtools::saveCloud("ranges-serialise-cloud.ply", map);

  key = Key(0, 0, 0, 0, 0, 0);
  Voxel<const float> clearance(&map, map.layout().clearanceLayer(), key);
  ASSERT_TRUE(clearance.isValid());
  const unsigned max_failures = 20;
  unsigned failure_count = 0;
  do
  {
    ASSERT_TRUE(clearance.isValid());
    const float epsilon = 1e-3f;
    const float dist_to_region_origin = glm::length(glm::vec3(clearance.key().localKey()));
    const float expected_range =
      (dist_to_region_origin <= clearance_process->searchRadius()) ? dist_to_region_origin : -1.0f;
    float clearance_value;
    clearance.read(&clearance_value);
    if (std::abs(expected_range - clearance_value) > epsilon)
    {
      std::cout << "Fail: " << clearance.key() << " to RO: " << dist_to_region_origin << " expect: " << expected_range
                << " actual: " << clearance_value << '\n';
      ++failure_count;
    }
    EXPECT_NEAR(expected_range, clearance_value, epsilon);
  } while (clearance.nextInRegion() && failure_count < max_failures);

  // About to invalidate the map.
  clearance.reset();

  EXPECT_LE(failure_count, max_failures) << "Too many failure";

  clearance_process.reset(nullptr);

  // Now save the map with clearance data embedded and reload that.
  // Save and load the map.
  std::cout << "Save map file " << map_file << std::endl;
  ohm::save(map_file, map);

  // Clear the map.
  map.clear();

  // And load what we saved.
  std::cout << "Load map file " << map_file << std::endl;
  ohm::load(map_file, map);

  // More queries.
  std::cout << "Query cycle 2: " << std::flush;
  clearance_process.reset(new ClearanceProcess(search_range, kQfGpuEvaluate));
  clearance_process->calculateForExtents(map, map.voxelCentreGlobal(key), map.voxelCentreGlobal(key), false);
  end_time = TimingClock::now();
  std::cout << (end_time - start_time) << std::endl;

  // Now check obstacle ranges in the map.
  std::cout << "Post query\n";
  key = Key(0, 0, 0, 0, 0, 0);
  showVoxel(map, key, 0.0f);
  map.stepKey(key, 0, 1);
  showVoxel(map, key, 1.0f);
  map.stepKey(key, 0, 1);
  showVoxel(map, key, 2.0f);

  saveClearanceCloud("ranges-serialise-region.ply", map, glm::i16vec3(0, 0, 0), clearance_process->searchRadius());
  ohmtools::saveCloud("ranges-serialise-cloud.ply", map);

  key = Key(0, 0, 0, 0, 0, 0);
  clearance = Voxel<const float>(&map, map.layout().clearanceLayer(), key);
  // const float *values = (const float *)map.layout().layer(1).voxels(*map.region(glm::i16vec3(0, 0, 0)));
  ASSERT_TRUE(clearance.isValid());
  failure_count = 0;
  do
  {
    ASSERT_TRUE(clearance.isValid());
    const float epsilon = 1e-3f;
    const float dist_to_region_origin = glm::length(glm::vec3(clearance.key().localKey()));
    const float expected_range =
      (dist_to_region_origin <= clearance_process->searchRadius()) ? dist_to_region_origin : -1.0f;
    float clearance_value;
    clearance.read(&clearance_value);
    if (std::abs(expected_range - clearance_value) > epsilon)
    {
      std::cout << "Fail: " << clearance.key() << " to RO: " << dist_to_region_origin << " expect: " << expected_range
                << " actual: " << clearance_value << '\n';
      ++failure_count;
    }
    EXPECT_NEAR(expected_range, clearance_value, epsilon);
  } while (clearance.nextInRegion() && failure_count < max_failures);
  clearance.reset();

  EXPECT_LE(failure_count, max_failures) << "Too many failure";

  clearance_process.reset(nullptr);

  EXPECT_LE(failure_count, max_failures) << "Too many failure";
}


void testMapOuterEdge(OccupancyMap &map, float search_range, bool unknown_as_occupied)
{
  const glm::u8vec3 region_size = map.regionVoxelDimensions();
  const auto start_time = TimingClock::now();

  std::cout << "Obstacle query: " << std::flush;
  // No need to sync the map yet.
  Key key(0, 0, 0, 0, 0, 0);

  ClearanceProcess clearance_process(search_range, kQfGpuEvaluate | !!unknown_as_occupied * kQfUnknownAsOccupied);
  clearance_process.calculateForExtents(map, map.voxelCentreGlobal(key), map.voxelCentreGlobal(key));
  const auto end_time = TimingClock::now();
  std::cout << (end_time - start_time) << std::endl;

  // Now check obstacle ranges in the map.
  std::cout << "Post query\n";

  // Don't bother saving the cloud. There are no occupied voxels: we treat unknown as occupied.
  if (unknown_as_occupied)
  {
    saveClearanceCloud("ranges-outer-unknown-region.ply", map, glm::i16vec3(0, 0, 0), clearance_process.searchRadius());
  }
  else
  {
    saveClearanceCloud("ranges-outer-region.ply", map, glm::i16vec3(0, 0, 0), clearance_process.searchRadius());
    ohmtools::saveCloud("ranges-outer-cloud.ply", map);
  }

  // Iterate the outer edge of the region. Note that this loop will recheck various faces.
  // Iterate XY, YZ, ZX.
  Voxel<const float> clearance(&map, map.layout().clearanceLayer());
  ASSERT_TRUE(clearance.isLayerValid());
  for (int i = 0; i < 3; ++i)
  {
    const int primary_axis = i;
    const int second_axis = (i + 1) % 3;
    const int third_axis = (i + 2) % 3;
    for (int b = 0; b < region_size[second_axis]; ++b)
    {
      for (int a = 0; a < region_size[primary_axis]; ++a)
      {
        glm::u8vec3 local_key;
        local_key[primary_axis] = a;
        local_key[second_axis] = b;
        local_key[third_axis] = 0;
        clearance.setKey(Key(glm::i16vec3(0), local_key));
        float clearance_value;
        clearance.read(&clearance_value);
        ASSERT_TRUE(clearance.isValid());
        EXPECT_NEAR(clearance_value, map.resolution(), 1e-2f);
        local_key[third_axis] = region_size[third_axis] - 1;
        clearance.setKey(Key(glm::i16vec3(0), local_key));
        ASSERT_TRUE(clearance.isValid());
        EXPECT_NEAR(clearance_value, map.resolution(), 1e-2f);
      }
    }
  }
}


TEST(Ranges, OuterEdgeFromUnknown)
{
  // Test the voxels from the outside propagate into the ROI correctly.
  const double resolution = 1.0;
  const glm::u8vec3 region_size(32);
  const float search_range = float(resolution) * 2;
  OccupancyMap map(resolution, region_size);

  // Build a map in which we clear all voxels except the one at key (0, 0, 0 : 0, 0, 0).
  // We clear region (0, 0, 0) then make sure we enable kQfUnknownAsOccupied.
  // This will treat the boarder voxels as occupied.
  ohmgen::fillMapWithEmptySpace(map, 0, 0, 0, region_size.x, region_size.y, region_size.z);
  testMapOuterEdge(map, search_range, true);
}


TEST(Ranges, OuterEdgeBordered)
{
  // Test the voxels from the outside propagate into the ROI correctly.
  const double resolution = 1.0;
  const glm::u8vec3 region_size(32);
  const float search_range = float(resolution) * 2;

  OccupancyMap map(resolution, region_size);
  // Build an empty cube map where the cube lies just outside the region.
  ohmgen::boxRoom(map, -glm::dvec3(0.5 * resolution) * glm::dvec3(region_size) - glm::dvec3(resolution),
                  glm::dvec3(0.5 * resolution) * glm::dvec3(region_size), 1);
  testMapOuterEdge(map, search_range, false);
}


TEST(Ranges, OuterCorners)
{
  // Add one voxel outside the ROI and ensure it propagates into the ROI.
  const double resolution = 1.0;
  const glm::u8vec3 region_size(32);
  const float search_range = float(resolution) * 8;
  OccupancyMap map(resolution, region_size);

  std::cout << "Populate map: " << std::flush;
  auto start_time = TimingClock::now();

  // Build a map in which we clear all voxels then add an something at key (-1, -1, -1 : 31, 31, 31).
  ohmgen::fillMapWithEmptySpace(map, 0, 0, 0, region_size.x, region_size.y, region_size.z);

  // Add obstacles at all the corners just outside the ROI.
  const glm::i16vec3 origin_offset[] = {
    glm::i16vec3(-1, -1, -1),
    glm::i16vec3(region_size.x, -1, -1),
    glm::i16vec3(-1, region_size.y, -1),
    glm::i16vec3(region_size.x, region_size.y, -1),
    glm::i16vec3(-1, -1, region_size.z),
    glm::i16vec3(region_size.x, -1, region_size.z),
    glm::i16vec3(-1, region_size.y, region_size.z),
    glm::i16vec3(region_size.x, region_size.y, region_size.z),
  };

  {
    Voxel<float> occupancy(&map, map.layout().occupancyLayer());
    ASSERT_TRUE(occupancy.isLayerValid());
    for (auto &&offset : origin_offset)
    {
      Key key(0, 0, 0, 0, 0, 0);
      map.moveKey(key, offset.x, offset.y, offset.z);
      occupancy.setKey(key);
      ASSERT_TRUE(occupancy.isValid());
      occupancy.write(map.occupancyThresholdValue() + map.hitValue());
    }
  }

  auto end_time = TimingClock::now();
  std::cout << (end_time - start_time) << std::endl;

  std::cout << "Obstacle query: " << std::flush;
  start_time = TimingClock::now();
  // No need to sync the map yet.
  Key key(0, 0, 0, 0, 0, 0);

  ClearanceProcess clearance_process(search_range, kQfGpuEvaluate);
  clearance_process.calculateForExtents(map, map.voxelCentreGlobal(key), map.voxelCentreGlobal(key), true);
  end_time = TimingClock::now();
  std::cout << (end_time - start_time) << std::endl;

  // Now check obstacle ranges in the map.
  std::cout << "Post query\n";

  saveClearanceCloud("ranges-outer-corners-region.ply", map, glm::i16vec3(0, 0, 0), clearance_process.searchRadius());
  ohmtools::saveCloud("ranges-outer-corners-cloud.ply", map);

  const Key test_keys[] = { Key(0, 0, 0, 0, 0, 0),
                            Key(0, 0, 0, region_size.x - 1, 0, 0),
                            Key(0, 0, 0, 0, region_size.y - 1, 0),
                            Key(0, 0, 0, region_size.x - 1, region_size.y - 1, 0),
                            Key(0, 0, 0, 0, 0, region_size.z - 1),
                            Key(0, 0, 0, region_size.x - 1, 0, region_size.z - 1),
                            Key(0, 0, 0, 0, region_size.y - 1, region_size.z - 1),
                            Key(0, 0, 0, region_size.x - 1, region_size.y - 1, region_size.z - 1) };

  {
    Voxel<const float> clearance(&map, map.layout().clearanceLayer());
    ASSERT_TRUE(clearance.isLayerValid());
    for (const auto &test_key : test_keys)
    {
      clearance.setKey(test_key);
      float clearance_value;
      clearance.read(&clearance_value);
      ASSERT_TRUE(clearance.isValid());
      EXPECT_NEAR(clearance_value, glm::length(glm::vec3(float(map.resolution()))), 1e-2f);
    }
  }
}


void scalingTest(bool gpu, bool voxel_mean = false, unsigned region_dim = 8)
{
  const double resolution = 0.25;
  const glm::u8vec3 region_size(region_dim);
  const float search_range = 2.0f;

  ohm::Profile profile;
  OccupancyMap map(resolution, region_size, (voxel_mean) ? MapFlag::kVoxelMean : MapFlag::kNone);

  // Offset the map origin so that 0, 0, 0 is the centre of a voxel.
  map.setOrigin(glm::dvec3(-0.5 * resolution));

  // Populate a map with points the following coordinates:
  // 1. (0.5, 0, 0)
  // 2. (0, 0.75, 0)
  // 3. (0, 0, 1)
  // We will focus on the voxel at (0, 0, 0) and use weighting to make it report the closest obstacle as
  // 1, 2 and 3 respectively.

  integrateHit(map, map.voxelKey(glm::dvec3(2 * resolution, 0, 0)));
  integrateHit(map, map.voxelKey(glm::dvec3(0, 3 * resolution, 0)));
  integrateHit(map, map.voxelKey(glm::dvec3(0, 0, 4 * resolution)));

  const char *label = gpu ? "GPU" : "CPU";
  std::cout << "Evaluating " << label << std::endl;
  ohm::ProfileMarker mark(label, &profile);

  ClearanceProcess clearance_process(search_range, kQfGpuEvaluate * !!gpu);

  const auto make_query = [&clearance_process, &map, &profile](const char *context, const glm::vec3 &axis_scaling,
                                                               float expected, bool report_scaling) {
    ohm::ProfileMarker mark("Query", &profile);
    unsigned flags = clearance_process.queryFlags();
    flags &= ~kQfReportUnscaledResults;
    flags |= !report_scaling * kQfReportUnscaledResults;
    clearance_process.setQueryFlags(flags);
    clearance_process.setAxisScaling(axis_scaling);
    clearance_process.reset();
    clearance_process.calculateForExtents(map, glm::dvec3(0), glm::dvec3(0));
    const Key origin_key = map.voxelKey(glm::dvec3(0, 0, 0));
    const Voxel<const float> clearance(&map, map.layout().clearanceLayer(), origin_key);
    ASSERT_TRUE(clearance.isValid());
    float clearance_value;
    clearance.read(&clearance_value);
    EXPECT_NEAR(expected, clearance_value, 1e-2f) << context;
  };

  // Don't report scale results for the first tests.
  // No weighting first. Should get 0.5.
  make_query("no scale", glm::vec3(1.0f), float(2 * resolution), false);

  // De-emphasise X. Should report a range of 0.75.
  make_query("less X", glm::vec3(4, 1.0f, 1.0f), float(3 * resolution), false);

  // Emphasise Z. Should report a range of 1.
  make_query("more Z", glm::vec3(1, 1, 1.0f / 3.0f), float(4 * resolution), false);

  // Now report scaled results.
  make_query("scaled results", glm::vec3(1.1f, 1.1f, 1.0f / 4.0f), float(4 * resolution / 4.0f), true);
}


TEST(Ranges, Scaling)
{
  scalingTest(false);
}


TEST(Ranges, ScalingGpu)
{
  scalingTest(true);
}


TEST(Ranges, VoxelMeanGpu)
{
  // A duplicate of ScalingGpu with voxel mean positioning enabled (but not really used).
  scalingTest(true, true);
}
}  // namespace ranges


================================================
FILE: tests/ohmtestgpu/GpuRayPatternTests.cpp
================================================
// Copyright (c) 2019
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <ohm/OccupancyMap.h>
#include <ohm/VoxelData.h>

#include <ohmgpu/GpuMap.h>

#include <ohmtestcommon/RayPatternTestUtil.h>

#include <3esservermacros.h>

#include <cstdio>
#include <vector>

#include <glm/ext.hpp>

#include <gtest/gtest.h>
#include "ohm/ClearingPattern.h"
#include "ohm/OccupancyMap.h"

using namespace ohm;

namespace raypattern
{
TEST(RayPattern, Clearing)
{
  // Build a map of a solid line of voxels.
  const unsigned voxel_count = 20;
  ohm::OccupancyMap map;
  ohm::GpuMap gpu_map(&map, true);

  // Ensure a single miss erases a single hit.
  map.setHitProbability(0.51f);
  map.setMissProbability(0.0f);

  ohm::Key key(0, 0, 0, 0, 0, 0);
  {
    ohm::Voxel<float> voxel_write(&map, map.layout().occupancyLayer());
    ASSERT_TRUE(voxel_write.isLayerValid());
    for (unsigned i = 0; i < voxel_count; ++i)
    {
      voxel_write.setKey(key);
      ASSERT_TRUE(voxel_write.isValid());
      ohm::integrateHit(voxel_write);
      ASSERT_TRUE(ohm::isOccupied(voxel_write));
      map.moveKey(key, 1, 0, 0);
    }
  }

  // Now create a clearing pattern of a single ray large enough to erase all the voxels.
  // We build the line along Y and rotate it to X with a quaternion.
  RayPattern line_pattern;
  line_pattern.addPoint(glm::dvec3(0, map.resolution() * voxel_count, 0));
  ClearingPattern clearing(&line_pattern, false);

  // Set the key to the voxel we want to check.
  key = ohm::Key(0, 0, 0, 0, 0, 0);
  // Translate the ray pattern to start at the centre of the voxel at key.
  const glm::dvec3 pattern_translate = map.voxelCentreGlobal(key);
  // Setup the quaternion to rotate from Y to X axis.
  const glm::dquat rotation = glm::angleAxis(-0.5 * M_PI, glm::dvec3(0, 0, 1));
  ohm::Voxel<const float> voxel_read(&map, map.layout().occupancyLayer());
  ASSERT_TRUE(voxel_read.isLayerValid());
  for (unsigned i = 0; i < voxel_count; ++i)
  {
    // Validate we have an occupied voxel at key.
    voxel_read.setKey(key);
    ASSERT_TRUE(voxel_read.isValid());
    ASSERT_TRUE(ohm::isOccupied(voxel_read));

    // Apply the pattern.
    clearing.apply(&gpu_map, pattern_translate, rotation);
    gpu_map.syncVoxels();

    // Validate we have removed a voxel.
    ASSERT_FALSE(ohm::isOccupied(voxel_read));

    // Next key.
    map.moveKey(key, 1, 0, 0);
  }
  voxel_read.reset();
}

TEST(RayPattern, Exclude)
{
  // First build a simple map with three voxels of interest along X: { unobserved, free, occupied, occupied }
  ohm::OccupancyMap map(1.0);
  ohm::GpuMap gpu_map(&map, true);
  ohmtestutil::RayPatternExcludeTest(map, gpu_map, [&gpu_map]() { gpu_map.syncVoxels(); });
}
}  // namespace raypattern


================================================
FILE: tests/ohmtestgpu/GpuRaysQueryTests.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <ohmgpu/RaysQueryGpu.h>

#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyType.h>
#include <ohm/QueryFlag.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/VoxelOccupancy.h>

#include <logutil/LogUtil.h>
#include <ohmutil/GlmStream.h>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <random>

#include <gtest/gtest.h>

namespace raysquerytests
{
TEST(RaysQuery, Gpu)
{
  // Note this is a comparative test to ensure GPU results match CPU. The algorithm validation occurs in
  // RaysQueryTests.cpp (the CPU equivalent test).
  const double base_scale = 10.0;
  const std::array<double, 3> query_scale = { 1.2, 1.2, 0.6 };
  const double resolution = 0.1;
  const std::vector<glm::dvec3> rays =  //
    {
      glm::dvec3(0.0), glm::dvec3(base_scale, 0, 0),                      //
      glm::dvec3(0.0), glm::dvec3(0, base_scale, 0),                      //
      glm::dvec3(0.0), glm::dvec3(0, 0, base_scale),                      //
      glm::dvec3(0.0), glm::dvec3(-base_scale, 0, 0),                     //
      glm::dvec3(0.0), glm::dvec3(0, -base_scale, 0),                     //
      glm::dvec3(0.0), glm::dvec3(0, 0, -base_scale),                     //
      glm::dvec3(0.0), glm::dvec3(base_scale, base_scale, 0),             //
      glm::dvec3(0.0), glm::dvec3(0, base_scale, base_scale),             //
      glm::dvec3(0.0), glm::dvec3(base_scale, 0, base_scale),             //
      glm::dvec3(0.0), glm::dvec3(-base_scale, -base_scale, 0),           //
      glm::dvec3(0.0), glm::dvec3(0, -base_scale, -base_scale),           //
      glm::dvec3(0.0), glm::dvec3(-base_scale, 0, -base_scale),           //
      glm::dvec3(0.0), glm::dvec3(base_scale, base_scale, base_scale),    //
      glm::dvec3(0.0), glm::dvec3(-base_scale, -base_scale, -base_scale)  //
    };

  // Build the map.
  ohm::OccupancyMap map(resolution);
  ohm::RayMapperOccupancy mapper(&map);

  // First add just the samples into the map, keeping the rest unobserved.
  for (size_t i = 1; i < rays.size(); i += 2)
  {
    ohm::integrateHit(map, map.voxelKey(rays[i]));
  }

  // Construct the the new RaysQuery object now so we can test the reset as well.
  // Scoped to ensure the query_gpu releases GPU resources before teh occupancy map - specifically the map's GpuCache.
  {
    ohm::RaysQuery query_cpu;
    ohm::RaysQueryGpu query_gpu;
    query_cpu.setMap(&map);
    query_gpu.setMap(&map);

    // Iterations:
    // 0 : samples only in the map, no free space.
    // 1 : samples and free space in the map.
    // 2 : as 2, query_gpu running CPU code.
    const size_t max_iterations = 3;
    for (size_t iteration = 0; iteration < max_iterations; ++iteration)
    {
      if (iteration == 2)
      {
        // For the 3rd iteration, run the GPU query in CPU mode instead.
        query_gpu.setQueryFlags((~ohm::kQfGpu) & query_gpu.queryFlags());
      }
      else
      {
        EXPECT_EQ(query_gpu.queryFlags() & ohm::kQfGpu, ohm::kQfGpu);
      }

      // Scale the rays for lookup.
      for (size_t i = 0; i < rays.size(); i += 2)
      {
        query_cpu.addRay(rays[i], rays[i + 1] * query_scale[iteration]);
        query_gpu.addRay(rays[i], rays[i + 1] * query_scale[iteration]);
      }

      // Make the query.
      query_cpu.execute();
      query_gpu.execute();

      // Compare results.
      ASSERT_EQ(query_cpu.numberOfResults(), rays.size() / 2);
      ASSERT_EQ(query_gpu.numberOfResults(), rays.size() / 2);
      const double *ranges_cpu = query_cpu.ranges();
      const double *unobserved_volumes_cpu = query_cpu.unobservedVolumes();
      const ohm::OccupancyType *terminal_types_cpu = query_cpu.terminalOccupancyTypes();
      const double *ranges_gpu = query_gpu.ranges();
      const double *unobserved_volumes_gpu = query_gpu.unobservedVolumes();
      const ohm::OccupancyType *terminal_types_gpu = query_gpu.terminalOccupancyTypes();
      for (size_t i = 0; i < query_cpu.numberOfResults(); ++i)
      {
        EXPECT_NEAR(ranges_cpu[i], ranges_gpu[i], 1e-4f) << "[" << i << "]";
        // We can get a fair amount of deviation in the volume due to floating point error on the GPU - it's single
        // precision.
        EXPECT_NEAR(unobserved_volumes_cpu[i], unobserved_volumes_gpu[i], 2.5 * resolution) << "[" << i << "]";
        EXPECT_EQ(terminal_types_cpu[i], terminal_types_gpu[i]) << "[" << i << "]";
      }

      // Now do a full ray integration and redo the comparison.
      if (iteration == 0)
      {
        mapper.integrateRays(rays.data(), rays.size());
      }

      // Hard reset to clear the rays
      query_cpu.reset(true);
      query_gpu.reset(true);
    }
  }
}

TEST(RaysQuery, CpuVsGpu)
{
  // Rays query timing test
  // Note this is a comparative test to ensure GPU results match CPU. The algorithm validation occurs in
  // RaysQueryTests.cpp (the CPU equivalent test).
  const double base_scale = 10.0;
  const double query_scale = 1.2;
  const double resolution = 0.1;
  const std ::vector<size_t> ray_counts = { 10, 100, 1000, 2000, 5000, 10000 };
  using Clock = std::chrono::high_resolution_clock;

  // Make some rays.
  std::mt19937 rand_engine;
  std::uniform_real_distribution<double> rand(-base_scale, base_scale);
  std::vector<glm::dvec3> rays;
  // Pure occupancy map.
  ohm::OccupancyMap map(resolution, ohm::MapFlag::kNone);

  {
    // Scoped to ensure the query_gpu releases GPU resources before the occupancy map - specifically the map's
    // GpuCache.
    ohm::RaysQuery query_cpu;
    ohm::RaysQueryGpu query_gpu;
    query_cpu.setMap(&map);
    query_gpu.setMap(&map);
    bool first_iteration = true;

    // ohm::GpuMap gpu_map(&map);

    for (const auto ray_count : ray_counts)
    {
      rays.clear();
      // Add the additional rays (random).
      while (rays.size() < ray_count * 2)
      {
        rays.emplace_back(glm::dvec3(0.0));
        rays.emplace_back(glm::dvec3(rand(rand_engine), rand(rand_engine), rand(rand_engine)));
      }

      // (Re)build the map with the new points.
      map.clear();

// FIXME(KS): There is something going wrong with synching the map back to CPU in the 10000 ray test.
// This is a sporadic failure and the reliably presents as 850 ray trace discrepancies between CPU and GPU. It is
// in fact the CPU has the wrong data for these rays. All other rays are fine. The pass rate is maybe 50/50.
#if 0
      // gpu_map.integrateRays(rays.data(), rays.size());
      // gpu_map.syncVoxels();
#else   // #
      ohm::RayMapperOccupancy mapper(&map);
      mapper.integrateRays(rays.data(), rays.size());
#endif  // #

      EXPECT_EQ(query_gpu.queryFlags() & ohm::kQfGpu, ohm::kQfGpu);

      // Add scaled the rays for lookup.
      for (size_t i = 0; i < rays.size(); i += 2)
      {
        query_cpu.addRay(rays[i], rays[i + 1] * query_scale);
        query_gpu.addRay(rays[i], rays[i + 1] * query_scale);
      }

      if (first_iteration)
      {
        // Prime the GpuRays query to ensure the GPU program is loaded.
        query_gpu.execute();
        first_iteration = false;
      }

      // Make the query.
      const auto cpu_time_start = Clock::now();
      query_cpu.execute();
      const auto cpu_time_end = Clock::now();
      const auto gpu_time_start = Clock::now();
      query_gpu.execute();
      const auto gpu_time_end = Clock::now();

      // Compare results.
      ASSERT_EQ(query_cpu.numberOfResults(), rays.size() / 2);
      ASSERT_EQ(query_gpu.numberOfResults(), rays.size() / 2);
      const double *ranges_cpu = query_cpu.ranges();
      const double *unobserved_volumes_cpu = query_cpu.unobservedVolumes();
      const ohm::OccupancyType *terminal_types_cpu = query_cpu.terminalOccupancyTypes();
      const double *ranges_gpu = query_gpu.ranges();
      const double *unobserved_volumes_gpu = query_gpu.unobservedVolumes();
      const ohm::OccupancyType *terminal_types_gpu = query_gpu.terminalOccupancyTypes();
      // Maximum failures is the less of 5 or a fifth of the ray count.
      // const unsigned allowed_mismatches = 0;
      const unsigned allowed_mismatches = std::min(5u, unsigned(ray_count) / 5u);
      unsigned mismatches = 0;
      for (size_t i = 0; i < query_cpu.numberOfResults(); ++i)
      {
        // Check the range delta.
        float range_epsilon = 1e-4f;
        if (std::abs(ranges_cpu[i] - ranges_gpu[i]) >= range_epsilon)
        {
          if (mismatches < allowed_mismatches)
          {
            range_epsilon = float(std::sqrt(3.0 * map.resolution() * map.resolution()));
          }
          else
          {
            const auto precision = std::cout.precision();
            std::cout << std::setprecision(17);
            std::cout << rays[i * 2 + 0] << " -> " << rays[i * 2 + 1] << " [" << mismatches << "]" << std::endl;
            std::cout << std::setprecision(precision);
          }
          ++mismatches;
        }

        EXPECT_NEAR(ranges_cpu[i], ranges_gpu[i], range_epsilon) << "[" << i << "]";
        // We can get a fair amount of deviation in the volume due to floating point error on the GPU - it's single
        // precision.
        EXPECT_NEAR(unobserved_volumes_cpu[i], unobserved_volumes_gpu[i], 2.5 * resolution) << "[" << i << "]";
        EXPECT_EQ(terminal_types_cpu[i], terminal_types_gpu[i]) << "[" << i << "]";
      }

      EXPECT_LE(mismatches, allowed_mismatches);

      std::cout << "Rays: " << query_cpu.numberOfRays() << " cpu: " << (cpu_time_end - cpu_time_start)
                << " gpu: " << (gpu_time_end - gpu_time_start) << std::endl;

      // Hard reset to clear the rays
      query_cpu.reset(true);
      query_gpu.reset(true);
    }
  }
}
}  // namespace raysquerytests


================================================
FILE: tests/ohmtestgpu/GpuSerialisationTests.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include "ohmtestcommon/OhmTestUtil.h"

#include <ohm/CalculateSegmentKeys.h>
#include <ohm/Key.h>
#include <ohm/KeyList.h>
#include <ohm/LineQuery.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyType.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/VoxelOccupancy.h>
#include <ohmgpu/ClearanceProcess.h>
#include <ohmgpu/OhmGpu.h>

#include <ohmtools/OhmCloud.h>
#include <ohmtools/OhmGen.h>

#include <logutil/LogUtil.h>

#include <ohmutil/Profile.h>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <memory>
#include <random>

#include <gtest/gtest.h>

namespace searialisationtests
{
class ProgressDisplay : public ohm::SerialiseProgress
{
public:
  unsigned target() const { return target_; }

  void setTarget(unsigned target) { target_ = target; }

  unsigned progress() const { return progress_; }

  void setProgress(unsigned progress) { progress_ = progress; }

  bool quit() const override { return false; }

  void setTargetProgress(unsigned target) override { target_ = target; }
  void incrementProgress(unsigned inc = 1u) override
  {
    const unsigned cur_decimation = 100 * progress_ / target_;
    progress_ += inc;
    const unsigned decimation = 100 * progress_ / target_;
    if (decimation > cur_decimation)
    {
      std::cout << "\r" << progress_ << "/" << target_ << std::flush;
    }
  }

  void reset() { target_ = progress_ = 0; }

private:
  unsigned target_ = 0;
  unsigned progress_ = 0;
};


TEST(Serialisation, GpuClearance)
{
  const std::string base_name = "gpu-clearance";
  const std::string map_name = base_name + ".ohm";
  const std::string cloud_name = base_name + "-cloud.ply";
  const std::string clearance_name = base_name + "-clearance.ply";
  // Profile profile;
  int error_code = 0;
  const double boundary_distance = 2.5;
  ohm::OccupancyMap save_map(0.25);
  ohm::OccupancyMap load_map(1);  // Initialise at the wrong resolution. Will be fixed on load.

  // Build a cloud with real samples around a cubic boundary. Does not cover every voxel in the boundary.
  ohmgen::boxRoom(save_map, glm::dvec3(-boundary_distance), glm::dvec3(boundary_distance));

  // Calculate clearance values.
  ohm::ClearanceProcess clearance(1.0f, ohm::kQfGpuEvaluate);
  clearance.update(save_map, 0);

  ProgressDisplay progress;
  std::cout << "Saving" << std::endl;
  error_code = ohm::save(map_name.c_str(), save_map, &progress);
  std::cout << std::endl;
  ASSERT_EQ(error_code, 0);

  ohmtools::saveCloud(cloud_name.c_str(), save_map);
  glm::dvec3 min_ext{};
  glm::dvec3 max_ext{};
  save_map.calculateExtents(&min_ext, &max_ext);
  ohmtools::saveClearanceCloud(clearance_name.c_str(), save_map, min_ext, max_ext, boundary_distance);

  std::cout << "Validate header" << std::endl;
  ohm::MapVersion version;
  error_code = ohm::loadHeader(map_name.c_str(), load_map, &version);
  ASSERT_EQ(error_code, 0);
  ASSERT_EQ(version.major, ohm::kCurrentVersion.major);
  ASSERT_EQ(version.minor, ohm::kCurrentVersion.minor);
  ASSERT_EQ(version.patch, ohm::kCurrentVersion.patch);

  progress.reset();
  std::cout << "Loading" << std::endl;
  error_code = ohm::load(map_name.c_str(), load_map, &progress);
  std::cout << std::endl;
  ASSERT_EQ(error_code, 0);

  ohmtestutil::compareMaps(load_map, save_map, ohmtestutil::kCfCompareExtended | ohmtestutil::kCfExpectClearance);
}


// Legacy code used to generate the test map for Serialisation.Upgrade.
void cubicRoomLegacy(ohm::OccupancyMap &map, float boundary_range, int voxel_step)
{
  int extents = int(boundary_range / map.resolution());

  const auto build_walls = [&map, extents, voxel_step](int a0, int a1, int a2) {
    const double map_res = map.resolution();
    ohm::KeyList ray;
    glm::dvec3 point;
    glm::dvec3 origin = map.origin();
    for (int i = -extents + 1; i <= extents; i += voxel_step)
    {
      for (int j = -extents + 1; j <= extents; j += voxel_step)
      {
        for (int k = 0; k < 2; ++k)
        {
          point = map.origin();
          point[a0] = i * map_res;
          point[a1] = j * map_res;
          point[a2] = (k == 0 ? 1.0 : -1.0) * extents * map_res;
          calculateSegmentKeys(ray, map, origin, point, false);
          for (auto key : ray)
          {
            ohm::integrateMiss(map, key);
          }
          ohm::integrateHit(map, map.voxelKey(point));
        }
      }
    }
  };

  build_walls(0, 1, 2);
  build_walls(1, 2, 0);
  build_walls(0, 2, 1);
}
}  // namespace searialisationtests


================================================
FILE: tests/ohmtestgpu/GpuTestMain.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <ohmgpu/OhmGpu.h>

#include <gtest/gtest.h>

#include "ohmtestcommon/OhmTestUtil.h"

int main(int argc, char **argv)
{
  ohmtestutil::setApplicationPath(argv[0]);

  ::testing::InitGoogleTest(&argc, argv);
  int err = ohm::configureGpuFromArgs(argc, argv, true);
  if (err)
  {
    std::cerr << "failed to initialise GPU" << std::endl;
    return err;
  }
  err = RUN_ALL_TESTS();
  return err;
}


================================================
FILE: tests/ohmtestgpu/GpuTests.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include <ohmgpu/GpuTransformSamples.h>
#include <ohmgpu/OhmGpu.h>

#include <logutil/LogUtil.h>
#include <ohmtools/OhmCloud.h>

#include <gputil/gpuBuffer.h>
#include <gputil/gpuDevice.h>
#include <gputil/gpuEvent.h>
#include <gputil/gpuPinnedBuffer.h>

#include <glm/ext.hpp>
#include <glm/glm.hpp>

#include <chrono>
#include <fstream>
#include <iostream>
#include <random>
#include "gputil/gpuPlatform.h"

using namespace ohm;

namespace
{
using Clock = std::chrono::high_resolution_clock;
}

namespace gpumap
{
void gpuTransformSamples(const std::vector<glm::dvec3> &samples_global, bool compare_performance)
{
  ASSERT_GT(samples_global.size(), 0u);

  // Generate a base reference time using a unix style timestamp for now.
  // This tests likely data input types.
  auto unix_timestamp = std::chrono::milliseconds(std::time(nullptr));
  const double base_time = std::chrono::duration_cast<std::chrono::duration<double>>(unix_timestamp).count();
  const double time_increment = 1e-3;

  // Work out a time range based on the number of samples and time increment.
  // Make the interval larger than required to cover the samples.
  // const double time_max = base_time + time_increment * (samples_global.size() + 10);
  const unsigned transforms_count = 10;

  // Generate a moving local frame representing a moving sensor.
  std::vector<double> timestamps(transforms_count);
  std::vector<glm::dvec3> translations(transforms_count);
  std::vector<glm::dquat> rotations(transforms_count);

  const glm::dvec3 start_point(-0.42);
  const glm::dvec3 end_point(10.0f);

  const glm::dquat start_rotation(1, 0, 0, 0);
  const glm::dquat end_rotation = glm::rotate(glm::dquat(1, 0, 0, 0), glm::pi<double>(), glm::dvec3(0, 0, 1));

  double timestamp = base_time;
  // Ensure time range covers the samples with padding at either end.
  const double end_time = base_time + samples_global.size() * time_increment + 1.5 * time_increment;
  double interpolation_step = 1.0 / (transforms_count - 1);
  for (unsigned i = 0; i < transforms_count; ++i)
  {
    timestamps[i] = base_time + (end_time - base_time) * (i / double(transforms_count - 1));
    translations[i] = start_point + (i * interpolation_step) * (end_point - start_point);
    rotations[i] = start_rotation * glm::slerp(start_rotation, end_rotation, i * interpolation_step);
  }

  std::vector<double> sample_times(samples_global.size());
  timestamp = base_time + 0.67 * time_increment;  // Reset timestamp for samples.
  for (size_t i = 0; i < samples_global.size(); ++i)
  {
    ASSERT_GT(timestamp, timestamps.front());
    ASSERT_LT(timestamp, timestamps.back());
    if (i > 0)
    {
      ASSERT_GT(timestamp, sample_times[i - 1]);
    }
    sample_times[i] = timestamp;
    timestamp += time_increment;
  }

  // Transform samples into a moving, local frame.
  std::vector<glm::dvec3> samples_local(samples_global.size());
  glm::dvec3 p;
  glm::dvec3 translation;
  glm::dquat rotation;
  double lerp;
  unsigned tidx = 0;

  for (size_t i = 0; i < samples_global.size(); ++i)
  {
    // Find the appropriate transform indices.
    while (timestamps[tidx + 1] < sample_times[i])
    {
      ++tidx;
      ASSERT_LT(tidx + 1, timestamps.size()) << "out of bounds";
    }

    lerp = (sample_times[i] - timestamps[tidx]) / (timestamps[tidx + 1] - timestamps[tidx]);

    translation = translations[tidx] + lerp * (translations[tidx + 1] - translations[tidx]);
    rotation = rotations[tidx] * glm::slerp(rotations[tidx], rotations[tidx + 1], lerp);

    // Apply inverse transform for global => local
    p = glm::inverse(rotation) * (samples_global[i] - translation);
    samples_local[i] = p;
    // Validate the transformation back to global.
    // printf("Gen: %f(%f)  T(%f %f %f) R(%f %f %f %f)\n", sample_times[i] - base_time, lerp, translation.x,
    //       translation.y, translation.z, rotation.w, rotation.x, rotation.y, rotation.z);
    p = rotation * samples_local[i] + translation;
    glm::dvec3 diff = samples_global[i] - p;
    ASSERT_NEAR(glm::length(diff), 0.0, 1e-7);
  }

  // Perform reverse transform for performance testing and to make sure the transforms are right.
  // Note we use double precision transforms here, the GPU is single precision.
  Clock::duration cpu_time;
  if (compare_performance)
  {
    std::vector<glm::dvec3> samples_global_cpu(samples_local.size());

    const auto cpu_start = Clock::now();
    unsigned tidx = 0;
    for (size_t i = 0; i < samples_local.size(); ++i)
    {
      timestamp = sample_times[i];
      p = samples_local[i];

      // Find the appropriate transform indices.
      while (timestamps[tidx + 1] < timestamp)
      {
        ++tidx;
        ASSERT_LT(tidx + 1, timestamps.size()) << "out of bounds";
      }

      lerp = (sample_times[i] - timestamps[tidx]) / (timestamps[tidx + 1] - timestamps[tidx]);

      translation = translations[tidx] + lerp * (translations[tidx + 1] - translations[tidx]);
      rotation = rotations[tidx] * glm::slerp(rotations[tidx], rotations[tidx + 1], lerp);

      samples_global_cpu[i] = rotation * p + translation;
    }
    cpu_time = Clock::now() - cpu_start;
    std::cout << "CPU Execution time: " << cpu_time << std::endl;

    // Validate.
    glm::dvec3 sample, expect;
    glm::dvec3 diff;
    unsigned failure_count = 0;
    for (size_t i = 0; i < samples_global.size(); ++i)
    {
      expect = samples_global[i];
      sample = samples_global_cpu[i];
      diff = expect - sample;
      if (std::abs(glm::length(diff)) > 1e-7)
      {
        ++failure_count;
      }
    }

    EXPECT_EQ(failure_count, 0) << "failures encountered during performance comparison.";
  }

  // Do the GPU transformation.
  gputil::Device gpu = ohm::gpuDevice();
  GpuTransformSamples transformation(gpu);

  gputil::Buffer output_buffer(gpu, 0, gputil::kBfReadWriteHost);
  gputil::Event completion_event;
  const auto gpu_start = Clock::now();
  gputil::Queue gpuQueue = gpu.defaultQueue();
  unsigned ray_count = transformation.transform(
    timestamps.data(), translations.data(), rotations.data(), transforms_count, sample_times.data(),
    samples_local.data(), unsigned(samples_local.size()), gpuQueue, output_buffer, completion_event);

  ASSERT_GT(ray_count, 0u);
  ray_count /= 2;
  ASSERT_EQ(ray_count, samples_global.size());

  completion_event.wait();

  const auto gpu_time = Clock::now() - gpu_start;
  std::cout << "GPU Execution time: " << gpu_time << std::endl;

  // Read back the results.
  ASSERT_GE(output_buffer.size(), sizeof(gputil::float3) * 2 * ray_count);
  // Results are rays: sensor origin, global sample
  // Note have to copy out single precision results as that is what the GPU uses.
  std::vector<glm::vec3> rays(2 * ray_count);
  gputil::PinnedBuffer rays_buffer(output_buffer, gputil::kPinRead);
  rays_buffer.readElements<gputil::float3>(rays.data(), 2 * ray_count);
  rays_buffer.unpin();

  // Validate results.
  unsigned failure_count = 0;
  glm::dvec3 sample, expect;
  glm::dvec3 diff;
  for (size_t i = 0; i < samples_global.size(); ++i)
  {
    expect = samples_global[i];
    sample = rays[i * 2 + 1];
    diff = expect - sample;
    const double diff_epsilon = 1e-4;
    if (std::abs(glm::length(diff)) > diff_epsilon)
    {
      if (failure_count < 10)
      {
        EXPECT_NEAR(glm::length(diff), 0.0, diff_epsilon);
      }
      ++failure_count;
    }
  }
  EXPECT_EQ(failure_count, 0);

  // Not currently fast enough. Revisit once I have a clear idea on how many input points to expect.
  //#ifdef NDEBUG
  //    if (compare_performance)
  //    {
  //      EXPECT_LE(gpu_time, cpu_time) << "CPU execution less than GPU.";
  //    }
  //#endif // NDEBUG
}


TEST(Gpu, TransformSample)
{
  // Just transform a single sample for this test.
  gpuTransformSamples({ glm::dvec3(10.0f, 20.0f, 15.0f) }, false);
}


TEST(Gpu, TransformSamples)
{
  // Just transform a single sample for this test.
  std::vector<glm::dvec3> samples_global;
  std::mt19937 rand_engine;
  std::uniform_real_distribution<double> rand(-10.0f, 25.0f);
  const unsigned sample_count = 16 * 1024u;

  samples_global.reserve(sample_count);
  while (samples_global.size() < sample_count)
  {
    samples_global.emplace_back(glm::dvec3(rand(rand_engine), rand(rand_engine), rand(rand_engine)));
  }

  gpuTransformSamples(samples_global, true);
}
}  // namespace gpumap


================================================
FILE: tests/ohmtestgpu/GpuTouchTimeTests.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include <ohm/DefaultLayer.h>
#include <ohm/Key.h>
#include <ohm/NdtMap.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapperNdt.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/Voxel.h>
#include <ohm/VoxelTouchTime.h>

#include <ohmgpu/GpuMap.h>
#include <ohmgpu/GpuNdtMap.h>

#include <glm/gtx/norm.hpp>
#include <glm/vec3.hpp>

#include <random>

namespace touchtime
{
void testTouchTime(ohm::OccupancyMap &map, ohm::GpuMap &mapper)
{
  const unsigned iterations = 10;
  const unsigned ray_count = 1000u;
  const double time_base = 1000.0;
  const double time_step = 0.5;
  std::vector<glm::dvec3> rays;
  std::vector<double> timestamps;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-1.0, 1.0);

  // Set the map origin to avoid (0, 0, 0) being on a voxel boundary.
  map.setOrigin(glm::dvec3(-0.5 * map.resolution()));

  ASSERT_TRUE(map.touchTimeEnabled());

  rays.reserve(2 * ray_count);
  for (unsigned i = 0; i < iterations; ++i)
  {
    rays.clear();
    timestamps.clear();
    for (unsigned r = 0; r < ray_count; ++r)
    {
      // Sample is at the origin. We'll build random rays around that.
      glm::dvec3 origin;
      do
      {
        origin = glm::dvec3(uniform(rng), uniform(rng), uniform(rng));
      } while (glm::length2(origin) < 1e-6);  // Make sure it's not degenerate.
      // Normalise the origin ray, then expand it out to be larger than a single voxel.
      origin = glm::normalize(origin);
      origin *= map.resolution() * 3;
      rays.emplace_back(origin);
      rays.emplace_back(glm::dvec3(0));

      timestamps.emplace_back(time_base + r * time_step);
    }
    // Now use the ray mapper
    mapper.integrateRays(rays.data(), rays.size(), nullptr, timestamps.data(), ohm::kRfDefault);
    mapper.syncVoxels();

    // Check the result.
    ohm::Voxel<uint32_t> time_voxel(&map, map.layout().layerIndex(ohm::default_layer::touchTimeLayerName()));
    ASSERT_TRUE(time_voxel.isLayerValid());
    time_voxel.setKey(map.voxelKey(glm::dvec3(0)));
    ASSERT_TRUE(time_voxel.isValid());

    const double extracted_time = ohm::decodeVoxelTouchTime(map.firstRayTime(), time_voxel.data());
    EXPECT_EQ(extracted_time, timestamps.back());

    time_voxel.write(0u);
    EXPECT_EQ(time_voxel.data(), 0);
  }
}

TEST(TouchTime, WithOccupancy)
{
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kTouchTime);
  ohm::GpuMap mapper(&map, true);
  testTouchTime(map, mapper);
}

TEST(TouchTime, WithNdt)
{
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kTouchTime);
  ohm::GpuNdtMap mapper(&map, true);
  testTouchTime(map, mapper);
}
}  // namespace touchtime


================================================
FILE: tests/ohmtestgpu/GpuTraversalTests.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include <ohm/Key.h>
#include <ohm/OccupancyMap.h>
#include <ohm/RayMapperNdt.h>
#include <ohm/RayMapperOccupancy.h>
#include <ohm/Voxel.h>
#include <ohmgpu/GpuNdtMap.h>

#include <ohmtestcommon/TraversalTest.h>

#include <glm/vec3.hpp>

namespace traversaltests
{
TEST(Traversal, Through)
{
  // For this test, we want to ensure that the traversal accumulates correctly. To do that we select a set of rays
  // which pass through a voxel perpendicular to various faces of that voxel. In this way we know that the traversal
  // should accumulate at approximately the voxel resolution per ray.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal);
  ohm::GpuMap mapper(&map, true);
  ohmtestcommon::traversal::testThrough(map, mapper, [&mapper] { mapper.syncVoxels(); });
}

TEST(Traversal, ThroughNoMean)
{
  // Same as Through, but without sub-voxel positioning. Realistically this isn't useful information as the traversal
  // model needs the number of hits on a voxel which is stored in the subvoxel layer. However, it is a valid code path
  // and we want to test that.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kTraversal);
  ohm::GpuMap mapper(&map, true);
  ohmtestcommon::traversal::testThrough(map, mapper, [&mapper] { mapper.syncVoxels(); });
}

TEST(Traversal, ThroughNdt)
{
  // As Through, but using the NDT mapper.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal);
  ohm::GpuNdtMap mapper(&map, true);
  ohmtestcommon::traversal::testThrough(map, mapper, [&mapper] { mapper.syncVoxels(); });
}

TEST(Traversal, ThroughNdtTm)
{
  // As Through, but using the NDT mapper in TM mode. Explicitly tested because it used a different kernel invocation.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal);
  ohm::GpuNdtMap mapper(&map, true, ohm::NdtMode::kTraversability);
  ohmtestcommon::traversal::testThrough(map, mapper, [&mapper] { mapper.syncVoxels(); });
}

TEST(Traversal, Into)
{
  // For this test, we want to ensure that the traversal accumulates correctly in the final voxel. To do that we select
  // a set of rays which end in a voxel a voxel entering perpendicular to various faces of that voxel. In this way we
  // know that the traversal should accumulate at approximately half the voxel resolution per ray.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal);
  ohm::GpuMap mapper(&map, true);
  ohmtestcommon::traversal::testInto(map, mapper, [&mapper] { mapper.syncVoxels(); });
}

TEST(Traversal, IntoNoMean)
{
  // Same as Into without voxel mean. See ThroughNoMean for justification.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kTraversal);
  ohm::GpuMap mapper(&map, true);
  ohmtestcommon::traversal::testInto(map, mapper, [&mapper] { mapper.syncVoxels(); });
}

TEST(Traversal, IntoNdt)
{
  // As Into, but using the NDT mapper.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal);
  ohm::GpuNdtMap mapper(&map, true);
  ohmtestcommon::traversal::testInto(map, mapper, [&mapper] { mapper.syncVoxels(); });
}

TEST(Traversal, IntoNdtTm)
{
  // As Into, but using the NDT mapper in TM mode. Explicitly tested because it used a different kernel invocation.
  ohm::OccupancyMap map(0.1f, ohm::MapFlag::kVoxelMean | ohm::MapFlag::kTraversal);
  ohm::GpuNdtMap mapper(&map, true, ohm::NdtMode::kTraversability);
  ohmtestcommon::traversal::testInto(map, mapper, [&mapper] { mapper.syncVoxels(); });
}
}  // namespace traversaltests


================================================
FILE: tests/ohmtestgpu/GpuTsdfTests.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/QueryFlag.h>
#include <ohm/RayMapperTsdf.h>
#include <ohm/VoxelData.h>

#include <ohmgpu/GpuTsdfMap.h>
#include <ohmgpu/LineKeysQueryGpu.h>

namespace tsdf
{
TEST(Tsdf, Basic)
{
  // This is a very rudimentary test.
  // See ohmtest Tsdf.Basic
  const double resolution = 0.1;
  const glm::u8vec3 region_size(32);

  // Originally was using 1.0 for each axis. However, this yielded a discrepancy in the key calculation between
  // the different OpenCL programs used, with voxel steps occuring in a different order - floating point error as far
  // as I can tell. By biasing, we ensure the next step select is not choosing between the same magnitudes on different
  // axes.
  const glm::dvec3 bias(1.0, 0.9, 0.8);
  const std::vector<glm::dvec3> rays = {
    glm::dvec3(0.0), glm::dvec3(bias.x, 0.0, 0.0),        glm::dvec3(0.0), glm::dvec3(-bias.x, 0.0, 0.0),
    glm::dvec3(0.0), glm::dvec3(bias.x, bias.y, 0.0),     glm::dvec3(0.0), glm::dvec3(-bias.x, bias.y, 0.0),
    glm::dvec3(0.0), glm::dvec3(0.0, bias.y, 0.0),        glm::dvec3(0.0), glm::dvec3(bias.x, -bias.y, 0.0),
    glm::dvec3(0.0), glm::dvec3(-bias.x, 0.0, bias.z),    glm::dvec3(0.0), glm::dvec3(bias.x, bias.y, bias.z),
    glm::dvec3(0.0), glm::dvec3(-bias.x, bias.y, bias.z), glm::dvec3(0.0), glm::dvec3(bias.x, 0.0, bias.z),
    glm::dvec3(0.0), glm::dvec3(bias.x, -bias.y, bias.z), glm::dvec3(0.0), glm::dvec3(-bias.x, 0.0, bias.z),
    glm::dvec3(0.0), glm::dvec3(bias.x, bias.y, -bias.z), glm::dvec3(0.0), glm::dvec3(-bias.x, bias.y, -bias.z),
    glm::dvec3(0.0), glm::dvec3(bias.x, 0.0, -bias.z),    glm::dvec3(0.0), glm::dvec3(bias.x, -bias.y, -bias.z),
  };

  // Test core voxel mean positioning
  ohm::OccupancyMap map(resolution, region_size, ohm::MapFlag::kTsdf);
  ohm::GpuTsdfMap tsdf_mapper(&map);

  // Offset the map origin to trace between voxel centres.
  map.setOrigin(glm::dvec3(-0.5 * map.resolution()));
  // Set large truncation distance so our values matche computeDistance() results.
  tsdf_mapper.setDefaultTruncationDistance(10.0);

  // Use the GPU ray calculator in order to touch the same voxels. We can get different results using
  // ohm::MapLineWalker due to floating point precision differences.
  ohm::LineKeysQueryGpu query(map, ohm::kQfGpuEvaluate);
  query.setRays(rays.data(), rays.size());
  query.execute();

  ASSERT_EQ(query.numberOfResults(), rays.size() / 2u);

  for (size_t i = 0; i < rays.size(); i += 2)
  {
    // Clear previous results.
    map.clear();
    // Must initialise the voxel reference after the map clear or it may become invalid.
    ohm::Voxel<const ohm::VoxelTsdf> tsdf(&map, map.layout().layerIndex(ohm::default_layer::tsdfLayerName()));
    ASSERT_TRUE(tsdf.isLayerValid()) << "ray index " << i / 2u;
    // Trace ray.
    tsdf_mapper.integrateRays(&rays[i], 2, nullptr, nullptr, 0);
    tsdf_mapper.syncVoxels();

    const size_t index_offset = query.resultIndices()[i / 2u];
    const size_t key_count = query.resultCounts()[i / 2u];

    for (size_t k = 0; k < key_count; ++k)
    {
      const ohm::Key key = query.intersectedVoxels()[index_offset + k];
      ohm::setVoxelKey(key, tsdf);
      ASSERT_TRUE(tsdf.isValid()) << "ray index " << i / 2u;
      const ohm::VoxelTsdf tsdf_data = tsdf.data();
      EXPECT_NEAR(tsdf_data.distance, ohm::computeDistance(rays[i], rays[i + 1], map.voxelCentreGlobal(key)), 1e-6)
        << "ray index " << i / 2u;
    }
  }
}

TEST(Tsdf, Truncation)
{
  // This is a very rudimentary test.
  // See ohmtest Tsdf.Basic
  const double resolution = 0.1;
  const glm::u8vec3 region_size(32);

  // Originally was using 1.0 for each axis. However, this yielded a discrepancy in the key calculation between
  // the different OpenCL programs used, with voxel steps occuring in a different order - floating point error as far
  // as I can tell. By biasing, we ensure the next step select is not choosing between the same magnitudes on different
  // axes.
  const glm::dvec3 bias(1.0, 0.9, 0.8);
  const std::vector<glm::dvec3> rays = {
    glm::dvec3(0.0), glm::dvec3(bias.x, 0.0, 0.0),        glm::dvec3(0.0), glm::dvec3(-bias.x, 0.0, 0.0),
    glm::dvec3(0.0), glm::dvec3(bias.x, bias.y, 0.0),     glm::dvec3(0.0), glm::dvec3(-bias.x, bias.y, 0.0),
    glm::dvec3(0.0), glm::dvec3(0.0, bias.y, 0.0),        glm::dvec3(0.0), glm::dvec3(bias.x, -bias.y, 0.0),
    glm::dvec3(0.0), glm::dvec3(-bias.x, 0.0, bias.z),    glm::dvec3(0.0), glm::dvec3(bias.x, bias.y, bias.z),
    glm::dvec3(0.0), glm::dvec3(-bias.x, bias.y, bias.z), glm::dvec3(0.0), glm::dvec3(bias.x, 0.0, bias.z),
    glm::dvec3(0.0), glm::dvec3(bias.x, -bias.y, bias.z), glm::dvec3(0.0), glm::dvec3(-bias.x, 0.0, bias.z),
    glm::dvec3(0.0), glm::dvec3(bias.x, bias.y, -bias.z), glm::dvec3(0.0), glm::dvec3(-bias.x, bias.y, -bias.z),
    glm::dvec3(0.0), glm::dvec3(bias.x, 0.0, -bias.z),    glm::dvec3(0.0), glm::dvec3(bias.x, -bias.y, -bias.z),
  };

  // Test core voxel mean positioning
  ohm::OccupancyMap map(resolution, region_size, ohm::MapFlag::kTsdf);
  ohm::GpuTsdfMap tsdf_mapper(&map);

  // Offset the map origin to trace between voxel centres.
  map.setOrigin(glm::dvec3(-0.5 * map.resolution()));
  // Set a small truncation distance. We expect voxels to quickly reach the truncation distance.
  tsdf_mapper.setDefaultTruncationDistance(float(map.resolution()));

  // Use the GPU ray calculator in order to touch the same voxels. We can get different results using
  // ohm::MapLineWalker due to floating point precision differences.
  ohm::LineKeysQueryGpu query(map, ohm::kQfGpuEvaluate);
  query.setRays(rays.data(), rays.size());
  query.execute();

  ASSERT_EQ(query.numberOfResults(), rays.size() / 2u);

  for (size_t i = 0; i < rays.size(); i += 2)
  {
    // Clear previous results.
    map.clear();

    // We integrate ray twice, ensuring the distance stays truncated.
    for (int j = 0; j < 2; ++j)
    {
      // Must initialise the voxel reference after the map clear or it may become invalid.
      ohm::Voxel<const ohm::VoxelTsdf> tsdf(&map, map.layout().layerIndex(ohm::default_layer::tsdfLayerName()));
      ASSERT_TRUE(tsdf.isLayerValid()) << "ray index " << i / 2u;
      // Trace ray.
      tsdf_mapper.integrateRays(&rays[i], 2, nullptr, nullptr, 0);
      tsdf_mapper.syncVoxels();

      const size_t index_offset = query.resultIndices()[i / 2u];
      const size_t key_count = query.resultCounts()[i / 2u];

      for (size_t k = 0; k < key_count; ++k)
      {
        const ohm::Key key = query.intersectedVoxels()[index_offset + k];
        ohm::setVoxelKey(key, tsdf);
        ASSERT_TRUE(tsdf.isValid()) << "ray index " << i / 2u;
        const ohm::VoxelTsdf tsdf_data = tsdf.data();
        EXPECT_NEAR(tsdf_data.distance,
                    std::min(tsdf_mapper.defaultTruncationDistance(),
                             ohm::computeDistance(rays[i], rays[i + 1], map.voxelCentreGlobal(key))),
                    1e-6)
          << "ray index " << i / 2u;
      }
    }
  }
}
}  // namespace tsdf


================================================
FILE: tests/ohmtestgpu/GpuVoxelMeanTests.cpp
================================================
// Copyright (c) 2017
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include <ohm/Aabb.h>
#include <ohm/KeyList.h>
#include <ohm/MapProbability.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/VoxelData.h>
#include <ohmgpu/GpuCache.h>
#include <ohmgpu/GpuLayerCache.h>
#include <ohmgpu/GpuMap.h>

#include <logutil/LogUtil.h>

#include <ohmtools/OhmCloud.h>
#include <ohmutil/GlmStream.h>

#include <chrono>
#include <fstream>
#include <iostream>
#include <random>

using namespace ohm;

namespace voxelmean
{
typedef std::chrono::high_resolution_clock TimingClock;

struct VoxelMeanResult
{
  glm::dvec3 expected_position;
  glm::dvec3 reported_position;
  glm::dvec3 voxel_centre;
};

void printVoxelPositionResults(const std::vector<VoxelMeanResult> &voxel_mean_results, bool common_voxel_centre,
                               double map_resolution)
{
  if (voxel_mean_results.empty())
  {
    return;
  }

  std::cout << std::setfill(' ');

  // Error checking first, then pretty reporting.
  for (const VoxelMeanResult &result : voxel_mean_results)
  {
    EXPECT_NEAR(result.expected_position.x, result.reported_position.x, map_resolution / 1e3);
    EXPECT_NEAR(result.expected_position.y, result.reported_position.y, map_resolution / 1e3);
    EXPECT_NEAR(result.expected_position.z, result.reported_position.z, map_resolution / 1e3);
  }

  if (common_voxel_centre)
  {
    std::cout << "Voxel centre: " << voxel_mean_results.front().voxel_centre << std::endl;
  }

  glm::dvec3 pos_error;
  const int col = 30;
  std::ostringstream s;
  std::cout << std::left;
  std::cout << std::setw(col) << "Input position" << std::setw(col) << "Voxel mean";
  if (!common_voxel_centre)
  {
    std::cout << std::setw(col) << "Centre";
  }
  std::cout << std::setw(col) << "error" << '\n';

  for (const VoxelMeanResult &result : voxel_mean_results)
  {
    pos_error = result.expected_position - result.reported_position;

    s << result.expected_position;
    std::cout << std::setw(col) << s.str();
    s.str(std::string());
    s << result.reported_position;
    std::cout << std::setw(col) << s.str();
    s.str(std::string());
    if (!common_voxel_centre)
    {
      s << result.voxel_centre;
      std::cout << std::setw(col) << s.str();
      s.str(std::string());
    }
    s << pos_error;
    std::cout << std::setw(col) << s.str();
    s.str(std::string());
    std::cout << std::endl;
  }
}

TEST(VoxelMean, Basic)
{
  const double resolution = 0.5;
  const glm::u8vec3 region_size(32);

  // Test core voxel mean positioning
  OccupancyMap map(resolution, region_size, MapFlag::kVoxelMean);

  static const glm::dvec3 positions[] =  //
    {
      //
      glm::dvec3(0.0),   //
      glm::dvec3(0.05),  //
      glm::dvec3(0.15),  //
      glm::dvec3(0.20),  //
      glm::dvec3(0.25),  //
      glm::dvec3(0.30),  //
      glm::dvec3(0.35),  //
      glm::dvec3(0.40),  //
      glm::dvec3(0.45),  //
      glm::dvec3(0.50),  //
    };

  std::vector<VoxelMeanResult> results;

  Voxel<ohm::VoxelMean> voxel(&map, map.layout().meanLayer(), map.voxelKey(glm::dvec3(0.5 * resolution)));
  ASSERT_TRUE(voxel.isValid());
  for (unsigned i = 0; i < sizeof(positions) / sizeof(positions[0]); ++i)
  {
    VoxelMeanResult sub_vox;

    setPositionSafe(voxel, positions[i]);

    sub_vox.expected_position = positions[i];
    sub_vox.reported_position = positionSafe(voxel);
    sub_vox.voxel_centre = map.voxelCentreGlobal(voxel.key());

    results.push_back(sub_vox);
  }
  voxel.reset();

  printVoxelPositionResults(results, true, map.resolution());
}

TEST(VoxelMean, LayoutToggle)
{
  // Test enabling and disabling voxel mean layout.
  const double resolution = 0.5;
  const glm::u8vec3 region_size(32);

  // Test core voxel mean positioning
  OccupancyMap map(resolution, region_size);
  // Setup a GPU cache to validate the change in cache size.
  GpuMap gpu_wrap(&map, true, 2048);  // Borrow pointer.

  ASSERT_TRUE(gpu_wrap.gpuOk());

  const GpuLayerCache *gpu_occupancy_cache = gpu_wrap.gpuCache()->layerCache(kGcIdOccupancy);
  const size_t without_voxel_means_chunk_size = gpu_occupancy_cache->chunkSize();

  // First integrate without voxel mean positioning
  glm::dvec3 sample_pos = glm::dvec3(1.1);
  ohm::integrateHit(map, sample_pos);

  glm::dvec3 voxel_centre = map.voxelCentreGlobal(map.voxelKey(sample_pos));

  {
    Voxel<const ohm::VoxelMean> voxel_mean(&map, map.layout().meanLayer(), map.voxelKey(sample_pos));
    // Note: voxel_mean has no valid layer, but the key is valid.
    glm::dvec3 voxel_pos = positionSafe(voxel_mean);
    EXPECT_EQ(voxel_centre.x, voxel_pos.x);
    EXPECT_EQ(voxel_centre.y, voxel_pos.y);
    EXPECT_EQ(voxel_centre.z, voxel_pos.z);
  }

  // Now enable voxel mean positioning.
  // voxel becomes invalid.
  // Cache the voxel layout so we can add and remove the voxel mean layer.
  MapLayout cached_layout = map.layout();

  gpu_occupancy_cache = nullptr;
  map.addVoxelMeanLayer();
  gpu_occupancy_cache = gpu_wrap.gpuCache()->layerCache(kGcIdOccupancy);

  EXPECT_NE(gpu_wrap.gpuCache()->layerCache(kGcIdVoxelMean), nullptr);

  const size_t with_voxel_means_chunk_size = gpu_occupancy_cache->chunkSize();
  Voxel<ohm::VoxelMean> voxel_mean(&map, map.layout().meanLayer(), map.voxelKey(sample_pos));
  Voxel<const float> voxel_occupancy(voxel_mean, map.layout().occupancyLayer());
  ASSERT_TRUE(voxel_mean.isValid());
  ASSERT_TRUE(voxel_occupancy.isValid());

  EXPECT_EQ(with_voxel_means_chunk_size, without_voxel_means_chunk_size);
  unsigned active_gpu_layers = 0;
  for (unsigned i = 0; i < gpu_wrap.gpuCache()->layerCount(); ++i)
  {
    if (gpu_wrap.gpuCache()->layerCache(i) != nullptr)
    {
      ++active_gpu_layers;
    }
  }
  EXPECT_EQ(active_gpu_layers, 2);

  EXPECT_TRUE(ohm::isOccupied(voxel_occupancy));

  // Set the voxel position.
  setPositionSafe(voxel_mean, sample_pos, 1);

  // Position should no longer match the voxel centre.
  glm::dvec3 voxel_pos = positionSafe(voxel_mean);
  EXPECT_NE(voxel_centre.x, voxel_pos.x);
  EXPECT_NE(voxel_centre.y, voxel_pos.y);
  EXPECT_NE(voxel_centre.z, voxel_pos.z);

  EXPECT_NEAR(voxel_pos.x, sample_pos.x, resolution / 1000.0);
  EXPECT_NEAR(voxel_pos.y, sample_pos.y, resolution / 1000.0);
  EXPECT_NEAR(voxel_pos.z, sample_pos.z, resolution / 1000.0);

  voxel_mean.reset();
  voxel_occupancy.reset();

  // Now remove voxel mean positioning.
  gpu_occupancy_cache = nullptr;
  map.updateLayout(cached_layout);
  EXPECT_EQ(gpu_wrap.gpuCache()->layerCount(), kGcIdOccupancy + 1);
  gpu_occupancy_cache = gpu_wrap.gpuCache()->layerCache(kGcIdOccupancy);
  const size_t without_voxel_means_chunk_size2 = gpu_occupancy_cache->chunkSize();

  voxel_mean = Voxel<ohm::VoxelMean>(&map, map.layout().meanLayer(), map.voxelKey(sample_pos));
  voxel_occupancy = Voxel<const float>(voxel_mean, map.layout().occupancyLayer());

  EXPECT_EQ(without_voxel_means_chunk_size2, without_voxel_means_chunk_size);

  // Expect occupancy to be unchanged.
  ASSERT_FALSE(voxel_mean.isValid());
  ASSERT_TRUE(voxel_occupancy.isValid());
  EXPECT_TRUE(ohm::isOccupied(voxel_occupancy));

  // Expect the position to match the voxel centre.
  voxel_pos = positionSafe(voxel_mean);
  EXPECT_EQ(voxel_centre.x, voxel_pos.x);
  EXPECT_EQ(voxel_centre.y, voxel_pos.y);
  EXPECT_EQ(voxel_centre.z, voxel_pos.z);
  voxel_mean.reset();
  voxel_occupancy.reset();
}

TEST(VoxelMean, Gpu)
{
  const double resolution = 0.5;
  const unsigned batch_size = 1;
  const glm::u8vec3 region_size(32);

  // Make a ray.
  std::vector<glm::dvec3> rays;
  rays.emplace_back(glm::dvec3(0));
  rays.emplace_back(glm::dvec3(1.1));
  rays.emplace_back(glm::dvec3(0));
  rays.emplace_back(glm::dvec3(-2.4));
  rays.emplace_back(glm::dvec3(0));
  rays.emplace_back(glm::dvec3(1, -2.2, -3.3));

  // Test basic map populate using GPU and ensure it matches CPU (close enough).
  OccupancyMap map(resolution, region_size, MapFlag::kVoxelMean);
  GpuMap gpu_wrap(&map, true, unsigned(batch_size * 2));  // Borrow pointer.

  ASSERT_TRUE(gpu_wrap.gpuOk());

  gpu_wrap.integrateRays(rays.data(), unsigned(rays.size()));
  gpu_wrap.syncVoxels();

  std::vector<VoxelMeanResult> results;
  Voxel<const ohm::VoxelMean> voxel(&map, map.layout().meanLayer());
  ASSERT_TRUE(voxel.isLayerValid());
  for (size_t i = 1; i < rays.size(); i += 2)
  {
    voxel.setKey(map.voxelKey(rays[i]));
    ASSERT_TRUE(voxel.isValid());
    VoxelMeanResult sub_vox;

    sub_vox.expected_position = rays[i];
    sub_vox.reported_position = positionSafe(voxel);
    sub_vox.voxel_centre = map.voxelCentreGlobal(voxel.key());

    results.push_back(sub_vox);
  }
  voxel.reset();

  printVoxelPositionResults(results, false, map.resolution());
}

TEST(VoxelMean, Compare)
{
  const double resolution = 0.5;
  const unsigned batch_size = 1;
  const glm::u8vec3 region_size(32);

  // Make a ray.
  std::vector<glm::dvec3> rays;
  rays.emplace_back(glm::dvec3(0));
  rays.emplace_back(glm::dvec3(1.1));
  rays.emplace_back(glm::dvec3(0));
  rays.emplace_back(glm::dvec3(-2.4));
  rays.emplace_back(glm::dvec3(0));
  rays.emplace_back(glm::dvec3(1, -2.2, -3.3));

  // Test basic map populate using GPU and ensure it matches CPU (close enough).
  OccupancyMap cpu_map(resolution, region_size, MapFlag::kVoxelMean);
  OccupancyMap gpu_map(resolution, region_size, MapFlag::kVoxelMean);
  GpuMap gpu_wrap(&gpu_map, true, unsigned(batch_size * 2));  // Borrow pointer.

  // In this test we don't adjust the voxel mean weighting. We just validate we get the same results in GPU and CPU.

  ASSERT_TRUE(gpu_wrap.gpuOk());

  cpu_map.integrateRays(rays.data(), unsigned(rays.size()));
  gpu_wrap.integrateRays(rays.data(), unsigned(rays.size()));
  gpu_wrap.syncVoxels();

  std::vector<VoxelMeanResult> results;
  Voxel<const ohm::VoxelMean> cpu_voxel(&cpu_map, cpu_map.layout().meanLayer());
  Voxel<const ohm::VoxelMean> gpu_voxel(&gpu_map, gpu_map.layout().meanLayer());
  ASSERT_TRUE(cpu_voxel.isLayerValid());
  ASSERT_TRUE(gpu_voxel.isLayerValid());
  for (size_t i = 1; i < rays.size(); i += 2)
  {
    cpu_voxel.setKey(cpu_map.voxelKey(rays[i]));
    gpu_voxel.setKey(gpu_map.voxelKey(rays[i]));
    EXPECT_TRUE(cpu_voxel.isValid());
    EXPECT_EQ(cpu_voxel.isValid(), gpu_voxel.isValid());
    if (cpu_voxel.isValid() && gpu_voxel.isValid())
    {
      VoxelMeanResult sub_vox;

      sub_vox.expected_position = positionSafe(cpu_voxel);
      sub_vox.reported_position = positionSafe(gpu_voxel);
      sub_vox.voxel_centre = cpu_map.voxelCentreGlobal(cpu_voxel.key());

      results.push_back(sub_vox);
    }
  }
  cpu_voxel.reset();
  gpu_voxel.reset();

  printVoxelPositionResults(results, false, cpu_map.resolution());
}
}  // namespace voxelmean


================================================
FILE: tests/ohmtestheightmap/CMakeLists.txt
================================================
# Setup of GTEST changed at CMake 3.5.
cmake_minimum_required(VERSION 3.5)



set(SOURCES
  HeightmapTests.cpp
)

add_executable(ohmtestheightmap ${SOURCES})
leak_track_target_enable(ohmtestheightmap CONDITION OHM_LEAK_TRACK)
leak_track_suppress(ohmtestheightmap CONDITION OHM_LEAK_TRACK
  ${OHM_LEAK_SUPPRESS_TBB}
)

set_target_properties(ohmtestheightmap PROPERTIES FOLDER tests)
if(MSVC)
  set_target_properties(ohmtestheightmap PROPERTIES DEBUG_POSTFIX "d")
endif(MSVC)

target_include_directories(ohmtestheightmap
  PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
)

target_link_libraries(ohmtestheightmap PUBLIC ohmtestcommon ohmtools ohmheightmap ohmutil)

target_link_libraries(ohmtestheightmap
  PRIVATE
    ${GTEST_LIBRARIES}
    ${GTEST_MAIN_LIBRARIES}
    glm::glm
    $<BUILD_INTERFACE:$<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>>
    $<BUILD_INTERFACE:$<$<BOOL:${OHM_FEATURE_EIGEN}>:Eigen3::Eigen>>
)

target_include_directories(ohmtestheightmap
  SYSTEM PRIVATE
    "${GTEST_INCLUDE_DIRS}"
    $<$<NOT:$<BOOL:${OHM_TES_DEBUG}>>:${3ES_INCLUDE_DIRS}>
  )

add_test(NAME ohmtestheightmap COMMAND ohmtestheightmap --gtest_output=xml:test-reports/)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

# install(TARGETS ohmtestheightmap DESTINATION bin)


================================================
FILE: tests/ohmtestheightmap/HeightmapTests.cpp
================================================
// Copyright (c) 2018
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include <ohmheightmap/Heightmap.h>
#include <ohmheightmap/HeightmapMesh.h>
#include <ohmheightmap/HeightmapVoxel.h>
#include <ohmheightmap/TriangleNeighbours.h>

#include <ohm/KeyRange.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/Trace.h>
#include <ohm/VoxelData.h>
#include <ohm/VoxelLayout.h>

#include <logutil/LogUtil.h>
#include <ohmtools/OhmCloud.h>
#include <ohmtools/OhmGen.h>
#include <ohmutil/PlyMesh.h>
#include <ohmutil/Profile.h>

#include <sstream>
#include <unordered_set>
#include <utility>

using namespace ohm;

namespace
{
const double kBoxHalfExtents = 2.5;

/// Test heightmap generation parameters.
struct HeightmapParams
{
  /// Half extents of the ground surface.
  double map_half_extents = 6.0;
  /// Half extents of a platform to place above the surface.
  double platform_half_extents = 2.0;
  /// Height of the platform.
  double platform_height = 1.5;
  /// Generate some virtual surfaces in the test map?
  bool generate_virtual_surfaces = false;
  /// Do virtual surfaces occlude visibility of real surfaces below them? This simulates the way in which virtual
  /// surfaces would be generated in real situations where they represent an optimistic best possible slope estimate
  /// observed. Real voxels below our virtual surface voxels are removed as if they have not been observed.
  /// Ignored if @c generate_virtual_surfaces is false.
  bool virtual_surface_occlusion = true;
};

/// Populated with information about the heightmap generated in @c populateMultiLevelMap() to be used for validation.
struct HeightmapGeneratedInfo
{
  /// Set of voxel keys for voxels which represent a potential surface.
  std::unordered_set<ohm::Key, ohm::Key::Hash> surface;
  /// Set of voxel keys for voxels which are potential virtual surfaces.
  std::unordered_set<ohm::Key, ohm::Key::Hash> virtual_surface;
  /// Set of voxel keys for voxels at which the platform joins the floor. This will generally be excluded from a layered
  /// heightmap.
  std::unordered_set<ohm::Key, ohm::Key::Hash> seam;
};

/// Try make the given occupancy voxel virtual. This is a helper function for @c populateMultiLevelMap().
void tryMakeVirtual(ohm::Voxel<float> &occupancy, const HeightmapParams &params, HeightmapGeneratedInfo &info)
{
  // Note we don't forcibly set the miss value so as not to unnecessarily erode occupied voxels.
  ohm::integrateMiss(occupancy);
  if (ohm::isFree(occupancy))
  {
    // Potential virtual.
    ohm::Key key = occupancy.key();
    info.virtual_surface.insert(key);
    // Occlude ptential surfacec below by changing them to unobserved.
    if (params.virtual_surface_occlusion)
    {
      // Walk down to the base level eating voxels.
      const ohm::OccupancyMap &map = *occupancy.map();
      do
      {
        map.moveKeyAlongAxis(key, 2, -1);
        occupancy.setKey(key);
        if (ohm::isOccupied(occupancy))
        {
          occupancy.write(ohm::unobservedOccupancyValue());
          info.surface.erase(key);
        }
      } while (map.voxelCentreGlobal(key)[2] > 0);
    }
  }
}

/// Populate an occupancy map with multiple layers. The generated map is a flat square with smaller raised section
/// connected to the lower surface by a ramp on two sides.
///
/// Virtual surfaces may be optionally included in the map as shallower ramps overshadowing one platform ramp and
/// another descending from the platform as a psuedo third ramp.
///
/// @param map The map to populate (assumed empty)
/// @param generate_virtual_surfaces True to include virtual surfaces in the map.
/// @return The height of the platform above the lower surface.
HeightmapGeneratedInfo populateMultiLevelMap(ohm::OccupancyMap &map, const HeightmapParams &params)
{
  HeightmapGeneratedInfo info;

  ohm::Voxel<float> occupancy(&map, map.layout().occupancyLayer());
  // ASSERT_TRUE(occupancy.isLayerValid());

  // Build extents to traverse in keys. This will ensure we touch every voxel correctly.
  ohm::KeyRange key_range(map.voxelKey(glm::dvec3(-params.map_half_extents, -params.map_half_extents, 0)),
                          map.voxelKey(glm::dvec3(params.map_half_extents, params.map_half_extents, 0)), map);

  // Create the floor.
  for (const Key &key : key_range)
  {
    occupancy.setKey(key);
    occupancy.write(map.hitValue());
    info.surface.insert(key);
  }

  key_range = ohm::KeyRange(
    map.voxelKey(glm::dvec3(-params.platform_half_extents, -params.platform_half_extents, params.platform_height)),
    map.voxelKey(glm::dvec3(params.platform_half_extents, params.platform_half_extents, params.platform_height)), map);

  // Create the raised platform.
  for (const Key &key : key_range)
  {
    occupancy.setKey(key);
    occupancy.write(map.hitValue());
    info.surface.insert(key);
  }

  // Create a vertical range to iterate from platform to floor. We'll walk out the X axis as well as we iterate.
  Key side_range_min_key1 = map.voxelKey(glm::dvec3(-params.platform_half_extents, -params.platform_half_extents, 0));
  Key side_range_min_key2 = map.voxelKey(glm::dvec3(params.platform_half_extents, -params.platform_half_extents, 0));
  map.moveKeyAlongAxis(side_range_min_key1, 2, 1);
  map.moveKeyAlongAxis(side_range_min_key2, 2, 1);
  const ohm::KeyRange side_key_ranges[2] = {
    ohm::KeyRange(
      side_range_min_key1,
      map.voxelKey(glm::dvec3(-params.platform_half_extents, -params.platform_half_extents, params.platform_height)),
      map),
    ohm::KeyRange(
      side_range_min_key2,
      map.voxelKey(glm::dvec3(params.platform_half_extents, -params.platform_half_extents, params.platform_height)),
      map)
  };

  int y_range_voxel_count = int((2 * params.platform_half_extents) / map.resolution());
  // Build a ramp either side. We do 2 iterations, one for each size.
  for (int i = 0; i < 2; ++i)
  {
    // Because we will be walking up, the x offset has to start at the maximum value.
    // We walk X in lockstep with the height for a 45 degree slope, so we read the Z axis range.
    int x_offset = -(side_key_ranges[i].range()[2]);

    // First iteration marks the seam between ground and platform.
    bool first_row = true;
    for (const Key &ref_key : side_key_ranges[i])
    {
      Key key = ref_key;
      // Second side used x_offset
      map.moveKeyAlongAxis(key, 0, x_offset * (i == 0 ? 1 : -1));
      // Create a second range to walk along the Y axis.
      ohm::KeyRange y_key_range(key, key, map);
      map.moveKeyAlongAxis(key, 1, y_range_voxel_count);
      y_key_range.setMaxKey(key);

      // Walk the Y axis line.
      for (const Key &key : y_key_range)
      {
        occupancy.setKey(key);
        occupancy.write(map.hitValue());
        info.surface.insert(key);

        if (first_row)
        {
          // Voxels below the first row are the seam.
          ohm::Key seam_key = key;
          map.moveKeyAlongAxis(seam_key, 2, -1);
          info.seam.insert(seam_key);
        }
      }
      // Setup for next iteration.
      ++x_offset;
      first_row = false;
    }
  }

  if (params.generate_virtual_surfaces)
  {
    // Add two virtual surfaces.
    // 1. Create a third, virtual ramp from the platform where there is no real ramp.
    // 2. Hide a ramp at a shallower slope.

    // 1. Virtual ramp
    const ohm::KeyRange virtual_ramp_range = ohm::KeyRange(
      map.voxelKey(glm::dvec3(-params.platform_half_extents, -params.platform_half_extents, 0)),
      map.voxelKey(glm::dvec3(-params.platform_half_extents, -params.platform_half_extents, params.platform_height)),
      map);

    // This loop is the same as above, but swaps X and Y axes. Cut & paste is simpler for this unit test than trying to
    // generalise the code.
    int x_range_voxel_count = int((2 * params.platform_half_extents) / map.resolution());
    // Build a ramp either side. We do 2 iterations, one for each size.
    // Because we will be walking up, the x offset has to start at the maximum value.
    // We walk X in lockstep with the height for a 45 degree slope, so we read the Z axis range.
    int y_offset = -(virtual_ramp_range.range()[2]);
    for (const Key &ref_key : virtual_ramp_range)
    {
      Key key = ref_key;
      // Second side used y_offset
      map.moveKeyAlongAxis(key, 1, y_offset * 1);
      // Create a second range to walk along the Y axis.
      ohm::KeyRange x_key_range(key, key, map);
      map.moveKeyAlongAxis(key, 0, x_range_voxel_count);
      x_key_range.setMaxKey(key);

      // Walk the Y axis line.
      for (const Key &key2 : x_key_range)
      {
        occupancy.setKey(key2);
        tryMakeVirtual(occupancy, params, info);
      }
      // Setup for next iteration.
      ++y_offset;
    }

    // 2. Hide a ramp at a shallower slope.
    y_range_voxel_count = int((2 * params.platform_half_extents) / map.resolution());
    // Because we will be walking up, the x offset has to start at the maximum value.
    // We walk X in lockstep with the height for a 45 degree slope, so we read the Z axis range.
    int x_offset = -2 * (side_key_ranges[0].range()[2]);
    for (const Key &ref_key : side_key_ranges[0])
    {
      // We walk 2 voxels along X for each 1 in Y to create a shallower slope.
      for (int i = 0; i < 2; ++i)
      {
        Key key = ref_key;
        // Second side used x_offset
        map.moveKeyAlongAxis(key, 0, x_offset * 1);
        // Create a second range to walk along the Y axis.
        ohm::KeyRange y_key_range(key, key, map);
        map.moveKeyAlongAxis(key, 1, y_range_voxel_count);

        y_key_range.setMaxKey(key);

        // Walk the Y axis line.
        for (const Key &key2 : y_key_range)
        {
          occupancy.setKey(key2);
          tryMakeVirtual(occupancy, params, info);
        }
        // Setup for next iteration.
        ++x_offset;
      }
    }
  }

  // Validate the virtual surface set: some of the collected voxels will be supported by an occupied voxel. That does
  // not qualify as a virtual surface candidate.
  for (auto iter = info.virtual_surface.begin(); iter != info.virtual_surface.end();)
  {
    Key key = *iter;
    bool valid_virtual_surface = false;
    occupancy.setKey(key);
    if (ohm::isFree(occupancy))
    {
      // It's a valid free voxel. Now check that the voxel below is null or unobserved.
      map.moveKeyAlongAxis(key, 2, -1);  // Move the key to the voxel below.
      occupancy.setKey(key);             // Update the voxel reference to the new key.
      if (ohm::isUnobservedOrNull(occupancy))
      {
        // It's a valid virtual surface.
        valid_virtual_surface = true;
      }
    }

    if (valid_virtual_surface)
    {
      ++iter;
    }
    else
    {
      // Not a valid virtual surface voxel - remove it.
      iter = info.virtual_surface.erase(iter);
    }
  }

  return info;
}

void heightmapBoxTest(const std::string &prefix, UpAxis axis, std::shared_ptr<Heightmap> *map_out = nullptr)
{
  Profile profile;
  const float boundary_distance = float(kBoxHalfExtents);
  OccupancyMap map(0.2);

  // Build a cloud with real samples around a cubic boundary. Does not cover every voxel in the boundary.
  ohmgen::boxRoom(map, glm::dvec3(-boundary_distance), glm::dvec3(boundary_distance));

  std::shared_ptr<Heightmap> heightmap(new Heightmap(0.2, 1.0, axis));
  if (map_out)
  {
    *map_out = heightmap;
  }
  heightmap->setOccupancyMap(&map);

  ProfileMarker mark_heightmap("heightmap", &profile);
  const double base_height = 0.0;
  heightmap->buildHeightmap(base_height * heightmap->upAxisNormal());
  mark_heightmap.end();

  // Verify output. Boundaries should be at ~ +boundary_distance (top of walls). All other voxels should be at
  // ~ -boundary_distance. Only approximage due to quantisation.

  const Key min_key = heightmap->heightmap().voxelKey(glm::dvec3(-boundary_distance));
  const Key max_key = heightmap->heightmap().voxelKey(glm::dvec3(boundary_distance));

  // Convert axis reference into an index [0, 2].
  const int axis_index = (int(axis) >= 0) ? int(axis) : -int(axis) - 1;

  double ground_height = (int(axis) >= 0) ?
                           map.voxelCentreGlobal(map.voxelKey(glm::dvec3(-boundary_distance)))[axis_index] :
                           -1.0 * map.voxelCentreGlobal(map.voxelKey(glm::dvec3(boundary_distance)))[axis_index];
  double top_of_wall_height = (int(axis) >= 0) ?
                                map.voxelCentreGlobal(map.voxelKey(glm::dvec3(boundary_distance)))[axis_index] :
                                -1.0 * map.voxelCentreGlobal(map.voxelKey(glm::dvec3(-boundary_distance)))[axis_index];

  std::string filename;
  if (!prefix.empty())
  {
    filename = prefix + "-source.ohm";
    ohm::save(filename.c_str(), map);
    filename = prefix + ".ohm";
    ohm::save(filename.c_str(), heightmap->heightmap());

    filename = prefix + "-source.ply";
    ohmtools::saveCloud(filename.c_str(), map);
    // filename = prefix + "-2d.ply";
    // ohmtools::saveCloud(filename.c_str(), heightmap->heightmap());

    // Save the 2.5d heightmap.
    PlyMesh heightmapCloud;
    glm::dvec3 coord;
    Voxel<const float> occupancy(&heightmap->heightmap(), heightmap->heightmap().layout().occupancyLayer());
    Voxel<const HeightmapVoxel> heigthmap_voxel(&heightmap->heightmap(), heightmap->heightmapVoxelLayer());
    for (auto iter = heightmap->heightmap().begin(); iter != heightmap->heightmap().end(); ++iter)
    {
      occupancy.setKey(*iter);
      if (isOccupied(occupancy))
      {
        heigthmap_voxel.setKey(occupancy);
        HeightmapVoxel voxel;
        heigthmap_voxel.read(&voxel);
        // Get the coordinate of the voxel.
        coord = heightmap->heightmap().voxelCentreGlobal(*iter);
        coord += double(voxel.height) * Heightmap::upAxisNormal(axis);
        // Add to the cloud.
        heightmapCloud.addVertex(coord);
      }
    }

    filename = prefix + ".ply";
    heightmapCloud.save(filename.c_str(), true);
  }

  // Helper function to work out if we are at the top of the box or not.
  // Basically, at the limits of the box extents, we should report the top of the wall.
  const auto is_top_of_wall = [](const Key &key, const Key &min_key, const Key &max_key, int *test_axes) {
    if (key.axisMatches(test_axes[0], min_key) || key.axisMatches(test_axes[0], max_key) ||
        key.axisMatches(test_axes[1], min_key) || key.axisMatches(test_axes[1], max_key))
    {
      return true;
    }

    return false;
  };

  Key key = min_key;

  // Tricky stuff to resolve indexing the plane axes.
  int axis_indices[3];
  switch (UpAxis(heightmap->upAxisIndex()))
  {
  case UpAxis::kX:
    axis_indices[0] = 1;
    axis_indices[1] = 2;
    axis_indices[2] = 0;
    break;
  case UpAxis::kY:
    axis_indices[0] = 0;
    axis_indices[1] = 2;
    axis_indices[2] = 1;
    break;
  default:
  case UpAxis::kZ:
    axis_indices[0] = 0;
    axis_indices[1] = 1;
    axis_indices[2] = 2;
    break;
  }

  // Walk the plane.
  key.setRegionAxis(axis_indices[2], 0);
  key.setLocalAxis(axis_indices[2], 0);
  Voxel<const HeightmapVoxel> voxel(&heightmap->heightmap(), heightmap->heightmapVoxelLayer());
  for (; key.isBounded(axis_indices[1], min_key, max_key); heightmap->heightmap().stepKey(key, axis_indices[1], 1))
  {
    for (key.setAxisFrom(axis_indices[0], min_key); key.isBounded(axis_indices[0], min_key, max_key);
         heightmap->heightmap().stepKey(key, axis_indices[0], 1))
    {
      // Get the plane voxel and validate.
      voxel.setKey(key);

      double expected_height = ground_height;
      bool wall = false;
      if (is_top_of_wall(key, min_key, max_key, axis_indices))
      {
        expected_height = top_of_wall_height;
        wall = true;
      }

      ASSERT_TRUE(voxel.isValid()) << (wall ? "top" : "floor");

      HeightmapVoxel voxel_content;
      voxel.read(&voxel_content);
      // Need the equality to handle when both values are inf.
      if (voxel_content.height + base_height != expected_height)
      {
        ASSERT_NEAR(voxel_content.height + base_height, expected_height, 1e-9) << (wall ? "top" : "floor");
      }
    }
  }
}
}  // namespace


TEST(Heightmap, Box)
{
  heightmapBoxTest("heightmap-box", UpAxis::kZ);
}


TEST(Heightmap, AxisX)
{
  heightmapBoxTest("heightmap-x", UpAxis::kX);
}


TEST(Heightmap, AxisY)
{
  heightmapBoxTest("heightmap-y", UpAxis::kY);
}


TEST(Heightmap, AxisZ)
{
  heightmapBoxTest("heightmap-z", UpAxis::kZ);
}


TEST(Heightmap, AxisNegX)
{
  heightmapBoxTest("heightmap-negx", UpAxis::kNegX);
}


TEST(Heightmap, AxisNegY)
{
  heightmapBoxTest("heightmap-negy", UpAxis::kNegY);
}


TEST(Heightmap, AxisNegZ)
{
  heightmapBoxTest("heightmap-negz", UpAxis::kNegZ);
}


TEST(Heightmap, Layout)
{
  std::shared_ptr<Heightmap> heightmap;
  heightmapBoxTest("", UpAxis::kZ, &heightmap);
  const MapLayout &layout = heightmap->heightmap().layout();

  EXPECT_EQ(layout.layerCount(), 2);
  const MapLayer *occupancy_layer = layout.layer(default_layer::occupancyLayerName());
  const MapLayer *heightmap_layer = layout.layer(HeightmapVoxel::kHeightmapLayer);
  ASSERT_NE(occupancy_layer, nullptr);
  ASSERT_NE(heightmap_layer, nullptr);

  EXPECT_EQ(occupancy_layer->layerIndex(), 0);
  EXPECT_EQ(heightmap_layer->layerIndex(), 1);

  EXPECT_EQ(occupancy_layer->voxelByteSize(), sizeof(float));
  EXPECT_EQ(heightmap_layer->voxelByteSize(), sizeof(HeightmapVoxel));

  VoxelLayoutConst occupancy_voxel = occupancy_layer->voxelLayout();
  VoxelLayoutConst heightmap_voxel = heightmap_layer->voxelLayout();

  EXPECT_EQ(occupancy_voxel.memberCount(), 1);
  EXPECT_STREQ(occupancy_voxel.memberName(0), default_layer::occupancyLayerName());
  EXPECT_EQ(occupancy_voxel.memberOffset(0), 0);
  EXPECT_EQ(occupancy_voxel.memberSize(0), sizeof(float));

  const auto validate_heightmap_voxel_layout = [](const VoxelLayoutConst &layout) {
    EXPECT_EQ(layout.voxelByteSize(), sizeof(HeightmapVoxel));
    EXPECT_EQ(layout.memberCount(), 8);
    EXPECT_STREQ(layout.memberName(0), "height");
    EXPECT_STREQ(layout.memberName(1), "clearance");
    EXPECT_STREQ(layout.memberName(2), "normal_x");
    EXPECT_STREQ(layout.memberName(3), "normal_y");
    EXPECT_STREQ(layout.memberName(4), "normal_z");
    EXPECT_STREQ(layout.memberName(5), "layer");
    EXPECT_STREQ(layout.memberName(6), "flags");
    EXPECT_STREQ(layout.memberName(7), "contributing_samples");
    for (int i = 0; i < 5; ++i)
    {
      EXPECT_EQ(layout.memberOffset(i), i * sizeof(float));
      EXPECT_EQ(layout.memberSize(i), sizeof(float));
    }
    EXPECT_EQ(layout.memberOffset(5), 5 * sizeof(float));
    EXPECT_EQ(layout.memberSize(5), sizeof(uint8_t));
    EXPECT_EQ(layout.memberSize(6), sizeof(uint8_t));
    EXPECT_EQ(layout.memberSize(7), sizeof(uint16_t));
  };

  validate_heightmap_voxel_layout(heightmap_voxel);
}


TEST(Heightmap, Mesh)
{
  std::shared_ptr<Heightmap> heightmap;
  heightmapBoxTest("", UpAxis::kZ, &heightmap);
  HeightmapMesh mesh;

  bool ok = mesh.buildMesh(*heightmap);
  ASSERT_TRUE(ok);

  PlyMesh ply;
  mesh.extractPlyMesh(ply);
  ply.save("hmm.ply", true);

  // Since we have generated a heightmap from a box, we can assume and verify the following characteristics:
  // - Verify all neighbour information.
  // - By inspecting triangle neighbours, we can validate triangle normals.

  const size_t vertex_count = mesh.vertexCount();
  const size_t triangle_count = mesh.triangleCount();
  const glm::dvec3 up = heightmap->upAxisNormal();

  const double height_low = -kBoxHalfExtents;
  const double height_high = kBoxHalfExtents;
  for (size_t t = 0; t < triangle_count; ++t)
  {
    const TriangleNeighbours &neighbours = mesh.triangleNeighbours()[t];

    // Check the triangle's neighbours.
    // Get the height of the vertices. All should be either height_low or height_height.
    for (int i = 3; i < 3; ++i)
    {
      glm::dvec3 vertex = mesh.vertices()[mesh.triangles()[t * 3 + i]];
      double vertex_height = glm::dot(up, vertex);
      if (vertex_height < 0)
      {
        EXPECT_NEAR(vertex_height, height_low, 1e-4);
      }
      else
      {
        EXPECT_NEAR(vertex_height, height_high, 1e-4);
      }
    }

    // Confirm the neighbour indices match.
    for (int i = 0; i < 3; ++i)
    {
      const unsigned nt = neighbours.neighbours[i];

      if (nt == ~0u)
      {
        // No neighbour.
        continue;
      }

      int e0, e1;
      int ne0, ne1;
      unsigned v0, v1;
      unsigned nv0, nv1;

      e0 = i;
      e1 = (e0 + 1) % 3;

      // Get the triangle edge vertex indices.
      v0 = mesh.triangles()[t * 3 + e0];
      v1 = mesh.triangles()[t * 3 + e1];

      ASSERT_LT(v0, vertex_count);
      ASSERT_LT(v1, vertex_count);

      // Define edge indices in the neighbour.
      ne0 = neighbours.neighbour_edge_indices[i];
      ASSERT_GE(ne0, 0);
      ASSERT_LT(ne0, 3);

      ne1 = (ne0 + 1) % 3;

      // Get the neighbour edge vertex indices.
      nv0 = mesh.triangles()[nt * 3 + ne0];
      nv1 = mesh.triangles()[nt * 3 + ne1];

      ASSERT_LT(ne0, vertex_count);
      ASSERT_LT(ne1, vertex_count);

      // Validate the edge indices match.
      // Index order may be reversed due to winding, so make both have lower value index come first.
      if (v0 > v1)
      {
        std::swap(v0, v1);
      }
      if (nv0 > nv1)
      {
        std::swap(nv0, nv1);
      }

      EXPECT_EQ(v0, nv0) << "vertex mismatch between triangles: " << t << "," << nt;
      EXPECT_EQ(v1, nv1) << "vertex mismatch between triangles: " << t << "," << nt;
    }
  }
}


namespace
{
enum class SurfaceType
{
  kVoid,
  kVirtual,
  kReal
};

const char *kSurfaceName[] = { "void", "virtual", "real" };

enum class HeightmapSelect
{
  kNull,  // Should only be used to indicate the expected result is not to choise a voxel.
  kBelow,
  kAbove
};

enum class SurfaceMode
{
  kNone,
  kVirtual,
  kPromoteVirtual
};

const char *kModeName[] = { "no-virtual", "virtual", "promoted-virtual" };

struct HeightmapSelectTest
{
  SurfaceType type_below;
  SurfaceType type_above;
  HeightmapSelect closer;

  void fillContext(std::ostream &o) const
  {
    const char *mark_below = (closer == HeightmapSelect::kBelow) ? "*" : " ";
    const char *mark_above = (closer != HeightmapSelect::kBelow) ? "*" : " ";
    o << " below" << mark_below << "<" << kSurfaceName[(int)type_below] << ">";
    o << " above" << mark_above << "<" << kSurfaceName[(int)type_above] << ">";
  }
};

struct HeightmapTestResult
{
  HeightmapVoxelType surface;
  HeightmapSelect select;
};

void addVirtualHeightmapVoxel(ohm::OccupancyMap *map, double range)
{
  const glm::dvec3 pos = glm::dvec3(0, 0, range);
  ohm::integrateMiss(*map, map->voxelKey(pos));
}

void addRealHeightmapVoxel(ohm::OccupancyMap *map, double range)
{
  const glm::dvec3 pos = glm::dvec3(0, 0, range);
  ohm::integrateHit(*map, pos);
}

void testSurface(SurfaceMode surface_mode, const HeightmapSelectTest &test_data, const HeightmapTestResult &expected,
                 const std::string &context)
{
  // Build a constrained region map. Ensures we have to step over void regions.
  ohm::OccupancyMap map(1.0, glm::u8vec3(8, 8, 2));
  // Offset the map so that 0, 0, 0 is at the centre of a voxel.
  map.setOrigin(glm::dvec3(-0.5 * map.resolution()));
  // Set ranges based on map resolution
  const double selected_voxel_range = 5.0 * map.resolution();
  const double other_voxel_range = selected_voxel_range + 2.0 * map.resolution();

  // Set ranges for above/below based on select
  const double range_below = (test_data.closer == HeightmapSelect::kBelow) ? selected_voxel_range : other_voxel_range;
  const double range_above = (test_data.closer != HeightmapSelect::kBelow) ? selected_voxel_range : other_voxel_range;
  const double expected_height = (expected.select == HeightmapSelect::kBelow) ? -range_below : range_above;

  // Add the voxels
  // Add below
  if (test_data.type_below == SurfaceType::kVirtual)
  {
    addVirtualHeightmapVoxel(&map, -range_below);
  }
  else if (test_data.type_below == SurfaceType::kReal)
  {
    addRealHeightmapVoxel(&map, -range_below);
  }
  // Add above.
  if (test_data.type_above == SurfaceType::kVirtual)
  {
    addVirtualHeightmapVoxel(&map, range_above);
  }
  else if (test_data.type_above == SurfaceType::kReal)
  {
    addRealHeightmapVoxel(&map, range_above);
  }

  // Now generate the heightmap.
  ohm::Heightmap heightmap(map.resolution(), 0.0);  // Ignore clearance
  heightmap.setOccupancyMap(&map);
  // Match map origins.
  heightmap.heightmap().setOrigin(map.origin());
  // Setup virtul surface support.
  heightmap.setGenerateVirtualSurface(surface_mode != SurfaceMode::kNone);
  heightmap.setPromoteVirtualBelow(surface_mode == SurfaceMode::kPromoteVirtual);
  // Create a AABB which limits the search space to right near the origin column.
  ohm::Aabb clip;
  clip.setMaxExtents(glm::dvec3(0.5 * map.resolution(), 0.5 * map.resolution(), 2 * other_voxel_range));
  clip.setMinExtents(-clip.maxExtents());
  // Build
  heightmap.buildHeightmap(glm::dvec3(0), clip);

  // Validate the selected voxel.
  ohm::Key hm_key = heightmap.heightmap().voxelKey(glm::dvec3(0));
  // ensure zero layer.
  hm_key.setRegionAxis(2, 0);
  hm_key.setLocalAxis(2, 0);

  // Validate the height of the voxel.
  glm::dvec3 pos{};
  ohm::HeightmapVoxel info{};
  HeightmapVoxelType voxel_type = heightmap.getHeightmapVoxelInfo(hm_key, &pos, &info);
  EXPECT_EQ((int)voxel_type, (int)expected.surface) << context;
  if (expected.surface != HeightmapVoxelType::kVacant && expected.surface != HeightmapVoxelType::kUnknown)
  {
    EXPECT_EQ(pos.z, expected_height) << context;
  }
}
}  // namespace

TEST(Heightmap, SurfaceSelection)
{
  // Test virtual surface generation.
  // We have the following voxel combinations to test:
  // - Real below, real above, below closer
  // - Real below, real above, above closer
  // - Real below, virtual above, below closer
  // - Real below, virtual above, above closer
  // - Virtual below, real above, below closer
  // - Virtual below, real above, above closer
  // - Virtual below, virtual above, below closer
  // - Virtual below, virtual above, above closer
  // - Real below, void above
  // - Void below, real above
  // - Virtual below, void above
  // - Void below, virtual above
  // - Void below, void above
  //
  // We run all test configurations in three surface modes: i. no virtual surfaces, ii.allow virtual surfaces preferring
  // real, iii. promote virtual surfaces below over real above.

  const HeightmapSelectTest surface_tests[] =  //
    {                                          // No virtual surface
      HeightmapSelectTest{ SurfaceType::kReal, SurfaceType::kReal, HeightmapSelect::kBelow },
      HeightmapSelectTest{ SurfaceType::kReal, SurfaceType::kReal, HeightmapSelect::kAbove },
      HeightmapSelectTest{ SurfaceType::kReal, SurfaceType::kVirtual, HeightmapSelect::kBelow },
      HeightmapSelectTest{ SurfaceType::kReal, SurfaceType::kVirtual, HeightmapSelect::kAbove },
      HeightmapSelectTest{ SurfaceType::kVirtual, SurfaceType::kReal, HeightmapSelect::kBelow },
      HeightmapSelectTest{ SurfaceType::kVirtual, SurfaceType::kReal, HeightmapSelect::kAbove },
      HeightmapSelectTest{ SurfaceType::kVirtual, SurfaceType::kVirtual, HeightmapSelect::kBelow },
      HeightmapSelectTest{ SurfaceType::kVirtual, SurfaceType::kVirtual, HeightmapSelect::kAbove },
      HeightmapSelectTest{ SurfaceType::kReal, SurfaceType::kVoid, HeightmapSelect::kAbove },
      HeightmapSelectTest{ SurfaceType::kVoid, SurfaceType::kReal, HeightmapSelect::kBelow },
      HeightmapSelectTest{ SurfaceType::kVirtual, SurfaceType::kVoid, HeightmapSelect::kAbove },
      HeightmapSelectTest{ SurfaceType::kVoid, SurfaceType::kVirtual, HeightmapSelect::kBelow },
      HeightmapSelectTest{ SurfaceType::kVoid, SurfaceType::kVoid, HeightmapSelect::kAbove }
    };
  const size_t test_count = sizeof(surface_tests) / sizeof(surface_tests[0]);

  // Build expected results for the 3 different modes.
  const HeightmapTestResult test_results[3][test_count] =  //
    {                                                      // No virtual surfaces.
      {
        //
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kAbove },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kAbove },
        HeightmapTestResult{ HeightmapVoxelType::kUnknown, HeightmapSelect::kNull },
        HeightmapTestResult{ HeightmapVoxelType::kUnknown, HeightmapSelect::kNull },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kAbove },
        HeightmapTestResult{ HeightmapVoxelType::kUnknown, HeightmapSelect::kNull },
        HeightmapTestResult{ HeightmapVoxelType::kUnknown, HeightmapSelect::kNull },
        HeightmapTestResult{ HeightmapVoxelType::kUnknown, HeightmapSelect::kBelow },
      },
      // Allow virtual surfaces.
      {
        //
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kAbove },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kAbove },
        HeightmapTestResult{ HeightmapVoxelType::kVirtualSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kVirtualSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kAbove },
        HeightmapTestResult{ HeightmapVoxelType::kVirtualSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kVirtualSurface, HeightmapSelect::kAbove },
        HeightmapTestResult{ HeightmapVoxelType::kUnknown, HeightmapSelect::kBelow },
      },
      // Promote virtual surfaces.
      {
        //
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kVirtualSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kVirtualSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kVirtualSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kVirtualSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kSurface, HeightmapSelect::kAbove },
        HeightmapTestResult{ HeightmapVoxelType::kVirtualSurface, HeightmapSelect::kBelow },
        HeightmapTestResult{ HeightmapVoxelType::kVirtualSurface, HeightmapSelect::kAbove },
        HeightmapTestResult{ HeightmapVoxelType::kUnknown, HeightmapSelect::kBelow },
      }
    };

  const int debug_mode_start = 0;
  const int debug_mode_last = 2;
  const size_t debug_test_start = 0;
  const size_t debug_test_last = test_count - 1;
  for (int m = debug_mode_start; m < std::min(3, debug_mode_last + 1); ++m)
  {
    SurfaceMode mode = (SurfaceMode)m;
    for (size_t t = debug_test_start; t < std::min(test_count, debug_test_last + 1); ++t)
    {
      // Build a context string for error reporting.
      std::ostringstream str;
      str << t << ": " << kModeName[m];
      surface_tests[t].fillContext(str);
      // Run test
      testSurface(mode, surface_tests[t], test_results[m][t], str.str());
    }
  }
}

TEST(Heightmap, Clearance)
{
  ohm::OccupancyMap map(0.1);
  const HeightmapParams params;
  populateMultiLevelMap(map, params);
  ohm::save("clearance-map.ohm", map);
  ohmtools::saveCloud("clearance-map.ply", map);

  // Define a heightmap for which there is no clearance requirement. The map we just generated has a smooth, flat
  // surface with a raised platform above like this (2D section):
  ///       _____
  //       /     \          backslashes in ascii
  //      /       \         are fun
  // ____/_________\____
  //
  // So the reference_map will consist of the lower surface voxels, but also indicate the clearance height for each
  // of these to the surface above. We'll later use this to validate the clearance constrained heigthmap.
  ohm::Heightmap reference_heightmap(map.resolution(), 0);
  reference_heightmap.setOccupancyMap(&map);
  reference_heightmap.buildHeightmap(glm::dvec3(0));

  // Now add a clearance constraint.
  const double clearance_constraint = 0.5 * params.platform_height;
  ohm::Heightmap constrained_heightmap(map.resolution(), clearance_constraint);
  constrained_heightmap.setOccupancyMap(&map);
  constrained_heightmap.setUseFloodFill(true);
  constrained_heightmap.buildHeightmap(glm::dvec3(0));

  ohm::save("clearance-hm.ohm", constrained_heightmap.heightmap());
  ohmtools::saveCloud("clearance-hm.ply", constrained_heightmap.heightmap());

  // Now validate the clearance of each voxel in the constrained heigthmap and validate against the reported clearance
  // from the reference map.
  KeyRange validation_range;
  constrained_heightmap.heightmap().calculateExtents(nullptr, nullptr, &validation_range);

  ohm::Voxel<ohm::HeightmapVoxel> ref_voxel(&reference_heightmap.heightmap(),
                                            reference_heightmap.heightmapVoxelLayer());
  ohm::Voxel<ohm::HeightmapVoxel> constrained_voxel(&constrained_heightmap.heightmap(),
                                                    constrained_heightmap.heightmapVoxelLayer());

  ASSERT_TRUE(ref_voxel.isLayerValid());
  ASSERT_TRUE(constrained_voxel.isLayerValid());

  // Note: Key references in the two heightmaps should exactly match.
  for (const auto &key : validation_range)
  {
    ref_voxel.setKey(key);
    constrained_voxel.setKey(key);

    ASSERT_TRUE(ref_voxel.isValid());
    ASSERT_TRUE(constrained_voxel.isValid());

    const HeightmapVoxel ref = ref_voxel.data();
    const HeightmapVoxel test_value = constrained_voxel.data();

    if (test_value.clearance > 0)
    {
      EXPECT_GE(test_value.clearance, clearance_constraint);
    }

    // Validate that we've chose the correct voxel.
    if (ref.clearance >= clearance_constraint)
    {
      EXPECT_EQ(test_value.height, ref.height);
    }
  }
}

enum class LayeredTestStart
{
  kUnderPlatform,
  kOnPlatform,
  kOutside
};

void heightmapLayeredTest(const std::string &name, LayeredTestStart start_location,
                          ohm::HeightmapMode mode = ohm::HeightmapMode::kLayeredFill,
                          std::unique_ptr<ohm::OccupancyMap> *heightmap_out = nullptr)
{
  // For this test we use the multi-layered map generation. For validation, we set the target clearance to zero which
  // allows the heightmap to be a precise representation of the original map. We can then convert the heightmap back
  // into a normal occupancy map and compare that against the input map. It should be identical.
  ohm::Trace trace(std::string("heightmap-") + name + ".3es");  // Setup debug trace for 3rd Eye Scene visualisation.
  // Create the input map.
  ohm::OccupancyMap map(0.1);
  const HeightmapParams params;
  const HeightmapGeneratedInfo heightmap_info = populateMultiLevelMap(map, params);
  ohm::save(name + "-map.ohm", map);
  ohmtools::saveCloud(name + "-map.ply", map);

  // Disable any clearance constraint except for planar mode.
  // The use of planar mode is only for base layer testing. We need a ~1+ voxel clearance in order to get a planar
  // map which exactly matches the layered map base layer.
  const double clearance_constraint = (mode != ohm::HeightmapMode::kPlanar) ? -1.0 : 1.1 * map.resolution();
  ohm::Heightmap layered_heightmap(map.resolution(), clearance_constraint);
  layered_heightmap.setOccupancyMap(&map);
  layered_heightmap.heightmap().setOrigin(map.origin());

  // Build the layered heightmap.
  layered_heightmap.setMode(mode);

  glm::dvec3 seed_pos(0);
  switch (start_location)
  {
  case LayeredTestStart::kUnderPlatform:
    seed_pos = glm::dvec3(0, 0, 0);
    break;
  case LayeredTestStart::kOnPlatform:
    seed_pos = glm::dvec3(0, 0, params.platform_height);
    break;
  case LayeredTestStart::kOutside:
    // Set a position outside the observed bounds of the map. We'll choose the minimum extents of the map.
    {
      ohm::Key seed_key;
      map.calculateExtents(nullptr, nullptr, &seed_key, nullptr);
      seed_pos = map.voxelCentreGlobal(seed_key);
    }
    break;
  default:
    break;
  }
  layered_heightmap.buildHeightmap(seed_pos);

  ohmtools::SaveCloudOptions save_opt;
  ohmtools::ColourHeightmapType colour_by_type(layered_heightmap.heightmap());
  save_opt.colour_select = [&colour_by_type](const ohm::Voxel<const float> &occupancy) {
    return colour_by_type.select(occupancy);
  };
  save_opt.export_free = true;
  ohm::save(name + "-hm.ohm", layered_heightmap.heightmap());
  ohmtools::saveHeightmapCloud(name + "-hm.ply", layered_heightmap.heightmap(),
                               ohmtools::SaveHeightmapCloudOptions(save_opt));

  if (heightmap_out)
  {
    heightmap_out->reset(layered_heightmap.heightmap().clone());
  }

  // Now convert the heightmap back into a normal occupancy map.
  ohm::OccupancyMap test_map(map.resolution());
  test_map.setOrigin(map.origin());

  // HACK: want to visualise the seam.
  ohm::PlyMesh ply;
  for (const ohm::Key &key : heightmap_info.seam)
  {
    const glm::dvec3 pos = map.voxelCentreGlobal(key);
    ply.addVertex(pos);
  }
  ply.save("seam.ply", true);

  ohm::Voxel<float> new_occupancy(&test_map, test_map.layout().occupancyLayer());
  ASSERT_TRUE(new_occupancy.isLayerValid());
  unsigned test_voxel_count = 0;
  for (auto iter = layered_heightmap.heightmap().begin(); iter != layered_heightmap.heightmap().end(); ++iter)
  {
    glm::dvec3 voxel_pos{};
    auto hm_voxel_type = layered_heightmap.getHeightmapVoxelInfo(*iter, &voxel_pos);
    if (hm_voxel_type == HeightmapVoxelType::kSurface)
    {
      // Get the voxel position from the heightmap.
      new_occupancy.setKey(test_map.voxelKey(voxel_pos));
      ASSERT_TRUE(new_occupancy.isValid());
      ohm::integrateHit(new_occupancy);
      ++test_voxel_count;
    }
  }

  new_occupancy.reset();

  if (mode == ohm::HeightmapMode::kPlanar)
  {
    // Do not validate the planar map. It's a special case for base layer validation.
    return;
  }

  // Ensure we've extracted something.
  ASSERT_GT(test_voxel_count, 0u);
  ohm::save(name + "-map-out.ohm", test_map);
  ohmtools::saveCloud(name + "-map-out.ply", test_map);

  // Now validate the extracted test_map against the original map. We should exactly reconstruct the original map.
  ohm::Voxel<const float> ref_occupancy(&map, map.layout().occupancyLayer());
  ohm::Voxel<const float> test_occupancy(&test_map, test_map.layout().occupancyLayer());

  unsigned ref_voxel_count = 0;
  for (auto iter = map.begin(); iter != map.end(); ++iter)
  {
    ref_occupancy.setKey(iter);  // Set key from iterator for efficiency (chunk pointer already available)
    ASSERT_TRUE(ref_occupancy.isValid());

    // Skip seam voxels. These won't be present in the test map.
    if (heightmap_info.seam.find(*iter) != heightmap_info.seam.end())
    {
      continue;
    }

    test_occupancy.setKey(*iter);  // Set from Key as we are referencing a different map object.
    ASSERT_TRUE(test_occupancy.isValid());
    // Check that the occupancy types match. The actual occupancies may differ.
    const auto ref_occupancy_type = ohm::occupancyType(ref_occupancy);
    const auto test_occupancy_type = ohm::occupancyType(test_occupancy);
    EXPECT_EQ(test_occupancy_type, ref_occupancy_type);
    if (ref_occupancy_type == ohm::kOccupied)
    {
      ++ref_voxel_count;
    }
  }

  ASSERT_GT(ref_voxel_count, 0u);
  ASSERT_EQ(test_voxel_count, ref_voxel_count);
}

TEST(Heightmap, LayeredAbove)
{
  // For this test we use the multi-layered map generation. For validation, we set the target clearance to zero which
  // allows the heightmap to be a precise representation of the original map. We can then convert the heightmap back
  // into a normal occupancy map and compare that against the input map. It should be identical.
  heightmapLayeredTest("layered-above", LayeredTestStart::kOnPlatform);
}


TEST(Heightmap, LayeredBelow)
{
  heightmapLayeredTest("layered-below", LayeredTestStart::kUnderPlatform);
}


TEST(Heightmap, LayeredBase)
{
  // For this test, we also check the "base layer" functionality of the kLayeredFill mode. See below.
  std::unique_ptr<ohm::OccupancyMap> layered_heightmap;
  heightmapLayeredTest("layered-base", LayeredTestStart::kUnderPlatform, ohm::HeightmapMode::kLayeredFill,
                       &layered_heightmap);
  std::unique_ptr<ohm::OccupancyMap> planar_heightmap;
  heightmapLayeredTest("layere-base-planar", LayeredTestStart::kUnderPlatform, ohm::HeightmapMode::kPlanar,
                       &planar_heightmap);

  // Validate the base layer functionality. We expect the base layer to exact match the kPlanar map. The
  // heightmapLayeredTest() function has some special handling for the planar mode.

  // Walk the layered heightmap. For each base layer voxel, check that the voxel in the planar map has the same
  // coordinate (mostly interested in height).
  ohm::Voxel<const float> layered_occupancy(layered_heightmap.get(), layered_heightmap->layout().occupancyLayer());
  ohm::Voxel<const float> planar_occupancy(planar_heightmap.get(), layered_heightmap->layout().occupancyLayer());
  ohm::Voxel<const ohm::HeightmapVoxel> layered_hm_voxel(
    layered_heightmap.get(), layered_heightmap->layout().layerIndex(ohm::HeightmapVoxel::kHeightmapLayer));
  ohm::Voxel<const ohm::HeightmapVoxel> planar_hm_voxel(
    planar_heightmap.get(), planar_heightmap->layout().layerIndex(ohm::HeightmapVoxel::kHeightmapLayer));

  ASSERT_TRUE(layered_occupancy.isLayerValid() && layered_hm_voxel.isLayerValid());
  ASSERT_TRUE(planar_occupancy.isLayerValid() && planar_hm_voxel.isLayerValid());

  const ohm::Key zero_key(0, 0, 0, 0, 0, 0);
  ohm::PlyMesh failed_points;
  for (auto iter = layered_heightmap->begin(); iter != layered_heightmap->end(); ++iter)
  {
    ohm::setVoxelKey(iter, layered_occupancy, layered_hm_voxel);
    ASSERT_TRUE(layered_occupancy.isValid() && layered_hm_voxel.isValid());
    if (ohm::isOccupied(layered_occupancy) && layered_hm_voxel.data().layer == kHvlBaseLayer)
    {
      // We have a base layer voxel. Validate.
      // Planar map should use the same key values so long as the vertical offsets are zero.
      ohm::Key planar_key = iter.key();
      planar_key.setAxisFrom(2, zero_key);

      ohm::setVoxelKey(planar_key, planar_occupancy, planar_hm_voxel);
      ASSERT_TRUE(planar_occupancy.isValid());
      ASSERT_TRUE(planar_hm_voxel.isValid());

      // Calculate the positions.
      glm::dvec3 layered_pos = layered_heightmap->voxelCentreGlobal(iter.key());
      glm::dvec3 planar_pos = layered_heightmap->voxelCentreGlobal(planar_key);

      // Add in height.
      layered_pos[2] += layered_hm_voxel.data().height;
      planar_pos[2] += planar_hm_voxel.data().height;

      // Validate.
      const glm::dvec3 delta = layered_pos - planar_pos;
      EXPECT_NEAR(glm::dot(delta, delta), 0, 1e-3 * 1e-3);

      if (glm::dot(delta, delta) >= 1e-3 * 1e-3)
      {
        failed_points.addVertex(layered_pos, ohm::Colour::kColours[ohm::Colour::kRed]);
        failed_points.addVertex(planar_pos, ohm::Colour::kColours[ohm::Colour::kGreen]);
      }
    }
  }

  failed_points.save("layered-base-failed.ply", true);
}

TEST(Heightmap, LayeredExternal)
{
  heightmapLayeredTest("layered-external", LayeredTestStart::kOutside);
}


/// Parameters for @c testHeightmapVirtualSurface()
///
/// Note that we will not find all voxels from the original map. Quirks based on mode are listed below.
///
/// - @c ohm::HeightmapMode::kPlanar misses 2215 voxels below the platform and 41 seam voxels.
/// - @c ohm::HeightmapMode::kSimpleFill also misses 2215, but these are the platform voxels and 41 seam voxels.
/// - @c ohm::HeightmapMode::kLayeredFill[Unordered] misses 41 seam voxels. Seam removal deals with this.
struct VirtualSurfaceTestParams
{
  /// Heightmap generation mode.
  ohm::HeightmapMode mode = ohm::HeightmapMode::kPlanar;
  /// Heightmap floor search distance.
  double floor = 0.5;
  /// Heightmap ceiling search distance. Large to prefer finding the lower surface below the platform.
  double ceiling = 2.0;
  /// Permitted number of surface voxels missed in the heightmap.
  unsigned allowed_missed_surface_count = 0;
};


void testHeightmapVirtualSurface(const std::string &name, const VirtualSurfaceTestParams test_params)
{
  // For this test, we build the multilevel map, then add a virtual surface at an angle over one of the ramp slopes.
  // When we generate a map at the lower level, we should see no impact of the virtual surface.
  // When we generate at the higher level, we should see the virtual surface obscure parts of the ramp.
  // allows the heightmap to be a precise representation of the original map. We can then convert the heightmap back
  // into a normal occupancy map and compare that against the input map. It should be identical.
  ohm::Trace trace("heightmap-virtual-surface-" + name + ".3es");  // Setup debug trace for 3rd Eye Scene visualisation.
  // Create the input map.
  ohm::OccupancyMap map(0.1);
  HeightmapParams params;
  ohmtools::SaveCloudOptions save_opt;
  // Ensure we select the virtual surfaces by removing other options below.
  params.generate_virtual_surfaces = true;
  HeightmapGeneratedInfo heightmap_info = populateMultiLevelMap(map, params);

  ohmtools::ColourByType colour_by_type(map);
  save_opt.colour_select = [&colour_by_type](const ohm::Voxel<const float> &occupancy) {
    return colour_by_type.select(occupancy);
  };
  save_opt.export_free = true;
  ohm::save("virtual-surface-" + name + ".ohm", map);
  ohmtools::saveCloud("virtual-surface-" + name + ".ply", map, save_opt);

  HeightmapGeneratedInfo validation_info = heightmap_info;
  // Disable any clearance constraint.
  const double clearance_constraint = 0.0;
  ohm::Heightmap layered_heightmap(map.resolution(), clearance_constraint);
  layered_heightmap.setOccupancyMap(&map);
  layered_heightmap.heightmap().setOrigin(map.origin());

  // Build the layered heightmap.
  layered_heightmap.setMode(test_params.mode);
  layered_heightmap.setGenerateVirtualSurface(true);
  layered_heightmap.setCeiling(test_params.floor);
  layered_heightmap.setCeiling(test_params.ceiling);
  layered_heightmap.buildHeightmap(glm::dvec3(0, 0, 1.1 * params.platform_height));

  ohmtools::ColourHeightmapType colour_by_heightmap_type(layered_heightmap.heightmap());
  save_opt.colour_select = [&colour_by_heightmap_type](const ohm::Voxel<const float> &occupancy) {
    return colour_by_heightmap_type.select(occupancy);
  };
  save_opt.export_free = true;
  ohm::save("virtual-surface-" + name + "-hm.ohm", layered_heightmap.heightmap());
  ohmtools::saveHeightmapCloud("virtual-surface-" + name + "-hm.ply", layered_heightmap.heightmap(), save_opt);

  // Walk the heightmap extracting all the voxels. With a zero clearance constraint, we should have an exact
  // representation of the original map with the addition of virtual surfaces.
  ohm::Voxel<const float> src_occupancy(&map, map.layout().occupancyLayer());
  glm::dvec3 hm_pos{};
  for (auto iter = layered_heightmap.heightmap().begin(); iter != layered_heightmap.heightmap().end(); ++iter)
  {
    const ohm::HeightmapVoxelType voxel_type = layered_heightmap.getHeightmapVoxelInfo(*iter, &hm_pos, nullptr);
    switch (voxel_type)
    {
    case ohm::HeightmapVoxelType::kSurface:
      // Validate that this corresponds to an occupied voxel in the source map.
      src_occupancy.setKey(map.voxelKey(hm_pos));
      ASSERT_EQ(ohm::occupancyType(src_occupancy), ohm::kOccupied);
      // Remove this from the list of surface keys. Well validate we touched everything below.
      // We must first convert the heightmap position into a key within the source map.
      validation_info.surface.erase(map.voxelKey(hm_pos));
      break;
    case ohm::HeightmapVoxelType::kVirtualSurface:
      // Validate that this corresponds to a free voxel in the source map.
      src_occupancy.setKey(map.voxelKey(hm_pos));
      ASSERT_EQ(ohm::occupancyType(src_occupancy), ohm::kFree);
      // Remove this from the list of virtual surface keys. Well validate we touched everything below.
      // We must first convert the heightmap position into a key within the source map.
      validation_info.virtual_surface.erase(map.voxelKey(hm_pos));
      break;
    case ohm::HeightmapVoxelType::kVacant:
      // Validate that this corresponds to an unobserved or null voxel in the source map.
      ASSERT_TRUE(ohm::isUnobservedOrNull(src_occupancy));
      break;
    default:
      // no op
      break;
    }
  }

  // None of the modes find all voxels in a seam. remove those from the detection set.
  for (auto iter = validation_info.surface.begin(); iter != validation_info.surface.end();)
  {
    // Check for seam voxel.
    auto seam_iter = validation_info.seam.find(*iter);
    if (seam_iter != validation_info.seam.end())
    {
      // Remove the seam voxel.
      iter = validation_info.surface.erase(iter);
    }
    else
    {
      // No removal. Continue iteration.
      ++iter;
    }
  }

  // Ensure we have represented every relevant voxel in the original map.
  EXPECT_EQ(validation_info.surface.size(), test_params.allowed_missed_surface_count);
  EXPECT_EQ(validation_info.virtual_surface.size(), 0);
}


TEST(Heightmap, VirtualSurfacePlanar)
{
  VirtualSurfaceTestParams params;
  params.mode = ohm::HeightmapMode::kPlanar;
  // Allow missing the voxels below the platform.
  params.allowed_missed_surface_count = 2215;
  testHeightmapVirtualSurface("planar", params);
}


TEST(Heightmap, VirtualSurfaceFill)
{
  VirtualSurfaceTestParams params;
  params.mode = ohm::HeightmapMode::kSimpleFill;
  // Allow missing the voxels of the platform.
  params.allowed_missed_surface_count = 2215;
  testHeightmapVirtualSurface("fill", params);
}


TEST(Heightmap, VirtualSurfaceSimpleLayered)
{
  VirtualSurfaceTestParams params;
  params.mode = ohm::HeightmapMode::kLayeredFillUnordered;
  params.floor = params.ceiling = 0.5;
  testHeightmapVirtualSurface("simple-layered", params);
}


TEST(Heightmap, VirtualSurfaceLayered)
{
  // For this test, we build the multilevel map, then add a virtual surface at an angle over one of the ramp slopes.
  // When we generate a map at the lower level, we should see no impact of the virtual surface.
  // When we generate at the higher level, we should see the virtual surface obscure parts of the ramp.
  // allows the heightmap to be a precise representation of the original map. We can then convert the heightmap back
  // into a normal occupancy map and compare that against the input map. It should be identical.
  ohm::Trace trace("heightmap-virtual-surface-layered.3es");  // Setup debug trace for 3rd Eye Scene visualisation.
  // Create the input map.
  ohm::OccupancyMap map(0.1);
  HeightmapParams params;
  // Ensure we select the virtual surfaces by removing other options below.
  params.generate_virtual_surfaces = true;
  params.virtual_surface_occlusion = false;
  HeightmapGeneratedInfo validation_info = populateMultiLevelMap(map, params);
  ohm::save("layered-virtual-surface-layered.ohm", map);
  ohmtools::saveCloud("layered-virtual-surface-layered.ply", map);

  // Disable any clearance constraint.
  const double clearance_constraint = 0.0;
  ohm::Heightmap layered_heightmap(map.resolution(), clearance_constraint);
  layered_heightmap.setOccupancyMap(&map);
  layered_heightmap.heightmap().setOrigin(map.origin());

  // Build the layered heightmap.
  layered_heightmap.setMode(ohm::HeightmapMode::kLayeredFill);
  layered_heightmap.setGenerateVirtualSurface(true);
  // Use tight ceiling/floor constraints to ensure we capture some virtual surfaces.
  layered_heightmap.setCeiling(3 * map.resolution());
  layered_heightmap.setFloor(5 * map.resolution());
  layered_heightmap.buildHeightmap(glm::dvec3(0, 0, 1.1 * params.platform_height));

  ohmtools::SaveCloudOptions save_opt;
  ohmtools::ColourHeightmapType colour_by_type(layered_heightmap.heightmap());
  save_opt.colour_select = [&colour_by_type](const ohm::Voxel<const float> &occupancy) {
    return colour_by_type.select(occupancy);
  };
  save_opt.export_free = true;
  ohm::save("layered-virtual-surface-layered-hm.ohm", layered_heightmap.heightmap());
  ohmtools::saveHeightmapCloud("layered-virtual-surface-layered-hm.ply", layered_heightmap.heightmap(), save_opt);

  // // Walk the heightmap extracting all the voxels. With a zero clearance constraint, we should have an exact
  // // representation of the original map with the addition of virtual surfaces.
  // ohm::Voxel<const float> src_occupancy(&map, map.layout().occupancyLayer());
  // glm::dvec3 hm_pos{};
  // for (auto iter = layered_heightmap.heightmap().begin(); iter != layered_heightmap.heightmap().end(); ++iter)
  // {
  //   const ohm::HeightmapVoxelType voxel_type = layered_heightmap.getHeightmapVoxelInfo(*iter, &hm_pos, nullptr);
  //   switch (voxel_type)
  //   {
  //   case ohm::HeightmapVoxelType::kSurface:
  //     // Validate that this corresponds to an occupied voxel in the source map.
  //     src_occupancy.setKey(map.voxelKey(hm_pos));
  //     ASSERT_EQ(ohm::occupancyType(src_occupancy), ohm::kOccupied);
  //     // Remove this from the list of surface keys. Well validate we touched everything below.
  //     // We must first convert the heightmap position into a key within the source map.
  //     validation_info.surface.erase(map.voxelKey(hm_pos));
  //     break;
  //   case ohm::HeightmapVoxelType::kVirtualSurface:
  //     // Validate that this corresponds to a free voxel in the source map.
  //     src_occupancy.setKey(map.voxelKey(hm_pos));
  //     ASSERT_EQ(ohm::occupancyType(src_occupancy),k.voxelKey(hm_pos));
  //     break;
  //   case ohm::HeightmapVoxelType::kVacant:
  //     // Validate that this corresponds to an unobserved or null voxel in the source map.
  //     ASSERT_TRUE(ohm::isUnobservedOrNull(src_occupancy));
  //     break;
  //   default:
  //     // no op
  //     break;
  //   }
  // }

  // // Ensure we have represented every relevant voxel in the original map.
  // EXPECT_TRUE(validation_info.surface.empty());
  // EXPECT_TRUE(validation_info.virtual_surface.empty());
}


================================================
FILE: tests/slamiotest/CMakeLists.txt
================================================
# Setup of GTEST changed at CMake 3.5.
cmake_minimum_required(VERSION 3.5)



# Eigen required to support some tests - NDT in particular
find_package(Eigen3 QUIET)

set(SOURCES
  Loader.cpp
  SlamCloudLoader.cpp
)

add_executable(slamiotest ${SOURCES})
leak_track_target_enable(slamiotest CONDITION OHM_LEAK_TRACK)
# leak_track_suppress(slamiotest CONDITION OHM_LEAK_TRACK
#   ${OHM_LEAK_SUPPRESS_TBB}
# )

set_target_properties(slamiotest PROPERTIES FOLDER tests)
if(MSVC)
  set_target_properties(slamiotest PROPERTIES DEBUG_POSTFIX "d")
endif(MSVC)

target_include_directories(slamiotest
  PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
)

target_include_directories(slamiotest
  SYSTEM PRIVATE
    "${GTEST_INCLUDE_DIRS}"
  )

target_link_libraries(slamiotest
  PRIVATE
    slamio
    ohmutil
    ${GTEST_LIBRARIES}
    ${GTEST_MAIN_LIBRARIES}
  )

add_test(NAME slamiotest COMMAND slamiotest --gtest_output=xml:test-reports/)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

# install(TARGETS slamiotest DESTINATION bin)


================================================
FILE: tests/slamiotest/Loader.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include "slamio/Points.h"
#include "slamio/PointCloudReaderPly.h"

#include <ohmutil/PlyPointStream.h>

#include <glm/glm.hpp>

#include <fstream>
#include <limits>
#include <random>
#include <vector>

namespace slamio
{
TEST(Loader, Ply)
{
  // Create some points and save them to a ply.
  const std::string test_ply_name = "test_loader.ply";
  const size_t point_count = 100;
  std::vector<slamio::CloudPoint> points(point_count);

  ohm::PlyPointStream ply_out;

  ply_out.setProperties({
    ohm::PlyPointStream::Property{ "timestamp", ohm::PlyPointStream::Type::kFloat64 },
    ohm::PlyPointStream::Property{ "x", ohm::PlyPointStream::Type::kFloat64 },
    ohm::PlyPointStream::Property{ "y", ohm::PlyPointStream::Type::kFloat64 },
    ohm::PlyPointStream::Property{ "z", ohm::PlyPointStream::Type::kFloat64 },
    ohm::PlyPointStream::Property{ "nx", ohm::PlyPointStream::Type::kFloat64 },
    ohm::PlyPointStream::Property{ "ny", ohm::PlyPointStream::Type::kFloat64 },
    ohm::PlyPointStream::Property{ "nz", ohm::PlyPointStream::Type::kFloat64 },
    ohm::PlyPointStream::Property{ "red", ohm::PlyPointStream::Type::kUInt8 },
    ohm::PlyPointStream::Property{ "green", ohm::PlyPointStream::Type::kUInt8 },
    ohm::PlyPointStream::Property{ "blue", ohm::PlyPointStream::Type::kUInt8 },
    ohm::PlyPointStream::Property{ "alpha", ohm::PlyPointStream::Type::kUInt8 },
    ohm::PlyPointStream::Property{ "intensity", ohm::PlyPointStream::Type::kUInt16 },
  });

  std::ofstream ply_out_stream;

  ply_out_stream.open(test_ply_name);
  EXPECT_TRUE(ply_out_stream.is_open());

  ply_out.open(ply_out_stream);

  for (size_t i = 0; i < points.size(); ++i)
  {
    slamio::CloudPoint &pt = points[i];
    pt.timestamp = i * 1.0;
    pt.position.x = i * 1.0;
    pt.position.y = i * -1.0;
    pt.position.z = i * 1.0;
    pt.normal = pt.position;
    pt.colour.r = 1.0f;
    pt.colour.g = 1.0f;
    pt.colour.b = 1.0f;
    pt.colour.a = 1.0f;
    pt.intensity = 1.0f;

    ply_out.setProperty("timestamp", pt.timestamp);
    ply_out.setPointPosition(pt.position);
    ply_out.setPointNormal(pt.normal);
    ply_out.setProperty("red", uint8_t(pt.colour.r * 255.0f));
    ply_out.setProperty("green", uint8_t(pt.colour.g * 255.0f));
    ply_out.setProperty("blue", uint8_t(pt.colour.b * 255.0f));
    ply_out.setProperty("alpha", uint8_t(pt.colour.a * 255.0f));
    ply_out.setProperty("intensity", uint16_t(pt.intensity * float(std::numeric_limits<uint16_t>::max())));
    ply_out.writePoint();
  }

  ply_out.close();

  // Now read the PLY and compare.
  slamio::PointCloudReaderPly reader;
  reader.setDesiredChannels(DataChannel::Time | DataChannel::Position | DataChannel::Normal | DataChannel::Colour |
                            DataChannel::Intensity);

  ASSERT_TRUE(reader.open(test_ply_name.c_str()));
  ASSERT_EQ(reader.pointCount(), point_count);

  // Start reading points
  size_t read_count = 0;
  slamio::CloudPoint pt;
  while (reader.readNext(pt))
  {
    const float ef = 1e-4f;
    const auto &ref_pt = points[read_count];
    EXPECT_EQ(pt.timestamp, ref_pt.timestamp) << read_count;
    EXPECT_EQ(pt.position.x, ref_pt.position.x) << read_count;
    EXPECT_EQ(pt.position.y, ref_pt.position.y) << read_count;
    EXPECT_EQ(pt.position.z, ref_pt.position.z) << read_count;
    EXPECT_EQ(pt.normal.x, ref_pt.normal.x) << read_count;
    EXPECT_EQ(pt.normal.y, ref_pt.normal.y) << read_count;
    EXPECT_EQ(pt.normal.z, ref_pt.normal.z) << read_count;
    EXPECT_NEAR(pt.colour.r, ref_pt.colour.r, ef) << read_count;
    EXPECT_NEAR(pt.colour.g, ref_pt.colour.g, ef) << read_count;
    EXPECT_NEAR(pt.colour.b, ref_pt.colour.b, ef) << read_count;
    EXPECT_NEAR(pt.colour.a, ref_pt.colour.a, ef) << read_count;
    EXPECT_NEAR(pt.intensity, ref_pt.intensity, ef) << read_count;
    ++read_count;
  }

  EXPECT_EQ(read_count, point_count);
}
}  // namespace slamio


================================================
FILE: tests/slamiotest/SlamCloudLoader.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas
#include <gtest/gtest.h>

#include "slamio/PointCloudReader.h"
#include "slamio/SlamCloudLoader.h"
#include "slamio/SlamIO.h"

#include <glm/glm.hpp>

#include <fstream>
#include <limits>
#include <random>
#include <vector>

namespace
{
const size_t traj_sample_hz = 100;
const size_t sample_count = 10000;
const double data_time = 10.0;
const double room_extents = 10.0;
const glm::dvec3 traj_start(0, 0, 0.5);
const glm::dvec3 traj_end(0.5 * room_extents, 0.5 * room_extents, 1.5);
const double e0 = 1e-12;

glm::dvec4 generateTrajectoryPoint(double time)
{
  const glm::dvec4 pt = glm::dvec4(traj_start + (traj_end - traj_start) * time / data_time, time);
  return pt;
}

void writeTimestampedPlyCloud(const std::string &path, const std::vector<glm::dvec4> &timestamped_points)
{
  std::ofstream out(path.c_str(), std::ios::binary);

  out << "ply\n";
  out << "format ascii 1.0\n";
  out << "comment Test data for ras slamio\n";
  out << "element vertex " << timestamped_points.size() << '\n';
  out << "property double time\n";
  out << "property double x\n";
  out << "property double y\n";
  out << "property double z\n";
  out << "end_header\n";
  out << std::flush;

  out.precision(std::numeric_limits<double>::max_digits10);
  for (const auto &time_point : timestamped_points)
  {
    out << time_point.w << ' ' << time_point.x << ' ' << time_point.y << ' ' << time_point.z << '\n';
  }

  out << std::flush;
  out.close();
}

void writeTextTrajectory(const std::string &path, const std::vector<glm::dvec4> &timestamped_points)
{
  std::ofstream out(path.c_str(), std::ios::binary);

  out << "time x y z\n";
  out.precision(std::numeric_limits<double>::max_digits10);
  for (const auto &time_point : timestamped_points)
  {
    out << time_point.w << ' ' << time_point.x << ' ' << time_point.y << ' ' << time_point.z << '\n';
  }

  out << std::flush;
  out.close();
}

void generateSlamCloud(std::vector<glm::dvec4> *timestamped_samples, std::vector<glm::dvec4> *timestamped_trajectory)
{
  // Generate a simplistic trajectory and cloud.

  // Generate trajectory.
  if (timestamped_trajectory)
  {
    for (double time = 0.0; time <= data_time; time += 1.0 / traj_sample_hz)
    {
      timestamped_trajectory->emplace_back(generateTrajectoryPoint(time));
    }
  }

  // Generate large sphere around the trajectory region.
  if (timestamped_samples)
  {
    std::mt19937 rand_engine(0x12345678);
    std::uniform_real_distribution<double> unit_rand(-1, 1);
    for (size_t i = 0; i < sample_count; ++i)
    {
      const double time = data_time / sample_count;
      glm::dvec3 pt(unit_rand(rand_engine), unit_rand(rand_engine), unit_rand(rand_engine));
      pt = glm::normalize(pt);
      pt *= room_extents;
      timestamped_samples->emplace_back(glm::dvec4(pt, time));
    }
  }
}
}  // namespace


namespace slamio
{
TEST(SlamIO, SlamRead)
{
  std::vector<glm::dvec4> samples;
  std::vector<glm::dvec4> trajectory;

  generateSlamCloud(&samples, &trajectory);
  const std::string sample_file = "slam-samples.ply";
  const std::string trajectory_file = "slam-trajectory.ply";
  writeTimestampedPlyCloud(sample_file.c_str(), samples);
  writeTimestampedPlyCloud(trajectory_file.c_str(), trajectory);

  slamio::SlamCloudLoader reader;
  reader.setErrorLog([](const char *msg) { std::cerr << msg << std::flush; });

  ASSERT_TRUE(reader.openWithTrajectory(sample_file.c_str(), trajectory_file.c_str()));

  slamio::SamplePoint sample{};
  size_t i = 0;
  while (reader.nextSample(sample))
  {
    ASSERT_NEAR(sample.timestamp, samples[i].w, e0);
    ASSERT_NEAR(sample.sample.x, samples[i].x, e0);
    ASSERT_NEAR(sample.sample.y, samples[i].y, e0);
    ASSERT_NEAR(sample.sample.z, samples[i].z, e0);
    // Generate a trajectory point at the sample time and check it's near the reported position.
    const glm::dvec3 traj_pt = glm::dvec3(generateTrajectoryPoint(sample.timestamp));
    ASSERT_NEAR(sample.origin.x, traj_pt.x, e0);
    ASSERT_NEAR(sample.origin.y, traj_pt.y, e0);
    ASSERT_NEAR(sample.origin.z, traj_pt.z, e0);
    ++i;
  }
}

TEST(SlamIO, CloudRead)
{
  std::vector<glm::dvec4> samples;

  generateSlamCloud(&samples, nullptr);
  const std::string sample_file = "cloud-read-samples.ply";
  writeTimestampedPlyCloud(sample_file, samples);

  auto reader = slamio::createCloudReaderFromFilename(sample_file.c_str());
  ASSERT_NE(reader, nullptr);
  // reader->setErrorLog([](const char *msg) { std::cerr << msg << std::flush; });

  ASSERT_TRUE(reader->open(sample_file.c_str()));
  ASSERT_EQ(reader->pointCount(), samples.size());

  slamio::CloudPoint point{};
  for (size_t i = 0; i < samples.size(); ++i)
  {
    ASSERT_TRUE(reader->readNext(point));
    ASSERT_NEAR(point.timestamp, samples[i].w, e0);
    ASSERT_NEAR(point.position.x, samples[i].x, e0);
    ASSERT_NEAR(point.position.y, samples[i].y, e0);
    ASSERT_NEAR(point.position.z, samples[i].z, e0);
  }
}

TEST(SlamIO, TrajectoryRead)
{
  std::vector<glm::dvec4> trajectory;

  generateSlamCloud(nullptr, &trajectory);
  const std::string trajectory_file = "trajectory-read-trajectory.txt";
  writeTextTrajectory(trajectory_file, trajectory);

  auto reader = slamio::createCloudReaderFromFilename(trajectory_file.c_str());
  ASSERT_NE(reader, nullptr);
  // reader->setErrorLog([](const char *msg) { std::cerr << msg << std::flush; });

  ASSERT_TRUE(reader->open(trajectory_file.c_str()));
  // Note: point count will not be known for a text trajectory.

  slamio::CloudPoint point{};
  for (size_t i = 0; i < trajectory.size(); ++i)
  {
    ASSERT_TRUE(reader->readNext(point));
    ASSERT_EQ(point.timestamp, trajectory[i].w);
    ASSERT_NEAR(point.timestamp, trajectory[i].w, e0);
    ASSERT_NEAR(point.position.x, trajectory[i].x, e0);
    ASSERT_NEAR(point.position.y, trajectory[i].y, e0);
    ASSERT_NEAR(point.position.z, trajectory[i].z, e0);
  }
}
}  // namespace slamio


================================================
FILE: todo.md
================================================
# TODO

- The default hit/miss and min/max for ohmpop are not tuned for NDT (based on VLP-16 input)
  - Max is certainly too low and will result in erosion
- (More) GPU optimisation pass. Since memory access is so irregular, best to look at avoiding branching
  - Look for effects of different GPU hardware
  - Look for differences between OpenCL/CUDA
- Convert to RAII (new/delete have been targetted)
- `inline` keyword is overused for methods inlined in the class body. This is a legacy of when MSC would not imply inline. It's hard to say when this changed, anecdotally speaking, but the documentation from [Visual Studio 2015](https://learn.microsoft.com/en-us/cpp/cpp/inline-functions-cpp?view=msvc-140) is clear that's its no longer necessary, or just a misinterpretation


================================================
FILE: utils/CMakeLists.txt
================================================

find_package(ZLIB)
find_package(TBB)

add_subdirectory(ohm2ply)
add_subdirectory(ohmcmp)
add_subdirectory(ohmfilter)
add_subdirectory(ohmheightmap)
add_subdirectory(ohminfo)
add_subdirectory(ohmpop)
add_subdirectory(ohmprob)
add_subdirectory(ohmquery)
add_subdirectory(ohmsubmap)

if(OHM_FEATURE_HEIGHTMAP_IMAGE)
  add_subdirectory(ohmhm2img)
endif(OHM_FEATURE_HEIGHTMAP_IMAGE)


================================================
FILE: utils/ohm2ply/CMakeLists.txt
================================================
find_package(ZLIB)

set(SOURCES
  ohm2ply.cpp
)

add_executable(ohm2ply ${SOURCES})
leak_track_target_enable(ohm2ply CONDITION OHM_LEAK_TRACK)

set_target_properties(ohm2ply PROPERTIES FOLDER utils)
if(MSVC)
  set_target_properties(ohm2ply PROPERTIES DEBUG_POSTFIX "d")
endif(MSVC)

target_link_libraries(ohm2ply
  PUBLIC
    ohm
    ohmheightmap
    ohmtools
    ohmutil
  PRIVATE
    glm::glm
    $<BUILD_INTERFACE:ZLIB::ZLIB>
)

clang_tidy_target(ohm2ply)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

install(TARGETS ohm2ply DESTINATION bin)


================================================
FILE: utils/ohm2ply/ohm2ply.cpp
================================================
//
// author Kazys Stepanas
//
#include <glm/glm.hpp>

#include <ohm/DefaultLayer.h>
#include <ohm/Key.h>
#include <ohm/KeyList.h>
#include <ohm/MapInfo.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyType.h>
#include <ohm/VoxelData.h>

#include <ohmheightmap/Heightmap.h>
#include <ohmheightmap/HeightmapMesh.h>
#include <ohmheightmap/HeightmapSerialise.h>
#include <ohmheightmap/HeightmapVoxel.h>

#include <ohmtools/OhmCloud.h>
#include <ohmutil/PlyMesh.h>
#include <ohmutil/ProgressMonitor.h>

// CovarianceVoxel.h must be included later due to GPU suppport for this file.
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp>
#include <glm/gtx/transform.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

#include <ohm/CovarianceVoxel.h>

#include <algorithm>
#include <array>
#include <chrono>
#include <csignal>
#include <cstddef>
#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <unordered_set>

namespace
{
int g_quit = 0;

void onSignal(int arg)
{
  if (arg == SIGINT || arg == SIGTERM)
  {
    ++g_quit;
  }
}

enum ExportMode
{
  kExportOccupancy,
  kExportOccupancyCentre,
  kExportDensity,
  kExportObserved,
  kExportClearance,
  kExportHeightmap,
  kExportHeightmapMesh,
  kExportCovariance,
  kExportTsdf
};

/// Voxel mode: export voxels as...
enum VoxelMode
{
  kVoxelPoint,
  kVoxelVoxel
};

enum ColourMode
{
  kColourNone,
  kColourHeight,
  kColourIntensity,
  kColourLayer,
  kColourOccupancy,
  kColourType
};

struct ColourModeOrValue
{
  ColourMode mode = kColourNone;  ///< kColourNone implies use of value.
  ohm::Colour value = ohm::Colour::kColours[ohm::Colour::kLightSeaGreen];

  explicit inline ColourModeOrValue(ColourMode mode = kColourNone)
    : mode(mode)
  {}

  explicit inline ColourModeOrValue(ohm::Colour value)
    : value(value)
  {}
};

struct HeightmapOptions
{
  bool collapse = false;  ///< Collapse a layered heightmap into a 2.5D heightmap.
};

struct Options
{
  std::string map_file;
  std::string ply_file;
  // expire regions older than this
  double expiry_time = 0;
  float cull_distance = 0;
  float threshold = -1.0f;
  float max_intensity = 100.0f;
  float colour_scale = 3.0f;
  ExportMode mode = kExportOccupancy;
  ColourModeOrValue colour = ColourModeOrValue(kColourHeight);
  VoxelMode voxel_mode = kVoxelPoint;

  HeightmapOptions heightmap;
};

template <typename NUMERIC>
bool optionValue(const char *arg, int argc, char *argv[], NUMERIC &value)  // NOLINT(modernize-avoid-c-arrays)
{
  std::string str_value;
  if (optionValue(arg, argc, argv, str_value))
  {
    std::istringstream instr(str_value);
    instr >> value;
    return !instr.fail();
  }

  return false;
}

class LoadMapProgress : public ohm::SerialiseProgress
{
public:
  explicit LoadMapProgress(ProgressMonitor &monitor)
    : monitor_(monitor)
  {}

  bool quit() const override { return ::g_quit > 1; }

  void setTargetProgress(unsigned target) override { monitor_.beginProgress(ProgressMonitor::Info(target)); }
  void incrementProgress(unsigned inc) override { monitor_.incrementProgressBy(inc); }

private:
  ProgressMonitor &monitor_;
};


/// Build a sphere approximation with an icosahedron.
/// @todo Make one subdivision for better spheres.
void makeUnitSphere(std::vector<glm::dvec3> &vertices, std::vector<unsigned> &indices)
{
  // We start with two hexagonal rings to approximate the sphere.
  // All subdivision occurs on a unit radius sphere, at the origin. We translate and
  // scale the vertices at the end.
  vertices.clear();
  indices.clear();

  const double ring_control_angle = 25.0 / 180.0 * M_PI;
  const double ring_height = std::sin(ring_control_angle);
  const double ring_radius = std::cos(ring_control_angle);
  const double hex_angle = 2.0 * M_PI / 6.0;
  const double ring2_offset_angle = 0.5 * hex_angle;
  const std::array<glm::dvec3, 14> initial_vertices =  // NOLINT(readability-magic-numbers)
    {
      glm::dvec3(0, 0, 1),

      // Upper hexagon.
      glm::dvec3(ring_radius, 0, ring_height),
      glm::dvec3(ring_radius * std::cos(hex_angle), ring_radius * std::sin(hex_angle), ring_height),
      glm::dvec3(ring_radius * std::cos(2 * hex_angle), ring_radius * std::sin(2 * hex_angle), ring_height),
      glm::dvec3(ring_radius * std::cos(3 * hex_angle), ring_radius * std::sin(3 * hex_angle), ring_height),
      glm::dvec3(ring_radius * std::cos(4 * hex_angle), ring_radius * std::sin(4 * hex_angle), ring_height),
      glm::dvec3(ring_radius * std::cos(5 * hex_angle), ring_radius * std::sin(5 * hex_angle), ring_height),

      // Lower hexagon.
      glm::dvec3(ring_radius * std::cos(ring2_offset_angle), ring_radius * std::sin(ring2_offset_angle), -ring_height),
      glm::dvec3(ring_radius * std::cos(ring2_offset_angle + hex_angle),
                 ring_radius * std::sin(ring2_offset_angle + hex_angle), -ring_height),
      glm::dvec3(ring_radius * std::cos(ring2_offset_angle + 2 * hex_angle),
                 ring_radius * std::sin(ring2_offset_angle + 2 * hex_angle), -ring_height),
      glm::dvec3(ring_radius * std::cos(ring2_offset_angle + 3 * hex_angle),
                 ring_radius * std::sin(ring2_offset_angle + 3 * hex_angle), -ring_height),
      glm::dvec3(ring_radius * std::cos(ring2_offset_angle + 4 * hex_angle),
                 ring_radius * std::sin(ring2_offset_angle + 4 * hex_angle), -ring_height),
      glm::dvec3(ring_radius * std::cos(ring2_offset_angle + 5 * hex_angle),
                 ring_radius * std::sin(ring2_offset_angle + 5 * hex_angle), -ring_height),

      glm::dvec3(0, 0, -1),
    };

  const std::array<unsigned, 72> initial_indices =  // NOLINT(readability-magic-numbers)
    { 0,  1,  2, 0,  2,  3, 0, 3,  4, 0, 4,  5, 0, 5,  6,  0,  6,  1,  1,  7,  2,  2,  8,  3,
      3,  9,  4, 4,  10, 5, 5, 11, 6, 6, 12, 1, 7, 8,  2,  8,  9,  3,  9,  10, 4,  10, 11, 5,
      11, 12, 6, 12, 7,  1, 7, 13, 8, 8, 13, 9, 9, 13, 10, 10, 13, 11, 11, 13, 12, 12, 13, 7 };

  for (const auto vertex : initial_vertices)
  {
    vertices.emplace_back(vertex);
  }

  for (auto index : initial_indices)
  {
    indices.emplace_back(index);
  }
}
}  // namespace


// Custom option parsing. Must come before we include Options.h/cxxopt.hpp
void badArg(const std::string &arg);

std::istream &operator>>(std::istream &in, ExportMode &mode)
{
  std::string mode_str;
  in >> mode_str;
  if (mode_str == "occupancy")
  {
    mode = kExportOccupancy;
  }
  else if (mode_str == "occupancy-centre")
  {
    mode = kExportOccupancyCentre;
  }
  else if (mode_str == "density")
  {
    mode = kExportDensity;
  }
  else if (mode_str == "observed")
  {
    mode = kExportObserved;
  }
  else if (mode_str == "clearance")
  {
    mode = kExportClearance;
  }
  else if (mode_str == "heightmap")
  {
    mode = kExportHeightmap;
  }
  else if (mode_str == "heightmap-mesh")
  {
    mode = kExportHeightmapMesh;
  }
  else if (mode_str == "covariance")
  {
    mode = kExportCovariance;
  }
  else if (mode_str == "tsdf")
  {
    mode = kExportTsdf;
  }
  else
  {
    badArg(mode_str);
  }
  return in;
}

std::ostream &operator<<(std::ostream &out, const ExportMode mode)
{
  switch (mode)
  {
  case kExportOccupancy:
    out << "occupancy";
    break;
  case kExportOccupancyCentre:
    out << "occupancy-centre";
    break;
  case kExportObserved:
    out << "observed";
    break;
  case kExportDensity:
    out << "density";
    break;
  case kExportClearance:
    out << "clearance";
    break;
  case kExportHeightmap:
    out << "heightmap";
    break;
  case kExportHeightmapMesh:
    out << "heightmap-mesh";
    break;
  case kExportCovariance:
    out << "covariance";
    break;
  case kExportTsdf:
    out << "tsdf";
    break;
  default:
    out << "<unknown>";
  }
  return out;
}


std::istream &operator>>(std::istream &in, VoxelMode &mode)
{
  std::string mode_str;
  in >> mode_str;
  if (mode_str == "point")
  {
    mode = kVoxelPoint;
  }
  else if (mode_str == "voxel")
  {
    mode = kVoxelVoxel;
  }
  else
  {
    badArg(mode_str);
  }
  return in;
}

std::ostream &operator<<(std::ostream &out, const VoxelMode mode)
{
  switch (mode)
  {
  case kVoxelPoint:
    out << "point";
    break;
  case kVoxelVoxel:
    out << "voxel";
    break;
  default:
    out << "<unknown>";
  }
  return out;
}


std::istream &operator>>(std::istream &in, ColourModeOrValue &colour)
{
  std::string mode_str;
  in >> mode_str;
  if (mode_str == "none")
  {
    colour.mode = kColourNone;
  }
  else if (mode_str == "height")
  {
    colour.mode = kColourHeight;
  }
  else if (mode_str == "intensity")
  {
    colour.mode = kColourIntensity;
  }
  else if (mode_str == "occupancy")
  {
    colour.mode = kColourOccupancy;
  }
  else if (mode_str == "layer")
  {
    colour.mode = kColourLayer;
  }
  else if (mode_str == "type")
  {
    colour.mode = kColourType;
  }
  else
  {
    // Try value parsing.
    colour.mode = kColourNone;
    bool ok = true;
    int channel = 0;
    char ch = '\0';

    std::istringstream str_in(mode_str);
    str_in >> channel >> ch;
    ok = ok && !str_in.fail() && 0 <= channel && channel <= 255 && ch == ',';
    colour.value.rgba[ohm::Colour::kR] = channel;

    str_in >> channel >> ch;
    ok = ok && !str_in.fail() && 0 <= channel && channel <= 255 && ch == ',';
    colour.value.rgba[ohm::Colour::kG] = channel;

    str_in >> channel;
    ok = ok && !str_in.fail() && 0 <= channel && channel <= 255;
    colour.value.rgba[ohm::Colour::kB] = channel;

    if (!ok)
    {
      badArg(mode_str);
    }
  }
  return in;
}

std::ostream &operator<<(std::ostream &out, const ColourModeOrValue colour)
{
  switch (colour.mode)
  {
  case kColourNone:
    out << int(colour.value.r()) << ',' << int(colour.value.g()) << ',' << int(colour.value.b());
    break;
  case kColourHeight:
    out << "height";
    break;
  case kColourIntensity:
    out << "intensity";
    break;
  case kColourOccupancy:
    out << "occupancy";
    break;
  case kColourLayer:
    out << "layer";
    break;
  case kColourType:
    out << "type";
    break;
  default:
    out << "<unknown>";
  }
  return out;
}


// Must be after argument streaming operators.
#include <ohmutil/Options.h>

void badArg(const std::string &arg)
{
  // Must be declared before, but included after cxxopt.
  throw cxxopts::invalid_option_format_error(arg);
}


int parseOptions(Options *opt, int argc, char *argv[])  // NOLINT(modernize-avoid-c-arrays)
{
  cxxopts::Options opt_parse(argv[0], "Convert an occupancy map to a point cloud. Defaults to generate a positional "
                                      "point cloud, but can generate a clearance cloud as well.");
  opt_parse.positional_help("<map.ohm> <cloud.ply>");

  try
  {
    // Build GPU options set.
    // clang-format off
    opt_parse.add_options()
      ("help", "Show help.")
      ("colour-scale", "Colour max scaling value for colouring a clearance or heightmap cloud. Max colour at this range..", cxxopts::value(opt->colour_scale))
      ("colour", "Colour control for exported point clouds {none,height,intensity,occupancy,layer,type,<r,g,b>}.", optVal(opt->colour))
      ("cloud", "The output cloud file (ply).", cxxopts::value(opt->ply_file))
      ("cull", "Remove regions farther than the specified distance from the map origin.", cxxopts::value(opt->cull_distance)->default_value(optStr(opt->cull_distance)))
      ("map", "The input map file (ohm).", cxxopts::value(opt->map_file))
      ("mode", "Export mode [occupancy,occupancy-centre,density,clearance,covariance,heightmap,heightmap-mesh]: select "
               "which data to export from the map. occupancy and occupancy-centre differ only in that the latter "
               "forces positioning on voxel centres. density uses the density calculation and requires the map have "
               "traversability and voxel mean info - also uses threshold as a density threshold instead of occupancy",
               cxxopts::value(opt->mode)->default_value(optStr(opt->mode)))
      ("expire", "Expire regions with a timestamp before the specified time. These are not exported.", cxxopts::value(opt->expiry_time))
      ("threshold", "Override the map's occupancy threshold or set the density threshold. Only points passing the "
                    "threshold occupied points are exported.",
                    cxxopts::value(opt->threshold)->default_value(optStr(opt->threshold)))
      ("max-intensity", "Maximum expected intensity value. For use with --colour=intensity, this is the value at which the colour saturates.", optVal(opt->max_intensity))
      ("voxel-mode", "Voxel export mode [point,voxel]: select the ply representation for voxels.", cxxopts::value(opt->voxel_mode)->default_value(optStr(opt->voxel_mode)))
      ;

    opt_parse.add_options("Heightmap")
      ("heightmap-2d", "Reduce to a 2.5D heightmap cloud regardless of input heightmap. This collapses a layered heightmap.", optVal(opt->heightmap.collapse))
      ;
    // clang-format on

    opt_parse.parse_positional({ "map", "cloud" });

    cxxopts::ParseResult parsed = opt_parse.parse(argc, argv);

    if (parsed.count("help") || parsed.arguments().empty())
    {
      // show usage.
      std::cout << opt_parse.help({ "", "Heightmap" }) << std::endl;
      return 1;
    }

    if (opt->map_file.empty())
    {
      std::cerr << "Missing input map file name" << std::endl;
      return -1;
    }
    if (opt->ply_file.empty())
    {
      std::cerr << "Missing output file name" << std::endl;
      return -1;
    }
  }
  catch (const cxxopts::OptionException &e)
  {
    std::cerr << "Argument error\n" << e.what() << std::endl;
    return -1;
  }

  return 0;
}


int exportPointCloud(const Options &opt, ProgressMonitor &prog, LoadMapProgress &load_progress)
{
  ohm::OccupancyMap map(1.0f);

  prog.startThread();
  int res = ohm::load(opt.map_file.c_str(), map, &load_progress);
  prog.endProgress();

  std::cout << std::endl;

  if (res != 0)
  {
    std::cerr << "Failed to load map. Error(" << res << "): " << ohm::serialiseErrorCodeString(res) << std::endl;
    return res;
  }

  // Validate the required layer is present.
  switch (opt.mode)
  {
  case kExportOccupancy:
  case kExportOccupancyCentre:
  case kExportObserved:
    if (map.layout().layer("occupancy") == nullptr)
    {
      std::cerr << "Missing 'occupancy' layer" << std::endl;
      res = -1;
    }
    break;
  case kExportClearance:
    if (map.layout().layer("clearance") == nullptr)
    {
      std::cerr << "Missing 'clearance' layer" << std::endl;
      res = -1;
    }
    break;
  case kExportDensity:
    if (map.layout().traversalLayer() == -1)
    {
      std::cout << "Missing 'traversal' layer" << std::endl;
      res = -1;
    }
    if (map.layout().meanLayer() == -1)
    {
      std::cout << "Missing 'mean' layer" << std::endl;
      res = -1;
    }
    break;
  case kExportHeightmap:
  {
    const ohm::MapLayer *layer = map.layout().layer(ohm::HeightmapVoxel::kHeightmapLayer);
    if (!layer)
    {
      std::cerr << "Missing '" << ohm::HeightmapVoxel::kHeightmapLayer << "' layer" << std::endl;
      res = -1;
    }
    else if (layer->voxelByteSize() < sizeof(ohm::HeightmapVoxel))
    {
      std::cerr << "Layer '" << ohm::HeightmapVoxel::kHeightmapLayer << "' is not large enough. Expect "
                << sizeof(ohm::HeightmapVoxel) << " actual " << layer->voxelByteSize() << std::endl;
      res = -1;
    }

    break;
  }
  case kExportTsdf:
  {
    if (map.layout().layerIndex(ohm::default_layer::tsdfLayerName()) == -1)
    {
      std::cout << "Missing 'tsdf' layer" << std::endl;
      res = -1;
    }
  }
  break;
  default:
    std::cout << "Invalid mode for point cloud: " << opt.mode << std::endl;
    res = -1;
    break;
  }

  if (res != 0)
  {
    return res;
  }

  if (opt.threshold >= 0 && opt.mode != kExportDensity)
  {
    map.setOccupancyThresholdProbability(opt.threshold);
  }

  if (opt.cull_distance > 0)
  {
    std::cout << "Culling regions beyond range : " << opt.cull_distance << std::endl;
    const unsigned removed = map.removeDistanceRegions(map.origin(), opt.cull_distance);
    std::cout << "Removed " << removed << " regions" << std::endl;
    ;
  }
  if (opt.expiry_time > 0)
  {
    std::cout << "Expiring regions before time: " << opt.expiry_time << std::endl;
    unsigned removed = map.expireRegions(opt.expiry_time);
    std::cout << "Removed " << removed << " regions" << std::endl;
  }

  std::cout << "Converting to PLY cloud" << std::endl;
  const size_t region_count = map.regionCount();
  // uint64_t point_count = 0;

  prog.beginProgress(ProgressMonitor::Info(region_count));

  const auto save_progress_callback = [&prog](size_t progress, size_t /*target*/) { prog.updateProgress(progress); };
  ohmtools::ColourByHeight colour_by_height(map);
  ohmtools::ColourByType colour_by_type(map);
  ohmtools::ColourByIntensity colour_by_intensity(map, opt.max_intensity);
  ohmtools::ColourByOccupancy colour_by_occupancy(map);
  ohmtools::ColourHeightmapType colour_by_heightmap_type(map);
  ohmtools::ColourHeightmapLayer colour_by_heightmap_layer(map);

  uint64_t export_count = 0;
  switch (opt.mode)
  {
  case kExportOccupancy:
    // fallthrough
  case kExportOccupancyCentre:
  case kExportObserved:
  {
    ohmtools::SaveCloudOptions save_opt;
    save_opt.ignore_voxel_mean = opt.mode != kExportOccupancy;
    save_opt.export_free = opt.mode == kExportObserved;
    // Default colour mode for saveCloud() is colour by height.
    save_opt.allow_default_colour_selection = (opt.colour.mode == kColourHeight);
    switch (opt.colour.mode)
    {
    case kColourNone:
      save_opt.colour_select = [&opt](const ohm::Voxel<const float> &) { return opt.colour.value; };
      break;
    case kColourType:
      save_opt.colour_select = [&colour_by_type](const ohm::Voxel<const float> &occupancy) {
        return colour_by_type.select(occupancy);
      };
      break;
    case kColourHeight:
      save_opt.colour_select = [&colour_by_height](const ohm::Voxel<const float> &occupancy) {
        return colour_by_height.select(occupancy);
      };
      break;
    case kColourOccupancy:
      save_opt.colour_select = [&colour_by_occupancy](const ohm::Voxel<const float> &occupancy) {
        return colour_by_occupancy.select(occupancy);
      };
      break;
    case kColourIntensity:
    {
      if (!colour_by_intensity.isValid())
      {
        std::cerr << "Cannot colour by intensity. Intenstiy layer not found." << std::endl;
        return -1;
      }
      save_opt.colour_select = [&colour_by_intensity](const ohm::Voxel<const float> &occupancy) {
        return colour_by_intensity.select(occupancy);
      };
      break;
    }
    default:
      std::cerr << "Unsupported colour mode for occupancy export: " << opt.colour << std::endl;
      return -1;
    }
    if (opt.voxel_mode == kVoxelVoxel)
    {
      export_count = saveVoxels(opt.ply_file.c_str(), map, save_opt, save_progress_callback);
    }
    else
    {
      export_count = saveCloud(opt.ply_file.c_str(), map, save_opt, save_progress_callback);
    }
    break;
  }
  case kExportDensity:
  {
    ohmtools::SaveDensityCloudOptions save_opt;
    save_opt.ignore_voxel_mean = false;
    save_opt.allow_default_colour_selection = true;
    save_opt.density_threshold = opt.threshold;
    export_count = saveDensityCloud(opt.ply_file.c_str(), map, save_opt, save_progress_callback);
    break;
  }
  case kExportHeightmap:
  {
    ohmtools::SaveHeightmapCloudOptions save_opt;
    save_opt.ignore_voxel_mean = false;
    save_opt.export_free = true;
    save_opt.collapse = opt.heightmap.collapse;
    switch (opt.colour.mode)
    {
    case kColourNone:
      save_opt.colour_select = [&opt](const ohm::Voxel<const float> &) { return opt.colour.value; };
      break;
    case kColourHeight:
      save_opt.colour_select = [&colour_by_height](const ohm::Voxel<const float> &occupancy) {
        return colour_by_height.select(occupancy);
      };
      break;
    case kColourLayer:
      save_opt.colour_select = [&colour_by_heightmap_layer](const ohm::Voxel<const float> &occupancy) {
        return colour_by_heightmap_layer.select(occupancy);
      };
      break;
    case kColourOccupancy:
      save_opt.colour_select = [&colour_by_occupancy](const ohm::Voxel<const float> &occupancy) {
        return colour_by_occupancy.select(occupancy);
      };
      break;
    case kColourType:
      save_opt.colour_select = [&colour_by_heightmap_type](const ohm::Voxel<const float> &occupancy) {
        return colour_by_heightmap_type.select(occupancy);
      };
      break;
    case kColourIntensity:
    {
      if (!colour_by_intensity.isValid())
      {
        std::cerr << "Cannot colour by intensity. Intenstiy layer not found." << std::endl;
        return -1;
      }
      save_opt.colour_select = [&colour_by_intensity](const ohm::Voxel<const float> &occupancy) {
        return colour_by_intensity.select(occupancy);
      };
      break;
    }
    default:
      std::cerr << "Unsupported colour mode for heightmap export: " << opt.colour << std::endl;
      return -1;
    }
    if (opt.voxel_mode == kVoxelVoxel)
    {
      export_count = ohmtools::saveHeightmapVoxels(opt.ply_file.c_str(), map, save_opt, save_progress_callback);
    }
    else
    {
      export_count = ohmtools::saveHeightmapCloud(opt.ply_file.c_str(), map, save_opt, save_progress_callback);
    }
    break;
  }
  case kExportClearance:
  {
    glm::dvec3 min_ext;
    glm::dvec3 max_ext;
    map.calculateExtents(&min_ext, &max_ext);
    export_count = ohmtools::saveClearanceCloud(opt.ply_file.c_str(), map, min_ext, max_ext, opt.colour_scale,
                                                ohm::kFree, save_progress_callback);
    break;
  }
  case kExportTsdf:
  {
    float surface_distance =
      static_cast<float>(map.mapInfo().get("tsdf-default-truncation-distance", ohm::MapValue("", 0.1f)));
    if (opt.threshold <= 0)
    {
      surface_distance = std::min(surface_distance, float(map.resolution()));
    }
    else
    {
      surface_distance = opt.threshold;
    }

    ohmtools::ColourByHeight colour_by_height(map);
    ohmtools::ColourSelectTsdf colour_select = {};

    switch (opt.colour.mode)
    {
    case kColourNone:
      colour_select = [&opt](const ohm::Voxel<const ohm::VoxelTsdf> &) { return opt.colour.value; };
      break;
    case kColourHeight:
      colour_select = [&colour_by_height](const ohm::Voxel<const ohm::VoxelTsdf> &voxel) {
        return colour_by_height.select(voxel.key());
      };
      break;
    default:
      std::cerr << "Unsupported colour mode for TSDF export: " << opt.colour << std::endl;
      return -1;
    }

    // TODO(KS): set surface distance based on default truncation distance. For that we need to write the TSDF
    // parameters to the map info.
    if (opt.voxel_mode == kVoxelVoxel)
    {
      export_count =
        ohmtools::saveTsdfVoxels(opt.ply_file.c_str(), map, surface_distance, colour_select, save_progress_callback);
    }
    else
    {
      export_count =
        ohmtools::saveTsdfCloud(opt.ply_file.c_str(), map, surface_distance, colour_select, save_progress_callback);
    }
    break;
  }
  default:
    std::cout << "Invalid mode for point cloud: " << opt.mode << std::endl;
    return -1;
  }

  prog.endProgress();
  prog.pause();

  std::cout << "\nExported " << export_count << " point(s)" << std::endl;

  return res;
}


int exportHeightmapMesh(const Options &opt, ProgressMonitor &prog, LoadMapProgress &load_progress)
{
  ohm::Heightmap heightmap;
  ohm::PlyMesh ply;
  prog.startThread();
  int res = ohm::load(opt.map_file.c_str(), heightmap, &load_progress);
  prog.endProgress();
  prog.pause();
  std::cout << std::endl;

  if (res != 0)
  {
    std::cerr << "Failed to load heightmap. Error(" << res << "): " << ohm::serialiseErrorCodeString(res) << std::endl;
    return res;
  }

  ohm::HeightmapMesh mesh;
  mesh.buildMesh(heightmap);
  mesh.extractPlyMesh(ply);

  if (!g_quit)
  {
    if (!ply.save(opt.ply_file.c_str(), true))
    {
      res = -1;
    }
  }

  return res;
}

int exportCovariance(const Options &opt, ProgressMonitor &prog, LoadMapProgress &load_progress)
{
  ohm::OccupancyMap map(1.0f);
  ohm::PlyMesh ply;

  prog.startThread();
  int res = ohm::load(opt.map_file.c_str(), map, &load_progress);
  prog.endProgress();

  std::cout << std::endl;

  if (res != 0)
  {
    std::cerr << "Failed to load map. Error(" << res << "): " << ohm::serialiseErrorCodeString(res) << std::endl;
    return res;
  }

  // Validate we have covariance and voxel mean.
  if (map.layout().meanLayer() == -1)
  {
    std::cerr << "Missing voxel mean layer" << std::endl;
    res = -1;
  }

  if (map.layout().covarianceLayer() == -1)
  {
    std::cerr << "Missing covariance layer" << std::endl;
    res = -1;
  }

  if (res != 0)
  {
    return res;
  }

  std::vector<glm::dvec3> sphere_verts;
  std::vector<unsigned> sphere_inds;
  makeUnitSphere(sphere_verts, sphere_inds);

  const auto add_ellipsoid =
    [&sphere_verts, &sphere_inds](ohm::PlyMesh &ply, const glm::dmat4 &transform, const ohm::Colour &colour)  //
  {
    unsigned index_offset = ~0u;

    for (const auto &v : sphere_verts)
    {
      index_offset = std::min(ply.addVertex(glm::dvec3(transform * glm::dvec4(v, 1.0)), colour), index_offset);
    }

    for (size_t i = 0; i < sphere_inds.size(); i += 3)
    {
      ply.addTriangle(sphere_inds[i + 0] + index_offset, sphere_inds[i + 1] + index_offset,
                      sphere_inds[i + 2] + index_offset, colour);
    }
  };

  const size_t region_count = map.regionCount();
  glm::i16vec3 last_region = map.begin().key().regionKey();

  ohm::Voxel<const float> occupancy(&map, map.layout().occupancyLayer());
  ohm::Voxel<const ohm::VoxelMean> mean(&map, map.layout().meanLayer());
  ohm::Voxel<const ohm::CovarianceVoxel> covariance(&map, map.layout().covarianceLayer());

  ohmtools::ColourSelect colour_select;
  ohmtools::ColourByHeight colour_by_height(map);
  ohmtools::ColourByType colour_by_type(map);
  ohmtools::ColourByIntensity colour_by_intensity(map, opt.max_intensity);
  ohmtools::ColourByOccupancy colour_by_occupancy(map);
  switch (opt.colour.mode)
  {
  case kColourNone:
    colour_select = [&opt](const ohm::Voxel<const float> &) { return opt.colour.value; };
    break;
  case kColourType:
    colour_select = [&colour_by_type](const ohm::Voxel<const float> &occupancy) {
      return colour_by_type.select(occupancy);
    };
    break;
  case kColourHeight:
    colour_select = [&colour_by_height](const ohm::Voxel<const float> &occupancy) {
      return colour_by_height.select(occupancy);
    };
    break;
  case kColourIntensity:
  {
    if (!colour_by_intensity.isValid())
    {
      std::cerr << "Cannot colour by intensity. Intenstiy layer not found." << std::endl;
      return -1;
    }
    colour_select = [&colour_by_intensity](const ohm::Voxel<const float> &occupancy) {
      return colour_by_intensity.select(occupancy);
    };
    break;
  }
  case kColourOccupancy:
    colour_select = [&colour_by_occupancy](const ohm::Voxel<const float> &occupancy) {
      return colour_by_occupancy.select(occupancy);
    };
    break;
  default:
    std::cerr << "Unsupported colour mode for occupancy export: " << opt.colour << std::endl;
    return -1;
  }

  if (!occupancy.isLayerValid())
  {
    std::cerr << "Missing " << ohm::default_layer::occupancyLayerName() << " layer" << std::endl;
    res = -1;
  }

  if (!mean.isLayerValid())
  {
    std::cerr << "Missing " << ohm::default_layer::meanLayerName() << " layer" << std::endl;
    res = -1;
  }

  if (!covariance.isLayerValid())
  {
    std::cerr << "Missing " << ohm::default_layer::covarianceLayerName() << " layer" << std::endl;
    res = -1;
  }

  if (res != 0)
  {
    return res;
  }

  prog.beginProgress(ProgressMonitor::Info(region_count));

  for (auto iter = map.begin(); iter != map.end() && !g_quit; ++iter)
  {
    ohm::setVoxelKey(*iter, occupancy, mean, covariance);
    if (last_region != iter->regionKey())
    {
      prog.incrementProgress();
      last_region = iter.key().regionKey();
    }

    if (!ohm::isOccupied(occupancy))
    {
      continue;
    }

    const glm::dvec3 pos = ohm::positionSafe(mean);
    ohm::CovarianceVoxel cov;
    covariance.read(&cov);

    // Add an ellipsoid to the PLY
    glm::dquat rot;
    glm::dvec3 scale;
    ohm::covarianceUnitSphereTransformation(&cov, &rot, &scale);
    // For rendering niceness, we scale up a bit to get better overlap between voxels.
    const double scale_factor = std::sqrt(3.0);
    scale *= scale_factor;

    const glm::dmat4 transform = glm::translate(pos) * glm::mat4_cast(rot) * glm::scale(scale);
    add_ellipsoid(ply, transform, colour_select(occupancy));
  }

#if OHM_COV_DEBUG
  ohm::covDebugStats();
#endif  // OHM_COV_DEBUG

  if (!g_quit)
  {
    if (!ply.save(opt.ply_file.c_str(), true))
    {
      res = -1;
    }
  }

  return res;
}

int main(int argc, char *argv[])
{
  Options opt;
  std::cout.imbue(std::locale(""));

  int res = parseOptions(&opt, argc, argv);

  if (res)
  {
    return res;
  }

  signal(SIGINT, onSignal);
  signal(SIGTERM, onSignal);

  std::cout << "Loading map " << opt.map_file.c_str() << std::endl;
  std::cout << "Export mode: " << opt.mode << std::endl;
  ProgressMonitor prog(10);
  LoadMapProgress load_progress(prog);

  prog.setDisplayFunction([](const ProgressMonitor::Progress &prog, bool final) {
    // if (!opt.quiet)
    {
      std::ostringstream out;
      out.imbue(std::locale(""));
      out << '\r';

      if (!prog.info.info.empty())
      {
        out << prog.info.info << " : ";
      }

      const auto fill_width = std::numeric_limits<decltype(prog.progress)>::digits10;
      out << std::setfill(' ') << std::setw(fill_width) << prog.progress;
      if (prog.info.total)
      {
        out << " / " << std::setfill(' ') << std::setw(fill_width) << prog.info.total;
      }
      out << "    ";
      if (final)
      {
        out << '\n';
      }
      std::cout << out.str() << std::flush;
    }
  });

  switch (opt.mode)
  {
  case kExportOccupancy:
  case kExportOccupancyCentre:
  case kExportObserved:
  case kExportDensity:
  case kExportClearance:
  case kExportHeightmap:
  case kExportTsdf:
    res = exportPointCloud(opt, prog, load_progress);
    break;
  case kExportHeightmapMesh:
    res = exportHeightmapMesh(opt, prog, load_progress);
    break;
  case kExportCovariance:
    res = exportCovariance(opt, prog, load_progress);
    break;
  }

  prog.joinThread();

  return res;
}


================================================
FILE: utils/ohmcmp/CMakeLists.txt
================================================

find_package(ZLIB)

set(SOURCES
  ohmcmp.cpp
)

add_executable(ohmcmp ${SOURCES})
leak_track_target_enable(ohmcmp CONDITION OHM_LEAK_TRACK)

set_target_properties(ohmcmp PROPERTIES FOLDER utils)
if(MSVC)
  set_target_properties(ohmcmp PROPERTIES DEBUG_POSTFIX "d")
endif(MSVC)

target_link_libraries(ohmcmp
  PUBLIC
    ohm
    ohmutil
  PRIVATE
    glm::glm
    $<BUILD_INTERFACE:ZLIB::ZLIB>
)

clang_tidy_target(ohmcmp)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

install(TARGETS ohmcmp DESTINATION bin)


================================================
FILE: utils/ohmcmp/ohmcmp.cpp
================================================
// Copyright (c) 2021
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

// ohmcmp is an experimental utility for comparing two ohm maps.

#include <glm/glm.hpp>

#include <ohm/CompareMaps.h>
#include <ohm/DefaultLayer.h>
#include <ohm/MapInfo.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/VoxelData.h>
#include <ohm/VoxelLayout.h>

#include <logutil/LogUtil.h>
#include <ohmutil/Options.h>

#include <algorithm>
#include <chrono>
#include <csignal>
#include <cstddef>
#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <locale>
#include <sstream>
#include <unordered_map>
#include <vector>

namespace
{
struct Options
{
  std::string input_map_file;
  std::string ref_map_file;
  std::vector<std::string> layers;
  unsigned verbosity = 1;
  bool compare_layout = false;
  bool compare_voxels = false;
  bool stop_on_error = false;
  bool tolerances = false;
};
}  // namespace


int parseOptions(Options *opt, int argc, char *argv[])  // NOLINT(modernize-avoid-c-arrays)
{
  cxxopts::Options opt_parse(argv[0], "\nAn experimental tool for comparing two ohm maps.\n");
  opt_parse.positional_help("<map.ohm>");

  try
  {
    // clang-format off
    opt_parse.add_options()
      ("help", "Show help.")
      ("input", "The input map file (ohm) to validate.", cxxopts::value(opt->input_map_file))
      ("ref", "The reference map file (ohm) to validate against.", cxxopts::value(opt->ref_map_file))
      ("layers", "List of layers to limit comparison to. Affects layout and voxel comparison.", cxxopts::value(opt->layers))
      ("layout", "Compare map layouts and report differences?", optVal(opt->compare_layout))
      ("stop-on-error", "Stop on the first error?", optVal(opt->stop_on_error))
      ("tolerances", "Allow some error tolerance?.", optVal(opt->tolerances))
      ("verbosity", "Verbosity level [0, 2].", optVal(opt->verbosity))
      ("voxels", "Compare voxel content? Limited to the list of layers specified.", optVal(opt->compare_voxels))
      ;
    // clang-format on

    opt_parse.parse_positional({ "input", "ref" });

    cxxopts::ParseResult parsed = opt_parse.parse(argc, argv);

    if (parsed.count("help") || parsed.arguments().empty())
    {
      // show usage.
      std::cout << opt_parse.help({ "", "Group" }) << std::endl;
      return 1;
    }

    if (opt->input_map_file.empty())
    {
      std::cerr << "Missing input map" << std::endl;
      return -1;
    }

    if (opt->ref_map_file.empty())
    {
      std::cerr << "Missing reference map" << std::endl;
      return -1;
    }
  }
  catch (const cxxopts::OptionException &e)
  {
    std::cerr << "Argument error\n" << e.what() << std::endl;
    return -1;
  }

  return 0;
}

using ToleranceMap = std::unordered_map<std::string, std::shared_ptr<ohm::MapLayer>>;

void buildDefaultLayerTolerances(ToleranceMap &map)
{
  // TODO(KS): validate the default tolerances.
  // Build a map of tolerances for the default layers.

  // Occupancy layer
  std::shared_ptr<ohm::MapLayer> layer = std::make_shared<ohm::MapLayer>(ohm::default_layer::occupancyLayerName());
  ohm::compare::configureTolerance(*layer, "occupancy", 1e2f);
  map.emplace(std::make_pair(std::string(layer->name()), layer));

  // Cannot support a difference in the quantised mean value.
  // // Mean layer
  // layer = std::make_shared<ohm::MapLayer>(ohm::default_layer::meanLayerName());
  // ohm::compare::configureTolerance(*layer, "count", uint32_t(1));
  // map.emplace(std::make_pair(std::string(layer->name()), layer));

  // Mean layer
  layer = std::make_shared<ohm::MapLayer>(ohm::default_layer::covarianceLayerName());
  // Cannot support a difference in the quantised mean value.
  const float p_err = 1e-2f;
  ohm::compare::configureTolerance(*layer, "P00", p_err);
  ohm::compare::configureTolerance(*layer, "P01", p_err);
  ohm::compare::configureTolerance(*layer, "P11", p_err);
  ohm::compare::configureTolerance(*layer, "P02", p_err);
  ohm::compare::configureTolerance(*layer, "P12", p_err);
  ohm::compare::configureTolerance(*layer, "P22", p_err);
  map.emplace(std::make_pair(std::string(layer->name()), layer));

  layer = std::make_shared<ohm::MapLayer>(ohm::default_layer::clearanceLayerName());
  ohm::compare::configureTolerance(*layer, "clearance", 1e-3f);
  map.emplace(std::make_pair(std::string(layer->name()), layer));

  layer = std::make_shared<ohm::MapLayer>(ohm::default_layer::intensityLayerName());
  ohm::compare::configureTolerance(*layer, "mean", 1e-3f);
  ohm::compare::configureTolerance(*layer, "cov", 1e-3f);
  map.emplace(std::make_pair(std::string(layer->name()), layer));

  layer = std::make_shared<ohm::MapLayer>(ohm::default_layer::intensityLayerName());
  ohm::compare::configureTolerance(*layer, "mean", 1e-3f);
  ohm::compare::configureTolerance(*layer, "cov", 1e-3f);
  map.emplace(std::make_pair(std::string(layer->name()), layer));
}

// Extract the list of layers in map
std::vector<std::string> buildLayerList(const ohm::OccupancyMap &map)
{
  std::vector<std::string> layers;

  const auto &layout = map.layout();

  for (size_t i = 0; i < layout.layerCount(); ++i)
  {
    layers.emplace_back(layout.layer(i).name());
  }

  return layers;
}

// Remove items from layers which are not also in filter.
void filterLayers(std::vector<std::string> *layers, const std::vector<std::string> &filter)
{
  for (auto iter = layers->begin(); iter != layers->end(); /*no op*/)
  {
    if (std::find(filter.begin(), filter.end(), *iter) != filter.end())
    {
      // Keep this item.
      ++iter;
    }
    else
    {
      // Remove this item.
      iter = layers->erase(iter);
    }
  }
}

struct OccupancyStats
{
  uint64_t occupied = 0;
  uint64_t free = 0;
};

OccupancyStats calcuateOccupancyStats(const ohm::OccupancyMap &map, const std::string &context)
{
  OccupancyStats stats{};
  ohm::Voxel<const float> occupancy_voxel(&map, map.layout().occupancyLayer());

  if (occupancy_voxel.isLayerValid())
  {
    std::cout << "Counting " << context << " voxels" << std::endl;

    for (auto iter = map.begin(); iter != map.end(); ++iter)
    {
      occupancy_voxel.setKey(iter);
      switch (ohm::occupancyType(occupancy_voxel))
      {
      case ohm::kOccupied:
        ++stats.occupied;
        break;
      case ohm::kFree:
        ++stats.free;
        break;
      default:
        break;
      }
    }
  }

  return stats;
}

int main(int argc, char *argv[])
{
  Options opt;

  std::cout.imbue(std::locale(""));

  int res = 0;
  res = parseOptions(&opt, argc, argv);

  if (res)
  {
    return res;
  }

  ohm::OccupancyMap input_map(1.0f);
  ohm::OccupancyMap ref_map(1.0f);

  ohm::MapVersion input_version{};
  ohm::MapVersion ref_version{};

  size_t input_region_count = 0;
  size_t ref_region_count = 0;

  // Decide whether we need to load the map content, or just the headers.
  if (opt.compare_voxels)
  {
    // Full load.
    std::cout << "Loading input map " << opt.input_map_file << std::endl;
    res = ohm::load(opt.input_map_file, input_map, nullptr, &input_version);
    if (res != 0)
    {
      std::cout << "Loading failed" << std::endl;
      return res;
    }
    std::cout << "Loading reference map " << opt.ref_map_file << std::endl;
    res = ohm::load(opt.ref_map_file, ref_map, nullptr, &ref_version);
    if (res != 0)
    {
      std::cout << "Loading failed" << std::endl;
      return res;
    }

    input_region_count = input_map.regionCount();
    ref_region_count = ref_map.regionCount();
  }
  else
  {
    // Load header only
    std::cout << "Loading input map header " << opt.input_map_file << std::endl;
    res = ohm::loadHeader(opt.input_map_file, input_map, &input_version, &input_region_count);
    if (res != 0)
    {
      std::cout << "Loading failed" << std::endl;
      return res;
    }
    std::cout << "Loading reference map header " << opt.ref_map_file << std::endl;
    res = ohm::loadHeader(opt.ref_map_file, ref_map, &ref_version, &ref_region_count);
    if (res != 0)
    {
      std::cout << "Loading failed" << std::endl;
      return res;
    }
  }


  std::vector<std::string> input_layer_list = buildLayerList(input_map);
  std::vector<std::string> ref_layer_list = buildLayerList(ref_map);

  if (!opt.layers.empty())
  {
    filterLayers(&input_layer_list, opt.layers);
    filterLayers(&ref_layer_list, opt.layers);
  }

  const auto log = +[](ohm::compare::Severity severity, const std::string &msg)  //
  {
    std::ostream *out = &std::cout;
    std::string prefix;
    switch (severity)
    {
    case ohm::compare::Severity::kWarning:
      out = &std::cerr;
      prefix = "warning : ";
      break;
    case ohm::compare::Severity::kError:
      out = &std::cerr;
      prefix = "error : ";
      break;
    default:
      break;
    }

    *out << prefix << msg << std::endl;
  };

  std::array<ohm::compare::Log, 3> logs;
  for (size_t i = 0; i < logs.size(); ++i)
  {
    logs[i] = (opt.verbosity >= i) ? log : ohm::compare::emptyLog;
  }

  if (input_layer_list != ref_layer_list)
  {
    std::ostringstream str;
    str << "Layer mismatch\n";
    str << "  input map:\n";
    for (const auto &layer : input_layer_list)
    {
      str << "    " << layer << '\n';
    }
    str << "  reference map:\n";
    for (const auto &layer : ref_layer_list)
    {
      str << "    " << layer << '\n';
    }
    log(ohm::compare::Severity::kWarning, str.str());
  }

  unsigned compare_flags = 0;
  compare_flags |= ohm::compare::kContinue * !opt.stop_on_error;

  bool ok = true;

  if (opt.compare_layout)
  {
    bool layers_ok = true;
    for (const auto &layer_name : input_layer_list)
    {
      const bool layer_ok = ohm::compare::compareLayoutLayer(input_map, ref_map, layer_name, compare_flags, logs[1]);
      layers_ok = layer_ok && layers_ok;
      std::cout << layer_name << ' ' << (layer_ok ? "ok" : "failed") << std::endl;
    }
    std::cout << "Layers " << (layers_ok ? "ok" : "failed") << std::endl;
    ok = ok && layers_ok;
  }

  if (opt.compare_voxels)
  {
    const OccupancyStats input_stats = calcuateOccupancyStats(input_map, "input");
    std::cout << "  occupied: " << input_stats.occupied << std::endl;
    std::cout << "  free    : " << input_stats.free << std::endl;
    const OccupancyStats ref_stats = calcuateOccupancyStats(ref_map, "ref");
    std::cout << "  occupied: " << ref_stats.occupied << std::endl;
    std::cout << "  free    : " << ref_stats.free << std::endl;
    std::cout << "Comparing layers" << std::endl;

    std::cout << "Comparing voxels" << std::endl;
    bool voxels_ok = true;

    ToleranceMap tolerances;
    if (opt.tolerances)
    {
      buildDefaultLayerTolerances(tolerances);
    }
    for (const auto &layer_name : input_layer_list)
    {
      std::shared_ptr<ohm::MapLayer> tolerance = tolerances[layer_name];

      auto voxel_result =
        ohm::compare::compareVoxels(input_map, ref_map, layer_name, tolerance.get(), compare_flags, logs[2]);
      voxels_ok = voxel_result.layout_match && voxel_result.voxels_failed == 0 && voxels_ok;
      std::cout << layer_name << " layout " << (voxel_result.layout_match ? "ok" : "failed") << " passed "
                << voxel_result.voxels_passed << " failed " << voxel_result.voxels_failed << std::endl;
    }
    std::cout << "Voxels " << (voxels_ok ? "ok" : "failed") << std::endl;
    ok = ok && voxels_ok;
  }

  if (!ok)
  {
    res = 1;
  }

  return res;
}


================================================
FILE: utils/ohmfilter/CMakeLists.txt
================================================

find_package(ZLIB)

set(SOURCES
ohmfilter.cpp
)

add_executable(ohmfilter ${SOURCES})
leak_track_target_enable(ohmfilter CONDITION OHM_LEAK_TRACK)

set_target_properties(ohmfilter PROPERTIES FOLDER utils)
if(MSVC)
  set_target_properties(ohmfilter PROPERTIES DEBUG_POSTFIX "d")
endif(MSVC)

target_link_libraries(ohmfilter
  PUBLIC
    ohm
    ohmutil
    slamio
  PRIVATE
    glm::glm
    $<BUILD_INTERFACE:ZLIB::ZLIB>
)

clang_tidy_target(ohmfilter)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

install(TARGETS ohmfilter DESTINATION bin)


================================================
FILE: utils/ohmfilter/ohmfilter.cpp
================================================
//
// author Kazys Stepanas
//
#include <glm/glm.hpp>

#include <ohm/CovarianceVoxel.h>
#include <ohm/Key.h>
#include <ohm/MapLayout.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/VoxelData.h>

#include <ohmutil/PlyPointStream.h>
#include <ohmutil/ProgressMonitor.h>

#include <slamio/SlamCloudLoader.h>

#include <glm/gtc/matrix_access.hpp>

#include <csignal>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>

namespace
{
int g_quit = 0;

void onSignal(int arg)
{
  if (arg == SIGINT || arg == SIGTERM)
  {
    ++g_quit;
  }
}

using FilterFunction = std::function<bool(double, const glm::dvec3 &, const ohm::Key &)>;

struct Options
{
  std::string map_file;
  std::string cloud_in;
  std::string traj_in;
  std::string cloud_out;
  double expected_value_tolerance = -1;
  bool occupancy_only = false;
  bool quiet = false;
};

class LoadMapProgress : public ohm::SerialiseProgress
{
public:
  explicit LoadMapProgress(ProgressMonitor &monitor)
    : monitor_(monitor)
  {}

  bool quit() const override { return ::g_quit > 1; }

  void setTargetProgress(unsigned target) override { monitor_.beginProgress(ProgressMonitor::Info(target)); }
  void incrementProgress(unsigned inc) override { monitor_.incrementProgressBy(inc); }

private:
  ProgressMonitor &monitor_;
};

bool filterPointByCovariance(const glm::dvec3 &point, const glm::dvec3 &mean, const glm::dmat3 &cov_sqrt,
                             double threshold)
{
  // Transient point assessement vs NDT:
  // With:
  //  P:covariance
  //  S:covariance square root
  //  a:test value
  // a = (point - mean)^T P^{-1} (point - mean)
  // where P=covariance
  // expected value is dimensionality (3)
  // so to pass, we need a < 3.
  //
  // Using CovarianceVoxel we have the square-root of P:
  // P = S S^T
  // so:
  // v = S^{-1} (point - mean)
  // a = v^T v

  const glm::dvec3 divergence_from_mean = point - mean;
  const auto v = glm::inverse(cov_sqrt) * divergence_from_mean;
  const double value = glm::dot(v, v);
  const double expected_value = 3.0;  // In R3 => expected value is 3.
  return std::abs(value) < expected_value + threshold;
}
}  // namespace

// Must be after argument streaming operators.
#include <ohmutil/Options.h>

int parseOptions(Options *opt, int argc, char *argv[])  // NOLINT(modernize-avoid-c-arrays)
{
  cxxopts::Options opt_parse(argv[0], "Convert an occupancy map to a point cloud. Defaults to generate a positional "
                                      "point cloud, but can generate a clearance cloud as well.");
  opt_parse.positional_help("<map.ohm> <cloud.ply>");

  try
  {
    // Build GPU options set.
    // clang-format off
    opt_parse.add_options()
      ("help", "Show help.")
      ("map", "The input map file (ohm).", cxxopts::value(opt->map_file))
      ("cloud-in", "The input cloud file.", cxxopts::value(opt->cloud_in))
      ("traj", "Optional trajectory file input.", cxxopts::value(opt->traj_in))
      ("cloud-out", "The output cloud file (ply).", cxxopts::value(opt->cloud_out))
      ("occupancy-only", "Force using only occupancy information, even if NDT is present.", optVal(opt->occupancy_only))
      ("tolerance", "Additional tolerance above the expected value. A zero value enabled the direct comparison of each "
                    "point against it's voxel's covariance while a positive value expands the tolerance beyond the "
                    "expected value. A negative value disables this check.",
        cxxopts::value(opt->expected_value_tolerance))
      ("quiet", "Limited log output.", optVal(opt->quiet))
      ;
    // clang-format on

    opt_parse.parse_positional({ "map", "cloud-in", "cloud-out" });

    cxxopts::ParseResult parsed = opt_parse.parse(argc, argv);

    if (parsed.count("help") || parsed.arguments().empty())
    {
      // show usage.
      std::cout << opt_parse.help({ "" }) << std::endl;
      return 1;
    }

    if (opt->map_file.empty())
    {
      std::cerr << "Missing input map file name" << std::endl;
      return -1;
    }
    if (opt->cloud_in.empty())
    {
      std::cerr << "Missing input cloud file name" << std::endl;
      return -1;
    }
    if (opt->cloud_out.empty())
    {
      std::cerr << "Missing output file name" << std::endl;
      return -1;
    }
  }
  catch (const cxxopts::OptionException &e)
  {
    std::cerr << "Argument error\n" << e.what() << std::endl;
    return -1;
  }

  return 0;
}

bool filterCloud(const Options &opt, const ohm::OccupancyMap &map, ProgressMonitor *prog)
{
  // Use the SlamCloudLoader with no trajectory specified to load the cloud - it's just convenient.
  slamio::SlamCloudLoader cloud_loader;
  cloud_loader.setErrorLog([](const char *msg) { std::cerr << msg << std::flush; });

  if (!cloud_loader.openWithTrajectory(opt.cloud_in.c_str(), opt.traj_in.c_str()))
  {
    std::cerr << "Error: Unable to load cloud file " << opt.cloud_in << std::endl;
    return false;
  }

  prog->beginProgress(ProgressMonitor::Info("filtering", cloud_loader.numberOfPoints()));
  prog->unpause();

  // Prepare required data layers.
  ohm::Voxel<const ohm::CovarianceVoxel> voxel_cov(&map, map.layout().covarianceLayer());
  ohm::Voxel<const ohm::VoxelMean> voxel_mean(&map, map.layout().meanLayer());
  ohm::Voxel<const float> voxel_occ(&map, map.layout().occupancyLayer());

  if (!voxel_occ.isLayerValid())
  {
    std::cerr << "Error: Map missing occupancy layer" << std::endl;
    return false;
  }

  // Select the filter function depending on available layers.
  FilterFunction filter;
  glm::dmat3 cov_sqrt{};
  if (!opt.occupancy_only && voxel_cov.isLayerValid() && voxel_mean.isLayerValid())
  {
    if (!opt.quiet)
    {
      std::cout << "Filtering with NDT information" << std::endl;
    }
    filter = [&](double /* timestamp */, const glm::dvec3 &point, const ohm::Key &key) -> bool {
      ohm::setVoxelKey(key, voxel_occ, voxel_mean, voxel_cov);
      if (ohm::isOccupied(voxel_occ))
      {
        const glm::dvec3 mean = ohm::positionUnsafe(voxel_mean);
        ohm::CovarianceVoxel cov_data;
        voxel_cov.read(&cov_data);
        cov_sqrt = ohm::covarianceSqrtMatrix(&cov_data);
        if (opt.expected_value_tolerance >= 0)
        {
          return filterPointByCovariance(point, mean, cov_sqrt, opt.expected_value_tolerance);
        }
        return true;
      }

      return false;
    };
  }
  else
  {
    if (!opt.quiet)
    {
      std::cout << "Filtering using occupancy only" << std::endl;
    }
    filter = [&](double /* timestamp */, const glm::dvec3 & /* point */, const ohm::Key &key) -> bool {
      voxel_occ.setKey(key);
      return ohm::isOccupied(voxel_occ);
    };
  }

  std::ofstream out(opt.cloud_out.c_str(), std::ios::binary);
  using Property = ohm::PlyPointStream::Property;
  using Type = ohm::PlyPointStream::Type;
  ohm::PlyPointStream ply({ Property{ "x", Type::kFloat64 }, Property{ "y", Type::kFloat64 },
                            Property{ "z", Type::kFloat64 }, Property{ "time", Type::kFloat64 } },
                          out);

  if (!ply.isOpen())
  {
    std::cerr << "Error: Unable to write to " << opt.cloud_out << std::endl;
    return false;
  }
  if (!opt.quiet)
  {
    std::cout << "Exporting to " << opt.cloud_out << std::endl;
  }
  ohm::Key key;
  std::uint64_t point_count = 0;
  std::uint64_t export_count = 0;
  const bool with_trajectory = !opt.traj_in.empty();
  slamio::SamplePoint sample{};
  while (cloud_loader.nextSample(sample))
  {
    key = map.voxelKey(sample.sample);
    if (filter(sample.timestamp, sample.sample, key))
    {
      ply.setPointPosition(sample.sample);
      if (with_trajectory)
      {
        ply.setPointNormal(sample.sample - sample.origin);
      }
      ply.setPointTimestamp(sample.timestamp);
      ply.writePoint();
      ++export_count;
    }
    prog->incrementProgress();
    ++point_count;
  }
  prog->endProgress();
  prog->pause();
  if (!opt.quiet)
  {
    std::cout << std::endl;
  }

  cloud_loader.close();
  ply.close();
  out.close();

  if (!opt.quiet)
  {
    std::cout << "Removed: " << point_count - export_count << std::endl;
    std::cout << "Exported: " << export_count << "/" << point_count << std::endl;
  }

  return true;
}


int main(int argc, char *argv[])
{
  Options opt;
  std::cout.imbue(std::locale(""));

  int res = parseOptions(&opt, argc, argv);

  if (res)
  {
    return res;
  }

  signal(SIGINT, onSignal);
  signal(SIGTERM, onSignal);

  if (!opt.quiet)
  {
    std::cout << "Loading map " << opt.map_file.c_str() << std::endl;
  }
  ProgressMonitor prog(10);
  LoadMapProgress load_progress(prog);
  if (!opt.quiet)
  {
    prog.startThread(true);
  }

  prog.setDisplayFunction([&opt](const ProgressMonitor::Progress &prog, bool final) {
    if (!opt.quiet)
    {
      std::ostringstream out;
      out.imbue(std::locale(""));
      out << '\r';

      if (!prog.info.info.empty())
      {
        out << prog.info.info << " : ";
      }

      const auto fill_width = std::numeric_limits<decltype(prog.progress)>::digits10;
      out << std::setfill(' ') << std::setw(fill_width) << prog.progress;
      if (prog.info.total)
      {
        out << " / " << std::setfill(' ') << std::setw(fill_width) << prog.info.total;
      }
      out << "    ";
      if (final)
      {
        out << '\n';
      }
      std::cout << out.str() << std::flush;
    }
  });

  ohm::OccupancyMap map;
  res = ohm::load(opt.map_file.c_str(), map, &load_progress);

  prog.endProgress();
  prog.pause();
  if (!opt.quiet)
  {
    std::cout << std::endl;
  }

  if (res == 0)
  {
    if (!filterCloud(opt, map, &prog))
    {
      res = -1;
    }
  }
  else
  {
    std::cerr << "Error: Unable to read map '" << opt.map_file << "' : " << ohm::serialiseErrorCodeString(res)
              << std::endl;
  }

  prog.joinThread();

  return res;
}


================================================
FILE: utils/ohmheightmap/CMakeLists.txt
================================================
find_package(ZLIB)

set(SOURCES
  ohmheightmap.cpp
)

# Can't add as ohmheightmap as the library target is called that.
# Use ohmheightmaputil then change the output name.
add_executable(ohmheightmaputil ${SOURCES})
leak_track_target_enable(ohmheightmaputil CONDITION OHM_LEAK_TRACK)

set_target_properties(ohmheightmaputil PROPERTIES OUTPUT_NAME ohmheightmap)

set_target_properties(ohmheightmaputil PROPERTIES FOLDER utils)
if(MSVC)
  set_target_properties(ohmheightmaputil PROPERTIES DEBUG_POSTFIX "d")
endif(MSVC)

target_link_libraries(ohmheightmaputil
  PUBLIC
    ohmheightmap
    ohmtools
    ohmutil
  PRIVATE
    glm::glm
    $<BUILD_INTERFACE:ZLIB::ZLIB>
)

target_include_directories(ohmheightmaputil
  SYSTEM PRIVATE
    $<$<NOT:$<BOOL:${OHM_TES_DEBUG}>>:${3ES_INCLUDE_DIRS}>
)

clang_tidy_target(ohmheightmaputil)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

install(TARGETS ohmheightmaputil DESTINATION bin)


================================================
FILE: utils/ohmheightmap/ohmheightmap.cpp
================================================
//
// author Kazys Stepanas
//
// Utility for generating an ohm heightmap from an ohm occupancy map.

#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/Trace.h>
#include <ohm/Voxel.h>

#include <ohmheightmap/Heightmap.h>
#include <ohmheightmap/HeightmapVoxel.h>

#include <logutil/LogUtil.h>
#include <ohmutil/GlmStream.h>
#include <ohmutil/ProgressMonitor.h>
#include <ohmutil/SafeIO.h>
#include <ohmutil/ScopedTimeDisplay.h>

#include <glm/glm.hpp>

#include <algorithm>
#include <chrono>
#include <csignal>
#include <cstddef>
#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <locale>
#include <sstream>
#include <unordered_set>

#include <3esservermacros.h>

/// Declare a function for throwing exceptions for bad arguments. The exception isn't included yet due to cxxoptions
/// oddities.
void badArgParse(const std::string &info);

namespace ohm
{
// Custom option parsing. Must come before we include Options.h/cxxopt.hpp
std::istream &operator>>(std::istream &in, HeightmapMode &mode)
{
  std::string mode_str;
  in >> mode_str;
  bool valid_mode = false;
  mode = heightmapModeFromString(mode_str, &valid_mode);
  if (!valid_mode)
  {
    badArgParse(mode_str);
  }
  return in;
}

std::ostream &operator<<(std::ostream &out, const HeightmapMode mode)
{
  std::string mode_str = heightmapModeToString(mode);
  out << mode_str;
  return out;
}


std::istream &operator>>(std::istream &in, UpAxis &up)
{
  std::string up_str;
  in >> up_str;
  if (up_str == "x")
  {
    up = UpAxis::kX;
  }
  else if (up_str == "y")
  {
    up = UpAxis::kY;
  }
  else if (up_str == "z")
  {
    up = UpAxis::kZ;
  }
  else if (up_str == "-x")
  {
    up = UpAxis::kNegX;
  }
  else if (up_str == "-y")
  {
    up = UpAxis::kNegY;
  }
  else if (up_str == "-z")
  {
    up = UpAxis::kNegZ;
  }
  else
  {
    badArgParse(up_str);
  }
  return in;
}

std::ostream &operator<<(std::ostream &out, const UpAxis up)
{
  switch (up)
  {
  case UpAxis::kX:
    out << "x";
    break;
  case UpAxis::kY:
    out << "y";
    break;
  case UpAxis::kZ:
    out << "z";
    break;
  case UpAxis::kNegX:
    out << "-x";
    break;
  case UpAxis::kNegY:
    out << "-y";
    break;
  case UpAxis::kNegZ:
    out << "-z";
    break;
  default:
    out << "<unknown>";
    break;
  }
  return out;
}
}  // namespace ohm

// Must be after argument streaming operators.
#include <ohmutil/Options.h>

void badArgParse(const std::string &info)
{
  throw cxxopts::invalid_option_format_error(info);
}


namespace
{
using Clock = std::chrono::high_resolution_clock;

int g_quit = 0;

void onSignal(int arg)
{
  if (arg == SIGINT || arg == SIGTERM)
  {
    ++g_quit;
  }
}

struct Options
{
  std::string map_file;
  std::string heightmap_file;
  std::string trace;  // 3es trace file (if available)
  ohm::UpAxis axis_id = ohm::UpAxis::kZ;
  ohm::HeightmapMode mode = ohm::HeightmapMode::kSimpleFill;
  glm::dvec3 seed_pos{ 0, 0, 0 };
  double clearance = 2.0;
  double floor = -1;
  double ceiling = -1;
  unsigned virtual_surface_filter_threshold = 0;
  bool virtual_surfaces = false;
  bool no_voxel_mean = false;
};


class LoadMapProgress : public ohm::SerialiseProgress
{
public:
  explicit LoadMapProgress(ProgressMonitor &monitor)
    : monitor_(monitor)
  {}

  bool quit() const override { return ::g_quit > 1; }

  void setTargetProgress(unsigned target) override { monitor_.beginProgress(ProgressMonitor::Info(target)); }
  void incrementProgress(unsigned inc) override { monitor_.incrementProgressBy(inc); }

private:
  ProgressMonitor &monitor_;
};
}  // namespace


int parseOptions(Options *opt, int argc, char *argv[])  // NOLINT(modernize-avoid-c-arrays)
{
  cxxopts::Options opt_parse(argv[0], "\nCreate a heightmap from an occupancy map.\n");
  opt_parse.positional_help("<map.ohm> <heightmap.ohm>");

  try
  {
    std::ostringstream mode_help;
    mode_help << "Heightmap expansion mode (";
    std::string append_str = "";
    for (ohm::HeightmapMode mode = ohm::HeightmapMode::kFirst; mode <= ohm::HeightmapMode::kLast;
         mode = ohm::HeightmapMode(int(mode) + 1))
    {
      mode_help << append_str << ohm::heightmapModeToString(mode);
      append_str = ", ";
    }
    mode_help << ").";

    opt_parse.add_options()("help", "Show help.")("i", "The input map file (ohm).", cxxopts::value(opt->map_file))  //
      ("o", "The output heightmap file (ohm).", cxxopts::value(opt->heightmap_file))                                //
      ("ceiling",
       "Ceiling applied to ground voxel searches. Limits how far up to search. Non-negative to enable. Affected by "
       "'--mode'.",
       optVal(opt->ceiling))                                                                                 //
      ("clearance", "The required height clearance for a heightmap surface voxel.", optVal(opt->clearance))  //
      ("floor",
       "Floor applied to ground voxel searches. Limits how far down to search. Non-negative to enable. Affected by "
       "'--mode'.",
       optVal(opt->floor))                                                                           //
      ("mode", mode_help.str(), optVal(opt->mode))                                                   //
      ("no-voxel-mean", "Ignore voxel mean positioning if available?.", optVal(opt->no_voxel_mean))  //
      ("seed", "Seed position from which to build the heightmap. Specified as a 3 component vector such as '0,0,1'.",
       optVal(opt->seed_pos))                                                        //
      ("up", "Specifies the up axis {x,y,z,-x,-y,-z}.", optVal(opt->axis_id))        //
      ("virtual", "Allow virtual surfaces?", cxxopts::value(opt->virtual_surfaces))  //
      ("virtual-filter-threshold",
       "Remove virtual surface voxels with fewer than this number of occupied neighbours. Layered modes only.",
       optVal(opt->virtual_surface_filter_threshold))  //
      ;

    if (ohm::trace::available())
    {
      opt_parse.add_options()(
        "trace", "Enable debug tracing to the given file name to generate a 3es file. High performance impact.",
        cxxopts::value(opt->trace));
    }

    opt_parse.parse_positional({ "i", "o" });

    cxxopts::ParseResult parsed = opt_parse.parse(argc, argv);

    if (parsed.count("help") || parsed.arguments().empty())
    {
      // show usage.
      std::cout << opt_parse.help({ "", "Group" }) << std::endl;
      return 1;
    }

    if (opt->map_file.empty())
    {
      std::cerr << "Missing input map" << std::endl;
      return -1;
    }

    if (opt->heightmap_file.empty())
    {
      std::cerr << "Missing output name" << std::endl;
      return -1;
    }
  }
  catch (const cxxopts::OptionException &e)
  {
    std::cerr << "Argument error\n" << e.what() << std::endl;
    return -1;
  }

  return 0;
}

int main(int argc, char *argv[])
{
  const auto start_time = Clock::now();
  Options opt;

  std::cout.imbue(std::locale(""));

  int res = 0;
  res = parseOptions(&opt, argc, argv);

  if (res)
  {
    return res;
  }

  // Initialise 3ES
  std::unique_ptr<ohm::Trace> trace;
  if (!opt.trace.empty())
  {
    trace = std::make_unique<ohm::Trace>(opt.trace.c_str());
  }

  signal(SIGINT, onSignal);
  signal(SIGTERM, onSignal);

  printf("Loading map %s\n", opt.map_file.c_str());
  ProgressMonitor prog(10);
  LoadMapProgress load_progress(prog);
  ohm::OccupancyMap map(1.0);
  ohm::MapVersion version;

  prog.setDisplayFunction([](const ProgressMonitor::Progress &prog, bool final) {
    std::ostringstream str;
    str << '\r';
    str << prog.progress;
    if (prog.info.total)
    {
      str << " / " << prog.info.total;
    }
    str << "      ";
    if (final)
    {
      str << '\n';
    }
    std::cout << str.str() << std::flush;
  });

  prog.startThread();
  res = ohm::load(opt.map_file.c_str(), map, &load_progress, &version);
  prog.endProgress();

  std::cout << std::endl;

  if (res != 0)
  {
    std::cerr << "Failed to load map. Error(" << res << "): " << ohm::serialiseErrorCodeString(res) << std::endl;
    return res;
  }

  std::cout << "Loaded in " << (Clock::now() - start_time) << std::endl;


  std::cout << "Generating heightmap" << std::endl;

  const auto heightmap_start_time = Clock::now();

  ohm::Heightmap heightmap(map.resolution(), opt.clearance, opt.axis_id);
  heightmap.setMode(opt.mode);
  heightmap.setOccupancyMap(&map);
  heightmap.heightmap().setOrigin(map.origin());

  heightmap.setCeiling(opt.ceiling >= 0 ? opt.ceiling : heightmap.ceiling());
  heightmap.setFloor(opt.floor >= 0 ? opt.floor : heightmap.floor());
  heightmap.setIgnoreVoxelMean(opt.no_voxel_mean);
  heightmap.setGenerateVirtualSurface(opt.virtual_surfaces);
  heightmap.setVirtualSurfaceFilterThreshold(opt.virtual_surface_filter_threshold);

  heightmap.buildHeightmap(opt.seed_pos);
  heightmap.checkForBaseLayerDuplicates(std::cerr);

  const auto heightmap_end_time = Clock::now();

  std::cout << "Heightmap generated in " << (heightmap_end_time - heightmap_start_time) << std::endl;

  std::cout << "Saving " << opt.heightmap_file << std::endl;
  ohm::save(opt.heightmap_file.c_str(), heightmap.heightmap(), nullptr);

  return res;
}


================================================
FILE: utils/ohmhm2img/CMakeLists.txt
================================================
find_package(PNG REQUIRED)
find_package(ZLIB)

set(SOURCES
  ohmhm2img.cpp
)

add_executable(ohmhm2img ${SOURCES})
leak_track_target_enable(ohmhm2img CONDITION OHM_LEAK_TRACK)

set_target_properties(ohmhm2img PROPERTIES FOLDER utils)
if(MSVC)
  set_target_properties(ohmhm2img PROPERTIES DEBUG_POSTFIX "d")
endif(MSVC)

target_link_libraries(ohmhm2img
  PUBLIC
    ohm
    ohmheightmap
    ohmheightmapimage
  PRIVATE
    glm::glm
    $<BUILD_INTERFACE:PNG::PNG>
    $<BUILD_INTERFACE:ZLIB::ZLIB>
)

clang_tidy_target(ohmhm2img)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

install(TARGETS ohmhm2img DESTINATION bin)


================================================
FILE: utils/ohmhm2img/ohmhm2img.cpp
================================================
//
// author Kazys Stepanas
//
// Utility for generating an image from an ohm heightmap.

#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/Voxel.h>

#include <ohmheightmap/Heightmap.h>
#include <ohmheightmap/HeightmapMesh.h>
#include <ohmheightmap/HeightmapSerialise.h>
#include <ohmheightmap/HeightmapVoxel.h>

#include <ohmheightmapimage/HeightmapImage.h>

#include <logutil/LogUtil.h>
#include <ohmutil/ProgressMonitor.h>
#include <ohmutil/SafeIO.h>
#include <ohmutil/ScopedTimeDisplay.h>

#include <png.h>

#include <algorithm>
#include <chrono>
#include <csignal>
#include <cstddef>
#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <locale>
#include <sstream>


namespace
{
int g_quit = 0;

void onSignal(int arg)
{
  if (arg == SIGINT || arg == SIGTERM)
  {
    ++g_quit;
  }
}

enum ExportMode
{
  kNormals16 = ohm::HeightmapImage::kImageNormals,
  kNormals8 = ohm::HeightmapImage::kImageNormals888,
  kHeights = ohm::HeightmapImage::kImageHeights,
  kTraversability
};

enum ExportImageType
{
  kExportError = -1,
  kExportRGB8,
  kExportRGB16,
  kExportGrey8,
  kExportGrey16
};

struct Options
{
  std::string map_file;
  std::string image_file;
  ExportMode image_mode = kNormals16;
  ohm::HeightmapMesh::NormalsMode normals_mode = ohm::HeightmapMesh::kNormalsAverage;
  double traverse_angle = 45.0;  // NOLINT(readability-magic-numbers)

  ohm::HeightmapImage::ImageType imageType() const
  {
    switch (image_mode)
    {
    case kNormals16:
      return ohm::HeightmapImage::kImageNormals;
    case kNormals8:
      return ohm::HeightmapImage::kImageNormals888;
    case kHeights:
      return ohm::HeightmapImage::kImageHeights;
    case kTraversability:
      return ohm::HeightmapImage::kImageNormals;
    default:
      break;
    }

    return ohm::HeightmapImage::kImageNormals;
  }
};


class LoadMapProgress : public ohm::SerialiseProgress
{
public:
  explicit LoadMapProgress(ProgressMonitor &monitor)
    : monitor_(monitor)
  {}

  bool quit() const override { return ::g_quit > 1; }

  void setTargetProgress(unsigned target) override { monitor_.beginProgress(ProgressMonitor::Info(target)); }
  void incrementProgress(unsigned inc) override { monitor_.incrementProgressBy(inc); }

private:
  ProgressMonitor &monitor_;
};

ExportImageType convertImage(std::vector<uint8_t> &export_pixels, const uint8_t *raw,
                             const ohm::HeightmapImage::BitmapInfo &info, const Options &opt)
{
  if (info.type == ohm::HeightmapImage::kImageVertexColours888)
  {
    export_pixels.resize(size_t(info.image_width) * size_t(info.image_height) * 3u);
    memcpy(export_pixels.data(), raw, export_pixels.size());
    return kExportRGB8;
  }

  if (opt.image_mode == kNormals16 && info.type == ohm::HeightmapImage::kImageNormals)
  {
    // Need to convert float colour to u16
    export_pixels.clear();
    export_pixels.reserve(info.image_width * info.image_height * 3 * sizeof(uint16_t));

    float red;
    float green;
    float blue;
    uint16_t red16;
    uint16_t green16;
    uint16_t blue16;

    const auto convert_colour = [](float c) -> uint16_t {
      return uint16_t(c * float(std::numeric_limits<uint16_t>::max()));
    };

    const auto push_channel = [](std::vector<uint8_t> &out, uint16_t c) {
      const size_t insert_index = out.size();
      // Push the stride in bytes.
      for (size_t i = 0; i < sizeof(uint16_t); ++i)
      {
        out.push_back(0);
      }

      // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
      *reinterpret_cast<uint16_t *>(&out[insert_index]) = c;
    };

    for (size_t i = 0; i < size_t(info.image_width) * size_t(info.image_height) * size_t(info.bpp);
         i += 3 * sizeof(float))
    {
      // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
      red = *reinterpret_cast<const float *>(raw + i);
      // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
      green = *reinterpret_cast<const float *>(raw + i + sizeof(float));
      // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
      blue = *reinterpret_cast<const float *>(raw + i + 2 * sizeof(float));

      red16 = convert_colour(red);
      green16 = convert_colour(green);
      blue16 = convert_colour(blue);

      // No data: black
      if (red * red + green * green + blue * blue < 0.5f)
      {
        red16 = green16 = blue16 = 0;
      }

      push_channel(export_pixels, red16);
      push_channel(export_pixels, green16);
      push_channel(export_pixels, blue16);
    }

    return kExportRGB16;
  }

  if (opt.image_mode == kNormals8 && info.type == ohm::HeightmapImage::kImageNormals888)
  {
    export_pixels.resize(size_t(info.image_width) * size_t(info.image_height) * 3u);
    memcpy(export_pixels.data(), raw, export_pixels.size());
    return kExportRGB8;
  }

  if (opt.image_mode == kHeights && info.bpp == sizeof(float))
  {
    export_pixels.resize(info.image_width * info.image_height * sizeof(uint16_t));
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    const auto *depth_pixels = reinterpret_cast<const float *>(raw);
    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
    auto *depth_out = reinterpret_cast<uint16_t *>(export_pixels.data());

    for (size_t i = 0; i < size_t(info.image_width) * size_t(info.image_height); ++i)
    {
      depth_out[i] = uint16_t(1.0f - depth_pixels[i] * float(std::numeric_limits<uint16_t>::max()));
    }

    return kExportGrey16;
  }

  if (opt.image_mode == kTraversability && info.type == ohm::HeightmapImage::kImageNormals)
  {
    static_assert(sizeof(glm::vec3) == sizeof(float) * 3, "glm::vec3 mismatch");
    export_pixels.resize(size_t(info.image_width) * size_t(info.image_height));

    const uint8_t c_unknown = 127u;
    const uint8_t c_blocked = 0u;
    const uint8_t c_free = 255u;
    glm::vec3 normal{};
    const glm::vec3 flat(0, 0, 1);
    float dot;
    const auto free_threshold = float(std::cos(M_PI * opt.traverse_angle / 180.0));

    for (size_t i = 0; i < size_t(info.image_width) * size_t(info.image_height); ++i)
    {
      memcpy(&normal, &raw[i * sizeof(normal)], sizeof(normal));
      if (glm::dot(normal, normal) > 0.5f * 0.5f)
      {
        normal = 2.0f * normal - glm::vec3(1.0f);
        normal = glm::normalize(normal);
        dot = glm::dot(normal, flat);
        if (dot >= free_threshold)
        {
          export_pixels[i] = c_free;
        }
        else
        {
          export_pixels[i] = c_blocked;
        }
      }
      else
      {
        // No data.
        export_pixels[i] = c_unknown;
      }
    }

    return kExportGrey8;
  }

  return kExportError;
}


bool savePng(const char *filename, const std::vector<uint8_t> &raw, ExportImageType type, unsigned w, unsigned h)
{
  png_image image;

  // Initialize the 'png_image' structure.
  memset(&image, 0, sizeof(image));
  image.version = PNG_IMAGE_VERSION;
  image.width = int(w);
  image.height = int(h);
  image.flags = 0;
  image.colormap_entries = 0;

  int row_stride = int(w);
  switch (type)
  {
  case kExportRGB8:
    image.format = PNG_FORMAT_RGB;
    row_stride = -int(w * 3);
    break;
  case kExportRGB16:
    image.format = PNG_FORMAT_RGB | PNG_IMAGE_FLAG_16BIT_sRGB;  // NOLINT(hicpp-signed-bitwise)
    row_stride = -int(w * 3);
    break;
  case kExportGrey8:
    image.format = PNG_FORMAT_GRAY;
    row_stride = -int(w);
    break;
  case kExportGrey16:
    image.format = PNG_FORMAT_LINEAR_Y;
    row_stride = -int(w);
    break;
  default:
    image.format = PNG_FORMAT_GRAY;
    row_stride = -int(w);
    break;
  }

  // Negative row stride to flip the image.
  return png_image_write_to_file(&image, filename, false,  // convert_to_8bit,
                                 raw.data(),
                                 row_stride,  // row_stride
                                 nullptr      // colormap
  );
}
}  // namespace


// Custom option parsing. Must come before we include Options.h/cxxopt.hpp
std::istream &operator>>(std::istream &in, ExportMode &mode)
{
  std::string mode_str;
  in >> mode_str;
  if (mode_str == "norm8")
  {
    mode = kNormals8;
  }
  else if (mode_str == "norm16")
  {
    mode = kNormals16;
  }
  else if (mode_str == "height")
  {
    mode = kHeights;
  }
  else if (mode_str == "traverse")
  {
    mode = kTraversability;
  }
  // else
  // {
  //   throw cxxopts::invalid_option_format_error(modeStr);
  // }
  return in;
}

std::ostream &operator<<(std::ostream &out, const ExportMode mode)
{
  switch (mode)
  {
  case kNormals8:
    out << "norm8";
    break;
  case kNormals16:
    out << "norm16";
    break;
  case kHeights:
    out << "height";
    break;
  case kTraversability:
    out << "traverse";
    break;
  }
  return out;
}

namespace ohm
{
std::istream &operator>>(std::istream &in, HeightmapMesh::NormalsMode &mode)
{
  std::string mode_str;
  in >> mode_str;
  if (mode_str == "average" || mode_str == "avg")
  {
    mode = HeightmapMesh::kNormalsAverage;
  }
  else if (mode_str == "worst")
  {
    mode = HeightmapMesh::kNormalsWorst;
  }
  // else
  // {
  //   throw cxxopts::invalid_option_format_error(modeStr);
  // }
  return in;
}

std::ostream &operator<<(std::ostream &out, const HeightmapMesh::NormalsMode mode)
{
  switch (mode)
  {
  case HeightmapMesh::kNormalsAverage:
    out << "average";
    break;
  case HeightmapMesh::kNormalsWorst:
    out << "worst";
    break;
  }
  return out;
}
}  // namespace ohm

// Must be after argument streaming operators.
#include <ohmutil/Options.h>

int parseOptions(Options *opt, int argc, char *argv[])  // NOLINT(modernize-avoid-c-arrays)
{
  cxxopts::Options opt_parse(argv[0], "\nCreate a heightmap from an occupancy map.\n");
  opt_parse.positional_help("<heightmap.ohm> <image.png>");

  try
  {
    opt_parse.add_options()("help", "Show help.")                                       //
      ("i", "The input heightmap file (ohm).", cxxopts::value(opt->map_file))           //
      ("o", "The output heightmap image file (png).", cxxopts::value(opt->image_file))  //
      ("m,mode",
       "The image output mode [norm8, norm16, height, traverse]. norm8 exports a normal map image with 8 bits per "
       "pixel. norm16 "
       "uses 16 bits per pixel. height is a greyscale image where the colour is the relative heights. traverse colours "
       "by traversability black (non-traversable), white (traversable), grey (unknown) based on the --traverse-angle "
       "argument.",
       cxxopts::value(opt->image_mode)->default_value(optStr(opt->image_mode)))  //
      ("traverse-angle", "The maximum traversable angle (degrees) for use with mode=traverse.",
       cxxopts::value(opt->traverse_angle)->default_value(optStr(opt->traverse_angle)))  //
      ("normals",
       "Defines how vertex normals are calculated: [average/avg, worst]. average averages triangle normals, worst "
       "selects the least horizontal triangle normal for a vertex.",
       cxxopts::value(opt->normals_mode)->default_value(optStr(opt->normals_mode)))  //
      ;

    opt_parse.parse_positional({ "i", "o" });

    cxxopts::ParseResult parsed = opt_parse.parse(argc, argv);

    if (parsed.count("help") || parsed.arguments().empty())
    {
      // show usage.
      std::cout << opt_parse.help({ "", "Group" }) << std::endl;
      return 1;
    }

    if (opt->map_file.empty())
    {
      std::cerr << "Missing input map" << std::endl;
      return -1;
    }

    if (opt->image_file.empty())
    {
      std::cerr << "Missing output name" << std::endl;
      return -1;
    }
  }
  catch (const cxxopts::OptionException &e)
  {
    std::cerr << "Argument error\n" << e.what() << std::endl;
    return -1;
  }

  return 0;
}


std::string generateYamlName(const std::string &image_file)
{
  // Find replace the extension with .yaml
  auto last_dot_pos = image_file.find_last_of('.');
  std::string yaml_name;
  if (last_dot_pos != std::string::npos)
  {
    yaml_name = image_file.substr(0, last_dot_pos);
  }
  else
  {
    yaml_name = image_file;
  }

  yaml_name += ".yaml";
  return yaml_name;
}


bool saveMetaData(const std::string &yaml_file, const Options &opt, ohm::Heightmap &heightmap,
                  const ohm::HeightmapImage::BitmapInfo &info, ExportImageType image_format)
{
  std::ofstream out(yaml_file.c_str());

  if (!out.is_open())
  {
    return false;
  }

  // Set high precision output.
  out << std::setprecision(std::numeric_limits<double>::max_digits10);

  int white_colour = std::numeric_limits<uint8_t>::max();
  const int black_colour = 0;

  // Path to the heightmap image.
  out << "image: " << opt.image_file << '\n';
  // Describe how the image was generated.
  out << "image_mode: " << opt.image_mode << "\n";
  out << "image_format: ";
  switch (image_format)
  {
  case kExportError:
    out << "error";
    break;
  case kExportRGB8:
    white_colour = std::numeric_limits<uint8_t>::max();
    out << "RGB8";
    break;
  case kExportRGB16:
    white_colour = std::numeric_limits<uint16_t>::max();
    out << "RGB16";
    break;
  case kExportGrey8:
    white_colour = std::numeric_limits<uint8_t>::max();
    out << "mono8";
    break;
  case kExportGrey16:
    white_colour = std::numeric_limits<uint16_t>::max();
    out << "mono16";
    break;
  default:
    out << "unknown";
    break;
  }
  out << "\n";

  // Convert the bitmap image global min extents into a 2D value.
  glm::dvec3 min_ext_2d =
    info.image_extents.minExtents() - glm::dot(heightmap.upAxisNormal(), info.image_extents.minExtents());
  glm::dvec3 origin(0.0);
  origin.x = glm::dot(min_ext_2d, heightmap.surfaceAxisA());
  origin.y = glm::dot(min_ext_2d, heightmap.surfaceAxisB());
  origin.z = 0;  // Yaw

  // Image resolution in units/pixel. Units are the same as those used to generate the heightmap, generally metres.
  out << "resolution: " << heightmap.heightmap().resolution() << '\n';
  // The 2D pose of the lower-left pixel in the map, as (x, y, yaw), with yaw as counterclockwise rotation (degrees)
  // yaw = 0 means no rotation).
  out << "origin: [" << origin.x << ", " << origin.y << ", " << origin.z << "]\n";

  // Mode specific information
  switch (opt.image_mode)
  {
  case kTraversability:
    // Pixels with this colour value are considered obstructed.
    out << "obstructed_value: " << black_colour << "\n";  // Black
    // Pixels with this colour value are considered free/traversable.
    out << "free_value: " << white_colour << "\n";  // White
    // Traversability angle (degrees)
    out << "traverse_angle: " << opt.traverse_angle << "\n";
    break;
  default:
    break;
  }

  out << "up: [" << heightmap.upAxisNormal().x << ", " << heightmap.upAxisNormal().y << ", "
      << heightmap.upAxisNormal().z << "]\n";

  out << std::flush;
  out.close();

  return true;
}


int main(int argc, char *argv[])
{
  Options opt;

  std::cout.imbue(std::locale(""));

  int res = 0;
  res = parseOptions(&opt, argc, argv);

  if (res)
  {
    return res;
  }

  signal(SIGINT, onSignal);
  signal(SIGTERM, onSignal);

  printf("Loading map %s\n", opt.map_file.c_str());
  ProgressMonitor prog(10);
  LoadMapProgress load_progress(prog);
  ohm::Heightmap heightmap;
  ohm::MapVersion version;

  prog.setDisplayFunction([](const ProgressMonitor::Progress &prog, bool final) {
    std::ostringstream str;
    str << '\r';
    str << prog.progress;
    if (prog.info.total)
    {
      str << " / " << prog.info.total;
    }
    str << "      ";
    if (final)
    {
      str << '\n';
    }
    std::cout << str.str() << std::flush;
  });

  prog.startThread();
  res = ohm::load(opt.map_file.c_str(), heightmap, &load_progress, &version);
  prog.endProgress();

  std::cout << std::endl;

  if (res != 0)
  {
    std::cerr << "Failed to load heightmap. Error(" << res << "): " << ohm::serialiseErrorCodeString(res) << std::endl;
    return res;
  }

  ohm::HeightmapImage hm_image(opt.imageType());
  ohm::HeightmapImage::BitmapInfo info;
  ohm::HeightmapMesh mesh_builder(opt.normals_mode);

  mesh_builder.buildMesh(heightmap);

  hm_image.generateBitmap(mesh_builder, heightmap.upAxis());
  const uint8_t *image = hm_image.bitmap(&info);
  if (!image)
  {
    std::cerr << "Failed to generate heightmap image" << std::endl;
    return 1;
  }

  std::vector<uint8_t> export_pixels;
  ExportImageType export_type = convertImage(export_pixels, image, info, opt);

  std::cout << "Saving " << opt.image_file << std::endl;
  if (!savePng(opt.image_file.c_str(), export_pixels, export_type, info.image_width, info.image_height))
  {
    std::cerr << "Failed to save heightmap image" << std::endl;
    return 1;
  }

  std::string info_yaml_name = generateYamlName(opt.image_file);
  if (!saveMetaData(info_yaml_name, opt, heightmap, info, export_type))
  {
    std::cerr << "Failed to save yaml" << std::endl;
    return 1;
  }

  return res;
}


================================================
FILE: utils/ohminfo/CMakeLists.txt
================================================

find_package(ZLIB)

set(SOURCES
  ohminfo.cpp
)

add_executable(ohminfo ${SOURCES})
leak_track_target_enable(ohminfo CONDITION OHM_LEAK_TRACK)

set_target_properties(ohminfo PROPERTIES FOLDER utils)
if(MSVC)
  set_target_properties(ohminfo PROPERTIES DEBUG_POSTFIX "d")
endif(MSVC)

target_link_libraries(ohminfo PUBLIC ohm ohmutil)

target_link_libraries(ohminfo
  PRIVATE
    glm::glm
    $<BUILD_INTERFACE:ZLIB::ZLIB>
)

clang_tidy_target(ohminfo)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

install(TARGETS ohminfo DESTINATION bin)


================================================
FILE: utils/ohminfo/ohminfo.cpp
================================================
//
// author Kazys Stepanas
//
#include <glm/glm.hpp>

#include <ohm/DefaultLayer.h>
#include <ohm/MapInfo.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/VoxelData.h>
#include <ohm/VoxelLayout.h>

#include <ohmutil/Options.h>

#include <logutil/LogUtil.h>

#include <algorithm>
#include <chrono>
#include <csignal>
#include <cstddef>
#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <locale>
#include <sstream>
#include <unordered_set>

namespace
{
int g_quit = 0;

void onSignal(int arg)
{
  if (arg == SIGINT || arg == SIGTERM)
  {
    ++g_quit;
  }
}

struct Options
{
  std::string map_file;
  bool calculate_extents = false;
  bool detail = false;
};
}  // namespace


int parseOptions(Options *opt, int argc, char *argv[])  // NOLINT(modernize-avoid-c-arrays)
{
  cxxopts::Options opt_parse(argv[0], "\nProvide information about the contents of an occupancy map file.\n");
  opt_parse.positional_help("<map.ohm>");

  try
  {
    // clang-format off
    opt_parse.add_options()
      ("help", "Show help.")
      ("i,map", "The input map file (ohm) to load.",cxxopts::value(opt->map_file))
      ("extents", "Report map extents? Requires region traversal", optVal(opt->calculate_extents)->implicit_value("true"))
      ("detail","Traverse voxels for detailed information? min ""occupancy, max occupancy, max samples (if available)", optVal(opt->detail)->implicit_value("true"));
    // clang-format on

    opt_parse.parse_positional({ "map" });

    cxxopts::ParseResult parsed = opt_parse.parse(argc, argv);

    if (parsed.count("help") || parsed.arguments().empty())
    {
      // show usage.
      std::cout << opt_parse.help({ "", "Group" }) << std::endl;
      return 1;
    }

    if (opt->map_file.empty())
    {
      std::cerr << "Missing input map" << std::endl;
      return -1;
    }
  }
  catch (const cxxopts::OptionException &e)
  {
    std::cerr << "Argument error\n" << e.what() << std::endl;
    return -1;
  }

  return 0;
}

void showMapInfo(const ohm::MapInfo &info)
{
  unsigned item_count = info.extract(nullptr, 0);

  std::cout << "Meta data items: " << item_count << std::endl;
  if (item_count)
  {
    std::vector<ohm::MapValue> items;
    items.resize(item_count);
    item_count = info.extract(items.data(), item_count);
    std::sort(items.begin(), items.end(), [](const ohm::MapValue &a, const ohm::MapValue &b) {
      return std::less<std::string>{}(a.name(), b.name());
    });

    ohm::MapValue str_value;
    for (unsigned i = 0; i < item_count; ++i)
    {
      str_value = items[i].toStringValue();
      std::cout << "  " << str_value.name() << " : " << static_cast<const char *>(str_value) << std::endl;
    }
  }

  std::cout << std::endl;
}

int main(int argc, char *argv[])
{
  Options opt;

  std::cout.imbue(std::locale(""));

  int res = 0;
  res = parseOptions(&opt, argc, argv);

  if (res)
  {
    return res;
  }

  signal(SIGINT, onSignal);
  signal(SIGTERM, onSignal);

  ohm::OccupancyMap map(1.0f);
  ohm::MapVersion version;

  size_t region_count = 0;
  res = ohm::loadHeader(opt.map_file.c_str(), map, &version, &region_count);

  if (res != 0)
  {
    std::cerr << "Failed to load map. Error(" << res << "): " << ohm::serialiseErrorCodeString(res) << std::endl;
    return res;
  }

  std::cout << "File format version: " << version.major << '.' << version.minor << '.' << version.patch << std::endl;
  std::cout << std::endl;

  std::cout << "Estimated memory (CPU only): " << logutil::Bytes(map.calculateApproximateMemory()) << std::endl;

  std::cout << "Voxel resolution: " << map.resolution() << std::endl;
  std::cout << "Map origin: " << map.origin() << std::endl;
  std::cout << "Region spatial dimensions: " << map.regionSpatialResolution() << std::endl;
  std::cout << "Region voxel dimensions: " << map.regionVoxelDimensions() << " : " << map.regionVoxelVolume()
            << std::endl;
  std::cout << "Region count: " << region_count << std::endl;
  std::cout << std::endl;

  std::cout << "Occupancy threshold: " << map.occupancyThresholdProbability() << " (" << map.occupancyThresholdValue()
            << ")" << std::endl;
  std::cout << "Hit probability: " << map.hitProbability() << " (" << map.hitValue() << ")" << std::endl;
  std::cout << "Miss probability: " << map.missProbability() << " (" << map.missValue() << ")" << std::endl;
  std::cout << "Probability min/max: [" << map.minVoxelProbability() << "," << map.maxVoxelProbability() << "]"
            << std::endl;
  std::cout << "Value min/max: [" << map.minVoxelValue() << "," << map.maxVoxelValue() << "]" << std::endl;
  std::cout << "Saturation min/max: [" << (map.saturateAtMinValue() ? "on" : "off") << ","
            << (map.saturateAtMaxValue() ? "on" : "off") << "]" << std::endl;
  ;

  std::cout << "Touched stamp: " << map.stamp() << std::endl;
  std::cout << "Flags: " << std::endl;
  if (map.flags() != ohm::MapFlag::kNone)
  {
    unsigned bit = 1;
    for (unsigned i = 0; i < sizeof(ohm::MapFlag) * 8; ++i, bit <<= 1u)
    {
      if (unsigned(map.flags()) & bit)
      {
        std::cout << "  " << mapFlagToString(ohm::MapFlag(bit)) << std::endl;
      }
    }
  }
  else
  {
    std::cout << "  None" << std::endl;
  }

  std::cout << std::endl;

  // Meta info.
  showMapInfo(map.mapInfo());

  // Data needing chunks to be partly loaded.
  // - Extents
  // - Region count
  // - Memory footprint

  const ohm::MapLayout &layout = map.layout();
  std::string indent;
  std::string vox_size_str;
  std::string region_size_str;
  std::cout << "Layers: " << layout.layerCount() << std::endl;

  for (size_t i = 0; i < layout.layerCount(); ++i)
  {
    const ohm::MapLayer &layer = layout.layer(i);
    ohm::VoxelLayoutConst voxels = layer.voxelLayout();
    indent = "  ";
    std::cout << indent << layer.name() << std::endl;
    indent += "  ";
    std::cout << indent << "serialised? " << ((layer.flags() & ohm::MapLayer::kSkipSerialise) == 0 ? "true" : "false")
              << std::endl;
    std::cout << indent << "subsampling: " << layer.subsampling() << std::endl;
    std::cout << indent << "voxels: " << layer.dimensions(map.regionVoxelDimensions()) << " : "
              << layer.volume(layer.dimensions(map.regionVoxelDimensions())) << std::endl;

    std::cout << indent << "voxel byte size: " << logutil::Bytes(voxels.voxelByteSize()) << std::endl;
    std::cout << indent << "region byte size: "
              << logutil::Bytes(voxels.voxelByteSize() * layer.volume(layer.dimensions(map.regionVoxelDimensions())))
              << std::endl;

    indent += "  ";
    std::cout << std::setw(4) << std::setfill('0');
    for (size_t i = 0; i < voxels.memberCount(); ++i)
    {
      std::cout << indent << "0x" << std::hex << voxels.memberOffset(i) << " "
                << ohm::DataType::name(voxels.memberType(i)) << " " << voxels.memberName(i) << " (0x"
                << voxels.memberSize(i) << ")" << std::endl;
    }
    std::cout << std::setw(0) << std::dec;
  }

  // Load full map if required
  if (opt.calculate_extents || opt.detail)
  {
    // Reload the map for full extents.
    res = ohm::load(opt.map_file.c_str(), map);
    if (res)
    {
      std::cerr << "Failed to load map regions. Error code: " << res << std::endl;
      return res;
    }
  }

  if (opt.calculate_extents)
  {
    glm::dvec3 min_ext(0.0);
    glm::dvec3 max_ext(0.0);
    ohm::Key min_key(ohm::Key::kNull);
    ohm::Key max_key(ohm::Key::kNull);
    map.calculateExtents(&min_ext, &max_ext, &min_key, &max_key);

    std::cout << std::endl;
    std::cout << "Spatial Extents: " << min_ext << " - " << max_ext << std::endl;
    std::cout << "Key Extents: " << min_key << " - " << max_key << std::endl;
  }

  if (opt.detail)
  {
    float min_occupancy = std::numeric_limits<float>::max();
    float max_occupancy = -std::numeric_limits<float>::max();
    float min_intensity = std::numeric_limits<float>::max();
    float max_intensity = -std::numeric_limits<float>::max();
    uint64_t free_voxels = 0;
    uint64_t occupied_voxels = 0;
    uint64_t total_point_count = 0;
    unsigned max_point_count = 0;

    ohm::Voxel<const float> voxel(&map, map.layout().occupancyLayer());
    ohm::Voxel<const ohm::VoxelMean> mean(&map, map.layout().meanLayer());
    ohm::Voxel<const ohm::IntensityMeanCov> intensity(&map, map.layout().intensityLayer());
    if (voxel.isLayerValid())
    {
      for (auto iter = map.begin(); iter != map.end() && !g_quit; ++iter)
      {
        ohm::setVoxelKey(iter, voxel, mean, intensity);
        float value;
        voxel.read(&value);
        if (value != ohm::unobservedOccupancyValue())
        {
          min_occupancy = std::min(value, min_occupancy);
          max_occupancy = std::max(value, max_occupancy);

          const bool is_occupied = (value >= map.occupancyThresholdValue());
          free_voxels += !!(value < map.occupancyThresholdValue());
          occupied_voxels += !!is_occupied;

          if (is_occupied)
          {
            if (mean.isValid())
            {
              ohm::VoxelMean mean_info;
              mean.read(&mean_info);
              max_point_count = std::max<unsigned>(mean_info.count, max_point_count);
              total_point_count += mean_info.count;
            }

            if (intensity.isValid())
            {
              const float intensity_value = intensity.data().intensity_mean;
              min_intensity = std::min(min_intensity, intensity_value);
              max_intensity = std::max(max_intensity, intensity_value);
            }
          }
        }
      }

      std::cout << "Probability max: " << ohm::valueToProbability(max_occupancy) << " (" << max_occupancy << ")"
                << std::endl;
      std::cout << "Probability min: " << ohm::valueToProbability(min_occupancy) << " (" << min_occupancy << ")"
                << std::endl;
      std::cout << "Free voxels: " << free_voxels << std::endl;
      std::cout << "Occupied voxels: " << occupied_voxels << std::endl;

      if (mean.isLayerValid())
      {
        std::cout << "Max voxel samples: " << max_point_count << std::endl;
        std::cout << "Average voxel samples: " << total_point_count / occupied_voxels << std::endl;
      }

      if (intensity.isLayerValid())
      {
        std::cout << "Minimum intensity: " << min_intensity << std::endl;
        std::cout << "Maximum intensity: " << max_intensity << std::endl;
      }
    }
    else
    {
      std::cout << "No " << ohm::default_layer::occupancyLayerName() << " layer" << std::endl;
    }
  }

  return res;
}


================================================
FILE: utils/ohmpop/CMakeLists.txt
================================================
if(NOT TARGET slamio)
  message(STATUS "Skipping ohmpop. slamio not built")
  return()
endif()

if(OHM_FEATURE_CUDA AND OHM_USE_DEPRECATED_CMAKE_CUDA)
  # Need to find CUDA for deprecated project configuration
  find_package(CUDA)
endif(OHM_FEATURE_CUDA AND OHM_USE_DEPRECATED_CMAKE_CUDA)

configure_file(OhmPopConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/ohmpop/OhmPopConfig.h")

set(SOURCES
  OhmPopConfig.in.h
  "${CMAKE_CURRENT_BINARY_DIR}/ohmpop/OhmPopConfig.h"
)

function(_ohmpop_setup GPU_MODE)
  set(TARGET_NAME ohmpop${GPU_MODE})
  if(NOT GPU_MODE STREQUAL "cpu")
    set(OHMLIB_NAME ohm${GPU_MODE})
    list(APPEND SOURCES
      ohmpopmaingpu.cpp
    )
  else(NOT GPU_MODE STREQUAL "cpu")
    set(OHMLIB_NAME ohm)
    list(APPEND SOURCES
      ohmpopmaincpu.cpp
    )
  endif(NOT GPU_MODE STREQUAL "cpu")

  set(TARGET_NAME ohmpop${GPU_MODE})

  if(GPU_MODE STREQUAL "cuda" AND OHM_USE_DEPRECATED_CMAKE_CUDA)
    cuda_add_executable(${TARGET_NAME} ${SOURCES})
  else()
    add_executable(${TARGET_NAME} ${SOURCES})
  endif()
  leak_track_target_enable(${TARGET_NAME} CONDITION OHM_LEAK_TRACK)

  set_target_properties(${TARGET_NAME} PROPERTIES FOLDER utils)
  if(MSVC)
    set_target_properties(${TARGET_NAME} PROPERTIES DEBUG_POSTFIX "d")
  endif(MSVC)

  target_include_directories(${TARGET_NAME}
    PUBLIC
      $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/ohmpop>
      $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
  )

  target_link_libraries(${TARGET_NAME} PRIVATE ${OHMLIB_NAME} ohmapp slamio ohmtools ohmutil)

  target_link_libraries(${TARGET_NAME}
    PRIVATE
      glm::glm
  )

  if(GPU_MODE STREQUAL "cpu")
    target_compile_definitions(${TARGET_NAME} PRIVATE "-DOHMPOP_CPU")
  else()
    target_compile_definitions(${TARGET_NAME} PRIVATE "-DOHMPOP_GPU")
    target_link_libraries(${TARGET_NAME} PRIVATE ohmapp${GPU_MODE})
  endif(GPU_MODE STREQUAL "cpu")

  install(TARGETS ${TARGET_NAME} DESTINATION bin)
endfunction(_ohmpop_setup)

if(OHM_FEATURE_OPENCL)
  _ohmpop_setup(ocl)
  clang_tidy_target(ohmpopocl)
  # Required to run NVIDIA OpenCL
  leak_track_default_options(ohmpopocl CONDITION OHM_LEAK_TRACK ${OHM_ASAN_OPTIONS_CUDA})
  leak_track_suppress(ohmpopocl CONDITION OHM_LEAK_TRACK
    ${OHM_LEAK_SUPPRESS_OCL}
    "libpdal_base"
  )
endif(OHM_FEATURE_OPENCL)
if(OHM_FEATURE_CUDA)
  _ohmpop_setup(cuda)
  clang_tidy_target(ohmpopcuda)
  leak_track_default_options(ohmpopcuda CONDITION OHM_LEAK_TRACK ${OHM_ASAN_OPTIONS_CUDA})
  leak_track_suppress(ohmpopcuda CONDITION OHM_LEAK_TRACK
    ${OHM_LEAK_SUPPRESS_CUDA}
    "libpdal_base"
  )
endif(OHM_FEATURE_CUDA)
_ohmpop_setup(cpu)
clang_tidy_target(ohmpopcpu)
leak_track_suppress(ohmpopcpu CONDITION OHM_LEAK_TRACK
  "libpdal_base"
)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})


================================================
FILE: utils/ohmpop/OhmPopConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMPOPCONFIG_H
#define OHMPOPCONFIG_H

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX
#include <cmath>

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

#endif  // OHMPOPCONFIG_H


================================================
FILE: utils/ohmpop/ohmpopmaincpu.cpp
================================================
//
// author Kazys Stepanas
//
#include <ohmapp/OhmAppCpu.h>
#include <ohmapp/SlamIOSource.h>
#include <ohmapp/ohmappmain.inl>

int main(int argc, char *argv[])
{
  return ohmappMain<ohmapp::OhmAppCpu, ohmapp::SlamIOSource>(argc, argv);
}


================================================
FILE: utils/ohmpop/ohmpopmaingpu.cpp
================================================
//
// author Kazys Stepanas
//
#include <ohmapp/OhmAppGpu.h>
#include <ohmapp/SlamIOSource.h>
#include <ohmapp/ohmappmain.inl>

int main(int argc, char *argv[])
{
  return ohmappMain<ohmapp::OhmAppGpu, ohmapp::SlamIOSource>(argc, argv);
}


================================================
FILE: utils/ohmprob/CMakeLists.txt
================================================

find_package(ZLIB)

set(SOURCES
ohmprob.cpp
)

add_executable(ohmprob ${SOURCES})
leak_track_target_enable(ohmprob CONDITION OHM_LEAK_TRACK)

set_target_properties(ohmprob PROPERTIES FOLDER utils)
if(MSVC)
  set_target_properties(ohmprob PROPERTIES DEBUG_POSTFIX "d")
endif(MSVC)

target_link_libraries(ohmprob PUBLIC ohm ohmutil)

target_link_libraries(ohmprob
  PRIVATE
    glm::glm
    $<BUILD_INTERFACE:ZLIB::ZLIB>
)

clang_tidy_target(ohmprob)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

install(TARGETS ohmprob DESTINATION bin)


================================================
FILE: utils/ohmprob/ohmprob.cpp
================================================
// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas

// A very simply utility which performs the probabilty-to-value or reverse conversion used in ohm.

#include <ohm/MapProbability.h>

// Must be after argument streaming operators.
#include <ohmutil/Options.h>

#include <iomanip>
#include <limits>

namespace
{
using real = double;  // NOLINT(readability-identifier-naming)

struct Options
{
  real value{ 0 };
  bool reverse{ false };
};
}  // namespace

int parseOptions(Options *opt, int argc, char *argv[])  // NOLINT(modernize-avoid-c-arrays)
{
  cxxopts::Options opt_parse(argv[0], "Make a probability to value conversion or the --reverse .");
  opt_parse.positional_help("<value>");

  try
  {
    // Build GPU options set.
    // clang-format off
    opt_parse.add_options()
      ("help", "Show help.")
      ("v,value", "The input map file (ohm).", cxxopts::value(opt->value))
      ("r,reverse", "Reverse lookup? The input value is treated as a probability and converted to a value.",
               optVal(opt->reverse))
      ;
    // clang-format on

    opt_parse.parse_positional({ "value" });

    cxxopts::ParseResult parsed = opt_parse.parse(argc, argv);

    if (parsed.count("help") || parsed.arguments().empty())
    {
      // show usage.
      std::cout << opt_parse.help() << std::endl;
      return 1;
    }
  }
  catch (const cxxopts::OptionException &e)
  {
    std::cerr << "Argument error\n" << e.what() << std::endl;
    return -1;
  }

  return 0;
}


int main(int argc, char *argv[])
{
  Options opt;
  std::cout.imbue(std::locale(""));

  int res = parseOptions(&opt, argc, argv);

  if (res)
  {
    return res;
  }

  const real converted = (!opt.reverse) ? ohm::probabilityToValue(opt.value) : ohm::valueToProbability(opt.value);
  std::cout << std::fixed << std::setprecision(std::numeric_limits<real>::digits10 + 1) << converted << std::endl;
  return 0;
}


================================================
FILE: utils/ohmquery/CMakeLists.txt
================================================

set(TES_ENABLE ${OHM_TES_DEBUG})
configure_file(OhmQueryConfig.in.h "${CMAKE_CURRENT_BINARY_DIR}/ohmquery/OhmQueryConfig.h")
set(SOURCES "${CMAKE_CURRENT_BINARY_DIR}/ohmquery/OhmQueryConfig.h")

set(SOURCES
  ohmquery.cpp
  OhmQueryConfig.in.h
  "${CMAKE_CURRENT_BINARY_DIR}/ohmquery/OhmQueryConfig.h"
)

function(_ohmquery_setup GPU_MODE)
  set(TARGET_NAME ohmquery${GPU_MODE})
  add_executable(${TARGET_NAME} ${SOURCES})
  leak_track_target_enable(${TARGET_NAME} CONDITION OHM_LEAK_TRACK)

  set_target_properties(${TARGET_NAME} PROPERTIES FOLDER utils)
  if(MSVC)
    set_target_properties(${TARGET_NAME} PROPERTIES DEBUG_POSTFIX "d")
  endif(MSVC)

  target_include_directories(${TARGET_NAME}
    PUBLIC
      $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/ohmquery>
      $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
  )

  target_include_directories(${TARGET_NAME}
    SYSTEM PRIVATE
      $<$<NOT:$<BOOL:${OHM_TES_DEBUG}>>:${3ES_INCLUDE_DIRS}>
  )

  target_link_libraries(${TARGET_NAME}
    PUBLIC
      ohm${GPU_MODE}
      gputil${GPU_MODE}
      ohmutil
    PRIVATE
      $<BUILD_INTERFACE:$<$<BOOL:${OHM_TES_DEBUG}>:3es::3es-core>>
      glm::glm
      $<BUILD_INTERFACE:ZLIB::ZLIB>
  )


  install(TARGETS ${TARGET_NAME} DESTINATION bin)
endfunction(_ohmquery_setup)

if(OHM_FEATURE_OPENCL)
  _ohmquery_setup(ocl)
  clang_tidy_target(ohmqueryocl)
  # Required to run NVIDIA OpenCL
  leak_track_default_options(ohmqueryocl CONDITION OHM_LEAK_TRACK ${OHM_ASAN_OPTIONS_CUDA})
  leak_track_suppress(ohmqueryocl CONDITION OHM_LEAK_TRACK
    ${OHM_LEAK_SUPPRESS_OCL}
    ${OHM_LEAK_SUPPRESS_TBB}
  )
endif(OHM_FEATURE_OPENCL)
if(OHM_FEATURE_CUDA)
  _ohmquery_setup(cuda)
  clang_tidy_target(ohmquerycuda)
  leak_track_default_options(ohmquerycuda CONDITION OHM_LEAK_TRACK ${OHM_ASAN_OPTIONS_CUDA})
  leak_track_suppress(ohmquerycuda CONDITION OHM_LEAK_TRACK
    ${OHM_LEAK_SUPPRESS_CUDA}
    ${OHM_LEAK_SUPPRESS_TBB}
  )
endif(OHM_FEATURE_CUDA)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})


================================================
FILE: utils/ohmquery/OhmQueryConfig.in.h
================================================
//
// Project configuration header. This is a generated header; do not modify
// it directly. Instead, modify the config.h.in version and run CMake again.
//
#ifndef OHMQUERYCONFIG_H
#define OHMQUERYCONFIG_H

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif  // _USE_MATH_DEFINES
#ifndef NOMINMAX
#define NOMINMAX
#endif  // NOMINMAX
#include <cmath>

#ifdef _MSC_VER
// Avoid dubious security warnings for plenty of legitimate code
#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif  // _SCL_SECURE_NO_WARNINGS
#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif  // _CRT_SECURE_NO_WARNINGS
//#define _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES 1
#endif  // _MSC_VER

#include <ohm/OhmConfig.h>

#endif  // OHMQUERYCONFIG_H


================================================
FILE: utils/ohmquery/ohmquery.cpp
================================================
//
// author Kazys Stepanas
//
#include "OhmQueryConfig.h"

#include <3esservermacros.h>
#include <glm/glm.hpp>

#include <ohm/LineQuery.h>
#include <ohm/MapSerialise.h>
#include <ohm/NearestNeighbours.h>
#include <ohm/OccupancyMap.h>
#include <ohm/OccupancyType.h>
#include <ohm/OccupancyUtil.h>
#include <ohm/QueryFlag.h>
#include <ohm/Trace.h>
#include <ohm/Voxel.h>

#include <ohmgpu/GpuMap.h>
#include <ohmgpu/LineQueryGpu.h>
#include <ohmgpu/OhmGpu.h>

#include <logutil/LogUtil.h>
#include <ohmutil/GlmStream.h>
#include <ohmutil/PlyMesh.h>
#include <ohmutil/ProgressMonitor.h>
#include <ohmutil/SafeIO.h>

#include <ohm/DebugIDs.h>

#include <algorithm>
#include <atomic>
#include <chrono>
#include <cinttypes>
#include <csignal>
#include <cstddef>
#include <fstream>
#include <iostream>
#include <locale>
#include <sstream>

namespace
{
using TimingClock = std::chrono::high_resolution_clock;

int g_quit = 0;

void onSignal(int arg)
{
  if (arg == SIGINT || arg == SIGTERM)
  {
    ++g_quit;
  }
}
struct Options
{
  struct Neighbours
  {
    glm::dvec3 point = glm::dvec3(0);
    float radius = -1;
  };

  struct Line
  {
    glm::dvec3 start = glm::dvec3(0);
    glm::dvec3 end = glm::dvec3(0);
    float radius = -1;
  };

  struct Ranges
  {
    glm::dvec3 min = glm::dvec3(0);
    glm::dvec3 max = glm::dvec3(0);
    float radius = -1;
  };

  std::string map_file;
  std::string output_base;
  Neighbours neighbours;
  Ranges ranges;
  Line line;
  int repeat = 0;
  bool unknown_as_occupied = true;
  bool use_gpu = false;
  bool gpu_compare = false;
  bool hard_reset_on_repeat = false;
  bool quiet = false;

  inline bool haveQuery() const { return neighbours.radius > 0 || line.radius > 0 || ranges.radius > 0; }

  void print() const;
};


void Options::print() const
{
  std::cout << "Map: " << map_file << std::endl;
  std::cout << "Output: " << output_base << std::endl;
  if (neighbours.radius >= 0)
  {
    std::cout << "Nearest neighbours: " << neighbours.point << " R: " << neighbours.radius << std::endl;
  }
  if (line.radius >= 0)
  {
    std::cout << "Line " << line.start << "->" << line.end << " R: " << line.radius << std::endl;
  }
  if (ranges.radius >= 0)
  {
    std::cout << "Ranges: " << ranges.min << "->" << ranges.max << " R: " << ranges.radius << std::endl;
  }
}


class LoadMapProgress : public ohm::SerialiseProgress
{
public:
  explicit LoadMapProgress(ProgressMonitor &monitor)
    : monitor_(monitor)
  {}

  bool quit() const override { return ::g_quit > 1; }

  void setTargetProgress(unsigned target) override { monitor_.beginProgress(ProgressMonitor::Info(target)); }
  void incrementProgress(unsigned inc) override { monitor_.incrementProgressBy(inc); }

private:
  ProgressMonitor &monitor_;
};
}  // namespace


inline std::istream &operator>>(std::istream &in, Options::Neighbours &n)
{
  glm::dvec4 v;
  //::operator>>(in, v);
  in >> v;
  n.point[0] = v[0];
  n.point[1] = v[1];
  n.point[2] = v[2];
  n.radius = float(v[3]);
  return in;
}

inline std::ostream &operator<<(std::ostream &out, const Options::Neighbours &n)
{
  out << n.point[0] << ',' << n.point[1] << ',' << n.point[2] << ',' << n.radius;
  return out;
}

inline std::istream &operator>>(std::istream &in, Options::Line &l)
{
  std::array<double, 7> v;
  parseVector(in, v.data(), int(v.size()));
  l.start[0] = v[0];
  l.start[1] = v[1];
  l.start[2] = v[2];
  l.end[0] = v[3];
  l.end[1] = v[4];
  l.end[2] = v[5];
  l.radius = float(v[6]);
  return in;
}

inline std::ostream &operator<<(std::ostream &out, const Options::Line &l)
{
  out << l.start[0] << ',' << l.start[1] << ',' << l.start[2] << ',' << l.end[0] << ',' << l.end[1] << ',' << l.end[2]
      << ',' << l.radius;
  return out;
}

inline std::istream &operator>>(std::istream &in, Options::Ranges &r)
{
  std::array<double, 7> v;
  parseVector(in, v.data(), int(v.size()));
  r.min[0] = v[0];
  r.min[1] = v[1];
  r.min[2] = v[2];
  r.max[0] = v[3];
  r.max[1] = v[4];
  r.max[2] = v[5];
  r.radius = float(v[6]);
  return in;
}

inline std::ostream &operator<<(std::ostream &out, const Options::Ranges &r)
{
  out << r.min[0] << ',' << r.min[1] << ',' << r.min[2] << ',' << r.max[0] << ',' << r.max[1] << ',' << r.max[2] << ','
      << r.radius;
  return out;
}

// This is messy :(
// Must come after streaming operators for custom command line arguments are defined.
#include <ohmutil/Options.h>

int parseOptions(Options *opt, int argc, char *argv[])  // NOLINT(modernize-avoid-c-arrays)
{
  cxxopts::Options opt_parse(argv[0],
                             "\nLoads an occupancy map file and runs a single query on the map, exporting the\n"
                             "results to a new PLY cloud. trajectory file. The trajectory marks the scanner\n"
                             "trajectory with timestamps loosely corresponding to cloud point timestamps.\n"
                             "Trajectory points are interpolated for each cloud point based on corresponding\n"
                             "times in the trajectory.");
  opt_parse.positional_help(
    "<map.ohm> [<output.ply>] <--near=x,y,z,r | --line=x1,y1,z1,x2,y2,z2,r>");  // | --ranges=x1,y1,z1,x2,y2,z2,r>");

  try
  {
    // clang-format off
    opt_parse.add_options()
      ("gpu", "Use GPU based queries where possible", optVal(opt->use_gpu))
      ("q,quiet", "Run in quiet mode. Suppresses progress messages.", optVal(opt->use_gpu))
      ("gpu-compare", "Compare CPU and GPU results for the query. Implies '--gpu'.", optVal(opt->gpu_compare))
      ("hard-reset", "Perform a hard reset when repeatedly executing a query (--repeat option). Soft reset is the default.", optVal(opt->hard_reset_on_repeat))
      ("map", "The input map file to load <<mapfile>-near.ply> and <<mapfile>-line.ply>", optVal(opt->map_file))
      ("o,output", "Sets the base PLY file names to save results to. Defaults to <<mapfile>-near.ply> and <<mapfile>-line.ply>", optVal(opt->output_base))
      ("line", "Perform a line segment test from (x1,y1,z1) to (x2,y2,z2) considering voxels withing radius r of the line segment.", optVal(opt->line), "x1,y1,z1,x2,y2,z2,r")
      ("near", "Perform a nearest neighbours query at the point (x,y,z) with a radius of r.", optVal(opt->neighbours), "x,y,z,r")
      // ("ranges", "Calculate the nearest occupied voxel for each voxel in the specified min/max extents, (x1,y1,z1) to (x2,y2,z2).", optVal(opt->ranges), "x1,y1,z1,x2,y2,z2,r")
      ("repeat", "Repeat the query N times. For timing evaluation.", optVal(opt->repeat))
      ("uao", "Treat unknown space as occupied/obstructed?", optVal(opt->unknown_as_occupied))
    ;
    // clang-format off

    opt_parse.parse_positional({ "map", "output" });

    cxxopts::ParseResult parsed = opt_parse.parse(argc, argv);

    if (parsed.count("help") || parsed.arguments().empty())
    {
      // show usage.
      std::cout << opt_parse.help({ "", "Group" }) << std::endl;
      return 1;
    }

    bool ok = true;

    if (opt->map_file.empty())
    {
      ok = false;
      std::cerr << "Missing input map" << std::endl;
    }

    if (opt->ranges.min.x > opt->ranges.max.x ||
        opt->ranges.min.y > opt->ranges.max.y ||
        opt->ranges.min.z > opt->ranges.max.z)
    {
      ok = false;
      std::cerr << "Minimum range " << opt->ranges.min << " exceeds maximum " << opt->ranges.max << std::endl;
    }

    if (!ok)
    {
      return -1;
    }
  }
  catch (const cxxopts::OptionException &e)
  {
    std::cerr << "Argument error\n" << e.what() << std::endl;
    return -1;
  }

  if (opt->gpu_compare)
  {
    opt->use_gpu = true;
  }

  return 0;
}

void initialiseDebugCategories(const Options &/*opt*/)
{
  // TES_CATEGORY(ohm::g_tes, "Map", Category::kMap, 0, true);
  // TES_CATEGORY(ohm::g_tes, "Populate", Category::kPopulate, 0, true);
  // TES_IF(opt.rays & Rays_Lines)
  // {
  //   TES_CATEGORY(ohm::g_tes, "Rays", Category::kRays, Category::kPopulate, (opt.rays & Rays_Lines) != 0);
  // }
  // TES_IF(opt.rays & Rays_Voxels)
  // {
  //   TES_CATEGORY(ohm::g_tes, "Free", Category::kFreeCells, Category::kPopulate, (opt.rays & Rays_Lines) == 0);
  // }
  // TES_IF(opt.samples)
  // {
  //   TES_CATEGORY(ohm::g_tes, "Occupied", Category::kOccupiedCells, Category::kPopulate, true);
  // }
  // TES_CATEGORY(ohm::g_tes, "Info", Category::kInfo, 0, true);
}

void saveQueryCloud(const ohm::OccupancyMap &map, const ohm::Query &query, const Options &opt,
                    const std::string &suffix, float colour_range = 0.0f)
{
  const size_t result_count = query.numberOfResults();
  const ohm::Key *keys = query.intersectedVoxels();
  const double *ranges = query.ranges();
  glm::dvec3 voxel_pos;

  ohm::PlyMesh ply;
  for (size_t i = 0; i < result_count; ++i)
  {
    const ohm::Key &key = keys[i];
    uint8_t c = std::numeric_limits<uint8_t>::max();
    if (colour_range > 0 && ranges)
    {
      const double range_value = ranges[i];
      c = uint8_t(std::numeric_limits<uint8_t>::max() * std::max(0.0, (colour_range - range_value) / colour_range));
    }
    voxel_pos = map.voxelCentreGlobal(key);
    ply.addVertex(voxel_pos, ohm::Colour(c, std::numeric_limits<uint8_t>::max() / 2, 0));
  }

  std::string str = opt.output_base;
  str += suffix + ".ply";
  printf("Saving results to %s\n", str.c_str());
  ply.save(str.c_str(), true);
}


#ifdef FIXME
void saveRangesCloud(const ohm::OccupancyMap &map, const ohm::VoxelRanges &query, const std::string &suffix,
                     ProgressMonitor &prog, const Options &opt,
                     std::vector<glm::vec4> *pointsOut = nullptr,
                     bool clearValues = false)
{
  std::string str = opt.outputBase;
  str += suffix + ".ply";
  printf("Exporting ranges cloud to %s\n", str.c_str());
  ohm::PlyMesh ply;
  glm::vec3 v;
  auto mapEndIter = map.end();
  size_t regionCount = map.regionCount();
  glm::i16vec3 lastRegion = map.begin().key().regionKey();
  glm::i16vec3 minRegion, maxRegion;
  uint64_t pointCount = 0;

  prog.beginProgress(ProgressMonitor::Info(regionCount));

  minRegion = map.regionKey(query.minExtents());
  maxRegion = map.regionKey(query.maxExtents());

  const float colourScale = query.searchRadius();
  for (auto iter = map.begin(); iter != mapEndIter && g_quit < 2; ++iter)
  {
    const ohm::VoxelConst voxel = *iter;
    if (lastRegion != iter.key().regionKey())
    {
      prog.incrementProgress();
      lastRegion = iter.key().regionKey();
    }

    // Ensure the voxel is in a region we have calculated data for.
    if (minRegion.x <= lastRegion.x && lastRegion.x <= maxRegion.x &&
        minRegion.y <= lastRegion.y && lastRegion.y <= maxRegion.y &&
        minRegion.z <= lastRegion.z && lastRegion.z <= maxRegion.z)
    {
      if (voxel.isOccupied() || voxel.isFree())
      {
        const float rangeValue = voxel.clearance();
        if (rangeValue >= 0)
        {
          auto c =
            uint8_t(std::numeric_limits<uint8_t>::max() * std::max(0.0f, (colourScale - rangeValue) / colourScale));
          v = map.voxelCentreLocal(voxel.key());
          ply.addVertex(v, Colour(c, 128, 0));
          if (pointsOut)
          {
            pointsOut->push_back(glm::vec4(v, rangeValue));
          }
          ++pointCount;
        }
      }

      if (clearValues && !voxel.isNull())
      {
        auto mutable_voxel = voxel.makeMutable();
        mutable_voxel.setClearance(-1.0f);
        mutable_voxel.touchMap(map.layout().clearanceLayer());
      }
    }
  }

  ply.save(str.c_str(), true);

  prog.endProgress();
  prog.pause();

  if (!opt.quiet)
  {
    printf("\nExported %" PRIu64 " points\n", pointCount);
  }
}
#endif // FIXME


void showTiming(const char *info, const TimingClock::time_point &start_time, const TimingClock::time_point &end_time,
                int cycles)
{
  std::string timing_str;
  TimingClock::duration exec_time = end_time - start_time;
  logutil::timeString(timing_str, exec_time);
  if (cycles <= 1)
  {
    printf("%s query completed in %s\n", info, timing_str.c_str());
  }
  else
  {
    printf("%s query completed %d queries in %s\n", info, cycles, timing_str.c_str());
    exec_time /= cycles;
    logutil::timeString(timing_str, exec_time);
    printf("average time per query: %s\n", timing_str.c_str());
  }
}


bool compareCpuGpuQuery(const char *query_name, ohm::Query &query,
                        const float epsilon = 1e-5f)
{
  std::string timing_info_str;
  TimingClock::time_point query_start;
  TimingClock::time_point query_end;

  // CPU execution.
  query.setQueryFlags(query.queryFlags() & ~ohm::kQfGpu);
  query_start = TimingClock::now();
  query.reset();
  query.execute();
  query_end = TimingClock::now();

  std::vector<ohm::Key> keys;
  std::vector<double> ranges;

  keys.resize(query.numberOfResults());
  ranges.resize(query.numberOfResults());

  if (query.numberOfResults())
  {
    for (size_t i = 0; i < query.numberOfResults(); ++i)
    {
      keys[i] = query.intersectedVoxels()[i];
      ranges[i] = query.ranges()[i];
    }
  }

  std::cout <<
            "Comparing CPU/GPU execution. Note GPU execution time may be better on repeated queries due to setup overhead and caching gains."
            << std::endl;
  timing_info_str = query_name;
  timing_info_str += " CPU";
  showTiming(timing_info_str.c_str(), query_start, query_end, 1);

  // GPU execution
  query.setQueryFlags(query.queryFlags() | ohm::kQfGpuEvaluate);
  query_start = TimingClock::now();
  query.reset();
  query.execute();
  query_end = TimingClock::now();

  timing_info_str = query_name;
  timing_info_str += " GPU";
  showTiming(timing_info_str.c_str(), query_start, query_end, 1);

  std::cout << "Comparing " << keys.size() << " results" << std::endl;
  if (keys.size() != query.numberOfResults())
  {
    std::cerr << "Result count mismatch (CPU/GPU): " << keys.size() << '/' << query.numberOfResults() << std::endl;
  }

  // Compare results.
  bool results_match = true;
  double range_diff;
  bool key_found;

  // Look for duplicate GPU results.
  for (size_t i = 0; i < query.numberOfResults(); ++i)
  {
    const ohm::Key key = query.intersectedVoxels()[i];
    for (size_t j = i + 1; j < query.numberOfResults(); ++j)
    {
      if (key == query.intersectedVoxels()[j])
      {
        std::cerr << "Duplicate GPU key result: ("
                  << keys[i].regionKey().x << ',' << keys[i].regionKey().y << ',' << keys[i].regionKey().z
                  << "):[" << int(keys[i].localKey().x) << ',' << int(keys[i].localKey().y) << ',' << int(keys[i].localKey().z)
                  << ']' << std::endl;
      }
    }
  }

  // Compare CPU/GPU results.
  for (size_t i = 0; i < keys.size(); ++i)
  {
    key_found = false;
    // Find the key in the GPU result. Order from GPU is non-deterministic.
    for (size_t j = 0; j < query.numberOfResults(); ++j)
    {
      if (keys[i] == query.intersectedVoxels()[j])
      {
        key_found = true;
        range_diff = ranges[i] - query.ranges()[j];
        if (std::abs(range_diff) > epsilon)
        {
          std::cerr << "Range diff for voxel ("
                    << keys[i].regionKey().x << ',' << keys[i].regionKey().y << ',' << keys[i].regionKey().z
                    << "):[" << int(keys[i].localKey().x) << ',' << int(keys[i].localKey().y) << ',' << int(keys[i].localKey().z)
                    << "]: " << ranges[i] << "/" << query.ranges()[j] << ':' << range_diff << std::endl;
          results_match = false;
        }
        break;
      }
    }

    if (!key_found)
    {
      std::cerr << "No matching GPU key for voxel ("
                << keys[i].regionKey().x << ',' << keys[i].regionKey().y << ',' << keys[i].regionKey().z
                << "):[" << int(keys[i].localKey().x) << ',' << int(keys[i].localKey().y) << ',' << int(keys[i].localKey().z)
                << ']' << std::endl;
      results_match = false;
    }
  }

  return results_match;
}


void executeQuery(const char *query_name, const Options &opt, ohm::Query &query, const float range_epsilon = 1e-5f)
{
  if (!opt.gpu_compare)
  {
    int repeat = (opt.repeat > 0) ? opt.repeat : 1;
    const auto query_start = TimingClock::now();

    if (opt.use_gpu)
    {
      query.setQueryFlags(query.queryFlags() | ohm::kQfGpuEvaluate);
    }

    for (int i = 0; i < repeat; ++i)
    {
      query.reset(opt.hard_reset_on_repeat);
      query.execute();
    }
    const auto query_end = TimingClock::now();

    showTiming(query_name, query_start, query_end, repeat);
  }
  else
  {
    compareCpuGpuQuery(query_name, query, range_epsilon);
  }
}


int runQueries(const Options &opt)
{
  ohm::OccupancyMap map(1);
  ProgressMonitor prog(10);

  if (!opt.haveQuery())
  {
    printf("Nothing to do! Queries not specified.\n");
    return 1;
  }

  prog.setDisplayFunction([&opt](const ProgressMonitor::Progress & prog, bool final)
  {
    if (!opt.quiet)
    {
      if (prog.info.total)
      {
        printf("\r%7" PRIu64 " / %7" PRIu64 "    ", prog.progress, prog.info.total);
      }
      else
      {
        printf("\r%7" PRIu64 "    ", prog.progress);
      }
      if (final)
      {
        printf("\n");
      }
      fflush(stdout);
    }
  });
  // Start paused.
  prog.startThread(true);

  printf("Loading map %s\n", opt.map_file.c_str());
  LoadMapProgress load_progress(prog);
  prog.unpause();
  int err = ohm::load(opt.map_file.c_str(), map, nullptr);//&loadProgress);
  prog.endProgress();

  if (err)
  {
    printf("Failed to load map %s ", opt.map_file.c_str());
    switch (err)
    {
    case ohm::kSeFileCreateFailure:
      printf("file create failure\n");
      break;
    case ohm::kSeFileOpenFailure:
      printf("file open failure\n");
      break;
    case ohm::kSeFileWriteFailure:
      printf("file write failure\n");
      break;
    case ohm::kSeFileReadFailure:
      printf("file read failure\n");
      break;
    case ohm::kSeValueOverflow:
      printf("value overflow\n");
      break;
    case ohm::kSeUnsupportedVersion:
      printf("unsupported version\n");
      break;
    default:
      printf("unknown (%d)\n", err);
      break;
    }
    return 2;
  }

  //timing_clock::time_point queryStart, queryEnd;
  std::string str;
  unsigned query_flags = 0;
  query_flags |= !!opt.unknown_as_occupied * ohm::kQfUnknownAsOccupied;
  // queryFlags |= ohm::kQfGpuEvaluate;

  if (opt.neighbours.radius >= 0)
  {
    std::cout << "Running nearest neighbours " << opt.neighbours.point << " R: " << opt.neighbours.radius << std::endl;

    ohm::NearestNeighbours nn_query(map, opt.neighbours.point,
                                   opt.neighbours.radius, query_flags);
    executeQuery("nearest neighbours", opt, nn_query);
    saveQueryCloud(map, nn_query, opt, "-near");
  }

  if (opt.line.radius >= 0)
  {
    printf("Running line query (%lg %lg %lg)->(%lg %lg %lg) R: %lg\n",
           opt.line.start.x, opt.line.start.y, opt.line.start.z,
           opt.line.end.x, opt.line.end.y, opt.line.end.z,
           opt.line.radius
          );

    std::unique_ptr<ohm::LineQuery> line_query;
    if (opt.use_gpu)
    {
      line_query = std::make_unique<ohm::LineQueryGpu>(map, opt.line.start, opt.line.end, opt.line.radius, query_flags);
    }
    else
    {
      line_query = std::make_unique<ohm::LineQuery>(map, opt.line.start, opt.line.end, opt.line.radius, query_flags);
    }
    // Allow single voxel epsilon value.
    executeQuery("line query", opt, *line_query, float(map.resolution()));
    saveQueryCloud(map, *line_query, opt, "-line", opt.line.radius);
  }

#ifdef FIXME
  if (opt.ranges.radius > 0)
  {
    printf("Running ranges query (%lg %lg %lg)->(%lg %lg %lg) R: %lg\n",
           opt.ranges.min.x, opt.ranges.min.y, opt.ranges.min.z,
           opt.ranges.max.x, opt.ranges.max.y, opt.ranges.max.z,
           opt.ranges.radius
          );

    ohm::VoxelRanges rangesQuery(map, opt.ranges.min, opt.ranges.max, opt.ranges.radius, queryFlags);
    auto queryStart = timing_clock::now();
    if (opt.gpuCompare)
    {
      rangesQuery.setQueryFlags(rangesQuery.queryFlags() & ~ohm::kQfGpuEvaluate);
      printf("CPU: ");
      fflush(stdout);
    }
    else if (opt.useGpu)
    {
      rangesQuery.setQueryFlags(rangesQuery.queryFlags() | ohm::kQfGpuEvaluate);
    }

    rangesQuery.reset();
    rangesQuery.execute();
    auto queryEnd = timing_clock::now();

    showTiming("ranges query", queryStart, queryEnd, 1);

    if (opt.gpuCompare)
    {
      std::vector<glm::vec4> gpuPoints, cpuPoints;
      // Extract CPU points.
      saveRangesCloud(map, rangesQuery, "-ranges-cpu", prog, opt, &cpuPoints, true);
      // Set GPU flag and run again on GPU.
      queryStart = timing_clock::now();
      rangesQuery.setQueryFlags(rangesQuery.queryFlags() | ohm::kQfGpuEvaluate);

      printf("GPU: ");
      fflush(stdout);
      rangesQuery.reset();
      rangesQuery.execute();
      queryEnd = timing_clock::now();
      showTiming("ranges query", queryStart, queryEnd, 1);

      // Extract CPU points
      saveRangesCloud(map, rangesQuery, "-ranges-gpu", prog, opt, &gpuPoints);

      // Compare.
      size_t pointLimit = cpuPoints.size();
      if (cpuPoints.size() != gpuPoints.size())
      {
        std::cout << "Point count difference. CPU: " << cpuPoints.size() << " GPU: " << gpuPoints.size() << std::endl;
        pointLimit = std::min(cpuPoints.size(), gpuPoints.size());
      }

      size_t coordMismatch = 0;
      size_t rangeMismatch = 0;
      float distSqr = 0;
      const float epsilon = 1e-4f;
      const float rangeEpsilon = float(0.3 * map.resolution());
      glm::vec3 separation;

      std::cout << "Comparing with allowed obstacle range epsilon of " << rangeEpsilon << std::endl;
      if (cpuPoints.size() == gpuPoints.size())
      {
        for (size_t i = 0; i < pointLimit; ++i)
        {
          separation = glm::vec3(cpuPoints[i]) - glm::vec3(gpuPoints[i]);
          distSqr = glm::dot(separation, separation);
          if (distSqr > epsilon)
          {
            ++coordMismatch;
          }
          // Only check range if coordinates are ok. We expect that if the coordinates don't
          // match, then neither do the ranges.
          else if (std::abs(cpuPoints[i].w - gpuPoints[i].w) > rangeEpsilon)
          {
            ++rangeMismatch;
            // Let's have a look at the generating voxel.
            ohm::Key key = map.voxelKey(glm::vec3(cpuPoints[i]));
            ohm::VoxelConst voxel = map.voxel(key);
            printf("  Range mismatch @ (%f %f %f) %f != %f : Node type %s\n",
                   cpuPoints[i].x, cpuPoints[i].y, cpuPoints[i].z,
                   cpuPoints[i].w, gpuPoints[i].w,
                   ohm::occupancyTypeToString(map.occupancyType(voxel))
                  );
          }
        }
      }
      else
      {
        for (size_t i = 0; i < gpuPoints.size(); ++i)
        {
          bool foundMatch = false;
          for (size_t j = 0; j < cpuPoints.size(); ++j)
          {
            separation = glm::vec3(cpuPoints[j]) - glm::vec3(gpuPoints[i]);
            distSqr = glm::dot(separation, separation);
            if (distSqr <= epsilon)
            {
              foundMatch = true;
              // Only check range if coordinates are ok. We expect that if the coordinates don't
              // match, then neither do the ranges.
              if (std::abs(cpuPoints[j].w - gpuPoints[i].w) > rangeEpsilon)
              {
                ++rangeMismatch;
                // Let's have a look at the generating voxel.
                ohm::Key key = map.voxelKey(glm::vec3(gpuPoints[j]));
                ohm::VoxelConst voxel = map.voxel(key);
                printf("  Range mismatch @ (%f %f %f) : %f != %f. Node [%d %d %d] %s\n",
                       gpuPoints[i].x, gpuPoints[i].y, gpuPoints[i].z,
                       gpuPoints[i].w, cpuPoints[j].w,
                       int(key.localKey().x), int(key.localKey().y), int(key.localKey().z),
                       ohm::occupancyTypeToString(map.occupancyType(voxel))
                      );
              }
              break;
            }
          }

          if (!foundMatch)
          {
            ++coordMismatch;
          }
        }
      }

      if (coordMismatch || rangeMismatch)
      {
        std::cerr << "Comparison failure. " << coordMismatch << " coordinate mismatch(es), "
                  << rangeMismatch << " range mismatch(es) of " << pointLimit << " points." << std::endl;
      }
    }
    else
    {
      saveRangesCloud(map, rangesQuery, "-ranges", prog, opt);
    }
  }
#endif // FIXME

  return 0;
}

int main(int argc, char *argv[])
{
  Options opt;

  std::cout.imbue(std::locale(""));

  int res = parseOptions(&opt, argc, argv);

  if (res)
  {
    return res;
  }

  signal(SIGINT, onSignal);
  signal(SIGTERM, onSignal);

  // Generate output name based on input if not specified.
  if (opt.output_base.empty())
  {
    const auto extension_start = opt.map_file.find_last_of('.');
    if (extension_start != std::string::npos)
    {
      opt.output_base = opt.map_file.substr(0, extension_start);
    }
    else
    {
      opt.output_base = opt.map_file;
    }
  }

  opt.print();

  // Initialise TES
  ohm::trace::init("ohmquery.3es");

#ifdef TES_ENABLE
  std::cout << "Starting with " << ohm::g_tes->connectionCount() << " connection(s)." << std::endl;
#endif // TES_ENABLE

  initialiseDebugCategories(opt);

  if (opt.use_gpu)
  {
    res = ohm::configureGpuFromArgs(argc, argv);
    if (res)
    {
      std::cerr << "Failed to configure GPU." << std::endl;
      return res;
    }
  }

  res = runQueries(opt);
  ohm::trace::done();
  return res;
}


================================================
FILE: utils/ohmsubmap/CMakeLists.txt
================================================

find_package(ZLIB)

set(SOURCES
  ohmsubmap.cpp
)

add_executable(ohmsubmap ${SOURCES})
leak_track_target_enable(ohmsubmap CONDITION OHM_LEAK_TRACK)

set_target_properties(ohmsubmap PROPERTIES FOLDER utils)
if(MSVC)
  set_target_properties(ohmsubmap PROPERTIES DEBUG_POSTFIX "d")
endif(MSVC)

target_link_libraries(ohmsubmap
  PUBLIC
    ohm
    ohmutil
  PRIVATE
    glm::glm
    $<BUILD_INTERFACE:ZLIB::ZLIB>
)

clang_tidy_target(ohmsubmap)

source_group("source" REGULAR_EXPRESSION ".*$")
# Needs CMake 3.8+:
# source_group(TREE "${CMAKE_CURRENT_LIST_DIR}" PREFIX source FILES ${SOURCES})

install(TARGETS ohmsubmap DESTINATION bin)


================================================
FILE: utils/ohmsubmap/ohmsubmap.cpp
================================================
//
// author Kazys Stepanas
//
#include <glm/glm.hpp>

#include <ohm/Aabb.h>
#include <ohm/MapLayer.h>
#include <ohm/MapSerialise.h>
#include <ohm/OccupancyMap.h>

#include <logutil/LogUtil.h>

#include <chrono>
#include <csignal>
#include <cstddef>
#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <locale>
#include <sstream>

#include <ohmutil/Options.h>

namespace
{
int g_quit = 0;

void onSignal(int arg)
{
  if (arg == SIGINT || arg == SIGTERM)
  {
    ++g_quit;
  }
}

struct V3Arg
{
  double x = 0;
  double y = 0;
  double z = 0;
};

struct Options
{
  std::string map_in;
  std::string map_out;
  ohm::Aabb box = ohm::Aabb(0);
  glm::dvec3 centre = glm::dvec3(0);
  glm::dvec3 extents = glm::dvec3(0);
};
}  // namespace


int parseOptions(Options *opt, int argc, char *argv[])  // NOLINT(modernize-avoid-c-arrays)
{
  cxxopts::Options opt_parse(argv[0], "\nExtract a submap from an existing map.\n");
  opt_parse.positional_help("<map-in.ohm> <map-out.ohm>");

  try
  {
    opt_parse.add_options()("help", "Show help.")                                                                 //
      ("i,map", "The input map file (ohm) to load.", cxxopts::value(opt->map_in))                                 //
      ("o", "The input map file (ohm) to load.", cxxopts::value(opt->map_out))                                    //
      ("box", "Specify the submap extents as six floats; minx,miny,minz,maxx,maxy,maxz", optVal(opt->box))        //
      ("centre", "Centre of the extents specified extents as three floats; x,y,z", optVal(opt->centre))           //
      ("extents", "Specify the submap extents diagonal as three floats (positive); x,y,z", optVal(opt->extents))  //
      ;

    opt_parse.parse_positional({ "i", "o" });

    cxxopts::ParseResult parsed = opt_parse.parse(argc, argv);

    if (parsed.count("help") || parsed.arguments().empty())
    {
      // show usage.
      std::cout << opt_parse.help({ "", "Group" }) << std::endl;
      return 1;
    }

    if (opt->map_in.empty())
    {
      std::cerr << "Missing input map file name" << std::endl;
      return -1;
    }

    if (opt->map_out.empty())
    {
      std::cerr << "Missing output map file name" << std::endl;
      return -1;
    }

    const double epsilon = 1e-6;
    if (glm::length(opt->box.diagonal()) < epsilon)
    {
      // No box. Try centre/extents setup.
      opt->box = ohm::Aabb::fromCentreHalfExtents(opt->centre, 0.5 * opt->extents);

      if (glm::length(opt->box.diagonal()) < epsilon)
      {
        // Still bad.
        std::cerr << "Extents not specified or too small" << std::endl;
        return -1;
      }
    }

    std::cout << "Extents: (" << opt->box << ")" << std::endl;

    if (opt->box.halfExtents().x <= 0 || opt->box.halfExtents().y <= 0 && opt->box.halfExtents().z <= 0)
    {
      std::cerr << "Zero or negative extents" << std::endl;
      return -1;
    }
  }
  catch (const cxxopts::OptionException &e)
  {
    std::cerr << "Argument error\n" << e.what() << std::endl;
    return -1;
  }

  return 0;
}

int main(int argc, char *argv[])
{
  Options opt;

  std::cout.imbue(std::locale(""));

  int res = 0;
  res = parseOptions(&opt, argc, argv);

  if (res)
  {
    return res;
  }

  signal(SIGINT, onSignal);
  signal(SIGTERM, onSignal);

  ohm::OccupancyMap map(1.0f);

  std::cout << "Loading" << std::flush;
  res = ohm::load(opt.map_in.c_str(), map);
  std::cout << std::endl;

  if (res != 0)
  {
    std::cerr << "Failed to load map. Error(" << res << "): " << ohm::serialiseErrorCodeString(res) << std::endl;
    return res;
  }

  std::cout << "Cloning" << std::flush;
  // Clone the map
  const std::unique_ptr<ohm::OccupancyMap> map_copy(map.clone(opt.box.minExtents(), opt.box.maxExtents()));
  std::cout << std::endl;

  std::cout << "Saving" << std::flush;
  res = ohm::save(opt.map_out.c_str(), *map_copy);
  std::cout << std::endl;

  return res;
}


================================================
FILE: vcpkg.json
================================================
{
  "$schema": "https://raw.githubusercontent.com/microsoft/vcpkg/master/scripts/vcpkg.schema.json",
  "name": "ohm",
  "version": "0.2.0",
  "default-features": [
    "eigen",
    "heightmap",
    "threads"
  ],
  "features": {
    "cuda": {
      "description": "Enable CUDA acceleration libraries.",
      "dependencies": [
        {
          "name": "cuda",
          "platform": "!windows"
        }
      ]
    },
    "eigen": {
      "description": "Enable Eigen support for faster Geometry operation.",
      "dependencies": [
        "eigen3"
      ]
    },
    "heightmap": {
      "description": "Support heightmap generation."
    },
    "heightmap-image": {
      "description": "Support heightmap rendering to image.",
      "dependencies": [
        "glew",
        "glfw3",
        "libpng",
        "opengl"
      ]
    },
    "opencl": {
      "description": "Enable OpenCL acceleration libraries.",
      "dependencies": [
        "opencl"
      ]
    },
    "pdal": {
      "description": "Enable PDAL point cloud loader.",
      "dependencies": [
        {
          "name": "pdal",
          "features": [
            "draco",
            "e57"
          ]
        }
      ]
    },
    "threads": {
      "description": "Build with Intel Threading Building Blocks.",
      "dependencies": [
        "tbb"
      ]
    },
    "test": {
      "description": "With unit tests",
      "dependencies": [
        "gtest"
      ]
    }
  },
  "dependencies": [
    "glm",
    "zlib"
  ]
}