#include <fcntl.h>
#include <termios.h>
#include <error.h>
#include <sys/ioctl.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>


// ========================================
// SICK communication data packet structure
// ========================================

class SickLMSPacket
{
public:
  SickLMSPacket(int l=0);
  ~SickLMSPacket();

  int Resize(int newsize);
  int Setup(unsigned char cmd,unsigned char dest=0);
  int CalculateCRC(void);
  int SetCRC(void);
  int CheckCRC(void);
  void Dump(void);
  unsigned char *GetAllByte(void)  { return buff; }
  unsigned char *GetDataArea(void) { return buff+5; }
  unsigned char GetCMD(void) { return buff[4]; }
  int GetAllSize(void)  { return len+7; }
  int GetDataSize(void) { return len; }
protected:
  unsigned char *buff;   // packet it self
  int len;
};

SickLMSPacket::SickLMSPacket(int l)
{
  buff=NULL;
  Resize(l);
}

SickLMSPacket::~SickLMSPacket()
{
  if(buff) delete[] buff;
}

int SickLMSPacket::Resize(int newsize)
{
  if(buff) delete[] buff;
  len=newsize;
  buff=new unsigned char [len+7]; // {STX, ADR, LL, LH, CMD, ([] , ST,) CRCL,CRCH }
  return 0;
}

int SickLMSPacket::Setup(unsigned char cmd, unsigned char dest)
{
  if(buff==NULL) return -1;
  buff[0]=0x02; // STX
  buff[1]=dest;
  buff[2]=(len+1)&0xff; // {STX, ADR, LL, LH, << CMD, ([] , ST,)>> CRCL,CRCH }
  buff[3]=((len+1)>>8)&0xff;
  buff[4]=cmd;
  return 0;
}

int SickLMSPacket::CalculateCRC(void)
{
  if(buff==NULL) return 0;
  unsigned short crc;
  unsigned char abdata[2];
  unsigned char *d=buff;
  int l=len+5;
  crc=0;
  abdata[0]=0;
  while(l--)
    {
      abdata[1]=abdata[0];
      abdata[0]=*d;  d++;
      if(crc & 0x8000)
	{
	  crc=(crc&0x7fff)<<1;
	  crc^=0x8005;  // GEN_POL
	}
      else
	{
	  crc<<=1;
	}
      crc^=((unsigned short)(abdata[0])|((unsigned short)(abdata[1])<<8));
    }
  // printf("CRC: %04X\n",crc);
  return crc;
}

int SickLMSPacket::SetCRC(void)
{
  if(buff==NULL) return -1;
  unsigned short crc=CalculateCRC();
  buff[len+5]=crc&0xff;
  buff[len+6]=(crc>>8)&0xff;
  return 0;
}

int SickLMSPacket::CheckCRC(void)
{
  if(buff==NULL) return -1;
  unsigned short crc=CalculateCRC();
  if((buff[len+5]!=(crc&0xff))||
     (buff[len+6]!=((crc>>8)&0xff)))
    return -1;
  return 0;
}

void SickLMSPacket::Dump(void)
{
  if(buff==NULL) { printf("Invalid buffer\n"); return ; }
  switch(buff[0])
    {
    case 0x02: //STX
      printf("S02 "); break;
    case 0x06: // ACK
      printf("ACK\n"); return;
    case 0x10: // DLE
      printf("DLE\n"); return;
    case 0x15: // NACK
      printf("NACK\n"); return; 
    default:
      printf("!%02X ",buff[0]);
    }
  int l,i;
  printf("A%02X ",buff[1]);
  printf("LL%02X ",buff[2]);
  printf("LH%02X ",buff[3]);
  printf("C%02X ",buff[4]);
  l=len;
  putchar('[');
  for(i=0;i<l;i++) 
    printf("%02X ",buff[i+5]);
  printf("] CL%02X CH%02X\n",buff[l+5],buff[l+6]);
}



