/*
  StarForth — Steady-State Virtual Machine Runtime

  Copyright (c) 2023–2025 Robert A. James
  All rights reserved.

  This file is part of the StarForth project.

  Licensed under the StarForth License, Version 1.0 (the "License");
  you may not use this file except in compliance with the License.

  You may obtain a copy of the License at:
      https://github.com/star.4th@proton.me/StarForth/LICENSE.txt

  This software is provided "AS IS", WITHOUT WARRANTY OF ANY KIND,
  express or implied, including but not limited to the warranties of
  merchantability, fitness for a particular purpose, and noninfringement.

 See the License for the specific language governing permissions and
 limitations under the License.

 StarForth — Steady-State Virtual Machine Runtime
  Copyright (c) 2023–2025 Robert A. James
  All rights reserved.

  This file is part of the StarForth project.

  Licensed under the StarForth License, Version 1.0 (the "License");
  you may not use this file except in compliance with the License.

  You may obtain a copy of the License at:
       https://github.com/star.4th@proton.me/StarForth/LICENSE.txt

  This software is provided "AS IS", WITHOUT WARRANTY OF ANY KIND,
  express or implied, including but not limited to the warranties of
  merchantability, fitness for a particular purpose, and noninfringement.

  See the License for the specific language governing permissions and
  limitations under the License.

 */

#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <assert.h>

#include "../include/q48_16.h"

/* ============================================================================
 * Core Arithmetic: Multiply
 * ============================================================================
 *
 * Formula: (a / 2^16) * (b / 2^16) * 2^16 = (a * b) / 2^16
 *
 * Implementation note:
 * - Compute a * b in 128-bit intermediate (if available)
 * - Shift right by 16 bits
 * - For platforms without uint128_t, use uint64_t with overflow check
 */

/**
 * @brief Multiply two Q48.16 fixed-point values.
 *
 * Computes @c (a * b) >> 16 to maintain the Q48.16 scale. Uses
 * @c __uint128_t on GCC/Clang (128-bit intermediate, no precision loss)
 * and falls back to a manual 64-bit four-partial-product path with carry
 * tracking on platforms that lack @c __SIZEOF_INT128__.
 *
 * @param a First operand in Q48.16 format
 * @param b Second operand in Q48.16 format
 * @return Product in Q48.16 format (truncated, not rounded)
 */
q48_16_t q48_mul(q48_16_t a, q48_16_t b)
{
    /* Use __uint128_t if available (GCC/Clang) */
#ifdef __SIZEOF_INT128__
    __uint128_t prod = (__uint128_t)a * (__uint128_t)b;
    return (q48_16_t)(prod >> 16);
#else
    /* Fallback: manual 64-bit multiplication with overflow detection */
    /* Split a and b into high/low parts */
    uint64_t a_hi = a >> 32;
    uint64_t a_lo = a & 0xFFFFFFFFULL;
    uint64_t b_hi = b >> 32;
    uint64_t b_lo = b & 0xFFFFFFFFULL;

    /* Compute 4 partial products */
    uint64_t p_ll = a_lo * b_lo;         /* Low * Low */
    uint64_t p_lh = a_lo * b_hi;         /* Low * High */
    uint64_t p_hl = a_hi * b_lo;         /* High * Low */
    uint64_t p_hh = a_hi * b_hi;         /* High * High */

    /* Accumulate with carry tracking */
    uint64_t carry = 0;
    uint64_t mid = p_lh + p_hl;
    if (mid < p_lh) carry++;              /* Detect overflow in middle term */

    uint64_t result_hi = p_hh + (mid >> 32) + (carry << 32);
    uint64_t result_lo = p_ll + ((mid & 0xFFFFFFFFULL) << 32);
    if (result_lo < p_ll) result_hi++;

    /* Return: (result_hi << 32 | result_lo) >> 16 */
    /* = (result_hi << 16) | (result_lo >> 16) */
    return (result_hi << 16) | (result_lo >> 16);
#endif
}

/* ============================================================================
 * Core Arithmetic: Divide
 * ============================================================================
 *
 * Formula: (a / 2^16) / (b / 2^16) * 2^16 = (a << 16) / b
 *
 * Precondition: b > 0 (division by zero check in caller)
 */

