#include <c_Float.h>
#include <c_Memory.h>

uint32_t c_Float_Add(uint32_t a, uint32_t b) {
    // 1. 提取特殊狀態與快速零值返回
    if ((a & ~C_FLOAT_SIGN_MASK) == 0) return b;
    if ((b & ~C_FLOAT_SIGN_MASK) == 0) return a;

    uint32_t sign_a = a & C_FLOAT_SIGN_MASK;
    uint32_t sign_b = b & C_FLOAT_SIGN_MASK;
    int32_t exp_a   = (int32_t)((a & C_FLOAT_EXP_MASK) >> 23);
    int32_t exp_b   = (int32_t)((b & C_FLOAT_EXP_MASK) >> 23);
    uint32_t frac_a = a & C_FLOAT_FRAC_MASK;
    uint32_t frac_b = b & C_FLOAT_FRAC_MASK;

    // 處理 NaN 或 Infinity 特殊邊界
    if (exp_a == 255 || exp_b == 255) {
        // 簡化處理：若任一為 NaN 或 Inf，返回 NaN/Inf 傳播
        if ((exp_a == 255 && frac_a != 0) || (exp_b == 255 && frac_b != 0)) {
            return 0x7FC00000U; // Quiet NaN
        }
        return (exp_a == 255) ? a : b;
    }

    // 2. 補齊隱含位 1
    frac_a = (exp_a == 0) ? (frac_a << 1) : (frac_a | C_FLOAT_HIDDEN_BIT);
    frac_b = (exp_b == 0) ? (frac_b << 1) : (frac_b | C_FLOAT_HIDDEN_BIT);
    if (exp_a == 0) exp_a = 1;
    if (exp_b == 0) exp_b = 1;

    // 为了保留精確捨入，將尾數左移 3 位，騰出位置存放 G (Guard), R (Round), S (Sticky) 位
    frac_a <<= 3;
    frac_b <<= 3;

    int32_t exp_res = exp_a;
    uint32_t sticky = 0;

    // 3. 對階（Align Exponents）
    if (exp_a > exp_b) {
        int32_t shift = exp_a - exp_b;
        if (shift > 26) {
            frac_b = 0;
            sticky = 1;
        } else {
            sticky = (frac_b & ~((~0U) << shift)) != 0;
            frac_b >>= shift;
        }
        frac_b |= sticky;
        exp_res = exp_a;
    } else if (exp_b > exp_a) {
        int32_t shift = exp_b - exp_a;
        if (shift > 26) {
            frac_a = 0;
            sticky = 1;
        } else {
            sticky = (frac_a & ~((~0U) << shift)) != 0;
            frac_a >>= shift;
        }
        frac_a |= sticky;
        exp_res = exp_b;
    }

    // 4. 尾數運算（考慮正負號）
    uint32_t sign_res;
    uint32_t frac_res;

    if (sign_a == sign_b) {
        // 同號相加
        sign_res = sign_a;
        frac_res = frac_a + frac_b;
    } else {
        // 異號相減
        if (frac_a >= frac_b) {
            sign_res = sign_a;
            frac_res = frac_a - frac_b;
        } else {
            sign_res = sign_b;
            frac_res = frac_b - frac_a;
        }
        // 互相抵消為 0
        if (frac_res == 0) return 0x00000000U;
    }

    // 5. 規格化（Normalization）
    // 情況 A：相加導致尾數溢出（例如超出原本的包含隱含位與 GRS 的範圍）
    // 正常擴展後，隱含的 1 應該在第 26 位 (FLOAT_HIDDEN_BIT << 3 = 0x04000000)
    if (frac_res & (1U << 27)) {
        frac_res = (frac_res >> 1) | (frac_res & 1); // 移出的位與最低位做或運算保持 Sticky
        exp_res++;
    } else {
        // 情況 B：相減導致尾數變小，需要左移規格化
        while (!(frac_res & (1U << 26)) && exp_res > 1) {
            frac_res <<= 1;
            exp_res--;
        }
        // 降級為非規格化數
        if (!(frac_res & (1U << 26)) && exp_res == 1) {
            exp_res = 0;
        }
    }

    // 6. 溢出至無限大檢查
    if (exp_res >= 255) {
        return sign_res | C_FLOAT_EXP_MASK; // 返回 ±Infinity
    }

    // 7. IEEE 754 預設：向最接近偶數捨入（Round-to-Nearest-Even）
    // 此时 frac_res 的低 3 位即為 G, R, S
    uint32_t round_bits = frac_res & 7U;
    frac_res >>= 3; // 移除 GRS 位，回歸 24 位尾數（含隱含位）

    // 捨入判斷條件：
    // round_bits > 4 (即 101, 110, 111) -> 必然進位
    // round_bits == 4 (即 100，正中央) -> 觀察最低有效位(LSB)，LSB 為 1（奇數）則進位，為 0（偶數）則捨去
    if ((round_bits > 4) || ((round_bits == 4) && (frac_res & 1U))) {
        frac_res++;
        // 捨入可能再次引發尾數溢出
        if (frac_res & (1U << 24)) {
            frac_res >>= 1;
            exp_res++;
            if (exp_res >= 255) return sign_res | C_FLOAT_EXP_MASK;
        }
    }

    // 如果是非規格化數，exp_res 為 0，frac_res 本身就不該移除隱含位，直接拼接
    // 如果是規格化數，需要清除第 23 位的隱含位 1
    if (exp_res != 0) {
        frac_res &= C_FLOAT_FRAC_MASK;
    }

    // 8. 拼裝回 32 位標準格式
    return sign_res | ((uint32_t)exp_res << 23) | frac_res;
}

