// 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


#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 */


#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


#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 */


#ifndef HOPS_LINEARMODEL_HPP
#define HOPS_LINEARMODEL_HPP

#define _USE_MATH_DEFINES

#include <math.h>
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/LU>
#include <utility>

namespace hops {
    template<typename Matrix, typename Vector>
    class LinearModel {
    public:
        using MatrixType = Matrix;
        using VectorType = Vector;

        LinearModel(VectorType measuredData, MatrixType dataCovariance, MatrixType linearModel);

        /**
         * @brief Evaluates the negative log likelihood for input x.
         * @param x
         * @return
         */
        typename MatrixType::Scalar computeNegativeLogLikelihood(const VectorType &x) const;

        MatrixType computeExpectedFisherInformation(const VectorType &) const;

        VectorType computeLogLikelihoodGradient(const VectorType &x) const;

    private:
        VectorType measuredData;
        MatrixType dataCovariance;
        MatrixType inverseCovariance;
        MatrixType linearModel;
        typename MatrixType::Scalar logNormalizationConstant;
    };

    template<typename MatrixType, typename VectorType>
    LinearModel<MatrixType, VectorType>::LinearModel(
            VectorType measuredData,
            MatrixType dataCovariance,
            MatrixType linearModel) :
            measuredData(std::move(measuredData)),
            dataCovariance(std::move(dataCovariance)),
            linearModel(std::move(linearModel)) {
        Eigen::LLT<MatrixType, Eigen::Upper> solver(this->dataCovariance);
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> matrixL = solver.matrixL();
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> matrixU = solver.matrixU();
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> inverseMatrixL = matrixL.inverse();
        inverseCovariance = inverseMatrixL * inverseMatrixL.transpose();

        logNormalizationConstant = -static_cast<typename MatrixType::Scalar>(this->measuredData.rows()) / 2 *
                                   std::log(2 * M_PI)
                                   - matrixL.diagonal().array().log().sum();
    }

    template<typename MatrixType, typename VectorType>
    typename MatrixType::Scalar
    LinearModel<MatrixType, VectorType>::computeNegativeLogLikelihood(const VectorType &x) const {
        return -logNormalizationConstant +
               0.5 * static_cast<typename MatrixType::Scalar>((linearModel * x - measuredData).transpose() *
                                                              inverseCovariance * (linearModel * x - measuredData));
    }

    template<typename MatrixType, typename VectorType>
    MatrixType
    LinearModel<MatrixType, VectorType>::computeExpectedFisherInformation(const VectorType &) const {
        return inverseCovariance;
    }

    template<typename MatrixType, typename VectorType>
    VectorType
    LinearModel<MatrixType, VectorType>::computeLogLikelihoodGradient(const VectorType &x) const {
        return -inverseCovariance * (linearModel*x - measuredData);
    }
}

#endif //HOPS_LINEARMODEL_HPP


#include "statistics.hpp"

#include <armadillo>

void moment_function_4th(const arma::vec &seq,
                         const int width,
                         arma::vec &momfunc4)
{
  int num_seq = seq.n_rows;
  int idx = 0;
  for (int k = 0; k < width; ++k) {
    for (int j = 0; j <= k; ++j) {
      for (int i = 0; i <= j; ++i) {
        if (i == 0 && k > 0 && k == j) continue;
        double sum = 0;
        for (int n = 0; n < num_seq - k; ++n) {
          sum += seq(n) * seq(n + i) * seq(n + j) * seq(n + k);
        }
        momfunc4(idx) += sum;
        idx++;
      }
    }
  }
}

void cumulants_2nd(const arma::vec &seq,
                   const int width,
                   arma::vec &cum2)
{
  for (int i = 0; i < width; ++i) {
    double sum = 0.;
    for (int j = 0; j <= seq.n_elem - width; ++j) {
      sum += seq(j) * seq(j + i);
    }
    cum2(i) += sum;
  }
}

void cumulants_4th(const arma::vec &seq,
                   const int n2,
                   const int n4,
                   arma::vec &cum4)
{
  int num_seq = seq.n_rows;
  arma::vec cum2(n2, arma::fill::zeros);
  cumulants_2nd(seq, n2, cum2);
  double pwr = cum2(0) / num_seq;
  double scale2 = 1.0 / cum2(0);
  cum2 *= scale2;

  arma::vec mfunc4(n4, arma::fill::zeros);
  moment_function_4th(seq, n2, mfunc4);
  double scale4 = 1.0 / (num_seq * std::pow(pwr, 2));

  int idx = 0;
  for (int k = 0; k < n2; ++k) {
    for (int j = 0; j <= k; ++j) {
      for (int i = 0; i <= j; ++i) {
        if (i == 0 && k > 0 && k == j) continue;
        double m4g = cum2(i) * cum2(k - j) + cum2(j) * cum2(k - i)
          + cum2(k) * cum2(j - i);
        cum4(idx) += (scale4 * mfunc4(idx) - m4g) * (n2 - k) / n2;

        idx++;
      }
    }
  }
}


