#include "Base/Const/Units.h"
#include "Base/Vector/RotMatrix.h"
#include "Sample/Particle/Rotations.h"
#include "Tests/GTestWrapper/google_test.h"
#include <memory>

TEST(Rotation, XRotations)
{
    double angle = 1.0;
    RotationX rot(angle);
    auto rot_matrix = rot.rotMatrix();
    auto P_rot = std::unique_ptr<Rotation>(Rotation::newRotation(rot_matrix));
    auto* p_rot_cast = dynamic_cast<RotationX*>(P_rot.get());
    ASSERT_NE(p_rot_cast, nullptr);
    EXPECT_DOUBLE_EQ(p_rot_cast->angle(), angle);
}

TEST(Rotation, YRotations)
{
    double angle = 1.0;
    RotationY rot(angle);
    auto rot_matrix = rot.rotMatrix();
    auto P_rot = std::unique_ptr<Rotation>(Rotation::newRotation(rot_matrix));
    auto* p_rot_cast = dynamic_cast<RotationY*>(P_rot.get());
    ASSERT_NE(p_rot_cast, nullptr);
    EXPECT_DOUBLE_EQ(p_rot_cast->angle(), angle);
}

TEST(Rotation, ZRotations)
{
    double angle = 1.0;
    RotationZ rot(angle);
    auto rot_matrix = rot.rotMatrix();
    auto P_rot = std::unique_ptr<Rotation>(Rotation::newRotation(rot_matrix));
    auto* p_rot_cast = dynamic_cast<RotationZ*>(P_rot.get());
    ASSERT_NE(p_rot_cast, nullptr);
    EXPECT_DOUBLE_EQ(p_rot_cast->angle(), angle);
}

TEST(Rotation, EulerRotations)
{
    const double epsilon = 1e-12;
    double alpha = 1.0;
    double beta = 0.2;
    double gamma = -0.5;
    RotationEuler rot(alpha, beta, gamma);
    auto rot_matrix = rot.rotMatrix();
    auto P_rot = std::unique_ptr<Rotation>(Rotation::newRotation(rot_matrix));
    auto* p_rot_cast = dynamic_cast<RotationEuler*>(P_rot.get());
    ASSERT_NE(p_rot_cast, nullptr);
    EXPECT_NEAR(p_rot_cast->alpha(), alpha, epsilon);
    EXPECT_NEAR(p_rot_cast->beta(), beta, epsilon);
    EXPECT_NEAR(p_rot_cast->gamma(), gamma, epsilon);
}

//! Verifies that Rotation::transformed applies the elementary axis rotations with the
//! right-handed active convention, and that the identity rotation leaves a vector unchanged.
TEST(Rotation, TransformedAxisRotations)
{
    constexpr double eps = 1e-14;

    const R3 from_z = RotationZ(90 * Units::deg).transformed(R3(1, 0, 0)); // x -> y
    EXPECT_NEAR(from_z.x(), 0., eps);
    EXPECT_NEAR(from_z.y(), 1., eps);
    EXPECT_NEAR(from_z.z(), 0., eps);

    const R3 from_x = RotationX(90 * Units::deg).transformed(R3(0, 1, 0)); // y -> z
    EXPECT_NEAR(from_x.x(), 0., eps);
    EXPECT_NEAR(from_x.y(), 0., eps);
    EXPECT_NEAR(from_x.z(), 1., eps);

    const R3 from_y = RotationY(90 * Units::deg).transformed(R3(0, 0, 1)); // z -> x
    EXPECT_NEAR(from_y.x(), 1., eps);
    EXPECT_NEAR(from_y.y(), 0., eps);
    EXPECT_NEAR(from_y.z(), 0., eps);

    EXPECT_EQ(IdentityRotation().transformed(R3(1, 2, 3)), R3(1, 2, 3));
}

