JsonStreamer.cpp

/* ----------------------
* Copyright 2023 Université Libre de Bruxelles(ULB), Universidad Politécnica de Madrid(UPM), CREAL, Deutsches Zentrum für Luft - und Raumfahrt(DLR)
 
* 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%3E
 
* 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 permissionsand
* limitations under the License.
---------------------- */
 
 
#include "JsonStreamer.h"
 
#include <array>
#include <iostream>
 
VULKAN_HPP_DEFAULT_DISPATCH_LOADER_DYNAMIC_STORAGE;
 
static std::pair<vk::Format, size_t> vkColorFormatFromParams(const rvs::Parameters& params) {
  switch(params.getColorBitDepth()){
    case 16:
      return {vk::Format::eG16B16R163Plane420Unorm, 6};
    case 10:
      return {vk::Format::eG10X6B10X6R10X63Plane420Unorm3Pack16, 6};
    default:
      return {vk::Format::eG8B8R83Plane420Unorm, 3};
  }
}
 
static std::pair<vk::Format, size_t> vkDepthFormatFromParams(const rvs::Parameters& params) {
  switch(params.getDepthBitDepth()){
    case 16:
      return {vk::Format::eR16Unorm, 2};
    case 10:
      return {vk::Format::eR10X6UnormPack16, 2};
    default:
      return {vk::Format::eR8Unorm, 1};
  }
}
 
int JsonStreamer::ReadStream::frameIndex(Clock::duration time) const {
  return (time / framePeriod) % frameCount;
}
 
bool JsonStreamer::ReadStream::nextFrameReady(Clock::duration time) const {
 
  return streamedFrame != frameIndex(time);
}
 
JsonStreamer::JsonStreamer(std::span<const uint8_t, VK_UUID_SIZE> uuid)
{
  initSettings();
  initVk(uuid);
  initVkResources();
}
 
void JsonStreamer::initVk(std::span<const uint8_t, VK_UUID_SIZE> uuid) {
  vk::DynamicLoader dl;
  VULKAN_HPP_DEFAULT_DISPATCHER.init(dl.getProcAddress<PFN_vkGetInstanceProcAddr>("vkGetInstanceProcAddr"));
 
  constexpr auto neededInstanceExts = std::array{
      VK_KHR_EXTERNAL_MEMORY_CAPABILITIES_EXTENSION_NAME,
      VK_KHR_EXTERNAL_SEMAPHORE_CAPABILITIES_EXTENSION_NAME,
  };
 
  vk::ApplicationInfo ainfos(
    "JsonStreamer", VK_MAKE_VERSION(0,0,1), "No Engine", VK_MAKE_VERSION(0,0,0), VK_API_VERSION_1_2
  );
 
  instance = vk::createInstanceUnique(
        vk::InstanceCreateInfo(
          {}, //Flags
          &ainfos,
          {},
          neededInstanceExts
          ));
 
  VULKAN_HPP_DEFAULT_DISPATCHER.init(*instance);
 
  constexpr auto neededDeviceExts = std::array{
    #ifdef WIN32
      VK_KHR_EXTERNAL_MEMORY_WIN32_EXTENSION_NAME,
      VK_KHR_EXTERNAL_SEMAPHORE_WIN32_EXTENSION_NAME,
    #else
      VK_KHR_EXTERNAL_MEMORY_FD_EXTENSION_NAME,
      VK_KHR_EXTERNAL_SEMAPHORE_FD_EXTENSION_NAME
    //no need for this
    #endif
  };
 
  auto phyDevs = instance->enumeratePhysicalDevices();
 
  auto it = std::find_if(phyDevs.begin(), phyDevs.end(), [&](const vk::PhysicalDevice& phy){
    auto idProps = phy.getProperties2<vk::PhysicalDeviceProperties2, vk::PhysicalDeviceIDProperties>().get<vk::PhysicalDeviceIDProperties>();
    return strncmp((const char*)idProps.deviceUUID.data(), (const char*)uuid.data(), VK_UUID_SIZE) == 0;
  });
 
  if(it == phyDevs.end()){
    throw std::runtime_error("Could not find physical device corresponding to UUID");
  }
  phyDevice = *it; //TODO find device with right UUID
 
  auto props = phyDevice.getProperties();
  std::cout << "Loaded streaming device : " << props.deviceName << std::endl;
 
  float priority[] = { 1.f,1.0f };
 
  queueFamilyIndex = 0;
  auto queuesInfos = std::array{vk::DeviceQueueCreateInfo({}, queueFamilyIndex, 2, priority)};
 
  device = phyDevice.createDeviceUnique(
        vk::DeviceCreateInfo(
          {}, //Flags
          queuesInfos, //Queues
          {}, //Layers
          neededDeviceExts
          ));
 
  VULKAN_HPP_DEFAULT_DISPATCHER.init(*device);
 
  queue = device->getQueue(queueFamilyIndex, 0);
  syncQueue = device->getQueue(queueFamilyIndex, 1);
}
 
