#pragma once

#include "Box.hpp"
#include <algorithm>

namespace geometry {
    /// Bounding frustum
    struct Frustum {
        glm::vec4 planes[6];

        enum FrustumSide
        {
            RIGHT = 0,  // The RIGHT side of the frustum
            LEFT = 1,   // The LEFT	 side of the frustum
            BOTTOM = 2, // The BOTTOM side of the frustum
            TOP = 3,    // The TOP side of the frustum
            BACK = 4,   // The BACK	side of the frustum
            FRONT = 5   // The FRONT side of the frustum
        };

        void normalize(glm::vec4 &plane) {
            plane /= (float)sqrt(plane.x * plane.x + plane.y * plane.y + plane.z * plane.z);
        }

        Frustum(const glm::mat4& view_matrix, const glm::mat4& proj_matrix) {
            const float *proj = &proj_matrix[0][0];
            const float *modl = &view_matrix[0][0];
            float clip[16]; //clipping planes

            clip[0] = modl[0] * proj[0] + modl[1] * proj[4] + modl[2] * proj[8] + modl[3] * proj[12];
            clip[1] = modl[0] * proj[1] + modl[1] * proj[5] + modl[2] * proj[9] + modl[3] * proj[13];
            clip[2] = modl[0] * proj[2] + modl[1] * proj[6] + modl[2] * proj[10] + modl[3] * proj[14];
            clip[3] = modl[0] * proj[3] + modl[1] * proj[7] + modl[2] * proj[11] + modl[3] * proj[15];

            clip[4] = modl[4] * proj[0] + modl[5] * proj[4] + modl[6] * proj[8] + modl[7] * proj[12];
            clip[5] = modl[4] * proj[1] + modl[5] * proj[5] + modl[6] * proj[9] + modl[7] * proj[13];
            clip[6] = modl[4] * proj[2] + modl[5] * proj[6] + modl[6] * proj[10] + modl[7] * proj[14];
            clip[7] = modl[4] * proj[3] + modl[5] * proj[7] + modl[6] * proj[11] + modl[7] * proj[15];

            clip[8] = modl[8] * proj[0] + modl[9] * proj[4] + modl[10] * proj[8] + modl[11] * proj[12];
            clip[9] = modl[8] * proj[1] + modl[9] * proj[5] + modl[10] * proj[9] + modl[11] * proj[13];
            clip[10] = modl[8] * proj[2] + modl[9] * proj[6] + modl[10] * proj[10] + modl[11] * proj[14];
            clip[11] = modl[8] * proj[3] + modl[9] * proj[7] + modl[10] * proj[11] + modl[11] * proj[15];

            clip[12] = modl[12] * proj[0] + modl[13] * proj[4] + modl[14] * proj[8] + modl[15] * proj[12];
            clip[13] = modl[12] * proj[1] + modl[13] * proj[5] + modl[14] * proj[9] + modl[15] * proj[13];
            clip[14] = modl[12] * proj[2] + modl[13] * proj[6] + modl[14] * proj[10] + modl[15] * proj[14];
            clip[15] = modl[12] * proj[3] + modl[13] * proj[7] + modl[14] * proj[11] + modl[15] * proj[15];

            planes[RIGHT] = glm::vec4(clip[3] - clip[0], clip[7] - clip[4], clip[11] - clip[8], clip[15] - clip[12]);
            normalize(planes[RIGHT]);

            planes[LEFT] = glm::vec4(clip[3] + clip[0], clip[7] + clip[4], clip[11] + clip[8], clip[15] + clip[12]);
            normalize(planes[LEFT]);

            planes[BOTTOM] = glm::vec4(clip[3] + clip[1], clip[7] + clip[5], clip[11] + clip[9], clip[15] + clip[13]);
            normalize(planes[BOTTOM]);

            planes[TOP] = glm::vec4(clip[3] - clip[1], clip[7] - clip[5], clip[11] - clip[9], clip[15] - clip[13]);
            normalize(planes[TOP]);

            planes[BACK] = glm::vec4(clip[3] - clip[2], clip[7] - clip[6], clip[11] - clip[10], clip[15] - clip[14]);
            normalize(planes[BACK]);

            planes[FRONT] = glm::vec4(clip[3] + clip[2], clip[7] + clip[6], clip[11] + clip[10], clip[15] + clip[14]);
            normalize(planes[FRONT]);
        }

        /// Check if box is contained
        inline bool contains(const Box &box) const {
            bool inside = true;
            //test all 6 frustum planes
            for (int i = 0; i<6; i++) {
                //pick closest point to plane and check if it behind the plane
                //if yes - object outside frustum
                float d = std::max<float>(box.Min.x * planes[i].x, box.Max.x * planes[i].x)
                        + std::max<float>(box.Min.y * planes[i].y, box.Max.y * planes[i].y)
                        + std::max<float>(box.Min.z * planes[i].z, box.Max.z * planes[i].z)
                        + planes[i].w;
                inside &= d > 0;
                //return false; //with flag works faster
            }
            return inside;
        }
    };
}