uint32_t c_Float_Mul(uint32_t a, uint32_t b) {
    // 1. 快速提取
    uint32_t sign_a = C_FLOAT_GET_SIGN(a), sign_b = C_FLOAT_GET_SIGN(b);
    uint32_t exp_a  = C_FLOAT_GET_EXP(a),  exp_b  = C_FLOAT_GET_EXP(b);
    uint32_t frac_a = C_FLOAT_GET_FRAC(a), frac_b = C_FLOAT_GET_FRAC(b);

    uint32_t sign_res = sign_a ^ sign_b;

    // 2. 特殊值处理 (0, Inf, NaN)
    if (exp_a == 255 || exp_b == 255) return c_Float_Pack(sign_res, 255, 0); // 简化处理为Inf
    if (a == 0 || b == 0) return c_Float_Pack(sign_res, 0, 0);

    // 3. 补齐隐藏位 1
    frac_a |= C_FLOAT_HIDDEN_BIT;
    frac_b |= C_FLOAT_HIDDEN_BIT;

    // 4. 指数相加并减去 Bias
    int32_t exp_res = (int32_t)exp_a + (int32_t)exp_b - C_FLOAT_EXP_BIAS;

    // 5. 尾数相乘：24位 * 24位 = 48位，需要用 uint64_t 接收
    uint64_t prod = (uint64_t)frac_a * (uint64_t)frac_b;

    // 6. 规格化 (Normalization)
    // 正常 1.x * 1.x 的范围在 [1.0, 4.0) 之间。
    // 如果积 >= 2.0 (即第 47 位为 1，第 46 位是原本的隐藏位 1 发生溢出)，需要右移 1 位
    if (prod & (1ULL << 47)) {
        prod >>= 1;
        exp_res++;
    }

    // 7. 从 64 位乘积中提取出 23 位尾数（移除第 46 位的隐藏 1）
    // 此时隐藏位 1 在第 46 位 (1ULL << 46)，尾数在低 46 位
    uint32_t frac_res = (uint32_t)((prod >> 23) & C_FLOAT_FRAC_MASK);

    // 8. 边界与下溢/上溢检查
    if (exp_res >= 255) return c_Float_Pack(sign_res, 255, 0); // 上溢至 Inf
    if (exp_res <= 0)   return c_Float_Pack(sign_res, 0, 0);   // 下溢至 0

    return c_Float_Pack(sign_res, exp_res, frac_res);
}

