/*
    Copyright (c) 2014, Philipp Krähenbühl
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:
        * Redistributions of source code must retain the above copyright
        notice, this list of conditions and the following disclaimer.
        * Redistributions in binary form must reproduce the above copyright
        notice, this list of conditions and the following disclaimer in the
        documentation and/or other materials provided with the distribution.
        * Neither the name of the Stanford University nor the
        names of its contributors may be used to endorse or promote products
        derived from this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY Philipp Krähenbühl ''AS IS'' AND ANY
    EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL Philipp Krähenbühl BE LIABLE FOR ANY
    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
	 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
	 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
	 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "qp.h"
#include "util.h"
#include <Eigen/Dense>

// #define VERBOSE
#ifdef VERBOSE
#include <iostream>
#endif

const int MAX_ITER = 1000;

struct ActiveSet {
	int n;       // Current size
	ArrayXb in; // in(i) : Is variable i in the active set
	ArrayXi A;  // All variables in the active set
	ActiveSet( int max_n=0 ):n(0),in(ArrayXb::Zero(max_n)),A(-ArrayXi::Ones(max_n)){}
	int add( int i ) { // Add variable i to the active set
		eassert( !in[i] );
		in[i] = true;
		A(n) = i;
		return n++;
	}
	void removeLast() { // Remove the last entry from the active set
		eassert( n > 0 );
		--n;
		eassert( in[A(n)] );
		in[A(n)] = false;
		A(n) = -1;
	}
};

template<typename T>
T dist( T a, T b ) {
	a = std::abs(a);
	b = std::abs(b);
	if (a > b) return a*std::sqrt(1 + (b/a)*(b/a));
	if (a < b) return b*std::sqrt(1 + (a/b)*(a/b));
	return a*std::sqrt(2.0);
}

template<typename T>
bool addConstraint( RMatrixX<T> & R, RMatrixX<T> & J, VectorX<T> d, ActiveSet & active_set, int l ) {
	const int n = R.rows();
	const int na = active_set.n;
	/* Givens rotation Vodoo */
	for( int j=n-1; j>na; j-- ) {
		T cc = d[j-1], ss = d[j];
		T h = dist( cc, ss );
		if( h == 0 )
			continue;
		d[j] = 0;
		ss /= h;
		cc /= h;
		if( cc < 0 ){
			cc =-cc;
			ss =-ss;
			d[j-1] = -h;
		}
		else
			d[j-1] = h;
		T xny = ss / (1. + cc);
		RowVectorX<T> t1 = J.row(j-1);
		RowVectorX<T> t2 = J.row(j);
		J.row(j-1) = t1*cc + t2*ss;
		J.row(j)   = xny * (t1 + J.row(j-1)) - t2;
	}
	active_set.add( l );
	R.col(na).head(na+1) = d.head(na+1);
	if( na > 0 && std::abs(d[na]) <= std::numeric_limits<T>::epsilon() * R.diagonal().head(na).array().abs().maxCoeff() )
		// Degenerate R
		return false;
	return true;
}

template<typename T>
void deleteConstraint( RMatrixX<T> & R, RMatrixX<T> & J, VectorX<T> & u, ActiveSet & active_set, int l ) {
	const int n = active_set.n;
	// Find the element to remove
	int qq = -1;
	for( int i=0; i<n; i++ )
		if( active_set.A[i] == l ){
			qq = i;
			break;
		}
	eassert( qq >= 0 );

	// Move all elements that follow down by ones and update the QR factorization
	for( int i=qq; i < n-1; i++ ) {
		u[i] = u[i+1];
		R.col(i) = R.col(i+1);
		active_set.A[i] = active_set.A[i+1];
	}
	// And remove the last element from the active set
	R.col(n-1).setZero();
	u[n-1] = 0;
	active_set.A[n-1] = l;
	active_set.removeLast();
	/* Some more givens rotation Vodoo to restore R and J */
	for( int j = qq; j < n-1; j++ ) {
		T cc = R(j,j), ss = R(j+1,j);
		T h = dist( cc, ss );
		if( h == 0 )
			continue;
		R(j+1,j) = 0;
		ss /= h;
		cc /= h;
		if( cc < 0 ){
			cc =-cc;
			ss =-ss;
			R(j,j) = -h;
		}
		else
			R(j,j) = h;
		T xny = ss / (1. + cc);
		const int nr = n-j-1;
		if( nr>0 ) {
			RowVectorX<T> t1 = R.row(j).segment(j+1,nr);
			RowVectorX<T> t2 = R.row(j+1).segment(j+1,nr);
			R.row(j).segment(j+1,nr)   = t1*cc + t2*ss;
			R.row(j+1).segment(j+1,nr) = xny * (t1 + R.row(j).segment(j+1,nr)) - t2;
		}
		{
			RowVectorX<T> t1 = J.row(j);
			RowVectorX<T> t2 = J.row(j+1);
			J.row(j)   = t1*cc + t2*ss;
			J.row(j+1) = xny * (t1 + J.row(j)) - t2;
		}
	}
}

