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

int main(int argc, char** argv) {

	int speed1, speed2;

	Joystick joy;
	joyParams params;
	joy.dev = "/dev/input/js0";	
	joy.init();

	Roboteq ax3500;
	ax3500.dev = "/dev/ttyS0";	
	ax3500.connect();

	while (1) {
		params = joy.update();
		if (params.updated == TRUE) {
			usleep(25000);
			speed1 = params.axisData[0];
			if (params.buttonData[0] == 1) {
				ax3500.setSpeed(1, DIRECTION_FORWARD, 0);
				ax3500.setSpeed(2, DIRECTION_FORWARD, 0);
				printf("BREAK! \n");
			}
			else if (params.buttonData[1] == 1) {
				ax3500.reset();
				printf("RESET! \n");
			}
			else {
				if (speed1 >= 0) {
					ax3500.setSpeed(1, DIRECTION_FORWARD, speed1/259);
					printf("Motor 1 going fowards at %i.\n", speed1/259);
				}
				else {
					ax3500.setSpeed(1, DIRECTION_REVERSE, -(speed1/259));
					printf("Motor 1 going backwards at %i.\n", -(speed1/259));
				}
				speed2 = params.axisData[1];
				if (speed2 <= 0) {
					ax3500.setSpeed(2, DIRECTION_FORWARD, speed2/259);
					printf("Motor 2 going fowards at %i.\n", -(speed2/259));
				}
				else {
					ax3500.setSpeed(2, DIRECTION_REVERSE, -(speed2/259));
					printf("Motor 2 going backwards at %i.\n", speed2/259);
				}
			}
		}
	}

	return -1;
}