/*
 * math_util.c
 *
 *  Created on: 2/08/2021
 *      Author: alexrayne <alexraynepe196@gmail.com>
  ------------------------------------------------------------------------
    Copyright (c) alexrayne
 */

#include "math_util.h"



/// @brief compares a == b with tolerance
/// @return true if  a +-tol == b+-tol
bool is_samef(float a, float b, float tol){
    return ((a + tol) > (b - tol))
            && ((a - tol) < (b + tol))
            ;
}

bool in_rangef(const float x, const float l, const float h){
    if (x < l)
        return false;
    if (x > h)
        return false;
    return true;
}



//*****************************************************************

/**
 * \brief    Fast Square root algorithm
 *
 * Fractional parts of the answer are discarded. That is:
 *      - SquareRoot(3) --> 1
 *      - SquareRoot(4) --> 2
 *      - SquareRoot(5) --> 2
 *      - SquareRoot(8) --> 2
 *      - SquareRoot(9) --> 3
 *
 * \param[in] a_nInput - unsigned integer for which to find the square root
 *
 * \return Integer square root of the input value.
 */

unsigned        sqrt32( uint32_t x)
{
    uint32_t op  = x;
    unsigned res = 0;
    enum { Bits = sizeof(x)*8 , };
    uint32_t one = 1uL << (Bits-2); // The second-to-top bit is set: use 1u << 14 for uint16_t type; use 1uL<<30 for uint32_t type


    // "one" starts at the highest power of four <= than the argument.
    while (one > op)
    {
        one >>= 2;
    }

    while (one != 0)
    {
        if (op >= res + one)
        {
            op = op - (res + one);
            res = res +  2 * one;
        }
        res >>= 1;
        one >>= 2;
    }

    /* Un comment to do rounding to nearest integer */
    if (op > res)
    {
        res++;
    }

    return res;
}

unsigned        sqrt32_ceil( uint32_t x){
    uint32_t op  = x;
    uint32_t res = 0;
    enum { Bits = sizeof(x)*8 , };
    uint32_t one = 1uL << (Bits-2); // The second-to-top bit is set: use 1u << 14 for uint16_t type; use 1uL<<30 for uint32_t type


    // "one" starts at the highest power of four <= than the argument.
    while (one > op)
    {
        one >>= 2;
    }

    while (one != 0)
    {
        if (op >= res + one)
        {
            op = op - (res + one);
            res = res +  2 * one;
        }
        res >>= 1;
        one >>= 2;
    }

    /* Un comment to do rounding to nearest integer */
    if (op > 0)
    {
        res++;
    }

    return res;
}

unsigned        sqrt32_floor( uint32_t x){
    uint32_t op  = x;
    uint32_t res = 0;
    enum { Bits = sizeof(x)*8 , };
    uint32_t one = 1uL << (Bits-2); // The second-to-top bit is set: use 1u << 14 for uint16_t type; use 1uL<<30 for uint32_t type


    // "one" starts at the highest power of four <= than the argument.
    while (one > op)
    {
        one >>= 2;
    }

    while (one != 0)
    {
        if (op >= res + one)
        {
            op = op - (res + one);
            res = res +  2 * one;
        }
        res >>= 1;
        one >>= 2;
    }

    return res;
}




uint32_t        sqrt64( uint64_t x){
    uint64_t op  = x;
    uint32_t res = 0;
    enum { Bits = sizeof(x)*8 , };
    uint64_t one = 1uLL << (Bits-2); // The second-to-top bit is set: use 1u << 14 for uint16_t type; use 1uL<<30 for uint32_t type


    // "one" starts at the highest power of four <= than the argument.
    while (one > op)
    {
        one >>= 2;
    }

    while (one != 0)
    {
        if (op >= res + one)
        {
            op = op - (res + one);
            res = res +  2 * one;
        }
        res >>= 1;
        one >>= 2;
    }

    /* Un comment to do rounding to nearest integer */
    if (op > res)
    {
        res++;
    }

    return res;
}

uint32_t        sqrt64_ceil( uint64_t x){
    uint64_t op  = x;
    uint32_t res = 0;
    enum { Bits = sizeof(x)*8 , };
    uint64_t one = 1uLL << (Bits-2); // The second-to-top bit is set: use 1u << 14 for uint16_t type; use 1uL<<30 for uint32_t type


    // "one" starts at the highest power of four <= than the argument.
    while (one > op)
    {
        one >>= 2;
    }

    while (one != 0)
    {
        if (op >= res + one)
        {
            op = op - (res + one);
            res = res +  2 * one;
        }
        res >>= 1;
        one >>= 2;
    }

    /* Un comment to do rounding to nearest integer */
    if (op > 0)
    {
        res++;
    }

    return res;
}


uint32_t        sqrt64_floor( uint64_t x){
    uint64_t op  = x;
    uint32_t res = 0;
    enum { Bits = sizeof(x)*8 , };
    uint64_t one = 1uLL << (Bits-2); // The second-to-top bit is set: use 1u << 14 for uint16_t type; use 1uL<<30 for uint32_t type


    // "one" starts at the highest power of four <= than the argument.
    while (one > op)
    {
        one >>= 2;
    }

    while (one != 0)
    {
        if (op >= res + one)
        {
            op = op - (res + one);
            res = res +  2 * one;
        }
        res >>= 1;
        one >>= 2;
    }

    return res;
}


long mul_ml( unsigned long m, long x) {
    unsigned long ux = (x >= 0)? x : -x;
    unsigned long long tmp = (unsigned long long)ux * m;
    ux = qround64(tmp, 32);
    if (x >= 0)
        return ux;
    else
        return -ux;
}

long mul_ms( unsigned short m, short x){
    unsigned short ux = (x >= 0)? x : -x;
    unsigned long tmp = (unsigned long )ux * m;
    ux = qround32(tmp, 16);
    if (x >= 0)
        return ux;
    else
        return -ux;
}



//*****************************************************************
uintptr_t   align_up(uintptr_t x, unsigned page){
    uintptr_t mask = page-1;
    return (x+mask) & ~mask;
}

/// @return x aligned to top of page = page(+1)-1
uintptr_t   align_top(uintptr_t x, unsigned page){
    return align_up(x, page)-1;
}


uintptr_t   align_dn(uintptr_t x, unsigned page){
    uintptr_t mask = page-1;
    return x & ~mask;
}

/// @return x aligned to top of page = page(+1)-1
unsigned   align_full_pages(unsigned x, unsigned page){
    x += page-1;
    x = x >> ctz(page);
    return x;
}
