#include <gtest/gtest.h>

#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include <libutils/timer.h>
#include <phg/matching/gms_matcher.h>
#include <phg/sfm/fmatrix.h>
#include <phg/sfm/ematrix.h>
#include <phg/sfm/sfm_utils.h>
#include <phg/sfm/defines.h>
#include <Eigen/SVD>
#include <phg/sfm/triangulation.h>
#include <phg/sfm/resection.h>
#include <phg/utils/point_cloud_export.h>

#include "utils/test_utils.h"

namespace {

    void filterMatchesF(const std::vector<cv::DMatch> &matches, const std::vector<cv::KeyPoint> keypoints_query,
                        const std::vector<cv::KeyPoint> keypoints_train, const cv::Matx33d &F, std::vector<cv::DMatch> &result, double threshold_px)
    {
        result.clear();

        for (const cv::DMatch &match : matches) {
            cv::Vec2f pt1 = keypoints_query[match.queryIdx].pt;
            cv::Vec2f pt2 = keypoints_train[match.trainIdx].pt;

            if (phg::epipolarTest(pt1, pt2, F, threshold_px)) {
                result.push_back(match);
            }
        }
    }

    // Fundamental matrix has to be of rank 2. See Hartley & Zisserman, p.243
    bool checkFmatrixSpectralProperty(const matrix3d &Fcv)
    {
        Eigen::MatrixXd F;
        copy(Fcv, F);

        Eigen::JacobiSVD<Eigen::MatrixXd> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::VectorXd s = svd.singularValues();

        std::cout << "checkFmatrixSpectralProperty: s: " << s.transpose() << std::endl;

        double thresh = 1e10;
        return s[0] > thresh * s[2] && s[1] > thresh * s[2];
    }

    // Essential matrix has to be of rank 2, and two non-zero singular values have to be equal. See Hartley & Zisserman, p.257
    bool checkEmatrixSpectralProperty(const matrix3d &Fcv)
    {
        Eigen::MatrixXd F;
        copy(Fcv, F);

        Eigen::JacobiSVD<Eigen::MatrixXd> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::VectorXd s = svd.singularValues();

        std::cout << "checkEmatrixSpectralProperty: s: " << s.transpose() << std::endl;

        double thresh = 1e10;

        bool rank2 = s[0] > thresh * s[2] && s[1] > thresh * s[2];
        bool equal = (s[0] < (1.0 + thresh) * s[1]) && (s[1] < (1.0 + thresh) * s[0]);

        return rank2 && equal;
    }

    template <typename MAT>
    double matRMS(const MAT &a, const MAT &b)
    {
        MAT d = (a - b);
        d = d.mul(d);
        double rms = std::sqrt(cv::sum(d)[0] / (a.cols * a.rows));
        return rms;
    }

    vector3d relativeOrientationAngles(const matrix3d &R0, const vector3d &O0, const matrix3d &R1, const vector3d &O1)
    {
        vector3d a = R0 * vector3d{0, 0, 1};
        vector3d b = O0 - O1;
        vector3d c = R1 * vector3d{0, 0, 1};

        double norma = cv::norm(a);
        double normb = cv::norm(b);
        double normc = cv::norm(c);

        if (norma == 0 || normb == 0 || normc == 0) {
            throw std::runtime_error("norma == 0 || normb == 0 || normc == 0");
        }

        a /= norma;
        b /= normb;
        c /= normc;

        vector3d cos_vals;

        cos_vals[0] = a.dot(c);
        cos_vals[1] = a.dot(b);
        cos_vals[2] = b.dot(c);

        return cos_vals;
    }

}

#define TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps) \
EXPECT_FALSE(phg::epipolarTest(pt0, pt1, F, std::max(0.0, t - eps))); \
EXPECT_TRUE(phg::epipolarTest(pt0, pt1, F, t + eps));

TEST (SFM, EpipolarDist) {

    const vector2d pt0 = {0, 0};
    const double eps = 1e-5;

    {
        // line: y = 0
        const double l[3] = {0, 1, 0};
        const matrix3d F = {0, 0, l[0], 0, 0, l[1], 0, 0, l[2]};

        vector2d pt1;
        double t;

        pt1 = {0, 0};
        t = 0;
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)

        pt1 = {1000, 0};
        t = 0;
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)

        pt1 = {0, 1000};
        t = 1000;
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)
    }

    {
        // line: y = x
        const double l[3] = {1, -1, 0};
        const matrix3d F = {0, 0, l[0], 0, 0, l[1], 0, 0, l[2]};

        vector2d pt1;
        double t;

        pt1 = {0, 0};
        t = 0;
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)

        pt1 = {1, 1};
        t = 0;
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)

        pt1 = {-1, -1};
        t = 0;
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)

        pt1 = {-1, 1};
        t = std::sqrt(2);
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)

        pt1 = {10, 0};
        t = 10 / std::sqrt(2);
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)
    }

    {
        // line: y = x + 1
        const double l[3] = {1, -1, 1};
        const matrix3d F = {0, 0, l[0], 0, 0, l[1], 0, 0, l[2]};

        vector2d pt1;
        double t;

        pt1 = {0, 1};
        t = 0;
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)

        pt1 = {1, 2};
        t = 0;
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)

        pt1 = {-1, 0};
        t = 0;
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)

        pt1 = {-1, 2};
        t = std::sqrt(2);
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)

        pt1 = {10, 1};
        t = 10 / std::sqrt(2);
        TEST_EPIPOLAR_LINE(pt0, pt1, F, t, eps)
    }
}

TEST (SFM, FmatrixSimple) {

    std::vector<cv::Vec2d> pts0, pts1;
    std::srand(1);
    for (int i = 0; i < 8; ++i) {
        pts0.push_back({(double) (std::rand() % 100), (double) (std::rand() % 100)});
        pts1.push_back({(double) (std::rand() % 100), (double) (std::rand() % 100)});
    }

    matrix3d F = phg::findFMatrix(pts0, pts1);
    matrix3d Fcv = phg::findFMatrixCV(pts0, pts1);

    EXPECT_TRUE(checkFmatrixSpectralProperty(F));
    EXPECT_TRUE(checkFmatrixSpectralProperty(Fcv));
}

TEST (SFM, EmatrixSimple) {

    phg::Calibration calib(360, 240);
    std::cout << "EmatrixSimple: calib: \n" << calib.K() << std::endl;

    std::vector<cv::Vec2d> pts0, pts1;
    std::srand(1);
    for (int i = 0; i < 8; ++i) {
        pts0.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())});
        pts1.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())});
    }

    matrix3d F = phg::findFMatrix(pts0, pts1, 10);
    matrix3d E = phg::fmatrix2ematrix(F, calib, calib);

    EXPECT_TRUE(checkEmatrixSpectralProperty(E));
}

