/**** Includes ****/
#include "board/utils/utils.h"
#include "board/halfbridge.h"
#include "hb_control.h"

/**** Private definitions ****/
/**** Private constants ****/
// Fault condition thresholds
static const uint16_t SUPPLY_VOLT_MIN_W = HW_HB_SUPPLY_VOLT_MIN_W;
static const uint16_t SUPPLY_VOLT_MIN_F = HW_HB_SUPPLY_VOLT_MIN_F;

static const uint16_t SUPPLY_VOLT_MAX_W = HW_HB_SUPPLY_VOLT_MAX_W;
static const uint16_t SUPPLY_VOLT_MAX_F = HW_HB_SUPPLY_VOLT_MAX_F;

static const uint16_t SUPPLY_CURRENT_MAX_W = HW_HB_SUPPLY_CURRENT_MAX_W;
static const uint16_t SUPPLY_CURRENT_MAX_F = HW_HB_SUPPLY_CURRENT_MAX_F;

static const uint16_t SUPPLY_POWER_MAX_W = HW_HB_SUPPLY_POWER_MAX_W;
static const uint16_t SUPPLY_POWER_MAX_F = HW_HB_SUPPLY_POWER_MAX_F;

static const uint16_t OUT_CURRENT_MAX_W = HW_HB_OUT_CURRENT_MAX_W;
static const uint16_t OUT_CURRENT_MAX_F = HW_HB_OUT_CURRENT_MAX_F;

static const uint16_t OUT_VOLTAGE_MAX_W = HW_HB_OUT_VOLTAGE_MAX_W;
static const uint16_t OUT_VOLTAGE_MAX_F = HW_HB_OUT_VOLTAGE_MAX_F;

static const uint16_t OUT_POWER_MAX_W = HW_HB_OUT_POWER_MAX_W;
static const uint16_t OUT_POWER_MAX_F = HW_HB_OUT_POWER_MAX_F;

static const uint16_t OUT_RESISTANCE_MIN_W = HW_HB_OUT_RESISTANCE_MIN_W;
static const uint16_t OUT_RESISTANCE_MIN_F = HW_HB_OUT_RESISTANCE_MIN_F;

static const uint16_t OUT_RESISTANCE_MAX_W = HW_HB_OUT_RESISTANCE_MAX_W;
static const uint16_t OUT_RESISTANCE_MAX_F = HW_HB_OUT_RESISTANCE_MAX_F;

static const uint16_t FAULT_DELAY = HW_HB_FAULT_DELAY;
static const uint16_t WTOF_DELAY = HW_HB_WTOF_DELAY;

/**** Private variables ****/

/**** Private function declarations ****/
// Protection functions
static void faults_chs_reset(hb_fault_chs_t* ch_list);
static void faults_init(hb_faults_t* faults, hb_fault_chs_t* en_list);
static void faults_logic(hb_meas_t* measurements, hb_faults_t* faults);

static uint8_t faults_check(hb_faults_t* faults, hb_fault_chs_t* en_list, hb_fault_chs_t* fb_list, fault_lvl_t check);
static uint8_t warnings_feedback(hb_faults_t* faults, hb_fault_chs_t* en_list, hb_fault_chs_t* fb_list);
static uint8_t faults_feedback(hb_faults_t* faults, hb_fault_chs_t* en_list, hb_fault_chs_t* fb_list);
static uint8_t faults_is_any(hb_fault_chs_t* fb_list);

static uint8_t is_outside(uint16_t meas, uint16_t min, uint16_t max);

static uint8_t fuse_subprocess(hb_feedback_t* hb_fb, hb_control_t* hb_ctrl);
static void target_to_control(int16_t target, uint16_t ref_voltage, uint16_t* pwm_out, uint8_t* low_out, hb_fault_chs_t* fault_ch_en);

/**** Public function definitions ****/
void hb_init(hb_feedback_t* hb_fb, hb_control_t* hb_ctrl)
{
	// Initialize control
	hb_ctrl->enabled = 0;

	// Initialize protection structures
	faults_init(&hb_ctrl->out_faults, &hb_ctrl->out_faults_en);

	fuse_reset(&hb_ctrl->out_fuse);
	hb_ctrl->out_fuse_cfg.cooldown_time = 1000;
	hb_ctrl->out_fuse_cfg.retry_time = 1000;

	// Initialize feedback structure
	bsp_hb_read_meas(&hb_fb->analog);
	faults_chs_reset(&hb_fb->faults);
	faults_chs_reset(&hb_fb->warnings);

	hb_fb->enabled = 0;
	hb_fb->warning_act = 0;
	hb_fb->fault_act = 0;
	hb_fb->fused = 0;

	bsp_hb_write_pwm(0);
	bsp_hb_write_low(0);
}

