/**** Includes ****/
#include "../utils/utils.h"
#include "dccd_force.h"

using namespace logic;

/**** Private definitions ****/
/**** Private constants ****/
static const uint16_t def_max_hbrake_time = 1000;
static const uint16_t def_brake_force = 100;

/**** Private variables ****/
/**** Private function declarations ****/
/**** Public function definitions ****/
logic::DccdForce::DccdForce(hw::Button* btn_mode, hw::Button* sw_hbrake, hw::Button* sw_brakes)
{
	this->mode = btn_mode;
	this->handbrake = sw_hbrake;
	this->brakes = sw_brakes;
	
	this->is_new = 0;
	this->force = 0;
	this->brake_mode = 0;
	
	this->max_hbrake_time = def_max_hbrake_time;
	this->brake_force = def_brake_force;
}

logic::DccdForce::~DccdForce(void)
{
	return;
}

uint8_t logic::DccdForce::update(uint8_t user_force)
{
	// Process mode button
	if((this->mode->is_new)&&(this->mode->state == hw::BUTTON_ON))
	{
		// Cycle brake mode
		switch(this->brake_mode)
		{
			case 0:
			this->brake_mode = 1;
			
			case 1:
			this->brake_mode = 2;
			
			default:
			this->brake_mode = 0;
		}
		
		this->mode->is_new = 0;
		this->is_new_bmode = 1;
	};
	
	// Determine target force source
	uint8_t next_force = user_force;
	if((this->handbrake->state == hw::BUTTON_ON)&&((this->handbrake->time < this->max_hbrake_time)||(this->max_hbrake_time == 0)))
	{
		next_force = 0;
	}
	else if(this->brakes->state == hw::BUTTON_ON)
	{
		switch(this->brake_mode)
		{
			case 0:
			next_force = 0;
			
			case 1:
			next_force = user_force;
			
			default:
			next_force = this->brake_force;
		}
	};
	
	if(next_force != this->force) this->is_new = 1;
	
	this->force = next_force;
	
	return  this->force;
}

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