#include "boundary.hpp"
#include "eigen_ext.hpp"
#include "parameters.hpp"
#include <cassert>
#include <iostream>

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace pear {
void boundary_vector(Vec &xp, Vec &yp, MatI &boundary, Vec &Bu, Vec &Bv,
                     Mat &Kbu, Mat &Kbv) {
  /*
  INPUTS
  xp : vector with points x coordinates
  yp : vector with points y coordinates
  g1 : vector of indices of first points of the gamma1 boundary segment
  g2 : vector of indices of second ponts of the gamma1 boundary segment

  OUTPUTS
  Bu, Bv : boundary vector
  Kbu, Kbv : boundary vector term dependent on Cu and Cv
   */
  int np = xp.rows();
  int nb = boundary.rows();
  int ng = 2 * nb - boundary.col(2).sum();

  VecI g1(ng);
  VecI g2(ng);
  compute_skin(boundary, g1, g2);

  // COMPUTE DISTANCES
  Vec hpx =
      (pear::extract<Vec>(xp, g1) - pear::extract<Vec>(xp, g2)).array().pow(2);
  Vec hpy =
      (pear::extract<Vec>(yp, g1) - pear::extract<Vec>(yp, g2)).array().pow(2);

  Vec h = (hpy + hpx).array().pow(.5);

  // COMPUTE BOUNDARY FUNC
  Vec bu(np);
  Vec bv(np);
  Vec kbu(np);
  Vec kbv(np);
  boundary_func(xp, bu, bv, kbu, kbv);

  // BB
  Vec bbu1 =
      (h.array() *
       (2 * pear::extract<Vec>(bu, g1) + pear::extract<Vec>(bu, g2)).array() /
       6);

  Vec bbu2 =
      (h.array() *
       (pear::extract<Vec>(bu, g1) + 2 * pear::extract<Vec>(bu, g2)).array() /
       6);

  Vec bbv1 =
      (h.array() *
       (2 * pear::extract<Vec>(bv, g1) + pear::extract<Vec>(bv, g2)).array() /
       6);

  Vec bbv2 =
      (h.array() *
       (pear::extract<Vec>(bu, g1) + 2 * pear::extract<Vec>(bu, g2)).array() /
       6);

  // KBB
  Vec kbbu1 =
      (h.array() *
       (3 * pear::extract<Vec>(kbu, g1) + pear::extract<Vec>(kbu, g2)).array() /
       12);

  Vec kbbu2 =
      (h.array() *
       (pear::extract<Vec>(kbu, g1) + 3 * pear::extract<Vec>(kbu, g2)).array() /
       12);

  Vec kbbum =
      (h.array() *
       (pear::extract<Vec>(kbu, g1) + pear::extract<Vec>(kbu, g2)).array() /
       12);

  Vec kbbv1 =
      (h.array() *
       (3 * pear::extract<Vec>(kbv, g1) + pear::extract<Vec>(kbv, g2)).array() /
       12);

  Vec kbbv2 =
      (h.array() *
       (pear::extract<Vec>(kbv, g1) + 3 * pear::extract<Vec>(kbv, g2)).array() /
       12);

  Vec kbbvm =
      (h.array() *
       (pear::extract<Vec>(kbv, g1) + pear::extract<Vec>(kbv, g2)).array() /
       12);

  // FILL BOUNDARY VECTOR
  for (int idxg = 0; idxg < ng; idxg++) {
    Bu(g1(idxg)) += bbu1(idxg);
    Bu(g2(idxg)) += bbu2(idxg);
    Bv(g2(idxg)) += bbv1(idxg);
    Bv(g2(idxg)) += bbv2(idxg);
  }

  // FILL BOUNDARY MATRIX

  for (int idxg = 0; idxg < ng; idxg++) {
    Kbu(g1(idxg), g1(idxg)) += kbbu1(idxg);
    Kbu(g2(idxg), g2(idxg)) += kbbu2(idxg);
    Kbu(g1(idxg), g2(idxg)) += kbbum(idxg);
    Kbu(g2(idxg), g1(idxg)) += kbbum(idxg);

    Kbv(g1(idxg), g1(idxg)) += kbbv1(idxg);
    Kbv(g2(idxg), g2(idxg)) += kbbv2(idxg);
    Kbv(g1(idxg), g2(idxg)) += kbbvm(idxg);
    Kbv(g2(idxg), g1(idxg)) += kbbvm(idxg);
  }
}

void boundary_func(Vec &xp, Vec &bu, Vec &bv, Vec &kbu, Vec &kbv) {
  bu = xp * pear::hu * pear::Cuamb;
  bv = xp * pear::hv * pear::Cvamb;

  kbu = -pear::hu * xp;
  kbv = -pear::hv * xp;
}

void compute_skin(MatI &boundary, VecI &g1, VecI &g2) {
  int nb = boundary.rows();
  int ng = 2 * nb - boundary.col(2).sum();

  int idxg = 0;
  for (int idxb = 0; idxb < nb; idxb++) {
    if (boundary(idxb, 2) == 1) {
      g1(idxg) = boundary(idxb, 0);
      g2(idxg) = boundary(idxb, 1);
      idxg++;
    }
  }
}
} // namespace pear


//
// Created by david on 2019-05-27.
//

#include <Eigen/QR>
#include <general/class_tic_toc.h>
#include <math/svd.h>

std::optional<long long> svd::solver::count = 0;

svd::solver::solver() {
    setLogLevel(2);
    t_wrk = std::make_unique<class_tic_toc>();
    t_adj = std::make_unique<class_tic_toc>();
    t_jac = std::make_unique<class_tic_toc>();
    t_svd = std::make_unique<class_tic_toc>();
    if(not count) count = 0;
}
void svd::solver::copy_settings(const svd::settings &svd_settings) {
    if(svd_settings.threshold) threshold = svd_settings.threshold.value();
    if(svd_settings.switchsize) switchsize = svd_settings.switchsize.value();
    if(svd_settings.loglevel) setLogLevel(svd_settings.loglevel.value());
    if(svd_settings.use_bdc) use_bdc = svd_settings.use_bdc.value();
    if(svd_settings.use_lapacke) use_lapacke = svd_settings.use_lapacke.value();
    if(svd_settings.profile and svd_settings.profile.value()) enableProfiling();
}

svd::solver::solver(const svd::settings &svd_settings) : solver() { copy_settings(svd_settings); }

svd::solver::solver(std::optional<svd::settings> svd_settings) : solver() {
    if(svd_settings) copy_settings(svd_settings.value());
}

void svd::solver::enableProfiling() {
    t_wrk->set_properties(true, 5, "work");
    t_adj->set_properties(true, 5, "adjoint");
    t_jac->set_properties(true, 5, "jacobi");
    t_svd->set_properties(true, 5, "bdcsvd");
}
void svd::solver::disableProfiling() {
    t_wrk->set_properties(false, 0, "");
    t_adj->set_properties(false, 0, "");
    t_jac->set_properties(false, 0, "");
    t_svd->set_properties(false, 0, "");
}

void svd::solver::setLogLevel(size_t logLevel) {
    if(not svd::log)
        tools::Logger::setLogger(svd::log, "svd", logLevel, true);
    else
        tools::Logger::setLogLevel(svd::log, logLevel);
    svd::log->set_pattern("[%Y-%m-%d %H:%M:%S.%e][%n]%^[%=8l]%$ %v");
}

/*! \brief Performs SVD on a matrix
 *  This function is defined in cpp to avoid long compilation times when having Eigen::BDCSVD included everywhere in headers.
 *  Performs rigorous checks to ensure stability of DMRG.
 *  In some cases Eigen::BCDSVD/JacobiSVD will fail with segfault. Here we use a patched version of Eigen that throws an error
 *  instead so we get a chance to catch it and use lapack svd instead.
 *   \param mat_ptr Pointer to the matrix. Supported are double * and std::complex<double> *
 *   \param rows Rows of the matrix
 *   \param cols Columns of the matrix
 *   \param rank_max Maximum number of singular values
 *   \return The U, S, and V matrices (with S as a vector) extracted from the Eigen::BCDSVD SVD object.
 */
template<typename Scalar>
std::tuple<svd::solver::MatrixType<Scalar>, svd::solver::VectorType<Scalar>, svd::solver::MatrixType<Scalar>, long>
    svd::solver::do_svd(const Scalar *mat_ptr, long rows, long cols, std::optional<long> rank_max) {
    if(use_lapacke) {
        try {
            return do_svd_lapacke(mat_ptr, rows, cols, rank_max);
        } catch(const std::exception &ex) {
            svd::log->warn(FMT_COMPILE("Lapacke failed to perform SVD: {} | Trying Eigen"), ex.what());
            return do_svd_eigen(mat_ptr, rows, cols, rank_max);
        }
    }else {
        try {
            return do_svd_eigen(mat_ptr, rows, cols, rank_max);
        } catch(const std::exception &ex) {
            svd::log->warn(FMT_COMPILE("Eigen failed to perform SVD: {} | Trying Lapacke"), ex.what());
            return do_svd_lapacke(mat_ptr, rows, cols, rank_max);
        }
    }
}

//! \relates svd::class_SVD
//! \brief force instantiation of do_svd for type 'double'
template std::tuple<svd::solver::MatrixType<double>, svd::solver::VectorType<double>, svd::solver::MatrixType<double>, long>
    svd::solver::do_svd(const double *, long, long, std::optional<long>);

using cplx = std::complex<double>;
//! \relates svd::class_SVD
//! \brief force instantiation of do_svd for type 'std::complex<double>'
template std::tuple<svd::solver::MatrixType<cplx>, svd::solver::VectorType<cplx>, svd::solver::MatrixType<cplx>, long>
    svd::solver::do_svd(const cplx *, long, long, std::optional<long>);

template<typename Scalar>
Eigen::Tensor<Scalar, 2> svd::solver::pseudo_inverse(const Eigen::Tensor<Scalar, 2> &tensor) {
    if(tensor.dimension(0) <= 0) { throw std::runtime_error("pseudo_inverse error: Dimension is zero: tensor.dimension(0)"); }
    if(tensor.dimension(1) <= 0) { throw std::runtime_error("pseudo_inverse error: Dimension is zero: tensor.dimension(1)"); }
    Eigen::Map<const MatrixType<Scalar>> mat(tensor.data(), tensor.dimension(0), tensor.dimension(1));
    return Textra::TensorCast(mat.completeOrthogonalDecomposition().pseudoInverse());
}

//! \relates svd::class_SVD
//! \brief force instantiation of pseudo_inverse for type 'double'
template Eigen::Tensor<double, 2> svd::solver::pseudo_inverse(const Eigen::Tensor<double, 2> &tensor);
//! \relates svd::class_SVD
//! \brief force instantiation of pseudo_inverse for type 'std::complex<double>'
template Eigen::Tensor<cplx, 2> svd::solver::pseudo_inverse(const Eigen::Tensor<cplx, 2> &tensor);


///////////////////////////////////////////////////////////////////////////////
// weighted_count.hpp                                                        //
//                                                                           //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_ACCUMULATORS_STATISTICS_DETAIL_WEIGHTED_COUNT_HPP_ER_2010
#define BOOST_ACCUMULATORS_STATISTICS_DETAIL_WEIGHTED_COUNT_HPP_ER_2010

#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/weight.hpp>
#include <boost/accumulators/framework/accumulators/external_accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>

// This file is a modification of Boost.Accumulator's count.hpp by Eric Niebler.

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_count_impl
    template<typename Weight>
    struct weighted_count_impl
      : accumulator_base
    {
        typedef Weight weighted_sample;

        // for boost::result_of
        typedef weighted_sample result_type;

        template<typename Args>
        weighted_count_impl(Args const &args)
          : weighted_count_(
                numeric::zero<Weight>::value
            )
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            // what about overflow?
            this->weighted_count_ += args[ weight ];
        }

        result_type result(dont_care) const
        {
            return this->weighted_count_;
        }

    private:

        weighted_sample weighted_count_;
    };

} // namespace impl

namespace tag
{
    struct weighted_count
      : depends_on<>
    {
        typedef accumulators::impl::weighted_count_impl<mpl::_2> impl;
    };

/*
    template<typename VariateType, typename VariateTag>
    struct weighted_count_of_variates
      : depends_on<>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::weighted_count_impl<VariateType, mpl::_2, VariateTag> impl;
    };

    struct abstract_weighted_count_of_variates
      : depends_on<>
    {
    };
*/
}

namespace extract
{
    extractor<tag::weighted_count> const weighted_count = {};
//    extractor<tag::abstract_weighted_count_of_variates> const weighted_count_of_variates = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_count)
//    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_count_of_variates)
}

using extract::weighted_count;
//using extract::weighted_count_of_variates;

/*
template<typename VariateType, typename VariateTag>
struct feature_of<tag::weighted_count_of_variates<VariateType, VariateTag> >
  : feature_of<tag::abstract_weighted_count_of_variates>
{
};
*/

}} // namespace boost::accumulators

#endif


#include "SimulationCore_pcp.h"

#include <Eigen/SparseLU>

#define Keep_Newmark_Coefficients
#include "Step_S2D_CHM_s_FEM_uUp.h"

namespace
{
	typedef Model_S2D_CHM_s_FEM_uUp::ShapeFuncValue ShapeFuncValue;
	typedef Model_S2D_CHM_s_FEM_uUp::GaussPoint GaussPoint_fem;
	typedef Model_S2D_CHM_s_FEM_uUp::Element Element_fem;
	typedef Model_S2D_CHM_s_FEM_uUp::Node Node_fem;
	typedef Model_S2D_CHM_s_FEM_uUp::DOF DOF;

