//////////////////////////////////////////////////////////////////////////
//DeviceExternalCommunication sample: Simple network dialer
//Usage:
//Run associated simulation then use Telnet on port 20000 for dialing with the robot
//Uses D, E, S, F keys in order to make the robot to move.
//////////////////////////////////////////////////////////////////////////

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

#define DEFAULTMODASERVER    "localhost"
#define DEFAULTROBOTNAME    "/robot0"

//**************************************************************************************
void DoDialer(ModaCPP::DeviceExternalCommunicationPort *pDevice,ModaCPP::DeviceMotor *pMLeft,ModaCPP::DeviceMotor *pMRight)
    {
    //receives chars and moves the robot according to sent keys.
    while(!_kbhit())
        {
        char temp[255];

        //receive data then display it
        int receivedbytes=pDevice->SyncRead(temp,sizeof(temp)-1);
        if(receivedbytes>=1)
            {
            float left=0;
            float right=0;
            float speed=100.0f;

            temp[receivedbytes]=0;
            _cprintf("received '%s'\r\n",temp);

            //move the robot
            switch(temp[0])
                {
                case 'E':
                case 'e':
                    left=right=speed;
                    break;
                case 'D':
                case 'd':
                    left=right=-speed;
                    break;
                case 'F':
                case 'f':
                    left=speed;
                    right=-speed;
                    break;
                case 'S':
                case 's':
                    left=-speed;
                    right=speed;
                    break;
                }
            pMLeft->SetVelocityDPS(left);
            pMRight->SetVelocityDPS(right);
            }

        //send echo to telnet
        pDevice->Write(xkode::lib::String("OK!!"),false);
        }
    }

//**************************************************************************************
int main(int argc, char* argv[])
    {
    ModaCPP::Connection *connection=new ModaCPP::Connection(true);

    xkode::lib::String Server=DEFAULTMODASERVER;
    xkode::lib::String RobotName=DEFAULTROBOTNAME;

    //process command line arguments
    ModaCPP::CommandLine::ProcessCommandLine(argc,argv);
    if(ModaCPP::CommandLine::ExistsValue("/robot"))    RobotName=ModaCPP::CommandLine::GetArgumentValue("/robot");
    if(ModaCPP::CommandLine::ExistsValue("/server"))Server     =ModaCPP::CommandLine::GetArgumentValue("/server");

    //Try connect to MODA server
    if(connection->Connect(Server))
        {
        _cprintf("Connection ok to moda server\r\n");
        //Find the robot
        ModaCPP::RobotPHX *robot=connection->QueryRobotPHX(RobotName);
        if(robot)
            {
            //Find the devices
            _cprintf("Robot found\r\n");
            ModaCPP::DeviceExternalCommunicationPort *pDevice=robot->QueryDeviceExternalCommunicationPort("port");
            ModaCPP::DeviceMotor *pMLeft=robot->QueryDeviceMotor("joint_left/axis_red/motor");
            ModaCPP::DeviceMotor *pMRight=robot->QueryDeviceMotor("joint_right/axis_red/motor");
            if(pDevice && pMLeft && pMRight)
                {
                //OK !
                _cprintf("Device 'port' and motors OK\r\n");
                _cprintf("Use Telnet on Port 20000 and press keys E, D, S, F in order to make the robot to move\r\n");
                DoDialer(pDevice,pMLeft,pMRight);
                delete pDevice;
                delete pMLeft;
                delete pMRight;
                }
            else
                {
                _cprintf("Device 'port' or motors devices not found\r\n");
                }
            delete robot;
            }
        else
            {
            _cprintf("Robot '%s' not found\r\n",RobotName.GetData());
            }
        }
    else
        {
        _cprintf("Unable to connect to moda server '%s'\r\n",Server.GetData());
        }
    //Disconnect & delete
    connection->Disconnect();
    delete connection;
    _getch();
    return 0;
    }