#include <stdio.h>
#include <unistd.h>
#include <pthread.h>    
#include <assert.h>    

#include "../hubolib.h"
#include "../hubocfg.h" // Required for changing default I2C device.

using namespace HuboLib;

/*
Compile and link: 
	g++ GetDigitalInput.cpp -L../ -lhubo -lpthread -lrt -o GetDigitalInput
Run:
	sudo ./GetDigitalInput
Purpose:
	Reads the digital input as specified in the command line and returns the result.
*/

void BoostThreadPriority();

int main(int argc, char* argv[])
{
	// Lets have this program finish as soon as possible.
	BoostThreadPriority();

	if (argc != 2)
	{
		printf ("Usage: \n  GetDigitalInput <digital input to read>\n  example: GetDigitalInput 7  // this will read digital input 7 and return the result\n");
		return 0;
	}

	int channel = 0;
	if (sscanf (argv[1], "%d", &channel) != 1)
	{
		printf ("Digital input could not be determined!\n");
		return 0;
	}

	// If required - set the I2C device to work with. The Raspberry Pi uses "/dev/i2c-1" which is default, the Banana Pi uses "/dev/i2c-0"
	#ifdef BPI
	    g_I2CConfig.m_sI2CDevice = "/dev/i2c-0";
	#endif

	// Initialize the library once in your program.
	if (!Initialize())
	{
		printf ("Error: Initialize\n");
		return 0;
	}
	
	// Set the cycle time to 2ms (50Hz).
	Set_Cycle_Time(20);
	
	// Wait for input buffers to be filled.
	Wait_For_MCP23017_Buffered_Values();

	// Finally get the output.
	bool bOutValue = false;
	bool bResult = Get_DI_Channel (channel, bOutValue);

	// Free library resources.
	Uninitialize();
	
	printf ("%s\n", bOutValue ? "1" : "0");

	return bResult;
}

void BoostThreadPriority()
{
	// Scheduling params.
	sched_param param;
	int			policy;
	int			ret;
	pthread_t	threadHandle = pthread_self();

	// Get current scheduling parameters of thread.
	ret				= pthread_getschedparam (threadHandle, &policy, &param);
	assert (ret == 0);

	// Set scheduling parameters of thread to real time values (FIFO scheduling type and max prio).
	policy				 = SCHED_FIFO; // SCHED_RR;
	param.sched_priority = sched_get_priority_max(policy); // New max priority for new scheduling concept.
	ret					 = pthread_setschedparam(threadHandle, policy, &param);
	assert (ret == 0);
}
