/* Author: Wolfgang Bangerth, Ralf Hartmann, University of Heidelberg, 2001 */

/*    $Id: step-10.cc 27658 2012-11-21 13:22:11Z bangerth $       */
/*                                                                */
/*    Copyright (C) 2001-2004, 2006-2007, 2011-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// The first of the following include files are probably well-known by now and
// need no further explanation.
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/convergence_table.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/tria_boundary_lib.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

// This is the only new one: in it, we declare the MappingQ class
// which we will use for polynomial mappings of arbitrary order:
#include <deal.II/fe/mapping_q.h>

// And this again is C++:
#include <iostream>
#include <fstream>
#include <cmath>

// The last step is as in previous programs:
namespace Step10
{
  using namespace dealii;

  // Now, as we want to compute the value of $\pi$, we have to compare to
  // somewhat. These are the first few digits of $\pi$, which we define
  // beforehand for later use. Since we would like to compute the difference
  // between two numbers which are quite accurate, with the accuracy of the
  // computed approximation to $\pi$ being in the range of the number of
  // digits which a double variable can hold, we rather declare the reference
  // value as a long double and give it a number of extra digits:
  const long double pi = 3.141592653589793238462643;

  // Then, the first task will be to generate some output. Since this program
  // is so small, we do not employ object oriented techniques in it and do not
  // declare classes (although, of course, we use the object oriented features
  // of the library). Rather, we just pack the functionality into separate
  // functions. We make these functions templates on the number of space
  // dimensions to conform to usual practice when using deal.II, although we
  // will only use them for two space dimensions.
  //
  // The first of these functions just generates a triangulation of a circle
  // (hyperball) and outputs the Qp mapping of its cells for different values
  // of p. Then, we refine the grid once and do so again.
  template <int dim>
  void gnuplot_output()
  {
    std::cout << "Output of grids into gnuplot files:" << std::endl
              << "===================================" << std::endl;

    // So first generate a coarse triangulation of the circle and associate a
    // suitable boundary description to it. Note that the default values of
    // the HyperBallBoundary constructor are a center at the origin and a
    // radius equals one.
    Triangulation<dim> triangulation;
    GridGenerator::hyper_ball (triangulation);
    static const HyperBallBoundary<dim> boundary;
    triangulation.set_boundary (0, boundary);

    // Next generate output for this grid and for a once refined grid. Note
    // that we have hidden the mesh refinement in the loop header, which might
    // be uncommon but nevertheless works. Also it is strangely consistent
    // with incrementing the loop index denoting the refinement level.
    for (unsigned int refinement=0; refinement<2;
         ++refinement, triangulation.refine_global(1))
      {
        std::cout << "Refinement level: " << refinement << std::endl;

        // Then have a string which denotes the base part of the names of the
        // files into which we write the output. Note that in the parentheses
        // in the initializer we do arithmetic on characters, which assumes
        // that first the characters denoting numbers are placed consecutively
        // (which is probably true for all reasonable character sets
        // nowadays), but also assumes that the increment
        // refinement is less than ten. This is therefore more a
        // quick hack if we know exactly the values which the increment can
        // assume. A better implementation would use the
        // std::istringstream class to generate a name.
        std::string filename_base = "ball";
        filename_base += '0'+refinement;

        // Then output the present grid for Q1, Q2, and Q3 mappings:
        for (unsigned int degree=1; degree<4; ++degree)
          {
            std::cout << "Degree = " << degree << std::endl;

            // For this, first set up an object describing the mapping. This
            // is done using the MappingQ class, which takes as
            // argument to the constructor the polynomial degree which it
            // shall use.
            const MappingQ<dim> mapping (degree);
            // We note one interesting fact: if you want a piecewise linear
            // mapping, then you could give a value of 1 to the
            // constructor. However, for linear mappings, so many things can
            // be generated simpler that there is another class, called
            // MappingQ1 which does exactly the same is if you
            // gave an degree of 1 to the MappingQ
            // class, but does so significantly faster. MappingQ1
            // is also the class that is implicitly used throughout the
            // library in many functions and classes if you do not specify
            // another mapping explicitly.

            // In degree to actually write out the present grid with this
            // mapping, we set up an object which we will use for output. We
            // will generate Gnuplot output, which consists of a set of lines
            // describing the mapped triangulation. By default, only one line
            // is drawn for each face of the triangulation, but since we want
            // to explicitely see the effect of the mapping, we want to have
            // the faces in more detail. This can be done by passing the
            // output object a structure which contains some flags. In the
            // present case, since Gnuplot can only draw straight lines, we
            // output a number of additional points on the faces so that each
            // face is drawn by 30 small lines instead of only one. This is
            // sufficient to give us the impression of seeing a curved line,
            // rather than a set of straight lines.
            GridOut grid_out;
            GridOutFlags::Gnuplot gnuplot_flags(false, 30);
            grid_out.set_flags(gnuplot_flags);

            // Finally, generate a filename and a file for output using the
            // same evil hack as above:
            std::string filename = filename_base+"_mapping_q";
            filename += ('0'+degree);
            filename += ".dat";
            std::ofstream gnuplot_file (filename.c_str());

            // Then write out the triangulation to this file. The last
            // argument of the function is a pointer to a mapping object. This
            // argument has a default value, and if no value is given a simple
            // MappingQ1 object is taken, which we briefly
            // described above. This would then result in a piecewise linear
            // approximation of the true boundary in the output.
            grid_out.write_gnuplot (triangulation, gnuplot_file, &mapping);
          }
        std::cout << std::endl;
      }
  }

  // Now we proceed with the main part of the code, the approximation of
  // $\pi$. The area of a circle is of course given by $\pi r^2$, so having a
  // circle of radius 1, the area represents just the number that is searched
  // for. The numerical computation of the area is performed by integrating
  // the constant function of value 1 over the whole computational domain,
  // i.e. by computing the areas $\int_K 1 dx=\int_{\hat K} 1
  // \ \textrm{det}\ J(\hat x) d\hat x \approx \sum_i \textrm{det}
  // \ J(\hat x_i)w(\hat x_i)$,
  // where the sum extends over all quadrature points on all active cells in
  // the triangulation, with $w(x_i)$ being the weight of quadrature point
  // $x_i$. The integrals on each cell are approximated by numerical
  // quadrature, hence the only additional ingredient we need is to set up a
  // FEValues object that provides the corresponding `JxW' values of each
  // cell. (Note that `JxW' is meant to abbreviate Jacobian determinant
  // times weight; since in numerical quadrature the two factors always
  // occur at the same places, we only offer the combined quantity, rather
  // than two separate ones.) We note that here we won't use the FEValues
  // object in its original purpose, i.e. for the computation of values of
  // basis functions of a specific finite element at certain quadrature
  // points. Rather, we use it only to gain the `JxW' at the quadrature
  // points, irrespective of the (dummy) finite element we will give to the
  // constructor of the FEValues object. The actual finite element given to
  // the FEValues object is not used at all, so we could give any.
  template <int dim>
  void compute_pi_by_area ()
  {
    std::cout << "Computation of Pi by the area:" << std::endl
              << "==============================" << std::endl;

    // For the numerical quadrature on all cells we employ a quadrature rule
    // of sufficiently high degree. We choose QGauss that is of order 8 (4
    // points), to be sure that the errors due to numerical quadrature are of
    // higher order than the order (maximal 6) that will occur due to the
    // order of the approximation of the boundary, i.e. the order of the
    // mappings employed. Note that the integrand, the Jacobian determinant,
    // is not a polynomial function (rather, it is a rational one), so we do
    // not use Gauss quadrature in order to get the exact value of the
    // integral as done often in finite element computations, but could as
    // well have used any quadrature formula of like order instead.
    const QGauss<dim> quadrature(4);

    // Now start by looping over polynomial mapping degrees=1..4:
    for (unsigned int degree=1; degree<5; ++degree)
      {
        std::cout << "Degree = " << degree << std::endl;

        // First generate the triangulation, the boundary and the mapping
        // object as already seen.
        Triangulation<dim> triangulation;
        GridGenerator::hyper_ball (triangulation);

        static const HyperBallBoundary<dim> boundary;
        triangulation.set_boundary (0, boundary);

        const MappingQ<dim> mapping (degree);

        // We now create a dummy finite element. Here we could choose any
        // finite element, as we are only interested in the `JxW' values
        // provided by the FEValues object below. Nevertheless, we have to
        // provide a finite element since in this example we abuse the
        // FEValues class a little in that we only ask it to provide us with
        // the weights of certain quadrature points, in contrast to the usual
        // purpose (and name) of the FEValues class which is to provide the
        // values of finite elements at these points.
        const FE_Q<dim>     dummy_fe (1);

        // Likewise, we need to create a DoFHandler object. We do not actually
        // use it, but it will provide us with `active_cell_iterators' that
        // are needed to reinitialize the FEValues object on each cell of the
        // triangulation.
        DoFHandler<dim> dof_handler (triangulation);

        // Now we set up the FEValues object, giving the Mapping, the dummy
        // finite element and the quadrature object to the constructor,
        // together with the update flags asking for the `JxW' values at the
        // quadrature points only. This tells the FEValues object that it
        // needs not compute other quantities upon calling the
        // reinit function, thus saving computation time.
        //
        // The most important difference in the construction of the FEValues
        // object compared to previous example programs is that we pass a
        // mapping object as first argument, which is to be used in the
        // computation of the mapping from unit to real cell. In previous
        // examples, this argument was omitted, resulting in the implicit use
        // of an object of type MappingQ1.
        FEValues<dim> fe_values (mapping, dummy_fe, quadrature,
                                 update_JxW_values);

        // We employ an object of the ConvergenceTable class to store all
        // important data like the approximated values for $\pi$ and the error
        // with respect to the true value of $\pi$. We will also use functions
        // provided by the ConvergenceTable class to compute convergence rates
        // of the approximations to $\pi$.
        ConvergenceTable table;

        // Now we loop over several refinement steps of the triangulation.
        for (unsigned int refinement=0; refinement<6;
             ++refinement, triangulation.refine_global (1))
          {
            // In this loop we first add the number of active cells of the
            // current triangulation to the table. This function automatically
            // creates a table column with superscription `cells', in case
            // this column was not created before.
            table.add_value("cells", triangulation.n_active_cells());

            // Then we distribute the degrees of freedom for the dummy finite
            // element. Strictly speaking we do not need this function call in
            // our special case but we call it to make the DoFHandler happy --
            // otherwise it would throw an assertion in the FEValues::reinit
            // function below.
            dof_handler.distribute_dofs (dummy_fe);

            // We define the variable area as `long double' like we did for
            // the pi variable before.
            long double area = 0;

            // Now we loop over all cells, reinitialize the FEValues object
            // for each cell, and add up all the `JxW' values for this cell to
            // `area'...
            typename DoFHandler<dim>::active_cell_iterator
            cell = dof_handler.begin_active(),
            endc = dof_handler.end();
            for (; cell!=endc; ++cell)
              {
                fe_values.reinit (cell);
                for (unsigned int i=0; i<fe_values.n_quadrature_points; ++i)
                  area += fe_values.JxW (i);
              };

            // ...and store the resulting area values and the errors in the
            // table. We need a static cast to double as there is no
            // add_value(string, long double) function implemented. Note that
            // this also concerns the second call as the fabs
            // function in the std namespace is overloaded on its
            // argument types, so there exists a version taking and returning
            // a long double, in contrast to the global namespace
            // where only one such function is declared (which takes and
            // returns a double).
            table.add_value("eval.pi", static_cast<double> (area));
            table.add_value("error",   static_cast<double> (std::fabs(area-pi)));
          };

        // We want to compute the convergence rates of the `error'
        // column. Therefore we need to omit the other columns from the
        // convergence rate evaluation before calling
        // `evaluate_all_convergence_rates'
        table.omit_column_from_convergence_rate_evaluation("cells");
        table.omit_column_from_convergence_rate_evaluation("eval.pi");
        table.evaluate_all_convergence_rates(ConvergenceTable::reduction_rate_log2);

        // Finally we set the precision and scientific mode for output of some
        // of the quantities...
        table.set_precision("eval.pi", 16);
        table.set_scientific("error", true);

        // ...and write the whole table to std::cout.
        table.write_text(std::cout);

        std::cout << std::endl;
      };
  }

  // The following, second function also computes an approximation of $\pi$
  // but this time via the perimeter $2\pi r$ of the domain instead of the
  // area. This function is only a variation of the previous function. So we
  // will mainly give documentation for the differences.
  template <int dim>
  void compute_pi_by_perimeter ()
  {
    std::cout << "Computation of Pi by the perimeter:" << std::endl
              << "===================================" << std::endl;

    // We take the same order of quadrature but this time a `dim-1'
    // dimensional quadrature as we will integrate over (boundary) lines
    // rather than over cells.
    const QGauss<dim-1> quadrature(4);

    // We loop over all degrees, create the triangulation, the boundary, the
    // mapping, the dummy finite element and the DoFHandler object as seen
    // before.
    for (unsigned int degree=1; degree<5; ++degree)
      {
        std::cout << "Degree = " << degree << std::endl;
        Triangulation<dim> triangulation;
        GridGenerator::hyper_ball (triangulation);

        static const HyperBallBoundary<dim> boundary;
        triangulation.set_boundary (0, boundary);

        const MappingQ<dim> mapping (degree);
        const FE_Q<dim>     fe (1);

        DoFHandler<dim> dof_handler (triangulation);

        // Then we create a FEFaceValues object instead of a FEValues object
        // as in the previous function. Again, we pass a mapping as first
        // argument.
        FEFaceValues<dim> fe_face_values (mapping, fe, quadrature,
                                          update_JxW_values);
        ConvergenceTable table;

        for (unsigned int refinement=0; refinement<6;
             ++refinement, triangulation.refine_global (1))
          {
            table.add_value("cells", triangulation.n_active_cells());

            dof_handler.distribute_dofs (fe);

            // Now we run over all cells and over all faces of each cell. Only
            // the contributions of the `JxW' values on boundary faces are
            // added to the long double variable `perimeter'.
            typename DoFHandler<dim>::active_cell_iterator
            cell = dof_handler.begin_active(),
            endc = dof_handler.end();
            long double perimeter = 0;
            for (; cell!=endc; ++cell)
              for (unsigned int face_no=0; face_no<GeometryInfo<dim>::faces_per_cell; ++face_no)
                if (cell->face(face_no)->at_boundary())
                  {
                    // We reinit the FEFaceValues object with the cell
                    // iterator and the number of the face.
                    fe_face_values.reinit (cell, face_no);
                    for (unsigned int i=0; i<fe_face_values.n_quadrature_points; ++i)
                      perimeter += fe_face_values.JxW (i);
                  };
            // Then store the evaluated values in the table...
            table.add_value("eval.pi", static_cast<double> (perimeter/2.));
            table.add_value("error",   static_cast<double> (std::fabs(perimeter/2.-pi)));
          };

        // ...and end this function as we did in the previous one:
        table.omit_column_from_convergence_rate_evaluation("cells");
        table.omit_column_from_convergence_rate_evaluation("eval.pi");
        table.evaluate_all_convergence_rates(ConvergenceTable::reduction_rate_log2);

        table.set_precision("eval.pi", 16);
        table.set_scientific("error", true);

        table.write_text(std::cout);

        std::cout << std::endl;
      };
  }
}

// The following main function just calls the above functions in the order of
// their appearance. Apart from this, it looks just like the main functions of
// previous tutorial programs.
int main ()
{
  try
    {
      std::cout.precision (16);

      Step10::gnuplot_output<2>();

      Step10::compute_pi_by_area<2> ();
      Step10::compute_pi_by_perimeter<2> ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include <Eigen/CholmodSupport>
//#include <nasoq_eigen.h>
#include "../../include/Optimization/LineSearch.h"
#include "../../include/Optimization/LinearConstrainedSolver.h"

//static void getNumIter(double acc_thresh, int& inner_iter_ref, int& outer_iter_ref)
//{
//    if (acc_thresh >= 1e-3)
//    {
//        inner_iter_ref = outer_iter_ref = 0;
//    }
//    else if (acc_thresh < 1e-3 && acc_thresh >= 1e-6)
//    {
//        inner_iter_ref = outer_iter_ref = 1;
//    }
//    // if (acc_thresh >= 1e-3)
//    // {
//    //     inner_iter_ref = outer_iter_ref = 1;
//    // }
//    else if (acc_thresh < 1e-6 && acc_thresh >= 1e-10)
//    {
//        inner_iter_ref = outer_iter_ref = 2;
//    }
//    else if (acc_thresh < 1e-10 && acc_thresh >= 1e-13)
//    {
//        inner_iter_ref = outer_iter_ref = 3;
//    }
//    else
//    {
//        inner_iter_ref = outer_iter_ref = 9;
//    }
//}
//
//
void OptSolver::linearConstSolver(std::function<double(const Eigen::VectorXd&, Eigen::VectorXd*, Eigen::SparseMatrix<double>*, bool)> objFunc, std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)> findMaxStep, const Eigen::SparseMatrix<double>& Aeq, const Eigen::VectorXd& beq, const Eigen::SparseMatrix<double>& Aineq, const Eigen::VectorXd& bineq, Eigen::VectorXd& x0, int numIter, double gradTol, double xTol, double fTol, bool disPlayInfo, std::function<void(const Eigen::VectorXd&, double&, double&)> getNormFunc)
{}
//    const int DIM = x0.rows();
//    Eigen::VectorXd grad = Eigen::VectorXd::Zero(DIM);
//    Eigen::SparseMatrix<double> hessian;
//
//    Eigen::VectorXd neggrad, delta_x;
//    double maxStepSize = 1.0;
//    double reg = 1e-8;
//
//    bool isProj = true;
//
//    // bool isProj = false;
//    int i = 0;
//    for (; i < numIter; i++)
//    {
//        if(disPlayInfo)
//            std::cout << "\niter: " << i << std::endl;
//        double f = objFunc(x0, &grad, &hessian, isProj);
//
//        Eigen::SparseMatrix<double> H = hessian;
//        Eigen::SparseMatrix<double> I(DIM, DIM);
//        I.setIdentity();
//        hessian = H;
//        Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<double>> solver(hessian);
//
//
//
//        while (solver.info() != Eigen::Success)
//        {
//            if (disPlayInfo)
//            {
//                if (isProj)
//                    std::cout << "some small perturb is needed to remove round-off error, current reg = " << reg << std::endl;
//                else
//                    std::cout << "Matrix is not positive definite, current reg = " << reg << std::endl;
//            }
//
//            hessian = H + reg * I;
//            solver.compute(hessian);
//            reg = std::max(2 * reg, 1e-16);
//        }
//
//        // apply nasoq to find a descent direction
//        Eigen::VectorXd y, z;
//        nasoq::QPSettings qpsettings;
//        qpsettings.eps = 1e-8;
//        getNumIter(qpsettings.eps, qpsettings.inner_iter_ref, qpsettings.outer_iter_ref);
//        qpsettings.nasoq_variant = "PREDET";
//        qpsettings.diag_perturb = 1e-10;
//
//         int converged = nasoq::quadprog(hessian.triangularView<Eigen::Lower>(), grad, Aeq, beq, Aineq, bineq, delta_x, y, z, &qpsettings);
//
//         while (converged != nasoq::nasoq_status::Optimal)
//         {
//             if(reg > 100 * hessian.norm())
//             {
//                 std::cout << "reg is larger than 100 * ||H||, but nasoq still failed!" << std::endl;
//                 if (converged == nasoq::nasoq_status::Inaccurate)
//                     std::cout << "result may be inaccurate, only primal-feasibility is satisfied." << std::endl;
//                 else if (converged == nasoq::nasoq_status::Infeasible)
//                     std::cout << "infeasible, the problem is unbounded" << std::endl;
//                 else
//                     std::cout << "NotConverged" << std::endl;
//                 return;
//             }
//         	std::cout << "nasoq solver failed!, try to increase the reg: " << reg << std::endl;
//         	reg = std::max(2 * reg, 1e-16);
//         	hessian = H + reg * I;
//         	converged = nasoq::quadprog(hessian.triangularView<Eigen::Lower>(), grad, Aeq, beq, Aineq, bineq, delta_x, y, z, &qpsettings);
//         }
//
//        maxStepSize = findMaxStep(x0, delta_x);
//
//        double rate = LineSearch::backtrackingArmijo(x0, grad, delta_x, objFunc, maxStepSize);
//
//        if (!isProj)
//        {
//            reg *= 0.5;
//            reg = std::max(reg, 1e-16);
//        }
//
//        x0 = x0 + rate * delta_x;
//
//        double fnew = objFunc(x0, &grad, NULL, isProj);
//        if (disPlayInfo)
//        {
//            std::cout << "line search rate : " << rate << ", actual hessian : " << !isProj << ", reg = " << reg << std::endl;
//            std::cout << "f_old: " << f << ", f_new: " << fnew << ", grad norm: " << grad.norm() << ", delta x: " << rate * delta_x.norm() << ", delta_f: " << f - fnew << std::endl;
//            if (getNormFunc)
//            {
//                double gradz, gradw;
//                getNormFunc(grad, gradz, gradw);
//
//                double updatez, updatew;
//                getNormFunc(rate * delta_x, updatez, updatew);
//                std::cout << "z grad: " << gradz << ", w grad: " << gradw << ", z change: " << updatez << ", w change: " << updatew  << std::endl;
//            }
//        }
//
//        if ((f - fnew) / f < 1e-5 || rate * delta_x.norm() < 1e-5 || grad.norm() < 1e-4)
//            isProj = false;
//
//
//        if (rate < 1e-8)
//        {
//            std::cout << "terminate with small line search rate (<1e-8): L2-norm = " << grad.norm() << std::endl;
//            return;
//        }
//
//        if (grad.norm() < gradTol)
//        {
//            std::cout << "terminate with gradient L2-norm = " << grad.norm() << std::endl;
//            return;
//        }
//
//        if (rate * delta_x.norm() < xTol)
//        {
//            std::cout << "terminate with small variable change, gradient L2-norm = " << grad.norm() << std::endl;
//            return;
//        }
//
//        if (f - fnew < fTol)
//        {
//            std::cout << "terminate with small energy change, gradient L2-norm = " << grad.norm() << std::endl;
//            return;
//        }
//    }
//    if (i >= numIter)
//        std::cout << "terminate with reaching the maximum iteration, with gradient L2-norm = " << grad.norm() << std::endl;
//
//}


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ASINH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ASINH_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/oneosqrteps.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/average.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/hypot.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/log.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/sqr.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/function/is_equal.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF( asinh_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::double_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        A0 x =  bs::abs(a0);
        auto test = is_greater(x,Oneosqrteps<A0>());
        A0 z = if_else(test,dec(x), x+sqr(x)/bs::inc(hypot(One<A0>(), x)));
        #ifndef BOOST_SIMD_NO_INFINITIES
        z = if_else(is_equal(x, Inf<A0>()),x, z);
        #endif
        z =  if_plus(test, log1p(z), Log_2<A0>());
        return bitwise_xor(bitofsign(a0), z);
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF( asinh_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::single_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        // Exhaustive test for: boost::dispatch::functor<bs::tag::asinh_, bs::tag::sse4_2_>
        //              versus: float(boost::math::asinh(double)
        //              With T: bs::native<float, bs::tag::sse_, void>
        //            in range: [-3.40282e+38, 3.40282e+38]
        // 4278190076 values computed.
        // 3619320676 values (84.60%)  within 0.0 ULPs
        //  658843138 values (15.40%)  within 0.5 ULPs
        //      26262 values ( 0.00%)  within 1.0 ULPs
        A0 x = bs::abs(a0);
        bs::as_logical_t<A0> lthalf = is_less(x,Half<A0>());
        A0 x2 = bs::sqr(x);
        A0 z = Zero<A0>();
        std::size_t nb = nbtrue(lthalf);
        A0 bts = bitofsign(a0);
        if(nb > 0)
        {
          z = horn<A0
            , 0x3f800000
            , 0xbe2aa9ad
            , 0x3d9949b1
            , 0xbd2ee581
            , 0x3ca4d6e6
            > (x2)*x;

          if(nb >= A0::static_size) return  bitwise_xor(z, bts);
        }
        A0 tmp =  if_else(is_greater(x, Oneosqrteps<A0>()),
                          x, average(x, hypot(One<A0>(), x)));
       #ifndef  BOOST_SIMD_NO_NANS
        return if_nan_else(is_nan(a0), bitwise_xor(if_else(lthalf, z, log(tmp)+Log_2<A0>()), bts));
       #else
       return bitwise_xor(if_else(lthalf, z, log(tmp)+Log_2<A0>()), bts);
       #endif
      }
   };

} } }

#endif


/**
1. Trigger when receive new map/tf (message filter new map and lidar scan)
2. use icp to match scan and merge into a new topic
**/
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <cstring>
#include <vector>
#include <cmath>
#include <string>
#include <math.h>
#include <signal.h>
#include <Eigen/Geometry>
#include <fstream>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PointStamped.h>
#include <nav_msgs/Odometry.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/io.h>
#include <pcl/io/obj_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/common/centroid.h>
#include <pcl/common/common.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/gicp.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/extract_indices.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/tf.h>

