/************************************************************************************
URG RT-Component
Copyright (c) 2010, Segway Japan, Ltd.
All rights reserved.

Contact us if you use this software for sell.
If you use this software not for sell, you can use this software under BSD lisence.
See the files LICENSE.TXT and LICENSE-BSD.TXT for more details.                     
************************************************************************************/

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

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

#include <assert.h>
#include <errno.h>
#include <stdarg.h>
#include <sys/poll.h>


#include <time.h>
#include <sys/time.h>


#include "scip2.h"


#define DEBUGOUT   (0)

#define USEFILE    (1)



#include <coil/Time.h>


speed_t bitrate(int aBR)
{
  switch(aBR)
    {
    case 9600:
      return B9600;

    case 19200:
      return B19200;

    case 38400:
      return B38400;

    case 57600:
      return B57600;

    case 115200:
      return B115200;

    case 230400:
      return B230400;

    case 460800:
      return B460800;

    case 500000:
      return B500000;

    default: // invalid bitrate
      return B0;
    }
}

int setupSerial(int aFd, int aSpeed)
{
#if 1
  struct termios term;
  int fd = aFd;

  /* Retrieve current port settings. */
  if(tcgetattr(fd, &term) < 0) {
    fprintf(stderr, "tcgetattr(3) failed on: %s", strerror(errno));
    close(fd);
    return(-1);
  }

  cfmakeraw(&term);
  cfsetispeed(&term, B19200);
  cfsetospeed(&term, B19200);
  
  /* Write the changes back to the port. */
  if(tcsetattr(fd, TCSAFLUSH, &term) < 0 ) {
    fprintf(stderr, "tcsetattr(3) failed on: %s",  strerror(errno));
    close(fd);
    return(-1);
  }
  tcflush(fd, TCIOFLUSH );
  return 1;

#else
  int speed;
  struct termios tio;

  speed=bitrate(aSpeed);
  if(speed==0)
    return 0;  // invalid bitrate

  tcgetattr(aFd,&tio);

  cfsetispeed(&tio,speed); // bitrate
  cfsetospeed(&tio,speed); // bitrate

  tio.c_cflag=(tio.c_cflag & ~CSIZE) | CS8; // data bits = 8bit

  tio.c_iflag&= ~( BRKINT | ICRNL | ISTRIP );
  tio.c_iflag&= ~ IXON;    // no XON/XOFF
  tio.c_cflag&= ~PARENB;   // no parity
  tio.c_cflag&= ~CRTSCTS;  // no CTS/RTS
  tio.c_cflag&= ~CSTOPB;   // stop bit = 1bit

  // Other
  tio.c_lflag &= ~( ISIG | ICANON | ECHO );

  // Commit
  if(tcsetattr(aFd,TCSADRAIN,&tio)==0)
    return 1;

  return 0;
#endif
}



int fileNO(FILE* fp)
{
#if USEFILE
  return fileno(fp);
#else
  return (int)fp;
#endif
}



FILE* openSerial(char *aDevName, int aSpeed)
{
  FILE* dev;

#if USEFILE
  dev=fopen(aDevName,"w+");
#else
  dev=(FILE*) open(aDevName, O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR);
#endif

  if(dev==0)
    return 0;  // invalid device file

  // setup parameters
  if(setupSerial(fileNO(dev),aSpeed))
    return dev;

  // fail
  perror("setting term parameters");

#if USEFILE
  fclose(dev);
#else
  close((int)dev);
#endif
  return 0;
}





int Scip2_Write(char* buf, int buflen, FILE* fp)
{
#if USEFILE

  int ret = fwrite(buf, sizeof(char), buflen, fp);

#if DEBUGOUT
  char p[128];
  strncpy(p, buf, buflen);
  p[buflen] = 0;
  printf("%d = Scip2_Write %s\n", ret, p);
#endif
  return ret;//fwrite(buf, sizeof(char), buflen, fp);

#else
  int fd = (int)fp;
  int ret = write(fd, buf, buflen);

  return ret;
#endif
}