/**
 * @brief Divide two Q48.16 fixed-point values.
 *
 * Computes @c (a << 16) / b. Returns 0 if @c b == 0 (safe, but callers
 * should guard). Returns @c UINT64_MAX as a saturation sentinel when @c a
 * exceeds 2^48, which would overflow the 16-bit left-shift.
 *
 * @param a Dividend in Q48.16 format
 * @param b Divisor in Q48.16 format; 0 returns 0
 * @return Quotient in Q48.16 format, or UINT64_MAX on saturation
 */
q48_16_t q48_div(q48_16_t a, q48_16_t b)
{
    if (b == 0) {
        /* Division by zero: return 0 (safe, but caller should check) */
        return 0;
    }

    /* Shift a left by 16 bits to maintain Q48.16 precision */
    /* Be careful: this could overflow if a is too large */
    /* Max safe a: 2^48 (since we're shifting left by 16) */
    if (a > 0x0000FFFFFFFFFFFFULL) {
        /* a is too large for safe shifting */
        /* Return max Q48.16 to signal saturation */
        return 0xFFFFFFFFFFFFFFFFULL;
    }

    q48_16_t shifted = a << 16;
    return shifted / b;
}

/* ============================================================================
 * Conversions: u64 ↔ Q48.16
 * ============================================================================
 */

/**
 * @brief Convert a @c double to Q48.16 format.
 *
 * Multiplies @c d by 65536.0 and rounds to nearest. Intended for
 * testing and diagnostics only — not called in the inference engine
 * hot path to avoid floating-point dependency.
 *
 * @param d Value to convert
 * @return Nearest Q48.16 representation of @c d
 */
q48_16_t q48_from_double(double d)
{
    /* Convert double to Q48.16 */
    /* This is for testing/diagnostics only, not used in inference engine */
    return (q48_16_t)(d * 65536.0 + 0.5);  /* Round to nearest */
}

/**
 * @brief Convert a Q48.16 value to @c double.
 *
 * Divides @c q by 65536.0. Used for logging and RStudio dashboard output;
 * not called in the inference engine hot path.
 *
 * @param q Value in Q48.16 format
 * @return Equivalent @c double value
 */
double q48_to_double(q48_16_t q)
{
    /* Convert Q48.16 to double */
    /* For logging and RStudio dashboard output */
    return (double)q / 65536.0;
}

/* ============================================================================
 * Approximation: Natural Logarithm (Integer-Only, Newton-Raphson)
 * ============================================================================
 *
 * Purpose: ln(x) in Q48.16 format for exponential decay fitting
 *
 * Algorithm:
 * 1. Use bit position as coarse approximation
 *    - If x = 2^k * m where 1 <= m < 2, then ln(x) ≈ k*ln(2) + ln(m)
 *    - ln(2) in Q48.16 = 0xB17217F7 (approx)
 *
 * 2. Refine ln(m) using Newton-Raphson on exp()
 *    - For y = ln(x), we want e^y = x
 *    - Newton: y_{n+1} = y_n + (x - e^{y_n}) / e^{y_n}
 *    - Simplified: y_{n+1} = y_n - 1 + x / e^{y_n}
 *
 * 3. Do 4-5 iterations for Q48.16 precision
 */

/**
 * @brief Compute an approximation of @c ln(x) in Q48.16 format (integer-only).
 *
 * Used by @c infer_decay_slope_q48() (Loop #6) for log-linear OLS regression.
 * Algorithm: range-reduces @c x to the interval [1, 2) by tracking the
 * exponent @c k (so @c x = 2^k * m), applies 6 Newton-Raphson iterations on
 * @c e^y = m starting from the first-order approximation @c y_0 = m - 1,
 * then assembles the result as @c ln(x) = k*ln(2) + ln(m). Returns 0 for
 * @c x == 0 (ln undefined) and for @c x == 65536 (ln(1.0) == 0.0 exactly).
 * @c LN2_Q48 = 45426 (0.693147... × 65536).
 *
 * @param x Input value in Q48.16 format (uint64_t; value of 65536 = 1.0)
 * @return @c ln(x) in Q48.16 format, or 0 for x==0 or x==65536
 */
