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

#define MODASERVER "localhost"
#define PHX    "/robot"

using namespace ModaCPP;
using namespace xkode::lib;

int main(int argc, char* argv[])
{
ModaCPP::Connection *connection=new ModaCPP::Connection(true);
//Try connect to MODA server
if(connection->Connect(MODASERVER))
    {
    _cprintf("Connection ok to moda server\r\n");
    //Find the robot
    RobotPHX *robot=connection->QueryRobotPHX(PHX);
    if(robot)
        {
        _cprintf("robot found\r\n");
        DeviceAccelGyro *pAccelerometer=robot->QueryDeviceAccelGyro("box0/accel");
        if(pAccelerometer)
            {
            _cprintf("accelerometer found\r\n");
            while(1)
                {
                Moda::Commons::AXESXYZValues a=pAccelerometer->GetXYZInstantValues();
                if(pAccelerometer->GetLastSystemError()==MODA_EOK)
                    {
                    //Display axis XYZ accelerations
                    _cprintf("(%f %f %f) (%f %f %f) (%f %f %f)\r\n",
                                    a.LinearAccelerations[0],a.LinearAccelerations[1],a.LinearAccelerations[2],
                                    Math::ToDeg(a.AngularVelocities[0]),Math::ToDeg(a.AngularVelocities[1]),Math::ToDeg(a.AngularVelocities[2]),
                                    Math::ToDeg(a.Angles[0]),Math::ToDeg(a.Angles[1]),Math::ToDeg(a.Angles[2]) );
                    }
                else
                    {
                    _cprintf("Error %d while reading values\r\n",pAccelerometer->GetLastSystemError());
                    }
                connection->Sleep(20);
                }
            }
        else
            {
            _cprintf("accelerometer not found\r\n");
            }
        }
    else
        {
        _cprintf("robot not found\r\n");
        }
    }
else
    {
    _cprintf("Unable to connect to moda server\r\n");
    }
//Disconnect & delete
connection->Disconnect();
delete connection;
return 0;
}