/*  Copyright (C) 2017 Eric Wasylishen

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA

    See file, 'COPYING', for details.
*/

#pragma once

#include <initializer_list>
#include <cassert>
#include <cmath>
#include <string>
#include <algorithm>
#include <array>
#include <ostream>
#include <fmt/format.h>
#include "common/mathlib.hh"
#include "common/cmdlib.hh"

template<class T, size_t N>
class qvec
{
protected:
    std::array<T, N> v;

    template<class T2, size_t N2>
    friend class qvec;

public:
    using value_type = T;

    inline qvec() = default;
    
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-value"
#endif
    template<typename... Args,
        typename = std::enable_if_t<sizeof...(Args) && std::is_convertible_v<std::common_type_t<Args...>, T>>>
    constexpr qvec(Args... a) :
        v({})
    {
        constexpr size_t count = sizeof...(Args);

        // special case for single argument
        if constexpr (count == 1) {
            for (auto &e : v)
                ((e = a, true) || ...);
        }
        // multiple arguments; copy up to min(N, `count`),
        // leave `count` -> N as zeroes
        else {
            constexpr size_t copy_size = min(N, count);
            size_t i = 0;
            ((i++ < copy_size ? (v[i - 1] = a, true) : false), ...);
        }
    }
#ifdef __clang__
#pragma clang diagnostic pop
#endif

    // copy from C-style array, exact lengths only
    template<typename T2>
    constexpr qvec(const T2 (&array)[N])
    {
        for (size_t i = 0; i < N; i++)
            v[i] = static_cast<T>(array[i]);
    }

    // copy from std::array, exact lengths only
    template<typename T2>
    constexpr qvec(const std::array<T2, N> &array)
    {
        for (size_t i = 0; i < N; i++)
            v[i] = static_cast<T>(array[i]);
    }

    /**
     * Casting from another vector type of the same length
     */
    template<class T2>
    constexpr qvec(const qvec<T2, N> &other)
    {
        for (size_t i = 0; i < N; i++)
            v[i] = static_cast<T>(other[i]);
    }

    /**
     * Casting from another vector type of the same type but
     * different length
     */
    template<size_t N2>
    constexpr qvec(const qvec<T, N2> &other)
    {
        constexpr size_t minSize = min(N, N2);

        // truncates if `other` is longer than `this`
        for (size_t i = 0; i < minSize; i++)
            v[i] = other[i];

        // zero-fill if `other` is smaller than `this`
        if constexpr (N2 < N) {
            for (size_t i = minSize; i < N; i++)
                v[i] = 0;
        }
    }

    /**
     * Extending a vector
     */
    constexpr qvec(const qvec<T, N - 1> &other, T value)
    {
        std::copy(other.begin(), other.end(), v.begin());
        v[N - 1] = value;
    }

    [[nodiscard]] constexpr size_t size() const { return N; }

    // Sort support
    [[nodiscard]] constexpr bool operator<(const qvec &other) const { return v < other.v; }
    [[nodiscard]] constexpr bool operator<=(const qvec &other) const { return v <= other.v; }
    [[nodiscard]] constexpr bool operator>(const qvec &other) const { return v > other.v; }
    [[nodiscard]] constexpr bool operator>=(const qvec &other) const { return v >= other.v; }
    [[nodiscard]] constexpr bool operator==(const qvec &other) const { return v == other.v; }
    [[nodiscard]] constexpr bool operator!=(const qvec &other) const { return v != other.v; }

    [[nodiscard]] constexpr const T &at(const size_t idx) const
    {
        assert(idx >= 0 && idx < N);
        return v[idx];
    }

    [[nodiscard]] constexpr T &at(const size_t idx)
    {
        assert(idx >= 0 && idx < N);
        return v[idx];
    }

    [[nodiscard]] constexpr const T &operator[](const size_t idx) const { return at(idx); }
    [[nodiscard]] constexpr T &operator[](const size_t idx) { return at(idx); }

    template<typename F>
    [[nodiscard]] inline auto operator+(const qvec<F, N> &other) const
    {
        qvec<decltype(T() + F()), N> v;

        for (size_t i = 0; i < N; i++) {
            v[i] = at(i) + other[i];
        }

        return v;
    }

    template<typename F>
    [[nodiscard]] inline auto operator-(const qvec<F, N> &other) const
    {
        qvec<decltype(T() - F()), N> v;

        for (size_t i = 0; i < N; i++) {
            v[i] = at(i) - other[i];
        }

        return v;
    }

