/**********************************************************************
 * NAME:        failsafe_toggles.c                                     *
 * AUTHOR:      Jonas Holmberg                                         *
 * PURPOSE:     Give the ability to create boolean flags and failsafe  *
 * 				variables.                                	    	   *
 * INFORMATION: Create any boolean variable that should be able to be  *
 * 				used throughout the system. These can be boolean vales *
 * 				for standard checks, failsafes etc. These can also be  *
 * 				linked to a RC channel and value range wherein the     *
 * 				value will be set to one.							   *
 *	                                                                   *
 * GLOBAL VARIABLES:                                                   *
 * Variable		  Type		     Description                           *
 * --------		  ----		     -----------                           *
 * systemFlags	  boolFlags_t    Represents a bitfield of all the      *
 * 					 			 boolean values in the system.	       *
 * flagConfigArr  flags_Configuration_t Configuration values for any   *
 * 									    flag that should be changeable *
 * 									    from the RC controller.		   *
 **********************************************************************/

#include "drivers/failsafe_toggles.h"
#include "drivers/sbus.h"

/* Stuct containing values for all the flags and failsafe booleans sin the system */
boolFlags_t systemFlags = {{0}};

/* Array of flag configurations. These are values that can be set by RC, should be read from eeprom. */
flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
		[FLAG_CONFIGURATION_ARM] = {
			.minRange = 1800,
			.maxRange = 2100,
			.channelNumber = 8,
			.flagId = systemFlags_armed_id,
		},
		[FLAG_CONFIGURATION_FLIGHTMODE_ACCELEROMETER] = {
			.minRange = 1400,
			.maxRange = 2100,
			.channelNumber = 6,
			.flagId = systemFlags_flightmode_acceleromter_id,
		},
		[FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER] = {
			.minRange = 0,
			.maxRange = 0,
			.channelNumber = 0,
			.flagId = systemFlags_flightmode_barometer_id,
		},
		[FLAG_CONFIGURATION_FLIGHTMODE_COMPASS] = {
			.minRange = 0,
			.maxRange = 0,
			.channelNumber = 0,
			.flagId = systemFlags_flightmode_compass_id,
		},
		[FLAG_CONFIGURATION_FLIGHTMODE_GPS] = {
			.minRange = 0,
			.maxRange = 0,
			.channelNumber = 0,
			.flagId = systemFlags_flightmode_gps_id,
		},
		[FLAG_CONFIGURATION_MIXERFULLSCALE] = {
			.minRange = 1900,
			.maxRange = 2100,
			.channelNumber = 5,
			.flagId = systemFlags_mixerfullscale_id,
		},
		[FLAG_CONFIGURATION_MIXERLOWSCALE] = {
			.minRange = 900,
			.maxRange = 1100,
			.channelNumber = 5,
			.flagId = systemFlags_mixerlowscale_id,
		},
		[FLAG_CONFIGURATION_FLIGHTMODE_3] = {
			.minRange = 0,
			.maxRange = 0,
			.channelNumber = 0,
			.flagId = systemFlags_flightMode_3_id,
		},

		/*Stick controls*/
		[FLAG_CONFIGURATION_THROTTLEMAX] = {
			.minRange = 1950,
			.maxRange = 2000,
			.channelNumber = 3,
			.flagId = systemFlags_throttleMax_id,
		},
		[FLAG_CONFIGURATION_STICKLEFT] = { //negative
			.minRange = 1000,
			.maxRange = 1100,
			.channelNumber = 1,
			.flagId = systemFlags_stickLeft_id,
		},
		[FLAG_CONFIGURATION_STICKRIGHT] = {	//positive
			.minRange = 1900,
			.maxRange = 2000,
			.channelNumber = 1,
			.flagId = systemFlags_stickRight_id,
		},
		[FLAG_CONFIGURATION_STICKUP] = { //negative
			.minRange = 1000,
			.maxRange = 1100,
			.channelNumber = 2,
			.flagId = systemFlags_stickUp_id,
		},
		[FLAG_CONFIGURATION_STICKDOWN] = { //positive
			.minRange = 1900,
			.maxRange = 2000,
			.channelNumber = 2,
			.flagId = systemFlags_stickDown_id,
		},
		[FLAG_CONFIGURATION_STICKCENTERH] = {
			.minRange = 1400,
			.maxRange = 1600,
			.channelNumber = 1,
			.flagId = systemFlags_stickCenterH_id,
		},
		[FLAG_CONFIGURATION_STICKCENTERV] = {
			.minRange = 1400,
			.maxRange = 1600,
			.channelNumber = 2,
			.flagId = systemFlags_stickCenterV_id,
		},
		[FLAG_CONFIGURATION_THROTTLELEFT] = {
			.minRange = 2000,
			.maxRange = 1900,
			.channelNumber = 4,
			.flagId = systemFlags_throttleLeft_id,
		}


};

