#ifndef FESA_MATH_VECTOR3_OPERATIONS_H_
#define FESA_MATH_VECTOR3_OPERATIONS_H_

#include <array>
#include <cstddef>

#include "fesa/math/matrix.h"
#include "fesa/math/vector3.h"

namespace fesa::math_internal {

/// @brief Reduces four source-ordered geometric vectors with scalar-left products.
inline Vector3 WeightedSum(const std::array<double, 4>& weights,
                           const std::array<Vector3, 4>& values) {
  Vector3 result{};
  for (std::size_t node = 0U; node < 4U; ++node) {
    result = result + weights[node] * values[node];
  }
  return result;
}

/// @brief Cancels the first-node anchor before the same ordered reduction.
inline Vector3 DerivativeSum(const std::array<double, 4>& derivatives,
                             const std::array<Vector3, 4>& values) {
  std::array<Vector3, 4> relative{};
  for (std::size_t node = 0U; node < 4U; ++node) {
    relative[node] = values[node] - values[0U];
  }
  return WeightedSum(derivatives, relative);
}

/// @brief Forms the positive cross-product matrix in fixed Cartesian coordinates.
inline Matrix Skew(const Vector3& vector) {
  Matrix result{3U, 3U};
  result(0U, 1U) = -vector.Z();
  result(0U, 2U) = vector.Y();
  result(1U, 0U) = vector.Z();
  result(1U, 2U) = -vector.X();
  result(2U, 0U) = -vector.Y();
  result(2U, 1U) = vector.X();
  return result;
}

/// @brief Applies a fixed 3-by-3 geometric frame with the original component graph.
inline Vector3 Multiply(const Matrix& matrix, const Vector3& vector) {
  return Vector3{
      matrix(0U, 0U) * vector.X() + matrix(0U, 1U) * vector.Y() +
          matrix(0U, 2U) * vector.Z(),
      matrix(1U, 0U) * vector.X() + matrix(1U, 1U) * vector.Y() +
          matrix(1U, 2U) * vector.Z(),
      matrix(2U, 0U) * vector.X() + matrix(2U, 1U) * vector.Y() +
          matrix(2U, 2U) * vector.Z()};
}

}  // namespace fesa::math_internal

#endif  // FESA_MATH_VECTOR3_OPERATIONS_H_