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

#define MODASERVER "localhost"

//using TUTORIAL 3 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 motors
        ModaCPP::DeviceContact *pLeftContact;
        ModaCPP::DeviceContact *pMiddleContact;
        ModaCPP::DeviceContact *pRightContact;

        pLeftContact    =robot->QueryDeviceContact("bump_left/contact");
        pMiddleContact    =robot->QueryDeviceContact("bump_middle/contact");
        pRightContact    =robot->QueryDeviceContact("bump_right/contact");


        //check all is OK
        if(pLeftContact    && pMiddleContact && pRightContact)
            {
            //create devices group
            ModaCPP::DevicesGroupContact *pContactsGroup=new ModaCPP::DevicesGroupContact(robot->GetConnection());
            pContactsGroup->AddDevice(pLeftContact);
            pContactsGroup->AddDevice(pMiddleContact);
            pContactsGroup->AddDevice(pRightContact);

            MU8 ContactsStates[3];
            MU32 ContactsCounters[3];

            while(!_kbhit())
                {
                //devices group unique call : fast (1 network call)
                pContactsGroup->GetContacts(ContactsStates,3);

                /*Is equivalent to read the 3 sensors separately: slow (3 network calls)
                ContactsStates[0]=pLeftContact->IsTutching();
                ContactsStates[1]=pMiddleContact->IsTutching();
                ContactsStates[2]=pRightContact->IsTutching();
                */

                pContactsGroup->GetContactsCounts(ContactsCounters,3);

                _cprintf("%d (%d) %d (%d) %d (%d)\r\n"
                    ,(int) ContactsStates[0],ContactsCounters[0]
                    ,(int) ContactsStates[1],ContactsCounters[1]
                    ,(int) ContactsStates[2],ContactsCounters[2]
                    );
                robot->Sleep(100);
                }
            }
        else
            {
            _cprintf("one or more contact 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;
}