#define USEASLIB

#include <math.h>
#include <poll.h>

#define DEBUGPRINT
#include "sickcomif.cc"

#define extern extern "C"
#include "eggx.h" 
#undef extern

// EGGX variables for map window 
int wnd;
int winw=800,winh=600;
float ofx=400, ofy=100, magx=0.15, magy=0.15;

// Window Initialize
int InitializeGraph(void)
{
  gsetinitialparsegeometry("-0+0");
  wnd=gopen(winw,winh);
  gsetbgcolor(wnd,"white") ;
  gclr(wnd);
  layer(wnd,0,1) ;
  return 0;
}

// Graph supporting functions
void trans(float &x,float &y)
{
  x=x*magx+ofx;
  y=y*magy+ofy;
}

void revtrans(float &x,float &y)
{
  x=(x-ofx)/magx;
  y=(x-ofy)/magy;
}

inline void tline(int wn,float x,float y,int mode)
{
  trans(x,y);
  line(wn,x,y,mode);
}

inline void tline(int wn,float sx,float sy,float ex,float ey)
{
  trans(sx,sy);  trans(ex,ey);
  line(wn,sx,sy,PSET);
  line(wn,ex,ey,PENDOWN);
}

inline void tdrawsym(int wn,float x,float y,int size,int nsym)
{
  trans(x,y);
  drawsym(wn,x,y,size,nsym);
}

void drawaxis(void)
{
  int i;
  int tick=100;
  int mc=5;
  float sx=0,sy=0,ex=winw,ey=winh,t;
  revtrans(sx,sy);  revtrans(ex,ey);
  if(ey<sy) ey=sy;
  if(ex<sx) { t=sx; sx=ex; ex=t; }
  newrgbcolor(wnd,128,192,128);
  for(i= tick;i<ex;i+=tick)
    tline(wnd, i,0, i,ey);
  for(i=-tick;i>sx;i-=tick)
    tline(wnd, i,0, i,ey);
  for(i= tick;i<ey;i+=tick)
    tline(wnd,sx,i,ex,i);

  tick*=mc;
  newrgbcolor(wnd,64,96,64);
  for(i= tick;i<ex;i+=tick)
    tline(wnd, i,0, i,ey);
  for(i=-tick;i>sx;i-=tick)
    tline(wnd, i,0, i,ey);
  for(i= tick;i<ey;i+=tick)
    tline(wnd,sx,i,ex,i);

  newrgbcolor(wnd,0,0,255);
  tline(wnd,sx,0,ex,0);
  tline(wnd,0,0,0,ey);
}



// Structure for LMS measured data
struct LMSdata
{
  double angle;  // detected angle
  int distance;  // distance (raw value)
  double x;      // detected position
  double y;      //   from angle and distance
  LMSdata(void) { angle=0; distance=0; x=y=0; }
};

// LMS measuring mode  ( in sickcomif.cc )
const int LMSMeasureMode=LMSCV_100_025;

const int interpolate=4;
const int lmsdatasize=180*interpolate+1;

// Received data
LMSdata lmsdata[lmsdatasize];

// convert and store received data into LMSdata
int SetLMSData(int seq,int intp,int d)
{
  if(interpolate==2) intp=(intp>>1)&1;  // 0-50
  if(interpolate==4) intp=(intp)&3;     // 0-25-50-75
  if(interpolate==1) intp=0;
  int index=seq*interpolate+intp;
  if((index<0)||(index>=lmsdatasize)) return -1;
  lmsdata[index].angle=seq+(double)intp/interpolate;
  lmsdata[index].distance=d;
  double rad=lmsdata[index].angle/180*M_PI;
  lmsdata[index].x=cos(rad)*d;
  lmsdata[index].y=sin(rad)*d;
  return 0;
}

// convert LMS200 distance packet int LMSdata structure
void UpdateMeasure(SickLMSPacket *r)
{
  unsigned char *d=r->GetDataArea();
  unsigned short *w;
  w=(unsigned short *)(d);

  int res=(w[0]&(1<<14))?1:10;   // resolution
  int i;
  int l=w[0]&0x3ff;

  if(w[0]&(1<<13))
    {  // partial scan data 0-50/0-25-50-75 
      int p=((w[0]>>11)&3);
      for(i=0;i<l;i++)
	{
	  int d=(w[i+1]&0x7fff)*res;
	  SetLMSData(i,p,d);
	}
    }
  else
    {  // complete scan data
      for(i=0;i<l;i++)
	{
	  int d=(w[i+1]&0x7fff)*res;
	  SetLMSData(i/interpolate,(i%interpolate)*(4/interpolate),d);
	}
    }
}

