/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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/config/CConfigFile.h>
#include <mrpt/hwdrivers/CPhidgetInterfaceKitProximitySensors.h>
#include <mrpt/obs/CObservationRange.h>
#include <mrpt/system/CTicTac.h>

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

using namespace mrpt::hwdrivers;
using namespace mrpt::system;
using namespace mrpt::obs;
using namespace mrpt::config;
using namespace std;

/** Usage : ./test <conf file name>.ini
 */
int main(int argc, char** argv)
{
	try
	{
		string confFileName;
		if (argc < 2) confFileName = string("./conf.ini");
		else
			confFileName = string(argv[1]);

		CPhidgetInterfaceKitProximitySensors ik;
		string section("IK_1");
		CConfigFile conf(confFileName);
		ik.loadConfig(conf, section);
		ik.initialize();
		CObservationRange obs;
		do
		{
			ik.doProcess();
			ik.getObservation(obs);
			for (size_t i = 0; i < obs.sensedData.size(); i++)
			{
				cout << obs.sensedData[i].sensedDistance << "\t";
			}
			cout << endl;
			std::this_thread::sleep_for(10ms);
		} while (!mrpt::system::os::kbhit());

		return 0;
	}
	catch (const std::exception& e)
	{
		std::cerr << e.what();
		return 1;
	}
}
