FESADev/src/fesa/analysis/thermal_time_grid.cpp

#include "fesa/analysis/thermal_time_grid.h"

#include <array>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <limits>

namespace fesa {
namespace {

// Fixed four base-2^32 digits. Contract bounds all exact values to 122 bits.
struct UInt128 {
  std::array<std::uint32_t, 4> digit{};
};

/// @brief Orders unsigned integers from their most significant digit.
int Compare(const UInt128& a, const UInt128& b) {
  for (int i = 3; i >= 0; --i) {
    if (a.digit[i] != b.digit[i]) return a.digit[i] < b.digit[i] ? -1 : 1;
  }
  return 0;
}

/// @brief Adds with explicit carry; false means the fixed storage overflowed.
bool Add(const UInt128& a, const UInt128& b, UInt128& result) {
  std::uint64_t carry = 0;
  for (std::size_t i = 0; i < 4; ++i) {
    const std::uint64_t sum = std::uint64_t{a.digit[i]} + b.digit[i] + carry;
    result.digit[i] = static_cast<std::uint32_t>(sum);
    carry = sum >> 32;
  }
  return carry == 0;
}

/// @brief Subtracts nonnegative magnitudes with explicit borrow.
bool Subtract(const UInt128& a, const UInt128& b, UInt128& result) {
  std::uint64_t borrow = 0;
  for (std::size_t i = 0; i < 4; ++i) {
    const std::uint64_t sub = std::uint64_t{b.digit[i]} + borrow;
    result.digit[i] =
        static_cast<std::uint32_t>(std::uint64_t{a.digit[i]} - sub);
    borrow = std::uint64_t{a.digit[i]} < sub ? 1U : 0U;
  }
  return borrow == 0;
}

/// @brief Places a significand on the exact 2^-88 scaled lattice.
bool Shift(std::uint64_t value, int shift, UInt128& result) {
  result = {};
  if (shift < 0) return false;
  for (int bit = 0; bit < 64; ++bit) {
    if ((value & (std::uint64_t{1} << bit)) == 0) continue;
    const int target = bit + shift;
    if (target >= 128) return false;
    result.digit[target / 32] |= std::uint32_t{1} << (target % 32);
  }
  return true;
}

/// @brief Multiplies by at most 2^32+1 using bounded schoolbook products.
bool Multiply(const UInt128& a, std::uint64_t multiplier, UInt128& result) {
  result = {};
  for (std::size_t j = 0; j < 2; ++j) {
    const auto factor = static_cast<std::uint32_t>(multiplier >> (32 * j));
    std::uint64_t carry = 0;
    for (std::size_t i = 0; i < 4; ++i) {
      if (i + j >= 4) {
        if (a.digit[i] != 0 && factor != 0) return false;
        continue;
      }
      const std::uint64_t product =
          std::uint64_t{a.digit[i]} * factor + result.digit[i + j] + carry;
      result.digit[i + j] = static_cast<std::uint32_t>(product);
      carry = product >> 32;
    }
    if (carry != 0) return false;
  }
  return true;
}

struct Cell {
  UInt128 center;
  UInt128 lower;
  UInt128 upper;
  bool closed;
};

/// @brief Constructs ORIGINAL binary64 cells, including subnormal/maxfinite
/// gaps.
bool MakeCell(double value, int period_exponent, Cell& cell) {
  std::uint64_t bits = 0;
  std::memcpy(&bits, &value, sizeof(bits));
  const int exponent_bits = static_cast<int>((bits >> 52) & 0x7ffU);
  const std::uint64_t fraction = bits & ((std::uint64_t{1} << 52) - 1);
  const std::uint64_t significand =
      exponent_bits == 0 ? fraction : fraction + (std::uint64_t{1} << 52);
  const int exponent = exponent_bits == 0 ? -1074 : exponent_bits - 1075;
  const int lattice_shift = exponent - period_exponent + 88;
  UInt128 lower_gap;
  UInt128 upper_gap;
  // At a normal binade boundary only the predecessor gap is halved. The
  // smallest normal has subnormal neighbors with the same original spacing.
  const int boundary = fraction == 0 && exponent_bits > 1 ? 1 : 0;
  cell.closed = (bits & 1) == 0;
  return Shift(significand, lattice_shift, cell.center) &&
         Shift(1, lattice_shift - 1 - boundary, lower_gap) &&
         Shift(1, lattice_shift - 1, upper_gap) &&
         Subtract(cell.center, lower_gap, cell.lower) &&
         Add(cell.center, upper_gap, cell.upper);
}

/// @brief Returns source-neutral utility failure for parser/procedure
/// decoration.
Status GridFailure(const char* code, const char* message) {
  return Status::Failure(
      FailureCategory::kModel,
      {{Severity::kError, code, {}, "HEAT TRANSFER", "", message}});
}

}  // namespace

Result<ThermalTimeGrid> ThermalTimeGrid::Create(double period, double increment,
                                                std::uint32_t maximum) {
  if (!std::isfinite(period) || !std::isfinite(increment) || period <= 0 ||
      increment <= 0 || maximum == 0) {
    return Result<ThermalTimeGrid>::Failure(
        GridFailure("invalid-thermal-time-grid",
                    "Time controls must be finite and positive."));
  }
  if (increment >= period) {
    return Result<ThermalTimeGrid>::Success(
        ThermalTimeGrid{period, increment, 1U, period});
  }
  const double quotient = period / increment;
  const std::uint64_t limit = std::uint64_t{maximum} + 1U;
  if (!std::isfinite(quotient) || quotient > static_cast<double>(limit)) {
    return Result<ThermalTimeGrid>::Failure(GridFailure(
        "thermal-increment-limit", "Thermal increment count exceeds INC."));
  }
  // No floating-to-integer cast precedes the finite, positive count bound.
  std::uint64_t floor = static_cast<std::uint64_t>(std::floor(quotient));
  const double fraction = quotient - static_cast<double>(floor);
  const std::uint64_t nearest =
      floor + ((fraction > .5 || (fraction == .5 && (floor & 1) != 0)) ? 1 : 0);
  int exponent = 0;
  std::frexp(period, &exponent);
  Cell period_cell{};
  Cell increment_cell{};
  UInt128 lower;
  UInt128 upper;
  bool exact_ok = MakeCell(period, exponent, period_cell) &&
                  MakeCell(increment, exponent, increment_cell) &&
                  Multiply(increment_cell.lower, nearest, lower) &&
                  Multiply(increment_cell.upper, nearest, upper);
  const int first = Compare(period_cell.upper, lower);
  const int second = Compare(upper, period_cell.lower);
  const bool closed = period_cell.closed && increment_cell.closed;
  const bool close = nearest != 0 && (first > 0 || (first == 0 && closed)) &&
                     (second > 0 || (second == 0 && closed));
  std::uint64_t count = nearest;
  if (!close && exact_ok) {
    UInt128 product;
    UInt128 next;
    exact_ok = Multiply(increment_cell.center, floor, product) &&
               Multiply(increment_cell.center, floor + 1, next);
    if (Compare(product, period_cell.center) > 0) {
      --floor;
    } else if (Compare(next, period_cell.center) <= 0) {
      ++floor;
    }
    exact_ok = exact_ok && Multiply(increment_cell.center, floor, product) &&
               Multiply(increment_cell.center, floor + 1, next) &&
               Compare(product, period_cell.center) <= 0 &&
               Compare(next, period_cell.center) > 0;
    count = floor + (Compare(product, period_cell.center) == 0 ? 0U : 1U);
  }
  if (!exact_ok) {
    return Result<ThermalTimeGrid>::Failure(GridFailure(
        "invalid-thermal-time-grid", "Exact time lattice bounds failed."));
  }
  if (count == 0 || count > maximum) {
    return Result<ThermalTimeGrid>::Failure(GridFailure(
        "thermal-increment-limit", "Thermal increment count exceeds INC."));
  }
  const double last =
      close ? increment
            : std::fma(-static_cast<double>(count - 1), increment, period);
  ThermalTimeGrid grid{period, increment, static_cast<std::uint32_t>(count),
                       last};
  const auto frame = grid.Frame(grid.Count());
  if (!frame.HasValue())
    return Result<ThermalTimeGrid>::Failure(frame.GetStatus());
  // For j <= 2^32, adjacent exact j*d are separated by more than 2^19 ULPs
  // (subnormal products are exact). Thus the terminal check also suffices for
  // O(1) preflight; every requested frame still checks its own stored interval.
  return Result<ThermalTimeGrid>::Success(grid);
}

ThermalTimeGrid::ThermalTimeGrid(double period, double increment,
                                 std::uint32_t count, double final_delta)
    : period_{period},
      increment_{increment},
      count_{count},
      final_delta_{final_delta} {}

std::uint32_t ThermalTimeGrid::Count() const noexcept { return count_; }
double ThermalTimeGrid::Time(std::uint32_t index) const noexcept {
  return index == count_ ? period_ : static_cast<double>(index) * increment_;
}
double ThermalTimeGrid::Delta(std::uint32_t index) const noexcept {
  return index == count_ ? final_delta_ : increment_;
}
Result<ThermalTimeFrame> ThermalTimeGrid::Frame(std::uint32_t index) const {
  if (index == 0 || index > count_) {
    return Result<ThermalTimeFrame>::Failure(
        GridFailure("invalid-thermal-time-frame",
                    "Completed frame index is outside the grid."));
  }
  const double time = Time(index);
  const double delta = Delta(index);
  const double previous = Time(index - 1);
  if (!std::isfinite(time) || !std::isfinite(previous) ||
      !std::isfinite(delta) || !(time > previous) || !(delta > 0)) {
    return Result<ThermalTimeFrame>::Failure(
        GridFailure("invalid-thermal-time-frame",
                    "Stored times must be finite and strictly increasing."));
  }
  return Result<ThermalTimeFrame>::Success({time, delta});
}

}  // namespace fesa