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

using namespace bsp;

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

/**** Public function definitions ****/
hw::OutReg::OutReg(void)
{
	return;
}

hw::OutReg::~OutReg(void)
{
	return;
}

void hw::OutReg::init(outRegCfg_t* cfg)
{
	this->pwm_high = cfg->pwm_high;
	this->dout_low = cfg->dout_low;
	this->ubat = cfg->ubat;
	this->uout = cfg->uout;
	this->iout = cfg->iout;
	
	this->voltage = 0;
	this->current = 0;
	this->out_on = 0;
	this->lock = 0;
	
	this->cc_mode_en = 0;
	this->update_ain = 0;
	this->cc_tolerance = 75;
}

void hw::OutReg::write_voltage(uint16_t voltage)
{
	this->voltage = voltage;
}

void hw::OutReg::write_current(uint16_t current)
{
	this->current = current;
	this->current_bot = util::sat_subtract(current, this->cc_tolerance);
	this->current_top = util::sat_add(current, this->cc_tolerance);
}

void hw::OutReg::write_on(uint8_t state)
{
	this->out_on = state;
}

void hw::OutReg::write_lock(uint8_t state)
{
	this->lock = state;
}

uint16_t hw::OutReg::read_voltage(void)
{
	return this->voltage;
}

uint16_t hw::OutReg::read_current(void)
{
	return this->current;
}

void hw::OutReg::process(void)
{
	// Update analog input
	if(this->update_ain)
	{
		this->ubat->read();
		this->uout->read();
		this->iout->read();
	};
	
	// Check if turned off
	if((out_on == 0)||(this->lock != 0))
	{
		
		this->pwm_high->write((uint16_t)0);
		this->dout_low->write(0);
		return;
	}
	else if(this->dout_low->last_writen == 0)
	{
		this->dout_low->write(1);
	};

	// Calculate next duty cycle setting
	uint16_t next_duty = this->pwm_high->get_set_duty();
	
	if((this->voltage==0)||(this->current==0))
	{
		// Off but not HiZ
		next_duty = 0;
	}
	else if((this->cc_mode_en)&&(this->iout->last_read > this->current_bot))
	{
		// Constant current mode - Change voltage to be within current limit
		if(util::is_in_range(this->iout->last_read, this->current_bot, this->current_top)==0)
		{
			// Current outside of tolerance. Recalculate duty cycle.
			uint32_t temp = (uint32_t)this->pwm_high->get_set_duty() * (uint32_t)this->current;
			temp /= this->iout->last_read;
			next_duty = util::sat_cast(temp);
		};
	}
	else
	{
		// Constant voltage mode
		next_duty = util::sat_ratio(this->voltage, this->ubat->last_read);
	}
	
	this->pwm_high->write(next_duty);
	
	return;
}

void hw::OutReg::force_off(void)
{
	// Turn off output - HiZ
	this->pwm_high->write((uint16_t)0);
	this->dout_low->write(0);
	
	// Update targets
	this->voltage = 0;
	this->current = 0;
	this->out_on = 0;
	this->lock = 1;
}

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