/* test piece for the CRC generation for SQM-160
   written by Takeo Nakano 2012-08-17
*/
#include <stdio.h>
#include <string.h>

char sendbuf[80];
static char *calc_crc (const char *data);

int main(){
  char *sendbufp;
  char dstr[10];

  gets((char *)dstr);
  sendbufp = calc_crc(dstr);

  printf("STRING: %s\n", sendbufp);

  return 0;
}

char *calc_crc(const char *data){
  unsigned short crc;
  size_t ndata;
  int i, j;

  crc = 0x3FFF;

  ndata = strlen(data);

  sendbuf[0] = '!';
  sendbuf[1] = ndata + 34;
  sendbuf[2] = '\0';
  strcat(sendbuf, data);

#ifdef DEBUG
  printf("before %x\n", crc);
#endif

  for(i=1;i<=ndata+1;i++){
    crc ^= sendbuf[i];
      for(j=0;j<8;j++){
	if ((crc & 0x01) == 1){
	  crc >>= 1;
	  crc ^= 0x2001;
	} else {
	  crc >>= 1;
	}
#ifdef DEBUG
	printf("%d %d %x\n", i, j, crc);
#endif
      }
  }

  sendbuf[ndata+2] = ((unsigned char)(crc & 0x7F)) + 34;
  sendbuf[ndata+3] = ((unsigned char)((crc >> 7) & 0x7F)) + 34;
  sendbuf[ndata+4] = '\0';

  return sendbuf;
}