#include "lib_nxwms.h"

unsigned char commandbuf[18];
unsigned char ackbuf[20];
unsigned char databuf[200];

//
// open & close function
//
int open_nxwms(char* device){
    int fd;
    struct termios buf;
    speed_t rate;
    char *devname;

    if(device == NULL){
      devname = DefaultDevice;
    } else devname = device;
    
    if ((fd = open(devname, O_RDWR)) == -1) {
        fprintf(stderr, "init_nxwms: Device open error %s\n", devname);
        exit(-1);
    } else fprintf(stderr, "init_nxwms: Device opened successfully %s\n", devname);
  
    if(tcgetattr(fd, &buf)==-1) exit(-1);

    bzero(&buf, sizeof(buf));
    buf.c_oflag = OPOST;
    buf.c_iflag = 0;//IGNPAR | ICRNL;;
    buf.c_cflag = (CS8 | CREAD);
    buf.c_lflag = 0;// ICANON;

    buf.c_cc[VMIN] = 0;  // set min byte for break
    buf.c_cc[VTIME] = 0; // set break time for read
    // 0 0: no wait

    switch (Baudrate) {
    case 1200: rate = B1200; break;
    case 2400: rate = B2400; break;
    case 4800: rate = B4800; break;
    case 9600: rate = B9600; break;
    case 19200: rate = B19200; break;
    case 38400: rate = B38400; break;
    case 57600: rate = B57600; break;
    case 115200: rate = B115200; break;
    case 230400: rate = B230400; break;
    case 460800: rate = B460800; break;
    case 1000000: rate = B1000000; break;
    default: rate = Baudrate;
    }

    cfsetispeed(&buf, rate);
    cfsetospeed(&buf, rate);
    if(tcsetattr(fd, TCSANOW, &buf)==-1) exit(-1);

    fprintf(stderr,"Open Succeeded at %d[bps], fd is %d.\n",Baudrate, fd);
    
    return fd;
}

void close_nxwms(int fd){
    close(fd);
}
//
// basic functions for writing & reading via serial communication
//
unsigned char make_bcc(unsigned char* bccbuf, unsigned char length){
    unsigned char bcc=0;
    unsigned char loop_i=0;

    for(loop_i=0;loop_i<length;loop_i++){
        bcc^=bccbuf[loop_i];
    }

    return bcc;
}