int Scip2_Gets(char* buf, int buflenmax, FILE* fp)
{
#if USEFILE
  return (int) fgets(buf, buflenmax, fp);
#else

  int fd = (int)fp;
  int readnum = 0;
  int readadd = 0;

  //  printf("\n");

  do {
    if (readadd > 0) {
      readnum += readadd;
    }
    readadd = read(fd, &buf[readnum], 1);

    
    //    printf("%02x ", buf[readnum]);
    
  } while(buf[readnum] != '\n');

  if (readadd > 0) {
    readnum += readadd;
  }
  buf[readnum] = 0;

#if DEBUGOUT
  printf("%d = Scip2_Read %s\n", readnum, buf);
#endif
  
  return readnum;

#endif
}




void Scip2_SCIP2_0(FILE* aPort)
{

  char line[80];

#if DEBUGOUT
  printf("Scip2_SCIP2_0\n");
#endif

  Scip2_Write((char*)"SCIP2.0\n",  8, aPort);

  //echo back
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    perror("Could not change SCIP2.0");
    return;
  }

#if DEBUGOUT
  printf(line);
#endif

  //status
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    perror("Cound not change SCIP2.0");
    return;
  }

#if DEBUGOUT
  printf(line);
#endif

  //last LF
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    perror("Cound not change SCIP2.0");
    return;
  }

#if DEBUGOUT
  printf("SCIP2.0!\n");
#endif

}


//version info
void Scip2_VV(FILE* aPort)
{
  char line[80];

#if DEBUGOUT
  printf("Scip2_VV\n");
#endif
  
  Scip2_Write("VV\n",  3, aPort);
  
  //echo back
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    perror("Could not change SCIP2.0");
    return;
  }

#if DEBUGOUT
  printf(line);
#endif

  //status
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    perror("Cound not change SCIP2.0");
    return;
  }

#if DEBUGOUT
  printf(line);
#endif

  {
    int i;
    for (i = 0; i < 6; i++) {
      if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
        return;
      }
#if DEBUGOUT
      printf(line);
#endif
    }
  }

}


//light on
void Scip2_BM(FILE* aPort)
{
  char line[80];

#if DEBUGOUT
  printf("Scip2_BM\n");
#endif
  
  Scip2_Write("BM\n",  3, aPort);
  
  //echo back
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return;
  }
#if DEBUGOUT
  printf(line);
#endif

  //status
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return;
  }

#if DEBUGOUT
  printf(line);
#endif

  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return;
  }

#if DEBUGOUT
  printf(line);
#endif
}

//light off
void Scip2_QT(FILE* aPort)
{
  char line[80];

#if DEBUGOUT
  printf("Scip2_QT\n");
#endif
  
  Scip2_Write("QT\n",  3, aPort);
  
  //echo back
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return;
  }
#if DEBUGOUT
  printf(line);
#endif

  //status
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return;
  }

#if DEBUGOUT
  printf(line);
#endif

  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return;
  }

#if DEBUGOUT
  printf(line);
#endif
}


//parameter
void Scip2_PP(FILE* aPort)
{
  char line[80];

#if DEBUGOUT
  printf("Scip2_PP\n");
#endif
  
  Scip2_Write("PP\n",  3, aPort);
  
  //echo back
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return;
  }

#if DEBUGOUT
  printf(line);
#endif

  //status
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return;
  }

#if DEBUGOUT
  printf(line);
#endif

  {
    int i;
    for (i = 0; i < 9; i++) {
      if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
        return;
      }
#if DEBUGOUT
      printf(line);
#endif
    }
  }

}

//status info
void Scip2_II(FILE* aPort)
{
  char line[80];

#if DEBUGOUT
  printf("Scip2_II\n");
#endif
  
  Scip2_Write("II\n", 3, aPort);
  
  //echo back
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return;
  }

#if DEBUGOUT
  printf(line);
#endif

  //status
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return;
  }

#if DEBUGOUT
  printf(line);
#endif

  {
    int readlines = 7;
    int i;
    for (i = 0; i < readlines; i++) {
      if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
        return;
      }
      //  printf(line);//test

	  if (strstr(line, "SBPS")) {//here needs test (URG-04LX has this line)
		  readlines++;
	  }

#if DEBUGOUT
      printf(line);
#endif
    }
  }

}


