#include "cframe.h"
#include "datatypes/vector.h"
#include "physics/util.h"
#include <glm/ext/matrix_transform.hpp>
#include <reactphysics3d/mathematics/Transform.h>
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/euler_angles.hpp>
// #include "meta.h" // IWYU pragma: keep

Data::CFrame::CFrame(float x, float y, float z, float R00, float R01, float R02, float R10, float R11, float R12, float R20, float R21, float R22)
    : translation(x, y, z)
    , rotation({
        // { R00, R01, R02 },
        // { R10, R11, R12 },
        // { R20, R21, R22 },
        { R00, R10, R20 },
        { R01, R11, R21 },
        { R02, R12, R22 },
    }) {
}

Data::CFrame::CFrame(glm::vec3 translation, glm::mat3 rotation)
    : translation(translation)
    , rotation(rotation) {
}

Data::CFrame::CFrame(Data::Vector3 position, glm::quat quat)
    : translation(position)
    , rotation(quat) {
}

Data::CFrame::CFrame(const rp::Transform& transform) : Data::CFrame::CFrame(rpToGlm(transform.getPosition()), rpToGlm(transform.getOrientation())) {
}

glm::mat3 lookAt(Data::Vector3 position, Data::Vector3 lookAt, Data::Vector3 up) {
    // https://github.com/sgorsten/glm/issues/29#issuecomment-743989030
	Data::Vector3 f = (lookAt - position).Unit(); // Forward/Look
	Data::Vector3 u = up.Unit(); // Up
	Data::Vector3 s = f.Cross(u).Unit(); // Right
	u = s.Cross(u);

	return {
		{ s.X(), u.X(), -f.X() },
		{ s.Y(), u.Y(), -f.Y() },
		{ s.Z(), u.Z(), -f.Z() },
    };
}

Data::CFrame::CFrame(Data::Vector3 position, Data::Vector3 lookAt, Data::Vector3 up)
    : translation(position)
    , rotation(::lookAt(position, lookAt, up)) {
}

Data::CFrame::~CFrame() = default;
const Data::TypeInfo Data::CFrame::TYPE = {
    .name = "CoordinateFrame",
    .deserializer = &Data::CFrame::Deserialize,
};

const Data::TypeInfo& Data::CFrame::GetType() const { return Data::Vector3::TYPE; };

const Data::String Data::CFrame::ToString() const {
    return std::to_string(X()) + ", " + std::to_string(Y()) + ", " + std::to_string(Z());
}

Data::CFrame::operator glm::mat4() const {
    // Always make sure to translate the position first, then rotate. Matrices work backwards
    return glm::translate(glm::mat4(1.0f), this->translation) * glm::mat4(this->rotation);
}

Data::CFrame::operator rp::Transform() const {
    return rp::Transform(glmToRp(translation), glmToRp(rotation));
}

Data::Vector3 Data::CFrame::ToEulerAnglesXYZ() {
    float x;
    float y;
    float z;
    glm::extractEulerAngleXYZ(glm::mat4(this->rotation), x, y, z);
    return Data::Vector3(x, y, z);
}

Data::CFrame Data::CFrame::FromEulerAnglesXYZ(Data::Vector3 vector) {
    glm::mat3 mat = glm::eulerAngleXYZ(vector.X(), vector.Y(), vector.Z());
    return Data::CFrame(Data::Vector3::ZERO, glm::column(mat, 2), (Data::Vector3)glm::column(mat, 1)); // Getting LookAt (3rd) and Up (2nd) vectors
}

// Operators

Data::CFrame Data::CFrame::operator +(Data::Vector3 vector) const {
    return CFrame { this->translation + glm::vec3(vector), this->rotation };
}

Data::CFrame Data::CFrame::operator -(Data::Vector3 vector) const {
    return *this + -vector;
}

// Serialization

void Data::CFrame::Serialize(pugi::xml_node* node) const {
    node->append_child("X").text().set(std::to_string(this->X()));
    node->append_child("Y").text().set(std::to_string(this->Y()));
    node->append_child("Z").text().set(std::to_string(this->Z()));
    node->append_child("R00").text().set(std::to_string(this->rotation[0][0]));
    node->append_child("R01").text().set(std::to_string(this->rotation[1][0]));
    node->append_child("R02").text().set(std::to_string(this->rotation[2][0]));
    node->append_child("R10").text().set(std::to_string(this->rotation[0][1]));
    node->append_child("R11").text().set(std::to_string(this->rotation[1][1]));
    node->append_child("R12").text().set(std::to_string(this->rotation[2][1]));
    node->append_child("R20").text().set(std::to_string(this->rotation[0][2]));
    node->append_child("R21").text().set(std::to_string(this->rotation[1][2]));
    node->append_child("R22").text().set(std::to_string(this->rotation[2][2]));
}


Data::Variant Data::CFrame::Deserialize(pugi::xml_node* node) {
    return Data::CFrame(
        node->child("X").text().as_float(),
        node->child("Y").text().as_float(),
        node->child("Z").text().as_float(),
        node->child("R00").text().as_float(),
        node->child("R01").text().as_float(),
        node->child("R02").text().as_float(),
        node->child("R10").text().as_float(),
        node->child("R11").text().as_float(),
        node->child("R12").text().as_float(),
        node->child("R20").text().as_float(),
        node->child("R21").text().as_float(),
        node->child("R22").text().as_float()
    );
}