int make_command_packet(unsigned char magick, unsigned char prdct_id, unsigned char* args){
    int wlen;
    unsigned char* bccstartp=&(commandbuf[5]);

    commandbuf[0] = 0x55; //Header
    commandbuf[1] = 0x55; //Header
        
    switch(magick){
    case IDSetting:
        commandbuf[2] = 0x06;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //ID
        commandbuf[5] = 0x01;
        commandbuf[6] = args[1]; //new ID
        commandbuf[7] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[8] = 0xaa; //Footer
        wlen = 9;
        break;
    case StartMeasure:
        commandbuf[2] = 0x0b;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //detID
        commandbuf[5] = 0x02;
        commandbuf[6] = args[1]; //measuretime YY
        commandbuf[7] = args[2]; //measuretime MM
        commandbuf[8] = args[3]; //measuretime DD
        commandbuf[9] = args[4]; //measuretime hh
        commandbuf[10] = args[5]; //measuretime mm
        commandbuf[11] = args[6]; //00:memory, 01:monitor
        commandbuf[12] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[13] = 0xaa;
        wlen = 14;
        break;
    case StopMeasure:
        commandbuf[2] = 0x05;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //detID
        commandbuf[5] = 0x04;
        commandbuf[6] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[7] = 0xaa; //Footer
        wlen = 8;
        break;
    case StatusRead:
        commandbuf[2] = 0x05;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //detID
        commandbuf[5] = 0x05;
        commandbuf[6] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[7] = 0xaa; //Footer
        wlen = 8;        
        break;
    case FileInfoRead:
        commandbuf[2] = 0x05;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //detID
        commandbuf[5] = 0x07;
        commandbuf[6] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[7] = 0xaa; //Footer
        wlen = 8;
        break;
    case FileDataRead:
        commandbuf[2] = 0x0e;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //detID 
        commandbuf[5] = 0x09;
        commandbuf[6] = args[1]; //File no
        commandbuf[7] = args[2]; //starting record no0
        commandbuf[8] = args[3]; //starting record no1
        commandbuf[9] = args[4]; //starting record no2
        commandbuf[10] = args[5]; //starting record no3
        commandbuf[11] = args[6]; //record no0
        commandbuf[12] = args[7]; //record no1
        commandbuf[13] = args[8]; //record no2
        commandbuf[14] = args[9]; //record no3
        commandbuf[15] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[16] = 0xaa;
        wlen = 17;
        break;
    case MeaureFreq:
        commandbuf[2] = 0x07;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //detID
        commandbuf[5] = 0x0b;
        commandbuf[6] = args[1]; //freq0
        commandbuf[7] = args[2]; //freq1
        commandbuf[8] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[9] = 0xaa; //Footer
        wlen = 10;
        break;
    case MeasureTime:
        commandbuf[2] = 0x08;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //detID
        commandbuf[5] = 0x0c;
        commandbuf[6] = args[1]; //Hour
        commandbuf[7] = args[2]; //Min
        commandbuf[8] = args[3]; //Sec
        commandbuf[9] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[10] = 0xaa; //Footer
        wlen = 11;
        break;
    case MagGainSetting:
        commandbuf[2] = 0x06;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //detID
        commandbuf[5] = 0x0d;
        commandbuf[6] = args[1]; //gain
        commandbuf[7] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[8] = 0xaa; //Footer
        wlen = 9;
        break;
    case WirelessChannel:
        commandbuf[2] = 0x06;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //detID
        commandbuf[5] = 0x0e;
        commandbuf[6] = args[1]; //channel
        commandbuf[7] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[8] = 0xaa; //Footer
        wlen = 9;
        break;
    case MemoryClear:
        commandbuf[2] = 0x05;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //detID
        commandbuf[5] = 0x12;
        commandbuf[6] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[7] = 0xaa; //Footer
        wlen = 8;        
        break;
    case Initialize:
        commandbuf[2] = 0x05;
        commandbuf[3] = prdct_id;
        commandbuf[4] = args[0]; //detID
        commandbuf[5] = 0x11;
        commandbuf[6] = make_bcc(bccstartp, commandbuf[2]-4); //BCC
        commandbuf[7] = 0xaa; //Footer
        wlen = 8;
        break;
    default:
        wlen=0;
        break;
    }

    return wlen;
}


void write_command(int fd, unsigned char magick, unsigned char prdct_id, unsigned char* args){
    int wlen;
    int i, res;
    
    wlen = make_command_packet(magick, prdct_id, args);

    res = write(fd, commandbuf, wlen);

    fprintf(stderr, "command: ");
    for(i=0;i<wlen;i++)fprintf(stderr,"0x%x ",commandbuf[i]);
    fprintf(stderr,"\n");
    //fprintf(stderr, "written length is actually %d bytes\n", res);
}


int read_packet(int fd, int length, unsigned char* buf, int wirelessflag){
  static unsigned char emb[256];
  static int emb_n = 0;
  int mfirst = 1;

  unsigned char* p;
  int res,rlen;
  int errcnt=0;
  int i,j;

  rlen = 0;
  while(1){
    if(mfirst==1){ /* 前回の0xaaの後の情報 */
      p=buf;
      for(i=0; i<emb_n; i++) p[i] = emb[i];
      res = emb_n;
      emb_n = 0;
      mfirst--;
    }
    else res=read(fd, p, length);

    if(res > 0){
      for(i=0; i<res; i++, p++){
	if (rlen == 0){
	  if ( p[0] == 0x55 ){
	    for(j=0; j<res-i; j++) buf[j] = p[j];
	    rlen = 1;
	    p = buf;
	  }}
	else if ( rlen == 1 ){
	  if( buf[1] == 0x55 ) rlen++;
	  else rlen = 0;
	}
	else {
	  rlen++;
	  if(rlen == length){
	    for(j=0; j+1<res-i; j++) emb[j] = p[j+1];
	    emb_n = res - i - 1;
	    /* if( emb_n > 0) { /\* debug *\/ */
	    /*   int k=0; */
	    /*   fprintf(stderr,"emb packet: "); */
	    /*   for(k=0;k<emb_n;k++) fprintf(stderr,"0x%x ",emb[k]); */
	    /*   fprintf(stderr,"\n"); */
	    /* } */
	    if(p[0] == 0xaa) return rlen;
	    else{
	      errcnt++;
	      rlen = 0;
	      if(emb_n > 0) mfirst = 1;
	      else p = buf;
	    }}}}}
    else {
      errcnt++;
      if(wirelessflag==1) usleep(10 * 1000);
      else usleep(6 * 1000);
    }

    if(errcnt > 10) {
      fprintf(stderr, "Error! can't read packet\n");
      return 0;
    }}

  return 0;
}

