#include "math/dense_blas_internal.h"
#include "math/jet_linear_solve.h"

#include <gtest/gtest.h>

#include <array>
#include <cstddef>
#include <limits>
#include <stdexcept>

namespace fesa::dense_blas_internal {
namespace {

// Forms RHS and residuals independently of the production dense adapter.
Matrix FactorRhs(const Matrix& a, const Matrix& x, const bool transpose,
                 const bool right) {
  Matrix b{x.Rows(), x.Columns()};
  for (std::size_t i = 0; i < b.Rows(); ++i)
    for (std::size_t j = 0; j < b.Columns(); ++j)
      for (std::size_t k = 0; k < a.Rows(); ++k)
        b(i, j) += right ? x(i, k) * (transpose ? a(j, k) : a(k, j))
                         : (transpose ? a(k, i) : a(i, k)) * x(k, j);
  return b;
}

void ExpectFactorSolution(const Matrix& a, const Matrix& x, const Matrix& b,
                          const Matrix& expected, const bool transpose,
                          const bool right) {
  ASSERT_EQ(x.Rows(), expected.Rows());
  ASSERT_EQ(x.Columns(), expected.Columns());
  const auto residual = FactorRhs(a, x, transpose, right);
  for (std::size_t i = 0; i < x.Rows(); ++i)
    for (std::size_t j = 0; j < x.Columns(); ++j) {
      EXPECT_DOUBLE_EQ(x(i, j), expected(i, j));
      EXPECT_DOUBLE_EQ(residual(i, j), b(i, j));
    }
}

TEST(DenseFactor, LuSolvesRectangularRhsOnBothSidesAndTransposes) {
  Matrix a{3U, 3U};
  const double values[]{0, 2, 1, 1, 1, 0, 2, 0, 1};
  for (std::size_t i = 0; i < 9U; ++i) a.Data()[i] = values[i];
  auto factor = DenseFactor::Lu(a);
  ASSERT_TRUE(factor.HasValue());
  EXPECT_DOUBLE_EQ(factor.Value().Determinant(), -4.0);
  for (const bool right : {false, true})
    for (const bool transpose : {false, true}) {
      Matrix expected{right ? 2U : 3U, right ? 3U : 2U};
      for (std::size_t i = 0; i < 6U; ++i)
        expected.Data()[i] = static_cast<double>(i) - 2.0;
      const auto b = FactorRhs(a, expected, transpose, right);
      const auto solved = right ? factor.Value().SolveRight(b, transpose)
                                : factor.Value().Solve(b, transpose);
      ASSERT_TRUE(solved.HasValue());
      ExpectFactorSolution(a, solved.Value(), b, expected, transpose, right);
    }
}

TEST(DenseFactor, CholeskySolvesAndWhiteningRetainsScaleAndOrientation) {
  Matrix a{2U, 2U};
  a(0, 0) = 4;
  a(0, 1) = 2;
  a(1, 0) = 2;
  a(1, 1) = 5;
  const auto factor = DenseFactor::Cholesky(a);
  ASSERT_TRUE(factor.HasValue());
  EXPECT_DOUBLE_EQ(factor.Value().Determinant(), 16.0);
  const auto lower = factor.Value().Lower();
  ASSERT_TRUE(lower.HasValue());
  EXPECT_DOUBLE_EQ(lower.Value()(0, 0), 2.0);
  EXPECT_DOUBLE_EQ(lower.Value()(0, 1), 0.0);
  EXPECT_DOUBLE_EQ(lower.Value()(1, 0), 1.0);
  EXPECT_DOUBLE_EQ(lower.Value()(1, 1), 2.0);
  for (const bool right : {false, true})
    for (const bool transpose : {false, true}) {
      Matrix expected{right ? 3U : 2U, right ? 2U : 3U};
      for (std::size_t i = 0; i < 6U; ++i)
        expected.Data()[i] = static_cast<double>(i) - 2.0;
      const auto b = FactorRhs(a, expected, transpose, right);
      const auto solved = right ? factor.Value().SolveRight(b, transpose)
                                : factor.Value().Solve(b, transpose);
      ASSERT_TRUE(solved.HasValue());
      ExpectFactorSolution(a, solved.Value(), b, expected, transpose, right);
      const auto triangular_rhs =
          FactorRhs(lower.Value(), expected, transpose, right);
      const auto triangular =
          right ? factor.Value().SolveLowerRight(triangular_rhs, transpose)
                : factor.Value().SolveLower(triangular_rhs, transpose);
      ASSERT_TRUE(triangular.HasValue());
      ExpectFactorSolution(lower.Value(), triangular.Value(), triangular_rhs,
                           expected, transpose, right);
    }
  // W = I * L^-T / sqrt(4); verify W^T (4*A) W = I without a procedure.
  Matrix basis{3U, 2U};
  basis(0, 0) = 0.5;
  basis(1, 1) = 0.5;
  const auto w = factor.Value().SolveLowerRight(basis, true);
  ASSERT_TRUE(w.HasValue());
  for (std::size_t i = 0; i < 2U; ++i)
    for (std::size_t j = 0; j < 2U; ++j) {
      double gram = 0;
      for (std::size_t k = 0; k < 2U; ++k)
        for (std::size_t l = 0; l < 2U; ++l)
          gram += w.Value()(k, i) * (4 * a(k, l)) * w.Value()(l, j);
      EXPECT_DOUBLE_EQ(gram, i == j ? 1.0 : 0.0);
    }
}

TEST(DenseFactor, OwnsSnapshotAndFailedCandidatesPreserveReusableFactor) {
  Matrix a{2U, 2U};
  a(0, 0) = 2;
  a(1, 1) = 4;
  auto factor = DenseFactor::Lu(a);
  ASSERT_TRUE(factor.HasValue());
  a(0, 0) = 8;
  const auto changed = DenseFactor::Lu(a);
  ASSERT_TRUE(changed.HasValue());
  Matrix rhs{2U, 1U, 8};
  const auto old_solution = factor.Value().Solve(rhs);
  ASSERT_TRUE(old_solution.HasValue());
  EXPECT_DOUBLE_EQ(old_solution.Value()(0, 0), 4);
  EXPECT_DOUBLE_EQ(changed.Value().Solve(rhs).Value()(0, 0), 1);
  EXPECT_FALSE(DenseFactor::Lu(Matrix{2U, 2U}).HasValue());
  EXPECT_FALSE(DenseFactor::Cholesky(Matrix{2U, 2U}).HasValue());
  a(0, 0) = -1;
  EXPECT_FALSE(DenseFactor::Cholesky(a).HasValue());
  EXPECT_FALSE(DenseFactor::Lu(Matrix{2U, 3U}).HasValue());
  a(0, 0) = (std::numeric_limits<double>::quiet_NaN)();
  EXPECT_FALSE(DenseFactor::Lu(a).HasValue());
  EXPECT_FALSE(DenseFactor::Cholesky(a).HasValue());
  EXPECT_FALSE(factor.Value().Solve(Matrix{3U, 1U}).HasValue());
  EXPECT_FALSE(factor.Value().SolveRight(Matrix{1U, 3U}).HasValue());
  EXPECT_FALSE(factor.Value().SolveLower(rhs).HasValue());
  rhs(0, 0) = (std::numeric_limits<double>::infinity)();
  EXPECT_FALSE(factor.Value().Solve(rhs).HasValue());
  EXPECT_DOUBLE_EQ(old_solution.Value()(0, 0), 4);
  rhs(0, 0) = 8;
  EXPECT_DOUBLE_EQ(factor.Value().Solve(rhs).Value()(0, 0), 4);
  Matrix tiny{1U, 1U, (std::numeric_limits<double>::min)()};
  const auto tiny_factor = DenseFactor::Lu(tiny);
  ASSERT_TRUE(tiny_factor.HasValue());
  EXPECT_FALSE(tiny_factor.Value()
                   .Solve(Matrix{1U, 1U, (std::numeric_limits<double>::max)()})
                   .HasValue());
  EXPECT_DOUBLE_EQ(tiny_factor.Value().Solve(tiny).Value()(0, 0), 1);
}

TEST(DenseFactor, EmptyAndInfinityConditionBoundaryAreExplicit) {
  for (const bool positive : {false, true}) {
    const auto empty = positive ? DenseFactor::Cholesky(Matrix{0U, 0U})
                                : DenseFactor::Lu(Matrix{0U, 0U});
    ASSERT_TRUE(empty.HasValue());
    const auto left = empty.Value().Solve(Matrix{0U, 3U});
    ASSERT_TRUE(left.HasValue());
    EXPECT_EQ(left.Value().Columns(), 3U);
    const auto right = empty.Value().SolveRight(Matrix{4U, 0U});
    ASSERT_TRUE(right.HasValue());
    EXPECT_EQ(right.Value().Rows(), 4U);
    EXPECT_FALSE(empty.Value().ReciprocalConditionInfinity().HasValue());
    for (const double diagonal : {0x1p-31, 0x1p-32, 0x1p-33}) {
      Matrix a{2U, 2U};
      a(0, 0) = 1;
      a(1, 1) = diagonal;
      const auto factor =
          positive ? DenseFactor::Cholesky(a) : DenseFactor::Lu(a);
      ASSERT_TRUE(factor.HasValue());
      const auto rcond = factor.Value().ReciprocalConditionInfinity();
      ASSERT_TRUE(rcond.HasValue());
      EXPECT_DOUBLE_EQ(rcond.Value(), diagonal);
      EXPECT_EQ(rcond.Value() >= 0x1p-32, diagonal >= 0x1p-32);
      EXPECT_EQ(factor.Value().Solve(Matrix{2U, 0U}).Value().Columns(), 0U);
      EXPECT_EQ(factor.Value().SolveRight(Matrix{0U, 2U}).Value().Rows(), 0U);
    }
  }
}

TEST(DenseFactor, GeneralDimensionAndNonDiagonalConditionUseFullCoefficient) {
  Matrix a{8U, 8U};
  a(0, 0) = 4;
  for (std::size_t i = 1; i < 8U; ++i) {
    a(i, i) = 5;
    a(i, i - 1U) = 2;
    a(i - 1U, i) = 2;
  }
  const auto factor = DenseFactor::Cholesky(a);
  ASSERT_TRUE(factor.HasValue());
  Matrix expected{8U, 3U};
  for (std::size_t i = 0; i < 24U; ++i)
    expected.Data()[i] = static_cast<double>(i) - 8;
  const auto rhs = FactorRhs(a, expected, false, false);
  const auto solved = factor.Value().Solve(rhs);
  ASSERT_TRUE(solved.HasValue());
  ExpectFactorSolution(a, solved.Value(), rhs, expected, false, false);
  Matrix general{2U, 2U};
  general(0, 0) = 2;
  general(0, 1) = 1;
  general(1, 1) = 1;
  const auto lu = DenseFactor::Lu(general);
  ASSERT_TRUE(lu.HasValue());
  EXPECT_DOUBLE_EQ(lu.Value().ReciprocalConditionInfinity().Value(), 1.0 / 3.0);
  EXPECT_FALSE(lu.Value().Lower().HasValue());
  Matrix overflow{2U, 2U, (std::numeric_limits<double>::max)()};
  overflow(1, 0) = -overflow(1, 0);
  EXPECT_FALSE(DenseFactor::Lu(overflow).HasValue());
}

TEST(DenseBlasInternal, CDup003ConvertsZeroNormalAndOverflowLengths) {
  const auto zero = ToMklSize(0U);
  ASSERT_TRUE(zero.HasValue());
  EXPECT_EQ(zero.Value(), 0);

  const auto normal = ToMklSize(3U);
  ASSERT_TRUE(normal.HasValue());
  EXPECT_EQ(normal.Value(), 3);

  const std::size_t overflow =
      static_cast<std::size_t>((std::numeric_limits<MKL_INT>::max)()) + 1U;
  const auto rejected = ToMklSize(overflow);
  EXPECT_FALSE(rejected.HasValue());
  EXPECT_FALSE(rejected.GetStatus().IsOk());
}

TEST(DenseBlasInternal, CDup003CopiesZeroAndNonzeroContiguousValues) {
  EXPECT_NO_THROW(CopyValues(nullptr, 0U, nullptr));

  const std::array<double, 3U> source{1.25, -2.5, 4.0};
  std::array<double, 3U> destination{};
  CopyValues(source.data(), source.size(), destination.data());

  EXPECT_EQ(destination, source);
}


TEST(DenseBlasInternal, TransposeLeftMatchesIndependentRectangularProduct) {
  Matrix left{3U, 2U};
  Matrix right{3U, 4U};
  for (std::size_t row = 0; row < 3U; ++row) {
    for (std::size_t column = 0; column < 2U; ++column)
      left(row, column) = static_cast<double>(1U + row * 2U + column);
    for (std::size_t column = 0; column < 4U; ++column)
      right(row, column) = static_cast<double>(row + column) - 2.0;
  }
  const Matrix product = MultiplyTransposeLeft(left, right);
  ASSERT_EQ(product.Rows(), 2U);
  ASSERT_EQ(product.Columns(), 4U);
  for (std::size_t row = 0; row < 2U; ++row)
    for (std::size_t column = 0; column < 4U; ++column) {
      double expected = 0.0;
      for (std::size_t inner = 0; inner < 3U; ++inner)
        expected += left(inner, row) * right(inner, column);
      EXPECT_DOUBLE_EQ(product(row, column), expected);
    }
  const auto gram = MultiplyTransposeLeft(left, left);
  EXPECT_DOUBLE_EQ(gram(0U, 0U), 35.0);
  EXPECT_DOUBLE_EQ(gram(0U, 1U), 44.0);
  EXPECT_DOUBLE_EQ(gram(1U, 1U), 56.0);
}

TEST(DenseBlasInternal, TransposeLeftRetainsEmptyAndFailureContracts) {
  const auto zeros = MultiplyTransposeLeft(Matrix{0U, 2U}, Matrix{0U, 3U});
  ASSERT_EQ(zeros.Rows(), 2U);
  ASSERT_EQ(zeros.Columns(), 3U);
  EXPECT_DOUBLE_EQ(zeros(1U, 2U), 0.0);
  const auto empty = MultiplyTransposeLeft(
      Matrix{(std::numeric_limits<std::size_t>::max)(), 0U},
      Matrix{(std::numeric_limits<std::size_t>::max)(), 0U});
  EXPECT_EQ(empty.Rows(), 0U);
  EXPECT_EQ(empty.Columns(), 0U);
  EXPECT_THROW(static_cast<void>(MultiplyTransposeLeft(Matrix{1U, 2U},
                                                       Matrix{2U, 1U})),
               std::invalid_argument);
  EXPECT_THROW(static_cast<void>(MultiplyTransposeLeft(
                   Matrix{1U, 0U}, Matrix{1U, 1U,
                       (std::numeric_limits<double>::quiet_NaN)()})),
               std::invalid_argument);
}

TEST(DenseBlasInternal, RawProductsPreserveDestinationOnNonfiniteInput) {
  const double left[]{1.0, 2.0, -3.0, 4.0};
  const double right[]{5.0, -6.0, 7.0, 8.0, -9.0, 10.0};
  std::array<double, 6U> destination{};
  ASSERT_TRUE(MultiplyTransposeRight(left, 2U, right, 3U, 2U,
                                      destination.data()));
  EXPECT_EQ(destination, (std::array<double, 6U>{-7.0, 23.0, 11.0,
                                                -39.0, 11.0, 67.0}));
  const double invalid[]{(std::numeric_limits<double>::infinity)(), 1.0};
  destination.fill(42.0);
  EXPECT_FALSE(MultiplyTransposeRight(invalid, 1U, right, 3U, 2U,
                                       destination.data()));
  EXPECT_FALSE(OuterProduct(left, 2U, invalid, 2U, destination.data()));
  for (const double value : destination) EXPECT_DOUBLE_EQ(value, 42.0);
  ASSERT_TRUE(OuterProduct(left, 2U, right, 3U, destination.data()));
  EXPECT_EQ(destination, (std::array<double, 6U>{5.0, -6.0, 7.0,
                                                10.0, -12.0, 14.0}));
}

TEST(DenseBlasInternal, RawProductZeroDimensionsAndOverflowAreExplicit) {
  EXPECT_TRUE(OuterProduct(nullptr, 0U, nullptr, 3U, nullptr));
  EXPECT_TRUE(MultiplyTransposeRight(nullptr, 2U, nullptr, 0U, 3U, nullptr));
  std::array<double, 6U> zeros{1.0, 2.0, 3.0, 4.0, 5.0, 6.0};
  ASSERT_TRUE(MultiplyTransposeRight(nullptr, 2U, nullptr, 3U, 0U,
                                      zeros.data()));
  for (const double value : zeros) EXPECT_DOUBLE_EQ(value, 0.0);
  EXPECT_THROW(MultiplyTransposeRight(nullptr, 1U, nullptr, 1U, 1U, nullptr),
               std::invalid_argument);
  const auto overflow =
      static_cast<std::size_t>((std::numeric_limits<MKL_INT>::max)()) + 1U;
  EXPECT_THROW(MultiplyTransposeRight(nullptr, overflow, nullptr, 0U, 0U,
                                       nullptr), std::length_error);
  double unchanged = 42.0;
  EXPECT_THROW(CopyValues(&unchanged, overflow, &unchanged), std::length_error);
  EXPECT_DOUBLE_EQ(unchanged, 42.0);
}

TEST(DenseBlasInternal, ContiguousPrimitivesCheckEmptyAndOverflowBeforeUse) {
  EXPECT_DOUBLE_EQ(DotValues(nullptr, 0U, nullptr), 0.0);
  EXPECT_DOUBLE_EQ(NormValues(nullptr, 0U), 0.0);
  EXPECT_NO_THROW(ScaleValues(2.0, nullptr, 0U));
  EXPECT_NO_THROW(AxpyValues(2.0, nullptr, 0U, nullptr));
  const auto overflow =
      static_cast<std::size_t>((std::numeric_limits<MKL_INT>::max)()) + 1U;
  double unchanged = 42.0;
  EXPECT_THROW(DotValues(nullptr, overflow, nullptr), std::length_error);
  EXPECT_THROW(NormValues(nullptr, overflow), std::length_error);
  EXPECT_THROW(ScaleValues(2.0, &unchanged, overflow), std::length_error);
  EXPECT_THROW(AxpyValues(2.0, nullptr, overflow, &unchanged), std::length_error);
  EXPECT_DOUBLE_EQ(unchanged, 42.0);
}

}  // namespace
}  // namespace fesa::dense_blas_internal

