#pragma once

#include "./math.hpp"

namespace geometry {
    template <typename U, typename P>
    struct OBB;

    /// Abstract Axis Aligned Box
    template<typename U>
    struct AABB {
        using vec = glm::vec<3, U>;

        AABB(const vec& min, const vec& max): min(min), max(max) {
            assert(glm::all(glm::greaterThanEqual(max, min)) && "min > min");
        }
        inline static AABB<U> FromCenter(const vec& center, const vec& extent) { return AABB<U>(center - extent, center + extent); }
        inline static AABB<U> FromMin(const vec& min, const vec& size) { return AABB<U>(min, min + size); }
        template <typename V>
        inline static AABB<U> From(const AABB<V> &v) { return AABB<U>(v.min, v.max); }

        vec min;
        vec max;

        inline AABB<U> operator+(const vec& v) const { return AABB<U>(min + v, max + v); }

        constexpr vec getSize() const { return max - min; }
        constexpr vec getCenter() const { return (min + max) / (U)2; }
        constexpr vec getExtent() const { return getSize() / (U)2; }

        /// Biggest box after pivot around zero
        AABB<U> rotate(const glm::quat& pivot) const {
            const auto mat = glm::toMat4(pivot);
            return FromCenter(mat * glm::vec<4, U>(getCenter(), 1), glm::abs(mat) * glm::vec<4, U>(getExtent(), 0));
        }
        /// Pivot around center not zero
        /// Take glm::abs(rot)
        inline static AABB<U> FromCenterAbs(const vec& center, const vec& extent, const glm::mat4& rot_abs) {
            const vec new_extent = rot_abs * glm::vec<4, U>(extent, 0);
            return AABB<U>(center - new_extent, center + new_extent);
        }
        /// Pivot around center not zero
        inline static AABB<U> FromCenter(const vec& center, const vec& extent, const glm::mat4& rot) {
            return FromCenterAbs(center, extent, glm::abs(rot));
        }
        /// Pivot around center not zero
        inline static AABB<U> FromCenter(const vec& center, const vec& extent, const glm::quat& pivot) {
            return FromCenter(center, extent, glm::toMat4(pivot));
        }
        /// Pivot around center not zero
        /// Take glm::abs(rot)
        inline static AABB<U> FromMinAbs(const vec& min, const vec& size, const glm::mat4& rot_abs) {
            const auto extent = size / (U)2;
            return FromCenterAbs(min + extent, extent, rot_abs);
        }
        /// Pivot around center not zero
        inline static AABB<U> FromMin(const vec& min, const vec& size, const glm::mat4& rot) {
            return FromMinAbs(min, size, glm::abs(rot));
        }
        /// Pivot around center not zero
        inline static AABB<U> FromMin(const vec& min, const vec& size, const glm::quat& pivot) {
            return FromMin(min, size, glm::toMat4(pivot));
        }
        /// Pivot around center not zero
        template <typename V>
        inline static AABB<U> From(const AABB<V> &v, const glm::quat& pivot) {
            return FromMin(v.min, v.getSize(), pivot);
        }

        constexpr bool contains(const vec& pos) const {
            return pos.x >= min.x && pos.x <= max.x &&
                   pos.y >= min.y && pos.y <= max.y &&
                   pos.z >= min.z && pos.z <= max.z;
        }
        constexpr bool contains(const AABB<U>& box) const {
            return box.min.x >= min.x && box.max.x <= max.x &&
                   box.min.y >= min.y && box.max.y <= max.y &&
                   box.min.z >= min.z && box.max.z <= max.z;
        }

        /// Intersects or contains box
        constexpr bool intersects(const AABB<U>& box) const {
            return box.max.x >= min.x && box.min.x <= max.x &&
                   box.max.y >= min.y && box.min.y <= max.y &&
                   box.max.z >= min.z && box.min.z <= max.z;
        }
        /// Intersects or contained by sphere
        constexpr bool intersected(const vec& center, U radius) const {
            return distance2(center) <= glm::pow2(radius);
        }
        /// Intersects or contains sphere
        constexpr bool intersects(const vec& center, U radius) const {
            return glm::length2(center - closest(center)) <= glm::pow2(radius);
        }
        /// Intersects or contains box
        template<typename P>
        constexpr bool intersects(const OBB<U, P>& box) const {
            return box.intersected(*this);
        }

        enum class ContainmentType {
            Disjoint = false, Intersects, Contains
        };
        ContainmentType containment(const AABB<U>& box) const {
            if (!intersects(box))
                return ContainmentType::Disjoint;

            if (contains(box))
                return ContainmentType::Contains;

            return ContainmentType::Intersects;
        }

