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

/*-----------------------------------------------------------------------------
	APPLICATION: MEX-grabber
	FILE: mexgrabber.cpp
	AUTHORS: Jesus Briales Garcia <jesusbriales@gmail.com>
			 Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>

	For instructions and details, see:
	 https://www.mrpt.org/list-of-mrpt-apps/application-MEX-grabber
  -----------------------------------------------------------------------------*/

#include <mrpt/config/CConfigFile.h>
#include <mrpt/core/round.h>
#include <mrpt/hwdrivers/CGenericSensor.h>
#include <mrpt/img/CImage.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

// Matlab MEX interface headers
#include <mexplus.h>

// Temporal includes
MRPT_TODO(
	"This won't be necessary when CObservation2DRangeScan::writeToMatlab is "
	"fully functional")
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservation2DRangeScan.h>

#include <mutex>
#include <thread>

using namespace mrpt::obs;

// Force here using mexPrintf instead of printf
#define printf mexPrintf

using namespace mrpt;
using namespace mrpt::system;
using namespace mrpt::hwdrivers;
using namespace std;
using namespace mexplus;

const std::string GLOBAL_SECTION_NAME = "global";

// Forward declarations:
struct TThreadParams
{
	CConfigFile* cfgFile;
	string sensor_label;
};

void SensorThread(TThreadParams params);

CGenericSensor::TListObservations global_list_obs;
std::mutex cs_global_list_obs;

bool allThreadsMustExit = false;

// Thread handlers vector stored as global (persistent MEX variables)
vector<std::thread> lstThreads;

// State variables
bool mex_is_running = false;

// Configuration variables
MRPT_TODO("Set as variable controlled from Matlab")
size_t max_num_obs = 50;

/* Important:
 * All global variables will be stored between MEX calls,
 * and can be used by running threads backstage. */

