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

/**** Private definitions ****/
typedef struct {
	uint8_t active;
	uint8_t time;
} warn_t;

typedef struct {
	warn_t supply_voltage;
	warn_t supply_current;
	warn_t supply_power;
	warn_t output_voltage;
	warn_t output_current;
	warn_t output_power;
	warn_t output_open;
	warn_t output_short;
	warn_t output_mismatch;
} warnings_t;

typedef struct {
	uint8_t supply_uvlo;
	uint8_t supply_ovp;
	uint8_t supply_ocp;
	uint8_t supply_opp;
	uint8_t output_ovp;
	uint8_t output_ocp;
	uint8_t output_opp;
	uint8_t output_open;
	uint8_t output_short;
} faults_t;

/**** Private constants ****/
static const uint16_t LIM_SUPPLY_UVLO =  8000; //mV
static const uint16_t LIM_SUPPLY_OVP  = 18000; //mV
static const uint16_t LIM_SUPPLY_OCP  =  7000; //mA
static const uint16_t LIM_SUPPLY_OPP  = 40000; //mW

static const uint16_t LIM_OUTPUT_OVP = 12000;  //mV
static const uint16_t LIM_OUTPUT_OCP =  7000;  //mA
static const uint16_t LIM_OUTPUT_OPP = 40000;  //mW

static const uint16_t LIM_OUTPUT_OPEN = 40000; //mR
static const uint16_t LIM_OUTPUT_SHORT =  750; //mR

static const uint16_t LIM_OUTPUT_MATCH = 100; //mV

static const uint8_t OCP_WARNING_LIMIT   = 10; //cycles
static const uint8_t SHORT_WARNING_LIMIT = 50; //cycles

static const uint16_t MAX_OUTPUT_VOLTAGE = 10000; //mV
static const uint16_t MIN_OUTPUT_VOLTAGE  =  100; //mV

static const uint16_t COOLDOWN_TIME = 5000;

/**** Private variables ****/
static warnings_t warnings;
static faults_t faults;

static uint16_t target_output = 0;
static uint16_t adj_target = 0;

static int32_t hb_volt_sum = 0;

static uint8_t new_target = 1;

static uint8_t steady_state = 0;

static faultState_t fault_state = F_NONE;

static uint16_t cooldown_timer = 0;

static outState_t out_state = O_OFF;

/**** Private function declarations ****/
static void Process_Warnings(analog_t* meas);
static uint8_t Process_Faults(void);

static void ProcessWarningTime(warn_t* w);
static uint8_t isAnyFaultActive(faults_t* f);
static uint8_t isFaultWarningActive(warnings_t* w, faults_t* f);
static void ResetFaults(faults_t* f);

/**** Public function definitions ****/
void Output_Enable(void)
{
	ResetFaults(&faults);
	target_output = 0;
	out_state = O_ACTIVE;
	HB_SetTarget(0) ;
	HB_SetLowSide(1);
	HB_Enable();
}

void Output_Update(analog_t* meas)
{	
	Process_Warnings(meas);
	
	// Convert Warnings to Faults
	uint8_t active_fault = Process_Faults();
	
	/// Ignore faults
	active_fault = 0;
	
	// Determine coil state	
	switch(out_state)
	{
		case O_ACTIVE:
		if(active_fault)
		{
			// Disable output
			HB_Disable();
			out_state = O_FAULTED;
			break;
		};
		
		//Do target adjustment logic
		if(steady_state >= 10)
		{
			//Calculate average HB voltage
			hb_volt_sum /= 11;
			
			// Calculate feedback adjusted HB output
			int32_t error = hb_volt_sum - (int32_t)target_output;
			int32_t temp = (int32_t)adj_target - error;
			// Limit to 16bits
			if(temp<=0) adj_target = 0;
			else if(temp >= 0x0000FFFF) adj_target = 0xFFFF;
			else adj_target = (uint16_t)temp;
			steady_state = 0;
			hb_volt_sum = 0;
		}
		else
		{
			hb_volt_sum += meas->hb_voltage;
			steady_state++;
		}
		
		// Closed loop or open loop target set
		if(new_target)
		{
			// Set open-loop HB output
			HB_SetTarget(target_output);
			adj_target = target_output;
			steady_state = 0;
			new_target = 0;
			hb_volt_sum = 0;
		}
		else
		{
			HB_SetTarget(adj_target);
		}

		// Update output
		HB_UpdateOutput(meas->supply_voltage);
		break;
		
		case O_FAULTED:
		if(!active_fault)
		{
			//Return to normal state
			HB_Enable();
			out_state = O_ACTIVE;
		}
		break;
		
		default:  //OFF
		if(HB_IsEnabled()) HB_Disable();
		break;
	}
}

void Output_SetTarget(uint16_t voltage)
{
	if(voltage > MAX_OUTPUT_VOLTAGE) voltage = MAX_OUTPUT_VOLTAGE;
	else if((voltage > 0)&&(voltage < MIN_OUTPUT_VOLTAGE)) voltage = MIN_OUTPUT_VOLTAGE;
	
	if(voltage != target_output) new_target = 1;
	target_output = voltage;
}

outState_t Output_GetOutputState(void)
{
	return out_state;
}