#include <icp_map_merging/cbshot.h>

using namespace pcl;

double last_timestamp = 1556114916.473415;

// std::string PATH = "/home/argsubt/ros_docker_ws/hw4/map.pcd";
std::string PATH = "/home/argsubt/ros_docker_ws/localization_comp/SDC Localization Competition Data/map/nuscenes_map.pcd";
// std::string BAG = "/home/argsubt/ros_docker_ws/localization_comp/SDC Localization Competition Data/sdc_localization_1.bag";
std::string baseFrame = "car";
std::string lidarFrame = "nuscenes_lidar";

class IcpMapping{
    public:
        IcpMapping();
        void pcCallback(const sensor_msgs::PointCloud2ConstPtr &pc);
        Eigen::Matrix4f get_transfrom(std::string link_name);
        Eigen::Matrix4f get_initial_guess();

    private:
        ros::NodeHandle nh;
        ros::Subscriber pc_sub;
        ros::Publisher pc_pub;
        ros::Publisher map_filtered_pub;
        ros::Publisher odom_pub;
        std::ofstream outputFile;
        int id = 1;

        Eigen::Matrix4f trans_lidar2base;
        Eigen::Matrix4f init_trans;
        Eigen::Matrix4f inverse_trans;
        pcl::PointCloud<PointXYZI>::Ptr icp_map;
        pcl::PointCloud<PointXYZI>::Ptr icp_map_voxel;
        pcl::PointCloud<PointXYZI>::Ptr cropped_map;
        pcl::PointCloud<PointXYZI>::Ptr pc_input;
        pcl::PointCloud<PointXYZI>::Ptr pc_input_voxel;
        //pcl::PointCloud<PointXYZI>::Ptr pc_input_voxel;
        pcl::PointCloud<PointXYZI>::Ptr pc_last;

