///////////////////////////////////////////////////////
// Sample from Samples/Devices/AbsoluteCompass/C++
///////////////////////////////////////////////////////

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

#define MODASERVER "localhost"

using namespace ModaCPP;
using namespace xkode::lib;

//-------------------------------------------------------------------------------
//global vars
ModaCPP::Connection *connection;
DeviceMotor *pLeft;
DeviceMotor *pRight;
DeviceAbsolutCompass *pCompass;

DeviceLightSource *pLightN;
DeviceLightSource *pLightS;
DeviceLightSource *pLightW;
DeviceLightSource *pLightE;

//-------------------------------------------------------------------------------
float CalculateError(float currentangle,float togodeg)
    {
    currentangle=MathFunctions::Mod_MPI_PI(currentangle);
    float error=currentangle-MathFunctions::ToRad(togodeg);
    return( xkode::lib::Math::fmod_MPI_PI_FLOAT(error));
    }

//-------------------------------------------------------------------------------
void RegulateAngle(float togodeg,DeviceLightSource *pLight)
    {
    float angle,error;

    pLight->SetIntensity(0.7f);
    while(true)
        {
        angle=pCompass->GetAngleRAD();
        error=CalculateError(angle,togodeg);

        _cprintf("togo: %f currentangle: %f, error:%f\r\n",togodeg,MathFunctions::ToDeg(angle),MathFunctions::ToDeg(error));

        pLeft->SetVelocityRPS(2.0f*error);
        pRight->SetVelocityRPS(-2.0f*error);
        connection->Sleep(20);

        if( fabs(error)<=xkode::lib::Math::ToRad(5.0f))
            {
            pLight->SetIntensity(1.0f);
            pLeft->SetVelocityRPS(0);
            pRight->SetVelocityRPS(0);
            connection->Sleep(2000);
            break;
            }
        }
    pLight->SetIntensity(0.0f);
    }

//-------------------------------------------------------------------------------
int main(int argc, char* argv[])
{
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
    MyScene *pScene=connection->QueryRobotPHX2<MyScene>("/");

    if(pScene && pScene->IsValid() )
        {
        _cprintf("scene ok\r\n");

        pLeft=pScene->GetMotorByIndex(MyScene_MOTOR01_INDEX);
        pRight=pScene->GetMotorByIndex(MyScene_MOTOR00_INDEX);
        pCompass=pScene->GetAbsolutCompassByIndex(MyScene_ABSOLUTCOMPASS00_INDEX);

        pLightN=pScene->GetLightSourceByPath("lightN/rlight0/l");
        pLightS=pScene->GetLightSourceByPath("lightS/rlight0/l");
        pLightW=pScene->GetLightSourceByPath("lightW/rlight0/l");
        pLightE=pScene->GetLightSourceByPath("lightE/rlight0/l");

        //sitch off lights
        pLightN->SetIntensity(0);
        pLightS->SetIntensity(0);
        pLightW->SetIntensity(0);
        pLightE->SetIntensity(0);

        //force devices on state
        pCompass->On();
        pRight->On();
        pLeft->On();
        connection->Sleep(100);

        while(!_kbhit())
            {
            //rotate in north direction
            RegulateAngle(0,pLightN);

            //rotate in north west
            RegulateAngle(90,pLightW);

            //rotate in north south
            RegulateAngle(180,pLightS);

            //rotate in north east
            RegulateAngle(270,pLightE);
            }
        delete pScene;
        }
    else
        {
        _cprintf("robot not found\r\n");
        }
    }
else
    {
    _cprintf("Unable to connect to moda server\r\n");
    }
//Disconnect & delete
connection->Disconnect();
delete connection;
_getch();
return 0;
}