// SPDX-FileCopyrightText: 2024 Filip Leonarski, Paul Scherrer Institute <filip.leonarski@psi.ch>
// SPDX-License-Identifier: GPL-3.0-only

#include <catch2/catch_all.hpp>
#include "../common/Coord.h"

TEST_CASE("Coord_Comparison", "[LinearAlgebra][Coord]") {
    Coord a{1, 2, 3};
    Coord b{3, 4, 5};

    CHECK(!(a == b));
}

TEST_CASE("Coord_Neg", "[LinearAlgebra][Coord]") {
    Coord a{1, 2, 3};

    auto neg = -a;
    CHECK(neg.x == -a.x);
    CHECK(neg.y == -a.y);
    CHECK(neg.z == -a.z);
}

TEST_CASE("Coord_Sum", "[LinearAlgebra][Coord]") {
    Coord a{1, 2, 3};
    Coord b{3, 4, 5};

    auto sum = a + b;
    CHECK(sum.x == a.x + b.x);
    CHECK(sum.y == a.y + b.y);
    CHECK(sum.z == a.z + b.z);

    CHECK(a + b == b + a);
}

TEST_CASE("Coord_diff", "[LinearAlgebra][Coord]") {
    Coord a{1, 2, 3};
    Coord b{3, 4, 5};

    auto diff = a - b;
    CHECK(diff.x == a.x - b.x);
    CHECK(diff.y == a.y - b.y);
    CHECK(diff.z == a.z - b.z);

    CHECK(a - b == -(b - a));
}

TEST_CASE("Coord_Scalar_mult", "[LinearAlgebra][Coord]") {
    Coord a{1, 2, 3};

    auto scalar_mult = 5 * a;
    CHECK(scalar_mult.x == 5 * a.x);
    CHECK(scalar_mult.y == 5 * a.y);
    CHECK(scalar_mult.z == 5 * a.z);
    CHECK(5 * a == a * 5);
}

TEST_CASE("Coord_Scalar_div", "[LinearAlgebra][Coord]") {
    Coord a{1, 2, 3};

    auto scalar_div = a / 4;
    CHECK(scalar_div.x == Catch::Approx(a.x * 0.25));
    CHECK(scalar_div.y == Catch::Approx(a.y * 0.25));
    CHECK(scalar_div.z == Catch::Approx(a.z * 0.25));
}

TEST_CASE("Coord_dot", "[LinearAlgebra][Coord]") {

    Coord a{1, 2, 3};
    Coord b{3, 4, 5};

    auto dot_mult = a * b;
    CHECK (dot_mult == a.x * b.x + a.y * b.y + a.z * b.z);
    CHECK (a * b == b * a);

    CHECK(Coord(1, 0, 0) % Coord(1, 0, 0) == Coord(0, 0, 0));
    CHECK(Coord(1, 0, 0) % Coord(0, 1, 0) == Coord(0, 0, 1));
    CHECK(Coord(0, 1, 0) % Coord(0, 0, 1) == Coord(1, 0, 0));
    CHECK(Coord(0, 0, 1) % Coord(1, 0, 0) == Coord(0, 1, 0));
}

TEST_CASE("Coord_vec_mult", "[LinearAlgebra][Coord]") {
    Coord a{1, 2, 3};
    Coord b{3, 4, 5};

    auto vec_mult = a % b;
    REQUIRE (a % b == -(b % a));

    CHECK(vec_mult.x == a.y * b.z - a.z * b.y);
    CHECK(vec_mult.y == a.z * b.x - a.x * b.z);
    CHECK(vec_mult.z == a.x * b.y - a.y * b.x);
}

TEST_CASE("Coord_operator_bracket","[LinearAlgebra][Coord]") {
    Coord a{1,2,3};
    REQUIRE(a[0] == 1);
    REQUIRE(a[1] == 2);
    REQUIRE(a[2] == 3);
    REQUIRE_THROWS(a[4]);
    REQUIRE_THROWS(a[-1]);
    REQUIRE_THROWS(a[56667]);
}

TEST_CASE("Coord_normalize","[LinearAlgebra][Coord]") {
    Coord a{4,0,0};
    REQUIRE(a.Length() == 4.0);
    REQUIRE(a.Normalize().Length() == Catch::Approx(1.0));
    REQUIRE(a.Normalize().x == Catch::Approx(1.0));
    REQUIRE(a.Normalize().y == 0.0);
    REQUIRE(a.Normalize().z == 0.0);

    Coord b{3, 4, 5};
    REQUIRE(b.Length() == Catch::Approx(sqrt(3*3+4*4+5*5)));
    REQUIRE(b.Normalize().Length() == Catch::Approx(1.0));
    REQUIRE(b.Normalize().x == Catch::Approx(3 /sqrt(3*3+4*4+5*5)));
    REQUIRE(b.Normalize().y == Catch::Approx(4 /sqrt(3*3+4*4+5*5)));
    REQUIRE(b.Normalize().z == Catch::Approx(5 /sqrt(3*3+4*4+5*5)));
}

TEST_CASE("RotMatrix_default") {
    Coord a{3.0f, 4.67f, 8.906f};
    RotMatrix r;

    Coord b = r * a;
    REQUIRE(b.x == a.x);
    REQUIRE(b.y == a.y);
    REQUIRE(b.z == a.z);
}

TEST_CASE("RotMatrix_cust") {
    Coord a{3.0f, 4.67f, 8.906f};

    RotMatrix r_90(M_PI/2.0f, {0,0,1}); // rotate 90 deg by z
    RotMatrix r_180(M_PI, {1,0,0}); // rotate 180 deg by x

    Coord b = r_180 * a;
    CHECK(b.x == Catch::Approx(a.x));
    CHECK(b.y == Catch::Approx(-a.y));
    CHECK(b.z == Catch::Approx(-a.z));

    Coord c = r_90 * a;
    CHECK(c.z == Catch::Approx(a.z));
    CHECK(c.x == Catch::Approx(-a.y));
    CHECK(c.y == Catch::Approx(a.x));
}

TEST_CASE("RotMatrix_invert") {
    Coord a{3.0f, 4.67f, 8.906f};

    RotMatrix matrix1(100.0f, {1.0, 0.0, 1.0});
    RotMatrix matrix2(50.0f, {0.0, 1.0, 0.001});

    RotMatrix matrix3 = matrix1 * matrix2;
    RotMatrix matrix3_t = matrix3.transpose();
    RotMatrix matrix3_i = matrix3.invert();

    Coord b = matrix3_t * (matrix3 * a);
    CHECK(b.x == Catch::Approx(a.x));
    CHECK(b.y == Catch::Approx(a.y));
    CHECK(b.z == Catch::Approx(a.z));

    Coord c = matrix3_i * (matrix3 * a);
    CHECK(c.x == Catch::Approx(a.x));
    CHECK(c.y == Catch::Approx(a.y));
    CHECK(c.z == Catch::Approx(a.z));

    Coord d = (matrix3 * matrix3_i) * a;
    CHECK(d.x == Catch::Approx(a.x));
    CHECK(d.y == Catch::Approx(a.y));
    CHECK(d.z == Catch::Approx(a.z));
}