uint32_t c_Float_Div(uint32_t a, uint32_t b) {
    uint32_t sign_a = C_FLOAT_GET_SIGN(a), sign_b = C_FLOAT_GET_SIGN(b);
    uint32_t exp_a  = C_FLOAT_GET_EXP(a),  exp_b  = C_FLOAT_GET_EXP(b);
    uint32_t frac_a = C_FLOAT_GET_FRAC(a), frac_b = C_FLOAT_GET_FRAC(b);

    uint32_t sign_res = sign_a ^ sign_b;

    // 0 分母异常检查
    if (b == 0) return c_Float_Pack(sign_res, 255, 0); // 1.0 / 0.0 = Inf
    if (a == 0) return c_Float_Pack(sign_res, 0, 0);

    frac_a |= C_FLOAT_HIDDEN_BIT;
    frac_b |= C_FLOAT_HIDDEN_BIT;

    int32_t exp_res = (int32_t)exp_a - (int32_t)exp_b + C_FLOAT_EXP_BIAS;

    // 为了保留除法后的23位尾数精度，将分子左移 23 位后做整数除法
    uint64_t num = (uint64_t)frac_a << 23;
    uint32_t quot = (uint32_t)(num / frac_b);

    // 规格化：1.x / 1.x 的范围在 (0.5, 2.0) 之间。
    // 如果商小于 1.0（即隐藏位 1 没落在第 23 位，落在了第 22 位），需要左移 1 位，指数减 1
    if (!(quot & C_FLOAT_HIDDEN_BIT)) {
        quot <<= 1;
        exp_res--;
    }

    if (exp_res >= 255) return c_Float_Pack(sign_res, 255, 0);
    if (exp_res <= 0)   return c_Float_Pack(sign_res, 0, 0);

    return c_Float_Pack(sign_res, exp_res, quot & C_FLOAT_FRAC_MASK);
}

int c_Float_Cmp(uint32_t a, uint32_t b) {
    // 1. 处理 NaN：依据 IEEE 754，NaN 参与比较永远返回不相等（或未定义）
    if (c_Float_IsNAN(a) || c_Float_IsNAN(b)) {
        return 0; // 软浮点库通常在此处设置不合法比较标志位
    }

    // 2. 特殊情况：+0.0 (0x00000000) 和 -0.0 (0x80000000) 在逻辑上是相等的
    if (((a | b) & ~C_FLOAT_SIGN_MASK) == 0) {
        return 0;
    }

    // 提取符号位
    uint32_t sign_a = a & C_FLOAT_SIGN_MASK;
    uint32_t sign_b = b & C_FLOAT_SIGN_MASK;

    // 3. 符号不同
    if (sign_a != sign_b) {
        // a 是负数，b 是正数 => a < b
        // a 是正数，b 是负数 => a > b
        return sign_a ? -1 : 1;
    }

    // 4. 符号相同：将原始二进制位转换为有符号 32 位整型进行直观比较
    int32_t ia = (int32_t)a;
    int32_t ib = (int32_t)b;

    if (sign_a) {
        // 如果都是负数，二进制数值越大，其代表的实际浮点数反而越小 (例如 -2.0 的二进制码大于 -1.0)
        if (ia > ib) return -1;
        if (ia < ib) return 1;
        return 0;
    } else {
        // 如果都是正数，二进制数值越大，其实际浮点数就越大
        if (ia > ib) return 1;
        if (ia < ib) return -1;
        return 0;
    }
}

int c_Double_Cmp(uint64_t a, uint64_t b) {
    // 1. 处理 NaN
    if (c_Double_IsNAN(a) || c_Double_IsNAN(b)) {
        return 0;
    }

    // 2. 处理 +0.0 与 -0.0 相等的情况
    if (((a | b) & ~C_DOUBLE_SIGN_MASK) == 0) {
        return 0;
    }

    uint64_t sign_a = a & C_DOUBLE_SIGN_MASK;
    uint64_t sign_b = b & C_DOUBLE_SIGN_MASK;

    // 3. 符号不同
    if (sign_a != sign_b) {
        return sign_a ? -1 : 1;
    }

    // 4. 符号相同：转为有符号 64 位整型比较
    int64_t ia = (int64_t)a;
    int64_t ib = (int64_t)b;

    if (sign_a) {
        // 均为负数
        if (ia > ib) return -1;
        if (ia < ib) return 1;
        return 0;
    } else {
        // 均为正数
        if (ia > ib) return 1;
        if (ia < ib) return -1;
        return 0;
    }
}