q48_16_t q48_log_approx(uint64_t x)
{
    if (x == 0) {
        /* ln(0) is undefined; return 0 for safety */
        return 0;
    }

    if (x == 65536) {
        /* ln(1.0 in Q48.16) = 0 */
        return 0;
    }

    /* ln(2) in Q48.16 ≈ 0.693147180559945... */
    /* 0.693147 * 65536 = 45426.5 ≈ 45426 or 45427 */
    const q48_16_t LN2_Q48 = 45426;  /* 0x0000B17E in Q48.16 */

    /* Step 1: Find bit position k such that x = 2^k * m, 1 <= m < 2 */
    int k = 0;
    uint64_t m = x;

    /* Count leading zeros to find position */
    if (m >= 131072) {  /* m >= 2 in Q48.16 */
        while (m >= 131072) {
            m >>= 1;
            k++;
        }
    } else if (m < 65536) {  /* m < 1 in Q48.16 */
        while (m < 65536) {
            m <<= 1;
            k--;
        }
    }

    /* Step 2: Compute ln(m) where 1 <= m < 2 using Newton-Raphson */
    /* Initial guess: y_0 = (m - 1) for small values */
    q48_16_t y = (m > 65536) ? (m - 65536) : 0;

    /* Newton iterations: y_{n+1} = y_n + (m - e^{y_n}) / e^{y_n} */
    for (int iter = 0; iter < 6; iter++) {
        /* Compute e^y */
        q48_16_t exp_y = q48_exp_approx(y);

        if (exp_y == 0) break;  /* Safety check */

        /* Compute (m - exp_y) / exp_y, handling both positive and negative delta */
        int64_t delta_signed = (int64_t)m - (int64_t)exp_y;
        q48_16_t correction;

        if (delta_signed > 0) {
            correction = q48_div((q48_16_t)delta_signed, exp_y);
            y = q48_add(y, correction);
        } else if (delta_signed < 0) {
            correction = q48_div((q48_16_t)(-delta_signed), exp_y);
            if (y > correction) {
                y = q48_sub(y, correction);
            } else {
                y = 0;
            }
        }

        /* Early exit if correction is very small */
        if (delta_signed < 100 && delta_signed > -100) break;
    }

    /* Step 3: Combine ln(x) = k*ln(2) + ln(m) */
    q48_16_t result = y;
    if (k > 0) {
        result = q48_add(result, q48_mul(q48_from_u64(k), LN2_Q48));
    } else if (k < 0) {
        result = q48_sub(result, q48_mul(q48_from_u64(-k), LN2_Q48));
    }

    return result;
}

/* ============================================================================
 * Approximation: Exponential (Integer-Only, Taylor Series)
 * ============================================================================
 *
 * Purpose: e^x in Q48.16 format (supporting function for ln_approx)
 *
 * Algorithm: Taylor series around x=0
 * e^x = 1 + x + x^2/2! + x^3/3! + x^4/4! + ...
 *
 * For Q48.16 arithmetic, compute 6-8 terms for good precision.
 */

/**
 * @brief Compute an approximation of @c e^q in Q48.16 format (Taylor series).
 *
 * Supporting function for @c q48_log_approx() Newton iterations. Uses the
 * Taylor series @c 1 + x + x²/2! + … up to 10 terms, with an early-exit when
 * a term falls below 50 (sub-LSB in Q48.16). Handles negative @c q by
 * computing @c e^|q| then returning @c 1/result. Returns 1.0 (65536) for
 * @c q == 0, @c UINT64_MAX for @c q >= 16.0, and 0 for @c q <= -16.0.
 *
 * @param q Exponent in Q48.16 format (signed interpretation)
 * @return @c e^q in Q48.16 format
 */
q48_16_t q48_exp_approx(q48_16_t q)
{
    /* Handle special cases */
    if (q == 0) {
        return 65536;  /* e^0 = 1 in Q48.16 */
    }

    /* Check for overflow/underflow */
    int64_t q_signed = (int64_t)q;

    if (q_signed >= 1048576) {  /* q >= 16 */
        return 0xFFFFFFFFFFFFFFFFULL;  /* Overflow */
    }

    if (q_signed <= -1048576) {  /* q <= -16 */
        return 0;  /* Underflow to 0 */
    }

    /* For negative x, compute e^(-x) = 1 / e^x */
    int is_negative = (q_signed < 0);
    q48_16_t x = is_negative ? (q48_16_t)(-q_signed) : q;

    /* Taylor series: e^x = 1 + x + x^2/2! + x^3/3! + ... */
    q48_16_t result = 65536;  /* 1.0 in Q48.16 */
    q48_16_t term = x;        /* First term = x */

    result = q48_add(result, term);

    /* Compute subsequent terms */
    for (int n = 2; n <= 10; n++) {
        /* term = term * x / n */
        term = q48_mul(term, x);
        term = q48_div(term, q48_from_u64(n));

        result = q48_add(result, term);

        /* Early exit if term becomes negligible */
        if (term < 50) break;
    }

    /* If original was negative, return 1/result */
    if (is_negative) {
        result = q48_div(65536, result);  /* 1.0 / result */
    }

    return result;
}

