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

using namespace hw;

/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
/**** Private function declarations ****/

/**** Public function definitions ****/
hw::RegOut::RegOut(bsp::Hafbridge* hbridge, bsp::AnalogIn* sup_u, bsp::AnalogIn* out_u, bsp::AnalogIn* out_i)
{
	this->hbridge = hbridge;
	this->supply_voltage = sup_u;
	this->out_voltage = out_u;
	this->out_currnet = out_i;
	
	this->max_voltage = 6750;
	this->max_current = 4500;
	this->min_voltage = 200;
	
	this->set_voltage = 0;
	this->out_impedance = 0xFFFF;
	
	this->hbridge->disable();
}

hw::RegOut::~RegOut(void)
{
	this->hbridge->write((uint16_t)0x0000);
	this->hbridge->disable();
	return;
}

void hw::RegOut::update(void)
{
	// Calculate output impedance
	if((this->out_currnet == 0)||(this->out_voltage->last_read == 0)) this->out_impedance = 0xFFFF;
	else this->out_impedance = util::sat_div_kilo(this->out_voltage->last_read, this->out_currnet->last_read);
		
	// Recalculate output set voltage
	if((this->max_voltage==0)||(this->max_current==0)) this->set_voltage = 0;
	else this->set_voltage = util::sat_mul_kilo(this->max_current, this->out_impedance);
	
	// Limit set voltage
	if(this->set_voltage > this->max_voltage) this->set_voltage = this->max_voltage;
	
	if((this->set_voltage>0)&&(this->set_voltage < this->min_voltage)) this->set_voltage = this->min_voltage;
	
	// Set output
	this->hbridge->write(util::sat_ratio(this->set_voltage, this->supply_voltage->last_read));
}

void hw::RegOut::enable(void)
{
	this->hbridge->enable();
}

void hw::RegOut::disable(void)
{
	this->hbridge->disable();	
}

uint8_t hw::RegOut::is_enabled(void)
{
	return this->hbridge->is_enabled();
}

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