namespace fesa::math_internal {
namespace {
// Taylor coefficients of t^i s^j through total degree three. This independent
// polynomial oracle deliberately uses coefficients, not derivative channels.
struct Polynomial {
  std::array<std::array<double, 4>, 4> c{};
};
Polynomial operator-(Polynomial a, const Polynomial& b) {
  for (std::size_t i = 0; i <= 3; ++i)
    for (std::size_t j = 0; i + j <= 3; ++j) a.c[i][j] -= b.c[i][j];
  return a;
}
Polynomial operator*(const Polynomial& a, const Polynomial& b) {
  Polynomial result;
  for (std::size_t i = 0; i <= 3; ++i)
    for (std::size_t j = 0; i + j <= 3; ++j)
      for (std::size_t k = 0; k <= i; ++k)
        for (std::size_t l = 0; l <= j; ++l)
          result.c[i][j] += a.c[k][l] * b.c[i-k][j-l];
  return result;
}
Polynomial PolynomialConstant(double value) {
  Polynomial p;
  p.c[0][0] = value;
  return p;
}
double PolynomialRead(const Polynomial& p, std::size_t degree,
                       std::size_t channel) {
  return p.c[channel][degree-channel];
}
void PolynomialWrite(Polynomial& p, std::size_t degree, std::size_t channel,
                      double value) {
  p.c[channel][degree-channel] = value;
}
TEST(JetLinearSolve, RecoversMixedCubicCoefficientsWithOnePrimalFactor) {
  std::array<std::array<Polynomial, 3>, 3> a{};
  const double values[]{0, 2, 1, 1, 1, 0, 2, 0, 1};
  for (std::size_t i = 0; i < 3; ++i)
    for (std::size_t j = 0; j < 3; ++j) {
      a[i][j] = PolynomialConstant(values[3*i+j]);
      for (std::size_t degree = 1; degree <= 3; ++degree)
        for (std::size_t c = 0; c <= degree; ++c)
          a[i][j].c[c][degree-c] = (static_cast<double>(i+2*j+c)-2) / 16;
    }
  std::array<Polynomial, 3> expected{}, rhs{};
  for (std::size_t i = 0; i < 3; ++i)
    for (std::size_t degree = 0; degree <= 3; ++degree)
      for (std::size_t c = 0; c <= degree; ++c)
        expected[i].c[c][degree-c] = (static_cast<double>(i+degree+2*c)-3) / 8;
  // Form the full product independently using exact dyadic long-double sums.
  for (std::size_t row = 0; row < 3; ++row)
    for (std::size_t i = 0; i <= 3; ++i)
      for (std::size_t j = 0; i+j <= 3; ++j) {
        long double sum = 0;
        for (std::size_t column = 0; column < 3; ++column)
          for (std::size_t k = 0; k <= i; ++k)
            for (std::size_t l = 0; l <= j; ++l)
              sum += static_cast<long double>(a[row][column].c[k][l]) *
                     expected[column].c[i-k][j-l];
        rhs[row].c[i][j] = static_cast<double>(sum);
      }
  Matrix primal{3, 3};
  for (std::size_t i = 0; i < 3; ++i)
    for (std::size_t j = 0; j < 3; ++j) primal(i,j) = a[i][j].c[0][0];
  const auto factor = dense_blas_internal::DenseFactor::Lu(primal);
  ASSERT_TRUE(factor.HasValue());
  std::array<Polynomial, 3> solved{};
  ASSERT_TRUE(SolveJetAction(a, rhs, factor.Value(), {1,2,3,4},
                            PolynomialConstant, PolynomialRead,
                            PolynomialWrite, solved));
  for (std::size_t row = 0; row < 3; ++row)
    for (std::size_t i = 0; i <= 3; ++i)
      for (std::size_t j = 0; i+j <= 3; ++j)
        EXPECT_NEAR(solved[row].c[i][j], expected[row].c[i][j], 2e-14);
}
TEST(JetLinearSolve, FailedChannelSolvePreservesCandidateAndInputs) {
  std::array<std::array<Polynomial, 3>, 3> a{};
  Matrix primal{3, 3};
  for (std::size_t i = 0; i < 3; ++i) {
    primal(i,i) = 1;
    a[i][i] = PolynomialConstant(1);
  }
  const auto factor = dense_blas_internal::DenseFactor::Lu(primal);
  ASSERT_TRUE(factor.HasValue());
  std::array<Polynomial, 3> rhs{}, solved{};
  rhs[1].c[1][0] = std::numeric_limits<double>::infinity();
  solved[0].c[0][0] = 17;
  EXPECT_FALSE(SolveJetAction(a, rhs, factor.Value(), {1,2,3,4},
                             PolynomialConstant, PolynomialRead,
                             PolynomialWrite, solved));
  EXPECT_EQ(solved[0].c[0][0], 17);
  EXPECT_EQ(a[1][1].c[0][0], 1);
  EXPECT_EQ(rhs[1].c[1][0], std::numeric_limits<double>::infinity());
}
}  // namespace
}  // namespace fesa::math_internal