//! Verifies that newProduct concatenates rotations in the documented order (first the
//! right operand, then the left): newProduct(L,R) acts as L applied after R. Uses two
//! non-commuting rotations so that the order is observable.
TEST(Rotation, NewProductComposesInOrder)
{
    constexpr double eps = 1e-14;
    const RotationZ rz(90 * Units::deg);
    const RotationX rx(90 * Units::deg);
    const R3 v(1, 0, 0);

    // rx after rz: (1,0,0) -Rz-> (0,1,0) -Rx-> (0,0,1)
    const std::unique_ptr<Rotation> rx_after_rz(newProduct(rx, rz));
    const R3 a = rx_after_rz->transformed(v);
    const R3 a_ref = rx.transformed(rz.transformed(v));
    EXPECT_NEAR(a.x(), a_ref.x(), eps);
    EXPECT_NEAR(a.y(), a_ref.y(), eps);
    EXPECT_NEAR(a.z(), a_ref.z(), eps);
    EXPECT_NEAR(a.z(), 1., eps);

    // the opposite order gives a different result: (1,0,0) is fixed by Rx, then Rz -> (0,1,0)
    const std::unique_ptr<Rotation> rz_after_rx(newProduct(rz, rx));
    const R3 b = rz_after_rx->transformed(v);
    const R3 b_ref = rz.transformed(rx.transformed(v));
    EXPECT_NEAR(b.x(), b_ref.x(), eps);
    EXPECT_NEAR(b.y(), b_ref.y(), eps);
    EXPECT_NEAR(b.z(), b_ref.z(), eps);
    EXPECT_NEAR(b.y(), 1., eps);
}

//! Verifies that the product of two rotations about the same axis adds their angles and
//! is reconstructed as a rotation about that same axis.
TEST(Rotation, NewProductSameAxisAddsAngles)
{
    const std::unique_ptr<Rotation> prod(
        newProduct(RotationZ(30 * Units::deg), RotationZ(50 * Units::deg)));
    auto* as_z = dynamic_cast<RotationZ*>(prod.get());
    ASSERT_NE(as_z, nullptr);
    EXPECT_NEAR(as_z->angle(), 80 * Units::deg, 1e-12);
}

//! Verifies isIdentity for the identity rotation and for zero-angle rotations, and that a
//! nonzero rotation is not reported as identity.
TEST(Rotation, IsIdentity)
{
    EXPECT_TRUE(IdentityRotation().isIdentity());
    EXPECT_TRUE(RotationX(0.).isIdentity());
    EXPECT_TRUE(RotationEuler(0., 0., 0.).isIdentity());
    EXPECT_FALSE(RotationZ(1.).isIdentity());
}

//! Verifies zInvariant: rotations about the z axis (including identity and a pure-z Euler
//! rotation with beta=0) are z-invariant; rotations that tilt the z axis are not.
TEST(Rotation, ZInvariant)
{
    EXPECT_TRUE(RotationZ(0.7).zInvariant());
    EXPECT_TRUE(IdentityRotation().zInvariant());
    EXPECT_TRUE(RotationEuler(0.3, 0., 0.5).zInvariant());
    EXPECT_FALSE(RotationX(0.7).zInvariant());
    EXPECT_FALSE(RotationY(0.7).zInvariant());
    EXPECT_FALSE(RotationEuler(0.3, 0.4, 0.5).zInvariant());
}

//! Verifies that newInverse yields the inverse rotation: applying a rotation and then its
//! inverse restores the original vector, for axis and Euler rotations alike.
TEST(Rotation, NewInverseRoundTrip)
{
    constexpr double eps = 1e-13;
    const R3 v(0.3, -0.7, 1.1);
    auto roundtrip = [&](const Rotation& rot) {
        const std::unique_ptr<Rotation> inv(rot.newInverse());
        const R3 w = inv->transformed(rot.transformed(v));
        EXPECT_NEAR(w.x(), v.x(), eps);
        EXPECT_NEAR(w.y(), v.y(), eps);
        EXPECT_NEAR(w.z(), v.z(), eps);
    };
    roundtrip(RotationX(0.7));
    roundtrip(RotationY(-1.2));
    roundtrip(RotationZ(2.5));
    roundtrip(RotationEuler(0.3, 0.4, 0.5));
}

//! Verifies that the inverse of an axis rotation is the same axis rotation with the
//! negated angle.
TEST(Rotation, NewInverseNegatesAxisAngle)
{
    const RotationZ rz(0.9);
    const std::unique_ptr<Rotation> inv(rz.newInverse());
    auto* as_z = dynamic_cast<RotationZ*>(inv.get());
    ASSERT_NE(as_z, nullptr);
    EXPECT_DOUBLE_EQ(as_z->angle(), -0.9);
}

//! Verifies that newRotation reconstructs the identity rotation from an identity matrix.
TEST(Rotation, NewRotationFromIdentityMatrix)
{
    const std::unique_ptr<Rotation> rot(Rotation::newRotation(RotMatrix{}));
    EXPECT_NE(dynamic_cast<IdentityRotation*>(rot.get()), nullptr);
    EXPECT_TRUE(rot->isIdentity());
}