	void print_sparse_mat(Eigen::SparseMatrix<double> &mat,
		std::fstream &out_file, const char *mat_name = nullptr)
	{
		if (mat_name)
			out_file << mat_name << "\n";
		size_t row_num = mat.rows();
		size_t col_num = mat.cols();
		double value;
		for (size_t i = 0; i < row_num; ++i)
		{
			for (size_t j = 0; j < col_num; ++j)
			{
				value = mat.coeff(i, j);
				out_file << value << ", ";
			}
			out_file << "\n";
		}
	}
};

Step_S2D_CHM_s_FEM_uUp::Step_S2D_CHM_s_FEM_uUp() :
	Step(&solve_substep_S2D_CHM_s_FEM_uUp),
	model(nullptr), kmat_col(nullptr) {}

Step_S2D_CHM_s_FEM_uUp::~Step_S2D_CHM_s_FEM_uUp() {}

int Step_S2D_CHM_s_FEM_uUp::init_calculation(void)
{
	if (is_first_step) {}

	kmat_col = new double[model->dof_num];

	// for debug
	out_file.open("debug_mat_out_CHM_s_FEM_uUp.csv", std::ios::binary | std::ios::out);

	return 0;
}

int Step_S2D_CHM_s_FEM_uUp::finalize_calculation(void)
{
	if (kmat_col)
	{
		delete[] kmat_col;
		kmat_col = nullptr;
	}

	// for debug
	out_file.close();

	return 0;
}

int solve_substep_S2D_CHM_s_FEM_uUp(void *_self)
{
	// for debug
	//static std::fstream out_f2("out_f2.txt", std::ios::binary | std::ios::out);

	Step_S2D_CHM_s_FEM_uUp &self = *(Step_S2D_CHM_s_FEM_uUp *)(_self);
	Model_S2D_CHM_s_FEM_uUp &model = *self.model;
	double dt = self.dtime;
	double dt2 = dt * dt;

	// list of non-zeros coefficients
	MatrixCoefficientSet<> &g_kmat_coefs = self.g_kmat_coefs;
	g_kmat_coefs.init(model.dof_num);
	Eigen::VectorXd g_fvec(model.dof_num);
	g_fvec.setZero();

	size_t l2g_dof_id_map[20];
	double e_kmat[20][20], e_fvec[20];
	for (size_t e_id = 0; e_id < model.elem_num; ++e_id)
	{
		Element_fem &e = model.elems[e_id];
		// form elemental stiffness matrix and force vector
		self.form_elem_stiffness_mat_and_force_vec(e, e_kmat, e_fvec);

		// map from local dof id to global dof id
		// solid phase ux
		l2g_dof_id_map[0] = model.n_id_to_dof_id(e.n1_id, DOF::usx);
		l2g_dof_id_map[1] = model.n_id_to_dof_id(e.n2_id, DOF::usx);
		l2g_dof_id_map[2] = model.n_id_to_dof_id(e.n3_id, DOF::usx);
		l2g_dof_id_map[3] = model.n_id_to_dof_id(e.n4_id, DOF::usx);
		// solid phase uy
		l2g_dof_id_map[4] = model.n_id_to_dof_id(e.n1_id, DOF::usy);
		l2g_dof_id_map[5] = model.n_id_to_dof_id(e.n2_id, DOF::usy);
		l2g_dof_id_map[6] = model.n_id_to_dof_id(e.n3_id, DOF::usy);
		l2g_dof_id_map[7] = model.n_id_to_dof_id(e.n4_id, DOF::usy);
		// fluid phase ux
		l2g_dof_id_map[8]  = model.n_id_to_dof_id(e.n1_id, DOF::ufx);
		l2g_dof_id_map[9]  = model.n_id_to_dof_id(e.n2_id, DOF::ufx);
		l2g_dof_id_map[10] = model.n_id_to_dof_id(e.n3_id, DOF::ufx);
		l2g_dof_id_map[11] = model.n_id_to_dof_id(e.n4_id, DOF::ufx);
		// fluid phase uy
		l2g_dof_id_map[12] = model.n_id_to_dof_id(e.n1_id, DOF::ufy);
		l2g_dof_id_map[13] = model.n_id_to_dof_id(e.n2_id, DOF::ufy);
		l2g_dof_id_map[14] = model.n_id_to_dof_id(e.n3_id, DOF::ufy);
		l2g_dof_id_map[15] = model.n_id_to_dof_id(e.n4_id, DOF::ufy);
		// p
		l2g_dof_id_map[16] = model.n_id_to_dof_id(e.n1_id, DOF::p);
		l2g_dof_id_map[17] = model.n_id_to_dof_id(e.n2_id, DOF::p);
		l2g_dof_id_map[18] = model.n_id_to_dof_id(e.n3_id, DOF::p);
		l2g_dof_id_map[19] = model.n_id_to_dof_id(e.n4_id, DOF::p);
		//print_vec(l2g_dof_id_map);
		// add to global matrix and vector
		size_t g_dof_id1, g_dof_id2;
		for (size_t l_id1 = 0; l_id1 < 20; ++l_id1)
		{
			// add to global matrix
			g_dof_id1 = l2g_dof_id_map[l_id1];
			for (size_t l_id2 = 0; l_id2 < 20; ++l_id2)
			{
				g_dof_id2 = l2g_dof_id_map[l_id2];
				g_kmat_coefs.add_coefficient(g_dof_id1, g_dof_id2, e_kmat[l_id1][l_id2]);
			}
			// add to global force vector
			g_fvec[g_dof_id1] += e_fvec[l_id1];
		}
	}

	// apply external force body force and traction
	size_t dof_id;
	// traction
	double tfs[4];
	for (size_t t_id = 0; t_id < model.tx_num; ++t_id)
	{
		TractionBC_2DFEM &tx = model.txs[t_id];
		model.cal_traction_bc(tx, tfs);
		Element_fem &e = model.elems[tx.elem_id];
		dof_id = model.n_id_to_dof_id(e.n1_id, DOF::usx);
		g_fvec[dof_id] += tfs[0];
		dof_id = model.n_id_to_dof_id(e.n2_id, DOF::usx);
		g_fvec[dof_id] += tfs[1];
		dof_id = model.n_id_to_dof_id(e.n3_id, DOF::usx);
		g_fvec[dof_id] += tfs[2];
		dof_id = model.n_id_to_dof_id(e.n4_id, DOF::usx);
		g_fvec[dof_id] += tfs[3];
	}
	for (size_t t_id = 0; t_id < model.ty_num; ++t_id)
	{
		TractionBC_2DFEM &ty = model.tys[t_id];
		model.cal_traction_bc(ty, tfs);
		Element_fem &e = model.elems[ty.elem_id];
		dof_id = model.n_id_to_dof_id(e.n1_id, DOF::usy);
		g_fvec[dof_id] += tfs[0];
		dof_id = model.n_id_to_dof_id(e.n2_id, DOF::usy);
		g_fvec[dof_id] += tfs[1];
		dof_id = model.n_id_to_dof_id(e.n3_id, DOF::usy);
		g_fvec[dof_id] += tfs[2];
		dof_id = model.n_id_to_dof_id(e.n4_id, DOF::usy);
		g_fvec[dof_id] += tfs[3];
	}
	// body force ...  to be finished

	// apply displacement boundary condition
	double dig_term;
	for (size_t bc_id = 0; bc_id < model.usx_num; ++bc_id)
	{
		DisplacementBC &dbc = model.usxs[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::usx);
		dig_term = g_kmat_coefs.del_col_and_row(dof_id, self.kmat_col);
		for (size_t row_id = 0; row_id < model.dof_num; ++row_id)
			g_fvec[row_id] -= self.kmat_col[row_id] * dbc.u;
		g_fvec[dof_id] = dig_term * dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.usy_num; ++bc_id)
	{
		DisplacementBC &dbc = model.usys[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::usy);
		dig_term = g_kmat_coefs.del_col_and_row(dof_id, self.kmat_col);
		for (size_t row_id = 0; row_id < model.dof_num; ++row_id)
			g_fvec[row_id] -= self.kmat_col[row_id] * dbc.u;
		g_fvec[dof_id] = dig_term * dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.ufx_num; ++bc_id)
	{
		DisplacementBC &dbc = model.ufxs[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::ufx);
		dig_term = g_kmat_coefs.del_col_and_row(dof_id, self.kmat_col);
		for (size_t row_id = 0; row_id < model.dof_num; ++row_id)
			g_fvec[row_id] -= self.kmat_col[row_id] * dbc.u;
		g_fvec[dof_id] = dig_term * dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.ufy_num; ++bc_id)
	{
		DisplacementBC &dbc = model.ufys[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::ufy);
		dig_term = g_kmat_coefs.del_col_and_row(dof_id, self.kmat_col);
		for (size_t row_id = 0; row_id < model.dof_num; ++row_id)
			g_fvec[row_id] -= self.kmat_col[row_id] * dbc.u;
		g_fvec[dof_id] = dig_term * dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.pbc_num; ++bc_id)
	{
		PressureBC &pbc = model.pbcs[bc_id];
		dof_id = model.n_id_to_dof_id(pbc.node_id, DOF::p);
		dig_term = g_kmat_coefs.del_col_and_row(dof_id, self.kmat_col);
		for (size_t row_id = 0; row_id < model.dof_num; ++row_id)
			g_fvec[row_id] -= self.kmat_col[row_id] * pbc.p;
		g_fvec[dof_id] = dig_term * pbc.p;
	}

	// solve
	//g_kmat_coefs.print_with_iter();
	Eigen::SparseMatrix<double> g_kmat(model.dof_num, model.dof_num);
	g_kmat.setFromTriplets(g_kmat_coefs.begin(), g_kmat_coefs.end());
	//print_sparse_mat(g_kmat, self.out_file,nullptr);
	Eigen::SparseLU<Eigen::SparseMatrix<double> > solver(g_kmat);
	Eigen::VectorXd g_du_vec = solver.solve(g_fvec);
	//std::cout << g_fvec << "\n";
	//std::cout << g_du_vec << "\n";

	// reapply disp bc
	for (size_t bc_id = 0; bc_id < model.usx_num; ++bc_id)
	{
		DisplacementBC &dbc = model.usxs[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::usx);
		g_du_vec[dof_id] = dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.usy_num; ++bc_id)
	{
		DisplacementBC &dbc = model.usys[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::usy);
		g_du_vec[dof_id] = dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.ufx_num; ++bc_id)
	{
		DisplacementBC &dbc = model.ufxs[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::ufx);
		g_du_vec[dof_id] = dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.ufy_num; ++bc_id)
	{
		DisplacementBC &dbc = model.ufys[bc_id];
		dof_id = model.n_id_to_dof_id(dbc.node_id, DOF::ufy);
		g_du_vec[dof_id] = dbc.u;
	}
	for (size_t bc_id = 0; bc_id < model.pbc_num; ++bc_id)
	{
		PressureBC &pbc = model.pbcs[bc_id];
		dof_id = model.n_id_to_dof_id(pbc.node_id, DOF::p);
		g_du_vec[dof_id] = pbc.p;
	}

	// update nodal variables
	double da, dv, du;
	for (size_t n_id = 0; n_id < model.node_num; ++n_id)
	{
		Node_fem &n = model.nodes[n_id];
		// sx
		dof_id = model.n_id_to_dof_id(n_id, DOF::usx);
		du = g_du_vec[dof_id];
		n.ux_s += du;
		dv = gamma / (beta * dt) * du - gamma / beta * n.vx_s + dt * (1.0 - gamma / (2.0 * beta)) * n.ax_s;
		n.vx_s += dv;
		da = du / (gamma * dt2) - n.vx_s / (gamma * dt) - n.ax_s / (2.0 * gamma);
		n.ax_s += da;
		// sy
		dof_id = model.n_id_to_dof_id(n_id, DOF::usy);
		du = g_du_vec[dof_id];
		n.uy_s += du;
		dv = gamma / (beta * dt) * du - gamma / beta * n.vy_s + dt * (1.0 - gamma / (2.0 * beta)) * n.ay_s;
		n.vy_s += dv;
		da = du / (gamma * dt2) -  n.vy_s / (gamma * dt) - n.ay_s / (2.0 * gamma);
		n.ay_s += da;
		// fx
		dof_id = model.n_id_to_dof_id(n_id, DOF::ufx);
		du = g_du_vec[dof_id];
		n.ux_f += du;
		dv = gamma / (beta * dt) * du - gamma / beta * n.vx_f + dt * (1.0 - gamma / (2.0 * beta)) * n.ax_f;
		n.vx_f += dv;
		da = 1.0 / (gamma * dt2) * du - 1.0 / (gamma * dt) * n.vx_f - 1.0 / (2.0 * gamma) * n.ax_f;
		n.ax_f += da;
		// fy
		dof_id = model.n_id_to_dof_id(n_id, DOF::ufy);
		du = g_du_vec[dof_id];
		n.uy_f += du;
		dv = gamma / (beta * dt) * du - gamma / beta * n.vy_f + dt * (1.0 - gamma / (2.0 * beta)) * n.ay_s;
		n.vy_f += dv;
		da = 1.0 / (gamma * dt2) * du - 1.0 / (gamma * dt) * n.vy_f - 1.0 / (2.0 * gamma) * n.ay_s;
		n.ay_f += da;
		// p
		dof_id = model.n_id_to_dof_id(n_id, DOF::p);
		n.p += g_du_vec[dof_id];
	}

	// Update element variables
	ShapeFuncValue &sf1 = model.gp1_sf;
	ShapeFuncValue &sf2 = model.gp2_sf;
	ShapeFuncValue &sf3 = model.gp3_sf;
	ShapeFuncValue &sf4 = model.gp4_sf;
	double dux_s1, duy_s1, dux_f1, duy_f1, dp1;
	double dux_s2, duy_s2, dux_f2, duy_f2, dp2;
	double dux_s3, duy_s3, dux_f3, duy_f3, dp3;
	double dux_s4, duy_s4, dux_f4, duy_f4, dp4;
	for (size_t e_id = 0; e_id < model.elem_num; ++e_id)
	{
		Element_fem &e = model.elems[e_id];
		dux_s1 = g_du_vec[model.n_id_to_dof_id(e.n1_id, DOF::usx)];
		duy_s1 = g_du_vec[model.n_id_to_dof_id(e.n1_id, DOF::usy)];
		dux_f1 = g_du_vec[model.n_id_to_dof_id(e.n1_id, DOF::ufx)];
		duy_f1 = g_du_vec[model.n_id_to_dof_id(e.n1_id, DOF::ufy)];
		dp1    = g_du_vec[model.n_id_to_dof_id(e.n1_id, DOF::p)];
		dux_s2 = g_du_vec[model.n_id_to_dof_id(e.n2_id, DOF::usx)];
		duy_s2 = g_du_vec[model.n_id_to_dof_id(e.n2_id, DOF::usy)];
		dux_f2 = g_du_vec[model.n_id_to_dof_id(e.n2_id, DOF::ufx)];
		duy_f2 = g_du_vec[model.n_id_to_dof_id(e.n2_id, DOF::ufy)];
		dp2    = g_du_vec[model.n_id_to_dof_id(e.n2_id, DOF::p)];
		dux_s3 = g_du_vec[model.n_id_to_dof_id(e.n3_id, DOF::usx)];
		duy_s3 = g_du_vec[model.n_id_to_dof_id(e.n3_id, DOF::usy)];
		dux_f3 = g_du_vec[model.n_id_to_dof_id(e.n3_id, DOF::ufx)];
		duy_f3 = g_du_vec[model.n_id_to_dof_id(e.n3_id, DOF::ufy)];
		dp3    = g_du_vec[model.n_id_to_dof_id(e.n3_id, DOF::p)];
		dux_s4 = g_du_vec[model.n_id_to_dof_id(e.n4_id, DOF::usx)];
		duy_s4 = g_du_vec[model.n_id_to_dof_id(e.n4_id, DOF::usy)];
		dux_f4 = g_du_vec[model.n_id_to_dof_id(e.n4_id, DOF::ufx)];
		duy_f4 = g_du_vec[model.n_id_to_dof_id(e.n4_id, DOF::ufy)];
		dp4    = g_du_vec[model.n_id_to_dof_id(e.n4_id, DOF::p)];

		self.update_gauss_point(e.gp1, sf1, dt, dt2,
			dux_s1, dux_s2, dux_s3, dux_s4,
			duy_s1, duy_s2, duy_s3, duy_s4,
			dux_f1, dux_f2, dux_f3, dux_f4,
			duy_f1, duy_f2, duy_f3, duy_f4,
			dp1, dp2, dp3, dp4);

		self.update_gauss_point(e.gp2, sf2, dt, dt2,
			dux_s1, dux_s2, dux_s3, dux_s4,
			duy_s1, duy_s2, duy_s3, duy_s4,
			dux_f1, dux_f2, dux_f3, dux_f4,
			duy_f1, duy_f2, duy_f3, duy_f4,
			dp1, dp2, dp3, dp4);

		self.update_gauss_point(e.gp3, sf3, dt, dt2,
			dux_s1, dux_s2, dux_s3, dux_s4,
			duy_s1, duy_s2, duy_s3, duy_s4,
			dux_f1, dux_f2, dux_f3, dux_f4,
			duy_f1, duy_f2, duy_f3, duy_f4,
			dp1, dp2, dp3, dp4);

		self.update_gauss_point(e.gp4, sf4, dt, dt2,
			dux_s1, dux_s2, dux_s3, dux_s4,
			duy_s1, duy_s2, duy_s3, duy_s4,
			dux_f1, dux_f2, dux_f3, dux_f4,
			duy_f1, duy_f2, duy_f3, duy_f4,
			dp1, dp2, dp3, dp4);
	}

	return 0;
}

