#pragma once

#include "fesa/Core/Types.hpp"

#include <cmath>
#include <limits>
#include <optional>
#include <stdexcept>

namespace fesa {

struct Vec3 {
  Real x = 0.0;
  Real y = 0.0;
  Real z = 0.0;
};

inline Vec3 operator+(const Vec3& a, const Vec3& b) {
  return {a.x + b.x, a.y + b.y, a.z + b.z};
}

inline Vec3 operator-(const Vec3& a, const Vec3& b) {
  return {a.x - b.x, a.y - b.y, a.z - b.z};
}

inline Vec3 operator*(Real scalar, const Vec3& value) {
  return {scalar * value.x, scalar * value.y, scalar * value.z};
}

inline Real dot(const Vec3& a, const Vec3& b) {
  return a.x * b.x + a.y * b.y + a.z * b.z;
}

inline Vec3 cross(const Vec3& a, const Vec3& b) {
  return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x};
}

inline Real norm(const Vec3& value) {
  return std::sqrt(dot(value, value));
}

inline bool isFinite(Real value) {
  return std::isfinite(value);
}

inline bool isFinite(const Vec3& value) {
  return isFinite(value.x) && isFinite(value.y) && isFinite(value.z);
}

inline std::optional<Vec3> normalizedIfValid(const Vec3& value, Real tolerance = 1.0e-12) {
  const Real length = norm(value);
  if (!isFinite(length) || length <= tolerance) {
    return std::nullopt;
  }
  return (1.0 / length) * value;
}

inline Vec3 normalized(const Vec3& value) {
  const Real length = norm(value);
  if (length <= std::numeric_limits<Real>::epsilon()) {
    throw std::runtime_error("zero-length vector");
  }
  return (1.0 / length) * value;
}

}  // namespace fesa