///////////////////////////////////////////////////
//Sample from Samples/devices/lidar
///////////////////////////////////////////////////

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

#define MODASERVER    "localhost"
#define WORLD        "/"
#define DEVICENAME    "zone0/lidar"

using namespace ModaCPP;

//*********************************************************************************
void DisplayScan(Moda::Commons::LidarScan  *pScan)
    {
    //display directly on screen
    int dx=200,dy=200;

    HDC hdcecr=GetDC(NULL);
    HBRUSH hBrush=CreateSolidBrush(RGB(255,255,255));
    HPEN hPen=CreatePen(PS_SOLID,1,RGB(0,0,0));
    HBRUSH hSaveBrush;
    HPEN hSavePen;

    //save data
    hSaveBrush=(HBRUSH) SelectObject(hdcecr,hBrush);
    hSavePen=(HPEN) SelectObject(hdcecr,hPen);

    //paint data
    //Rectangle(hdcecr,0,0,dx,dy);
    if(pScan->ValuesCount)
        {
        int lines=pScan->ValuesCount/pScan->ValuesPerLine;
        float measureheight=float(dy)/float(lines);
        float measurewidth=float(dx)/float(pScan->ValuesPerLine);
        float minvalue=999.0f;
        float maxvalue=-999.0f;

        for(int i=0;i<pScan->ValuesCount;i++)
            {
            if(pScan->Values[i]>maxvalue)
                {
                //ignore out of range values
                if(pScan->Values[i]<pScan->RangeMax)    maxvalue=pScan->Values[i];
                }

            if(pScan->Values[i]<minvalue) minvalue=pScan->Values[i];
            }

        //check
        //all values are out ouf range
        if(maxvalue<=0)
            {
            maxvalue=pScan->RangeMax;
            minvalue=0;
            }

        for(int line=0;line<lines;line++)
            {
            for(int index=0;index<pScan->ValuesPerLine;index++)
                {
                float measure=pScan->Values[line*pScan->ValuesPerLine+index];
                if(measure>maxvalue)    measure=maxvalue;
                if(measure<minvalue)    measure=minvalue;

                int color=255-int( ((measure-minvalue)/(maxvalue-minvalue))*255.0f);

                HBRUSH hTemp=CreateSolidBrush(RGB(color,0,0));
                SelectObject(hdcecr,hTemp);

                int x=dx-(int) ((index+1)*measurewidth);
                int y=(int) (line*measureheight);

                Rectangle(hdcecr,x,y,x+max(1,1+(int)measurewidth),y+max(1,1+(int)measureheight));
                SelectObject(hdcecr,hSaveBrush);
                DeleteObject(hTemp);
                }
            }
        }

    //restore data
    SelectObject(hdcecr,hSaveBrush);
    SelectObject(hdcecr,hSavePen);

    DeleteObject(hBrush);
    DeleteObject(hPen);

    ReleaseDC(NULL,hdcecr);
    }

//*********************************************************************************
void EnumWorkingModes(DeviceLidar *pLidar)
    {
    xkode::lib::Array<Moda::Commons::LidarWorkingMode> _Modes;
    pLidar->GetWorkingModes(_Modes);
    _cprintf("Enum Working modes:\r\n");
    for(int i=0;i<_Modes.Count();i++)
        {
        Moda::Commons::LidarWorkingMode &m=_Modes[i];
        _cprintf("->%s\r\n",m.ToString().GetData());
        }
    _cprintf("Enum end.\r\n");
    }

//*********************************************************************************
int main(int argc, char* argv[])
    {
    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(WORLD);
        if(pWorld)
            {
            _cprintf("world found\r\n");
            DeviceLidar *pLidar=pWorld->QueryDeviceLidar(DEVICENAME);
            if(pLidar)
                {
                _cprintf("device found\r\n");

                //enum working modes
                EnumWorkingModes(pLidar);

                //scan
                int time=pWorld->GetTime();
                int workingmode=0;
                while(!_kbhit())
                    {
                    xkode::lib::Array<MU8> _Scan;
                    M32 DeviceResponse;
                    M32 SystemResponse=pLidar->GetScanWaitComplete(_Scan,&DeviceResponse);
                    if(_Scan.Count()!=0)
                        {
                        //Data is valid if array is not empty
                        Moda::Commons::LidarScan *pScan=(Moda::Commons::LidarScan *)_Scan.GetData();
                        DisplayScan(pScan);
                        }
                    else
                        {
                        _cprintf("error while getting scan data (system:%d device:%d)\r\n",SystemResponse,DeviceResponse);
                        }
                    }
                delete pLidar;
                }
            else
                {
                _cprintf("device %s not found\r\n",DEVICENAME);
                }
            delete pWorld;
            }
        else
            {
            _cprintf("world not found\r\n");
            }
        }
    else
        {
        _cprintf("Unable to connect to moda server\r\n");
        }
    //Disconnect & delete
    connection->Disconnect();
    delete connection;
    _getch();
    return 0;
    }