/* Small utility functions useful for point-based registration */
#include <Util.hpp>

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <exception>

#include <Eigen/Dense>
#include <Exceptions.hpp>

bool isApproxEqual(double a, double b, double eps) {
    // Check whether a and b are equal, to within tolerance eps.
    auto diff = std::abs(a - b);
    return diff < eps;
}

bool isApproxEqual(double a, double b) {
    // Epsilon not specified, default to 0.001;
    return isApproxEqual(a, b, 0.001);
}

Eigen::Vector3d find_pointset_average(const Eigen::MatrixXd& pointset) {
    auto average = pointset.rowwise().mean();
    return average;
}

Eigen::MatrixXd residuals_from_point(const Eigen::MatrixXd& pointset, const Eigen::Vector3d& point) {
    return pointset.colwise() - point;
}

Eigen::VectorXd distances_between_pointsets(const Eigen::MatrixXd& pointset, const Eigen::MatrixXd& pointset_dash) {
    return (pointset - pointset_dash).colwise().norm();
}

double root_mean_square(const Eigen::VectorXd& v) {
    return sqrt((v.cwiseProduct(v)).mean());
}

Eigen::Matrix4d compose_final_transform(const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation) {
    // Compose 3x3 rotation and 3d vector translation to get a joint rotation+translation.
    // Final transform is a 4x4 rigid transform matrix, with rotation part in the top-left 3x3, and translation in the top-right 3x1.
    Eigen::Matrix4d final_transform;
    final_transform.block(0,0,3,3) << rotation;
    final_transform.block(0,3,3,1) << translation;
    final_transform.block(3,0,1,3) << 0, 0, 0;
    final_transform.block(3,3,1,1) << 1;

    return final_transform;
}

Eigen::MatrixXd apply_transform(const Eigen::MatrixXd& pointset, const Eigen::Matrix4d& transform) {
    // Need to add a one on the end of each vector, for the translation part of the transform.
    Eigen::MatrixXd pointset_augmented(4,pointset.cols());
    pointset_augmented.block(0,0,3,pointset.cols()) << pointset;
    pointset_augmented.block(3,0,1,pointset.cols()) << Eigen::MatrixXd::Constant(1, pointset.cols(), 1);

    auto proposed_pointset = transform * pointset_augmented;

    // Now remove the unnecessary bottom row of the transformed pointset.
    auto proposed_pointset_reduced = proposed_pointset.block(0,0,3,pointset.cols());

    return proposed_pointset_reduced;
}

Eigen::MatrixXd load_pointcloud_from_file(std::string filename) {
    int max_points = 1E6;
    int line_counter = 0;
    Eigen::MatrixXd points(3,max_points);

    std::ifstream infile;
    infile.exceptions(std::ifstream::failbit | std::ifstream::badbit);

    try {
        infile.open(filename);

        double x, y, z;

        while(infile >> x >> y >> z) {
            points.col(line_counter) << x, y, z;
            line_counter++;
        }

    } catch(std::ifstream::failure e) {
        if(!infile.eof()) {
            // Any reason other than EOF is a failure. TODO: give a specific error message for non-existent file.
            std::cerr << "Could not read file " << filename << std::endl;
            throw(PointMatchingEx);
        } else{
            infile.close();

            auto pointcloud = points.block(0,0,3,line_counter);
            return pointcloud;
        }
    }

}

Eigen::Matrix4d load_transform_from_file(std::string filename) {
    int line_counter = 0;
    Eigen::Matrix4d transform;

    std::ifstream infile;
    infile.exceptions(std::ifstream::failbit | std::ifstream::badbit);

    try {
        infile.open(filename);

        double a, b, c, d;

        while(infile >> a >> b >> c >> d) {
            transform.row(line_counter) << a, b, c, d;
            line_counter++;
        }

    } catch(std::ifstream::failure e) {
        if(!infile.eof()) {
            // Any reason other than EOF is a failure. TODO: give a specific error message for non-existent file.
            std::cerr << "Could not read file " << filename << std::endl;
            throw(PointMatchingEx);
        } else if(infile.eof() && line_counter == 4) {
            infile.close();
            return transform;
        } else {
            std::cerr << "Could not read file " << filename << std::endl;
            throw(PointMatchingEx);
        }
    }

}

