// SPDX-FileCopyrightText: The Eigen Authors
// SPDX-License-Identifier: MPL-2.0

#include "main.h"
#include "fp_control.h"

struct ZeroOnlyScalar {
  double value;
  ZeroOnlyScalar(double x = 0) : value(x) {}
  friend ZeroOnlyScalar abs(const ZeroOnlyScalar& x) { return numext::abs(x.value); }
  friend ZeroOnlyScalar operator*(const ZeroOnlyScalar& x, const ZeroOnlyScalar& y) { return x.value * y.value; }
  friend bool operator<=(const ZeroOnlyScalar& x, const ZeroOnlyScalar& y) { return x.value <= y.value; }
};

struct ApproxOnlyScalar {
  double value;
  ApproxOnlyScalar(double x = 0) : value(x) {}
};

namespace Eigen {
template <>
struct NumTraits<ZeroOnlyScalar> : GenericNumTraits<ZeroOnlyScalar> {};
template <>
struct NumTraits<ApproxOnlyScalar> : GenericNumTraits<ApproxOnlyScalar> {};
namespace internal {
template <>
struct scalar_fuzzy_impl<ApproxOnlyScalar> {
  static bool isApprox(const ApproxOnlyScalar& x, const ApproxOnlyScalar& y, const ApproxOnlyScalar& precision) {
    return internal::isApprox(x.value, y.value, precision.value);
  }
};
}  // namespace internal
}  // namespace Eigen

void check_fuzzy_custom_scalars() {
  // Neither operation may instantiate the other scalar predicate, including in C++14.
  Matrix<ZeroOnlyScalar, 2, 1> zero;
  zero(0) = 0;
  zero(1) = 0;
  VERIFY(zero.isZero(ZeroOnlyScalar(1e-6)));
  zero(1) = 1;
  VERIFY(!zero.isZero(ZeroOnlyScalar(1e-6)));
  Matrix<ZeroOnlyScalar, Dynamic, 1> dynamicZero = zero;
  VERIFY(!dynamicZero.isZero(ZeroOnlyScalar(1e-6)));
  dynamicZero.resize(0);
  VERIFY(dynamicZero.isZero(ZeroOnlyScalar(1e-6)));

  Matrix<ApproxOnlyScalar, 2, 1> constant;
  constant(0) = 0.5;
  constant(1) = 0.5;
  VERIFY(constant.isApproxToConstant(ApproxOnlyScalar(0.5), ApproxOnlyScalar(1e-6)));
  constant(0) = 1;
  VERIFY(!constant.isApproxToConstant(ApproxOnlyScalar(0.5), ApproxOnlyScalar(1e-6)));
  Matrix<ApproxOnlyScalar, Dynamic, 1> dynamicConstant = constant;
  VERIFY(!dynamicConstant.isApproxToConstant(ApproxOnlyScalar(0.5), ApproxOnlyScalar(1e-6)));
  dynamicConstant.resize(0);
  VERIFY(dynamicConstant.isApproxToConstant(ApproxOnlyScalar(0.5), ApproxOnlyScalar(1e-6)));
}

template <typename Scalar, int Order>
void check_fuzzy_loops() {
  using Mat = Matrix<Scalar, Dynamic, Dynamic, Order>;
  const Scalar nan = NumTraits<Scalar>::quiet_NaN();
  for (Index rows : {0, 1, 3, 17, 31, 32, 33}) {
    for (Index cols : {0, 1, 5, 19, 31, 32, 33}) {
      Mat a = Mat::Zero(rows, cols);
      for (int variant = 0; variant < 6; ++variant) {
        if (variant == 1) a.setIdentity();
        if (variant == 2) a.setConstant(Scalar(0.5));
        if (variant == 3 && a.size()) a(a.rows() - 1, a.cols() - 1) = nan;
        if (variant == 4) a.setConstant(nan);
        if (variant == 5) a.setConstant(NumTraits<Scalar>::infinity());
        for (Scalar precision : {Scalar(0), Scalar(0.1), Scalar(-0.1), nan, NumTraits<Scalar>::infinity()}) {
          bool zero = true, constant = true, upper = true, lower = true;
          Scalar upperMax = Scalar(-1), lowerMax = Scalar(-1);
          for (Index j = 0; j < cols; ++j) {
            for (Index i = 0; i < rows; ++i) {
              zero &= internal::isMuchSmallerThan(a(i, j), Scalar(1), precision);
              constant &= internal::isApprox(a(i, j), Scalar(0.5), precision);
              const Scalar value = numext::abs(a(i, j));
              if (i <= j && value > upperMax) upperMax = value;
              if (i >= j && value > lowerMax) lowerMax = value;
            }
          }
          for (Index j = 0; j < cols; ++j)
            for (Index i = 0; i < rows; ++i) {
              if (i > j && numext::abs(a(i, j)) > upperMax * precision) upper = false;
              if (i < j && numext::abs(a(i, j)) > lowerMax * precision) lower = false;
            }
          VERIFY_IS_EQUAL(a.isZero(precision), zero);
          VERIFY_IS_EQUAL(a.isApproxToConstant(Scalar(0.5), precision), constant);
          VERIFY_IS_EQUAL(a.isUpperTriangular(precision), upper);
          VERIFY_IS_EQUAL(a.isLowerTriangular(precision), lower);
        }
      }
    }
  }
}

