#include "Components/TransformComponent.h"
#include "Components/GameObject.h"
#include "Scene/Scene.h"
#include <algorithm>

namespace XCEngine {
namespace Components {

TransformComponent::TransformComponent() = default;

void TransformComponent::SetDirty() {
    m_dirty = true;
    for (auto* child : m_children) {
        if (child) {
            child->SetDirty();
        }
    }
}

void TransformComponent::UpdateWorldTransform() const {
    if (!m_dirty) {
        return;
    }

    if (m_parent) {
        const Math::Matrix4x4& parentMatrix = m_parent->GetLocalToWorldMatrix();
        Math::Matrix4x4 localMatrix = Math::Matrix4x4::TRS(m_localPosition, m_localRotation, m_localScale);
        m_localToWorldMatrix = parentMatrix * localMatrix;
    } else {
        m_localToWorldMatrix = Math::Matrix4x4::TRS(m_localPosition, m_localRotation, m_localScale);
    }

    m_localToWorldMatrix.Decompose(m_worldPosition, m_worldRotation, m_worldScale);
    m_dirty = false;
}

const Math::Matrix4x4& TransformComponent::GetLocalToWorldMatrix() const {
    UpdateWorldTransform();
    return m_localToWorldMatrix;
}

Math::Matrix4x4 TransformComponent::GetWorldToLocalMatrix() const {
    return GetLocalToWorldMatrix().Inverse();
}

Math::Vector3 TransformComponent::GetPosition() const {
    UpdateWorldTransform();
    return m_worldPosition;
}

void TransformComponent::SetPosition(const Math::Vector3& position) {
    if (m_parent) {
        Math::Matrix4x4 worldToParent = m_parent->GetWorldToLocalMatrix();
        m_localPosition = worldToParent.MultiplyPoint(position);
    } else {
        m_localPosition = position;
    }
    SetDirty();
}

Math::Quaternion TransformComponent::GetRotation() const {
    UpdateWorldTransform();
    return m_worldRotation;
}

void TransformComponent::SetRotation(const Math::Quaternion& rotation) {
    if (m_parent) {
        Math::Quaternion parentInverse = m_parent->GetRotation().Inverse();
        m_localRotation = parentInverse * rotation;
    } else {
        m_localRotation = rotation;
    }
    SetDirty();
}

Math::Vector3 TransformComponent::GetScale() const {
    UpdateWorldTransform();
    return m_worldScale;
}

void TransformComponent::SetScale(const Math::Vector3& scale) {
    if (m_parent) {
        const Math::Vector3& parentScale = m_parent->GetScale();
        m_localScale = Math::Vector3(
            parentScale.x != 0.0f ? scale.x / parentScale.x : 0.0f,
            parentScale.y != 0.0f ? scale.y / parentScale.y : 0.0f,
            parentScale.z != 0.0f ? scale.z / parentScale.z : 0.0f
        );
    } else {
        m_localScale = scale;
    }
    SetDirty();
}

Math::Vector3 TransformComponent::GetLocalEulerAngles() const {
    return m_localRotation.ToEulerAngles() * Math::RAD_TO_DEG;
}

void TransformComponent::SetLocalEulerAngles(const Math::Vector3& eulers) {
    m_localRotation = Math::Quaternion::FromEulerAngles(eulers * Math::DEG_TO_RAD);
    SetDirty();
}

Math::Vector3 TransformComponent::GetForward() const {
    return GetRotation() * Math::Vector3::Forward();
}

Math::Vector3 TransformComponent::GetRight() const {
    return GetRotation() * Math::Vector3::Right();
}

Math::Vector3 TransformComponent::GetUp() const {
    return GetRotation() * Math::Vector3::Up();
}

void TransformComponent::SetParent(TransformComponent* parent, bool worldPositionStays) {
    if (m_parent == parent) {
        return;
    }

    Math::Vector3 worldPos = worldPositionStays ? GetPosition() : m_localPosition;
    Math::Quaternion worldRot = worldPositionStays ? GetRotation() : m_localRotation;
    Math::Vector3 worldScale = worldPositionStays ? GetScale() : m_localScale;

    if (m_parent) {
        auto& siblings = m_parent->m_children;
        siblings.erase(std::remove(siblings.begin(), siblings.end(), this), siblings.end());
    }

    m_parent = parent;

    if (m_parent) {
        m_parent->m_children.push_back(this);
    }

    if (worldPositionStays) {
        SetPosition(worldPos);
        Math::Quaternion newLocalRot = m_parent ? m_parent->GetRotation().Inverse() * worldRot : worldRot;
        m_localRotation = newLocalRot;
        SetScale(worldScale);
    }

    SetDirty();
}

TransformComponent* TransformComponent::GetChild(size_t index) const {
    if (index < m_children.size()) {
        return m_children[index];
    }
    return nullptr;
}

TransformComponent* FindInHierarchy(GameObject* go, const std::string& name);

TransformComponent* TransformComponent::Find(const std::string& name) const {
    GameObject* go = m_gameObject;
    if (!go) {
        return nullptr;
    }
    
    Scene* scene = go->GetScene();
    if (!scene) {
        return nullptr;
    }

    for (auto* rootGO : scene->GetRootGameObjects()) {
        TransformComponent* found = FindInHierarchy(rootGO, name);
        if (found) {
            return found;
        }
    }
    return nullptr;
}

TransformComponent* FindInHierarchy(GameObject* go, const std::string& name) {
    if (!go) {
        return nullptr;
    }
    if (go->GetName() == name) {
        return go->GetTransform();
    }
    for (size_t i = 0; i < go->GetChildCount(); ++i) {
        if (TransformComponent* found = FindInHierarchy(go->GetChild(i), name)) {
            return found;
        }
    }
    return nullptr;
}

void TransformComponent::DetachChildren() {
    for (auto* child : m_children) {
        if (child) {
            child->m_parent = nullptr;
        }
    }
    m_children.clear();
}

void TransformComponent::SetSiblingIndex(int index) {
    m_siblingIndex = index;
}

void TransformComponent::SetAsFirstSibling() {
    if (m_parent) {
        m_parent->SetSiblingIndex(0);
    }
}

void TransformComponent::SetAsLastSibling() {
    if (m_parent) {
        m_siblingIndex = static_cast<int>(m_parent->GetChildCount()) - 1;
    }
}

void TransformComponent::LookAt(const Math::Vector3& target) {
    LookAt(target, Math::Vector3::Up());
}

void TransformComponent::LookAt(const Math::Vector3& target, const Math::Vector3& up) {
    Math::Vector3 forward = Math::Vector3::Normalize(target - GetPosition());
    if (Math::Vector3::Magnitude(forward) < Math::EPSILON) {
        return;
    }
    SetRotation(Math::Quaternion::LookRotation(forward, up));
}

void TransformComponent::Rotate(const Math::Vector3& eulers, Space relativeTo) {
    Math::Quaternion rotation = Math::Quaternion::FromEulerAngles(eulers * Math::DEG_TO_RAD);
    if (relativeTo == Space::Self) {
        m_localRotation = m_localRotation * rotation;
    } else {
        m_localRotation = rotation * m_localRotation;
    }
    SetDirty();
}

void TransformComponent::Rotate(const Math::Vector3& axis, float angle, Space relativeTo) {
    Math::Quaternion rotation = Math::Quaternion::FromAxisAngle(axis, angle * Math::DEG_TO_RAD);
    if (relativeTo == Space::Self) {
        m_localRotation = m_localRotation * rotation;
    } else {
        m_localRotation = rotation * m_localRotation;
    }
    SetDirty();
}

void TransformComponent::Translate(const Math::Vector3& translation, Space relativeTo) {
    if (relativeTo == Space::Self) {
        m_localPosition = m_localPosition + translation;
    } else {
        m_localPosition = m_localPosition + translation;
    }
    SetDirty();
}

Math::Vector3 TransformComponent::TransformPoint(const Math::Vector3& point) const {
    return GetLocalToWorldMatrix().MultiplyPoint(point);
}

Math::Vector3 TransformComponent::InverseTransformPoint(const Math::Vector3& point) const {
    return GetWorldToLocalMatrix().MultiplyPoint(point);
}

Math::Vector3 TransformComponent::TransformDirection(const Math::Vector3& direction) const {
    return GetRotation() * direction;
}

Math::Vector3 TransformComponent::InverseTransformDirection(const Math::Vector3& direction) const {
    return GetRotation().Inverse() * direction;
}

void TransformComponent::NotifyHierarchyChanged() {
    SetDirty();
}

} // namespace Components
} // namespace XCEngine