// Solves a strictly convex QP of the form
//  minimize_x  x' Q x  +  c' x
//  subject to  A x <= b
template<typename T>
VectorX<T> qp( const RMatrixX<T> & Q, const VectorX<T> & c, const RMatrixX<T> & A, const VectorX<T> & b ) {
#ifdef VERBOSE
	printf("=================== SOLVING THE QP ===================\n" );
#endif
	const int n = c.size(), m = b.size();
	const T inf = std::numeric_limits<T>::infinity();

	if( Q.cols() != n || Q.rows() != n )
		throw std::invalid_argument( "Expected quadratic matrix Q of size n x n, where n=|c|!" );
	if( A.cols() != n || A.rows() != m )
		throw std::invalid_argument( "Expected inequality constraint matrix A of size |b| x n!" );

	RMatrixX<T> R = RMatrixX<T>::Zero(n,n), J = RMatrixX<T>::Zero(n,n);
	ActiveSet active_set(m);
	VectorX<T> x = VectorX<T>::Zero(n), s = VectorX<T>::Zero(m), u = VectorX<T>::Zero(m);

	/*
	 * Preprocessing phase
	 */

	/* decompose the matrix Q in the form LL^T */
	LLT<RMatrixX<T>,Lower> chol(Q);

	/* compute the inverse of the factorized matrix Q^-1, this is the initial value for J */
	// J = L^-1
	J = chol.matrixL().solve(RMatrixX<T>::Identity(n,n));

	/*
	 * Find the unconstrained minimizer x, which is a feasible point in the dual space
	 * x = -Q^-1 * c
	 */
	x = -chol.solve(c);
	for( int iter=0; iter < MAX_ITER; iter++ ) {

		/* step 1: choose a violated constraint */

		/* compute s(x) = - A * x + b for all elements of K \ A */
		s = -A * x + b;

#ifdef VERBOSE
		printf("Iteration %d   %f %f\n", iter, std::abs(s.array().min(0.f).sum())/m, std::numeric_limits<T>::epsilon() );
#endif

		/* Are there any more infeasibilities? |s|_1 < EPS*cond*100 */
		if ( -s.minCoeff() <= 100*std::numeric_limits<T>::epsilon() )
			return x;

		/* Step 2: check for feasibility and determine a new S-pair */
		int new_constraint = 0;
		T ss = VectorX<T>(s.array() * ((T)1-active_set.in.cast<T>())).minCoeff( &new_constraint );

		/* Are all constraints feasible? */
		if (ss >= 0.0) return x;

		/* Get the new constraint */
		VectorX<T> np = -A.row(new_constraint).transpose();

#ifdef VERBOSE
		printf("New constraint %d / %d\n", new_constraint, m);
#endif
		/* Step 2a: determine step direction and take partial steps */
		/* compute z = H np: the step direction in the primal space (through J, see the paper) */
		T unA = 0; // u(nA)
		while(1) {
			/* Get the number of active constraints */
			const int nA = active_set.n;

			VectorX<T> d = J * np;
			VectorX<T> z = J.bottomRows(d.size()-nA).transpose() * d.tail(d.size()-nA);

			/* compute N* np (if q > 0): the negative of the step direction in the dual space */
			VectorX<T> r = R.topLeftCorner(nA,nA).template triangularView<Upper>().solve(d.head(nA));

			/* Step 2b: compute step length t1 before violating dual feasibility */
			int l = -1;
			T t1 = inf;
			if( r.size() > 0 ) {
				t1 = (r.array() > 0).select( u.head(nA).array() / r.array(), inf ).minCoeff( &l );
				l = active_set.A[l];
			}

			/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
			T t2 = inf;
			if (std::abs(z.dot(z)) > std::numeric_limits<T>::epsilon()) // i.e. z != 0
				t2 = -ss / z.dot(np);

			/* the step is chosen as the minimum of t1 and t2 */
			T t = std::min<T>(t1, t2);

// #ifdef VERBOSE
// 			printf("  t = %f  %f %f    l = %d\n", t, t1, t2, l );
// 			std::cout<<"  ss = "<<ss<<"  z'np = "<<z.dot(np)<<std::endl;
// 			std::cout<<"  x = "<<x.transpose()<<std::endl;
// 			std::cout<<"  s = "<<s.transpose()<<std::endl;
// 			std::cout<<"  np = "<<np.transpose()<<std::endl;
// 			std::cout<<"  u = "<<u.head(nA).transpose()<<std::endl;
// 			std::cout<<"  r = "<<r.transpose()<<std::endl;
// 			std::cout<<"  z = "<<z.transpose()<<std::endl;
// 			std::cout<<"  d = "<<d.transpose()<<std::endl;
// 			std::cout<<"  R = "<<R.topLeftCorner(nA,nA)<<std::endl;
// 			std::cout<<"  J = "<<J<<std::endl;
// #endif

			/* Step 2c: determine new S-pair and take step: */

			/* case (i): no step in primal or dual space */
			if (t >= inf) /* QPP is infeasible */
				return VectorX<T>::Constant( n, inf );

			// Dual step
			u.head(nA) -= t * r;
			unA += t;

			// Primal step
			if( t2 < inf ) {
				x += t * z;
				ss = -A.row(new_constraint) * x + b(new_constraint);
			}

			/* case (ii): a patial step has taken */
			if (t2 > t1) {
#ifdef VERBOSE
				printf("  * delete %d\n", l );
#endif
				/* drop constraint l from the active set A */
				deleteConstraint<T>(R, J, u, active_set, l);
			}
			else
				break;
		}
		/* case (iii): step in primal and dual space */
		/* full step has been taken */
		/* add constraint ip to the active set*/
#ifdef VERBOSE
		printf("  * add %d\n", new_constraint );
#endif
		if (!addConstraint<T>(R, J, J*np, active_set, new_constraint))
			throw std::invalid_argument( "Degenerate R matrix, going to give up!" );
		u[active_set.n-1] = unA; // Update the lagrangian variable
	}
	return x;
}

// Solves a strictly convex sparse QP of the form
//  minimize_x  x' diag(Q) x  +  c' x
//  subject to  A x <= b
template<typename T>
VectorX<T> sparseQp( const VectorX<T> & Q, const VectorX<T> & c, const SRMatrixX<T> & A, const VectorX<T> & b ) {
	const int n = c.size(), m = b.size();
	const T inf = std::numeric_limits<T>::infinity();

	if( Q.size() != n )
		throw std::invalid_argument( "Expected vector Q of size n=|c|!" );
	if( A.cols() != n || A.rows() != m )
		throw std::invalid_argument( "Expected inequality constraint matrix A of size |b| x n!" );

	RMatrixX<T> R = RMatrixX<T>::Zero(n,n), J = RMatrixX<T>::Zero(n,n);
	ActiveSet active_set(m);
	VectorX<T> x = VectorX<T>::Zero(n), s = VectorX<T>::Zero(m), u = VectorX<T>::Zero(m);

	/*
	 * Preprocessing phase
	 */

	/* decompose the matrix Q in the form LL^T */
	const VectorX<T> L_inv = 1.0 / Q.array().sqrt();

	/* compute the inverse of the factorized matrix Q^-1, this is the initial value for J */
	// J = L^-1
	J = L_inv.asDiagonal();

	/*
	 * Find the unconstrained minimizer x, which is a feasible point in the dual space
	 * x = -Q^-1 * c
	 */
	x = -c.array() / Q.array();
	for( int iter=0; iter < MAX_ITER; iter++ ) {

		/* step 1: choose a violated constraint */

		/* compute s(x) = - A * x + b for all elements of K \ A */
		s = -A * x + b;

#ifdef VERBOSE
		printf("Iteration %d  %f %f\n", iter, std::abs(s.array().min(0.f).sum())/m, std::numeric_limits<T>::epsilon());
#endif

		/* Are there any more infeasibilities? |s|_1 < EPS*cond*100 */
		if ( -s.minCoeff() <= std::numeric_limits<T>::epsilon() )
			return x;

		/* Step 2: check for feasibility and determine a new S-pair */
		int new_constraint = 0;
		T ss = VectorX<T>(s.array() * ((T)1-active_set.in.cast<T>())).minCoeff( &new_constraint );

		/* Are all constraints feasible? */
		if (ss >= 0.0) return x;

		/* Get the new constraint */
		VectorX<T> np = -RowVectorX<T>(A.row(new_constraint)).transpose();

#ifdef VERBOSE
		printf("New constraint %d / %d\n", new_constraint, m);
#endif
		/* Step 2a: determine step direction and take partial steps */
		/* compute z = H np: the step direction in the primal space (through J, see the paper) */
		T unA = 0; // u(nA)
		while(1) {
			/* Get the number of active constraints */
			const int nA = active_set.n;

			VectorX<T> d = J * np;
			VectorX<T> z = J.bottomRows(d.size()-nA).transpose() * d.tail(d.size()-nA);

			/* compute N* np (if q > 0): the negative of the step direction in the dual space */
			VectorX<T> r = R.topLeftCorner(nA,nA).template triangularView<Upper>().solve(d.head(nA));

			/* Step 2b: compute step length t1 before violating dual feasibility */
			int l = -1;
			T t1 = inf;
			if( r.size() > 0 ) {
				t1 = (r.array() > 0).select( u.head(nA).array() / r.array(), inf ).minCoeff( &l );
				l = active_set.A[l];
			}

			/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
			T t2 = inf;
			if (std::abs(z.dot(z)) > std::numeric_limits<T>::epsilon()) // i.e. z != 0
				t2 = -ss / z.dot(np);

			/* the step is chosen as the minimum of t1 and t2 */
			T t = std::min<T>(t1, t2);

#ifdef VERBOSE
			printf("  t = %f  %f %f    l = %d\n", t, t1, t2, l );
			std::cout<<"  ss = "<<ss<<"  z'np = "<<z.dot(np)<<std::endl;
			std::cout<<"  x = "<<x.transpose()<<std::endl;
			std::cout<<"  s = "<<s.transpose()<<std::endl;
			std::cout<<"  np = "<<np.transpose()<<std::endl;
			std::cout<<"  u = "<<u.head(nA).transpose()<<std::endl;
			std::cout<<"  r = "<<r.transpose()<<std::endl;
			std::cout<<"  z = "<<z.transpose()<<std::endl;
			std::cout<<"  d = "<<d.transpose()<<std::endl;
			std::cout<<"  R = "<<R.topLeftCorner(nA,nA)<<std::endl;
			std::cout<<"  J = "<<J<<std::endl;
#endif

			/* Step 2c: determine new S-pair and take step: */

			/* case (i): no step in primal or dual space */
			if (t >= inf) /* QPP is infeasible */
				return VectorX<T>::Constant( n, inf );

			// Dual step
			u.head(nA) -= t * r;
			unA += t;

			// Primal step
			if( t2 < inf ) {
				x += t * z;
				ss = np.dot( x ) + b(new_constraint);
			}

			/* case (ii): a patial step has taken */
			if (t2 > t1) {
#ifdef VERBOSE
				printf("  * delete %d\n", l );
#endif
				/* drop constraint l from the active set A */
				deleteConstraint<T>(R, J, u, active_set, l);
			}
			else
				break;
		}
		/* case (iii): step in primal and dual space */
		/* full step has been taken */
		/* add constraint ip to the active set*/
#ifdef VERBOSE
		printf("  * add %d\n", new_constraint );
#endif
		if (!addConstraint<T>(R, J, J*np, active_set, new_constraint))
			throw std::invalid_argument( "Degenerate R matrix, going to give up!" );
		u[active_set.n-1] = unA; // Update the lagrangian variable
	}
	return x;
}