template<typename Closure>
void JsonStreamer::oneTimeSubmit(Closure&& func, vk::SubmitInfo submitInfo) {
  static thread_local vk::UniqueCommandPool commandPool;
  static thread_local vk::UniqueFence oneTimeFence;
 
  if(!commandPool) {
    commandPool = device->createCommandPoolUnique(
          vk::CommandPoolCreateInfo({}, queueFamilyIndex)); //TODO choose proper queueFamilyIndex
  }
 
  if(!oneTimeFence) {
    oneTimeFence = device->createFenceUnique(vk::FenceCreateInfo());
  }
 
  auto commandBuffers = device->allocateCommandBuffersUnique(
        vk::CommandBufferAllocateInfo(
          *commandPool,
          vk::CommandBufferLevel::ePrimary,
          1));
  auto& cmdbuf = *commandBuffers.front();
 
  cmdbuf.begin(vk::CommandBufferBeginInfo{});
  func(cmdbuf);
  cmdbuf.end();
 
  submitInfo.setCommandBuffers(cmdbuf);
 
  device->resetFences(*oneTimeFence);
  {
    std::scoped_lock l(queueMutex);
    queue.submit(submitInfo, *oneTimeFence);
  }
  (void)device->waitForFences(*oneTimeFence, VK_TRUE, UINT64_MAX);
}
 
