/**
 * Copyright (c) 2026 Dominic Masters
 *
 * This software is released under the MIT License.
 * https://opensource.org/licenses/MIT
 */

#include "entity/entitymanager.h"

// Lazily recompute worldTransform from the parent chain.
static void entityPositionUpdateWorld(entityposition_t *pos) {
  if(!pos->dirty) return;

  if(pos->parentEntityId == ENTITY_ID_INVALID) {
    glm_mat4_copy(pos->localTransform, pos->worldTransform);
  } else {
    entityposition_t *parent = componentGetData(
      pos->parentEntityId, pos->parentComponentId, COMPONENT_TYPE_POSITION
    );
    entityPositionUpdateWorld(parent);
    glm_mat4_mul(parent->worldTransform, pos->localTransform, pos->worldTransform);
  }

  pos->dirty = false;
}

void entityPositionMarkDirty(entityposition_t *pos) {
  pos->dirty = true;
  for(uint8_t i = 0; i < pos->childCount; i++) {
    entityposition_t *child = componentGetData(
      pos->childEntityIds[i], pos->childComponentIds[i], COMPONENT_TYPE_POSITION
    );
    entityPositionMarkDirty(child);
  }
}

void entityPositionInit(
  const entityid_t entityId,
  const componentid_t componentId
) {
  entityposition_t *pos = componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );

  glm_vec3_zero(pos->position);
  glm_vec3_zero(pos->rotation);
  glm_vec3_one(pos->scale);
  glm_mat4_identity(pos->localTransform);
  glm_mat4_identity(pos->worldTransform);
  pos->dirty = false;
  pos->parentEntityId = ENTITY_ID_INVALID;
  pos->parentComponentId = COMPONENT_ID_INVALID;
  pos->childCount = 0;
}

void entityPositionLookAt(
  const entityid_t entityId,
  const componentid_t componentId,
  vec3 target,
  vec3 up,
  vec3 eye
) {
  entityposition_t *pos = componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );
  glm_lookat(eye, target, up, pos->localTransform);
  entityPositionDecompose(pos);
  entityPositionMarkDirty(pos);
}

void entityPositionGetTransform(
  const entityid_t entityId,
  const componentid_t componentId,
  mat4 dest
) {
  entityposition_t *pos = componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );
  entityPositionUpdateWorld(pos);
  glm_mat4_copy(pos->worldTransform, dest);
}

void entityPositionGetLocalTransform(
  const entityid_t entityId,
  const componentid_t componentId,
  mat4 dest
) {
  entityposition_t *pos = componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );
  glm_mat4_copy(pos->localTransform, dest);
}

void entityPositionGetPosition(
  const entityid_t entityId,
  const componentid_t componentId,
  vec3 dest
) {
  entityposition_t *pos = componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );
  glm_vec3_copy(pos->position, dest);
}

void entityPositionSetPosition(
  const entityid_t entityId,
  const componentid_t componentId,
  vec3 position
) {
  entityposition_t *pos = componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );
  glm_vec3_copy(position, pos->position);
  entityPositionRebuild(pos);
}

void entityPositionGetRotation(
  const entityid_t entityId,
  const componentid_t componentId,
  vec3 dest
) {
  entityposition_t *pos = componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );
  glm_vec3_copy(pos->rotation, dest);
}

void entityPositionSetRotation(
  const entityid_t entityId,
  const componentid_t componentId,
  vec3 rotation
) {
  entityposition_t *pos = componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );
  glm_vec3_copy(rotation, pos->rotation);
  entityPositionRebuild(pos);
}

void entityPositionGetScale(
  const entityid_t entityId,
  const componentid_t componentId,
  vec3 dest
) {
  entityposition_t *pos = componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );
  glm_vec3_copy(pos->scale, dest);
}

void entityPositionSetScale(
  const entityid_t entityId,
  const componentid_t componentId,
  vec3 scale
) {
  entityposition_t *pos = componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );
  glm_vec3_copy(scale, pos->scale);
  entityPositionRebuild(pos);
}

void entityPositionSetParent(
  const entityid_t entityId,
  const componentid_t componentId,
  const entityid_t parentEntityId,
  const componentid_t parentComponentId
) {
  entityposition_t *pos = componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );

  // Remove from old parent's child list.
  if(pos->parentEntityId != ENTITY_ID_INVALID) {
    entityposition_t *oldParent = componentGetData(
      pos->parentEntityId, pos->parentComponentId, COMPONENT_TYPE_POSITION
    );
    for(uint8_t i = 0; i < oldParent->childCount; i++) {
      if(
        oldParent->childEntityIds[i] == entityId &&
        oldParent->childComponentIds[i] == componentId
      ) {
        oldParent->childCount--;
        for(uint8_t j = i; j < oldParent->childCount; j++) {
          oldParent->childEntityIds[j]    = oldParent->childEntityIds[j + 1];
          oldParent->childComponentIds[j] = oldParent->childComponentIds[j + 1];
        }
        break;
      }
    }
  }

  pos->parentEntityId    = parentEntityId;
  pos->parentComponentId = parentComponentId;

  // Register with new parent.
  if(parentEntityId != ENTITY_ID_INVALID) {
    entityposition_t *parent = componentGetData(
      parentEntityId, parentComponentId, COMPONENT_TYPE_POSITION
    );
    if(parent->childCount < ENTITY_POSITION_CHILDREN_MAX) {
      parent->childEntityIds[parent->childCount]    = entityId;
      parent->childComponentIds[parent->childCount] = componentId;
      parent->childCount++;
    }
  }

  entityPositionMarkDirty(pos);
}