/**
 * @brief 软浮点双精度打包函数
 * @param sign     符号位 (0 或 1)
 * @param exp      解包/运算后的有符号指数 (已减去或未加上 Bias 均可，此处传入带 Bias 的期望值)
 * @param frac64   运算后暂存在 64 位整型中的高精度尾数 (假设规格化后隐含位在第 52 位，低位留有舍入残余)
 * @return         组合好的 IEEE 754 64位无符号整数 (可直接对应 double)
 */
uint64_t c_Double_Pack(uint32_t sign, int32_t exp, uint64_t frac64) {

    // 1. 动态规格化：若运算导致尾数高位溢出 (例如第 53 位为 1)，需要右移尾数并增加指数
    if (frac64 & (C_DOUBLE_HIDDEN_BIT << 1)) {
        frac64 >>= 1;
        exp++;
    }

    // 2. 下溢处理：指数太小，转换为非规格化数
    if (exp <= 0) {
        // 如果指数极小，直接移出范围，变回 0
        if (exp < -52) {
            frac64 = 0;
        } else {
            // 右移尾数以对齐非规格化数的指数位置 (exp = 0)
            int32_t shift = 1 - exp;
            frac64 >>= shift;
        }
        exp = 0; // 非规格化数的指数域强制为 0
    }

    // 3. 执行 IEEE 754 默认的“向最接近偶数舍入 (Round-to-Nearest-Even)”
    // 假设经过上述操作后，标准 52 位尾数在 frac64 的低 52 位，若有更低位则是运算残留
    // 为了演示标准舍入，假设传入的 frac64 在低位保留了扩充精度（例如左移了 3 位留给 GRS）
    // 此处简化演示：基于常规截断进行最邻近舍入处理
    // 在工业级库中，通常传入 frac 时会带有额外的 round_bits 变量

    // 4. 上溢检查：指数超过最大限制 (2047)，打包为无穷大
    if (exp >= 0x7FF) {
        return ((uint64_t)sign << 63) | C_DOUBLE_EXP_MASK; // 返回 +/- Inf
    }

    // 5. 最终清除尾数域外的隐含 1 (因为 IEEE 754 编码中不存储规格化数的最高位 1)
    uint64_t final_frac = frac64 & C_DOUBLE_FRAC_MASK;

    // 6. 位移拼接
    uint64_t packed_value = ((uint64_t)sign << 63) |
                            ((uint64_t)exp << 52)  |
                            final_frac;

    return packed_value;
}


/* ------------------------------------------------------------------------------------------------------------------ */
/*  */

// 輔助函數：處理 64 位元尾數與 3 位元 GRS 捨入殘餘
static uint64_t round_and_pack_double(uint64_t sign, int32_t exp, uint64_t frac64) {
    uint32_t round_bits = frac64 & 7U;
    frac64 >>= 3; // 移除 GRS 位，恢復為包含隱含位的 53 位元尾數

    // 向最接近偶數捨入
    if ((round_bits > 4) || ((round_bits == 4) && (frac64 & 1ULL))) {
        frac64++;
        if (frac64 & (C_DOUBLE_HIDDEN_BIT << 1)) {
            frac64 >>= 1;
            exp++;
        }
    }

    if (exp >= 2047) return sign | C_DOUBLE_EXP_MASK; // 溢出至無限大
    if (exp <= 0) return sign;                      // 下溢至 0

    return sign | ((uint64_t)exp << 52) | (frac64 & C_DOUBLE_FRAC_MASK);
}