void JsonStreamer::initSettings() {
  const char* jsonPath = getenv("HVT_JSON_PATH");
 
  if(!jsonPath) {
    std::cerr << "No HVT_JSON_PATH env variable set, cannot load settings" << std::endl;
    throw HvtResult::HVT_ERROR_NOT_FOUND;
  }
 
  auto path = std::filesystem::path(jsonPath);
  if(!std::filesystem::exists(path)) {
    std::cerr << "File " << path << " does not exist" << std::endl;
    throw HvtResult::HVT_ERROR_NOT_FOUND;
  }
 
  auto dir = std::filesystem::path(path).parent_path();
 
  config = rvs::Config::loadFromFile(path.filename().string(), dir.string());
 
  size_t numViews = config->InputCameraNames.size();
  if(config->params_real.size() != numViews ||
     config->depth_names.size() != numViews ||
     config->texture_names.size() != numViews){
    throw std::runtime_error("File and settings size mismatch");
  }
 
  auto frameRate = 30; //TODO read this from somewhere....
  using namespace std::chrono_literals;
  auto framePeriod = std::chrono::nanoseconds(1s) / frameRate;
 
  for(size_t i = 0; i < numViews; i++) {
    const auto& params = config->params_real.at(i);
    const auto& colorFileName = config->texture_names.at(i);
    const auto& depthFileName = config->depth_names.at(i);
 
    std::filesystem::path cp(colorFileName);
    auto colorFilePath = cp.is_absolute() ? cp : dir / cp;
 
    std::filesystem::path dp(depthFileName);
    auto depthFilePath = dp.is_absolute() ? dp : dir / dp;
 
 
    auto colorFile = std::ifstream(colorFilePath, std::ifstream::ate | std::ifstream::binary | std::ifstream::in);
    if(!colorFile.is_open()) {
      throw std::runtime_error(colorFileName); //TODO better error message
    }
 
    auto depthFile = std::ifstream(depthFilePath, std::ifstream::ate | std::ifstream::binary | std::ifstream::in);
    if(!depthFile.is_open()) {
      throw std::runtime_error(depthFileName);
    }
 
    auto [colorFormat, colorHalfBytesPerPixel] = vkColorFormatFromParams(params);
    auto [depthFormat, depthBytesPerPixel] = vkDepthFormatFromParams(params);
 
    size_t pixelCount = params.getSize().width * params.getSize().height;
 
    size_t colorSize = (pixelCount * colorHalfBytesPerPixel) / 2;
    size_t depthSize = pixelCount * depthBytesPerPixel;
 
    size_t depthStride = depthSize * 3 / 2;
    if (config->params_real[i].getDepthColorFormat() == rvs::ColorFormat::YUV400) {
        depthStride = depthSize;
    }
 
    auto colorFileSize = colorFile.tellg();
    auto depthFileSize = depthFile.tellg();
    auto frameCount = colorFileSize / colorSize;
    if(frameCount != (depthFileSize / depthStride)) {
      throw std::runtime_error("Mismatching depth and color framecount");
    }
 
 
 
    HvtIntrinsics intrinsics;
    HvtProjectionType ptype;
    if(params.getProjectionType() == "Perspective"){
      ptype = HvtProjectionType::HVT_PROJECTION_PERSPECTIVE;
      intrinsics.perspective = HvtIntrinsicsPerspective{
        .focalX = params.getFocal()[0],
        .focalY = params.getFocal()[1],
        .principlePointX = params.getPrinciplePoint()[0],
        .principlePointY = params.getPrinciplePoint()[1]
      };
    } else if(params.getProjectionType() == "Equirectangular") {
      ptype = HvtProjectionType::HVT_PROJECTION_EQUIRECTANGULAR;
      intrinsics.equirectangular = HvtIntrinsicsEquirectangular{
        .verticalRange = {params.getVerRange()[0], params.getVerRange()[1]},
        .horizontalRange = {params.getHorRange()[0], params.getHorRange()[1]},
      };
    } else {
      throw std::runtime_error("Unknown projection type " + params.getProjectionType());
    }
 
    HvtExtrinsics extrinsics = {
      .position = {params.getPosition()[0], params.getPosition()[1], params.getPosition()[2]},
      .rotation = {params.getRotation()[0], params.getRotation()[1], params.getRotation()[2]}
    };
 
    readStreams.push_back(
          ReadStream{
            .colorFile = std::move(colorFile),
            .depthFile = std::move(depthFile),
            .colorFrameStride = colorSize,
            .colorFrameSize = colorSize,
            .depthFrameStride = depthStride,
            .depthFrameSize = depthSize,
            .resolution = {params.getSize().width, params.getSize().height},
            .projectionType = ptype,
            .intrinsics = intrinsics,
            .extrinsics = extrinsics,
            .anear = params.getDepthRange()[0],
            .afar = params.getDepthRange()[1],
            .colorFormat = colorFormat,
            .depthFormat = depthFormat,
            .framePeriod = framePeriod,
            .frameCount = (int)frameCount
          });
  }
}
 
