#include <algo/raycast.hpp>

#include <algo/matrix.hpp>
#include <edit/scene.hpp>
#include <gfx/camera.hpp>
#include <gfx/mesh.hpp>
#include <utils/math.hpp>

#include <algorithm>
#include <iostream>
#include <limits>
#include <map>

using scene_ptr = std::shared_ptr<Scene>;

std::pair<float, float> intersect_axis(float ray_direction_a, float ray_position_a, 
                                                  float LH_first_a, float LH_second_a)
{
    float t_low;
    float t_high;
    if (ray_direction_a == 0 && LH_first_a <= ray_position_a && ray_position_a <= LH_second_a)
    {
        t_low = -std::numeric_limits<float>::infinity();
        t_high = std::numeric_limits<float>::infinity();
    }
    else
    {
        t_low = glm::min((LH_first_a - ray_position_a) / ray_direction_a, 
                         (LH_second_a - ray_position_a) / ray_direction_a);
        t_high = glm::max((LH_first_a - ray_position_a) / ray_direction_a,
                          (LH_second_a - ray_position_a) / ray_direction_a);
    }

    return { t_low, t_high };
}

std::pair<float, float> intersect(glm::vec3 ray_position, glm::vec3 ray_direction, 
                                             std::pair<glm::vec3, glm::vec3> LH)
{
    auto [ tx, tX ] = intersect_axis(ray_direction.x, ray_position.x, LH.first.x, LH.second.x);
    auto [ ty, tY ] = intersect_axis(ray_direction.y, ray_position.y, LH.first.y, LH.second.y);
    auto [ tz, tZ ] = intersect_axis(ray_direction.z, ray_position.z, LH.first.z, LH.second.z);

    float t0 = glm::max(tx, glm::max(ty, tz));
    float t1 = glm::min(tX, glm::min(tY, tZ));

    return {t0, t1};
}

std::pair<glm::vec3, node_ptr> find_intersection_rec(const std::vector<node_ptr> &where, glm::vec3 ray_position, glm::vec3 ray_direction, node_ptr &nearest_node, float &nearest_t)
{
    for (auto &node : where)
    {
        for (auto &object : node->getObjects())
        {
            auto obj = dynamic_pointer_cast<Mesh>(object);
            if (!obj || !obj->isSelectable())
                continue;
            
            auto LH_world = obj->getAABB().getLHWorld(node);
            auto [ t0, t1 ] = intersect(ray_position, ray_direction, LH_world);

            if ( t1 < t0 || t1 < 0)
                continue;
            if (t0 < 0 && t1 < nearest_t)
            {
                nearest_t = t1;
                nearest_node = node;
            }
            else if (t0 < nearest_t)
            {
                nearest_t = t0;
                nearest_node = node;
            }
        }
    }

    for (auto &node : where)
    {
        find_intersection_rec(node->getChildren(), ray_position, ray_direction, nearest_node, nearest_t);
    }

    return { ray_position + (nearest_t * ray_direction), nearest_node };
}

std::pair<glm::vec3, node_ptr> find_intersection(const std::vector<node_ptr> &where, glm::vec3 ray_position, glm::vec3 ray_direction)
{
    node_ptr nearest_node = nullptr;
    float nearest_t = std::numeric_limits<float>::infinity();
    return find_intersection_rec(where, ray_position, ray_direction, nearest_node, nearest_t);
}

glm::ivec3 find_hit_plane(std::array<glm::vec3, 8> &AABB_local, glm::vec3 hit_coords)
{
    auto counter = glm::ivec3(0);
    float epsilon = 0.0001f;
    
    for (auto &vertex : AABB_local)
    {
        if (glm::epsilonEqual(vertex.x, hit_coords.x, epsilon))
            counter.x += 1;
        else if (glm::epsilonEqual(vertex.y, hit_coords.y, epsilon))
            counter.y += 1;
        else if (glm::epsilonEqual(vertex.z, hit_coords.z, epsilon))
            counter.z += 1;
    }

    return counter;
}

std::array<glm::vec3, 4> filter_vertices_on_plane(std::array<glm::vec3, 8> &AABB_local, glm::vec3 hit_coords, int plane_axis_index)
{
    std::array<glm::vec3, 4> side_vertices;
    float epsilon = 0.0001f;
    int i = 0;
    for (auto &vertex : AABB_local)
    {
        if (glm::epsilonEqual(vertex[plane_axis_index], hit_coords[plane_axis_index], epsilon))
        {
            side_vertices[i] = vertex;
            ++i;
        }
        if (i >= 4)
            break;
    }
    return side_vertices;
}