TEST (SFM, EmatrixDecomposeSimple) {

    phg::Calibration calib(360, 240);
    std::cout << "EmatrixSimple: calib: \n" << calib.K() << std::endl;

    std::vector<cv::Vec2d> pts0, pts1;
    std::srand(1);
    for (int i = 0; i < 8; ++i) {
        pts0.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())});
        pts1.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())});
    }

    matrix3d F = phg::findFMatrix(pts0, pts1, 10);
    matrix3d E = phg::fmatrix2ematrix(F, calib, calib);

    matrix34d P0, P1;
    phg::decomposeEMatrix(P0, P1, E, pts0, pts1, calib, calib);

    matrix3d R;
    R = P1.get_minor<3, 3>(0, 0);
    vector3d T;
    T(0) = P1(0, 3);
    T(1) = P1(1, 3);
    T(2) = P1(2, 3);

    matrix3d E1 = phg::composeEMatrixRT(R, T);
    matrix3d E2 = phg::composeFMatrix(P0, P1);

    EXPECT_NE(E(2, 2), 0);
    EXPECT_NE(E1(2, 2), 0);
    EXPECT_NE(E2(2, 2), 0);

    E /= E(2, 2);
    E1 /= E1(2, 2);
    E2 /= E2(2, 2);

    double rms1 = matRMS(E, E1);
    double rms2 = matRMS(E, E2);
    double rms3 = matRMS(E1, E2);

    std::cout << "E: \n" << E << std::endl;
    std::cout << "E1: \n" << E1 << std::endl;
    std::cout << "E2: \n" << E2 << std::endl;
    std::cout << "RMS1: " << rms1 << std::endl;
    std::cout << "RMS2: " << rms2 << std::endl;
    std::cout << "RMS3: " << rms3 << std::endl;

    double eps = 1e-10;
    EXPECT_LT(rms1, eps);
    EXPECT_LT(rms2, eps);
    EXPECT_LT(rms3, eps);
}

TEST (SFM, TriangulationSimple) {

    vector4d X = {0, 0, 2, 1};

    matrix34d P0 = matrix34d::eye();
    vector3d x0 = {0, 0, 1};

    // P1
    vector3d O = {2, 0, 0};
    double alpha = M_PI_4;
    double s = std::sin(alpha);
    double c = std::cos(alpha);
    matrix3d R = { c, 0, s,
                   0, 1, 0,
                  -s, 0, c};
    vector3d T = -R * O;
    matrix34d P1 = {
             R(0, 0), R(0, 1), R(0, 2), T[0],
             R(1, 0), R(1, 1), R(1, 2), T[1],
             R(2, 0), R(2, 1), R(2, 2), T[2]
    };

    // x1
    vector3d x1 = {0, 0, 1};

    std::cout << "P1:\n" << P1 << std::endl;
    std::cout << "x2:\n" << P0 * X << std::endl;
    std::cout << "x3:\n" << P1 * X << std::endl;

    matrix34d Ps[2] = {P0, P1};
    vector3d xs[2] = {x0, x1};

    vector4d X1 = phg::triangulatePoint(Ps, xs, 2);
    std::cout << "X1:\n" << X1 << std::endl;

    EXPECT_NE(X1[3], 0);
    X1 /= X1[3];

    vector4d d = X - X1;
    std::cout << "|X - X1| = " << cv::norm(d) << std::endl;

    double eps = 1e-10;
    EXPECT_LT(cv::norm(d), eps);
}

TEST (SFM, FmatrixMatchFiltering) {

    using namespace cv;

    cv::Mat img1 = cv::imread("data/src/test_sfm/saharov/IMG_3023.JPG");
    cv::Mat img2 = cv::imread("data/src/test_sfm/saharov/IMG_3024.JPG");

    std::cout << "detecting points..." << std::endl;
    cv::Ptr<cv::FeatureDetector> detector = cv::SIFT::create();
    std::vector<cv::KeyPoint> keypoints1, keypoints2;
    cv::Mat descriptors1, descriptors2;
    detector->detectAndCompute( img1, cv::noArray(), keypoints1, descriptors1 );
    detector->detectAndCompute( img2, cv::noArray(), keypoints2, descriptors2 );

    std::cout << "matching points..." << std::endl;
    std::vector<std::vector<DMatch>> knn_matches;

    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED);
    matcher->knnMatch( descriptors1, descriptors2, knn_matches, 2 );

    std::vector<DMatch> good_matches(knn_matches.size());
    for (int i = 0; i < (int) knn_matches.size(); ++i) {
        good_matches[i] = knn_matches[i][0];
    }

    std::cout << "filtering matches GMS..." << std::endl;
    std::vector<DMatch> good_matches_gms;
    phg::filterMatchesGMS(good_matches, keypoints1, keypoints2, img1.size(), img2.size(), good_matches_gms);

    std::cout << "filtering matches F..." << std::endl;
    std::vector<DMatch> good_matches_gms_plus_f;
    std::vector<DMatch> good_matches_f;
    double threshold_px = 3;
    {
        std::vector<cv::Vec2d> points1, points2;
        for (const cv::DMatch &match : good_matches) {
            cv::Vec2f pt1 = keypoints1[match.queryIdx].pt;
            cv::Vec2f pt2 = keypoints2[match.trainIdx].pt;
            points1.push_back(pt1);
            points2.push_back(pt2);
        }
        matrix3d F = phg::findFMatrix(points1, points2, threshold_px);
        filterMatchesF(good_matches, keypoints1, keypoints2, F, good_matches_f, threshold_px);
    }
    {
        std::vector<cv::Vec2d> points1, points2;
        for (const cv::DMatch &match : good_matches_gms) {
            cv::Vec2f pt1 = keypoints1[match.queryIdx].pt;
            cv::Vec2f pt2 = keypoints2[match.trainIdx].pt;
            points1.push_back(pt1);
            points2.push_back(pt2);
        }
        matrix3d F = phg::findFMatrix(points1, points2, threshold_px);
        filterMatchesF(good_matches_gms, keypoints1, keypoints2, F, good_matches_gms_plus_f, threshold_px);
    }

    drawMatches(img1, img2, keypoints1, keypoints2, good_matches_gms, "data/debug/test_sfm/matches_GMS.jpg");
    drawMatches(img1, img2, keypoints1, keypoints2, good_matches_f, "data/debug/test_sfm/matches_F.jpg");
    drawMatches(img1, img2, keypoints1, keypoints2, good_matches_gms_plus_f, "data/debug/test_sfm/matches_GMS_plus_F.jpg");

    std::cout << "n matches gms: " << good_matches_gms.size() << std::endl;
    std::cout << "n matches F: " << good_matches_f.size() << std::endl;
    std::cout << "n matches gms + F: " << good_matches_gms_plus_f.size() << std::endl;

    EXPECT_GT(good_matches_gms_plus_f.size(), 0.5 * good_matches_gms.size());
    EXPECT_GT(good_matches_f.size(), 0.5 * good_matches_gms.size());

    EXPECT_GT(good_matches_f.size(), 0.5 * good_matches_gms_plus_f.size());
    EXPECT_GT(good_matches_gms_plus_f.size(), 0.5 * good_matches_f.size());
}

