/* +---------------------------------------------------------------------------+
   |                 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/CFileGZInputStream.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::CFileGZInputStream 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;
	}
}
