#include "geometrycentral/stripes.h"
#include <Eigen/SparseCholesky>

// ONLY WORKS FOR MESHES WITHOUT BOUNDARY
Stripes::Stripes(HalfedgeMesh* m, Geometry<Euclidean>* g) : mesh(m), geom(g), phi(m), r(m), field(m),
                                                                singularities(m), branchCover(m), omega(m) {
    assert(mesh->nBoundaryLoops() == 0);
}

void Stripes::setup() {
    VertexData<double> s(mesh);
    // Compute s_i at each vertex
    for (VertexPtr v : mesh->vertices()) {
        if (v.isBoundary()) {
            s[v] = 1.0;
        } else {
            double sum = 0;
            for (HalfedgePtr he : v.outgoingHalfedges()) {
                sum += geom->angle(he.next());
            }
            s[v] = 2*M_PI / sum;
        }
    }

    // Compute transport at edges r_ij <- e^ip_ij
    for (VertexPtr v : mesh->vertices()) {
        HalfedgePtr he = v.halfedge();
        double angle = 0;
        double s_i = s[v];
        do {
            phi[he] = angle;
            angle += s_i * geom->angle(he.next());
            he = he.next().next().twin();
        } while (he != v.halfedge());
    }

    // Compute r_ij
    std::complex<double> i(0, 1);
    for (VertexPtr v : mesh->vertices()) {
        for (HalfedgePtr he : v.outgoingHalfedges()) {
            double theta_ij = phi[he];
            double theta_ji = phi[he.twin()] + M_PI;
            double rho_ij = theta_ij - theta_ji;
            r[he] = std::exp(i * n * rho_ij);
        }
    }
}

Eigen::SparseMatrix<std::complex<double>> Stripes::assembleM() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<std::complex<double>> M(n,n);
    std::vector<Eigen::Triplet<std::complex<double>>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he_ij = f.halfedge();
        size_t i = vertexIndices[he_ij.vertex()];
        size_t j = vertexIndices[he_ij.next().vertex()];
        size_t k = vertexIndices[he_ij.prev().vertex()];

        double area = geom->area(f);
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i, i, area/3.));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j, j, area/3.));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k, k, area/3.));
    }
    M.setFromTriplets(triplets.begin(),triplets.end());
    return M;
}

Eigen::SparseMatrix<std::complex<double>> Stripes::assembleA() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<std::complex<double>> A(n,n);
    std::vector<Eigen::Triplet<std::complex<double>>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he_ij = f.halfedge();
        HalfedgePtr he_jk = f.halfedge().next();
        HalfedgePtr he_ki = f.halfedge().prev();

        size_t i = vertexIndices[he_ij.vertex()];
        size_t j = vertexIndices[he_jk.vertex()];
        size_t k = vertexIndices[he_ki.vertex()];

        double a = geom->cotan(he_jk);
        double b = geom->cotan(he_ki);
        double c = geom->cotan(he_ij);

        std::complex<double> r_ij = r[he_ij];
        std::complex<double> r_ji = r[he_ij.twin()];
        std::complex<double> r_jk = r[he_jk];
        std::complex<double> r_kj = r[he_jk.twin()];
        std::complex<double> r_ki = r[he_ki];
        std::complex<double> r_ik = r[he_ki.twin()];

        // row i
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,i,b + c));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,j,-c * r_ij));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,k,-b * r_ik));

        // row j
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,i,-c * r_ji));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,j,c + a));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,k,-a * r_jk));

        // row k
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,i,-b * r_ki));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,j,-a * r_kj));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,k,a + b));
    }
    A.setFromTriplets(triplets.begin(),triplets.end());
    return A;
}

Eigen::MatrixXcd Stripes::principalEigenvector(Eigen::SparseMatrix<std::complex<double>> A, Eigen::SparseMatrix<std::complex<double>> B) {
    // LL^T <- Cholesky(A)
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<std::complex<double>>> solver;
    solver.compute(A);

    // u <- UniformRand(-1,1)
    Eigen::MatrixXcd x = Eigen::MatrixXcd::Random(mesh->nVertices(),1);

    // inverse power iteration to find eigenvector belonging to the smallest eigenvalue
    for (int i = 0; i < nPowerIterations; i++) {
        x = solver.solve(B * x);
        std::complex<double> norm2 = (x.transpose() * B * x)(0,0);
        x = x / sqrt(norm2);
    }
    return x;
}

Eigen::MatrixXcd Stripes::principalEigenvector(Eigen::SparseMatrix<double> A, Eigen::SparseMatrix<double> B) {
    // LL^T <- Cholesky(A)
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
    solver.compute(A);

    // u <- UniformRand(-1,1)
    Eigen::MatrixXcd x = Eigen::MatrixXcd::Random(2*mesh->nVertices(),1);

    // inverse power iteration to find eigenvector belonging to the smallest eigenvalue
    for (int i = 0; i < nPowerIterations; i++) {
        x = solver.solve(B * x);
        std::complex<double> norm2 = (x.transpose() * B * x)(0,0);
        x = x / sqrt(norm2);
    }
    return x;
}

VertexData<std::complex<double>> Stripes::computeField() {
    std::cout << "Computing Cross Field... ";
    // Algorithm 1 : Setup
    setup();

    // Algorithm 2 : Smoothest Field
    Eigen::SparseMatrix<std::complex<double>> A = assembleA();
    Eigen::SparseMatrix<std::complex<double>> M = assembleM();
    A = A + eps * M;
    Eigen::MatrixXcd x = principalEigenvector(A,M);

    // map resulting vector to VertexData
    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (VertexPtr v : mesh->vertices()) {
        std::complex<double> c = x(vertexIndices[v],0);
        if (std::abs(c) == 0) {
            field[v] = 0;
        } else {
            field[v] = c / std::abs(c);
        }
    }
    std::cout << "Done!" << std::endl;
    return field;
}

FaceData<int> Stripes::computeSingularities() {
    std::cout << "Computing Singularities... ";
    // first, compute Omega_ijk <- arg(r_ij r_jk r_ki)
    FaceData<double> Omega(mesh);
    for (FacePtr f : mesh->faces()) {
        std::complex<double> r_ij = r[f.halfedge()];
        std::complex<double> r_jk = r[f.halfedge().next()];
        std::complex<double> r_ki = r[f.halfedge().prev()];
        Omega[f] = std::arg(r_ij * r_jk * r_ki);
    }

    // next, compute w_ij for each e_ij, such that u_j = e^iw_ij * r_ij * u_i
    // w_ij = arg(u_j / (r_ij * u_i))
    HalfedgeData<double> w(mesh);
    for (HalfedgePtr he : mesh->allHalfedges()) {
        std::complex<double> u_i = field[he.vertex()];
        std::complex<double> u_j = field[he.twin().vertex()];
        std::complex<double> r_ij = r[he];
        w[he] = std::arg(u_j * r_ij / u_i);
    }

    // finally, compute index for each triangle t
    // (1/2pi) * (w_ij + w_jk + w_ki + Omega_ijk)
    int total = 0;
    for (FacePtr f : mesh->faces()) {
        double w_ij = w[f.halfedge()];
        double w_jk = w[f.halfedge().next()];
        double w_ki = w[f.halfedge().prev()];
        double Omega_ijk = Omega[f];
        double phi = (w_ij + w_jk + w_ki - Omega_ijk) / (2.0 * M_PI);
        singularities[f] = std::round(phi);
        total += singularities[f];
    }
    std::cout << "Sum: " << total << std::endl;
    return singularities;
}

void Stripes::edgeData() {
    std::cout<< "Computing EdgeData... ";
    std::complex<double> i(0, 1);
    for (EdgePtr e : mesh->edges()) {
        HalfedgePtr he_ij = e.halfedge();
        HalfedgePtr he_ji = he_ij.twin();

        // disambiguate the big vectors
        std::complex<double> f_ij = std::pow(field[he_ij.vertex()], 1.0 / n);
        std::complex<double> f_ji = std::pow(field[he_ji.vertex()], 1.0 / n);

        // we need to recompute r_ij here without raising to the nth power,
        // as raising to the nth power and then taking the nth root is not always an identity operation
        double theta_ij = phi[he_ij];
        double theta_ji = phi[he_ij.twin()] + M_PI;
        double rho_ij = theta_ji - theta_ij;
        std::complex<double> r_ij = std::exp(i * rho_ij);
        std::complex<double> s_ij = f_ji / (f_ij * r_ij);
        double ang = std::arg(s_ij);

        double sign;
        if ( ang >= -M_PI_2 && ang < M_PI_2 ) {
            sign = 1;
            branchCover[e] = 0;
        } else {
            sign = -1;
            branchCover[e] = 1;
        }

        double phi_i = std::arg(f_ij);
        double phi_j = std::arg(sign * f_ji);
        double l_ij = geom->length(e);
        omega[e] = lambda * (l_ij / 2.0) * (cos(phi_i - theta_ij) + cos(phi_j - theta_ji));
    }
    std::cout << "Done!" << std::endl;
}

Eigen::SparseMatrix<double> Stripes::EnergyMatrix() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<double> A(2*n,2*n);
    std::vector<Eigen::Triplet<double>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (EdgePtr e : mesh->edges()) {
        HalfedgePtr he_ij = e.halfedge();

        // cotan weights
        double cotA = geom->cotan(he_ij);
        double cotB = geom->cotan(he_ij.twin());
        if (singularities[he_ij.face()] != 0) cotA = 0;
        if (singularities[he_ij.twin().face()] != 0) cotB = 0;
        double w = (cotA + cotB) / 2.0;

        // indices
        size_t i = 2 * vertexIndices[he_ij.vertex()];
        size_t j = 2 * vertexIndices[he_ij.twin().vertex()];

        // add diagonal terms
        triplets.push_back( Eigen::Triplet<double>(i,i,w) );
        triplets.push_back( Eigen::Triplet<double>(i+1,i+1,w) );
        triplets.push_back( Eigen::Triplet<double>(j,j,w) );
        triplets.push_back( Eigen::Triplet<double>(j+1,j+1,w) );

        // transport coefficient components
        double x = w * cos(omega[e]);
        double y = w * sin(omega[e]);

        // stays on same sheet
        if (branchCover[e] == 0) {
            // A_ij
            triplets.push_back( Eigen::Triplet<double>(i,j,-x) ); triplets.push_back( Eigen::Triplet<double>(i,j+1,-y) );
            triplets.push_back( Eigen::Triplet<double>(i+1,j,y) ); triplets.push_back( Eigen::Triplet<double>(i+1,j+1,-x) );
            // A_ji
            triplets.push_back( Eigen::Triplet<double>(j,i,-x) ); triplets.push_back( Eigen::Triplet<double>(j,i+1,y) );
            triplets.push_back( Eigen::Triplet<double>(j+1,i,-y) ); triplets.push_back( Eigen::Triplet<double>(j+1,i+1,-x) );
        } else {
            // A_ij
            triplets.push_back( Eigen::Triplet<double>(i,j,-x) ); triplets.push_back( Eigen::Triplet<double>(i,j+1,y) );
            triplets.push_back( Eigen::Triplet<double>(i+1,j,y) ); triplets.push_back( Eigen::Triplet<double>(i+1,j+1,x) );
            // A_ji
            triplets.push_back( Eigen::Triplet<double>(j,i,-x) ); triplets.push_back( Eigen::Triplet<double>(j,i+1,y) );
            triplets.push_back( Eigen::Triplet<double>(j+1,i,y) ); triplets.push_back( Eigen::Triplet<double>(j+1,i+1,x) );
        }
    }

    A.setFromTriplets(triplets.begin(),triplets.end());
    return A;
}

Eigen::SparseMatrix<double> Stripes::MassMatrix() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<double> M(2*n,2*n);
    std::vector<Eigen::Triplet<double>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he_ij = f.halfedge();
        size_t i = vertexIndices[he_ij.vertex()];
        size_t j = vertexIndices[he_ij.next().vertex()];
        size_t k = vertexIndices[he_ij.prev().vertex()];

        double area = geom->area(f);
        triplets.push_back(Eigen::Triplet<double>(2*i, 2*i, area/3.));
        triplets.push_back(Eigen::Triplet<double>(2*i+1, 2*i+1, area/3.));
        triplets.push_back(Eigen::Triplet<double>(2*j, 2*j, area/3.));
        triplets.push_back(Eigen::Triplet<double>(2*j+1, 2*j+1, area/3.));
        triplets.push_back(Eigen::Triplet<double>(2*k, 2*k, area/3.));
        triplets.push_back(Eigen::Triplet<double>(2*k+1, 2*k+1, area/3.));
    }
    M.setFromTriplets(triplets.begin(),triplets.end());
    return M;
}