void Step_S2D_CHM_s_FEM_uUp::update_gauss_point(
	Model_S2D_CHM_s_FEM_uUp::GaussPoint &gp,
	Model_S2D_CHM_s_FEM_uUp::ShapeFuncValue &sf,
	double dt, double dt2,
	double dux_s1, double dux_s2, double dux_s3, double dux_s4,
	double duy_s1, double duy_s2, double duy_s3, double duy_s4,
	double dux_f1, double dux_f2, double dux_f3, double dux_f4,
	double duy_f1, double duy_f2, double duy_f3, double duy_f4,
	double dp1, double dp2, double dp3, double dp4)
{
	double dux_s, duy_s, dux_f, duy_f, dp;
	double dvx_s, dvy_s, dvx_f, dvy_f;
	double dax_s, day_s, dax_f, day_f;
	double de11, de22, de12, ds11, ds22, ds12;
	double de_vol_s, de_vol_f;

	// displacement
	dux_s = dux_s1 * sf.N1 + dux_s2 * sf.N2 + dux_s3 * sf.N3 + dux_s4 * sf.N4;
	duy_s = duy_s1 * sf.N1 + duy_s2 * sf.N2 + duy_s3 * sf.N3 + duy_s4 * sf.N4;
	dux_f = dux_f1 * sf.N1 + dux_f2 * sf.N2 + dux_f3 * sf.N3 + dux_f4 * sf.N4;
	duy_f = duy_f1 * sf.N1 + duy_f2 * sf.N2 + duy_f3 * sf.N3 + duy_f4 * sf.N4;
	// velocity
	dvx_s = gamma / (beta * dt) * dux_s - gamma / beta * gp.vx_s
		  + dt * (1.0 - gamma / (2.0 * beta)) * gp.ax_s;
	dvy_s = gamma / (beta * dt) * duy_s - gamma / beta * gp.vy_s
		  + dt * (1.0 - gamma / (2.0 * beta)) * gp.ay_s;
	dvx_f = gamma / (beta * dt) * dux_f - gamma / beta * gp.vx_f
		  + dt * (1.0 - gamma / (2.0 * beta)) * gp.ax_f;
	dvy_f = gamma / (beta * dt) * duy_f - gamma / beta * gp.vy_f
		  + dt * (1.0 - gamma / (2.0 * beta)) * gp.ay_f;
	// acceleration
	dax_s = dux_s / (beta * dt2) - gp.vx_s / (beta * dt) - gp.ax_s / (2.0 * beta);
	day_s = duy_s / (beta * dt2) - gp.vy_s / (beta * dt) - gp.ay_s / (2.0 * beta);
	dax_f = dux_f / (beta * dt2) - gp.vx_f / (beta * dt) - gp.ax_f / (2.0 * beta);
	day_f = duy_f / (beta * dt2) - gp.vy_f / (beta * dt) - gp.ay_f / (2.0 * beta);
	gp.ux_s += dux_s;
	gp.uy_s += duy_s;
	gp.ux_f += dux_f;
	gp.uy_f += duy_f;
	gp.vx_s += dvx_s;
	gp.vy_s += dvy_s;
	gp.vx_f += dvx_f;
	gp.vy_f += dvy_f;
	gp.ax_s += dax_s;
	gp.ay_s += day_s;
	gp.ax_f += dax_f;
	gp.ay_f += day_f;

	// pore pressure
	gp.p += dp1 * sf.N1 + dp2 * sf.N2 + dp3 * sf.N3 + dp4 * sf.N4;

	// strain increment
	de11 = dux_s1 * sf.dN1_dx + dux_s2 * sf.dN2_dx
		 + dux_s3 * sf.dN3_dx + dux_s4 * sf.dN4_dx;
	de22 = duy_s1 * sf.dN1_dy + duy_s2 * sf.dN2_dy
		 + duy_s3 * sf.dN3_dy + duy_s4 * sf.dN4_dy;
	de12 = (dux_s1 * sf.dN1_dy + dux_s2 * sf.dN2_dy
		  + dux_s3 * sf.dN3_dy + dux_s4 * sf.dN4_dy
		  + duy_s1 * sf.dN1_dx + duy_s2 * sf.dN2_dx
		  + duy_s3 * sf.dN3_dx + duy_s4 * sf.dN4_dx) * 0.5;
	gp.e11 += de11;
	gp.e22 += de22;
	gp.e12 += de12;

	// update stress
	double E_tmp = gp.E / (1.0 + gp.niu) / (1.0 - 2.0 * gp.niu);
	ds11 = E_tmp * ((1.0 - gp.niu) * de11 + gp.niu * de22);
	ds22 = E_tmp * (gp.niu * de11 + (1.0 - gp.niu) * de22);
	ds12 = 2.0 * gp.E / (2.0 * (1.0 + gp.niu)) * de12;
	gp.s11 += ds11;
	gp.s22 += ds22;
	gp.s12 += ds12;

	// volumetric strain of solid phase
	de_vol_s = de11 + de22;
	// "volumetric strain" of fluid phase
	de_vol_f = -(1.0 - gp.n) / gp.n * de_vol_s
		- (dux_f1 * sf.dN1_dx + dux_f2 * sf.dN2_dx + dux_f3 * sf.dN3_dx + dux_f4 * sf.dN4_dx)
		- (duy_f1 * sf.dN1_dy + duy_f2 * sf.dN2_dy + duy_f3 * sf.dN3_dy + duy_f4 * sf.dN4_dy);

	// porosity
	//gp.n = (de_vol_s + gp.n) / (1.0 + de_vol_s);
	// fluid density
	//gp.density_f += gp.density_f * de_vol_f;
}


#include <iostream>
#include <Eigen/Sparse>

int main(){
	Eigen::Matrix<double, 4,4> A;
	Eigen::VectorXd x;
	x.fill(0);
	int n = x.size();

	A=Eigen::MatrixXd::Random(4,4);
	A(0,1)=1;
	std::cout << A << std::endl << n << std::endl;

	return 0;

}


#define BOOST_TEST_MODULE blas

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/vector/all.h++>

#include <mla/operations/level1/axpy.h++>

typedef boost::mpl::list<
	mla::vector::Dense<float>,
	mla::vector::Dense<double>,
	mla::vector::SparseCS<float>,
	mla::vector::SparseCS<double>
> vector_type_list;

BOOST_AUTO_TEST_SUITE(test_boost_level1)

BOOST_AUTO_TEST_CASE_TEMPLATE( blas_level1_test_axpy_zero, VectorType, vector_type_list )
{
	using namespace mla;

	VectorType x(3);
	vector::Dense<typename VectorType::scalar_type > y(3);

	y.setZero();

	typename VectorType::scalar_type value = 1.0f;
	axpy( value, x, y);

	for( unsigned int i = 0; i < y.size(); i++)
	{
		BOOST_CHECK_CLOSE(y.getValue(i), 0.0f, 0.001f);
	}
}

BOOST_AUTO_TEST_CASE_TEMPLATE( blas_level1_test_axpy_one, VectorType, vector_type_list )
{
	using namespace mla;

	VectorType x(3);
	x.setValue(0, 1.0f);
	x.setValue(1, 1.0f);
	x.setValue(2, 1.0f);

	vector::Dense<typename VectorType::scalar_type > y(3);
	y.setValue(0, 1.0f);
	y.setValue(1, 1.0f);
	y.setValue(2, 1.0f);

	typename VectorType::scalar_type value = 1.0f;
	axpy( value, x, y);

	for( unsigned int i = 0; i < y.size(); i++)
	{
		BOOST_CHECK_CLOSE(y.getValue(i), 2.0f, 0.001f);
	}
}

BOOST_AUTO_TEST_SUITE_END()


#define PCL_NO_PRECOMPILE
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <thread>         // std::thread
#include <mutex>          // std::mutex
#include <chrono>

#include <kitti_data_loader.h>
#include <kitti_time_stamp_data.h>

#include "boost/program_options.hpp"

#include <Eigen/Core>

#include "ground_estimation.h"
#include <Eigen/Dense>
#include <string>

#include <sstream>

#include <pcl_threaded_viewer.h>

using Ground = Eigen::Vector3f;
using GroundPlane = std::vector<Ground>;
using MasurementMat = Eigen::Vector3f;
using H = MasurementMat;
using cij = bool;
using C = std::vector<cij>;
using GP = std::vector<float>;
using F = Eigen::Matrix3f;

#define GET_INDEX(x,y,n) ((y)*(n)+(x))

float calculateHeightProbability(const PointXYZR p, const PointXYZGround Xi, const float sig_up_2, const float sig_down_2)
{
    Eigen::Vector3f hij(1.0f,Xi.x-p.x,Xi.y-p.y);
    Eigen::Vector3f xi(Xi.z, Xi.sx, Xi.sy);
    float dz = p.z - hij.transpose()*xi;
    float prob = 0.0;
    if(dz > 0)
    {
        prob = exp(-dz*dz/(2.0*sig_up_2));
    }
    else
    {
        prob = exp(-dz*dz/(2.0*sig_down_2));
    }
    return prob;
}

