text: /*
   * Copyright (C) 2018 Open Source Robotics Foundation
   *
   * 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.
   *
  */
  
  #include <ignition/common/Mesh.hh>
  #include <ignition/common/MeshManager.hh>
  #include <ignition/common/SubMesh.hh>
  
  #include <ignition/math/Color.hh>
  #include <ignition/math/Helpers.hh>
  #include <ignition/math/Vector3.hh>
  
  #include "ignition/rendering/RenderTypes.hh"
  #include "ignition/rendering/ogre/OgreCamera.hh"
  #include "ignition/rendering/ogre/OgreGpuRays.hh"
  
  /// \internal
  /// \brief Private data for the OgreGpuRays class
  class ignition::rendering::OgreGpuRaysPrivate
  {
    /// \brief Event triggered when new gpu rays range data are available.
    /// \param[in] _frame New frame containing raw gpu rays data.
    /// \param[in] _width Width of frame.
    /// \param[in] _height Height of frame.
    /// \param[in] _channels Number of channels
    /// \param[in] _format Format of frame.
    public: ignition::common::EventT<void(const float *,
                 unsigned int, unsigned int, unsigned int,
                 const std::string &)> newGpuRaysFrame;
  
    /// \brief Raw buffer of gpu rays data.
    public: float *gpuRaysBuffer = nullptr;
  
    /// \brief Outgoing gpu rays data, used by newGpuRaysFrame event.
    public: float *gpuRaysScan = nullptr;
  
    /// \brief Pointer to Ogre material for the first rendering pass.
    public: Ogre::Material *matFirstPass = nullptr;
  
    /// \brief Pointer to Ogre material for the sencod rendering pass.
    public: Ogre::Material *matSecondPass = nullptr;
  
    /// \brief Temporary pointer to the current material.
    public: Ogre::Material *currentMat = nullptr;
  
    /// \brief An array of first pass textures.
    public: Ogre::Texture *firstPassTextures[3];
  
    /// \brief Second pass texture.
    public: Ogre::Texture *secondPassTexture = nullptr;
  
    /// \brief Temporary pointer to the current render target.
    public: Ogre::Texture *currentTexture = nullptr;
  
    /// \brief Ogre orthorgraphic camera used in the second pass for
    /// undistortion.
    public: Ogre::Camera *orthoCam = nullptr;
  
    /// \brief Pointer to the ogre camera
    public: Ogre::Camera *ogreCamera = nullptr;
  
    /// \brief Ogre scenenode where the orthorgraphic camera is attached to.
    public: Ogre::SceneNode *pitchNodeOrtho = nullptr;
  
    /// \brief Ogre mesh used to create a canvas for undistorting range values
    /// in the second rendering pass.
    public: common::Mesh *undistMesh = nullptr;
  
    /// \brief Pointer to visual that holds the canvas.
    public: VisualPtr visual;
  
    /// \brief Number of first pass textures.
    public: unsigned int textureCount = 0u;
  
    /// \brief A list of camera angles for first pass rendering.
    public: double cameraYaws[4];
  
    /// \brief Image width of first pass.
    public: unsigned int w1st = 0u;
  
    /// \brief Image height of first pass.
    public: unsigned int h1st = 0u;
  
    /// \brief Image width of second pass.
    public: unsigned int w2nd = 0u;
  
    /// \brief Image height of second pass.
    public: unsigned int h2nd = 0u;
  
    /// \brief List of texture unit indices used during the second
    /// rendering pass.
    public: std::vector<int> texIdx;
  
    /// Number of second pass texture units created.
    public: unsigned int texCount = 0;
  
    /// \brief Dummy render texture for the gpu rays
    public: RenderTexturePtr renderTexture;
  
    /// \brief Cos horizontal field-of-view.
    public: double chfov = 0;
  
    /// \brief Cos vertical field-of-view.
    public: double cvfov = 0;
  
    /// \brief Horizontal half angle.
    public: double horzHalfAngle = 0;
  
    /// \brief Vertical half angle.
    public: double vertHalfAngle = 0;
  
    /// \brief Number of cameras needed to generate the rays.
    public: unsigned int cameraCount = 1;
  };
  
  using namespace ignition;
  using namespace rendering;
  
  //////////////////////////////////////////////////
  OgreGpuRays::OgreGpuRays()
    : dataPtr(new OgreGpuRaysPrivate)
  {
    // r = depth, g = retro, and b = n/a
    this->channels = 3u;
  }
  
  //////////////////////////////////////////////////
  OgreGpuRays::~OgreGpuRays()
  {
    this->Destroy();
  }
  
  //////////////////////////////////////////////////
  void OgreGpuRays::Init()
  {
    BaseGpuRays::Init();
  
    // create internal camera for 1st pass
    this->CreateCamera();
  
    // create dummy render texture
    this->CreateRenderTexture();
  }
  
  //////////////////////////////////////////////////
  void OgreGpuRays::Destroy()
  {
    if (this->dataPtr->gpuRaysBuffer)
    {
      delete [] this->dataPtr->gpuRaysBuffer;
      this->dataPtr->gpuRaysBuffer = nullptr;
    }
  
    if (this->dataPtr->gpuRaysScan)
    {
      delete [] this->dataPtr->gpuRaysScan;
      this->dataPtr->gpuRaysScan = nullptr;
    }
  
    for (unsigned int i = 0; i < this->dataPtr->textureCount; ++i)
    {
      if (this->dataPtr->firstPassTextures[i])
      {
        Ogre::TextureManager::getSingleton().remove(
            this->dataPtr->firstPassTextures[i]->getName());
        this->dataPtr->firstPassTextures[i] = nullptr;
      }
    }
  
    if (this->dataPtr->secondPassTexture)
    {
      Ogre::TextureManager::getSingleton().remove(
          this->dataPtr->secondPassTexture->getName());
      this->dataPtr->secondPassTexture = nullptr;
    }
  
    if (this->dataPtr->matSecondPass)
    {
      Ogre::MaterialManager::getSingleton().remove(
          this->dataPtr->matSecondPass->getName());
      this->dataPtr->matSecondPass = nullptr;
    }
  
    if (this->scene && this->dataPtr->orthoCam)
    {
      this->scene->OgreSceneManager()->destroyCamera(this->dataPtr->orthoCam);
      this->dataPtr->orthoCam = nullptr;
    }
  
    this->dataPtr->visual.reset();
    this->dataPtr->texIdx.clear();
    this->dataPtr->texCount = 0u;
  }
  
  /////////////////////////////////////////////////
  void OgreGpuRays::CreateRenderTexture()
  {
    RenderTexturePtr base = this->scene->CreateRenderTexture();
    this->dataPtr->renderTexture =
        std::dynamic_pointer_cast<OgreRenderTexture>(base);
    this->dataPtr->renderTexture->SetWidth(1);
    this->dataPtr->renderTexture->SetHeight(1);
  }
  
  /////////////////////////////////////////////////
  void OgreGpuRays::CreateCamera()
  {
    // Create ogre camera object
    Ogre::SceneManager *ogreSceneManager  = this->scene->OgreSceneManager();
    if (ogreSceneManager == nullptr)
    {
      ignerr << "Scene manager cannot be obtained" << std::endl;
      return;
    }
  
    this->dataPtr->ogreCamera = ogreSceneManager->createCamera(
        this->Name() + "_Camera");
    if (this->dataPtr->ogreCamera == nullptr)
    {
      ignerr << "Ogre camera cannot be created" << std::endl;
      return;
    }
  
    this->ogreNode->attachObject(this->dataPtr->ogreCamera);
    this->dataPtr->ogreCamera->setFixedYawAxis(false);
    this->dataPtr->ogreCamera->yaw(Ogre::Degree(-90));
    this->dataPtr->ogreCamera->roll(Ogre::Degree(-90));
    this->dataPtr->ogreCamera->setAutoAspectRatio(true);
  }
  
  /////////////////////////////////////////////////
  void OgreGpuRays::ConfigureCameras()
  {
    // horizontal gpu rays setup
    this->SetHFOV(this->AngleMax() - this->AngleMin());
  
    if (this->HFOV().Radian() > 2.0 * IGN_PI)
    {
      this->SetHFOV(2.0 * IGN_PI);
      ignwarn << "Horizontal FOV for GPU rays is capped at 180 degrees.\n";
    }
  
    this->SetHorzHalfAngle((this->AngleMax() + this->AngleMin()).Radian() / 2.0);
  
    // determine number of cameras to use
    if (this->HFOV().Radian() > 2.8)
    {
      if (this->HFOV().Radian()  > 5.6)
      {
        this->dataPtr->cameraCount = 3;
      }
      else
      {
        this->dataPtr->cameraCount = 2;
      }
    }
    else
    {
      this->dataPtr->cameraCount = 1;
    }
  
    // horizontal fov of single frame
    this->SetHFOV(this->HFOV().Radian() / this->dataPtr->cameraCount);
    this->SetCosHorzFOV(this->HFOV().Radian());
  
    // Fixed minimum resolution of texture to reduce steps in ranges
    // when hitting surfaces where the angle between ray and surface is small.
    // Also have to keep in mind the GPU's max. texture size
    unsigned int horzRangeCountPerCamera =
        std::max(2048U, this->RangeCount() / this->dataPtr->cameraCount);
    unsigned int vertRangeCountPerCamera = this->VerticalRangeCount();
  
    // vertical laser setup
    double vfovAngle;
  
    if (this->VerticalRangeCount() > 1)
    {
      vfovAngle = (this->VerticalAngleMax() - this->VerticalAngleMin()).Radian();
    }
    else
    {
      vfovAngle = 0;
  
      if (this->VerticalAngleMax() != this->VerticalAngleMin())
      {
        ignwarn << "Only one vertical ray but vertical min. and max. angle "
            "are not equal. Min. angle is used.\n";
        this->SetVerticalAngleMax(this->VerticalAngleMin().Radian());
      }
    }
  
    if (vfovAngle > IGN_PI / 2.0)
    {
      vfovAngle = IGN_PI / 2.0;
      ignwarn << "Vertical FOV for GPU laser is capped at 90 degrees.\n";
    }
  
    this->SetVFOV(vfovAngle);
    this->SetVertHalfAngle((this->VerticalAngleMax()
                     + this->VerticalAngleMin()).Radian() / 2.0);
  
    this->SetVerticalAngleMin(this->VertHalfAngle() - (vfovAngle / 2));
    this->SetVerticalAngleMax(this->VertHalfAngle() + (vfovAngle / 2));
  
    // Assume camera always stays horizontally even if vert. half angle of
    // laser is not 0. Add padding to camera vfov.
    double vfovCamera = vfovAngle + 2.0 * std::abs(this->VertHalfAngle());
  
    // Add padding to vertical camera FOV to cover all possible rays
    // for given laser vert. and horiz. FOV
    vfovCamera = 2.0 * atan(tan(vfovCamera / 2.0) / cos(
          this->HFOV().Radian() / 2.0));
  
    if (vfovCamera > 2.8)
    {
      ignerr << "Vertical FOV of internal camera exceeds 2.8 radians.\n";
    }
  
    this->SetCosVertFOV(vfovCamera);
  
    // If vertical ray is not 1 adjust horizontal and vertical
    // ray count to maintain aspect ratio
    if (this->vSamples > 1)
    {
      double cameraAspectRatio =
        tan(this->HFOV().Radian() / 2.0) / tan(vfovCamera / 2.0);
  
      this->SetRayCountRatio(cameraAspectRatio);
      this->rangeCountRatio = cameraAspectRatio;
  
      if ((horzRangeCountPerCamera / this->rangeCountRatio) >
           vertRangeCountPerCamera)
      {
        vertRangeCountPerCamera = static_cast<unsigned int>(
            std::round(horzRangeCountPerCamera / this->rangeCountRatio));
      }
      else
      {
        horzRangeCountPerCamera = static_cast<unsigned int>(std::round(
            vertRangeCountPerCamera * this->rangeCountRatio));
      }
    }
    else
    {
      // In case of 1 vert. ray, set a very small vertical FOV for camera
      this->SetRayCountRatio(horzRangeCountPerCamera);
      double camVFOV = 2.0 * atan(tan(
            this->HFOV().Radian() / 2.0) / this->RayCountRatio());
      this->SetCosVertFOV(camVFOV);
    }
  
    // Configure first pass texture size
    this->Set1stTextureSize(horzRangeCountPerCamera, vertRangeCountPerCamera);
    // Configure second pass texture size
    this->SetRangeCount(this->RangeCount(), this->VerticalRangeCount());
  
    // Set ogre cam properties
    this->dataPtr->ogreCamera->setAspectRatio(this->RayCountRatio());
    this->dataPtr->ogreCamera->setFOVy(Ogre::Radian((this->CosVertFOV())));
    this->dataPtr->ogreCamera->setNearClipDistance(this->NearClipPlane());
    this->dataPtr->ogreCamera->setFarClipDistance(this->FarClipPlane());
    this->dataPtr->ogreCamera->setRenderingDistance(this->FarClipPlane());
    this->dataPtr->ogreCamera->yaw(Ogre::Radian(this->HorzHalfAngle()));
  }
  
  /////////////////////////////////////////////////////////
  void OgreGpuRays::CreateGpuRaysTextures()
  {
    this->ConfigureCameras();
  
    this->CreateOrthoCam();
  
    this->dataPtr->textureCount = this->dataPtr->cameraCount;
  
    if (this->dataPtr->textureCount == 2)
    {
      this->dataPtr->cameraYaws[0] = -this->HFOV().Radian() / 2.0;
      this->dataPtr->cameraYaws[1] = +this->HFOV().Radian();
      this->dataPtr->cameraYaws[2] = 0;
      this->dataPtr->cameraYaws[3] = -this->HFOV().Radian() / 2.0;
    }
    else
    {
      this->dataPtr->cameraYaws[0] = -this->HFOV().Radian();
      this->dataPtr->cameraYaws[1] = +this->HFOV().Radian();
      this->dataPtr->cameraYaws[2] = +this->HFOV().Radian();
      this->dataPtr->cameraYaws[3] = -this->HFOV().Radian();
    }
  
    // Configure first pass textures that are not yet configured properly
    for (unsigned int i = 0; i < this->dataPtr->textureCount; ++i)
    {
      std::stringstream texName;
      texName << this->Name() << "_first_pass_" << i;
      this->dataPtr->firstPassTextures[i] =
        Ogre::TextureManager::getSingleton().createManual(
        texName.str(), "General", Ogre::TEX_TYPE_2D,
        this->dataPtr->w1st, this->dataPtr->h1st, 0,
  #if OGRE_VERSION_LT_1_10_1
        Ogre::PF_FLOAT32_RGB, Ogre::TU_RENDERTARGET).getPointer();
  #else
        Ogre::PF_FLOAT32_RGB, Ogre::TU_RENDERTARGET).get();
  #endif
  
      Ogre::RenderTarget *rt =
          this->dataPtr->firstPassTextures[i]->getBuffer()->getRenderTarget();
      rt->setAutoUpdated(false);
  
      // Setup the viewport to use the texture
      Ogre::Viewport *vp =
          rt->addViewport(this->dataPtr->ogreCamera);
      vp->setClearEveryFrame(true);
      vp->setOverlaysEnabled(false);
      vp->setShadowsEnabled(false);
      vp->setSkiesEnabled(false);
      vp->setBackgroundColour(
          Ogre::ColourValue(this->dataMaxVal, 0.0, 1.0));
      vp->setVisibilityMask(IGN_VISIBILITY_ALL &
          ~(IGN_VISIBILITY_GUI | IGN_VISIBILITY_SELECTABLE));
    }
  
    this->dataPtr->matFirstPass = dynamic_cast<Ogre::Material *>(
        Ogre::MaterialManager::getSingleton().getByName("GpuRaysScan1st").get());
    this->dataPtr->matFirstPass->load();
    this->dataPtr->matFirstPass->setCullingMode(Ogre::CULL_NONE);
  
    // Configure second pass texture
    this->dataPtr->secondPassTexture =
        Ogre::TextureManager::getSingleton().createManual(
        this->Name() + "_second_pass",
        "General",
        Ogre::TEX_TYPE_2D,
        this->dataPtr->w2nd, this->dataPtr->h2nd, 0,
        Ogre::PF_FLOAT32_RGB,
  #if OGRE_VERSION_LT_1_10_1
        Ogre::TU_RENDERTARGET).getPointer();
  #else
        Ogre::TU_RENDERTARGET).get();
  #endif
  
    Ogre::RenderTarget *rt =
        this->dataPtr->secondPassTexture->getBuffer()->getRenderTarget();
    rt->setAutoUpdated(false);
  
    // Setup the viewport to use the texture
    Ogre::Viewport *vp = rt->addViewport(this->dataPtr->orthoCam);
    vp->setClearEveryFrame(true);
    vp->setOverlaysEnabled(false);
    vp->setShadowsEnabled(false);
    vp->setSkiesEnabled(false);
    vp->setBackgroundColour(Ogre::ColourValue(0.0, 1.0, 0.0));
    vp->setVisibilityMask(
        IGN_VISIBILITY_ALL & ~(IGN_VISIBILITY_GUI | IGN_VISIBILITY_SELECTABLE));
  
    Ogre::Matrix4 p = this->BuildScaledOrthoMatrix(
        0, static_cast<float>(this->dataPtr->w2nd / 10.0),
        0, static_cast<float>(this->dataPtr->h2nd / 10.0),
        0.01f, 0.02f);
  
    this->dataPtr->orthoCam->setCustomProjectionMatrix(true, p);
  
    // Set GpuRaysScan2nd material
    this->dataPtr->matSecondPass = dynamic_cast<Ogre::Material *>(
        Ogre::MaterialManager::getSingleton().getByName("GpuRaysScan2nd").get());
    // clone the material since we're modifying it's definitions
    this->dataPtr->matSecondPass = this->dataPtr->matSecondPass->clone(
        this->Name() + "_" + this->dataPtr->matSecondPass->getName()).get();
    this->dataPtr->matSecondPass->load();
  
    Ogre::Technique *technique =
      this->dataPtr->matSecondPass->getTechnique(0);
    IGN_ASSERT(technique,
        "OgreGpuRays material script error: technique not found");
  
    Ogre::Pass *pass = technique->getPass(0);
    IGN_ASSERT(pass,
        "OgreGpuRays material script error: pass not found");
    pass->removeAllTextureUnitStates();
    Ogre::TextureUnitState *texUnit = nullptr;
    for (unsigned int i = 0; i < this->dataPtr->textureCount; ++i)
    {
      unsigned int texIndex = this->dataPtr->texCount++;
      {
        texUnit = pass->createTextureUnitState(
              this->dataPtr->firstPassTextures[i]->getName(), texIndex);
  
        this->dataPtr->texIdx.push_back(texIndex);
  
        texUnit->setTextureFiltering(Ogre::TFO_NONE);
        texUnit->setTextureAddressingMode(Ogre::TextureUnitState::TAM_MIRROR);
      }
    }
  
    this->CreateCanvas();
  }
  
  /////////////////////////////////////////////////
  void OgreGpuRays::UpdateRenderTarget(Ogre::RenderTarget *_target,
                     Ogre::Material *_material, Ogre::Camera *_cam,
                     const bool _updateTex)
  {
    Ogre::SceneManager *sceneMgr = this->scene->OgreSceneManager();
    Ogre::RenderSystem *renderSys = sceneMgr->getDestinationRenderSystem();
    // Get pointer to the material pass
    Ogre::Pass *pass = _material->getBestTechnique()->getPass(0);
  
    // Render the depth texture
    // OgreSceneManager::_render function automatically sets farClip to 0.
    // Which normally equates to infinite distance. We don't want this. So
    // we have to set the distance every time.
    _cam->setFarClipDistance(this->FarClipPlane());
  
    Ogre::AutoParamDataSource autoParamDataSource;
  
    Ogre::Viewport *vp = _target->getViewport(0);
  
    // Need this line to render the ground plane. No idea why it's necessary.
    renderSys->_setViewport(vp);
    sceneMgr->_setPass(pass, true, false);
    autoParamDataSource.setCurrentPass(pass);
    autoParamDataSource.setCurrentViewport(vp);
    autoParamDataSource.setCurrentRenderTarget(_target);
    autoParamDataSource.setCurrentSceneManager(sceneMgr);
    autoParamDataSource.setCurrentCamera(_cam, true);
  
    renderSys->setLightingEnabled(false);
    renderSys->_setFog(Ogre::FOG_NONE);
  
    pass->_updateAutoParams(&autoParamDataSource, 1);
  
    if (_updateTex)
    {
      pass->getFragmentProgramParameters()->setNamedConstant("tex1",
        this->dataPtr->texIdx[0]);
      if (this->dataPtr->texIdx.size() > 1)
      {
        pass->getFragmentProgramParameters()->setNamedConstant("tex2",
          this->dataPtr->texIdx[1]);
        if (this->dataPtr->texIdx.size() > 2)
          pass->getFragmentProgramParameters()->setNamedConstant("tex3",
            this->dataPtr->texIdx[2]);
      }
    }
  
    // NOTE: We MUST bind parameters AFTER updating the autos
    if (pass->hasVertexProgram())
    {
      renderSys->bindGpuProgram(
          pass->getVertexProgram()->_getBindingDelegate());
  
      renderSys->bindGpuProgramParameters(Ogre::GPT_VERTEX_PROGRAM,
      pass->getVertexProgramParameters(), 1);
    }
  
    if (pass->hasFragmentProgram())
    {
      renderSys->bindGpuProgram(
      pass->getFragmentProgram()->_getBindingDelegate());
  
      renderSys->bindGpuProgramParameters(Ogre::GPT_FRAGMENT_PROGRAM,
      pass->getFragmentProgramParameters(), 1);
    }
  
    _target->update(false);
  }
  
  //////////////////////////////////////////////////
  void OgreGpuRays::Render()
  {
    Ogre::SceneManager *sceneMgr = this->scene->OgreSceneManager();
  
    sceneMgr->_suppressRenderStateChanges(true);
    sceneMgr->addRenderObjectListener(this);
  
    for (unsigned int i = 0; i < this->dataPtr->textureCount; ++i)
    {
      if (this->dataPtr->textureCount > 1)
      {
        // Cannot call Camera::RotateYaw because it rotates in world frame,
        // but we need rotation in camera local frame
        this->Node()->roll(Ogre::Radian(this->dataPtr->cameraYaws[i]));
      }
  
      this->dataPtr->currentMat = this->dataPtr->matFirstPass;
      this->dataPtr->currentTexture = this->dataPtr->firstPassTextures[i];
  
      this->UpdateRenderTarget(
          this->dataPtr->firstPassTextures[i]->getBuffer()->getRenderTarget(),
          this->dataPtr->matFirstPass,
          this->dataPtr->ogreCamera, false);
    }
  
    if (this->dataPtr->textureCount > 1)
        this->Node()->roll(Ogre::Radian(this->dataPtr->cameraYaws[3]));
  
    sceneMgr->removeRenderObjectListener(this);
  
    this->dataPtr->visual->SetVisible(true);
  
    this->UpdateRenderTarget(
        this->dataPtr->secondPassTexture->getBuffer()->getRenderTarget(),
        this->dataPtr->matSecondPass,
        this->dataPtr->orthoCam, true);
  
    this->dataPtr->visual->SetVisible(false);
  
    sceneMgr->_suppressRenderStateChanges(false);
  }
  
  //////////////////////////////////////////////////
  void OgreGpuRays::PreRender()
  {
    if (this->dataPtr->textureCount == 0)
      this->CreateGpuRaysTextures();
  }
  
  //////////////////////////////////////////////////
  void OgreGpuRays::PostRender()
  {
    for (unsigned int i = 0; i < this->dataPtr->textureCount; ++i)
    {
      auto rt =
          this->dataPtr->firstPassTextures[i]->getBuffer()->getRenderTarget();
      rt->swapBuffers();
    }
    auto rt = this->dataPtr->secondPassTexture->getBuffer()->getRenderTarget();
    rt->swapBuffers();
  
    const Ogre::Viewport *secondPassViewport = rt->getViewport(0);
    unsigned int width = secondPassViewport->getActualWidth();
    unsigned int height = secondPassViewport->getActualHeight();
  
    size_t size = Ogre::PixelUtil::getMemorySize(
      width, height, 1, Ogre::PF_FLOAT32_RGB);
    int len = width * height * this->Channels();
  
    if (!this->dataPtr->gpuRaysBuffer)
    {
      this->dataPtr->gpuRaysBuffer = new float[len];
    }
  
    Ogre::PixelBox dstBox(width, height,
          1, Ogre::PF_FLOAT32_RGB, this->dataPtr->gpuRaysBuffer);
  
    auto pixelBuffer = this->dataPtr->secondPassTexture->getBuffer();
    pixelBuffer->blitToMemory(dstBox);
  
    if (!this->dataPtr->gpuRaysScan)
    {
      this->dataPtr->gpuRaysScan = new float[len];
    }
  
    memcpy(this->dataPtr->gpuRaysScan, this->dataPtr->gpuRaysBuffer, size);
  
    this->dataPtr->newGpuRaysFrame(this->dataPtr->gpuRaysScan,
        width, height, this->Channels(), "PF_FLOAT32_RGB");
  }
  
  //////////////////////////////////////////////////
  const float* OgreGpuRays::Data() const
  {
    return this->dataPtr->gpuRaysScan;
  }
  
  //////////////////////////////////////////////////
  void OgreGpuRays::Copy(float *_dataDest)
  {
    auto rt = this->dataPtr->secondPassTexture->getBuffer()->getRenderTarget();
    const Ogre::Viewport *secondPassViewport = rt->getViewport(0);
    unsigned int width = secondPassViewport->getActualWidth();
    unsigned int height = secondPassViewport->getActualHeight();
  
    size_t size = Ogre::PixelUtil::getMemorySize(
      width, height, 1, Ogre::PF_FLOAT32_RGB);
  
    memcpy(_dataDest, this->dataPtr->gpuRaysScan, size);
  }
  
  /////////////////////////////////////////////////
  void OgreGpuRays::CreateOrthoCam()
  {
    // create ogre camera object
    Ogre::SceneManager *ogreSceneManager;
    ogreSceneManager = this->scene->OgreSceneManager();
    if (ogreSceneManager == nullptr)
    {
      ignerr << "Scene manager cannot be obtained" << std::endl;
      return;
    }
  
    this->dataPtr->orthoCam = ogreSceneManager->createCamera(
        this->Name() + "_Ortho_Camera");
    if (this->dataPtr->orthoCam == nullptr)
    {
      ignerr << "Ogre camera cannot be created" << std::endl;
      return;
    }
  
    Ogre::SceneNode *rootSceneNode = std::dynamic_pointer_cast<
        ignition::rendering::OgreNode>(this->scene->RootVisual())->Node();
    this->dataPtr->pitchNodeOrtho = rootSceneNode->createChildSceneNode();
    this->dataPtr->pitchNodeOrtho->attachObject(this->dataPtr->orthoCam);
  
    // Use X/Y as horizon, Z up
    this->dataPtr->orthoCam->pitch(Ogre::Degree(90));
  
    // Don't yaw along variable axis, causes leaning
    this->dataPtr->orthoCam->setFixedYawAxis(true, Ogre::Vector3::UNIT_Z);
    this->dataPtr->orthoCam->setDirection(1, 0, 0);
    this->dataPtr->orthoCam->setAutoAspectRatio(true);
  
    if (this->dataPtr->orthoCam)
    {
      this->dataPtr->orthoCam->setNearClipDistance(0.01f);
      this->dataPtr->orthoCam->setFarClipDistance(0.02f);
      this->dataPtr->orthoCam->setRenderingDistance(0.02f);
  
      this->dataPtr->orthoCam->setProjectionType(Ogre::PT_ORTHOGRAPHIC);
    }
  }
  
  /////////////////////////////////////////////////
  Ogre::Matrix4 OgreGpuRays::BuildScaledOrthoMatrix(const float _left,
      const float _right, const float _bottom, const float _top,
      const float _near, const float _far)
  {
    float invw = 1 / (_right - _left);
    float invh = 1 / (_top - _bottom);
    float invd = 1 / (_far - _near);
  
    Ogre::Matrix4 proj = Ogre::Matrix4::ZERO;
    proj[0][0] = 2 * invw;
    proj[0][3] = -(_right + _left) * invw;
    proj[1][1] = 2 * invh;
    proj[1][3] = -(_top + _bottom) * invh;
    proj[2][2] = -2 * invd;
    proj[2][3] = -(_far + _near) * invd;
    proj[3][3] = 1;
  
    return proj;
  }
  
  /////////////////////////////////////////////////
  void OgreGpuRays::Set1stTextureSize(
      const unsigned int _w, const unsigned int _h)
  {
    this->dataPtr->w1st = _w;
    this->dataPtr->h1st = _h;
  }
  
  /////////////////////////////////////////////////
  void OgreGpuRays::SetRangeCount(
      const unsigned int _w, const unsigned int _h)
  {
    this->dataPtr->w2nd = _w;
    this->dataPtr->h2nd = _h;
  }
  
  /////////////////////////////////////////////////
  void OgreGpuRays::CreateMesh()
  {
    std::string meshName = this->Name() + "_undistortion_mesh";
  
    common::Mesh *mesh = new common::Mesh();
    mesh->SetName(meshName);
  
    common::SubMesh *submesh = new common::SubMesh();
  
    double dx, dy;
    submesh->SetPrimitiveType(common::SubMesh::POINTS);
  
    if (this->dataPtr->h2nd == 1)
    {
      dy = 0;
    }
    else
    {
      dy = 0.1;
    }
  
    dx = 0.1;
  
    // startX ranges from 0 to -(w2nd/10) at dx=0.1 increments
    // startY ranges from h2nd/10 to 0 at dy=0.1 decrements
    // see OgreGpuRays::Set2ndPassTarget() on how the ortho cam is set up
    double startX = dx;
    double startY = this->dataPtr->h2nd/10.0;
  
    // half of actual camera vertical FOV without padding
    double phi = this->VFOV().Radian() / 2.0;
    double phiCamera = phi + std::abs(this->VertHalfAngle());
    double theta = this->CosHorzFOV() / 2.0;
  
    if (this->dataPtr->h2nd == 1)
    {
      phi = 0;
    }
  
    // index of ray
    unsigned int ptsOnLine = 0;
  
    // total gpu rays hfov
    double thfov = this->dataPtr->textureCount * this->CosHorzFOV();
    double hstep = thfov / (this->dataPtr->w2nd - 1);
    double vstep = 2.0 * phi / (this->dataPtr->h2nd - 1);
  
    if (this->dataPtr->h2nd == 1)
    {
      vstep = 0;
    }
  
    for (unsigned int j = 0; j < this->dataPtr->h2nd; ++j)
    {
      double gamma = 0;
      if (this->dataPtr->h2nd != 1)
      {
        // gamma: current vertical angle w.r.t. camera
        gamma = vstep * j - phi + this->VertHalfAngle();
      }
  
      for (unsigned int i = 0; i < this->dataPtr->w2nd; ++i)
      {
        // current horizontal angle from start of gpu rays scan
        double delta = hstep * i;
  
        // index of texture that contains the depth value
        unsigned int texture = static_cast<unsigned int>(
            delta / this->CosHorzFOV());
  
        // cap texture index and horizontal angle
        if (texture > this->dataPtr->textureCount-1)
        {
          texture -= 1;
          delta -= hstep;
        }
  
        startX -= dx;
        if (ptsOnLine == this->dataPtr->w2nd)
        {
          ptsOnLine = 0;
          startX = 0;
          startY -= dy;
        }
        ptsOnLine++;
  
        // the texture/1000.0 value is used in the gpu rays_2nd_pass.frag shader
        // as a trick to determine which camera texture to use when stitching
        // together the final depth image.
        submesh->AddVertex(texture/1000.0, startX, startY);
  
        // first compute angle from the start of current camera's horizontal
        // min angle, then set delta to be angle from center of current camera.
        delta = delta - (texture * this->CosHorzFOV());
        delta = delta - theta;
  
        // adjust uv coordinates of depth texture to match projection of current
        // gpu ray the depth image plane.
        double u = 0.5 - tan(delta) / (2.0 * tan(theta));
        double v = 0.5 - (tan(gamma) * cos(theta)) /
            (2.0 * tan(phiCamera) * cos(delta));
  
        submesh->AddTexCoord(u, v);
        submesh->AddIndex(this->dataPtr->w2nd * j + i);
      }
    }
  
    mesh->AddSubMesh(*submesh);
  
    this->dataPtr->undistMesh = mesh;
  
    common::MeshManager::Instance()->AddMesh(this->dataPtr->undistMesh);
  }
  
  /////////////////////////////////////////////////
  void OgreGpuRays::CreateCanvas()
  {
    this->CreateMesh();
  
    this->dataPtr->visual = this->scene->CreateVisual(
        this->Name() + "second_pass_canvas");
  
    Ogre::SceneNode *visualSceneNode =  std::dynamic_pointer_cast<
      ignition::rendering::OgreNode>(this->dataPtr->visual)->Node();
  
    Ogre::Node *visualParent = visualSceneNode->getParent();
    if (visualParent != nullptr)
    {
      visualParent->removeChild(visualSceneNode);
    }
    this->dataPtr->pitchNodeOrtho->addChild(visualSceneNode);
  
    // Convert mesh from common::Mesh to rendering::mesh and add it to
    // the canvas visual
    MeshPtr renderingMesh = this->scene->CreateMesh(
        this->dataPtr->undistMesh);
    this->dataPtr->visual->AddGeometry(renderingMesh);
  
    this->dataPtr->visual->SetLocalPosition(0.01, 0, 0);
    this->dataPtr->visual->SetLocalRotation(0, 0, 0);
  
    MaterialPtr canvasMaterial =
      this->scene->CreateMaterial(this->Name() + "_green");
    canvasMaterial->SetAmbient(ignition::math::Color(0, 1, 0, 1));
    this->dataPtr->visual->SetMaterial(canvasMaterial);
  
    this->dataPtr->visual->SetVisible(true);
  }
  
  /////////////////////////////////////////////////
  void OgreGpuRays::notifyRenderSingleObject(Ogre::Renderable *_rend,
        const Ogre::Pass* /*pass*/, const Ogre::AutoParamDataSource* /*source*/,
        const Ogre::LightList* /*lights*/, bool /*supp*/)
  {
    // TODO(anyone): this function sets the retro for each obj
    // but currently just sets it to 0
  
    Ogre::Vector4 retro = Ogre::Vector4(0, 0, 0, 0);
    if (!_rend->hasCustomParameter(1))
    {
      _rend->setCustomParameter(1, Ogre::Vector4(0, 0, 0, 0));
    }
  
    Ogre::Pass *pass =
      this->dataPtr->currentMat->getBestTechnique()->getPass(0);
    Ogre::RenderSystem *renderSys =
      this->scene->OgreSceneManager()->getDestinationRenderSystem();
  
    Ogre::AutoParamDataSource autoParamDataSource;
  
    Ogre::RenderTarget *rt =
      this->dataPtr->currentTexture->getBuffer()->getRenderTarget();
    Ogre::Viewport *vp = rt->getViewport(0);
  
    renderSys->_setViewport(vp);
    autoParamDataSource.setCurrentRenderable(_rend);
    autoParamDataSource.setCurrentPass(pass);
    autoParamDataSource.setCurrentViewport(vp);
    autoParamDataSource.setCurrentRenderTarget(rt);
    autoParamDataSource.setCurrentSceneManager(this->scene->OgreSceneManager());
    autoParamDataSource.setCurrentCamera(this->dataPtr->ogreCamera, true);
  
    pass->_updateAutoParams(&autoParamDataSource,
        Ogre::GPV_GLOBAL | Ogre::GPV_PER_OBJECT);
    pass->getFragmentProgramParameters()->setNamedConstant("retro", retro[0]);
    pass->getFragmentProgramParameters()->setNamedConstant(
        "max", static_cast<float>(this->dataMaxVal));
    pass->getFragmentProgramParameters()->setNamedConstant(
        "min", static_cast<float>(this->dataMinVal));
    renderSys->bindGpuProgram(
        pass->getVertexProgram()->_getBindingDelegate());
  
    renderSys->bindGpuProgramParameters(Ogre::GPT_VERTEX_PROGRAM,
        pass->getVertexProgramParameters(),
        Ogre::GPV_GLOBAL | Ogre::GPV_PER_OBJECT);
  
    renderSys->bindGpuProgram(
        pass->getFragmentProgram()->_getBindingDelegate());
  
    renderSys->bindGpuProgramParameters(Ogre::GPT_FRAGMENT_PROGRAM,
        pass->getFragmentProgramParameters(),
        (Ogre::GPV_GLOBAL | Ogre::GPV_PER_OBJECT));
  }
  
  //////////////////////////////////////////////////
  ignition::common::ConnectionPtr OgreGpuRays::ConnectNewGpuRaysFrame(
      std::function<void(const float *_frame, unsigned int _width,
      unsigned int _height, unsigned int _channels,
      const std::string &/*_format*/)> _subscriber)
  {
    return this->dataPtr->newGpuRaysFrame.Connect(_subscriber);
  }
  
  //////////////////////////////////////////////////
  RenderTargetPtr OgreGpuRays::RenderTarget() const
  {
    return this->dataPtr->renderTexture;
  }
  
  //////////////////////////////////////////////////
  double OgreGpuRays::CosHorzFOV() const
  {
    return this->dataPtr->chfov;
  }
  
  //////////////////////////////////////////////////
  void OgreGpuRays::SetCosHorzFOV(const double _chfov)
  {
    this->dataPtr->chfov = _chfov;
  }
  
  //////////////////////////////////////////////////
  double OgreGpuRays::CosVertFOV() const
  {
    return this->dataPtr->cvfov;
  }
  
  //////////////////////////////////////////////////
  void OgreGpuRays::SetCosVertFOV(const double _cvfov)
  {
    this->dataPtr->cvfov = _cvfov;
  }
  
  //////////////////////////////////////////////////
  void OgreGpuRays::SetHorzHalfAngle(const double _angle)
  {
    this->dataPtr->horzHalfAngle = _angle;
  }
  
  //////////////////////////////////////////////////
  void OgreGpuRays::SetVertHalfAngle(const double _angle)
  {
    this->dataPtr->vertHalfAngle = _angle;
  }
  
  //////////////////////////////////////////////////
  double OgreGpuRays::HorzHalfAngle() const
  {
    return this->dataPtr->horzHalfAngle;
  }
  
  //////////////////////////////////////////////////
  double OgreGpuRays::VertHalfAngle() const
  {
    return this->dataPtr->vertHalfAngle;
  }
  