namespace
{
// Defines MEX API for new.
MEX_DEFINE(new)(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
	printf(" mex-grabber - Part of the MRPT\n");
	printf(
		" MRPT C++ Library: %s - Sources timestamp: %s\n",
		MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());
	printf(
		"-------------------------------------------------------------------"
		"\n");

	mexplus::InputArguments input(nrhs, prhs, 1);
	//    mexplus::OutputArguments output(nlhs, plhs, 1);

	//    if (mex_is_running)
	//    {
	//        printf("[mex-grabber::new] Application is already running\n");
	//        return;
	//    }

	// Initialize global (persistent) variables
	mex_is_running = true;
	allThreadsMustExit = false;

	string INI_FILENAME(input.get<string>(0));
	ASSERT_FILE_EXISTS_(INI_FILENAME)

	CConfigFile iniFile(INI_FILENAME);

	// ------------------------------------------
	//			Load config from file:
	// ------------------------------------------
	int time_between_launches = 300;
	double SF_max_time_span = 0.25;	 // Seconds
	bool use_sensoryframes = false;
	int GRABBER_PERIOD_MS = 1000;

	MRPT_LOAD_CONFIG_VAR(
		time_between_launches, int, iniFile, GLOBAL_SECTION_NAME);
	MRPT_LOAD_CONFIG_VAR(SF_max_time_span, float, iniFile, GLOBAL_SECTION_NAME);
	MRPT_LOAD_CONFIG_VAR(use_sensoryframes, bool, iniFile, GLOBAL_SECTION_NAME);
	MRPT_LOAD_CONFIG_VAR(GRABBER_PERIOD_MS, int, iniFile, GLOBAL_SECTION_NAME);

	// ----------------------------------------------
	// Launch threads:
	// ----------------------------------------------
	std::vector<std::string> sections;
	iniFile.getAllSections(sections);

	for (std::vector<std::string>::iterator it = sections.begin();
		 it != sections.end(); ++it)
	{
		if (*it == GLOBAL_SECTION_NAME || it->empty() ||
			iniFile.read_bool(*it, "rawlog-grabber-ignore", false, false))
			continue;  // This is not a sensor:

		// cout << "Launching thread for sensor '" << *it << "'" << endl;

		TThreadParams threParms;
		threParms.cfgFile = &iniFile;
		threParms.sensor_label = *it;

		std::thread thre = std::thread(SensorThread, threParms);

		lstThreads.emplace_back(std::move(thre));
		std::this_thread::sleep_for(
			std::chrono::milliseconds(time_between_launches));
	}

	printf("[mex-grabber::new] All threads launched\n");
}  // End of "new" method

// Defines MEX API for read (acquire observations in Matlab form)
MEX_DEFINE(read)(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
	// ----------------------------------------------
	// Run:
	// ----------------------------------------------
	//    mexplus::InputArguments input(nrhs, prhs, 1);
	mexplus::OutputArguments output(nlhs, plhs, 1);

	CGenericSensor::TListObservations copy_of_global_list_obs;

	// See if we have observations and process them:
	{
		std::lock_guard<std::mutex> lock(cs_global_list_obs);
		copy_of_global_list_obs.clear();

		if (!global_list_obs.empty())
		{
			CGenericSensor::TListObservations::iterator itEnd =
				global_list_obs.begin();
			std::advance(itEnd, global_list_obs.size() / 2);
			copy_of_global_list_obs.insert(global_list_obs.begin(), itEnd);
			global_list_obs.erase(global_list_obs.begin(), itEnd);
		}
	}  // End of cs lock

	// Read from list of observations to mxArray cell array (store any kind of
	// objects)
	MxArray cell_obs(MxArray::Cell(1, copy_of_global_list_obs.size()));
	size_t index = 0;

	for (CGenericSensor::TListObservations::iterator it =
			 copy_of_global_list_obs.begin();
		 it != copy_of_global_list_obs.end(); ++it)
	{
		MxArray struct_obs(it->second->writeToMatlab());

		// Special cases (TEMPORAL)
		if (IS_CLASS(it->second, CObservation2DRangeScan))
		{
			// Get Points Map from 2D Range Scan
			CObservation2DRangeScan::Ptr LRF_obs =
				std::dynamic_pointer_cast<CObservation2DRangeScan>(it->second);
			struct_obs.set(
				"map",
				LRF_obs->buildAuxPointsMap<mrpt::maps::CSimplePointsMap>()
					->writeToMatlab());
		}
		cell_obs.set(index, struct_obs.release());
		index++;
	}

	// Returns created struct
	output.set(0, cell_obs.release());

	// No need to sleep, since this function only applies when user requested
	// from Matlab
}  // End of "read" method

// Defines MEX API for delete.
MEX_DEFINE(delete)(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
	//    mexplus::InputArguments input(nrhs, prhs, 1);
	//    mexplus::OutputArguments output(nlhs, plhs, 0);
	if (allThreadsMustExit)
	{
		printf(
			"[main thread] Ended due to other thread signal to exit "
			"application.\n");
	}

	// Wait all threads:
	// ----------------------------
	allThreadsMustExit = true;
	std::this_thread::sleep_for(300ms);
	printf("\nWaiting for all threads to close...\n");
	for (vector<std::thread>::iterator th = lstThreads.begin();
		 th != lstThreads.end(); ++th)
		th->join();

	cout << endl
		 << "[mex-grabber::delete] mex-grabber application finished" << endl;
	std::this_thread::sleep_for(1000ms);  // Time for the wxSubsystem to close
	// all remaining windows and avoid
	// crash... (any better way?)
	mex_is_running = false;
}  // End of "delete" method

}  // End of namespace