namespace {

    void transform(matrix3d &R, vector3d &O)
    {
        matrix4d H = matrix4d::diag({1, -1, -1, 1});
        matrix3d Rinv = H.inv().get_minor<3, 3>(0, 0);

        auto tmp = H * vector4d({O[0], O[1], O[2], 1.0});
        O = {tmp[0] / tmp[3], tmp[1] / tmp[3], tmp[2] / tmp[3]};
        R = R * Rinv;
    }

}

TEST (SFM, RelativePosition2View) {

    using namespace cv;

    const cv::Mat img1 = cv::imread("data/src/test_sfm/saharov/IMG_3023.JPG");
    const cv::Mat img2 = cv::imread("data/src/test_sfm/saharov/IMG_3024.JPG");

    const phg::Calibration calib0(img1.cols, img1.rows);
    const phg::Calibration calib1(img2.cols, img2.rows);

    std::cout << "detecting points..." << std::endl;
    cv::Ptr<cv::FeatureDetector> detector = cv::SIFT::create();
    std::vector<cv::KeyPoint> keypoints1, keypoints2;
    cv::Mat descriptors1, descriptors2;
    detector->detectAndCompute( img1, cv::noArray(), keypoints1, descriptors1 );
    detector->detectAndCompute( img2, cv::noArray(), keypoints2, descriptors2 );

    std::cout << "matching points..." << std::endl;
    std::vector<std::vector<DMatch>> knn_matches;

    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED);
    matcher->knnMatch( descriptors1, descriptors2, knn_matches, 2 );

    std::vector<DMatch> good_matches(knn_matches.size());
    for (int i = 0; i < (int) knn_matches.size(); ++i) {
        good_matches[i] = knn_matches[i][0];
    }

    std::cout << "filtering matches GMS..." << std::endl;
    std::vector<DMatch> good_matches_gms;
    phg::filterMatchesGMS(good_matches, keypoints1, keypoints2, img1.size(), img2.size(), good_matches_gms);

    std::vector<cv::Vec2d> points1, points2;
    for (const cv::DMatch &match : good_matches_gms) {
        cv::Vec2f pt1 = keypoints1[match.queryIdx].pt;
        cv::Vec2f pt2 = keypoints2[match.trainIdx].pt;
        points1.push_back(pt1);
        points2.push_back(pt2);
    }

    matrix3d F = phg::findFMatrix(points1, points2);
    matrix3d E = phg::fmatrix2ematrix(F, calib0, calib1);

    matrix34d P0, P1;
    phg::decomposeEMatrix(P0, P1, E, points1, points2, calib0, calib1);

    matrix3d R0, R1;
    vector3d O0, O1;
    phg::decomposeUndistortedPMatrix(R0, O0, P0);
    phg::decomposeUndistortedPMatrix(R1, O1, P1);
    transform(R0, O0);
    transform(R1, O1);
    P0 = phg::composeCameraMatrixRO(R0, O0);
    P1 = phg::composeCameraMatrixRO(R1, O1);

    std::cout << "Camera positions: " << std::endl;
    std::cout << "R0:\n" << R0 << std::endl;
    std::cout << "O0: " << O0.t() << std::endl;
    std::cout << "R1:\n" << R1 << std::endl;
    std::cout << "O1: " << O1.t() << std::endl;

    {
        vector3d relative_cos_vals = relativeOrientationAngles(R0, O0, R1, O1);
        std::cout << "relative_cos_vals: " << relative_cos_vals << std::endl;
        vector3d relative_cos_vals_expected = {0.966827, -0.141921, 0.115634};
        EXPECT_LT(cv::norm(relative_cos_vals - relative_cos_vals_expected), 0.05);
    }

    std::cout << "exporting point cloud..." << std::endl;
    std::vector<vector3d> point_cloud;
    std::vector<cv::Vec3b> point_cloud_colors;

    matrix34d Ps[2] = {P0, P1};
    for (int i = 0; i < (int) good_matches_gms.size(); ++i) {
        vector3d ms[2] = {calib0.unproject(points1[i]), calib1.unproject(points2[i])};
        vector4d X = phg::triangulatePoint(Ps, ms, 2);

        if (X(3) == 0) {
            std::cerr << "infinite point" << std::endl;
            continue;
        }

        point_cloud.push_back(vector3d{X(0) / X(3), X(1) / X(3), X(2) / X(3)});
        point_cloud_colors.push_back(img1.at<cv::Vec3b>(points1[i][1], points1[i][0]));
    }

    point_cloud.push_back(O0);
    point_cloud_colors.push_back(cv::Vec3b{0, 0, 255});
    point_cloud.push_back(O0 + R0 * cv::Vec3d(0, 0, 1));
    point_cloud_colors.push_back(cv::Vec3b(255, 0, 0));

    point_cloud.push_back(O1);
    point_cloud_colors.push_back(cv::Vec3b{0, 0, 255});
    point_cloud.push_back(O1 + R1 * cv::Vec3d(0, 0, 1));
    point_cloud_colors.push_back(cv::Vec3b(255, 0, 0));

    std::cout << "exporting " << point_cloud.size() << " points..." << std::endl;
    phg::exportPointCloud(point_cloud, "data/debug/test_sfm/point_cloud_2_cameras.ply", point_cloud_colors);
}