int read_ackpacket(int fd, int length, int wirelessflag){
    int res = 0;
    res = read_packet(fd, length, ackbuf, wirelessflag);
    return res;
}

unsigned char check_ack_packet(int fd, unsigned char magick, int wirelessflag){
    unsigned char ackcode;
    int acklen=9;
    int res;
    
    switch(magick){
    case IDSetting:
        ackcode = 0x81;
        break;
    case StartMeasure:
        ackcode = 0x82;
        break;
    case StopMeasure:
        ackcode = 0x84;
        break;
    case StatusRead:
        ackcode = 0x85;        
        break;
    case FileInfoRead:
        ackcode = 0x87;
        break;
    case FileDataRead:
        ackcode = 0x89;
        break;
    case MeaureFreq:
        ackcode = 0x8b;
        break;
    case MeasureTime:
        ackcode = 0x8c;
        break;
    case MagGainSetting:
        ackcode = 0x8d;
        break;
    case WirelessChannel:
        ackcode = 0x85;
        break;
    case MemoryClear:
        ackcode = 0x92;
        break;
    case Initialize:
        ackcode = 0x91;
        break;
    default:
        return 0x00;
    }


    int i;
    for(i=0; i<20; i++){
      res = read_ackpacket(fd, acklen, wirelessflag);
      if( res > 0 ){
	if( ackbuf[5]==ackcode ){
	  fprintf(stderr, "Ack Succeeded.\n");
	  return ackbuf[6];
	}}
      else
	return 0x00;
    }
    return 0x00;
}


int read_datapacket(int fd, int length, int wirelessflag){
  int res = 0;
  int i;
  for(i=0; i<10; i++){
    res=read_packet(fd, length, databuf, wirelessflag);
    if(databuf[length-1] == 0xaa || res==0)
      return res;
  }
  return 0;
}


double conv_mag(double v)
{
    return (v/1300.0);

}

double conv_acc(double v, unsigned char prdct_id)
{
  switch(prdct_id){
  case 0x01: //通常仕様
    return ((3.3*v/4095)-1.75)/0.0266;
  case 0x03: //低G仕様
    return ((3.3*v/4095)-1.65)/0.19;
  case 0x0e: //超小型5G/300dps
    return ((3.3*v/65535)-1.65)/0.19;
  default:
    printf("Acc Convert Error. ProductID is not supported.\n");
    break;
  }
  return 0.0;
}

double conv_gyro(double v, unsigned char prdct_id)
{
  switch(prdct_id){
  case 0x01: //通常仕様
    return ((3.3*v/4095)-1.35)/5.6/0.000136;
  case 0x03: //低G仕様
    return ((3.3*v/4095)-1.35)/5.6/0.00067;
  case 0x0e: //超小型5G/300dps
    return -(((3.3*v/65535)-1.5)/0.003624);
  default:
    printf("Gyro Convert Error. ProductID is not supported.\n");
    break;
  }
  return 0.0;
}

