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

#define MODASERVER  "localhost"
#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::DeviceMotor *pFrontLeftMotor;
        ModaCPP::DeviceMotor *pBackLeftMotor;
        ModaCPP::DeviceMotor *pFrontRightMotor;
        ModaCPP::DeviceMotor *pBackRightMotor;

        pFrontLeftMotor =robot->QueryDeviceMotor("joint_front_left/axis/motor");
        pFrontRightMotor=robot->QueryDeviceMotor("joint_front_right/axis/motor");
        pBackLeftMotor  =robot->QueryDeviceMotor("joint_back_left/axis/motor");
        pBackRightMotor =robot->QueryDeviceMotor("joint_back_right/axis/motor");

        //check all is OK
        if(pFrontLeftMotor    && pFrontRightMotor && pBackLeftMotor && pBackRightMotor)
            {
            //create devices group
            ModaCPP::DevicesGroupMotor *pMotorsGroup=new ModaCPP::DevicesGroupMotor(robot->GetConnection());
            pMotorsGroup->AddDevice(pFrontLeftMotor);
            pMotorsGroup->AddDevice(pBackLeftMotor);
            pMotorsGroup->AddDevice(pFrontRightMotor);
            pMotorsGroup->AddDevice(pBackRightMotor);

            float Speeds[4]={0,0,0,0};

            while(!_kbhit())
                {
                //devices group unique call : fast (1 network call)
                pMotorsGroup->SetVelocitiesDPS(Speeds,4);

                /*Is equivalent to set the 4 motors speeds separately: slow (4 network calls)
                pFrontLeftMotor->SetVelocityDPS(Speeds[0]);
                pBackLeftMotor->SetVelocityDPS(Speeds[1]);
                pFrontRightMotor->SetVelocityDPS(Speeds[2]);
                pBackRightMotor->SetVelocityDPS(Speeds[3]);
                */

                Speeds[0]+=1.0f;
                Speeds[1]+=1.0f;
                Speeds[2]+=0.5f;
                Speeds[3]+=0.5f;

                robot->Wait(100);
                }
            }
        else
            {
            _cprintf("one or more motor 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;
}