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

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

using namespace ModaCPP;

#define DEFAULT_MODA_SERVER "localhost"
#define DEFAULT_ROBOTNAME    "/robot0"

//***********************************************************************
class MyCustomRobot:public MyRobot
    {
    private:
        ModaCPP::DeviceDistance *_pLeftSensor;
        ModaCPP::DeviceDistance *_pRightSensor;
        ModaCPP::DeviceMotor    *_pLeftMotor;
        ModaCPP::DeviceMotor    *_pRightMotor;

    public:
        //Constructor: do not make any MODA calls because the object is not
        //fully created
        MyCustomRobot(xkode::lib::String PHXName):MyRobot(PHXName)
            {
            }

        //called by system if PHX was found in the simulation tree.
        virtual void OnStartup(void)
            {
            //startup the automatic robot (by querying its devices ...)
            MyRobot::OnStartup();

            //distance sensors devices
            _pLeftSensor=GetDistanceByIndex(MyRobot_DISTANCE01_INDEX);
            _pRightSensor=GetDistanceByIndex(MyRobot_DISTANCE00_INDEX);

            //motors devices
            _pLeftMotor    =GetMotorByIndex(MyRobot_MOTOR01_INDEX);
            _pRightMotor=GetMotorByIndex(MyRobot_MOTOR00_INDEX);
            }

        //open loop
        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) GetConnection()->Sleep(PerformWait);
            }
    };

//***********************************************************************
int main(int argc, char* argv[])
    {
    _cprintf("Avoidobstacle using class creator\r\n");

    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
        MyCustomRobot *pPHX=pConnection->QueryRobotPHX2<MyCustomRobot>(RobotName);
        if(pPHX && pPHX->IsValid())
            {
            _cprintf("robot was found and is valid\r\n");
            while(!_kbhit())
                {
                //step the robot
                pPHX->Step();
                //update every 50 simulated ms
                pConnection->Sleep(50);
                }
            delete pPHX;
            }
        else
            {
            _cprintf("robot %s was not found or is not valid\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;
    }