void calculateCloudProbability(PointXZYR_Cloud &cloud,const PointXZYGround_Cloud &Xi, float s1, float s2,float dl, float limit, int n)
{
    for (auto &point : cloud.points)
    {
        int nx = round((point.x+limit)/ dl);
        int ny = round((point.y+limit)/ dl);
        if(nx < 0 || nx >= n) continue;
        if(ny < 0 || ny >= n) continue;
        auto xi = Xi.points[GET_INDEX(nx,ny,n)];
        float pc = calculateHeightProbability(point,xi,s1,s2);
        //use r as the probability
        point.r = pc;
    }
}

void calculateNewXandP(PointXZYGround_Cloud &Xi_K_1, const PointXZYGround_Cloud &Xi_K, PointXZYGround_Cloud &Xi_K_m_1, const PointXZYR_Cloud &cloud, const PointXZYGround_Cloud &X, float dl, float limit, float alpha, float beta, float gamma, int n)
{
    for ( auto &xi : Xi_K_1.points)
    {
        xi.sx = 0;
        xi.sy = 0;
        xi.z = 0;
    }

    for (auto &point : cloud.points)
    {
        int nx = round((point.x + limit) / dl);
        int ny = round((point.y + limit) / dl);
        if (nx < 0 || nx >= n) continue;
        if (ny < 0 || ny >= n) continue;
        size_t index = GET_INDEX(nx, ny, n);
        auto xi = Xi_K.points[index];
        auto &xi_k_1 = Xi_K_1.points[index];
        Eigen::Vector3f hij(1.0f, xi.x - point.x, xi.y - point.y);
        Eigen::Vector3f result = alpha*point.r*point.z*hij;
        xi_k_1.z += result(0);
        xi_k_1.sx += result(1);
        xi_k_1.sy += result(2);
    }

    for (auto &xi_k_1 : Xi_K_1.points)
    {
        int nx = round((xi_k_1.x + limit) / dl);
        int ny = round((xi_k_1.y + limit) / dl);
        if (nx < 0 || nx >= n) continue;
        if (ny < 0 || ny >= n) continue;

        if (nx - 1 > 0)
        {
            F f;
            f << 1.0f, -dl, 0,
                    0.0f, 1.0f, 0.0f,
                    0.0f, 0.0f, 1.0f;
            auto xi_1 = Xi_K.points[GET_INDEX(nx-1,ny,n)];
            Eigen::Vector3f xi_k(xi_1.z, xi_1.sx, xi_1.sy);
            Eigen::Vector3f result = beta*f.transpose()*xi_k;
            xi_k_1.z += result(0);
            xi_k_1.sx += result(1);
            xi_k_1.sy += result(2);
        }
        if (nx + 1 < n)
        {
            F f;
            f << 1.0f, dl, 0,
                    0.0f, 1.0f, 0.0f,
                    0.0f, 0.0f, 1.0f;
            auto xi_1 = Xi_K.points[GET_INDEX(nx + 1, ny, n)];
            Eigen::Vector3f xi_k(xi_1.z, xi_1.sx, xi_1.sy);
            Eigen::Vector3f result = beta*f.transpose()*xi_k;
            xi_k_1.z += result(0);
            xi_k_1.sx += result(1);
            xi_k_1.sy += result(2);
        }

        if (ny - 1 > 0)
        {
            F f;
            f << 1.0f, 0, -dl,
                    0.0f, 1.0f, 0.0f,
                    0.0f, 0.0f, 1.0f;
            auto xi_1 = Xi_K.points[GET_INDEX(nx , ny - 1, n)];
            Eigen::Vector3f xi_k(xi_1.z, xi_1.sx, xi_1.sy);
            Eigen::Vector3f result = beta*f.transpose()*xi_k;
            xi_k_1.z += result(0);
            xi_k_1.sx += result(1);
            xi_k_1.sy += result(2);
        }
        if (ny + 1 < n)
        {
            F f;
            f << 1.0f, 0, dl,
                    0.0f, 1.0f, 0.0f,
                    0.0f, 0.0f, 1.0f;
            auto xi_1 = Xi_K.points[GET_INDEX(nx , ny + 1, n)];
            Eigen::Vector3f xi_k(xi_1.z, xi_1.sx, xi_1.sy);
            Eigen::Vector3f result = beta*f.transpose()*xi_k;
            xi_k_1.z += result(0);
            xi_k_1.sx += result(1);
            xi_k_1.sy += result(2);
        }

        auto xi_k_m1 = Xi_K_m_1.points[GET_INDEX(nx, ny, n)];
        xi_k_1.z += gamma*xi_k_m1.z;
        xi_k_1.sx += gamma*xi_k_m1.sx;
        xi_k_1.sy += gamma*xi_k_m1.sy;
    }
}

std::vector<std::string> getListFile(std::string path)
{
    std::vector<std::string> files;

    if (!path.empty())
    {
        namespace fs = boost::filesystem;

        fs::path apk_path(path);
        fs::recursive_directory_iterator end;

        for (fs::recursive_directory_iterator i(apk_path); i != end; ++i)
        {
            const fs::path cp = (*i);
            files.push_back(cp.string());
        }
    }

    std::sort(files.begin(), files.end(), [](const std::string &lhs, const std::string &rhs) { return lhs < rhs; });
    return files;
}

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

    namespace po = boost::program_options;

    // Declare the supported options.
    po::options_description desc("Allowed options");
    desc.add_options()
            ("help", "produce help message")
            ("kitti_lidar_path", po::value<std::string>(), "set lidar data path")
            ("kitti_oxts_path", po::value<std::string>(), "set oxs data path")
            ("kitti_timestamp_file", po::value<std::string>(), "set time stamp fiel path")
            ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << desc << "\n";
        return 0;
    }

    if (vm.count("kitti_lidar_path")) {
        std::cout << "Kitti LiDAR Data Path was set to "
                  << vm["kitti_lidar_path"].as<std::string>() << ".\n";
    } else {
        std::cout << "Kitti Data Path was not set.\n";
        return 1;
    }

    if (vm.count("kitti_oxts_path")) {
        std::cout << "Kitti OXS Data Path was set to "
                  << vm["kitti_oxts_path"].as<std::string>() << ".\n";
    } else {
        std::cout << "Kitti OXS Data Path was not set.\n";
        return 1;
    }

    if (vm.count("kitti_timestamp_file")) {
        std::cout << "Kitti TimeStamp fiel was set to "
                  << vm["kitti_timestamp_file"].as<std::string>() << ".\n";
    } else {
        std::cout << "Kitti TimeStamp file was not set.\n";
        return 1;
    }

    int nx = 100;
    int ny = 100;
    int N = nx*ny;
    float dl = .50;
    float x_limit = nx*dl/2;
    float y_limit = ny*dl/2;

    F f;
    f << 1.0f, -dl, -dl,
            0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 1.0f;

    float sig_up_2 = .05*.05;
    float sig_down_2 = .5*.5;

    PointXZYGround_Cloud ground_cloud;
    ground_cloud.width    = nx;
    ground_cloud.height   = ny;
    ground_cloud.is_dense = true;
    ground_cloud.points.resize (nx * ny);
    PointXZYGround_Cloud X_cloud;
    X_cloud.width    = nx;
    X_cloud.height   = ny;
    X_cloud.is_dense = true;
    X_cloud.points.resize (nx * ny);
    PointXZYGround_Cloud P_cloud;
    P_cloud.width    = nx;
    P_cloud.height   = ny;
    P_cloud.is_dense = true;
    P_cloud.points.resize (nx * ny);
    pcl::PointCloud<PointXYZR> prob_cloud;
    for(int y = 0; y < ny;y++)
    {
        float ly = dl*y-y_limit;
        for(int x = 0; x < nx;x++)
        {
            float lx = dl*x-x_limit;
            PointXYZGround p{lx,ly,0.0,0.0,0.0};
            ground_cloud.points[GET_INDEX(x,y,nx)] = p;
            X_cloud.points[GET_INDEX(x,y,nx)] = p;
            P_cloud.points[GET_INDEX(x,y,nx)] = p;
        }
    }
    PLCVisualizerWorker worker;
    worker.run();
    auto files = getListFile(vm["kitti_lidar_path"].as<std::string>());
    auto gps_files = getListFile(vm["kitti_oxts_path"].as<std::string>());

    int counter = 0;
    KITTITimeStampData time_stamp_loader{vm["kitti_timestamp_file"].as<std::string>()};

    auto timestamps = time_stamp_loader.run();

    auto format = [](float data)
    {
        return std::to_string(data);
    };

    auto format_time = [](const KITTITimeStamp &stamp)->std::string
    {
        char buff[100];
        strftime (buff, 100, "%F %H:%M:%S.", &stamp.tm);
        long nanoseconds = stamp.nanoseconds.count();
        return std::string(buff)+std::to_string(nanoseconds);
    };
    for (auto file : files)
    {
        // Read in the cloud data
        //pcl::PCDReader reader;
        pcl::PointCloud<PointXYZR>::Ptr cloud_f(new pcl::PointCloud<PointXYZR>);
        //reader.read ("table_scene_lms400.pcd", *cloud);
        KITTIDataLoader loader{ file };
        KITTIIMUGPSData gps{gps_files[counter]};
        auto gps_map = gps.run();
        pcl::PointCloud<PointXYZR>::Ptr cloud = loader.run();
        pcl::PointCloud<PointXYZR>::Ptr cloud_filtered(new pcl::PointCloud<PointXYZR>);
        pcl::PointCloud<PointXYZR>::Ptr cloud_remaining(new pcl::PointCloud<PointXYZR>);
        // Create the filtering object: filter data to 100x100 m area
        pcl::ConditionAnd<PointXYZR>::Ptr range_cond(new pcl::ConditionAnd<PointXYZR>());
        range_cond->addComparison(pcl::FieldComparison<PointXYZR>::Ptr(new pcl::FieldComparison<PointXYZR>("x", pcl::ComparisonOps::LT, x_limit)));
        range_cond->addComparison(pcl::FieldComparison<PointXYZR>::Ptr(new pcl::FieldComparison<PointXYZR>("x", pcl::ComparisonOps::GT, -x_limit)));
        range_cond->addComparison(pcl::FieldComparison<PointXYZR>::Ptr(new pcl::FieldComparison<PointXYZR>("y", pcl::ComparisonOps::LT, y_limit)));
        range_cond->addComparison(pcl::FieldComparison<PointXYZR>::Ptr(new pcl::FieldComparison<PointXYZR>("y", pcl::ComparisonOps::GT, -y_limit)));
        pcl::ConditionalRemoval<PointXYZR> range_filt;
        range_filt.setCondition(range_cond);
        range_filt.setInputCloud(cloud);
        range_filt.setKeepOrganized(false);

        // The indices_x array indexes all points of cloud_in that have x between 0.0 and 1000.0
        range_filt.filter(*cloud_filtered);
        // The indices_rem array indexes all points of cloud_in that have x smaller than 0.0 or larger than 1000.0
        //indices_rem = range_filt.getRemovedIndices ();

        //viewer.addPointCloud<PointXYZR> (cloud, "cloud");
        pcl::visualization::PointCloudColorHandlerCustom<PointXYZR>::Ptr plane_color_handler{ new pcl::visualization::PointCloudColorHandlerCustom<PointXYZR>(cloud, 255, 0, 255) };
        pcl::visualization::PointCloudColorHandlerCustom<PointXYZR>::Ptr off_scene_model_color_handler{ new pcl::visualization::PointCloudColorHandlerCustom<PointXYZR>(cloud_filtered, 0, 255, 255) };
        //viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud_filtered");
        //viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud");
        worker.addCloud(cloud, plane_color_handler,"cloud");
        worker.addCloud(cloud_filtered, off_scene_model_color_handler, "cloud_filtered");

        PCLTextInfo info = { file,0,0 };
        PCLTextInfo lat = { format(gps_map["vf"]),0,20 };
        PCLTextInfo lon = { format(gps_map["vl"]),0,40 };

        auto tm = timestamps.at(counter);
        PCLTextInfo time = { format_time(tm),0,60 };

        worker.addText("File",info);
        worker.addText("vf", lat);
        worker.addText("vl", lon);
        worker.addText("timestamp", time);
        std::cout << "File: " << file << std::endl;
        for (auto pair : gps_map)
        {
            //std::cout << pair.first << " : " << pair.second << std::endl;
        }

        std::cout << "Time: " << format_time(tm) << std::endl;

        ++counter;

    }
    while (!worker.wasStopped())
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    // worker.stop();
    while (!worker.wasStopped())
    {
        std::cout << "Waiting to stop" << std::endl;
    }

    return (0);
}


#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

int main()
{
  boost::random::mt19937 gen;
  boost::random::uniform_int_distribution<> dist(1, 6);

  if (dist(gen) > 6) {
    return 1;
  }

  return 0;
}


#include <bits/stdc++.h>
using namespace std;
// --------------------------------------------------------

// reference: https://boostjp.github.io/tips/multiprec-int.html
#include <boost/multiprecision/cpp_int.hpp>
typedef boost::multiprecision::cpp_int bint;

int main() {
    ios::sync_with_stdio(false);
    cin.tie(0);
    cout << fixed << setprecision(15);

    bint A; cin >> A;
    bint B; cin >> B;

    bint ans = A + B;
    cout << ans << '\n';

    return 0;
}
// Verify: http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=NTL_2_A&lang=ja
//     please use language: C++11 (not C++17)


