#include "Base/Const/Units.h"
#include "Sample/HardParticle/Polyhedra.h"
#include "Sample/HardParticle/Sphere.h"
#include "Sample/Material/Vacuum.h"
#include "Sample/Particle/Compound.h"
#include "Sample/Particle/Particle.h"
#include "Sample/Particle/Rotations.h"
#include "Tests/GTestWrapper/google_test.h"
#include <memory>

//! Verifies that rotate and translate commute: rotation leaves the position
//! unchanged, so both call orders yield the same position and orientation.
TEST(ParticleAlignment, RotateTranslateCommute)
{
    const Vacuum material;
    const R3 position(1.0, 2.0, 3.0);
    const RotationY rotation(37 * Units::deg);

    Particle p1(material, Sphere(5.));
    p1.rotate(rotation);
    p1.translate(position);

    Particle p2(material, Sphere(5.));
    p2.translate(position);
    p2.rotate(rotation);

    EXPECT_EQ(p1.particlePosition(), position);
    EXPECT_EQ(p2.particlePosition(), position);
    const R3 probe(1.0, 0.0, 0.0);
    EXPECT_EQ(p1.particleRotation()->transformed(probe), p2.particleRotation()->transformed(probe));
}

//! Verifies that successive rotations compose in order of application,
//! about axes of the unrotated frame.
TEST(ParticleAlignment, SuccessiveRotationsCompose)
{
    const Vacuum material;
    Particle particle(material, Sphere(5.));
    particle.rotate(RotationZ(90 * Units::deg));
    particle.rotate(RotationX(90 * Units::deg));

    // (1,0,0) -> Rz -> (0,1,0) -> Rx -> (0,0,1)
    const R3 result = particle.particleRotation()->transformed(R3(1.0, 0.0, 0.0));
    EXPECT_NEAR(result.x(), 0.0, 1e-14);
    EXPECT_NEAR(result.y(), 0.0, 1e-14);
    EXPECT_NEAR(result.z(), 1.0, 1e-14);
}

//! Verifies that applyParentTransform rotates the particle together with its
//! position, as one rigid whole, and then translates it.
TEST(ParticleAlignment, ParentTransformRotatesPositionAsRigidWhole)
{
    const Vacuum material;
    Particle particle(material, Sphere(5.));
    particle.rotate(RotationZ(90 * Units::deg));
    particle.translate(R3(0.0, 0.0, 12.0));

    const RotationX parent_rotation(90 * Units::deg);
    particle.applyParentTransform(&parent_rotation, R3(1.0, 2.0, 3.0));

    // position: Rx(90deg)*(0,0,12) = (0,-12,0), then + (1,2,3)
    const R3 position = particle.particlePosition();
    EXPECT_NEAR(position.x(), 1.0, 1e-12);
    EXPECT_NEAR(position.y(), -10.0, 1e-12);
    EXPECT_NEAR(position.z(), 3.0, 1e-12);

    // orientation: (1,0,0) -> Rz -> (0,1,0) -> Rx -> (0,0,1)
    const R3 result = particle.particleRotation()->transformed(R3(1.0, 0.0, 0.0));
    EXPECT_NEAR(result.x(), 0.0, 1e-14);
    EXPECT_NEAR(result.y(), 0.0, 1e-14);
    EXPECT_NEAR(result.z(), 1.0, 1e-14);
}

//! Verifies that a rotated compound decomposes into components whose offsets
//! are rotated along with the compound, as one rigid whole.
TEST(ParticleAlignment, CompoundDecomposesAsRigidWhole)
{
    const Vacuum material;
    const Particle particle(material, Sphere(5.));
    Compound compound{};
    compound.addComponent(particle, R3(0.0, 0.0, 12.0));
    compound.rotate(RotationX(90 * Units::deg));
    compound.translate(R3(1.0, 2.0, 3.0));

    const auto components = compound.decompose();
    ASSERT_EQ(components.size(), 1u);
    const R3 position = components[0]->particlePosition();
    EXPECT_NEAR(position.x(), 1.0, 1e-12);
    EXPECT_NEAR(position.y(), -10.0, 1e-12);
    EXPECT_NEAR(position.z(), 3.0, 1e-12);
}

//! Verifies that a rotation propagates through a nested compound: the offset of
//! an inner component is rotated as one rigid whole across both compound levels,
//! and the leaf orientation composes outer-then-inner rotations.
TEST(ParticleAlignment, NestedCompoundDecomposesAsRigidWhole)
{
    const Vacuum material;
    const Particle leaf(material, Sphere(5.));

    Compound inner{};
    inner.addComponent(leaf, R3(4.0, 0.0, 0.0));
    inner.rotate(RotationZ(90 * Units::deg));

    Compound outer{};
    outer.addComponent(inner);
    outer.rotate(RotationX(90 * Units::deg));
    outer.translate(R3(1.0, 2.0, 3.0));

    const auto components = outer.decompose();
    ASSERT_EQ(components.size(), 1u);

    // position: (4,0,0) -> Rz(90) -> (0,4,0) -> Rx(90) -> (0,0,4), then + (1,2,3)
    const R3 position = components[0]->particlePosition();
    EXPECT_NEAR(position.x(), 1.0, 1e-12);
    EXPECT_NEAR(position.y(), 2.0, 1e-12);
    EXPECT_NEAR(position.z(), 7.0, 1e-12);

    // orientation: (1,0,0) -> Rz -> (0,1,0) -> Rx -> (0,0,1)
    const R3 result = components[0]->particleRotation()->transformed(R3(1.0, 0.0, 0.0));
    EXPECT_NEAR(result.x(), 0.0, 1e-12);
    EXPECT_NEAR(result.y(), 0.0, 1e-12);
    EXPECT_NEAR(result.z(), 1.0, 1e-12);
}