int get_sensordata(int fd, unsigned char prdct_id, int mode, double* accs, double* gyros, double* mags, int wirelessflag){
  int i,j;
  int basepos, apos, gpos, mpos;

  int acsgn[3], aaxis[3];// 加速度のSGN
  switch(prdct_id){
  case 0x03:
    acsgn[0] = 1; acsgn[1] = -1; acsgn[2] = 1;
    aaxis[0] = 1; aaxis[1] = 0; aaxis[2] = 2;
    break;
  default:
    for(i=0; i<3; i++){
      acsgn[i] = -1;
      aaxis[i] = i;
    }
    break;
  }

  int res = read_datapacket(fd, 101, wirelessflag);
  /* fprintf(stderr,"ack: "); */
  /* for(i=0;i<res;i++)fprintf(stderr,"0x%x ",databuf[i]); */
  /* fprintf(stderr,"\n"); */

  if(res != 101 ) return 0;

  if((databuf[5]==0x83) && (databuf[6]==0x01)) fprintf(stderr, "Warning! Battery is low!\n");

  // get all data
  double tmpaccs[15], tmpgyros[15], tmpmags[15];
  if(databuf[5]==0x83){
    basepos = 9;
    for(j=0; j<5; j++){
      //mag
      mpos = basepos+18*j;
      for(i=0;i<3;i++) tmpmags[j*3+i] = conv_mag(databuf[mpos+i*2]*256+databuf[mpos+i*2+1]);
      //acc
      apos = mpos+6;
      for(i=0;i<3;i++) tmpaccs[j*3+i] = conv_acc(databuf[apos+aaxis[i]*2]*256+databuf[apos+aaxis[i]*2+1], prdct_id) * acsgn[i];

      //gyro
      gpos = apos+6;
      for(i=0;i<3;i++) tmpgyros[j*3+i] = conv_gyro(databuf[gpos+i*2]*256+databuf[gpos+i*2+1], prdct_id) * -1;
    }
  } else {
    fprintf(stderr, "Couldn't read the data\n");
    return 0;
  }

  //
  switch(mode){
  case 0: // latest
    for(i=0; i<3; i++){
      accs[i]  = tmpaccs[i];
      gyros[i] = tmpgyros[i];
      mags[i]  = tmpmags[i];
    }
    return 1;

  case 1: // average
    for(i=0; i<3; i++){
      accs[i]  = 0.0;
      gyros[i] = 0.0;
      mags[i]  = 0.0;
    }
    for(j=0; j<5; j++){
      for(i=0; i<3; i++){
	accs[i]  += tmpaccs[j*3+i];
	gyros[i] += tmpgyros[j*3+i];
	mags[i]  += tmpmags[j*3+i];
      }}
    for(i=0; i<3; i++){
      accs[i] /= 5.0;
      gyros[i] /= 5.0;
      mags[i] /= 5.0;
    }
    return 1;

  case 2: // all
    for(i=0; i<15; i++){
      accs[i]  = tmpaccs[i];
      gyros[i] = tmpgyros[i];
      mags[i]  = tmpmags[i];
    }
    return 1;

  default:
    printf("Not Supported Mode %d\n", mode);
    break;
  }
  return 0;
}

//
// utility functions
//
void set_id(int fd, unsigned char prdct_id, unsigned char id, int wirelessflag){
    unsigned char res;
    write_command(fd, IDSetting, prdct_id, &id);
    res = check_ack_packet(fd, IDSetting, wirelessflag);
    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
    }
}

void start_measure(int fd, unsigned char prdct_id, unsigned char id, unsigned char mode, int wirelessflag){
    time_t timer;
    struct tm *date;
    unsigned char argbuf[8];

    date = localtime(&timer);
    
    argbuf[0] = id;

    argbuf[1] = (unsigned char)(date->tm_year%100);
    argbuf[2] = (unsigned char)(date->tm_mon+1);
    argbuf[3] = (unsigned char)(date->tm_mday);
    argbuf[4] = (unsigned char)(date->tm_hour);
    argbuf[5] = (unsigned char)(date->tm_min);

    argbuf[6] = mode;

    write_command(fd, StartMeasure, prdct_id, argbuf);
    printf("waiting start");
    usleep(10 * 1000);
    double accs[15], gyros[15], mags[15];
    while(get_sensordata(fd, prdct_id, 2, accs, gyros, mags, wirelessflag) == 0){
      write_command(fd, StartMeasure, prdct_id, argbuf);
      usleep(10 * 1000);
      printf(".");
    }
    printf("done\n");
    printf("start measure\n");
}