#include <cmath>
#include <iostream>
#include <fstream>
#include <vector>
#include <complex>
#include <algorithm>
#include <chrono>
#include <boost/program_options.hpp>
#include <boost/spirit/include/karma.hpp>
#include <boost/container/flat_map.hpp>
#include <sndfile.h>
#include <OpenImageIO/imageio.h>
#ifdef ENABLE_CUDA
#include <cufft.h>
#include <cufftXt.h>
#else
#include <fftw3.h>
#endif

struct fft_failed : public std::runtime_error {
  fft_failed( const std::string &what ) : std::runtime_error( what ) {}
  fft_failed( const char *what ) : std::runtime_error( what ) {}
};

struct fft_initialization_failed : public fft_failed {
  fft_initialization_failed( const std::string &what ) : fft_failed( what ) {}
  fft_initialization_failed( const char *what ) : fft_failed( what ) {}
};
struct fft_allocation_failed : public fft_failed {
  fft_allocation_failed( const std::string &what ) : fft_failed( what ) {}
  fft_allocation_failed( const char *what ) : fft_failed( what ) {}
};
struct fft_execution_failed : public fft_failed {
  fft_execution_failed( const std::string &what ) : fft_failed( what ) {}
  fft_execution_failed( const char *what ) : fft_failed( what ) {}
};
struct fft_data_transfar_failed : public fft_failed {
  fft_data_transfar_failed( const std::string &what ) : fft_failed( what ) {}
  fft_data_transfar_failed( const char *what ) : fft_failed( what ) {}
};

#ifdef ENABLE_CUDA

#define checkCudaErrors( expr, exception ) \
{ \
  auto cuda_result = expr; \
  if( cuda_result != cudaSuccess ) \
    throw exception ( cudaGetErrorString( cudaGetLastError() ) ); \
}

#define checkCuFFTErrors( expr, exception ) \
{ \
  auto cuda_result = expr; \
  if( cuda_result != CUFFT_SUCCESS ) \
    throw exception ( cudaGetErrorString( cudaGetLastError() ) ); \
}

struct fft_detail {
  float *window;
  size_t resolution;
  size_t interval;
  size_t width;
  float *envelope;
};

__device__ inline void atomicAdd(float* address, float value) {
  while( ( old = atomicExch( address, atomicExch( address, 0.0f ) + old ) ) != 0.0f );
}

__device__ cufftReal input_cb(
  void *src,
  size_t offset,
  void *callerInfo,
  void *sharedPtr
) {
  fft_detail *detail = (fft_detail*)callerInfo;
  size_t index = offset % detail->resolution;
  size_t batch = offset / detail->resolution;
  int16_t element = ((int16_t*)src)[ index + batch * detail->interval ];
  return ( cufftReal )( element/32767.f * detail->window[ index ] );
}

__device__ void output_cb(
  void *dataOut,
  size_t offset,
  cufftComplex element,
  void *callerInfo,
  void *sharedPtr
) {
  fft_detail *detail = (fft_detail*)callerInfo;
  size_t index = offset % ( (detail->resolution/2) + 1 );
  size_t batch = offset / ( (detail->resolution/2) + 1 );
  if( index < detail->width ) {
    float abs = cuCabsf( element );
    atomicAdd( detail->envelope + detail->width * batch, abs );
    ( (float*)dataOut )[ index + detail->width * batch ] = abs;
  }
}

__device__ cufftCallbackLoadR input_cb_ptr_d = input_cb;
__device__ cufftCallbackStoreC output_cb_ptr_d = output_cb;

__global__ void generate_window( fft_detail *detail ) {
  size_t index = threadIdx.x + blockIdx.x * 1024;
  detail->window[ index ] = sinf( float( M_PI ) * float( index ) / detail->resolution );
}

__global__ void generate_window( float *window, unsigned int resolution ) {
  size_t index = threadIdx.x + blockIdx.x * 1024;
  window[ index ] = sinf( float( M_PI ) * float( index ) / resolution );
}

using window_list_t = boost::container::flat_map< unsigned int, std::shared_ptr< float > >;

window_list_t generate_window() {
  window_list_t result;
  for( unsigned int i = 16u; i != 65536u; i <<= 1 ) {
    float *window;
    checkCudaErrors( cudaMalloc( &window, sizeof(float)*i ), fft_allocation_failed );
    std::shared_ptr< float > wrapped( window, &cudaFree );
    if( i > 1024 ) generate_window<<< i/1024, 1024 >>>( window, i );
    else generate_window<<< 1, i >>>( window, i );
    result.insert( result.end(), std::make_pair( i, wrapped ) );
  }
  checkCudaErrors( cudaDeviceSynchronize(), fft_initialization_failed );
  return std::move( result );
}

std::vector< float > fft( const window_list_t &window, const std::vector< int16_t > &data, size_t resolution, size_t interval, size_t width ) {
  const auto window_iter = window.find( resolution );
  if( window_iter == window.end() ) throw fft_initialization_failed( "invalid resolution" );
  const size_t batch = ( data.size() - resolution )/interval + 1;
  int16_t *envelope;
  checkCudaErrors(cudaMalloc( &envelope, sizeof(float)*batch), fft_allocation_failed );
  std::shared_ptr< float > wrapped_envelope( envelope, &cudaFree );
  checkCudaErrors( cudaMemset( envelope, 0, batch * sizeof(float) ), fft_initialization_failed );
  fft_detail *detail;
  checkCudaErrors(cudaMallocManaged( &detail, sizeof(fft_detail),cudaMemAttachGlobal), fft_allocation_failed );
  std::shared_ptr< fft_detail > wrapped_detail( detail, &cudaFree );
  detail->resolution = resolution;
  detail->interval = interval;
  detail->width = width;
  detail->window = window_iter->second.get();
  detail->envelope = envelope;
  int16_t *input;
  checkCudaErrors(cudaMalloc( &input, sizeof(int16_t)*data.size()), fft_allocation_failed );
  std::shared_ptr< int16_t > wrapped_input( input, &cudaFree );
  checkCudaErrors(cudaMemcpy( input, data.data(), sizeof(int16_t)*data.size(), cudaMemcpyHostToDevice ), fft_data_transfar_failed );
  float *output;
  checkCudaErrors(cudaMalloc( &output, sizeof(float)*width*batch), fft_allocation_failed );
  std::shared_ptr< float > wrapped_output( output, &cudaFree );
  cufftHandle plan;
  checkCuFFTErrors( cufftCreate( &plan ), fft_initialization_failed );
  int signal_size = resolution;
  size_t work_size;
  checkCuFFTErrors( cufftMakePlanMany( plan, 1, &signal_size, 0, 0, 0, 0, 0, 0, CUFFT_R2C, batch, &work_size ), fft_initialization_failed );
  cufftCallbackLoadR input_cb_ptr_h;
  checkCudaErrors( cudaMemcpyFromSymbol( &input_cb_ptr_h, input_cb_ptr_d, sizeof( cufftCallbackLoadR ) ), fft_data_transfar_failed );
  cufftCallbackStoreC output_cb_ptr_h;
  checkCudaErrors( cudaMemcpyFromSymbol( &output_cb_ptr_h, output_cb_ptr_d, sizeof( cufftCallbackStoreC ) ), fft_data_transfar_failed );
  checkCuFFTErrors( cufftXtSetCallback( plan, (void **)&input_cb_ptr_h, CUFFT_CB_LD_REAL, (void **)&detail ), fft_initialization_failed );
  checkCuFFTErrors( cufftXtSetCallback( plan, (void **)&output_cb_ptr_h, CUFFT_CB_ST_COMPLEX, (void **)&detail ), fft_initialization_failed );
  checkCuFFTErrors( cufftExecR2C( plan, (cufftReal*)input, (cufftComplex *)output ), fft_execution_failed );
  std::vector< float > result( width*batch );
  checkCudaErrors( cudaDeviceSynchronize(), fft_execution_failed );
  checkCudaErrors(cudaMemcpy( result.data(), output, sizeof(float)*width*batch, cudaMemcpyDeviceToHost ),fft_data_transfar_failed);
  return std::move( result );
}
#else
using window_list_t = boost::container::flat_map< unsigned int, std::vector< float > >;

window_list_t generate_window() {
  window_list_t result;
  for( unsigned int i = 16u; i != 65536u; i <<= 1 ) {
    float *window;
    std::vector< float > w( i );
    for( unsigned int j = 0u; j != i; ++j )
      w[ j ] = sinf( float( M_PI ) * float( j ) / i );
    result.insert( result.end(), std::make_pair( i, std::move( w ) ) );
  }
  return std::move( result );
}

std::vector< float > fft( const window_list_t &window, const std::vector< int16_t > &data, size_t resolution, size_t interval, size_t width ) {
  const auto window_iter = window.find( resolution );
  if( window_iter == window.end() ) throw fft_initialization_failed( "invalid resolution" );
  const size_t batch = ( data.size() - resolution )/interval + 1;
  std::shared_ptr< float > input( (float*)fftwf_malloc(sizeof(float)*resolution), &fftwf_free );
  if( !input ) throw fft_allocation_failed( "unable to allocate memory for input" );
  std::shared_ptr< fftwf_complex > output( (fftwf_complex*)fftwf_malloc(sizeof(fftwf_complex)*resolution), &fftwf_free );
  if( !output ) throw fft_allocation_failed( "unable to allocate memory for output" );
  std::vector< float > result;
  fftwf_plan plan = fftwf_plan_dft_r2c_1d( resolution, input.get(), output.get(), FFTW_ESTIMATE );
  if( !plan ) fft_initialization_failed( "unable to create the plan" );
  for( size_t current_batch = 0u; current_batch != batch; ++current_batch ) {
    for( size_t i = 0u; i != resolution; ++i )
      input.get()[ i ] = data[ i + current_batch * interval ]/32767.f * window_iter->second[ i ];
    fftwf_execute( plan );
    for( size_t i = 0u; i != width; ++i )
      result.push_back( std::abs( std::complex< float >( output.get()[ i ][ 0 ], output.get()[ i ][ 1 ] ) ) );
  }
  fftwf_destroy_plan( plan );
  return std::move( result );
}

#endif

float accumulate( const std::vector< float > &input, int x, float kp, const std::vector< std::tuple< int, float > > &window ) {
  float sum = std::accumulate(
    window.begin(), window.end(), 0.f,
    [&]( float s, const std::tuple< int, float > &v ) -> float {
      int pos = x + std::get< 0 >( v );
      if( pos < 0 || pos >= input.size() ) return s;
      else return s + std::get< 1 >( v ) * input[ x + std::get< 0 >( v ) ];
    }
  );
  return sum / kp;
}

std::tuple< int, int, int > segment_envelope( const std::vector< float > &input, unsigned int sample_rate ) {
  std::vector< float > grad;
  for( size_t i = 1u; i != input.size(); ++i )
    grad.emplace_back( ( input[ i ] - input[ i - 1u ] )*sample_rate );
  std::vector< float > release( input.size(), 0 );
  const auto blank = std::distance(
    input.rbegin(),
    std::find_if( input.rbegin(), input.rend(), []( float v ) { return v != 0; } )
  ) + 1;
  float min_grad = std::abs( grad[ input.size() - ( 1 + blank ) ] );
  for( size_t i = 1u; i != input.size() - blank; ++i ) {
    min_grad = std::min( min_grad, std::max( 0.f, -grad[ input.size() - ( i + blank ) ] ) );
    release[ input.size() - ( i + blank ) ] = min_grad * ( float( i ) / sample_rate );
  }
  const auto release_pos = std::distance( release.begin(), std::max_element( release.begin(), release.end() ) );
  float delay = 0.f;
  const float max = *std::max_element( input.begin(), std::next( input.begin(), release_pos ) );
  const auto p0 = std::find_if( input.begin(), std::next( input.begin(), release_pos ), [&]( float v ){ return v >= max * 0.2f; } );
  const auto p1 = std::find_if( input.begin(), std::next( input.begin(), release_pos ), [&]( float v ){ return v >= max * 0.4f; } );
  const auto tangent = float( std::distance( p0, p1 ) )/( *p1 - *p0 );
  const auto intercept = float( std::distance( input.begin(), p0 ) ) - tangent * *p0;
  const int delay_pos = std::max( int( intercept ), 0 );
  std::vector< float > attack( input.size(), 0 );
  min_grad = 1.f/tangent;
  for( size_t i = 0u; i != release_pos - delay_pos; ++i ) {
    min_grad = std::min( min_grad, std::max( 0.f, grad[ i + delay_pos ] ) );
    attack[ i + delay_pos ] = min_grad * ( float( i ) / sample_rate );
  }
  const auto attack_pos = std::distance( attack.begin(), std::max_element( attack.begin(), attack.end() ) );
  std::cout << attack_pos << " " << release_pos << std::endl;
  return std::make_tuple( delay_pos, attack_pos, release_pos );
}

float rect( float x ) {
  x -= floorf( x );
  if( x < 0.5f ) return -1.f;
  else return 1.f;
}

float tri( float x ) {
  x -= floorf( x );
  if( x < 0.25f ) return 4.f*x;
  else if( x < 0.75f ) return -4.f*x+2.f;
  else return 4.f*x-4.f;
}