#include "fhd_math.h"
#include <math.h>
#include <float.h>
#include <assert.h>
#include <Eigen/Dense>

fhd_vec2 fhd_vec2_sub(fhd_vec2 a, fhd_vec2 b) {
  fhd_vec2 r;
  r.x = a.x - b.x;
  r.y = a.y - b.y;
  return r;
}

fhd_vec2 fhd_vec2_normalize(fhd_vec2 v) {
  const float a = sqrtf(v.x * v.x + v.y * v.y);
  return fhd_vec2{v.x / a, v.y / a};
}

fhd_vec2 fhd_vec2_mul(fhd_vec2 a, float s) { return {a.x * s, a.y * s}; }

fhd_vec2 fhd_vec2_mul_pcw(fhd_vec2 a, fhd_vec2 b) {
  return {a.x * b.x, a.y * b.y};
}

float fhd_vec2_distance(fhd_vec2 a, fhd_vec2 b) {
  return hypotf(b.x - a.x, b.y - a.y);
}

float fhd_vec2_length(fhd_vec2 v) { return sqrtf(v.x * v.x + v.y * v.y); }

fhd_vec3 fhd_vec3_normalize(fhd_vec3 v) {
  const float a = sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
  return fhd_vec3{v.x / a, v.y / a, v.z / a};
}

float fhd_vec3_dot(fhd_vec3 a, fhd_vec3 b) {
  return a.x * b.x + a.y * b.y + a.z * b.z;
}

fhd_vec3 fhd_vec3_sub(fhd_vec3 a, fhd_vec3 b) {
  fhd_vec3 r;
  r.x = a.x - b.x;
  r.y = a.y - b.y;
  r.z = a.z - b.z;
  return r;
}

fhd_vec3 fhd_vec3_cross(fhd_vec3 u, fhd_vec3 v) {
  const float t1 = u.x - u.y;
  const float t2 = v.y + v.z;
  const float t3 = u.x * v.z;
  const float t4 = t1 * t2 - t3;

  fhd_vec3 r;
  r.x = v.y * (t1 - u.z) - t4;
  r.y = u.z * v.x - t3;
  r.z = t4 - u.y * (v.x - t2);

  return r;
}

void fhd_aabb_expand(fhd_aabb* bbox, fhd_vec2 point) {
  if (point.x < bbox->top_left.x)
    bbox->top_left.x = point.x;
  else if (point.x > bbox->bot_right.x)
    bbox->bot_right.x = point.x;

  if (point.y < bbox->top_left.y)
    bbox->top_left.y = point.y;
  else if (point.y > bbox->bot_right.y)
    bbox->bot_right.y = point.y;
}

fhd_vec2 fhd_aabb_center(const fhd_aabb* bbox) {
  return {(bbox->bot_right.x + bbox->top_left.x) * 0.5f,
          (bbox->bot_right.y + bbox->top_left.y) * 0.5f};
}

bool fhd_aabb_overlap(const fhd_aabb* a, const fhd_aabb* b) {
  if (a->bot_right.x < b->top_left.x || a->top_left.x > b->bot_right.x)
    return false;
  if (a->bot_right.y < b->top_left.y || a->top_left.y > b->bot_right.y)
    return false;

  return true;
}

fhd_vec2 fhd_aabb_size(const fhd_aabb* a) {
  fhd_vec2 r;
  r.x = a->bot_right.x - a->top_left.x;
  r.y = a->bot_right.y - a->top_left.y;
  return r;
}

fhd_aabb fhd_aabb_from_points(const fhd_vec2* points, int len) {
  assert(len >= 2);

  fhd_aabb bbox = {{FLT_MAX, FLT_MAX}, {-FLT_MAX, -FLT_MAX}};

  for (int i = 0; i < len; i++) {
    fhd_aabb_expand(&bbox, points[i]);
  }

  return bbox;
}

