/**** Includes ****/
#include "hal/udccd_hal.h"
#include "inputs.h"

/**** Private definitions ****/

/**** Private variables ****/
static const uint8_t def_debounce_lim = 10;

/**** Private function declarations ****/
static void InitDefaultInput(inCh_t* inp);
static void ProcessInput(uint8_t read_lvl, inCh_t* inp);

/**** Public function definitions ****/
void Inputs_DefInit(inputs_t* inputs)
{
	InitDefaultInput(&inputs->handbrake);
	InitDefaultInput(&inputs->brakes);
	InitDefaultInput(&inputs->dimm);
	InitDefaultInput(&inputs->up);
	InitDefaultInput(&inputs->down);
	InitDefaultInput(&inputs->mode);
}

void Inputs_UpdateAll(inputs_t* inputs)
{
	// Chassis inputs
	ProcessInput(HAL_ReadLvl_Handbrake(), &inputs->handbrake);
	ProcessInput(HAL_ReadLvl_Brake(), &inputs->brakes);
	ProcessInput(HAL_ReadLvl_Dimm(), &inputs->dimm);

	// User inputs
	ProcessInput(HAL_ReadLvl_BtnUp(), &inputs->up);
	ProcessInput(HAL_ReadLvl_BtnDown(), &inputs->down);
	ProcessInput(HAL_ReadLvl_BtnMode(), &inputs->mode);
}

void Inputs_SetHanbrakePullUp(uint8_t on)
{
	if(on) HAL_SetPull_Handbrake(HIGH);
	else HAL_SetPull_Handbrake(HIZ);
}

/**** Private function definitions ****/
static void InitDefaultInput(inCh_t* inp)
{
	inp->is_active = 0;
	inp->is_new = 0;
	inp->state_timer = 0;
	inp->cfg.act_level = LOW;
	inp->cfg.dbnc_treshold = def_debounce_lim;
	inp->proc.level = LOW;
	inp->proc.dbnc_counter = 0;
}

static void ProcessInput(uint8_t read_lvl, inCh_t* inp)
{
	if(inp->state_timer < 0xFFFF) inp->state_timer++;
	
	if(read_lvl != inp->proc.level)
	{
		//Debounce ongoing
		inp->proc.dbnc_counter++;
		if(inp->proc.dbnc_counter < inp->cfg.dbnc_treshold) return;
		
		//Save level
		inp->proc.level = read_lvl;
		//Change state
		if(inp->proc.level == inp->cfg.act_level) inp->is_active = 1;
		else inp->is_active = 0;
		// Update new flag
		inp->is_new = 1;
		// Reset state timer
		inp->state_timer = 0;
		return;
	}
	else
	{
		//Debounce failed
		if(inp->proc.dbnc_counter) inp->proc.dbnc_counter = 0;
	}
}