using namespace std;
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/io.h>  // for parallel

//#ifdef linux
//#include <linux/parport.h> // parallel
//#include <linux/ppdev.h> // parallel
//#endif
/*
#ifdef __FreeBSD__
#include <dev/ppbus/ppi.h> // parallel
#include <dev/ppbus/ppbconf.h> // parallel
#endif
*/
#include <unistd.h> // serial
#include <termios.h> // serial
#include "io.h"
#include "settings.h"


IO::IO() {
	int c, res;
	struct termios tty; /* will be used for new port settings */
	char buf[255];      /* buffer used to store received characters */

	// set up serial port
	fd_serial = open(SERIALPORT, O_RDWR | O_NOCTTY ); 
	if (fd_serial < 0) {
		cout << "IO::InitServo: Unable to write to parallel port (" << SERIALPORT << "), are you root?" << endl;
		exit(-1);
	}
	tcgetattr(fd_serial, &oldtty); /* save current port settings */
	bzero(&tty, sizeof(tty)); /* InitServoialize the port settings structure to all zeros */
	/* then set the baud rate, handshaking and a few other settings */
	tty.c_iflag = 0;
	tty.c_lflag = 0;             /* set input mode (non-canonical, no echo,...) */
	tty.c_oflag = 0;
	tty.c_cflag = BAUDRATE | CS8 | CLOCAL;
	tty.c_cc[VTIME]    = 0;      /* inter-character timer unused */
	tty.c_cc[VMIN]     = 1;      /* blocking read until first char received */

	tcflush(fd_serial, TCIFLUSH);
	tcsetattr(fd_serial, TCSANOW, &tty);

	// set up parallel port
//	fd_parallel = open(PARALLELPORT, O_RDWR); 
//	if (fd_parallel < 0) {
//		cout << "IO::InitParallel: Unable to open parallel port (" << PARALLELPORT << "), are you root?" << endl;
//		exit(-1);
//	}
}


IO::~IO() {
	tcsetattr(fd_serial, TCSANOW, &oldtty); /* restore the old port settings before quitting */
}


unsigned char IO::GetParallel(int port) {
	// :: Get the value from the parallel port ::
	unsigned char val;
	
	val = (char)inb(PARALLELPORT + PORT_B);

	int i;
	if (DEBUG) {
		cout << "IO::GetParallel: ";
		if ((val & 0x01) == 0x01) { printf("1"); } else { printf("0"); } // 0000 0001
		if ((val & 0x02) == 0x02) { printf("1"); } else { printf("0"); } // 0000 0010
		if ((val & 0x04) == 0x04) { printf("1"); } else { printf("0"); } // 0000 0100
		if ((val & 0x08) == 0x08) { printf("1"); } else { printf("0"); } // 0000 1000
		if ((val & 0x10) == 0x10) { printf("1"); } else { printf("0"); } // 0001 0000
		if ((val & 0x20) == 0x20) { printf("1"); } else { printf("0"); } // 0010 0000
		if ((val & 0x40) == 0x40) { printf("1"); } else { printf("0"); } // 0100 0000
		if ((val & 0x80) == 0x80) { printf("1"); } else { printf("0"); } // 1000 0000
		cout << "." << endl;
	}
	return val;
}



void IO::SendSerial(unsigned char *val, int length) {
	// :: Send a serial byte down the line ::
	int i;
	if (DEBUG) {
		cout << "IO::SendSerial: ";
		for (i = 0; i < length; i++) {
			printf("%x", val[i]);
			if (i < length-1) {
				printf(", ");
			}
		}
		cout << "." << endl;
	}
	write(fd_serial, val, length);
}


bool IO::SetMotor(int num, int val) {
	// :: Convert to an actual value, then send down the serial line the motor control codes ::
	if (val < -100 || val > 100) {
		if (DEBUG) {
			cout << "IO::SetMotor: val is out of range" << endl;
		}
		return false;
	} else {
		int direction = FORWARD;
		int new_speed = (int)((FULL_SPEED - MIN_SPEED) * ((double)val/100.0));
//		if (val > 0) { new_speed += MIN_SPEED; } else { new_speed -= MIN_SPEED; } // adjust for stall speed
		if (val < 0) {
			direction = BACKWARD;
		}
		unsigned char out[4];
		out[0] = 0x80; // start byte
		out[1] = 0x00; // deviceid (0=motor)
		out[2] = (num*2)+direction; // msb(7) = motor num, lsb = direction
//cout << "val=" << val << "; dir=" << direction << "; out2=" << (int)out[2] << ";" << endl;
		out[3] = abs(new_speed); // max 0x7f
		SendSerial(out, 4);
		if (DEBUG) {
//			cout << "SetMotor: #" << num << ":" << num*2+direction << "=" << new_speed << endl;
		}
	}
	return true;
}


bool IO::SetServo(int num, int percent) {
	// :: Set the position of a servo ::
	if (percent < 0 || percent > 100) {
		if (DEBUG) {
			cout << "IO::SetServo: percent is out of range, use 0-100" << endl;
		}
		return false;
	} else {
		unsigned char out[6];
		out[0] = 0x80; // start byte
		out[1] = 0x01; // deviceid (1=servo)
		out[2] = 0x04; // 2=7bit position
		out[3] = SERVO_ARM;
// min: 0x0374   max: 0x4a7c
		out[4] = (char)(0x03 + ((0x4a - 0x03) * (float)((float)percent/100.0))); // max 0x03-0x4a
		out[5] = 0x74;
//		out[4] = 0x03;
//		out[5] = 0x74;
		SendSerial(out, 6);
		if (DEBUG) {
			cout << "SetServo: #" << num << "=" << percent << endl;
		}
	}
	return true;
}