// ============================================================================
// scene_graph.c - 场景图实现
// ============================================================================

#include "scene_graph.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>

void scene_init(Scene* scene) {
    memset(scene, 0, sizeof(Scene));
    scene->next_id = 1;
    // 创建根节点
    scene->root = &scene->nodes[0];
    memset(scene->root, 0, sizeof(SceneNode));
    scene->root->id = scene->next_id++;
    strcpy(scene->root->name, "Root");
    scene->root->scale = (Vec3){1, 1, 1};
    scene->root->visible = 1;
    scene->root->dirty = 1;
    scene->node_count = 1;
}

void scene_destroy(Scene* scene) {
    // 清空子节点引用 (不释放静态数组)
    for (int i = 0; i < scene->node_count; i++) {
        scene->nodes[i].parent = NULL;
        scene->nodes[i].child_count = 0;
    }
    scene->node_count = 0;
}

SceneNode* scene_create_node(Scene* scene, const char* name) {
    if (scene->node_count >= MAX_NODES) return NULL;
    SceneNode* node = &scene->nodes[scene->node_count++];
    memset(node, 0, sizeof(SceneNode));
    node->id = scene->next_id++;
    strncpy(node->name, name, 63);
    node->scale = (Vec3){1, 1, 1};
    node->visible = 1;
    node->dirty = 1;
    return node;
}

void scene_destroy_node(Scene* scene, SceneNode* node) {
    if (!node || node == scene->root) return;
    // 移除子引用
    if (node->parent) {
        scene_remove_child(node->parent, node);
    }
    // 递归销毁子节点
    while (node->child_count > 0) {
        scene_destroy_node(scene, node->children[0]);
    }
    node->id = 0;  // 标记为空
}

SceneNode* scene_find_node(Scene* scene, int id) {
    for (int i = 0; i < scene->node_count; i++) {
        if (scene->nodes[i].id == id) return &scene->nodes[i];
    }
    return NULL;
}

SceneNode* scene_find_by_name(Scene* scene, const char* name) {
    for (int i = 0; i < scene->node_count; i++) {
        if (strcmp(scene->nodes[i].name, name) == 0) return &scene->nodes[i];
    }
    return NULL;
}

void scene_set_parent(SceneNode* child, SceneNode* parent) {
    if (!child || !parent || child == parent) return;
    if (child->parent) scene_remove_child(child->parent, child);
    child->parent = parent;
    if (parent->child_count < MAX_CHILDREN) {
        parent->children[parent->child_count++] = child;
    }
    child->dirty = 1;
}

void scene_add_child(SceneNode* parent, SceneNode* child) {
    scene_set_parent(child, parent);
}

void scene_remove_child(SceneNode* parent, SceneNode* child) {
    if (!parent || !child) return;
    for (int i = 0; i < parent->child_count; i++) {
        if (parent->children[i] == child) {
            parent->children[i] = parent->children[parent->child_count - 1];
            parent->child_count--;
            child->parent = NULL;
            child->dirty = 1;
            return;
        }
    }
}

void scene_set_position(SceneNode* node, float x, float y, float z) {
    node->position = (Vec3){x, y, z};
    node->dirty = 1;
}

void scene_set_rotation_euler(SceneNode* node, float rx, float ry, float rz) {
    // 欧拉角转四元数 (YXZ 顺序)
    float cr = cosf(rx * 0.5f), sr = sinf(rx * 0.5f);
    float cp = cosf(ry * 0.5f), sp = sinf(ry * 0.5f);
    float cy = cosf(rz * 0.5f), sy = sinf(rz * 0.5f);
    node->rotation = (Quat){
        sr * cp * cy - cr * sp * sy,
        cr * sp * cy + sr * cp * sy,
        cr * cp * sy - sr * sp * cy,
        cr * cp * cy + sr * sp * sy
    };
    node->dirty = 1;
}

void scene_set_scale(SceneNode* node, float sx, float sy, float sz) {
    node->scale = (Vec3){sx, sy, sz};
    node->dirty = 1;
}

void scene_set_rotation_quat(SceneNode* node, const Quat* q) {
    node->rotation = *q;
    node->dirty = 1;
}

void scene_mark_dirty(SceneNode* node) {
    node->dirty = 1;
    for (int i = 0; i < node->child_count; i++) {
        scene_mark_dirty(node->children[i]);
    }
}

