/*-----------------------------------------------------------------------------
	APPLICATION: mextest
	FILE: mex_test.cpp
	AUTHORS: Jesus Briales Garcia <jesusbriales@gmail.com>

	For instructions and details, see:
	 http://
  -----------------------------------------------------------------------------*/

#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/slam/CActionCollection.h>
#include <mrpt/slam/CSensoryFrame.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

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

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

// Global variable
int counter = 0;
int timer = 0;

bool allThreadsMustExit = false;

// Thread handler stored as global
std::thread thre;

void timerThread();
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
	plhs[0] = mexplus::from(counter);

	allThreadsMustExit = false;

	// Create independent thread
	if (thre.idThread == 0)	 // Unitialized
	{
		printf("Thread was unitialized, launching new timerThread\n");
		thre = std::thread(timerThread);
	}

	printf("Counter: %d\n", counter);
	printf("Timer: %d\n", timer);

	counter++;
	// Set exit value for threads
	// allThreadsMustExit = true;

	// Test nested structs in Matlab
	const char* fields[] = {"field1", "test2", "what3"};
	const char* fields2[] = {"a", "b"};
	MxArray struct_array(MxArray::Struct(3, fields, 1, 2));
	MxArray other_struct(MxArray::Struct(2, fields2, 1, 2));
	other_struct.set("a", 1, 0);
	other_struct.set("b", 2, 0);
	//    struct_array.set("field1",other_struct.release(), 0);
	struct_array.set("field1", other_struct.release(), 0);
	struct_array.set("test2", "testing nested structs", 1);

	plhs[1] = struct_array.release();
	//    plhs[2] = other_struct.release();

	// exitThread(); // To finish mexFunction without finishing the other
	// threads
}

void timerThread()
{
	int process_period_ms = 3000;

	uint timerCounter = 0;
	while (!allThreadsMustExit)
	{
		printf("Inside loop counter: %d\n", timerCounter++);

		std::this_thread::sleep_for(1000ms);
		timer++;
	}
	// printf("timerThread loop exited\n");
}
