////////////////////////////////////////////////////////////
//Simple AvoidObstacle from documentation tutorial
////////////////////////////////////////////////////////////

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

using namespace ModaCPP;

#define DEFAULT_MODA_SERVER "localhost"
#define DEFAULT_ROBOTNAME    "/creupsrobot0"

//***********************************************************************
class MyRobot
    {
    private:
        ModaCPP::RobotPHX        *_pPHX;
        ModaCPP::DeviceDistance *_pLeftSensor;
        ModaCPP::DeviceDistance *_pRightSensor;
        ModaCPP::DeviceMotor    *_pLeftMotor;
        ModaCPP::DeviceMotor    *_pRightMotor;

    public:
        MyRobot(ModaCPP::RobotPHX *pPHX)
            {
            _pPHX=pPHX;

            //query distance sensors devices
            _pLeftSensor=_pPHX->QueryDeviceDistance("zone_left/mysensor");
            _pRightSensor=_pPHX->QueryDeviceDistance("zone_right/mysensor");

            if(_pLeftSensor && _pRightSensor)    _cprintf("Distance sensors OK\r\n");
            else                                _cprintf("Distance sensors ERROR\r\n");

            //query motors devices
            _pLeftMotor=_pPHX->QueryDeviceMotor("hinge_left/a2/mymotor");
            _pRightMotor=_pPHX->QueryDeviceMotor("hinge_right/a1/mymotor");

            if(_pLeftMotor && _pRightMotor)    _cprintf("Motors OK\r\n");
            else                            _cprintf("Motors ERROR\r\n");
            }

        bool IsValid(void)
            {
            if(_pLeftSensor && _pRightSensor && _pLeftMotor && _pRightMotor)    return(true);
            else                                                                return(false);
            }

        void Step(void)
            {
            //Do step
            #define LARGE 1.0f
            #define MEDIUM 0.5f
            #define MINI 0.2f

            #define MAXSPEED 720
            #define MEDIUMSPEED 360

            float lm,rm;
            float ld,rd;

            int PerformWait=0;

            ld=_pLeftSensor->GetMeasure();
            rd=_pRightSensor->GetMeasure();

            if( (ld>=LARGE) && (rd>=LARGE))
                {
                lm=MAXSPEED;
                rm=MAXSPEED;
                }
            else
                {
                if( (ld>=MEDIUM) && (rd>=MEDIUM))
                    {
                    PerformWait=200;
                    if(ld>rd)
                        {
                        lm=MEDIUMSPEED;
                        rm=MAXSPEED;
                        }
                    else
                        {
                        rm=MEDIUMSPEED;
                        lm=MAXSPEED;
                        }
                    }
                else
                    {
                    if(ld>rd)
                        {
                        lm=-MEDIUMSPEED;
                        rm=MEDIUMSPEED;
                        }
                    else
                        {
                        lm=MEDIUMSPEED;
                        rm=-MEDIUMSPEED;
                        }
                    PerformWait=250;
                    }
                }
            _pLeftMotor->SetVelocityDPS(lm);
            _pRightMotor->SetVelocityDPS(rm);

            //force long rotations
            if(PerformWait) _pPHX->GetConnection()->Sleep(PerformWait);
            }
    };

//***********************************************************************
int main(int argc, char* argv[])
    {
    CommandLine::ProcessCommandLine(argc,argv);
    //CommandLine::DisplayArguments();

    //manage command line argument
    xkode::lib::String RobotName=DEFAULT_ROBOTNAME;
    xkode::lib::String ServerAdress=DEFAULT_MODA_SERVER;

    if(ModaCPP::CommandLine::ExistsValue("/robot"))    RobotName=ModaCPP::CommandLine::GetArgumentValue("/robot");
    if(ModaCPP::CommandLine::ExistsValue("/server"))ServerAdress=ModaCPP::CommandLine::GetArgumentValue("/server");

    _cprintf("Using %s on server %s\r\n",RobotName.GetData(),ServerAdress.GetData());

    ModaCPP::Connection *pConnection=new ModaCPP::Connection(true);
    //Try connect to MODA server
    if(pConnection->Connect(ServerAdress))
        {
        _cprintf("Connection ok to moda server\r\n");
        //Find the robot
        ModaCPP::RobotPHX *pPHX=pConnection->QueryRobotPHX(RobotName);
        if(pPHX)
            {
            _cprintf("robot found, searching devices\r\n");
            MyRobot *pMyRobot=new MyRobot(pPHX);
            if(pMyRobot->IsValid())
                {
                while(!_kbhit())
                    {
                    //step the robot
                    pMyRobot->Step();
                    //update every 50 simulated ms
                    pConnection->Sleep(50);
                    }
                }
            delete pMyRobot;
            delete pPHX;
            }
        else
            {
            _cprintf("robot %s found\r\n",RobotName.GetData());
            }
        }
    else
        {
        _cprintf("Unable to connect to moda server %s\r\n",ServerAdress.GetData());
        }
    //Disconnect & delete
    pConnection->Disconnect();
    delete pConnection;
    _getch();
    return 0;
    }