/***********************************************************************
* BRIEF:	   Could be used to set start values for some values
* INFORMATION: Possible to set the values for any of the failsafes
***********************************************************************/
void initFlags()
{
	//Could be used to set start values for failsafes.
}

/***********************************************************************
* BRIEF:       Process RC channels that should operate on some flags
* INFORMATION: Reads the value of the RC channels that and checks if
* 			   any of the channels should handle some flag value in the
* 			   system. If it should it will update its state.
***********************************************************************/
void flags_ProcessRcChannel(int rcChannel_ID, int value)
{
	for (int i = 0; i < FLAG_CONFIGURATION_COUNT; i++)
	{
		if (flagConfigArr[i].channelNumber == rcChannel_ID)
		{
			if(value >= flagConfigArr[i].minRange && value <= flagConfigArr[i].maxRange)
			{
				flags_Set_ID(flagConfigArr[i].flagId);
			}
			else
			{
				flags_Clear_ID(flagConfigArr[i].flagId);
			}
		}
	}
}

/***********************************************************************
* BRIEF:       Process RC channels that should operate on some flags
* INFORMATION: Reads the value of the RC channels that and checks if
* 			   any of the channels should handle some flag value in the
* 			   system. If it should it will update its state.
***********************************************************************/
void flags_ProcessRcChannel_Improved(uint8_t minChannel, uint8_t maxChannel)
{
	//Loop through all the active flag configuration values
	for (int i = 0; i < FLAG_CONFIGURATION_COUNT; i++)
	{
		int currentChannelNumb = flagConfigArr[i].channelNumber;
		//Check if the current Flag channel is within the set bounds
		if(currentChannelNumb >= minChannel && currentChannelNumb <= maxChannel)
		{
			//Get the value for the channel that the current flag is linked to
			int value = getChannelValue(sbusChannelData, currentChannelNumb);

			if(value >= flagConfigArr[i].minRange && value <= flagConfigArr[i].maxRange)
			{
				flags_Set_ID(flagConfigArr[i].flagId);
			}
			else
			{
				flags_Clear_ID(flagConfigArr[i].flagId);
			}
		}
		else
		{
			continue;
		}
	}
}

/***********************************************************************
* BRIEF:       Set flag to true(1)
* INFORMATION: Given a mask set that value to true
***********************************************************************/
void flags_Set_MASK(int mask)
{
	systemFlags.intRepresentation |= mask;
}

/***********************************************************************
* BRIEF:       Set flag to false(0)
* INFORMATION: Given a mask set that value to false
***********************************************************************/
void flags_Clear_MASK(int mask)
{
	systemFlags.intRepresentation &= ~(mask);
}

/***********************************************************************
* BRIEF:       Toggle a flag
* INFORMATION: Given a mask changes the current value of a flag to its
* 			   opposite.
***********************************************************************/
void flags_Toggle_MASK(int mask)
{
	systemFlags.intRepresentation ^= mask;
}

/***********************************************************************
* BRIEF:	   Checks if a flag is set
* INFORMATION: Given a mask value, check if that is set to true in the
* 			   bitfield
***********************************************************************/
bool flags_IsSet_MASK(int mask)
{
	return !( (systemFlags.intRepresentation ^ mask) & mask );
}

/***********************************************************************
* BRIEF:       Set flag to true(1)
* INFORMATION: Given an id set that value to true
***********************************************************************/
void flags_Set_ID(int id)
{
	flags_Set_MASK(getFlagMaskValue(id));
}

/***********************************************************************
* BRIEF:       Set flag to false(0)
* INFORMATION: Given an id set that value to false
***********************************************************************/
void flags_Clear_ID(int id)
{
	flags_Clear_MASK(getFlagMaskValue(id));
}

/***********************************************************************
* BRIEF:       Toggle a flag
* INFORMATION: Given an id changes the current value of a flag to its
* 			   opposite.
***********************************************************************/
void flags_Toggle_ID(int id)
{
	flags_Toggle_MASK(getFlagMaskValue(id));
}

/***********************************************************************
* BRIEF:	   Checks if a flag is set
* INFORMATION: Given an id value, check if that is set to true in the
* 			   bitfield
***********************************************************************/
bool flags_IsSet_ID(int id)
{
	return flags_IsSet_MASK(getFlagMaskValue(id));
}