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

using namespace dccd;

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

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

void dccd::CoilReg::init(dccd::DccdHw* dccd_hw)
{	
	this->lock_current = 4500;
	this->ref_resistance = 1500;
	this->cc_max_resistance = 2000;
	this->cc_min_resistance = 1000;
	this->target_force = 0;
	this->set_force = 0;
	this->disable_protection = 0;
	
	// Config output protection
	this->hardware->out_voltage.under_treshold = 0;
	this->hardware->out_voltage.over_treshold = 0xFFFF;
	this->hardware->out_voltage.hold_time = 200;
	this->hardware->out_voltage.cooldown_time = 1000;
	this->hardware->out_voltage.auto_reset = 1;
}

void dccd::CoilReg::process(void)
{	
	// Fix target force
	if(this->target_force > 100) this->target_force = 100;
	
	// Check protection
	if((this->disable_protection==0)&&(this->hardware->out_voltage.fault!=0))
	{
		// HiZ
		this->hardware->outreg.write_voltage(0);
		this->hardware->outreg.write_current(0);
		this->hardware->outreg.write_on(0);
		this->hardware->out_voltage.under_treshold = 0;
		this->set_force = -1;
		return;
	};

	// Check for target changes
	if(this->target_force == this->set_force) return;
	
	// Update set force
	this->set_force = this->target_force;
	if(this->set_force < 0)
	{
		// HiZ
		this->hardware->outreg.write_voltage(0);
		this->hardware->outreg.write_current(0);
		this->hardware->outreg.write_on(0);
		this->hardware->out_voltage.under_treshold = 0;
	}
	else if(this->set_force == 0)
	{
		// Open
		this->hardware->outreg.write_voltage(0);
		this->hardware->outreg.write_current(0);
		this->hardware->outreg.write_on(1);
		this->hardware->out_voltage.under_treshold = 0;
	}
	else
	{
		// Calculate current and voltage settings
		this->hardware->outreg.write_current(util::percent_of((uint8_t)this->set_force, this->lock_current));
		uint16_t resistance = this->ref_resistance;
		if(this->hardware->outreg.cc_mode_en) resistance = this->cc_max_resistance;
		this->hardware->outreg.write_voltage(util::sat_mul_kilo(this->hardware->outreg.read_current(), resistance));
		this->hardware->outreg.write_on(1);
		// Calculate min. voltage
		if(this->disable_protection==0) this->hardware->out_voltage.under_treshold = util::sat_mul_kilo(this->hardware->outreg.read_current(), cc_min_resistance);
	}
}

uint8_t dccd::CoilReg::is_fault(void)
{
	return this->hardware->out_voltage.fault;
}

uint8_t dccd::CoilReg::read_act_force(void)
{
	if(this->set_force < 0) return 0;
	else return (uint8_t)this->set_force;
}

void dccd::CoilReg::cfg_set_cv_mode(void)
{
	this->hardware->outreg.cc_mode_en = 0;
}

void dccd::CoilReg::cfg_set_cc_mode(void)
{
	this->hardware->outreg.cc_mode_en = 1;
}

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