/**** Includes ****/
#include "force.h"

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

/**** Private constants ****/
static const int8_t BUTTONS_STEP = 10;
static const uint16_t BUTTON_HOLD_TIME = 250;
static const uint16_t MODE_HOLD_TIME = 500;

/**** Private variables ****/
static inputMode_t input_mode = IM_BUTTONS;

static uint8_t user_force = 0;
static uint8_t new_user_force = 0;

static brakeMode_t brake_mode = BM_OPEN;
static uint8_t new_brake_mode = 1;

/**** Private function declarations ****/
static uint8_t SaturatedAdd(uint8_t base, int8_t delta);

/**** Public function definitions ****/
inputMode_t Force_GetInputMode(void)
{
	return input_mode;
}

void Force_CfgInputMode(inputMode_t in_mode)
{
	input_mode = in_mode;
}

void Force_SetUserForce(uint8_t force)
{
	if(force > 100) force = 100;
	user_force = force;
}

void Force_SetBrakeMode(uint8_t bmode)
{
	brake_mode = bmode;
}

uint8_t Force_Update(inputs_t* inputs, analog_t* meas)
{
	// Process user inputs
	if(input_mode==IM_POT)
	{
		// Process potentiometer
		if(meas->pot_voltage <= 500) user_force = 0;
		else if(meas->pot_voltage >= 4500 ) user_force = 100;
		else
		{
			uint16_t pot_u = meas->pot_voltage;
			pot_u /= 50;
			
			//Limit to 100
			if(pot_u > 100) user_force = 100;
			else if(pot_u < 10) user_force = 10;
			else user_force = (uint8_t)pot_u;
		}
	}
	else
	{
		// Process +/- timer
		if((inputs->down.is_active)&&(inputs->down.state_timer > BUTTON_HOLD_TIME))
		{
			inputs->down.is_new = 1;
			inputs->down.state_timer = 0;
		};
		
		if((inputs->up.is_active)&&(inputs->up.state_timer > BUTTON_HOLD_TIME))
		{
			inputs->up.is_new = 1;
			inputs->up.state_timer = 0;
		};
		
		// Process +/- logic
		if((inputs->down.is_new)&&(inputs->down.is_active))
		{
			user_force = SaturatedAdd(user_force, -1 * BUTTONS_STEP);
			new_user_force = 1;
		}
		else if((inputs->up.is_new)&&(inputs->up.is_active))
		{
			user_force = SaturatedAdd(user_force, BUTTONS_STEP);
			new_user_force = 1;
		};
		
		inputs->down.is_new = 0;
		inputs->up.is_new = 0;
	}
	
	// Process mode timer
	if((inputs->mode.is_active)&&(inputs->mode.state_timer > MODE_HOLD_TIME))
	{
		inputs->mode.is_new = 1;
		inputs->mode.state_timer = 0;
	};
	
	// Process mode logic
	if((inputs->mode.is_new)&&(inputs->mode.is_active))
	{
		//Cycle mode
		switch(brake_mode)
		{
			case BM_OPEN:
			brake_mode = BM_KEEP;
			break;
			
			case BM_KEEP:
			brake_mode = BM_LOCK;
			break;
			
			case BM_LOCK:
			brake_mode = BM_OPEN;
			break;
			
			default:
			brake_mode = BM_OPEN;
			break;
		}
		new_brake_mode = 1;
	};
	inputs->mode.is_new = 0;
	
	
	// Determine next target force from inputs
	if(inputs->handbrake.is_active)
	{
		return 0;
	}
	else if(inputs->brakes.is_active)
	{
		switch(brake_mode)
		{
			case BM_LOCK:
			return 100;
			
			case BM_KEEP:
			return user_force;
			
			default:
			return 0;
		}
	}
	else
	{
		return user_force;
	}
}

uint8_t Force_IsNewUserForce(void)
{
	return new_user_force;
}

void Force_ResetNewUserForce(void)
{
	new_user_force = 0;
}

brakeMode_t Force_GetBrakeMode(void)
{
	return brake_mode;
}

uint8_t Force_IsNewBrakeMode(void)
{
	return new_brake_mode;
}

void Force_ResetNewBrakeMode(void)
{
	new_brake_mode = 0;
}

/**** Private function definitions ****/
static uint8_t SaturatedAdd(uint8_t base, int8_t delta)
{
	int16_t temp = (int16_t)base + delta;
	if(temp < 0) return 0;
	else if(temp >= 100) return 100;
	else return (uint8_t)temp;
}