#include "fixedpoint.h"

#include <limits.h>
#include <stddef.h>

static const int32_t s_fixed_atan_table_q30[] = {
    843314857, 497837829, 263043836, 133525159,
    67021687,  33543516,  16775851,  8388437,
    4194283,   2097149,   1048575,   524288,
    262144,    131072,    65536,     32768,
};

static uint32_t fixed_isqrt_u64(uint64_t value)
{
    uint64_t bit = (uint64_t)1 << 62;
    uint64_t result = 0;

    while (bit > value)
    {
        bit >>= 2;
    }

    while (bit != 0)
    {
        if (value >= result + bit)
        {
            value -= result + bit;
            result = (result >> 1) + bit;
        }
        else
        {
            result >>= 1;
        }

        bit >>= 2;
    }

    return (uint32_t)result;
}

int32_t fixed_sqrt(int32_t value, uint8_t frac_bits)
{
    uint64_t scaled_value;
    uint32_t root;

    if (value <= 0)
    {
        return 0;
    }

    scaled_value = ((uint64_t)(uint32_t)value) << frac_bits;
    root = fixed_isqrt_u64(scaled_value);
    if (root > (uint32_t)INT32_MAX)
    {
        return INT32_MAX;
    }

    return (int32_t)root;
}

int32_t fixed_inv_sqrt(int32_t value, uint8_t frac_bits)
{
    int32_t root;

    if (value <= 0)
    {
        return 0;
    }

    root = fixed_sqrt(value, frac_bits);
    return FIXED_DIV(FIXED_ONE(frac_bits), root, frac_bits);
}

int32_t fixed_atan2(int32_t y, int32_t x, uint8_t frac_bits)
{
    int64_t x_q30;
    int64_t y_q30;
    int64_t angle_q30 = 0;
    int shift;

    if (x == 0)
    {
        if (y > 0)
        {
            return FIXED_HALF_PI(frac_bits);
        }
        if (y < 0)
        {
            return -FIXED_HALF_PI(frac_bits);
        }
        return 0;
    }

    if (x < 0)
    {
        if (y >= 0)
        {
            return fixed_atan2(-y, -x, frac_bits) + FIXED_PI(frac_bits);
        }
        return fixed_atan2(-y, -x, frac_bits) - FIXED_PI(frac_bits);
    }

    shift = 30 - frac_bits;
    if (shift >= 0)
    {
        x_q30 = (int64_t)x << shift;
        y_q30 = (int64_t)y << shift;
    }
    else
    {
        x_q30 = (int64_t)x >> (-shift);
        y_q30 = (int64_t)y >> (-shift);
    }

    for (size_t i = 0; i < sizeof(s_fixed_atan_table_q30) / sizeof(s_fixed_atan_table_q30[0]); ++i)
    {
        int64_t x_next;
        int64_t y_next;

        if (y_q30 > 0)
        {
            x_next = x_q30 + (y_q30 >> i);
            y_next = y_q30 - (x_q30 >> i);
            angle_q30 += s_fixed_atan_table_q30[i];
        }
        else
        {
            x_next = x_q30 - (y_q30 >> i);
            y_next = y_q30 + (x_q30 >> i);
            angle_q30 -= s_fixed_atan_table_q30[i];
        }

        x_q30 = x_next;
        y_q30 = y_next;
    }

    if (shift >= 0)
    {
        return (int32_t)(angle_q30 >> shift);
    }
    return (int32_t)(angle_q30 << (-shift));
}

int32_t fixed_asin(int32_t value, uint8_t frac_bits)
{
    const int32_t one = FIXED_ONE(frac_bits);
    const int32_t clamped = FIXED_CLAMP(value, -one, one);
    const int32_t one_minus_square = one - FIXED_MUL(clamped, clamped, frac_bits);

    if (one_minus_square <= 0)
    {
        return clamped >= 0 ? FIXED_HALF_PI(frac_bits) : -FIXED_HALF_PI(frac_bits);
    }

    return fixed_atan2(clamped, fixed_sqrt(one_minus_square, frac_bits), frac_bits);
}