    template<typename S>
    [[nodiscard]] inline auto operator*(const S &scale) const
    {
        qvec<decltype(T() * S()), N> v;

        for (size_t i = 0; i < N; i++) {
            v[i] = at(i) * scale;
        }

        return v;
    }
    
    template<typename F>
    [[nodiscard]] inline auto operator*(const qvec<F, N> &scale) const
    {
        qvec<decltype(T() * F()), N> v;

        for (size_t i = 0; i < N; i++) {
            v[i] = at(i) * scale[i];
        }

        return v;
    }
    
    template<typename S>
    [[nodiscard]] inline auto operator/(const S &scale) const
    {
        qvec<decltype(T() / S()), N> v;

        for (size_t i = 0; i < N; i++) {
            v[i] = at(i) / scale;
        }

        return v;
    }
    
    template<typename F>
    [[nodiscard]] inline auto operator/(const qvec<F, N> &scale) const
    {
        qvec<decltype(T() / F()), N> v;

        for (size_t i = 0; i < N; i++) {
            v[i] = at(i) / scale[i];
        }

        return v;
    }

    [[nodiscard]] inline auto operator-() const
    {
        qvec<decltype(-T()), N> v;

        for (size_t i = 0; i < N; i++) {
            v[i] = -at(i);
        }

        return v;
    }

    template<typename F>
    inline qvec operator+=(const qvec<F, N> &other)
    {
        return *this = *this + other;
    }
    template<typename F>
    inline qvec operator-=(const qvec<F, N> &other)
    {
        return *this = *this - other;
    }
    template<typename S>
    inline qvec operator*=(const S &scale)
    {
        return *this = *this * scale;
    }
    template<typename F>
    inline qvec &operator*=(const qvec<F, N> &other)
    {
        return *this = *this * other;
    }
    template<typename S>
    inline qvec operator/=(const S &scale)
    {
        return *this = *this / scale;
    }
    template<typename F>
    inline qvec operator/=(const qvec<F, N> &other)
    {
        return *this = *this * other;
    }

    [[nodiscard]] constexpr qvec<T, 3> xyz() const
    {
        static_assert(N >= 3);
        return qvec<T, 3>(*this);
    }

    // stream support
    auto stream_data() { return std::tie(v); }

    // iterator support
    constexpr auto begin() { return v.begin(); }
    constexpr auto end() { return v.end(); }
    constexpr auto begin() const { return v.begin(); }
    constexpr auto end() const { return v.end(); }
    constexpr auto cbegin() const { return v.cbegin(); }
    constexpr auto cend() const { return v.cend(); }

    // for Google Test
    friend std::ostream &operator<<(std::ostream &os, const qvec &v) {
        return os << fmt::format("{}", v);
    }
};

// Fmt support
template<class T, size_t N>
struct fmt::formatter<qvec<T, N>> : formatter<T>
{
    template<typename FormatContext>
    auto format(const qvec<T, N> &p, FormatContext &ctx) -> decltype(ctx.out())
    {
        for (size_t i = 0; i < N - 1; i++) {
            formatter<T>::format(p[i], ctx);
            format_to(ctx.out(), " ");
        }

        return formatter<T>::format(p[N - 1], ctx);
    }
};

