#include "ut_utils.h"

#include "..\..\src\hw\board\utils\utils.h"

static int ut_util_invert_8b(uint8_t x, uint8_t exp_out)
{
    printf("   Input:%d\n", x);

    uint8_t out = util_invert_8b(x);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_invert_8b_test(void)
{
    printf("******************************************************\n");
    printf("uint8_t util_invert_8b(uint8_t x)\n");

    int test_res;
    int pass = 1;

    uint8_t inp;
    uint8_t exp_out;

    inp = 0;
    exp_out = 1;
    test_res = ut_util_invert_8b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 1;
    exp_out = 0;
    test_res = ut_util_invert_8b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 257;
    exp_out = 0;
    test_res = ut_util_invert_8b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = -1;
    exp_out = 0;
    test_res = ut_util_invert_8b(inp, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_util_sat_add_8b(uint8_t x, uint8_t y, uint8_t exp_out)
{
    printf("   Input: x:%d y:%d\n", x, y);

    uint8_t out = util_sat_add_8b(x,y);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_sat_add_8b_test(void)
{
    printf("******************************************************\n");
    printf("uint8_t util_sat_add_8b(uint8_t x, uint8_t y)\n");

    int test_res;
    int pass = 1;

    uint8_t inp_x;
    uint8_t inp_y;
    uint8_t exp_out;

    inp_x = 0;
    inp_y = 0;
    exp_out = 0;
    test_res = ut_util_sat_add_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 5;
    inp_y = 6;
    exp_out = 11;
    test_res = ut_util_sat_add_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 4;
    inp_y = 9;
    exp_out = 13;
    test_res = ut_util_sat_add_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 254;
    inp_y = 1;
    exp_out = 255;
    test_res = ut_util_sat_add_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 1;
    inp_y = 254;
    exp_out = 255;
    test_res = ut_util_sat_add_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 1;
    inp_y = 255;
    exp_out = 255;
    test_res = ut_util_sat_add_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 255;
    inp_y = 1;
    exp_out = 255;
    test_res = ut_util_sat_add_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 255;
    inp_y = 255;
    exp_out = 255;
    test_res = ut_util_sat_add_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    return pass;
}


static int ut_util_sat_subtract_8b(uint8_t x, uint8_t y, uint8_t exp_out)
{
    printf("   Input: x:%d y:%d\n", x, y);

    uint8_t out = util_sat_subtract_8b(x,y);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_sat_subtract_8b_test(void)
{
    printf("******************************************************\n");
    printf("uint8_t util_sat_subtract_8b(uint8_t x, uint8_t y)\n");

    int test_res;
    int pass = 1;

    uint8_t inp_x;
    uint8_t inp_y;
    uint8_t exp_out;

    inp_x = 0;
    inp_y = 0;
    exp_out = 0;
    test_res = ut_util_sat_subtract_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 50;
    inp_y = 10;
    exp_out = 40;
    test_res = ut_util_sat_subtract_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 70;
    inp_y = 0;
    exp_out = 70;
    test_res = ut_util_sat_subtract_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 60;
    inp_y = 50;
    exp_out = 10;
    test_res = ut_util_sat_subtract_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 10;
    inp_y = 50;
    exp_out = 0;
    test_res = ut_util_sat_subtract_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 0;
    inp_y = 255;
    exp_out = 0;
    test_res = ut_util_sat_subtract_8b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_util_sat_add_16b(uint16_t x, uint16_t y, uint16_t exp_out)
{
    printf("   Input: x:%d y:%d\n", x, y);

    uint16_t out = util_sat_add_16b(x,y);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_sat_util_sat_add_16b_test(void)
{
    printf("******************************************************\n");
    printf("uint16_t util_sat_add_16b(uint16_t x, uint16_t y)\n");

    int test_res;
    int pass = 1;

    uint16_t inp_x;
    uint16_t inp_y;
    uint16_t exp_out;

    inp_x = 0;
    inp_y = 0;
    exp_out = 0;
    test_res = ut_util_sat_add_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 10;
    inp_y = 15;
    exp_out = 25;
    test_res = ut_util_sat_add_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 25;
    inp_y = 35;
    exp_out = 60;
    test_res = ut_util_sat_add_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 0;
    inp_y = 0xFFFF;
    exp_out = 0xFFFF;
    test_res = ut_util_sat_add_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 100;
    inp_y = 0xFFFF;
    exp_out = 0xFFFF;
    test_res = ut_util_sat_add_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 45867;
    inp_y = 47841;
    exp_out = 0xFFFF;
    test_res = ut_util_sat_add_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 0xFFFF;
    inp_y = 0xFFFF;
    exp_out = 0xFFFF;
    test_res = ut_util_sat_add_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_util_sat_subtract_16b(uint16_t x, uint16_t y, uint16_t exp_out)
{
    printf("   Input: x:%d y:%d\n", x, y);

    uint16_t out = util_sat_subtract_16b(x,y);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_sat_subtract_16b_test(void)
{
    printf("******************************************************\n");
    printf("uint16_t util_sat_subtract_16b(uint16_t x, uint16_t y)\n");

    int test_res;
    int pass = 1;

    uint16_t inp_x;
    uint16_t inp_y;
    uint16_t exp_out;

    inp_x = 0;
    inp_y = 0;
    exp_out = 0;
    test_res = ut_util_sat_subtract_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 50;
    inp_y = 10;
    exp_out = 40;
    test_res = ut_util_sat_subtract_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 70;
    inp_y = 0;
    exp_out = 70;
    test_res = ut_util_sat_subtract_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 60;
    inp_y = 50;
    exp_out = 10;
    test_res = ut_util_sat_subtract_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 10;
    inp_y = 50;
    exp_out = 0;
    test_res = ut_util_sat_subtract_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 401;
    inp_y = 0x3FFF;
    exp_out = 0;
    test_res = ut_util_sat_subtract_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 0;
    inp_y = 0x7FFF;
    exp_out = 0;
    test_res = ut_util_sat_subtract_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    inp_x = 0;
    inp_y = 0xFFFF;
    exp_out = 0;
    test_res = ut_util_sat_subtract_16b(inp_x, inp_y, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_util_limit_u32b_to_u16b(uint32_t inp, uint16_t exp_out)
{
    printf("   Input: %d\n", inp);

    uint16_t out = util_limit_u32b_to_u16b(inp);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_limit_u32b_to_u16b_test(void)
{
    printf("******************************************************\n");
    printf("uint16_t util_limit_u32b_to_u16b(uint32_t in)\n");

    int test_res;
    int pass = 1;

    uint32_t inp;
    uint16_t exp_out;

    inp = 0;
    exp_out = 0;
    test_res = ut_util_limit_u32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 0x0000FFFE;
    exp_out = 0xFFFE;
    test_res = ut_util_limit_u32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 0x0000FFFF;
    exp_out = 0xFFFF;
    test_res = ut_util_limit_u32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 0x0001FFFF;
    exp_out = 0xFFFF;
    test_res = ut_util_limit_u32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 0x7FFFFFFF;
    exp_out = 0xFFFF;
    test_res = ut_util_limit_u32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 0xFFFFFFFF;
    exp_out = 0xFFFF;
    test_res = ut_util_limit_u32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_util_limit_s32b_to_u16b(int32_t inp, uint16_t exp_out)
{
    printf("   Input: %d\n", inp);

    uint16_t out = util_limit_s32b_to_u16b(inp);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_limit_s32b_to_u16b_test(void)
{
    printf("******************************************************\n");
    printf("uint16_t util_limit_s32b_to_u16b(int32_t in)\n");

    int test_res;
    int pass = 1;

    int32_t inp;
    uint16_t exp_out;

    inp = 0;
    exp_out = 0;
    test_res = ut_util_limit_s32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = -1;
    exp_out = 0;
    test_res = ut_util_limit_s32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = -32768;
    exp_out = 0;
    test_res = ut_util_limit_s32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = -2147483648;
    exp_out = 0;
    test_res = ut_util_limit_s32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 1;
    exp_out = 1;
    test_res = ut_util_limit_s32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 32767;
    exp_out = 32767;
    test_res = ut_util_limit_s32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 65535;
    exp_out = 0xFFFF;
    test_res = ut_util_limit_s32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 1000000000;
    exp_out = 0xFFFF;
    test_res = ut_util_limit_s32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    inp = 2147483647;
    exp_out = 0xFFFF;
    test_res = ut_util_limit_s32b_to_u16b(inp, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_util_convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset, uint16_t exp_out)
{
    printf("   Input: Raw:%d Mul:%d Div:%d Offset:%d\n", raw, mul, div, offset);
    uint16_t out = util_convert_muldivoff(raw, mul, div, offset);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_convert_muldivoff_test(void)
{
    printf("******************************************************\n");
    printf("uint16_t util_convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset) \n");

    int test_res;
    int pass = 1;

    uint16_t raw;
    uint8_t mul;
    uint8_t div;
    int16_t offset;
    uint16_t exp_out;

    // Normal case
    raw = 100;
    mul = 1;
    div = 1;
    offset = 100;
    exp_out = 200;
    test_res = ut_util_convert_muldivoff(raw, mul, div, offset, exp_out);
    if(!test_res) pass = 0;

    // Normal case
    raw = 0;
    mul = 1;
    div = 1;
    offset = 0;
    exp_out = 0;
    test_res = ut_util_convert_muldivoff(raw, mul, div, offset, exp_out);
    if(!test_res) pass = 0;

    // max raw
    raw = 0xFFFF;
    mul = 1;
    div = 1;
    offset = 0;
    exp_out = 0xFFFF;
    test_res = ut_util_convert_muldivoff(raw, mul, div, offset, exp_out);
    if(!test_res) pass = 0;

    // Saturation +
    raw = 40000;
    mul = 1;
    div = 1;
    offset = 30000;
    exp_out = 0xFFFF;
    test_res = ut_util_convert_muldivoff(raw, mul, div, offset, exp_out);
    if(!test_res) pass = 0;

    // Saturation -
    raw = 20000;
    mul = 1;
    div = 1;
    offset = -30000;
    exp_out = 0;
    test_res = ut_util_convert_muldivoff(raw, mul, div, offset, exp_out);
    if(!test_res) pass = 0;

    // Multiplier
    raw = 10;
    mul = 30;
    div = 1;
    offset = 200;
    exp_out = 500;
    test_res = ut_util_convert_muldivoff(raw, mul, div, offset, exp_out);
    if(!test_res) pass = 0;

    // Divider
    raw = 10;
    mul = 30;
    div = 6;
    offset = 200;
    exp_out = 250;
    test_res = ut_util_convert_muldivoff(raw, mul, div, offset, exp_out);
    if(!test_res) pass = 0;

    // Zero mul
    raw = 10;
    mul = 0;
    div = 6;
    offset = 99;
    exp_out = 99;
    test_res = ut_util_convert_muldivoff(raw, mul, div, offset, exp_out);
    if(!test_res) pass = 0;

    // Zero div
    raw = 10;
    mul = 2;
    div = 0;
    offset = 80;
    exp_out = 100;
    test_res = ut_util_convert_muldivoff(raw, mul, div, offset, exp_out);
    if(!test_res) pass = 0;

    // Zero div
    raw = 0xFFFE;
    mul = 255;
    div = 255;
    offset = -1;
    exp_out = 0xFFFD;
    test_res = ut_util_convert_muldivoff(raw, mul, div, offset, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_util_sat_mul_kilo(uint16_t xk, uint16_t yk, uint16_t exp_out)
{
    printf("   Input: X:%d Y:%d \n", xk, yk);

    uint16_t out = util_sat_mul_kilo(xk, yk);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_sat_mul_kilo_test(void)
{
    printf("******************************************************\n");
    printf("uint16_t util_sat_mul_kilo(uint16_t xk, uint16_t yk) \n");

    int test_res;
    int pass = 1;

    uint16_t xk;
    uint16_t yk;
    uint16_t exp_out;

    // Normal case
    xk = 1000;
    yk = 1000;
    exp_out = 1000;
    test_res = ut_util_sat_mul_kilo(xk, yk, exp_out);
    if(!test_res) pass = 0;

    // Normal case
    xk = 60000;
    yk = 1;
    exp_out = 60;
    test_res = ut_util_sat_mul_kilo(xk, yk, exp_out);
    if(!test_res) pass = 0;

    // Normal case
    xk = 2;
    yk = 30000;
    exp_out = 60;
    test_res = ut_util_sat_mul_kilo(xk, yk, exp_out);
    if(!test_res) pass = 0;

    // Normal case
    xk = 0xFFFF;
    yk = 0xFFFF;
    exp_out = 0xFFFF;
    test_res = ut_util_sat_mul_kilo(xk, yk, exp_out);
    if(!test_res) pass = 0;

    // Normal case
    xk = 8095;
    yk = 8095;
    exp_out = 65529;
    test_res = ut_util_sat_mul_kilo(xk, yk, exp_out);
    if(!test_res) pass = 0;

    // Saturation
    xk = 10000;
    yk = 20000;
    exp_out = 0xFFFF;
    test_res = ut_util_sat_mul_kilo(xk, yk, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_util_sat_div_kilo(uint16_t top, uint16_t bot, uint16_t exp_out)
{
    printf("   Input: Top:%d Bottom:%d \n", top, bot);

    uint16_t out = util_sat_div_kilo(top, bot);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_sat_div_kilo_test(void)
{
    printf("******************************************************\n");
    printf("uint16_t util_sat_div_kilo(uint16_t top, uint16_t bot) \n");

    int test_res;
    int pass = 1;

    uint16_t top;
    uint16_t bot;
    uint16_t exp_out;

    // Normal case
    top = 1000;
    bot = 1000;
    exp_out = 1000;
    test_res = ut_util_sat_div_kilo(top, bot, exp_out);
    if(!test_res) pass = 0;

     // Normal case
    top = 12000;
    bot = 6000;
    exp_out = 2000;
    test_res = ut_util_sat_div_kilo(top, bot, exp_out);
    if(!test_res) pass = 0;

    // Saturation
    top = 40000;
    bot = 500;
    exp_out = 0xFFFF;
    test_res = ut_util_sat_div_kilo(top, bot, exp_out);
    if(!test_res) pass = 0;

    // Zero
    top = 40000;
    bot = 0;
    exp_out = 0xFFFF;
    test_res = ut_util_sat_div_kilo(top, bot, exp_out);
    if(!test_res) pass = 0;

    // Zero
    top = 0;
    bot = 2000;
    exp_out = 0;
    test_res = ut_util_sat_div_kilo(top, bot, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_util_sat_ratio_16b(uint16_t top, uint16_t bot, uint16_t exp_out)
{
    printf("   Input: Top:%d Bottom:%d \n", top, bot);

    uint16_t out = util_sat_ratio_16b(top, bot);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_sat_ratio_16b_test(void)
{
    printf("******************************************************\n");
    printf("uint16_t util_sat_ratio_16b(uint16_t top, uint16_t bot) \n");

    int test_res;
    int pass = 1;

    uint16_t top;
    uint16_t bot;
    uint16_t exp_out;

    // Normal case
    top = 1000;
    bot = 1000;
    exp_out = 0xFFFF;
    test_res = ut_util_sat_ratio_16b(top, bot, exp_out);
    if(!test_res) pass = 0;

    // Normal case
    top = 0xFFFF;
    bot = 0xFFFF;
    exp_out = 0xFFFF;
    test_res = ut_util_sat_ratio_16b(top, bot, exp_out);
    if(!test_res) pass = 0;

    // Normal case
    top = 6000;
    bot = 12000;
    exp_out = 0x7FFF;
    test_res = ut_util_sat_ratio_16b(top, bot, exp_out);
    if(!test_res) pass = 0;

    // Normal case
    top = 11000;
    bot = 44000;
    exp_out = 0x3FFF;
    test_res = ut_util_sat_ratio_16b(top, bot, exp_out);
    if(!test_res) pass = 0;

    // Zero
    top = 0;
    bot = 212;
    exp_out = 0;
    test_res = ut_util_sat_ratio_16b(top, bot, exp_out);
    if(!test_res) pass = 0;

    // Zero
    top = 158;
    bot = 0;
    exp_out = 0xFFFF;
    test_res = ut_util_sat_ratio_16b(top, bot, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_util_percent_to_16b(uint8_t percent, uint16_t exp_out)
{
    printf("   Input: Percent:%d \n", percent);

    uint16_t out = util_percent_to_16b(percent);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_percent_to_16b_test(void)
{
    printf("******************************************************\n");
    printf("uint16_t util_percent_to_16b(uint8_t percent) \n");

    int test_res;
    int pass = 1;

    uint8_t percent;
    uint16_t exp_out;

    // 0%
    percent = 0;
    exp_out = 0;
    test_res = ut_util_percent_to_16b(percent, exp_out);
    if(!test_res) pass = 0;

    // 25%
    percent = 25;
    exp_out = 16383;
    test_res = ut_util_percent_to_16b(percent, exp_out);
    if(!test_res) pass = 0;

    // 50%
    percent = 50;
    exp_out = 0x7FFF;
    test_res = ut_util_percent_to_16b(percent, exp_out);
    if(!test_res) pass = 0;

    // 75%
    percent = 75;
    exp_out = 49151;
    test_res = ut_util_percent_to_16b(percent, exp_out);
    if(!test_res) pass = 0;

    // 100%
    percent = 100;
    exp_out = 65535;
    test_res = ut_util_percent_to_16b(percent, exp_out);
    if(!test_res) pass = 0;

    // 200%
    percent = 200;
    exp_out = 0xFFFF;
    test_res = ut_util_percent_to_16b(percent, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_util_interpolate_1d_u16b(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis, uint16_t exp_out)
{
    printf("   Input: X:%d \n", x);
    printf("X values, Y values\n");
    for(uint8_t i=0; i<len_axis; i++)
    {
        printf("%d, %d \n", x_axis[i], y_values[i]);
    }

    uint16_t out = util_interpolate_1d_u16b(x, x_axis, y_values, len_axis);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_interpolate_1d_u16b_test(void)
{
    printf("******************************************************\n");
    printf("uint16_t util_interpolate_1d_u16b(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis) \n");

    int test_res;
    int pass = 1;

    uint16_t x;
    uint16_t x_axis[4];
    uint16_t y_values[4];
    uint8_t len_axis = 4;
    uint16_t exp_out;

    // Below X0
    x = 500;
    x_axis[0] = 1000;
    x_axis[1] = 2000;
    x_axis[2] = 3000;
    x_axis[3] = 4000;
    y_values[0] = 5000;
    y_values[1] = 6000;
    y_values[2] = 7000;
    y_values[3] = 8000;
    len_axis = 4;
    exp_out = 5000;
    test_res = ut_util_interpolate_1d_u16b(x, x_axis, y_values, len_axis, exp_out);
    if(!test_res) pass = 0;

    // 1st
    x = 1500;
    x_axis[0] = 1000;
    x_axis[1] = 2000;
    x_axis[2] = 3000;
    x_axis[3] = 4000;
    y_values[0] = 5000;
    y_values[1] = 6000;
    y_values[2] = 7000;
    y_values[3] = 8000;
    len_axis = 4;
    exp_out = 5500;
    test_res = ut_util_interpolate_1d_u16b(x, x_axis, y_values, len_axis, exp_out);
    if(!test_res) pass = 0;

    // 2nd
    x = 2750;
    x_axis[0] = 1000;
    x_axis[1] = 2000;
    x_axis[2] = 3000;
    x_axis[3] = 4000;
    y_values[0] = 5000;
    y_values[1] = 6000;
    y_values[2] = 7000;
    y_values[3] = 8000;
    len_axis = 4;
    exp_out = 6750;
    test_res = ut_util_interpolate_1d_u16b(x, x_axis, y_values, len_axis, exp_out);
    if(!test_res) pass = 0;

    // 3rd
    x = 3300;
    x_axis[0] = 1000;
    x_axis[1] = 2000;
    x_axis[2] = 3000;
    x_axis[3] = 4000;
    y_values[0] = 5000;
    y_values[1] = 6000;
    y_values[2] = 7000;
    y_values[3] = 8000;
    len_axis = 4;
    exp_out = 7300;
    test_res = ut_util_interpolate_1d_u16b(x, x_axis, y_values, len_axis, exp_out);
    if(!test_res) pass = 0;

    // Above X3
    x = 5000;
    x_axis[0] = 1000;
    x_axis[1] = 2000;
    x_axis[2] = 3000;
    x_axis[3] = 4000;
    y_values[0] = 5000;
    y_values[1] = 6000;
    y_values[2] = 7000;
    y_values[3] = 8000;
    len_axis = 4;
    exp_out = 8000;
    test_res = ut_util_interpolate_1d_u16b(x, x_axis, y_values, len_axis, exp_out);
    if(!test_res) pass = 0;

    // On point
    x = 2000;
    x_axis[0] = 1000;
    x_axis[1] = 2000;
    x_axis[2] = 3000;
    x_axis[3] = 4000;
    y_values[0] = 5000;
    y_values[1] = 6000;
    y_values[2] = 7000;
    y_values[3] = 8000;
    len_axis = 4;
    exp_out = 6000;
    test_res = ut_util_interpolate_1d_u16b(x, x_axis, y_values, len_axis, exp_out);
    if(!test_res) pass = 0;

    // Triangle function 1
    x = 2600;
    x_axis[0] = 1000;
    x_axis[1] = 2000;
    x_axis[2] = 3000;
    x_axis[3] = 4000;
    y_values[0] = 5000;
    y_values[1] = 6000;
    y_values[2] = 5000;
    y_values[3] = 6000;
    len_axis = 4;
    exp_out = 5400;
    test_res = ut_util_interpolate_1d_u16b(x, x_axis, y_values, len_axis, exp_out);
    if(!test_res) pass = 0;

    // Triangle function 2
    x = 3600;
    x_axis[0] = 1000;
    x_axis[1] = 2000;
    x_axis[2] = 3000;
    x_axis[3] = 4000;
    y_values[0] = 5000;
    y_values[1] = 6000;
    y_values[2] = 5000;
    y_values[3] = 6000;
    len_axis = 4;
    exp_out = 5600;
    test_res = ut_util_interpolate_1d_u16b(x, x_axis, y_values, len_axis, exp_out);
    if(!test_res) pass = 0;

    // Wrong X values order 1
    x = 2100;
    x_axis[0] = 1000;
    x_axis[1] = 2000;
    x_axis[2] = 3000;
    x_axis[3] = 2157;
    y_values[0] = 5000;
    y_values[1] = 6000;
    y_values[2] = 7000;
    y_values[3] = 8000;
    len_axis = 4;
    exp_out = 6100;
    test_res = ut_util_interpolate_1d_u16b(x, x_axis, y_values, len_axis, exp_out);
    if(!test_res) pass = 0;

    // Wrong X values order 2
    x = 3500;
    x_axis[0] = 1000;
    x_axis[1] = 2000;
    x_axis[2] = 1440;
    x_axis[3] = 541;
    y_values[0] = 5000;
    y_values[1] = 6000;
    y_values[2] = 7000;
    y_values[3] = 8000;
    len_axis = 4;
    exp_out = 8000;
    test_res = ut_util_interpolate_1d_u16b(x, x_axis, y_values, len_axis, exp_out);
    if(!test_res) pass = 0;

    return pass;
}

static int ut_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, uint16_t exp_out)
{
    printf("   Input: X:%d Y:%d \n", x, y);

    uint16_t out = util_interpolate_2d_u16b(x, y, x_axis, len_x_axis, y_axis, len_y_axis, z_values);

    printf("  Output:%d\n", out);
    printf("Expected:%d\n", exp_out);

    if(out==exp_out)
    {
        printf("PASS\n\n");
        return 1;
    }
    else
    {
        printf("FAIL\n\n");
        return 0;
    }
}

int ut_util_interpolate_2d_u16b_test(void)
{
    printf("******************************************************\n");
    printf("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) \n");

    int test_res;
    int pass = 1;

    uint16_t x;
    uint16_t y;
    uint16_t x_axis[2];
    uint8_t len_x_axis;
    uint16_t y_axis[2];
    uint8_t len_y_axis;
    uint16_t z_values[2][2];
    uint16_t exp_out;

    // Midpoint
    x = 500;
    y = 500;
    x_axis[0] = 0;
    x_axis[1] = 1000;
    len_x_axis = 2;
    y_axis[0] = 0;
    y_axis[1] = 1000;
    len_y_axis = 2;
    z_values[0][0] = 0;
    z_values[0][1] = 1000;
    z_values[1][0] = 2000;
    z_values[1][1] = 3000;
    exp_out = 1500;
    test_res = ut_util_interpolate_2d_u16b(x, y, x_axis, len_x_axis, y_axis, len_y_axis, &z_values[0][0], exp_out);
    if(!test_res) pass = 0;

    // Corner 0,0
    x = 0;
    y = 0;
    x_axis[0] = 0;
    x_axis[1] = 1000;
    len_x_axis = 2;
    y_axis[0] = 0;
    y_axis[1] = 1000;
    len_y_axis = 2;
    z_values[0][0] = 0;
    z_values[0][1] = 1000;
    z_values[1][0] = 2000;
    z_values[1][1] = 3000;
    exp_out = 0;
    test_res = ut_util_interpolate_2d_u16b(x, y, x_axis, len_x_axis, y_axis, len_y_axis, &z_values[0][0], exp_out);
    if(!test_res) pass = 0;

    // Corner 0,1
    x = 1500;
    y = 0;
    x_axis[0] = 0;
    x_axis[1] = 1000;
    len_x_axis = 2;
    y_axis[0] = 0;
    y_axis[1] = 1000;
    len_y_axis = 2;
    z_values[0][0] = 0;
    z_values[0][1] = 1000;
    z_values[1][0] = 2000;
    z_values[1][1] = 3000;
    exp_out = 1000;
    test_res = ut_util_interpolate_2d_u16b(x, y, x_axis, len_x_axis, y_axis, len_y_axis, &z_values[0][0], exp_out);
    if(!test_res) pass = 0;

    // Corner 1,0
    x = 0;
    y = 1500;
    x_axis[0] = 0;
    x_axis[1] = 1000;
    len_x_axis = 2;
    y_axis[0] = 0;
    y_axis[1] = 1000;
    len_y_axis = 2;
    z_values[0][0] = 0;
    z_values[0][1] = 1000;
    z_values[1][0] = 2000;
    z_values[1][1] = 3000;
    exp_out = 2000;
    test_res = ut_util_interpolate_2d_u16b(x, y, x_axis, len_x_axis, y_axis, len_y_axis, &z_values[0][0], exp_out);
    if(!test_res) pass = 0;

    // Corner 1,1
    x = 1500;
    y = 1500;
    x_axis[0] = 0;
    x_axis[1] = 1000;
    len_x_axis = 2;
    y_axis[0] = 0;
    y_axis[1] = 1000;
    len_y_axis = 2;
    z_values[0][0] = 0;
    z_values[0][1] = 1000;
    z_values[1][0] = 2000;
    z_values[1][1] = 3000;
    exp_out = 3000;
    test_res = ut_util_interpolate_2d_u16b(x, y, x_axis, len_x_axis, y_axis, len_y_axis, &z_values[0][0], exp_out);
    if(!test_res) pass = 0;

    // X axis mid
    x = 500;
    y = 0;
    x_axis[0] = 0;
    x_axis[1] = 1000;
    len_x_axis = 2;
    y_axis[0] = 0;
    y_axis[1] = 1000;
    len_y_axis = 2;
    z_values[0][0] = 0;
    z_values[0][1] = 1000;
    z_values[1][0] = 2000;
    z_values[1][1] = 3000;
    exp_out = 500;
    test_res = ut_util_interpolate_2d_u16b(x, y, x_axis, len_x_axis, y_axis, len_y_axis, &z_values[0][0], exp_out);
    if(!test_res) pass = 0;

    // Other X axis mid
    x = 500;
    y = 1000;
    x_axis[0] = 0;
    x_axis[1] = 1000;
    len_x_axis = 2;
    y_axis[0] = 0;
    y_axis[1] = 1000;
    len_y_axis = 2;
    z_values[0][0] = 0;
    z_values[0][1] = 1000;
    z_values[1][0] = 2000;
    z_values[1][1] = 3000;
    exp_out = 2500;
    test_res = ut_util_interpolate_2d_u16b(x, y, x_axis, len_x_axis, y_axis, len_y_axis, &z_values[0][0], exp_out);
    if(!test_res) pass = 0;

    // Y axis mid
    x = 0;
    y = 500;
    x_axis[0] = 0;
    x_axis[1] = 1000;
    len_x_axis = 2;
    y_axis[0] = 0;
    y_axis[1] = 1000;
    len_y_axis = 2;
    z_values[0][0] = 0;
    z_values[0][1] = 1000;
    z_values[1][0] = 2000;
    z_values[1][1] = 3000;
    exp_out = 1000;
    test_res = ut_util_interpolate_2d_u16b(x, y, x_axis, len_x_axis, y_axis, len_y_axis, &z_values[0][0], exp_out);
    if(!test_res) pass = 0;

    // Other Y axis mid
    x = 1000;
    y = 500;
    x_axis[0] = 0;
    x_axis[1] = 1000;
    len_x_axis = 2;
    y_axis[0] = 0;
    y_axis[1] = 1000;
    len_y_axis = 2;
    z_values[0][0] = 0;
    z_values[0][1] = 1000;
    z_values[1][0] = 2000;
    z_values[1][1] = 3000;
    exp_out = 2000;
    test_res = ut_util_interpolate_2d_u16b(x, y, x_axis, len_x_axis, y_axis, len_y_axis, &z_values[0][0], exp_out);
    if(!test_res) pass = 0;

    return pass;
}