const unsigned int LMSCV_180_100=((180)<<16)|(100);
const unsigned int LMSCV_180_050=((180)<<16)|( 50);
const unsigned int LMSCV_180_025=((180)<<16)|( 25);
const unsigned int LMSCV_100_100=((100)<<16)|(100);
const unsigned int LMSCV_100_050=((100)<<16)|( 50);
const unsigned int LMSCV_100_025=((100)<<16)|( 25);
const unsigned char LMSCB_38400=0x40;
const unsigned char LMSCB_19200=0x41;
const unsigned char LMSCB_9600=0x42;
const unsigned char LMSCB_500000=0x48;

// ========================================
// SICK interface class
// ========================================

class SickLMSIF
{
public:
  // constructor with device name 
  SickLMSIF(const char *devname);        
  ~SickLMSIF();

  // set communication speed
  // local==0: send command to LMS200
  // local!=0(default): only change PC device speed
  int SetComSpeed(int baud,int local=1); 

  // Is device(PC dev) opened?
  int IsValid(void) { return valid; }

  int SendPacket(SickLMSPacket *p);
  // Receive LMS packet
  // block!=0 : wait until whole packet received
  // block==0 : no wait if no received byte
  SickLMSPacket *ReceivePacket(int block=0);

  // Built in LMS200 commands
  int LSCheckStatus(void);  // CMD 31
  int LSReset(void);        // CMD 10

  int LSSetOperationMode(unsigned char mode);
  int LSChangeBaud(unsigned char bm)  // CMD 20-40-43
  { return LSSetOperationMode(bm); }
  int LSMeasureContinuous()   // CMD 20-24
  { return LSSetOperationMode(0x24); }
  int LSMeasurePartialContinuous()   // CMD 20-24
  { return LSSetOperationMode(0x2a); }
  int LSMeasureStop()         // CMD 20-25
  { return LSSetOperationMode(0x25); }

  int LSRequestMeasure(int lmsn=0); // CMD 30
  int LSChangeVariant(unsigned int mode);  // CMD 3B, LMSCV

  // wait ACK and throw away rc packets
  int WaitACK(int rc=0);

protected:
  int valid;

  int fd;

  SickLMSPacket *workpack;
  int workpos,worklen;
  unsigned char workbuff[10];
  int workmode;

  char *dev;
};


// Constractor , open port
SickLMSIF::SickLMSIF(const char *devname)
{
  workpack=NULL; workmode=0;
  dev=NULL;
  valid=0;

  dev=new char [strlen(devname)+1];
  strcpy(dev,devname);
  
  fd=open(dev,O_RDWR|O_NONBLOCK);
  if(fd<0)
    {
       perror("The port is not found.");
       return;
    }
  SetComSpeed(9600,1); // default is 9600 , only local 
  valid=1;
}

SickLMSIF::~SickLMSIF()
{
  if(valid) close(fd);
  if(workpack) delete workpack;
}

// Set speed to (device and LMS200)
int SickLMSIF::SetComSpeed(int baud,int local)
{
  if(!valid) return -1;

  if(!local)
    {
      switch(baud)
	{
      	case   9600: LSChangeBaud(LMSCB_9600);   WaitACK(1);  break;
	case  19200: LSChangeBaud(LMSCB_19200);  WaitACK(1);  break;
	case  38400: LSChangeBaud(LMSCB_38400);  WaitACK(1);  break;
	case 500000: LSChangeBaud(LMSCB_500000); WaitACK(1);  break;
	}
      usleep(500000);
    }

  close(fd);
  fd=open(dev,O_RDWR|O_NONBLOCK);
  if(fd<0)
    {
      perror("Port reopen failed.");
      valid=0;
      }

  struct termios options;
  switch(baud)
    {
    case 9600:
      tcgetattr(fd, &options);
      cfmakeraw (&options);
      cfsetspeed(&options,B9600);
      tcsetattr(fd, TCSANOW, &options);
      break;
    case 19200:
      tcgetattr(fd, &options);
      cfmakeraw (&options);
      cfsetspeed(&options,B19200);
      tcsetattr(fd, TCSANOW, &options);
      break;
    case 38400:
      tcgetattr(fd, &options);
      cfmakeraw (&options);
      cfsetspeed(&options,B38400);
      tcsetattr(fd, TCSANOW, &options);
      break;
    case 500000:
      tcgetattr(fd, &options);
      cfmakeraw (&options);
      cfsetspeed(&options,B500000);
      tcsetattr(fd, TCSANOW, &options);
      break;
    default:
      printf("can not set %d baud\n",baud);
    }
  return 0;
}

