#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include "logutils.h"
#include "ioutils.h"

#define SICK_MAX_MSG_LENGTH 1024

// sick guarantees 14ms, but we have to deal with the possibility that
// we might be context swapped out... so we inflate this number.

#define SICK_TIMEOUT_FACTOR (1)

#define SICK_RX_TIMEOUT_MS (100*SICK_TIMEOUT_FACTOR)
#define SICK_CHANGE_OPMODE_TIMEOUT_MS (3100*SICK_TIMEOUT_FACTOR)
#define SICK_MAX_RETRIES (4)
#define SICK_WARN_RETRIES (2)
#define SICK_MAX_RETRIES_UNEXPECTED_PACKETS 100

#include "sick.h"

static int sick_computeChecksum(unsigned char *buf,unsigned int buflen);
static int sick_testBaud(sick_t *s, int baud);
static void sick_processUnexpectedTelegram(unsigned char *buf);

int sick_processScanData(sick_t *s, unsigned char *buf, int *data, int *nsamples);

int sick_reconfigure(sick_t *s);

int sick_configWrite(sick_t *s, unsigned char *newparams);
int sick_configRead(sick_t *s);

#define LOGCHANNEL "SICK"

sick_t *sick_create()
{
  sick_t *s;

  s=(sick_t*) calloc(sizeof(sick_t),1);

  return s;
}

void sick_destroy(sick_t *s)
{
  close(s->serialfd);

  free(s);
}

int sick_connect(sick_t *s, char *port)
{
  char buf[SICK_MAX_MSG_LENGTH];

  s->serialfd=serial_open(port);
  if (s->serialfd==-1)
    {
      return 0;
    }

  if (!sick_autonegotiateBaud(s))
    {
      log(LOG_ERROR,LOGCHANNEL,"Autonegotiate baud failed");
       return 0;
    }

  if (!sick_configRead(s))
    {
      log(LOG_ERROR,LOGCHANNEL,"Couldn't read current scanner configuration.");
      return 0;
    }

  if (!sick_reconfigure(s))
    {
      log(LOG_ERROR,LOGCHANNEL,"Couldn't reconfigure scanner.");
      return 0;
    }

  if (!sick_continuousEnable(s,0))
    {
      log(LOG_ERROR,LOGCHANNEL,"Couldn't disable continuous mode");
      return 0;
    }

  if (!sick_requestType(s, buf, SICK_MAX_MSG_LENGTH-1))
    {
      log(LOG_ERROR,LOGCHANNEL,"Couldn't determine scanner type");
      return 0;
    }

  if (!sick_switchVariant(s, 180, 100))
    {
      log(LOG_ERROR,LOGCHANNEL,"Unable to set variant to 180/0.5");
      return 0;
    }

  log(LOG_VVERBOSE,LOGCHANNEL,"Connected to %s", buf);
  return 1;
}

int sick_writeTelegram(sick_t *s, unsigned char cmd, const void *buf, 
		      unsigned int buflen)
{
  unsigned char outbuf[SICK_MAX_MSG_LENGTH];
  unsigned char inbuf[SICK_MAX_MSG_LENGTH];
  int checksum;
  int tries;

  if (buflen>=(SICK_MAX_MSG_LENGTH-7))
    {
      log(LOG_ERROR,LOGCHANNEL,"Ludicrous buffer length %i",buflen);
      return 0;
    }

  outbuf[0]=0x02;
  outbuf[1]=cmd;
  outbuf[2]=buflen & 0xff;
  outbuf[3]=buflen>>8;

  if (buflen>0)
    memcpy(&outbuf[4], buf, buflen);

  checksum=sick_computeChecksum(outbuf, buflen + 4);

  outbuf[buflen+4]=checksum & 0x00ff;
  outbuf[buflen+5]=checksum >> 8;
  
  if (writeall(s->serialfd, (char*) outbuf, 6+buflen)!=((int) (6+buflen)))
    {
      log(LOG_ERROR,LOGCHANNEL,"serial_write didn't write whole length");
      return 0;
    }

  // get the ack and make sure it's 0x06

  tries=0;

 waitACK:
  inbuf[0]=0x00;

  if (!sick_readTelegram(s, inbuf, SICK_RX_TIMEOUT_MS))
    {
      log(LOG_DEBUG,LOGCHANNEL,"write ACK timeout");
      return 0;
    }

  if (inbuf[0]==0x15)
    {
      log(LOG_DEBUG,LOGCHANNEL,"NAK");
      return 0;
    }

  if (inbuf[0]==0x06)
    return 1;

  if (inbuf[0]==0x02) // it was an uneected packet
    sick_processUnexpectedTelegram(inbuf);

  // try it again.
  tries++;
  if (tries<SICK_MAX_RETRIES_UNEXPECTED_PACKETS)
    goto waitACK;

  log(LOG_DEBUG,LOGCHANNEL,"Write not acknowledged");
  return 0;
}

