#include <controls/obj_control.hpp>

#include <algo/raycast.hpp>

void ObjectController::forward()
{
    bound_node->translatePosVec(step * getFrontVec());
}
void ObjectController::backward()
{
    bound_node->translatePosVec(-step * getFrontVec());
}
void ObjectController::left()
{
    bound_node->translatePosVec(-step * glm::normalize(glm::cross(getFrontVec(), getUpVec())));
}
void ObjectController::right()
{
    bound_node->translatePosVec(step * glm::normalize(glm::cross(getFrontVec(), getUpVec())));
}
void ObjectController::up()
{
    bound_node->translatePosVec(step * getUpVec());
}
void ObjectController::down()
{
    bound_node->translatePosVec(-step * getUpVec());
}

void ObjectController::mouseRotate(const osi::Mouse &mouse)
{
    // TO DO: check for start at object?
    if (!mouse.down().contains("MouseLeft"))
        return;

    glm::i32vec2 offset = mouse.pos_delta();
    float xoffset = static_cast<float>(offset.x);

    /* FUN FACT: This code worked correctly before the camera rotation was fixed */
    // auto inv_rotation = glm::inverse(bound_node->getRotationMat());
    // glm::vec3 inv_rot_y = glm::column(inv_rotation, 1);
    // bound_node->rotateRotationQuat(glm::angleAxis(xoffset * step, inv_rot_y));

    bound_node->rotateRotationQuat(glm::angleAxis(xoffset * step, glm::vec3(0.0f, 1.0f, 0.0f)));
}

void ObjectController::mouseHorizontalMove(const glm::vec3 &ray_position, const glm::vec3 &ray_direction, glm::vec3 &prev_plane_intersect, bool &object_moving)
{
    auto new_pos = get_horizontal_plane_intersect(bound_node, ray_position, ray_direction);
    if (new_pos == bound_node->getPositionLocal()) // No valid intersection was found
        return;

    auto path = make_path(new_pos, prev_plane_intersect, object_moving);
    path.y = 0.0f;

    bound_node->translatePosVecWorld(path);

    prev_plane_intersect = new_pos;
}

void ObjectController::mouseElevationMove(const glm::vec3 &ray_position, const glm::vec3 &ray_direction, const glm::quat &camera_rotation, glm::vec3 &prev_plane_intersect, bool &object_moving)
{
    auto new_pos = get_vertical_plane_intersect(bound_node, camera_rotation, ray_position, ray_direction);
    if (new_pos == bound_node->getPositionLocal()) // No valid intersection was found
        return;

    auto path = make_path(new_pos, prev_plane_intersect, object_moving);
    path.x = 0.0f;
    path.z = 0.0f;

    bound_node->translatePosVecWorld(path);

    prev_plane_intersect = new_pos;
}

glm::vec3 make_path(glm::vec3 &new_pos, glm::vec3 &prev_plane_intersect, bool &object_moving)
{
    if (object_moving)
        return new_pos - prev_plane_intersect;
    
    prev_plane_intersect = new_pos;
    object_moving = true;
    return glm::vec3(0.0f, 0.0f, 0.0f);
}