void Stripes::computeStripes() {
    std::cout << "Computing Stripes... ";
    Eigen::SparseMatrix<double> A = EnergyMatrix();
    Eigen::SparseMatrix<double> B = MassMatrix();
    Eigen::MatrixXcd x = principalEigenvector(A,B);
    std::cout << "Done!" << std::endl;
    std::cout << x.rows() << "," << x.cols() << std::endl;
}


#pragma once
// Add a backcheck on the correspondence

#include <vector>
#include <string>
#include <Eigen/Dense>
#include "nanoflannWrapper.hpp"
#include "utils/visualization/progressbar.h"

Eigen::MatrixXd sub_space(const Eigen::MatrixXd &cloud, std::vector<int> indices) {
    Eigen::MatrixXd out(3, indices.size());
    for (int i=0; i<indices.size(); i++)
        out.col(i) = cloud.col(indices.at(i));
    return out;
}

template <typename T>
T median_filter(std::vector<T> vector)
{
    std::sort(vector.begin(), vector.end());
    return vector[ std::round( vector.size()/2 ) ];
}

Eigen::Vector3d get_median_point( std::vector<int> pointIdxNKNSearch, Eigen::MatrixXd & cloud )
{
    int x=0, y=1, z=2;
    Eigen::Vector3d median_point;
    std::vector< double > x_vector, y_vector, z_vector;
    for (int i=0; i<pointIdxNKNSearch.size(); ++i)
    {
        x_vector.push_back( cloud(pointIdxNKNSearch[i], x) );
        y_vector.push_back( cloud(pointIdxNKNSearch[i], y) );
        z_vector.push_back( cloud(pointIdxNKNSearch[i], z) );
    }
    median_point(x) = median_filter(x_vector);
    median_point(y) = median_filter(y_vector);
    median_point(z) = median_filter(z_vector);
    return median_point;
}

Eigen::Vector3d point_association(std::string method,
                                Eigen::Vector3d p_i,
                                Eigen::Vector3d n_i,
	                            Eigen::MatrixXd & P,
	                            Eigen::MatrixXd & N)
{
    // used for readability
    int x=0, y=1, z=2;
	Eigen::VectorXd points_dist(P.cols());
	Eigen::VectorXd normals_dist(P.cols());
	for (int i = 0; i < P.cols(); ++i)
	{
		points_dist(i) = sqrt( (p_i(x) - P.col(i)(x)) * (p_i(x) - P.col(i)(x))
			                  +(p_i(y) - P.col(i)(y)) * (p_i(y) - P.col(i)(y))
			                  +(p_i(z) - P.col(i)(z)) * (p_i(z) - P.col(i)(z)));
		normals_dist(i) = n_i(x)*N.col(i)(x)
		                + n_i(y)*N.col(i)(y)
		                + n_i(z)*N.col(i)(z);
	}
	Eigen::MatrixXd::Index pointIndex;
	if (method == "closest_point")
	{
		points_dist.minCoeff(&pointIndex);
	}
	if (method == "normals_distances")
	{
		normals_dist.maxCoeff(&pointIndex);
	}
	if (method == "weighted")
	{
		( points_dist/0.1 - normals_dist ).minCoeff(&pointIndex);
	}
	if (method == "hybrid")
	{
		//pointIndex = ( points_dist.array().pow(31)/normals_dist ).maxCoeff(&pointIndex);
		pointIndex = 1;
		//pointIndex = ( pow(points_dist, 31)/normals_dist ).maxCoeff(&pointIndex);
	}

    return P.row(pointIndex);
}

int find_correspondence(std::string method,
                        Eigen::Vector3d v,
                        Eigen::Vector3d n,
	                    const Eigen::MatrixXd & cloud,
	                    const Eigen::MatrixXd & normals,
                        nanoflann_wrapper kd_tree,
                        int K)
{
    // downsampling of the cloud and normals with K closest points
    std::vector<int> closest_point_indices(K);
    closest_point_indices = kd_tree.return_k_closest_points(v, K);
    Eigen::MatrixXd sample_cloud = sub_space(cloud, closest_point_indices);
    Eigen::MatrixXd sample_normals = sub_space(normals, closest_point_indices);

    // build the distance between points and normals
    int x=0, y=1, z=2;
    Eigen::VectorXd points_distance(cloud.cols());
	Eigen::VectorXd normals_distance(cloud.cols());
    for (int i = 0; i < cloud.cols(); ++i)
	{
		points_distance(i) = sqrt( (v(x) - cloud.col(i)(x)) * (v(x) - cloud.col(i)(x))
			                      +(v(y) - cloud.col(i)(y)) * (v(y) - cloud.col(i)(y))
			                      +(v(z) - cloud.col(i)(z)) * (v(z) - cloud.col(i)(z)));
		normals_distance(i) = n(x)*normals.col(i)(x)
		                    + n(y)*normals.col(i)(y)
		                    + n(z)*normals.col(i)(z);
	}

    // return the min distance according to the selected method
	Eigen::MatrixXd::Index pointIndex;
	if (method == "closest_point")
	{
		points_distance.minCoeff(&pointIndex);
	}
	if (method == "normals_distances")
	{
		normals_distance.maxCoeff(&pointIndex);
	}
	if (method == "weighted")
	{
		( points_distance/0.1 - normals_distance ).minCoeff(&pointIndex);
	}

    return pointIndex;
}

void get_surface_association(Eigen::MatrixXd V_source, Eigen::MatrixXd N_source,
                             Eigen::MatrixXd V_target, Eigen::MatrixXd N_target,
                             Eigen::MatrixXd &source_position,
                             Eigen::MatrixXd &target_position)
{
    // this need to be moved out of hardcoded parameters into config file
    int x=0, y=1, z=2;                                  // used to access the points
    double distance_threshold = 0.05;                   // used to check that the correspondence of the correspondence is not too far
    int skip_points = 5;                               // pseudo downsampling
    int K = 50;                                         // limit the search space

    std::vector<Eigen::Vector3d> correspondences_on_source;
    std::vector<Eigen::Vector3d> correspondences_on_target;

    nanoflann_wrapper tree_target(V_target.transpose());
    nanoflann_wrapper tree_source(V_source.transpose());
    std::vector<int> closest_point_indices(K);

    // for loop
    progressbar bar(V_source.cols()/skip_points);
    for (int i = 0; i < V_source.cols(); i = i+skip_points) {
        int point_index_original = i*skip_points;

        // find the correspondence of the source on the target
        int correspondence_on_target = find_correspondence("weighted",
                                                           V_source.col(point_index_original),
                                                           N_source.col(point_index_original),
	                                                       V_target,
	                                                       N_target,
                                                           tree_target,
                                                           K);

        // find the correspondence of the source on the target
        int correspondence_on_source = find_correspondence("weighted",
                                                           V_target.col(correspondence_on_target),
                                                           N_target.col(correspondence_on_target),
	                                                       V_source,
	                                                       N_source,
                                                           tree_source,
                                                           K);

        // check the distance between the points
        if ( (V_source.col(point_index_original)-V_source.col(correspondence_on_source)).norm() < distance_threshold ) {
            correspondences_on_source.push_back(V_source.col(point_index_original));
            correspondences_on_target.push_back(V_target.col(correspondence_on_target));
        }

        bar.update();
    }

    // push back into an Eigen Matrices
    source_position.resize(3, correspondences_on_source.size());
    for (int i=0; i<correspondences_on_source.size(); i++)
        source_position.col(i) = correspondences_on_source.at(i);

    target_position.resize(3, correspondences_on_target.size());
    for (int i=0; i<correspondences_on_target.size(); i++)
        target_position.col(i) = correspondences_on_target.at(i);

    /*
    int x=0, y=1, z=2;
    //for (int i = 0; i < template_cloud->size(); i = i+10)
    //{
    //    template_cloud_downsampled->points.push_back(template_cloud->points[i]);
    //    template_normals_downsampled->points.push_back(template_normals->points[i]);
    //}
    double distance_threshold = 0.05;                   // used to check that the correspondence of the correspondence is not too far
    int skip_points = 10;                               // pseudo downsampling
    int K = 50;                                         // limit the search space
    int number_of_correspondences = int(round(V_source.cols()/skip_points));

    // generate downsampled cloud
    Eigen::MatrixXd V_source_downsampled, N_source_downsampled;
    V_source_downsampled.resize(3, number_of_correspondences);
    N_source_downsampled.resize(3, number_of_correspondences);
    for (int i = 0; i < number_of_correspondences; i++) {
        V_source_downsampled.col(i) = V_source.col(i*skip_points);
        N_source_downsampled.col(i) = N_source.col(i*skip_points);
    }

    // create kd-tree
    nanoflann_wrapper tree(V_target.transpose());
    std::vector<int> pointIdxNKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(K);

    pointIdxNKNSearch.clear();

    Eigen::Vector3d median_point;

    source_position.resize(3, number_of_correspondences);
    target_position.resize(3, number_of_correspondences);

    for (int i = 0; i < number_of_correspondences; i++)
    {
        // WTF this is not used ????? LOOOOL
		pointIdxNKNSearch = tree.return_k_closest_points(V_source.col(i*skip_points).transpose(), K);

        //median_point = point_association("closest_point",
        //                                    V_source.col(i*skip_points),
        //                                    N_source.col(i*skip_points),
        //                                    V_target,
        //                                    N_target);

        median_point = V_target.col(pointIdxNKNSearch[0]);

        target_position.col(i) <<  median_point(x), median_point(y), median_point(z);
        source_position.col(i) << V_source.col(i*skip_points)(x),
                                  V_source.col(i*skip_points)(y),
                                  V_source.col(i*skip_points)(z);
    }
    */
}


#pragma once

#include <fftw3.h>
#include <complex>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <type_traits>

#include "shift.hpp"

template <typename D1, typename D2>
struct enable_if_cc
    : public std::enable_if<std::is_same<typename D1::Scalar, std::complex<double>>::value &&
                            std::is_same<typename D2::Scalar, std::complex<double>>::value>
{
};

template <typename D1, typename D2>
struct enable_if_cr
    : public std::enable_if<std::is_same<typename D1::Scalar, std::complex<double>>::value &&
                            std::is_same<typename D2::Scalar, double>::value>
{
};

template <typename D1, typename D2>
struct enable_if_rc
    : public std::enable_if<std::is_same<typename D1::Scalar, double>::value &&
                            std::is_same<typename D2::Scalar, std::complex<double>>::value>
{
};

struct FFT
{
  typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> array_t;
  typedef Eigen::Array<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
      complex_array_t;

  typedef std::complex<double> cdouble;

  /**
   * @brief forward transform
   *
   * @param[out] dst
   * @param[in] src
   */
  template <typename DERIVED1, typename DERIVED2>
  typename enable_if_cr<DERIVED1, DERIVED2>::type fft2(Eigen::DenseBase<DERIVED1> &dst,
                                                       const Eigen::DenseBase<DERIVED2> &src,
                                                       bool scale = true) const;

  template <typename DERIVED1, typename DERIVED2>
  typename enable_if_cc<DERIVED1, DERIVED2>::type fft2(Eigen::DenseBase<DERIVED1> &dst,
                                                       const Eigen::DenseBase<DERIVED2> &src,
                                                       bool scale = true) const;

  /**
   * @brief inverse transform (does not preserve input)
   *
   * @param[out] dst
   * @param[in] src   full spectrum
   */
  template <typename DERIVED1, typename DERIVED2>
  typename enable_if_rc<DERIVED1, DERIVED2>::type ifft2(Eigen::DenseBase<DERIVED1> &dst,
                                                        Eigen::DenseBase<DERIVED2> &src) const;