std::vector< int16_t > load_monoral( const std::string &filename, bool norm ) {
  SF_INFO info;
  info.frames = 0;
  info.samplerate = 0;
  info.channels = 0;
  info.format = 0;
  info.sections = 0;
  info.seekable = 0;
  std::cout << filename << std::endl;
  auto audio_file = sf_open( filename.c_str(), SFM_READ, &info );
  if( !audio_file ) {
    std::cerr << "Unable to open audio file" << std::endl;
    throw -1;
  }
  std::vector< int16_t > monoral;
  std::vector< int16_t > multi_channel;
  multi_channel.resize( info.frames * info.channels );
  const auto read_count = sf_read_short( audio_file, multi_channel.data(), info.frames * info.channels );
  if( read_count == 0u ) {
    std::cerr << "Unable to read audio file" << std::endl;
    throw -1;
  }
  sf_close( audio_file );
  if( read_count % info.channels != 0 ) {
    std::cerr << "Invalid audio file" << std::endl;
    throw -1;
  }
  multi_channel.resize( read_count );
  monoral.reserve( read_count / info.channels );
  for( auto iter = multi_channel.begin(); iter != multi_channel.end(); ) {
    auto next_iter = std::next( iter, info.channels );
    monoral.emplace_back( std::accumulate( iter, next_iter, int16_t( 0 ) ) / info.channels );
    iter = next_iter;
  }
  const auto audio_begin = std::find_if( monoral.begin(), monoral.end(), []( int16_t v ) { return v != 0; } );
  std::cout << std::distance( monoral.begin(), audio_begin ) << std::endl;
  monoral.erase( monoral.begin(), audio_begin );
  const auto max_iter = std::max_element( monoral.begin(), monoral.end() );
  const auto min_iter = std::min_element( monoral.begin(), monoral.end() );
  const int16_t max =
    std::max(
      ( max_iter != monoral.end() ) ? std::abs( *max_iter ) : int16_t( 0 ),
      ( min_iter != monoral.end() ) ? std::abs( *min_iter ) : int16_t( 0 )
    );
  if( norm ) {
    if( max != 0 ) {
      for( auto &elem: monoral )
        elem = int32_t( elem ) * 32768 / max;
    }
  }
  return monoral;
}

struct spectrum_image {
  spectrum_image(
    const window_list_t &window,
    const std::vector< int16_t > &audio,
    int x_,
    uint8_t note,
    unsigned int sample_rate_,
    uint32_t resolution_,
    uint32_t interval_,
    bool delayed
  ) : x( x_ ), resolution( resolution_ ), interval( interval_ ), sample_rate( sample_rate_ ) {
    y = audio.size() / ( sample_rate / interval );
    const int channels = 3;
    pixels.reserve( x * y * channels );
    const float freq = exp2f( ( ( float( note ) +  3.f ) / 12.f ) ) * 6.875f;
    std::vector< uint32_t > harmonic;
    for( size_t i = 0u; i != 16u; ++ i ) {
      harmonic.emplace_back( ceilf( freq * std::pow( 2.f, float( i ) ) * float( resolution ) * 2.f /float( sample_rate ) ) );
    }
    uint32_t base = harmonic.front();
    std::cout << "base : " << base << std::endl;
    size_t offset = 0;
    size_t y_pos = 0;
    size_t mute_since = 0;
    const auto converted = fft( window, audio, resolution, sample_rate/interval, x );
    const auto begin_date = std::chrono::high_resolution_clock::now();
    for( auto iter = converted.begin(); iter != converted.end(); iter += x ) {
      auto end = std::next( iter, x );
      if( !delayed || ( x > base && *std::next( iter, base ) != 0 ) ) {
        delayed = false;
        auto harmonic_iter = harmonic.begin();
        size_t x_pos = 0;
        for( const auto &l : boost::make_iterator_range( iter, std::next( iter, x ) ) ) {
          const auto p = uint8_t( std::min( std::max( 80.f * std::log10( std::max( l, 1.f ) ), 0.f ), 255.f ) );
          if( *harmonic_iter == x_pos ) {
            pixels.emplace_back( 128 + p / 2 );
            pixels.emplace_back( p );
            pixels.emplace_back( p );
            ++harmonic_iter;
          }
          else if( y_pos % interval == 0 ) {
            pixels.emplace_back( 128 + p / 2 );
            pixels.emplace_back( p );
            pixels.emplace_back( p );
          }
          else {
            pixels.emplace_back( p );
            pixels.emplace_back( p );
            pixels.emplace_back( p );
          }
          ++x_pos;
        }
        envelope.emplace_back( std::accumulate( iter, end, 0.f ) );
        ++y_pos;
        if( std::find_if( iter, end, []( float v ) { return v != 0; } ) != end ) mute_since = y_pos;
      }
      else --y;
    }
    const auto end_date = std::chrono::high_resolution_clock::now();
    std::cout << "Elapsed: " << std::chrono::duration_cast< std::chrono::microseconds >( end_date - begin_date ).count() << "us" << std::endl;
    pixels.resize( x * mute_since * channels );
  }
  std::vector< float > envelope;
  std::vector< uint8_t > pixels;
  int x;
  int y;
  uint32_t resolution;
  uint32_t interval;
  uint32_t sample_rate;
};

int main( int argc, char* argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("input,i", boost::program_options::value<std::string>(),  "入力ファイル")
    ("output,o", boost::program_options::value<std::string>(),  "出力ファイル")
    ("envelope,e", boost::program_options::value<std::string>(),  "エンベロープ")
    ("note,n", boost::program_options::value<int>()->default_value(60),  "音階")
    ("width,w", boost::program_options::value<int>()->default_value(1024),  "幅")
    ("resolution,r", boost::program_options::value<int>()->default_value(13),  "分解能")
    ("interval,j", boost::program_options::value<int>()->default_value(100),  "間隔");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") || !params.count("input") || !params.count("output") || !params.count( "note" ) || !params.count( "envelope" ) ) {
    std::cout << options << std::endl;
    return 0;
  }
  const std::string input_filename = params["input"].as<std::string>();
  const std::string output_filename = params["output"].as<std::string>();
  const auto audio = load_monoral( input_filename, true );
  const int x = params["width"].as<int>();
  const auto window = generate_window();
  const spectrum_image image( window, audio, x, params["note"].as<int>(), 44100, 1 << params["resolution"].as<int>(), params["interval"].as<int>(), true );
  const int channels = 3;
  using namespace OIIO_NAMESPACE;
  ImageOutput *out = ImageOutput::create( output_filename );
  if ( !out ) {
    std::cerr << "Unable to open output file" << std::endl;
    return -1;
  }
  ImageSpec spec ( image.x, image.y, channels, TypeDesc::UINT8);
  out->open( output_filename, spec );
  out->write_image( TypeDesc::UINT8, image.pixels.data() );
 out->close();

  std::fstream envelope_file( params["envelope"].as<std::string>(), std::ios::out );
  for( int i = 0; i != image.envelope.size(); ++i ) {
    std::string line;
    namespace karma = boost::spirit::karma;
    karma::generate( std::back_inserter( line ), karma::float_ << '\t' << karma::float_ << karma::eol, boost::fusion::make_vector( float( i )/image.interval, image.envelope[ i ] ) );
    envelope_file.write( line.c_str(), line.size() );
  }
  envelope_file.close();
  const auto grad = segment_envelope( image.envelope, image.interval );
  std::cout << "delay : " << std::get< 0 >( grad )/float( image.interval ) << std::endl;
  std::cout << "attack : " << std::get< 1 >( grad )/float( image.interval ) << std::endl;
  std::cout << "release : " << std::get< 2 >( grad )/float( image.interval ) << std::endl;
//  for( size_t i = 0; i != grad.size(); ++i )
//    std::cout << i * 0.01f << " " << grad[ i ] << " " << image.envelope[ i ] << std::endl;
//  ImageOutput::destroy( out );
}


// Component
#include "RosConversionHelper.hpp"

// Ros
#include <tf/tf.h>
#include <tf/transform_datatypes.h>

// Libraries
#include <boost/cstdfloat.hpp>
#include <boost/shared_ptr.hpp>

// Standard
#include <utility>

namespace local_planner
{

float64_t RosConversionHelper::quaternionMsgToYawR(const geometry_msgs::Quaternion& q)
{
    tf::Quaternion new_q;
    tf::quaternionMsgToTF(q, new_q);
    float64_t roll_r, pitch_r, yaw_r;
    tf::Matrix3x3(std::move(new_q)).getRPY(roll_r, pitch_r, yaw_r);

    return yaw_r;
}

} // namespace local_planner


#include <boost/math/quadrature/gauss_kronrod.hpp>


// SPDX-FileCopyrightText: 2020 Sotiris Papatheodorou, Imperial College London
// SPDX-License-Identifier: BSD-3-Clause

#ifndef __SENSOR_HPP
#define __SENSOR_HPP

#include <cmath>

#include <Eigen/Dense>
#include "se/image_utils.hpp"
#include "se/image/image.hpp"
#include "se/projection.hpp"

namespace se {

  struct SensorConfig {
    // General
    int width = 0;
    int height = 0;
    bool left_hand_frame = false;
    float near_plane = 0.f;
    float far_plane = INFINITY;
    // Pinhole camera
    float fx = nan("");
    float fy = nan("");
    float cx = nan("");
    float cy = nan("");
    // LIDAR
    Eigen::VectorXf beam_azimuth_angles = Eigen::VectorXf(1);
    Eigen::VectorXf beam_elevation_angles = Eigen::VectorXf(1);

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

  struct PinholeCamera {
    PinholeCamera(const SensorConfig& c);

    PinholeCamera(const PinholeCamera& pinhole_camera,
                  const float          scaling_factor);

    /**
     * \brief Determine the corresponding image value of the projected pixel for a point_C in camera frame.
     *
     * \param sensor          Reference to the used sensor used for the projection.
     * \param point_C         3D coordinates of the point to be projected in camera frame.
     * \param depth_image     Image
     * \param depth_value     Reference to the depth value to be determined.
     * \param valid_predicate Functor indicating if the fetched pixel value is valid.
     *
     * \return is_valid   Returns true if the projection is successful and false if the projection is unsuccessful
     *                    or the pixel value is invalid.
     */
    template <typename ValidPredicate>
    bool projectToPixelValue(const Eigen::Vector3f&  point_C,
                             const se::Image<float>& image,
                             float&                  image_value,
                             ValidPredicate          valid_predicate) const {
      Eigen::Vector2f pixel_f;
      if (model.project(point_C, &pixel_f) != srl::projection::ProjectionStatus::Successful) {
        return false;
      }
      const Eigen::Vector2i pixel = se::round_pixel(pixel_f);
      image_value = image(pixel.x(), pixel.y());
      // Return false for invalid depth measurement
      if (!valid_predicate(image_value)) {
        return false;
      }
      return true;
    }

    template <typename ValidPredicate>
    bool getPixelValue(const Eigen::Vector2f&  pixel_f,
                       const se::Image<float>& image,
                       float&                  image_value,
                       ValidPredicate          valid_predicate) const {
      if (!model.isInImage(pixel_f)) {
        return false;
      }
      Eigen::Vector2i pixel = se::round_pixel(pixel_f);
      image_value = image(pixel.x(), pixel.y());
      // Return false for invalid depth measurement
      if (!valid_predicate(image_value)) {
        return false;
      }
      return true;
    }

    /**
     * \brief Computes the scale corresponding to the back-projected pixel size
     * in voxel space
     * \param[in] block_centre    The coordinates of the VoxelBlock
     *                            centre in the camera frame.
     * \param[in] voxel_dim       The voxel edge length in meters.
     * \param[in] last_scale      Scale from which propagate up voxel
     *                            values.
     * \param[in] min_scale       Finest scale at which data has been
     *                            integrated into the voxel block (-1 if no
     *                            data has been integrated yet).
     * \param[in] max_block_scale The maximum allowed scale within a
     *                            VoxelBlock.
     * \return The scale that should be used for the integration.
     */
    int computeIntegrationScale(const Eigen::Vector3f& block_centre,
                                const float            voxel_dim,
                                const int              last_scale,
                                const int              min_scale,
                                const int              max_block_scale) const;

    /**
     * \brief Return the minimum distance at which measurements are available
     * along the ray passing through pixels x and y.
     *
     * This differs from the PinholeCamera::near_plane since the near_plane is
     * a z-value while nearDist is a distance along a ray.
     *
     * \param[in] ray_C The ray starting from the camera center and expressed
     *                  in the camera frame along which nearDist
     *                  will be computed.
     * \return The minimum distance along the ray through the pixel at which
     *         valid measurements may be encountered.
     */
    float nearDist(const Eigen::Vector3f& ray_C) const;

    /**
     * \brief Return the maximum distance at which measurements are available
     * along the ray passing through pixels x and y.
     *
     * This differs from the PinholeCamera::far_plane since the far_plane is a
     * z-value while farDist is a distance along a ray.
     *
     * \param[in] ray_C The ray starting from the camera center and expressed
     *                  in the camera frame along which nearDist
     *                  will be computed.
     * \return The maximum distance along the ray through the pixel at which
     *         valid measurements may be encountered.
     */
    float farDist(const Eigen::Vector3f& ray_C) const;

    /**
     * \brief Convert a point in the sensor frame into a depth measurement.
     * For the PinholeCamera this means returning the z-coordinate
     * of the point.
     *
     * \param[in] point_C A point observed by the sensor expressed in the
     *                    sensor frame.
     * \return The depth value that the sensor would get from this point.
     */
    float measurementFromPoint(const Eigen::Vector3f& point_C) const;