TEST (SFM, Resection) {

    using namespace cv;

    const cv::Mat img1 = cv::imread("data/src/test_sfm/saharov/IMG_3023.JPG");
    const cv::Mat img2 = cv::imread("data/src/test_sfm/saharov/IMG_3024.JPG");

    const phg::Calibration calib0(img1.cols, img1.rows);
    const phg::Calibration calib1(img2.cols, img2.rows);

    std::cout << "detecting points..." << std::endl;
    cv::Ptr<cv::FeatureDetector> detector = cv::SIFT::create();
    std::vector<cv::KeyPoint> keypoints1, keypoints2;
    cv::Mat descriptors1, descriptors2;
    detector->detectAndCompute( img1, cv::noArray(), keypoints1, descriptors1 );
    detector->detectAndCompute( img2, cv::noArray(), keypoints2, descriptors2 );

    std::cout << "matching points..." << std::endl;
    std::vector<std::vector<DMatch>> knn_matches;

    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED);
    matcher->knnMatch( descriptors1, descriptors2, knn_matches, 2 );

    std::vector<DMatch> good_matches(knn_matches.size());
    for (int i = 0; i < (int) knn_matches.size(); ++i) {
        good_matches[i] = knn_matches[i][0];
    }

    std::cout << "filtering matches GMS..." << std::endl;
    std::vector<DMatch> good_matches_gms;
    phg::filterMatchesGMS(good_matches, keypoints1, keypoints2, img1.size(), img2.size(), good_matches_gms);

    std::vector<cv::Vec2d> points1, points2;
    for (const cv::DMatch &match : good_matches_gms) {
        cv::Vec2f pt1 = keypoints1[match.queryIdx].pt;
        cv::Vec2f pt2 = keypoints2[match.trainIdx].pt;
        points1.push_back(pt1);
        points2.push_back(pt2);
    }

    matrix3d F = phg::findFMatrix(points1, points2);
    matrix3d E = phg::fmatrix2ematrix(F, calib0, calib1);

    matrix34d P0, P1;
    phg::decomposeEMatrix(P0, P1, E, points1, points2, calib0, calib1);

    matrix3d R0, R1;
    vector3d O0, O1;
    phg::decomposeUndistortedPMatrix(R0, O0, P0);
    phg::decomposeUndistortedPMatrix(R1, O1, P1);
    transform(R0, O0);
    transform(R1, O1);
    P0 = phg::composeCameraMatrixRO(R0, O0);
    P1 = phg::composeCameraMatrixRO(R1, O1);

    std::cout << "Camera positions: " << std::endl;
    std::cout << "R0:\n" << R0 << std::endl;
    std::cout << "O0: " << O0.t() << std::endl;
    std::cout << "R1:\n" << R1 << std::endl;
    std::cout << "O1: " << O1.t() << std::endl;

    std::vector<cv::Vec3d> Xs;
    std::vector<cv::Vec2d> x0s;
    std::vector<cv::Vec2d> x1s;

    matrix34d Ps[2] = {P0, P1};
    for (int i = 0; i < (int) good_matches_gms.size(); ++i) {
        vector3d ms[2] = {calib0.unproject(points1[i]), calib1.unproject(points2[i])};
        vector4d X = phg::triangulatePoint(Ps, ms, 2);

        if (X(3) == 0) {
            std::cerr << "infinite point" << std::endl;
            continue;
        }

        Xs.push_back(vector3d{X(0) / X(3), X(1) / X(3), X(2) / X(3)});
        x0s.push_back(points1[i]);
        x1s.push_back(points2[i]);
    }

    matrix34d P0res = phg::findCameraMatrix(calib0, Xs, x0s);
    matrix34d P1res = phg::findCameraMatrix(calib1, Xs, x1s);

    double rms0 = matRMS(P0res, P0);
    double rms1 = matRMS(P1res, P1);
    double rms2 = matRMS(P0, P1);

    EXPECT_LT(rms0, 0.005);
    EXPECT_LT(rms1, 0.005);
    EXPECT_LT(rms0, 0.05 * rms2);
    EXPECT_LT(rms1, 0.05 * rms2);
}

