#ifndef UTILS_H_
#define UTILS_H_

/**** Includes ****/
#include <stdint.h>

/**** Public definitions ****/

/**** Public function declarations ****/
uint8_t util_invert_8b(uint8_t x);

uint16_t util_limit_u32b_to_u16b(uint32_t in);
uint16_t util_limit_s32b_to_u16b(int32_t in);

uint16_t util_convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset);
uint16_t util_sat_mul_kilo(uint16_t xk, uint16_t yk);
uint16_t util_sat_div_kilo(uint16_t top, uint16_t bot);
uint16_t util_sat_ratio_16b(uint16_t top, uint16_t bot);
uint16_t util_percent_to_16b(uint8_t percent);

uint8_t util_sat_add_8b(uint8_t x, uint8_t y);
uint8_t util_sat_subtract_8b(uint8_t x, uint8_t y);

uint16_t util_sat_add_16b(uint16_t x, uint16_t y);
uint16_t util_sat_subtract_16b(uint16_t x, uint16_t y);

uint16_t util_interpolate_1d_u16b(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis);
uint16_t util_interpolate_2d_u16b(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values);

#ifdef TESTING
// Access to private functions for unit testing
static uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis);
static uint16_t interpolate_u16b(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1);
static uint16_t index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x);
#endif

#endif /* UTILS_H_ */