/* +---------------------------------------------------------------------------+
   |                 The Mobile Robot Programming Toolkit (MRPT)               |
   |                                                                           |
   |                          https://www.mrpt.org/                            |
   |                                                                           |
   | Copyright (c) 2005-2013, Individual contributors, see AUTHORS file        |
   | Copyright (c) 2005-2013, MAPIR group, University of Malaga                |
   | Copyright (c) 2012-2013, University of Almeria                            |
   | 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 the copyright holders 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 HOLDERS 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.                                               |
   +---------------------------------------------------------------------------+
 */

#include <mrpt/io/CCompressedInputStream.h>
#include <mrpt/maps.h>  // For converting into point maps
#include <mrpt/obs.h>   // For loading from the rawlog
#include <mrpt/synch.h>
#include <mrpt/system.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>

#include <iostream>

size_t rawlogEntry = 0;

struct ThreadData
{
  ThreadData() : new_timestamp(INVALID_TIMESTAMP) {}

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr new_cloud;
  mrpt::system::TTimeStamp new_timestamp;
};

ThreadData td;
std::mutex td_cs;

void viewerUpdate(pcl::visualization::PCLVisualizer& viewer)
{
  std::stringstream ss;
  ss << "Rawlog entry: " << rawlogEntry;
  viewer.removeShape("text", 0);
  viewer.addText(ss.str(), 10, 50, "text", 0);

  static mrpt::system::TTimeStamp last_time = INVALID_TIMESTAMP;

  {  // Mutex protected
    std::lock_guard<std::mutex> lock(td_cs);
    if (td.new_timestamp != last_time)
    {
      last_time = td.new_timestamp;
      viewer.removePointCloud("cloud", 0);
      viewer.addPointCloud(td.new_cloud, "cloud", 0);
      viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3.0);

      const size_t N = td.new_cloud->size();
      std::cout << "Showing new point cloud of size=" << N << std::endl;

      static bool first = true;
      if (N && first)
      {
        first = false;
        // viewer.resetCameraViewpoint("cloud");
      }

#if 0
			std::cout << mrpt::format(
				"camera: clip = %f %f\n"
				"        focal = %f %f %f\n"
				"        pos   = %f %f %f\n"
				"        view  = %f %f %f\n",
				viewer.camera_.clip[0],viewer.camera_.clip[1],
				viewer.camera_.focal[0],viewer.camera_.focal[1],viewer.camera_.focal[2],
				viewer.camera_.pos[0],viewer.camera_.pos[1],viewer.camera_.pos[2],
				viewer.camera_.view[0],viewer.camera_.view[1],viewer.camera_.view[2]);
#endif
    }
  }
}

void viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
{
  viewer.setBackgroundColor(0.3, 0.3, 0.3);
  viewer.addCoordinateSystem(1.0, 0);
  viewer.initCameraParameters();
  viewer.camera_.pos[2] = 30;
  viewer.updateCamera();
}

int main(int argc, char** argv)
{
  try
  {
    if (argc != 2)
    {
      std::cerr << "Usage: " << argv[0] << " <DATASET.rawlog>\n";
      return 1;
    }

    std::cout << "Opening: " << argv[1] << std::endl;
    mrpt::io::CCompressedInputStream fil(argv[1]);
    bool rawlog_eof = false;

    pcl::visualization::CloudViewer viewer("Cloud Viewer from MRPT's rawlog");

    // This will only get called once
    viewer.runOnVisualizationThreadOnce(viewerOneOff);

    // This will get called once per visualization iteration
    viewer.runOnVisualizationThread(viewerUpdate);

    while (!viewer.wasStopped())
    {
      mrpt::obs::CActionCollection::Ptr actions;
      mrpt::obs::CSensoryFrame::Ptr SF;
      mrpt::obs::CObservation::Ptr obs;

      if (!rawlog_eof)
      {
        if (!mrpt::obs::CRawlog::getActionObservationPairOrObservation(
                fil, actions, SF, obs, rawlogEntry))
        {
          rawlog_eof = true;
          std::cerr << "End of rawlog file!! Close the window to exit\n";
        }
        else
        {
          // Can generate a point cloud from this data?
          // TODO: Process Kinect observations differently to extract
          // RGB data.
          mrpt::maps::CPointsMap::Ptr new_map;
          if (SF)
          {
            new_map = mrpt::make_aligned_shared<mrpt::maps::CSimplePointsMap>();
            // new_map->insertionOptions.minDistBetweenLaserPoints =
            // 0;
            SF->insertObservationsInto(new_map);
          }
          else if (obs)
          {
            new_map = mrpt::make_aligned_shared<mrpt::maps::CSimplePointsMap>();
            // new_map->insertionOptions.minDistBetweenLaserPoints =
            // 0;
            new_map->insertObservation(obs.get());
          }

          if (new_map)
          {
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

            // Convert MRPT point maps -> PCL point cloud.
            new_map->getPCLPointCloud(*cloud);

            {  // Mutex protected
              std::lock_guard<std::mutex> lock(td_cs);
              td.new_timestamp = mrpt::Clock::now();
              td.new_cloud = cloud;
            }

            std::this_thread::sleep_for(30ms);  // Delay to allow the point cloud to show
                                                // up.
          }
        }
      }

      std::this_thread::sleep_for(1ms);
    }
    return 0;
  }
  catch (std::exception& e)
  {
    std::cerr << e.what() << std::endl;
    return 1;
  }
}
