#include <common/node.hpp>

#include <algorithm>
#include <iterator>

/* PROTECTED */
Node::Node(glm::vec3 _position_vec, glm::vec3 rotation_vec, glm::vec3 _scale_vec)
: position_vec{_position_vec}, scale_vec{_scale_vec}
{    
    setRotationQuat(rotation_vec);
}

Node::Node(glm::vec3 _position_vec, glm::quat _rotation_quat, glm::vec3 _scale_vec)
: position_vec{_position_vec}, rotation_quat{glm::normalize(_rotation_quat)}, scale_vec(_scale_vec) {}

Node::Node(const Node &other)
{
    position_vec = other.position_vec;
    rotation_quat = other.rotation_quat;
    scale_vec = other.scale_vec;
    model_world = other.model_world == nullptr ? nullptr : std::make_unique<glm::mat4>(*other.model_world);
}

Node::Node(const Node &&other)
{
    position_vec = other.position_vec;
    rotation_quat = other.rotation_quat;
    scale_vec = other.scale_vec;
    model_world = std::move(other.model_world);
}

Node& Node::operator=(const Node &other)
{
    position_vec = other.position_vec;
    rotation_quat = other.rotation_quat;
    scale_vec = other.scale_vec;
    model_world = other.model_world == nullptr ? nullptr : std::make_unique<glm::mat4>(*other.model_world);

    return *this;
}

Node& Node::operator=(const Node &&other)
{
    position_vec = other.position_vec;
    rotation_quat = other.rotation_quat;
    scale_vec = other.scale_vec;
    model_world = std::move(other.model_world);

    return *this;
}

glm::vec3 Node::get_position_from_mat(glm::mat4 mat)
{ 
    return mat[3]; 
}

glm::vec3 Node::get_scale_from_mat(glm::mat4 mat)
{ 
    glm::vec3 scale;
    for (int i = 0; i < 3; ++i)
        scale[i] = glm::length(glm::vec3(mat[i]));
    return scale;
}

glm::quat Node::get_rotation_from_mat(glm::mat4 mat)
{
    auto scale = get_scale_from_mat(mat);
    const glm::mat3 rotation_mat(
        glm::vec3(mat[0]) / scale[0],
        glm::vec3(mat[1]) / scale[1],
        glm::vec3(mat[2]) / scale[2]);
    return glm::quat_cast(rotation_mat);
}

void Node::decompose_model_mat(const glm::mat4& mat, glm::vec3& position, glm::quat& rotation, glm::vec3& scale)
{
    position = mat[3];
    for(int i = 0; i < 3; ++i)
        scale[i] = glm::length(glm::vec3(mat[i]));

    const glm::mat3 rotation_mat(
        glm::vec3(mat[0]) / scale[0],
        glm::vec3(mat[1]) / scale[1],
        glm::vec3(mat[2]) / scale[2]);
    rotation = glm::quat_cast(rotation_mat);
}

/* PUBLIC */
node_ptr Node::create(glm::vec3 _position_vec, glm::vec3 rotation_vec, glm::vec3 _scale_vec)
{
    return node_ptr{ new Node(_position_vec, rotation_vec, _scale_vec) };
}

node_ptr Node::create(glm::vec3 _position_vec, glm::quat _rotation_quat, glm::vec3 _scale_vec)
{
    return node_ptr{ new Node(_position_vec, _rotation_quat, _scale_vec) };
}

node_ptr Node::create(const glm::mat4 &matrix)
{
    glm::vec3 position;
    glm::quat rotation; 
    glm::vec3 scale;
    decompose_model_mat(matrix, position, rotation, scale);

    return Node::create(position, rotation, scale);
}

node_ptr Node::create(const glm::mat4 &&matrix)
{
    glm::vec3 position;
    glm::quat rotation; 
    glm::vec3 scale;
    decompose_model_mat(matrix, position, rotation, scale);
    
    return Node::create(position, rotation, scale);
}

node_ptr Node::copy_frame(node_ptr other_node)
{
    return node_ptr{ new Node(*other_node) };
}

node_ptr Node::copy_shallow(node_ptr other_node)
{
    auto node = Node::copy_frame(other_node);

    node->setChildren(other_node->getChildren());
    node->setObjects(other_node->getObjects());

    return node;
}

node_ptr Node::copy_deep(node_ptr other_node)
{
    auto node = Node::copy_frame(other_node);

    node->setObjects(other_node->getObjects()); 
       
    for (auto &child : other_node->getChildren())
        node->addChild(Node::copy_deep(child));

    return node;
}

/* GETTERS */
    /* LOCAL */
glm::vec3 Node::getPositionLocal() const { return position_vec; }
glm::quat Node::getRotationLocal() const { return rotation_quat; }
glm::vec3 Node::getScaleLocal() const { return scale_vec; }