// copied from Sick documentation with a couple typographical
// cleanups. merged carmen's suggestions for endianness

static int sick_computeChecksum(unsigned char *buf, unsigned int buflen)
{
  unsigned char crc[2];
  unsigned char abData[2];

  crc[0]=0; // lsb
  crc[1]=0; // msb

  abData[0]=0; // lsb
  abData[1]=0; // msb
  
  while (buflen--)
    {
      abData[1]=abData[0];
      abData[0]=*buf++;

      if (crc[1] & 0x80)
	{
	  crc[1]<<=1;
	  if (crc[0]&0x80)
	    crc[1]|=0x1;
	  crc[0]<<=1;

	  crc[1] ^= 0x80;
	  crc[0] ^= 0x05;
	}
      else
	{
	  crc[1]<<=1;
	  if (crc[0]&0x80)
	    crc[1]|=0x1;
	  crc[0]<<=1;
	}

      crc[0] ^= abData[0];
      crc[1] ^= abData[1];
    }

  return (crc[1]<<8)+crc[0];
}

int sick_setBaud(sick_t *s, int baud)
{
  unsigned char buf[SICK_MAX_MSG_LENGTH];

  buf[0]=0x20;
  switch (baud)
    {
    case 38400:
      buf[1]=0x40;
      break;
    case 19200:
      buf[1]=0x41;
      break;
    case 9600:
      buf[1]=0x42;
      break;
    case 500000:
      buf[1]=0x48;
      break;
    default: // unsupported baudrate
      return 0;
    }

  if (!sick_doTransactionEx(s, 0x00, buf, 2,
			    0xa0, buf, SICK_CHANGE_OPMODE_TIMEOUT_MS))
    return 0;

		     
  if (buf[5]!=0x00) // success?
    return 0;

  // hurrah!
  serial_setbaud(s->serialfd, baud);

  log(LOG_VERBOSE,LOGCHANNEL,"Baud rate changed to %i",baud);
  return 1;
}

// attempt to read a packet or telegram from the scanner.
// ACKS and NAKS are considered 1 byte packets, both of which return success.
// returns length read.
int sick_readTelegram(sick_t *s, unsigned char *buf, int mstimeout)
{
  int length;
  int checksum;

  // wait for something that looks like a packet.
 wait:
  buf[0]=0x00;

  if (!readfullytimeout(s->serialfd, buf, 1, mstimeout))
    return 0;

  if (buf[0]==0x02) // STX: it's the beginning of a whole telegram.
    goto telegram;
  if (buf[0]==0x06) // ACK
    return 1;
  if (buf[0]==0x15) // NAK
    return 1;

  // it's some garbage byte.
  printf("*");
  //  printf("*%02X",buf[0]);
  //  fflush(NULL);
  //      printf("discarding a byte %02x\n", ((unsigned int) buf[0])&0x00ff);
  goto wait;

 telegram:

  // read the rest of the header
  if (!readfullytimeout(s->serialfd, &buf[1], 3, SICK_RX_TIMEOUT_MS))
    {
      log(LOG_VVERBOSE,LOGCHANNEL,"Timeout waiting for rest of header");
      return 0;
    }

  length=buf[2]+(buf[3]*256);
  //  printf("H:%i\n",length);
  
  if (length>(SICK_MAX_MSG_LENGTH-7))
    {
      log(LOG_VVERBOSE,LOGCHANNEL,"Bogus length field, length=%i",length);
      return 0;
    }

  if (!readfullytimeout(s->serialfd, &buf[4], length+2, SICK_RX_TIMEOUT_MS))
    {
      log(LOG_VVERBOSE,LOGCHANNEL,"Timeout waiting for message body");
      return 0;
    }
  
  checksum=buf[4+length]+(buf[5+length]*256);
  if (checksum!=sick_computeChecksum(buf,4+length))
    {
      log(LOG_VVERBOSE,LOGCHANNEL,"checksum mismatch (length=%i, %04X!=%04X)",
	  length, checksum, sick_computeChecksum(buf,4+length));
      return 0;
    }
  
  return 1;
}

