/****************************************************************
 $Id: sensor.c,v 1.1 2002/08/07 14:34:57 okazaki Exp $

    Default device: /dev/ttyS0
    (cf.) # chmod a+rw /dev/ttyS0

****************************************************************/
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>

int fd = 0;
char *port = "/dev/ttyS0";

void connect(int vtime, int vmin);
void disconnect(void);
void usage(void);
void PutG(void);

int main(int argc, char *argv[])
{
    int c, i;
    int disconnectFlag = 0;

    /* option */
    while ((c = getopt(argc, argv, "dp:h")) != -1) {
	switch(c) {
	case 'd':
	    disconnectFlag = 1;
	    break;
	case 'p':
	    port = optarg;
	    break;
	case 'h':
	    usage();
	default:
	    break;
	}
    }

    connect(0, 1);	/* wait a charactor when API:"read" */

    for (i = 0; i < 1000; i++) {
	printf("%8d ", i);
	PutG();
	usleep(1000*10);
    }
    
    /* disconnect */
    if (disconnectFlag) {
	fprintf(stderr, "disconnect %s\n", port);
	disconnect();
    }

    close(fd); /* disconnect by c_cflag |= HUPCL */

    return 0;
}

/****************************************************************
* sub routines
****************************************************************/
/* connect serial port */
void connect(int vtime, int vmin)
{
    struct termios setting;

    fd = open(port, O_RDWR | O_NOCTTY);
    if (fd < 0){
	perror(port);
	exit(-1);
    }
    fprintf(stderr, "connect %s\n", port);

    bzero(&setting, sizeof(setting));
    setting.c_cflag = CS8 | CLOCAL | CREAD;
    setting.c_iflag = IGNBRK | IGNPAR;
    setting.c_oflag = OPOST | CRDLY | BSDLY;
    setting.c_lflag = 0;
    setting.c_cc[VTIME] = vtime;
    setting.c_cc[VMIN] = vmin;

    tcflush(fd, TCIOFLUSH);
    cfsetispeed(&setting, B9600);
    cfsetospeed(&setting, B9600);
    tcsetattr(fd,TCSANOW,&setting);
}

/*  disconnect serial port */
void disconnect(void)
{
    struct termios setting;

    tcgetattr(fd, &setting);
    setting.c_cflag |=  HUPCL;

    tcflush(fd, TCIOFLUSH);
    tcsetattr(fd,TCSANOW,&setting);
}


/* print help document */
void usage(void)
{
    printf("Usage:");
    printf("\tsensor [-hd] [-p serial-device]\n");
    printf("\t-h help\n");
    printf("\t-d power off serial port\n");
    printf("\t\tdefault serial-device : /dev/ttyS0\n");
    
    exit(1);
}

/* print one charactor */
void PutG(void)
{
    char buf[4];
    int x, y;
    static double ax, ay, vx, vy, lx, ly;

    buf[0] = 'G';
    write(fd, &buf, 1);
    read(fd, &buf, 4);
    x = ((0xff & buf[0]) << 8) + (0xff & buf[1]);
    y = ((0xff & buf[2]) << 8) + (0xff & buf[3]);
    //printf("%04x %04x\n", x, y);
    ax = ((double)x/10000 - 0.4863) / 0.125;
    ay = ((double)y/10000 - 0.4904100) / 0.125;
    vx += ax;
    vy += ay;
    lx += vx;
    ly += vy;
    printf("%9.4f %9.4f %9.4f %9.4f %9.4f %9.4f\n", ax, ay, vx, vy, lx, ly);
}