int totalwrote = 0;
int totalcnt = 0;

int Scip2_GD(FILE* aPort, int aStart, int aEnd, int aSkip, int* scan)
{
  char req[32];
  char line[80];
  int* pt = NULL;
  unsigned int i = 0;

  //tcflush(fileno(aPort), TCIOFLUSH);

  sprintf(req,"GD%04d%04d%02d\n", aStart, aEnd, aSkip);

#if DEBUGOUT
  printf("\n");
  printf("req:%s", req);
#endif

  unsigned int wrote = 0;
  while (wrote < strlen(req)) {
    

     wrote += Scip2_Write(req+wrote,  strlen(req)-wrote, aPort);


#if DEBUGOUT
     printf("wrote %d\n", wrote);
#endif
  }

  totalwrote += wrote;
#if DEBUGOUT
  printf("totalcnt %d totalwrote %d\n", totalcnt, totalwrote);
  totalcnt++;
#endif


  // echo-back
  if(Scip2_Gets(line,sizeof(line),aPort)==0) {
      return 0;
  }
#if DEBUGOUT
  printf(line);
#endif

  // status
  if(Scip2_Gets(line,sizeof(line),aPort)==0) {
      return 0;
  }
#if DEBUGOUT
  printf(line);
#endif

  // timestamp
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return 0;
  }
#if DEBUGOUT
  printf(line);
#endif


  unsigned int timestamp =
     (((line[0]-0x30)&0x3f)<<18)
    |(((line[1]-0x30)&0x3f)<<12)
    |(((line[2]-0x30)&0x3f)<<6)
    |  (line[3]-0x30);


  
  


  // result

    char encData[3*(1127+1)] = {0,};
    int lineEnd;


    while(1) {

      if (Scip2_Gets(line,sizeof(line),aPort) == 0) {
	printf("return NULL\n");
      }
#if DEBUGOUT
      printf(line);
#endif
      if(!strcmp("\n",line))
      {
	//end of result
	break;
      }

      lineEnd = strlen(line)-2;
      line[lineEnd] = 0x00;
      strcat(encData, line);
#if DEBUGOUT
      printf(" len %d\n", strlen(encData));
#endif
    }


#if DEBUGOUT
    printf("read end\n");
#endif

    //DECODE
    if (scan == NULL) {
      scan=(int*)malloc(sizeof(int)*((aEnd-aStart)/aSkip+1));
    }
    pt=scan;    

#if DEBUGOUT
    printf("encData len %d\n", strlen(encData));
#endif

    int cnt = 0;
    i = 0;
    while (i < strlen(encData)-1) {
      *pt= ((encData[i] - 0x30)<<12) + ((encData[i+1] - 0x30)<<6) + (encData[i+2] - 0x30);
      i += 3;
      pt++;
      cnt++;
    }


  return timestamp;
}


int Scip2_MD(FILE* aPort, int aStart, int aEnd, int aSkip, int aSkipScan, int aSends, int* scan)
{
  char req[64];
  char line[80];
  int* pt = NULL;
  unsigned int i = 0;

  sprintf(req,"MD%04d%04d%02d%01d%02d\n", aStart, aEnd, aSkip, aSkipScan, aSends);

  Scip2_Write(req,strlen(req),aPort);


  // echo-back
  if(Scip2_Gets(line,sizeof(line),aPort)==0) {
      return 0;
  }
#if DEBUGOUT
  printf(line);
#endif

  // status
  if(Scip2_Gets(line,sizeof(line),aPort)==0) {
      return 0;
  }
#if DEBUGOUT
  printf(line);
#endif

  // timestamp
  if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
    return 0;
  }
#if DEBUGOUT
  printf(line);
