#include "stdafx.h"
#include "Modacpp.h"
#include "conio.h"

#define MODASERVER "localhost"

//using TUTORIAL 4 world
#define MYROBOTNAME    "/robot1"

int main(int argc, char* argv[])
{
ModaCPP::Connection *connection=new ModaCPP::Connection(true);
if(connection->Connect(MODASERVER))
    {
    _cprintf("Connection ok to moda server\r\n");
    ModaCPP::RobotPHX *robot=connection->QueryRobotPHX(MYROBOTNAME);
    if(robot)
        {
        _cprintf("robot found\r\n");

        //find the robot sensors
        ModaCPP::DeviceDistance *pIRLeft;
        ModaCPP::DeviceDistance *pIRRight;
        ModaCPP::DeviceDistance *pIRBack;

        pIRLeft =robot->QueryDeviceDistance("irright/ray/device_ir");
        pIRRight=robot->QueryDeviceDistance("irleft/ray/device_ir");
        pIRBack =robot->QueryDeviceDistance("irback/ray/device_ir");

        //check all is OK
        if(pIRLeft && pIRRight && pIRBack)
            {
            //create devices group
            ModaCPP::DevicesGroupDistance *pDistanceGroup=new ModaCPP::DevicesGroupDistance(robot->GetConnection());
            pDistanceGroup->AddDevice(pIRLeft);
            pDistanceGroup->AddDevice(pIRRight);
            pDistanceGroup->AddDevice(pIRBack);

            //read measures
            const int MeasuresCount=3;

            float Measures[MeasuresCount];
            while(!_kbhit())
                {
                //Devices group unique call : fast (1 network call)
                pDistanceGroup->GetMeasures(Measures,MeasuresCount);

                /*Is equivalent to read the 3 devices measures separately: slow (3 network calls)
                Measures[0]=pIRLeft->GetMeasure();
                Measures[1]=pIRRight->GetMeasure();
                Measures[2]=pIRBack->GetMeasure();
                */

                //display measures
                for(int i=0;i<MeasuresCount;i++)    _cprintf("group : %f ",Measures[i]);
                _cprintf("\r\n");
                robot->Sleep(100);
                }
            }
        else
            {
            _cprintf("one or more distance sensor(s) missing\r\n");
            }
        }
    else
        {
        _cprintf("robot not found\r\n");
        }
    }
else
    {
    _cprintf("Unable to connect to moda server\r\n");
    }

connection->Disconnect();
delete connection;
return 0;
}