void sick_displayTelegram(unsigned char *b)
{
  int i=0;
  int length=0;

  if (b[0]!=0x02)
    {
      printf("invalid telegram\n");
      return;
    }

  if (b[4]==0x92)
    {
      printf("NACK/Incorrect command\n");
    }

  printf("header : ");

  for (i=0;i<4;i++)
    {
      printf("%02X ", b[i]);
    }

  length=b[2]+(b[3]*256);
  printf(" (address = %i, length = %i)\n", b[1], length);

  if (length>SICK_MAX_MSG_LENGTH-7)
    {
      printf(" (length is too long, not showing data)\n");
      return;
    }

  for (i=0;i<length;i++)
    {
      if (i%16==0)
	printf("%04X : ", i);

      printf("%02X ", b[4+i]);

      if (i%16==15)
	printf("\n");
    }
 
  printf("\n");
}

int sick_autonegotiateBaud(sick_t *s)
{
  int bauds[]={500000, 9600,  38400, 500000, 9600, 38400, -1};
  int idx=0;

  int oldlevel=logGetLevel();
  logLevel(oldlevel<LOG_OUTPUT ? oldlevel : LOG_OUTPUT);

  log(LOG_OUTPUT, LOGCHANNEL, "Autonegotiating baudrate");

  while (bauds[idx]>0)
    {
      log(LOG_OUTPUT,LOGCHANNEL,"Trying %i baud", bauds[idx]);

      if (sick_testBaud(s, bauds[idx]))
	{
	  log(LOG_OUTPUT,LOGCHANNEL,"Found SICK at %i baud", bauds[idx]);
	  logLevel(oldlevel);
	  return bauds[idx];
	}
      else
	log(LOG_OUTPUT,LOGCHANNEL,"No luck at %i baud", bauds[idx]);

      idx++;
    }

  logLevel(oldlevel);

  return 0;
}

int sick_testBaud(sick_t *s, int baud)
{
  int trials=0;

  serial_setbaud(s->serialfd, baud);

  sick_continuousEnable(s, 0);
  usleep(200000);
  readflush(s->serialfd);
  sick_continuousEnable(s, 0);
  
  for (trials=0;trials<1;trials++)
    {
      if (sick_requestStatus(s))
      //      if (sick_requestScan(s, buf))
	return 1;

      usleep(200000);
    }

  return 0;
}

int sick_scanDataSize(sick_t *s)
{
  int i=s->fieldofview*100/s->resolution;
  
  i++;
  printf("scan size: %i\n",i);
  return i;
}

int sick_requestScan(sick_t *s, int *data, int *nsamples)
{
  unsigned char buf[SICK_MAX_MSG_LENGTH];

  buf[0]=0x30; // request for measured values
  buf[1]=0x01; // measured value mode, non-interlaced

  if (!sick_doTransaction(s, 0x00, buf, 2, 0xb0, buf))
    return 0;

  return sick_processScanData(s, buf, data, nsamples);
}

void sick_processUnexpectedTelegram(unsigned char *buf)
{
  //  printf("unexpected telegram:\n");
  //  sick_displayTelegram(buf);
}

int sick_requestStatus(sick_t *s)
{
  unsigned char buf[SICK_MAX_MSG_LENGTH];

  buf[0]=0x31; 
  
  if (!sick_doTransaction(s, 0x00, buf, 1, 0xb1, buf))
    return 0;

  return 1;
}

/* inbuf MUST be at least SICK_MAX_MSG_LENGTH. caller CAN
   reuse same buffer for inbuf and outbuf */

int sick_doTransaction(sick_t *s, unsigned char addr, 
		       const unsigned char *outbuf, int outbuflen,
		       unsigned char incmd, unsigned char *inbuf)
{
  return sick_doTransactionEx(s, addr, outbuf, outbuflen,
			      incmd, inbuf, SICK_RX_TIMEOUT_MS);
}

