#include <iostream>
#include <Camera.hpp>

#define _USE_MATH_DEFINES
#include <cmath>

constexpr double TOL_EQ_TEST = 1e-9;
constexpr double M_PI = 3.14159265358979323846;

static size_t num_tests    = 0;
static size_t num_tests_ok = 0;
static size_t num_tests_ko = 0;

#define PRINT_VAR(x) std::cout << #x << "\t= " << (x) << std::endl

// EXPECT_EQ macro
#define EXPECT_EQ(actual, expected) EXPECT_EQ_IMPL(actual, expected, __FILE__, __LINE__)
#define EXPECT_EQ_IMPL(actual, expected, file, line) \
    do { \
        num_tests++; \
        auto&& e = (expected); \
        auto&& a = (actual); \
        if (e != a) { \
            std::cout << file << ":" << line << " : " << "Expected: " << (#expected) << " = " << e << ", actual: " << (#actual) << " = "  << a << " " << std::endl; \
            num_tests_ko++; \
        } \
        else { \
            num_tests_ok++; \
        } \
    } while (false)

// EXPECT_DOUBLE_EQ macro
#define EXPECT_DOUBLE_EQ(actual, expected) EXPECT_DOUBLE_EQ_IMPL(actual, expected, __FILE__, __LINE__)
#define EXPECT_DOUBLE_EQ_IMPL(actual, expected, file, line) \
    do { \
        num_tests++; \
        double e = (expected); \
        double a = (actual); \
        if (std::abs(e - a) > TOL_EQ_TEST) { \
            std::cout << file << ":" << line << " : " << "Expected: " << (#expected) << " = " << e << ", actual: " << (#actual) << " = "  << a << " " << std::endl; \
            num_tests_ko++; \
        } \
        else { \
            num_tests_ok++; \
        } \
    } while (false)

// EXPECT_VEC_EQ macro
#define EXPECT_VEC_EQ(actual, expected) EXPECT_VEC_EQ_IMPL(actual, expected, __FILE__, __LINE__)
#define EXPECT_VEC_EQ_IMPL(actual, expected, file, line) \
    do { \
        for(unsigned int i = 0 ; i < (expected).size() ; i++) { \
            EXPECT_DOUBLE_EQ_IMPL((actual)[i], (expected)[i], file, line); \
        } \
    } while (false)

// EXPECT_MAT_EQ macro
#define EXPECT_MAT_EQ(actual, expected) EXPECT_MAT_EQ_IMPL(actual, expected, __FILE__, __LINE__)
#define EXPECT_MAT_EQ_IMPL(actual, expected, file, line) \
    do { \
        for(unsigned int i = 0 ; i < (expected).rows() ; i++) { \
            for(unsigned int j = 0 ; j < (expected).rows() ; j++) { \
                EXPECT_DOUBLE_EQ_IMPL((actual)(i,j), (expected)(i,j), file, line); \
            } \
        } \
    } while (false)

#define PRINT_TESTS_SUMMARY() \
    do { \
        std::cout << "Tests summary: " << num_tests << " tests in total\n               " << num_tests_ok << "/" << num_tests << " OK (" << ((100.0*(double)num_tests_ok)/num_tests) << " %)\n               " << num_tests_ko << "/" << num_tests << " KO (" << ((100.0*(double)num_tests_ko)/num_tests) << " %)" << std::endl; \
    } while (false)

int main() {
    std::cout.precision(16);
    std::cout << "Running Unit Tests\n";
    
    {// Camera
        std::cout << "Camera\n";
        {// Constructor, accessors and setters
            std::cout << "Constructor, accessors and setters\n";
            
            // Create a camera object
            Camera camera;

            // Check constructor values using the getters
            Eigen::Matrix3d default_R;// Rotation matrix with default fwd and up vectors.
            default_R <<  0, 0, -1,
                         -1, 0,  0,
                          0, 1,  0;
            EXPECT_EQ(camera.GetForward(), Eigen::Vector3d(1, 0, 0));
            EXPECT_EQ(camera.GetUp(), Eigen::Vector3d(0, 0, 1));
            EXPECT_DOUBLE_EQ(camera.GetFOV(), 90.0*M_PI/180.0);
            EXPECT_MAT_EQ(camera.GetR(), default_R);
            EXPECT_MAT_EQ(camera.GetR_T(), default_R.transpose());

            // Check target computation
            Eigen::Vector3d target(1, 1, 1);
            camera.SetTarget(target);

            Eigen::Matrix3d target_R;
            target_R <<  0.7071067811865476, -0.4082482904638631, -0.5773502691896258,
                        -0.7071067811865476, -0.4082482904638631, -0.5773502691896258,
                        -0.                ,  0.8164965809277261, -0.5773502691896258;

            EXPECT_VEC_EQ(camera.GetForward(), Eigen::Vector3d(0.57735027, 0.57735027, 0.57735027));
            EXPECT_MAT_EQ(camera.GetR(), target_R);

            // Set some values using the setters
            Eigen::Vector3d fwd(1, 0, 0);
            Eigen::Vector3d up(0, 1, 0);
            Eigen::Matrix3d R;
            R << 0, 0, 1,
                 1, 0, 0,
                 0, 1, 0;
            double fov = M_PI / 3.0;

            camera.SetForward(fwd)
                  .SetUp(up)
                  .SetR(R)
                  .SetFOV(fov);

            // Check the values using the getters
            EXPECT_EQ(camera.GetForward(), fwd);
            EXPECT_EQ(camera.GetUp(), up);
            EXPECT_EQ(camera.GetR(), R);
            EXPECT_DOUBLE_EQ(camera.GetFOV(), fov);

            // Test point projections
            camera.SetFOV(20*DEG2RAD);
            camera.SetUp(Eigen::Vector3d(0, 0, 1));
            camera.SetTarget(Eigen::Vector3d(1.,.5,.3));
            
            EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d( 0.0,  0.0)), Eigen::Vector3d(0.0, 0.0, -1.0));
            EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d( 1.0,  0.0)), Eigen::Vector3d(0.1736481776669304,                    0, -0.9848077530122081));
            EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d( 1.0,  1.0)), Eigen::Vector3d(0.1710878697460355,   0.1710878697460355, -0.970287525247814));
            EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d( 1.0, -1.0)), Eigen::Vector3d(0.1710878697460355,  -0.1710878697460355, -0.970287525247814));
            EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(-1.0, -1.0)), Eigen::Vector3d(-0.1710878697460355, -0.1710878697460355, -0.970287525247814));

            EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(0.0, 0.0)), Eigen::Vector3d(0.86386842558136, 0.43193421279068, 0.259160527674408));
            EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, 0.0)), Eigen::Vector3d(0.9284021489814165, 0.2700565097745997, 0.2552232969284919));
            EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, 1.0)), Eigen::Vector3d(0.8750553718495241, 0.2462456324858795, 0.416702753385336));
            EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, -1.0)), Eigen::Vector3d(0.9543717844957083, 0.2859038388089716, 0.08621770069290194));
            EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(-1.0, -1.0)), Eigen::Vector3d(0.8013461417446023, 0.5919551243111837, 0.08621770069290194));

            EXPECT_VEC_EQ(camera.ProjectPointToSensorCameraFrame(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(0.0, 0.0))), Eigen::Vector3d(0.0, 0.0, -1.0/tan(camera.GetFOV()/2.0)));
            EXPECT_VEC_EQ(camera.ProjectPointToSensorCameraFrame(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(1.0, 1.0))), Eigen::Vector3d(1.0, 1.0, -1.0/tan(camera.GetFOV()/2.0)));
            EXPECT_VEC_EQ(camera.ProjectPointToSensorCameraFrame(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(-1.0, 0.5))), Eigen::Vector3d(-1.0, 0.5, -1.0/tan(camera.GetFOV()/2.0)));

            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.0,  0.0, -1.0/tan(camera.GetFOV()/2.0))), true);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.0,  0.0, -1.0/tan(camera.GetFOV()/2.0))), true);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.0,  1.0, -1.0/tan(camera.GetFOV()/2.0))), true);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.0, -1.0, -1.0/tan(camera.GetFOV()/2.0))), true);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d(-1.0, -1.0, -1.0/tan(camera.GetFOV()/2.0))), true);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d(-1.5, -0.5, -1.0/tan(camera.GetFOV()/2.0))), false);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.5, -0.5, -1.0/tan(camera.GetFOV()/2.0))), false);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.5, -1.5, -1.0/tan(camera.GetFOV()/2.0))), false);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.5,  1.5, -1.0/tan(camera.GetFOV()/2.0))), false);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.5, -0.5, +1.0/tan(camera.GetFOV()/2.0))), false);

            // Check static methods
            unsigned int width = 640, height = 480;
            EXPECT_VEC_EQ(Camera::PixelToNormalizedCoordinates(       0,       0, width, height), Eigen::Vector2d(-1.0, 1.0));
            EXPECT_VEC_EQ(Camera::PixelToNormalizedCoordinates(height-1,       0, width, height), Eigen::Vector2d(-1.0, -1.0));
            EXPECT_VEC_EQ(Camera::PixelToNormalizedCoordinates(height-1, width-1, width, height), Eigen::Vector2d(1.0, -1.0));
            EXPECT_VEC_EQ(Camera::PixelToNormalizedCoordinates(       0, width-1, width, height), Eigen::Vector2d(1.0, 1.0));
        }
    }

    PRINT_TESTS_SUMMARY();

    return 0;
}