//! Verifies that applyParentTransform with a null rotation is a pure translation: the
//! position shifts by the translation and the orientation is left untouched.
TEST(ParticleAlignment, ParentTransformNullRotationIsPureTranslation)
{
    const Vacuum material;
    Particle particle(material, Sphere(5.));
    particle.rotate(RotationZ(40 * Units::deg));
    particle.translate(R3(1.0, 2.0, 3.0));

    const R3 probe(1.0, 0.0, 0.0);
    const R3 before = particle.particleRotation()->transformed(probe);

    particle.applyParentTransform(nullptr, R3(10.0, 20.0, 30.0));

    EXPECT_EQ(particle.particlePosition(), R3(11.0, 22.0, 33.0));
    EXPECT_EQ(particle.particleRotation()->transformed(probe), before);
}

//! Verifies that applyParentTransform with the identity rotation is a pure translation: the
//! position shifts by the translation and the child's existing orientation is preserved.
TEST(ParticleAlignment, ParentTransformIdentityRotationIsPureTranslation)
{
    const Vacuum material;
    Particle particle(material, Sphere(5.));
    particle.rotate(RotationZ(40 * Units::deg));
    particle.translate(R3(1.0, 2.0, 3.0));

    const R3 probe(1.0, 0.0, 0.0);
    const R3 before = particle.particleRotation()->transformed(probe);

    const IdentityRotation identity;
    particle.applyParentTransform(&identity, R3(10.0, 20.0, 30.0));

    EXPECT_EQ(particle.particlePosition(), R3(11.0, 22.0, 33.0));
    const R3 after = particle.particleRotation()->transformed(probe);
    EXPECT_NEAR(after.x(), before.x(), 1e-13);
    EXPECT_NEAR(after.y(), before.y(), 1e-13);
    EXPECT_NEAR(after.z(), before.z(), 1e-13);
}

//! Verifies that rotating by a rotation and then by its inverse restores the original
//! orientation. The tolerance is loose because composing rotations reconstructs them through
//! a matrix-to-Euler-angle extraction (newProduct -> newRotation), which is only accurate to
//! about 1e-7 for a generic Euler rotation.
TEST(ParticleAlignment, RotateThenInverseRestoresOrientation)
{
    const Vacuum material;
    Particle particle(material, Sphere(5.));
    const RotationEuler rotation(0.3, 0.4, 0.5);
    particle.rotate(rotation);
    const std::unique_ptr<Rotation> inverse(rotation.newInverse());
    particle.rotate(*inverse);

    const R3 probe(0.3, -0.7, 1.1);
    const R3 result = particle.particleRotation()->transformed(probe);
    EXPECT_NEAR(result.x(), probe.x(), 1e-6);
    EXPECT_NEAR(result.y(), probe.y(), 1e-6);
    EXPECT_NEAR(result.z(), probe.z(), 1e-6);
}

//! Verifies that rotating by the identity rotation leaves the orientation unchanged.
TEST(ParticleAlignment, RotateByIdentityLeavesOrientation)
{
    const Vacuum material;
    Particle particle(material, Sphere(5.));
    particle.rotate(RotationY(35 * Units::deg));
    const R3 probe(1.0, 0.0, 0.0);
    const R3 before = particle.particleRotation()->transformed(probe);

    particle.rotate(IdentityRotation());
    const R3 after = particle.particleRotation()->transformed(probe);
    EXPECT_NEAR(after.x(), before.x(), 1e-14);
    EXPECT_NEAR(after.y(), before.y(), 1e-14);
    EXPECT_NEAR(after.z(), before.z(), 1e-14);
}

//! Verifies the z-extent and height accessors of a translated particle: absolute extents
//! include the position, while intrinsic extents and heights do not. A Box occupies
//! [0, height] in its own frame, so the arithmetic is known exactly.
TEST(ParticleAlignment, ZExtentAccessorsUnderTranslation)
{
    const Vacuum material;
    Particle particle(material, Box(10.0, 6.0, 4.0));
    particle.translate(R3(7.0, 8.0, 10.0));

    EXPECT_DOUBLE_EQ(particle.bottomZ(), 10.0);
    EXPECT_DOUBLE_EQ(particle.topZ(), 14.0);
    EXPECT_DOUBLE_EQ(particle.centerZ(), 12.0);
    EXPECT_DOUBLE_EQ(particle.extensionZ(), 4.0);

    EXPECT_DOUBLE_EQ(particle.intrinsicBottomZ(), 0.0);
    EXPECT_DOUBLE_EQ(particle.intrinsicTopZ(), 4.0);
    EXPECT_DOUBLE_EQ(particle.intrinsicCenterZ(), 2.0);

    EXPECT_DOUBLE_EQ(particle.height(), 4.0);
    EXPECT_DOUBLE_EQ(particle.height_above_center(), 2.0);
    EXPECT_DOUBLE_EQ(particle.height_below_center(), 2.0);
}

//! Verifies that a 90 deg rotation about the y axis tips a box onto its side, so that its
//! intrinsic z extent (height) becomes the box's lateral x dimension. The detailed rotated
//! anchoring is covered exhaustively by ParticlePlacementTest; here we only check that the
//! height accessor tracks the rotation.
TEST(ParticleAlignment, HeightTracksRotation)
{
    const Vacuum material;
    Particle particle(material, Box(10.0, 6.0, 4.0));
    particle.rotate(RotationY(90 * Units::deg));
    EXPECT_NEAR(particle.height(), 10.0, 1e-12);
}
