#include "thermal_shell_geometry.h"

#include <algorithm>
#include <array>
#include <cmath>
#include <cstdint>
#include <limits>
#include <map>
#include <set>
#include <string>
#include <utility>
#include <vector>

#include "fesa/model/model_types.h"
#include "math/dense_blas_internal.h"

namespace fesa {
namespace thermal_internal {
namespace {

/// @brief Represents an exact input vector by three singleton intervals.
IntervalVector Singleton(const Vector3& point) {
  return {{Interval{point.X()}, Interval{point.Y()}, Interval{point.Z()}}};
}

/// @brief Checks a candidate direction against the ORIGINAL interval geometry.
bool Certify(const Geometry& geometry, Vector3 direction, double& lower) {
  const double magnitude = direction.Norm();
  if (!(magnitude > 0) || !std::isfinite(magnitude)) return false;
  direction = direction / magnitude;
  const auto s = Singleton(direction);
  lower = std::numeric_limits<double>::infinity();
  for (double xi : {-1., 1.}) {
    for (double eta : {-1., 1.}) {
      const auto w = Cross(geometry.bi + Interval{eta} * geometry.di,
                           geometry.ci + Interval{xi} * geometry.di);
      const Interval projection = Dot(s, w);
      if (!projection.IsFinite() || !(projection.lower > 0)) return false;
      lower = std::min(lower, projection.lower);
    }
  }
  const Interval bound = Interval{lower} / Sqrt(NormSquared(s));
  lower = bound.lower;
  return bound.IsFinite() && lower > 0;
}

}  // namespace

Status GeometryFailure(const SourceEntityId& source,
                       const SourceLocation& location, const char* code,
                       const char* message) {
  return Status::Failure(
      FailureCategory::kModel,
      {{Severity::kError, code, location, "DS4",
        source.instance_name + ":" + source.source_label_text, message}});
}

Result<Geometry> BuildGeometry(const std::array<Vector3, 4>& coordinates,
                               const SourceEntityId& source,
                               const SourceLocation& location) {
  double largest = 0;
  for (std::size_t i = 0; i < 4; ++i) {
    if (!coordinates[i].IsFinite()) {
      return Result<Geometry>::Failure(
          GeometryFailure(source, location, "invalid-thermal-shell-geometry",
                          "Coordinates must be finite."));
    }
    for (std::size_t j = 0; j < i; ++j) {
      if (coordinates[i] == coordinates[j]) {
        return Result<Geometry>::Failure(
            GeometryFailure(source, location, "invalid-thermal-shell-geometry",
                            "Patch has repeated coordinates."));
      }
    }
    for (double component : coordinates[i].Components()) {
      largest = std::max(largest, std::abs(component));
    }
  }
  int coordinate_exponent = 0;
  std::frexp(largest, &coordinate_exponent);
  std::array<Vector3, 4> offsets{};
  std::array<IntervalVector, 4> intervals{};
  double length = 0;
  // Scaling before subtraction prevents overflow even for opposite huge inputs.
  for (std::size_t i = 1; i < 4; ++i) {
    std::array<double, 3> offset{};
    for (std::size_t j = 0; j < 3; ++j) {
      offset[j] = std::ldexp(coordinates[i][j], -coordinate_exponent) -
                  std::ldexp(coordinates[0][j], -coordinate_exponent);
      intervals[i][j] =
          Scale(Interval{coordinates[i][j]}, -coordinate_exponent) -
          Scale(Interval{coordinates[0][j]}, -coordinate_exponent);
      length = std::max(length, std::abs(offset[j]));
    }
    offsets[i] = Vector3{offset};
  }
  int offset_exponent = 0;
  std::frexp(length, &offset_exponent);
  for (std::size_t i = 1; i < 4; ++i) {
    std::array<double, 3> offset{};
    for (std::size_t j = 0; j < 3; ++j) {
      offset[j] = std::ldexp(offsets[i][j], -offset_exponent);
      intervals[i][j] = Scale(intervals[i][j], -offset_exponent);
    }
    offsets[i] = Vector3{offset};
  }
  Geometry geometry{};
  geometry.length_exponent = coordinate_exponent + offset_exponent;
  geometry.b = .25 * (offsets[1] + offsets[2] - offsets[3]);
  geometry.c = .25 * (offsets[2] + offsets[3] - offsets[1]);
  geometry.d = .25 * (offsets[2] - offsets[1] - offsets[3]);
  geometry.bi = Interval{.25} * (intervals[1] + intervals[2] - intervals[3]);
  geometry.ci = Interval{.25} * (intervals[2] + intervals[3] - intervals[1]);
  geometry.di = Interval{.25} * (intervals[2] - intervals[1] - intervals[3]);
  const Vector3 w0 = geometry.b.Cross(geometry.c);
  const Vector3 u = geometry.b.Cross(geometry.d);
  const Vector3 v = geometry.d.Cross(geometry.c);
  std::vector<Vector3> candidates;
  candidates.reserve(10);
  for (double xi : {-1., 1.}) {
    for (double eta : {-1., 1.}) candidates.push_back(w0 + xi * u + eta * v);
  }
  // Projection here is the exact constrained-edge minimizer, not a geometry
  // clamp.
  for (double edge : {-1., 1.}) {
    if (v.Dot(v) > 0) {
      const double eta = std::clamp(-v.Dot(w0 + edge * u) / v.Dot(v), -1., 1.);
      candidates.push_back(w0 + edge * u + eta * v);
    }
    if (u.Dot(u) > 0) {
      const double xi = std::clamp(-u.Dot(w0 + edge * v) / u.Dot(u), -1., 1.);
      candidates.push_back(w0 + xi * u + edge * v);
    }
  }
  const double uu = u.Dot(u);
  const double vv = v.Dot(v);
  const double uv = u.Dot(v);
  const double determinant = uu * vv - uv * uv;
  if (determinant > 0 && std::isfinite(determinant)) {
    Matrix metric{2, 2};
    metric(0,0) = uu; metric(0,1) = uv;
    metric(1,0) = uv; metric(1,1) = vv;
    Matrix rhs{2, 1};
    rhs(0,0) = -u.Dot(w0); rhs(1,0) = -v.Dot(w0);
    const auto factor = dense_blas_internal::DenseFactor::Lu(metric);
    if (factor.HasValue()) {
      const auto stationary = factor.Value().Solve(rhs);
      if (stationary.HasValue()) {
        const double xi = stationary.Value()(0,0);
        const double eta = stationary.Value()(1,0);
        if (std::abs(xi) <= 1 && std::abs(eta) <= 1)
          candidates.push_back(w0 + xi * u + eta * v);
      }
    }
  }
  // The center is also useful for affine or rank-one normal images.
  candidates.push_back(w0);
  for (const auto& candidate : candidates) {
    if (Certify(geometry, candidate, geometry.scaled_jacobian_lower)) {
      return Result<Geometry>::Success(geometry);
    }
  }
  return Result<Geometry>::Failure(
      GeometryFailure(source, location, "uncertain-thermal-shell-geometry",
                      "No strictly positive whole-patch interval bound was "
                      "certified (lower <= 0)."));
}

Result<SurfacePoint> EvaluateSurface(const Geometry& geometry, double xi,
                                     double eta, Interval xi_interval,
                                     Interval eta_interval,
                                     const SourceEntityId& source,
                                     const SourceLocation& location) {
  const Vector3 a1 = geometry.b + eta * geometry.d;
  const Vector3 a2 = geometry.c + xi * geometry.d;
  const auto a1i = geometry.bi + eta_interval * geometry.di;
  const auto a2i = geometry.ci + xi_interval * geometry.di;
  const Vector3 w = a1.Cross(a2);
  const auto wi = Cross(a1i, a2i);
  const double jacobian = w.Norm();
  const Interval determinant_interval = NormSquared(wi);
  const double determinant = w.Dot(w);
  // Retain the original scalar range/scale certificate. Each canonical
  // metric action is consumed immediately by its finite gate, never retained
  // as inverse coefficients or used to compute a physical gradient.
  const auto finite_metric_action = [&](Interval numerator) {
    const Interval action = numerator / determinant_interval;
    return action.IsFinite() &&
           Scale(action, -2 * geometry.length_exponent).IsFinite();
  };
  if (!(determinant_interval.lower > 0) ||
      !finite_metric_action(Dot(a2i, a2i)) ||
      !finite_metric_action(Interval{0} - Dot(a1i, a2i)) ||
      !finite_metric_action(Dot(a1i, a1i)) || !(jacobian > 0) ||
      !std::isfinite(determinant) || determinant <= 0 ||
      !Scale(Dot(a1i, a1i), 2 * geometry.length_exponent).IsFinite() ||
      !Scale(Dot(a2i, a2i), 2 * geometry.length_exponent).IsFinite()) {
    return Result<SurfacePoint>::Failure(GeometryFailure(
        source, location, "uncertain-thermal-shell-metric",
        "Metric inverse enclosure is not finite or excludes no zero divisor."));
  }
  // Factor the physical tangent constraints rather than the rounded Gram
  // matrix, which can lose rank while the certified patch is still regular.
  Matrix constraints{3, 3};
  for (std::size_t j = 0; j < 3; ++j) {
    constraints(0,j) = a1[j];
    constraints(1,j) = a2[j];
    constraints(2,j) = w[j] / jacobian;
  }
  const auto factor = dense_blas_internal::DenseFactor::Lu(constraints);
  if (!factor.HasValue())
    return Result<SurfacePoint>::Failure(GeometryFailure(
        source, location, "uncertain-thermal-shell-metric",
        "Surface gradient enclosure is not finite."));
  const Interval reciprocal_determinant = Interval{1} / determinant_interval;
  SurfacePoint point{};
  point.jacobian = std::ldexp(jacobian, 2 * geometry.length_exponent);
  point.jacobian_interval =
      Scale(Sqrt(determinant_interval), 2 * geometry.length_exponent);
  point.axes[0] = a1 / a1.Norm();
  point.axes[2] = w / jacobian;
  point.axes[1] = point.axes[2].Cross(point.axes[0]);
  constexpr std::array<double, 4> kXi{{-1., 1., 1., -1.}};
  constexpr std::array<double, 4> kEta{{-1., -1., 1., 1.}};
  for (std::size_t i = 0; i < 4; ++i) {
    point.shape[i] = .25 * (1 + kXi[i] * xi) * (1 + kEta[i] * eta);
    point.shape_interval[i] = Interval{.25} *
                              (Interval{1} + Interval{kXi[i]} * xi_interval) *
                              (Interval{1} + Interval{kEta[i]} * eta_interval);
    const double dx = .25 * kXi[i] * (1 + kEta[i] * eta);
    const double dy = .25 * kEta[i] * (1 + kXi[i] * xi);
    const Interval dxi = Interval{.25 * kXi[i]} *
                         (Interval{1} + Interval{kEta[i]} * eta_interval);
    const Interval dyi = Interval{.25 * kEta[i]} *
                         (Interval{1} + Interval{kXi[i]} * xi_interval);
    Matrix rhs{3, 1};
    rhs(0,0) = dx; rhs(1,0) = dy;
    const auto gradient = factor.Value().Solve(rhs);
    if (!gradient.HasValue())
      return Result<SurfacePoint>::Failure(GeometryFailure(
          source, location, "uncertain-thermal-shell-metric",
          "Surface gradient enclosure is not finite."));
    // The real solution obeys a1.g=dx, a2.g=dy, w.g=0. Its exterior-product
    // identity encloses the solution directly. The original outward endpoint
    // graph is retained component by component; no interval factor pivot or
    // interval inverse/basis is introduced and no low-order term is discarded.
    const auto first_numerator = Cross(a2i, wi);
    const auto second_numerator = Cross(wi, a1i);
    std::array<double, 3> components{};
    for (std::size_t j = 0; j < 3; ++j) {
      components[j] = std::ldexp(gradient.Value()(j,0), -geometry.length_exponent);
      point.gradient_interval[i][j] =
          Scale(dxi * (reciprocal_determinant * first_numerator[j]) +
                    dyi * (reciprocal_determinant * second_numerator[j]),
                -geometry.length_exponent);
    }
    point.gradient[i] = Vector3{components};
    if (!point.gradient[i].IsFinite() ||
        !IsFinite(point.gradient_interval[i])) {
      return Result<SurfacePoint>::Failure(
          GeometryFailure(source, location, "uncertain-thermal-shell-metric",
                          "Surface gradient enclosure is not finite."));
    }
  }
  if (!(point.jacobian > 0) || !std::isfinite(point.jacobian) ||
      !point.jacobian_interval.IsFinite() ||
      !(point.jacobian_interval.lower > 0) || !point.axes[0].IsFinite() ||
      !point.axes[1].IsFinite() || !point.axes[2].IsFinite()) {
    return Result<SurfacePoint>::Failure(GeometryFailure(
        source, location, "uncertain-thermal-shell-metric",
        "Surface measure or frame is not finite and positive."));
  }
  return Result<SurfacePoint>::Success(point);
}
}  // namespace thermal_internal

namespace {
using Edge = std::pair<EntityIndex, EntityIndex>;

/// @brief Forms a deterministic undirected source edge key.
Edge EdgeKey(EntityIndex first, EntityIndex second) {
  return std::minmax(first, second);
}

/// @brief Rejects possible open-edge contact only for an actual source
/// candidate.
Status CheckPartialEdge(const ModelDefinition& definition, EntityIndex p,
                        EntityIndex q, EntityIndex v,
                        const Ds4ThermalElementDefinition& owner) {
  using namespace thermal_internal;
  const Vector3 x{definition.nodes[p].coordinates};
  const Vector3 y{definition.nodes[q].coordinates};
  const Vector3 z{definition.nodes[v].coordinates};
  double largest = 0;
  for (std::size_t j = 0; j < 3; ++j) {
    largest =
        std::max({largest, std::abs(x[j]), std::abs(y[j]), std::abs(z[j])});
  }
  int exponent = 0;
  std::frexp(largest, &exponent);
  IntervalVector a;
  IntervalVector b;
  std::size_t axis = 0;
  double best = 0;
  for (std::size_t j = 0; j < 3; ++j) {
    a[j] = Scale(Interval{y[j]}, -exponent) - Scale(Interval{x[j]}, -exponent);
    b[j] = Scale(Interval{z[j]}, -exponent) - Scale(Interval{x[j]}, -exponent);
    const double lower = a[j].ContainsZero() ? 0
                                             : std::min(std::abs(a[j].lower),
                                                        std::abs(a[j].upper));
    if (lower > best) {
      best = lower;
      axis = j;
    }
  }
  if (best > 0) {
    const Interval lambda = b[axis] / a[axis];
    if (lambda.upper < 0 || lambda.lower > 1) return Status::Ok();
    for (std::size_t j = 0; j < 3; ++j) {
      if (!(b[j] - lambda * a[j]).ContainsZero()) return Status::Ok();
    }
  }
  return GeometryFailure(owner.source_id, owner.location,
                         "uncertain-thermal-shell-connection",
                         "Source-sharing coarse/partial edges have unresolved "
                         "or nonconforming open-edge contact.");
}
}  // namespace

Status ValidateThermalShellMesh(const ModelDefinition& definition) {
  using namespace thermal_internal;
  struct Incidence {
    EntityIndex first;
    EntityIndex second;
    std::size_t element;
  };
  std::map<Edge, std::vector<Incidence>> edges;
  std::map<EntityIndex, std::map<EntityIndex, std::vector<EntityIndex>>> links;
  std::set<std::array<EntityIndex, 4>> patches;
  std::map<std::pair<std::string, std::int64_t>, EntityIndex> source_nodes;
  for (std::size_t index = 0; index < definition.thermal_elements.size();
       ++index) {
    const auto& element = definition.thermal_elements[index];
    for (const EntityIndex node : element.node_indices) {
      const auto& source = definition.nodes[node].source_id;
      const auto inserted = source_nodes.emplace(
          std::make_pair(source.instance_name, source.source_label), node);
      if (!inserted.second && inserted.first->second != node) {
        return GeometryFailure(
            element.source_id, element.location,
            "invalid-thermal-shell-topology",
            "One source node identity has multiple indices.");
      }
    }
    auto sorted = element.node_indices;
    std::sort(sorted.begin(), sorted.end());
    if (std::adjacent_find(sorted.begin(), sorted.end()) != sorted.end() ||
        !patches.insert(sorted).second) {
      return GeometryFailure(element.source_id, element.location,
                             "invalid-thermal-shell-topology",
                             "Repeated node or duplicate element patch.");
    }
    std::array<Vector3, 4> coordinates{};
    for (std::size_t i = 0; i < 4; ++i)
      coordinates[i] =
          Vector3{definition.nodes[element.node_indices[i]].coordinates};
    const auto geometry =
        BuildGeometry(coordinates, element.source_id, element.location);
    if (!geometry.HasValue()) return geometry.GetStatus();
    // Domain never publishes geometry that fails at any kernel/recovery point.
    const double gauss = std::sqrt(3. / 5.);
    for (double xi : {-gauss, 0., gauss}) {
      for (double eta : {-gauss, 0., gauss}) {
        const auto point = EvaluateSurface(
            geometry.Value(), xi, eta, Interval{Down(xi), Up(xi)},
            Interval{Down(eta), Up(eta)}, element.source_id, element.location);
        if (!point.HasValue()) return point.GetStatus();
      }
    }
    const double recovery = 1 / std::sqrt(3.);
    for (double xi : {-recovery, recovery}) {
      for (double eta : {-recovery, recovery}) {
        const auto point = EvaluateSurface(
            geometry.Value(), xi, eta, Interval{Down(xi), Up(xi)},
            Interval{Down(eta), Up(eta)}, element.source_id, element.location);
        if (!point.HasValue()) return point.GetStatus();
      }
    }
    for (std::size_t i = 0; i < 4; ++i) {
      const EntityIndex node = element.node_indices[i];
      const EntityIndex next = element.node_indices[(i + 1) % 4];
      const EntityIndex previous = element.node_indices[(i + 3) % 4];
      edges[EdgeKey(node, next)].push_back({node, next, index});
      links[node][previous].push_back(next);
      links[node][next].push_back(previous);
    }
  }
  for (const auto& item : edges) {
    const auto& incidence = item.second;
    if (incidence.size() > 2 ||
        (incidence.size() == 2 && incidence[0].first == incidence[1].first)) {
      const auto& element =
          definition.thermal_elements[incidence.back().element];
      return GeometryFailure(
          element.source_id, element.location, "invalid-thermal-shell-topology",
          "Shared edge must have at most two opposite incidences.");
    }
  }
  for (const auto& vertex : links) {
    const auto& graph = vertex.second;
    std::set<EntityIndex> visited;
    std::vector<EntityIndex> pending{graph.begin()->first};
    std::size_t ends = 0;
    bool invalid = false;
    for (const auto& item : graph) {
      const auto& adjacent = item.second;
      if (adjacent.size() == 1) ++ends;
      if (adjacent.size() > 2 ||
          (adjacent.size() == 2 && adjacent[0] == adjacent[1]))
        invalid = true;
    }
    while (!pending.empty()) {
      const auto node = pending.back();
      pending.pop_back();
      if (!visited.insert(node).second) continue;
      for (const auto next : graph.at(node)) pending.push_back(next);
    }
    if (invalid || visited.size() != graph.size() || (ends != 0 && ends != 2)) {
      const auto& incidence =
          edges.at(EdgeKey(vertex.first, graph.begin()->first))[0];
      const auto& element = definition.thermal_elements[incidence.element];
      return GeometryFailure(element.source_id, element.location,
                             "invalid-thermal-shell-topology",
                             "Vertex link is not one path or cycle.");
    }
  }
  // Enumerate only incident SOURCE edges, never all mesh coordinates vs edges.
  std::map<EntityIndex, std::set<EntityIndex>> neighbors;
  for (const auto& item : edges) {
    neighbors[item.first.first].insert(item.first.second);
    neighbors[item.first.second].insert(item.first.first);
  }
  for (const auto& item : edges) {
    for (const auto endpoint : {item.first.first, item.first.second}) {
      const EntityIndex other =
          endpoint == item.first.first ? item.first.second : item.first.first;
      const auto& owner = definition.thermal_elements[item.second[0].element];
      for (const auto partial : neighbors[endpoint]) {
        if (partial == other) continue;
        const auto& instance =
            definition.nodes[endpoint].source_id.instance_name;
        if (definition.nodes[other].source_id.instance_name != instance ||
            definition.nodes[partial].source_id.instance_name != instance)
          continue;
        const Status status =
            CheckPartialEdge(definition, endpoint, other, partial, owner);
        if (!status.IsOk()) return status;
      }
    }
  }
  return Status::Ok();
}

}  // namespace fesa