entityposition_t *entityPositionGet(
  const entityid_t entityId,
  const componentid_t componentId
) {
  return componentGetData(
    entityId, componentId, COMPONENT_TYPE_POSITION
  );
}

void entityPositionRebuild(entityposition_t *pos) {
  glm_mat4_identity(pos->localTransform);
  glm_translate(pos->localTransform, pos->position);
  glm_rotate_x(pos->localTransform, pos->rotation[0], pos->localTransform);
  glm_rotate_y(pos->localTransform, pos->rotation[1], pos->localTransform);
  glm_rotate_z(pos->localTransform, pos->rotation[2], pos->localTransform);
  glm_scale(pos->localTransform, pos->scale);
  entityPositionMarkDirty(pos);
}

void entityPositionDisposeDeep(
  const entityid_t entityId,
  const componentid_t componentId
) {
  entityposition_t *pos = entityPositionGet(entityId, componentId);

  // Detach from parent so the parent's child list stays consistent.
  if(pos->parentEntityId != ENTITY_ID_INVALID) {
    entityPositionSetParent(entityId, componentId, ENTITY_ID_INVALID, COMPONENT_ID_INVALID);
  }

  // Copy the child list before disposing self (entityDispose invalidates pos).
  uint8_t childCount = pos->childCount;
  entityid_t childEntityIds[ENTITY_POSITION_CHILDREN_MAX];
  componentid_t childComponentIds[ENTITY_POSITION_CHILDREN_MAX];
  for(uint8_t i = 0; i < childCount; i++) {
    childEntityIds[i]    = pos->childEntityIds[i];
    childComponentIds[i] = pos->childComponentIds[i];
    // Sever the child's parent link so it won't try to modify our disposed data.
    entityposition_t *child = entityPositionGet(childEntityIds[i], childComponentIds[i]);
    child->parentEntityId    = ENTITY_ID_INVALID;
    child->parentComponentId = COMPONENT_ID_INVALID;
  }

  entityDispose(entityId);

  for(uint8_t i = 0; i < childCount; i++) {
    entityPositionDisposeDeep(childEntityIds[i], childComponentIds[i]);
  }
}

void entityPositionDecompose(entityposition_t *pos) {
  // Translation: column 3
  pos->position[0] = pos->localTransform[3][0];
  pos->position[1] = pos->localTransform[3][1];
  pos->position[2] = pos->localTransform[3][2];

  // Scale: length of each basis column (xyz only)
  pos->scale[0] = sqrtf(
    pos->localTransform[0][0] * pos->localTransform[0][0] +
    pos->localTransform[0][1] * pos->localTransform[0][1] +
    pos->localTransform[0][2] * pos->localTransform[0][2]
  );
  pos->scale[1] = sqrtf(
    pos->localTransform[1][0] * pos->localTransform[1][0] +
    pos->localTransform[1][1] * pos->localTransform[1][1] +
    pos->localTransform[1][2] * pos->localTransform[1][2]
  );
  pos->scale[2] = sqrtf(
    pos->localTransform[2][0] * pos->localTransform[2][0] +
    pos->localTransform[2][1] * pos->localTransform[2][1] +
    pos->localTransform[2][2] * pos->localTransform[2][2]
  );

  // Normalize columns to isolate the rotation matrix.
  float invS0 = pos->scale[0] > 0.0f ? 1.0f / pos->scale[0] : 0.0f;
  float invS1 = pos->scale[1] > 0.0f ? 1.0f / pos->scale[1] : 0.0f;
  float invS2 = pos->scale[2] > 0.0f ? 1.0f / pos->scale[2] : 0.0f;

  mat4 r;
  glm_mat4_identity(r);
  r[0][0] = pos->localTransform[0][0] * invS0;
  r[0][1] = pos->localTransform[0][1] * invS0;
  r[0][2] = pos->localTransform[0][2] * invS0;
  r[1][0] = pos->localTransform[1][0] * invS1;
  r[1][1] = pos->localTransform[1][1] * invS1;
  r[1][2] = pos->localTransform[1][2] * invS1;
  r[2][0] = pos->localTransform[2][0] * invS2;
  r[2][1] = pos->localTransform[2][1] * invS2;
  r[2][2] = pos->localTransform[2][2] * invS2;

  // Extract XYZ euler angles (R = Rx * Ry * Rz, column-major)
  float sinBeta = glm_clamp(r[2][0], -1.0f, 1.0f);
  pos->rotation[1] = asinf(sinBeta);
  float cosBeta = cosf(pos->rotation[1]);

  if(fabsf(cosBeta) > 1e-6f) {
    pos->rotation[0] = atan2f(-r[2][1], r[2][2]);
    pos->rotation[2] = atan2f(-r[1][0], r[0][0]);
  } else {
    // Gimbal lock: pin Z to 0, recover X.
    pos->rotation[2] = 0.0f;
    pos->rotation[0] = (sinBeta > 0.0f)
      ? atan2f(r[0][1], r[1][1])
      : -atan2f(r[0][1], r[1][1]);
  }
}