int sick_doTransactionEx(sick_t *s, unsigned char addr, 
		       const unsigned char *outbuf, int outbuflen,
		       unsigned char incmd, unsigned char *inbuf,
		       int mstimeout)
{
  int tries=0;

 writeagain:
  if (!sick_writeTelegram(s, addr, outbuf, outbuflen))
    {
      log(tries>SICK_WARN_RETRIES ? LOG_VERBOSE : LOG_VVERBOSE,LOGCHANNEL,
	  "write fail (%i)",tries);

      if (tries<SICK_MAX_RETRIES)
	{
	  tries++;
	  goto writeagain;
	}

      return 0;
    }

 readagain:
  if (!sick_readTelegram(s, inbuf, mstimeout))
    {
      log(LOG_VVERBOSE,LOGCHANNEL,"read timeout (tries=%i)",tries);
      return 0; // timeout
    }

  // was this our reply packet, or something else?
  if (inbuf[0]!=0x02 || inbuf[4]!=incmd)
    {
      sick_processUnexpectedTelegram(inbuf);
      tries++;
      if (tries<SICK_MAX_RETRIES)
	goto readagain;
      return 0;
    }

  // success.
  return 1; 
}

int sick_configRead(sick_t *s)
{
  unsigned char buf[SICK_MAX_MSG_LENGTH];

  buf[0] =0x74; // configuration read command
  if (!sick_doTransaction(s, 0x00, buf, 1, 0xf4, buf))
    {
      return 0;
    }

  // we read the configuration, yay.
  // make a copy.
  memcpy(s->params, &buf[5], SICK_PARAMS_LENGTH);

  int ranges[]={8, 8, 8, 16, 16, 32, 32};

  if (s->params[6]==0x01)
    log(LOG_VVERBOSE,LOGCHANNEL,"1 unit = 1 mm, range=%im",ranges[s->params[5]]);
  else if (s->params[6]==0x00)
    log(LOG_VVERBOSE,LOGCHANNEL,"1 unit = 1 cm, range=%im",ranges[s->params[5]]*10);
  else
    log(LOG_ERROR,LOGCHANNEL,"Unknown resolution setting!");

  
  return 1;
}

int sick_configWrite(sick_t *s, unsigned char *newparams)
{
  unsigned char buf[SICK_MAX_MSG_LENGTH];

  // don't write if the params are the same.
  if (!memcmp(s->params, newparams, SICK_PARAMS_LENGTH))
    return 1;

  log(LOG_OUTPUT,LOGCHANNEL,"writing new config parameters to EPROM");

  // switch to config mode
  buf[0]=0x20;
  buf[1]=0x0;
  memcpy(&buf[2], "SICK_LMS",8);

  if (!sick_doTransactionEx(s, 0x00, buf, 10, 0xa0, buf, 
			    SICK_CHANGE_OPMODE_TIMEOUT_MS))
    {
      return 0;
    }

  if (buf[5]!=0x0)
    return 0;
  
  buf[0] =0x77; // reconfigure command
  memcpy(&buf[1], newparams, SICK_PARAMS_LENGTH);

  if (!sick_doTransactionEx(s, 0x00, buf, SICK_PARAMS_LENGTH+1, 0xf7, buf, 
			    SICK_CHANGE_OPMODE_TIMEOUT_MS))
    {
      return 0;
    }

  // update failed?
  if (buf[5]!=1)
    return 0;

  // update was good! remember these settings
  memcpy(s->params, newparams, SICK_PARAMS_LENGTH);

  log(LOG_OUTPUT,LOGCHANNEL,"EPROM write success");

  // switch to monitoring mode
  buf[0]=0x20;
  buf[1]=0x25;

  if (!sick_doTransactionEx(s, 0x00, buf, 2, 0xa0, buf,
			    SICK_CHANGE_OPMODE_TIMEOUT_MS))
    {
      return 0;
    }

  if (buf[5]!=0x0)
    return 0;

  return 1;

}

int sick_continuousEnable(sick_t *s, int enable)
{
  unsigned char buf[SICK_PARAMS_LENGTH];

  buf[0]=0x20;
  buf[1]= enable ? 0x24 : 0x25; // both are uninterlaced

  if (!sick_doTransactionEx(s, 0x00, buf, 2, 0xa0, buf,
			    SICK_CHANGE_OPMODE_TIMEOUT_MS))
    {
      return 0;
    }

  if (buf[5]!=0x0)
    return 0;

  return 1;
}