#endif


  unsigned int timestamp =
     (((line[0]-0x30)&0x3f)<<18)
    |(((line[1]-0x30)&0x3f)<<12)
    |(((line[2]-0x30)&0x3f)<<6)
    |  (line[3]-0x30);



  //---------------------------
  // BUG? 
  //    MD0000108001000 echo back
  //    99b             status
  //    ***             timestamp
  //---------------------------
  {
    int i = 0;
    for (i = 0; i < 3; i++) {

      if (Scip2_Gets(line, sizeof(line), aPort) == 0) {
	return 0;
      }
#if DEBUGOUT
      printf(line);
#endif
    }
  }



  // result

    char encData[3*(1127+1)] = {0,};
    int lineEnd;

    while(1) {

      if (Scip2_Gets(line,sizeof(line),aPort) == 0) {
	printf("return 0\n");
      }
#if DEBUGOUT
      printf(line);
#endif
      if(!strcmp("\n",line))
      {
	//end of result
	break;
      }

      lineEnd = strlen(line)-2;
      line[lineEnd] = 0x00;
      strcat(encData, line);
#if DEBUGOUT
      printf(" len %d\n", strlen(encData));
#endif
    }



    //DECODE
    if (scan == NULL) {
      scan=(int*)malloc(sizeof(int)*((aEnd-aStart)/aSkip+1));
    }
    pt=scan;    

    int cnt = 0;
    i = 0;
    while (i < strlen(encData)-1) {
      *pt= ((encData[i] - 0x30)<<12) + ((encData[i+1] - 0x30)<<6) + (encData[i+2] - 0x30);
      i += 3;
      pt++;
      cnt++;
    }


  return timestamp;
}



FILE* Scip2_Start(char* device)
{
#if DEBUGOUT
  printf("Scip2_Start\n");
#endif
  FILE* dev = openSerial(device, 19200);

  if(dev == NULL) {
    printf("Could not open serial port\n");
    exit(1);
  }

#if 0
  Scip2_SCIP2_0(dev);
#endif
  Scip2_QT(dev);
  Scip2_VV(dev);
  Scip2_BM(dev);
  Scip2_PP(dev);
  Scip2_II(dev);

  return dev;
}


void Scip2_End(FILE* dev)
{
  if (dev) 
  {
    Scip2_QT(dev);

    fclose(dev);
    dev = NULL;
  }
}




//----------------------------------
//from takeuchi-san   thanks!
//----------------------------------
//#define DEBUG
#ifdef DEBUG
#define DB(x) x
#else
#define DB(x)
#endif

/*同期用パラメータ*/
double time_offset;
double time_factor = 1000;


bool writeloop(FILE* dev, char* buf, int size)
{
#if 0
  int ret = Scip2_Write(buf, size, dev);
  DB(printf("write %d\n", size);)
#else
  DB(printf("writeloop %s\nwrote ", buf);)
  int wrote = 0;
  while (wrote < size) {
    wrote += fwrite(buf+wrote,size-wrote,1,dev);
    DB(printf("%d ", wrote);)
  }
  DB(printf("\n");)
  return (wrote == size)? true : false;
#endif
}