void start_measure_withtime(int fd, unsigned char prdct_id, unsigned char id, unsigned char mode, int year, int month, int day, int hour, int min){
    unsigned char argbuf[8];
    
    argbuf[0] = id;

    argbuf[1] = (unsigned char)(year%100);
    argbuf[2] = (unsigned char)month;
    argbuf[3] = (unsigned char)day;
    argbuf[4] = (unsigned char)hour;
    argbuf[5] = (unsigned char)min;

    argbuf[6] = mode;
    
    write_command(fd, StartMeasure, prdct_id, &id);
}

int stop_measure(int fd, unsigned char prdct_id, unsigned char id, int wirelessflag){
    unsigned char res;
        
    write_command(fd, StopMeasure, prdct_id, &id);
    res = check_ack_packet(fd, StopMeasure, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
	return 0;
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
	return 0;
    }
    return 1;
}



int check_is_data_running(int fd, unsigned char prdct_id, unsigned char id, int wirelessflag){
  usleep(1000);
  int flag = 0;
  double accs[15], gyros[15], mags[15];
  if(get_sensordata(fd, prdct_id, 2, accs, gyros, mags, wirelessflag) !=0) flag =1;

  return flag;
}

int stop_measure_with_check(int fd, unsigned char prdct_id, unsigned char id, int wirelessflag){
  if( check_is_data_running(fd, prdct_id, id, wirelessflag) == 1 )
    return (stop_measure(fd, prdct_id, id, wirelessflag));
  return 1;
}


void get_status(int fd, unsigned char prdct_id, unsigned char id, unsigned char* status_buf, int wirelessflag){
    unsigned char res;
    int i;
        
    write_command(fd, StatusRead, prdct_id, &id);
    res = check_ack_packet(fd, StatusRead, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
    } else {
        read_datapacket(fd, 18, wirelessflag);
        if(databuf[5] == 0x86)
            for(i=0;i<9;i++)status_buf[i]=databuf[i+6];
        else fprintf(stderr,"Data read error\n");
    }
}

void get_fileinfo(int fd, unsigned char prdct_id, unsigned char id, unsigned char* fileinfo_buf, int wirelessflag){
    unsigned char res;
    int i;
        
    write_command(fd, FileInfoRead, prdct_id, &id);
    res = check_ack_packet(fd, FileInfoRead, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
    } else {
        read_datapacket(fd, 91, wirelessflag);
        if(databuf[5] == 0x88)
            for(i=0;i<84;i++) fileinfo_buf[i]=databuf[i+5]; //including response code
        else fprintf(stderr,"Data read error\n");
    }

    res = check_ack_packet(fd, FileInfoRead, wirelessflag);
    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
    }
}

void get_filedata(int fd, unsigned char prdct_id, unsigned char id, int fileno, long int start_recordno, long int record_num, unsigned char* filedata_buf, int wirelessflag){
    unsigned char argbuf[10];
    unsigned char res;
    long la4,la3,la2,la1;
    int i;

    argbuf[0] = id;

    argbuf[1] = (unsigned char)fileno;


    la1 = start_recordno % 256;
    la2 = start_recordno / 256;
    la3 = la2 / 256;
    la4 = la3 / 256;
    
    la3 = la3 % 256;
    la2 = la2 % 256;
    
    argbuf[2] = (unsigned char)la4;
    argbuf[3] = (unsigned char)la3;
    argbuf[4] = (unsigned char)la2;
    argbuf[5] = (unsigned char)la1;

    la1 = record_num % 256;
    la2 = record_num / 256;
    la3 = la2 / 256;
    la4 = la3 / 256;
    
    la3 = la3 % 256;
    la2 = la2 % 256;
    
    argbuf[6] = (unsigned char)la4;
    argbuf[7] = (unsigned char)la3;
    argbuf[8] = (unsigned char)la2;
    argbuf[9] = (unsigned char)la1;
    
    write_command(fd, FileDataRead, prdct_id, argbuf);
    res = check_ack_packet(fd, FileDataRead, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
    } else {
        read_datapacket(fd, 76, wirelessflag);
        if(databuf[5] == 0x8a)
            for(i=0;i<69;i++) filedata_buf[i]=databuf[i+6];
        else fprintf(stderr,"Data read error\n");
    }

    res = check_ack_packet(fd, FileDataRead, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
    }
}