/**** Private function definitions ****/
static void Process_Warnings(analog_t* meas)
{
	// Supply UVLO and OVP
	if((meas->supply_voltage > LIM_SUPPLY_OVP)||(meas->supply_voltage < LIM_SUPPLY_UVLO)) warnings.supply_voltage.active = 1;
	else warnings.supply_voltage.active = 0;
	
	// Supply OCP
	if(meas->supply_current > LIM_SUPPLY_OCP) warnings.supply_current.active = 1;
	else warnings.supply_current.active = 0;
	
	// Supply OPP
	if(meas->supply_power > LIM_SUPPLY_OPP) warnings.supply_power.active = 1;
	else warnings.supply_power.active = 0;
	
	// Halfbridge output conditions
	// Output Target mismatch
	if(HB_IsOutputMatch(meas->hb_voltage, LIM_OUTPUT_MATCH) == 0) warnings.output_mismatch.active = 1;
	else warnings.output_mismatch.active = 0;
	
	// Output OCP
	if((HB_IsLowOn())&&(meas->hb_currnet > LIM_OUTPUT_OCP)) warnings.output_current.active = 1;
	else warnings.output_current.active = 0;
	
	// Output OVP
	if((HB_IsLowOn())&&(meas->hb_voltage > LIM_OUTPUT_OVP)) warnings.output_voltage.active = 1;
	else warnings.output_voltage.active = 0;
	
	// Output OPP
	if((HB_IsEnabled())&&(meas->hb_power > LIM_OUTPUT_OPP)) warnings.output_power.active = 1;
	else warnings.output_power.active = 0;
	
	// Output Short
	if((HB_IsEnabled())&&(meas->hb_resistance < LIM_OUTPUT_SHORT)) warnings.output_short.active = 1;
	else warnings.output_short.active = 0;
	
	// Output Open - Load loss
	if((HB_IsEnabled())&&(meas->hb_resistance > LIM_OUTPUT_OPEN)) warnings.output_open.active = 1;
	else warnings.output_open.active = 0;
	
	ProcessWarningTime(&warnings.supply_voltage);
	ProcessWarningTime(&warnings.supply_current);
	ProcessWarningTime(&warnings.supply_power);
	ProcessWarningTime(&warnings.output_mismatch);
	ProcessWarningTime(&warnings.output_voltage);
	ProcessWarningTime(&warnings.output_current);
	ProcessWarningTime(&warnings.output_power);
	ProcessWarningTime(&warnings.output_open);
	ProcessWarningTime(&warnings.output_short);
}

static uint8_t Process_Faults(void)
{
	// Check warnings to escalate to fault	
	// Supply OCP
	if(warnings.supply_current.time > OCP_WARNING_LIMIT) faults.supply_ocp = 1;
	
	// Output OCP
	if(warnings.output_current.time > OCP_WARNING_LIMIT) faults.output_ocp = 1;
	
	// Output short
	if(warnings.output_short.time > SHORT_WARNING_LIMIT) faults.output_short = 1;
	
	switch(fault_state)
	{	
		case F_ACTIVE:
		// Check if fault still active
		if(!isFaultWarningActive(&warnings, &faults))
		{
			// Fault cause ended, go to cooldown
			cooldown_timer = COOLDOWN_TIME;
			fault_state = F_COOLDOWN;
		};
		break;
		
		case F_COOLDOWN:
		// Check if fault reoccurs
		if(isFaultWarningActive(&warnings, &faults))
		{
			fault_state = F_ACTIVE;
			break;
		};

		// Wait for cooldown timer, reset fault flags
		if(cooldown_timer) cooldown_timer--;
		else
		{
			ResetFaults(&faults);
			fault_state = F_NONE;
		}
		break;
		
		default: //NONE
		// Check for new faults
		if(isAnyFaultActive(&faults))
		{
			// Start fault process
			fault_state = F_ACTIVE;
		};
		break;
	}
	
	if(fault_state != F_NONE) return 1;
	else return 0;
}

static void ProcessWarningTime(warn_t* w)
{
	if((w->active)&&(w->time < 0xFF)) w->time++;
	else if(w->active == 0) w->time = 0;
}

static uint8_t isAnyFaultActive(faults_t* f)
{
	if(f->supply_uvlo) return 1;
	if(f->supply_ovp) return 1;
	if(f->supply_ocp) return 1;
	if(f->supply_opp) return 1;
	if(f->output_ovp) return 1;
	if(f->output_ocp) return 1;
	if(f->output_opp) return 1;
	if(f->output_open) return 1;
	if(f->output_short) return 1;
	
	return 0;
}

static uint8_t isFaultWarningActive(warnings_t* w, faults_t* f)
{
	if((f->supply_uvlo)  && (w->supply_voltage.active)) return 1;
	if((f->supply_ovp)   && (w->supply_voltage.active)) return 1;
	if((f->supply_ocp)   && (w->supply_current.active)) return 1;
	if((f->supply_opp)   && (w->supply_power.active)  ) return 1;
	if((f->output_ovp)   && (w->output_voltage.active)) return 1;
	if((f->output_ocp)   && (w->output_current.active)) return 1;
	if((f->output_opp)   && (w->output_power.active)  ) return 1;
	if((f->output_open)  && (w->output_open.active)   ) return 1;
	if((f->output_short) && (w->output_short.active)  ) return 1;
	
	return 0;
}

static void ResetFaults(faults_t* f)
{
	f->supply_uvlo = 0;
	f->supply_ovp = 0;
	f->supply_ocp = 0;
	f->supply_opp = 0;
	f->output_ovp = 0;
	f->output_ocp = 0;
	f->output_opp = 0;
	f->output_open = 0;
	f->output_short = 0;
}