        sensor_msgs::PointCloud2 ros_cloud_msg;

	    tf::TransformListener listener;
        tf::TransformBroadcaster broadcaster;

        cbshot cb;

        // PointCloud<PointXYZI>::Ptr downsample(PointCloud<PointXYZI>::Ptr &in);
        VoxelGrid<PointXYZI> sor;
};

IcpMapping::IcpMapping(){
    icp_map.reset(new PointCloud<PointXYZI>());
    pc_input.reset(new PointCloud<PointXYZI>());
    pc_input_voxel.reset(new PointCloud<PointXYZI>());
    icp_map_voxel.reset(new PointCloud<PointXYZI>());
    cropped_map.reset(new PointCloud<PointXYZI>());
    pc_last.reset(new PointCloud<PointXYZI>());

    pc_pub = nh.advertise<sensor_msgs::PointCloud2> ("/cloud_trans", 1);
    map_filtered_pub = nh.advertise<sensor_msgs::PointCloud2> ("/map_cropped", 1);
    odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);

    // pcl::io::loadPCDFile<pcl::PointXYZI>(PATH, *icp_map);
    // std::cout << "Loaded" << endl;

    ros_cloud_msg = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/map_filtered", nh));
    fromROSMsg(ros_cloud_msg, *icp_map_voxel);

    // std::cerr << "PointCloud before filtering: " << icp_map->width * icp_map->height
    //     << " data points (" << pcl::getFieldsList (*icp_map) << ")." << std::endl;
    // sor.setInputCloud(icp_map);
    // sor.setLeafSize(0.5f, 0.5f, 0.5f);
    // sor.filter(*icp_map_voxel);

    //=======passthrough filter=====================
    PassThrough<PointXYZI> pass;
    pass.setInputCloud (icp_map_voxel);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0.8, 5);
    pass.filter (*icp_map_voxel);

    // CropBox<PointXYZI> crop;

    // crop.setMin(Eigen::Vector4f(-2000,-2000,1.5,1.0)); //给定立体空间
    // crop.setMax(Eigen::Vector4f(2000,2000,20,1.0));  //数据随意给的，具体情况分析
    // crop.setInputCloud(icp_map_voxel);
    // crop.setKeepOrganized(true);
    // crop.setUserFilterValue(0.1f);
    // crop.setNegative(false);
    // crop.filter(*icp_map_voxel);
    // std::cerr << "PointCloud after filtering: " << icp_map_voxel->width * icp_map_voxel->height
    //     << " data points (" << pcl::getFieldsList (*icp_map_voxel) << ")." << std::endl;

    toROSMsg(*icp_map_voxel, ros_cloud_msg);
	ros_cloud_msg.header.frame_id = "world";
    map_filtered_pub.publish(ros_cloud_msg);

    // cb.cloud2 = *icp_map;
    // cb.calculate_normals_map(0.5);
    // std::vector<int> indices;
    // pcl::removeNaNNormalsFromPointCloud(cb.cloud2_normals, cb.cloud2_normals, indices);

    // boost::shared_ptr<std::vector<int> > indicesptr (new std::vector<int> (indices));
    // pcl::ExtractIndices<PointXYZI> eifilter (true); // Initializing with true will allow us to extract the removed indices
    // eifilter.setInputCloud (cb.cloud2.makeShared());
    // eifilter.setIndices (indicesptr);
    // eifilter.filter (cb.cloud2);

    // cb.calculate_voxel_grid_map(0.8);
    // cb.calculate_SHOT_map(4);

    //ros::Duration(1).sleep();
    outputFile.open("result2.csv");
    outputFile << "id,x,y,z,yaw,pitch,roll\n";

    //pc_last_batch.reset(new PointCloud<PointXYZI>());
    //wait for gps
    std::cout << "waiting for gps" << std::endl;
    init_trans = get_initial_guess();
    std::cout << "initial guess get" << std::endl;
    std::cout << init_trans << std::endl;

    pc_sub = nh.subscribe("/lidar_points", 400, &IcpMapping::pcCallback, this);
    trans_lidar2base = get_transfrom(lidarFrame);
}

// PointCloud<PointXYZI> IcpMapping::downsample(PointCloud<PointXYZI>::Ptr &in){
//     PointCloud<PointXYZI>::Ptr pc_filtered;

//     sor.setInputCloud(in);
//     sor.setLeafSize(0.01f, 0.01f, 0.01f);
//     sor.filter(*pc_filtered);

//     return *pc_filtered;
// }