// angle is e.g. 100 or 180
// resolution is 100ths of a degree, must be 25,50, or 100.
// if you use 25, the data will be interleaved.
int sick_switchVariant(sick_t *s, int angle, int resolution)
{
  unsigned char buf[SICK_MAX_MSG_LENGTH];

  buf[0]=0x3b;
  buf[1]=angle&0xff;
  buf[2]=angle/256;
  buf[3]=resolution&0xff;
  buf[4]=resolution/256;

  if (!sick_doTransaction(s, 0x00, buf, 5, 0xbb, buf))
    {
      return 0;
    }

  if (buf[5]==0x01)
    {
      s->fieldofview=angle;
      s->resolution=resolution;
      return 1;
    }

  return 0;
}

int sick_requestType(sick_t *s, char *type, int maxlen)
{
  unsigned char buf[SICK_MAX_MSG_LENGTH];

  buf[0]=0x3a;
  if (!sick_doTransaction(s, 0x00, buf, 1, 0xba, buf))
    {
      return 0;
    }

  int len=buf[2]+(buf[3]*256);
  buf[5+len-2]=0;
  strncpy(type, (char*) &buf[5], maxlen);

  return 1;
}

int sick_continuousData(sick_t *s, int *data, int *nsamples)
{
  unsigned char buf[SICK_MAX_MSG_LENGTH];
  
  if (!sick_readTelegram(s, buf, SICK_RX_TIMEOUT_MS))
    {
      printf("read telegram returned 0\n");
      return 0;
    }

  return sick_processScanData(s, buf, data, nsamples);
}

int sick_processScanData(sick_t *s, unsigned char *buf, int *data, int *nsamples)
{
  int statusword;
  int partialscan;
  int partialscannumber;
  int units;
  int numsamples;

  int i, d;

  // it must be a real packet, not an ACK/NAK
  if (buf[0]!=0x02)
    return 0;

  statusword = buf[5] + (buf[6] << 8);
  numsamples = statusword & 0x01ff;
  partialscannumber = (statusword >> 11 ) & 0x0003;
  partialscan = statusword & 0x2000;
  units = statusword >> 14;

  *nsamples=numsamples;

  // sanity check the # of samples
  int correctnumsamples=s->fieldofview*100/s->resolution+1;
  if (numsamples!=correctnumsamples)
    {
      log(LOG_WARN,LOGCHANNEL,"numsamples=%i, not %i",numsamples,correctnumsamples);
      //      return 0;
    }

  for (i=0;i<numsamples;i++)
    {
      d=buf[7+i*2]+(buf[8+i*2]<<8);
      d&=0x7fff;

      data[i]=d;
    }

  return 1;

}

// set sick to our defaults (maximum range)
int sick_reconfigure(sick_t *s)
{
  unsigned char buf[SICK_PARAMS_LENGTH];

  buf[0] =0x00; // A: maximum diameter of objects to be ignored, e.g. 7 -> 70mm
  buf[1] =0x00;
  buf[2] =0x46; // B: peak threshold
  buf[3] =0x00; // B
  buf[4] =0x00; // C
  buf[5] =0x06; // D: range/scaling. (0x06=32meters) (0x01 current)
  buf[6] =0x01; // E: resolution (0x01=mm, 0x00=cm) (0x00 current)
  buf[7] =0x00; // F:
  buf[8] =0x00; // G:
  buf[9]=0x02; // H: multiple evaluation
  buf[10]=0x02; // I: restart behavior
  buf[11]=0x02; // J: restart time (0x01 current)
  buf[12]=0x00; // K: 2nd multiple evaluation
  buf[13]=0x00; // L: contour A
  buf[14]=0x0a; // M
  buf[15]=0x0a; // N
  buf[16]=0x50; // O
  buf[17]=0x64; // P
  buf[18]=0x00; // Q
  buf[19]=0x0a; // R: contour B
  buf[20]=0x0a; // S
  buf[21]=0x50; // T
  buf[22]=0x64; // U
  buf[23]=0x00; // V
  buf[24]=0x0a; // W
  buf[25]=0x0a; // X
  buf[26]=0x50; // Y
  buf[27]=0x64; // Z
  buf[28]=0x00; // A1
  buf[29]=0x00; // A2
  buf[30]=0x00; // A3
  buf[31]=0x00;
  buf[32]=0x02; // A4
  buf[33]=0x00;

  return sick_configWrite(s, buf);
}