FESADev/src/fesa/math/high_low_arithmetic.h

#ifndef FESA_MATH_HIGH_LOW_ARITHMETIC_H_
#define FESA_MATH_HIGH_LOW_ARITHMETIC_H_

#include <cmath>
#include <cfenv>
#include <cstdint>
#include <cstring>
#include <initializer_list>
#include <limits>
#if defined(_MSC_VER) && defined(_M_X64)
#include <intrin.h>
#endif
#if defined(_M_X64) || defined(__SSE2__)
#include <xmmintrin.h>
#endif

namespace fesa::math_internal {
/// @brief Checks the rounding and gradual-underflow premises without mutation.
inline bool ArithmeticEnvironmentSupported() {
  if (std::fegetround() != FE_TONEAREST) return false;
#if defined(_M_X64) || defined(__SSE2__)
  // MXCSR FTZ, DAZ and rounding-control bits must all be clear.
  if ((_mm_getcsr() & 0xE040U) != 0U) return false;
#endif
  return true;
}
/// @brief Two binary64 components of one physical value; raw parts are kept.
/// @note Arithmetic requires normalized finite parts and nearest rounding.
/// This is an expansion with bounded rounding, not an IEEE quad type.
struct HighLow {
  double high{0};
  double low{0};
  HighLow() = default;
  // Scalar lifting canonicalizes exact signed zero; raw pair inputs do not.
  HighLow(double value) : high(value == 0 ? 0.0 : value) {}
  HighLow(double high_part, double low_part)
      : high(high_part), low(low_part) {}
};

/// @brief Gives an invalid arithmetic result for a checked caller to reject.
inline HighLow InvalidHighLow() {
  return {std::numeric_limits<double>::quiet_NaN(), 0};
}
/// @brief Classifies finite binary64 values without floating arithmetic.
inline bool IsFiniteBinary64(double value) {
  static_assert(sizeof(double) == sizeof(std::uint64_t) &&
                std::numeric_limits<double>::is_iec559 &&
                std::numeric_limits<double>::digits == 53);
  std::uint64_t bits = 0;
  std::memcpy(&bits, &value, sizeof(bits));
  return (bits & 0x7ff0000000000000ULL) != 0x7ff0000000000000ULL;
}
/// @brief Checks both physical components, before comparisons or publication.
inline bool IsFinite(HighLow value) {
  return IsFiniteBinary64(value.high) && IsFiniteBinary64(value.low);
}
/// @brief Recovers the exact error of supported finite binary64 addition.
/// @note Requests local expansion at the measured MSVC x64 call boundary.
#if defined(_MSC_VER) && defined(_M_X64)
__forceinline
#else
inline
#endif
HighLow TwoSum(double left, double right) {
  const double sum = left + right;
  const double bridge = sum - left;
  const double restored_left = sum - bridge;
  const double error = (left - restored_left) + (right - bridge);
  if (!IsFiniteBinary64(sum) || !IsFiniteBinary64(bridge) ||
      !IsFiniteBinary64(restored_left) || !IsFiniteBinary64(error))
    return InvalidHighLow();
  return sum == 0 && error == 0 ? HighLow{} : HighLow{sum, error};
}
/// @brief Requires the raw pair to equal its nearest-rounding normalization.
inline bool IsNormalized(HighLow value) {
  if (!IsFinite(value)) return false;
  if (value.high == 0 && value.low == 0)
    return !std::signbit(value.high) && !std::signbit(value.low);
  // If RN(high + low) == high, TwoSum's bridge is zero and its error is
  // exactly low. The finite and canonical exact-zero guards remain above.
  return value.high + value.low == value.high;
}
/// @brief Rounds the full physical value to one finite output value.
inline double Rounded(HighLow value) {
  return IsNormalized(value) ? value.high + value.low
                             : std::numeric_limits<double>::quiet_NaN();
}
/// @brief Adds normalized expansions; rounding of discarded tail is bounded.
inline HighLow Add(HighLow left, HighLow right) {
  if (!IsNormalized(left) || !IsNormalized(right)) return InvalidHighLow();
  // Exact-zero additions only normalize signed zero in the retained pair.
  // The nonzero graph and its discarded-tail association stay unchanged.
  if (left.high == 0 && left.low == 0)
    return {right.high, right.low == 0 ? 0.0 : right.low};
  if (right.high == 0 && right.low == 0)
    return {left.high, left.low == 0 ? 0.0 : left.low};
  const auto head = TwoSum(left.high, right.high);
  const auto tail = TwoSum(left.low, right.low);
  const auto middle = TwoSum(head.low, tail.high);
  const auto joined = TwoSum(head.high, middle.high);
  const double remainder = (joined.low + middle.low) + tail.low;
  if (!IsFinite(head) || !IsFinite(tail) || !IsFinite(middle) ||
      !IsFinite(joined) || !IsFiniteBinary64(remainder))
    return InvalidHighLow();
  return TwoSum(joined.high, remainder);
}
/// @brief Negates both components without collapsing the physical value.
inline HighLow Negate(HighLow value) {
  if (!IsNormalized(value)) return InvalidHighLow();
  return value.high == 0 && value.low == 0
             ? HighLow{}
             : HighLow{-value.high, -value.low};
}
/// @brief Subtracts full expansions, including cancellations between highs.
inline HighLow Subtract(HighLow left, HighLow right) {
  return Add(left, Negate(right));
}
/// @brief Returns the exponent of the least nonzero binary64 significand bit.
/// @pre value is finite and nonzero; gradual underflow is enabled.
/// @note Keeps the small integer search inside its measured MSVC x64 caller.
#if defined(_MSC_VER) && defined(_M_X64)
__forceinline
#else
inline
#endif
int LowestBitExponent(double value) {
  static_assert(sizeof(double) == sizeof(std::uint64_t) &&
                std::numeric_limits<double>::is_iec559 &&
                std::numeric_limits<double>::digits == 53);
  std::uint64_t bits = 0;
  std::memcpy(&bits, &value, sizeof(bits));
  const auto encoded_exponent = (bits >> 52U) & 0x7ffU;
  auto significand = bits & 0xfffffffffffffULL;
  const int exponent = encoded_exponent == 0U
                           ? -1074
                           : static_cast<int>(encoded_exponent) - 1075;
  if (encoded_exponent != 0U) significand |= 1ULL << 52U;
#if defined(_MSC_VER) && defined(_M_X64)
  // The finite, nonzero precondition makes the significand nonzero.
  unsigned long trailing = 0;
  _BitScanForward64(&trailing, significand);
  return exponent + static_cast<int>(trailing);
#else
  int trailing = 0;
  // Preserve the portable search outside the supported MSVC x64 path.
  for (const int shift : {32, 16, 8, 4, 2, 1}) {
    const auto mask = (1ULL << shift) - 1U;
    if ((significand & mask) == 0U) {
      significand >>= shift;
      trailing += shift;
    }
  }
  return exponent + trailing;
#endif
}
/// @brief Gives a product and its FMA residual when the dyadic tail fits.
/// @note Rejects underflow-risk exact tails, even if the rounded high is finite.
inline HighLow TwoProduct(double left, double right) {
  if (!IsFiniteBinary64(left) || !IsFiniteBinary64(right)) return InvalidHighLow();
  if (left == 0 || right == 0) return {};
  if (LowestBitExponent(left) + LowestBitExponent(right) < -1074)
    return InvalidHighLow();
  const double product = left * right;
  if (!IsFiniteBinary64(product)) return InvalidHighLow();
  const double error = std::fma(left, right, -product);
  if (!IsFiniteBinary64(error)) return InvalidHighLow();
  return TwoSum(product, error);
}
/// @brief Retains all four component products in deterministic order.
inline HighLow Multiply(HighLow left, HighLow right) {
  if (!IsNormalized(left) || !IsNormalized(right)) return InvalidHighLow();
  auto product = TwoProduct(left.high, right.high);
  // Omitted terms are exact zero; retain the zero normalization that preceded
  // a nonzero low-high term and the final canonicalization/invalid check.
  if (right.low != 0)
    product = Add(product, TwoProduct(left.high, right.low));
  else if (left.low != 0)
    product = Add(product, HighLow{});
  if (left.low != 0)
    product = Add(product, TwoProduct(left.low, right.high));
  return Add(product, left.low != 0 && right.low != 0
                          ? TwoProduct(left.low, right.low) : HighLow{});
}
/// @brief Orders canonical values; 2 means unordered/unsupported input.
inline int Compare(HighLow left, HighLow right) {
  if (!IsNormalized(left) || !IsNormalized(right)) return 2;
  if (left.high < right.high) return -1;
  if (left.high > right.high) return 1;
  if (left.low < right.low) return -1;
  if (left.low > right.low) return 1;
  return 0;
}
/// @brief Takes the absolute value using the full physical sign.
inline HighLow Abs(HighLow value) {
  const int sign = Compare(value, HighLow{});
  return sign == 2 ? InvalidHighLow() : (sign < 0 ? Negate(value) : value);
}
/// @brief Refines a binary64 quotient twice using the full residual.
/// @note Unsupported products/quotient underflow propagate an invalid result.
inline HighLow Divide(HighLow numerator, HighLow denominator) {
  if (!IsNormalized(numerator) || !IsNormalized(denominator) ||
      Compare(denominator, HighLow{}) == 0)
    return InvalidHighLow();
  if (Compare(numerator, HighLow{}) == 0) return {};
  const double first = numerator.high / denominator.high;
  if (!IsFiniteBinary64(first) || first == 0) return InvalidHighLow();
  auto quotient = HighLow{first};
  for (int iteration = 0; iteration < 2; ++iteration) {
    const auto residual = Subtract(numerator, Multiply(denominator, quotient));
    if (!IsNormalized(residual)) return InvalidHighLow();
    const double correction = residual.high / denominator.high;
    if (!IsFiniteBinary64(correction) || (residual.high != 0 && correction == 0))
      return InvalidHighLow();
    quotient = Add(quotient, HighLow{correction});
  }
  return quotient;
}
}  // namespace fesa::math_internal

#endif  // FESA_MATH_HIGH_LOW_ARITHMETIC_H_