#include "rs232.h"
#include <stdio.h>
#include <ncurses.h>

int port; // port of the arduinov
typedef struct servo servo;
struct servo {
	int Degree; // current degree 
	unsigned char ADegree; // actual degree
	int Center; // center degree from calibration // difference from 90
	int Limit; //limit, eg 0- 180
	int Rate; // how many degrees to increase per time
	char Header; // character that is sent 
	char Name[] ; // name
};
servo B_S = { 90,90,0,180,1,'B',"Base Servo" }; // base servo position
servo J_S = { 90,90,0,180,1,'J',"Joint Servo"}; // joint servo
servo A_S = { 90,90,0,180,1,'A',"Manipulator Assembly Servo"};// manipulator assembly servo
servo M_S = { 90,90,0,180,1,'J',"Manipulator Servo"}; // manipulator servo
int maxx;
int maxy;
int loop;
int change (servo rservo) {
}
int update ( servo *rservop,int increment) {
	servo rservo = *rservop;
	int err;
	char tempstring [10]; // 10 byte buffer, sprintf should end with null
	rservo.Degree =rservo.Degree + increment*rservo.Rate;
	if (rservo.Degree + rservo.Center > rservo.Limit) {
		rservo.Degree = rservo.Limit - rservo.Center;
		mvprintw(maxy*3/4+1,0," %s at limit %d degrees",rservo.Name,rservo.Limit);
	}
	if (rservo.Degree + rservo.Center < 0) {
		rservo.Degree = 0 - rservo.Center;
		mvprintw(maxy*3/4,0," %s at limit %d degrees",rservo.Name,0);
	}
	rservo.ADegree = rservo.Degree + rservo.Center;
	*rservop = rservo;
	err = RS232_SendBuf(port,&(rservo.Header),1);
	if (err) {
		mvprintw(maxy*3/4,0,"unable to send header ");
		return err;
	}
	err = RS232_SendBuf(port,&(rservo.ADegree),1);
	if (err) {
		mvprintw(maxy*3/4,0,"unable to send ADegree ");
		return err;
	} else {
		mvprintw(maxy*3/4,0,"sent ADegree %d",rservo.ADegree);	
	}
	//sprintf(tempstring,"%d",rservo.Degree);
	//RS232_cputs(port,tempstring); // no error , returns void
	return 0;
}
int startcomm (int rport) {
	int err = RS232_OpenComport(rport ,9600,"8N1");
	if (err) {
		mvprintw(maxy/2,maxx/2,"couldn't open port %s",comports[port]);
		return err;
	}
	mvprintw(maxy/2,maxx/2,"opened %s succesfully",comports[port]);
	return 0;
}
int end () {
	echo();
	nocbreak();
	endwin();
	RS232_CloseComport(port);
}
int init () {
	WINDOW  *errw; // because initscr is only function 
	int err;
	printf("initscr ...\n");
	errw = initscr(); // present window called scr , returns *WINDOW, not int	
	if (errw == NULL) {
		printf("initscr failed\n");
		return 1;
	}
	maxx = getmaxx(stdscr);
	maxy = getmaxy(stdscr);
	cbreak(); // no need for enter
	noecho(); // no display char
	keypad(stdscr,TRUE); // allows f-keys
	mvprintw(maxy/2,maxx/2,"%d %d",maxy,maxx); //w here is just to indicate curses
	err = startcomm(port);
	if (err) {
		return err;
	}
	return 0;
}
int display() {
	// y then x, top > bottom , left >  right
	char tempstring[40];
	mvprintw(0,0,"port:	%s",comports[port]);
	mvprintw(1,0,"aparent angle | actual angle");
	mvprintw(2,0,"base servo angle:	%d  |  %u  ",B_S.Degree,B_S.ADegree);
	
	mvprintw(3,0,"joint servo angle:	%u   |   %d  ",J_S.Degree,(unsigned int)J_S.ADegree);	
	
	mvprintw(4,0,"manipulator assembly servo angle:	%d  |  %u  ",A_S.Degree,(unsigned int)A_S.ADegree);	
	
	mvprintw(5,0,"manipulator servo angle :	%d  |  %u  ",M_S.Degree,(unsigned int)M_S.ADegree);
	return 0;


}
int setting() {
	int rloop = 1;
	int key;
	int i = 0;
	int v = port;
	clear();
	while (rloop) {
		clear();
		while (i < 38) {
			if (i == v) {
				mvprintw(i,maxx/2+5,"<--");
			}
			if (i == port) {
				mvprintw(i,maxx/2-14,"[*] %s",comports[i]);
			} else {
				mvprintw(i,maxx/2-10,"%s",comports[i]);	
			}
			i++;
		}
		mvprintw(38,0," s and d for the base servo, f to center\n w and e for joint servo r to center\n x and c for manipulator v to set\n i and k for up and down assembly servo o to center");
		i = 0;
		key = getch();
		switch (key) {
			case KEY_UP :
				v--;
			break;
			case KEY_DOWN :
				v++;
			break;
			case ' ' :
				RS232_CloseComport(port);
				port = v;
				int err = startcomm(port);
				rloop = 0;
				clear();
				return 0;
			break;
			case 'q' :
				clear();
				rloop = 0;
			break;
		}
		if (v >= 38) {
			v = 0;
		}
		if (v < 0) {
			v = 37;
		}
	}

	return 0;
}
int input() {
	int err;
	int key;
	key = getch();
			
	switch (key) {
		case 'q':
			mvprintw(maxy/2,maxx/2,"quitting",maxy,maxx);
			loop = 0;
		break;
		case 's' :
			update(&B_S,-1);
		break;
		case 'd' :
			update(&B_S,+1);
		break;
		case 'f' :
			B_S.Center = B_S.Degree - 90;
		break;
		
		case 'w' :
			update(&J_S,-1);
		break;
		case 'e' :
			update(&J_S,+1);
		break;
		case 'r' :
			J_S.Center = J_S.Degree - 90;
		break;

		case 'x' :
			update(&M_S,-1);
		break;
		case 'c' :
			update(&M_S,+1);
		break;
		case 'v' :
			M_S.Center = M_S.Degree - 90;
		break;

		case 'i' :
			update(&A_S,-1);
		break;
		case 'k' :
			update(&A_S,+1);
		break;
		case 'o' :
			A_S.Center = A_S.Degree - 90;
		break;



		case 'p' :
			setting();
		break;
	
	}
	if (err) {
		return err;
	}
	return 0;
}
int main () {
	loop = 1;
	init();
	while (loop == 1) {
		display();
		input();
	}
	end();
}