TEST (SFM, ReconstructNViews) {

    using namespace cv;

    std::vector<cv::Mat> imgs;
    imgs.push_back(cv::imread("data/src/test_sfm/saharov/IMG_3023.JPG"));
    imgs.push_back(cv::imread("data/src/test_sfm/saharov/IMG_3024.JPG"));
    imgs.push_back(cv::imread("data/src/test_sfm/saharov/IMG_3025.JPG"));

    std::vector<vector3d> expected_orientations;
    expected_orientations.push_back({0.966827, -0.141921, 0.115634});
    expected_orientations.push_back({0.972914, -0.0489595, 0.183026});

    std::vector<phg::Calibration> calibs;
    for (const auto &img : imgs) {
        calibs.push_back(phg::Calibration(img.cols, img.rows));
    }

    const int n_imgs = imgs.size();

    std::cout << "detecting points..." << std::endl;
    std::vector<std::vector<cv::KeyPoint>> keypoints(n_imgs);
    std::vector<std::vector<int>> track_ids(n_imgs);
    std::vector<cv::Mat> descriptors(n_imgs);
    cv::Ptr<cv::FeatureDetector> detector = cv::SIFT::create();
    for (int i = 0; i < (int) imgs.size(); ++i) {
        detector->detectAndCompute(imgs[i], cv::noArray(), keypoints[i], descriptors[i]);
        track_ids[i].resize(keypoints[i].size(), -1);
    }

    std::cout << "matching points..." << std::endl;
    using Matches = std::vector<cv::DMatch>;
    std::vector<std::vector<Matches>> matches(n_imgs);
    for (int i = 0; i < n_imgs; ++i) {
        matches[i].resize(n_imgs);
        for (int j = 0; j < n_imgs; ++j) {
            if (i == j) {
                continue;
            }

            std::vector<std::vector<DMatch>> knn_matches;
            std::cout << "flann matching..." << std::endl;
            Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED);
            matcher->knnMatch( descriptors[i], descriptors[j], knn_matches, 2 );
            std::vector<DMatch> good_matches(knn_matches.size());
            for (int k = 0; k < (int) knn_matches.size(); ++k) {
                good_matches[k] = knn_matches[k][0];
            }

            std::cout << "filtering matches GMS..." << std::endl;
            std::vector<DMatch> good_matches_gms;
            phg::filterMatchesGMS(good_matches, keypoints[i], keypoints[j], imgs[i].size(), imgs[j].size(), good_matches_gms);

            matches[i][j] = good_matches_gms;
        }
    }

    std::vector<phg::Track> tracks;
    std::vector<vector3d> tie_points;
    std::vector<matrix34d> cameras(n_imgs);
    std::vector<char> aligned(n_imgs);

    // align first two cameras
    {
        // matches from first to second image in specified sequence
        const Matches &good_matches_gms = matches[0][1];
        const std::vector<cv::KeyPoint> &keypoints0 = keypoints[0];
        const std::vector<cv::KeyPoint> &keypoints1 = keypoints[1];
        const phg::Calibration &calib0 = calibs[0];
        const phg::Calibration &calib1 = calibs[1];

        std::vector<cv::Vec2d> points0, points1;
        for (const cv::DMatch &match : good_matches_gms) {
            cv::Vec2f pt1 = keypoints0[match.queryIdx].pt;
            cv::Vec2f pt2 = keypoints1[match.trainIdx].pt;
            points0.push_back(pt1);
            points1.push_back(pt2);
        }

        matrix3d F = phg::findFMatrix(points0, points1);
        matrix3d E = phg::fmatrix2ematrix(F, calib0, calib1);

        matrix34d P0, P1;
        phg::decomposeEMatrix(P0, P1, E, points0, points1, calib0, calib1);

        {
            matrix3d R0, R1;
            vector3d O0, O1;
            phg::decomposeUndistortedPMatrix(R0, O0, P0);
            phg::decomposeUndistortedPMatrix(R1, O1, P1);
            transform(R0, O0);
            transform(R1, O1);
            P0 = phg::composeCameraMatrixRO(R0, O0);
            P1 = phg::composeCameraMatrixRO(R1, O1);
        }

        cameras[0] = P0;
        cameras[1] = P1;
        aligned[0] = true;
        aligned[1] = true;

        matrix34d Ps[2] = {P0, P1};
        for (int i = 0; i < (int) good_matches_gms.size(); ++i) {
            vector3d ms[2] = {calib0.unproject(points0[i]), calib1.unproject(points1[i])};
            vector4d X = phg::triangulatePoint(Ps, ms, 2);

            if (X(3) == 0) {
                std::cerr << "infinite point" << std::endl;
                continue;
            }

            vector3d X3d{X(0) / X(3), X(1) / X(3), X(2) / X(3)};

            tie_points.push_back(X3d);

            phg::Track track;
            track.img_kpt_pairs.push_back({0, good_matches_gms[i].queryIdx});
            track.img_kpt_pairs.push_back({1, good_matches_gms[i].trainIdx});
            track_ids[0][good_matches_gms[i].queryIdx] = tracks.size();
            track_ids[1][good_matches_gms[i].trainIdx] = tracks.size();
            tracks.push_back(track);
        }
    }

    // append remaining cameras one by one
    for (int i_camera = 2; i_camera < n_imgs; ++i_camera) {

        const std::vector<cv::KeyPoint> &keypoints0 = keypoints[i_camera];
        const phg::Calibration &calib0 = calibs[i_camera];

        std::vector<vector3d> Xs;
        std::vector<vector2d> xs;
        for (int i_camera_prev = 0; i_camera_prev < i_camera; ++i_camera_prev) {
            const Matches &good_matches_gms = matches[i_camera][i_camera_prev];
            for (const cv::DMatch &match : good_matches_gms) {
                int track_id = track_ids[i_camera_prev][match.trainIdx];
                if (track_id != -1) {
                    Xs.push_back(tie_points[track_id]);
                    cv::Vec2f pt = keypoints0[match.queryIdx].pt;
                    xs.push_back(pt);
                }
            }
        }

        matrix34d P = phg::findCameraMatrix(calib0, Xs, xs);

        cameras[i_camera] = P;
        aligned[i_camera] = true;

        for (int i_camera_prev = 0; i_camera_prev < i_camera; ++i_camera_prev) {
            const std::vector<cv::KeyPoint> &keypoints1 = keypoints[i_camera_prev];
            const phg::Calibration &calib1 = calibs[i_camera_prev];
            const Matches &good_matches_gms = matches[i_camera][i_camera_prev];
            for (const cv::DMatch &match : good_matches_gms) {
                int track_id = track_ids[i_camera_prev][match.trainIdx];
                if (track_id == -1) {
                    matrix34d Ps[2] = {P, cameras[i_camera_prev]};
                    cv::Vec2f pts[2] = {keypoints0[match.queryIdx].pt, keypoints1[match.trainIdx].pt};
                    vector3d ms[2] = {calib0.unproject(pts[0]), calib1.unproject(pts[1])};
                    vector4d X = phg::triangulatePoint(Ps, ms, 2);

                    if (X(3) == 0) {
                        std::cerr << "infinite point" << std::endl;
                        continue;
                    }

                    tie_points.push_back({X(0) / X(3), X(1) / X(3), X(2) / X(3)});

                    phg::Track track;
                    track.img_kpt_pairs.push_back({i_camera, match.queryIdx});
                    track.img_kpt_pairs.push_back({i_camera_prev, match.trainIdx});
                    track_ids[i_camera][match.queryIdx] = tracks.size();
                    track_ids[i_camera_prev][match.trainIdx] = tracks.size();
                    tracks.push_back(track);
                } else {
                    phg::Track &track = tracks[track_id];
                    track.img_kpt_pairs.push_back({i_camera, match.queryIdx});
                    track_ids[i_camera][match.queryIdx] = track_id;
                }
            }
        }
    }

    if (tie_points.size() != tracks.size()) {
        throw std::runtime_error("tie_points.size() != tracks.size()");
    }

    std::vector<cv::Vec3b> tie_points_colors;
    for (int i = 0; i < (int) tie_points.size(); ++i) {
        const phg::Track &track = tracks[i];
        int img = track.img_kpt_pairs.front().first;
        int kpt = track.img_kpt_pairs.front().second;
        cv::Vec2f px = keypoints[img][kpt].pt;
        tie_points_colors.push_back(imgs[img].at<cv::Vec3b>(px[1], px[0]));
    }

    for (int i_camera = 0; i_camera < n_imgs; ++i_camera) {
        if (!aligned[i_camera]) {
            throw std::runtime_error("camera " + std::to_string(i_camera) + " is not aligned");
        }

        matrix3d R;
        vector3d O;
        phg::decomposeUndistortedPMatrix(R, O, cameras[i_camera]);

        tie_points.push_back(O);
        tie_points_colors.push_back(cv::Vec3b(0, 0, 255));
        tie_points.push_back(O + R * cv::Vec3d(0, 0, 1));
        tie_points_colors.push_back(cv::Vec3b(255, 0, 0));
    }

    for (int i = 1; i < n_imgs; ++i) {
        matrix3d R0, R1;
        vector3d O0, O1;
        phg::decomposeUndistortedPMatrix(R0, O0, cameras[i - 1]);
        phg::decomposeUndistortedPMatrix(R1, O1, cameras[i]);

        vector3d relative_cos_vals = relativeOrientationAngles(R0, O0, R1, O1);
        std::cout << "relative_cos_vals: " << relative_cos_vals << std::endl;
        vector3d relative_cos_vals_expected = expected_orientations[i - 1];
        EXPECT_LT(cv::norm(relative_cos_vals - relative_cos_vals_expected), 0.05);
    }

    std::cout << "exporting " << tie_points.size() << " points..." << std::endl;
    phg::exportPointCloud(tie_points, "data/debug/test_sfm/point_cloud_N_cameras.ply", tie_points_colors);

}


