/**** 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;
}

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;
}

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 ****/