namespace qv
{
template<class T>
[[nodiscard]] constexpr qvec<T, 3> cross(const qvec<T, 3> &v1, const qvec<T, 3> &v2)
{
    return qvec<T, 3>(v1[1] * v2[2] - v1[2] * v2[1], v1[2] * v2[0] - v1[0] * v2[2], v1[0] * v2[1] - v1[1] * v2[0]);
}

template<typename T, typename U, typename L = std::common_type_t<typename T::value_type, typename U::value_type>>
[[nodiscard]] constexpr L dot(const T &v1, const U &v2)
{
    static_assert(std::size(T()) == std::size(U()), "Can't dot() with two differently-sized vectors");

    L result = 0;
    for (size_t i = 0; i < std::size(v1); i++) {
        result += v1[i] * v2[i];
    }
    return result;
}

template<size_t N, class T>
[[nodiscard]] qvec<T, N> floor(const qvec<T, N> &v1)
{
    qvec<T, N> res;
    for (size_t i = 0; i < N; i++) {
        res[i] = std::floor(v1[i]);
    }
    return res;
}

template<size_t N, class T>
[[nodiscard]] qvec<T, N> pow(const qvec<T, N> &v1, const qvec<T, N> &v2)
{
    qvec<T, N> res;
    for (size_t i = 0; i < N; i++) {
        res[i] = std::pow(v1[i], v2[i]);
    }
    return res;
}

template<size_t N, class T>
[[nodiscard]] constexpr T min(const qvec<T, N> &v)
{
    T res = std::numeric_limits<T>::largest();
    for (auto &c : v) {
        res = ::min(c, res);
    }
    return res;
}

template<size_t N, class T>
[[nodiscard]] constexpr T max(const qvec<T, N> &v)
{
    T res = std::numeric_limits<T>::lowest();
    for (auto &c : v) {
        res = ::max(c, res);
    }
    return res;
}

template<size_t N, class T>
[[nodiscard]] constexpr qvec<T, N> min(const qvec<T, N> &v1, const qvec<T, N> &v2)
{
    qvec<T, N> res;
    for (size_t i = 0; i < N; i++) {
        res[i] = ::min(v1[i], v2[i]);
    }
    return res;
}

template<size_t N, class T>
[[nodiscard]] constexpr qvec<T, N> max(const qvec<T, N> &v1, const qvec<T, N> &v2)
{
    qvec<T, N> res;
    for (size_t i = 0; i < N; i++) {
        res[i] = ::max(v1[i], v2[i]);
    }
    return res;
}

template<size_t N, class T>
[[nodiscard]] constexpr T length2(const qvec<T, N> &v1)
{
    T len2 = 0;
    for (size_t i = 0; i < N; i++) {
        len2 += (v1[i] * v1[i]);
    }
    return len2;
}

template<size_t N, class T>
[[nodiscard]] inline T length(const qvec<T, N> &v1)
{
    return std::sqrt(length2(v1));
}

template<size_t N, class T>
[[nodiscard]] inline qvec<T, N> normalize(const qvec<T, N> &v1, T &len)
{
    return v1 / (len = length(v1));
}

template<size_t N, class T>
[[nodiscard]] inline qvec<T, N> normalize(const qvec<T, N> &v1)
{
    return v1 / length(v1);
}

template<size_t N, class T>
[[nodiscard]] inline T normalizeInPlace(qvec<T, N> &v1)
{
    T len = length(v1);
    v1 /= len;
    return len;
}

template<size_t N, class T>
[[nodiscard]] inline T distance(const qvec<T, N> &v1, const qvec<T, N> &v2)
{
    return length(v2 - v1);
}

template<typename T>
[[nodiscard]] inline std::string to_string(const qvec<T, 3> &v1)
{
    return fmt::format("{}", v1);
}

template<typename T>
[[nodiscard]] inline bool epsilonEqual(const T &v1, const T &v2, T epsilon)
{
    return fabs(v1 - v2) <= epsilon;
}

template<size_t N, class T>
[[nodiscard]] inline bool epsilonEqual(const qvec<T, N> &v1, const qvec<T, N> &v2, T epsilon)
{
    for (size_t i = 0; i < N; i++) {
        if (!epsilonEqual(v1[i], v2[i], epsilon))
            return false;
    }
    return true;
}

template<size_t N, class T>
[[nodiscard]] inline bool epsilonEmpty(const qvec<T, N> &v1, T epsilon)
{
    return epsilonEqual({}, v1, epsilon);
}

template<size_t N, class T>
[[nodiscard]] constexpr bool equalExact(const qvec<T, N> &v1, const qvec<T, N> &v2)
{
    for (size_t i = 0; i < N; i++) {
        if (v1[i] != v2[i])
            return false;
    }
    return true;
}

template<size_t N, class T>
[[nodiscard]] constexpr bool emptyExact(const qvec<T, N> &v1)
{
    return equalExact({}, v1);
}

template<size_t N, class T>
[[nodiscard]] inline size_t indexOfLargestMagnitudeComponent(const qvec<T, N> &v)
{
    size_t largestIndex = 0;
    T largestMag = 0;

    for (size_t i = 0; i < N; ++i) {
        const T currentMag = std::fabs(v[i]);

        if (currentMag > largestMag) {
            largestMag = currentMag;
            largestIndex = i;
        }
    }

    return largestIndex;
}

template<size_t N, typename T>
[[nodiscard]] inline T TriangleArea(const qvec<T, N> &v0, const qvec<T, N> &v1, const qvec<T, N> &v2)
{
    return static_cast<T>(0.5) * qv::length(qv::cross(v2 - v0, v1 - v0));
}

template<typename Iter, typename T = typename std::iterator_traits<Iter>::value_type>
[[nodiscard]] inline T PolyCentroid(Iter begin, Iter end)
{
    using value_type = typename T::value_type;
    size_t num_points = end - begin;

    if (!num_points)
        return qvec<value_type, 3>(std::numeric_limits<value_type>::quiet_NaN());
    else if (num_points == 1)
        return *begin;
    else if (num_points == 2)
        return avg(*begin, *(begin + 1));

    T poly_centroid{};
    value_type poly_area = 0;

    const T &v0 = *begin;
    for (auto it = begin + 2; it != end; ++it) {
        const T &v1 = *(it - 1);
        const T &v2 = *it;

        const value_type triarea = TriangleArea(v0, v1, v2);
        const T tricentroid = avg(v0, v1, v2);

        poly_area += triarea;
        poly_centroid = poly_centroid + (tricentroid * triarea);
    }

    poly_centroid /= poly_area;

    return poly_centroid;
}

template<typename Iter, typename T = typename std::iterator_traits<Iter>::value_type, typename F = typename T::value_type>
[[nodiscard]] inline F PolyArea(Iter begin, Iter end)
{
    Q_assert((end - begin) >= 3);

    float poly_area = 0;

    const T &v0 = *begin;
    for (auto it = begin + 2; it != end; ++it) {
        const T &v1 = *(it - 1);
        const T &v2 = *it;

        poly_area += TriangleArea(v0, v1, v2);
    }

    return poly_area;
}

template<typename T>
[[nodiscard]] inline qvec<T, 3> Barycentric_FromPoint(const qvec<T, 3> &p, const qvec<T, 3> &t0, const qvec<T, 3> &t1, const qvec<T, 3> &t2)
{
    const auto v0 = t1 - t0;
    const auto v1 = t2 - t0;
    const auto v2 = p - t0;
    T d00 = qv::dot(v0, v0);
    T d01 = qv::dot(v0, v1);
    T d11 = qv::dot(v1, v1);
    T d20 = qv::dot(v2, v0);
    T d21 = qv::dot(v2, v1);
    T invDenom = (d00 * d11 - d01 * d01);
    invDenom = 1.0 / invDenom;

    qvec<T, 3> res;
    res[1] = (d11 * d20 - d01 * d21) * invDenom;
    res[2] = (d00 * d21 - d01 * d20) * invDenom;
    res[0] = 1.0 - res[1] - res[2];
    return res;
}

// from global illumination total compendium p. 12
template<typename T>
[[nodiscard]] inline qvec<T, 3> Barycentric_Random(T r1, T r2)
{
    qvec<T, 3> res;
    res[0] = 1.0 - sqrt(r1);
    res[1] = r2 * sqrt(r1);
    res[2] = 1.0 - res[0] - res[1];
    return res;
}

/// Evaluates the given barycentric coord for the given triangle
template<typename T>
[[nodiscard]] constexpr qvec<T, 3> Barycentric_ToPoint(const qvec<T, 3> &bary, const qvec<T, 3> &t0, const qvec<T, 3> &t1, const qvec<T, 3> &t2)
{
    return (t0 * bary[0]) + (t1 * bary[1]) + (t2 * bary[2]);
}

// Snap vector to nearest axial component
template<typename T>
[[nodiscard]] inline qvec<T, 3> Snap(qvec<T, 3> normal, const T &epsilon = NORMAL_EPSILON)
{
    for (auto &v : normal) {
        if (fabs(v - 1) < epsilon) {
            normal = {};
            v = 1;
            break;
        }
        if (fabs(v - -1) < epsilon) {
            normal = {};
            v = -1;
            break;
        }
    }
    
    return normal;
}
}; // namespace qv