int SickLMSIF::SendPacket(SickLMSPacket *p)
{
  if(!valid) return -1;
  p->SetCRC();
  int l=p->GetAllSize();
  unsigned char *d=p->GetAllByte();
  while(l)
    {
      int r=write(fd,d,l);
      if(r<0)
	{
	  perror("SickLMSIF::SendPacket");
	  return -1;
	}
      d+=r;
      l-=r;
    }
  return 0;
}

SickLMSPacket *SickLMSIF::ReceivePacket(int block)
{
  int r;
  do
    {
      if(workmode==0) // init
	{
	  workpos=0;
	  worklen=0;
	  workmode++;
	}
      if(workmode==1) // recieve header part (4byte)
	{
	  r=read(fd,workbuff+workpos,4-workpos);
	  if(r<0)
	    {
	      if(errno==EAGAIN) r=0;
	      else
		{
		  perror("SickLMSIF::ReceivePacket");
		  return NULL;
		}
	    }
	  if((r==0)&&(!block)) break;
	  if(r==0) { /*usleep(500);*/ continue; }

	  workpos+=r;
	  // printf("|%d|",workpos);
	  if((workbuff[0]==0x06) ||  // ACK
	     (workbuff[0]==0x10) ||  // DLE
	     (workbuff[0]==0x15))    // NACK
	    {
	      SickLMSPacket *p=new SickLMSPacket;
	      (p->GetAllByte())[0]=workbuff[0];
	      workbuff[0]=workbuff[1];
	      workbuff[1]=workbuff[2];
	      workbuff[2]=workbuff[3];
	      workpos--;
	      return p;
	    }
	  if(workpos==4) workmode=2;
	}
      if(workmode==2)
	{
	  int len=workbuff[2]|((unsigned short)(workbuff[3])<<8);
	  len=len-1;
	  if(workpack==NULL) workpack=new SickLMSPacket(len);
	  memcpy(workpack->GetAllByte(),workbuff,4);
	  workmode=3;
	  workpos=4; worklen=len+1+2;  // CMD, [DATA,ST] CRC*2 
	}
      if(workmode==3)
	{
	  r=read(fd,workpack->GetAllByte()+workpos,worklen);
	  // printf("%d",r);
	  if(r<0)
	    {
	      if(errno==EAGAIN) r=0;
	      else
		{
		  perror("SickLMSIF::ReceivePacket");
		  return NULL;
		}
	    }
	  if((r==0)&&(!block)) break;
	  if(r==0) { /*usleep(1000);*/ continue; }
	  worklen-=r;
	  workpos+=r;
	  if(worklen==0) break;
	}
    } while(block);
  if((workmode==3)&&(worklen==0))
    {
      workmode=0;
      SickLMSPacket *r=workpack;
      workpack=NULL;
      return r;
    }
  return NULL;
}


int SickLMSIF::LSCheckStatus(void)  // CMD 31
{
  SickLMSPacket p(0);
  p.Setup(0x31);  p.SetCRC();
  SendPacket(&p);
  return 0;
}

int SickLMSIF::LSReset(void)        // CMD 10
{
  SickLMSPacket p(0);
  p.Setup(0x10);  p.SetCRC();
  SendPacket(&p);
  return 0;
}

int SickLMSIF::LSSetOperationMode(unsigned char mode)
{
  SickLMSPacket p(1);
  p.Setup(0x20);  p.GetDataArea()[0]=mode;
  p.SetCRC();
  SendPacket(&p);
  return 0;
}

