#include "servo.h"
#include "pca9685_pwm.h"
#include <iostream>
using namespace std;

int main() {

	int		menu(void);

	int		choice, ret_code;
	float		angle_hd1810mg, angle_parallax_conrot;
	float		freq = DEFAULT_FREQ;	// i.e. 50 Hz
	float		pcycl_width;

	PWM_Board	my_pwm;

	pcycl_width = 1000000/freq;		// in usecs
	ret_code = my_pwm.setFreq(freq);
	if (ret_code < 0) {
	   cout << "   ERROR in setting frequency - Err Code " << ret_code << endl << endl;
	   return 1;
	}

	// HD-1810MG Digital Mini Servo Specs (manufacturer: Power HD):
	// range = 145, pulse cycle width = 20000, minpulse = 750, maxpulse = 2250; channel 8
	Servo		hd1810mg(140, pcycl_width, 880, 2200, my_pwm, 8);  // tested safe defaults

	// Parallax Continuous Rotation Servo Specs:
	// range = 360, pulse cycle width = 20000, minpulse = 1300, maxpulse = 1700; channel 9
	Servo		parallax_conrot(360, pcycl_width, 1300, 1700, my_pwm, 9);

	choice = menu();
	while (1) {
	  switch (choice) {

		case 0:	cout << " Getting Position " << endl;
			cout << "   HD-1810MG shaft is at " << hd1810mg.getPosition() << " deg" << endl;
			cout << "   Parallax Continuous Rotation servo shaft is at "
			     << parallax_conrot.getPosition() << " deg" << endl << endl;
			choice = menu();
			break;

		case 1:	cout << " Servo Angle for hd1810mg ? --> ";
			cin >> angle_hd1810mg;
			ret_code = hd1810mg.setPosition(angle_hd1810mg, my_pwm);
			if (ret_code < 0)
			  cout << "   ERROR setting angle - Err Code " << ret_code << endl << endl;
			else
			  cout << "   Servo angle set!" << endl << endl;
			choice = menu();
			break;

		case 2:	cout << " Centering shaft " << endl << endl;
			ret_code = hd1810mg.center(my_pwm);
			if(ret_code < 0)
			  cout << "\n ERROR centering shaft - Err Code " << ret_code << endl;
			choice = menu();
			break;

		case 3:	cout << " Turning Pulses Off to Sleep" << endl << endl;
			ret_code = hd1810mg.sleep(my_pwm);
			if(ret_code < 0)
			  cout << "\n ERROR in turning off pulses - Err Code " << ret_code << endl;
			choice = menu();
			break;

		case 4: cout << " Speed & Direction for Parallax Continuous Rotation Servo? --> ";
			cin >> angle_parallax_conrot;
			ret_code = parallax_conrot.setPosition(angle_parallax_conrot, my_pwm);
			if (ret_code < 0)
			  cout << "   ERROR setting speed & direction - Err Code " << ret_code << endl << endl;
			else
			  cout << "   Servo angle set!" << endl << endl;
			choice = menu();
			break;

		case 5: cout << " Halt Rotation for Parallax Continuous Rotation Servo? --> ";
			ret_code = parallax_conrot.center(my_pwm);
			if (ret_code < 0)
			  cout << "   ERROR halting rotations - Err Code " << ret_code << endl << endl;
			else
			  cout << "   Servo rotations halted!" << endl << endl;
			choice = menu();
			break;

		case 6:	cout << " Turning Pulses Off to Sleep Parallax Continuous Rotation Servo" << endl << endl;
			ret_code = parallax_conrot.sleep(my_pwm);
			if(ret_code < 0)
			  cout << "\n ERROR in turning off pulses - Err Code " << ret_code << endl;
			choice = menu();
			break;
		
		default:hd1810mg.sleep(my_pwm);
			parallax_conrot.sleep(my_pwm);
			cout <<" Quitting!" << endl << endl; return 0;
			break;
	  } // switch
	} // while
}


int menu(void) {

	int sel;

	cout << endl << " Please Select:" << endl;
	cout << "    (0): Get Position (all servos)" << endl;
	cout << "    (1): Set Position (std range-limited servo)" << endl;
	cout << "    (2): Center the shaft (std range-limited servo)" << endl;
	cout << "    (3): Sleep (std range-limited servo)" << endl;
	cout << "    (4): Set Speed & Direction (continuous rotation servo)" << endl;
	cout << "    (5): Halt rotations (continuous rotation servo)" << endl;
	cout << "    (6): Sleep (continuous rotation servo)" << endl;
	cout << "    (7): Exit            --> ";

	cin >> sel; cout << endl << endl;
	return sel;
}