/**** Includes ****/
#include "coil.h"

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

/**** Private constants ****/

/**** Private variables ****/
static uint16_t lock_current = 4500; //mA
static uint16_t min_current = 100; //mA
static uint16_t target_current = 0; //mA
static uint8_t new_target = 1;

static uint16_t adj_target_current = 0; //mA
static uint16_t target_voltage = 0; //mV

static uint8_t act_force = 0;

static uint16_t nominal_resitance = 1500; //mR

static int32_t sum_current = 0;
static uint8_t sum_cnt = 0;


/**** Private function declarations ****/
static uint16_t UpdateVoltage(uint16_t cur, uint16_t res);

/**** Public function definitions ****/
void Coil_SetLockCurrent(uint16_t lock_i)
{
	lock_current = lock_i;
}

void Coil_SetTarget_Force(uint8_t force)
{
	// Check if worth doing
	if(force == act_force) return;
	
	// Calculate new target current
	act_force = force;
	
	// Check simple answer
	uint16_t new_current = 0;
	if(force==0) new_current = 0;
	else if(force >= 100) new_current = lock_current;
	else
	{
		uint32_t temp = (uint32_t)force * lock_current;
		temp /= 100;
		
		if(temp > 0x0000FFFF) new_current = lock_current;
		else new_current = (uint16_t) temp;
	}
	
	// Update new target
	Coil_SetTarget_Current(new_current);
}

void Coil_SetTarget_Current(uint16_t current)
{
	if(current >= lock_current) current = lock_current;
	else if((current > 0)&&(current <= min_current)) current = min_current;
	
	if(current != target_current) new_target = 1;
	target_current = current;
}

uint16_t Coil_GetTargetVolatge(void)
{
	return target_voltage;
}

uint16_t Coil_Update(analog_t* meas)
{
	// Collect average current
	sum_current += meas->hb_currnet;
	sum_cnt++;
	
	// Update measurement
	if(sum_cnt >= 10)
	{
		// Calculate average
		sum_current /= sum_cnt;
		
		// Calculate error
		int32_t error = sum_current - (int32_t)target_current;
		int32_t temp = (int32_t)adj_target_current - error;
		
		// Limit to 16bits
		if(temp<0) adj_target_current = 0;
		else if(temp > 0x0000FFFF) adj_target_current = 0xFFFF;
		else adj_target_current = (uint16_t)temp;
		sum_cnt = 0;
		sum_current = 0;
	};
	
	// Closed loop or open loop target set
	if(new_target)
	{
		// Set open-loop HB output
		target_voltage = UpdateVoltage(target_current, nominal_resitance);
		adj_target_current = target_current;
		new_target = 0;
		sum_cnt = 0;
		sum_current = 0;
	}
	else
	{
		target_voltage = UpdateVoltage(adj_target_current, nominal_resitance);
	}
	
	return target_voltage;
}
	
/**** Private function definitions ****/
static uint16_t UpdateVoltage(uint16_t cur, uint16_t res)
{
	// Update settable voltage
	if(cur==0) return 0;
	else
	{
		uint32_t volt = (uint32_t)cur * res;
		volt /= 1000;
		if(volt > 0x0000FFFF) return 0xFFFF;
		else return (uint16_t)volt;
	}
}