using qvec2f = qvec<float, 2>;
using qvec3f = qvec<float, 3>;
using qvec4f = qvec<float, 4>;

using qvec2d = qvec<double, 2>;
using qvec3d = qvec<double, 3>;
using qvec4d = qvec<double, 4>;

using qvec2i = qvec<int32_t, 2>;
using qvec3i = qvec<int32_t, 3>;

using qvec3s = qvec<int16_t, 3>;
using qvec3b = qvec<uint8_t, 3>;

using qvec4b = qvec<uint8_t, 4>;

template<class T>
class qplane3
{
public:
    qvec<T, 3> normal;
    T dist;

    constexpr qplane3() = default;
    constexpr qplane3(const qvec<T, 3> &normal, const T &dist) : normal(normal), dist(dist) { }

    // convert from plane of a different type
    template<typename T2>
    constexpr qplane3(const qplane3<T2> &plane) : qplane3(plane.normal, static_cast<T2>(plane.dist)) { }

private:
    auto as_tuple() const { return std::tie(normal, dist); }

public:
    // Sort support
    [[nodiscard]] constexpr bool operator<(const qplane3 &other) const { return as_tuple() < other.as_tuple(); }
    [[nodiscard]] constexpr bool operator<=(const qplane3 &other) const { return as_tuple() <= other.as_tuple(); }
    [[nodiscard]] constexpr bool operator>(const qplane3 &other) const { return as_tuple() > other.as_tuple(); }
    [[nodiscard]] constexpr bool operator>=(const qplane3 &other) const { return as_tuple() >= other.as_tuple(); }
    [[nodiscard]] constexpr bool operator==(const qplane3 &other) const { return as_tuple() == other.as_tuple(); }
    [[nodiscard]] constexpr bool operator!=(const qplane3 &other) const { return as_tuple() != other.as_tuple(); }

