#include "fesa/Math/Math.hpp"

#include <cmath>
#include <cstdint>
#include <stdexcept>
#include <string>
#include <type_traits>
#include <vector>

namespace {

void check(bool value, const char* message) {
  if (!value) {
    throw std::runtime_error(message);
  }
}

void checkNear(fesa::Real actual, fesa::Real expected, fesa::Real tolerance, const char* message) {
  if (std::fabs(actual - expected) > tolerance) {
    throw std::runtime_error(message);
  }
}

}  // namespace

int main() {
  static_assert(std::is_same_v<fesa::SparseIndex, std::int64_t>, "SparseIndex must remain int64");
  static_assert(std::is_same_v<fesa::EquationId, std::int64_t>, "EquationId must remain int64");

  const fesa::Vec3 x{1.0, 0.0, 0.0};
  const fesa::Vec3 y{0.0, 1.0, 0.0};
  const auto z = fesa::cross(x, y);
  checkNear(fesa::dot(x, y), 0.0, 1.0e-12, "Vector dot product changed");
  checkNear(z.z, 1.0, 1.0e-12, "Vector cross product changed");
  checkNear(fesa::norm(fesa::Vec3{3.0, 4.0, 0.0}), 5.0, 1.0e-12, "Vector norm changed");
  check(fesa::normalizedIfValid(fesa::Vec3{0.0, 0.0, 0.0}) == std::nullopt, "Invalid normalization changed");

  fesa::SparsePattern pattern;
  pattern.equation_count = 3;
  pattern.entries.push_back({0, 0});
  pattern.entries.push_back({0, 2});
  check(pattern.nonzeroCount() == 2, "Sparse nonzero count changed");
  check(pattern.contains(0, 2), "Sparse pattern lookup changed");
  check(!pattern.contains(2, 0), "Sparse pattern lookup became nondirectional");

  fesa::DenseMatrix matrix(2, 2);
  matrix(0, 0) = 2.0;
  matrix(0, 1) = 1.0;
  matrix(1, 0) = 1.0;
  matrix(1, 1) = 3.0;
  const auto product = matrix.multiply({1.0, 2.0});
  checkNear(product[0], 4.0, 1.0e-12, "DenseMatrix multiply row 0 changed");
  checkNear(product[1], 7.0, 1.0e-12, "DenseMatrix multiply row 1 changed");

  const fesa::LinearSolver& solver = fesa::GaussianEliminationSolver{};
  const auto solved = solver.solve(matrix, {1.0, 2.0});
  check(solved.ok(), "Gaussian solver unexpectedly failed");
  checkNear(solved.x[0], 0.2, 1.0e-12, "Gaussian solver x0 changed");
  checkNear(solved.x[1], 0.6, 1.0e-12, "Gaussian solver x1 changed");

  fesa::DenseMatrix singular(2, 2);
  singular(0, 0) = 1.0;
  singular(0, 1) = 2.0;
  singular(1, 0) = 2.0;
  singular(1, 1) = 4.0;
  const auto failed = solver.solve(singular, {1.0, 2.0});
  check(!failed.ok(), "Singular solve unexpectedly passed");
  check(fesa::containsDiagnostic(failed.diagnostics, "FESA-SINGULAR-SOLVER"), "Singular diagnostic code changed");

  return 0;
}