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

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

volatile int STOP=FALSE;
void SetPVA(unsigned long p, unsigned long v, unsigned long a);
void StopMotor(unsigned char value);
void ReadStatus(char string[]);

int fd;

main(int argc, char **argv)
{
  int i,count,ret;
  unsigned char buf[40];
  struct termios oldtio,newtio;
 unsigned long p; unsigned long v; unsigned long a;
 unsigned long velocity =   100000;
 unsigned long acceleration = 100000;

 argc--;*argv++;
 if(argc-- > 0) p= atol(*argv++);

 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]     = 0;   /* blocking read until 1 chars received */

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




#if(0)
while(1) {
 RequestStatus ();
 count = 0;
 do {
printf("A\n");
   ret = read(fd,&buf[count],3-count);
   count += ret;
  } while (count < 3);
 if((buf[0] & 1) == 1) break;
}

 WaitDone();
#endif
 while(1) {
#if(1)
   for(i=0;i<400;i+=40) {
	SetPVA(i,velocity,acceleration);
	usleep(100*1000);
  }
#else
   SetPVA(0,velocity,acceleration);
   SetPVA(50000,velocity,acceleration);
#endif
 }
 tcsetattr(fd,TCSANOW,&oldtio);
}



void
SetPVA(unsigned long p, unsigned long v, unsigned long ac){
    unsigned char a[40];
    int i = 0;
    int j;
    unsigned char sum = 0;
	int ret;

    a[i++] = 0xAA;
    a[i++] = 0x00;
    a[i++] = 0xd4;
    a[i++] = 0x97;

    a[i++] = p & 0xFF;
    a[i++] = (p >>  8) & 0xff;
    a[i++] = (p >> 16) & 0xff;
    a[i++] = (p >> 24) & 0xff;

    a[i++] = v & 0xFF;
    a[i++] = (v >>  8) & 0xff;
    a[i++] = (v >> 16) & 0xff;
    a[i++] = (v >> 24) & 0xff;

    a[i++] = ac & 0xFF;
    a[i++] = (ac >>  8) & 0xff;
    a[i++] = (ac >> 16) & 0xff;
    a[i++] = (ac >> 24) & 0xff;

	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: ");
		}
	}
	ReadStatus("SetPVA");
    WaitDone();
}

void
ReadStatus (char string[]) {
    unsigned char buf[4];
    int ret;
    int count = 0;

 do {
   count += read(fd,&buf[count],2-count);
  } while (count < 2);
}

ReadAuxStatus () {
    unsigned char buf[4];
    int ret;
    int count = 0;

 do {
   count += read(fd,&buf[count],3-count);
  } while (count < 3);
	printf("%d: 0x%02x %02x %02x\n",count,buf[0],buf[1],buf[2]);
}

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

    a[i++] = 0xAA;
    a[i++] = 0x00;
    a[i++] = 0x13;
    a[i++] = 0x08;

	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: ");
		}
	}
}

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

    a[i++] = 0xAA;
    a[i++] = 0x00;
    a[i++] = 0x17;
    a[i++] = value;

	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: ");
		}
	}
	ReadStatus("StopMotor");
}

WaitDone() {
  int ret,count;
  int nd = 0;
  char buf[40];
  while(1) {
   RequestStatus ();
   count = 0;
   do {
     ret = read(fd,&buf[count],3-count);
     count += ret;
    } while (count < 3);
#if(0)
printf("Count = %03d Aux = %02x Stat = %02x\n",nd,buf[1],buf[0]);
#endif
   if((buf[0] & 1) == 1) break;
   nd++;
  }
}