    [[nodiscard]] inline T distAbove(const qvec<T, 3> &pt) const { return qv::dot(pt, normal) - dist; }

    [[nodiscard]] constexpr const qvec<T, 4> vec4() const
    {
        return qvec<T, 4>(normal[0], normal[1], normal[2], dist);
    }

    [[nodiscard]] constexpr qplane3 operator-() const { return { -normal, -dist }; }

    // generic case
    template<typename F>
    inline F distance_to(const qvec<F, 3> &point) const
    {
        return qv::dot(point, normal) - dist;
    }
};

using qplane3f = qplane3<float>;
using qplane3d = qplane3<double>;

namespace qv
{
template<typename T>
[[nodiscard]] bool epsilonEqual(const qplane3<T> &p1, const qplane3<T> &p2, T normalEpsilon = NORMAL_EPSILON, T distEpsilon = DIST_EPSILON)
{
    return epsilonEqual(p1.normal, p2.normal, normalEpsilon) &&
           epsilonEqual(p1.dist, p2.dist, distEpsilon);
}
}

/**
 * Row x Col matrix of T.
 */
template<class T, size_t NRow, size_t NCol>
class qmat
{
public:
    /**
     * Column-major order. [ (row0,col0), (row1,col0), .. ]
     */
    std::array<T, NRow * NCol> m_values;

public:
    /**
     * Identity matrix if square, otherwise fill with 0
     */
    constexpr qmat() : m_values({})
    {
        if constexpr (NRow == NCol) {
            // identity matrix
            for (size_t i = 0; i < NCol; i++) {
                this->at(i, i) = 1;
            }
        }
    }

    /**
     * Fill with a value
     */
    inline qmat(const T &val) { m_values.fill(val); }

    // copy constructor
    constexpr qmat(const qmat &other) : m_values(other.m_values) { }

    /**
     * Casting from another matrix type of the same size
     */
    template<class T2>
    constexpr qmat(const qmat<T2, NRow, NCol> &other)
    {
        for (size_t i = 0; i < NRow * NCol; i++)
            this->m_values[i] = static_cast<T>(other.m_values[i]);
    }

    // initializer list, column-major order
    constexpr qmat(std::initializer_list<T> list)
    {
        assert(list.size() == NRow * NCol);
        std::copy(list.begin(), list.end(), m_values.begin());
    }
    
    // Sort support
    [[nodiscard]] constexpr bool operator<(const qmat &other) const { return m_values < other.m_values; }
    [[nodiscard]] constexpr bool operator<=(const qmat &other) const { return m_values <= other.m_values; }
    [[nodiscard]] constexpr bool operator>(const qmat &other) const { return m_values > other.m_values; }
    [[nodiscard]] constexpr bool operator>=(const qmat &other) const { return m_values >= other.m_values; }
    [[nodiscard]] constexpr bool operator==(const qmat &other) const { return m_values == other.m_values; }
    [[nodiscard]] constexpr bool operator!=(const qmat &other) const { return m_values != other.m_values; }

    // access to elements

    [[nodiscard]] constexpr T &at(size_t row, size_t col)
    {
        assert(row >= 0 && row < NRow);
        assert(col >= 0 && col < NCol);
        return m_values[col * NRow + row];
    }

    [[nodiscard]] constexpr T at(size_t row, size_t col) const
    {
        assert(row >= 0 && row < NRow);
        assert(col >= 0 && col < NCol);
        return m_values[col * NRow + row];
    }

    // access row
    [[nodiscard]] inline qvec<T, NCol> row(size_t row) const
    {
        assert(row >= 0 && row < NRow);
        qvec<T, NCol> v;
        for (size_t i = 0; i < NCol; i++) {
            v[i] = at(row, i);
        }
        return v;
    }

    inline void set_row(size_t row, const qvec<T, NCol> &values)
    {
        for (size_t i = 0; i < NCol; i++) {
            at(row, i) = values[i];
        }
    }