glm::vec3 find_side_direction(glm::vec3 &hit_coords, std::pair<glm::vec3, glm::vec3> &LH_local, int plane_axis_index)
{
    auto side_direction = glm::vec3(0.0f);
    float epsilon = 0.0001f;
    side_direction[plane_axis_index] = glm::epsilonEqual(hit_coords[plane_axis_index], LH_local.first[plane_axis_index], epsilon) ? -1.0f : 1.0f;

    return side_direction;
}

std::pair<glm::vec3, std::array<glm::vec3, 4>> find_side(std::array<glm::vec3, 8> &AABB_local, std::pair<glm::vec3, glm::vec3> &LH_local, glm::vec3 hit_coords)
{
    auto counter = find_hit_plane(AABB_local, hit_coords);

    int plane_axis_index = counter.x > counter.y && counter.x > counter.z
                           ? 0 
                           : (counter.y > counter.z) ? 1 : 2;

    std::array<glm::vec3, 4> side_vertices = filter_vertices_on_plane(AABB_local, hit_coords, plane_axis_index);

    auto side_direction = find_side_direction(hit_coords, LH_local, plane_axis_index);

    return { side_direction, side_vertices };
}

std::pair<glm::vec3, std::array<glm::vec3, 4>> find_cube_side(node_ptr node, glm::vec3 hit_coords)
{
    glm::vec3 side_direction;
    std::array<glm::vec3, 4> side_vertices;
    for (auto &object : node->getObjects())
        {
            auto obj = dynamic_pointer_cast<Mesh>(object);
            if (!obj)
                continue;

            auto AABB_local = obj->getAABB().getFullAABB();
            auto LH_local = obj->getAABB().getLH();
            glm::vec3 hit_local = node->getInverseModelMatWorld() * glm::vec4(hit_coords, 1);
            std::tie(side_direction, side_vertices) = find_side(AABB_local, LH_local, hit_local);
            break; // possibly not needed - should receive selection_AABB node
        }
    return { side_direction, side_vertices };
}

std::pair<glm::vec3, glm::vec3> get_pick_ray_perspective(const camera_ptr &camera, const glm::mat4 view, const osi::Mouse &mouse, const osi::Window &window)
{
    // https://gamedev.stackexchange.com/questions/12360/how-do-you-determine-which-object-surface-the-users-pointing-at-with-lwjgl/12370#12370
    //get the mouse position in screenSpace coords
    float aspect_ratio = camera->getWindowSize().x / camera->getWindowSize().y;
    float screen_space_X = ((float) mouse.pos().x / (window.size().x / 2) - 1.0f) * aspect_ratio;
    float screen_space_Y = (1.0f - (float) mouse.pos().y / (window.size().y / 2));

    float view_ratio = glm::tan((glm::pi<float>() / (180.f/camera->getFOV()) / 2.0f));  // * zoomFactor;

    screen_space_X = screen_space_X * view_ratio;
    screen_space_Y = screen_space_Y * view_ratio;

    //Find the far and near camera spaces
    float near_plane = 0.1f;
    float far_plane = 100.0f;
    glm::vec4 camera_space_near = glm::vec4(screen_space_X * near_plane, screen_space_Y * near_plane, -near_plane, 1);
    glm::vec4 camera_space_far = glm::vec4(screen_space_X * far_plane,  screen_space_Y * far_plane,  -far_plane, 1);

    //Unproject the 2D window into 3D to see where in 3D we're actually clicking
    glm::mat4 inv_view = glm::inverse(view);
    // Matrix4f.transform(invView, cameraSpaceNear, world_space_near); ??????????????
    glm::vec4 world_space_near = inv_view * camera_space_near;
    // Matrix4f.transform(invView, cameraSpaceFar, world_space_far); ???????????????
    glm::vec4 world_space_far = camera_space_far * world_space_far;

    //calculate the ray position and direction
    glm::vec3 ray_position = glm::vec3(world_space_near.x, world_space_near.y, world_space_near.z);
    glm::vec3 ray_direction = -glm::vec3(world_space_far.x - world_space_near.x, 
                                        world_space_far.y - world_space_near.y, 
                                        world_space_far.z - world_space_near.z);

    ray_direction = camera->getNode()->getRotationWorld() * ray_direction;

    ray_direction = glm::normalize(ray_direction);

    return {ray_position, ray_direction};
}