Eigen::Matrix4f IcpMapping::get_initial_guess(){
    Eigen::Matrix4f trans;
    tf::Transform transform;

    geometry_msgs::PointStampedConstPtr gps_point;
    //gps_point = ros::topic::waitForMessage<geometry_msgs::PointStamped>("/gps", nh);

    tf::Quaternion q;
    q.setRPY(0, 0, 0.09369360536);
    Eigen::Quaternionf eq(q.w(), q.x(), q.y(), q.z());
	Eigen::Matrix3f mat = eq.toRotationMatrix();

    // trans << mat(0,0), mat(0,1), mat(0,2), (*gps_point).point.x,
	// 		mat(1,0), mat(1,1), mat(1,2), (*gps_point).point.y,
	// 		mat(2,0), mat(2,1), mat(2,2), (*gps_point).point.z,
	// 		0, 0, 0, 1;

    trans << mat(0,0), mat(0,1), mat(0,2), 1774.8362265,
			mat(1,0), mat(1,1), mat(1,2), 866.367052112,
			mat(2,0), mat(2,1), mat(2,2), 0.0153020292018,
			0, 0, 0, 1;
	return trans;
}

Eigen::Matrix4f IcpMapping::get_transfrom(std::string link_name){
	tf::StampedTransform transform;
	Eigen::Matrix4f trans;

	try{
		ros::Duration five_seconds(5.0);
		listener.waitForTransform(baseFrame, link_name, ros::Time(0), five_seconds);
		listener.lookupTransform(baseFrame, link_name, ros::Time(0), transform);
	}
	catch (tf::TransformException ex){
		ROS_ERROR("%s",ex.what());
		return trans;
	}
	Eigen::Quaternionf q(transform.getRotation().getW(), \
		transform.getRotation().getX(), transform.getRotation().getY(), transform.getRotation().getZ());
	Eigen::Matrix3f mat = q.toRotationMatrix();
	trans << mat(0,0), mat(0,1), mat(0,2), transform.getOrigin().getX(),
			mat(1,0), mat(1,1), mat(1,2), transform.getOrigin().getY(),
			mat(2,0), mat(2,1), mat(2,2), transform.getOrigin().getZ(),
			0, 0, 0, 1;
	return trans;
}

void IcpMapping::pcCallback(const sensor_msgs::PointCloud2ConstPtr &pc){
	// ROS_INFO("Map to BL");
    // transformPointCloud (*icp_map_voxel, *cropped_map, init_trans.inverse());

    CropBox<PointXYZI> crop;
    // crop.setMin(Eigen::Vector4f(-30,-15,1.5,1.0)); //给定立体空间
    // crop.setMax(Eigen::Vector4f(30,15,20,1.0));  //数据随意给的，具体情况分析
    // crop.setInputCloud(cropped_map);
    // crop.setKeepOrganized(true);
    // crop.setUserFilterValue(0.1f);
    // crop.setNegative(false);
    // crop.filter(*cropped_map);

    toROSMsg(*pc_last, ros_cloud_msg);
	ros_cloud_msg.header.frame_id = "world";
    //pc_pub.publish(ros_cloud_msg);

    // transformPointCloud (*cropped_map, *cropped_map, init_trans);

    ROS_INFO("localizing");
	ros::Time timestamp = pc->header.stamp;
    fromROSMsg(*pc, *pc_input);

    transformPointCloud (*pc_input, *pc_input, trans_lidar2base);
    // if (id != 1){
    //     Eigen::Matrix4f tmp;
    //     tmp << 1, 0, 0, 10,
    //             0, 1, 0, 0,
    //             0, 0, 1, 0,
    //             0, 0, 0, 1;
    //     transformPointCloud (*pc_input, *pc_input, tmp);
    // }
    //=======cropbox filter=========================

    //crop outside
    crop.setMin(Eigen::Vector4f(-30,-40,1.2,1.0)); //给定立体空间
    crop.setMax(Eigen::Vector4f(30,20,20,1.0));  //数据随意给的，具体情况分析
    crop.setInputCloud(pc_input);
    crop.setKeepOrganized(true);
    crop.setUserFilterValue(0.1f);
    crop.setNegative(false);
    crop.filter(*pc_input);

    //crop inside
    crop.setMin(Eigen::Vector4f(-30,-3,-10,1.0)); //给定立体空间
    crop.setMax(Eigen::Vector4f(30,12,30,1.0));  //数据随意给的，具体情况分析
    crop.setInputCloud(pc_input);
    crop.setKeepOrganized(true);
    crop.setUserFilterValue(0.1f);
    crop.setNegative(true);
    crop.filter(*pc_input);

    std::cout << init_trans << std::endl;
    transformPointCloud (*pc_input, *pc_input, init_trans);

    ROS_INFO("transformed to world");

    //=======cropbox filter=========================
    // SHOT

    sor.setInputCloud(pc_input);
    sor.setLeafSize(0.5f, 0.5f, 0.5f);
    sor.filter(*pc_input_voxel);

    toROSMsg(*pc_input_voxel, ros_cloud_msg);
	ros_cloud_msg.header.frame_id = "world";
    //pc_pub.publish(ros_cloud_msg);
    //ros::Duration(1).sleep();
    // cb.cloud1 = *pc_input;
    // cb.calculate_normals_cloud(0.5);
    // std::vector<int> indices;
    // pcl::removeNaNNormalsFromPointCloud(cb.cloud1_normals, cb.cloud1_normals, indices);

    // boost::shared_ptr<std::vector<int> > indicesptr (new std::vector<int> (indices));
    // pcl::ExtractIndices<PointXYZI> eifilter (true); // Initializing with true will allow us to extract the removed indices
    // eifilter.setInputCloud (cb.cloud1.makeShared());
    // eifilter.setIndices (indicesptr);
    // eifilter.filter (cb.cloud1);

    // cb.calculate_voxel_grid_cloud(0.8);
    // cb.calculate_SHOT_cloud(4);

    // printf("inf1");
    // typename pcl::PointCloud<SHOT352>::iterator itr;
    // for (itr = cb.cloud1_shot.begin(); itr != cb.cloud1_shot.end(); itr++)
    // {
    //     std::cout << *itr << std::endl;
    // }
    // printf("inf2");

    // pcl::Correspondences corresp;
    // pcl::registration::CorrespondenceEstimation<pcl::SHOT352, pcl::SHOT352> shot_corr;
    // shot_corr.setInputSource(cb.cloud1_shot.makeShared());
    // shot_corr.setInputTarget(cb.cloud2_shot.makeShared());
    // std::cout << "Status : Calculated SHOT descriptors and finding the correspondences" << endl;
    // cout << "May take a while...depends on the feature descriptor and its support size" << endl;
    // shot_corr.determineReciprocalCorrespondences(corresp);

    // pcl::CorrespondencesConstPtr correspond = boost::make_shared< pcl::Correspondences >(corresp);

    // std::cout << "RANSAC rejections" << endl;
    // pcl::Correspondences corr;
    // pcl::registration::CorrespondenceRejectorSampleConsensus< pcl::PointXYZI > Ransac_based_Rejection;
    // Ransac_based_Rejection.setInputSource(cb.cloud1_keypoints.makeShared());
    // Ransac_based_Rejection.setInputTarget(cb.cloud2_keypoints.makeShared());
    // Ransac_based_Rejection.setInputCorrespondences(correspond);
    // Ransac_based_Rejection.getCorrespondences(corr);

    // Eigen::Matrix4f ransac_mat = Ransac_based_Rejection.getBestTransformation();
    // cout << "RANSAC based Transformation Matrix : \n" << ransac_mat << endl;

    // pcl::transformPointCloud(*pc_input, *pc_input, ransac_mat);

    // toROSMsg(*pc_input, ros_cloud_msg);
	// ros_cloud_msg.header.frame_id = "world";
    // pc_pub.publish(ros_cloud_msg);

    //=======passthrough filter=====================
    // PassThrough<PointXYZI> pass;
    // pass.setInputCloud (pc_input);
    // pass.setFilterFieldName ("z");
    // pass.setFilterLimits (-0.5, 1.5);
    // pass.filter (*pc_input);

    // toROSMsg(*pc_input_voxel, ros_cloud_msg);
	// ros_cloud_msg.header.frame_id = "world";
    // pc_pub.publish(ros_cloud_msg);

    if (icp_map_voxel->empty()){
        ROS_INFO("Empty cloud");
    }
    else{
        ROS_INFO("Aligning");
        IterativeClosestPoint<PointXYZI, PointXYZI> icp;
        if (id != 1){
            icp.setInputSource(pc_input_voxel);
            icp.setInputTarget(pc_last);
            icp.setMaximumIterations (2000);
            icp.setTransformationEpsilon (1e-10);
            icp.setRANSACOutlierRejectionThreshold (0.08);
            icp.setMaxCorrespondenceDistance (8);
            //icp.setEuclideanFitnessEpsilon (0.01);
            Eigen::Matrix4f guess;
            guess << 1, 0, 0, 0.5,
                     0, 1, 0, 0.0,
                     0, 0, 1, 0.0,
                     0, 0, 0, 1.0;
            icp.align(*pc_input_voxel, guess);
            ROS_INFO("Score: %lf", icp.getFitnessScore());
            init_trans = (icp.getFinalTransformation()) * init_trans ;
        }
        //transformPointCloud (*pc_input, *pc_input, (icp.getFinalTransformation()));
        // *icp_map += *pc_input;
        // sor.setInputCloud(icp_map_voxel);
        // sor.setLeafSize(0.2f, 0.2f, 0.2f);
        // sor.filter(*icp_map_voxel);

        //copyPointCloud(*pc_input, *pc_last);
        icp.setInputSource(pc_input_voxel);
        icp.setInputTarget(icp_map_voxel);
        icp.setMaximumIterations (2000);
        icp.setTransformationEpsilon (1e-10);
        icp.setRANSACOutlierRejectionThreshold (0.08);
        icp.setMaxCorrespondenceDistance (8);
        icp.align(*pc_input_voxel);
        ROS_INFO("Score: %lf", icp.getFitnessScore());
        init_trans = (icp.getFinalTransformation()) * init_trans ;
        std::cout << init_trans << std::endl;
        copyPointCloud(*pc_input_voxel, *pc_last);
        // Eigen::MatrixXd ICP_COV(6,6);
        // Eigen::Matrix4f final_trans = icp.getFinalTransformation();
        // ICP_COV = Eigen::MatrixXd::Zero(6,6);
        // calculate_ICP_COV(*pc_input_voxel, *icp_map_voxel, final_trans, ICP_COV);
    }
    Eigen::Matrix3f rot;
	rot << init_trans(0,0), init_trans(0,1), init_trans(0,2),
			init_trans(1,0), init_trans(1,1), init_trans(1,2),
			init_trans(2,0), init_trans(2,1), init_trans(2,2);
    Eigen::Quaternionf quat(rot);
    quat.normalize();
    Eigen::Matrix3f mats = quat.toRotationMatrix();
	init_trans << mats(0,0), mats(0,1), mats(0,2), init_trans(0,3),
			mats(1,0), mats(1,1), mats(1,2), init_trans(1,3),
			mats(2,0), mats(2,1), mats(2,2), init_trans(2,3),
			0, 0, 0, 1;

    ROS_INFO("Publishing");

    toROSMsg(*pc_input_voxel, ros_cloud_msg);
	ros_cloud_msg.header.frame_id = "world";
    pc_pub.publish(ros_cloud_msg);

    tf::Transform world_trans;
    Eigen::Affine3f affine_trans;
    affine_trans.matrix() = init_trans;
    tf::transformEigenToTF(affine_trans.cast<double>(), world_trans);
    broadcaster.sendTransform(tf::StampedTransform(world_trans, ros::Time::now(), "world", baseFrame));

    nav_msgs::Odometry odom;
    odom.header.stamp = ros::Time::now();
    odom.header.frame_id = "world";

    //set the position
    odom.pose.pose.position.x = world_trans.getOrigin().getX();
    odom.pose.pose.position.y = world_trans.getOrigin().getY();
    odom.pose.pose.position.z = world_trans.getOrigin().getZ();

    geometry_msgs::Quaternion quat_msg;
    quaternionTFToMsg(world_trans.getRotation() , quat_msg);
    odom.pose.pose.orientation = quat_msg;

    tfScalar yaw, pitch, roll;
    tf::Matrix3x3 mat(world_trans.getRotation());
    mat.getEulerYPR(yaw, pitch, roll);

    ROS_INFO("%d, %lf", id, timestamp.toSec());
    //write to csv
    outputFile << id++ << "," << odom.pose.pose.position.x << ","<< odom.pose.pose.position.y << ","<< odom.pose.pose.position.z << ","<< yaw << ","<< pitch << ","<< roll << "\n";
    std::cout << timestamp.toSec() << "," << odom.pose.pose.position.x << ","<< odom.pose.pose.position.y << ","<< odom.pose.pose.position.z << ","<< yaw << ","<< pitch << ","<< roll << std::endl;

    // if (std::abs(timestamp.toSec() - last_timestamp) <= 0.001){
    //     ROS_INFO("close: %lf, %lf",timestamp.toSec(), last_timestamp);
    //     outputFile.close();
    // }

    if (id > 396){
        ROS_INFO("close: %lf, %lf",timestamp.toSec(), last_timestamp);
        outputFile.close();
    }

    //publish the message
    odom_pub.publish(odom);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "icp_mapping");

    IcpMapping IM;

    ROS_INFO("\033[1;32m---->\033[0m Icp Mapping Started.");

    ros::spin();
    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_INCLUDE_CONSTANTS_REAL_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_INCLUDE_CONSTANTS_REAL_HPP_INCLUDED