int set_measurefrequency(int fd, unsigned char prdct_id, unsigned char id, int freq, int wirelessflag){
    unsigned char argbuf[8];    
    unsigned char res;
    
    switch(freq){
    case 1:
    case 5:
    case 10:
    case 20:
    case 50:
    case 100:
    case 200:
    case 500:
    case 1000:        
        break;
    default:
        fprintf(stderr, "%d[Hz] is not supported.\n",freq);
        return 0;
    }

    argbuf[0] = id;
    
    argbuf[1]=(unsigned char)(freq/256);
    argbuf[2]=(unsigned char)(freq%256);

    write_command(fd, MeaureFreq, prdct_id, argbuf);
    res = check_ack_packet(fd, MeaureFreq, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
	return 0;
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
	return 0;
    }
    return 1;
}

int set_measuretime(int fd, unsigned char prdct_id, unsigned char id, int hour, int min, int sec, int wirelessflag){
    unsigned char argbuf[8];    
    unsigned char res;

    if(hour>=256)hour=255;
    else if(hour<0)hour=0;
    
    if(min>=60)min=59;
    else if(min<0)min=0;

    if(sec>=60)sec=59;
    else if(sec<0)sec=0;
    
    argbuf[0] = id;

    argbuf[1] = (unsigned char)hour;
    argbuf[2] = (unsigned char)min;
    argbuf[3] = (unsigned char)sec;    

    write_command(fd, MeasureTime, prdct_id, argbuf);
    res = check_ack_packet(fd, MeasureTime, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
	return 0;
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
	return 0;
    }
    return 1;
}

void set_maggain(int fd, unsigned char prdct_id, unsigned char id, unsigned char gain, int wirelessflag){
    unsigned char argbuf[4];    
    unsigned char res;

    if(gain>=8){
        fprintf(stderr, "Unsupported parameter!\n");
        return;
    }
    
    argbuf[0] = id;
    argbuf[1] = gain;
    
    write_command(fd, MagGainSetting, prdct_id, argbuf);
    res = check_ack_packet(fd, MagGainSetting, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
    }
}

void set_wirelesschannel(int fd, unsigned char prdct_id, unsigned char id, unsigned char channel, int wirelessflag){
    unsigned char argbuf[2];
    unsigned char res;

    argbuf[0] = id;
    argbuf[1] = channel;
    
    write_command(fd, WirelessChannel, prdct_id, argbuf);
    res = check_ack_packet(fd, WirelessChannel, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
    }
}

void set_wirelessonoff(int fd, unsigned char prdct_id, unsigned char id, unsigned char onoff, int wirelessflag){
    unsigned char argbuf[2];
    unsigned char res;

    argbuf[0] = id;
    argbuf[1] = onoff;
    
    write_command(fd, WirelessOnOff, prdct_id, argbuf);
    res = check_ack_packet(fd, WirelessOnOff, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
    }
}

void memory_allclear(int fd, unsigned char prdct_id, unsigned char id, int wirelessflag){
    unsigned char res;
    
    write_command(fd, MemoryClear, prdct_id, &id);
    res = check_ack_packet(fd, MemoryClear, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
    }
}

void initialize_settings(int fd, unsigned char prdct_id, unsigned char id, int wirelessflag){
    unsigned char res;
    
    write_command(fd, Initialize, prdct_id, &id);
    res = check_ack_packet(fd, Initialize, wirelessflag);

    if(res==0x00){
        fprintf(stderr,"Failed to get ack packet\n");
    } else if(res != AckComplete){
        fprintf(stderr,"Status error %x\n", res);
    }
}