uint64_t c_Double_Add(uint64_t a, uint64_t b) {
    if ((a & ~C_DOUBLE_SIGN_MASK) == 0) return b;
    if ((b & ~C_DOUBLE_SIGN_MASK) == 0) return a;

    uint64_t sign_a = a & C_DOUBLE_SIGN_MASK;
    uint64_t sign_b = b & C_DOUBLE_SIGN_MASK;
    int32_t exp_a   = (int32_t)((a & C_DOUBLE_EXP_MASK) >> 52);
    int32_t exp_b   = (int32_t)((b & C_DOUBLE_EXP_MASK) >> 52);
    uint64_t frac_a = a & C_DOUBLE_FRAC_MASK;
    uint64_t frac_b = b & C_DOUBLE_FRAC_MASK;

    // 特殊值傳播 (NaN / Inf)
    if (exp_a == 2047 || exp_b == 2047) {
        if ((exp_a == 2047 && frac_a != 0) || (exp_b == 2047 && frac_b != 0)) return 0x7FF8000000000000ULL; // NaN
        return (exp_a == 2047) ? a : b;
    }

    // 補齊隱含位并左移 3 位釋放 GRS 空間
    frac_a = (exp_a == 0) ? (frac_a << 1) : (frac_a | C_DOUBLE_HIDDEN_BIT);
    frac_b = (exp_b == 0) ? (frac_b << 1) : (frac_b | C_DOUBLE_HIDDEN_BIT);
    if (exp_a == 0) exp_a = 1;
    if (exp_b == 0) exp_b = 1;

    frac_a <<= 3;
    frac_b <<= 3;

    int32_t exp_res = exp_a;
    uint64_t sticky = 0;

    // 對階（Align）
    if (exp_a > exp_b) {
        int32_t shift = exp_a - exp_b;
        if (shift > 56) { frac_b = 0; sticky = 1; }
        else { sticky = (frac_b & ~((~0ULL) << shift)) != 0; frac_b >>= shift; }
        frac_b |= sticky;
        exp_res = exp_a;
    } else if (exp_b > exp_a) {
        int32_t shift = exp_b - exp_a;
        if (shift > 56) { frac_a = 0; sticky = 1; }
        else { sticky = (frac_a & ~((~0ULL) << shift)) != 0; frac_a >>= shift; }
        frac_a |= sticky;
        exp_res = exp_b;
    }

    uint64_t sign_res, frac_res;
    if (sign_a == sign_b) {
        sign_res = sign_a;
        frac_res = frac_a + frac_b;
    } else {
        if (frac_a >= frac_b) {
            sign_res = sign_a;
            frac_res = frac_a - frac_b;
        } else {
            sign_res = sign_b;
            frac_res = frac_b - frac_a;
        }
        if (frac_res == 0) return 0x0000000000000000ULL;
    }

    // 規格化
    if (frac_res & (C_DOUBLE_HIDDEN_BIT << 4)) {
        frac_res = (frac_res >> 1) | (frac_res & 1);
        exp_res++;
    } else {
        while (!(frac_res & (C_DOUBLE_HIDDEN_BIT << 3)) && exp_res > 1) {
            frac_res <<= 1;
            exp_res--;
        }
        if (!(frac_res & (C_DOUBLE_HIDDEN_BIT << 3)) && exp_res == 1) exp_res = 0;
    }

    return round_and_pack_double(sign_res, exp_res, frac_res);
}

// 內部輔助函數：32位交叉相乘，手動模擬 64x64->128位元乘法
C_STATIC_FORCE_INLINE
void mul64_to_128(uint64_t a, uint64_t b, uint64_t *res_hi, uint64_t *res_lo) {
    uint64_t a_hi = a >> 32, a_lo = a & 0xFFFFFFFFULL;
    uint64_t b_hi = b >> 32, b_lo = b & 0xFFFFFFFFULL;

    uint64_t p0 = a_lo * b_lo;
    uint64_t p1 = a_hi * b_lo;
    uint64_t p2 = a_lo * b_hi;
    uint64_t p3 = a_hi * b_hi;

    uint64_t mid = p1 + (p0 >> 32) + (p2 & 0xFFFFFFFFULL);
    *res_lo = (mid << 32) | (p0 & 0xFFFFFFFFULL);
    *res_hi = p3 + (mid >> 32) + (p2 >> 32);
}