void write_matrix_to_file(const Eigen::MatrixXd& matrix, std::string filename) {
    std::ofstream outfile(filename);
    if(outfile.is_open()) {
        outfile << matrix;
    }
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example showing how to define custom kernel functions for use with
    the machine learning tools in the dlib C++ Library.

    This example assumes you are somewhat familiar with the machine learning
    tools in dlib.  In particular, you should be familiar with the krr_trainer
    and the matrix object.  So you may want to read the krr_classification_ex.cpp
    and matrix_ex.cpp example programs if you haven't already.
*/

#include <iostream>
#include <dlib/svm.h>

using namespace std;
using namespace dlib;

// ----------------------------------------------------------------------------------------

/*
    Here we define our new kernel.  It is the UKF kernel from
        Facilitating the applications of support vector machine by using a new kernel
        by Rui Zhang and Wenjian Wang.

    In the context of the dlib library a kernel function object is an object with
    an interface with the following properties:
        - a public typedef named sample_type
        - a public typedef named scalar_type which should be a float, double, or
          long double type.
        - an overloaded operator() that operates on two items of sample_type
          and returns a scalar_type.
        - a public typedef named mem_manager_type that is an implementation of
          dlib/memory_manager/memory_manager_kernel_abstract.h or
          dlib/memory_manager_global/memory_manager_global_kernel_abstract.h or
          dlib/memory_manager_stateless/memory_manager_stateless_kernel_abstract.h
        - an overloaded == operator that tells you if two kernels are
          identical or not.

    Below we define such a beast for the UKF kernel.  In this case we are expecting the
    sample type (i.e. the T type) to be a dlib::matrix.  However, note that you can design
    kernels which operate on any type you like so long as you meet the above requirements.
*/

template < typename T >
struct ukf_kernel
{
    typedef typename T::type             scalar_type;
    typedef          T                   sample_type;
    // If your sample type, the T, doesn't have a memory manager then
    // you can use dlib::default_memory_manager here.
    typedef typename T::mem_manager_type mem_manager_type;

    ukf_kernel(const scalar_type g) : sigma(g) {}
    ukf_kernel() : sigma(0.1) {}

    scalar_type sigma;

    scalar_type operator() (
        const sample_type& a,
        const sample_type& b
    ) const
    {
        // This is the formula for the UKF kernel from the above referenced paper.
        return 1/(length_squared(a-b) + sigma);
    }

    bool operator== (
        const ukf_kernel& k
    ) const
    {
        return sigma == k.sigma;
    }
};

// ----------------------------------------------------------------------------------------

/*
    Here we define serialize() and deserialize() functions for our new kernel.  Defining
    these functions is optional.  However, if you don't define them you won't be able
    to save your learned decision_function objects to disk.
*/

template < typename T >
void serialize ( const ukf_kernel<T>& item, std::ostream& out)
{
    // save the state of the kernel to the output stream
    serialize(item.sigma, out);
}

template < typename T >
void deserialize ( ukf_kernel<T>& item, std::istream& in )
{
    deserialize(item.sigma, in);
}

// ----------------------------------------------------------------------------------------

/*
    This next thing, the kernel_derivative specialization is optional.  You only need
    to define it if you want to use the dlib::reduced2() or dlib::approximate_distance_function()
    routines.  If so, then you need to supply code for computing the derivative of your kernel as
    shown below.  Note also that you can only do this if your kernel operates on dlib::matrix
    objects which represent column vectors.
*/

namespace dlib
{
    template < typename T >
    struct kernel_derivative<ukf_kernel<T> >
    {
        typedef typename T::type             scalar_type;
        typedef          T                   sample_type;
        typedef typename T::mem_manager_type mem_manager_type;

        kernel_derivative(const ukf_kernel<T>& k_) : k(k_){}

        sample_type operator() (const sample_type& x, const sample_type& y) const
        {
            // return the derivative of the ukf kernel with respect to the second argument (i.e. y)
            return 2*(x-y)*std::pow(k(x,y),2);
        }

        const ukf_kernel<T>& k;
    };
}

// ----------------------------------------------------------------------------------------

int main()
{
    // We are going to be working with 2 dimensional samples and trying to perform
    // binary classification on them using our new ukf_kernel.
    typedef matrix<double, 2, 1> sample_type;

    typedef ukf_kernel<sample_type> kernel_type;

    // Now let's generate some training data
    std::vector<sample_type> samples;
    std::vector<double> labels;
    for (double r = -20; r <= 20; r += 0.9)
    {
        for (double c = -20; c <= 20; c += 0.9)
        {
            sample_type samp;
            samp(0) = r;
            samp(1) = c;
            samples.push_back(samp);

            // if this point is less than 13 from the origin
            if (sqrt(r*r + c*c) <= 13)
                labels.push_back(+1);
            else
                labels.push_back(-1);

        }
    }
    cout << "samples generated: " << samples.size() << endl;
    cout << "  number of +1 samples: " << sum(mat(labels) > 0) << endl;
    cout << "  number of -1 samples: " << sum(mat(labels) < 0) << endl;

    // A valid kernel must always give rise to kernel matrices which are symmetric
    // and positive semidefinite (i.e. have nonnegative eigenvalues).  This next
    // bit of code makes a kernel matrix and checks if it has these properties.
    const matrix<double> K = kernel_matrix(kernel_type(0.1), randomly_subsample(samples, 500));
    cout << "\nIs it symmetric? (this value should be 0): "<< min(abs(K - trans(K))) << endl;
    cout << "Smallest eigenvalue (should be >= 0):      "  << min(real_eigenvalues(K)) << endl;

    // here we make an instance of the krr_trainer object that uses our new kernel.
    krr_trainer<kernel_type> trainer;
    trainer.use_classification_loss_for_loo_cv();

    // Finally, let's test how good our new kernel is by doing some leave-one-out cross-validation.
    cout << "\ndoing leave-one-out cross-validation" << endl;
    for (double sigma = 0.01; sigma <= 100; sigma *= 3)
    {
        // tell the trainer the parameters we want to use
        trainer.set_kernel(kernel_type(sigma));

        std::vector<double> loo_values;
        trainer.train(samples, labels, loo_values);

        // Print sigma and the fraction of samples correctly classified during LOO cross-validation.
        const double classification_accuracy = mean_sign_agreement(labels, loo_values);
        cout << "sigma: " << sigma << "     LOO accuracy: " << classification_accuracy << endl;
    }

    const kernel_type kern(10);
    // Since it is very easy to make a mistake while coding a derivative it is a good idea
    // to compare your derivative function against a numerical approximation and see if
    // the results are similar.  If they are very different then you probably made a
    // mistake.  So here we compare the results at a test point.
    cout << "\nThese vectors should match, if they don't then we coded the kernel_derivative wrong!" << endl;
    cout << "approximate derivative: \n" <<               derivative(kern)(samples[0],samples[100]) << endl;
    cout << "exact derivative: \n" << kernel_derivative<kernel_type>(kern)(samples[0],samples[100]) << endl;

}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/ext/boost/mpl/vector.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/tuple.hpp>

#include <boost/mpl/fold.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/vector.hpp>

#include <type_traits>
namespace hana = boost::hana;
namespace mpl = boost::mpl;

namespace with_mpl {
//! [mpl]
using types = mpl::vector<long, float, short, float, long, long double>;
using number_of_floats = mpl::fold<
    types,
    mpl::int_<0>,
    mpl::if_<std::is_floating_point<mpl::_2>,
        mpl::next<mpl::_1>,
        mpl::_1
    >
>::type;
static_assert(number_of_floats::value == 3, "");
//! [mpl]
}

namespace with_hana {
//! [hana]
constexpr auto types = hana::tuple_t<long, float, short, float, long, long double>;
BOOST_HANA_CONSTEXPR_LAMBDA auto number_of_floats = hana::foldl(
    types,
    hana::int_<0>,
    [](auto count, auto t) {
        return hana::if_(hana::trait<std::is_floating_point>(t),
            count + hana::int_<1>,
            count
        );
    }
);
BOOST_HANA_CONSTANT_CHECK(number_of_floats == hana::int_<3>);
//! [hana]
}

int main() { }


//
// Created by jack on 9/13/20.
//

#include "feature_extracter.h"

#include <Eigen/Dense>
#include <iostream>
#include <vector>

#include "glog/logging.h"

namespace nautilus::input_processing {

std::vector<Eigen::Vector2f> GetNeighborhood(
    const std::vector<Eigen::Vector2f>& points, size_t point_index,
    size_t neighbors_per_side, double max_neighbor_distance) {
  std::vector<Eigen::Vector2f> neighbors;
  // Left side neighbors.
  for (size_t neighbor_idx =
           std::max(static_cast<size_t>(0), point_index - neighbors_per_side);
       neighbor_idx < point_index; neighbor_idx++) {
    if ((points[point_index] - points[neighbor_idx]).norm() <=
        max_neighbor_distance) {
      neighbors.push_back(points[neighbor_idx]);
    }
  }
  // Right side neighbors.
  for (size_t neighbor_idx = point_index + 1;
       neighbor_idx < std::min(points.size(), point_index + neighbors_per_side);
       neighbor_idx++) {
    neighbors.push_back(points[neighbor_idx]);
  }
  return neighbors;
}

Eigen::Vector2f ComputeMean(const std::vector<Eigen::Vector2f>& neighborhood) {
  // Compute the mean and return the mean vector.
  Eigen::Vector2f mean_vector(0, 0);
  for (const auto& p : neighborhood) {
    mean_vector += p;
  }
  return (1.0 / neighborhood.size()) * mean_vector;
}

std::vector<std::pair<double, Eigen::Vector2f>> ComputeSmoothnessScores(
    const std::vector<Eigen::Vector2f>& points,
    std::vector<float>* unsorted_scores, int neighbors_per_side,
    double max_neighbor_distance, int min_neighbor_num) {
  // For each point used the smoothness formula.
  // This formula is computing a scatter matrix for every point and its
  // neighborhood. Then the smoothness score is the smallest eigenvalue /
  // largest eigenvalue.
  std::vector<std::pair<double, Eigen::Vector2f>> smoothness_scores;
  for (size_t i = 0; i < points.size(); i++) {
    const auto& point = points[i];
    // Get the points around point, and then include point.
    std::vector<Eigen::Vector2f> neighborhood =
        GetNeighborhood(points, i, neighbors_per_side, max_neighbor_distance);
    if (neighborhood.size() < static_cast<size_t>(min_neighbor_num)) {
      // Skip this iteration if not enough neighbors.
      continue;
    }
    neighborhood.push_back(point);
    // Compute the neighborhood of point and all the points around it.
    const Eigen::Vector2f mean_vector = ComputeMean(neighborhood);
    // Now get the scatter matrix using the formula summation from 1 to n of all
    // points (Xi - m)(Xi - m)^T
    Eigen::Matrix2f scatter_matrix;
    scatter_matrix << 0, 0, 0, 0;
    for (const auto& p : neighborhood) {
      Eigen::Matrix2f temp =
          (p - mean_vector) * ((p - mean_vector).transpose());
      scatter_matrix += temp;
    }
    Eigen::EigenSolver<Eigen::Matrix2f> eigen_solver;
    eigen_solver.compute(scatter_matrix);
    // Smoothness score is smaller eigen value / larger eigen value so it is [0,
    // 1].
    auto eigen_values = eigen_solver.eigenvalues();
    CHECK_EQ(eigen_values.rows(), 2);
    double eigen_value_1 = eigen_values(0, 0).real();
    double eigen_value_2 = eigen_values(1, 0).real();
    double smoothness_score = std::min(eigen_value_1, eigen_value_2) /
                              std::max(eigen_value_1, eigen_value_2);
    if (smoothness_score < 0 || smoothness_score > 1) {
      std::cout << smoothness_score << std::endl;
      std::cout << "Eigen Values: " << eigen_value_1 << " " << eigen_value_2
                << std::endl;
    }
    smoothness_scores.emplace_back(smoothness_score, point);
    unsorted_scores->push_back(smoothness_score);
  }
  return smoothness_scores;
}

FeatureExtractor::FeatureExtractor(const std::vector<Eigen::Vector2f>& points,
                                   double threshold, double distance_threshold,
                                   int neighbor_num, int max_edge_num,
                                   int max_planar_num, int min_neighbor_num)
    : threshold_(threshold),
      distance_threshold_(distance_threshold),
      neighbors_per_side_(neighbor_num),
      max_edge_number_(max_edge_num),
      max_planar_number_(max_planar_num),
      min_neighbor_num_(min_neighbor_num) {
  // Compute the smoothness scores of every point once.
  smoothness_points_ =
      ComputeSmoothnessScores(points, &unordered_scores_, neighbors_per_side_,
                              max_neighbor_distance_, min_neighbor_num_);
  // Now sort the points based on their smoothness.
  std::sort(smoothness_points_.begin(), smoothness_points_.end(),
            [](const std::pair<double, Eigen::Vector2f>& point_a,
               const std::pair<double, Eigen::Vector2f>& point_b) {
              return point_a.first < point_b.first;
            });
}

bool validFeaturePoint(std::pair<double, Eigen::Vector2f> point,
                       std::vector<Eigen::Vector2f> points, double threshold,
                       double distance_threshold, size_t max_size,
                       bool is_edge = false) {
  // Planar points must be less than the threshold, and edge points must be more
  // than the threshold.
  if (!is_edge && point.first > threshold) {
    return false;
  }
  if (is_edge && point.first < threshold) {
    return false;
  }
  // We can accept points.
  if (points.size() >= max_size) {
    return false;
  }
  // Not close to any of the other points.
  for (const auto& p : points) {
    if ((p - point.second).norm() < distance_threshold) {
      return false;
    }
  }
  return true;
}

std::vector<Eigen::Vector2f> FeatureExtractor::GetPlanarPoints() {
  std::vector<Eigen::Vector2f> planar_points;
  // Super simple O(n^2) algorithm to find the planar points.
  for (size_t i = 0; i < smoothness_points_.size(); i++) {
    if (validFeaturePoint(smoothness_points_[i], planar_points, threshold_,
                          distance_threshold_, max_planar_number_)) {
      planar_points.push_back(smoothness_points_[i].second);
    }
  }
  return planar_points;
}

std::vector<Eigen::Vector2f> FeatureExtractor::GetEdgePoints() {
  std::vector<Eigen::Vector2f> edge_points;
  for (int i = smoothness_points_.size() - 1; i >= 0; i--) {
    if (validFeaturePoint(smoothness_points_[i], edge_points, threshold_,
                          distance_threshold_, max_edge_number_, true)) {
      edge_points.push_back(smoothness_points_[i].second);
    }
  }
  return edge_points;
}

std::vector<float> FeatureExtractor::GetSmoothnessScores() {
  return unordered_scores_;
}
}  // namespace nautilus::input_processing


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <g2o/core/auto_differentiation.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <chrono>
#include <sophus/se3.hpp>

using namespace std;
using namespace cv;

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  double min_dist = 10000, max_dist = 0;

  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d(
    (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
    (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
  );
}

// Solve ICP with linear algebra (SVD solution)
void pose_estimation_3d3d(vector<Eigen::Vector3d> pts1,
                          vector<Eigen::Vector3d> pts2,
                          Eigen::Matrix3d &R, Eigen::Vector3d &t) {
  Eigen::Vector3d c1(0.0, 0.0, 0.0), c2(0.0, 0.0, 0.0);
  for (auto& p : pts1)
    c1 += p;
  c1 /= pts1.size();
  for (auto& p : pts2)
    c2 += p;
  c2 /= pts2.size();

  for (auto& p : pts1)
    p -= c1;
  for (auto& p : pts2)
    p -= c2;

  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
  for (int i = 0; i < pts1.size(); ++i)
  {
    W += pts1[i] * pts2[i].transpose();
  }
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
  R = svd.matrixU() * svd.matrixV().transpose();
  if (R.determinant() < 0)
    R = -R;

  t = c1 - R * c2;
}

class VertexPose: public g2o::BaseVertex<6, Sophus::SE3d>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {
      _estimate = Sophus::SE3d();
    }

    virtual void oplusImpl(const double* update) override {
      Eigen::Matrix<double, 6, 1> u;
      u << update[0], update[1], update[2], update[3], update[4], update[5];
      _estimate = Sophus::SE3d::exp(u) * _estimate;
    }

  virtual bool read(istream &in) override {}
  virtual bool write(ostream &out) const override {}
};

class EdgeICP: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeICP(const Eigen::Vector3d& X2): _X2(X2) {}

    virtual void computeError() override {
      const VertexPose* v = static_cast<VertexPose*>(_vertices[0]);
      Sophus::SE3d Rt = v->estimate();
      _error = measurement() - Rt * _X2;
    }

    virtual void linearizeOplus() override {
      // fill _jacobianOPlusXi;
      const VertexPose* v = static_cast<VertexPose*>(_vertices[0]);
      Sophus::SE3d Rt = v->estimate();
      Eigen::Vector3d X2_transf = Rt * _X2;
      _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
      _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(X2_transf);
    }

    // Auto-diff is not working well. No idea why
    // template <class T>
    // bool operator() (const T* params, T* errors) const {
    //   const Eigen::Map<const Sophus::SE3<T>> Rt_sophus(params);
    //   Eigen::Matrix<T, 3, 1> X2(T(_X2.x()), T(_X2.y()), T(_X2.z()));
    //   Eigen::Matrix<T, 3, 1> X2_transf = Rt_sophus * X2;
    //   errors[0] = T(_measurement[0]) - X2_transf.x();
    //   errors[1] = T(_measurement[1]) - X2_transf.y();
    //   errors[2] = T(_measurement[2]) - X2_transf.z();
    //   return true;
    // }
    // G2O_MAKE_AUTO_AD_FUNCTIONS;

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}

  private:
    Eigen::Vector3d _X2;
};