VectorXf qp( const RMatrixXf & Q, const VectorXf & c, const RMatrixXf & A, const VectorXf & b ) {
	return qp<float>( Q, c, A, b );
}
VectorXd qp( const RMatrixXd & Q, const VectorXd & c, const RMatrixXd & A, const VectorXd & b ) {
	return qp<double>( Q, c, A, b );
}
VectorXf sparseQp( const VectorXf & Q, const VectorXf & c, const SRMatrixXf & A, const VectorXf & b ) {
	return sparseQp<float>( Q, c, A, b );
}
VectorXd sparseQp( const VectorXd & Q, const VectorXd & c, const SRMatrixXd & A, const VectorXd & b ) {
	return sparseQp<double>( Q, c, A, b );
}


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <mitkBaseDataIOFactory.h>
#include <mitkBaseData.h>
#include <mitkImageCast.h>
#include <mitkImageToItk.h>
#include <metaCommand.h>
#include "mitkCommandLineParser.h"
#include <usAny.h>
#include <itkImageFileWriter.h>
#include <itkImageFileReader.h>
#include <mitkIOUtil.h>
#include <boost/lexical_cast.hpp>
#include <itkShCoefficientImageExporter.h>
#include <itkFlipImageFilter.h>

#define _USE_MATH_DEFINES
#include <math.h>

template<int shOrder>
int StartShConversion(int argc, char* argv[])
{
    mitkCommandLineParser parser;

    parser.setTitle("Export SH Image");
    parser.setCategory("Preprocessing Tools");
    parser.setDescription("");
    parser.setContributor("MBI");

    parser.setArgumentPrefix("--", "-");
    parser.addArgument("input", "i", mitkCommandLineParser::InputFile, "Input:", "MITK SH image", us::Any(), false);
    parser.addArgument("output", "o", mitkCommandLineParser::InputFile, "Output", "MRtrix SH image", us::Any(), false);
    parser.addArgument("shOrder", "sh", mitkCommandLineParser::Int, "SH order:", "spherical harmonics order");

    map<string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
    if (parsedArgs.size()==0)
        return EXIT_FAILURE;

    string inFile = us::any_cast<string>(parsedArgs["input"]);
    string outFile = us::any_cast<string>(parsedArgs["output"]);

    try
    {

        typedef itk::Image< float, 4 > OutImageType;
        typedef itk::Image< itk::Vector< float, (shOrder*shOrder + shOrder + 2)/2 + shOrder >, 3 > InputImageType;

        typename InputImageType::Pointer itkInImage = InputImageType::New();
        typedef itk::ImageFileReader< InputImageType > ReaderType;
        typename ReaderType::Pointer reader = ReaderType::New();
        std::cout << "reading " << inFile;
        reader->SetFileName(inFile.c_str());
        reader->Update();
        itkInImage = reader->GetOutput();

        // extract directions from fiber bundle
        typename itk::ShCoefficientImageExporter<float, shOrder>::Pointer filter = itk::ShCoefficientImageExporter<float,shOrder>::New();
        filter->SetInputImage(itkInImage);
        filter->GenerateData();
        OutImageType::Pointer outImage = filter->GetOutputImage();

        typedef itk::ImageFileWriter< OutImageType > WriterType;
        WriterType::Pointer writer = WriterType::New();
        writer->SetFileName(outFile.c_str());
        writer->SetInput(outImage);
        writer->Update();
    }
    catch (itk::ExceptionObject e)
    {
        std::cout << e;
        return EXIT_FAILURE;
    }
    catch (std::exception e)
    {
        std::cout << e.what();
        return EXIT_FAILURE;
    }
    catch (...)
    {
        std::cout << "ERROR!?!";
        return EXIT_FAILURE;
    }
    return EXIT_SUCCESS;
}

int main(int argc, char* argv[])
{

    mitkCommandLineParser parser;
    parser.setArgumentPrefix("--", "-");
    parser.addArgument("input", "i", mitkCommandLineParser::InputFile, "Input image", "MITK SH image", us::Any(), false);
    parser.addArgument("output", "o", mitkCommandLineParser::OutputFile, "Output image", "MRtrix SH image", us::Any(), false);
    parser.addArgument("shOrder", "sh", mitkCommandLineParser::Int, "Spherical harmonics order", "spherical harmonics order");

    parser.setCategory("Preprocessing Tools");
    parser.setTitle("Export SH Image");
    parser.setDescription("");
    parser.setContributor("MBI");

    map<string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
    if (parsedArgs.size()==0)
        return EXIT_FAILURE;

    int shOrder = -1;
    if (parsedArgs.count("shOrder"))
        shOrder = us::any_cast<int>(parsedArgs["shOrder"]);

    switch (shOrder)
    {
    case 4:
        return StartShConversion<4>(argc, argv);
    case 6:
        return StartShConversion<6>(argc, argv);
    case 8:
        return StartShConversion<8>(argc, argv);
    case 10:
        return StartShConversion<10>(argc, argv);
    case 12:
        return StartShConversion<12>(argc, argv);
    }
    return EXIT_FAILURE;
}


#include <general/nmspc_tensor_extra.h>
#include <general/nmspc_tensor_omp.h>
#include <tools/common/contraction.h>
#include <tools/common/fmt.h>

#include <iostream>
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/IterativeSolvers>