float fhd_plane_point_dist(fhd_plane p, fhd_vec3 q) {
  return (fhd_vec3_dot(p.n, q) - p.d) / fhd_vec3_dot(p.n, p.n);
}

fhd_plane fhd_make_plane(fhd_vec3 a, fhd_vec3 b, fhd_vec3 c) {
  fhd_plane p;
  p.n = fhd_vec3_normalize(
      fhd_vec3_cross(fhd_vec3_sub(b, a), fhd_vec3_sub(c, a)));
  p.d = fhd_vec3_dot(p.n, a);
  return p;
}

float fhd_fast_atan2(float y, float x) {
  const float THRQTR_PI = 3.f * F_PI_4;
  float r, angle;
  float abs_y = fabs(y) + 1e-10f;
  if (x < 0.0f) {
    r = (x + abs_y) / (abs_y - x);
    angle = THRQTR_PI;
  } else {
    r = (x - abs_y) / (x + abs_y);
    angle = F_PI_4;
  }
  angle += (0.1963f * r * r - 0.9817f) * r;
  if (y < 0.0f)
    return -angle;
  else
    return angle;
}

fhd_vec3 fhd_pcl_normal(const fhd_vec3* points, int len) {
  Eigen::Matrix3f A = Eigen::Matrix3f::Zero();
  Eigen::Vector3f b = Eigen::Vector3f::Zero();
  for (int i = 0; i < len; i++) {
    const fhd_vec3 v = points[i];
    A(0, 0) += v.x * v.x;
    A(0, 1) += v.x * v.y;
    A(0, 2) += v.x;
    A(1, 0) += v.x * v.y;
    A(1, 1) += v.y * v.y;
    A(1, 2) += v.y;
    A(2, 0) += v.x;
    A(2, 1) += v.y;

    b(0) += v.x * v.z;
    b(1) += v.y * v.z;
    b(2) += v.z;
  }

  A(2, 2) += float(len);
  Eigen::Vector3f solution = A.llt().solve(b);
  return fhd_vec3_normalize(fhd_vec3{solution(0), solution(1), solution(2)});
}


#ifndef Utils_LinearRegression_hpp
#define Utils_LinearRegression_hpp

/** @file LinearRegression.hpp
  * @brief
  * @author C.D. Clark III
  * @date 07/07/17
  */

#include <Eigen/Dense>
using namespace Eigen;

namespace RUC {

/*
 * @breif Perform least-squared linear regression on a set of x-y data points.
 *
 * @param x vector containing x coordinates of point to fit.
 * @param y vector containing y coordinates of point to fit.
 * @return A 2x1 matrix containing the y-intercept (element 0) and slope (element 1) of the fit.
 */
template<typename T>
Matrix<T,2,1> LinearRegression( const Matrix<T,Dynamic,1> &x, const Matrix<T,Dynamic,1> &y )
{
  // given vectors x = | x0 | and y = | y0 | of x,y pairs,
  //                   | x1 | and     | y1 |
  //                   | .. | and     | .. |
  //                   | xn | and     | yn |
  //
  // construct matrix X = | 1 x0 ] see https://en.wikipedia.org/wiki/Linear_least_squares_(mathematics)
  //                      | 1 x1 ]
  //                      | .... ]
  //                      | 1 xn ]
  //
  // then coefficients matrix B = | b | that minimize sum of residuals is
  //                              | m |
  //
  // is found by solveing X^T X B = X^T y,
  // so
  //       B = (X^T X)^-1 X^T y;

  // matrix X
  assert( x.size() == y.size() );

  auto N = x.size();

  Matrix<T,Dynamic,2> X(N,2);
  // fill first column with 1's
  X.block(0,0,N,1) = Matrix<T,Dynamic,1>::Constant(N,1,1);
  // fill second column with x values
  X.block(0,1,N,1) = x;

  // need the transpose of X
  auto Xtrans = X.transpose();

  auto fit = (Xtrans*X).inverse()*Xtrans*y;

  Matrix<T,2,1> ret;
  ret[0] = fit[0];
  ret[1] = fit[1];

  return ret;
}

}

#endif // include protector


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

using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::MatrixXf;
using Eigen::VectorXd;
using Eigen::VectorXf;
using namespace std;