MEX_DISPATCH  // Don't forget to add this if MEX_DEFINE() is used.

	// ------------------------------------------------------
	//				SensorThread
	// ------------------------------------------------------
	void
	SensorThread(TThreadParams params)
{
	try
	{
		string driver_name = params.cfgFile->read_string(
			params.sensor_label, "driver", "", true);

		CGenericSensor::Ptr sensor =
			CGenericSensor::createSensorPtr(driver_name);
		if (!sensor)
		{
			cerr << endl
				 << "***ERROR***: Class name not recognized: " << driver_name
				 << endl;
			allThreadsMustExit = true;
		}

		// Load common & sensor specific parameters:
		sensor->loadConfig(*params.cfgFile, params.sensor_label);

		cout << format("[thread_%s] Starting...", params.sensor_label.c_str())
			 << " at " << sensor->getProcessRate() << " Hz" << endl;

		ASSERTMSG_(
			sensor->getProcessRate() > 0,
			"process_rate must be set to a valid value (>0 Hz).");
		int process_period_ms = round(1000.0 / sensor->getProcessRate());

		// Init device:
		sensor->initialize();

		while (!allThreadsMustExit)
		{
			TTimeStamp t0 = now();

			// Process
			sensor->doProcess();

			// Get new observations
			CGenericSensor::TListObservations lstObjs;
			sensor->getObservations(lstObjs);

			{
				std::lock_guard<std::mutex> lock(cs_global_list_obs);
				// Control maximum number of stored observations to prevent
				// excesive growth of list between calls
				if (global_list_obs.size() < 2 * max_num_obs)  // .size() is
					// returning 2
					// countings for
					// each pair
					global_list_obs.insert(lstObjs.begin(), lstObjs.end());
			}

			lstObjs.clear();

			// wait until the process period:
			TTimeStamp t1 = now();
			double At = timeDifference(t0, t1);
			int At_rem_ms = process_period_ms - At * 1000;
			if (At_rem_ms > 0)
				std::this_thread::sleep_for(
					std::chrono::milliseconds(At_rem_ms));
		}

		sensor.reset();
		cout << format("[thread_%s] Closing...", params.sensor_label.c_str())
			 << endl;
	}
	catch (const std::exception& e)
	{
		printf("[mex-grabber::Exception] %s\n", e.what());
		allThreadsMustExit = true;
	}
	catch (...)
	{
		printf("[mex-grabber::UntypedException]\n");
		allThreadsMustExit = true;
	}
}

// ------------------------------------------------------
//					MAIN THREAD
//
// For testing outside Matlab
// ------------------------------------------------------
int main(int argc, const char* argv[])
{
	try
	{
		printf(" MEX-grabber - Part of the MRPT\n");
		printf(
			" MRPT C++ Library: %s - Sources timestamp: %s\n",
			MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());
		printf(
			"------------------------------------------------------------------"
			"-\n");
		printf(" This is a test for Matlab MEX functionalities\n");
		printf(
			"------------------------------------------------------------------"
			"-\n");

		// Launch threads with "new"
		const mxArray* mxIn[2];
		mxIn[0] = mexplus::from("new");
		mxIn[1] = mexplus::from(
			argv[1]);  // Read config file path, first argv is function name
		mxArray* mxOut[1];
		mexFunction(1, mxOut, 2, mxIn);

		std::this_thread::sleep_for(
			5000ms);  // Time for the sensor to read before collecting

		// Read frames with "read"
		mxIn[0] = mexplus::from("read");
		mexFunction(1, mxOut, 1, mxIn);

		// Finish applicatin with "delete"
		mxIn[0] = mexplus::from("delete");
		mexFunction(1, mxOut, 1, mxIn);

		// Repete whole process
		// Launch threads with "new"
		mxIn[0] = mexplus::from("new");
		mxIn[1] = mexplus::from(
			argv[1]);  // Read config file path, first argv is function name
		mexFunction(1, mxOut, 2, mxIn);

		// Read frames with "read"
		mxIn[0] = mexplus::from("read");
		mexFunction(1, mxOut, 1, mxIn);

		// Finish applicatin with "delete"
		mxIn[0] = mexplus::from("delete");
		mexFunction(1, mxOut, 1, mxIn);

		return 0;
	}
	catch (const std::exception& e)
	{
		printf("[mex-grabber::Exception] %s\n", e.what());
		return -1;
	}
	catch (...)
	{
		printf("[mex-grabber::UntypedException]\n");
		return -1;
	}
}