uint64_t c_Double_Mul(uint64_t a, uint64_t b) {
    uint64_t sign_res = (a ^ b) & C_DOUBLE_SIGN_MASK;
    int32_t exp_a = (int32_t)((a & C_DOUBLE_EXP_MASK) >> 52);
    int32_t exp_b = (int32_t)((b & C_DOUBLE_EXP_MASK) >> 52);
    uint64_t frac_a = (a & C_DOUBLE_FRAC_MASK) | C_DOUBLE_HIDDEN_BIT;
    uint64_t frac_b = (b & C_DOUBLE_FRAC_MASK) | C_DOUBLE_HIDDEN_BIT;

    if (exp_a == 2047 || exp_b == 2047) return sign_res | C_DOUBLE_EXP_MASK; // 簡化為 Inf
    if ((a & ~C_DOUBLE_SIGN_MASK) == 0 || (b & ~C_DOUBLE_SIGN_MASK) == 0) return sign_res; // 返回 ±0

    int32_t exp_res = exp_a + exp_b - 1023;

    uint64_t prod_hi, prod_lo;
    mul64_to_128(frac_a, frac_b, &prod_hi, &prod_lo); // 得到 106 位的精確積

    // 規格化校準：隱含位本應在 52 位元，52*2 = 104 位元。
    // prod_hi 預期會包含溢出位。需要將 128 位元結果右移，使其完美對齊「保留53位 + 3位GRS」的規格
    uint64_t frac_res;
    if (prod_hi & (1ULL << 41)) { // 發生進位
        frac_res = (prod_hi << 23) | (prod_lo >> 41);
        frac_res |= ((prod_lo & 0x1FFFFFFFFFFULL) != 0); // Sticky 位感知
        exp_res++;
    } else {
        frac_res = (prod_hi << 24) | (prod_lo >> 40);
        frac_res |= ((prod_lo & 0xFFFFFFFFFFULL) != 0);
    }

    return round_and_pack_double(sign_res, exp_res, frac_res);
}

uint64_t c_Double_Div(uint64_t a, uint64_t b) {
    uint64_t sign_res = (a ^ b) & C_DOUBLE_SIGN_MASK;
    int32_t exp_a = (int32_t)((a & C_DOUBLE_EXP_MASK) >> 52);
    int32_t exp_b = (int32_t)((b & C_DOUBLE_EXP_MASK) >> 52);
    uint64_t frac_a = (a & C_DOUBLE_FRAC_MASK) | C_DOUBLE_HIDDEN_BIT;
    uint64_t frac_b = (b & C_DOUBLE_FRAC_MASK) | C_DOUBLE_HIDDEN_BIT;

    if ((b & ~C_DOUBLE_SIGN_MASK) == 0) return sign_res | C_DOUBLE_EXP_MASK; // 除以 0 返回 Inf
    if ((a & ~C_DOUBLE_SIGN_MASK) == 0) return sign_res;                   // 0 除以任何數返回 0

    int32_t exp_res = exp_a - exp_b + 1023;

    // 手動執行 56 輪位元逐位長除法 (53位尾數 + 3位 GRS)
    uint64_t quotient = 0;
    uint64_t remainder = frac_a;

    for (int i = 0; i < 56; i++) {
        quotient <<= 1;
        if (remainder >= frac_b) {
            remainder -= frac_b;
            quotient |= 1ULL;
        }
        remainder <<= 1;
    }

    // 餘數如果不為 0，則與 Sticky 位進行 OR 運算
    if (remainder != 0) {
        quotient |= 1ULL;
    }

    // 規格化商
    if (!(quotient & (C_DOUBLE_HIDDEN_BIT << 3))) {
        quotient <<= 1;
        exp_res--;
    }

    return round_and_pack_double(sign_res, exp_res, quotient);
}