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

using namespace util;

/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
/**** Private function declarations ****/
#ifndef TESTING
#endif

/**** Public function definitions ****/
uint8_t util::invert(uint8_t x)
{
	if(x!=0) return 0;
	else return 1;
}

uint16_t util::invert(uint16_t x)
{
	if(x!=0) return 0;
	else return 1;
}

uint32_t util::invert(uint32_t x)
{
	if(x!=0) return 0;
	else return 1;
}

uint8_t util::sat_add(uint8_t x, uint8_t y)
{
	uint8_t z = x + y;
	// Check for overflow
	if((z < x)||(z < y)) return 0xFF;
	else return z;
}

uint16_t util::sat_add(uint16_t x, uint16_t y)
{
	uint16_t z = x + y;
	// Check for overflow
	if((z < x)||(z < y)) return 0xFF;
	else return z;
}

uint32_t util::sat_add(uint32_t x, uint32_t y)

{
	uint32_t z = x + y;
	// Check for overflow
	if((z < x)||(z < y)) return 0xFF;
	else return z;
}

uint8_t util::sat_subtract(uint8_t x, uint8_t y)
{
	uint8_t z = x - y;
	// Check for underflow
	if(z > x) return 0;
	else return z;
}

uint16_t util::sat_subtract(uint16_t x, uint16_t y)
{
	uint16_t z = x - y;
	// Check for underflow
	if(z > x) return 0;
	else return z;
}
uint32_t util::sat_subtract(uint32_t x, uint32_t y)
{
	uint32_t z = x - y;
	// Check for underflow
	if(z > x) return 0;
	else return z;
}

uint8_t util::abs_subtract(uint8_t x, uint8_t y)
{
	if(x > y) return x - y;
	else return y-x;
}

uint16_t util::abs_subtract(uint16_t x, uint16_t y)
{
	if(x > y) return x - y;
	else return y-x;
}

uint32_t util::abs_subtract(uint32_t x, uint32_t y)
{
	if(x > y) return x - y;
	else return y-x;
}


uint16_t util::sat_cast(uint32_t x)
{
	if(x > 0x0000FFFF) return 0xFFFF;
	else return (uint16_t)x;
}

uint16_t util::sat_cast(int32_t x)
{
	if(x < 0) return 0x0000;
	else if(x > 0x0000FFFF) return 0xFFFF;
	else return (uint16_t)x;
}

uint8_t util::is_timed_out(uint16_t time, uint16_t limit)
{
	if(time >= limit) return 1;
	else return 0;
}

uint8_t util::is_in_range(uint16_t value, uint16_t min, uint16_t max)
{
	if((value >= min)&&(value <= max)) return 1;
	else return 0;
}

uint16_t util::time_delta(uint16_t start, uint16_t end)
{
	if(end >= start) return (end-start);
	uint16_t temp = 0xFFFF - start;
	return temp + end;
}

uint32_t util::time_delta(uint32_t start, uint32_t end)
{
	if(end >= start) return (end-start);
	uint32_t temp = 0xFFFFFFFF - start;
	return temp + end;
}

uint16_t util::time_delta(uint16_t start, uint16_t end, uint16_t max)
{
	if(end >= start) return (end-start);
	uint16_t temp = max - start;
	return temp + end;
}

uint32_t util::time_delta(uint32_t start, uint32_t end, uint32_t max)
{
	if(end >= start) return (end-start);
	uint32_t temp = max - start;
	return temp + end;
}

uint16_t util::convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset)
{
	int32_t temp = (int32_t)raw;

	temp = temp * mul;
	if(div>1) temp /= div;
	temp += offset;

	return sat_cast(temp);
}

uint16_t util::sat_mul_kilo(uint16_t xk, uint16_t yk)
{
	uint32_t temp = (uint32_t)xk * (uint32_t)yk;
	temp /= 1000;

	return sat_cast(temp);
}

uint16_t util::sat_div_kilo(uint16_t top, uint16_t bot)
{
	//Sanity check bot
	if(bot==0) return 0xFFFF; //aka infinity

	uint32_t temp = (uint32_t)top * 1000;
	temp /= (uint32_t)bot;

	return sat_cast(temp);
}

uint16_t util::sat_ratio(uint16_t top, uint16_t bot)
{
	//Sanity check bot
	if(bot==0) return 0xFFFF; //aka infinity

	//Easy option
	if(top>=bot) return 0xFFFF;

	uint32_t temp = (uint32_t)top * 0x0000FFFF;
	temp /= (uint32_t)bot;

	return sat_cast(temp);
}

uint16_t util::percent_to_16b(uint8_t percent)
{
	uint32_t temp = (uint32_t)percent * 0x0000FFFF;
	temp /= 100;

	// Limit to 16 bits
	uint16_t pwm = sat_cast(temp);

	return pwm;
}

uint16_t util::percent_of(uint8_t percent, uint16_t value)
{
	if(percent == 0) return 0;
	else if(percent >= 100) return value;
	
	uint32_t temp = (uint32_t)value * percent;
	return temp/100;
}

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