class ANN
{
private:
    MatrixXd Wh, W0;
    VectorXd bh, b0;
    int input_layer = 5 * 5, hidden_layer = 16, output_layer;
    double lr = 1.;
    double eps = 1e-6;

public:
    ANN(vector<MatrixXd *> &train_symbols)
    {
        double HI = 2., LO = -2.;
        double range = HI - LO;

        output_layer = train_symbols.size();

        Wh = MatrixXd::Random(hidden_layer, input_layer);
        Wh = (Wh + MatrixXd::Constant(hidden_layer, input_layer, 1.)) * range / 2.;
        Wh = (Wh + MatrixXd::Constant(hidden_layer, input_layer, LO));

        bh = VectorXd::Zero(hidden_layer);

        W0 = MatrixXd::Random(output_layer, hidden_layer);
        W0 = (W0 + MatrixXd::Constant(output_layer, hidden_layer, 1.)) * range / 2.;
        W0 = (W0 + MatrixXd::Constant(output_layer, hidden_layer, LO));

        b0 = VectorXd::Zero(output_layer);
        train(train_symbols);
    }

    double sigmoid(double x)
    {
        return 1.0 / (1.0 + exp(-x));
    }

    void train(vector<MatrixXd *> train_symbols)
    {
        MatrixXd EW0, EWh, EI0, EIh;
        do
        {
            int i = 0;
            for (std::vector<MatrixXd *>::iterator it = train_symbols.begin(); it != train_symbols.end(); ++it)
            {
                MatrixXd yd = MatrixXd::Zero(output_layer, 1);
                yd(i) = 1.;
                // Forward pass
                MatrixXd X = **it;
                Map<VectorXd> X_flat(X.data(), X.size());

                MatrixXd Vh = Wh * X_flat + bh;
                MatrixXd Zh = Vh.unaryExpr([](double x) { return 1.0 / (1.0 + exp(-x)); });
                MatrixXd U0 = W0 * Zh + b0;
                MatrixXd Y0 = U0.unaryExpr([](double x) { return 1.0 / (1.0 + exp(-x)); });

                // Backward propagation
                MatrixXd EA0 = Y0 - yd;
                EI0 = EA0.cwiseProduct(Y0.cwiseProduct((MatrixXd)(1 - Y0.array())));
                EW0 = EI0 * Zh.transpose();
                MatrixXd EAh = W0.transpose() * EI0;
                EIh = EAh.cwiseProduct(Zh.cwiseProduct((MatrixXd)(1 - Zh.array())));
                EWh = EIh * X_flat.transpose();

                //Update params
                W0 = (W0 - lr * EW0);
                Wh = (Wh - lr * EWh);
                b0 = (b0 + lr * EI0);
                bh = (bh + lr * EIh);

                i += 1;
            }
        } while (!(EW0.isZero(eps) && EWh.isZero(eps) && EI0.isZero(eps) && EIh.isZero(eps)));
    }

    VectorXd *test(MatrixXd &symbol)
    {
        MatrixXd X = symbol;
        Map<VectorXd> X_flat(X.data(), X.size());

        MatrixXd Vh = Wh * X_flat + bh;
        MatrixXd Zh = Vh.unaryExpr([](double x) { return 1.0 / (1.0 + exp(-x)); });
        MatrixXd U0 = W0 * Zh + b0;
        MatrixXd *Y0 = new MatrixXd(U0.rows(), U0.cols());
        *Y0 << U0.unaryExpr([](double x) { return 1.0 / (1.0 + exp(-x)); });
        return (VectorXd *)Y0;
    }
};

int main()
{
    srand((unsigned int)time(0));
    vector<MatrixXd *> train_symbols;
    MatrixXd m1, m2;
    m1 = MatrixXd::Random(5, 5);
    train_symbols.push_back(&m1);
    m2 = MatrixXd::Random(5, 5);
    train_symbols.push_back(&m2);
    ANN a(train_symbols);
    VectorXd *y2 = a.test(m2);
    cout << *y2 << '\n';
    VectorXd *y1 = a.test(m1);
    cout << *y1 << '\n';
}


#include <cstdlib>
#include <iostream>
#include <fstream>
#include <iterator>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Random.h>

#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polyhedron_items_with_id_3.h>
#include <CGAL/IO/Polyhedron_iostream.h>

#include <CGAL/Surface_mesh_shortest_path.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/boost/graph/iterator.h>

