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

#include "hw/devices.h"

#include "logic/button_force.h"
#include "logic/dccd_force.h"

#include "logic/cfg_mem.h"

/**** Private definitions ****/
/**** Private constants ****/
static const uint16_t dsp_lock_bmode = 1000;
static const uint16_t dsp_lock_force = 50;

/**** Private variables ****/
static logic::CfgMemory cfg_mem = logic::CfgMemory();

static logic::ButtonForce button_force = logic::ButtonForce(&btn_up, &btn_down);
static logic::DccdForce dccd_force = logic::DccdForce(&btn_mode, &sw_hbrake, &sw_brakes);

/**** Private function declarations ****/
/**** Public function definitions ****/
int main(void)
{
	// HW setup
	devices_init();
	
	// Read saved config
	cfg_mem.init();
	
	#ifdef OVERRIDE_CONFIG
	cfg_mem.btn_force = 0;
	cfg_mem.bmode = 0;
	cfg_mem.pot_mode = 0;
	cfg_mem.dsp_brigth = 100;
	cfg_mem.dsp_dimm = 50;
	cfg_mem.brake_force = 100;
	cfg_mem.max_hbrake_time = 1000;
	cfg_mem.lock_current = 4200;
	cfg_mem.max_out_voltage = 6500;
	cfg_mem.min_out_voltage = 500;
	cfg_mem.save_all();
	#endif
	
	uint8_t user_force = 0;
	
	ccout.max_voltage = cfg_mem.max_out_voltage;
	ccout.min_voltage = cfg_mem.min_out_voltage;
	
	button_force.force = cfg_mem.btn_force;
	dccd_force.brake_mode = cfg_mem.bmode;
	dccd_force.max_hbrake_time = cfg_mem.max_hbrake_time;
	dccd_force.brake_force = cfg_mem.brake_force;
	
	// Super loop
	while(1)
	{				
		// Update inputs
		devices_update_inputs();
		
		// Update user setting
		button_force.update();
		
		// Select user force input
		if(cfg_mem.pot_mode) user_force = pot.percent;
		else user_force = button_force.force;
		
		// Calculate next target force
		dccd_force.update(user_force);
		
		// Override force in case of fault
		if((sup_fuse.fault)||(out_fuse.fault)) dccd_force.force = 0;
		
		// Convert force to current
		ccout.max_current = util::percent_of(dccd_force.force, cfg_mem.lock_current);
		
		// Execute outputs
		ccout.update();
		
		// Set display
		if(dccd_force.is_new_bmode)
		{
			uint8_t bmode_img = 0x03;
			switch(dccd_force.brake_mode)
			{
				case 1:
				bmode_img = 0x0C;
				break;
				
				case 2:
				bmode_img = 0x30;
				break;
				
				default:
				bmode_img = 0x03;
				break;
			}
			display.write(bmode_img);
			display.set_lock(dsp_lock_bmode);
			dccd_force.is_new_bmode = 0;
		}
		else if((button_force.is_new)&&(cfg_mem.pot_mode==0))
		{
			display.show_percent(dccd_force.force, hw::DisplayLed::LED_DSP_DOT10);
			display.set_lock(dsp_lock_force);
			button_force.is_new = 0;
		}
		else if(display.locked==0) display.show_percent(dccd_force.force, hw::DisplayLed::LED_DSP_DOT10);
		
		// Process dimm
		if(sw_dimm.state == hw::BUTTON_ON) display.set_brigthness(cfg_mem.dsp_dimm);
		else display.set_brigthness(cfg_mem.dsp_brigth);
		
		// Save user setting changes
		cfg_mem.btn_force = button_force.force;
		cfg_mem.bmode = dccd_force.brake_mode;
		cfg_mem.save();
		
		continue; // End of super loop
	}
	
	// Escape the matrix
	return 0;
}

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