#include <Eigen/Dense>

double mse(Eigen::MatrixXd X, Eigen::VectorXd W, Eigen::VectorXd Y);

Eigen::VectorXd _derivative(Eigen::MatrixXd X, Eigen::VectorXd W,
                            Eigen::VectorXd Y) {
  /*
  Computes the jth derivative given W, X, Y
  by following gradient descent where
       D_j J(W,X) = (Y - X * W) * X_j
  */
  return -1 * X.transpose() * (Y - X * W);
}

Eigen::VectorXd _gradient_descent_step(Eigen::MatrixXd X, Eigen::VectorXd W,
                                       Eigen::VectorXd Y, double alpha) {
  /*
  Runs gradient descent for each row in the dataset X
  */
  Eigen::VectorXd W_temp = W;
  for (int j = 0; j < X.rows(); j++) {
    W_temp -= alpha * _derivative(X, W, Y);
  }

  return W_temp;
}

Eigen::VectorXd gradient_descent(Eigen::MatrixXd X, Eigen::MatrixXd Y,
                                 int *iter, bool *converged,
                                 int max_iterations, double threshold,
                                 double learning_rate) {
  Eigen::VectorXd W;
  /*
    Starting from a random set of weights W, use
    gradient descent to determine the optimal W. Stop
    when either the max iterations (not converged)
    or the error threshold (converged) is reached.
  */
  *converged = true;
  W = Eigen::MatrixXd::Random(X.cols(), 1);

  for (*iter = 0; *iter < max_iterations; *iter = *iter + 1) {
    W = _gradient_descent_step(X, W, Y, learning_rate);

    double error = mse(X, W, Y);
    if (error < threshold) {
      *converged = true;
      break;
    }
  }

  return W;
}


#include<iostream>
#include <bits/stdc++.h>
#include <boost/bind.hpp>
using namespace std;

struct point
{
    int x, y;
};

int orientation(point p, point q, point r)
{
    int val = (q.y - p.y) * (r.x - q.x) -
              (q.x - p.x) * (r.y - q.y);

    if (val == 0) return 0;  // colinear
    return (val > 0)? 1: 2; // clock or counterclock wise
}

int pointLocation(point A, point B, point P)
    {
        int cp1 = (B.x - A.x) * (P.y - A.y) - (B.y - A.y) * (P.x - A.x);
        if (cp1 > 0)
            return 1;
        else if (cp1 == 0)
            return 0;
        else
            return -1;
    }

 int distance(point A, point B, point C)
    {
        int ABx = B.x - A.x;
        int ABy = B.y - A.y;
        int num = ABx * (A.y - C.y) - ABy * (A.x - C.x);
        if (num < 0)
            num = -num;
        return num;
    }

  void hullSet(point A, point B, vector<point>set,vector<point>&hull)
  {
  	vector<point>::iterator it;
  	point alpha=B;
  	 it=find_if(hull.begin(),hull.end(),boost::bind (&point::x, _1 ) == alpha.x );

  	if (set.size() == 0)
            return;

    if (set.size() == 1)
        {
            point p = set[0];
            set.erase(set.begin()+0);
            hull.insert(it, p);
            return;
        }

    int dist = INT_MIN;
        int furthestPoint = -1;
        for (int i = 0; i <set.size(); i++)
        {
            point p = set[i];
            int distan = distance(A, B, p);
            if (distan > dist)
            {
                dist = distan;
                furthestPoint = i;
            }
        }

     point P = set[furthestPoint];
        set.erase(set.begin()+furthestPoint-1);
        hull.insert(it, P);

    // Determine who's to the left of AP
        vector<point>leftSetAP;
        for (int i = 0;i < set.size(); i++)
        {
            point M = set[i];
            if (pointLocation(A, P, M) == 1)
            {
                leftSetAP.push_back(M);
            }
        }

        // Determine who's to the left of PB
        vector<point>leftSetPB;
        for (int i = 0; i < set.size(); i++)
        {
            point M = set[i];
            if (pointLocation(P, B, M) == 1)
            {
                leftSetPB.push_back(M);
            }
        }
        hullSet(A, P, leftSetAP, hull);
        hullSet(P, B, leftSetPB, hull);

  }

void convexHull(point sample[], int n)
{

	if(n<3) return;
	// Initialize Result
    vector<point> hull;
    vector<point> Lefthull;
    vector<point> Righthull;

	 int minPoint = -1, maxPoint = -1;
        int minX = INT_MAX;
        int maxX = INT_MIN;
        for (int i=0;i<n; i++)
        {
            if (sample[i].x < minX)
            {
                minX = sample[i].x;
                minPoint = i;
            }
            if (sample[i].x > maxX)
            {
                maxX =sample[i].x;
                maxPoint = i;
            }
        }

         hull.push_back(sample[minPoint]);
         hull.push_back(sample[maxPoint]);

        point A = sample[minPoint];
        point B = sample[maxPoint];

          for(int i=0;i<n; i++)
        {
            point p = sample[i];
            if (pointLocation(A, B, p) == -1)
               Lefthull.push_back(p);
            else if (pointLocation(A, B, p) == 1)
                 Righthull.push_back(p);
        }

        hullSet(A, B, Righthull, hull);
        hullSet(B, A, Lefthull, hull);

    // Print Result
    for (int i = 0; i < hull.size(); i++)
        cout << "(" << hull[i].x << ", "
              << hull[i].y << ")\n";

}

int main() {
	 int N;
cout << "Enter no of points N "<<endl;
cin>>N;
point sample[N];

for (int i = 0; i < N; i++)
{
  cin>>sample[i].x>>sample[i].y;
}
//add timer here to calculate execution time
clock_t start;
double duration;
start = clock();

cout << "The points in the convex hull are: "<<endl;
convexHull(sample, N);

duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
cout<<"time: "<<duration <<" seconds"<<endl;
return 0;
}


