/* fixed.c - Q16.16 Fixed-Point Math Implementation */

#include "fixed.h"

/* Conversion functions */
fixed_t int_to_fixed(int32_t i) {
    return i << 16;
}

int32_t fixed_to_int(fixed_t f) {
    return f >> 16;
}

fixed_t float_to_fixed(float f) {
    return (fixed_t)(f * 65536.0f);
}

float fixed_to_float(fixed_t f) {
    return (float)f / 65536.0f;
}

fixed_t fixed_add(fixed_t a, fixed_t b) {
    return a + b;
}

fixed_t fixed_sub(fixed_t a, fixed_t b) {
    return a - b;
}

fixed_t fixed_mul(fixed_t a, fixed_t b) {
    /* Extract high and low parts */
    int32_t a_hi = a >> 16;
    uint32_t a_lo = (uint32_t)a & 0xFFFFU;
    int32_t b_hi = b >> 16;
    uint32_t b_lo = (uint32_t)b & 0xFFFFU;
    
    /* Compute partial products */
    int32_t p0 = (int32_t)(a_lo * b_lo) >> 16;  /* Low * Low */
    int32_t p1 = a_hi * (int32_t)b_lo;          /* High * Low */
    int32_t p2 = (int32_t)a_lo * b_hi;          /* Low * High */
    int32_t p3 = (a_hi * b_hi) << 16;           /* High * High */
    
    /* Combine results */
    return p0 + p1 + p2 + p3;
}

fixed_t fixed_div(fixed_t a, fixed_t b) {
    if (b == 0) return 0; /* Handle division by zero */
    
    /* Determine sign */
    int negative = ((a < 0) ^ (b < 0));
    
    /* Work with absolute values */
    uint32_t ua = (a < 0) ? -a : a;
    uint32_t ub = (b < 0) ? -b : b;
    
    /* Perform division using long division in base 2^16 */
    uint32_t quotient = 0;
    uint32_t remainder = 0;
    int i;
    
    for (i = 0; i < 32; i++) {
        remainder <<= 1;
        if (ua & 0x80000000U) {
            remainder |= 1;
        }
        ua <<= 1;
        
        if (remainder >= ub) {
            remainder -= ub;
            quotient |= 1;
        }
        
        if (i < 31) {
            quotient <<= 1;
        }
    }
    
    return negative ? -(int32_t)quotient : (int32_t)quotient;
}

int fixed_eq(fixed_t a, fixed_t b) {
    return a == b;
}

int fixed_ne(fixed_t a, fixed_t b) {
    return a != b;
}

int fixed_lt(fixed_t a, fixed_t b) {
    return a < b;
}

int fixed_le(fixed_t a, fixed_t b) {
    return a <= b;
}

int fixed_gt(fixed_t a, fixed_t b) {
    return a > b;
}

int fixed_ge(fixed_t a, fixed_t b) {
    return a >= b;
}

/* Unary operations */
fixed_t fixed_neg(fixed_t f) {
    return -f;
}

fixed_t fixed_abs(fixed_t f) {
    return (f < 0) ? -f : f;
}

/* Square root using Newton-Raphson method */
fixed_t fixed_sqrt(fixed_t f) {
    if (f <= 0) return 0;
    
    fixed_t x = f;
    fixed_t prev;
    
    /* Newton-Raphson iteration: x = (x + f/x) / 2 */
    do {
        prev = x;
        x = fixed_div(fixed_add(x, fixed_div(f, x)), int_to_fixed(2));
    } while (fixed_gt(fixed_abs(fixed_sub(x, prev)), 1)); /* Precision to 1/65536 */
    
    return x;
}

/* Sine function using Taylor series */
fixed_t fixed_sin(fixed_t f) {
    /* Normalize angle to [-π, π] */
    fixed_t pi2 = fixed_mul(FIXED_PI, int_to_fixed(2));
    while (fixed_gt(f, FIXED_PI)) f = fixed_sub(f, pi2);
    while (fixed_lt(f, fixed_neg(FIXED_PI))) f = fixed_add(f, pi2);
    
    /* Taylor series: sin(x) = x - x³/3! + x⁵/5! - x⁷/7! + ... */
    fixed_t result = f;
    fixed_t term = f;
    fixed_t f_squared = fixed_mul(f, f);
    
    /* Calculate first few terms for reasonable precision */
    int i;
    for (i = 3; i <= 11; i += 2) {
        term = fixed_mul(term, f_squared);
        term = fixed_div(term, int_to_fixed(i * (i - 1)));
        
        if ((i / 2) % 2 == 0) {
            result = fixed_add(result, term);
        } else {
            result = fixed_sub(result, term);
        }
    }
    
    return result;
}

/* Cosine function using Taylor series */
fixed_t fixed_cos(fixed_t f) {
    /* cos(x) = 1 - x²/2! + x⁴/4! - x⁶/6! + ... */
    fixed_t result = FIXED_ONE;
    fixed_t term = FIXED_ONE;
    fixed_t f_squared = fixed_mul(f, f);
    
    int i;
    for (i = 2; i <= 12; i += 2) {
        term = fixed_mul(term, f_squared);
        term = fixed_div(term, int_to_fixed(i * (i - 1)));
        
        if ((i / 2) % 2 == 0) {
            result = fixed_add(result, term);
        } else {
            result = fixed_sub(result, term);
        }
    }
    
    return result;
}

/* Tangent function */
fixed_t fixed_tan(fixed_t f) {
    fixed_t cos_val = fixed_cos(f);
    if (cos_val == 0) return 0; /* Handle undefined case */
    return fixed_div(fixed_sin(f), cos_val);
}

/* Utility functions */
fixed_t fixed_min(fixed_t a, fixed_t b) {
    return (a < b) ? a : b;
}

fixed_t fixed_max(fixed_t a, fixed_t b) {
    return (a > b) ? a : b;
}

fixed_t fixed_clamp(fixed_t f, fixed_t min_val, fixed_t max_val) {
    if (f < min_val) return min_val;
    if (f > max_val) return max_val;
    return f;
}