#define USEASLIB

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

#define DEBUGPRINT
#include "sickcomif.cc"

// 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 PrintData(void)
{
  static int seq=0;
  seq++;
  if(seq%2!=0) return; // thin out
  int i;
  printf("%d ");
  for(i=0;i<lmsdatasize;i++)
    {
      printf("%4d ",lmsdata[i].distance);
    }
  printf("\n\n");
}


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;

  // 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
	    {
	      PrintData();
	    }
	  break;

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