void hb_enable(hb_control_t* hb_ctrl)
{
	bsp_hb_write_pwm(0);
	bsp_hb_write_low(1);
	hb_ctrl->enabled = 1;
}

void hb_disable(hb_control_t* hb_ctrl)
{
	hb_ctrl->enabled = 0;
	bsp_hb_write_pwm(0);
	bsp_hb_write_low(0);
}


void hb_process(int16_t target, hb_feedback_t* hb_fb, hb_control_t* hb_ctrl)
{
	uint16_t out_pwm = 0;
	uint8_t low_on = 0;

	// Read feedback
	bsp_hb_read_meas(&hb_fb->analog);

	// Process fuse
	uint8_t fuse_act = fuse_subprocess(hb_fb, hb_ctrl);

	// Act on fuse state
	if((fuse_act)||(!hb_ctrl->enabled))
	{
		// Turn off output
		bsp_hb_write_pwm(0);
		bsp_hb_write_low(0);
		return;
	};

	// Process new target
	target_to_control(target, hb_fb->analog.sup_voltage, &out_pwm, &low_on, &hb_ctrl->out_faults_en);

	// Apply new controls
	bsp_hb_write_low(low_on);
	bsp_hb_write_pwm(out_pwm);
}

/**** Private function definitions ****/
static void target_to_control(int16_t target, uint16_t ref_voltage, uint16_t* pwm_out, uint8_t* low_out, hb_fault_chs_t* fault_ch_en)
{
	if(target < 0)
	{
		// Fast decay
		*pwm_out = 0;
		*low_out = 0;
		// Set appropriate fault channels
		fault_ch_en->out_short = 0;
		fault_ch_en->out_ovp = 0;
	}
	else if(target == 0)
	{
		// Slow decay
		*pwm_out = 0;
		*low_out = 1;
		// Set appropriate fault channels
		fault_ch_en->out_short = 0;
		fault_ch_en->out_ovp = 1;
	}
	else
	{
		// Calculate target PWM
		*pwm_out =  util_sat_ratio_16b((uint16_t)target, ref_voltage);
		*low_out = 1;
		fault_ch_en->out_short = 1;
		fault_ch_en->out_ovp = 1;
	}
}

static uint8_t fuse_subprocess(hb_feedback_t* hb_fb, hb_control_t* hb_ctrl)
{
	// Process faults
	faults_logic(&hb_fb->analog, &hb_ctrl->out_faults);

	// Check if any enabled fault is active
	uint8_t warn_act = warnings_feedback(&hb_ctrl->out_faults, &hb_ctrl->out_faults_en, &hb_fb->warnings);
	uint8_t fault_act = faults_feedback(&hb_ctrl->out_faults, &hb_ctrl->out_faults_en, &hb_fb->faults);

	// Process fuse state
	uint8_t fuse_act = fuse_process(&hb_ctrl->out_fuse, fault_act, &hb_ctrl->out_fuse_cfg);

	// Copy feedback data
	hb_fb->enabled = hb_ctrl->enabled;
	hb_fb->warning_act = warn_act;
	hb_fb->fault_act = fault_act;
	hb_fb->fused = fuse_act;

	return fuse_act;
}

// Fault logic functions
static void faults_chs_reset(hb_fault_chs_t* ch_list)
{
	// Zero all channels
	ch_list->sup_uvp = 0;
	ch_list->sup_ovp = 0;
	ch_list->sup_ocp = 0;
	ch_list->sup_opp = 0;
	ch_list->out_ovp = 0;
	ch_list->out_ocp = 0;
	ch_list->out_opp = 0;
	ch_list->out_short = 0;
	ch_list->out_open = 0;
}

static void faults_init(hb_faults_t* faults, hb_fault_chs_t* en_list)
{
	// Init state
	fault_reset(&faults->sup_uvp);
	fault_reset(&faults->sup_ovp);
	fault_reset(&faults->sup_ocp);
	fault_reset(&faults->sup_opp);
	fault_reset(&faults->out_ovp);
	fault_reset(&faults->out_ocp);
	fault_reset(&faults->out_opp);
	fault_reset(&faults->out_short);
	fault_reset(&faults->out_open);

	// Init enabled channels
	faults_chs_reset(en_list);
	en_list->sup_ocp = 1;
	en_list->out_ocp = 1;
	en_list->out_short = 1;
}

