#include "fesa/Assembly/Assembly.hpp"

#include <cmath>
#include <stdexcept>
#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);
  }
}

fesa::Domain assemblyDomain() {
  fesa::Domain domain;
  domain.nodes[1] = {1, {0.0, 0.0, 0.0}};
  domain.nodes[2] = {2, {1.0, 0.0, 0.0}};
  domain.nodes[3] = {3, {1.0, 1.0, 0.0}};
  domain.nodes[4] = {4, {0.0, 1.0, 0.0}};
  domain.elements[1] = {1, fesa::ElementType::MITC4, {1, 2, 3, 4}, "EALL"};
  domain.element_sets["eall"] = {"EALL", {1}};
  domain.node_sets["left"] = {"LEFT", {1, 4}};
  domain.node_sets["right"] = {"RIGHT", {2, 3}};
  domain.materials["steel"] = {"STEEL", 1000.0, 0.30};
  domain.shell_sections.push_back({"EALL", "STEEL", 0.1});
  domain.boundary_conditions.push_back({"LEFT", 1, 6, 0.0});
  domain.boundary_conditions.push_back({"RIGHT", 1, 2, 0.0});
  domain.boundary_conditions.push_back({"RIGHT", 4, 6, 0.0});
  domain.loads.push_back({"2", 3, -1.0});
  domain.loads.push_back({"3", 3, -1.0});
  return domain;
}

}  // namespace

int main() {
  const auto domain = assemblyDomain();
  const fesa::DofManager dofs(domain);

  const auto pattern = fesa::buildReducedSparsePattern(domain, dofs);
  check(pattern.equation_count == dofs.freeDofCount(), "reduced sparse pattern equation count changed");
  check(pattern.nonzeroCount() > 0, "reduced sparse pattern should remain non-empty");

  const auto assembly = fesa::assembleSystem(domain, dofs);
  check(assembly.ok(), "assembly should remain valid");
  check(assembly.k_full.rows() == dofs.fullDofCount(), "full stiffness row count changed");
  check(assembly.k_full.cols() == dofs.fullDofCount(), "full stiffness column count changed");
  check(static_cast<fesa::LocalIndex>(assembly.f_full.size()) == dofs.fullDofCount(), "full load size changed");
  checkNear(assembly.f_full[static_cast<std::size_t>(dofs.fullIndex(2, fesa::Dof::UZ))], -1.0, 1.0e-15,
            "node 2 UZ load changed");
  checkNear(assembly.f_full[static_cast<std::size_t>(dofs.fullIndex(3, fesa::Dof::UZ))], -1.0, 1.0e-15,
            "node 3 UZ load changed");

  const auto reduced = fesa::projectToReducedSystem(assembly, dofs);
  check(reduced.ok(), "reduced system projection should remain valid");
  check(reduced.k.rows() == dofs.freeDofCount(), "reduced stiffness row count changed");
  check(reduced.k.cols() == dofs.freeDofCount(), "reduced stiffness column count changed");
  check(static_cast<fesa::LocalIndex>(reduced.f.size()) == dofs.freeDofCount(), "reduced load size changed");
  check(reduced.free_full_indices == dofs.freeFullIndices(), "free full-index map changed");
  checkNear(reduced.f[0], -1.0, 1.0e-15, "first reduced load changed");
  checkNear(reduced.f[1], -1.0, 1.0e-15, "second reduced load changed");

  fesa::DenseMatrix k(3, 3);
  k(0, 0) = 4.0;
  k(1, 1) = 5.0;
  k(2, 2) = 6.0;
  const std::vector<fesa::Real> u = {0.5, -0.25, 2.0};
  const std::vector<fesa::Real> f = {1.0, 2.0, -3.0};
  const auto rf = fesa::recoverFullReaction(k, u, f);
  check(rf.size() == 3, "full reaction size changed");
  checkNear(rf[0], 1.0, 1.0e-15, "RF component 0 changed");
  checkNear(rf[1], -3.25, 1.0e-15, "RF component 1 changed");
  checkNear(rf[2], 15.0, 1.0e-15, "RF component 2 changed");

  return 0;
}