// Solve ICP with non-linear optimization
void bundleAdjustment(
  const vector<Eigen::Vector3d> &pts1,
  const vector<Eigen::Vector3d> &pts2,
  Eigen::Matrix3d &R, Eigen::Vector3d &t) {

    Sophus::SE3d pose_init(R, t);

    typedef g2o::BlockSolverX BlockSolverType;
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
      g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    VertexPose *vertex_pose = new VertexPose();
    vertex_pose->setId(0);
    vertex_pose->setEstimate(pose_init);
    optimizer.addVertex(vertex_pose);

    for (int i = 0; i < pts1.size(); ++i)
    {
      EdgeICP *edge = new EdgeICP(pts2[i]);
      edge->setVertex(0, vertex_pose);
      edge->setMeasurement(pts1[i]);
      edge->setInformation(Eigen::Matrix3d::Identity());
      optimizer.addEdge(edge);
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(50);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optimization ICP costs time: " << time_used.count() << " seconds." << endl;

    Sophus::SE3d optim_pose = vertex_pose->estimate();
    R = optim_pose.so3().unit_quaternion().toRotationMatrix();
    t = optim_pose.translation();
  }

int main(int argc, char **argv) {
  // if (argc != 5) {
  //   cout << "usage: pose_estimation_3d3d img1 img2 depth1 depth2" << endl;
  //   return 1;
  // }
  string f1 = "../1.png"; //argv[1];
  string f2 = "../2.png"; //argv[2];
  string f3 = "../1_depth.png"; //argv[3];
  string f4 = "../2_depth.png"; //argv[3];

  Mat img_1 = imread(f1, CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(f2, CV_LOAD_IMAGE_COLOR);

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);

  Mat depth1 = imread(f3, CV_LOAD_IMAGE_UNCHANGED);
  Mat depth2 = imread(f4, CV_LOAD_IMAGE_UNCHANGED);
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  Eigen::Matrix3d K_eigen;
  K_eigen << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
  std::vector<Eigen::Vector3d> pts1, pts2;

  for (DMatch m:matches) {
    ushort d1 = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    ushort d2 = depth2.ptr<unsigned short>(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)];
    if (d1 == 0 || d2 == 0)   // bad depth
      continue;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
    float dd1 = float(d1) / 5000.0;
    float dd2 = float(d2) / 5000.0;
    pts1.push_back(Eigen::Vector3d(p1.x * dd1, p1.y * dd1, dd1));
    pts2.push_back(Eigen::Vector3d(p2.x * dd2, p2.y * dd2, dd2));
  }

  cout << "3d-3d pairs: " << pts1.size() << endl;
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  Eigen::Vector3d t = Eigen::Vector3d::Zero();
  pose_estimation_3d3d(pts1, pts2, R, t);
  cout << "ICP via SVD results: " << endl;
  cout << "R = " << R << endl;
  cout << "t = " << t.transpose() << endl;
  // cout << "R_inv = " << R.t() << endl;
  // cout << "t_inv = " << -R.t() * t << endl;

  // verify p1 = R * p2 + t
  double total_error = 0.0;
  for (int i = 0; i < pts1.size(); i++) {
    total_error += (pts1[i] - (R * pts2[i] + t)).norm();
  }
  std::cout << "Mean error (SVD): " << total_error / pts1.size() << "\n";

  cout << "calling bundle adjustment" << endl;
  R = Eigen::Matrix3d::Identity();
  t = Eigen::Vector3d::Zero();
  bundleAdjustment(pts1, pts2, R, t);

  // verify p1 = R * p2 + t
  total_error = 0.0;
  for (int i = 0; i < pts1.size(); i++) {
    total_error += (pts1[i] - (R * pts2[i] + t)).norm();
  }
  std::cout << "Mean error (BA): " << total_error / pts1.size() << "\n";
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// With contributions from Cornelius Steinhardt

#ifndef MTL_MATRIX_LU_INCLUDE
#define MTL_MATRIX_LU_INCLUDE

#include <cmath>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/utility/lu_matrix_type.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/upper.hpp>
#include <boost/numeric/mtl/matrix/lower.hpp>
#include <boost/numeric/mtl/matrix/permutation.hpp>
#include <boost/numeric/mtl/operation/adjoint.hpp>
#include <boost/numeric/mtl/operation/lower_trisolve.hpp>
#include <boost/numeric/mtl/operation/upper_trisolve.hpp>
#include <boost/numeric/mtl/operation/max_pos.hpp>
#include <boost/numeric/mtl/operation/swap_row.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace matrix {

/// LU factorization in place (without pivoting and optimization so far)
/** eps is tolerance for pivot element. If less or equal the matrix is considered singular.
    eps is given as double right now, might be refactored to the magnitude type of the value type in the future. **/
template <typename Matrix>
void inline lu(Matrix& LU, double eps= 0)
{
    vampir_trace<5023> tracer;
    using std::abs;
    MTL_THROW_IF(num_rows(LU) != num_cols(LU), matrix_not_square());

    for (std::size_t k= 0; k < num_rows(LU)-1; k++) {
	if(abs(LU[k][k]) <= eps) throw matrix_singular();
	irange r(k+1, imax); // Interval [k+1, n-1]
	LU[r][k]/= LU[k][k];
	LU[r][r]-= LU[r][k] * LU[k][r];
    }
}

/// LU factorization in place (with pivoting and without optimization so far)
/** eps is tolerance for pivot element. If less or equal the matrix is considered singular.
    eps is given as double right now, might be refactored to the magnitude type of the value type in the future. **/
template <typename Matrix, typename PermuationVector>
void inline lu(Matrix& A, PermuationVector& P, double eps= 0)
{
    vampir_trace<5024> tracer;
    using math::zero; using std::abs;
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type ncols = num_cols(A), nrows = num_rows(A);

    MTL_THROW_IF(ncols != nrows , matrix_not_square());
    P.change_dim(nrows);

    for (size_type i= 0; i < nrows; i++)
        P[i]= i;

    for (size_type i= 0; i < nrows-1; i++) {
	irange r(i+1, imax), ir(i, i+1); // Intervals [i+1, n-1], [i, i]
	size_type rmax= max_abs_pos(A[irange(i, imax)][ir]).first + i;
	swap_row(A, i, rmax);
	swap_row(P, i, rmax);

	if(abs(A[i][i]) <= eps) throw matrix_singular(); // other gmres test doesn't work

	A[r][i]/= A[i][i];              // Scale column i
	A[r][r]-= A[r][i] * A[i][r]; 	 // Decrease bottom right block of matrix
    }
}

/// LU factorization without factorization that returns the matrix
/** eps is tolerance for pivot element. If less or equal the matrix is considered singular.
    eps is given as double right now, might be refactored to the magnitude type of the value type in the future. **/
template <typename Matrix>
Matrix inline lu_f(const Matrix& A, double eps= 0)
{
    vampir_trace<5025> tracer;
    Matrix LU(A);
    lu(LU, eps);
    return LU;
}

/// Solve Ax = b by LU factorization without pivoting; vector x is returned
template <typename Matrix, typename Vector>
Vector inline lu_solve_straight(const Matrix& A, const Vector& b, double eps= 0)
{
    vampir_trace<5026> tracer;
    Matrix LU(A);
    lu(LU, eps);
    return upper_trisolve(upper(LU), unit_lower_trisolve(strict_lower(LU), b));
}

/// Solve LUx = b by with forward and backward-LU subsitution, lu(LU) was allready done
template <typename Matrix, typename Vector>
Vector inline lu_solve_apply(const Matrix& LU, const Vector& b)
{
    vampir_trace<5026> tracer;
    return upper_trisolve(upper(LU), unit_lower_trisolve(strict_lower(LU), b));
}

/// Apply the factorization L*U with permutation P on vector b to solve Ax = b
template <typename Matrix, typename PermVector, typename Vector>
Vector inline lu_apply(const Matrix& LU, const PermVector& P, const Vector& b)
{
    vampir_trace<5027> tracer;
    return upper_trisolve(upper(LU), unit_lower_trisolve(strict_lower(LU), Vector(matrix::permutation(P) * b)));
}

/// Solve Ax = b by LU factorization with column pivoting; vector x is returned
template <typename Matrix, typename Vector>
Vector inline lu_solve(const Matrix& A, const Vector& b, double eps= 0)
{
    vampir_trace<5028> tracer;
    mtl::vector::dense_vector<std::size_t, vector::parameters<> > P(num_rows(A));
    Matrix                    LU(A);

    lu(LU, P, eps);
    return lu_apply(LU, P, b);
}

/// Apply the factorization L*U with permutation P on vector b to solve adjoint(A)x = b
/** That is \f$P^{-1}(LU)^H x = b\f$ --> \f$x= P^{-1}L^{-H} U^{-H} b\f$ where \f$P^{{-1}^{{-1}^H}} = P^{-1}\f$ **/
template <typename Matrix, typename PermVector, typename Vector>
Vector inline lu_adjoint_apply(const Matrix& LU, const PermVector& P, const Vector& b)
{
    vampir_trace<5029> tracer;
    return Vector(trans(matrix::permutation(P)) * unit_upper_trisolve(adjoint(LU), lower_trisolve(adjoint(LU), b)));
}

/// Solve \f$adjoint(A)x = b\f$ by LU factorization with column pivoting; vector x is returned
template <typename Matrix, typename Vector>
Vector inline lu_adjoint_solve(const Matrix& A, const Vector& b, double eps= 0)
{
    vampir_trace<5030> tracer;
    mtl::vector::dense_vector<std::size_t, vector::parameters<> > P(num_rows(A));
    Matrix                    LU(A);

    lu(LU, P, eps);
    return lu_adjoint_apply(LU, P, b);
}

/// Class that keeps LU factorization (and permutation); using column pivoting
template <typename Matrix>
class lu_solver
{
    typedef typename mtl::traits::lu_matrix_type<Matrix>::type            matrix_type;
    typedef mtl::vector::dense_vector<std::size_t, vector::parameters<> > permutation_type;
  public:
    /// Construct from matrix \p A and use optionally threshold \p eps in factorization
    explicit lu_solver(const Matrix& A, double eps= 0)
      : LU(A), P(num_rows(A))
    {
	lu(LU, P, eps);
    }

    /// Solve A*x = b with factorization from constructor
    template <typename VectorIn, typename VectorOut>
    void solve(const VectorIn& b, VectorOut& x) const
    {
	x= upper_trisolve(upper(LU), unit_lower_trisolve(strict_lower(LU), VectorIn(matrix::permutation(P) * b)));
    }
    /// Solve \f$adjoint(A)x = b\f$ using LU factorization
    template <typename VectorIn, typename VectorOut>
    void adjoint_solve(const VectorIn& b, VectorOut& x) const
    {
	x= trans(matrix::permutation(P)) * unit_upper_trisolve(adjoint(LU), lower_trisolve(adjoint(LU), b));
    }

  private:
    matrix_type      LU;
    permutation_type P;
};

}} // namespace mtl::matrix

// ### For illustration purposes
#if 0

namespace mtl { namespace matrix {

template <typename Matrix>
void inline lu(Matrix& LU)
{
    MTL_THROW_IF(num_rows(LU) != num_cols(LU), matrix_not_square());

    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename Collection<Matrix>::size_type    size_type;

    size_type n= num_rows(LU);
    for (size_type k= 0; k < num_rows(LU); k++) {
	value_type pivot= LU[k][k];
	for (size_type j= k+1; j < n; j++) {
	    value_type alpha= LU[j][k]/= pivot;
	    for (size_type i= k+1; i < n; i++)
		LU[j][i]-= alpha * LU[k][i];
	}
    }
}

}} // namespace mtl::matrix

#endif

#endif // MTL_MATRIX_LU_INCLUDE


#include <iostream>
#include <typeinfo>
#define BOOST_TEST_MODULE DenseMIAMultTests
#include "MIAConfig.h"
#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

#include "DenseMIA.h"
#include "Index.h"