static void faults_logic(hb_meas_t* measurements, hb_faults_t* faults)
{
	uint8_t w_trig = 0;
	uint8_t f_trig = 0;

	fault_cfg_t fault_cfg;
	fault_cfg.delay = FAULT_DELAY;
	fault_cfg.wtof = WTOF_DELAY;

	// Check supply voltage
	w_trig = is_outside(measurements->sup_voltage, SUPPLY_VOLT_MIN_W, 0);
	f_trig = is_outside(measurements->sup_voltage, SUPPLY_VOLT_MIN_F, 0);
	fault_process(&faults->sup_uvp, w_trig, f_trig, &fault_cfg);

	w_trig = is_outside(measurements->sup_voltage, 0, SUPPLY_VOLT_MAX_W);
	f_trig = is_outside(measurements->sup_voltage, 0, SUPPLY_VOLT_MAX_F);
	fault_process(&faults->sup_ovp, w_trig, f_trig, &fault_cfg);

	// Check supply current
	w_trig = is_outside(measurements->sup_current, 0, SUPPLY_CURRENT_MAX_W);
	f_trig = is_outside(measurements->sup_current, 0, SUPPLY_CURRENT_MAX_F);
	fault_process(&faults->sup_ocp, w_trig, f_trig, &fault_cfg);

	// Check supply power
	w_trig = is_outside(measurements->sup_power, 0, SUPPLY_POWER_MAX_W);
	f_trig = is_outside(measurements->sup_power, 0, SUPPLY_POWER_MAX_F);
	fault_process(&faults->sup_opp, w_trig, f_trig, &fault_cfg);

	// Check output voltage
	w_trig = is_outside(measurements->out_voltage, 0, OUT_VOLTAGE_MAX_W);
	f_trig = is_outside(measurements->out_voltage, 0, OUT_VOLTAGE_MAX_F);
	fault_process(&faults->out_ovp, w_trig, f_trig, &fault_cfg);

	// Check output current
	w_trig = is_outside(measurements->out_current, 0, OUT_CURRENT_MAX_W);
	f_trig = is_outside(measurements->out_current, 0, OUT_CURRENT_MAX_F);
	fault_process(&faults->out_ocp, w_trig, f_trig, &fault_cfg);

	// Check output power
	w_trig = is_outside(measurements->out_power, 0, OUT_POWER_MAX_W);
	f_trig = is_outside(measurements->out_power, 0, OUT_POWER_MAX_F);
	fault_process(&faults->out_opp, w_trig, f_trig, &fault_cfg);

	// Check output resistance
	w_trig = is_outside(measurements->out_impedance, OUT_RESISTANCE_MIN_W, 0);
	f_trig = is_outside(measurements->out_impedance, OUT_RESISTANCE_MIN_F, 0);
	fault_process(&faults->out_short, w_trig, f_trig, &fault_cfg);

	w_trig = is_outside(measurements->out_impedance, 0, OUT_RESISTANCE_MAX_W);
	f_trig = is_outside(measurements->out_impedance, 0, OUT_RESISTANCE_MAX_F);
	fault_process(&faults->out_open, w_trig, f_trig, &fault_cfg);
}

static uint8_t warnings_feedback(hb_faults_t* faults, hb_fault_chs_t* en_list, hb_fault_chs_t* fb_list)
{
	return faults_check(faults, en_list, fb_list, FAULT_LVL_WARNING);
}

static uint8_t faults_feedback(hb_faults_t* faults, hb_fault_chs_t* en_list, hb_fault_chs_t* fb_list)
{
	return faults_check(faults, en_list, fb_list, FAULT_LVL_FAULT);
}

// Utilities
static uint8_t is_outside(uint16_t meas, uint16_t min, uint16_t max)
{
	if((meas < min)||((max!=0)&&(meas > max))) return 1;
	else return 0;
}

