/*
Compile and link:
	g++ JamDemo.cpp CSC5262.cpp -L../ -lhubo -lpthread -lrt -o JamDemo
Run:
	sudo ./JamDemo
Purpose:
	The demo uses a Raspberry Pi Zero and 2 cascaded Hubos to illustrate:
    - a PIR sensor on the master Hubo (DI 7) counting pulses that increment the digital outputs on the first Hubo slave, 
    - a reed contact closure on the master Hubo (DI 6) that switches the open collector output (DO 1) and 
	- a 2-way controller that reads a DS18x20 temperature and switches RCSocket (31/1) 
	  when the temperature is out of limits TEMP_ON and TEMP_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		();

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

// Initialize 1wire devices and search for at least one 1wire temperature sensor.
bool Initialize1WireDevices 	();
vector <string> 				g_1WireDeviceList;
int								g_FirstDS18x20Sensor = -1;

// Initialize Hubo hardware and at least one slave module that is cascaded to it.
bool InitializeHuboHardware		();
vector<int> 					g_SlaveAddressList;

// Update temperature controller 
// Sensor: 		first 1wire temperature sensor
// Actuator:	RCSocket 31/1
void TemperatureControl			();
bool GetTemperature 			(double& temperature);
#define TEMP_ON					(25.0)
#define TEMP_OFF				(27.0)

// PIR counter 
void PIRCounter();
unsigned short 					g_Count 		= 0;
#define PIR_DI_CHANNEL 			(7) // channel 7 is connected to the PIR.
#define SLAVE_INDEX		 		(0) // We use the first slave (index 0) found.

// Reed contact switching motor
void HandleReedContact			();
#define REED_DI_CHANNEL			(6) // channel 6 is connected to the reed contact closure.
#define MOTOR_DO_CHANNEL		(1) // the motor is connected on (open collector) output channel 1


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)
	{
		// Turn motor on output channel 1 on and of depending on reed contact on input channel 6.
		HandleReedContact();
		
		// Check PIR-Counter for new event.
		PIRCounter();
		
		// Update temperature controller.
		TemperatureControl();

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

void HandleReedContact ()
{
	bool 		bReedContactOpen 	 = true;
	static bool bLastReedContactOpen = true;
	
	if (!Get_DI_Channel(REED_DI_CHANNEL, bReedContactOpen))
	{
		printf ("Failed to retrieve DI channel.\n");
		return;
	}
	Set_DO_Channel (MOTOR_DO_CHANNEL, bReedContactOpen ? 0 : 1);
	if (bLastReedContactOpen != bReedContactOpen)
		printf ("Set DO channel %d to %s.\n", MOTOR_DO_CHANNEL, bReedContactOpen ? "0" : "1");
	
	bLastReedContactOpen = bReedContactOpen;
}

void PIRCounter()
{
	static bool 			lastPirState 	= false;
	bool 					newPirState		= false;
	
	if (!Get_DI_Channel(PIR_DI_CHANNEL, newPirState))
	{
		printf ("Failed to retrieve digital input.\n");
		return;
	}
	if (lastPirState== false && newPirState==true)
	{
		g_Count++;
		printf ("PIR signal %d caught.\n", g_Count);
		Set_Slave_DO_Channels (SLAVE_INDEX, (unsigned char) g_Count);
	}
	lastPirState = newPirState;
}

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

	BoostThreadPriority ();

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

	RestoreThreadPriority ();
}

bool GetTemperature (double& temperature)
{
	// Did we find a sensor?
	if (g_FirstDS18x20Sensor == -1)
		return false;

	// Double check whether the list has changed in the meantime.
	if (!Is_DS18x20_Devices(g_1WireDeviceList[g_FirstDS18x20Sensor].c_str()))
		return false;

	// Could we retrieve a temperature value?
	bool 	bCRC			= false;
	long 	t_duration_ms   = 0;
	if (!Get_DS18x20_Temperature (g_1WireDeviceList[g_FirstDS18x20Sensor].c_str(), temperature, bCRC, t_duration_ms))
		return false;

	// Correct CRC?
	if (!bCRC) 
		return false;
	
	// 85°C represent the reset value - thus we ignore it.
	if (temperature == 85.0)
		return false;	

	// The duration for an update should be less than one second typically.
	if (t_duration_ms > 2000)
		return false;	
	
	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 1-wire device list.
	if (!Initialize1WireDevices ())
	{
		printf ("A 1wire sensor could not be found.\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 and detecting slaves.\n");

	// Initialize the library once in your program.
	Initialize();
	Set_Cycle_Time(20);
	
	// We need to find one slave as we'd use the first slave for outputting the PIR signals.
	if (GetSlaveDeviceList (g_SlaveAddressList))
	{
		for (unsigned int i=0; i<g_SlaveAddressList.size(); i++)
			printf ("Found slave address 0x%02X.\n", g_SlaveAddressList[i]);
	}
	else
		return false;

	// 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");
	
	printf ("Waiting for MCP23017 (slaves) buffered values... ");
	Wait_For_MCP23017_Slaves_Buffered_Values ();
	printf ("received.\n");
	
	return true;
}
    
bool Initialize1WireDevices ()
{
	printf ("Initializing 1wire devices.\n");
	
	Get_1w_Devices(g_1WireDeviceList);

	// We should find the bus master and at least 1 temperature sensor.
	if (g_1WireDeviceList.size() < 2)
		return false;

	double 	temperature;
	bool 	bCRC;
	long 	t_duration_ms;

	// For each device in the list...
	for (int i=0; i<g_1WireDeviceList.size(); i++)
	{
		// ... print the device ID...
		printf ("%02d. %s:  ", i+1, g_1WireDeviceList[i].c_str());

		// ... and if it's a DS18x20 temperature sensor...
		if (Is_DS18x20_Devices(g_1WireDeviceList[i].c_str()))
		{
			// ... retrieve the data.
			bool bResult = Get_DS18x20_Temperature (g_1WireDeviceList[i].c_str(), temperature, bCRC, t_duration_ms);
			printf ("Result=%s ", bResult?"true":"false");
			printf ("bCRC=%s ", bCRC?"true":"false");
			printf ("temp=%lf duration=%ld index=%d\n", temperature, t_duration_ms, i);
			g_FirstDS18x20Sensor = i;
			return true;
		}
		else
			printf ("\n");
	}

	return false;
}

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

bool strToUChar(char* str, unsigned char& value)
{
	unsigned long v;
	if (sscanf(str, "%lu", &v) != 1)
		return false;
	value = (unsigned char)v;
	if (value != (unsigned long)v)
		return false;
	return true;
}

