/* move.c
Adam Palmer
http://www.adamsinfo.com

Parts of the code has been copied/modified from:
	Beej's guide to network programming - http://beej.us/guide/net/

This program starts a listening UDP server on 127.0.0.1:4950 it accepts single character at a time commands.
It converts each character to a relevant set of motor commands (single bytes) and outputs these bytes to the serial port
*/

#include <sys/types.h>
#include <netinet/in.h> 
#include <sys/socket.h> 
#include <sys/wait.h> 
#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#include <stdlib.h>

#define QUIET (5*1000) /* serial io pause time (usec) */
#define MYPORT 4950    /* the port users will be sending to */
#define MAXBUFLEN 100
#define DEBUG 0

void motor(int, int, unsigned char, unsigned char);

void motor(int fd, int fd2, unsigned char m1, unsigned char m2)
{
	write(fd, &m1, 1);
	usleep(QUIET);
	write(fd2, &m2, 1);
	usleep(QUIET);
	
}


int main(void)
{
	int fd; /* FD for Serial 1 */
	int fd2; /* FD for Serial 2 */

        int sockfd;
        struct sockaddr_in my_addr;    /* my address information */
        struct sockaddr_in their_addr; /* connector's address information */
        int addr_len, numbytes;
        char buf[MAXBUFLEN];

        if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
            perror("socket");
            exit(1);
        }

        my_addr.sin_family = AF_INET;         /* host byte order */
        my_addr.sin_port = htons(MYPORT);     /* short, network byte order */
        my_addr.sin_addr.s_addr = INADDR_ANY; /* auto-fill with my IP */
        bzero(&(my_addr.sin_zero), 8);        /* zero the rest of the struct */

        if (bind(sockfd, (struct sockaddr *)&my_addr, sizeof(struct sockaddr)) \
                                                                       == -1) {
            perror("bind");
            exit(1);
        }

        addr_len = sizeof(struct sockaddr);

	fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);
	fd2 = open("/dev/ttyUSB1", O_RDWR | O_NOCTTY | O_NDELAY);
	if (fd == -1)
	{
       		perror("open_port: Unable to open /dev/ttyUSB0 - ");
	} else {
		fcntl(fd2, F_SETFL, 0);
	}

	if (fd2 == -1)
 	{
		perror("open_port: Unable to open /dev/ttyUSB1 - ");
 	} else {
		fcntl(fd2, F_SETFL, 0);
	}

	struct termios oldtio, options;
	struct termios oldtio2, options2;

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

	/* Get the current options for the ports */
	tcgetattr(fd, &options);
	tcgetattr(fd2, &options2);

	/* Baud 9600 */
	cfsetispeed(&options, B9600);
	cfsetospeed(&options, B9600);
	options.c_cflag &= ~CSIZE;
	options.c_cflag |= CS8;
	options.c_oflag &= ~OPOST; //raw output

	cfsetispeed(&options2, B9600);
	cfsetospeed(&options2, B9600);
	options2.c_cflag &= ~CSIZE;
	options2.c_cflag |= CS8;
	options2.c_oflag &= ~OPOST; //raw output
	options.c_cflag |= (CLOCAL | CREAD);
	options2.c_cflag |= (CLOCAL | CREAD);

	/* Set new options for the ports */
	tcsetattr(fd, TCSANOW, &options);
	tcsetattr(fd2, TCSANOW, &options2);

	int c = 0;

	while (c != 'p')
	{
        	if ((numbytes=recvfrom(sockfd, buf, MAXBUFLEN, 0, (struct sockaddr *)&their_addr, &addr_len)) == -1) 
		{
			perror("recvfrom");
			exit(1);
	        }
        	buf[numbytes] = '\0';
		c = buf[0]; /* We only want on character, for the moment anyway. */
		if (DEBUG)
		{
			printf("got packet from %s\n",inet_ntoa(their_addr.sin_addr));
			printf("packet is %d bytes long\n",numbytes);
			printf("buf is: %s\n", buf);
			printf("c = %c\n",c);
		}

		switch(c)
		{
			case 's':
				motor(fd, fd2, 64, 64);
				motor(fd, fd2, 192, 192);
				break;
			case 'a':
				motor(fd, fd2, 192, 192);
				motor(fd, fd2, 1, 1);
				break;
			case 'd':
				motor(fd, fd2, 192, 192);
				motor(fd, fd2, 127, 127);
				break;
			case 'w':
				motor(fd, fd2, 64, 64);
				motor(fd, fd2, 128, 136); //trim slightly	
				break;
			case 'x':
				motor(fd, fd2, 64, 64);
				motor(fd, fd2, 255, 255);
				break;
			case 'k':
				motor(fd, fd2, 64, 64);
				motor(fd, fd2, 192, 192);
				motor(fd, fd2, 128, 255);
				motor(fd, fd2, 127, 1);
				break;
			case 'l':
				motor(fd, fd2, 64, 64);
				motor(fd, fd2, 192, 192);
				motor(fd, fd2, 255, 128);
				motor(fd, fd2, 1, 127);
				break;
			case 'q':
				motor(fd, fd2, 0, 0);
				motor(fd, fd2, 0, 0);
				break;
			default:
				break;
		}
	}

	/* Restore old settings */
        tcsetattr(fd,TCSANOW,&oldtio);
        tcsetattr(fd2,TCSANOW,&oldtio2);

	close(fd);
	close(fd2);
        close(sockfd);

	return 0;
}