  /**
  * @brief ifft2 (c -> r)
  *
  * @param[out] dst  dest
  * @param[in] src  will be overwritten by FFTW!
  */
  template <typename DERIVED1, typename DERIVED2>
  typename enable_if_cr<DERIVED1, DERIVED2>::type ifft2(Eigen::DenseBase<DERIVED1> &dst,
                                                        Eigen::DenseBase<DERIVED2> &src) const;

  /**
   * @brief ifft2 (c -> c)
   *
   * @param[out] dst  destination
   * @param[in] src  will be overwritten by FFTW!
   */
  template <typename DERIVED1, typename DERIVED2>
  typename enable_if_cc<DERIVED1, DERIVED2>::type ifft2(Eigen::DenseBase<DERIVED1> &dst,
                                                        Eigen::DenseBase<DERIVED2> &src) const;

  /**
   * @brief 2-dim fft (including fftshift)
   *
   * @param[out] dst  complex array (centered zero-frequency convention)
   * @param[in] src   real array
   * @param[in] scale if true: scales output by 1/numel(src)
   *
   * This is the inverse of ift for \var scale set to false.
   *
   */
  template <typename DERIVED1, typename DERIVED2>
  typename enable_if_cr<DERIVED1, DERIVED2>::type ft(Eigen::DenseBase<DERIVED1> &dst,
                                                     const Eigen::DenseBase<DERIVED2> &src,
                                                     bool scale = true) const;

  /**
   * complex, complex
   * @brief fft2 (including fftshift)
   *
   * @param[out] dst (in centered zero-frequency convention)
   * @param[int] src
   * @param bool  if true: scales output by 1/numel(src)
   *
   * This is the inverse of ift for \var scale set to false.
   *
   */
  template <typename DERIVED1, typename DERIVED2>
  typename enable_if_cc<DERIVED1, DERIVED2>::type ft(Eigen::DenseBase<DERIVED1> &dst,
                                                     const Eigen::DenseBase<DERIVED2> &src,
                                                     bool scale = true) const;

  /**
   * real, complex
   *
   * @brief ifft2 (including ifftshift)
   *
   * @param[out] dst   real array
   * @param[in] src   complex array
   *
   * Scales the output by 1/numel(src). Inverse of ft(dst, src, scale=false).
   *
   */
  template <typename DERIVED1, typename DERIVED2>
  typename enable_if_rc<DERIVED1, DERIVED2>::type ift(Eigen::DenseBase<DERIVED1> &dst,
                                                      const Eigen::DenseBase<DERIVED2> &src) const;

  /**
   * complex, complex
   * @brief complex ifft2 (including ifftshift)
   *
   * @param[out] dst
   * @param[in] src
   *
   * Scales the output by 1/numel(src). Inverse of ft(dst, src, scale=false).
   *
   */
  template <typename DERIVED1, typename DERIVED2>
  typename enable_if_cc<DERIVED1, DERIVED2>::type ift(Eigen::DenseBase<DERIVED1> &dst,
                                                      const Eigen::DenseBase<DERIVED2> &src) const;
};

// --------------------------------------------------------------------------------
template <typename DERIVED1, typename DERIVED2>
typename enable_if_cr<DERIVED1, DERIVED2>::type
FFT::fft2(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src, bool scale) const
{
  static_assert(sizeof(fftw_complex) == sizeof(cdouble), "type mismatch");
  // typedef double fftw_cdouble[2];
  typedef fftw_complex fftw_cdouble;
  const int n0 = src.rows();
  const int n1 = src.cols();
  int n[2] = {n0, n1};
  int embed[2] = {n0, n1};
  dst.derived().resize(n0, n1);
  int flags = FFTW_PRESERVE_INPUT | FFTW_ESTIMATE;
  fftw_cdouble *out = reinterpret_cast<fftw_cdouble *>(dst.derived().data());
  double *in = const_cast<double *>(src.derived().data());
  fftw_plan fwd_plan = fftw_plan_many_dft_r2c(2 /* rank */,
                                              n /* dims */,
                                              1 /* num dfts */,
                                              in,
                                              embed,
                                              1 /* stride */,
                                              embed[0] * embed[1],
                                              out,
                                              embed,
                                              1 /*stride */,
                                              embed[0] * embed[1],
                                              flags);
  assert(fwd_plan != NULL);
  fftw_execute(fwd_plan);

  // mirror coefficients
  for (int i = 0; i < n0; ++i) {
    int idest = (n0 - i) % n0;
    for (int j = 1; j < n1 / 2 + n1 % 2; ++j) {
      dst(idest, n1 - j) = std::conj(dst(i, j));
    }
  }
  if (scale) {
    double f = 1. / (n0 * n1);
    dst *= f;
  }
}

// --------------------------------------------------------------------------------
template <typename DERIVED1, typename DERIVED2>
typename enable_if_cc<DERIVED1, DERIVED2>::type
FFT::fft2(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src, bool scale) const
{
  static_assert(sizeof(fftw_complex) == sizeof(cdouble), "type mismatch");

  const int n0 = src.rows();
  const int n1 = src.cols();
  dst.derived().resize(n0, n1);
  fftw_complex *in =
      const_cast<fftw_complex *>(reinterpret_cast<const fftw_complex *>(src.derived().data()));
  fftw_complex *out = reinterpret_cast<fftw_complex *>(dst.derived().data());
  unsigned int flags = FFTW_ESTIMATE | FFTW_PRESERVE_INPUT;
  fftw_plan inv_plan = fftw_plan_dft_2d(n0, n1, in, out, FFTW_FORWARD, flags);

  assert(inv_plan != NULL);
  fftw_execute(inv_plan);
  if (scale) {
    double f = 1. / (n0 * n1);
    dst *= f;
  }
}

// ----------------------------------------------------------------------------
template <typename DERIVED1, typename DERIVED2>
typename enable_if_rc<DERIVED1, DERIVED2>::type
FFT::ifft2(Eigen::DenseBase<DERIVED1> &dst, Eigen::DenseBase<DERIVED2> &src) const
{
  static_assert(sizeof(fftw_complex) == sizeof(cdouble), "type mismatch");
  // typedef double fftw_cdouble[2];
  typedef fftw_complex fftw_cdouble;

  const int n0 = src.rows();
  const int n1 = src.cols();
  int n[2] = {n0, n1};
  dst.derived().resize(n0, n1);
  fftw_cdouble *in = reinterpret_cast<fftw_cdouble *>(src.derived().data());
  double *out = dst.derived().data();
  int embed[2] = {n0, n1};
  unsigned int flags = FFTW_ESTIMATE;
  fftw_plan inv_plan = fftw_plan_many_dft_c2r(
      2, n, 1, in, embed, 1, embed[0] * embed[1], out, embed, 1, embed[0] * embed[1], flags);
  assert(inv_plan != NULL);
  fftw_execute(inv_plan);

  double f = 1. / (n0 * n1);
  dst *= f;
}

// --------------------------------------------------------------------------------
template <typename DERIVED1, typename DERIVED2>
typename enable_if_cc<DERIVED1, DERIVED2>::type
FFT::ifft2(Eigen::DenseBase<DERIVED1> &dst, Eigen::DenseBase<DERIVED2> &src) const
{
  const int n0 = src.rows();
  const int n1 = src.cols();

  dst.derived().resize(n0, n1);

  fftw_complex *in = reinterpret_cast<fftw_complex *>(src.derived().data());
  fftw_complex *out = reinterpret_cast<fftw_complex *>(dst.derived().data());
  unsigned int flags = FFTW_ESTIMATE;
  // fftw_plan fftw_plan_dft_2d(int n0, int n1,
  //                          fftw_complex *in, fftw_complex *out,
  //                          int sign, unsigned flags);
  fftw_plan inv_plan = fftw_plan_dft_2d(n0, n1, in, out, FFTW_BACKWARD, flags);

  assert(inv_plan != NULL);
  fftw_execute(inv_plan);
  double f = 1. / (n0 * n1);
  dst *= f;
}

// --------------------------------------------------------------------------------
template <typename DERIVED1, typename DERIVED2>
typename enable_if_cr<DERIVED1, DERIVED2>::type
FFT::ifft2(Eigen::DenseBase<DERIVED1> &dst, Eigen::DenseBase<DERIVED2> &src) const
{
  // not implemented
  throw 1;
}

// --------------------------------------------------------------------------------
template <typename DERIVED1, typename DERIVED2>
typename enable_if_cr<DERIVED1, DERIVED2>::type
FFT::ft(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src, bool scale) const
{
  complex_array_t tmp(src.rows(), src.cols());
  this->fft2(tmp, src, scale);
  dst.derived().resize(tmp.rows(), tmp.cols());
  fftshift(dst, tmp);
}

// --------------------------------------------------------------------------------
template <typename DERIVED1, typename DERIVED2>
typename enable_if_cc<DERIVED1, DERIVED2>::type
FFT::ft(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src, bool scale) const
{
  complex_array_t tmp(src.rows(), src.cols());
  this->fft2(tmp, src, scale);
  dst.derived().resize(tmp.rows(), tmp.cols());
  fftshift(dst, tmp);
}

// --------------------------------------------------------------------------------
template <typename DERIVED1, typename DERIVED2>
typename enable_if_rc<DERIVED1, DERIVED2>::type
FFT::ift(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src) const
{
  complex_array_t tmp(src.rows(), src.cols());
  ifftshift(tmp, src);
  this->ifft2(dst, tmp);
}

// --------------------------------------------------------------------------------
template <typename DERIVED1, typename DERIVED2>
typename enable_if_cc<DERIVED1, DERIVED2>::type
FFT::ift(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src) const
{
  complex_array_t tmp(src.rows(), src.cols());
  ifftshift(tmp, src);
  this->ifft2(dst, tmp);
}


#include "constants.h"
#include "inset_state.h"
#include "interpolate_bilinearly.h"
#include <boost/multi_array.hpp>
#include <omp.h>

// Function to calculate the velocity at the grid points (x, y) with x =
// 0.5, 1.5, ..., lx-0.5 and y = 0.5, 1.5, ..., ly-0.5 at time t
void calculate_velocity(
    double t,
    FTReal2d &grid_fluxx_init,
    FTReal2d &grid_fluxy_init,
    FTReal2d &rho_ft,
    FTReal2d &rho_init,
    boost::multi_array<double, 2> *grid_vx,
    boost::multi_array<double, 2> *grid_vy,
    const unsigned int lx,
    const unsigned int ly
) {
  double rho;

#pragma omp parallel for private(rho)
  for (unsigned int i = 0; i < lx; ++i) {
    for (unsigned int j = 0; j < ly; ++j) {
      rho = rho_ft(0, 0) + (1.0 - t) * (rho_init(i, j) - rho_ft(0,0));
      (*grid_vx)[i][j] = -grid_fluxx_init(i, j) / rho;
      (*grid_vy)[i][j] = -grid_fluxy_init(i, j) / rho;
    }
  }
  return;
}

bool all_points_are_in_domain(
    double delta_t,
    boost::multi_array<XYPoint, 2> *proj,
    boost::multi_array<XYPoint, 2> *v_intp,
    const unsigned int lx,
    const unsigned int ly
) {
  // Return false if and only if there exists a point that would be outside
  // [0, lx] x [0, ly]
  for (unsigned int i = 0; i < lx; ++i) {
    for (unsigned int j = 0; j < ly; ++j) {
      double x = (*proj)[i][j].x + 0.5 * delta_t * (*v_intp)[i][j].x;
      double y = (*proj)[i][j].y + 0.5 * delta_t * (*v_intp)[i][j].y;
      if (x < 0.0 || x > lx || y < 0.0 || y > ly) {
        return false;
      }
    }
  }
  return true;
}