#include <boost/simd/constant/include/constants/pi.hpp>
#include <boost/simd/constant/include/constants/pio_2.hpp>
#include <boost/simd/constant/include/constants/inf.hpp>
#include <boost/simd/constant/include/constants/nan.hpp>
#include <boost/simd/constant/include/constants/moneo_6.hpp>
#include <boost/simd/constant/include/constants/oneo_5.hpp>
#include <boost/simd/constant/include/constants/oneo_6.hpp>
#include <boost/simd/constant/include/constants/oneo_7.hpp>
#include <boost/simd/constant/include/constants/oneo_8.hpp>
#include <boost/simd/constant/include/constants/oneo_9.hpp>
#include <boost/simd/constant/include/constants/oneo_10.hpp>
#include <boost/simd/constant/include/constants/oneo_12.hpp>
#include <boost/simd/constant/include/constants/oneo_15.hpp>
#include <boost/simd/constant/include/constants/oneo_120.hpp>
#include <boost/simd/constant/include/constants/oneo_180.hpp>
#include <boost/simd/constant/include/constants/half.hpp>
#include <boost/simd/constant/include/constants/minf.hpp>
#include <boost/simd/constant/include/constants/gold.hpp>
#include <boost/simd/constant/include/constants/cgold.hpp>
#include <boost/simd/constant/include/constants/mhalf.hpp>
#include <boost/simd/constant/include/constants/mzero.hpp>
#include <boost/simd/constant/include/constants/third.hpp>
#include <boost/simd/constant/include/constants/sqrt_2.hpp>
#include <boost/simd/constant/include/constants/log10_pi.hpp>
#include <boost/simd/constant/include/constants/sqrt_1o_5.hpp>
#include <boost/simd/constant/include/constants/sqrt_2o_2.hpp>
#include <boost/simd/constant/include/constants/sqrt_2o_3.hpp>
#include <boost/simd/constant/include/constants/quarter.hpp>
#include <boost/simd/constant/include/constants/twoto31.hpp>
#include <boost/simd/constant/include/constants/twotom10.hpp>
#include <boost/simd/constant/include/constants/twotonmb.hpp>
#include <boost/simd/constant/include/constants/real_splat.hpp>
#include <boost/simd/constant/include/constants/splitfactor.hpp>

#endif


#pragma once
#ifndef EWALD_3D_HPP
#define EWALD_3D_HPP

//no bounds checking in Eigen
#define EIGEN_NO_DEBUG

//Eigen
#include <Eigen/Dense>
//structure
#include "src/struc/structure_fwd.hpp"
//serialization
#include "src/mem/serialize.hpp"

#ifndef EWALD_PRINT_FUNC
#define EWALD_PRINT_FUNC 0
#endif

#ifndef EWALD_PRINT_STATUS
#define EWALD_PRINT_STATUS 0
#endif

#ifndef EWALD_PRINT_DATA
#define EWALD_PRINT_DATA 0
#endif

namespace Ewald3D{

//**********************************************************************************************************
//Utility Class
//**********************************************************************************************************

class Utility{
protected:
	//calculation parameters
	double prec_;//precision for the calculation
	double rMax_;//the maximum length of lattice vector included in the real-space sum
	double kMax_;//the maximum length of the lattice vector included in the reciprocal-space sum
	double alpha_;//the integral cutoff separating the real- and reciprocal-space sums
	double weight_;//weighting of real space calculations

	//unit cell
	std::vector<Eigen::Vector3d> R;//the lattice vectors to sum over
	std::vector<Eigen::Vector3d> K;//the reciprocal lattice vectors to sum over

	//electrostatics
	double eps_;//the dielectric constant of the outer material
public:
	Utility(){defaults();}
	Utility(double prec){init(prec);}
	Utility(const Utility& u);
	~Utility(){}

	//operators
	friend std::ostream& operator<<(std::ostream& out, const Utility& u);

	//access
	double prec()const{return prec_;}
	double rMax()const{return rMax_;}
	double kMax()const{return kMax_;}
	double alpha()const{return alpha_;}
	const double& eps()const{return eps_;}
	double& eps(){return eps_;}
	const double& weight()const{return weight_;}
	double& weight(){return weight_;}

	//member functions
	void defaults();
	void clear(){defaults();}
	void init(double prec);
};

//**********************************************************************************************************
//Coulomb Class
//**********************************************************************************************************

class Coulomb: public Utility{
private:
	//calculation parameters
	double vSelfR_,vSelfK_,vSelfC_;//the self-interaction strength for an ion in a periodic lattice

	//unit cell
	std::vector<double> kAmp;//the reciprocal space sum amplitudes
public:
	Coulomb(){defaults();}
	Coulomb(Structure& struc, double prec){init(struc,prec);}
	~Coulomb(){}

	//operators
	friend std::ostream& operator<<(std::ostream& out, const Coulomb& c);

	//access
	double phisR()const{return vSelfR_;}
	double phisK()const{return vSelfK_;}
	double phisC()const{return vSelfC_;}

	//member functions
	void defaults();
	void clear(){defaults();}
	void init(const Structure& struc, double prec);
	void init_alpha(const Structure& struc, double prec=0);

	//calculation - energy
	double energy(const Structure& struc)const;
	double energy_single(const Structure& struc);
	double energy_brute(const Structure& struc, int N)const;

	//calculation - potential
	double phi(const Structure& struc, const Eigen::Vector3d& dr)const;
	double phi(const Structure& struc, int nn)const;
	double phis()const;
	double potentialBrute(const Structure& struc, int n, int N)const;

	//calculation - electric field
	Eigen::Vector3d& efield(const Structure& struc, int n, Eigen::Vector3d&)const;
	Eigen::Vector3d& efieldBrute(const Structure& struc, int n, Eigen::Vector3d&, int N)const;
};

}

namespace serialize{

//**********************************************
// byte measures
//**********************************************

template <> int nbytes(const Ewald3D::Utility& obj);
template <> int nbytes(const Ewald3D::Coulomb& obj);

//**********************************************
// packing
//**********************************************

template <> int pack(const Ewald3D::Utility& obj, char* arr);
template <> int pack(const Ewald3D::Coulomb& obj, char* arr);

//**********************************************
// unpacking
//**********************************************

template <> int unpack(Ewald3D::Utility& obj, const char* arr);
template <> int unpack(Ewald3D::Coulomb& obj, const char* arr);

}

#endif


#include <pybind11/pybind11.h>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <unordered_map>
#include <vector>
#include <string>
#include "gurobi_c++.h"
#include <pybind11/stl.h>
#include <ctime>
#include <tuple>
#include <chrono>
#include <boost/functional/hash.hpp>

namespace py = pybind11;
using namespace pybind11::literals;
using namespace std::chrono;