#include <armadillo>

using namespace arma;

const static double π = 3.141592653589793238463;
const static float h = 0.74;
const static float l = 2.83;
const static float a = 0.95 + l;
const static float b = 0.50;

double measure(double θ){
  double tmp = std::fmod(θ,2*π);
  return tmp - ((int) (tmp/π))*2*π;
}

mat inverse(mat q){
  double d = q(0,0)*q(1,1)-q(0,1)*q(1,0);
  return {{q(1,1)/d,-q(0,1)/d},{-q(1,0)/d,q(0,0)/d}};
}

mat jacobian_motion(float θ, float v, float α, float dt){
  return {{1, 0, -dt*(v*std::sin(θ)+v/l*std::tan(α)*(a*std::cos(θ)-b*std::sin(θ)))},
          {0, 1,  dt*(v*std::cos(θ)-v/l*std::tan(α)*(a*std::sin(θ)+b*std::cos(θ)))},
          {0, 0,  1}};
}

vec equation_motion(float θ, float v1, float α, float dt){
  float v = v1/(1-std::tan(α)*h/l);
  return {dt*(v*std::cos(θ)-v/l*std::tan(α)*(a*std::sin(θ)+b*std::cos(θ))),
          dt*(v*std::sin(θ)+v/l*std::tan(α)*(a*std::cos(θ)-b*std::sin(θ))),
          dt*v/l*std::tan(α)};
}

mat jacobian_measurement(vec μ, vec m){
  //pb converting Col<uint> to uvec = Col<long long int>
  vec δ = m-μ(span(0,1));
  vec δ1 = sum(square(δ),0);
  double q = δ1(0);

  mat jcb = {{-δ(0),-δ(1),0,δ(0),δ(1)},
             {δ(1),-δ(0),-q,-δ(1),δ(0)}};
  jcb.row(0) *= (1/sqrt(q));
  jcb.row(1) *= (1/q);

  return jcb;
}

vec equation_measurement(vec μ, vec m){

  vec δ = m-μ(span(0,1));
  vec δ1 = sqrt(sum(square(δ),0));
  double q = δ1(0);
  double ϕ = atan2(δ(1),δ(0)) - μ(2) + π/2;
  return {q,ϕ};
}