template <typename Scalar, int Order>
void check_fuzzy_boundaries() {
  using Mat = Matrix<Scalar, Dynamic, Dynamic, Order>;
  using Predicate = internal::fuzzy_constant_predicate<Scalar, false>;
  STATIC_CHECK((internal::predicate_reduction<Mat, Predicate>::LinearAccess));
  STATIC_CHECK((!internal::predicate_reduction<Block<Mat>, Predicate>::LinearAccess));
  constexpr int packetSize = internal::packet_traits<Scalar>::size;
  // Sizes up to 9 * packetSize + 3 run the long path's 8-packet blocks, 4-packet block and remainder for every packet
  // size.
  for (Index size : {Index(1), Index(packetSize - 1), Index(packetSize), Index(packetSize + 1),
                     Index(2 * packetSize + 1), Index(4 * packetSize), Index(9 * packetSize + 3)}) {
    // Unaligned linear access, including a packet crossing a column boundary.
    std::vector<Scalar> data(3 * size + 1);
    Map<Mat, Unaligned> matrix(data.data() + 1, 3, size);
    for (Index k = 0; k < matrix.size(); ++k) {
      matrix.setZero();
      VERIFY(matrix.isZero());
      matrix(k) = NumTraits<Scalar>::quiet_NaN();
      VERIFY(!matrix.isZero());
      matrix.setConstant(Scalar(0.5));
      VERIFY(matrix.isApproxToConstant(Scalar(0.5)));
      matrix(k) = Scalar(1);
      VERIFY(!matrix.isApproxToConstant(Scalar(0.5)));
    }
  }
  for (Index inner : {Index(1), Index(packetSize), Index(packetSize + 1), Index(2 * packetSize + 1),
                      Index(4 * packetSize), Index(9 * packetSize + 3)}) {
    // Three inner vectors of the given length.
    const Index rows = Order == RowMajor ? 3 : inner, cols = Order == RowMajor ? inner : 3;
    Mat storage = Mat::Zero(rows + 2, cols + 2);
    auto block = storage.block(1, 1, rows, cols);
    for (Index k = 0; k < rows * cols; ++k) {
      block.setZero();
      VERIFY(block.isZero());
      block(k / cols, k % cols) = Scalar(1);
      VERIFY(!block.isZero());
      block.setConstant(Scalar(0.5));
      VERIFY(block.isApproxToConstant(Scalar(0.5)));
      block(k / cols, k % cols) = Scalar(1);
      VERIFY(!block.isApproxToConstant(Scalar(0.5)));
    }
  }
  Matrix<Scalar, 3, 3, Order> fixed = Matrix<Scalar, 3, 3, Order>::Zero();
  VERIFY(fixed.isZero());
  fixed(2, 2) = Scalar(1);
  VERIFY(!fixed.isZero());
  for (Index k = 0; k < fixed.size(); ++k) {
    fixed.setZero();
    fixed(k) = Scalar(1);
    VERIFY(!fixed.isZero());
    fixed.setConstant(Scalar(0.5));
    fixed(k) = Scalar(1);
    VERIFY(!fixed.isApproxToConstant(Scalar(0.5)));
  }
  Matrix<Scalar, packetSize + 1, 1> packetTail;
  for (Index k = 0; k < packetTail.size(); ++k) {
    packetTail.setZero();
    packetTail(k) = Scalar(1);
    VERIFY(!packetTail.isZero());
    packetTail.setConstant(Scalar(0.5));
    packetTail(k) = Scalar(1);
    VERIFY(!packetTail.isApproxToConstant(Scalar(0.5)));
  }
  Scalar data[18] = {};
  Map<Matrix<Scalar, Dynamic, 1>, Unaligned, InnerStride<2>> strided(data, 9);
  VERIFY(strided.isZero());
  strided(8) = Scalar(1);
  VERIFY(!strided.isZero());
}