// source aggregation formulation (SF)
std::tuple<bool, int> check_sf(py::object graph, py::list failures, py::dict traffic_mattrix, py::dict node_stub, py::float_ load_factor){
    // Create an environment
    GRBEnv env = GRBEnv();

    // Create an empty model
    GRBModel model = GRBModel(env);
    int opt_cnt  = 0;
    py::object g = py::module::import("networkx").attr("DiGraph");
    g = graph;
    std::unordered_map<std::string, double> node_srctraffic;
    std::unordered_map<std::string, double> node_dsttraffic;
    std::unordered_map<std::string, std::vector<GRBVar>> node_intraffic_map_vars;
    std::unordered_map<std::string, std::vector<GRBVar>> node_outtraffic_map_vars;
    std::unordered_map<std::string, std::vector<GRBLinExpr>> link_map_vars;

    std::unordered_map<std::string, std::pair<double, GRBLinExpr>> original_link_capacity;
    std::unordered_map<std::string, bool> l3node_map_stub;
    std::vector<GRBConstr> capacity_constr;

    for (auto item : node_stub) {
        l3node_map_stub[std::string(py::str(item.first))] = item.second.cast<bool>();
    };

    for(auto & src : traffic_mattrix){
        std::string flow_src = std::string(py::str(src.first));
        float src_agg_flow_size = 0;
        node_dsttraffic.clear();
        node_intraffic_map_vars.clear();
        node_outtraffic_map_vars.clear();

        for(auto dst: py::dict(py::reinterpret_borrow<py::object>(src.second))){
            src_agg_flow_size += stod(std::string(py::str(dst.second)));
            node_dsttraffic[std::string(py::str(dst.first))] = stod(std::string(py::str(dst.second)));
        }
        for(auto & edge_it: py::list(g.attr("edges"))){
            py::tuple edge = py::tuple(py::reinterpret_borrow<py::object>(edge_it));
            std::string src_str = std::string(py::str(edge[0]));
            std::string dst_str = std::string(py::str(edge[1]));
            std::string link_name = src_str + "_" + dst_str;
            std::string var_name = flow_src + "flow_src" + link_name;

            GRBVar lp_var = model.addVar(0.0, GRB_INFINITY, 0.0, GRB_CONTINUOUS, var_name);
            node_outtraffic_map_vars[src_str].push_back(lp_var);
            node_intraffic_map_vars[dst_str].push_back(lp_var);
            link_map_vars[link_name].push_back(lp_var);
        }
        for(auto & node: py::list(g.attr("nodes"))){
            std::string node_str = std::string(py::str(node));
            GRBLinExpr intraffic_sum = 0;
            GRBLinExpr outtraffic_sum = 0;
            for(auto & var: node_intraffic_map_vars[node_str]){
                intraffic_sum += var;
            }
            for(auto & var: node_outtraffic_map_vars[node_str]){
                outtraffic_sum += var;
            }
            if (node_str == flow_src) {
                // the src node, outtraffic_sum = src_agg_flow_size
                model.addConstr(outtraffic_sum == src_agg_flow_size);
            }
            else if (node_dsttraffic.find(node_str) != node_dsttraffic.end()){
                // dst node for some flow
                if (l3node_map_stub[node_str]) {
                    // stub node, avoid go-through traffic
                    model.addConstr(intraffic_sum == node_dsttraffic[node_str]);
                    model.addConstr(outtraffic_sum == 0);
                }
                else {
                    // non stub node, perimit go-through traffic
                    outtraffic_sum += node_dsttraffic[node_str];
                    model.addConstr(intraffic_sum == outtraffic_sum);
                }
            }
            else {
                // not src node, nor dst node for any flow
                // the go-through node
                if (l3node_map_stub[node_str]) {
                    // stub node, avoid go-through traffic
                    model.addConstr(intraffic_sum == 0);
                    model.addConstr(outtraffic_sum == 0);
                }
                else {
                    // non stub node, perimit go-through traffic
                    model.addConstr(intraffic_sum == outtraffic_sum);
                }
            }
        }
    }

    // add link capacity constraints
    for(auto & edge_it: py::list(g.attr("edges"))){
        py::tuple edge = py::tuple(py::reinterpret_borrow<py::object>(edge_it));
        std::string link_name = std::string(py::str(edge[0])) + "_" + std::string(py::str(edge[1]));
        GRBLinExpr link_traffic_aggr = 0;
        for(auto & var: link_map_vars[link_name]) {
            link_traffic_aggr += var;
        }
        py::object get_edge_data = g.attr("get_edge_data");
        py::dict temp = get_edge_data(*edge);

        int link_capa = stod(std::string(py::str(temp["capacity"])));
        double link_capa_adjust = link_capa * float(load_factor);
        model.addConstr(link_traffic_aggr <= link_capa_adjust);
        original_link_capacity[link_name] = std::make_pair(link_capa_adjust, link_traffic_aggr);
    }

    // adjust link capacity  constraints to failures
    for(auto & map_iter : failures){
        py::dict map = py::dict(py::reinterpret_borrow<py::object>(map_iter));
        while(!capacity_constr.empty()){
            model.remove(capacity_constr[capacity_constr.size()-1]);
            capacity_constr.pop_back();
        }
        model.reset();
        for(auto & src : map){
            for(auto & dst : py::dict(py::reinterpret_borrow<py::object>(src.second))){
                std::string src_dst = std::string(py::str(src.first)) + "_" + std::string(py::str(dst.first));
                std::string dst_src = std::string(py::str(dst.first)) + "_" + std::string(py::str(src.first));
                int capa_delta = stod(std::string(py::str(dst.second)));
                double capa_delta_adjust = capa_delta * float(load_factor);
                capacity_constr.push_back(model.addConstr(original_link_capacity[src_dst].second <= original_link_capacity[src_dst].first - capa_delta_adjust));
                capacity_constr.push_back(model.addConstr(original_link_capacity[dst_src].second <= original_link_capacity[dst_src].first - capa_delta_adjust));
            }
        }
        model.optimize();
        opt_cnt++;

        if(model.get(GRB_IntAttr_Status) != GRB_OPTIMAL) {
            return std::make_tuple(false, opt_cnt);
        }
    }
    return std::make_tuple(true, opt_cnt);
}

// flow formulation (FF)
std::tuple<bool, int> check(py::object graph, py::list failures, py::dict traffic_mattrix, py::dict node_stub, py::float_ load_factor){
    // Create an environment
    GRBEnv env = GRBEnv();

    // Create an empty model
    GRBModel model = GRBModel(env);
    int opt_cnt  = 0;
    py::object g = py::module::import("networkx").attr("DiGraph");
    g = graph;
    std::unordered_map<std::string, double> node_srctraffic;
    std::unordered_map<std::string, double> node_dsttraffic;
    std::unordered_map<std::string, std::vector<GRBVar>> node_intraffic_map_vars;
    std::unordered_map<std::string, std::vector<GRBVar>> node_outtraffic_map_vars;
    std::unordered_map<std::string, std::vector<GRBLinExpr>> link_map_vars;

    std::unordered_map<std::string, std::pair<double, GRBLinExpr>> original_link_capacity;
    std::unordered_map<std::string, bool> l3node_map_stub;
    std::vector<GRBConstr> capacity_constr;

    for (auto item : node_stub) {
        l3node_map_stub[std::string(py::str(item.first))] = item.second.cast<bool>();
    };

    int flow_idx = 0;
    for(auto & src : traffic_mattrix){
        for(auto dst: py::dict(py::reinterpret_borrow<py::object>(src.second))){
            std::string flow_src = std::string(py::str(src.first));
            std::string flow_dst = std::string(py::str(dst.first));
            int flow_size = stod(std::string(py::str(dst.second)));
            node_intraffic_map_vars.clear();
            node_outtraffic_map_vars.clear();
            for(auto & edge_it: py::list(g.attr("edges"))){
                py::tuple edge = py::tuple(py::reinterpret_borrow<py::object>(edge_it));
                std::string src_str = std::string(py::str(edge[0]));
                std::string dst_str = std::string(py::str(edge[1]));
                std::string link_name = src_str + "_" + dst_str;
                std::string var_name = std::to_string(flow_idx) + link_name;

                GRBVar lp_var = model.addVar(0.0, GRB_INFINITY, 0.0, GRB_CONTINUOUS, var_name);
                node_outtraffic_map_vars[src_str].push_back(lp_var);
                node_intraffic_map_vars[dst_str].push_back(lp_var);
                link_map_vars[link_name].push_back(lp_var);
            }
            for(auto & node: py::list(g.attr("nodes"))){
                std::string node_str = std::string(py::str(node));
                GRBLinExpr intraffic_sum = 0;
                GRBLinExpr outtraffic_sum = 0;
                for(auto & var: node_intraffic_map_vars[node_str]){
                    intraffic_sum += var;
                }
                for(auto & var: node_outtraffic_map_vars[node_str]){
                    outtraffic_sum += var;
                }

                if (node_str == flow_src) {
                    // the src node, outtraffic_sum = flow_size
                    model.addConstr(outtraffic_sum == flow_size);
                    model.addConstr(intraffic_sum == 0);
                }
                else if (node_str == flow_dst) {
                    // the dst node, intraffic_sum = flow_size
                    model.addConstr(intraffic_sum == flow_size);
                    model.addConstr(outtraffic_sum == 0);
                }
                else {
                    // the go-through node
                    if (l3node_map_stub[node_str]) {
                        // stub node, avoid go-through traffic
                        model.addConstr(intraffic_sum == 0);
                        model.addConstr(outtraffic_sum == 0);
                    }
                    else {
                        // non stub node, perimit go-through traffic
                        model.addConstr(intraffic_sum == outtraffic_sum);
                    }
                }
            }
            flow_idx += 1;
        }
    }

    // add link capacity constraints
    for(auto & edge_it: py::list(g.attr("edges"))){
        py::tuple edge = py::tuple(py::reinterpret_borrow<py::object>(edge_it));
        std::string link_name = std::string(py::str(edge[0])) + "_" + std::string(py::str(edge[1]));
        GRBLinExpr link_traffic_aggr = 0;
        for(auto & var: link_map_vars[link_name]) {
            link_traffic_aggr += var;
        }
        py::object get_edge_data = g.attr("get_edge_data");
        py::dict temp = get_edge_data(*edge);

        int link_capa = stod(std::string(py::str(temp["capacity"])));
        double link_capa_adjust = link_capa * float(load_factor);
        model.addConstr(link_traffic_aggr <= link_capa_adjust);
        original_link_capacity[link_name] = std::make_pair(link_capa_adjust, link_traffic_aggr);
    }

    // adjust link capacity  constraints to failures
    for(auto & map_iter : failures){
        py::dict map = py::dict(py::reinterpret_borrow<py::object>(map_iter));
        while(!capacity_constr.empty()){
            model.remove(capacity_constr[capacity_constr.size()-1]);
            capacity_constr.pop_back();
        }
        model.reset();
        for(auto & src : map){
            for(auto & dst : py::dict(py::reinterpret_borrow<py::object>(src.second))){
                std::string src_dst = std::string(py::str(src.first)) + "_" + std::string(py::str(dst.first));
                std::string dst_src = std::string(py::str(dst.first)) + "_" + std::string(py::str(src.first));
                int capa_delta = stod(std::string(py::str(dst.second)));
                double capa_delta_adjust = capa_delta * float(load_factor);
                capacity_constr.push_back(model.addConstr(original_link_capacity[src_dst].second <= original_link_capacity[src_dst].first - capa_delta_adjust));
                capacity_constr.push_back(model.addConstr(original_link_capacity[dst_src].second <= original_link_capacity[dst_src].first - capa_delta_adjust));
            }
        }
        model.optimize();
        opt_cnt++;

        if(model.get(GRB_IntAttr_Status) != GRB_OPTIMAL) {
            return std::make_tuple(false, opt_cnt);
        }
    }
    return std::make_tuple(true, opt_cnt);
}