        inline U distance2(const vec& pos) const {
            U dmin = 0;
            for (size_t i = 0; i < vec::length(); i++) {
                if (pos[i] < min[i])
                    dmin += glm::pow2(pos[i] - min[i]);
                else if(pos[i] > max[i])
                    dmin += glm::pow2(pos[i] - max[i]);
            }
            return dmin;
        }
        inline vec closest(const vec& pos) const {
            auto p = vec(0);
            for (size_t i = 0; i < vec::length(); i++) {
                if (pos[i] > max[i])
                    p[i] = max[i];
                else if (pos[i] < min[i])
                    p[i] = min[i];
                else
                    p[i] = pos[i];
            }
            return p;
        }



    };

    using faabb = AABB<float>;
    using lfaabb = AABB<long double>;
    using llaabb = AABB<long long>;

    /// Abstract Oriented Box
    template<typename U, typename P>
    struct OBB {
        using vec = glm::vec<3, U>;

        vec center;
        /// Half size (always positive)
        vec extent;
        /// Extent rotation around center
        glm::quat pivot;

        OBB(const vec& center, const vec& extent, const glm::quat& pivot): center(center), extent(extent), pivot(pivot) {
            assert(glm::all(glm::greaterThanEqual(extent, vec(0))) && "positive extent");
            assert(pivot != glm::quat() && "uninitialized pivot");
        }
        OBB(const AABB<U>& box, const glm::quat& pivot): OBB(box.getCenter(), box.getExtent(), pivot) { }

        inline OBB<U, P> operator+(const vec& v) const { return OBB<U, P>(center + v, extent, pivot); }

        constexpr vec getSize() const { return extent * (U)2; }
        constexpr inline glm::qua<P> getPivot() const { return (glm::qua<P>)pivot; }

        /// Intersects or contains sphere
        constexpr bool intersects(const vec& sp_c, U radius) const {
            const vec local_center = center + (glm::conjugate(getPivot()) * (sp_c-center));
            return AABB<U>::FromCenter(vec(0), extent).intersects(local_center, radius);
        }
        /// Intersects or contained by sphere
        constexpr bool intersected(const vec& sp_c, U radius) const {
            const vec local_center = center + (glm::conjugate(getPivot()) * (sp_c-center));
            return AABB<U>::FromCenter(vec(0), extent).intersected(local_center, radius);
        }
        /// Intersects or contained box
        constexpr bool intersected(const AABB<U>& box_aa) const {
            const auto this_sat = getBounding();
            const auto this_aa = AABB<U>::FromCenter(vec(0), extent);
            const auto box_sat = AABB<U>::FromCenter(box_aa.getCenter() - center, box_aa.getExtent(), glm::conjugate(pivot));
            return this_sat.intersects(box_aa) && this_aa.intersects(box_sat);
        }
        /// Intersects or contains box
        constexpr bool intersects(const AABB<U>& box_aa) const {
            const auto this_sat = getBounding();
            const auto this_aa = AABB<U>::FromCenter(vec(0), extent);
            const auto box_sat = AABB<U>::FromCenter(box_aa.getCenter() - center, box_aa.getExtent(), glm::conjugate(pivot));
            return box_aa.intersects(this_sat) && box_sat.intersects(this_aa);
        }
        /*
        /// Intersects or contains box
        constexpr bool intersects(const OBB<U>& box) const {
            return false; //FIXME: full sat
        }*/

        constexpr inline std::array<vec, 8> getPoints() const {
            std::array<vec, 8> ps;
            ps[0] = getPivot() * (center - extent);
            ps[1] = getPivot() * (center + extent * vec(-1, -1, +1));
            ps[2] = getPivot() * (center + extent * vec(-1, +1, -1));
            ps[3] = getPivot() * (center + extent * vec(-1, +1, +1));
            ps[4] = getPivot() * (center + extent * vec(+1, -1, -1));
            ps[5] = getPivot() * (center + extent * vec(+1, -1, +1));
            ps[6] = getPivot() * (center + extent * vec(+1, +1, -1));
            ps[7] = getPivot() * (center + extent);
            return ps;
        }
        AABB<U> getBounding() const {
            return AABB<U>::FromCenter(center, extent, pivot);
        }
    };

    using fobb = OBB<float, float>;
    using lfobb = OBB<long double, long double>;
    using llobb = OBB<long long, long double>;

}