#include "render/Camera.h"
#include <cmath>

namespace ZL {

    using Eigen::Vector3f;
    using Eigen::Matrix3f;
    using Eigen::Matrix4f;

    Camera::Camera()
        : position(Vector3f::Zero())
        , target(Vector3f::Zero())
        , rotation(Matrix3f::Identity())
    {
    }

    void Camera::followOrbit(const Vector3f& targetPos, float yaw, float pitch, float distance, float height)
    {
        target = targetPos;

        float cosP = std::cos(pitch);
        float sinP = std::sin(pitch);
        float cosY = std::cos(yaw);
        float sinY = std::sin(yaw);

        Vector3f offset;
        offset.x() = distance * sinY * cosP;
        offset.y() = height + distance * sinP;
        offset.z() = distance * cosY * cosP;

        position = target + offset;

        // Build camera basis to look at target
        Vector3f forward = (target - position).normalized();
        Vector3f worldUp(0.0f, 1.0f, 0.0f);

        Vector3f right = worldUp.cross(forward).normalized();
        Vector3f up = forward.cross(right);

        rotation.col(0) = right;
        rotation.col(1) = up;
        rotation.col(2) = -forward; // OpenGL convention
    }

    Matrix4f Camera::getViewMatrix() const
    {
        Matrix3f R_inv = rotation.transpose();

        Matrix4f view = Matrix4f::Identity();
        view.block<3, 3>(0, 0) = R_inv;
        view.block<3, 1>(0, 3) = R_inv * (-position);
        return view;
    }

} // namespace ZL