/* ============================================================================
 * Approximation: Square Root (Integer-Only, Newton-Raphson)
 * ============================================================================
 *
 * Purpose: sqrt(q) in Q48.16 for R² computation
 *
 * Algorithm: Newton-Raphson on x^2 = q
 * x_{n+1} = (x_n + q/x_n) / 2
 */

/**
 * @brief Compute an approximation of @c sqrt(q) in Q48.16 format (Newton-Raphson).
 *
 * Iterates @c x_{n+1} = (x_n + q/x_n) / 2 starting from @c (q >> 1) + 16384,
 * for up to 8 iterations. Terminates early when the delta between successive
 * estimates falls below 10 (approximately 0.00015 in Q48.16). Used for R²
 * computation in the inference engine. Returns 0 for @c q == 0, and 1.0
 * (65536) for @c q == 65536.
 *
 * @param q Input value in Q48.16 format
 * @return @c sqrt(q) in Q48.16 format
 */
q48_16_t q48_sqrt_approx(q48_16_t q)
{
    if (q == 0) {
        return 0;
    }

    if (q == 65536) {  /* sqrt(1.0) = 1.0 */
        return 65536;
    }

    /* Initial guess: x_0 = q >> 1 (roughly sqrt in Q48.16 space) */
    q48_16_t x = (q >> 1) + 16384;  /* Ensure non-zero start */

    for (int iter = 0; iter < 8; iter++) {
        /* Newton-Raphson: x_{n+1} = (x_n + q/x_n) / 2 */
        q48_16_t q_div_x = q48_div(q, x);
        q48_16_t x_next = (x + q_div_x) >> 1;

        /* Check convergence */
        q48_16_t delta = (x_next > x) ? (x_next - x) : (x - x_next);
        if (delta < 10) break;  /* Converged to within 0.00015 */

        x = x_next;
    }

    return x;
}

/* ============================================================================
 * Approximation: Sine / Cosine (Integer-Only, Taylor Series)
 * ============================================================================
 *
 * FABRIC.md item 4.3.3a -- needed by the Console drawing fabric's
 * CIRCLE/ARC/ELLIPSE (item 4.3.3b). Radian input, same Taylor-series
 * approach as q48_exp_approx above.
 *
 * PI_Q48 = 205887 (pi * 65536, rounded). TWO_PI_Q48 is derived as
 * 2 * PI_Q48 rather than independently rounded, so the range-reduction
 * boundary at +-PI_Q48 is self-consistent (no seam).
 */

/**
 * @brief Reduce a signed Q48.16 angle into [-PI_Q48, PI_Q48].
 *
 * A single integer division on the raw Q48.16 representations gives the
 * correct (scale-independent) quotient of how many full 2*pi turns to
 * remove, then a bounded fix-up loop (at most one or two iterations)
 * handles the remainder landing just outside the target interval.
 *
 * @param x Signed angle in Q48.16 format (any magnitude)
 * @return Equivalent angle in Q48.16 format, within [-PI_Q48, PI_Q48]
 */
static int64_t q48_reduce_angle(int64_t x)
{
    const int64_t PI_Q48 = 205887;          /* pi * 65536, rounded */
    const int64_t TWO_PI_Q48 = 2 * PI_Q48;  /* derived, not independently rounded */

    int64_t k = x / TWO_PI_Q48;
    x -= k * TWO_PI_Q48;

    while (x > PI_Q48) x -= TWO_PI_Q48;
    while (x < -PI_Q48) x += TWO_PI_Q48;

    return x;
}

/**
 * @brief Compute an approximation of @c sin(q) in Q48.16 format (radians).
 *
 * Range-reduces into [-pi, pi] via @c q48_reduce_angle(), then sums the
 * Taylor series @c x - x^3/3! + x^5/5! - ... for terms n = 3,5,7,9,11,
 * with an early exit when a term falls below 10 (sub-LSB in Q48.16). Sine
 * is odd (@c sin(-x) = -sin(x)), so the sign is factored out before the
 * series and reapplied to the result.
 *
 * @param q Angle in Q48.16 format (radians, any magnitude)
 * @return @c sin(q) in Q48.16 format
 */
