//////////////////////////////////////////////////////
//Sample from samples/devices/EmitterReceiver
//////////////////////////////////////////////////////

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

#define MODASERVER "localhost"

#define STARTPOINT "/"

#define RECEIVER1    "receiver1/zone_sensor/sensor"
#define RECEIVER2    "receiver2/zone_sensor/sensor"
#define EMITTER        "emitter/zone_sensor/sensor"
#define JACK        "slider/axis/j"
#define LIGHT1        "light1/l"
#define LIGHT2        "light2/l"

using namespace ModaCPP;

CRITICAL_SECTION csDisplaySection;

//---------------------------------------------------------------------
class AReceiver
    {
    public:
    DeviceEmitterReceiver *pReceiver;
    DeviceLightSource *pLight;

    AReceiver(DeviceEmitterReceiver *_pReceiver,DeviceLightSource *_pLight)
        {
        pReceiver=_pReceiver;
        pLight=_pLight;
        }
    };

//---------------------------------------------------------------------
void DoReceiver(void *pParam)
    {
    AReceiver *pParams=(AReceiver *)pParam;

    DeviceEmitterReceiver *pReceiver=pParams->pReceiver;
    DeviceLightSource *pLight=pParams->pLight;
    pLight->SetIntensity(0);

    int MySessionID=pReceiver->CreateCommunicationSession();
    xkode::lib::Array<MU8> Received;
    bool bToggle=false;
    while(true)
        {
        int SenderSessionID=0;

        M32 DeviceResponse=MODA_EOK;
        M32 SystemResponse=MODA_EOK;
        SystemResponse=pReceiver->ReceiveFromAll(MySessionID,SenderSessionID,Received,100,&DeviceResponse);

        if(Received.Count())
            {
            //received some bytes

            //toggle the light
            if(bToggle)    pLight->SetIntensity(100);
            else        pLight->SetIntensity(0);
            bToggle=!bToggle;

            //display
            EnterCriticalSection(&csDisplaySection);
            _cprintf("[%d]: received:%s from %d, SR=%d, DR:%d\r\n",MySessionID,Received.GetData(),SenderSessionID,SystemResponse,DeviceResponse);
            LeaveCriticalSection(&csDisplaySection);
            }
        else
            {
            //received nothing

            //switch off the light
            pLight->SetIntensity(0);

            //display
            EnterCriticalSection(&csDisplaySection);
            _cprintf("[%d]: SR=%d, DR:%d\r\n",MySessionID,SystemResponse,DeviceResponse);
            LeaveCriticalSection(&csDisplaySection);

            //wait a moment ...
            pReceiver->GetRobot()->GetConnection()->Sleep(100);
            }
        }
    }

//---------------------------------------------------------------------
int main(int argc, char* argv[])
{
//safe display
InitializeCriticalSection(&csDisplaySection);

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
    ModaCPP::RobotPHX *pWorld=connection->QueryRobotPHX(STARTPOINT);
    if(pWorld)
        {
        _cprintf("World PHX found\r\n");

        //find devices
        DeviceEmitterReceiver *pEmitter=pWorld->QueryDeviceEmitterReceiver(EMITTER);
        DeviceEmitterReceiver *pReceiver1=pWorld->QueryDeviceEmitterReceiver(RECEIVER1);
        DeviceEmitterReceiver *pReceiver2=pWorld->QueryDeviceEmitterReceiver(RECEIVER2);
        DeviceActuatingCylinder *pJack=pWorld->QueryDeviceActuatingCylinder(JACK);
        DeviceLightSource *pLight1=pWorld->QueryDeviceLightSource(LIGHT1);
        DeviceLightSource *pLight2=pWorld->QueryDeviceLightSource(LIGHT2);

        if(pEmitter && pReceiver1 && pReceiver2 && pJack && pLight1 && pLight2)
            {
            //devices are OK

            //create receivers threads
            DWORD dwID;
            CreateThread(NULL,0,(LPTHREAD_START_ROUTINE)DoReceiver,new AReceiver(pReceiver1,pLight1),0,&dwID);
            CreateThread(NULL,0,(LPTHREAD_START_ROUTINE)DoReceiver,new AReceiver(pReceiver2,pLight2),0,&dwID);

            //start moving the jack
            float testpos=0.7f;
            float pos=0.71f;
            pJack->GoPosition(pos);

            //emit data every 50 ms, from the emiter, move the emitter along the scene
            int loop=0;

            int EmitterSessionID=pEmitter->CreateCommunicationSession();
            _cprintf("Emitter: dialing with communication session %d\r\n",EmitterSessionID);
            if(EmitterSessionID>0)
                {
                while(!_kbhit())
                    {
                    //move the jack
                    float jackposition=pJack->GetPosition();
                    if(jackposition>=testpos) pJack->GoPosition(-pos);
                    if(jackposition<=-testpos) pJack->GoPosition(pos);

                    //emit data
                    xkode::lib::String temp(loop++);
                    pEmitter->SendToAll(EmitterSessionID, (MU8 *) temp.GetData(), temp.Length()+1);
                    connection->Sleep(50);
                    }

                pEmitter->DeleteCommunicationSession(EmitterSessionID);
                }
            else
                {
                _cprintf("Invalid session ID %d\\r\n",EmitterSessionID);
                }
            }
        else
            {
            _cprintf("Missing one or more device(s)\r\n");
            }
        }
    else
        {
        _cprintf("World PHX not found\r\n");
        }
    }
else
    {
    _cprintf("Unable to connect to moda server\r\n");
    }
//Disconnect & delete
connection->Disconnect();
delete connection;
DeleteCriticalSection(&csDisplaySection);
return 0;
}