#include <boost/variant.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef CGAL::Polyhedron_3<Kernel, CGAL::Polyhedron_items_with_id_3> Polyhedron_3;
typedef CGAL::Surface_mesh_shortest_path_traits<Kernel, Polyhedron_3> Traits;
typedef CGAL::Surface_mesh_shortest_path<Traits> Surface_mesh_shortest_path;
typedef Traits::Barycentric_coordinate Barycentric_coordinate;
typedef boost::graph_traits<Polyhedron_3> Graph_traits;
typedef Graph_traits::vertex_iterator vertex_iterator;
typedef Graph_traits::face_iterator face_iterator;
typedef Graph_traits::vertex_descriptor vertex_descriptor;
typedef Graph_traits::face_descriptor face_descriptor;
typedef Graph_traits::halfedge_descriptor halfedge_descriptor;

// A model of SurfaceMeshShortestPathVisitor storing simplicies
// using boost::variant
struct Sequence_collector
{
  typedef boost::variant< vertex_descriptor,
                         std::pair<halfedge_descriptor,double>,
                         std::pair<face_descriptor, Barycentric_coordinate> > Simplex;
  std::vector< Simplex > sequence;

  void operator()(halfedge_descriptor he, double alpha)
  {

    sequence.push_back( std::make_pair(he, alpha) );
  }

  void operator()(vertex_descriptor v)
  {
    sequence.push_back( v );
  }

  void operator()(face_descriptor f, Barycentric_coordinate alpha)
  {
    sequence.push_back( std::make_pair(f, alpha) );
  }
};

// A visitor to print what a variant contains using boost::apply_visitor
struct Print_visitor : public boost::static_visitor<> {
  int i;
  Polyhedron_3& g;

  Print_visitor(Polyhedron_3& g) :i(-1), g(g) {}

  void operator()(vertex_descriptor v)
  {
    std::cout << "#" << ++i << " : Vertex : " << get(boost::vertex_index, g)[v] << "\n";
  }

  void operator()(const std::pair<halfedge_descriptor,double>& h_a)
  {
    std::cout << "#" << ++i << " : Edge : " << get(CGAL::halfedge_index, g)[h_a.first] << " , ("
                                            << 1.0 - h_a.second << " , "
                                            << h_a.second << ")\n";
  }

  void operator()(const std::pair<face_descriptor, Barycentric_coordinate>& f_bc)
  {
    std::cout << "#" << ++i << " : Face : " << get(CGAL::face_index, g)[f_bc.first] << " , ("
                                            << f_bc.second[0] << " , "
                                            << f_bc.second[1] << " , "
                                            << f_bc.second[2] << ")\n";
  }
};

int main(int argc, char** argv)
{
  // read input polyhedron
  Polyhedron_3 polyhedron;
  std::ifstream input((argc>1)?argv[1]:"data/elephant.off");
  input >> polyhedron;
  input.close();

  // initialize indices of vertices, halfedges and facets
  CGAL::set_halfedgeds_items_id(polyhedron);

  // pick up a random face
  const size_t randSeed = argc > 2 ? std::atoi(argv[2]) : 7915421;
  CGAL::Random rand(randSeed);
  const int target_face_index = rand.get_int(0, num_faces(polyhedron));
  face_iterator face_it = faces(polyhedron).first;
  std::advance(face_it,target_face_index);
  // ... and define a barycentric coordinate inside the face
  Barycentric_coordinate face_location = {{0.25, 0.5, 0.25}};

  // construct a shortest path query object and add a source point
  Surface_mesh_shortest_path shortest_paths(polyhedron);
  shortest_paths.add_source_point(*face_it, face_location);

  // pick a random target point inside a face
  face_it = faces(polyhedron).first;
  std::advance(face_it, rand.get_int(0, num_faces(polyhedron)));

  // collect the sequence of simplicies crossed by the shortest path
  Sequence_collector sequence_collector;
  shortest_paths.shortest_path_sequence_to_source_points(*face_it, face_location, sequence_collector);

  // print the sequence using the visitor pattern
  Print_visitor print_visitor(polyhedron);
  for (size_t i = 0; i < sequence_collector.sequence.size(); ++i)
    boost::apply_visitor(print_visitor, sequence_collector.sequence[i]);

  return 0;
}


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
/**
* @file
* @author Christian Winne
* @ingroup openOR_core_math
*/

#ifndef openOR_core_Math_detail_determinant_impl_hpp
#define openOR_core_Math_detail_determinant_impl_hpp

#include <boost/mpl/assert.hpp>
#include <boost/mpl/void.hpp>
#include <boost/type_traits/is_same.hpp>

#include <openOR/Math/matrix.hpp>

namespace openOR {
	namespace Math {
		namespace Impl {

