#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++ DigitalSlaveOutput3.cpp -L../ -lhubo -lpthread -lrt -o DigitalSlaveOutput3.out
Run:
	sudo ./DigitalSlaveOutput3.out
Purpose:
	As long as the buffers are not filled the read-back of the DO channels will return initialization values - hence 0.
	Thus the read-back value might not be the one the hardware is currently set to. In order to test this sample you first
	need to set one or more of the slave outputs (e.g. use DigitalSlaveOutput1.cpp) and then run this program. 
*/

void BoostThreadPriority();

int main(void)
{
	printf("Prior to running this sample ensure that at least on digital output is set (e.g. use DigitalSlaveOutput1.cpp)!\n");
	int slaveNo = 0; // ... resulting in i2c address 0x21.
	unsigned char currentDigitalOutputs = 0;

	// 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 prior 
	// to us continuing reading back the output 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;
	}

	// Without this line below you might read initialization values of the buffer rather than
	// the real values of the output latches.
	Wait_For_MCP23017_Slaves_Buffered_Values();

	while (!currentDigitalOutputs)
	{
		Readback_Slave_DO_Channels(slaveNo, currentDigitalOutputs);
		printf("Outputs read back are: 0x%02X\n", currentDigitalOutputs);
	}

	// 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);
}