template<class _data_type>
void mult_work(size_t dim1, size_t dim2){

    LibMIA::MIAINDEX i;
    LibMIA::MIAINDEX j;
    LibMIA::MIAINDEX k;
    LibMIA::MIAINDEX l;
    LibMIA::MIAINDEX m;
    LibMIA::MIAINDEX n;

    LibMIA::DenseMIA<_data_type,4> a(dim1,dim2,dim1,dim2);
    LibMIA::DenseMIA<_data_type,4> b(dim2,dim2,dim1,dim1);
    LibMIA::DenseMIA<_data_type,2> b2(dim2,dim2);

    LibMIA::DenseMIA<_data_type,4> c;
    LibMIA::DenseMIA<_data_type,2> c2;
    LibMIA::DenseMIA<_data_type,4> c_result(dim1,dim1,dim1,dim1);
    LibMIA::DenseMIA<_data_type,2> c_result2(dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> c_result3(dim1,dim2,dim1,dim2);
    LibMIA::DenseMIA<_data_type,4> c_result4(dim2,dim2,dim2,dim2);

    LibMIA::DenseMIA<_data_type,1> d(dim2);
    LibMIA::DenseMIA<_data_type,2> d2(dim2,dim2);

    b.ones();

    //set each the values of a to increment along inner product indices
    size_t val,entries=((dim2*dim2+1)*dim2*dim2)/2;
    for(size_t _i=0;_i<dim1;++_i){
        for(size_t _k=0;_k<dim1;++_k){
            val=1;
            for(size_t _j=0;_j<dim2;++_j){
                for(size_t _l=0;_l<dim2;++_l){
                    a.at(_i,_j,_k,_l)=val++;
                }
            }
        }
    }

    for(size_t _i=0;_i<dim1;++_i){
        for(size_t _j=0;_j<dim1;++_j){

            for(size_t _k=0;_k<dim1;++_k){
                for(size_t _l=0;_l<dim1;++_l){
                    c_result.at(_i,_j,_k,_l)=entries;
                }
            }
        }
    }

    c(i,k,m,n)=a(i,j,k,l)*b(j,l,m,n);
    BOOST_CHECK_MESSAGE(c==c_result,std::string("Inner/Outer Product 1 for ")+typeid(_data_type).name() );

    c(i,k,m,n)=a(i,l,k,j)*b(l,j,m,n);
    BOOST_CHECK_MESSAGE(c==c_result,std::string("Inner/Outer Product 2 for ")+typeid(_data_type).name());

    c(i,k,m,n)=a(i,j,k,l)*b(l,j,m,n);
    BOOST_CHECK_MESSAGE(c==c_result,std::string("Inner/Outer Product 3 for ")+typeid(_data_type).name());

//    test inner and element-wise product. Have b be assigned a scalar value that increases while traversing i
    val=0;
    for(size_t _i=0;_i<dim1;++_i){
        val++;
        for(size_t _j=0;_j<dim1;++_j){
            c_result2.at(_i,_j)=val*entries;
            for(size_t _k=0;_k<dim2;++_k){
                for(size_t _l=0;_l<dim2;++_l){
                    b.at(_k,_l,_i,_j)=val;
                }
            }
        }
    }

    c2(i,j)=a(!i,k,!j,l)*b(k,l,!i,!j);
    BOOST_CHECK_MESSAGE(c2==c_result2,std::string("Inner/Element-Wise Product 1 for ")+typeid(_data_type).name());

    c2(i,j)=a(!i,l,!j,k)*b(k,l,!i,!j);
    BOOST_CHECK_MESSAGE(c2==c_result2,std::string("Inner/Element-Wise Product 2 for ")+typeid(_data_type).name());
    c2(j,i)=a(!j,l,!i,k)*b(k,l,!j,!i);
    BOOST_CHECK_MESSAGE(c2==c_result2,std::string("Inner/Element-Wise Product 3 for ")+typeid(_data_type).name());

    //test inter outer products
    val=1;
    a.ones();
    for(size_t _i=0;_i<dim2;++_i){

        for(size_t _j=0;_j<dim2;++_j){
            b2.at(_i,_j)=val++;
        }
    }

    c_result3.zeros();
    for(size_t _i=0;_i<dim1;++_i){
        for(size_t _k=0;_k<dim1;++_k){
            val=1;
            for(size_t _j=0;_j<dim2;++_j){
                for(size_t _l=0;_l<dim2;++_l){
                    c_result3.at(_i,_j,_k,_l)=val++;
                }
            }
        }
    }

    c(i,j,k,l)=a(i,!j,k,!l)*b2(!j,!l);

    BOOST_CHECK_MESSAGE(c==c_result3,std::string("Outer/Element-Wise Product 1 for ")+typeid(_data_type).name());
    c(i,j,k,l)=a(k,!j,i,!l)*b2(!j,!l);
    BOOST_CHECK_MESSAGE(c==c_result3,std::string("Outer/Element-Wise Product 2 for ")+typeid(_data_type).name());

    c(i,j,k,l)=a(k,!l,i,!j)*b2(!j,!l);
    BOOST_CHECK_MESSAGE(c==c_result3,std::string("Outer/Element-Wise Product 3 for ")+typeid(_data_type).name());

    c_result4.zeros();
    for(size_t _i=0;_i<dim2;++_i){
        for(size_t _j=0;_j<dim2;++_j){
            val=1;
            for(size_t _k=0;_k<dim2;++_k){
                for(size_t _l=0;_l<dim2;++_l){
                    c_result4.at(_i,_j,_k,_l)=val++;
                }
            }
        }
    }
    d2.ones();
    c(i,j,k,l)=b2(k,l)*d2(i,j);
    BOOST_CHECK_MESSAGE(c==c_result4,std::string("Outer/Outer Product 1 for ")+typeid(_data_type).name());

    c(i,j,k,l)=b2(k,l)*d2(j,i);
    BOOST_CHECK_MESSAGE(c==c_result4,std::string("Outer/Outer Product 2 for ")+typeid(_data_type).name());

    c(i,j,k,l)=d2(j,i)*b2(k,l);
    BOOST_CHECK_MESSAGE(c==c_result4,std::string("Outer/Outer Product 3 for ")+typeid(_data_type).name());

    val=1;
    for(size_t _i=0;_i<dim2;++_i)
        d.at(_i)=val++;

    size_t val2;
    c_result3.zeros();
    for(size_t _i=0;_i<dim1;++_i){
        for(size_t _k=0;_k<dim1;++_k){
            val=1;
            for(size_t _j=0;_j<dim2;++_j){
                val2=1;
                for(size_t _l=0;_l<dim2;++_l){
                    c_result3.at(_i,_j,_k,_l)=val++*val2++;
                }
            }
        }
    }

    c(i,j,k,l)=~(a(i,!j,k,!!l)*b2(!j,!!l))*d(!l);
    BOOST_CHECK_MESSAGE(c==c_result3,std::string("Repeated Element-Wise Product 1 for ")+typeid(_data_type).name());

    //repeat inner/outer product test but use a ternary inner product instead
    d2.ones();
    b.ones();
    for(size_t _i=0;_i<dim1;++_i){
        for(size_t _k=0;_k<dim1;++_k){
            val=1;
            for(size_t _j=0;_j<dim2;++_j){
                for(size_t _l=0;_l<dim2;++_l){
                    a.at(_i,_j,_k,_l)=val++;
                }
            }
        }
    }
    c(i,k,m,n)=~(a(i,!j,k,!l)*b(!j,!l,m,n))*d2(j,l);
    BOOST_CHECK_MESSAGE(c==c_result,std::string("Ternary Inner Product 1 for ")+typeid(_data_type).name() );
    c(i,k,m,n)=~(a(i,!j,k,!l)*b(!j,!l,m,n))*d2(l,j);
    BOOST_CHECK_MESSAGE(c==c_result,std::string("Ternary Inner Product 2 for ")+typeid(_data_type).name() );
    c(i,k,m,n)=~(a(i,!l,k,!j)*b(!j,!l,m,n))*d2(l,j);
    BOOST_CHECK_MESSAGE(c==c_result,std::string("Ternary Inner Product 3 for ")+typeid(_data_type).name() );

    //now we test two MIAs get reduced down to data_type because of a pure inner product
    d.ones();
    _data_type _data=d(l)*d(l);
    BOOST_CHECK_MESSAGE(_data==d.dim(0),std::string("Complete reduction 1 via inner product ")+typeid(_data_type).name() );

    d2.ones();
    _data=d2(i,j)*d2(i,j);
    BOOST_CHECK_MESSAGE(_data==d2.dim(0)*d2.dim(1),std::string("Complete reduction 2 via inner product ")+typeid(_data_type).name() );

}

BOOST_AUTO_TEST_CASE( DenseMIAMultTests )
{

       // mult_work<double>(2,2);

    mult_work<double>(3,3);
    mult_work<float>(3,3);
    mult_work<int>(3,3);
    mult_work<long>(3,3);

    mult_work<double>(4,3);
    mult_work<float>(4,3);
    mult_work<int>(4,3);
    mult_work<long>(4,3);

    mult_work<double>(3,4);
    mult_work<float>(3,4);
    mult_work<int>(3,4);
    mult_work<long>(3,4);

}


#ifndef EXPSUM_ESPRIT_HPP
#define EXPSUM_ESPRIT_HPP

#include <armadillo>

namespace expsum
{

//
// Naive implementation of ESPRIT algorithm
//
template <typename T>
class esprit
{
public:
    using size_type   = arma::uword;
    using value_type  = T;
    using vector_type = arma::Col<value_type>;
    using matrix_type = arma::Mat<value_type>;

    using real_type        = typename matrix_type::pod_type;
    using real_vector_type = arma::Col<real_type>;
    using real_matrix      = arma::Mat<real_type>;

    using complex_type        = std::complex<real_type>;
    using complex_vector_type = arma::Col<complex_type>;
    using complex_matrix_type = arma::Mat<complex_type>;

private:
    constexpr static const bool is_complex =
        arma::is_complex<value_type>::value;

    size_type nrows_;
    size_type ncols_;
    size_type nterms_;
    complex_vector_type exponent_;
    complex_vector_type weight_;

public:
    // Default constructor
    esprit() = default;

    //
    // @N number of sample data
    // @L window size. This is equals to the number of
    // rows of generalized Hankel matrix.
    //
    // @M maxumum number of terms used for the exponential sum.
    // `N >= L >= N / 2 + 1 >= 1` and `N - L + 1 >= M >= 1`.
    //
    esprit(size_type N, size_type L)
        : nrows_(L),
          ncols_(N - L + 1),
          nterms_(),
          exponent_(ncols_),
          weight_(ncols_)
    {
        assert(nrows_ >= ncols_ && ncols_ >= 1);
    }
    // Default copy constructor
    esprit(const esprit&) = default;
    // Defautl move constructor
    esprit(esprit&&) = default;
    // Default destructor
    ~esprit() = default;
    // Default assignment operator
    esprit& operator=(const esprit&) = default;
    // Default move assignment operator
    esprit& operator=(esprit&& rhs) = default;

    // @return number of sampling data
    size_type size() const
    {
        return nrows_ + ncols_ - 1;
    }

    // @return number of rows of trajectory matrix
    size_type nrows() const
    {
        return nrows_;
    }

    // @return number of columns of trajectory matrix. This should be a upper
    //         bound of the number of exponential functions.
    size_type ncols() const
    {
        return ncols_;
    }

    //
    //  Reset data sizes and reallocate memories for working space, if
    // necessary.
    //
    // @N number of sample data
    // @L window size. This is equals to the number of rows of generalized
    //    Hankel matrix.
    // @M maxumum number of terms used for the exponential sum.
    //    `N >= L >= N / 2 >= 1` and `N - L + 1 >= M >= 1`.
    //
    void resize(size_type N, size_type L, size_type M)
    {
        nrows_ = L;
        ncols_ = N - L + 1;

        assert(nrows_ >= M && ncols_ >= M && M >= 1);

        nterms_ = 0;
        exponent_.resize(M);
        weight_.resize(M);
    }

    //
    // Compute non-linear approximation of by as the exponential sums.
    //
    // @f values of the target function sampled on the equispaced grid. The
    //    first `size()` elemnets of `f` are used as a sampled data.
    //
    // @eps small positive number `(0 < eps < 1)` that controlls the accuracy of
    //      the fit.
    //
    // @x0 argument of first sampling data
    //
    // @delta spacing between neighbouring sample points
    //
    template <typename U>
    typename std::enable_if<arma::is_arma_type<U>::value>::type
    fit(const U& f, real_type x0, real_type delta, real_type eps);

    //
    // @return Vector view to the exponents.
    //
    auto exponents() const -> decltype(exponent_.head(nterms_))
    {
        return exponent_.head(nterms_);
    }
    //
    // @return Vector view to the weights.
    //
    auto weights() const -> decltype(weight_.head(nterms_))
    {
        return weight_.head(nterms_);
    }

    //
    // Evaluate exponential sum at a point
    //
    complex_type eval_at(real_type x) const
    {
        return arma::sum(arma::exp(-x * exponents()) % weights());
    }

private:
    template <typename U>
    typename std::enable_if<arma::is_arma_type<U>::value>::type
    compute_nodes(const U& f, real_type eps);
    void compute_weights(complex_vector_type& b);
};

template <typename T>
template <typename U>
typename std::enable_if<arma::is_arma_type<U>::value>::type
esprit<T>::fit(const U& f, real_type x0, real_type delta, real_type eps)
{
    assert(f.is_vec() && f.n_elem >= size());
    assert(real_type() < eps && eps < real_type(1));
    compute_nodes(f, eps);
    //
    // Calculate weights of exponentials
    //
    if (nterms_ > 0)
    {
        complex_vector_type b(size());
        for (size_type k = 0; k < size(); ++k)
        {
            b(k) = f(k);
        }
        compute_weights(b);
        //
        // Adjust computed paremeters
        //
        auto xi_ = exponent_.head(nterms_);
        auto w_  = weight_.head(nterms_);
        xi_      = -arma::log(xi_) / delta;
        if (x0 != real_type())
        {
            w_ %= arma::exp(-xi_ * x0);
        }
    }
}

template <typename T>
template <typename U>
typename std::enable_if<arma::is_arma_type<U>::value>::type
esprit<T>::compute_nodes(const U& f, real_type eps)
{
    //
    // Setup general fast Hankel matrix-vector product.
    //
    matrix_type H(nrows(), ncols());
    for (size_type j = 0; j < ncols(); ++j)
    {
        for (size_type i = 0; i < nrows(); ++i)
        {
            H(i, j) = f(i + j);
        }
    }
    // Compute SVD of Hankel matrix H.f
    real_vector_type sigma(ncols());
    matrix_type W(ncols(), ncols()); // right singular vector
    matrix_type dummy;               // left singular vector (not computed)
    arma::svd_econ(dummy, sigma, W, H, "right");

    size_type rank = 1;
    auto cutoff    = sigma(0) * eps;
    while (rank < sigma.size())
    {
        if (sigma(rank) < cutoff)
        {
            break;
        }
        ++rank;
    }

    auto W0 = W.submat(arma::span(0, ncols() - 2), arma::span(0, rank - 1));
    auto W1 = W.submat(arma::span(1, ncols() - 1), arma::span(0, rank - 1));
    //
    // Compute eigenvalue of matrix pencil zA - B, with matrix A, B defined
    // as follows:
    //
    matrix_type A(W0.t() * W0);
    matrix_type B(W1.t() * W0);

    complex_vector_type eigvals(exponent_.memptr(), rank,
                                /*copy_aux_mem*/ false, /*struct*/ true);
    if (!arma::eig_pair(eigvals, B, A))
    {
        throw std::runtime_error("(esprit::fit) eig_pair failed...");
    }

    nterms_ = rank;
}

template <typename T>
void esprit<T>::compute_weights(complex_vector_type& b)
{
    //
    // Solve (dual) Vanermonde system  V * x = b
    //
    //
    // REMARK: Here exponent(i) contains the value of exp(zeta(i)), not
    // exponet zeta(i) itself.
    //

    complex_matrix_type V(b.size(), nterms_);
    for (size_type i = 0; i < V.n_cols; ++i)
    {
        const auto z = exponent_(i);
        V(0, i) = real_type(1);
        for (size_type j = 1; j < V.n_rows; ++j)
        {
            V(j, i) = V(j - 1, i) * z; // z[i]**j
        }
    }

    complex_vector_type x(weight_.memptr(), nterms_, /*copy_aux_mem*/ false,
                          /*strict*/ true);
    arma::solve(x, V, b);
    return;
}

} // namespace: expsum

#endif /* EXPSUM_ESPRIT_HPP */


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include <boost/random.hpp>
#include "crocoddyl/core/solvers/box-qp.hpp"
#include "unittest_common.hpp"

using namespace boost::unit_test;
using namespace crocoddyl_unit_test;

void test_constructor() {
  // Setup the test
  std::size_t nx = random_int_in_range(1, 100);
  crocoddyl::BoxQP boxqp(nx);

  // Test dimension of the decision vector
  BOOST_CHECK(boxqp.get_nx() == nx);
}

void test_unconstrained_qp_with_identity_hessian() {
  std::size_t nx = random_int_in_range(2, 5);
  crocoddyl::BoxQP boxqp(nx);
  boxqp.set_reg(0.);

  Eigen::MatrixXd hessian = Eigen::MatrixXd::Identity(nx, nx);
  Eigen::VectorXd gradient = Eigen::VectorXd::Random(nx);
  Eigen::VectorXd lb = -std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
  Eigen::VectorXd ub = std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
  Eigen::VectorXd xinit(nx);
  crocoddyl::BoxQPSolution sol = boxqp.solve(hessian, gradient, lb, ub, xinit);

  // Checking the solution of the problem. Note that it the negative of the gradient since Hessian
  // is identity matrix
  BOOST_CHECK((sol.x + gradient).isMuchSmallerThan(1.0, 1e-9));

  // Checking the solution against a regularized case
  double reg = random_real_in_range(1e-9, 1e2);
  boxqp.set_reg(reg);
  crocoddyl::BoxQPSolution sol_reg = boxqp.solve(hessian, gradient, lb, ub, xinit);
  BOOST_CHECK((sol_reg.x + gradient / (1 + reg)).isMuchSmallerThan(1.0, 1e-9));

  // Checking the all bounds are free and zero clamped
  BOOST_CHECK(sol.free_idx.size() == nx);
  BOOST_CHECK(sol.clamped_idx.size() == 0);
  BOOST_CHECK(sol_reg.free_idx.size() == nx);
  BOOST_CHECK(sol_reg.clamped_idx.size() == 0);
}

void test_unconstrained_qp() {
  std::size_t nx = random_int_in_range(2, 5);
  crocoddyl::BoxQP boxqp(nx);
  boxqp.set_reg(0.);

  Eigen::MatrixXd H = Eigen::MatrixXd::Random(nx, nx);
  Eigen::MatrixXd hessian = H.transpose() * H;
  hessian = 0.5 * (hessian + hessian.transpose()).eval();
  Eigen::VectorXd gradient = Eigen::VectorXd::Random(nx);
  Eigen::VectorXd lb = -std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
  Eigen::VectorXd ub = std::numeric_limits<double>::infinity() * Eigen::VectorXd::Ones(nx);
  Eigen::VectorXd xinit(nx);
  crocoddyl::BoxQPSolution sol = boxqp.solve(hessian, gradient, lb, ub, xinit);

  // Checking the solution against the KKT solution
  Eigen::VectorXd xkkt = -hessian.inverse() * gradient;
  BOOST_CHECK((sol.x - xkkt).isMuchSmallerThan(1.0, 1e-9));

  // Checking the solution against a regularized KKT problem
  double reg = random_real_in_range(1e-9, 1e2);
  boxqp.set_reg(reg);
  crocoddyl::BoxQPSolution sol_reg = boxqp.solve(hessian, gradient, lb, ub, xinit);
  Eigen::VectorXd xkkt_reg = -(hessian + reg * Eigen::MatrixXd::Identity(nx, nx)).inverse() * gradient;
  BOOST_CHECK((sol_reg.x - xkkt_reg).isMuchSmallerThan(1.0, 1e-9));

  // Checking the all bounds are free and zero clamped
  BOOST_CHECK(sol.free_idx.size() == nx);
  BOOST_CHECK(sol.clamped_idx.size() == 0);
  BOOST_CHECK(sol_reg.free_idx.size() == nx);
  BOOST_CHECK(sol_reg.clamped_idx.size() == 0);
}

void test_box_qp_with_identity_hessian() {
  std::size_t nx = random_int_in_range(2, 5);
  crocoddyl::BoxQP boxqp(nx);
  boxqp.set_reg(0.);

  Eigen::MatrixXd hessian = Eigen::MatrixXd::Identity(nx, nx);
  Eigen::VectorXd gradient = Eigen::VectorXd::Ones(nx);
  for (std::size_t i = 0; i < nx; ++i) {
    gradient(i) *= random_real_in_range(-1., 1.);
  }
  Eigen::VectorXd lb = Eigen::VectorXd::Zero(nx);
  Eigen::VectorXd ub = Eigen::VectorXd::Ones(nx);
  Eigen::VectorXd xinit(nx);
  crocoddyl::BoxQPSolution sol = boxqp.solve(hessian, gradient, lb, ub, xinit);

  // The analytical solution is the a bounded, and negative, gradient
  Eigen::VectorXd negbounded_gradient(nx), negbounded_gradient_reg(nx);
  std::size_t nf = nx, nc = 0, nf_reg = nx, nc_reg = 0;
  double reg = random_real_in_range(1e-9, 1e2);
  for (std::size_t i = 0; i < nx; ++i) {
    negbounded_gradient(i) = std::max(std::min(-gradient(i), ub(i)), lb(i));
    negbounded_gradient_reg(i) = std::max(std::min(-gradient(i) / (1 + reg), ub(i)), lb(i));
    if (negbounded_gradient(i) != -gradient(i)) {
      nc += 1;
      nf -= 1;
    }
    if (negbounded_gradient_reg(i) != -gradient(i) / (1 + reg)) {
      nc_reg += 1;
      nf_reg -= 1;
    }
  }

  // Checking the solution of the problem. Note that it the negative of the gradient since Hessian
  // is identity matrix
  BOOST_CHECK((sol.x - negbounded_gradient).isMuchSmallerThan(1.0, 1e-9));

  // Checking the solution against a regularized case
  boxqp.set_reg(reg);
  crocoddyl::BoxQPSolution sol_reg = boxqp.solve(hessian, gradient, lb, ub, xinit);
  BOOST_CHECK((sol_reg.x - negbounded_gradient / (1 + reg)).isMuchSmallerThan(1.0, 1e-9));

  // Checking the all bounds are free and zero clamped
  BOOST_CHECK(sol.free_idx.size() == nf);
  BOOST_CHECK(sol.clamped_idx.size() == nc);
  BOOST_CHECK(sol_reg.free_idx.size() == nf_reg);
  BOOST_CHECK(sol_reg.clamped_idx.size() == nc_reg);
}

void register_unit_tests() {
  framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_constructor)));
  framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_unconstrained_qp_with_identity_hessian)));
  framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_unconstrained_qp)));
  framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_box_qp_with_identity_hessian)));
}

