/**
 * @file
 * @author  Walter Schilling (schilling@msoe.edu)
 * @version 1.0
 *
 * @section LICENSE This code is distributed to students to demonstrate general control of a light.
 *
 *
 * @section DESCRIPTION
 *
 * This program will allow the user to control two lights.  The lights will be controlled by pressing and releasing a pushbutton.
 */

#include <iostream>
#include <unistd.h>
#include <cstdlib>
#include "GPIO.h"
#include <unistd.h>

using namespace SWE4211RPi;
using namespace std;

#define SLEEPLENGTH (1)

/**
 * This program will control the LED.  It essentially will turn the LED on if the button is pressed and off if the button is released.
 */
int main(int argc, char* argv[]) {
	// Check to determine if the command line usage is correct or not.
	if (argc != 5) {
		cerr << "Usage: " << argv[0]
				<< " <Output GPIO Pin 1 Number> <Output GPIO Pin 2 Number> <Input GPIO Pin 1 Number> <Input GPIO Pin 2 Number>";
		exit(-1);
	}

	// Determine the period and the blink count.
	int GPIOPout0 = atoi(argv[1]);
	int GPIOPout1 = atoi(argv[2]);

	int GPIOPin0 = atoi(argv[3]);
	int GPIOPin1 = atoi(argv[4]);

	// Instantiate a new instance of a GPIO port.
	GPIO outGPIO0(GPIOPout0, GPIO::GPIO_OUT);
	GPIO outGPIO1(GPIOPout1, GPIO::GPIO_OUT);

	GPIO inGPIO0(GPIOPin0, GPIO::GPIO_IN);
	GPIO inGPIO1(GPIOPin1, GPIO::GPIO_IN);

	// Setup the operating thread to be a real time thread.
	struct sched_param p;
	p.__sched_priority = sched_get_priority_max(SCHED_FIFO);

	printf("%d\n", p.__sched_priority);

	if (sched_setscheduler(0, SCHED_FIFO, &p) != 0) {
		printf("C. Failed to set the scheduler\n");
		exit(-1);
	}

	// Loop over the data, turning things on and off as is necessary.
	while (1 == 1) {
		// Read pushbutton and set output accordingly.
		if (inGPIO0.getValue() == GPIO::GPIO_LOW) {
			// The button is pressed.  Turn the light on.
			outGPIO0.setValue(GPIO::GPIO::GPIO_LOW);
		} else {
			outGPIO0.setValue(GPIO::GPIO::GPIO_HIGH);
		}

		// Read the other pushbutton and handle it accordingly.
		if (inGPIO1.getValue() == GPIO::GPIO_LOW) {
			// The button is pressed.  Turn the light on.
			outGPIO1.setValue(GPIO::GPIO_LOW);
		} else {
			outGPIO1.setValue(GPIO::GPIO_HIGH);
		}

		usleep(SLEEPLENGTH);
	}
}