    /**
     * \brief Test whether a 3D point in camera coordinates is inside the
     * camera frustum.
     */
    bool pointInFrustum(const Eigen::Vector3f& point_C) const;

    /**
     * \brief Test whether a 3D point in camera coordinates is inside the
     * camera frustum.
     *
     * The difference from PinholeCamera::pointInFrustum is that it is assumed
     * that the far plane is at infinity.
     */
    bool pointInFrustumInf(const Eigen::Vector3f& point_C) const;

    /**
     * \brief Test whether a sphere in camera coordinates is inside the camera
     * frustum.
     *
     * It is tested whether the sphere's center is inside the camera frustum
     * offest outwards by the sphere's radius. This is a quick test that in
     * some rare cases may return a sphere as being visible although it isn't.
     */
    bool sphereInFrustum(const Eigen::Vector3f& center_C,
                         const float            radius) const;

    /**
     * \brief Test whether a sphere in camera coordinates is inside the camera
     * frustum.
     *
     * The difference from PinholeCamera::sphereInFrustum is that it is assumed
     * that the far plane is at infinity.
     */
    bool sphereInFrustumInf(const Eigen::Vector3f& center_C,
                            const float            radius) const;

    static std::string type() { return "pinholecamera"; }

    srl::projection::PinholeCamera<srl::projection::NoDistortion> model;
    bool  left_hand_frame;
    float near_plane;
    float far_plane;
    float scaled_pixel;
    /** \brief The horizontal field of view in radians. */
    float horizontal_fov;
    /** \brief The vertical field of view in radians. */
    float vertical_fov;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    private:
      void computeFrustumVertices();
      void computeFrustumNormals();

      static constexpr int num_frustum_vertices_ = 8;
      static constexpr int num_frustum_normals_ = 6;
      Eigen::Matrix<float, 4, num_frustum_vertices_> frustum_vertices_;
      Eigen::Matrix<float, 4, num_frustum_normals_> frustum_normals_;
  };

  struct OusterLidar {
    OusterLidar(const SensorConfig& c);

    OusterLidar(const OusterLidar& ouster_lidar,
                const float        scaling_factor);

    /**
     * \brief Determine the corresponding image value of the projected pixel for a point_C in camera frame.
     *
     * \param sensor          Reference to the used sensor used for the projection.
     * \param point_C         3D coordinates of the point to be projected in camera frame.
     * \param depth_image     Image
     * \param depth_value     Reference to the depth value to be determined.
     * \param valid_predicate Functor indicating if the fetched pixel value is valid.
     *
     * \return is_valid   Returns true if the projection is successful and false if the projection is unsuccessful
     *                    or the depth value is invalid.
     */
    template <typename ValidPredicate>
    bool projectToPixelValue(const Eigen::Vector3f&  point_C,
                             const se::Image<float>& image,
                             float&                  image_value,
                             ValidPredicate          valid_predicate) const {
      Eigen::Vector2f pixel_f;
      if (model.project(point_C, &pixel_f) != srl::projection::ProjectionStatus::Successful) {
        return false;
      }
      const Eigen::Vector2i pixel = se::round_pixel(pixel_f);
      image_value = image(pixel.x(), pixel.y());
      // Return false for invalid depth measurement
      if (!valid_predicate(image_value)) {
        return false;
      }
      return true;
    }

    /**
     * \brief Computes the scale corresponding to the back-projected pixel size
     * in voxel space
     * \param[in] block_centre    The coordinates of the VoxelBlock
     *                            centre in the camera frame.
     * \param[in] voxel_dim       The voxel edge length in meters.
     * \param[in] last_scale      Scale from which propagate up voxel
     *                            values.
     * \param[in] min_scale       Finest scale at which data has been
     *                            integrated into the voxel block (-1 if no
     *                            data has been integrated yet).
     * \param[in] max_block_scale The maximum allowed scale within a
     *                            VoxelBlock.
     * \return The scale that should be used for the integration.
     */
    int computeIntegrationScale(const Eigen::Vector3f& block_centre,
                                const float            voxel_dim,
                                const int              last_scale,
                                const int              min_scale,
                                const int              max_block_scale) const;

    /**
     * \brief Return the minimum distance at which measurements are available
     * along the ray passing through pixels x and y.
     *
     * This function just returns OusterLidar::near_plane.
     *
     * \param[in] ray_C The ray starting from the camera center and expressed
     *                  in the camera frame along which nearDist
     *                  will be computed.
     * \return The minimum distance along the ray through the pixel at which
     *         valid measurements may be encountered.
     */
    float nearDist(const Eigen::Vector3f& ray_C) const;

    /**
     * \brief Return the maximum distance at which measurements are available
     * along the ray passing through pixels x and y.
     *
     * This function just returns OusterLidar::far_plane.
     *
     * \param[in] ray_C The ray starting from the camera center and expressed
     *                  in the camera frame along which nearDist
     *                  will be computed.
     * \return The maximum distance along the ray through the pixel at which
     *         valid measurements may be encountered.
     */
    float farDist(const Eigen::Vector3f& ray_C) const;

    /**
     * \brief Convert a point in the sensor frame into a depth measurement.
     * For the OusterLidar this means returning the norm of the point.
     *
     * \param[in] point_C A point observed by the sensor expressed in the
     *                    sensor frame.
     * \return The depth value that the sensor would get from this point.
     */
    float measurementFromPoint(const Eigen::Vector3f& point_C) const;

    /**
     * \brief Test whether a 3D point in camera coordinates is inside the
     * sensor frustum.
     *
     * \todo Implement
     */
    bool pointInFrustum(const Eigen::Vector3f& point_C) const;

    /**
     * \brief Test whether a 3D point in camera coordinates is inside the
     * sensor frustum.
     *
     * \todo Implement
     *
     * The difference from OusterLidar::pointInFrustum is that it is assumed
     * that the far plane is at infinity.
     */
    bool pointInFrustumInf(const Eigen::Vector3f& point_C) const;

    /**
     * \brief Test whether a sphere in camera coordinates is inside the sensor
     * frustum.
     *
     * \todo Implement
     *
     * \todo Describe any issues/assumptions/approximations once implemented.
     */
    bool sphereInFrustum(const Eigen::Vector3f& center_C,
                         const float            radius) const;

    /**
     * \brief Test whether a sphere in camera coordinates is inside the sensor
     * frustum.
     *
     * \todo Implement
     *
     * The difference from OusterLidar::sphereInFrustum is that it is assumed
     * that the far plane is at infinity.
     */
    bool sphereInFrustumInf(const Eigen::Vector3f& center_C,
                            const float            radius) const;

    static std::string type() { return "ousterlidar"; }

    srl::projection::OusterLidar model;
    bool  left_hand_frame;
    float near_plane;
    float far_plane;
    float min_ray_angle;
    /** \brief The horizontal field of view in radians. */
    float horizontal_fov;
    /** \brief The vertical field of view in radians. */
    float vertical_fov;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

} // namespace se

#endif


#pragma once
//! c/c++ headers
#include <cmath>
#include <functional>
#include <map>
#include <memory>
#include <tuple>
#include <unordered_map>
#include <utility>
//! dependency headers
#include <armadillo>
#include <boost/functional/hash.hpp>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#include "nlohmann/json.hpp"
//! project headers
#include "correspondences/common/base.hpp"
#include "correspondences/common/types.hpp"
#include "correspondences/common/utilities.hpp"

namespace correspondences {
namespace qap {
/** @struct Config
 * @brief configuration parameters for optimization algorithm
 * @var Config::corr_threshold
 * value in optimum solution to declare a correspondence as valid {\in (0, 1)}
 * @var Config::n_pair_threshold
 * minimum number of pairwise consistencies
 * @var Config::min_corr
 * number of correspondences/matches to identify during optimization
 */
struct Config : CorrespondencesConfigBase {
  Config()
    : CorrespondencesConfigBase() {
      set_defaults();
    }

  explicit Config(nlohmann::json & config) :
    CorrespondencesConfigBase(config) {
      set_defaults();
      json_utils::check_for_param(config, "corr_threshold", corr_threshold);
      json_utils::check_for_param(config, "n_pair_threshold", n_pair_threshold);
      json_utils::check_for_param(config, "min_corr", min_corr);
    }

  void set_defaults() noexcept final {
    corr_threshold = 0.9;
    n_pair_threshold = 5;
    min_corr = 5;
  }

  double corr_threshold;
  size_t n_pair_threshold, min_corr;
};

/** @class ConstrainedObjective
 * @brief class definition for point set registration relaxation objective
 */
class ConstrainedObjective {
 public:
   /** @typedef ConstrainedObjective::ADVector
    * @brief typedef for CppAD automatic differentiation during optimization execution
    */
   using ADvector = CPPAD_TESTVECTOR(CppAD::AD<double>);

   /** ConstrainedObjective::ConstrainedObjective(source_pts, target_pts, config)
    * @brief constructor for constrained objective function
    *
    * @param[in] source_pts distribution of (columnar) source points
    * @param[in] target_pts distribution of (columnar) target points
    * @param[in] config `Config` instance with optimization parameters; see `Config` definition
    * @return
    */
   explicit ConstrainedObjective(arma::mat const & source_pts,
       arma::mat const & target_pts, Config const & config) :
     m_(static_cast<size_t>(source_pts.n_cols)), n_(static_cast<size_t>(target_pts.n_cols)),
     min_corr_(config.min_corr) {
     weights_ = generate_weight_tensor(source_pts, target_pts, config.epsilon,
         config.pairwise_dist_threshold);
     n_constraints_ = m_ + n_ + 2;
   }

   /** ConstrainedObjective::~ConstrainedObjective()
    * @brief destructor for constrained objective function
    *
    * @param[in]
    * @return
    */
   ~ConstrainedObjective();

   /** ConstrainedObjective::operator()
    * @brief operator overload for IPOPT
    *
    * @param[in][out] fgrad objective function evaluation (including constraints) at point `z`
    * @param[in] z point for evaluation
    * @return
    */
   void operator()(ADvector &fgrad, ADvector const & z) noexcept;

   /** ConstrainedObjective::num_constraints()
    * @brief return number of constraints for optimization objective
    *
    * @param[in]
    * @return copy of private member `n_constraints_`
    */
   size_t num_constraints() const noexcept { return n_constraints_; }

   /** ConstrainedObjective::num_source_pts()
    * @brief return number of source points for optimization objective
    *
    * @param[in]
    * @return copy of private member `m_`
    */
   size_t num_source_pts() const noexcept { return m_; }

   /** ConstrainedObjective::num_target_pts()
    * @brief return number of target points for optimization objective
    *
    * @param[in]
    * @return copy of private member `n_`
    */
   size_t num_target_pts() const noexcept { return n_; }

   /** ConstrainedObjective::num_min_corr()
    * @brief return minimum number of correspondences for optimization objective
    *
    * @param[in]
    * @return copy of private member `min_corr_`
    */
   size_t num_min_corr() const noexcept { return min_corr_; }

   /** ConstrainedObjective::state_length()
    * @brief get size of state vector for optimization objective
    *
    * @param[in]
    * @return (m_ + 1)*(n_ + 1)
    * @note includes slack variables
    */
   size_t state_length() const noexcept { return (m_ + 1) * (n_ + 1); }

   /** ConstrainedObjective::get_weight_tensor()
    * @brief get weight_tensor
    *
    * @param[in]
    * @return copy of private member `weights_`
    */
   WeightTensor get_weight_tensor() const noexcept { return weights_; }

 private:
   WeightTensor weights_;
   size_t m_, n_, min_corr_, n_constraints_;
};
}  // namespace qap

/** @class QAP : public CorrespondencesBase
 * @brief wrapper class definition for correspondence calculation by solving
 * a quadratic assignment problem (QAP)
 */
class QAP : public CorrespondencesBase {
 public:
   /** @typedef QAP::DVec
    * @brief typedef for CppAD test vector
    */
   using Dvec = CPPAD_TESTVECTOR(double);

   /** @typedef QAP::ipopt_status_t
    * @brief typedef for IPOPT return code
    */
   using ipopt_status_t = CppAD::ipopt::solve_result<Dvec>::status_type;

   /** QAP::QAP(source_pts, target_pts, config)
    * @brief constructor for optimization wrapper class
    *
    * @param[in] source_pts distribution of (columnar) source points
    * @param[in] target_pts distribution of (columnar) target points
    * @param[in] config `Config` instance with optimization parameters; see `Config` definition
    * @return
    */
   explicit QAP(arma::mat const & source_pts,
       arma::mat const & target_pts, qap::Config config) : config_(config) {
     /*std::cout << "####" << std::endl;
     std::cout << config_.epsilon << ", " << config_.pairwise_dist_threshold << ", "
       << config_.corr_threshold << ", " << config_.n_pair_threshold << ", "
       << config_.min_corr << std::endl;
     std::cout << "####" << std::endl;*/
     ptr_obj_ = std::make_unique<qap::ConstrainedObjective>(source_pts, target_pts, config);
     optimum_.resize(ptr_obj_->state_length());
   }

   /** QAP::~QAP()
    * @brief destructor for optimization wrapper class
    *
    * @param[in]
    * @return
    */
   ~QAP();

   /** QAP::calc_optimum()
    * @brief run IPOPT to find optimum for optimization objective
    *
    * @param[in]
    * @return solver status returned by IPOPT
    */
   ipopt_status_t calc_optimum() noexcept;

