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

#define BAUDRATE B57600
#define MODEMDEVICE "/dev/cua0"
#define _POSIX_SOURCE 1 /* POSIX compliant source */
#define FALSE 0
#define TRUE 1

volatile int STOP=FALSE;
void SetGain(unsigned short Kp, unsigned short Kd);
void RequestStatus ();

int fd;

  unsigned short Sd = 1;
  unsigned short Ki = 0;

main(int argc, char **argv)
{
  int count,ret;
  unsigned char buf[80];
  struct termios oldtio,newtio;
  unsigned short Kp, Kd;

 argc--;*argv++;
 if(argc-- > 0) Kp = atoi(*argv++);
 if(argc-- > 0) Kd = atoi(*argv++);
 if(argc-- > 0) Sd = atoi(*argv++);
 if(argc-- > 0) Ki = atoi(*argv++);
 printf("Setting Kp to %d, Kd to %d Sample divisor = %d\n",Kp,Kd,Sd);

 fd = open(MODEMDEVICE, O_RDWR);
 if (fd <0) {perror(MODEMDEVICE); exit(-1); }

 tcgetattr(fd,&oldtio); /* save current port settings */

 /* bzero(newtio, sizeof(newtio)); */
 newtio.c_cflag = BAUDRATE | CS8 | CREAD;
 newtio.c_iflag = 0;
 newtio.c_oflag = 0;

 /* set input mode (non-canonical, no echo,...) */
 newtio.c_lflag = 0;

 newtio.c_cc[VTIME]    = 0;   /* inter-character timer unused */
 newtio.c_cc[VMIN]     = 1;   /* blocking read until 1 chars received */

 tcflush(fd, TCIFLUSH);
 tcsetattr(fd,TCSANOW,&newtio);

 SetGain(Kp,Kd);
 count = 0;
 do {
   ret = read(fd,&buf[count],6-count);
printf("Read retunred %d butes\n",ret);
   count += ret;
  } while (count < 1);
  printf("Status = 0x%02x\n",buf[0]);

 tcsetattr(fd,TCSANOW,&oldtio);
}



void
SetGain (unsigned short Kp, unsigned short Kd) {
    unsigned char a[40];
    int i = 0;
    int j;
    unsigned char sum = 0;
	int ret;

    a[i++] = 0xAA;
    a[i++] = 0x80;
    a[i++] = 0xE6;

    a[i++] = Kp & 0xFF;		/* Kp */
    a[i++] = (Kp>>8) & 0xFF;

    a[i++] = Kd & 0xFF;		/* Kd */
    a[i++] = (Kd>>8) & 0xFF;

    a[i++] = Ki & 0xFF;		/* Ki */
    a[i++] = (Ki>>8) & 0xFF;

    a[i++] = 0x00;				/* Il */
    a[i++] = 0x05;

    a[i++] = 0xFF;				/* Output Limit */

    a[i++] = 0x00;				/* Current Limit Zero disables limit */

    a[i++] = 0xFF;				/* Error limit */
    a[i++] = 0x3F;

    a[i++] = Sd;				/* Servo rate divisor */

    a[i++] = 0x00;				/* Amp deadband */

	sum = 0;
    for(j=1;j<i;j++) sum += a[j];
    a[i] = sum & 0xFF;

printf("i = %d\n",i);
    for(j=0;j<=i;j++) {
		ret = write(fd,&a[j],1);
		if(ret != 1) {
			printf("write returned %d errno = %d\n",ret,errno);
			perror("Write: ");
		}
	}
}

void
RequestStatus () {
    unsigned char a[40];
    int i = 0;
    int j;
    unsigned char sum = 0;
	int ret;

    a[i++] = 0xAA;
    a[i++] = 0x80;
    a[i++] = 0x13;
    a[i++] = 0x01;

	sum = 0;
    for(j=1;j<i;j++) sum += a[j];
    a[i] = sum & 0xFF;

    for(j=0;j<=i;j++) {
		ret = write(fd,&a[j],1);
		if(ret != 1) {
			printf("write returned %d errno = %d\n",ret,errno);
			perror("Write: ");
		}
	}
}