template<typename Scalar_> class MatrixReplacement;
template<typename T> using DenseMatrix = Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>;

namespace Eigen {
    namespace internal {
// MatrixReplacement looks-like a SparseMatrix, so let's inherits its traits:
        template<typename Scalar_>
        struct traits<MatrixReplacement<Scalar_>> :  public Eigen::internal::traits<DenseMatrix<Scalar_> >
        {};
    }
}

// Example of a matrix-free wrapper from a user type to Eigen's compatible type
// For the sake of simplicity, this example simply wrap a Eigen::Matrix.
template<typename Scalar_>
class MatrixReplacement : public Eigen::EigenBase<MatrixReplacement<Scalar_>> {
    public:
    // Required typedefs, constants, and method:
    typedef Scalar_ Scalar;
    typedef double RealScalar;
    typedef int StorageIndex;
    enum {
        ColsAtCompileTime = Eigen::Dynamic,
        MaxColsAtCompileTime = Eigen::Dynamic,
        IsRowMajor = false
    };

    const Scalar_ *     envL = nullptr;
    const Scalar_ *     envR = nullptr;
    const Scalar_ *     mpo = nullptr;
    std::array<long, 3> shape_mps;
    std::array<long, 4> shape_mpo;
    std::array<long, 3> shape_envL;
    std::array<long, 3> shape_envR;
    std::vector<Scalar> shift_mpo;
    long                mps_size;
//    eig::Form           form = eig::Form::SYMM;
//    eig::Side           side = eig::Side::R;
    // Profiling
//    std::unique_ptr<class_tic_toc> t_multAx;
    mutable int         counter = 0;
    mutable std::vector<Scalar> tmp;

    [[nodiscard]] Eigen::Index rows() const { return static_cast<int>(mps_size); }; /*!< Linear size\f$d^2 \times \chi_L \times \chi_R \f$  */
    [[nodiscard]] Eigen::Index cols() const { return static_cast<int>(mps_size); }; /*!< Linear size\f$d^2 \times \chi_L \times \chi_R \f$  */

    template<typename Rhs>
    Eigen::Product<MatrixReplacement,Rhs,Eigen::AliasFreeProduct> operator*(const Eigen::MatrixBase<Rhs>& x) const {
        return Eigen::Product<MatrixReplacement,Rhs,Eigen::AliasFreeProduct>(*this, x.derived());
    }

    // Custom API:
    MatrixReplacement()  = default;

    void attachTensors(
        const Scalar_ *     envL_,      /*!< The left block tensor.  */
        const Scalar_ *     envR_,      /*!< The right block tensor.  */
        const Scalar_ *     mpo_,       /*!< The Hamiltonian MPO's  */
        std::array<long, 3> shape_mps_, /*!< An array containing the shapes of the mps  */
        std::array<long, 4> shape_mpo_  /*!< An array containing the shapes of the mpo  */
    ){
        envL       = envL_;
        envR       = envR_;
        mpo        = mpo_;
        shape_mps  = shape_mps_;
        shape_mpo  = shape_mpo_;
        shape_envL = {shape_mps_[1], shape_mps_[1], shape_mpo_[0]};
        shape_envR = {shape_mps_[2], shape_mps_[2], shape_mpo_[1]};
        if(envL == nullptr) throw std::runtime_error("Lblock is a nullptr!");
        if(envR == nullptr) throw std::runtime_error("Rblock is a nullptr!");
        if(mpo == nullptr) throw std::runtime_error("mpo is a nullptr!");
        mps_size = shape_mps[0] * shape_mps[1] * shape_mps[2];
//        t_multAx = std::make_unique<class_tic_toc>(true, 5, "Time MultAx");
    }

};

// Implementation of MatrixReplacement * Eigen::DenseVector though a specialization of internal::generic_product_impl:
namespace Eigen {
    namespace internal {

        template<typename Rhs, typename ReplScalar>
        struct generic_product_impl<MatrixReplacement<ReplScalar>, Rhs, DenseShape, DenseShape, GemvProduct> // GEMV stands for matrix-vector
            : generic_product_impl_base<MatrixReplacement<ReplScalar>,Rhs,generic_product_impl<MatrixReplacement<ReplScalar>,Rhs> >
        {
            typedef typename Product<MatrixReplacement<ReplScalar>,Rhs>::Scalar Scalar;

            template<typename Dest>
            static void scaleAndAddTo(Dest& dst, const MatrixReplacement<ReplScalar>& mat, const Rhs& rhs, const Scalar& alpha)
            {
                // This method should implement "dst += alpha * lhs * rhs" inplace,
                // however, for iterative solvers, alpha is always equal to 1, so let's not worry about it.
                assert(alpha==Scalar(1) && "scaling is not implemented");
                EIGEN_ONLY_USED_FOR_DEBUG(alpha);

//                auto token = mat.t_multAx->tic_token();
                mat.tmp.resize(static_cast<size_t>(dst.size()));
                Eigen::Map<Dest> tmp_map (mat.tmp.data(), dst.size());
//                std::cout << "dst size " << dst.size() << " | rhs size " << rhs.size() << std::endl;
//                if(dst.size() != rhs.size())
//                    dst.conservativeResize(rhs.size());

                tools::common::contraction::matrix_vector_product(tmp_map.data(), rhs.data(), mat.shape_mps,
                                                                  mat.mpo,  mat.shape_mpo,
                                                                  mat.envL, mat.shape_envL,
                                                                  mat.envR, mat.shape_envR);

                dst.noalias() += tmp_map;
                mat.counter++;
//
//                // Here we could simply call dst.noalias() += lhs.my_matrix() * rhs,
//                // but let's do something fancier (and less efficient):
//                for(Index i=0; i<lhs.cols(); ++i)
//                    dst += rhs(i) * lhs.my_matrix().col(i);
            }
        };

    }
}