std::pair<glm::vec3, glm::vec3> get_pick_ray_ortho(const camera_ptr &camera, const glm::mat4 view, const osi::Mouse &mouse, const osi::Window &window)
{
    // https://stackoverflow.com/a/66813405

    // get x and y coordinates relative to frustum width and height.
    // glOrthoWidth and glOrthoHeight are the sizeX and sizeY values 
    // you created your projection matrix with. If your frustum has a width of 100, 
    // x would become -50 when the mouse is left and +50 when the mouse is right.

    // NOTE: the / 2.0f / 128.0f corresponds to dividing numbers in render.cpp (get_projection_matrix)
    float x = +(2.0f * (float) mouse.pos().x / window.size().x  - 1) * (window.size().x / 2.0f / 128.0f);
    float y = -(2.0f * (float) mouse.pos().y / window.size().y - 1) * (window.size().y / 2.0f / 128.0f);

    // Now, you want to calculate the camera's local right and up vectors 
    // (depending on the camera's current view direction):
    glm::vec3 cameraRight = camera->getNode()->getRotationAxisXWorld();
    glm::vec3 cameraUp = camera->getNode()->getRotationAxisYWorld();

    // Finally, calculate the ray origin:
    glm::vec3 ray_position = camera->getNode()->getPositionWorld() + cameraRight * x + cameraUp * y;
    glm::vec3 ray_direction = -camera->getNode()->getRotationAxisZWorld();;

    ray_direction = glm::normalize(ray_direction);

    return {ray_position, ray_direction};
}

std::pair<glm::vec3, glm::vec3> get_pick_ray(const scene_ptr &scene, const osi::Mouse &mouse, const osi::Window &window)
{
    camera_ptr camera = scene->getActiveCameras()[0].lock(); // Should be changed to which viewport is in focus
    glm::mat4 view = get_view_matrix(camera);
    glm::mat4 projection = get_projection_matrix(camera);

    
    return camera->isPerspective() ? get_pick_ray_perspective(camera, view, mouse, window) 
                                   : get_pick_ray_ortho(camera, view, mouse, window);
}

float get_horizontal_intersect_parameter(node_ptr node, const glm::vec3 ray_position, const glm::vec3 ray_direction)
{
    ASSUMPTION(node);

    // NOTE: Worked incorrectly with normal (0, 1, 0)
    if (glm::dot(glm::vec3(0.0f, -1.0f, 0.0f), ray_direction) == 0)
        return -1;

    auto selection_y_pos = node->getPositionWorld().y;

    float t = -(glm::dot(glm::vec3(0.0f, -1.0f, 0.0f), ray_position) + selection_y_pos) / glm::dot(glm::vec3(0.0f, -1.0f, 0.0f), ray_direction);

    if (t < 0)      // No intersection
        return -1;
    if (t > 100.0f) // Far Plane
        t = 100.0f;

    return t;
}

glm::vec3 get_horizontal_plane_intersect(node_ptr node, const glm::vec3 ray_position, const glm::vec3 ray_direction)
{
    auto t = get_horizontal_intersect_parameter(node, ray_position, ray_direction);
    if (t < 0)
        return node->getPositionLocal();

    return ray_position + ray_direction * t;
}

float get_vertical_intersect_parameter(node_ptr node, const glm::quat &camera_rotation, const glm::vec3 ray_position, const glm::vec3 ray_direction)
{
    auto at_object_vertical_plane = Node::create(node->getPositionWorld(), camera_rotation);

    auto normal = glm::normalize(glm::cross(at_object_vertical_plane->getRotationAxisXWorld(), glm::vec3(0.0f, 1.0f, 0.0f)));
    auto d = glm::dot(normal, glm::vec3(0, 0, 0) - node->getPositionWorld()) / glm::length(normal);

    float t = -(glm::dot(normal, ray_position) + d) / glm::dot(normal, ray_direction);

    if (t < 0)      // No intersection
        return -1;
    if (t > 100.0f) // Far Plane
        t = 100.0f;

    return t;
}

glm::vec3 get_vertical_plane_intersect(node_ptr node, const glm::quat &camera_rotation, const glm::vec3 ray_position, const glm::vec3 ray_direction)
{
    auto t = get_vertical_intersect_parameter(node, camera_rotation, ray_position, ray_direction);
    if (t < 0)
        return node->getPositionLocal();

    return ray_position + ray_direction * t;
}