    [[nodiscard]] constexpr const qvec<T, NRow> &col(size_t col) const
    {
        assert(col >= 0 && col < NCol);
        return reinterpret_cast<const qvec<T, NRow> &>(m_values[col * NRow]);
    }

    inline void set_col(size_t col, const qvec<T, NRow> &values) const
    {
        reinterpret_cast<qvec<T, NRow> &>(m_values[col * NRow]) = values;
    }

    // multiplication by a vector

    [[nodiscard]] constexpr qvec<T, NRow> operator*(const qvec<T, NCol> &vec) const
    {
        qvec<T, NRow> res{};
        for (size_t i = 0; i < NRow; i++) { // for each row
            for (size_t j = 0; j < NCol; j++) { // for each col
                res[i] += this->at(i, j) * vec[j];
            }
        }
        return res;
    }

    // multiplication by a matrix

    template<size_t PCol>
    [[nodiscard]] constexpr qmat<T, NRow, PCol> operator*(const qmat<T, NCol, PCol> &other) const
    {
        qmat<T, NRow, PCol> res;
        for (size_t i = 0; i < NRow; i++) {
            for (size_t j = 0; j < PCol; j++) {
                T val = 0;
                for (size_t k = 0; k < NCol; k++) {
                    val += this->at(i, k) * other.at(k, j);
                }
                res.at(i, j) = val;
            }
        }
        return res;
    }

    // multiplication by a scalar

    [[nodiscard]] constexpr qmat operator*(const T &scalar) const
    {
        qmat res(*this);
        for (size_t i = 0; i < NRow * NCol; i++) {
            res.m_values[i] *= scalar;
        }
        return res;
    }
};

// Fmt support
template<class T, size_t NRow, size_t NCol>
struct fmt::formatter<qmat<T, NRow, NCol>> : formatter<qvec<T, NCol>>
{
    template<typename FormatContext>
    auto format(const qmat<T, NRow, NCol> &p, FormatContext &ctx) -> decltype(ctx.out())
    {
        for (size_t i = 0; i < NRow; i++) {
            format_to(ctx.out(), "[ ");
            formatter<qvec<T, NCol>>::format(p.row(i), ctx);
            format_to(ctx.out(), " ]");

            if (i != NRow - 1) {
                format_to(ctx.out(), "\n");
            }
        }

        return ctx.out();
    }
};

using qmat2x2f = qmat<float, 2, 2>;
using qmat3x3f = qmat<float, 3, 3>;
using qmat4x4f = qmat<float, 4, 4>;

using qmat2x2d = qmat<double, 2, 2>;
using qmat3x3d = qmat<double, 3, 3>;
using qmat4x4d = qmat<double, 4, 4>;

namespace qv
{
/**
 * These return a matrix filled with NaN if there is no inverse.
 */
[[nodiscard]] qmat4x4f inverse(const qmat4x4f &input);
[[nodiscard]] qmat4x4d inverse(const qmat4x4d &input);

[[nodiscard]] qmat2x2f inverse(const qmat2x2f &input);
}; // namespace qv

// "vec3" type. legacy; eventually will be replaced entirely
#define DEPRECATE_SNIFF [[deprecated]]

template<typename Ta, typename Tb, typename Tc>
DEPRECATE_SNIFF constexpr void VectorMA(const Ta &va, vec_t scale, const Tb &vb, Tc &vc)
{
    vc[0] = va[0] + scale * vb[0];
    vc[1] = va[1] + scale * vb[1];
    vc[2] = va[2] + scale * vb[2];
}

#undef DEPRECATE_SNIFF
#define DEPRECATE_SNIFF

template<typename Tx, typename Ty, typename Tout>
DEPRECATE_SNIFF constexpr void VectorSubtract(const Tx &x, const Ty &y, Tout &out)
{
    out[0] = x[0] - y[0];
    out[1] = x[1] - y[1];
    out[2] = x[2] - y[2];
}

template<typename Tx, typename Ty, typename Tout>
DEPRECATE_SNIFF constexpr void VectorAdd(const Tx &x, const Ty &y, Tout &out)
{
    out[0] = x[0] + y[0];
    out[1] = x[1] + y[1];
    out[2] = x[2] + y[2];
}

template<typename TFrom, typename TTo>
DEPRECATE_SNIFF constexpr void VectorCopy(const TFrom &in, TTo &out)
{
    out[0] = in[0];
    out[1] = in[1];
    out[2] = in[2];
}

template<typename TFrom, typename TScale, typename TTo>
DEPRECATE_SNIFF constexpr void VectorScale(const TFrom &v, TScale scale, TTo &out)
{
    out[0] = v[0] * scale;
    out[1] = v[1] * scale;
    out[2] = v[2] * scale;
}

