/************************************************************************/
/*																		*/
/*	RF096.cpp															*/
/*																		*/
/*	RFDevice sample program												*/
/*																		*/
/*	 Demonstrate how to:												*/
/*	 - Connect to RF096													*/
/*	 - Read measurements from RF096										*/
/*	 - Disconnect from RF096											*/
/*																		*/
/************************************************************************/




#include <iostream>
#include <RF096Device.h>

using namespace std;
#define MEASURES_TO_DO 5
#define VALUES_TO_PRINT 4

int main096(int argc, char *argv[])
{
	float pAngle[96], pDistance[96];

	cout << "This demo intends RF096 on IP 192.168.0.3 listening port 6008 and replying to port 6003 on PC" << endl;

	//	Initialize RFDevice
	RFDevice::Initialize();

	RFDevice::RF096Device *p = new RFDevice::RF096Device();
	if (!p)
	{
		cout << "Failed to construct RF096Device" << endl;
	}

	//	Connect to each RF096
	if (FALSE == p->Connect())
	{
		cout << "Failed to open port" << endl;
		goto l_fin2;
	}
	else
	{
		cout << "Port open" << endl;
	}

	// Say hello
	if (FALSE == p->Hello())
	{
		cout << "No reply to hello" << endl;
		goto l_fin;
	}
	else
	{
		cout << "Detected, assembly type " << p->GetAssemblyType() << endl;
	}

	//	Get MEASURES_TO_DO measures from RF096
	for (int k=0; k<MEASURES_TO_DO; k++)
	{
		if (p->GetNormalizedSingleMeasure(pAngle, pDistance) == FALSE)
		{
			cout << "Failed to read measure" << endl;
		}
		else
		{
			//	Print first VALUES_TO_PRINT values
			cout << "{ ";
			for (int j=0; j<VALUES_TO_PRINT; j++)
			{
				cout << pAngle[j] << "," << pDistance[j];
				if (j<VALUES_TO_PRINT) cout << "; ";
			}
			cout << "... }" << endl;
		}
	}

l_fin:
	//	Disconnect RF069
	if (p->Disconnect() == FALSE)
	{
		cout << "Failed to close port" << endl;
	}
	else
	{
		cout << "Port closed" << endl;
	}

l_fin2:
	//	Release memory
	delete p;

	//	Cleanup RFDevice
	RFDevice::Cleanup();

	return 0;
}