   /** PointRegRelaxation::find_correspondences()
    * @brief identify pairwise correspondences between source and target set given optimum found
    * during optimization
    *
    * @param[in]
    * @return solution status
    * @see status_e definition in types.hpp
    */
   status_e calc_correspondences(correspondences_t & corr) noexcept final;

   /** QAP::get_optimum()
    * @brief get optimization
    *
    * @param[in]
    * @return
    */
   arma::colvec get_optimum() const noexcept { return optimum_; }

   /** QAP::linear_projection
    * @brief Solve linear assignment problem:
    *  max c.t()*flatten(X) subject to linear constraints
    *
    * @note linear constraints are constructed within the function
    *
    * @param [in][out] opt_lp projection of optimal solution onto permutation matrices
    * @return true if converged, false otherwise
    */
   bool linear_projection(arma::colvec & opt_lp) const noexcept;

   /** QAP::num_consistent_pairs()
    * @brief get identified correspondences
    *
    * @param[in]
    * @return number of pairwise consistencies identified
    */
   size_t num_consistent_pairs() const noexcept { return ptr_obj_->get_weight_tensor().size(); }

 private:
   qap::Config config_;
   std::unique_ptr<qap::ConstrainedObjective> ptr_obj_;
   arma::colvec optimum_;
};
}  // namespace correspondences


//
// MIT License
//
// Copyright (c) Deif Lou
//
// 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.
//

#include <opencv2/imgproc.hpp>
#include <Eigen/Dense>
#include <limits>

#include "filter.h"
#include "filterwidget.h"
#include <imgproc/lut.h>
#include <imgproc/types.h>
#include <imgproc/colorconversion.h>
#include <misc/util.h>

#define MAX_IMAGE_SIZE 128
#define DEGREE 3
#define MIU .5
#define MIUSQR (MIU * MIU)
#define MATRIXCOLUMNS ((DEGREE + 1) * (DEGREE + 2) / 2 - 1)
#define BLURKERNELSIZE 5

Filter::Filter() :
    mOutputMode(CorrectedImageMode1)
{
}

Filter::~Filter()
{
}

ImageFilter *Filter::clone()
{
    Filter * f = new Filter();
    f->mOutputMode = mOutputMode;
    return f;
}

extern "C" QHash<QString, QString> getIBPPluginInfo();
QHash<QString, QString> Filter::info()
{
    return getIBPPluginInfo();
}

QImage Filter::process(const QImage &inputImage)
{
    if (inputImage.isNull() || inputImage.format() != QImage::Format_ARGB32)
        return inputImage;

    register int x, y, w = inputImage.width(), h = inputImage.height(), mean = 0, sw, sh, i, j;
    register HSL * bitsHSL = (HSL *)malloc(w * h * sizeof(HSL)), * bitsHSLsl;
    cv::Mat mlchannel(h, w, CV_8UC1);
    register unsigned char * mbits8;
    register double * mbits321, * mbits322;
    register double weight;

    // Convert to HSL
    convertBGRToHSL(inputImage.bits(), (unsigned char *)bitsHSL, w * h);

    // Separate L channel
    for (y = 0; y < h; y++)
    {
        bitsHSLsl = bitsHSL + y * w;
        mbits8 = mlchannel.ptr(y);
        for (x = 0; x < w; x++)
        {
            *mbits8 = bitsHSLsl->l;
            bitsHSLsl++;
            mbits8++;
        }
    }

    // Resample
    cv::Mat mInitial;
    if (w != MAX_IMAGE_SIZE || h != MAX_IMAGE_SIZE)
    {
        if (w > h)
        {
            sw = MAX_IMAGE_SIZE;
            sh = h * MAX_IMAGE_SIZE / w;
        }
        else
        {
            sh = MAX_IMAGE_SIZE;
            sw = w * MAX_IMAGE_SIZE / h;
        }
        sw = sw < 1 ? 1 : sw;
        sh = sh < 1 ? 1 : sh;

        cv::Mat mresized(sh, sw, CV_8UC1);
        cv::resize(mlchannel, mresized, cv::Size(sw, sh), 0, 0, cv::INTER_CUBIC);
        mInitial = mresized;
    }
    else
    {
        sw = w;
        sh = h;
        mInitial = mlchannel;
    }

    // Remove noise
    cv::Mat mMatUChar(sh, sw, CV_8UC1);
    cv::GaussianBlur(mInitial, mMatUChar, cv::Size(BLURKERNELSIZE, BLURKERNELSIZE), 0);

    // Convert to float and scale
    cv::Mat mMatDouble(sh, sw, CV_64FC1);
    for (y = 0; y < sh; y++)
    {
        mbits8 = mMatUChar.ptr(y);
        mbits321 = (double *)mMatDouble.ptr(y);
        for (x = 0; x < sw; x++)
        {
            *mbits321 = (*mbits8) / 255.;
            mbits8++;
            mbits321++;
        }
    }

    // Get the gradient of the blurred image
    cv::Mat mGradientX(sh, sw, CV_64FC1);
    cv::Mat mGradientY(sh, sw, CV_64FC1);
    cv::Sobel(mMatDouble, mGradientX, -1, 1, 0);
    cv::Sobel(mMatDouble, mGradientY, -1, 0, 1);

    // Calculate weights
    register int row = 0, column = 0, totalPixels = sw * sh;
    Eigen::DiagonalMatrix<float, Eigen::Dynamic> ls_W(totalPixels * 2);
    for (y = 0; y < sh; y++)
    {
        mbits321 = (double *)mGradientX.ptr(y);
        mbits322 = (double *)mGradientY.ptr(y);
        for (x = 0; x < sw; x++)
        {
            weight = sqrt(exp(-sqrt((*mbits321) * (*mbits321) + (*mbits322) * (*mbits322)) / MIUSQR));
            ls_W.diagonal()[row] = ls_W.diagonal()[row + totalPixels] = weight;
            row++;
            mbits321++;
            mbits322++;
        }
    }

    // Surface fitting using eigen
    // First set up the matrix A and the vector b for least squares fitting with the gradient image
    row = 0;
    column = 0;
    Eigen::MatrixXf ls_A(totalPixels * 2, MATRIXCOLUMNS);
    Eigen::VectorXf ls_b(totalPixels * 2);
    Eigen::VectorXf ls_x;

    for (y = 0; y < sh; y++)
    {
        mbits321 = (double *)mGradientX.ptr(y);
        mbits322 = (double *)mGradientY.ptr(y);
        for (x = 0; x < sw; x++)
        {
            column = 0;
            // Fill matrix A with the monomial terms of the polynomial surface
            for (i = 1; i <= DEGREE; i++)
            {
                for (j = 0; j <= i; j++)
                {
                    ls_A(row, column) = (i - j) * (x == 0 ? 1 : pow(x, i - j - 1)) * (y == 0 ? 1 : pow(y, j));
                    ls_A(row + totalPixels, column) = j * (x == 0 ? 1 : pow(x, i - j)) * (y == 0 ? 1 : pow(y, j - 1));

                    column++;
                }
            }
            // Fill vector b with the image gradient values
            ls_b(row) = *mbits321;
            ls_b(row + totalPixels) = *mbits322;

            mbits321++;
            mbits322++;
            row++;
        }
    }
    // Solve...
    ls_x = (ls_W * ls_A).jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(ls_W * ls_b);

    // Set up the matrix A and the vector b for least squares fitting with the blurred image
    ls_A = Eigen::MatrixXf(totalPixels, 2);
    ls_b = Eigen::VectorXf(totalPixels);
    ls_W.diagonal().conservativeResize(totalPixels);
    register double value;
    row = 0;
    for (y = 0; y < sh; y++)
    {
        mbits321 = (double *)mMatDouble.ptr(y);
        for (x = 0; x < sw; x++)
        {
            column = 0;
            value = 0.;
            for (i = 1; i <= DEGREE; i++)
            {
                for (j = 0; j <= i; j++)
                {
                    value += ls_x(column) * (x == 0 ? 1 : pow(x, i - j)) * (y == 0 ? 1 : pow(y, j));
                    column++;
                }
            }
            ls_A(row, 0) = value;
            ls_A(row, 1) = 1;
            ls_b(row) = *mbits321;

            mbits321++;
            row++;
        }
    }
    // Solve...
    ls_x = (ls_W * ls_A).jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(ls_W * ls_b);

    // Create the IIH model image
    row = 0;
    for (y = 0; y < sh; y++)
    {
        mbits321 = (double *)mMatDouble.ptr(y);
        for (x = 0; x < sw; x++)
        {
            *mbits321 = ls_A(row, 0) * ls_x(0) + ls_x(1);

            mbits321++;
            row++;
        }
    }

    // Remap due to out of range values and scale
    double minIIH, maxIIH, minIIHc, maxIIHc;
    cv::minMaxLoc(mMatDouble, &minIIH, &maxIIH);
    minIIHc = IBP_clamp(0., minIIH, 1.);
    maxIIHc = IBP_clamp(0., maxIIH, 1.);
    for (y = 0; y < sh; y++)
    {
        mbits321 = (double *)mMatDouble.ptr(y);
        mbits8 = mMatUChar.ptr(y);
        for (x = 0; x < sw; x++)
        {
            *mbits8 = round((minIIHc + ((*mbits321) - minIIH) * (maxIIHc - minIIHc) / (maxIIH - minIIH)) * 255.);
            mean += *mbits8;
            mbits8++;
            mbits321++;
        }
    }
    mean /= totalPixels;

    // Resample
    if (w != sw || h != sh)
        cv::resize(mMatUChar, mlchannel, cv::Size(w, h), 0, 0, cv::INTER_CUBIC);
    else
        mlchannel = mMatUChar;

    // Make output image
    if (mOutputMode == CorrectedImageMode1)
    {
        // Divide lightness channel
        for (y = 0; y < h; y++)
        {
            bitsHSLsl = bitsHSL + y * w;
            mbits8 = mlchannel.ptr(y);
            for (x = 0; x < w; x++)
            {
                bitsHSLsl->l = IBP_clamp(0, lut02[bitsHSLsl->l][IBP_clamp(1, *mbits8, 255)] * mean / 255, 255);
                bitsHSLsl++;
                mbits8++;
            }
        }
    }
    else if (mOutputMode == CorrectedImageMode2)
    {
        // Divide lightness channel
        for (y = 0; y < h; y++)
        {
            bitsHSLsl = bitsHSL + y * w;
            mbits8 = mlchannel.ptr(y);
            for (x = 0; x < w; x++)
            {
                bitsHSLsl->l = IBP_clamp(0, lut02[bitsHSLsl->l][IBP_clamp(1, *mbits8, 255)], 255);
                bitsHSLsl++;
                mbits8++;
            }
        }
    }
    else
    {
        for (y = 0; y < h; y++)
        {
            bitsHSLsl = bitsHSL + y * w;
            mbits8 = mlchannel.ptr(y);
            for (x = 0; x < w; x++)
            {
                bitsHSLsl->h = bitsHSLsl->s = 0;
                bitsHSLsl->l = *mbits8;
                bitsHSLsl++;
                mbits8++;
            }
        }
    }

    // Convert to RGB
    QImage finalImage = inputImage.copy();
    convertHSLToBGR((const unsigned char *)bitsHSL, finalImage.bits(), w * h);
    free(bitsHSL);

    return finalImage;
}

bool Filter::loadParameters(QSettings &s)
{
    QString outputModeStr;
    OutputMode outputMode;

    outputModeStr = s.value("outputmode", "correctedimagemode1").toString();
    if (outputModeStr == "correctedimagemode1")
        outputMode = CorrectedImageMode1;
    else if (outputModeStr == "correctedimagemode2")
        outputMode = CorrectedImageMode2;
    else if (outputModeStr == "iihcorrectionmodel")
        outputMode = IIHCorrectionModel;
    else
        return false;

    setOutputMode(outputMode);

    return true;
}

bool Filter::saveParameters(QSettings &s)
{
    s.setValue("outputmode", mOutputMode == CorrectedImageMode1 ? "correctedimagemode1" :
                             mOutputMode == CorrectedImageMode2 ? "correctedimagemode2" : "iihcorrectionmodel");
    return true;
}

QWidget *Filter::widget(QWidget *parent)
{
    FilterWidget * fw = new FilterWidget(parent);
    fw->setOutputMode(mOutputMode);
    connect(this, SIGNAL(outputModeChanged(Filter::OutputMode)), fw, SLOT(setOutputMode(Filter::OutputMode)));
    connect(fw, SIGNAL(outputModeChanged(Filter::OutputMode)), this, SLOT(setOutputMode(Filter::OutputMode)));
    return fw;
}

void Filter::setOutputMode(Filter::OutputMode om)
{
    if (om == mOutputMode)
        return;
    mOutputMode = om;
    emit outputModeChanged(om);
    emit parametersChanged();
}


#include <boost/test/unit_test.hpp>
#include "triangle.h"

BOOST_AUTO_TEST_SUITE(test_triangle)

BOOST_AUTO_TEST_CASE(test_triangle_trace)
{
	math::triangle<3> tri(math::vec<3>(0, 1, 0), math::vec<3>(0, 3, 1), math::vec<3>(1, 3, 0));
	math::scalar t;
	math::vec<3> v;
	bool result = tri.trace(math::ray<3>(math::vec<3>(-1, 2, 0), math::vec<3>(10, 0, 0)), t, v);
	BOOST_CHECK (result == true);
	BOOST_CHECK ((math::vec<3>(0.5f, 2, 0) - v).length() < math::EPSILON);
}

BOOST_AUTO_TEST_SUITE_END()