std::tuple<int, int, std::unordered_map<int, int>> ilp_solve_c(py::object graph, py::list failures, py::dict steady_tm, py::dict failures_tm, py::dict fibers,
    py::dict node_stub, py::float_ load_factor, unsigned int step_size, long int timelimit, py::float_ mipgapabs){

    // Create an environment
    GRBEnv env = GRBEnv();
    if (timelimit > 0) {
        env.set(GRB_DoubleParam_TimeLimit, timelimit);
    }
    env.set(GRB_DoubleParam_MIPGapAbs, float(mipgapabs));

    // Create an empty model
    GRBModel model = GRBModel(env);
    model.set(GRB_IntParam_DualReductions, 0);
    GRBLinExpr obj = 0.0;

    py::object g = py::module::import("networkx").attr("Graph");
    g = graph;
    std::unordered_map<std::string, double> node_srctraffic;
    std::unordered_map<std::string, double> node_dsttraffic;
    std::unordered_map<std::string, std::vector<GRBVar>> node_intraffic_map_vars;
    std::unordered_map<std::string, std::vector<GRBVar>> node_outtraffic_map_vars;
    std::unordered_map<std::string, std::vector<GRBLinExpr>> node_pair_map_traffic_vars;

    std::unordered_map<std::string, std::pair<double, GRBLinExpr>> original_link_capacity;
    std::unordered_map<std::string, bool> l3node_map_stub;
    std::vector<GRBConstr> capacity_constr;

    std::unordered_map<std::string, GRBVar> link_map_var;
    std::unordered_map<std::pair<std::string, std::string>, std::vector<std::pair<int, GRBVar>>, boost::hash<std::pair<std::string, std::string>>> node_pair_map_capa_vars;
    std::unordered_map<std::string, std::vector<std::pair<int, GRBVar>>> lease_capa_map_capa_vars;
    std::unordered_map<std::string, std::vector<std::tuple<float, int, GRBVar>>> fiber_spectrum_map_capa_vars;

    // <fiber_name, <lease_flag, max_bw, max_spectrum>>
    std::unordered_map<std::string, std::tuple<bool,int, int>> fiber_map_max_cstr;

    std::set<std::pair<std::string, std::string>> complete_node_pair;

    // handle stub node
    for (auto item : node_stub) {
        l3node_map_stub[std::string(py::str(item.first))] = item.second.cast<bool>();
    };

    // parse fiber max_constraints
    for (auto fiber_item : fibers) {
        std::string fiber_name = std::string(py::str(fiber_item.first));
        py::tuple tuple_py = py::reinterpret_borrow<py::object>(fiber_item.second);
        bool lease_flag = tuple_py[0].cast<bool>();
        int max_bw = tuple_py[1].cast<int>();
        int max_spectrum = tuple_py[2].cast<int>();
        fiber_map_max_cstr[fiber_name] = std::make_tuple(lease_flag, max_bw, max_spectrum);
    }

    // set the relationship between l3 link capas and (src, dst) bandwidth
    // also record the capa or spectrum of underlying fibers
    py::object get_edge_data = g.attr("get_edge_data");
    for(auto & edge_it: py::list(g.attr("edges"))){
        py::tuple edge = py::tuple(py::reinterpret_borrow<py::object>(edge_it));
        py::dict temp = get_edge_data(*edge);

        int link_capa = stod(std::string(py::str(temp["capacity"])));
        int max_delta_step = stod(std::string(py::str(temp["max_delta_step"])));

        std::string link_name = std::string(py::str(temp["name"]));
        GRBVar link_delta_var = model.addVar(0, GRB_INFINITY, 0, GRB_INTEGER, link_name);
        link_map_var[link_name] = link_delta_var;
        std::string src_str = std::string(py::str(edge[0]));
        std::string dst_str = std::string(py::str(edge[1]));

        node_pair_map_capa_vars[std::make_pair(src_str, dst_str)].push_back(std::make_pair(link_capa,link_delta_var));
        node_pair_map_capa_vars[std::make_pair(dst_str, src_str)].push_back(std::make_pair(link_capa,link_delta_var));

        complete_node_pair.insert(std::make_pair(src_str, dst_str));
        complete_node_pair.insert(std::make_pair(dst_str, src_str));

        if (max_delta_step >= 0) {
            model.addConstr(link_delta_var <= max_delta_step);
        }

        int link_cost = stod(std::string(py::str(temp["cost"])));
        obj += link_cost * step_size * link_delta_var;
        // fiber constraints related
        for (auto & fiber_item : py::dict(temp["fiber_map_spectrum"])) {
            std::string fiber_name = std::string(py::str(fiber_item.first));
            float spectrum_eff = std::stod(std::string(py::str(fiber_item.second)));
            if (std::get<0>(fiber_map_max_cstr[fiber_name])) {
                //lease_flag = true; check the max_capa in Leases
                lease_capa_map_capa_vars[fiber_name].push_back(std::make_pair(link_capa, link_delta_var));
            }
            else {
                //lease_flag = false; check the spectrum in Fibers
                fiber_spectrum_map_capa_vars[fiber_name].push_back(std::make_tuple(spectrum_eff, link_capa, link_delta_var));
            }
        }
    }

    // add lease capa constraints
    for (auto it : lease_capa_map_capa_vars) {
        std::string lease_name = it.first;
        int max_capa = std::get<1>(fiber_map_max_cstr[lease_name]);
        GRBLinExpr lease_capa = 0;
        for (auto & var: it.second) {
            lease_capa += var.first;
            lease_capa += step_size*var.second;
        }

        model.addConstr(lease_capa <= max_capa);
    }

    // add fiber spectrum constraints
    for (auto it : fiber_spectrum_map_capa_vars) {
        std::string fiber_name = it.first;
        int max_spectrum = std::get<2>(fiber_map_max_cstr[fiber_name]);
        GRBLinExpr fiber_spectrum = 0;
        for (auto & var: it.second) {
            fiber_spectrum += std::get<1>(var)*std::get<0>(var);
            fiber_spectrum += std::get<2>(var)*std::get<0>(var)*step_size;
        }
        model.addConstr(fiber_spectrum <= max_spectrum);
    }

    /***
    add the constraints for steady state
    ***/
    // in-out balance for source-aggr formulation
    node_pair_map_traffic_vars.clear();
    for(auto & src : steady_tm){
        std::string flow_src = std::string(py::str(src.first));
        float src_agg_flow_size = 0;
        node_dsttraffic.clear();
        node_intraffic_map_vars.clear();
        node_outtraffic_map_vars.clear();

        for(auto dst: py::dict(py::reinterpret_borrow<py::object>(src.second))){
            src_agg_flow_size += stod(std::string(py::str(dst.second)));
            node_dsttraffic[std::string(py::str(dst.first))] = stod(std::string(py::str(dst.second)));
        }

        for(auto node_pair : complete_node_pair) {
            std::string src_str = node_pair.first;
            std::string dst_str = node_pair.second;
            std::string node_pair_name = src_str + "_" + dst_str;
            std::string var_name = "steady" + flow_src + "flow_src" + node_pair_name;

            GRBVar lp_var = model.addVar(0.0, GRB_INFINITY, 0.0, GRB_CONTINUOUS, var_name);
            node_outtraffic_map_vars[src_str].push_back(lp_var);
            node_intraffic_map_vars[dst_str].push_back(lp_var);
            node_pair_map_traffic_vars[node_pair_name].push_back(lp_var);
        }
        for(auto & node: py::list(g.attr("nodes"))){
            std::string node_str = std::string(py::str(node));
            GRBLinExpr intraffic_sum = 0;
            GRBLinExpr outtraffic_sum = 0;
            for(auto & var: node_intraffic_map_vars[node_str]){
                intraffic_sum += var;
            }
            for(auto & var: node_outtraffic_map_vars[node_str]){
                outtraffic_sum += var;
            }
            if (node_str == flow_src) {
                // the src node, outtraffic_sum = src_agg_flow_size
                model.addConstr(outtraffic_sum == src_agg_flow_size);
            }
            else if (node_dsttraffic.find(node_str) != node_dsttraffic.end()){
                // dst node for some flow
                if (l3node_map_stub[node_str]) {
                    // stub node, avoid go-through traffic
                    model.addConstr(intraffic_sum == node_dsttraffic[node_str]);
                    model.addConstr(outtraffic_sum == 0);
                }
                else {
                    // non stub node, perimit go-through traffic
                    outtraffic_sum += node_dsttraffic[node_str];
                    model.addConstr(intraffic_sum == outtraffic_sum);
                }
            }
            else {
                // not src node, nor dst node for any flow
                // the go-through node
                if (l3node_map_stub[node_str]) {
                    // stub node, avoid go-through traffic
                    model.addConstr(intraffic_sum == 0);
                    model.addConstr(outtraffic_sum == 0);
                }
                else {
                    // non stub node, perimit go-through traffic
                    model.addConstr(intraffic_sum == outtraffic_sum);
                }
            }
        }
    }

    // add link capacity constraints
    for(auto node_pair : node_pair_map_capa_vars) {
        std::string src_str = node_pair.first.first;
        std::string dst_str = node_pair.first.second;
        std::string link_name = src_str + "_" + dst_str;
        GRBLinExpr link_traffic_aggr = 0;
        GRBLinExpr link_capa_aggr = 0;
        for(auto & var: node_pair_map_traffic_vars[link_name]) {
            link_traffic_aggr += var;
        }

        for(auto & var: node_pair_map_capa_vars[std::make_pair(src_str, dst_str)]) {
            // add init capa
            link_capa_aggr += int(var.first*float(load_factor));

            // add capa vars
            link_capa_aggr += int(step_size*float(load_factor))*var.second;
        }
        model.addConstr(link_traffic_aggr <= link_capa_aggr);
    }

    /***
    add the constraints for each failure
    ***/
    int failure_idx = -1;
    for(auto & list_iter : failures){
        failure_idx += 1;
        // in-out balance for source-aggr formulation
        node_pair_map_traffic_vars.clear();
        for(auto & src : failures_tm){
            std::string flow_src = std::string(py::str(src.first));
            float src_agg_flow_size = 0;
            node_dsttraffic.clear();
            node_intraffic_map_vars.clear();
            node_outtraffic_map_vars.clear();

            for(auto dst: py::dict(py::reinterpret_borrow<py::object>(src.second))){
                src_agg_flow_size += stod(std::string(py::str(dst.second)));
                node_dsttraffic[std::string(py::str(dst.first))] = stod(std::string(py::str(dst.second)));
            }

            for(auto node_pair : complete_node_pair) {
                std::string src_str = node_pair.first;
                std::string dst_str = node_pair.second;
                std::string node_pair_name = src_str + "_" + dst_str;
                std::string var_name = "spof" + std::to_string(failure_idx) + flow_src + "flow_src" + node_pair_name;
                GRBVar lp_var = model.addVar(0.0, GRB_INFINITY, 0.0, GRB_CONTINUOUS, var_name);
                node_outtraffic_map_vars[src_str].push_back(lp_var);
                node_intraffic_map_vars[dst_str].push_back(lp_var);
                node_pair_map_traffic_vars[node_pair_name].push_back(lp_var);
            }
            for(auto & node: py::list(g.attr("nodes"))){
                std::string node_str = std::string(py::str(node));
                GRBLinExpr intraffic_sum = 0;
                GRBLinExpr outtraffic_sum = 0;
                for(auto & var: node_intraffic_map_vars[node_str]){
                    intraffic_sum += var;
                }
                for(auto & var: node_outtraffic_map_vars[node_str]){
                    outtraffic_sum += var;
                }
                if (node_str == flow_src) {
                    // the src node, outtraffic_sum = src_agg_flow_size
                    model.addConstr(outtraffic_sum == src_agg_flow_size);
                }
                else if (node_dsttraffic.find(node_str) != node_dsttraffic.end()){
                    // dst node for some flow
                    if (l3node_map_stub[node_str]) {
                        // stub node, avoid go-through traffic
                        model.addConstr(intraffic_sum == node_dsttraffic[node_str]);
                        model.addConstr(outtraffic_sum == 0);
                    }
                    else {
                        // non stub node, perimit go-through traffic
                        outtraffic_sum += node_dsttraffic[node_str];
                        model.addConstr(intraffic_sum == outtraffic_sum);
                    }
                }
                else {
                    // not src node, nor dst node for any flow
                    // the go-through node
                    if (l3node_map_stub[node_str]) {
                        // stub node, avoid go-through traffic
                        model.addConstr(intraffic_sum == 0);
                        model.addConstr(outtraffic_sum == 0);
                    }
                    else {
                        // non stub node, perimit go-through traffic
                        model.addConstr(intraffic_sum == outtraffic_sum);
                    }
                }
            }
        }

        std::vector<std::string> failed_links_list = py::list(py::reinterpret_borrow<py::object>(list_iter)).cast<std::vector<std::string>>();

        // recompute node_pair_map_capa_vars because of the failure
        node_pair_map_capa_vars.clear();
        for(auto & edge_it: py::list(g.attr("edges"))){
            py::tuple edge = py::tuple(py::reinterpret_borrow<py::object>(edge_it));
            py::dict temp = get_edge_data(*edge);

            int link_capa = stod(std::string(py::str(temp["capacity"])));

            std::string link_name = std::string(py::str(temp["name"]));

            std::string src_str = std::string(py::str(edge[0]));
            std::string dst_str = std::string(py::str(edge[1]));

            if(std::find(failed_links_list.begin(), failed_links_list.end(), link_name) == failed_links_list.end()) {
                // not in the failed link vector
                node_pair_map_capa_vars[std::make_pair(src_str, dst_str)].push_back(std::make_pair(link_capa,link_map_var[link_name]));
                node_pair_map_capa_vars[std::make_pair(dst_str, src_str)].push_back(std::make_pair(link_capa,link_map_var[link_name]));
            }
        }

        // add link capacity constraints
        for(auto node_pair : complete_node_pair) {
            std::string src_str = node_pair.first;
            std::string dst_str = node_pair.second;
            std::string link_name = src_str + "_" + dst_str;
            GRBLinExpr link_traffic_aggr = 0;
            GRBLinExpr link_capa_aggr = 0;
            for(auto & var: node_pair_map_traffic_vars[link_name]) {
                link_traffic_aggr += var;
            }

            if (node_pair_map_capa_vars.find(std::make_pair(src_str, dst_str)) != node_pair_map_capa_vars.end()) {
                // the node_pair that has capacity under failure
                for(auto & var: node_pair_map_capa_vars[std::make_pair(src_str, dst_str)]) {
                    // add init capa
                    link_capa_aggr += int(var.first*float(load_factor));

                    // add capa vars
                    link_capa_aggr += int(step_size*float(load_factor))*var.second;
                }
            }
            model.addConstr(link_traffic_aggr <= link_capa_aggr);
        }
    }
    model.setObjective(obj, GRB_MINIMIZE);
    model.optimize();
    int sum_delta_capa = 0;
    std::unordered_map<int, int> opt_sol;
    if(model.get(GRB_IntAttr_Status) == GRB_OPTIMAL) {
        std::cout << "Obj: " << model.get(GRB_DoubleAttr_ObjVal) << std::endl;
        std::string sol_dict = "{";
        // print the solution
        for(auto & edge_it: py::list(g.attr("edges"))) {
            py::tuple edge = py::tuple(py::reinterpret_borrow<py::object>(edge_it));
            py::dict temp = get_edge_data(*edge);
            std::string link_name = std::string(py::str(temp["name"]));

            int link_capa = stod(std::string(py::str(temp["capacity"])));
            int link_idx = stod(std::string(py::str(temp["idx"])));
            int var_value = round(link_map_var[link_name].get(GRB_DoubleAttr_X));
            if (var_value > 0) {
                sum_delta_capa += step_size*var_value;
                opt_sol[link_idx] = var_value;
            }
            int link_final_capa = link_capa + step_size*var_value;
            sol_dict += "\"" + link_name + "\":" + std::to_string(link_final_capa) + ", ";
        }
        sol_dict = sol_dict.substr(0, sol_dict.size()-2) + "}";
        std::ofstream ilp_sol("ilp_sol.txt");
        ilp_sol << sol_dict;
        ilp_sol.close();
        std::cout << "sum_delta_capa: " << sum_delta_capa << std::endl;
        return std::make_tuple(model.get(GRB_DoubleAttr_ObjVal), sum_delta_capa, opt_sol);
    }
    else {
        std::cout << "don't get the GRB_OPTIMAL:" << model.get(GRB_IntAttr_Status) <<std::endl;
        return std::make_tuple(-1, sum_delta_capa, opt_sol);
    }
}