// The packet test of isApproxToConstant() holds for 0 <= prec <= 1 and a finite constant; other precisions and
// constants fall back to scalar tests. Coefficients of either sign, signed zeros and non-finite values at every
// position of a vector long enough for packet blocks and a scalar tail.
template <typename Scalar>
void check_fuzzy_constant_signs() {
  const Scalar inf = NumTraits<Scalar>::infinity(), nan = NumTraits<Scalar>::quiet_NaN();
  constexpr int packetSize = internal::packet_traits<Scalar>::size;
  Matrix<Scalar, Dynamic, 1> v(9 * packetSize + 3);
  for (Scalar value : {Scalar(0.5), Scalar(-0.5), Scalar(0), -Scalar(0), inf, -inf, NumTraits<Scalar>::highest()}) {
    for (Scalar precision : {Scalar(0), Scalar(1e-3), Scalar(0.5), Scalar(1), Scalar(1.5), Scalar(3), inf}) {
      for (Scalar x :
           {-value, value * Scalar(1.25), value * Scalar(-0.25), Scalar(0), -Scalar(0), inf, -inf, nan, Scalar(1)}) {
        for (Index k = 0; k < v.size(); ++k) {
          v.setConstant(value);
          v(k) = x;
          bool expected = true;
          for (Index i = 0; i < v.size(); ++i) expected &= internal::isApprox(v(i), value, precision);
          VERIFY_IS_EQUAL(v.isApproxToConstant(value, precision), expected);
        }
      }
    }
  }
}

template <typename Scalar, int Order>
void check_fuzzy_subnormals() {
  const ScopedFlushToZero gradual(FlushToZeroMode::None);
  // Require gradual scalar arithmetic, even if the packet unit flushes (ARMv7 NEON).
  if (subnormalInputProbe<Scalar>() == Scalar(0) || underflowProbe<Scalar>() == Scalar(0)) return;
  const Scalar subnormal = (std::numeric_limits<Scalar>::min)() / Scalar(4);
  using Mat = Matrix<Scalar, Dynamic, Dynamic, Order>;
  using Predicate = internal::fuzzy_constant_predicate<Scalar, false>;
  STATIC_CHECK(
      (!EIGEN_ARCH_ARM || !std::is_same<Scalar, float>::value || !internal::functor_traits<Predicate>::PacketAccess));
  for (Index size : {1, 2, 4, 8, 9, 17}) {
    Matrix<Scalar, Dynamic, 1> v = Matrix<Scalar, Dynamic, 1>::Zero(size);
    for (Index k = 0; k < size; ++k) {
      for (Scalar sign : {Scalar(1), Scalar(-1)}) {
        v.setZero();
        v(k) = sign * subnormal;
        VERIFY(!v.isZero(Scalar(0)));
        VERIFY(!v.isConstant(Scalar(0)));
        VERIFY(v.isZero(subnormal));
        VERIFY(!v.isZero(subnormal / Scalar(2)));
      }
    }
  }
  Mat c = Mat::Constant(4, 4, Scalar(2) * subnormal);
  VERIFY(!c.isApproxToConstant(subnormal, Scalar(0)));
  VERIFY(!c.isZero(subnormal));
  VERIFY(c.isApproxToConstant(Scalar(2) * subnormal, Scalar(0)));
  VERIFY(c.isZero(Scalar(2) * subnormal));
  VERIFY(!c.block(0, 0, 3, 3).isZero(subnormal));
  VERIFY(!c.block(0, 0, 3, 3).isApproxToConstant(subnormal, Scalar(0)));
  Matrix<Scalar, 3, 3, Order> fixed = c.template topLeftCorner<3, 3>();
  VERIFY(!fixed.isZero(subnormal));
  VERIFY(!fixed.isApproxToConstant(subnormal, Scalar(0)));

  // Normal operands can produce a subnormal difference in the approximate comparison.
  const Scalar normal = (std::numeric_limits<Scalar>::min)();
  c.setConstant(normal + subnormal);
  VERIFY(!c.isApproxToConstant(normal, Scalar(0)));
  VERIFY(c.isApproxToConstant(normal + subnormal, Scalar(0)));
}

EIGEN_DECLARE_TEST(fuzzy_matrix_checks) {
  for (int repeat = 0; repeat < g_repeat; ++repeat) {
    CALL_SUBTEST_1((check_fuzzy_boundaries<float, ColMajor>()));
    CALL_SUBTEST_1((check_fuzzy_loops<float, ColMajor>()));
    CALL_SUBTEST_1((check_fuzzy_subnormals<float, ColMajor>()));
    CALL_SUBTEST_2((check_fuzzy_boundaries<double, ColMajor>()));
    CALL_SUBTEST_2((check_fuzzy_loops<double, ColMajor>()));
    CALL_SUBTEST_2((check_fuzzy_subnormals<double, ColMajor>()));
    CALL_SUBTEST_3((check_fuzzy_boundaries<float, RowMajor>()));
    CALL_SUBTEST_3((check_fuzzy_loops<float, RowMajor>()));
    CALL_SUBTEST_3((check_fuzzy_subnormals<float, RowMajor>()));
    CALL_SUBTEST_4((check_fuzzy_boundaries<double, RowMajor>()));
    CALL_SUBTEST_4((check_fuzzy_loops<double, RowMajor>()));
    CALL_SUBTEST_4((check_fuzzy_subnormals<double, RowMajor>()));
    CALL_SUBTEST_5(check_fuzzy_custom_scalars());
    CALL_SUBTEST_6(check_fuzzy_constant_signs<float>());
    CALL_SUBTEST_6(check_fuzzy_constant_signs<double>());
  }
}