bool init_function() {
  register_unit_tests();
  return true;
}

int main(int argc, char* argv[]) { return ::boost::unit_test::unit_test_main(&init_function, argc, argv); }


#include "complexfun.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
#include <cmath>
#include <string>
#include <complex>

namespace
{
    const double pi= std::acos(-1.0);
    void validate(PASTNode astnode, ParsersHelper& ph, const std::string& fnname)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=2)
          throw std::runtime_error(fnname+" should have exactly one argument");
        auto secondCh = *astnode->ch.rbegin();
        ph.parse(secondCh);
        if (secondCh->token.tokenType != Complex)
          throw std::runtime_error("The argument of "+fnname+" must be Complex");
    }
}

namespace HT
{
    void magnitude(PASTNode astnode, ParsersHelper& ph)
    {
        validate(astnode, ph, "magnitude");
        auto w (boost::get<ComplexType>((*astnode->ch.rbegin())->token.info));
        w.toinexact();
        astnode->type = Simple;
        astnode->token.tokenType = Complex;

        auto res = std::hypotl(w.getRealD(), w.getImagD());
        astnode->token.info = ComplexType(res);

        astnode->remove();
    }
    void angle(PASTNode astnode, ParsersHelper& ph)
    {
        validate(astnode, ph, "magnitude");
        auto w (boost::get<ComplexType>((*astnode->ch.rbegin())->token.info));
        w.toinexact();
        astnode->type = Simple;
        astnode->token.tokenType = Complex;

        auto res = std::atan2(w.getImagD(), w.getRealD());
        if (std::fabs(res+ pi)<1e-6) res = -res;
        astnode->token.info = ComplexType(res);

        astnode->remove();
    }

}


/*
MIT License

Copyright (c) 2019 Xiaohong Chen

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#ifndef BOOST_UBLAS_GMRES_HPP
#define BOOST_UBLAS_GMRES_HPP

#include "preconditioner.hpp"
#include "krylov_solvers_config.hpp"

#include <tuple>
#include <cmath>
#include <type_traits>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/operation.hpp>

// test
#include <iostream>
#include <boost/numeric/ublas/io.hpp>

namespace boost { namespace numeric { namespace ublas {

namespace detail {
// Calculate the Given rotation matrix
template<typename T>
BOOST_UBLAS_INLINE std::tuple<T, T> givens_rotation(T x1, T x2) {
    T d = sqrt(x1*x1 + x2*x2);
    if (d == T/*zero*/()) divide_by_zero().raise();
    return std::make_tuple(x1/d, -x2/d);
}

// apply Givens rotations on the jth column of the upper Hessenberg matrix.
template<class V1, class V2>
void apply_givens_rotation(V1& h, V2& cs, V2& sn,
                           typename vector_traits<V2>::size_type j) {
    typedef typename V2::size_type size_type;
    typedef typename V2::value_type value_type;

    BOOST_UBLAS_CHECK (cs.size() == sn.size(), bad_size ());
    BOOST_UBLAS_CHECK (cs.size() > j, bad_size ());

    // apply Givens rotation over the first j - 1 entries on jth column of the upper Hessenberg matrix.
    for (size_type i = 0; i < j; ++i) {
        value_type tmp = cs(i)*h(i) - sn(i)*h(i+1);
        h(i+1) = sn(i)*h(i) + cs(i)*h(i+1);
        h(i) = tmp;
    }
    // update Givens rotation matrix.
    if (j < h.size() - 1) {
        BOOST_UBLAS_CHECK (h.size() > j + 1, bad_size ());
        std::tie(cs(j), sn(j)) = givens_rotation(h(j), h(j+1));
        // eliminate h(j+1).
        h(j) = cs(j)*h(j) - sn(j)*h(j+1);
        h(j+1) = value_type/*zero*/();
    }
    else {
        cs(j) = value_type(1.0);
        sn(j) = value_type/*zero*/();
    }
}

template<class F1, class F2, class M, class V>
void arnoldi(const F1& A, const F2& PInv, M& Q, V& h, typename matrix_traits<M>::size_type j) {
    typedef typename M::size_type size_type;
    typedef typename M::value_type value_type;
    typedef M matrix_type;
    typedef vector<value_type> vector_type;

    BOOST_UBLAS_CHECK(Q.size2() > j + 1, bad_size());

    matrix_column<matrix_type> q(Q, j+1);
    q.assign(PInv(A(column(Q, j))));
    size_type size1 = Q.size1();
    vector_range<V> hr(h, range(0, j+1));
    // CGS2
    axpy_prod(q, project(Q, range(0, size1), range(0, j+1)), hr, true);
    axpy_prod(project(Q, range(0, size1), range(0, j+1)), -hr, q, false);
    vector_type tmp(j+1, 0.0);
    axpy_prod(q, project(Q, range(0, size1), range(0, j+1)), tmp, false);
    axpy_prod(project(Q, range(0, size1), range(0, j+1)), -tmp, q, false);
    hr.plus_assign(tmp);

    if (j < size1 - 1) {
        BOOST_UBLAS_CHECK(h.size() > j + 1, bad_size());
        h(j+1) = norm_2(q);
        q /= h(j+1);
    }
    else {
        q *= value_type/*zero*/();
    }
}

template <typename T>
class is_matrix_expression
{
private:
    template<typename E>
    static constexpr std::true_type  test(const matrix_expression<E> *);
    static constexpr std::false_type test(...);
public:
    static constexpr bool value = decltype(test(std::declval<T*>()))::value;
    typedef decltype(test(std::declval<T*>())) type;
};

template <typename T>
constexpr bool is_matrix_expression_v = is_matrix_expression<T>::value;

template <typename T>
using is_matrix_expression_t = typename is_matrix_expression<T>::type;

template<class F1, class F2, class E, class V, typename Int, typename Floating>
std::tuple<Int, Floating>
gmres_impl(const F1& A, const F2& PInv, const vector_expression<E>& b, V& x,
           Int max_iter_, Int restart_iter_, Floating tol_, std::false_type) {
    typedef typename V::value_type value_type;
    typedef typename V::size_type size_type;
    typedef matrix<value_type, column_major> matrix_type;
    typedef banded_matrix<value_type, column_major> banded_matrix_type;
    typedef vector<value_type> vector_type;
    typedef unit_vector<value_type> unit_vector_type;
    typedef std::tuple<Int, Floating> return_type;

    BOOST_UBLAS_CHECK(b().size() == x.size(), bad_size());
    BOOST_UBLAS_CHECK(restart_iter_ > Int/*zero*/(), bad_argument());
    BOOST_UBLAS_CHECK(max_iter_ > Int/*zero*/(), bad_argument());
    BOOST_UBLAS_CHECK(tol_ > Floating/*zero*/(), bad_argument());

    size_type n = x.size();
    // adjust max_iter and restart_iter
    size_type max_iter = std::min(n, static_cast<size_type>(max_iter_));
    size_type restart_iter = std::min(n, static_cast<size_type>(restart_iter_));
    value_type tol = static_cast<value_type>(tol_);
    if (restart_iter > max_iter) restart_iter = max_iter;

    vector_type r = PInv(b - A(x));
    value_type r_norm = norm_2(r);
    value_type b_norm = norm_2(PInv(b));
    if (b_norm == value_type/*zero*/()) {
        x *= value_type/*zero*/();
        return return_type(Int/*zero*/(), Floating/*zero*/());
    }
    value_type error = norm_2(r) / b_norm;
    if (error < tol) return return_type(Int/*zero*/(), error);

    vector_type sn(restart_iter, value_type/*zero*/());
    vector_type cs(restart_iter, value_type/*zero*/());

    matrix_type Q(n, restart_iter + 1, value_type/*zero*/());
    banded_matrix_type H(n, restart_iter, 1, restart_iter);

    size_type num_iter = 1;
    while (num_iter <= max_iter) {
        column(Q, 0).assign(r / r_norm);
        vector_type beta = r_norm * unit_vector_type(restart_iter + 1, 0);

        size_type j = size_type/*zero*/();
        for (; j < restart_iter && num_iter <= max_iter; ++j, ++num_iter) {
            // run arnoldi
            matrix_column<banded_matrix_type> H_j(H, j);
            detail::arnoldi(A, PInv, Q, H_j, j);

            // eliminate the last element in H jth column and update the rotation matrix
            detail::apply_givens_rotation(H_j, cs, sn, j);

            // update the residual vector
            beta(j+1) = sn(j)*beta(j);
            beta(j) = cs(j)*beta(j);
            error  = std::abs(beta(j+1)) / b_norm;

            if (error <= tol) {
                // update x
                auto y = solve(project(H, range(0, j+1), range(0, j+1)),
                               project(beta, range(0, j+1)), upper_tag());
                axpy_prod(project(Q, range(0, n), range(0, j+1)), y, x, false);
                return return_type(num_iter, error);
            }
        }
        // update x
        auto y = solve(project(H, range(0, j), range(0, j)),
                       project(beta, range(0, j)), upper_tag());
        axpy_prod(project(Q, range(0, n), range(0, j)), y, x, false);

        r = PInv(b - A(x));
        r_norm = norm_2(r);
        error = norm_2(r) / b_norm;
        if (error < tol) return return_type(num_iter, error);
    }
    return return_type(max_iter, error);
}

