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

#include "../hubolib.h"
#include "../hubocfg.h"

using namespace BCM2835;
using namespace HuboLib;


/*
Compile and link: 
	g++ GPIOInputOutput.cpp -O2 -L../ -lhubo -lpthread -lrt -o GPIOInputOutput.out
Run:
	sudo ./GPIOInputOutput.out
Purpose:
	Configure, read from and write to GPIO pins directly.
*/

int main(void)
{
	// While we do not explicitly need to initialize the GPIO interface we still require to check for a correct initialisation.
	if (!IsGPIOInitialized())
	{
		printf ("The GPIO interface could not be initialised - terminating.\n");
		return -1;
	}
	
	// g_bcm2835Config.m_bUseMutexProtection = false;

	// We will use Broadcom pin 22 which is known as GPIO3 and located on pin header number 15.
	unsigned char pin = 22;
	
	// Configure the pin to be an input.
	FunctionSelectPin (pin, Input);
	
	// Read pin.
	printf ("Input pin %d is set to %d\n", pin, ReadPin(pin));
	
	// We set the pull up resistor on the pin.
	SetPullUpDown (pin, PUD_UP);
	printf ("Input pin %d with pull up is set to %d\n", pin, ReadPin(pin));
	
	// We set the pull down resistor on the pin.
	SetPullUpDown (pin, PUD_DOWN);
	printf ("Input pin %d with pull down is set to %d\n", pin, ReadPin(pin));
	
	// Configure the pin to be an output.
	FunctionSelectPin (pin, Output);

	// Let's set the output and read back it's value.
	WritePin (pin, 0);
	printf ("Cleared output pin %d is set to %d\n", pin, ReadPin(pin));
	WritePin (pin, 1);
	printf ("Set output pin %d is set to %d\n", pin, ReadPin(pin));
	
	
	// Some timing measurements.
	unsigned long long tStart, tStop;
	printf ("\n");

	// Lets see how fast we can write a value.
	tStart = GetTime_MicroSeconds();
	for (int i=0; i<1000000; i++)	
	{
		WritePin (pin, 1);
	}
	tStop = GetTime_MicroSeconds();
	printf ("1000000 samples written in %lluµs - thus %fMHz\n", tStop-tStart, 1000000.0/(tStop-tStart));

	// Lets see how fast we can read a value.
	tStart = GetTime_MicroSeconds();
	for (int i=0; i<1000000; i++)	
	{
		ReadPin (pin);
	}
	tStop = GetTime_MicroSeconds();
	printf ("1000000 samples read in %lluµs - thus %fMHz\n", tStop-tStart, 1000000.0/(tStop-tStart));

	
	// Precision of µsleep() vs. Delay_MicroSeconds().
	unsigned long long delay;
	printf ("\n");
	
	// Compare for short delays.
	delay = 30;
	tStart = GetTime_MicroSeconds();
	usleep (delay);
	tStop = GetTime_MicroSeconds();
	printf ("usleep(%lluµs) took %lluµs\n", delay, tStop-tStart);

	tStart = GetTime_MicroSeconds();
	Delay_MicroSeconds (delay);
	tStop = GetTime_MicroSeconds();
	printf ("Delay_MicroSeconds(%lluµs) took %lluµs\n", delay, tStop-tStart);

	// Compare for long delays.
	delay = 1000000;
	tStart = GetTime_MicroSeconds();
	usleep (delay);
	tStop = GetTime_MicroSeconds();
	printf ("usleep(%lluµs) took %lluµs\n", delay, tStop-tStart);

	tStart = GetTime_MicroSeconds();
	Delay_MicroSeconds (delay);
	tStop = GetTime_MicroSeconds();
	printf ("Delay_MicroSeconds(%lluµs) took %lluµs\n", delay, tStop-tStart);

	// Output of the above.
	/*
	Input pin 22 is set to 0
	Input pin 22 with pull up is set to 1
	Input pin 22 with pull down is set to 0
	Cleared output pin 22 is set to 0
	Set output pin 22 is set to 1

	1000000 samples written in 362509µs - thus 2.758552MHz
	1000000 samples read in 412230µs - thus 2.425830MHz

	usleep(30µs) took 129µs
	Delay_MicroSeconds(30µs) took 41µs
	usleep(1000000µs) took 1000077µs
	Delay_MicroSeconds(1000000µs) took 1000004µs
	*/	
	
	// If mutex protection is bypassed (g_bcm2835Config.m_bUseMutexProtection = false;) reading and writing 
	// to the GPIO's further increases in speed.
	/*
	Input pin 22 is set to 0
	Input pin 22 with pull up is set to 1
	Input pin 22 with pull down is set to 0
	Cleared output pin 22 is set to 0
	Set output pin 22 is set to 1

	1000000 samples written in 129636µs - thus 7.713907MHz
	1000000 samples read in 193106µs - thus 5.178503MHz

	usleep(30µs) took 207µs
	Delay_MicroSeconds(30µs) took 39µs
	usleep(1000000µs) took 1000078µs
	Delay_MicroSeconds(1000000µs) took 1000003µs
	*/
	
	return 0;
}