void JsonStreamer::initVkResources() {
  for(ReadStream& stream : readStreams) {
    auto colorSize = stream.colorFrameSize;
    auto depthSize = stream.depthFrameSize;
    auto buffer = device->createBufferUnique(
          vk::BufferCreateInfo(
            {}, //Flags,
            colorSize+depthSize,
            vk::BufferUsageFlagBits::eTransferSrc,
            vk::SharingMode::eExclusive)
          );
 
    auto reqs = device->getBufferMemoryRequirements(*buffer);
    auto memIndex = findMemoryType(reqs.memoryTypeBits, vk::MemoryPropertyFlagBits::eHostCoherent | vk::MemoryPropertyFlagBits::eHostVisible);
 
    auto memory = device->allocateMemoryUnique(
          vk::MemoryAllocateInfo(reqs.size, memIndex));
 
    device->bindBufferMemory(*buffer, *memory, 0);
    auto mappedStagging = device->mapMemory(*memory, 0, colorSize+depthSize);
 
    stream.stagingBuffer = std::move(buffer);
    stream.stagingMemory = std::move(memory);
    stream.stagingMapping = (char*)mappedStagging;
  }
}
 
uint32_t JsonStreamer::findMemoryType(uint32_t typeFilter, vk::MemoryPropertyFlags properties)
{
  vk::PhysicalDeviceMemoryProperties memProperties = phyDevice.getMemoryProperties();
  for (uint32_t i = 0; i < memProperties.memoryTypeCount; i++) {
    if ((typeFilter & (1 << i)) && (memProperties.memoryTypes[i].propertyFlags & properties) == properties) {
      return i;
    }
  }
  throw std::runtime_error("failed to find suitable memory type!");
}
 
void JsonStreamer::enumerateStreamsParameters(uint32_t* streamsCount, HvtRGBDStreamParameters* parameters) const {
  if(!parameters) {
    *streamsCount = readStreams.size();
    return;
  }
 
  if(*streamsCount != readStreams.size()){
    throw HvtResult::HVT_ERROR_WRONG_BUFFER_SIZE;
  }
 
  for(int i = 0; const auto& readStream : readStreams) {
    parameters[i] = HvtRGBDStreamParameters{
      .colorResolution = {(uint32_t)readStream.resolution.x, (uint32_t)readStream.resolution.y},
      .depthResolution = {(uint32_t)readStream.resolution.x, (uint32_t)readStream.resolution.y},
 
      .nearDepth = readStream.anear,
      .farDepth = readStream.afar,
 
      .colorFormat = (HvtImageFormat)readStream.colorFormat,
      .depthFormat = (HvtImageFormat)readStream.depthFormat,
 
      .slotCount = numSlots,
      .projectionType = readStream.projectionType
    };
    snprintf(parameters->name, HVT_MAX_STREAM_NAME_LENGHT, "%s", config->InputCameraNames.at(i).c_str());
    i++;
  }
}
 
