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

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

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));
    }

}