#include <ctype.h>
#include <stdio.h>
#include <fcntl.h>
#include <sys/signal.h>
#include <sys/types.h>
#include <termios.h>
#include <unistd.h>
#include <string.h>

#define PIVOT  0
#define PINCER 1
#define LOWER  2
#define UPPER  3

int static fd = 0;
unsigned char static pincer_pos = 0;
unsigned char static base_angle_mov = 0;
unsigned char static upper_mov = 0;
unsigned char static lower_mov = 0;
int static changeflag = 0;

void initserial() {

	fd = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY);
    if (fd <0) {perror("/dev/ttyS2"); }

	struct termios options;
	// Get the current options for the port...
	tcgetattr(fd, &options);
	// Set the baud rates to 19200...
	cfsetispeed(&options, B115200);
	cfsetospeed(&options, B115200);
	// Enable the receiver and set local mode...
	options.c_cflag |= (CLOCAL | CREAD);

	options.c_cflag &= ~PARENB;
	options.c_cflag &= ~CSTOPB;
	options.c_cflag &= ~CSIZE;
	options.c_cflag |= CS8;

	// Set the new options for the port...
	tcsetattr(fd, TCSANOW, &options);

}


void move() {
	if (changeflag) {
		char data[4], r;
		data[PIVOT] = base_angle_mov;
		data[PINCER] = pincer_pos;
		data[UPPER] = lower_mov;
		data[LOWER] = upper_mov;
		write(fd, &data, 4);
		usleep(1000);
		read(fd, &r, 1);
		printf("%c\n", (int)r);
		changeflag = 0;
	}		
}

void pincer_close() {
	if (pincer_pos == 0) {
		pincer_pos = 255;
		changeflag = 1;
	}
}

void pincer_open() {
	if (pincer_pos == 255) {
		pincer_pos = 0;
		changeflag = 1;
	}
	pincer_pos = 0;
}

void set_base_angle_mov(int a) {
	if ((unsigned char)a != base_angle_mov) {
		base_angle_mov = a;
		changeflag = 1;
	}
}

void stop_base_angle_mov() {
	set_base_angle_mov(0);
}

void set_upper_mov(int a) {
	if (a != upper_mov) {
		upper_mov = a;
		changeflag = 1;
	}
}

void stop_upper_mov() {
	set_upper_mov(0);
}

void set_lower_mov(int a) {
	if (a != lower_mov) {
		lower_mov = a;
		changeflag = 1;
	}
}

void stop_lower_mov() {
	set_lower_mov(0);
}

int decide_speed_arm(int radius, int abs_r) {
	if (abs_r < 2*radius) return 1;
	else return 2;
}