			template<class Type, int Dim>
			struct Determinant {
				typename MatrixTraits<Type>::ValueType operator()(const Type& mat) const {
					// function has to be implemented

					BOOST_MPL_ASSERT((boost::is_same<Type, boost::mpl::void_>));
				}
			};

			//determinant of 2x2 matrix
			template<class Type>
			struct Determinant<Type, 2> {
				typename MatrixTraits<Type>::ValueType operator()(const Type& mat) const {
					return Math::get<0, 0>(mat) * Math::get<1, 1>(mat) - Math::get<0, 1>(mat) * Math::get<1, 0>(mat);
				}
			};

			//determinant of 3x3 matrix
			template<class Type>
			struct Determinant<Type, 3> {
				typename MatrixTraits<Type>::ValueType operator()(const Type& mat) const {
					return Math::get<0, 0>(mat) * Math::get<1, 1>(mat) * Math::get<2, 2>(mat) +
						Math::get<0, 1>(mat) * Math::get<1, 2>(mat) * Math::get<2, 0>(mat) +
						Math::get<0, 2>(mat) * Math::get<1, 0>(mat) * Math::get<2, 1>(mat) -
						Math::get<2, 0>(mat) * Math::get<1, 1>(mat) * Math::get<0, 2>(mat) -
						Math::get<2, 1>(mat) * Math::get<1, 2>(mat) * Math::get<0, 0>(mat) -
						Math::get<2, 2>(mat) * Math::get<1, 0>(mat) * Math::get<0, 1>(mat);
				}
			};
			//determinant of 4x4 matrix
			template<class Type>
			struct Determinant<Type, 4> {
				typename MatrixTraits<Type>::ValueType operator()(const Type& mat) const {
					return   Math::get<0, 0>(mat) * Math::get<1, 1>(mat) * Math::get<2, 2>(mat) * Math::get<3, 3>(mat) +
						Math::get<0, 0>(mat) * Math::get<1, 2>(mat) * Math::get<2, 3>(mat) * Math::get<3, 1>(mat) +
						Math::get<0, 0>(mat) * Math::get<1, 3>(mat) * Math::get<2, 1>(mat) * Math::get<3, 2>(mat) +

						Math::get<0, 1>(mat) * Math::get<1, 0>(mat) * Math::get<2, 3>(mat) * Math::get<3, 2>(mat) +
						Math::get<0, 1>(mat) * Math::get<1, 2>(mat) * Math::get<2, 0>(mat) * Math::get<3, 3>(mat) +
						Math::get<0, 1>(mat) * Math::get<1, 3>(mat) * Math::get<2, 2>(mat) * Math::get<3, 0>(mat) +

						Math::get<0, 2>(mat) * Math::get<1, 0>(mat) * Math::get<2, 1>(mat) * Math::get<3, 3>(mat) +
						Math::get<0, 2>(mat) * Math::get<1, 1>(mat) * Math::get<2, 3>(mat) * Math::get<3, 0>(mat) +
						Math::get<0, 2>(mat) * Math::get<1, 3>(mat) * Math::get<2, 0>(mat) * Math::get<3, 1>(mat) +

						Math::get<0, 3>(mat) * Math::get<1, 0>(mat) * Math::get<2, 2>(mat) * Math::get<3, 1>(mat) +
						Math::get<0, 3>(mat) * Math::get<1, 1>(mat) * Math::get<2, 0>(mat) * Math::get<3, 2>(mat) +
						Math::get<0, 3>(mat) * Math::get<1, 2>(mat) * Math::get<2, 1>(mat) * Math::get<3, 0>(mat) -

						Math::get<0, 0>(mat) * Math::get<1, 1>(mat) * Math::get<2, 3>(mat) * Math::get<3, 2>(mat) -
						Math::get<0, 0>(mat) * Math::get<1, 2>(mat) * Math::get<2, 1>(mat) * Math::get<3, 3>(mat) -
						Math::get<0, 0>(mat) * Math::get<1, 3>(mat) * Math::get<2, 2>(mat) * Math::get<3, 1>(mat) -

						Math::get<0, 1>(mat) * Math::get<1, 0>(mat) * Math::get<2, 2>(mat) * Math::get<3, 3>(mat) -
						Math::get<0, 1>(mat) * Math::get<1, 2>(mat) * Math::get<2, 3>(mat) * Math::get<3, 0>(mat) -
						Math::get<0, 1>(mat) * Math::get<1, 3>(mat) * Math::get<2, 0>(mat) * Math::get<3, 2>(mat) -

						Math::get<0, 2>(mat) * Math::get<1, 0>(mat) * Math::get<2, 3>(mat) * Math::get<3, 1>(mat) -
						Math::get<0, 2>(mat) * Math::get<1, 1>(mat) * Math::get<2, 0>(mat) * Math::get<3, 3>(mat) -
						Math::get<0, 2>(mat) * Math::get<1, 3>(mat) * Math::get<2, 1>(mat) * Math::get<3, 0>(mat) -

						Math::get<0, 3>(mat) * Math::get<1, 0>(mat) * Math::get<2, 1>(mat) * Math::get<3, 2>(mat) -
						Math::get<0, 3>(mat) * Math::get<1, 1>(mat) * Math::get<2, 2>(mat) * Math::get<3, 0>(mat) -
						Math::get<0, 3>(mat) * Math::get<1, 2>(mat) * Math::get<2, 0>(mat) * Math::get<3, 1>(mat);
				}
			};

		}
	}
}

