/**** Includes ****/
#include <stdint.h>

#include "devices/config.h"
#include "devices/analog.h"
#include "devices/inputs.h"
#include "devices/memory.h"

#include "drivers/output.h"
#include "drivers/display.h"

#include "logic/force.h"
#include "logic/coil.h"
#include "logic/settings.h"

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

/**** Private constants ****/

/**** Private variables ****/
inputs_t inputs;
analog_t analog;

/**** Private function declarations ****/
void Setup(void);
void GatherData(uint8_t times);

/**** Public function definitions ****/
int main(void)
{
	Setup();
	
    while(1)
    {
		// Read all inputs
		GatherData(1);
		
		// Process force logic
		uint8_t force = Force_Update(&inputs, &analog);
		
		// Save values
		uint8_t new_bmode = Force_IsNewBrakeMode();
		uint8_t new_force = Force_IsNewUserForce();
		if(Force_GetInputMode()==IM_POT) new_force = 0;
		Setings_Update(new_force, force, new_bmode, Force_GetBrakeMode());
		
		// Set coil current target
		Coil_SetTarget_Force(force);
		
		// Calculate next target voltage
		uint16_t next_u = Coil_Update(&analog);
		
		// Set next output voltage
		Output_SetTarget(next_u);
		
		// Update output
		Output_Update(&analog);
		
		
		// Display logic
		if(Output_GetOutputState()==O_FAULTED)
		{
			// Show fault code
			Display_SetLock(0);
			Display_SetImage(0x33);
		}
		else if(Force_IsNewBrakeMode())
		{
			Display_SetLock(0);			
			switch(Force_GetBrakeMode())
			{
				case BM_LOCK:
				Display_SetImage(0x38);
				break;
				
				case BM_KEEP:
				Display_SetImage(0x1E);
				break;
				
				default:
				Display_SetImage(0x07);
				break;
			}
			Display_SetLock(1000);
		}
		else
		{
			Display_SetPercent(force, DOT10);
		}

		// Update display
		Display_Update(&inputs);
		
		// Reset new flags
		Force_ResetNewBrakeMode();
		Force_ResetNewUserForce();
    }
	
	return 0;
}

/**** Private function definitions ****/
void Setup(void)
{
	// Initialize low level hardware
	Init_HW();
	
	// Prime EEPROM
	//Setings_SaveDefault();
	
	// Configure inputs
	Inputs_DefInit(&inputs);
	
	inputs.handbrake.cfg.act_level = HIGH;
	inputs.handbrake.cfg.dbnc_treshold = 10;
	
	inputs.brakes.cfg.act_level = LOW;
	inputs.brakes.cfg.dbnc_treshold = 20;
	
	inputs.dimm.cfg.act_level = LOW;
	inputs.dimm.cfg.dbnc_treshold = 20;
	
	inputs.up.cfg.act_level = LOW;
	inputs.up.cfg.dbnc_treshold = 20;
	
	inputs.down.cfg.act_level = LOW;
	inputs.down.cfg.dbnc_treshold = 20;
	
	inputs.mode.cfg.act_level = LOW;
	inputs.mode.cfg.dbnc_treshold = 20;
	
	Inputs_SetHanbrakePullUp(0);
	
	// Configure display
	Display_CfgBacklight(100,50);
	// Show startup display
	Display_SetImage(0xFF);
	Display_Update(&inputs);
	
	// Configure force logic
	Force_CfgInputMode(IM_POT); //IM_BUTTONS IM_POT
	
	// Restore saved force
	Force_SetUserForce(Setings_GetForce());
	
	// Restore saved brake mode
	Force_SetBrakeMode(Setings_GetBrakeMode());

	// Prime analog channels
	GatherData(100);
	
	// Show default display
	Display_SetImage(0x01);
	Display_Update(&inputs);
	
	// Enable output
	Output_Enable();
	Output_SetTarget(0);
}

void GatherData(uint8_t times)
{
	do
	{
		Analog_UpdateAll(&analog);
		Inputs_UpdateAll(&inputs);
		if(times) times--;
	}
	while(times);
}