/* clang-format off */
template<typename Scalar>
void tools::common::contraction::matrix_inverse_vector_product(Scalar * res_ptr,
                                                       const Scalar * const mps_ptr, std::array<long,3> mps_dims,
                                                       const Scalar * const mpo_ptr, std::array<long,4> mpo_dims,
                                                       const Scalar * const envL_ptr, std::array<long,3> envL_dims,
                                                       const Scalar * const envR_ptr, std::array<long,3> envR_dims){

        // Here we return x <-- A^-1 * b
        // Where A^-1 * b is obtained by solving
        //       A*x = b
        // using an iterative matrix-free solver.
        {
            auto mps = Eigen::TensorMap<const Eigen::Tensor<const Scalar,3>>(mps_ptr,mps_dims);
            auto mpo = Eigen::TensorMap<const Eigen::Tensor<const Scalar,4>>(mpo_ptr,mpo_dims);
            auto envL = Eigen::TensorMap<const Eigen::Tensor<const Scalar,3>>(envL_ptr,envL_dims);
            auto envR = Eigen::TensorMap<const Eigen::Tensor<const Scalar,3>>(envR_ptr,envR_dims);

            if(mps.dimension(1) != envL.dimension(0))
                throw std::runtime_error(fmt::format("Dimension mismatch mps {} and envL {}", mps.dimensions(), envL.dimensions()));
            if(mps.dimension(2) != envR.dimension(0))
                throw std::runtime_error(fmt::format("Dimension mismatch mps {} and envR {}", mps.dimensions(), envR.dimensions()));
            if(mps.dimension(0) != mpo.dimension(2)) throw std::runtime_error(fmt::format("Dimension mismatch mps {} and mpo {}", mps.dimensions(), mpo.dimensions()));
            if(envL.dimension(2) != mpo.dimension(0))
                throw std::runtime_error(fmt::format("Dimension mismatch envL {} and mpo {}", envL.dimensions(), mpo.dimensions()));
            if(envR.dimension(2) != mpo.dimension(1))
                throw std::runtime_error(fmt::format("Dimension mismatch envR {} and mpo {}", envR.dimensions(), mpo.dimensions()));
        }

        // Define the "matrix-free" matrix replacement.
        MatrixReplacement<Scalar> matRepl;
        matRepl.attachTensors(envL_ptr, envR_ptr, mpo_ptr, mps_dims, mpo_dims);

        Eigen::Index MaxIters = 200000;
        double tolerance = 1e-14;
        Eigen::Map<Textra::VectorType<Scalar>> res(res_ptr, matRepl.rows());
        Eigen::Map<const Textra::VectorType<Scalar>> mps(mps_ptr, matRepl.rows());
        if constexpr (std::is_same_v<Scalar,std::complex<double>>){

            Eigen::BiCGSTAB<MatrixReplacement<Scalar>, Eigen::IdentityPreconditioner> bicg;
            bicg.compute(matRepl);
            bicg.setMaxIterations(MaxIters);
            bicg.setTolerance(tolerance);
            res = bicg.solve(mps);
//            std::cout << "BiCGSTAB: #iterations: " << bicg.iterations()
//                << ", #count: " << matRepl.counter
//                << ", estimated error: " << bicg.error()
//                << std::endl;

        }
//        {
//            Eigen::ConjugateGradient<MatrixReplacement<Scalar>, Eigen::Lower|Eigen::Upper, Eigen::IdentityPreconditioner> cg;
//            cg.setMaxIterations(20000);
//            cg.compute(matRepl);
//            res = cg.solve(mps);
//            std::cout << "CG      : #iterations: " << cg.iterations()
//                      << ", #count: " << matRepl.counter
//                      << ", estimated error: " << cg.error()
//                      << std::endl;
//        }
        if constexpr (std::is_same_v<Scalar,double>){
            Eigen::MINRES<MatrixReplacement<Scalar>, Eigen::Lower|Eigen::Upper, Eigen::IdentityPreconditioner> minres;
            minres.setMaxIterations(MaxIters);
            minres.setTolerance(tolerance);
            minres.compute(matRepl);
            res = minres.solve(mps);
            std::cout << "MINRES:   #iterations: " << minres.iterations()
                      << ", #count: " << matRepl.counter
                      << ", norm: " << res.norm()
                      << ", estimated error: " << minres.error()
                      << std::endl;
    //    std::cout << "x: \n" << x << std::endl;
        }

//        {
//            Eigen::ConjugateGradient<MatrixReplacement<Scalar>, Eigen::Lower|Eigen::Upper, Eigen::IdentityPreconditioner> cg;
//            cg.setMaxIterations(20000);
//            cg.compute(matRepl);
//            res = cg.solve(mps);
//            std::cout << "CG      : #iterations: " << cg.iterations()
//                      << ", #count: " << matRepl.counter
//                      << ", estimated error: " << cg.error()
//                      << std::endl;
//        }

}
using namespace tools::common::contraction;
template void tools::common::contraction::matrix_inverse_vector_product(
    real * res_ptr,
    const real * const mps_ptr, std::array<long,3> mps_dims,
    const real * const mpo_ptr, std::array<long,4> mpo_dims,
    const real * const envL_ptr, std::array<long,3> envL_dims,
    const real * const envR_ptr, std::array<long,3> envR_dims);
template void tools::common::contraction::matrix_inverse_vector_product(
    cplx * res_ptr,
    const cplx * const mps_ptr, std::array<long,3> mps_dims,
    const cplx * const mpo_ptr, std::array<long,4> mpo_dims,
    const cplx * const envL_ptr, std::array<long,3> envL_dims,
    const cplx * const envR_ptr, std::array<long,3> envR_dims);
/* clang-format on */


/**
 * Native StructureGrabber methods.
 */

#include <jni.h>
#include <android/log.h>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <pthread.h>

#include <fstream>

#include <opencv/cv.h>
#include "opencv2/highgui/highgui.hpp"

#include <OpenNI.h>

#include <boost/scoped_array.hpp>

#include <Eigen/Core>

#include "utils/include/TimeUtils.h"

using namespace std;
using namespace openni;
using namespace cv;
using namespace Eigen;
using namespace boost;

#define  LOG_TAG	"StructureGrabber"
#define  LOGI(...)	__android_log_print(ANDROID_LOG_INFO, LOG_TAG, __VA_ARGS__)
#define  LOGE(...)	__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__)

// Global variables accessible only within this source file
namespace
{
	pthread_mutex_t mutexStructure;

	scoped_array<DepthPixel> depthPtr;
	scoped_array<uint16_t> irPtr;
	double timestampVal;
	int resolutionX, resolutionY;
	int totalResolutionStructure;
	VideoStream depthStream, irStream;

	bool initializedStructure = false;
}

class ImageCallback : public VideoStream::NewFrameListener
{
	public:
		void onNewFrame(VideoStream& stream)
		{
			pthread_mutex_lock(&mutexStructure);

			stream.readFrame(&frameRef);

			DepthPixel* depthPtrTemp;
			Grayscale16Pixel* irPtrTemp;
			switch (frameRef.getVideoMode().getPixelFormat())
			{
				// Structure depth format
				case PIXEL_FORMAT_DEPTH_1_MM:
					depthPtrTemp = (DepthPixel*) frameRef.getData();
					memcpy(depthPtr.get(), depthPtrTemp, sizeof(DepthPixel) * totalResolutionStructure);
					timestampVal = ((double)getMillisecondsSinceEpoch() * 0.001);
					break;

				// Structure ir format
				case PIXEL_FORMAT_GRAY16:
					irPtrTemp = (Grayscale16Pixel*) frameRef.getData();
					memcpy(irPtr.get(), irPtrTemp, sizeof(Grayscale16Pixel) * totalResolutionStructure);
					timestampVal = ((double)getMillisecondsSinceEpoch() * 0.001);
					break;

				default:
					LOGI("Unsupported pixel format");
					break;
			}

			pthread_mutex_unlock(&mutexStructure);
		}

	private:
		VideoFrameRef frameRef;
};

// Global variables accessible only within this source file
namespace
{
	Device device;
	ImageCallback listenerDepth, listenerIR;

	Mat rgbIntrinsics;
	Mat rgbDistortion;
	Mat irIntrinsics;
	Mat irDistortion;
	Mat translationVectorRgbIr;
	Mat rotationMatrixRgbIr;
	Mat translationVectorIrRgb;
	Mat rotationMatrixIrRgb;

	Matrix3f eigenIntrinsicRgb;

	Matrix3f eigenRotationIrRgb;
	Vector3f eigenTranslationIrRgb;

	scoped_array<int> mapIr;
	scoped_array<int> revMapIr;
	scoped_array<int> mapRgb;
	scoped_array<int> revMapRgb;
	scoped_array<bool> maskIr;
	scoped_array<bool> maskRgb;

