#include "../include/roboteq.h"

Roboteq::Roboteq() {

	// Set up power-on settings
	defaultMotorParams.speed1 = 0;
	defaultMotorParams.speed2 = 0;
	defaultMotorParams.dir1 = DIRECTION_FORWARD;
	defaultMotorParams.dir2 = DIRECTION_FORWARD;
	defaultMotorParams.power1 = 0;
	defaultMotorParams.power2 = 0;
	defaultMotorParams.amps1 = 0;
	defaultMotorParams.amps2 = 0;
	dev = "/dev/ttyS0";
}

Roboteq::~Roboteq() {

	// Make sure the port is closed
	port.closePort();
}

int Roboteq::connect() {

	// Set up serial port
	port.dev = dev;
	port.baud = 9600;
	port.dataBits = 7;
	port.parity = PARITY_7E1;

	// Open serial port
	if (port.initPort() == TRUE) {
		// Clear read / write buffers
		port.flushPort();
		setRS232();
		return TRUE;
	}
	// Program should never get here
	else return FALSE;
}

int Roboteq::setSpeed(int channel, int direction, int speed) {

	// Buffer for command string
	char buf[4];
	buf[0] = '!';
	
	// Parse channel and direction
	switch (channel) {
		// Commands for channel 1
		case 1:
			if (direction == DIRECTION_FORWARD) buf[1] = 'A';
			else if (direction == DIRECTION_REVERSE) buf[1] = 'a';
			else return FALSE;
			break;
		// Commands for channel 2
		case 2:
			if (direction == DIRECTION_FORWARD) buf[1] = 'B';
			else if (direction == DIRECTION_REVERSE) buf[1] = 'b';
			else return FALSE;
			break;
		// Unexpected input, so return error
		default:
			return FALSE;
			break;
	}

	// Parse speed
	if (speed < 128 && speed >= 0) {
		// Convert int to hex string
		char buf2[16];
		// Make sure 0 speed is "00"
		if (speed == 0) {
			buf[2] = '0';
			buf[3] = '0';
		}
		// Make sure 1-15 is "0X"
		else if (speed > 0 && speed < 16) {
			snprintf(buf2, 16, "%X", speed);
			buf[2] = '0';
			buf[3] = buf2[1];	
		}
		// Otherwise just "XX"
		else {
			snprintf(buf2, 16, "%X", speed);
			buf[2] = buf2[0];
			buf[3] = buf2[1];	
		}
	}
	// Speed out of range, so return error
	else return FALSE;
	
	// Send the data, and make sure it's all gone
	port.sendData(buf);
	newLine();	

	return TRUE;
}

void Roboteq::newLine() {
	
	// Send the newline character after a command
	char c[1];
	c[0] = '\r';
	port.sendData(c);
}

int Roboteq::reset() {

	// Send reset string to serial port
	// 		String is "%rrrrrr"
	char buf[7];
	buf[0] = '%';
	for (int i = 0; i < 6; i++) buf[i + 1] = 'r';
	port.sendData(buf);
	newLine();
	return TRUE;
}
	
int Roboteq::setRS232() {
	
	// Create output string
	char buf[10];
	for (int i = 0; i < 10; i++) {
		buf[i] = '\r';
	}
	// Send it!
	port.sendData(buf);
	return TRUE;
}