// Function to integrate the equations of motion with the fast flow-based
// method
void InsetState::flatten_density()
{
  std::cerr << "In flatten_density()" << std::endl;

  // Constants for the numerical integrator
  const double inc_after_acc = 1.1;
  const double dec_after_not_acc = 0.75;
  const double abs_tol = (std::min(lx_, ly_) * 1e-6);

  // Resize proj_ multi-array if running for the first time
  if (proj_.shape()[0] != lx_ || proj_.shape()[1] != ly_) {
    proj_.resize(boost::extents[lx_][ly_]);
  }
  for (unsigned int i = 0; i < lx_; ++i) {
    for (unsigned int j = 0; j < ly_; ++j) {
      proj_[i][j].x = i + 0.5;
      proj_[i][j].y = j + 0.5;
    }
  }

  // Allocate memory for the velocity grid
  boost::multi_array<double, 2> grid_vx(boost::extents[lx_][ly_]);
  boost::multi_array<double, 2> grid_vy(boost::extents[lx_][ly_]);

  // Prepare Fourier transforms for the flux
  FTReal2d grid_fluxx_init;
  FTReal2d grid_fluxy_init;
  grid_fluxx_init.allocate(lx_, ly_);
  grid_fluxy_init.allocate(lx_, ly_);
  grid_fluxx_init.make_fftw_plan(FFTW_RODFT01, FFTW_REDFT01);
  grid_fluxy_init.make_fftw_plan(FFTW_REDFT01, FFTW_RODFT01);

  // eul[i][j] will be the new position of proj_[i][j] proposed by a simple
  // Euler step: move a full time interval delta_t with the velocity at time t
  // and position (proj_[i][j].x, proj_[i][j].y)
  boost::multi_array<XYPoint, 2> eul(boost::extents[lx_][ly_]);

  // mid[i][j] will be the new displacement proposed by the midpoint
  // method (see comment below for the formula)
  boost::multi_array<XYPoint, 2> mid(boost::extents[lx_][ly_]);

  // (vx_intp, vy_intp) will be the velocity at position (proj_.x, proj_.y) at
  // time t
  boost::multi_array<XYPoint, 2> v_intp(boost::extents[lx_][ly_]);

  // (vx_intp_half, vy_intp_half) will be the velocity at the midpoint
  // (proj_.x + 0.5*delta_t*vx_intp, proj_.y + 0.5*delta_t*vy_intp) at time
  // t + 0.5*delta_t
  boost::multi_array<XYPoint, 2> v_intp_half(boost::extents[lx_][ly_]);

  // Initialize the Fourier transforms of gridvx[] and gridvy[] at
  // every point on the lx_-times-ly_ grid at t = 0. We must typecast lx_ and ly_
  // as double-precision numbers. Otherwise the ratios in the denominator
  // will evaluate as zero.
  double dlx = lx_;
  double dly = ly_;

  // We temporarily insert the Fourier coefficients for the x-components and
  // y-components of the flux vector into grid_fluxx_init and grid_fluxy_init.
  // The reason for `+1` in `di+1` stems from the RODFT10 formula at:
  // https://www.fftw.org/fftw3_doc/1d-Real_002dodd-DFTs-_0028DSTs_0029.html
  for (unsigned int i = 0; i < lx_-1; ++i) {
    double di = i;
    for (unsigned int j = 0; j < ly_; ++j) {
      double denom = pi * ((di+1)/dlx + (j/(di+1)) * (j/dly) * (dlx/dly));
      grid_fluxx_init(i, j) =
        -rho_ft_(i+1, j) / denom;
    }
  }
  for (unsigned int j = 0; j < ly_; ++j) {
    grid_fluxx_init(lx_-1, j) = 0.0;
  }
  for (unsigned int i=0; i<lx_; ++i) {
    double di = i;
    for (unsigned int j = 0; j < ly_-1; ++j) {
      double denom = pi * ((di/(j+1)) * (di/dlx) * (dly/dlx) + (j+1)/dly);
      grid_fluxy_init(i, j) =
        -rho_ft_(i, j+1) / denom;
    }
  }
  for (unsigned int i=0; i<lx_; ++i) {
    grid_fluxy_init(i, ly_-1) = 0.0;
  }

  // Compute the flux vector and store the result in grid_fluxx_init and
  // grid_fluxy_init
  grid_fluxx_init.execute_fftw_plan();
  grid_fluxy_init.execute_fftw_plan();
  double t = 0.0;
  double delta_t = 1e-2;  // Initial time step.
  unsigned int iter = 0;

  // Integrate
  while (t < 1.0) {
    calculate_velocity(
      t,
      grid_fluxx_init,
      grid_fluxy_init,
      rho_ft_,
      rho_init_,
      &grid_vx,
      &grid_vy,
      lx_,
      ly_
    );
#pragma omp parallel for
    for (unsigned int i = 0; i < lx_; ++i) {
      for (unsigned int j = 0; j < ly_; ++j) {

        // We know, either because of the initialization or because of the
        // check at the end of the last iteration, that (proj_.x, proj_.y)
        // is inside the rectangle [0, lx_] x [0, ly_]. This fact guarantees
        // that interpolate_bilinearly() is given a point that cannot cause it
        // to fail.
        v_intp[i][j].x = interpolate_bilinearly(
          proj_[i][j].x,
          proj_[i][j].y,
          &grid_vx,
          'x',
          lx_,
          ly_
        );
        v_intp[i][j].y = interpolate_bilinearly(
          proj_[i][j].x,
          proj_[i][j].y,
          &grid_vy,
          'y',
          lx_,
          ly_
        );
      }
    }
    bool accept = false;
    while (!accept) {

      // Simple Euler step.

#pragma omp parallel for
      for (unsigned int i = 0; i < lx_; ++i) {
        for (unsigned int j = 0; j < ly_; ++j) {
          eul[i][j].x = proj_[i][j].x + v_intp[i][j].x * delta_t;
          eul[i][j].y = proj_[i][j].y + v_intp[i][j].y * delta_t;
        }
      }

      // Use "explicit midpoint method"
      // x <- x + delta_t * v_x(x + 0.5*delta_t*v_x(x,y,t),
      //                        y + 0.5*delta_t*v_y(x,y,t),
      //                        t + 0.5*delta_t)
      // and similarly for y.
      calculate_velocity(
        t + 0.5*delta_t,
        grid_fluxx_init,
        grid_fluxy_init,
        rho_ft_,
        rho_init_,
        &grid_vx,
        &grid_vy,
        lx_,
        ly_
      );

      // Make sure we do not pass a point outside [0, lx_] x [0, ly_] to
      // interpolate_bilinearly(). Otherwise decrease the time step below and
      // try again.
      accept = all_points_are_in_domain(delta_t, &proj_, &v_intp, lx_, ly_);
      if (accept) {

        // Okay, we can run interpolate_bilinearly()

#pragma omp parallel for
        for (unsigned int i = 0; i < lx_; ++i) {
          for (unsigned int j = 0; j < ly_; ++j) {
            v_intp_half[i][j].x = interpolate_bilinearly(
              proj_[i][j].x + 0.5*delta_t*v_intp[i][j].x,
              proj_[i][j].y + 0.5*delta_t*v_intp[i][j].y,
              &grid_vx,
              'x',
              lx_,
              ly_
            );
            v_intp_half[i][j].y = interpolate_bilinearly(
              proj_[i][j].x + 0.5*delta_t*v_intp[i][j].x,
              proj_[i][j].y + 0.5*delta_t*v_intp[i][j].y,
              &grid_vy,
              'y',
              lx_,
              ly_
            );
            mid[i][j].x = proj_[i][j].x + v_intp_half[i][j].x * delta_t;
            mid[i][j].y = proj_[i][j].y + v_intp_half[i][j].y * delta_t;

            // Do not accept the integration step if the maximum squared
            // difference between the Euler and midpoint proposals exceeds
            // abs_tol. Neither should we accept the integration step if one
            // of the positions wandered out of the domain. If one of these
            // problems occurred, decrease the time step.
            const double sq_dist =
              (mid[i][j].x-eul[i][j].x) * (mid[i][j].x-eul[i][j].x)
              + (mid[i][j].y-eul[i][j].y) * (mid[i][j].y-eul[i][j].y);
            if (sq_dist > abs_tol ||
                mid[i][j].x < 0.0 || mid[i][j].x > lx_ ||
                mid[i][j].y < 0.0 || mid[i][j].y > ly_) {
              accept = false;
            }
          }
        }
      }
      if (!accept) {
        delta_t *= dec_after_not_acc;
      }
    }

    // Control ouput
    if (iter % 10 == 0) {
      std::cerr << "iter = "
                << iter
                << ", t = "
                << t
                << ", delta_t = "
                << delta_t
                << "\n";
    }

    // When we get here, the integration step was accepted
    t += delta_t;
    ++iter;
    proj_ = mid;
    delta_t *= inc_after_acc;  // Try a larger step next time
  }
  grid_fluxx_init.destroy_fftw_plan();
  grid_fluxy_init.destroy_fftw_plan();
  grid_fluxx_init.free();
  grid_fluxy_init.free();
  return;
}


/** \file  TSBinomialExample.cpp
    \brief Test and demonstration program.
           Copyright 2006, 2015 by Erik Schloegl
     */

#include <iostream>
#include <cstdlib>
#include <boost/bind.hpp>
#include "GaussianHJM.hpp"
#include "TSBinomial.hpp"
#include "QFArrayUtil.hpp"
#include "TSPayoff.hpp"
#include "TSInstruments.hpp"
#include "ExponentialVol.hpp"

using namespace quantfin;

inline double positivePart(double x)
{
  return (x>0.0) ? x : 0.0;
}

/** Test and demonstration for HJM implementation.

    Command-line arguments:
      -# interest rate.
      -# volatility.
      -# number of time steps.
      -# maturity.
      -# moneyness.
  */
