/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/hwdrivers/CSwissRanger3DCamera.h>
#include <mrpt/img/TColor.h>
#include <mrpt/maps/CColouredPointsMap.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/opengl/CTexturedPlane.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/system/CTicTac.h>

#include <chrono>
#include <iostream>
#include <thread>

using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::math;
using namespace mrpt::gui;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::opengl;
using namespace mrpt::system;
using namespace mrpt::img;
using namespace std;

// ------------------------------------------------------
//				Test_SwissRanger
// ------------------------------------------------------
void Test_SwissRanger()
{
  CSwissRanger3DCamera cam;

  // Set params:
  cam.setOpenFromUSB(true);

  cam.setSave3D(true);
  cam.setSaveRangeImage(true);
  cam.setSaveIntensityImage(true);
  cam.setSaveConfidenceImage(false);

  // cam.enablePreviewWindow(true);

  // Open:
  cam.initialize();

  if (cam.isOpen())
    cout << "[Test_SwissRanger] Camera open, serial #" << cam.getCameraSerialNumber()
         << " resolution: " << cam.cols() << "x" << cam.rows()
         << " max. range: " << cam.getMaxRange() << endl;

  const double aspect_ratio = cam.rows() / double(cam.cols());

  {
    std::string ver;
    cam.getMesaLibVersion(ver);
    cout << "[Test_SwissRanger] Version: " << ver << "\n";
  }

  CObservation3DRangeScan obs;
  bool there_is_obs = true, hard_error;

  mrpt::gui::CDisplayWindow3D win3D("3D camera view", 800, 600);

  win3D.setCameraAzimuthDeg(140);
  win3D.setCameraElevationDeg(20);
  win3D.setCameraZoom(6.0);
  win3D.setCameraPointingToPoint(2.5, 0, 0);

  // mrpt::gui::CDisplayWindow  win2D("2D range image",200,200);
  // mrpt::gui::CDisplayWindow  winInt("Intensity range image",200,200);
  //	win2D.setPos(10,10);
  //	winInt.setPos(350,10);
  //	win3D.setPos(10,290);
  //	win3D.resize(400,200);

  // mrpt::opengl::CPointCloud::Ptr gl_points =
  // mrpt::opengl::CPointCloud::Create();
  mrpt::opengl::CPointCloudColoured::Ptr gl_points = mrpt::opengl::CPointCloudColoured::Create();
  gl_points->setPointSize(4.5);

  mrpt::opengl::CTexturedPlane::Ptr gl_img_range =
      mrpt::opengl::CTexturedPlane::Create(0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio);
  mrpt::opengl::CTexturedPlane::Ptr gl_img_intensity =
      mrpt::opengl::CTexturedPlane::Create(0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio);
  mrpt::opengl::CTexturedPlane::Ptr gl_img_intensity_rect =
      mrpt::opengl::CTexturedPlane::Create(0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio);

  {
    mrpt::opengl::Scene::Ptr& scene = win3D.get3DSceneAndLock();

    // Create the Opengl object for the point cloud:
    scene->insert(gl_points);
    scene->insert(mrpt::opengl::CGridPlaneXY::Create());
    scene->insert(mrpt::opengl::stock_objects::CornerXYZ());

    const int VW_WIDTH = 200;
    const int VW_HEIGHT = 150;
    const int VW_GAP = 10;

    // Create the Opengl objects for the planar images, as textured planes,
    // each in a separate viewport:
    win3D.addTextMessage(30, -10 - 1 * (VW_GAP + VW_HEIGHT), "Range data", 1);
    opengl::Viewport::Ptr viewRange = scene->createViewport("view2d_range");
    scene->insert(gl_img_range, "view2d_range");
    viewRange->setViewportPosition(5, -10 - 1 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
    viewRange->setTransparent(true);
    viewRange->getCamera().setOrthogonal(true);
    viewRange->getCamera().setAzimuthDegrees(90);
    viewRange->getCamera().setElevationDegrees(90);
    viewRange->getCamera().setZoomDistance(1.0);

    win3D.addTextMessage(30, -10 - 2 * (VW_GAP + VW_HEIGHT), "Intensity data", 2);
    opengl::Viewport::Ptr viewInt = scene->createViewport("view2d_int");
    scene->insert(gl_img_intensity, "view2d_int");
    viewInt->setViewportPosition(5, -10 - 2 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
    viewInt->setTransparent(true);
    viewInt->getCamera().setOrthogonal(true);
    viewInt->getCamera().setAzimuthDegrees(90);
    viewInt->getCamera().setElevationDegrees(90);
    viewInt->getCamera().setZoomDistance(1.0);

    win3D.addTextMessage(30, -10 - 3 * (VW_GAP + VW_HEIGHT), "Intensity data (undistorted)", 3);
    opengl::Viewport::Ptr viewIntRect = scene->createViewport("view2d_intrect");
    scene->insert(gl_img_intensity_rect, "view2d_intrect");
    viewIntRect->setViewportPosition(5, -10 - 3 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
    viewIntRect->setTransparent(true);
    viewIntRect->getCamera().setOrthogonal(true);
    viewIntRect->getCamera().setAzimuthDegrees(90);
    viewIntRect->getCamera().setElevationDegrees(90);
    viewIntRect->getCamera().setZoomDistance(1.0);

    win3D.unlockAccess3DScene();
    win3D.repaint();
  }

  CTicTac tictac;
  size_t nImgs = 0;

  bool endLoop = false;

  while (there_is_obs && !endLoop && win3D.isOpen())
  {
    // Grab new observation from the camera:
    cam.getNextObservation(obs, there_is_obs, hard_error);

    // Show ranges as 2D:
    if (there_is_obs && obs.hasRangeImage)
    {
      mrpt::img::CImage img = obs.rangeImage_getAsImage();

      win3D.get3DSceneAndLock();
      gl_img_range->assignImage(std::move(img));
      win3D.unlockAccess3DScene();
    }

    // Show intensity image:
    if (there_is_obs && obs.hasIntensityImage)
    {
      win3D.get3DSceneAndLock();
      gl_img_intensity->assignImage(obs.intensityImage);

      CImage undistortImg;
      obs.intensityImage.undistort(undistortImg, obs.cameraParams);
      gl_img_intensity_rect->assignImage(undistortImg);
      win3D.unlockAccess3DScene();
    }

    // Show 3D points:
    if (there_is_obs && obs.hasPoints3D)
    {
      // mrpt::maps::CSimplePointsMap  pntsMap;
      CColouredPointsMap pntsMap;
      pntsMap.colorScheme.scheme = CColouredPointsMap::cmFromIntensityImage;
      pntsMap.loadFromRangeScan(obs);

      win3D.get3DSceneAndLock();
      gl_points->loadFromPointsMap(&pntsMap);
      win3D.unlockAccess3DScene();
      win3D.repaint();
    }

    nImgs++;
    if (nImgs > 10)
    {
      win3D.get3DSceneAndLock();
      win3D.addTextMessage(0.01, 0.01, format("%.02f Hz", nImgs / tictac.Tac()), 100);
      win3D.unlockAccess3DScene();
      nImgs = 0;
      tictac.Tic();
    }

    // Process possible keyboard commands:
    // --------------------------------------
    if (win3D.keyHit())
    {
      const int key = tolower(win3D.getPushedKey());
      // cout << "key: " << key << endl;

      switch (key)
      {
        case 'h':
          cam.enableImageHistEqualization(!cam.isEnabledImageHistEqualization());
          break;
        case 'g':
          cam.enableConvGray(!cam.isEnabledConvGray());
          break;
        case 'd':
          cam.enableDenoiseANF(!cam.isEnabledDenoiseANF());
          break;
        case 'f':
          cam.enableMedianFilter(!cam.isEnabledMedianFilter());
          break;
        case 27:
          endLoop = true;
          break;
      }
    }

    win3D.get3DSceneAndLock();
    win3D.addTextMessage(
        0.08, 0.02,
        format(
            "Keyboard switches: H (hist.equal: %s) | G (convGray: %s) | D "
            "(denoise: %s) | F (medianFilter: %s)",
            cam.isEnabledImageHistEqualization() ? "ON" : "OFF",
            cam.isEnabledConvGray() ? "ON" : "OFF", cam.isEnabledDenoiseANF() ? "ON" : "OFF",
            cam.isEnabledMedianFilter() ? "ON" : "OFF"),
        110);
    win3D.unlockAccess3DScene();

    std::this_thread::sleep_for(1ms);
  }
}

int main(int argc, char** argv)
{
  try
  {
    Test_SwissRanger();
    return 0;
  }
  catch (const std::exception& e)
  {
    std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
    return -1;
  }
  catch (...)
  {
    printf("Another exception!!");
    return -1;
  }
}
