/************************************************************
machine_rtc.c
Machine Robot Real Time Control Demo
By Joshua Galbraith

The purpose of this demo is to demonstrate the CH Mindstorms 
Control Package's ability to control the machine robot model,
As well as demonstrate how to set up and get sensor data.
*************************************************************/ 
#include <conio.h>
#include <stdio.h>
#include <ch_nxt.h>

int main(){

	struct nxt_remote nxtr;	//creates the structure that holds information regarding the NXT
	int speed=100;			//used to control the motor speed.
	int angle=0;			//stores the angle input from the user.
	int quit = 0;			//used to break out of the control loop
	int result=0;
	double gearratio = (8.0/56)*(1.0/24); //found from gears used on the arm
	char dir=0;			//stores "direction to move" input from user.
	char color[5];
	char temp[20];
	char *temp_loc;
	printf("gear ratio: %f",gearratio);
	
	//call nxt_connect function and check for success
	printf("\nInitializing arm and assuming control...");
	
	result = nxt_connect(&nxtr);
	if (result ==0){
		while (!_kbhit()); //wait for key press
		nxt_disconnect();  //stop interfacing. This also stops the motors.
		return(0);
	}//if (result ==0)

	
	//Initialize sensor and check for success
	result = nxt_setsensor(PORT_1,TOUCH,BOOLEANMODE);
	if (result ==0){
		printf("\nSensor Setup failed. Exiting program.");
		while (!_kbhit());  //wait for key press
		nxt_disconnect();	//stop interfacing. This also stops the motors.
		return(0);
	}//if (result ==0)

	result = nxt_setsensor(PORT_3,LIGHT_ACTIVE,RAWMODE);
	if (result ==0){
		printf("\nSensor Setup failed. Exiting program.");
		while (!_kbhit());  //wait for key press
		nxt_disconnect();	//stop interfacing. This also stops the motors.
		return(0);
	}//if (result ==0)


	/*This is the user input loop, that gets the user input and 
	sends the commands to the NXT accordingly.
	w,s move arm up and down
	a,d move arm left and right
	q,e open and close the claw
	x stops the motor
	r quits the program
	f sets the speed (default to 100)    */	
	
		printf("\nArm Direction:     Claw Control:\n");
		printf("    [w]             [q] Claw Open\n");
        printf("     ^              [e] Claw Close\n");
        printf("[a]<-|->[d]        Other Commands:\n");
		printf("     v              [x] Stop Motors\n");
		printf("    [s]             [r] Exit Program\n");
		printf("                    [f] Set Speed\n");
		printf("Please Enter Command and Angle or Speed:\n");
		printf("[Command]   [Argument]\n");

		while(quit!=)
                {
			printf("\nEnter command: ");
			dir = getche();
		if ((dir=='w')||(dir=='a')||(dir=='s')||(dir=='d')||(dir=='f')){
			printf("  Enter angle or Speed: ");
			scanf("%d",&angle);
		}
		
		//	scanf("%c %d",&dir, &angle);
			
		switch(dir){
			case 'a': //Arm rotate left. Angle adjusted for arm rotation rather than motor rotation.
				nxt_motor_rotate(PORT_C, speed, (int)(angle/gearratio));
				break;			
			case 'd': //Arm rotate right. Angle ajusted for arm rotation rather than motor rotation.
				nxt_motor_rotate(PORT_C, -speed, (int)(angle/gearratio));
				break;
			case 'w': //Raise arm up.
				nxt_motor_rotate(PORT_B, speed, angle);
				break;
			case 's': //lower arm down
				nxt_motor_rotate(PORT_B, -speed, angle);
				break;
			case 'q': //claw open
				nxt_motor(PORT_A, -15, RUN_IDLE);
				Sleep(1000);
				nxt_motor(PORT_A,0,RUN_BRAKE);
				break;
			case 'e': //claw close
				nxt_motor(PORT_A, 15, RUN_IDLE);
				Sleep(1000);
				nxt_motor(PORT_A,0,RUN_BRAKE);	//puts motor in break mode
				break;
			case 'x': //stop
				nxt_motor_zero();
				nxt_motor(PORT_A, 0, OFF_IDLE);
				nxt_motor(PORT_B, 0, OFF_IDLE);
				nxt_motor(PORT_C,0,OFF_IDLE);	//puts motor in idle mode
				break;
			case 'r': //quit
				printf("\nQuit.");                   
				quit = 1;
				break;
			case 'f': //set speed
				speed = angle;
				printf("\nSpeed set to %d.",speed);
				break;
			default:
				printf("\n");
		}//switch(dir)
		Sleep(200);
		nxt_getsensor(PORT_1);
		if (nxtr.NXT_sensorvalraw[PORT_1]<500){
		printf("    The Ball was grabed ");
		nxt_getsensor(PORT_3);
		if (nxtr.NXT_sensorvalraw[PORT_3]<500){
			color[0]='r';color[1]='e';color[2]='d';color[3]=0;
			printf("and the color is %s\n",color);
		}
		else{
			color[0]='b';color[1]='l';color[2]='u';color[3]='e';color[4]=0;
			printf("and the color is %s\n",color);
		}
		}

	}
	
	
	nxt_disconnect();		//stop interfacing. This also stops the motors.
	while (!_kbhit());		//wait for key press
	return 0;				//quit main

}