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

using namespace dccd;

/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
/**** Private function declarations ****/
/**** Public function definitions ****/
dccd::DccdApp::DccdApp(void)
{
	return;
}

dccd::DccdApp::~DccdApp(void)
{
	return;
}

void dccd::DccdApp::init(DccdHw* dccd_hw)
{
	this->hardware = dccd_hw;
	
	// Set config	
	this->user_force.init(dccd_hw);
	this->user_force.btn_repeat_time = 300;
	this->user_force.pot_mode = 0;
	this->user_force.btn_step = 10;
	this->user_force.cfg_debounce(10);
	
	this->mode_btn.init(dccd_hw);
	this->mode_btn.btn_repeat_time = 700;
	this->mode_btn.cfg_debounce(10);
	
	this->tps.init(dccd_hw);
	this->tps.treshold_on = 65;
	this->tps.treshold_off = 55;
	
	this->hbrake.init(dccd_hw);
	this->hbrake.latch_time = 1000;
	this->hbrake.cfg_debounce(100);
	
	this->brakes.init(dccd_hw);
	this->brakes.cfg_debounce(100);
	
	this->coil_reg.init(dccd_hw);
	this->coil_reg.lock_current = 4500;
	this->coil_reg.ref_resistance = 1500;
	this->coil_reg.cc_max_resistance = 2000;
	this->coil_reg.cc_min_resistance = 1000;
	this->coil_reg.target_force = 0;
	this->coil_reg.disable_protection = 1;
	this->coil_reg.cfg_set_cc_mode();
	
	this->dsp.init(dccd_hw);
	this->dsp.brigth_pwm = 100;
	this->dsp.dimm_pwm = 25;
	this->dsp.next_image = 0x01;
	this->dsp.next_lock_lvl = 0;
	this->dsp.next_lock_time = 0;
	
	// Set variables
	this->hbrake_force = -1;
	this->brakes_open_force = -1;
	this->brakes_lock_force = 100;
	this->tps_lock_force = 100;
	
	// Variable config
	this->tps_enabled = 1;
	if(this->user_force.pot_mode!=0) this->tps_enabled = 0;
	
	// Initialize state
	this->hardware->read();
	
	this->coil_reg.process();
	
	this->dsp.force_backlight(this->dsp.brigth_pwm);
	this->dsp.process();
	
	this->hardware->write();
}

void dccd::DccdApp::process(void)
{
	// Update all inputs
	this->hardware->read();
	
	// Process mode
	this->mode_btn.process();
	if(this->mode_btn.is_new)
	{
		this->brakes.cycle_mode();
		// Save to EEPROM
		this->mode_btn.is_new = 0;
	};
	
	// Process user force
	this->user_force.process();
	
	//if(this->user_force.is_new_btn_force)
	//{
	//	// Save to EEPROM
	//}
	
	// Process pedals
	this->tps.process();
	this->hbrake.process();
	this->brakes.process();
	
	// Calculate new output force
	this->coil_reg.target_force =  this->calc_next_force();
	
	// Process coil driver
	this->coil_reg.process();
	
	// Process display logic
	this->dsp_logic();

	// Execute outputs
	this->hardware->write();
}

int8_t dccd::DccdApp::calc_next_force(void)
{
	if(this->hbrake.is_active)
	{
		return this->hbrake_force;
	}
	else if(this->brakes.is_active)
	{
		switch(this->brakes.mode)
		{
			case Brakes::OPEN:
			return this->brakes_open_force;
			
			case Brakes::KEEP:
			return (int8_t)(this->user_force.force);
			
			case Brakes::LOCK:
			return this->brakes_lock_force;
			
			default:
			return 0;
		}
	}
	else if((this->tps_enabled)&&(this->tps.is_active()))
	{
		return this->tps_lock_force;
	}
	else
	{
		return (int8_t)(this->user_force.force);
	}
}

void dccd::DccdApp::dsp_logic(void)
{
	// Display image
	if(this->brakes.is_new_mode)
	{
		this->brakes.is_new_mode = 0;
		uint8_t bmode_image;
		switch(this->brakes.mode)
		{
			case Brakes::OPEN:
			bmode_image = 0x07;
			break;
				
			case Brakes::KEEP:
			bmode_image = 0x1E;
			break;
				
			case Brakes::LOCK:
			bmode_image = 0x38;
			break;
				
			default:
			bmode_image = 0x07;
			this->brakes.mode = Brakes::OPEN;
			break;
		}
		this->dsp.write(bmode_image, 3, 2000);
	}
	else if(this->user_force.is_new_btn_force)
	{
		this->user_force.is_new_btn_force = 0;
		this->dsp.write_percent(this->user_force.force, 0, 2, 1000);
	}
	else if(this->coil_reg.is_fault())
	{
		this->dsp.write(0x21, 1, 0);
	}
	else
	{
		this->dsp.write_percent(this->coil_reg.read_act_force(), 0, 0, 0);
	};
		
	// Process display
	this->dsp.process();
}

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