template<class M, class F, class E, class V, typename Int, typename Floating>
std::tuple<Int, Floating>
gmres_impl(const M& A, const F& PInv, const vector_expression<E>& b, V& x,
           Int max_iter, Int restart_iter, Floating tol, std::true_type) {
    return gmres_impl([&A](const auto& v){return ublas::prod(A, v);}, PInv,
                      b, x, max_iter, restart_iter, tol, std::false_type());
}

} // end namespace detail

template <class M, class F = identity_precond<M> >
class gmres {
public:
    // param
    struct param {
        int max_iter = 10;
        int restart_iter = 10;
        double tol = 1e-6;
    };

    typedef std::tuple<int, double> return_type;

    gmres(const M& A)
        : A_(A), PInv_(A) {}

    gmres(const M& A, const param& p)
        : A_(A), PInv_(A), param_(p) {}

    gmres(const M& A, const F& PInv)
        : A_(A), PInv_(PInv) {}

    gmres(const M& A, F&& PInv)
        : A_(A), PInv_(std::move(PInv)) {}

    gmres(const M& A, const F& PInv, const param& p)
        : A_(A), PInv_(PInv), param_(p) {}

    gmres(const M& A, F&& PInv, const param& p)
        : A_(A), PInv_(PInv), param_(p) {}

    gmres(const gmres&) = delete;
    gmres(gmres&&) = delete;
    gmres& operator=(const gmres&) = delete;
    gmres& operator=(gmres&&) = delete;

    param get_param() const {
        return param_;
    }

    void set_param(const param& p) {
        param_ = p;
    }

    template<class E, class V>
    return_type operator()(const vector_expression<E>& b, V& x) const {
        return detail::gmres_impl(A_, PInv_, b, x,
                                  param_.max_iter,
                                  param_.restart_iter,
                                  param_.tol,
                                  detail::is_matrix_expression_t<M>());
    }

private:
    const M& A_;
    F PInv_;
    param param_;
};

} // end namespace ublas
} // end namespace numeric
} // end namespace boost

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

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

//[boost_fusion
//` Shows how to combine Boost.Fusion with Boost.Geometry

#include <iostream>

#include <boost/fusion/include/adapt_struct_named.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/adapted/boost_fusion.hpp>

struct sample_point
{
    double x, y, z;
};

BOOST_FUSION_ADAPT_STRUCT(sample_point, (double, x) (double, y) (double, z))
BOOST_GEOMETRY_REGISTER_BOOST_FUSION_CS(cs::cartesian)

int main()
{
    sample_point a, b, c;

    // Set coordinates the Boost.Geometry way (one of the ways)
    boost::geometry::assign_values(a, 3, 2, 1);

    // Set coordinates the Boost.Fusion way
    boost::fusion::at_c<0>(b) = 6;
    boost::fusion::at_c<1>(b) = 5;
    boost::fusion::at_c<2>(b) = 4;

    // Set coordinates the native way
    c.x = 9;
    c.y = 8;
    c.z = 7;

    std::cout << "Distance a-b: " << boost::geometry::distance(a, b) << std::endl;
    std::cout << "Distance a-c: " << boost::geometry::distance(a, c) << std::endl;

    return 0;
}

//]

//[boost_fusion_output
/*`
Output:
[pre
Distance a-b: 5.19615
Distance a-c: 10.3923
]
*/
//]


/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */

//#include "sp/util/timing.hpp"
//#include "sp/util/rand.hpp"
//#include "sp/util/typename.hpp"

#include <iostream>
#include <iomanip>
#define BOOST_TEST_MODULE sp_algo_nn
#include <boost/test/unit_test.hpp>
#include "sp/algo/nn.hpp"
#include "sp/algo/nn/gradient_check.hpp"
#include "assert_matrix.hpp"

using namespace sp::algo::nn;
using namespace sp::testing;

BOOST_AUTO_TEST_CASE(test_activation_tanh_fprop_bprop) {

    using tanh_act_layer = tanh_layer<volume_dims<1, 3, 3>>;
    tanh_act_layer layer;

    layer.configure(1, true);

    /* stage 2 */
    tensor_4 in = generate_inputs_for(layer);
    tensor_4 out(1, 1, 3, 3);

    tensor_4 prev_delta(1, 1, 3, 3);
    tensor_4 out_delta(1, 1, 3, 3);
    tensor_4 expected_out(1, 1, 3, 3);
    tensor_4 expected_prev_delta(1, 1, 3, 3);

    in.setValues({{{
        {0.0f, 1.0f, 2.0f},
        {1.0f, 2.0f, 3.0f},
        {2.0f, 3.0f, 4.0f},
    }}});
    prev_delta.setZero();
    out_delta.setValues({{{
        {-1.0f, 2.0f, 3.0f},
        { 4.0f, 5.0f, 6.0f},
        { 7.0f, 8.0f, 9.0f}
    }}});

    /* perform activation forward propagation*/
    layer.forward_prop(in, out);

    BOOST_REQUIRE((std::is_same_v<tanh_act_layer::activation_op_type, tanh_activation_op>));
    auto op = tanh_act_layer::activation_op_type();
    auto dop = tanh_act_layer::activation_op_deriv_type();
    expected_out.setValues({{{ //Simply performs acti(x) for every value in input
        {op(in(0, 0, 0, 0)), op(in(0, 0, 0, 1)), op(in(0, 0, 0, 2))},
        {op(in(0, 0, 1, 0)), op(in(0, 0, 1, 1)), op(in(0, 0, 1, 2))},
        {op(in(0, 0, 2, 0)), op(in(0, 0, 2, 1)), op(in(0, 0, 2, 2))},
    }}});

    assert_tensor_equals(expected_out, out);

    layer.backward_prop(in, prev_delta, out, out_delta);

    expected_prev_delta.setValues({{{ //act_delta(x, y, ...) = prev_delta(x, y, ...) * deriv_op(prev_delta(x, y, ...))
        {out_delta(0, 0, 0, 0) * dop(out(0, 0, 0, 0)), out_delta(0, 0, 0, 1) * dop(out(0, 0, 0, 1)), out_delta(0, 0, 0, 2) * dop(out(0, 0, 0, 2))},
        {out_delta(0, 0, 1, 0) * dop(out(0, 0, 1, 0)), out_delta(0, 0, 1, 1) * dop(out(0, 0, 1, 1)), out_delta(0, 0, 1, 2) * dop(out(0, 0, 1, 2))},
        {out_delta(0, 0, 2, 0) * dop(out(0, 0, 2, 0)), out_delta(0, 0, 2, 1) * dop(out(0, 0, 2, 1)), out_delta(0, 0, 2, 2) * dop(out(0, 0, 2, 2))},
    }}});

    assert_tensor_equals(expected_prev_delta, prev_delta);
}
BOOST_AUTO_TEST_CASE(test_activation_layer_grad_check) {
    using layer_type = tanh_layer<volume_dims<10, 3, 3>>;

    constexpr float_t epsilon = 1e-2f;
    constexpr size_t batch_size = 1;
    layer_type layer;

    layer.weight_initializer = gauss_weight_initializer(-1.0f, 1.0f);
    layer.bias_initializer = gauss_weight_initializer(-1.0f, 1.0f);

    /* setup weights and dimensions */
    layer.configure(batch_size, true);

    auto in = generate_inputs_for(layer, batch_size);

    for(size_t i = 0; i < 100; ++i) {
        auto in_selected  = gradient_random_input(layer);
        auto out_selected = gradient_random_output(layer);

        /* Perform numerical and analytics gradient */
        /* estimate */
        auto n = numerical_gradient (layer, in, in_selected, out_selected);
        /* actual */
        auto a = analytical_gradient(layer, in, in_selected, out_selected);

        /* Validate result */
        BOOST_CHECK_MESSAGE(std::abs(a-n) <= epsilon, "Gradient check |" << std::setprecision(15) << a << " - " << n << "| < " << epsilon);
    }
}


#ifndef FILTER_HPP
# define FILTER_HPP

#include <Eigen/Dense>

#include "nav_types.hpp"
#include "prop.hpp"

DiagonalMatrix<double,9> compute_process_noise(const double& dt) {
  DiagonalMatrix<double,9> Q;
  double qa = Q_ACCEL_PSD * dt,
    qv = qa * dt * 0.5,
    qr = qv * dt * 2.0 / 3.0,
    qw = Q_GYRO_PSD * dt;
  Q.diagonal() << qr, qr, qr, qa, qa, qa, qw, qw, qw;
  return Q;
}

class Filter {
public:
  Filter(const double& time, const Eigen::Matrix<double,6,1>& rv)
    : Phi(Cov::Identity())
    , P(Cov::Zero())
    , X(State::Zero())
    , dX(State::Zero())
  {
    // Initialize covariance to some reasonable values
    P.diagonal() <<
      rv,
      0.5, 0.5, 0.5,
      0.0001, 0.0001, 0.0001,
      0.00001, 0.00001, 0.00001,
      0.1, 0.1, 0.1,
      0.1, 0.1, 0.1,
      0.1, 0.1, 0.1,
      0.1, 0.1, 0.1,
      0.1, 0.1, 0.1;

    // Initialize state to some reasonable values
    X[0] = 6375000.0;
    X[4] = 10.0;
  }

  /** @brief Having propagated for a while in a Prop object, prepare
   **        the filter to receive an update at or just before the current
   **        time.
   *
   * This method computes the covariance at the current time using the
   * posterior covariance from the last update and the propagated
   * state transition matrix.
   *
   */
  void prepare_for_update(const Prop& prop) {
    double dt = prop.time - time;
    DiagonalMatrix<double,9> Q = compute_process_noise(dt);

    Phi << prop.Phi_xx, prop.Phi_xb, Matrix<double,21,30>::Zero();
    for (size_t ii = 0; ii < 21; ++ii) {
      Phi(ii+9,ii+9) = prop.Phi_bb.diagonal()(ii);
    }

    P = Phi * (P * Phi.transpose());
    for (size_t ii = 0; ii < 9; ++ii) {
      P(ii,ii) += Q.diagonal()(ii);
    }

    for (size_t ii = 0; ii < 3; ++ii) {
      X[ii]    = prop.r_imu_inrtl(ii);
      X[ii+3]  = prop.v_imu_inrtl(ii);
      X[ii+6]  = 0.0;
      X[ii+9]  = prop.b_acc(ii);
      X[ii+12] = prop.b_gyro(ii);
    }

    time = prop.time;
    // FIXME: Make sure this is symmetric after.
  }

  double time; /* (s)  time of state/covariance validity */
  Cov Phi;     /* (--) state transition matrix */
  Cov P;       /* (--) state covariance */
  State X;     /* (--) state */
  State dX;    /* (--) pending state update */
};

#endif


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_2.h>
#include <CGAL/Triangulation_face_base_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_with_info_2.h>
#include <boost/pending/disjoint_sets.hpp>
#include <vector>
#include <tuple>
#include <algorithm>
#include <iostream>
#include <queue>
#include <limits>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_2<K>   Vb;
typedef CGAL::Triangulation_face_base_with_info_2<int, K>  Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>            Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds>                  Delaunay;

typedef K::FT FT;

using namespace std;

struct Neighbor {
  int vertex;
  FT width;
};