glm::vec3 Node::getRotationAxisXLocal() const { return glm::normalize(glm::column(glm::toMat3(rotation_quat), 0)); }
glm::vec3 Node::getRotationAxisYLocal() const { return glm::normalize(glm::column(glm::toMat3(rotation_quat), 1)); }
glm::vec3 Node::getRotationAxisZLocal() const { return glm::normalize(glm::column(glm::toMat3(rotation_quat), 2)); }

glm::mat4 Node::getTranslationMatLocal() const { return glm::translate(glm::mat4(1.0f), position_vec); }
glm::mat4 Node::getRotationMatLocal() const { return glm::toMat4(rotation_quat); }
glm::mat4 Node::getInverseRotationMatLocal() const { return glm::inverse(getRotationMatLocal()); }
glm::mat4 Node::getScaleMatLocal() const { return glm::scale(glm::mat4(1.0f), scale_vec); }

glm::mat4 Node::getModelMatLocal() const { return getTranslationMatLocal() * getRotationMatLocal() * getScaleMatLocal(); }
glm::mat4 Node::getInverseModelMatLocal() const { return glm::inverse(getModelMatLocal()); }

    /* WORLD */
glm::vec3 Node::getPositionWorld() const { return get_position_from_mat(getModelMatWorld()); }
glm::quat Node::getRotationWorld() const { return get_rotation_from_mat(getModelMatWorld()); }
glm::vec3 Node::getScaleWorld() const { return get_scale_from_mat(getModelMatWorld()); }

glm::vec3 Node::getRotationAxisXWorld() const { return glm::normalize(glm::column(glm::toMat3(getRotationWorld()), 0)); }
glm::vec3 Node::getRotationAxisYWorld() const { return glm::normalize(glm::column(glm::toMat3(getRotationWorld()), 1)); }
glm::vec3 Node::getRotationAxisZWorld() const { return glm::normalize(glm::column(glm::toMat3(getRotationWorld()), 2)); }

glm::mat4 Node::getTranslationMatWorld() const { return glm::translate(glm::mat4(1.0f), get_position_from_mat(getModelMatWorld())); }
glm::mat4 Node::getRotationMatWorld() const { return glm::toMat4(get_rotation_from_mat(getModelMatWorld())); }
glm::mat4 Node::getInverseRotationMatWorld() const { return glm::inverse(getRotationMatWorld()); }
glm::mat4 Node::getScaleMatWorld() const { return glm::scale(glm::mat4(1.0f), get_scale_from_mat(getModelMatWorld())); }

glm::mat4 Node::getModelMatWorld() const 
{
    if (!model_world)
    {
        if (parent.expired())
            setModelMatWorld(getModelMatLocal());
        else
            setModelMatWorld(parent.lock()->getModelMatWorld() * getModelMatLocal());

        return *model_world;
    }

    return *model_world;
}
glm::mat4 Node::getInverseModelMatWorld() const { return glm::inverse(getModelMatWorld()); }

glm::mat4 Node::getParentModelMatWorld() const { return !parent.expired() ? parent.lock()->getModelMatWorld() : glm::mat4(1.0f); }
glm::mat4 Node::getParentInverseModelMatWorld() const { return !parent.expired() ? parent.lock()->getRotationMatWorld() : glm::mat4(1.0f); }
glm::mat4 Node::getParentRotationMatWorld() const { return  !parent.expired() ? parent.lock()->getInverseModelMatWorld() : glm::mat4(1.0f); }
glm::mat4 Node::getParentInverseRotationMatWorld() const { return  !parent.expired() ? parent.lock()->getInverseRotationMatWorld() : glm::mat4(1.0f); }

/* SETTERS */
    /* LOCAL */
void Node::setPosVec(glm::vec3 _position_vec) 
{ 
    position_vec = _position_vec; 
    invalidateModelMatWorld(); 
}
void Node::translatePosVec(glm::vec3 direction) 
{ 
    position_vec += direction; 
    invalidateModelMatWorld(); 
}
void Node::translatePosVec(glm::mat4 matrix)
{
    position_vec = matrix * glm::vec4(position_vec, 1.0f);
    invalidateModelMatWorld();
}
void Node::setRotationQuat(glm::vec3 rotation_vec) 
{ 
    rotation_quat = glm::normalize(glm::tquat(glm::radians(rotation_vec))); 
    invalidateModelMatWorld(); 
}
void Node::setRotationQuat(glm::quat _rotation_quat) 
{ 
    rotation_quat = glm::normalize(_rotation_quat); 
    invalidateModelMatWorld();
}
void Node::setRotationQuat(const glm::mat4 &rotation_matrix)
{
    rotation_quat = get_rotation_from_mat(rotation_matrix);
    invalidateModelMatWorld();
}
void Node::rotateRotationQuat(glm::vec3 added_rotation) 
{ 
    rotation_quat = glm::normalize(glm::tquat(glm::radians(added_rotation)) * rotation_quat); 
    invalidateModelMatWorld(); 
}
void Node::rotateRotationQuat(glm::quat added_rotation) 
{ 
    rotation_quat = glm::normalize(added_rotation * rotation_quat); 
    invalidateModelMatWorld(); 
}
void Node::setScale(glm::vec3 _scale_vec) 
{ 
    scale_vec = _scale_vec; 
    invalidateModelMatWorld(); 
}