q48_16_t q48_sin_approx(q48_16_t q)
{
    int64_t x = q48_reduce_angle((int64_t)q);

    int negative = (x < 0);
    if (negative) x = -x;

    q48_16_t xu = (q48_16_t)x;
    q48_16_t x2 = q48_mul(xu, xu);
    q48_16_t term = xu;
    q48_16_t result = xu;
    int subtract = 1;

    for (int n = 3; n <= 11; n += 2) {
        term = q48_mul(term, x2);
        term = q48_div(term, q48_from_u64((uint64_t)(n * (n - 1))));

        result = subtract ? q48_sub(result, term) : q48_add(result, term);
        subtract = !subtract;

        if (term < 10) break;
    }

    return negative ? (q48_16_t)(0ULL - result) : result;
}

/**
 * @brief Compute an approximation of @c cos(q) in Q48.16 format (radians).
 *
 * Range-reduces into [-pi, pi] via @c q48_reduce_angle(), then sums the
 * Taylor series @c 1 - x^2/2! + x^4/4! - ... for terms n = 2,4,6,8,10,
 * with an early exit when a term falls below 10. Cosine is even
 * (@c cos(-x) = cos(x)), so no sign correction is needed -- @c x^2 already
 * absorbs the sign of @c x.
 *
 * @param q Angle in Q48.16 format (radians, any magnitude)
 * @return @c cos(q) in Q48.16 format
 */
q48_16_t q48_cos_approx(q48_16_t q)
{
    int64_t x = q48_reduce_angle((int64_t)q);

    q48_16_t xu = (q48_16_t)(x < 0 ? -x : x);
    q48_16_t x2 = q48_mul(xu, xu);
    q48_16_t term = 65536;  /* 1.0 in Q48.16 */
    q48_16_t result = term;
    int subtract = 1;

    for (int n = 2; n <= 10; n += 2) {
        term = q48_mul(term, x2);
        term = q48_div(term, q48_from_u64((uint64_t)(n * (n - 1))));

        result = subtract ? q48_sub(result, term) : q48_add(result, term);
        subtract = !subtract;

        if (term < 10) break;
    }

    return result;
}

/* ============================================================================
 * Diagnostic / Testing Utilities
 * ============================================================================
 */

/**
 * @brief Format a Q48.16 value as a decimal string @c "INTEGER.FRAC".
 *
 * Writes into a static buffer — not thread-safe and not re-entrant.
 * The fractional part is expressed as 5 decimal digits
 * (@c frac_part * 100000 / 65536). Intended for diagnostic logging.
 *
 * @param q Value in Q48.16 format
 * @return Pointer to a static buffer containing the formatted string
 */
const char* q48_to_string(q48_16_t q)
{
    static char buf[64];
    uint64_t integer_part = q >> 16;
    uint64_t frac_part = q & 0xFFFFULL;

    /* Convert fractional part to 5 decimal digits */
    /* frac_part / 65536 * 100000 = frac_part * 100000 / 65536 */
    uint32_t frac_digits = (frac_part * 100000ULL) / 65536;

    snprintf(buf, sizeof(buf), "%lu.%05u", integer_part, frac_digits);
    return buf;
}

/**
 * @brief Test whether a Q48.16 value is valid.
 *
 * Currently accepts any @c uint64_t value as valid — all bit patterns are
 * representable in Q48.16. A more sophisticated implementation could check
 * for saturation sentinels or range limits. Returns 1 always.
 *
 * @param q Value to validate
 * @return 1 always (placeholder for future range validation)
 */
int q48_is_valid(q48_16_t q)
{
    /* Q48.16 is always valid if it fits in uint64_t */
    /* More sophisticated validation could check for overflow risk */
    return 1;  /* For now, all uint64_t values are valid Q48.16 */
}

/* ============================================================================
 * Future: FORTH Word Wrappers
 * ============================================================================
 *
 * When integrated with FORTH word registry, these functions become:
 *
 * Q.+     ( q1 q2 -- q_sum )    -> forth_Q_ADD
 * Q.-     ( q1 q2 -- q_diff )   -> forth_Q_SUB
 * Q.*     ( q1 q2 -- q_prod )   -> forth_Q_MUL
 * Q./     ( q1 q2 -- q_quot )   -> forth_Q_DIV
 * Q.ABS   ( q -- |q| )          -> forth_Q_ABS
 * Q.LOG   ( u -- ln(u) )        -> forth_Q_LOG
 * Q.EXP   ( q -- e^q )          -> forth_Q_EXP
 * Q.SQRT  ( q -- sqrt(q) )      -> forth_Q_SQRT
 *
 * Stack convention: All Q48.16 values pushed/popped as uint64_t
 */