mat inverse_measurement(vec μ, mat z){
  rowvec c = cos(z.row(1)+μ(2));
  rowvec s = sin(z.row(1)+μ(2));

  return mat(join_vert(z.row(0)%s,-z.row(0)%c)).each_col() + μ(span(0,1));
}


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2020, Aditya Mohan

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[dot_product
//` Calculate the dot product of two points

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/geometries/adapted/boost_array.hpp>

namespace bg = boost::geometry; /*< Convenient namespace alias >*/

BOOST_GEOMETRY_REGISTER_BOOST_ARRAY_CS(cs::cartesian)

int main()
{
     double dp1,dp2,dp3,dp4;
     bg::model::point<double, 3, bg::cs::cartesian> point1(1.0, 2.0, 3.0);
     bg::model::point<double, 3, bg::cs::cartesian> point2(4.0, 5.0, 6.0);

     //Example 1
     dp1 = bg::dot_product(point1, point2);

     std::cout << "Dot Product 1: "<< dp1 << std::endl;

     bg::model::point<double, 2, bg::cs::cartesian> point3(3.0, 2.0);
     bg::model::point<double, 2, bg::cs::cartesian> point4(4.0, 7.0);

     //Example 2
     dp2 = bg::dot_product(point3, point4);

     std::cout << "Dot Product 2: "<< dp2 << std::endl;

     boost::array<double, 2> a =  {1, 2};
     boost::array<double, 2> b =  {2, 3};

     //Example 3
     dp3 = bg::dot_product(a, b);

     std::cout << "Dot Product 3: "<< dp3 << std::endl;

     return 0;

}

//]

//[dot_product_output
/*`
Output:
[pre
Dot Product 1: 32
Dot Product 2: 26
Dot Product 3: 8
]
*/
//]


#include <iostream>
#include <cstdio>
#include <armadillo>
#include <ctime>
#include <unistd.h>

#include "comp_eig.hh"

using namespace std;
using namespace arma;

const double rmin = 0;
const double rmax = 6;
const double epsilon = 1e-10;

struct program_output {
  size_t steps;
  double step_time;
  double arma_time;
  double err;
};

// potential function
static constexpr double V(double r) {
  return r * r;
}

// calculate maximal off-diagonal element with respect to absolute value
// return square of maximal off-diagonal element
static double maxoff(const mat &A, size_t &i, size_t &j) {
  i = 1; j = 0; // indices of maximal lement
  double a = 0; // square of maximal element

  SizeMat s = size(A);

  for(size_t k = 0; k < s.n_rows; k++) {
    for(size_t l = 0; l < s.n_cols; l++) {
      if(k == l) continue;

      const double x = A(k, l);
      const double y = x * x;
      if(y > a)
        i = k, j = l, a = y;
    }
  }

  return a;
}

static void apply_rot_col(size_t k, size_t l, double t, mat &M) {
  if(k == l) return;

  const double c = 1 / sqrt( 1 + t * t );
  const double s = t * c;

  colvec kvec = c * M.col(k) - s * M.col(l);
  colvec lvec = s * M.col(k) + c * M.col(l);

  M.col(k) = kvec;
  M.col(l) = lvec;
}

static void apply_rot_row(size_t k, size_t l, double t, mat &M) {
  if(k == l) return;

  const double c = 1 / sqrt( 1 + t * t );
  const double s = t * c;

  rowvec kvec = c * M.row(k) - s * M.row(l);
  rowvec lvec = s * M.row(k) + c * M.row(l);

  M.row(k) = kvec;
  M.row(l) = lvec;
}

// single step with Jacobi's method
static bool jacobi_step(mat &B, mat &P) {
  // calculate maximal off-diagonal element with respect to absolute value
  size_t k, l;
  const double a = maxoff(B, k, l); // square of maximal element

  // if less than epsilon, stop
  if(a < epsilon)
    return false;

  // calculate sin θ and cos θ
  const double tau = (B(l, l) - B(k, k)) / (2 * B(k, l));
  const double t = (tau >= 0 ? - tau - sqrt( 1 + tau * tau ) : - tau + sqrt( 1 + tau * tau ));

  // apply similarity transform
  //   B = S.t() * B * S;
  apply_rot_col(k, l, t, B);
  apply_rot_row(k, l, t, B);

  // apply S^T to P
  //   P = P * S;
  apply_rot_col(k, l, t, P);

  return true;
}

// solve with Jacobi's method
// A is input matrix, P is eigenvector matrix (with column vector eigenvectors), L is eigenvalue vector
static void jacobi_solve(const mat &A, mat &P, vec &L, size_t &steps, double &step_time) {
  // get N from A matrix
  const size_t N = A.n_rows;
  assert(A.n_cols == N);

  std::cout << "N=" << N << std::endl;

  // B is similar matrix to A through matrix transforms
  mat B = A;

  // set P matrix (that will contain eigenvectors) to the identity matrix
  P.eye(size(A));

  clock_t time_start, time_end;
  double time_total = 0;
  steps = 0;
  while(true) {
    // timing
    time_start = clock();

    if(!jacobi_step(B, P))
      break;

    // timing
    time_end = clock();
    time_total += (double)(time_end - time_start) / CLOCKS_PER_SEC;
    steps++;
  }

  // export steps and time per step
  step_time = time_total / steps;

  std::cout << "JACOBI DONE (N = " << N << ", steps = " << steps << ", time per step = " << (1000 * step_time) << "ms)" << std::endl;

  // find eigenvalues
  L.set_size(N);
  for(size_t i = 0; i < N; i++)
    L(i) = B(i, i);
}

int run_program(size_t N, struct program_output &out) {
  const double h = (rmax - rmin) / N;

  // array of ρ valuses
  vec r(N);
  for(size_t i = 0; i < N; i++)
    r(i) = rmin + h * i;

  // calculate e_i, which are all the same
  double e = - 1 / (h * h);

  // calculate d_i, which depend on V(ρ_i)
  vec d(N);
  for(size_t i = 0; i < N; i++)
    d(i) = 2 / (h * h) + V(r[i]);

  // calculate matrix A
  mat A(N, N, arma::fill::zeros);
  for(size_t i = 0; i < N; i++) {
    A(i, i) = d(i);

    if(i > 0)
      A(i, i-1) = e;
    if(i < N - 1)
      A(i, i+1) = e;
  }

  // solve with Jacobi method
  vec eigenvalues;
  mat eigenvectors;
  jacobi_solve(A, eigenvectors, eigenvalues, out.steps, out.step_time);

  // solve with Armadillo's eig_sym
  clock_t arma_start, arma_finish;
  vec arma_eigenvalues;
  mat arma_eigenvectors;
  arma_start = clock();
  eig_sym(arma_eigenvalues, arma_eigenvectors, A);
  arma_finish = clock();
  out.arma_time = (double)(arma_finish - arma_start)/CLOCKS_PER_SEC;

  out.err = comp_eig(eigenvalues, eigenvectors, arma_eigenvalues, arma_eigenvectors);
}

#ifndef NO_MAIN
int main(int argc, char **argv) {
  const size_t Nvalues[] = { 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, 100 };
  const size_t Nlen = sizeof(Nvalues) / sizeof(*Nvalues);

  FILE *fp = fopen("b.dat", "w");
  fprintf(fp, "N          epsilon    steps      step_time  arma_time  error\n");

  for(size_t i = 0; i < Nlen; i++) {
    size_t N = i[Nvalues]; // aww yeah abusing valid C++ syntax for no reason

    struct program_output out;

    std::cout << "running with N = " << N << std::endl;

    run_program(N, out);

    // write result row
    fprintf(fp, "%-10ld %-10.5g %-10ld %-10.5g %-10.5g %-10.5g\n", N, epsilon, out.steps, out.step_time, out.arma_time, out.err);
    fflush(fp);
  }

  fclose(fp);
}
#endif


/*****************************************************************************
 * reduce.cpp     Blitz++ array reduction example
 *
 * This example illustrates the array reduction functions provided by
 * Blitz++.  These functions reduce an N dimensional array (or array
 * expression) to an N-1 dimensional array expression by summing, taking
 * the mean, etc.  These array reductions are currently provided: sum,
 * mean, min, max, minIndex, maxIndex, product, count, any and all.
 *****************************************************************************/

#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    Array<int, 2> A(4,4);

    A = 3,  1,  2,  4,
        8, -1, -5,  3,
        0,  9, -1,  4,
        1,  3,  1,  2;

    cout << "A = " << A << endl;

    /*
     * Reduce the array A to a one-dimensional array, by summing/taking
     * the mean/etc. of each row.
     */

    Array<int, 1> z(4);
    Array<float, 1> z2(4);
    secondIndex j;

    z = sum(A, j);
    cout << "sum(A,j) = " << endl << z << endl;

    z2 = mean(A, j);
    cout << "mean(A,j) = " << endl << z2 << endl;

    z = min(A, j);
    cout << "min(A,j) = " << endl << z << endl;

    z = minIndex(A, j);
    cout << "minIndex(A, j) = " << endl << z << endl;

    z = max(A, j);
    cout << "max(A, j) = " << endl << z << endl;

    z = maxIndex(A, j);
    cout << "maxIndex(A, j) = " << endl << z << endl;

    z = first((A < 0), j);
    cout << "first((A < 0), j) = " << endl << z << endl;

    z = product(A, j);
    cout << "product(A, j) = " << endl << z << endl;

    z = count( (A > 0), j);
    cout << "count((A > 0), j) = " << endl << z << endl;

    z = any((abs(A) > 4), j);
    cout << "any((abs(A) > 4), j) = " << endl << z << endl;

    z = all(A > 0, j);
    cout << "all(A > 0, j) = " << endl << z << endl;

    return 0;
}

/*
 * Output
 */
#if 0
A = 4 x 4
         3         1         2         4
         8        -1        -5         3
         0         9        -1         4
         1         3         1         2

sum(A,j) =
[         10         5        12         7 ]
mean(A,j) =
[        2.5      1.25         3      1.75 ]
min(A,j) =
[          1        -5        -1         1 ]
minIndex(A, j) =
[          1         2         2         0 ]
max(A, j) =
[          4         8         9         3 ]
maxIndex(A, j) =
[          3         0         1         1 ]
product(A, j) =
[         24       120         0         6 ]
count((A > 0), j) =
[          4         2         2         4 ]
any((abs(A) > 4), j) =
[          0         1         1         0 ]
all(A > 0, j) =
[          1         0         0         1 ]

#endif