int main(int argc,char* argv[])
{
  using std::cout;
  using std::endl;
  using std::flush;

  int i,j;
  try {
    double r = 0.05;
    if (argc>1) r = atof(argv[1]);
    double sgm = 0.3;
    if (argc>2) sgm = atof(argv[2]);
    int N = 10;
    if (argc>3) N = atoi(argv[3]);
    double mat = 1.5;
    if (argc>4) mat = atof(argv[4]);
    double K = 1.0;
    if (argc>5) K = atof(argv[5]);
    FlatTermStructure flat_ts(r,0.0,mat+10.0);
    Array<double,1> T(9);
    double T1 = mat;
    double T2 = T1 + 5.0;
    firstIndex idx;
    Array<double,1> SaSoT(N+1);
    double dtSaSo = T2/N;
    SaSoT = idx * dtSaSo;
    int iseg = find_segment(T1,SaSoT);
    if (std::abs(T1-SaSoT(iseg))>std::abs(T1-SaSoT(iseg+1))) iseg++;
    T1 = SaSoT(iseg);
    double strike = K * flat_ts(T2)/flat_ts(T1);
    double ZCBstrike = strike;
    /// Test SaSo binomial method
    cout << "Creating SaSo lattice... " << SaSoT << endl;
    TSBinomialMethod SaSo(flat_ts,sgm,SaSoT);
    cout << "Rolling back term structures... " << endl;
    SaSo.rollbackTermstructures();
    cout << "SaSo verified: " << SaSo.verify() << endl;
    ZCBoption zcbpayoff(ZCBstrike,T2);
    boost::function<double (const TermStructure& ts)> f;
    f = boost::bind(std::mem_fun(&ZCBoption::operator()),&zcbpayoff,_1);
    SaSo.apply_payoff(iseg,f);
    SaSo.rollback(iseg,0);
    cout << "SaSo lattice ZCB call: " << SaSo.result() << endl;
    SaSo.apply_payoff(iseg,f);
    SaSo.rollback(iseg);
    cout << "SaSo lattice ZCB call using state prices: " << SaSo.result() << endl;
    cout << "Attempting to reproduce Figure 8.4 of Clewlow/Strickland..." << endl;
    Array<double,1> CST(5);
    CST = idx;
    cout << "Time line: " << CST << endl;
    Array<double,1> PDB(5);
    PDB(0) = 1.0;
    for (i=1;i<5;i++) PDB(i) = PDB(i-1)/1.05;
    cout << "Initial bonds: " << PDB << endl;
    TSLogLinear CSts(CST,PDB);
    TSBinomialMethod CSmodel(CSts,0.1,CST);
	cout << "Short rates:" << endl;
	for (i=0;i<4;i++) {
	  for (j=0;j<=i;j++) {
		cout << CSmodel.short_rate(i,j) << ','; }
	  cout << endl; }
	cout << "State prices:" << endl;
	for (i=0;i<4;i++) {
	  for (j=0;j<=i;j++) {
		cout << CSmodel.state_price(i,j) << ','; }
	  cout << endl; }
    cout << "Testing calibration to caplets..." << endl;
    double mr = 0.1;
    ExponentialVol evol(sgm/10.0,mr);
    GaussianHJM emodel(&evol,&CSts);
    std::vector<TSEuropeanInstrument*> caplets,floorlets;
    double delta    = CST(2)-CST(1);
    double lvl      = CSts.simple_rate(CST(1),delta);
	double floorlvl = lvl;
    Caplet caplet1(emodel.caplet(CST(1),delta,lvl),CST(0),CST(1),lvl,delta);
    cout << caplet1.price() << ' ' << flush;
    caplets.push_back(&caplet1);
    Floorlet floorlet1(-1.0,CST(0),CST(1),floorlvl,delta);
    floorlets.push_back(&floorlet1);
    delta = CST(3)-CST(2);
    lvl   = CSts.simple_rate(CST(2),delta);
    Caplet caplet2(emodel.caplet(CST(2),delta,lvl),CST(0),CST(2),lvl,delta);
    cout << caplet2.price() << ' ' << flush;
    caplets.push_back(&caplet2);
    Floorlet floorlet2(-1.0,CST(0),CST(2),floorlvl,delta);
    floorlets.push_back(&floorlet2);
    delta = CST(4)-CST(3);
    lvl   = CSts.simple_rate(CST(3),delta);
    Caplet caplet3(emodel.caplet(CST(3),delta,lvl),CST(0),CST(3),lvl,delta);
    cout << caplet3.price() << ' ' << flush;
    caplets.push_back(&caplet3);
    Floorlet floorlet3(-1.0,CST(0),CST(3),floorlvl,delta);
    floorlets.push_back(&floorlet3);
    cout << endl;
    CSmodel.calibrate(caplets);
	cout << "Compare model price with input price" << endl;
    std::vector<TSEuropeanInstrument*>::iterator iter;
    for (iter=caplets.begin();iter!=caplets.end();iter++) {
      cout << CSmodel.price(**iter) << ' ' << (*iter)->price() << endl; }
	// Price an interest rate floor
	double floor = 0.0;
    for (iter=floorlets.begin();iter!=floorlets.end();iter++) {
      floor += CSmodel.price(**iter); }
	cout << "Price of interest rate floor with floor level " << floorlvl << ": " << floor << endl;
	// Instantiate larger lattice
	int N2 = 130;
    Array<double,1> SaSoT2(N2+1);
    double dtSaSo2 = T2/N2;
    SaSoT2 = idx * dtSaSo2;
    cout << "Creating SaSo lattice... " << SaSoT2 << endl;
    TSBinomialMethod SaSo2(flat_ts,sgm,SaSoT2);
    cout << "Rolling back term structures... " << endl;
    SaSo2.rollbackTermstructures();
    cout << "SaSo verified: " << SaSo2.verify() << endl;
	// Price a European swaption
	Swaption swaption(-1.0,0.0,SaSoT2(25),r,0.5,4);
	cout << "Maturity: " << SaSoT2(25) << endl;
	cout << "Swaption price: " << SaSo2.price(swaption) << endl;
	// Price a Bermudan swaption
    BermudanSwaption bermudan(-1.0,0.0,SaSoT2(25),r,0.5,4);
	if (!subset(bermudan.maturity(),SaSoT2)) throw std::logic_error("Timeline mismatch");
	cout << "Maturity: " << SaSoT2(25) << endl;
	cout << "Bermudan Swaption price: " << SaSo2.price(bermudan) << endl;
	// Price a barrier caplet
	Caplet capletB(-1.0,0.0,SaSoT2(50),lvl,delta);
	BarrierInstrument barrier_caplet(-1.0,0.0,capletB,SaSoT2(Range(0,50)),0.8*lvl,delta,-1);
	cout << "Caplet price: " << SaSo2.price(capletB) << endl;
	cout << "Barrier caplet price: " << SaSo2.price(barrier_caplet) << endl;

	} // end of try block

  catch (std::logic_error xcpt) {
    std::cerr << xcpt.what() << endl; }
  catch (std::runtime_error xcpt) {
    std::cerr << xcpt.what() << endl; }
  catch (...) {
    std::cerr << "Other exception caught" << endl; }

  return 0;
}


#define _CRT_SECURE_NO_WARNINGS
#pragma warning(disable:4819)

#include <iostream>
#include <string>
#include <random>
#include <sstream>
#include <iomanip>
#include <numeric>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

#include <opencv2/opencv.hpp>

cv::Mat capture(const cv::Mat& prj_im)
{
	//
	// Write your code for projection a projector image in "prj_im" and capture a camera image to "cam_im"
	//

	// This is dummy
	cv::Mat cam_im = prj_im.clone();

	return cam_im;
}

struct fitting_functor
{
	fitting_functor(const int inputs, const int values, const Eigen::VectorXd& input_x, const Eigen::VectorXd& input_y, const int im_num_)
		: inputs_(inputs), values_(values), im_num(im_num_), N(values / im_num_), x(input_x), y(input_y) {}

	Eigen::VectorXd x;
	Eigen::VectorXd y;

	int operator()(const Eigen::VectorXd& p, Eigen::VectorXd& fvec) const
	{
		// f = min(1, c_i * ((a * x + b)^g) + d_i);
		// p[0] --- p[N-1] : c_i
		// p[N] --- p[2*N-1] : d_i
		// p[2*N+0] : a
		// p[2*N+1] : b
		// p[2*N+2] : g

		for (int pix_idx = 0; pix_idx < N; ++pix_idx)
		{
			for (int im_idx = 0; im_idx < im_num; ++im_idx)
			{
				const int elem_idx = im_num * pix_idx + im_idx;

				const auto& c_i = p[pix_idx];
				const auto& d_i = p[N + pix_idx];
				const auto& a = p[2 * N + 0];
				const auto& b = p[2 * N + 1];
				const auto& g = p[2 * N + 2];

				const auto& x_ = x[im_idx];
				const auto& y_ = y[elem_idx];

				fvec[elem_idx] = std::pow(std::min(1.0, c_i * std::pow(std::max(0.0, a * x_ + b), g) + d_i) - y_, 2);
			}
		}
		return 0;
	}

	int df(const Eigen::VectorXd& p, Eigen::MatrixXd& fjac)
	{
		fjac.setZero();

		for (int pix_idx = 0; pix_idx < N; ++pix_idx)
		{
			for (int im_idx = 0; im_idx < im_num; ++im_idx)
			{
				const int elem_idx = im_num * pix_idx + im_idx;

				const auto& c_i = p[pix_idx];
				const auto& d_i = p[N + pix_idx];
				const auto& a = p[2 * N + 0];
				const auto& b = p[2 * N + 1];
				const auto& g = p[2 * N + 2];

				const auto& x_ = x[im_idx];
				const auto& y_ = y[elem_idx];

				const double E = std::max(0.0, a * x_ + b);
				const double D = std::pow(E, g);
				const double C = c_i * D + d_i;
				const double B = std::min(1.0, C);
				const double A = B - y_;

				const double uC = C > 1.0 ? 1.0 : 0.0;

				const double df_dd = 2.0 * A * (1.0 - uC);
				const double df_dc = df_dd * D;
				const double df_dg = E > 0.0 ? df_dc * c_i * std::log(E) : 0.0;
				const double df_db = df_dd * c_i * g * std::pow(E, g - 1);
				const double df_da = df_db * x_;

				fjac(elem_idx, pix_idx) = df_dc;
				fjac(elem_idx, N + pix_idx) = df_dd;
				fjac(elem_idx, 2 * N + 0) = df_da;
				fjac(elem_idx, 2 * N + 1) = df_db;
				fjac(elem_idx, 2 * N + 2) = df_dg;
			}
		}

		return 0;
	}

	const int inputs_;
	const int values_;
	const int N;
	const int im_num;
	int inputs() const { return inputs_; }
	int values() const { return values_; }
};

double opt_func(const int N, const int im_num, const Eigen::VectorXd& x, const Eigen::VectorXd& y, const Eigen::VectorXd& p)
{
	double error = 0.0;

	for (int pix_idx = 0; pix_idx < N; ++pix_idx)
	{
		for (int im_idx = 0; im_idx < im_num; ++im_idx)
		{
			const int elem_idx = im_num * pix_idx + im_idx;

			const auto& c_i = p[pix_idx];
			const auto& d_i = p[N + pix_idx];
			const auto& a = p[2 * N + 0];
			const auto& b = p[2 * N + 1];
			const auto& g = p[2 * N + 2];

			const auto& x_ = x[im_idx];
			const auto& y_ = y[elem_idx];

			error += std::pow(std::min(1.0, c_i * std::pow(a * x_ + b, g) + d_i) - y_, 2);
		}
	}

	return error;
}

Eigen::VectorXd opt_func_df(const int N, const int im_num, const Eigen::VectorXd& x, const Eigen::VectorXd& y, const Eigen::VectorXd& p)
{
	Eigen::VectorXd df = Eigen::VectorXd::Zero(2 * N + 3);

	for (int pix_idx = 0; pix_idx < N; ++pix_idx)
	{
		for (int im_idx = 0; im_idx < im_num; ++im_idx)
		{
			const int elem_idx = im_num * pix_idx + im_idx;

			const auto& c_i = p[pix_idx];
			const auto& d_i = p[N + pix_idx];
			const auto& a = p[2 * N + 0];
			const auto& b = p[2 * N + 1];
			const auto& g = p[2 * N + 2];

			const auto& x_ = x[im_idx];
			const auto& y_ = y[elem_idx];

			const double E = std::max(0.0, a * x_ + b);
			const double D = std::pow(E, g);
			const double C = c_i * D + d_i;
			const double B = std::min(1.0, C);
			const double A = B - y_;

			const double uC = C > 1.0 ? 1.0 : 0.0;

			const double df_dd = 2.0 * A * (1.0 - uC);
			const double df_dc = df_dd * D;
			const double df_dg = E > 0.0 ? df_dc * c_i * std::log(E) : 0.0;
			const double df_db = df_dd * c_i * g * std::pow(E, g - 1);
			const double df_da = df_db * x_;

			df[pix_idx] += df_dc;
			df[N + pix_idx] += df_dd;
			df[2 * N + 0] += df_da;
			df[2 * N + 1] += df_db;
			df[2 * N + 2] += df_dg;
		}
	}

	return df;
}