int SickLMSIF::LSRequestMeasure(int lmsn) // CMD 30
{
  SickLMSPacket p(1);
  p.Setup(0x30);  
  p.GetDataArea()[0]=lmsn;  // for LMI400 only lmsn-0x01 2byte
  p.GetDataArea()[0]=0x01;
  p.SetCRC();
  SendPacket(&p);
  return 0;
}

int SickLMSIF::LSChangeVariant(unsigned int mode)  // CMD 3B, LMSCV
{
  SickLMSPacket p(4);
  p.Setup(0x3b);
  p.GetDataArea()[0]=(mode>>16)&0xff;
  p.GetDataArea()[1]=(mode>>24)&0xff;
  p.GetDataArea()[2]=(mode    )&0xff;
  p.GetDataArea()[3]=(mode>> 8)&0xff;
  p.SetCRC();
  SendPacket(&p);
  return 0;
}

int SickLMSIF::WaitACK(int rc)
{
  if(!valid) return -1;
  SickLMSPacket *r;
  int dc=0,ec=0;
#ifdef DEBUGPRINT
  printf("\nwait ack");
#endif
  while(1)
    {
      r=ReceivePacket(0);
      if(r==NULL) 
	{ 
	  usleep(10000); 
#ifdef DEBUGPRINT
	  putchar('='); fflush(stdout); // no receive, wait
#endif
	  dc++; if(dc>50) return -1;
	  continue; 
	}
      dc=0;
      unsigned char m=r->GetAllByte()[0];
      delete r;
      if((m==0x06)||
	 (m==0x10)||
	 (m==0x15))
	break;
      else
	{
#ifdef DEBUGPRINT
	  puts("Not ACK");
#endif
	  ec++;
	  if(ec>20) return -1;
	}
    }
    for(int i=0;i<rc;i++)
    {
#ifdef DEBUGPRINT
      printf("\nwait %d/%d packet",i,rc);
#endif
      while(1)
	{
	  r=ReceivePacket(0);
	  if(r==NULL) 
	    { 
	      usleep(10000); 
#ifdef DEBUGPRINT
	      putchar('='); fflush(stdout);  // no receive, wait
#endif
	      dc++; if(dc>50) return -1;
	      continue; 
	    }
	  dc=0;
	  delete r;
	  break;
	}
    }
  return 0;
}


#ifndef USEASLIB

int main(void)
{
  int ret;
  unsigned char buff[1024];
  SickLMSIF lms("/dev/ttyS0");
  lms.SetComSpeed(38400,0);
  //lms.SetComSpeed(38400,1);
  //lms.SetComSpeed(9600,1);
  SickLMSPacket pack,*r;
  int c=0;

  if(!lms.IsValid())
    {
      return 1;
    }
 
  /*unsigned char cmd[7]={0x02,0x00,0x01,0x00,0x31,0x15,0x12};


    if(write(lms.fd,cmd,7)!=7)
    {
       perror("The port is not found.");
      return -1;
      }*/
  //lms.SendPacket(&pack);
  //lms.LSRequestMeasure();
  lms.LSChangeVariant(LMSCV_180_050); lms.WaitACK(1);
  //lms.LSMeasureContinuous();  lms.WaitACK(1);
  lms.LSMeasurePartialContinuous();  lms.WaitACK(1);
  puts("opened");
  int i,l;
  while(1)
    {
      r=lms.ReceivePacket(0);
      if(r==NULL) 
	{ 
	  usleep(10000); 
	  putchar('.'); fflush(stdout); 
	  continue; 
	}
      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);
	  for(i=0;i<l;i++)
	    printf(" %5d  ",(w[i+1])&0x1fff);
	  puts("");
	  c++;
	  if(c==5)
	    lms.LSMeasureStop();
	  break;
	}
      

      delete r;
      puts("");
    }
  return 0;
}

#endif