#include "qc/matrix.hpp"

#include <cmath>
#include <cstdio>
#include <stdexcept>

static void expect_near(const char* name, double a, double b, double tol) {
  if (std::abs(a - b) > tol) {
    std::fprintf(stderr, "FAIL %s: got %.12g expected %.12g\n", name, a, b);
    throw std::runtime_error(name);
  }
}

int main() {
  qc::Matrix A(2, 2);
  A(0, 0) = 4.0;
  A(0, 1) = 1.0;
  A(1, 0) = 1.0;
  A(1, 1) = 3.0;
  auto d = qc::symmetric_eigen_jacobi(std::move(A));
  const double rt5 = std::sqrt(5.0);
  expect_near("e0", d.eigenvalues[0], 0.5 * (7.0 - rt5), 1e-10);
  expect_near("e1", d.eigenvalues[1], 0.5 * (7.0 + rt5), 1e-10);

  qc::Matrix S(2, 2);
  S(0, 0) = 1.0;
  S(0, 1) = 0.5;
  S(1, 0) = 0.5;
  S(1, 1) = 1.0;
  qc::Matrix F(2, 2);
  F(0, 0) = 2.0;
  F(0, 1) = 0.25;
  F(1, 0) = 0.25;
  F(1, 1) = 3.0;
  qc::Vector w;
  qc::Matrix V;
  qc::generalized_symmetric_eigen(F, S, w, V);
  const double orth =
      V(0, 0) * (S(0, 0) * V(0, 0) + S(0, 1) * V(1, 0)) +
      V(1, 0) * (S(1, 0) * V(0, 0) + S(1, 1) * V(1, 0));
  expect_near("V^T S V", orth, 1.0, 1e-9);

  qc::Matrix h(2, 2);
  h(0, 0) = -1.12006617;
  h(0, 1) = -0.95775913;
  h(1, 0) = -0.95775913;
  h(1, 1) = -1.12006617;
  qc::Matrix S2(2, 2);
  S2(0, 0) = 1.00000026;
  S2(0, 1) = 0.65897215;
  S2(1, 0) = 0.65897215;
  S2(1, 1) = 1.00000026;
  qc::Vector ew;
  qc::Matrix ev;
  qc::generalized_symmetric_eigen(h, S2, ew, ev);
  expect_near("h2 core e0", ew[0], -1.25247731, 1e-5);
  expect_near("h2 core e1", ew[1], -0.47593449, 1e-5);

  std::puts("ok");
  return 0;
}