int main()
{
	//
	// modify these arguments for your usage
	//
	const int im_num = 25;
	const cv::Size cam_resized_size(8, 4);
	const cv::Size prj_size(640, 480);
	const std::string data_path = "./";

	Eigen::VectorXd prj_intensities = Eigen::VectorXd::Zero(im_num);
	for (int i = 0; i < im_num; ++i)
	{
		prj_intensities[i] = static_cast<double>(i) / (im_num - 1);
	}

	std::vector<cv::Mat> cam_im_vec;

	for (int i = 0; i < im_num; ++i)
	{
		std::cout << prj_intensities[i] * 255.0 << std::endl;

		const cv::Mat prj_im = cv::Mat::ones(prj_size, CV_8U) * static_cast<int>(prj_intensities[i] * 255.0);
		//const cv::Mat prj_im = cv::Mat::ones(setting.prj.size, CV_8U) * static_cast<int>(prj_intensities[i]);
		const cv::Mat cam_im = capture(prj_im);
		cv::imwrite(data_path + std::to_string(i) + "_org.bmp", cam_im);

		cv::Mat cam_im_resized;
		cv::resize(cam_im, cam_im_resized, cam_resized_size);

		cam_im_vec.push_back(cam_im_resized);

		cv::imshow("cam_im_resized", cam_im_resized);
		cv::waitKey(1);

		cv::imwrite(data_path + std::to_string(i) + ".bmp", cam_im_resized);
	}

	const int N = cam_resized_size.area();
	//const int N = 1;

	// y[0*im_num] --- p[1*im_num-1] : for cam_idx=0
	// y[1*im_num] --- p[2*im_num-1] : for cam_idx=1
	// ---
	// y[(N-1)*im_num] --- p[N*im_num-1] : for cam_idx=N
	Eigen::VectorXd cam_intensities_vec = Eigen::VectorXd::Zero(N * im_num);

	for (int cam_y = 0; cam_y < cam_resized_size.height; ++cam_y)
	{
		std::vector<uchar*> cam_ptrs(im_num);
		for (int i = 0; i < im_num; ++i)
		{
			cam_ptrs[i] = cam_im_vec[i].ptr<uchar>(cam_y);
		}

		for (int cam_x = 0; cam_x < cam_resized_size.width; ++cam_x)
		{
			const int cam_idx = cam_y * cam_resized_size.width + cam_x;

			for (int i = 0; i < im_num; ++i)
			{
				const auto val = cam_ptrs[i][cam_x];
				cam_intensities_vec[cam_idx * im_num + i] = static_cast<double>(val) / 255.0;
			}
		}
	}

	// f = min(1, c_i * ((a * x + b)^g) + d_i);
	// p[0] --- p[N-1] : c_i
	// p[N] --- p[2*N-1] : d_i
	// p[2*N+0] : a
	// p[2*N+1] : b
	// p[2*N+2] : g

	Eigen::VectorXd p(2 * N + 3);

	for (int i = 0; i < N; ++i)
	{
		p[i] = 1.0;
		p[N+i] = 0.0;
	}
	p[2 * N + 0] = 1.0;
	p[2 * N + 1] = 0.0;
	p[2 * N + 2] = 1.0;

	fitting_functor functor(2 * N + 3, N * im_num, prj_intensities, cam_intensities_vec, im_num);
	Eigen::LevenbergMarquardt<fitting_functor> lm(functor);

	Eigen::LevenbergMarquardtSpace::Status info = lm.minimize(p);

	std::cout << "lm.fnorm:" << std::endl;
	std::cout << lm.fnorm << std::endl;

	const Eigen::VectorXd c_vec = p.block(0, 0, N, 1);
	const Eigen::VectorXd d_vec = p.block(N, 0, N, 1);

	double min_val = std::numeric_limits<double>::max();
	int min_idx = 0;
	for (int i = 0; i < N; ++i)
	{
		const double c = c_vec[i];
		const double d = d_vec[i];
		const double a = p[2 * N + 0];
		const double b = p[2 * N + 1];
		const double g = p[2 * N + 2];

		double x = (std::pow((1.0 - d) / c, 1.0 / g) - b) / a;
		x = std::isnan(x) ? std::numeric_limits<double>::max() : x;

		if (x < min_val)
		{
			min_idx = i;
			min_val = x;
		}
	}

	//std::cout << c_vec.mean() << std::endl;
	//std::cout << d_vec.mean() << std::endl;
	//std::cout << c_vec[min_idx] << std::endl;
	//std::cout << d_vec[min_idx] << std::endl;
	//std::cout << min_val << std::endl;
	//std::cout << p.tail(3) << std::endl;
	//std::cout << info << std::endl;

	std::cout << "c_thr: " << c_vec[min_idx] << std::endl;
	std::cout << "d_thr: " << d_vec[min_idx] << std::endl;
	std::cout << "prj_thr: " << min_val << std::endl;
	std::cout << "a: " << p[2 * N + 0] << std::endl;
	std::cout << "b: " << p[2 * N + 1] << std::endl;
	std::cout << "g: " << p[2 * N + 2] << std::endl;
	std::cout << "k: " << (1.0 - d_vec[min_idx]) / c_vec[min_idx] << std::endl;

	/*
	cv::FileStorage fs(setting.optical_calibration_path, cv::FileStorage::WRITE);

	if (!fs.isOpened())
	{
		std::cout << "Error: Failed to open calibration file." << std::endl;
		throw std::runtime_error("optical_calib_open");
	}

	cv::write(fs, "a", p[2 * N + 0]);
	cv::write(fs, "b", p[2 * N + 1]);
	cv::write(fs, "gamma", p[2 * N + 2]);
	cv::write(fs, "k", (1.0 - d_vec[min_idx]) / c_vec[min_idx]);

	fs.release();
	*/

	cv::waitKey(-1);

	return 0;
}


// The template and inlines for the -*- C++ -*- rational number classes.
// Initially implemented by Wai-Shing Luk <luk036@gmail.com>
//

/** @file include/rational.hpp
 *  This is a C++ Library header.
 */

#ifndef FUN_RATIONAL_HPP
#define FUN_RATIONAL_HPP 1

#include <cassert>
#include <type_traits> // is_integral<T>
#include <boost/operators.hpp>

namespace fun
{
  /**
   * @defgroup rational (extended) Rational Number
   * @ingroup arithmetic
   *
   * Classes and functions for (extended) rational number.
   * Reference: MF103-
   * @{
   */

  // Forward declarations.
  //template<typename _Z> struct rational;

  /// greatest common divider
  template<typename _Z, class = typename
	    std::enable_if<std::is_integral<_Z>::value>::type>
  //xxx requires is_integral<_Z>::value
  inline constexpr _Z gcd(const _Z& a, const _Z& b) noexcept
  { return b == _Z(0) ? abs(a) : gcd(b, a%b); }

  /**
   *  Rational number.
   *
   *  @param  Z  Type of rational number elements
   *  @todo unit testing
   */
  template <typename _Z, class = typename
	    std::enable_if<std::is_integral<_Z>::value>::type>
  //xxx requires is_integral<_Z>::value
  struct rational : boost::ordered_field_operators<rational<_Z>,
		    boost::ordered_field_operators2<rational<_Z>, _Z> >
  {
    /// Value typedef.
    typedef _Z value_type;

    /// Default constructor.
    ///  Unspecified parameters default to 0.
    explicit
    rational(const _Z& p = _Z(), const _Z& q = _Z(1))
      : _num{p}, _denom{q}
    {
      assert(!(_num == _Z(0) && _denom == _Z(0)));
      normalize();
    }

    // Lets the compiler synthesize the copy constructor
    //rational (const rational<_Z>&) = default;

    /// Copy constructor
    template<typename _Up>
    explicit constexpr
    rational(const rational<_Up>& s) noexcept
      : _num{s.num()}, _denom{s.denom()}
    { }

    /// Return first element of rational number.
    constexpr _Z num() const noexcept { return _num; }

    /// Return second element of rational number.
    constexpr _Z denom() const noexcept { return _denom; }

    // Lets the compiler synthesize the assignment operator
    // rational<_Z>& operator= (const rational<_Z>&);
    /// Assign this rational number to rational number @a s.
    template<typename _Up>
    rational<_Z>& operator=(const rational<_Up>& s)
    { _num = s.num(); _denom = s.denom(); return *this; }

    /// Increase this rational number (prefix operator)
    rational<_Z>& operator++()
    { _num += _denom; return *this; }

    /// Decrease this rational number (prefix operator)
    rational<_Z>& operator--()
    { _num -= _denom; return *this; }

    /// Increase this rational number (postfix operator)
    rational<_Z> operator++(int)
    { rational<_Z> res(*this); ++(*this); return res; }

    /// Decrease this rational number (postfix operator)
    rational<_Z> operator--(int)
    { rational<_Z> res(*this); --(*this); return res; }

    /// Add @a s to this rational number.
    rational<_Z>& operator+=(const _Z& a)
    { _num += _denom * a; return *this; }

    /// Subtract @a s from this rational number.
    rational<_Z>& operator-=(const _Z& a)
    { _num -= _denom * a; return *this; }

    /// Multiply this rational number by @a a.
    rational<_Z>& operator*=(const _Z& a)
    { _num *= a; normalize(); return *this; }

    /// Divide this rational number by @a a.
    rational<_Z>& operator/=(const _Z& a)
    { _denom *= a; normalize(); return *this; }

    /// Add @a s to this rational number.
    template<typename _Up>
    rational<_Z>& operator+=(const rational<_Up>& s)
    {
      _num = _num * s.denom() + _denom * s.num();
      _denom *= s.denom();
      normalize();
      return *this;
    }

    /// Subtract @a s from this rational number.
    template<typename _Up>
    rational<_Z>& operator-=(const rational<_Up>& s)
    {
      _num = _num * s.denom() - _denom * s.num();
      _denom *= s.denom();
      normalize();
      return *this;
    }

    /// Multiply @a s to this rational number.
    template<typename _Up>
    rational<_Z>& operator*=(const rational<_Up>& s)
    {
      _num *= s.num();
      _denom *= s.denom();
      normalize();
      return *this;
    }

    /// Divide @a s to this rational number.
    template<typename _Up>
    rational<_Z>& operator/=(const rational<_Up>& s)
    {
      *this *= rational<_Z>(s.denom(), s.num());
      return *this;
    }

    /// Cast to double
    operator double () const { return double(num()) / denom(); }

  private:
    /// Normalize rational number.
    void normalize() {
      if (_denom < _Z()) {
	      _num = -_num;
	      _denom = -_denom;
      }
      _Z g = gcd(_num, _denom);
      _num /= g;
      _denom /= g;
    }

  private:
    _Z _num;
    _Z _denom;

  };

  // Operators:
  ///  Return new rational number @a r plus @a s.
  template<typename _Z, typename _Up>
  inline auto
  operator+(const rational<_Z>& r, const rational<_Up>& s)
    -> rational<decltype(r.num()*s.denom())>
  {
    auto num = r.num() * s.denom() + r.denom() * s.num();
    decltype(num) denom =  r.denom() * s.denom();
    return rational<decltype(num)> {num, denom};
  }

  ///  Return new rational number @a r plus @a s.
  template<typename _Z, typename _Up>
  inline auto
  operator-(const rational<_Z>& r, const rational<_Up>& s)
    -> rational<decltype(r.denom()*s.denom())>
  {
    //auto num = r.num() * s.denom() - r.denom() * s.num();
    //decltype(num) denom =  r.denom() * s.denom();
    return rational<decltype(r.denom()*s.denom())>
      { r.num() * s.denom() - r.denom() * s.num(),
	r.denom() * s.denom() };
  }

  ///  Return new rational number @a r times @a s.
  template<typename _Z, typename _Up>
  inline auto
  operator*(const rational<_Z>& r, const rational<_Up>& s)
    -> rational<decltype(r.num()*s.num())>
  {
    return rational<decltype(r.num()*s.num())>
      { r.num()*s.num(), r.denom()*s.denom() };
  }

  ///  Return new rational number @a r times @a s.
  template<typename _Z, typename _Up>
  inline auto
  operator/(const rational<_Z>& r, const rational<_Up>& s)
    -> decltype(r * s)
  {
    return r * rational<_Up>(s.denom(), s.num());
  }

  //xxx ///  Return new rational number @a r minus @a s.
  //xxx template<typename _Z>
  //xxx inline rational<_Z>
  //xxx operator-(rational<_Z> r, const rational<_Z>& s) { return r -= s; }
  //xxx
  //xxx //@{
  //xxx ///  Return new rational number @a r times @a a.
  //xxx template<typename _Z>
  //xxx inline rational<_Z>
  //xxx operator*(rational<_Z> r, const rational<_Z>& s) { return r *= s; }
  //xxx
  //xxx template<typename _Z>
  //xxx inline rational<_Z>
  //xxx operator*(rational<_Z> r, const _Z& a) { return r *= a; }
  //xxx
  //xxx template<typename _Z>
  //xxx inline rational<_Z>
  //xxx operator*(const _Z& a, rational<_Z> r) { return r *= a; }
  //xxx //@}
  //xxx
  //xxx ///  Return new rational number @a r divided by @a a.
  //xxx template<typename _Z>
  //xxx inline rational<_Z>
  //xxx operator/(rational<_Z> r, const _Z& a) { return r /= a; }

  /// Return @a r.
  template<typename _Z>
  inline constexpr rational<_Z>
  operator+(const rational<_Z>& r) noexcept { return r; }

  /// Return negation of @a r
  template<typename _Z>
  inline constexpr rational<_Z>
  operator-(const rational<_Z>& r) noexcept
  { return rational<_Z>(-r.num(), r.denom()); }

  /// Return true if @a r is equal to @a s.
  template<typename _Z, typename _Up>
  inline bool
  operator==(const rational<_Z>& r, const rational<_Up>& s)
  {
    assert(!(r.num() == 0 && r.denom() == 0)); // NaN
    assert(!(s.num() == 0 && s.denom() == 0)); // NaN
    return r.num() == s.num() && s.denom() == r.denom();
  }