	// Coordinates = RotIrToRgb * [(x - cx) / fx, (y - cy) / fy, 1]
	scoped_array<Vector3f> cachedCoordinates;
}

extern "C"
{
	JNIEXPORT jboolean JNICALL Java_it_unibo_slam_input_StructureGrabber_initNative(JNIEnv *env, jobject obj);
	JNIEXPORT void JNICALL Java_it_unibo_slam_input_StructureGrabber_getResolutionNative(JNIEnv *env, jobject obj,
			jintArray resolution);
	JNIEXPORT jfloat JNICALL Java_it_unibo_slam_input_StructureGrabber_getMaxDepthNative(JNIEnv *env, jobject obj);
	JNIEXPORT void JNICALL Java_it_unibo_slam_input_StructureGrabber_getIntrinsicParamsNative(JNIEnv *env, jobject obj,
			jfloatArray focal, jfloatArray center);
	JNIEXPORT jboolean JNICALL Java_it_unibo_slam_input_StructureGrabber_getFrame(JNIEnv *env, jobject obj,
			jdoubleArray timestamp, jfloatArray depthFrame);
}

void initStandandAndReverseUndistortMapping(Mat cameraMatrix, Mat distortionCoefficients,
											Size imageSize, int *mapXY, int *revMapXY,
											bool *mask = NULL)
{
	float centerX = cameraMatrix.at<float>(0, 2), centerY = cameraMatrix.at<float>(1, 2);
	float focalX = cameraMatrix.at<float>(0, 0), focalY = cameraMatrix.at<float>(1, 1);

	double k1 = distortionCoefficients.at<float>(0, 0);
	double k2 = distortionCoefficients.at<float>(1, 0);
	double p1 = distortionCoefficients.at<float>(2, 0);
	double p2 = distortionCoefficients.at<float>(3, 0);
	double k3 = (distortionCoefficients.rows > 4) ? distortionCoefficients.at<float>(4, 0) : 0;
	double k4 = (distortionCoefficients.rows > 5) ? distortionCoefficients.at<float>(5, 0) : 0;
	double k5 = (distortionCoefficients.rows > 6) ? distortionCoefficients.at<float>(6, 0) : 0;
	double k6 = (distortionCoefficients.rows > 7) ? distortionCoefficients.at<float>(7, 0) : 0;

	for (int i = 0; i < imageSize.width * imageSize.height; i++)
	{
		mapXY[i] = -1;
		revMapXY[i] = -1;
		if (mask != NULL)
			mask[i] = false;
	}

	float x, y, x2, y2, m2xy, r2, kr;
	int id = 0, indexBackProject = 0, u, v;
	for (float ey = 0; ey < imageSize.height; ey++)
	{
		y = (ey - centerY) / focalY;
		y2 = y*y;

		for (float ex = 0; ex < imageSize.width; ex++, id++)
		{
			x = (ex - centerX) / focalX;
			x2 = x*x;
			r2 = x2 + y2;
			m2xy = 2*x*y;

			kr = (1 + ((k3 * r2 + k2) * r2 + k1) * r2) / (1 + ((k6 * r2 + k5) * r2 + k4) * r2);
			u = (int)round(focalX * (x * kr + p1 * m2xy + p2 * (r2 + 2 * x2)) + centerX);
			v = (int)round(focalY * (y * kr + p1 * (r2 + 2 * y2) + p2 * m2xy) + centerY);

			if (u >= 0 && u < imageSize.width && v >= 0 && v < imageSize.height)
			{
				indexBackProject = u + v * imageSize.width;

				if (revMapXY[indexBackProject] == -1)
				{
					mapXY[id] = indexBackProject;
					revMapXY[indexBackProject] = id;
					if (mask != NULL)
						mask[id] = true;
				}
			}
		}
	}
}

void initializeMappingVariables(string calibrationFile, int width, int height)
{
	FileStorage storage(calibrationFile, FileStorage::READ);

	storage["RgbIntrinsics"] >> rgbIntrinsics;
	storage["RgbDistortion"] >> rgbDistortion;
	storage["IrIntrinsics"] >> irIntrinsics;
	storage["IrDistortion"] >> irDistortion;
	storage["T"] >> translationVectorRgbIr;
	storage["R"] >> rotationMatrixRgbIr;

	storage.release();

	rotationMatrixRgbIr.convertTo(rotationMatrixRgbIr, CV_32FC1);
	translationVectorRgbIr.convertTo(translationVectorRgbIr, CV_32FC1);
	rgbIntrinsics.convertTo(rgbIntrinsics, CV_32FC1);
	irIntrinsics.convertTo(irIntrinsics, CV_32FC1);
	rgbDistortion.convertTo(rgbDistortion, CV_32FC1);
	irDistortion.convertTo(irDistortion, CV_32FC1);

	rotationMatrixIrRgb = rotationMatrixRgbIr.t();
	translationVectorIrRgb = -1.0F * rotationMatrixIrRgb * translationVectorRgbIr;

	eigenIntrinsicRgb << 	rgbIntrinsics.at<float>(0, 0), rgbIntrinsics.at<float>(0, 1), rgbIntrinsics.at<float>(0, 2),
							rgbIntrinsics.at<float>(1, 0), rgbIntrinsics.at<float>(1, 1), rgbIntrinsics.at<float>(1, 2),
							rgbIntrinsics.at<float>(2, 0), rgbIntrinsics.at<float>(2, 1), rgbIntrinsics.at<float>(2, 2);

	eigenRotationIrRgb <<	rotationMatrixIrRgb.at<float>(0, 0), rotationMatrixIrRgb.at<float>(0, 1), rotationMatrixIrRgb.at<float>(0, 2),
							rotationMatrixIrRgb.at<float>(1, 0), rotationMatrixIrRgb.at<float>(1, 1), rotationMatrixIrRgb.at<float>(1, 2),
							rotationMatrixIrRgb.at<float>(2, 0), rotationMatrixIrRgb.at<float>(2, 1), rotationMatrixIrRgb.at<float>(2, 2);
	eigenTranslationIrRgb = Vector3f(	translationVectorIrRgb.at<float>(0, 0),
										translationVectorIrRgb.at<float>(1, 0),
										translationVectorIrRgb.at<float>(2, 0));

	mapIr.reset(new int[width * height]);
	revMapIr.reset(new int[width * height]);
	maskIr.reset(new bool[width * height]);
	initStandandAndReverseUndistortMapping(irIntrinsics, irDistortion, Size(width, height), mapIr.get(), revMapIr.get(), maskIr.get());

	mapRgb.reset(new int[width * height]);
	revMapRgb.reset(new int[width * height]);
	maskRgb.reset(new bool[width * height]);
	initStandandAndReverseUndistortMapping(rgbIntrinsics, rgbDistortion, Size(width, height), mapRgb.get(), revMapRgb.get(), maskRgb.get());

	float irCenterX = irIntrinsics.at<float>(0, 2);
	float irCenterY = irIntrinsics.at<float>(1, 2);
	float irFocalX = irIntrinsics.at<float>(0, 0);
	float irFocalY = irIntrinsics.at<float>(1, 1);

	Vector3f tempCoord(0, 0, 1);
	cachedCoordinates.reset(new Vector3f[width * height]);
	for (float v = 0, id = 0; v < height; v++)
	{
		tempCoord[1] = (v - irCenterY) / irFocalY;

		for (float u = 0; u < width; u++, id++)
		{
			tempCoord[0] = (u - irCenterX) / irFocalX;

			cachedCoordinates[id] = eigenRotationIrRgb * tempCoord;
		}
	}
}