void JsonStreamer::importStreamImages(const HvtStreamImagesExportInfo& exportInfos) {
  auto memtype = (vk::ExternalMemoryHandleTypeFlagBits)exportInfos.memoryType;
 
#ifdef WIN32
  if(memtype != vk::ExternalMemoryHandleTypeFlagBits::eOpaqueWin32) 
  #else 
  if(memtype != vk::ExternalMemoryHandleTypeFlagBits::eOpaqueFd)
  #endif
  {
    throw HvtResult::HVT_ERROR_UNSUPPORTED_MEMORY_TYPE;
  }
 
  auto isDepth = (bool)exportInfos.depth;
  auto& stream = readStreams.at(exportInfos.streamIndex);
  auto format = isDepth ? stream.depthFormat : stream.colorFormat;
  auto& slots = isDepth ? stream.depthSlots : stream.colorSlots;
 
  if(exportInfos.imagesCount != numSlots) {
    throw HvtResult::HVT_ERROR_WRONG_BUFFER_SIZE;
  }
 
  vk::ImageCreateInfo imgCreateInfo(
        {}, //Flags,
        vk::ImageType::e2D,
        format,
        vk::Extent3D(stream.resolution.x, stream.resolution.y, 1),
        1, //Mip count,
        1, //Array layers,
        vk::SampleCountFlagBits::e1,
        vk::ImageTiling::eOptimal,
        vk::ImageUsageFlagBits::eTransferDst,
        vk::SharingMode::eExclusive,
        {}, //Queue families
        vk::ImageLayout::eUndefined
  );
 
  for(int i = 0; i < numSlots; i++) {
    auto memoryInfos = exportInfos.pImages[i];
 
    auto imgImportInfo = vk::ExternalMemoryImageCreateInfo(memtype);
    auto createChain = vk::StructureChain(imgCreateInfo, imgImportInfo);
 
    auto image = device->createImageUnique(createChain.get<vk::ImageCreateInfo>());
    auto reqs = device->getImageMemoryRequirements(*image);
 
    auto memIndex = findMemoryType(reqs.memoryTypeBits, vk::MemoryPropertyFlagBits::eDeviceLocal);
 
    assert(reqs.size == memoryInfos.size);
    assert(reqs.alignment == memoryInfos.alignment);
 
    auto memoryAllocInfo = vk::StructureChain(
          vk::MemoryAllocateInfo(reqs.size, memIndex),
          #ifdef WIN32
          vk::ImportMemoryWin32HandleInfoKHR(memtype, memoryInfos.handle)
          #else
          vk::ImportMemoryFdInfoKHR(memtype, memoryInfos.handle)
          #endif
          );
 
    auto memory = device->allocateMemoryUnique(memoryAllocInfo.get<vk::MemoryAllocateInfo>());
 
    device->bindImageMemory(*image, *memory, 0);
 
    // Transition images to their unique layout
    oneTimeSubmit([&](vk::CommandBuffer& cmd){
      vk::ImageMemoryBarrier colorMemoryBarrier(
            {},
            vk::AccessFlagBits::eTransferWrite,
            vk::ImageLayout::eUndefined, //Don't care about what was there previously
            vk::ImageLayout::eTransferDstOptimal,
            VK_QUEUE_FAMILY_IGNORED, // No queue ownership transfer
            VK_QUEUE_FAMILY_IGNORED,
            *image,
            vk::ImageSubresourceRange(
              vk::ImageAspectFlagBits::ePlane0 | vk::ImageAspectFlagBits::ePlane1 | vk::ImageAspectFlagBits::ePlane2,
              0, 1,
              0, 1));
 
      vk::ImageMemoryBarrier depthMemoryBarrier(
            {},
            vk::AccessFlagBits::eTransferWrite,
            vk::ImageLayout::eUndefined,
            vk::ImageLayout::eTransferDstOptimal,
            VK_QUEUE_FAMILY_IGNORED,
            VK_QUEUE_FAMILY_IGNORED,
            *image,
            vk::ImageSubresourceRange(
              vk::ImageAspectFlagBits::eColor,
              0, 1,
              0, 1));
 
      cmd.pipelineBarrier(
            vk::PipelineStageFlagBits::eTransfer,
            vk::PipelineStageFlagBits::eTransfer,
            vk::DependencyFlagBits::eByRegion,
            {},
            {},
            isDepth ? depthMemoryBarrier : colorMemoryBarrier );
    });
 
    slots.at(i) = ImageSlot{
      .image = std::move(image),
      .memory = std::move(memory)
    };
  }
  (isDepth ? stream.importedDepth : stream.importedColor) =  true;
}
 
