//Sample from [installDir/Samples/Devices/Motor

#include "ModaCPP.h"

int main(int argc, char* argv[])
{
//process the command line
ModaCPP::CommandLine::ProcessCommandLine(argc,argv);
//Connect to MODA server
ModaCPP::Connection *pConnection=new ModaCPP::Connection(true);
if(pConnection->Connect( ModaCPP::CommandLine::GetArgumentValue("/modaserver","127.0.0.1"),ModaCPP::CommandLine::GetArgumentValueINT("/modaport",0),false))
    {
    _cprintf("Connection ok to moda server\r\n");

    //Find the robot
    ModaCPP::RobotPHX *robot=pConnection->QueryRobotPHX("/");
    if(robot)
        {
        _cprintf("Robot found in this world\r\n");
        ModaCPP::DeviceMotor *pMotorLeft=robot->QueryDeviceMotor("JointLeft/axis/motor");
        ModaCPP::DeviceMotor *pMotorRight=robot->QueryDeviceMotor("Jointright/axis/motor");
        ModaCPP::DeviceGPS *pGPS=robot->QueryDeviceGPS("socle/gps");
        if(pMotorLeft && pMotorRight && pGPS)
            {
            //read the GPS configuration
            GPSConfigStruct GPSConfig=pGPS->GetSettings();
            GPSValuesStruct GPS;
            _cprintf("Intrinsic error:%f, %f, %f (m) Noise:%f (m) Update: %u ms\r\n",GPSConfig.IntrinsicError.x,GPSConfig.IntrinsicError.y,GPSConfig.IntrinsicError.z,GPSConfig.Noise,GPSConfig.UpdateDelay);

            while(!_kbhit())
                {
                //Read GPS position
                pGPS->GetValues(&GPS);
                _cprintf("Position: %f %f %f, speed: %f, North/Dir: %f°\r\n",GPS.XPos,GPS.YPos,GPS.ZPos,GPS.LinearSpeed, Moda::Commons::MathFunctions::ToDeg(GPS.NorthAngleRad));

                //Move the robot
                pMotorLeft->SetVelocityDPS(90);
                pMotorRight->SetVelocityDPS(180);
                pConnection->Sleep(4000);

                //Read GPS position
                pGPS->GetValues(&GPS);
                _cprintf("Position: %f %f %f, speed: %f, North/Dir: %f°\r\n",GPS.XPos,GPS.YPos,GPS.ZPos,GPS.LinearSpeed, Moda::Commons::MathFunctions::ToDeg(GPS.NorthAngleRad));

                //Move the robot
                pMotorLeft->SetVelocityDPS(180);
                pMotorRight->SetVelocityDPS(90);
                pConnection->Sleep(4000);

                //Read GPS position
                pGPS->GetValues(&GPS);
                _cprintf("Position: %f %f %f, speed: %f, North/Dir: %f°\r\n",GPS.XPos,GPS.YPos,GPS.ZPos,GPS.LinearSpeed, Moda::Commons::MathFunctions::ToDeg(GPS.NorthAngleRad));

                //Move the robot
                pMotorLeft->SetVelocityDPS(90);
                pMotorRight->SetVelocityDPS(-90);
                pConnection->Sleep(4000);
                }
            delete pMotorLeft;
            delete pMotorRight;
            delete pGPS;
            }
        else
            {
            _cprintf("motor(s) and/or GPS not found ...\r\n");
            }
        delete robot;
        }
    else
        {
        _cprintf("Robot not found in this world\r\n");
        }
    }
else
    {
    _cprintf("Unable to connect to moda server : be sure Exec is running and MODA TCP/UDP ports are open\r\n");
    }
pConnection->Disconnect();
delete pConnection;
return 0;
}