#endif


// #include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <numeric>
#include <vector>
#include "kernel.h"
#include "distance.h"
#include "meanshift.h"

using namespace ModelFitting;

// #define EPSILON 0.00000001
// #define CLUSTER_EPSILON 0.5

MeanShift::MeanShift():
        max_iterations(100000),
		max_inner_iterations(10000)
{

}

MeanShift::MeanShift(
    double (*_distance_metirc)(cv::Mat &, cv::Mat &, const int &) ,
    double (*_kernel)(const double &, const double &)):
    MeanShift()
{
    this->calculate_distance = _distance_metirc;
    this->calculate_kernel = _kernel;
}

MeanShift::~MeanShift()
{
}

void MeanShift::cluster(
    cv::InputArray _data_pts,
    cv::OutputArray _clusters_centers,
    std::vector<std::vector<int>> &_cluster_pts,
    double kernel_bandwidth)
{
    // initialization
    const cv::Mat data_pts = _data_pts.getMat();
    const int dim_num = data_pts.cols;
    const int pts_num = data_pts.rows;

    std::cout << dim_num << ", " << pts_num << std::endl;
    // std::cout << data_pts << std::endl;
    int cluster_num = 0;
    const double bandwidth_sq = kernel_bandwidth * kernel_bandwidth;
    const double hald_bandwith = static_cast<double>(kernel_bandwidth/2);
    const double stop_threshold = static_cast<double>(1e-3*kernel_bandwidth);

    std::vector<int> init_pts_inds(pts_num); // initial points indices
    std::iota(init_pts_inds.begin(), init_pts_inds.end(), 0);

    // double stop_threshold = 1e-3 * kernel_bandwidth; // need casting

    std::vector<int> been_visited(pts_num,0);
    std::vector<cv::Mat> cluster_votes;//(pts_num,std::vector<int>(pts_num, 0));
    cv::Mat current_cluster_votes;
    std::vector<cv::Mat> clusters_centers;

    cv::RNG gen;
    int tmp_idx, pt_idx;
    cv::Mat current_mean, old_mean;
    std::vector<int> current_cluster_members(0);

    // Iterate over all data points
    int iters = 0;
    while (init_pts_inds.size() && iters++ < max_iterations)// && num pf iterations)
    {
        tmp_idx = gen.uniform(0,init_pts_inds.size());
        pt_idx = init_pts_inds[tmp_idx];
        //
        current_mean = data_pts.row(pt_idx);
        current_cluster_members.resize(0);
        current_cluster_votes = cv::Mat_<int>(1,pts_num, 0);//.resize(pts_num, 0);
        //
        int inner_iters = 0;
        while(inner_iters++ < max_inner_iterations)
        {
            // std::cout << max_inner_iterations << std::endl;
            // mean shift procedure
            // 1. Compute the mean shift vector m
            // 2. translate the old mean
            // 1.1 iterate over all points and find indices of points
            //     that its distances from the current mean is less than the bandwidth
            // 1.2 vote for those points as clusters
            // 1.3 pass those points ||(current_mean - pts_i)/bandwidth|| to the kernel
            // 1.4 sum the return values of the kernel
            old_mean = current_mean.clone();
            current_mean = cv::Mat::zeros(1, data_pts.cols, data_pts.type());

            meanshift(
                data_pts,
                pts_num,
                dim_num,
                old_mean,
                current_mean,
                current_cluster_votes,
                current_cluster_members,
                been_visited,
                kernel_bandwidth);

            // checking stopping condition for current cluster
            if(this->calculate_distance(current_mean, old_mean, dim_num) < stop_threshold)
            {
                // std::cout << this->calculate_distance(current_mean, old_mean, dim_num) << std::endl;
                // check for merge posibilities
                int merge_with = -1;
                for(size_t c = 0; c < cluster_num; c++)
                {
                    // distance from posible new cluster max to old cluster max
                    double dist_to_other = this->calculate_distance(current_mean, clusters_centers.at(c), dim_num);
                    if(dist_to_other < hald_bandwith)
                    {
                        merge_with = c;
                        break;
                    }
                }

                if (merge_with > -1)
                {
                    clusters_centers.at(merge_with) = 0.5 * (current_mean + clusters_centers.at(merge_with));
                    cluster_votes.at(merge_with) += current_cluster_votes;
                }
                else
                {
                    // increment cluster numbers
                    ++cluster_num;
                    // record the current mean
                    clusters_centers.push_back(current_mean);
                    cluster_votes.push_back(current_cluster_votes);
                }
                break;
            }
        }

        // remove visited points
        init_pts_inds.erase(
            std::remove_if(
                init_pts_inds.begin(),
                init_pts_inds.end(),
                [been_visited](int pts_idx){ return been_visited.at(pts_idx) == 1;}
            ),
            init_pts_inds.end()
        );
    }

    // assigning points to clusters
    std::vector<int> pts_cluster_votes(pts_num, 0);
    std::vector<int> pts_cluster_idx(pts_num, -1);

    for (size_t pt_idx = 0; pt_idx < pts_num; pt_idx++)
    {
        for (size_t cluster_idx = 0; cluster_idx < cluster_num; cluster_idx++)
        {
            if(pts_cluster_votes.at(pt_idx) < cluster_votes.at(cluster_idx).at<int>(pt_idx))
            {
                pts_cluster_votes.at(pt_idx) = cluster_votes.at(cluster_idx).at<int>(pt_idx);
                pts_cluster_idx.at(pt_idx) = cluster_idx;
            }
        }

    }

    // refactor centers as Mat(center_num, dim_num)
    _clusters_centers.create(static_cast<int>(cluster_votes.size()), dim_num, data_pts.type());
	cv::Mat const &clusters_ref = _clusters_centers.getMatRef();
	for (auto i = 0; i < clusters_centers.size(); ++i)
		clusters_centers[i].copyTo(clusters_ref.row(i));

    // refactor cluster points as a vector of vectors of points indices
    _cluster_pts.resize(cluster_votes.size());
	for (size_t i = 0; i < pts_cluster_idx.size(); ++i)
	{
		if (pts_cluster_idx[i] == -1)
			continue;
		_cluster_pts[pts_cluster_idx[i]].push_back(i);
	}

    return;
}