PYBIND11_MODULE(gurobi_c, m) {
    m.def("check_sf", &check_sf, "Checks if traffic matrix is feasible under topology", py::arg("g"), py::arg("failures"), py::arg("tm"), py::arg("stub"), py::arg("load_factor"));
    m.def("check", &check, "Checks if traffic matrix is feasible under topology", py::arg("g"), py::arg("failures"), py::arg("tm"), py::arg("stub"), py::arg("load_factor"));
    m.def("ilp_solve_c", &ilp_solve_c, "ilp solver", py::arg("g"), py::arg("failures"), py::arg("steady_tm"), py::arg("failures_tm"),
        py::arg("fibers"), py::arg("stub"), py::arg("load_factor"), py::arg("step_size"), py::arg("timelimit"), py::arg("mipabsgap"));
}


#define DEBUG 1
/**
 * File    : E.cpp
 * Author  : Kazune Takahashi
 * Created : 11/22/2019, 4:12:54 PM
 * Powered by Visual Studio Code
 */
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>
#include <string>
#include <complex>
#include <tuple>
#include <queue>
#include <stack>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <bitset>
#include <functional>
#include <random>
#include <chrono>
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
// ----- boost -----
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using boost::rational;
using namespace std;
using ll = long long;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{3000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{x % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
ll gcd(ll x, ll y) { return y ? gcd(y, x % y) : x; }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}
// ----- main() -----

int N;
vector<string> S;
ll dp[40][40][40][40];
ll dp2[40][40][40][40];

bool connectable(int i, int j)
{
  return S[i][j] == '1';
}

ll calc(int A, int B, int C, int D);
ll calc_unit(int A, int B, int C, int D);
void flush();

void flush()
{
  ll res{0LL};
  if (N >= 2)
  {
    for (auto i = 2; i <= 2 * N - 2; i++)
    {
      if (connectable(0, i))
      {
        res += calc(1, i - 1, i + 1, 2 * N - 1);
      }
    }
  }
  else if (connectable(0, 1))
  {
    res = 1;
  }
  else
  {
    res = 0;
  }
  cout << res << endl;
}

ll calc(int A, int B, int C, int D)
{
  if (dp2[A][B][C][D] >= 0)
  {
    return dp2[A][B][C][D];
  }
  if (A == B || C == D)
  {
    return dp2[A][B][C][D] = calc_unit(A, B, C, D);
  }
  ll res{0LL};
  for (auto i = A + 1; i <= B; i++)
  {
    for (auto j = C; j <= D - 1; j++)
    {
      res += calc(A, i - 1, j + 1, D) * calc_unit(i, B, C, j);
    }
  }
  res += calc_unit(A, B, C, D);
  return dp2[A][B][C][D] = res;
}

ll calc_unit(int A, int B, int C, int D)
{
  if (dp[A][B][C][D] >= 0)
  {
    return dp[A][B][C][D];
  }
  if (A == B && C == D)
  {
    return dp[A][B][C][D] = (connectable(A, C) ? 1 : 0);
  }
  ll res{0LL};
  if (A == B)
  {
    for (auto j = C + 1; j <= D - 1; j++)
    {
      if (connectable(A, j))
      {
        res += calc(C, j - 1, j + 1, D);
      }
    }
  }
  else if (C == D)
  {
    for (auto i = A + 1; i <= B - 1; i++)
    {
      if (connectable(i, C))
      {
        res += calc(A, i - 1, i + 1, B);
      }
    }
  }
  else
  {
    for (auto i = A + 1; i <= B - 1; i++)
    {
      for (auto j = C + 1; j <= D - 1; j++)
      {
        if (connectable(i, j))
        {
          res += calc(A, i - 1, i + 1, B) * calc(C, j - 1, j + 1, D);
        }
      }
    }
  }
  return dp[A][B][C][D] = res;
}

int main()
{
  cin >> N;
  S.resize(2 * N);
  for (auto i = 0; i < 2 * N; i++)
  {
    cin >> S[i];
  }
  fill(&dp[0][0][0][0], &dp[0][0][0][0] + 40 * 40 * 40 * 40, -1);
  fill(&dp2[0][0][0][0], &dp2[0][0][0][0] + 40 * 40 * 40 * 40, -1);
  flush();
}