JNIEXPORT jboolean JNICALL Java_it_unibo_slam_input_StructureGrabber_initNative(JNIEnv *env, jobject obj)
{
	if (initializedStructure)
		return true;

	pthread_mutex_init(&mutexStructure, NULL);

	char cwd[1024];
	getcwd(cwd, 1024);

	chdir("/sdcard/StructureCalibrator/config");

	Status status = OpenNI::initialize();
	if (status != STATUS_OK)
	{
		LOGE("OpenNI init failed: %s", OpenNI::getExtendedError());
		return false;
	}

	status = device.open(ANY_DEVICE);
	if (status != STATUS_OK)
	{
		LOGE("Couldn't open the device: %s", OpenNI::getExtendedError());
		return false;
	}

	status = depthStream.create(device, SENSOR_DEPTH);
	if (status != STATUS_OK)
	{
		LOGE("Couldn't create the depth stream: %s", OpenNI::getExtendedError());
		return false;
	}

	/*status = irStream.create(device, SENSOR_IR);
	if (status != STATUS_OK)
	{
		LOGE("Couldn't create the infrared stream: %s", OpenNI::getExtendedError());
		return false;
	}*/

	resolutionX = depthStream.getVideoMode().getResolutionX();
	resolutionY = depthStream.getVideoMode().getResolutionY();
	totalResolutionStructure = resolutionX * resolutionY;
	depthPtr.reset(new DepthPixel[totalResolutionStructure]);
	irPtr.reset(new Grayscale16Pixel[totalResolutionStructure]);

	initializeMappingVariables("/sdcard/CalibData.xml", resolutionX, resolutionY);

	depthStream.start();
	depthStream.addNewFrameListener(&listenerDepth);

	/*irStream.start();
	irStream.addNewFrameListener(&listenerIR);*/

	chdir(cwd);

	initializedStructure = true;

	return true;
}

JNIEXPORT void JNICALL Java_it_unibo_slam_input_StructureGrabber_getResolutionNative(JNIEnv *env, jobject obj,
		jintArray resolution)
{
	if (!initializedStructure)
	{
		jint *tempResolution = env->GetIntArrayElements(resolution, NULL);
		tempResolution[0] = 0;
		tempResolution[1] = 0;
		env->ReleaseIntArrayElements(resolution, tempResolution, 0);
		return;
	}

	// Depth and image resolution must be equal
	jint *tempResolution = env->GetIntArrayElements(resolution, NULL);
	tempResolution[0] = depthStream.getVideoMode().getResolutionX();
	tempResolution[1] = depthStream.getVideoMode().getResolutionY();
	env->ReleaseIntArrayElements(resolution, tempResolution, 0);
}

JNIEXPORT jfloat JNICALL Java_it_unibo_slam_input_StructureGrabber_getMaxDepthNative(JNIEnv *env, jobject obj)
{
	if (!initializedStructure)
		return -1;

	return 10.0F;//TODO(jfloat)(depthStream.getMaxPixelValue() * 0.001F);
}

JNIEXPORT void JNICALL Java_it_unibo_slam_input_StructureGrabber_getIntrinsicParamsNative(JNIEnv *env, jobject obj,
		jfloatArray focal, jfloatArray center)
{
	jfloat *tempFocal = env->GetFloatArrayElements(focal, NULL);
	jfloat *tempCenter = env->GetFloatArrayElements(center, NULL);

	if (!initializedStructure)
	{
		tempFocal[0] = 0;
		tempFocal[1] = 0;

		tempCenter[0] = 0;
		tempCenter[1] = 0;
	}
	else
	{
		tempFocal[0] = irIntrinsics.at<float>(0, 0);
		tempFocal[1] = irIntrinsics.at<float>(1, 1);

		tempCenter[0] = irIntrinsics.at<float>(0, 2);
		tempCenter[1] = irIntrinsics.at<float>(1, 2);
	}

	env->ReleaseFloatArrayElements(focal, tempFocal, 0);
	env->ReleaseFloatArrayElements(center, tempCenter, 0);
}

void mapDepthToRgb(DepthPixel *depthIn, float *depthOut)
{
	float rgbCenterX = rgbIntrinsics.at<float>(0, 2);
	float rgbCenterY = rgbIntrinsics.at<float>(1, 2);
	float rgbFocalX = rgbIntrinsics.at<float>(0, 0);
	float rgbFocalY = rgbIntrinsics.at<float>(1, 1);

	int tempIndex = 0, tempU = 0, tempV = 0;
	Vector3f rgbCoord(0, 0, 0);
	float u, v, tempDepth;
	for (int id = 0; id < totalResolutionStructure; id++)
	{
		if (maskIr[id] && depthIn[mapIr[id]] > 0)
		{
			tempDepth = (float)depthIn[mapIr[id]] * 0.001F;
			rgbCoord = ((cachedCoordinates[mapIr[id]] * tempDepth) + eigenTranslationIrRgb);

			if (tempDepth > 0 && rgbCoord.z() > 0)
			{
				rgbCoord.x() = rgbCoord.x() * rgbFocalX + rgbCenterX * rgbCoord.z();
				rgbCoord.y() = rgbCoord.y() * rgbFocalY + rgbCenterY * rgbCoord.z();

				/*u = rgbCoord.x() / rgbCoord.z();
				v = rgbCoord.y() / rgbCoord.z();

				bool useHigherU = false, useHigherV = false;
				if (round(u) == ceil(u))
					useHigherU = true;
				if (round(v) == ceil(v))
					useHigherV = true;*/

				/*for (int x = 0; x < 4; x++)
				{*/

					/*if (x == 0)
					{
						if (useHigherU)
							tempU = (int)u;
						else
							tempU = (int)u - 1;

						if (useHigherV)
							tempV = (int)ceil(v);
						else
							tempV = (int)v;
					}
					else if (x == 1)
					{
						if (useHigherU)
							tempU = (int)ceil(u);
						else
							tempU = (int)u;

						if (useHigherV)
							tempV = (int)ceil(v);
						else
							tempV = (int)v;
					}
					else if (x == 2)
					{
						if (useHigherU)
							tempU = (int)u;
						else
							tempU = (int)u - 1;

						if (useHigherV)
							tempV = (int)v;
						else
							tempV = (int)v - 1;
					}
					else if (x == 3)
					{
						if (useHigherU)
							tempU = (int)ceil(u);
						else
							tempU = (int)u;

						if (useHigherV)
							tempV = (int)v;
						else
							tempV = (int)v - 1;
					}*/

					tempU = (int)round(rgbCoord.x() / rgbCoord.z());
					tempV = (int)round(rgbCoord.y() / rgbCoord.z());

					if (tempU >= 0 && tempU < resolutionX && tempV >= 0 && tempV < resolutionY)
					{
						tempIndex = tempU + tempV * resolutionX;

						if (maskRgb[revMapRgb[tempIndex]])
						{
							if (depthOut[revMapRgb[tempIndex]] == 0)
								depthOut[revMapRgb[tempIndex]] = tempDepth;
							else if (depthOut[revMapRgb[tempIndex]] > 0 && depthOut[revMapRgb[tempIndex]] > tempDepth)
								depthOut[revMapRgb[tempIndex]] = tempDepth;
						}
					}
				//}
			}
		}
	}
}