static uint8_t faults_check(hb_faults_t* faults, hb_fault_chs_t* en_list, hb_fault_chs_t* fb_list, fault_lvl_t check)
{
	if((en_list->sup_uvp)&&(faults->sup_uvp.severity == check)) fb_list->sup_uvp = 1;
	if((en_list->sup_ovp)&&(faults->sup_ovp.severity == check)) fb_list->sup_ovp = 1;
	if((en_list->sup_ocp)&&(faults->sup_ocp.severity == check)) fb_list->sup_ocp = 1;
	if((en_list->sup_opp)&&(faults->sup_opp.severity == check)) fb_list->sup_opp = 1;
	if((en_list->out_ovp)&&(faults->out_ovp.severity == check)) fb_list->out_ovp = 1;
	if((en_list->out_ocp)&&(faults->out_ocp.severity == check)) fb_list->out_ocp = 1;
	if((en_list->out_opp)&&(faults->out_opp.severity == check)) fb_list->out_opp = 1;
	if((en_list->out_short)&&(faults->out_short.severity == check)) fb_list->out_short = 1;
	if((en_list->out_open)&&(faults->out_open.severity == check)) fb_list->out_open = 1;

	return faults_is_any(fb_list);
}

static uint8_t faults_is_any(hb_fault_chs_t* fb_list)
{
	if(fb_list->sup_uvp) return 1;
	if(fb_list->sup_ovp) return 1;
	if(fb_list->sup_ocp) return 1;
	if(fb_list->sup_opp) return 1;
	if(fb_list->out_ovp) return 1;
	if(fb_list->out_ocp) return 1;
	if(fb_list->out_opp) return 1;
	if(fb_list->out_short) return 1;
	if(fb_list->out_open) return 1;

	return 0;
}

#ifdef TESTING
uint8_t hb_is_equal_fb_struct(hb_feedback_t* f1, hb_feedback_t* f2)
{
	if(f1->analog.out_voltage   != f2->analog.out_voltage   ) return 0;
	if(f1->analog.out_current   != f2->analog.out_current   ) return 0;
	if(f1->analog.sup_voltage   != f2->analog.sup_voltage   ) return 0;
	if(f1->analog.sup_current   != f2->analog.sup_current   ) return 0;
	if(f1->analog.out_power     != f2->analog.out_power     ) return 0;
	if(f1->analog.sup_power     != f2->analog.sup_power     ) return 0;
	if(f1->analog.out_impedance != f2->analog.out_impedance ) return 0;
	if(f1->analog.low_side_ctrl != f2->analog.low_side_ctrl ) return 0;
	if(f1->analog.pwm           != f2->analog.pwm           ) return 0;
	if(f1->enabled              != f2->enabled              ) return 0;
	if(f1->warning_act          != f2->warning_act          ) return 0;
	if(f1->fault_act            != f2->fault_act            ) return 0;
	if(f1->fused                != f2->fused                ) return 0;
	if(f1->warnings.sup_uvp     != f2->warnings.sup_uvp     ) return 0;
	if(f1->warnings.sup_ovp     != f2->warnings.sup_ovp     ) return 0;
	if(f1->warnings.sup_ocp     != f2->warnings.sup_ocp     ) return 0;
	if(f1->warnings.sup_opp     != f2->warnings.sup_opp     ) return 0;
	if(f1->warnings.out_ovp     != f2->warnings.out_ovp     ) return 0;
	if(f1->warnings.out_ocp     != f2->warnings.out_ocp     ) return 0;
	if(f1->warnings.out_opp     != f2->warnings.out_opp     ) return 0;
	if(f1->warnings.out_short   != f2->warnings.out_short   ) return 0;
	if(f1->warnings.out_open    != f2->warnings.out_open    ) return 0;
	if(f1->faults.sup_uvp       != f2->faults.sup_uvp       ) return 0;
	if(f1->faults.sup_ovp       != f2->faults.sup_ovp       ) return 0;
	if(f1->faults.sup_ocp       != f2->faults.sup_ocp       ) return 0;
	if(f1->faults.sup_opp       != f2->faults.sup_opp       ) return 0;
	if(f1->faults.out_ovp       != f2->faults.out_ovp       ) return 0;
	if(f1->faults.out_ocp       != f2->faults.out_ocp       ) return 0;
	if(f1->faults.out_opp       != f2->faults.out_opp       ) return 0;
	if(f1->faults.out_short     != f2->faults.out_short     ) return 0;
	if(f1->faults.out_open      != f2->faults.out_open      ) return 0;

	return 1;
}

