#include <iostream>
#include <Camera.hpp>
#include <Image.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));
        }
    }

    {// Image
        std::cout << "Image\n";
        {
            std::cout << "Constructors\n";
            Image image1(640, 480);
            EXPECT_EQ(image1.GetWidth(), 640);
            EXPECT_EQ(image1.GetHeight(), 480);
            EXPECT_EQ(image1.GetDepth(), 1);

            Image image2(1080, 720, 3);
            EXPECT_EQ(image2.GetWidth(), 1080);
            EXPECT_EQ(image2.GetHeight(), 720);
            EXPECT_EQ(image2.GetDepth(), 3);
        }

        {
            std::cout << "Pixel accessors\n";
            Image image(20, 10, 3);
            image.SetPixelValue(5, 7, 0, 69);
            image.SetPixelValue(5, 7, 1, 100);
            image.SetPixelValue(5, 7, 2, 42);
            EXPECT_EQ(image.GetPixelValue(5, 7, 0), 69);
            EXPECT_EQ(image.GetPixelValue(5, 7, 1), 100);
            EXPECT_EQ(image.GetPixelValue(5, 7, 2), 42);

            // Test reference accessor
            image.PixelValue(5, 7, 0) = 13;
            image.PixelValue(5, 7, 1) = 37;
            image.PixelValue(5, 7, 2) = 74;
            EXPECT_EQ(image.PixelValue(5, 7, 0), 13);
            EXPECT_EQ(image.PixelValue(5, 7, 1), 37);
            EXPECT_EQ(image.PixelValue(5, 7, 2), 74);

            // Test Vector accessors
            EXPECT_EQ(image.GetPixel(5, 7), Eigen::Vector3i(13, 37, 74));
            EXPECT_EQ(image.GetPixel(Eigen::Vector2i(5, 7)), Eigen::Vector3i(13, 37, 74));

            image.SetPixel(5, 7, Eigen::Vector3i(1, 2, 3));
            EXPECT_EQ(image.GetPixel(5, 7), Eigen::Vector3i(1, 2, 3));
            
            image.SetPixel(Eigen::Vector2i(6, 8), Eigen::Vector3i(3, 2, 1));
            EXPECT_EQ(image.GetPixel(Eigen::Vector2i(6, 8)), Eigen::Vector3i(3, 2, 1));
        }

        {
            std::cout << "Test Fill(rgb)\n";
            Image image(3, 2, 3);
            image.Fill(Eigen::Vector3i(1, 2, 3));
            for (int i = 0; i < image.GetHeight(); ++i) {
                for (int j = 0; j < image.GetWidth(); ++j) {
                    EXPECT_EQ(image.GetPixel(i, j), Eigen::Vector3i(1, 2, 3));
                }
            }
        }

        {
            std::cout << "Test Fill(uchar)\n";
            Image image(3, 2, 3);
            image.Fill(69);
            for (int i = 0; i < image.GetHeight(); ++i) {
                for (int j = 0; j < image.GetWidth(); ++j) {
                    EXPECT_EQ(image.GetPixel(i, j), Eigen::Vector3i(69, 69, 69));
                }
            }
        }

        {
            std::cout << "Image loading\n";
            Image image("../test_img_4x3.png");
            EXPECT_EQ(image.GetWidth(), 4);
            EXPECT_EQ(image.GetHeight(), 3);
            EXPECT_EQ(image.GetDepth(), 3);

            for(int i = 0; i < image.GetHeight(); ++i) {
                for(int j = 0; j < image.GetWidth(); ++j) {
                    auto pixel = image.GetPixel(i, j);
                    std::cout << i << " " << j << " (" << pixel(0)/255.0 << ", " << pixel(1)/255.0 << ", " << pixel(2)/255.0 << ")\n";
                }
            }

            for(int i = 0 ; i < image.GetWidth()*image.GetHeight()*image.GetDepth() ; i++)
                std::cout << i << "\t" << (int)(image[i]) << "\t" << (image[i]/255.0) << "\n";
        }

        {
            std::cout << "Image saving\n";
            Image image(512, 256, 3);
            for(int i = 0; i < image.GetHeight(); ++i) {
                for(int j = 0; j < image.GetWidth(); ++j) {
                    image.SetPixel(i, j, Eigen::Vector3i(127, i%256, j%256));
                }
            }
            image.Save("test_img_512x256.png");
            image.Save("test_img_512x256.jpg");
            image.Save("test_img_512x256.bmp");
            image.Save("test_img_512x256.tga");
        }

        {
            std::cout << "Bilinear interpolation\n";
            Image image1(3, 3, 3);      // Source image
            Image image2(512, 512, 3);  // Interpolated image (nearest neighbor)
            Image image3(512, 512, 3);  // Interpolated image (bilinear)
            image1.SetPixel(0, 0, Eigen::Vector3i(255, 127, 0));
            image1.SetPixel(0, 1, Eigen::Vector3i(127, 192, 64));
            image1.SetPixel(0, 2, Eigen::Vector3i(100, 100, 50));
            image1.SetPixel(1, 0, Eigen::Vector3i(30, 0, 210));
            image1.SetPixel(1, 1, Eigen::Vector3i(192, 64, 0));
            image1.SetPixel(1, 2, Eigen::Vector3i(64, 150, 192));
            image1.SetPixel(2, 0, Eigen::Vector3i(100, 50, 150));
            image1.SetPixel(2, 1, Eigen::Vector3i(50, 120, 190));
            image1.SetPixel(2, 2, Eigen::Vector3i(240, 50, 15));

            for(int i = 0; i < image2.GetHeight(); ++i) {
                for(int j = 0; j < image2.GetWidth(); ++j) {
                    Eigen::Vector2d pos_float((double)i/image2.GetHeight()*(image1.GetHeight()-1), (double)j/image2.GetWidth()*(image1.GetWidth()-1));
                    image2.SetPixel(i, j, image1.GetPixelInterp(pos_float, Image::InterpMethod::NEAREST));
                    image3.SetPixel(i, j, image1.GetPixelInterp(pos_float, Image::InterpMethod::BILINEAR));
                }
            }

            image1.Save("interp_src_3x3.png");
            image2.Save("interp_nearest_512x512.png");
            image3.Save("interp_bilinear_512x512.png");
        }

        {
            std::cout << "Grayscale conversion\n";
            Image image("../venise.jpg");
            Image image_gray = image.Grayscale();
            Image image_luma = image.LumaREC709();
            image_gray.Save("venise_gray.jpg");
            image_luma.Save("venise_luma.jpg");
        }

        {
            std::cout << "Image normalization\n";
            Image image("../venise.jpg");
            image.HistogramNormalize(5);
            image.Save("venise_normalized.jpg");
        }
    }

    PRINT_TESTS_SUMMARY();

    return 0;
}