JNIEXPORT jboolean JNICALL Java_it_unibo_slam_input_StructureGrabber_getFrame(JNIEnv *env, jobject obj,
		jdoubleArray timestamp, jfloatArray depthFrame)
{
	if (!initializedStructure)
		return false;

	pthread_mutex_lock(&mutexStructure);

	/*Mat depth(480, 640, CV_16UC1, depthPtr.get());
	Mat ir(480, 640, CV_16UC1, irPtr.get());
	imwrite("sdcard/depthProva.png", depth * ((float)65535 / (float)3500));
	imwrite("sdcard/irProva.png", ir * ((float)65535 / (float)1023));*/

	jfloat *tempDepth = env->GetFloatArrayElements(depthFrame, NULL);
	/*for (int i = 0; i < totalResolutionStructure; i++)
		tempDepth[i] = irPtr[i] * 10.0F / 1023.0F;*/

	/*for (int i = 0; i < totalResolutionStructure; i++)
		tempDepth[i] = depthPtr[i] * 0.001F;*/
	mapDepthToRgb(depthPtr.get(), tempDepth);

	jdouble *tempTimestamp = env->GetDoubleArrayElements(timestamp, NULL);

	tempTimestamp[0] = (jdouble)timestampVal;

	env->ReleaseFloatArrayElements(depthFrame, tempDepth, 0);
	env->ReleaseDoubleArrayElements(timestamp, tempTimestamp, 0);

	pthread_mutex_unlock(&mutexStructure);

	return true;
}


#include <iostream>
#include <thread>
#include <chrono>
#include <boost/format.hpp>
#include <termios.h>

#include "flyonspeed_scheme.h"
#include "stallfence_scheme.h"
#include "isettings.h"
#include "iairdata.h"
#include "units.h"

constexpr double kAlphaBetaStep = 0.01;
constexpr int kDisplayHalfSize = 20;

class TestSettings : public airball::ISettings {
public:
  double v_full_scale() const override { return v_full_scale_; }
  double v_r() const override { return v_r_; }
  double v_fe() const override { return v_fe_; }
  double v_no() const override { return v_no_; }
  double v_ne() const override { return v_ne_; }
  double alpha_stall() const override { return alpha_stall_; }
  double alpha_stall_warning() const override { return alpha_stall_warning_; }
  double alpha_min() const override { return alpha_min_; }
  double alpha_x() const override { return alpha_x_; }
  double alpha_y() const override { return alpha_y_; }
  double alpha_ref() const override { return alpha_ref_; }
  double beta_full_scale() const override { return beta_full_scale_; }

  double beta_bias() const override { return 0; }
  double baro_setting() const override { return 0; }
  double ball_smoothing_factor() const override { return 0; }
  double vsi_smoothing_factor() const override { return 0; }
  int screen_width() const override { return 0; }
  int screen_height() const override { return 0; }
  bool show_altimeter() const override { return false; }
  bool show_link_status() const override { return false; }
  bool show_probe_battery_status() const override { return false; }
  std::string sound_scheme() const override { return ""; }
  double audio_volume() const override { return 1.0; }

  double v_full_scale_ = 0;
  double v_r_ = 0;
  double v_fe_ = 0;
  double v_no_ = 0;
  double v_ne_ = 0;
  double alpha_stall_ = 0;
  double alpha_stall_warning_ = 0;
  double alpha_min_ = 0;
  double alpha_x_ = 0;
  double alpha_y_ = 0;
  double alpha_ref_ = 0;
  double beta_full_scale_ = 0;
};

class TestAirdata : public airball::IAirdata {
public:
  TestAirdata() {
    balls_.emplace_back(Ball(0.0, 0.0, 0.0, 0.0));
  }

  double altitude() const override { return 0; }
  double climb_rate() const override {  return 0; }
  bool valid() const override { return true; }

  const Ball &smooth_ball() const override { return balls_[0]; }
  const std::vector<Ball> &raw_balls() const override { return balls_; }

  void set_ball(Ball b) { balls_[0]= b; }

private:
  std::vector<Ball> balls_;
};

void print_alpha_beta(double alpha, double beta) {
  int k = beta * kDisplayHalfSize;
  std::cout << "|";
  for (int i = -kDisplayHalfSize; i <= kDisplayHalfSize; i++) {
    if (i == k) {
      std::cout << "*";
    } else if (i == 0) {
      std::cout << ".";
    } else {
      std::cout << " ";
    }
  }
  std::cout << "|";
  std::cout << " alpha = " << boost::format("%05.02d") % alpha;
  std::cout << std::endl;
}

int main(int argc, char**argv) {
  if (argc < 2) {
    std::cerr << "Usage: " << argv[0] << " <device-name> <scheme-name>" << std::endl;
    return -1;
  }

  TestSettings settings;
  settings.alpha_min_ = 0.0;
  settings.alpha_y_ = 0.5;
  settings.alpha_ref_ = 0.75;
  settings.alpha_stall_ = 1.0;
  settings.alpha_stall_warning_ = 0.95;
  settings.beta_full_scale_ = 1.0;

  TestAirdata airdata;

  std::string device_name(argv[1]);
  std::string scheme_name(argv[2]);
  std::unique_ptr<airball::sound_scheme> scheme;

  if (scheme_name == "flyonspeed") {
    scheme.reset(new airball::flyonspeed_scheme(
        device_name,
        &settings,
        &airdata));
  } else if (scheme_name == "stallfence") {
    scheme.reset(new airball::stallfence_scheme(
        device_name,
        &settings,
        &airdata));
  } else {
    std::cerr << "Unknown scheme: " << scheme_name << std::endl;
    return -1;
  }

  if (!scheme->start()) {
    std::cout << "Error" << std::endl;
    return -1;
  }

  auto set_alpha_beta = [&](double alpha, double beta) {
    print_alpha_beta(alpha, beta);
    airdata.set_ball(airball::IAirdata::Ball(
        degrees_to_radians(alpha),
        degrees_to_radians(beta),
        0.0,
        0.0));
    scheme->update();
    std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(10));
  };

  double alpha = 0.0;
  double beta = 0.0;

  set_alpha_beta(alpha, beta);

  struct termios old_settings;
  tcgetattr(STDIN_FILENO, &old_settings);
  struct termios new_settings = old_settings;
  new_settings.c_lflag &= (~ICANON & ~ECHO);
  tcsetattr(STDIN_FILENO, TCSANOW, &new_settings);

  for (bool running = true; running; ) {
    char d = 0;
    if (!read(STDIN_FILENO, &d, 1)) {
      break;
    }
    bool set = true;
    switch (d) {
      case 'u':
        alpha += kAlphaBetaStep;
        break;
      case 'd':
        alpha -= kAlphaBetaStep;
        break;
      case 'r':
        beta += kAlphaBetaStep;
        break;
      case 'l':
        beta -= kAlphaBetaStep;
        break;
      case 'c':
        beta = 0;
        break;
      case 'x':
        running = false;
        set = false;
        break;
      default:
        set = false;
        break;
    }
    if (set) {
      set_alpha_beta(alpha, beta);
    }
  }

  tcsetattr(STDIN_FILENO, TCSANOW, &old_settings);

  return 0;
}