template<typename T>
DEPRECATE_SNIFF constexpr vec_t VectorLengthSq(const T &v)
{
    vec_t length = 0;
    for (int i = 0; i < 3; i++)
        length += v[i] * v[i];
    return length;
}

template<typename T>
DEPRECATE_SNIFF inline vec_t VectorLength(const T &v)
{
    vec_t length = VectorLengthSq(v);
    length = sqrt(length);
    return length;
}

template<typename T>
DEPRECATE_SNIFF inline vec_t VectorNormalize(T &v)
{
    vec_t length = 0;
    for (size_t i = 0; i < 3; i++)
        length += v[i] * v[i];
    length = sqrt(length);
    if (length == 0)
        return 0;

    for (size_t i = 0; i < 3; i++)
        v[i] /= (vec_t)length;

    return (vec_t)length;
}

// returns the normalized direction from `start` to `stop` in the `dir` param
// returns the distance from `start` to `stop`
template<typename Tstart, typename Tstop, typename Tdir>
inline vec_t GetDir(const Tstart &start, const Tstop &stop, Tdir &dir)
{
    VectorSubtract(stop, start, dir);
    return VectorNormalize(dir);
}

// Stores a normal, tangent and bitangent
struct face_normal_t
{
    qvec3f normal, tangent, bitangent;
};

qmat3x3d RotateAboutX(double radians);
qmat3x3d RotateAboutY(double radians);
qmat3x3d RotateAboutZ(double radians);
qmat3x3f RotateFromUpToSurfaceNormal(const qvec3f &surfaceNormal);

// Returns (0 0 0) if we couldn't determine the normal
qvec3f GLM_FaceNormal(std::vector<qvec3f> points);
std::pair<bool, qvec4f> GLM_MakeInwardFacingEdgePlane(const qvec3f &v0, const qvec3f &v1, const qvec3f &faceNormal);
std::vector<qvec4f> GLM_MakeInwardFacingEdgePlanes(const std::vector<qvec3f> &points);
bool GLM_EdgePlanes_PointInside(const std::vector<qvec4f> &edgeplanes, const qvec3f &point);
float GLM_EdgePlanes_PointInsideDist(const std::vector<qvec4f> &edgeplanes, const qvec3f &point);
qvec4f GLM_MakePlane(const qvec3f &normal, const qvec3f &point);
float GLM_DistAbovePlane(const qvec4f &plane, const qvec3f &point);
qvec3f GLM_ProjectPointOntoPlane(const qvec4f &plane, const qvec3f &point);
qvec4f GLM_PolyPlane(const std::vector<qvec3f> &points);
/// Returns the index of the polygon edge, and the closest point on that edge, to the given point
std::pair<int, qvec3f> GLM_ClosestPointOnPolyBoundary(const std::vector<qvec3f> &poly, const qvec3f &point);
/// Returns `true` and the interpolated normal if `point` is in the polygon, otherwise returns false.
std::pair<bool, qvec3f> GLM_InterpolateNormal(
    const std::vector<qvec3f> &points, const std::vector<face_normal_t> &normals, const qvec3f &point);
std::pair<bool, qvec3f> GLM_InterpolateNormal(
    const std::vector<qvec3f> &points, const std::vector<qvec3f> &normals, const qvec3f &point);
std::vector<qvec3f> GLM_ShrinkPoly(const std::vector<qvec3f> &poly, const float amount);
/// Returns (front part, back part)
std::pair<std::vector<qvec3f>, std::vector<qvec3f>> GLM_ClipPoly(const std::vector<qvec3f> &poly, const qvec4f &plane);

class poly_random_point_state_t
{
public:
    std::vector<qvec3f> points;
    std::vector<float> triareas;
    std::vector<float> triareas_cdf;
};

poly_random_point_state_t GLM_PolyRandomPoint_Setup(const std::vector<qvec3f> &points);
qvec3f GLM_PolyRandomPoint(const poly_random_point_state_t &state, float r1, float r2, float r3);

/// projects p onto the vw line.
/// returns 0 for p==v, 1 for p==w
float FractionOfLine(const qvec3f &v, const qvec3f &w, const qvec3f &p);

/**
 * Distance from `p` to the line v<->w (extending infinitely in either direction)
 */
float DistToLine(const qvec3f &v, const qvec3f &w, const qvec3f &p);

qvec3f ClosestPointOnLine(const qvec3f &v, const qvec3f &w, const qvec3f &p);

