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

using namespace hw;

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

/**** Public function definitions ****/
hw::CCoutput::CCoutput(bsp::Hafbridge* hbridge, bsp::AnalogIn* supply_u, bsp::AnalogIn* out_u, bsp::AnalogIn* out_i) : CVoutput(hbridge, supply_u)
{
	this->out_voltage = out_u;
	this->out_currnet = out_i;
	
	this->out_impedance = 0xFFFF;
	this->target_voltage = 0;
}

void hw::CCoutput::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);
	}
	
	// Check target
	if((this->target < this->min_out)&&(this->target > 0)) this->target = this->min_out;
	
	// Convert target current to voltage
	this->target_voltage = util::sat_mul_kilo(this->target, this->out_impedance);
	
	// Set output
	this->hbridge->write(util::sat_ratio(this->target_voltage, this->supply->last_read));
}

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