uint8_t hb_is_equal_ctrl_struct(hb_control_t* c1, hb_control_t* c2)
{
	if(c1->enabled                       != c2->enabled                       ) return 0;
	if(c1->out_faults.sup_uvp.severity   != c2->out_faults.sup_uvp.severity   ) return 0;
	if(c1->out_faults.sup_uvp.w_time     != c2->out_faults.sup_uvp.w_time     ) return 0;
	if(c1->out_faults.sup_uvp.f_time     != c2->out_faults.sup_uvp.f_time     ) return 0;
	if(c1->out_faults.sup_ovp.severity   != c2->out_faults.sup_ovp.severity   ) return 0;
	if(c1->out_faults.sup_ovp.w_time     != c2->out_faults.sup_ovp.w_time     ) return 0;
	if(c1->out_faults.sup_ovp.f_time     != c2->out_faults.sup_ovp.f_time     ) return 0;
	if(c1->out_faults.sup_ocp.severity   != c2->out_faults.sup_ocp.severity   ) return 0;
	if(c1->out_faults.sup_ocp.w_time     != c2->out_faults.sup_ocp.w_time     ) return 0;
	if(c1->out_faults.sup_ocp.f_time     != c2->out_faults.sup_ocp.f_time     ) return 0;
	if(c1->out_faults.sup_opp.severity   != c2->out_faults.sup_opp.severity   ) return 0;
	if(c1->out_faults.sup_opp.w_time     != c2->out_faults.sup_opp.w_time     ) return 0;
	if(c1->out_faults.sup_opp.f_time     != c2->out_faults.sup_opp.f_time     ) return 0;
	if(c1->out_faults.out_ovp.severity   != c2->out_faults.out_ovp.severity   ) return 0;
	if(c1->out_faults.out_ovp.w_time     != c2->out_faults.out_ovp.w_time     ) return 0;
	if(c1->out_faults.out_ovp.f_time     != c2->out_faults.out_ovp.f_time     ) return 0;
	if(c1->out_faults.out_ocp.severity   != c2->out_faults.out_ocp.severity   ) return 0;
	if(c1->out_faults.out_ocp.w_time     != c2->out_faults.out_ocp.w_time     ) return 0;
	if(c1->out_faults.out_ocp.f_time     != c2->out_faults.out_ocp.f_time     ) return 0;
	if(c1->out_faults.out_opp.severity   != c2->out_faults.out_opp.severity   ) return 0;
	if(c1->out_faults.out_opp.w_time     != c2->out_faults.out_opp.w_time     ) return 0;
	if(c1->out_faults.out_opp.f_time     != c2->out_faults.out_opp.f_time     ) return 0;
	if(c1->out_faults.out_short.severity != c2->out_faults.out_short.severity ) return 0;
	if(c1->out_faults.out_short.w_time   != c2->out_faults.out_short.w_time   ) return 0;
	if(c1->out_faults.out_short.f_time   != c2->out_faults.out_short.f_time   ) return 0;
	if(c1->out_faults.out_open.severity  != c2->out_faults.out_open.severity  ) return 0;
	if(c1->out_faults.out_open.w_time    != c2->out_faults.out_open.w_time    ) return 0;
	if(c1->out_faults.out_open.f_time    != c2->out_faults.out_open.f_time    ) return 0;
	if(c1->out_faults_en.sup_uvp         != c2->out_faults_en.sup_uvp         ) return 0;
	if(c1->out_faults_en.sup_ovp         != c2->out_faults_en.sup_ovp         ) return 0;
	if(c1->out_faults_en.sup_ocp         != c2->out_faults_en.sup_ocp         ) return 0;
	if(c1->out_faults_en.sup_opp         != c2->out_faults_en.sup_opp         ) return 0;
	if(c1->out_faults_en.out_ovp         != c2->out_faults_en.out_ovp         ) return 0;
	if(c1->out_faults_en.out_ocp         != c2->out_faults_en.out_ocp         ) return 0;
	if(c1->out_faults_en.out_opp         != c2->out_faults_en.out_opp         ) return 0;
	if(c1->out_faults_en.out_short       != c2->out_faults_en.out_short       ) return 0;
	if(c1->out_faults_en.out_open        != c2->out_faults_en.out_open        ) return 0;
	if(c1->out_fuse.state                != c2->out_fuse.state                ) return 0;
	if(c1->out_fuse.count                != c2->out_fuse.count                ) return 0;
	if(c1->out_fuse.timer                != c2->out_fuse.timer                ) return 0;
	if(c1->out_fuse_cfg.cooldown_time    != c2->out_fuse_cfg.cooldown_time    ) return 0;
	if(c1->out_fuse_cfg.retry_time       != c2->out_fuse_cfg.retry_time       ) return 0;

	return 1;
}
#endif