/**
 * Distance from `p` to the line segment v<->w.
 * i.e., 0 if `p` is between v and w.
 */
float DistToLineSegment(const qvec3f &v, const qvec3f &w, const qvec3f &p);

qvec3f ClosestPointOnLineSegment(const qvec3f &v, const qvec3f &w, const qvec3f &p);

float SignedDegreesBetweenUnitVectors(const qvec3f &start, const qvec3f &end, const qvec3f &normal);

enum class concavity_t
{
    Coplanar,
    Concave,
    Convex
};

concavity_t FacePairConcavity(
    const qvec3f &face1Center, const qvec3f &face1Normal, const qvec3f &face2Center, const qvec3f &face2Normal);

// Returns weights for f(0,0), f(1,0), f(0,1), f(1,1)
// from: https://en.wikipedia.org/wiki/Bilinear_interpolation#Unit_Square
inline qvec4f bilinearWeights(const float x, const float y)
{
    Q_assert(x >= 0.0f);
    Q_assert(x <= 1.0f);

    Q_assert(y >= 0.0f);
    Q_assert(y <= 1.0f);

    return qvec4f((1.0f - x) * (1.0f - y), x * (1.0f - y), (1.0f - x) * y, x * y);
}

// This uses a coordinate system where the pixel centers are on integer coords.
// e.g. the corners of a 3x3 pixel bitmap are at (-0.5, -0.5) and (2.5, 2.5).
inline std::array<std::pair<qvec2i, float>, 4> bilinearWeightsAndCoords(qvec2f pos, const qvec2i &size)
{
    Q_assert(pos[0] >= -0.5f && pos[0] <= (size[0] - 0.5f));
    Q_assert(pos[1] >= -0.5f && pos[1] <= (size[1] - 0.5f));

    // Handle extrapolation.
    for (int i = 0; i < 2; i++) {
        if (pos[i] < 0)
            pos[i] = 0;

        if (pos[i] > (size[i] - 1))
            pos[i] = (size[i] - 1);
    }

    Q_assert(pos[0] >= 0.f && pos[0] <= (size[0] - 1));
    Q_assert(pos[1] >= 0.f && pos[1] <= (size[1] - 1));

    qvec2i integerPart{static_cast<int>(qv::floor(pos)[0]), static_cast<int>(qv::floor(pos)[1])};
    qvec2f fractionalPart(pos - qv::floor(pos));

    // ensure integerPart + (1, 1) is still in bounds
    for (int i = 0; i < 2; i++) {
        if (fractionalPart[i] == 0.0f && integerPart[i] > 0) {
            integerPart[i] -= 1;
            fractionalPart[i] = 1.0f;
        }
    }
    Q_assert(integerPart[0] + 1 < size[0]);
    Q_assert(integerPart[1] + 1 < size[1]);

    Q_assert(qvec2f(integerPart) + fractionalPart == pos);

    // f(0,0), f(1,0), f(0,1), f(1,1)
    const qvec4f weights = bilinearWeights(fractionalPart[0], fractionalPart[1]);

    std::array<std::pair<qvec2i, float>, 4> result;
    for (int i = 0; i < 4; i++) {
        const float weight = weights[i];
        qvec2i pos(integerPart);

        if ((i % 2) == 1)
            pos[0] += 1;
        if (i >= 2)
            pos[1] += 1;

        Q_assert(pos[0] >= 0);
        Q_assert(pos[0] < size[0]);

        Q_assert(pos[1] >= 0);
        Q_assert(pos[1] < size[1]);

        result[i] = std::make_pair(pos, weight);
    }
    return result;
}

template<typename V>
V bilinearInterpolate(const V &f00, const V &f10, const V &f01, const V &f11, const float x, const float y)
{
    qvec4f weights = bilinearWeights(x, y);

    const V fxy = f00 * weights[0] + f10 * weights[1] + f01 * weights[2] + f11 * weights[3];

    return fxy;
}

template<typename V>
std::vector<V> PointsAlongLine(const V &start, const V &end, const float step)
{
    const V linesegment = end - start;
    const float len = qv::length(linesegment);
    if (len == 0)
        return {};

    std::vector<V> result;
    const int stepCount = static_cast<int>(len / step);
    result.reserve(stepCount + 1);
    const V dir = linesegment / len;
    for (int i = 0; i <= stepCount; i++) {
        result.push_back(start + (dir * (step * i)));
    }
    return result;
}

bool LinesOverlap(const qvec3f &p0, const qvec3f &p1, const qvec3f &q0, const qvec3f &q1,
    const vec_t &on_epsilon = DEFAULT_ON_EPSILON);