Semaphore* JsonStreamer::importSemaphore(const HvtSemaphoreExportInfo& exportInfos) {
  #ifdef WIN32
  if((vk::ExternalSemaphoreHandleTypeFlagBits)exportInfos.type != vk::ExternalSemaphoreHandleTypeFlagBits::eOpaqueWin32){
    #else
    if((vk::ExternalSemaphoreHandleTypeFlagBits)exportInfos.type != vk::ExternalSemaphoreHandleTypeFlagBits::eOpaqueFd){
    #endif
    throw HvtResult::HVT_ERROR_UNSUPPORTED_SEMAPHORE_TYPE;
  }
 
  auto sem = device->createSemaphoreUnique({});
  #ifdef WIN32
  device->importSemaphoreWin32HandleKHR(
        vk::ImportSemaphoreWin32HandleInfoKHR(
          *sem,
          {},
          vk::ExternalSemaphoreHandleTypeFlagBits::eOpaqueWin32,
          exportInfos.semaphore
        ));
  #else
  device->importSemaphoreFdKHR(
        vk::ImportSemaphoreFdInfoKHR(
          *sem,
                  {},
          vk::ExternalSemaphoreHandleTypeFlagBits::eOpaqueFd,
          exportInfos.semaphore
        ));
  #endif
  return new Semaphore{
    .sem = std::move(sem)
  };
}
 
void JsonStreamer::destroySemaphore(Semaphore* sem) const {
  delete sem;
}
 
void JsonStreamer::startStreaming() {
  //assert that we have everything ready for streaming
  for(auto& stream : readStreams) {
    if(!stream.importedColor || !stream.importedDepth) {
      throw HvtResult::HVT_ERROR_CALL_ORDER;
    }
  }
 
  // Start worker
  running = true;
  streamingThread = std::thread([&]{
    streamingLoop();
  });
}
 
void JsonStreamer::acquireStreamsFrames(const HvtAcquireStreamFramesInfo& infos) {
  if(infos.frameInfoCount != readStreams.size()) {
    throw HvtResult::HVT_ERROR_WRONG_BUFFER_SIZE;
  }
 
  swapPendingToReading();
 
  auto imageIndex = slotReadingIndex;
 
  for(int i = 0; i < infos.frameInfoCount; i++) {
    auto& stream = readStreams.at(i);
    auto& desc = infos.pStreamFrameInfos[i];
    desc.extrinsics = stream.extrinsics;
    desc.intrinsics = stream.intrinsics;
    desc.imageIndex = imageIndex;
  }
 
  if(infos.signalSemaphore){
    auto sem = Semaphore::check(infos.signalSemaphore);
    std::scoped_lock l(queueMutex);
    syncQueue.submit(
          vk::SubmitInfo(
            {},
            {},
            {},
            *sem->sem));
  }
}
 
void JsonStreamer::releaseStreamsFrames(Semaphore* waitSem) {
  std::scoped_lock l(queueMutex);
 
  if(waitSem) {
    vk::PipelineStageFlags stage = vk::PipelineStageFlagBits::eTopOfPipe;
    syncQueue.submit(
          vk::SubmitInfo(
            *waitSem->sem,
            stage,
            {},
            {}));
  }
}
 
void JsonStreamer::stopStreaming() {
  running = false;
  if(streamingThread.joinable()) {
    streamingThread.join();
  }
}
 
JsonStreamer::~JsonStreamer() {
  stopStreaming();
}
 
void JsonStreamer::streamingLoop() {
 
  auto startTime = Clock::now();
 
  while(running) {
 
    auto now = Clock::now();
    auto time = now-startTime;
 
    int readyCount = 0;
    for(auto& stream : readStreams){
      if(stream.nextFrameReady(time)){
        readyCount++;
      }
    }
 
    if(readyCount == readStreams.size()) {
 
      oneTimeSubmit([&](vk::CommandBuffer& cmd){
        for(int i = 0; auto& stream : readStreams){
          if(stream.nextFrameReady(time)){
            uploadFrame(cmd, i, stream, stream.frameIndex(time));
            stream.streamedFrame = stream.frameIndex(time);
          }
          i++;
        }
      });
 
      // Commit
      swapStreamingToPending();
    }
    std::this_thread::yield();
  }
}
 
void JsonStreamer::uploadFrame(vk::CommandBuffer cmd, int streamId, ReadStream &stream, int frame) {
  auto colorFileOffset = frame * stream.colorFrameStride;
  auto depthFileOffset = frame * stream.depthFrameStride;
 
  stream.colorFile.seekg(colorFileOffset, std::ios_base::beg);
  stream.depthFile.seekg(depthFileOffset, std::ios_base::beg);
 
  // Those call might potentially block a lot on slow hdds....
  stream.colorFile.read(stream.stagingMapping, stream.colorFrameSize);
  stream.depthFile.read(stream.stagingMapping+stream.colorFrameSize, stream.depthFrameSize);
 
  if(!stream.colorFile) {
    std::cerr << "Failed to read more than " << stream.colorFile.gcount() << " bytes of data" << std::endl;
  }
 
  if(!stream.depthFile) {
    std::cerr << "Failed to read more than " << stream.depthFile.gcount() << " bytes of data" << std::endl;
  }
 
  auto imageIndex = slotStreamingIndex;
 
  auto& dstColorSlot = stream.colorSlots.at(imageIndex);
  auto& dstDepthSlot = stream.depthSlots.at(imageIndex);
 
  {
    auto mainStride = (stream.colorFrameSize*2) / 3;
    auto secondStride = mainStride / 4;
    auto half_res = stream.resolution / 2;
 
    //Copy to color //TODO CHECK YUV PLANES
    cmd.copyBufferToImage(
          *stream.stagingBuffer,
          *dstColorSlot.image,
          vk::ImageLayout::eTransferDstOptimal,
          {
            vk::BufferImageCopy(
              0,
              stream.resolution.x,
              stream.resolution.y,
              vk::ImageSubresourceLayers(
                vk::ImageAspectFlagBits::ePlane0,
                0, 0, 1
              ),
              vk::Offset3D(0,0,0),
              vk::Extent3D(stream.resolution.x, stream.resolution.y, 1)
            ),
            vk::BufferImageCopy(
              mainStride,
              half_res.x,
              half_res.y,
              vk::ImageSubresourceLayers(
                vk::ImageAspectFlagBits::ePlane1,
                0, 0, 1
              ),
              vk::Offset3D(0,0,0),
              vk::Extent3D(half_res.x, half_res.y, 1)
            ),
            vk::BufferImageCopy(
              mainStride+secondStride,
              half_res.x,
              half_res.y,
              vk::ImageSubresourceLayers(
                vk::ImageAspectFlagBits::ePlane2,
                0, 0, 1
              ),
              vk::Offset3D(0,0,0),
              vk::Extent3D(half_res.x, half_res.y, 1)
            )
          });
  }
 
  //Copy to depth
  cmd.copyBufferToImage(
        *stream.stagingBuffer,
        *dstDepthSlot.image,
        vk::ImageLayout::eTransferDstOptimal,
        {vk::BufferImageCopy(
         stream.colorFrameSize,
         stream.resolution.x,
         stream.resolution.y,
         vk::ImageSubresourceLayers(
          vk::ImageAspectFlagBits::eColor,
          0, 0, 1
         ),
         vk::Offset3D(0,0,0),
         vk::Extent3D(stream.resolution.x, stream.resolution.y, 1)
         )});
}
 
void JsonStreamer::swapStreamingToPending() {
  std::scoped_lock l(indicesMutex);
  std::swap(slotPendingIndex, slotStreamingIndex);
  newDataInPending = true;
  //printf("Swapped %d to pending\n", slotPendingIndex);
}
 
void JsonStreamer::swapPendingToReading() {
  std::scoped_lock l(indicesMutex);
  if(newDataInPending) {
    std::swap(slotPendingIndex, slotReadingIndex);
    newDataInPending = false;
    //printf("Swapped %d to reading\n", slotReadingIndex);
  }
}
 
 
template<typename Closure>
HvtResult exceptionFirewall(Closure&& clos) {
  try {
    clos();
  } catch(HvtResult res) {
    return res;
  } catch(const std::exception& e) {
    std::cerr << "Catched exception at C boundary : \"" << e.what() << "\"" << std::endl;
    return HvtResult::HVT_ERROR_UNKNOWN;
  }  catch (...) {
    return HvtResult::HVT_ERROR_UNKNOWN;
  }
  return HvtResult::HVT_SUCESS;
}
 
template<typename T>
void checkNonNull(T ptr){
  if(!ptr){
    throw HvtResult::HVT_ERROR_INVALID_HANDLE;
  }
}
 
 
extern "C" {
 
HVTAPI_ATTR HvtResult HVTAPI_CALL hvtCreateStreamingContext(const HvtStreamingContextCreateInfo* createInfo, HvtStreamingContext* outStreamingContext) {
  return exceptionFirewall([&]{
    checkNonNull(createInfo);
 
    if(createInfo->headerVersion != HVT_HEADER_VERSION) {
      throw HvtResult::HVT_ERROR_HEADER_VERSION;
    }
 
    auto context = new JsonStreamer(createInfo->graphicsDeviceUUID);
    *outStreamingContext = context->to_handle();
  });
}
 
HVTAPI_ATTR HvtResult HVTAPI_CALL hvtEnumerateStreamsParameters(HvtStreamingContext streamingContext, uint32_t* pStreamParameterCount, HvtRGBDStreamParameters* pStreamParameters) {
  return exceptionFirewall([&]{
    auto context = JsonStreamer::check(streamingContext);
    checkNonNull(pStreamParameterCount);
    context->enumerateStreamsParameters(pStreamParameterCount, pStreamParameters);
  });
}
 
HVTAPI_ATTR HvtResult HVTAPI_CALL hvtExportStreamImages(HvtStreamingContext streamingContext, const HvtStreamImagesExportInfo* exportInfos) {
  return exceptionFirewall([&]{
    auto context = JsonStreamer::check(streamingContext);
    checkNonNull(exportInfos);
    context->importStreamImages(*exportInfos);
  });
}
 
HVTAPI_ATTR HvtResult HVTAPI_CALL hvtExportSemaphore(HvtStreamingContext streamingContext, const HvtSemaphoreExportInfo* exportInfo, HvtSemaphore* outSemaphore) {
  return exceptionFirewall([&]{
    auto context = JsonStreamer::check(streamingContext);
    checkNonNull(exportInfo);
    auto sem = context->importSemaphore(*exportInfo);
    *outSemaphore = sem->to_handle();
  });
}
 
HVTAPI_ATTR HvtResult HVTAPI_CALL hvtDestroySemaphore(HvtStreamingContext streamingContext, HvtSemaphore semaphore) {
  return exceptionFirewall([&]{
    auto context = JsonStreamer::check(streamingContext);
    context->destroySemaphore(Semaphore::check(semaphore));
  });
}
 
 
HVTAPI_ATTR HvtResult HVTAPI_CALL hvtStartStreaming(HvtStreamingContext streamingContext) {
  return exceptionFirewall([&]{
    auto context = JsonStreamer::check(streamingContext);
    context->startStreaming();
  });
}
 
HVTAPI_ATTR HvtResult HVTAPI_CALL hvtAcquireStreamsFrames(HvtStreamingContext streamingContext, const HvtAcquireStreamFramesInfo* infos) {
  return exceptionFirewall([&]{
    auto context = JsonStreamer::check(streamingContext);
    context->acquireStreamsFrames(*infos);
  });
}
 
HVTAPI_ATTR HvtResult HVTAPI_CALL hvtReleaseStreamsFrames(HvtStreamingContext streamingContext, HvtSemaphore waitSemaphore) {
  return exceptionFirewall([&]{
    auto context = JsonStreamer::check(streamingContext);
    context->releaseStreamsFrames(Semaphore::opt_check(waitSemaphore));
  });
}
 
HVTAPI_ATTR HvtResult HVTAPI_CALL hvtStopStreaming(HvtStreamingContext streamingContext) {
  return exceptionFirewall([&]{
    auto context = JsonStreamer::check(streamingContext);
    context->stopStreaming();
  });
}
 
HVTAPI_ATTR HvtResult HVTAPI_CALL hvtDestroyStreamingContext(HvtStreamingContext streamingContext) {
  return exceptionFirewall([&]{
    auto context = JsonStreamer::check(streamingContext);
    delete context;
  });
}
 
}