void Node::setFrame(glm::vec3 _position_vec, glm::vec3 rotation_vec, glm::vec3 _scale_vec)
{
    setPosVec(_position_vec);
    setRotationQuat(rotation_vec);
    setScale(_scale_vec);
    invalidateModelMatWorld(); 

}
void Node::setFrame(glm::vec3 _position_vec, glm::quat _rotation_quat, glm::vec3 _scale_vec)
{
    setPosVec(_position_vec);
    setRotationQuat(_rotation_quat);
    setScale(_scale_vec);
    invalidateModelMatWorld();
}
/* WORLD */
void Node::translatePosVecWorld(glm::vec3 direction) 
{ 
    position_vec += glm::vec3( getParentInverseModelMatWorld() * glm::vec4(direction, 0.0f)); 
    invalidateModelMatWorld(); 
}

void Node::normalizeQuat() { rotation_quat = glm::normalize(rotation_quat); }

/* WORLD MODEL MATRIX SETTERS */
void Node::setModelMatWorld(glm::mat4 _model_world) const { model_world = std::make_unique<glm::mat4>(_model_world); }
void Node::invalidateModelMatWorld() const 
{ 
    for (auto &child : children)
        child->invalidateModelMatWorld();

    model_world = nullptr;
}

/* NODE HIERARCHY METHODS */
bool Node::isParentOf(node_ptr other_node)
{
    for (auto &child : children)
    {
        if (child == other_node)
            return true;
        if (child->isParentOf(other_node))
            return true;
    }

    return false;
}

node_weak_ptr Node::getRootParent()
{
    if (parent.expired())
        return weak_from_this();
    return parent.lock()->getRootParent();
}

node_weak_ptr Node::getParent() const { return parent; }

std::vector<node_ptr> &Node::getChildren() { return children; }

void Node::addChild(node_ptr child, bool transform_to_parent_base) 
{
    child->setParent(weak_from_this(), transform_to_parent_base);
    children.emplace_back(child); 
}

void Node::removeChild(std::vector<node_ptr>::iterator it)
{
    it.operator*()->removeParent();
    children.erase(it);
}

void Node::removeChild(node_ptr child) 
{
    removeChild(std::find(children.begin(), children.end(), child));
}

void Node::setChildren(std::vector<node_ptr> &_children, bool transform_to_parent_base)
{ 
    clearChildren();
    extendChildren(_children, transform_to_parent_base);
}

void Node::removeChildren(const std::vector<node_ptr> &unwanted_children)
{
    for (const auto &unwanted_child : unwanted_children)
    {
        if (auto it = std::find(children.begin(), children.end(), unwanted_child);
            it != children.end())
        {
            removeChild(it);
        }
    }
}

void Node::clearChildren()
{
    for (auto it = children.begin(); it != children.end(); it = children.begin())
    {
        removeChild(it);
    }
}

void Node::extendChildren(const std::vector<node_ptr> &extra_children, bool transform_to_parent_base)
{
    for (auto &child : extra_children)
        addChild(child, transform_to_parent_base);
}

void Node::transferChild(node_ptr child, node_weak_ptr new_parent, bool transform_to_parent_base)
{
    children.erase(std::find(children.begin(), children.end(), child));
    new_parent.lock()->addChild(child, transform_to_parent_base);
}

const std::vector<objectbase_ptr> &Node::getObjects() const { return objects; }
std::vector<objectbase_ptr> &Node::getObjects() { return objects; }
void Node::setObjects(const std::vector<objectbase_ptr> &_objects) { objects = _objects; }

void Node::addObject(ObjectBase obj) { objects.emplace_back(std::make_shared<ObjectBase>(obj)); }
void Node::addObject(objectbase_ptr obj) { objects.emplace_back(obj); }

/* PROTECTED */

void Node::setParent(node_weak_ptr _parent, bool recalculate_basis) 
{
    if (recalculate_basis && !_parent.expired())
    {
        glm::vec3 position;
        glm::quat rotation; 
        glm::vec3 scale;
        decompose_model_mat(_parent.lock()->getInverseModelMatWorld() * getModelMatLocal(), position, rotation, scale);
        
        setPosVec(position);
        setRotationQuat(rotation);
        setScale(scale);
    }

    parent = _parent; 
    invalidateModelMatWorld();
}
void Node::removeParent() 
{
    glm::vec3 position;
    glm::quat rotation; 
    glm::vec3 scale;
    decompose_model_mat(getModelMatWorld(), position, rotation, scale);
    
    setPosVec(position);
    setRotationQuat(rotation);
    setScale(scale);

    parent.reset(); 
    invalidateModelMatWorld();
}