void MeanShift::meanshift(
    const cv::Mat &_data_pts,
    const int _pts_num,
    const int _dim_num,
    cv::Mat &_old_mean,
    cv::Mat &_new_mean,
    cv::Mat &_current_cluster_votes,
    std::vector<int> &_current_culster_members,
    std::vector<int> &_been_visited,
    const double _kernel_bandwidth)
{
    // mean shift procedure
    // 1. Compute the mean shift vector m
    // 2. translate the old mean
    // 1.1 iterate over all points and find indices of points
    //     that its distances from the current mean is less than the bandwidth
    double distance_val = 0;
    double kernel_val = 0;
    double normalizer = 0;
    int window_members_num = 0;
    cv::Mat current_pt;
    for (size_t idx = 0; idx < _pts_num; idx++)
    {
        // calculate the distance
        current_pt = _data_pts.row(idx);
        distance_val = this->calculate_distance(_old_mean, current_pt, _dim_num);

        if(distance_val <= _kernel_bandwidth)
        {
        // 1.2 vote for those points as clusters
            ++_current_cluster_votes.at<int>(idx);
        // 1.3 pass those points ||(current_mean - pts_i)/bandwidth|| to the kernel
            kernel_val = this->calculate_kernel(distance_val, _kernel_bandwidth);
            ++window_members_num;
        // 1.4 sum the return values of the kernel
            _new_mean += _data_pts.row(idx) * kernel_val;
            normalizer += kernel_val;
            _been_visited.at(idx) = 1;
            _current_culster_members.push_back(idx);

        }
        idx++;

    }
    if(window_members_num == 0)
    {
        _new_mean = _old_mean;
        return;
    }
    _new_mean /= normalizer;
}