  /// Return false if @a r is equal to @a s.
  template<typename _Z, typename _Up>
  inline constexpr bool
  operator!=(const rational<_Z>& r, const rational<_Up>& s) noexcept
  { return !(r == s); }

  /// Return true if @a r is less than @a s.
  template<typename _Z, typename _Up>
  inline constexpr bool
  operator<(const rational<_Z>& r, const rational<_Up>& s) noexcept
  { return r.num()*s.denom() < r.denom()*s.num(); }

  ///  Insertion operator for rational number values.
  template<typename _Z, class _Stream>
  _Stream& operator<<(_Stream& os, const rational<_Z>& r)
  {
    const auto& a = r.num();
    const auto& b = r.denom();
    _Z zero(0), one(1);
    if (b == one)  { os << a; return os; }
    if (b != zero) { os << '(' << a << '/' << b << ')'; return os; }
    if (a < zero)  { os << "-Inf"; return os; }
    if (a > zero)  { os << "Inf"; return os; }
    os << "NaN"; return os;
  }

}

#endif


/**
 * \file dcs/math/stats/distribution/exponential.hpp
 *
 * \brief The (Negative) Exponential probability distribution.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_EXPONENTIAL_HPP
#define DCS_MATH_STATS_DISTRIBUTION_EXPONENTIAL_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(103500) // 1.35
# 	error "Required Boost library version >= 1.35"
#endif

#include <boost/math/distributions/exponential.hpp>
#include <boost/random/exponential_distribution.hpp>
//#include <boost/random/variate_generator.hpp>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
//TODO
//#include <dcs/math/random/any_generator.hpp>
//#include <dcs/math/random/base_generator.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

/**
 * \brief The (Negative) Exponential distribution with rate parameter
 * \f$\lambda\f$.
 *
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * The probability density function (pdf):
 * \f[
 *   \Pr(x|\lambda) = \lambda e^{-\lambda x}
 * \f]
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class exponential_distribution//TODO>>: public base_distribution<RealT>
{
	public: typedef RealT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

	public: explicit exponential_distribution(support_type lambda=1)
		: dist_(lambda)
	{
		// empty
	}

	// compiler-generated copy ctor and assignment operator are fine

	/**
	 * \brief Generate a random number distributed according to this
	 * exponential distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \return A random number distributed according to this exponential
	 * distribution.
	 *
	 * A \c exponential random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\lambda) = \lambda e^{-\lambda x}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
		typedef ::boost::exponential_distribution<support_type> rdist_type;
//		typedef ::boost::variate_generator<UniformRandomGeneratorT&, rdist_type> variate_type;
//
//		return variate_type(rng, rdist_type(dist_.lambda()))();
		return rdist_type(dist_.lambda())(rng);
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * exponential distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A vector of random numbers distributed according to this
	 * exponential distribution.
	 *
	 * A \c exponential random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\lambda) = \lambda e^{-\lambda x}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, ::std::size_t n)
	{
		typedef ::boost::exponential_distribution<support_type> rdist_type;
//		typedef ::boost::variate_generator<UniformRandomGeneratorT&, rdist_type> variate_type;

		::std::vector<support_type> rnds(n);
		rdist_type rvg(dist_.lambda());

		for (; n > 0; --n)
		{
//			rnds.push_back(variate_type(rng, rdist_type(dist_.lambda()))());
			rnds.push_back(rvg(rng));
		}

		return rnds;
	}

	public: support_type lambda() const
	{
		return dist_.lambda();
	}

	public: support_type scale() const
	{
		return support_type(1)/dist_.lambda();
	}

	public: support_type location() const
	{
		return support_type(0);
	}

	public: support_type quantile(value_type p) const
	{
		return ::boost::math::quantile(dist_, p);
	}

//TODO
//	private: support_type do_rand(::dcs::math::random::base_generator<value_type>& rng) const
//	{
//		typedef ::boost::exponential_distribution<support_type> rdist_type;
//		typedef ::boost::variate_generator<UniformRandomGeneratorT&, rdist_type> variate_type;
//
//		return variate_type(rng, rdist_type(dist_.lambda()))();
//	}

//TODO
//	private: support_type do_rand(::dcs::math::random::any_generator<value_type>& rng) const
//	{
//		typedef ::boost::exponential_distribution<support_type> rdist_type;
//		typedef ::boost::variate_generator<UniformRandomGeneratorT&, rdist_type> variate_type;
//
//		return variate_type(rng, rdist_type(dist_.lambda()))();
//	}

	private: ::boost::math::exponential_distribution<value_type,policy_type> dist_;
};

template <
    typename CharT,
    typename CharTraitsT,
    typename RealT,
    typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, exponential_distribution<RealT,PolicyT> const& dist)
{
	return os << "Exp("
			  << "lambda=" <<  dist.lambda()
			  << ")";
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_EXPONENTIAL_HPP


//
// Implements traditional LQR for linear dynamics and cost.
//

#pragma once

#include <vector>

#include <Eigen/Dense>

namespace lqr
{
void compute_backup(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
        const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,
        const Eigen::MatrixXd &Vt1,
        Eigen::MatrixXd &Kt, Eigen::MatrixXd &Vt);

class LQR
{
public:

    LQR(const std::vector<Eigen::MatrixXd> &As,
        const std::vector<Eigen::MatrixXd> &Bs,
        const std::vector<Eigen::MatrixXd> &Qs,
        const std::vector<Eigen::MatrixXd> &Rs);

    LQR(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
        const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,
        const int T);

    void solve();

    void forward_pass(const Eigen::VectorXd &x0,
        std::vector<double> &costs,
        std::vector<Eigen::VectorXd> &states,
        std::vector<Eigen::VectorXd> &controls) const;

//private:
    int state_dim_ = -1;
    int control_dim_  = -1;

    std::vector<Eigen::MatrixXd> As_;
    std::vector<Eigen::MatrixXd> Bs_;
    std::vector<Eigen::MatrixXd> Qs_;
    std::vector<Eigen::MatrixXd> Rs_;

    int T_ = -1;

    std::vector<Eigen::MatrixXd> Ks_;
};

} // namespace lqr


/**
 * @file calc-jump.cpp
 *
 * @brief calc jump function.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 *
 * Compile:
 * g++ calc-jump.cpp -o calc-jump -lntl
 *
 * Run with 2^128 steps:
 * ./calc-jump 340282366920938463463374607431768211456 characteristic.19937.txt > sfmt-poly-128.txt
 *
 */
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <string>
#include <inttypes.h>
#include <stdint.h>
#include <time.h>
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/ZZ.h>
#include "SFMT-calc-jump.hpp"

using namespace NTL;
using namespace std;
using namespace sfmt;

static void read_file(GF2X& characteristic, long line_no, const string& file);

int main(int argc, char * argv[]) {
    if (argc <= 2) {
	cout << argv[0] << " jump-step characteristic-file [no.]" << endl;
	cout << "    jump-step: a number between zero and 2^{SFMT_MEXP}-1.\n"
	     << "               large decimal number is allowed." << endl;
	cout << "    characteristic-file: one of characteristic.{MEXP}.txt "
	     << "file" << endl;
	cout << "    [no.]: shows which characteristic polynomial in \n"
	     << "           the file should be used. 0 is used if omitted.\n"
	     << "           this is used for files in params directory."
	     << endl;
	return -1;
    }
    string step_string = argv[1];
    string filename = argv[2];
    long no = 0;
    if (argc > 3) {
	no = strtol(argv[3], NULL, 10);
    }
    GF2X characteristic;
    read_file(characteristic, no, filename);
    ZZ step;
    stringstream ss(step_string);
    ss >> step;
    string jump_str;
    calc_jump(jump_str, step, characteristic);
    cout << "jump polynomial:" << endl;
    cout << jump_str << endl;
    return 0;
}

static void read_file(GF2X& characteristic, long line_no, const string& file)
{
    ifstream ifs(file.c_str());
    string line;
    for (int i = 0; i < line_no; i++) {
	ifs >> line;
	ifs >> line;
    }
    if (ifs) {
	ifs >> line;
	line = "";
	ifs >> line;
    }
    stringtopoly(characteristic, line);
}


#include <iostream>
#include <boost/math/common_factor_rt.hpp>
#include "audio.hpp"
#include "fm.hpp"
#include "mainwindow.h"
#include "ui_mainwindow.h"

using namespace std;
using namespace boost;

MainWindow::MainWindow(FMSynth &fm, QWidget *parent) :
    QMainWindow(parent),
    fmsynth {fm},
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    vizwindow.show();

    audioEngine.init();
}

MainWindow::~MainWindow()
{
    audioEngine.close();
    delete ui;
}

void MainWindow::on_button_trigger_clicked()
{
    // set synth params
    fmsynth.carrier_frequency = ui->carrier_textbox->text().toFloat();
    fmsynth.modulating_frequency = ui->modulating_textbox->text().toFloat();
    fmsynth.modulation_index.set(ui->modulation_index_textbox->text().toFloat());

    // find ratio
    int carrier_frequency = ui->carrier_textbox->text().toInt();
    int modulating_frequency = ui->modulating_textbox->text().toInt();
    int gcd = math::gcd(carrier_frequency, modulating_frequency);
    int numerator = carrier_frequency / gcd;
    int denominator = modulating_frequency / gcd;
    ui->ratio->setText(QString("%1 / %2").arg(numerator).arg(denominator));

    // find peak deviation
    ui->peak_deviation->setText(QString("%1 Hz").arg(fmsynth.peak_deviation()));
}

void MainWindow::on_volume_sliderMoved(int position)
{
    const float a = pow(10, position / 20.0);
    audioEngine.volume.set(a);
    QString str {QString("%1 dB").arg(position)};
    ui->label_volume_db->setText(str);
}

void MainWindow::on_carrier_slider_sliderMoved(int position)
{
    const float frequency {440.0f * powf(2, position/100.0f)};
    ui->carrier_textbox->setText(QString("%1").arg(frequency));
    audioEngine.fmsynth.carrier_frequency = frequency;
}

void MainWindow::on_modulating_slider_sliderMoved(int position)
{
    const float frequency {static_cast<float>(position)};
    ui->modulating_textbox->setText(QString("%1").arg(frequency));
    audioEngine.fmsynth.modulating_frequency = frequency;
}

void MainWindow::on_modulation_index_slider_sliderMoved(int position)
{
    const float index {static_cast<float>(position) / 10.0f};
    ui->modulation_index_textbox->setText(QString("%1").arg(index));
    audioEngine.fmsynth.modulation_index.set(index);
}

void MainWindow::on_carrier_textbox_editingFinished()
{
    fmsynth.carrier_frequency = ui->carrier_textbox->text().toFloat();
}

void MainWindow::on_modulating_textbox_editingFinished()
{
    fmsynth.modulating_frequency = ui->modulating_textbox->text().toFloat();
}

void MainWindow::on_modulation_index_textbox_editingFinished()
{
    fmsynth.modulation_index.set(ui->modulation_index_textbox->text().toFloat());
}

void MainWindow::on_scale_sliderMoved(int position)
{
    vizwindow.scale = position/1000.0 * (2.0/7040.0 - 1.0/100.0) + 1.0/100.0;
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under 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)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <string>
#include <boost/graph/edmunds_karp_max_flow.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/read_dimacs.hpp>
#include <boost/graph/graph_utility.hpp>

// Use a DIMACS network flow file as stdin.
// edmunds-karp-eg < max_flow.dat
//
// Sample output:
//  c  The total flow:
//  s 13
//
//  c flow values:
//  f 0 6 3
//  f 0 1 6
//  f 0 2 4
//  f 1 5 1
//  f 1 0 0
//  f 1 3 5
//  f 2 4 4
//  f 2 3 0
//  f 2 0 0
//  f 3 7 5
//  f 3 2 0
//  f 3 1 0
//  f 4 5 4
//  f 4 6 0
//  f 5 4 0
//  f 5 7 5
//  f 6 7 3
//  f 6 4 0
//  f 7 6 0
//  f 7 5 0