bool readloop(FILE* dev, char* buf, int size)
{
#if 0
  int ret = Scip2_Gets(buf, size, dev);
  DB(pritnf("read %d\n", ret);
#else
  DB(printf("readloop %d\nread ", size);)
  int readed = 0;
  while(readed < size) {
    readed += fread(buf+readed,size-readed,1,dev);
    DB(printf("%d ", readed);)
  }
  DB(buf[size]=0;)
  DB(printf("readloop %s\n", buf);)
  return (readed == size)? true : false;
#endif
}

void Scip2_TimeSync(FILE* dev, int count)
{
  char line[80];
  char buf[20];
  int time,i;
  double pc_time, pc_time_b, urg_time,response_min=10000;
  double best_pc_time=0,best_urg_time=0;
  double offset,factor=1000;
  static double before_pc_time= 0,before_urg_time=0;
   
  //tcflush(ser_port,TCOFLUSH);
  //tcflush(ser_port,TCIFLUSH);
  urg_time = 0;
  /*enter time set mode*/ 
  DB(printf("Enter time set mode\n");)
  
  sprintf(buf,"TM0\n");
#if 0
  writeloop(dev, buf, 4);
  readloop(dev, buf, 6);
#else
  Scip2_Write(buf, 4, dev);
  Scip2_Gets(line, sizeof(line),dev);//echo back
  Scip2_Gets(line, sizeof(line),dev);//status and sum
  Scip2_Gets(line, sizeof(line),dev);//newline
#endif

  for(i = 0;i < count;i++){
    usleep(750);

    /*urg側要求*/
    sprintf(buf,"TM1\n");
    //tcflush(ser_port,TCIFLUSH);

    pc_time_b = coil::gettimeofday();

#if 0
    writeloop(dev,buf,4);
#else
    Scip2_Write(buf, 4, dev);
#endif

    /*PC側読み込み(最小)*/
    do{
      if (!Scip2_Gets(line, sizeof(line),dev)) { //echoback line にしてしまったがいいかな
	continue;
      }
    }while(buf[0] != 'T');
    
    /*PC側読み込み(最大)*/
    pc_time = coil::gettimeofday();

#if 0
    readloop(dev, buf, 12);
#else
    Scip2_Gets(line, sizeof(line),dev);//status
    Scip2_Gets(line, sizeof(line),dev);//timestamp
#endif

    /*タイムスタンプ読み取り*/
    time =
       (((line[0]-0x30)&0x3f)<<18)
      |(((line[1]-0x30)&0x3f)<<12)
      |(((line[2]-0x30)&0x3f)<<6)
      |  (line[3]-0x30);

#if 1
    Scip2_Gets(line, sizeof(line),dev);//newline
#endif

    DB(printf("pre %f urg %f post %f   response %f\n", pc_time_b, (double)time, pc_time, pc_time-pc_time_b);)

    if(pc_time-pc_time_b < response_min){
      response_min =pc_time-pc_time_b;
      best_pc_time = (pc_time_b+pc_time)/2.0;
      best_urg_time = time;
    }

  }

  if(before_urg_time >0){
    factor =
      ((double)best_urg_time - before_urg_time)
      /(best_pc_time - before_pc_time);

#ifdef DEBUG

  printf("\nresponse_min %f\n\n", response_min);

  printf("best_urg_time - before_urg_time = %f\n", best_urg_time - before_urg_time);
  printf("----------------------------------------- = factor %f\n", factor);
  printf("best_pc_time  - before_pc_time  = %f\n", best_pc_time  - before_pc_time);

#endif
  }

  before_pc_time  = best_pc_time;
  before_urg_time = best_urg_time;
  
  urg_time = best_urg_time / factor;
  offset   = best_pc_time - urg_time;

#ifdef DEBUG
  printf("offset = best_pc_time - (best_urg_time / factor)\n");
  printf("%f    = %f - ( %f / %f )\n", offset, best_pc_time, best_urg_time, factor);
#endif

  /*exit time set mode*/
  DB(printf("Exit time set mode\n");)
  
  sprintf(buf,"TM2\n");
#if 0
  writeloop(dev,buf,4);
  readloop(dev, buf, 8);
#else
  Scip2_Write(buf, 4, dev);
  Scip2_Gets(line, sizeof(line),dev);//echo back
  Scip2_Gets(line, sizeof(line),dev);//status and sum
  Scip2_Gets(line, sizeof(line),dev);//newline
#endif

#if 1
  Scip2_BM(dev);
#else

  sprintf(buf,"BM\n");
#if 0
  writeloop(dev, buf, 3);
  readloop(dev, buf, 8);
#else
  Scip2_Write(buf, 3, dev);
  Scip2_Gets(line, sizeof(line),dev);//echo back
  Scip2_Gets(line, sizeof(line),dev);//status and sum
  Scip2_Gets(line, sizeof(line),dev);//newline
#endif
#endif
  urg_time = best_urg_time / time_factor + time_offset;

#ifdef DEBUG
  printf("offset = %f factor = %f adjust=%f [err %f]\n",offset,factor,best_pc_time-urg_time,response_min);

  printf("urg_time = best_urg_time / time_factor + time_offset\n");
  printf("%f = %f / %f + %f\n", urg_time, best_urg_time, time_factor, time_offset);

  //tcflush(ser_port,TCIFLUSH);
#endif
  time_factor = factor;
  time_offset = offset;
}

double Scip2_TimeEstimate(int urg_time)
{
  return (double)urg_time / time_factor + time_offset;
}