void solve(int numPoints) {

  long x, y;
  vector<K::Point_2> pts(numPoints);
  for (int i = 0; i < numPoints; ++i) {
    cin >> x >> y;
    pts[i] = {x, y};
  }
  Delaunay dt;
  dt.insert(pts.begin(), pts.end());

  int id = 1;
  for (auto f = dt.finite_faces_begin(); f != dt.finite_faces_end(); ++f) {
    f->info() = id++;
  }

  int n = id;
  vector<vector<Neighbor>> neighbors(n);
  for (auto f = dt.finite_faces_begin(); f != dt.finite_faces_end(); ++f) {
    int u = f->info();

    for (int i = 0; i < 3; ++i) {
      auto f2 = f->neighbor(i);
      int v = dt.is_infinite(f2) ? 0 : f2->info();
      auto p1 = f->vertex((i + 1) % 3)->point();
      auto p2 = f->vertex((i + 2) % 3)->point();
      FT width = CGAL::squared_distance(p1, p2);

      neighbors[u].push_back({v, width});
      if (v == 0) {
        neighbors[v].push_back({u, width});
      }
    }
  }

  FT maxValue = FT(numeric_limits<long>::max());

  vector<FT> width(n, -1);
  priority_queue<tuple<FT, int>> queue;
  queue.push({maxValue, 0});

  while (!queue.empty()) {

    FT w = get<0>(queue.top());
    int u = get<1>(queue.top());
    queue.pop();

    if (width[u] != -1) {
      continue;
    }

    width[u] = w;

    for (Neighbor n : neighbors[u]) {
      FT wn = CGAL::min(n.width, w);
      queue.push({wn, n.vertex});
    }
  }

  int m;
  cin >> m;
  long d;
  for (int i = 0; i < m; ++i) {
    cin >> x >> y >> d;

    K::Point_2 p(x, y);

    if (CGAL::squared_distance(p, dt.nearest_vertex(p)->point()) < FT(d)) {
      cout << "n";
      continue;
    }

    auto f = dt.locate(p);
    int v = dt.is_infinite(f) ? 0 : f->info();

    if (width[v] >= FT(4) * FT(d)) {
      cout << "y";
    }
    else {
      cout << "n";
    }

  }
  cout << endl;
}

int main() {
  ios_base::sync_with_stdio(false);
  int n;
  cin >> n;
  while (n != 0) {
    solve(n);
    cin >> n;
  }
  return 0;
}


#include <iostream>
#include <vector>
#include <fstream>
#include <string> // for string class
#include <math.h>
#include <iostream>
#include <cmath>
#include <array>
#include <complex>
#include<time.h>
#include "mex.h"
#include <omp.h>

//#define EIGEN_USE_MKL_ALL
#include <Eigen/Dense>
// #include <Eigen/Sparse>
// #include <unsupported/Eigen/CXX11/Tensor>
// #include <Eigen/LU>
// #include <unsupported/Eigen/KroneckerProduct>
#include <Eigen/Core>
// #include <Eigen/SVD>
// #include<unsupported/Eigen/SparseExtra>

#define PI acos(-1.0)

using namespace Eigen;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  //*********************************************

  double *val_Node_Coord = (double *) mxGetPr(mxGetField(prhs[0], 0, "Node_Coord"));
  int m_Node_Coord = mxGetM(mxGetField(prhs[0], 0, "Node_Coord"));
  int n_Node_Coord = mxGetN(mxGetField(prhs[0], 0, "Node_Coord"));
  Eigen::MatrixXd Node_Coord = Map < MatrixXd > (val_Node_Coord,m_Node_Coord,n_Node_Coord);

  double *val_Node_Coord_ndgrid = (double *) mxGetPr(mxGetField(prhs[0], 0, "Node_Coord_ndgrid"));
  int m_Node_Coord_ndgrid = mxGetM(mxGetField(prhs[0], 0, "Node_Coord_ndgrid"));
  int n_Node_Coord_ndgrid = mxGetN(mxGetField(prhs[0], 0, "Node_Coord_ndgrid"));
  Eigen::MatrixXd Node_Coord_ndgrid = Map < MatrixXd > (val_Node_Coord_ndgrid,m_Node_Coord_ndgrid,n_Node_Coord_ndgrid);

  double *val_node_index_total= (double *) mxGetPr(mxGetField(prhs[0], 0, "node_index_total"));
  int m_node_index_total = mxGetM(mxGetField(prhs[0], 0, "node_index_total"));
  Eigen::VectorXd node_index_total = Map < VectorXd > (val_node_index_total,m_node_index_total);

  int nnode=Node_Coord_ndgrid.rows();
  Eigen::MatrixXd corr_index(nnode,1); corr_index.setZero();

// Eigen::MatrixXd a(3,3);
// a<<1,2,3,
// 4,5,6,
// 7,8,9;
// std::cout << a<< '\n';
// Eigen::VectorXd b=Eigen::Map<VectorXd>(a.data(), a.cols()*a.rows());
// std::cout << b.transpose() << '\n';
// std::cout <<Eigen::Map<VectorXd>(a.data(), a.cols()*a.rows()) << '\n';

  // int p, n;
  // double sum;
  // sum = 0.0;
  // n = 100;
  // #pragma omp parallel for shared(n) private(p) reduction(+: sum)
  // for(p = 0; p<n; p++){
  //   sum += 1.0;
  //   std::cout << "/* message */"<< sum << '\n';
  // }
  // mexPrintf("sum = %f\n",sum);
  int i=0;
  #pragma omp parallel for private(i) shared(Node_Coord_ndgrid,Node_Coord,node_index_total,corr_index)
  for (i = 0; i < nnode; i++) {
    Eigen::Vector3d coord = Node_Coord_ndgrid.row(i);
    MatrixXf::Index index;

    // Eigen::MatrixXd temp=Node_Coord.rowwise()-coord.transpose();
    // Eigen::VectorXd coordx = temp.rowwise().norm();;
    // std::cout << "/* ***** */" << '\n';
    // std::cout <<  coord.transpose() << '\n';
    // // std::cout << "/* message */" << '\n';
    // // std::cout << temp.block<5,3>(0,0) << '\n';
    // std::cout << "/* message */" << '\n';
    // std::cout << coordx.head(5) << '\n';
    // std::cout << "/* message */" << '\n';
    // coordx.minCoeff(&index);
    // std::cout << index << " ***  "<< node_index_total(index)<< '\n';

    (Node_Coord.rowwise()-coord.transpose()).rowwise().norm().minCoeff(&index);
    // std::cout << index << " ***  "<< node_index_total(index)<< '\n';
    corr_index(i,0)=node_index_total(index);
  }

  //	* Out put *//* Out put *//* Out put *//* Out put *//* Out put *//* Out put */
  plhs[0] = mxCreateDoubleMatrix((mwSize) nnode, (mwSize) 1, mxREAL);
  double *corr_index_out = mxGetPr(plhs[0]); // pointer pr_out will manage data in COLUMN Major.
  Eigen::Map < MatrixXd > (corr_index_out, nnode,1) = corr_index;
};


#include "TestWindow.h"
#include "planecalib/PlaneCalibSystem.h"
#include "planecalib/HomographyCalibration.h"
#include "planecalib/Map.h"
#include "planecalib/PoseTracker.h"
#include "../PlaneCalibApp.h"
#include <opencv2/imgproc.hpp>
#include <Eigen/Dense>
#include <ceres/ceres.h>

namespace planecalib
{
void TestWindow::showHelp() const
{
	BaseWindow::showHelp();
	MYAPP_LOG << "Just tests.";
}

class ImageInterpolation : public ceres::SizedCostFunction<1, 2> {
public:
	ImageInterpolation(const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy);

	virtual bool Evaluate(double const* const* parameters,
		double* residuals,
		double** jacobians) const;

protected:
	const cv::Mat1b &mImg;
	const cv::Mat1s &mImgDx;
	const cv::Mat1s &mImgDy;

	template<class T>
	T interpolate(const cv::Mat_<T> &img, const double x, const double y) const;
};

ImageInterpolation::ImageInterpolation(const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy):
	mImg(img), mImgDx(imgDx), mImgDy(imgDy)
{
}

bool ImageInterpolation::Evaluate(double const* const* parameters,
	double* residuals,
	double** jacobians) const
{
	int u0 = (int)parameters[0][0];
	int v0 = (int)parameters[0][1];
	//residuals[0] = mImg(v0, u0);

	double u = parameters[0][0];
	double v = parameters[0][1];
	residuals[0] = interpolate(mImg, u, v);
	if (jacobians)
	{
		//jacobians[0][0] = mImgDx(v0, u0);
		//jacobians[0][1] = mImgDy(v0, u0);
		jacobians[0][0] = interpolate(mImgDx, u, v);
		jacobians[0][1] = interpolate(mImgDy, u, v);
	}
	return true;
}

template<class T>
T ImageInterpolation::interpolate(const cv::Mat_<T> &img, const double x, const double y) const
{
	int x0 = (int)x;
	int y0 = (int)y;

	double dx = x - x0;
	double dy = y - y0;

	int x1 = x0+1;
	if (x1 == img.cols)
		x1--;
	int y1 = y0+1;
	if (y1 == img.rows)
		y1--;

	int v00 = (int)img(y0, x0);
	int v10 = (int)img(y1, x0);
	double v0 = v00*(1 - dy) + v10*dy;

	int v01 = (int)img(y0, x1);
	int v11 = (int)img(y1, x1);
	double v1 = v01*(1 - dy) + v11*dy;

	T val = T(v0*(1-dx)+v1*dx);
	return val;
}

class ErrorClass
{
public:
	ErrorClass(const cv::Mat1b &refImg, const cv::Mat1s &refDx, const cv::Mat1s &refDy, const std::vector<Eigen::Vector3i> &evalPositions, const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy);

	template<class T>
	void evalRaw(const T * const paramsA, const T * const paramsB, const T * const paramsC, T *residuals, T*validCount)  const;

	template<class T>
	bool operator() (const T * const paramsA, const T * const paramsB, const T * const paramsC, T *residuals)  const;

	const cv::Mat1b &mRefImg;
	const cv::Mat1s &mRefDx;
	const cv::Mat1s &mRefDy;
	const std::vector<Eigen::Vector3i> &mEvalPositions;
	const cv::Mat1b &mImg;
	const cv::Mat1s &mImgDx;
	const cv::Mat1s &mImgDy;

	ceres::CostFunctionToFunctor<1, 2> mInterp;
};

ErrorClass::ErrorClass(const cv::Mat1b &refImg, const cv::Mat1s &refDx, const cv::Mat1s &refDy, const std::vector<Eigen::Vector3i> &evalPositions, const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy) :
mRefImg(refImg), mRefDx(refDx), mRefDy(refDy), mEvalPositions(evalPositions), mImg(img), mImgDx(imgDx), mImgDy(imgDy), mInterp(new ImageInterpolation(img, imgDx, imgDy))
{
}

template<class T>
bool ErrorClass::operator() (const T * const paramsA, const T * const paramsB, const T * const paramsC, T *residuals)  const
{
	T validCountT;

	this->evalRaw(paramsA, paramsB, paramsC, residuals, &validCountT);
	for (int i = 0; i < (int)mEvalPositions.size(); i++)
	{
		residuals[i] /= validCountT;
	}

	return true;
}

template<class T>
void ErrorClass::evalRaw(const T * const paramsA, const T * const paramsB, const T * const paramsC, T *residuals, T*validCountT)  const
{
	Eigen::Map<Eigen::Matrix3Xi> evalPositionsMap((int*)mEvalPositions.data(), 3, mEvalPositions.size());
	Eigen::Matrix<T, 3, Eigen::Dynamic> refPos = evalPositionsMap.cast<T>();

	Eigen::Matrix<T, 3, 3> H;
	//H << T(1), T(0), params[0], T(0), T(1), params[1], T(0), T(0), T(1);
	H << paramsB[0], paramsB[1], paramsA[0], paramsB[2], paramsB[3], paramsA[1], paramsC[0], paramsC[1], paramsC[2];

	Eigen::Matrix<T, 3, Eigen::Dynamic> imgPos3 = H*refPos;

	Eigen::Matrix<T, 2, Eigen::Dynamic> imgPos;
	//Eigen::Matrix<T, 2, 100> imgPos;
	imgPos.resize(2, imgPos3.cols());
	imgPos.row(0) = imgPos3.row(0).array() / imgPos3.row(2).array();
	imgPos.row(1) = imgPos3.row(1).array() / imgPos3.row(2).array();

	int validCount = 0;
	for (int i = 0; i < (int)mEvalPositions.size(); i++)
	{
		T *imgPosi = &imgPos(0, i);

		double imgUd = CeresUtils::ToDouble(imgPosi[0]);
		double imgVd = CeresUtils::ToDouble(imgPosi[1]);
		if (imgUd < 0 || imgVd < 0 || imgUd > mImg.cols - 1 || imgVd > mImg.rows - 1)
		{
			residuals[i] = T(0);
			continue;
		}

		T imgVal;
		mInterp(imgPosi, &imgVal);

		auto &refPosi = mEvalPositions[i];
		T refVal = T(mRefImg(refPosi[1], refPosi[0]));

		residuals[i] = imgVal - refVal;
		validCount++;
	}

	*validCountT = T(validCount);
}

bool TestWindow::init(PlaneCalibApp *app, const Eigen::Vector2i &imageSize)
{
	BaseWindow::init(app, imageSize);

	mRefTexture.create(GL_LUMINANCE, eutils::ToSize(imageSize));
	mImgTexture.create(GL_LUMINANCE, eutils::ToSize(imageSize));
	mCostTexture.create(GL_LUMINANCE, eutils::ToSize(imageSize));
	mEvalPositionsTexture.create(GL_LUMINANCE, eutils::ToSize(imageSize));

	loadRefFrame();

	resize();

	return true;
}

void TestWindow::loadRefFrame()
{
	const int kMinGradientSq = 10 * 10;

	mRefKeyframe = mApp->getSystem().getMap().getKeyframes()[0].get();
	mRefImg = mApp->getSystem().getMap().getKeyframes()[0]->getImage(0);

	cv::Sobel(mRefImg, mRefDx, CV_16S, 1, 0, 1);
	cv::Sobel(mRefImg, mRefDy, CV_16S, 0, 1, 1);

	cv::Mat1b evalPosImg;
	evalPosImg.create(mRefImg.size());

	for (Eigen::Vector3i p(0, 0, 1); p[1] < mRefImg.rows; p[1]++)
	{
		for (p[0] = 0; p[0] < mRefImg.cols; p[0]++)
		{
			const auto &dx = mRefDx(p[1], p[0]);
			const auto &dy = mRefDy(p[1], p[0]);
			const auto dmSq = dx*dx + dy*dy;
			if (dmSq > kMinGradientSq)
			{
				mEvalPositions.push_back(p);
				evalPosImg(p[1], p[0]) = (uchar)(255 * 0.3f);
			}
			else
				evalPosImg(p[1], p[0]) = 0;
		}
	}

	mPose = Eigen::Matrix3f::Identity();

	mRefTexture.update(mRefImg);

	mCostTexture.update(mRefImg);

	mEvalPositionsTexture.update(evalPosImg);
}
void TestWindow::alignAffine(const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy, Eigen::Matrix3fr &pose, TextureHelper &tex)
{
	//Solver options
	ceres::Solver::Options options;
	options.linear_solver_type = ceres::CGNR;
	//options.dense_linear_algebra_library_type = ceres::LAPACK;

	options.max_num_iterations = 500;
	options.num_threads = 4;
	options.num_linear_solver_threads = 4;
	options.logging_type = ceres::SILENT;
	options.minimizer_progress_to_stdout = false;

	Eigen::Vector2d paramsA;
	Eigen::Vector4d paramsB;
	Eigen::Vector3d paramsC;
	//paramsA << 0,0;
	//paramsB << 1, 0, 0, 1;
	//paramsC << 0, 0, 1;

	Eigen::Matrix3fr pose0;
	//pose0 = Eigen::Matrix3f::Identity();
	pose0 = pose;
	paramsA << pose0(0, 2), pose0(1, 2);
	paramsB << pose0(0, 0), pose0(0, 1), pose0(1, 0), pose0(1, 1);
	paramsC << pose0(2, 0), pose0(2, 1), pose0(2, 2);

	//Problem
	ceres::Problem problem;
	problem.AddParameterBlock(paramsA.data(), paramsA.size());
	problem.AddParameterBlock(paramsB.data(), paramsB.size());
	problem.AddParameterBlock(paramsC.data(), paramsC.size());
	//problem.SetParameterBlockConstant(paramsB.data());
	problem.SetParameterBlockConstant(paramsC.data());

	auto *errorFunc = new ErrorClass(mRefImg, mRefDx, mRefDy, mEvalPositions, img, imgDx, imgDy);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<ErrorClass, ceres::DYNAMIC, 2, 4, 3>(
		errorFunc, mEvalPositions.size()),
		NULL, paramsA.data(), paramsB.data(), paramsC.data());

