/*
Compile and link:
	g++ TemController.cpp CSC5262.cpp -L../ -lhubo -lpthread -lrt -o TemController
Run:
	sudo ./TemController
Purpose:
	The demo uses a Raspberry Pi to illustrate:
	- a 2-way controller that reads a MCP9701 temperature on analog input channel 0 and switches RCSocket (31/1) and relay 7
	  when the temperature is out of limits TEMP_ON and TEMP_OFF.
	- a 2-way controller that reads the darkness of a foto resistor on analog input channel 1 and switches relay 6 
	  when the the digits are out of limits LIGHT_ON and LIGHT_OFF.
*/

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

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

using namespace std;
using namespace HuboLib;
using namespace BCM2835;


// Global initialization.
bool InitializeApplication		();

// Initialize Hubo hardware.
bool InitializeHuboHardware		();

// Helper for temporary priority changes.
bool SaveThreadPriority			();
void BoostThreadPriority		();
void RestoreThreadPriority		();
sched_param 					g_param;
int								g_policy 		= 0;
pthread_t						g_threadHandle 	= 0;

// RCSocket to control remote switches.
#define FAMILY_CODE 			(31)
#define TEMP_CONTROLLER_SOCKET 	(1)
#define REED_CONTACT_SOCKET 	(2)
CSC5262 rcSwitch				(350, 17, true);

// Temperature control
// Sensor: 		MCP9701(A) on analog input 0
// Actuator:	RCSocket 31/1, Relay 7
void TemperatureControl			();
bool GetTemperature 			(int channel, double& temperature);
#define TEMP_ON					(27.0)
#define TEMP_OFF				(29.0)
#define TEMP_SENSOR_AI_CHANNEL  (0)
#define TEMP_CONTROLLER_RELAY   (7)

// Light control 
// Sensor: 		foto resistor on voltage devider (of 5V) on analog input 1
// Actuator:	Relay 6
void BrightnessControl			();
bool GetDarkness	 			(int channel, unsigned short& brightness);
#define LIGHT_OFF				(2000)
#define LIGHT_ON				(2500)
#define LIGHT_SENSOR_AI_CHANNEL (1)
#define LIGHT_CONTROLLER_RELAY  (6)


int main(int argc, char *argv[])
{
	// Initialization.
	if (!InitializeApplication())
	{
		printf ("Failed to initialize. Are you running the program as sudoer?\n");
		return -1;
	}		

	while (1)
	{
		// Update brightsness controller.
		BrightnessControl();

		// Update temperature controller.
		TemperatureControl();

		// Sleep some time.
		Delay_MicroSeconds(1000L*1000L);
	}
	
	// Even if we should never get here...
	Uninitialize();
	
	return 0;
}

void BrightnessControl ()
{
	unsigned short darkness = 0;
	if (!GetDarkness (LIGHT_SENSOR_AI_CHANNEL, darkness))
		return;
	
	// printf ("Darkness = %d digit.\n", darkness);

	if (darkness > LIGHT_ON)
	{
		// Turn relay on.
		Set_DO_Channel(LIGHT_CONTROLLER_RELAY, true);
	}
	else
		if (darkness < LIGHT_OFF)
		{
			// Turn relay off.
			Set_DO_Channel(LIGHT_CONTROLLER_RELAY, false);
		}
		else 
			printf ("-> no change.\n");
}

bool GetDarkness (int channel, unsigned short& darkness)
{
	unsigned long 	adcCount; 
	double	 		volt;
	
	if (!Get_AI_Channel (channel, adcCount, volt))
		return false;
	
	darkness = (unsigned short) adcCount;

	return true;
}

void TemperatureControl ()
{
	double temperature = 0.0;
	if (!GetTemperature (TEMP_SENSOR_AI_CHANNEL, temperature))
		return;
	
	printf ("Temperature = %lf deg. C ", temperature);

	BoostThreadPriority ();

	if (temperature < TEMP_ON)
	{
		// Turn heating rc switch on.
		printf ("-> heating on.\n");
		rcSwitch.SwitchSocket(FAMILY_CODE, TEMP_CONTROLLER_SOCKET, true, 4, 10);
		
		// Turn relay on.
		Set_DO_Channel(TEMP_CONTROLLER_RELAY, true);
	}
	else
		if (temperature > TEMP_OFF)
		{
			// Turn heating rc switch off.
			printf ("-> heating off.\n");
			rcSwitch.SwitchSocket(FAMILY_CODE, TEMP_CONTROLLER_SOCKET, false, 4, 10);		

			// Turn relay off.
			Set_DO_Channel(TEMP_CONTROLLER_RELAY, false);
		}
		else 
			printf ("-> no change.\n");

	RestoreThreadPriority ();
}

bool GetTemperature (int channel, double& temperature)
{
	unsigned long 	adcCount; 
	double	 		volt;
	
	if (!Get_AI_Channel (channel, adcCount, volt))
		return false;
	
	temperature = Get_MCP9701_Temperature(volt);

	return true;
}

bool InitializeApplication()
{
	// Try reading thread scheduling information.
	if (!SaveThreadPriority())
	{
		printf ("Failed to save scheduling parameter. Are you running the program as sudoer?\n");
		return false;
	}	

	// If this call doesn't return true then we must not use the GPIO part of Hubo library!
	printf ("Initializing GPIO.\n");
	if (!IsGPIOInitialized ())
	{
		printf ("GPIO not properly initialized. Are you running the program as sudoer?\n");
		return false;
	}
	
	// Initialize Hubo hardware and expect at least one slave board installed.
	if (!InitializeHuboHardware ())
	{
		printf ("Hubo hardware containing at least one cascaded slave could not be found.\n");
		return false;
	}

	return true;
}

bool InitializeHuboHardware()
{
	// 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

	printf ("Initializing Hubo hardware.\n");

	// Initialize the library once in your program.
	Initialize();
	Set_Cycle_Time(20);

	// Wait until all background buffers are filled.

	// Define all channels to be read (set to 1) from the ADC.
	unsigned short overSampling[MAX_MCP3x08_CHANNELS] = { 1, 1, 1, 1, 1, 1, 1, 1 };
	Set_MCP3x08_Oversampling (overSampling);
	
	// Wait as long until we have valid values in the buffer.
	printf ("Waiting for MCP3x08 buffered values... ");
	Wait_For_MCP3x08_Buffered_Values();
	printf ("received.\n");

	printf ("Waiting for MCP23017 buffered values... ");
	Wait_For_MCP23017_Buffered_Values ();
	printf ("received.\n");

	return true;
}

bool SaveThreadPriority()
{
	printf ("Saving thread parameters.\n");

	// Thread handle.
	g_threadHandle = pthread_self();

	// Save current scheduling parameters of thread.
	int ret	= pthread_getschedparam (g_threadHandle, &g_policy, &g_param);
	
	assert (ret == 0);
	return ret == 0;
}

void BoostThreadPriority()
{
	// Scheduling params.
	sched_param param = g_param;

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

void RestoreThreadPriority()
{
	// Restore scheduling parameters of thread.
	int ret	= pthread_setschedparam(g_threadHandle, g_policy, &g_param);
	assert (ret == 0);
}