void DrawData(void)
{
  gclr(wnd);
  drawaxis();
  int i;
  newrgbcolor(wnd,192,192,192);
  for(i=0;i<lmsdatasize;i+=2)
    {
      tline(wnd,lmsdata[i].x,lmsdata[i].y,0,0);
    }
  newrgbcolor(wnd,255,0,0);
  for(i=0;i<lmsdatasize;i++)
    {
      tdrawsym(wnd,lmsdata[i].x,lmsdata[i].y,8,5);
    }

  copylayer(wnd,1,0) ;
}

void TerminateLMS(SickLMSIF *lms)
{
  for(int i=0;i<10;i++)
    lms->LSMeasureStop();
  puts("");
  sleep(1);
  while(lms->ReceivePacket(0)!=NULL)
    ;
  lms->SetComSpeed(9600,0);
  lms->LSMeasureStop();

}

int main(int argc,char **)
{
  const char *device="/dev/ttyUSB1";
  const int baudrate=500000;  /* 38400, 9600 */

  SickLMSIF lms(device);

  SickLMSPacket pack,*r;

  if(!lms.IsValid())
    return 1;

  InitializeGraph();
  DrawData();
  

  // stdin poll
  struct pollfd fds;

  int i,l,loopf=1,initf=1;
  int dc=0;
  while(loopf)
    {
      // stdin polling
      fds.fd=0; // stdin 
      fds.events=POLLIN;
      if(poll(&fds,1,0)>0)
	loopf=0; // loop out

      if(initf)
	{
	  if(argc>1)  // if arg found, only set speed of interface
	    lms.SetComSpeed(baudrate,1);
	  else
	    lms.SetComSpeed(baudrate,0);
	  //lms.LSRequestMeasure();
	  lms.LSChangeVariant(LMSMeasureMode); lms.WaitACK(1);
	  //lms.LSMeasureContinuous();  lms.WaitACK(1);
	  lms.LSMeasurePartialContinuous();  lms.WaitACK(0);
	  initf=0; dc=0;
	}

      r=lms.ReceivePacket(0);  // non-block: if not packet, return immediately
      if(r==NULL) 
	{ 
	  usleep(10000); putchar('.'); fflush(stdout); 
	  dc++;
	  if(dc>50) 	  // disconnection ?, re-init
	    {
	      TerminateLMS(&lms);
	      sleep(1);
	      initf=1;  
	    }
	  continue; 
	}
      dc=0;
      //r->Dump();
      //if(r->CheckCRC()<0) { puts("CRC Error"); }
      //else {puts("CRC OK"); }
      switch(r->GetCMD())
	{
	case 0x90: // startup message
	  l=r->GetDataSize();
	  printf("Init message: %*.*s\n",l,l,r->GetDataArea());
	  break;
	case 0xb0: // measured value;
	  unsigned char *d=r->GetDataArea();
	  unsigned short *w;
	  w=(unsigned short *)(d);
	  printf("res %s ",(w[0]&(1<<14))?"1mm":"1cm");
	  if(w[0]&(1<<13))
	    printf("PScan %d ",((w[0]>>11)&3)*25);
	  else
	    printf("CScan ");
	  l=w[0]&0x3ff;
	  printf("%d data\n",l);
	  int lc=0;
	  for(i=0;i<l;i++)
	    {
	      //printf(" %5d  ",(w[i+1])&0x1fff);
	      if(((w[i+1])&0x1fff)<200) lc++;
	    }
	  puts("");
	  if(lc>l/4)
	    loopf=0; // loop out

	  UpdateMeasure(r);
	  if(((w[0]>>11)&3)==3) // partial scan finished at 0.25deg
	    {
	      DrawData();
	    }
	  break;

	}
      delete r;
      puts("");
    }
  TerminateLMS(&lms);
  return 0;
}