	ceres::Solver::Summary summary;
	{
		ProfileSection s("TestSolve");
		ceres::Solve(options, &problem, &summary);
	}

	MYAPP_LOG << "-----------------Align affine-----------------\n";
	MYAPP_LOG << summary.FullReport();
	MYAPP_LOG << "ParamsA: " << paramsA << "\n";
	MYAPP_LOG << "ParamsB: " << paramsB << "\n";
	MYAPP_LOG << "ParamsC: " << paramsC << "\n";

	pose << paramsB[0], paramsB[1], paramsA[0], paramsB[2], paramsB[3], paramsA[1], paramsC[0], paramsC[1], paramsC[2];

	//Cost
	std::vector<double> residuals;
	double validCount;
	residuals.resize(errorFunc->mEvalPositions.size());
	errorFunc->evalRaw(paramsA.data(), paramsB.data(), paramsC.data(), residuals.data(), &validCount);
	MYAPP_LOG << "Valid count: " << validCount << "\n";

	cv::Mat1b costImg;
	costImg.create(errorFunc->mImg.size());
	costImg.setTo(0);
	for (int i = 0; i < errorFunc->mEvalPositions.size(); i++)
	{
		auto &pos = errorFunc->mEvalPositions[i];
		uchar val = cv::saturate_cast<uchar>(std::abs(residuals[i]) * 50);
		costImg(pos[1], pos[0]) = val;
	}

	tex.update(costImg);
}

void TestWindow::alignHomography(const cv::Mat1b &img, const cv::Mat1s &imgDx, const cv::Mat1s &imgDy, Eigen::Matrix3fr &pose, TextureHelper &tex)
{
	//Solver options
	ceres::Solver::Options options;
	options.linear_solver_type = ceres::CGNR;
	//options.dense_linear_algebra_library_type = ceres::LAPACK;

	options.max_num_iterations = 500;
	options.num_threads = 4;
	options.num_linear_solver_threads = 4;
	options.logging_type = ceres::SILENT;
	options.minimizer_progress_to_stdout = false;

	Eigen::Vector2d paramsA;
	Eigen::Vector4d paramsB;
	Eigen::Vector3d paramsC;
	//paramsA << 0,0;
	//paramsB << 1, 0, 0, 1;
	//paramsC << 0, 0, 1;
	Eigen::Matrix3fr pose0;
	//pose0 = Eigen::Matrix3f::Identity();
	pose0 = pose;
	paramsA << pose0(0, 2), pose0(1, 2);
	paramsB << pose0(0, 0), pose0(0, 1), pose0(1, 0), pose0(1, 1);
	paramsC << pose0(2, 0), pose0(2, 1), pose0(2, 2);

	//Problem
	ceres::Problem problem;
	problem.AddParameterBlock(paramsA.data(), paramsA.size());
	problem.AddParameterBlock(paramsB.data(), paramsB.size());

	//std::vector<int> constantIdx;
	//constantIdx.push_back(2);
	//problem.AddParameterBlock(paramsC.data(), paramsC.size(), new ceres::SubsetParameterization(3,constantIdx));
	problem.AddParameterBlock(paramsC.data(), paramsC.size());

	//problem.SetParameterBlockConstant(paramsB.data());
	//problem.SetParameterBlockConstant(paramsC.data());

	auto *errorFunc = new ErrorClass(mRefImg, mRefDx, mRefDy, mEvalPositions, img, imgDx, imgDy);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<ErrorClass, ceres::DYNAMIC, 2, 4, 3>(
		errorFunc, mEvalPositions.size()),
		NULL, paramsA.data(), paramsB.data(), paramsC.data());

	ceres::Solver::Summary summary;
	{
		ProfileSection s("TestSolve");
		ceres::Solve(options, &problem, &summary);
	}

	MYAPP_LOG << "-----------------Align homography-----------------\n";
	MYAPP_LOG << summary.FullReport();
	MYAPP_LOG << "ParamsA: " << paramsA << "\n";
	MYAPP_LOG << "ParamsB: " << paramsB << "\n";
	MYAPP_LOG << "ParamsC: " << paramsC << "\n";

	pose << paramsB[0], paramsB[1], paramsA[0], paramsB[2], paramsB[3], paramsA[1], paramsC[0], paramsC[1], paramsC[2];

	//Cost
	std::vector<double> residuals;
	double validCount;
	residuals.resize(errorFunc->mEvalPositions.size());
	errorFunc->evalRaw(paramsA.data(), paramsB.data(), paramsC.data(), residuals.data(), &validCount);
	MYAPP_LOG << "Valid count: " << validCount << "\n";

	cv::Mat1b costImg;
	costImg.create(errorFunc->mImg.size());
	costImg.setTo(0);
	for (int i = 0; i < errorFunc->mEvalPositions.size(); i++)
	{
		auto &pos = errorFunc->mEvalPositions[i];
		uchar val = cv::saturate_cast<uchar>(std::abs(residuals[i]) * 50);
		costImg(pos[1], pos[0]) = val;
	}

	tex.update(costImg);
}

void TestWindow::updateState()
{
	shared_lock<shared_mutex> lockRead(mApp->getSystem().getMap().getMutex());

	if (mApp->getSystem().getMap().getKeyframes()[0].get() != mRefKeyframe)
		loadRefFrame();

	const TrackingFrame *frame = mApp->getSystem().getTracker().getFrame();
	if (!frame)
		return;

	//mPose = Eigen::Matrix3f::Identity();
	//mPose0 = mPose;

	cv::Mat1b img;
	cv::Mat1s imgDx;
	cv::Mat1s imgDy;

	img = frame->getOriginalPyramid()[0];
	cv::Sobel(img, imgDx, CV_16S, 1, 0, 1);
	cv::Sobel(img, imgDy, CV_16S, 0, 1, 1);

	mImgTexture.update(img);

	//mPoseAffine = mPoseHomography = Eigen::Matrix3fr::Identity();

	//alignAffine(img, imgDx, imgDy, mPoseAffine, mCostAffineTexture);
	alignHomography(img, imgDx, imgDy, mPose, mCostTexture);

	//Corners
	Eigen::Matrix<float, 3, 4> refCorners;

	refCorners <<
		0, 0, mRefImg.cols, mRefImg.cols,
		0, mRefImg.rows, mRefImg.rows, 0,
		1,1,1,1;

	Eigen::Matrix<float, 3, 4> corners3;
	corners3 = mPose.inverse() * refCorners;
	mCornerPos.row(0) = corners3.row(0).array() / corners3.row(2).array();
	mCornerPos.row(1) = corners3.row(1).array() / corners3.row(2).array();
}

void TestWindow::resize()
{
	auto screenSize = UserInterfaceInfo::Instance().getScreenSize();
	mTiler.configDevice(Eigen::Vector2i::Zero(), screenSize, 2, 1);
	mTiler.fillTiles();

	//Tile 0 - Alignment
	float scale = 1.5f;
	Eigen::Vector2i sizeScaled = (scale*mImageSize.cast<float>()).cast<int>();
	Eigen::Vector2f padding = (sizeScaled - mImageSize).cast<float>();
	Eigen::Matrix4f offsetMat;
	offsetMat << 1, 0, 0, padding[0] / 2, 0, 1, 0, padding[1] / 2, 0, 0, 1, 0, 0, 0, 0, 1;

	mTiler.setImageMVP(0, sizeScaled);
	mTiler.multiplyMVP(0, offsetMat);

	//Tile 1 - Cost
	mTiler.setImageMVP(1, mImageSize);

	//Tile 2 - absolute
	int smallImgWidth = (int)(screenSize[0] * 0.2f);
	float imgAspect = (float)mImageSize[0] / mImageSize[1];
	int smallImgHeight = (int)(smallImgWidth / imgAspect);

	Eigen::Vector2i smallImgSize(smallImgWidth, smallImgHeight);
	Eigen::Vector2i smallImgOrigin(screenSize[0] - smallImgSize[0], 0);
	mTiler.addAbsoluteTile(smallImgOrigin, smallImgSize);
	mTiler.setImageMVP(2, mImageSize);
}

void TestWindow::draw()
{
	//Generic vertices
	std::vector<Eigen::Vector4f> vertices;
	std::vector<Eigen::Vector2f> textureCoords;
	TextureShader::CreateVertices(mImageSize, vertices, textureCoords);

	///////////////////////////
	// Homography
	// Alignment
	mTiler.setActiveTile(0);
	mShaders->getTextureWarp().setMVPMatrix(mTiler.getMVP());
	mShaders->getColor().setMVPMatrix(mTiler.getMVP());

	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_DST_ALPHA);
	mShaders->getTextureWarp().renderTextureAsAlpha(mRefTexture.getTarget(), mRefTexture.getId(), Eigen::Matrix3fr::Identity(), StaticColors::Green(), mImageSize);
	mShaders->getTextureWarp().renderTextureAsAlpha(mImgTexture.getTarget(), mImgTexture.getId(), mPose, StaticColors::Red(), mImageSize);

	mShaders->getColor().drawVertices(GL_LINE_LOOP, (Eigen::Vector2f*)mCornerPos.data(), mCornerPos.cols(), StaticColors::Green());

	//Cost
	mTiler.setActiveTile(1);
	mShaders->getTexture().setMVPMatrix(mTiler.getMVP());
	mShaders->getTexture().renderTexture(mCostTexture.getTarget(), mCostTexture.getId(), eutils::FromSize(mCostTexture.getSize()));

	//Absolute
	mTiler.setActiveTile(2);
	mShaders->getTexture().setMVPMatrix(mTiler.getMVP());
	mShaders->getTextureWarp().setMVPMatrix(mTiler.getMVP());
	mShaders->getTexture().renderTexture(mCurrentImageTextureTarget, mCurrentImageTextureId, mImageSize);

	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	mShaders->getTextureWarp().renderTextureAsAlpha(mEvalPositionsTexture.getTarget(), mEvalPositionsTexture.getId(), Eigen::Matrix3fr::Identity(), StaticColors::Blue(), mImageSize);
}

} /* namespace dtslam */