//
// Created by jholder on 21.10.21.
//

#include <Eigen/Dense>
#include <catch2/catch.hpp>

#include "Rod2d.hpp"

TEST_CASE("Rods") {
    Rod2d sphere(1);
    Rod2d rod(2);
    SECTION("Checking translation") {
        rod.setPos(rod.getPos() + Eigen::Vector2d(1, -1));
        auto newPos = rod.getPos();
        REQUIRE(newPos[0] == 1);
        REQUIRE(newPos[1] == -1);
        REQUIRE(sphere.getDiff_Sqrt()(0, 0) == 1.0);
        REQUIRE(sphere.getDiff_Sqrt()(0, 1) == 0.0);
        REQUIRE(sphere.getDiff_Sqrt()(1, 0) == 0.0);
        REQUIRE(sphere.getDiff_Sqrt()(1, 1) == 1.0);
    }
    SECTION("Checking rotation") {
        SECTION("Forwards == e 90") {
            //
            //     =====
            //
            rod.setE({0, 1});  // Lab system is 90 degree rotated
            auto test = Eigen::Vector2d({1, 0});
            auto erg = (rod.getE_Base_matrix() * test).normalized();
            auto e = rod.getE();
            REQUIRE(erg.isApprox(e));
        }
        SECTION("Forwards == e 45") {
            //       o
            //     o
            //   o
            rod.setE(Eigen::Vector2d({1, 1})
                         .normalized());  // Lab system is 90 degree rotated
            auto test = Eigen::Vector2d({1, 0});
            auto erg = (rod.getE_Base_matrix() * test).normalized();
            auto e = rod.getE();
            REQUIRE(erg.isApprox(e));
        }
    }
    SECTION("Angle Stuff") {
        const Eigen::Vector2d unitVec = {1, 0};
        double f1 = GENERATE(take(10, random(-100, 100)));
        double f2 = GENERATE(take(10, random(0, 100)));
        sphere.setE(Eigen::Vector2d({f1, f2}).normalized());
        Eigen::Rotation2D rotMat(acos(unitVec.dot(sphere.getE())));
        CHECK(rotMat.toRotationMatrix()(0, 0) ==
              Approx(sphere.getE_Base_matrix()(0, 0)).epsilon(10e-10));
        CHECK(rotMat.toRotationMatrix()(1, 0) ==
              Approx(sphere.getE_Base_matrix()(1, 0)).epsilon(10e-10));
        CHECK(rotMat.toRotationMatrix()(0, 1) ==
              Approx(sphere.getE_Base_matrix()(0, 1)).epsilon(10e-10));
        CHECK(rotMat.toRotationMatrix()(1, 1) ==
              Approx(sphere.getE_Base_matrix()(1, 1)).epsilon(10e-10));
    }
}