static void update_local_matrix(SceneNode* node) {
    // 旋转矩阵
    Mat4 rot;
    quat_to_matrix(&node->rotation, &rot);

    // 缩放矩阵
    Mat4 scl;
    mat4_identity(&scl);
    scl.m[0][0] = node->scale.x;
    scl.m[1][1] = node->scale.y;
    scl.m[2][2] = node->scale.z;

    // 平移矩阵
    Mat4 trl;
    mat4_translate(node->position.x, node->position.y, node->position.z, &trl);

    // local = trl * rot * scl
    Mat4 tmp;
    mat4_multiply(&rot, &scl, &tmp);
    mat4_multiply(&trl, &tmp, &node->local_matrix);
}

static void update_world_matrices(SceneNode* node) {
    if (node->dirty) {
        update_local_matrix(node);
        if (node->parent) {
            mat4_multiply(&node->parent->world_matrix, &node->local_matrix, &node->world_matrix);
        } else {
            node->world_matrix = node->local_matrix;
        }
        node->dirty = 0;
    }
    for (int i = 0; i < node->child_count; i++) {
        update_world_matrices(node->children[i]);
    }
}

void scene_update(Scene* scene) {
    if (scene->root) {
        update_world_matrices(scene->root);
    }
}

void scene_get_world_position(SceneNode* node, Vec3* out) {
    out->x = node->world_matrix.m[3][0];
    out->y = node->world_matrix.m[3][1];
    out->z = node->world_matrix.m[3][2];
}

void scene_get_world_rotation(SceneNode* node, Quat* out) {
    // 从世界矩阵提取四元数
    Mat4 m = node->world_matrix;
    float tr = m.m[0][0] + m.m[1][1] + m.m[2][2];
    if (tr > 0) {
        float s = 0.5f / sqrtf(tr + 1.0f);
        out->w = 0.25f / s;
        out->x = (m.m[2][1] - m.m[1][2]) * s;
        out->y = (m.m[0][2] - m.m[2][0]) * s;
        out->z = (m.m[1][0] - m.m[0][1]) * s;
    } else if (m.m[0][0] > m.m[1][1] && m.m[0][0] > m.m[2][2]) {
        float s = 2.0f * sqrtf(1.0f + m.m[0][0] - m.m[1][1] - m.m[2][2]);
        out->w = (m.m[2][1] - m.m[1][2]) / s;
        out->x = 0.25f * s;
        out->y = (m.m[0][1] + m.m[1][0]) / s;
        out->z = (m.m[0][2] + m.m[2][0]) / s;
    } else if (m.m[1][1] > m.m[2][2]) {
        float s = 2.0f * sqrtf(1.0f + m.m[1][1] - m.m[0][0] - m.m[2][2]);
        out->w = (m.m[0][2] - m.m[2][0]) / s;
        out->x = (m.m[0][1] + m.m[1][0]) / s;
        out->y = 0.25f * s;
        out->z = (m.m[1][2] + m.m[2][1]) / s;
    } else {
        float s = 2.0f * sqrtf(1.0f + m.m[2][2] - m.m[0][0] - m.m[1][1]);
        out->w = (m.m[1][0] - m.m[0][1]) / s;
        out->x = (m.m[0][2] + m.m[2][0]) / s;
        out->y = (m.m[1][2] + m.m[2][1]) / s;
        out->z = 0.25f * s;
    }
}

void scene_get_world_scale(SceneNode* node, Vec3* out) {
    // 从世界矩阵提取缩放 (各列向量的长度)
    out->x = sqrtf(node->world_matrix.m[0][0] * node->world_matrix.m[0][0] +
                    node->world_matrix.m[0][1] * node->world_matrix.m[0][1] +
                    node->world_matrix.m[0][2] * node->world_matrix.m[0][2]);
    out->y = sqrtf(node->world_matrix.m[1][0] * node->world_matrix.m[1][0] +
                    node->world_matrix.m[1][1] * node->world_matrix.m[1][1] +
                    node->world_matrix.m[1][2] * node->world_matrix.m[1][2]);
    out->z = sqrtf(node->world_matrix.m[2][0] * node->world_matrix.m[2][0] +
                    node->world_matrix.m[2][1] * node->world_matrix.m[2][1] +
                    node->world_matrix.m[2][2] * node->world_matrix.m[2][2]);
}