int
main()
{
  using namespace boost;

  typedef adjacency_list_traits < vecS, vecS, directedS > Traits;
  typedef adjacency_list < listS, vecS, directedS,
    property < vertex_name_t, std::string >,
    property < edge_capacity_t, long,
    property < edge_residual_capacity_t, long,
    property < edge_reverse_t, Traits::edge_descriptor > > > > Graph;

  Graph g;

  property_map < Graph, edge_capacity_t >::type
    capacity = get(edge_capacity, g);
  property_map < Graph, edge_reverse_t >::type rev = get(edge_reverse, g);
  property_map < Graph, edge_residual_capacity_t >::type
    residual_capacity = get(edge_residual_capacity, g);

  Traits::vertex_descriptor s, t;
  read_dimacs_max_flow(g, capacity, rev, s, t);

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  std::vector<default_color_type> color(num_vertices(g));
  std::vector<Traits::edge_descriptor> pred(num_vertices(g));
  long flow = edmunds_karp_max_flow
    (g, s, t, capacity, residual_capacity, rev, &color[0], &pred[0]);
#else
  long flow = edmunds_karp_max_flow(g, s, t);
#endif

  std::cout << "c  The total flow:" << std::endl;
  std::cout << "s " << flow << std::endl << std::endl;

  std::cout << "c flow values:" << std::endl;
  graph_traits < Graph >::vertex_iterator u_iter, u_end;
  graph_traits < Graph >::out_edge_iterator ei, e_end;
  for (tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
    for (tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
      if (capacity[*ei] > 0)
        std::cout << "f " << *u_iter << " " << target(*ei, g) << " "
          << (capacity[*ei] - residual_capacity[*ei]) << std::endl;

  return EXIT_SUCCESS;
}


#include <iostream>
#include <float.h>
#include <cmath>
#include "get_floor_f1Hf2.h"
#include "normalize2dpts.h"
#include <Eigen/Geometry>

using namespace Eigen;

std::vector<PoseDataVarFocal> get_floor_f1Hf2(MatrixXd &p1, MatrixXd &p2, Matrix3d &R1, Matrix3d &R2)
{
    int nbr_coeffs = 9 * 5;
    int nbr_unknowns = 4;

    // Save copies of the inverse rotation
    Matrix3d R1T = R1.transpose();
    Matrix3d R2T = R2.transpose();

    // Compute normalization matrices
    double scale1 = normalize2dpts(p1);
    double scale2 = normalize2dpts(p2);
    Vector3d s1, s2;
    s1 << scale1, scale1, 1.0;
    s2 << scale2, scale2, 1.0;
    DiagonalMatrix<double, 3> S1 = s1.asDiagonal();
    DiagonalMatrix<double, 3> S2 = s2.asDiagonal();

    // Normalize data
    MatrixXd x1(3, 3);
    MatrixXd x2(3, 3);
    x1 = S1 * p1;
    x2 = S2 * p2;

    // Setup DLT equations
    MatrixXd A(6,9);
    A << 0, 0, 0, -x2(2,0)*x1.col(0).transpose(), x2(1,0)*x1.col(0).transpose(),
         x2(2,0)*x1.col(0).transpose(), 0, 0, 0, -x2(0,0)*x1.col(0).transpose(),
         0, 0, 0, -x2(2,1)*x1.col(1).transpose(), x2(1,1)*x1.col(1).transpose(),
         x2(2,1)*x1.col(1).transpose(), 0, 0, 0, -x2(0,1)*x1.col(1).transpose(),
         0, 0, 0, -x2(2,2)*x1.col(2).transpose(), x2(1,2)*x1.col(2).transpose(),
         x2(2,2)*x1.col(2).transpose(), 0, 0, 0, -x2(0,2)*x1.col(2).transpose();

    JacobiSVD<MatrixXd> svd(A, ComputeFullV);
    ArrayXXd V = svd.matrixV();

    // Wrap input data to expected format
    VectorXd input(nbr_coeffs);
    input << V.col(6),
             V.col(7),
             V.col(8),
             Map<VectorXd>(R1T.data(), 9),
             Map<VectorXd>(R2T.data(), 9);

    // TODO: Not sure if this is necessary (assure const)
    const Map<VectorXd> input_data(input.data(), nbr_coeffs);

    // Extract solution
    MatrixXcd sols = solver_floor_f1Hf2(input_data);

    // Pre-processing: Remove complex-valued solutions
    double thresh = 1e-5;
    ArrayXd real_sols(5);
    real_sols = sols.imag().cwiseAbs().colwise().sum();
    int nbr_real_sols = (real_sols <= thresh).count();

    // Allocate space for putative (real) homographies

    // Since this is a 4 pt solver, we only return the solutions.
    std::vector<PoseDataVarFocal> posedata(nbr_real_sols);
    ArrayXd xx(nbr_unknowns);
    Matrix3d Htmp;
    double f1, f2;
    int cnt = 0;

    for (int i = 0; i < real_sols.size(); i++) {
        if (real_sols(i) <= thresh) {
            xx = sols.col(i).real();

            // Extract focal lengths
            f1 = 1 / xx[2] / scale1;
            f2 = xx[3] / scale2;

            // Construct putative homography
            VectorXd tmp(9);
            tmp << V.col(6) + xx[0] * V.col(7) + xx[1] * V.col(8);
            Htmp = Map<Matrix3d>(tmp.data(), 3 ,3);
            Htmp.transposeInPlace();
            Htmp = S2.inverse() * Htmp * S1;

            // Append
            posedata[cnt].focal_length1 = f1;
            posedata[cnt].focal_length2 = f2;
            posedata[cnt].homography = Htmp;
            cnt++;
        }
    }

    return posedata;
}

// ---------------- //
// MATLAB interface //
// ---------------- //

#ifdef MATLAB_MEX_FILE /* This macro is defined automatically when using MATLAB */
#define NUMBER_OF_FIELDS (sizeof(field_names)/sizeof(*field_names))

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (nrhs != 4) {
		mexErrMsgIdAndTxt("get_floor_f1Hf2:nrhs", "Four input arguments are required.");
	}
	if (nlhs != 1) {
		mexErrMsgIdAndTxt("get_floor_f1Hf2:nlhs", "One output arguments is required.");
	}
	if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0])) {
		mexErrMsgIdAndTxt("get_floor_f1Hf2:notDouble", "Input data must be type double.");
	}
	if(mxGetNumberOfElements(prhs[0]) != 9 && mxGetNumberOfElements(prhs[1]) != 9 && mxGetNumberOfElements(prhs[2]) != 9 && mxGetNumberOfElements(prhs[3]) != 9) {
		mexErrMsgIdAndTxt("get_floor_f1Hf2:incorrectSize", "Input dimensions incorrect.");
	}
    // Convert to expected input
    VectorXd x1_tmp = Map<VectorXd>(mxGetPr(prhs[0]), 9);
    VectorXd x2_tmp = Map<VectorXd>(mxGetPr(prhs[1]), 9);
    MatrixXd x1 = Map<MatrixXd>(x1_tmp.data(), 3, 3);
    MatrixXd x2 = Map<MatrixXd>(x2_tmp.data(), 3, 3);

    VectorXd R1_tmp = Map<VectorXd>(mxGetPr(prhs[2]), 9);
    VectorXd R2_tmp = Map<VectorXd>(mxGetPr(prhs[3]), 9);
    Matrix3d R1 = Map<Matrix3d>(R1_tmp.data(), 3, 3);
    Matrix3d R2 = Map<Matrix3d>(R2_tmp.data(), 3, 3);

    // Compute output
	std::vector<PoseDataVarFocal> posedata = get_floor_f1Hf2(x1, x2, R1, R2);

    // Wrap it all up
    std::size_t NUMBER_OF_STRUCTS = posedata.size();
    const char *field_names[] = {"H", "f1", "f2"};
    mwSize dims[2] = {1, NUMBER_OF_STRUCTS };
    int H_field, f1_field, f2_field;
    mwIndex i;

    plhs[0] = mxCreateStructArray(2, dims, NUMBER_OF_FIELDS, field_names);

    H_field = mxGetFieldNumber(plhs[0], "H");
    f1_field = mxGetFieldNumber(plhs[0], "f1");
    f2_field = mxGetFieldNumber(plhs[0], "f2");

	double* zr;
    for (i = 0; i < NUMBER_OF_STRUCTS; i++) {
        mxArray *field_value;

        // Create H
        field_value = mxCreateDoubleMatrix(3, 3, mxREAL);
        zr = mxGetPr(field_value);
        for (Index j = 0; j < posedata[i].homography.size(); j++) {
            zr[j] = posedata[i].homography(j);
        }
        mxSetFieldByNumber(plhs[0],i,H_field,field_value);

        // Create f1
        field_value = mxCreateDoubleMatrix(1, 1, mxCOMPLEX);
        zr = mxGetPr(field_value);
        zr[0] = posedata[i].focal_length1;
        mxSetFieldByNumber(plhs[0],i,f1_field,field_value);

        // Create f2
        field_value = mxCreateDoubleMatrix(1, 1, mxCOMPLEX);
        zr = mxGetPr(field_value);
        zr[0] = posedata[i].focal_length2;
        mxSetFieldByNumber(plhs[0],i,f2_field,field_value);
    }
}
#endif


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/eight_point_fundamental_matrix.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <glog/logging.h>

#include "theia/sfm/pose/util.h"

namespace theia {

using Eigen::JacobiSVD;
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;

bool NormalizedEightPointFundamentalMatrix(
    const std::vector<Vector2d>& image_1_points,
    const std::vector<Vector2d>& image_2_points,
    Matrix3d* fundamental_matrix) {
  CHECK_EQ(image_1_points.size(), image_2_points.size());
  CHECK_GE(image_1_points.size(), 8);

  std::vector<Vector2d> norm_img1_points(image_1_points.size());
  std::vector<Vector2d> norm_img2_points(image_2_points.size());

  // Normalize the image points.
  Matrix3d img1_norm_mat, img2_norm_mat;
  NormalizeImagePoints(image_1_points, &norm_img1_points, &img1_norm_mat);
  NormalizeImagePoints(image_2_points, &norm_img2_points, &img2_norm_mat);

  // Build the constraint matrix based on x2' * F * x1 = 0.
  Matrix<double, Eigen::Dynamic, 9> constraint_matrix(image_1_points.size(), 9);
  for (int i = 0; i < image_1_points.size(); i++) {
    constraint_matrix.block<1, 3>(i, 0) = norm_img1_points[i].homogeneous();
    constraint_matrix.block<1, 3>(i, 0) *= norm_img2_points[i].x();
    constraint_matrix.block<1, 3>(i, 3) = norm_img1_points[i].homogeneous();
    constraint_matrix.block<1, 3>(i, 3) *= norm_img2_points[i].y();
    constraint_matrix.block<1, 3>(i, 6) = norm_img1_points[i].homogeneous();
  }

  // Solve the constraint equation for F from nullspace extraction.
  // An LU decomposition is efficient for the minimally constrained case.
  // Otherwise, use an SVD.
  Matrix<double, 9, 1> normalized_fvector;
  if (image_1_points.size() == 8) {
    const auto lu_decomposition = constraint_matrix.fullPivLu();
    if (lu_decomposition.dimensionOfKernel() != 1) {
      return false;
    }
    normalized_fvector = lu_decomposition.kernel();
  } else {
    JacobiSVD<Matrix<double, Eigen::Dynamic, 9> > cmatrix_svd(
        constraint_matrix, Eigen::ComputeFullV);
    normalized_fvector = cmatrix_svd.matrixV().col(8);
  }

  // NOTE: This is the transpose of a valid fundamental matrix! We implement a
  // "lazy" transpose and defer it to the SVD a few lines below.
  Eigen::Map<const Matrix3d> normalized_fmatrix(normalized_fvector.data());

  // Find the closest singular matrix to F under frobenius norm. We can compute
  // this matrix with SVD.
  JacobiSVD<Matrix3d> fmatrix_svd(normalized_fmatrix.transpose(),
                                  Eigen::ComputeFullU | Eigen::ComputeFullV);
  Vector3d singular_values = fmatrix_svd.singularValues();
  singular_values[2] = 0.0;
  *fundamental_matrix = fmatrix_svd.matrixU() * singular_values.asDiagonal() *
                        fmatrix_svd.matrixV().transpose();

  // Correct for the point normalization.
  *fundamental_matrix =
      img2_norm_mat.transpose() * (*fundamental_matrix) * img1_norm_mat;

  return true;
}

}  // namespace theia