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

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

using namespace HuboLib;

/*
Compile and link: 
	g++ DigitalInput6.cpp -L../ -lhubo -lpthread -lrt -o DigitalInput6.out
Run:
	sudo ./DigitalInput6.out
Purpose:
	Read all digital inputs in one go.
	Prior to reading wait until the buffers have valid values.
	This will not only ensure that the input values are read from the hardware first
	but also make sure the output latches are read back from the hardware! 
*/

void BoostThreadPriority();

int main(void)
{
	// We need to boost the main threads priority in order to compete with the Hubo-libraries background thread.
	// Otherwise we cannot demonstrate the issue as the background thread is likely to first finish the update 
	// of the buffers prior to us continuing reading the input channels!
	BoostThreadPriority();
	
	// 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 1;
	}

	// Wait in order to be sure the values read are already initialized from the hardware.
	Wait_For_MCP23017_Buffered_Values();

	while (1) 
	{
		unsigned char allInputs = 0;
		
		// Read all 8 digital inputs (lets even wait and ensure that the background thread has finished initialization).
		while (!Get_DI_Channels(allInputs))
		{
			printf ("Error: library not yet initialized.\n");
			usleep(0);
		}
		
		// Invert logic.
		allInputs ^= 0xFF;
		
		printf ("Digital inputs (hex) = 0x%02X\n", allInputs);
		
		// Stop once we detect at least one input set.
		if (allInputs != 0)
			break;
		// usleep (1000L * 10L);
	} 

	// Free library resources.
	Uninitialize();
	
	return 0;
}

#include <pthread.h>    
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);
}
