// ---------------------------------------------------------------------
//
// Copyright (c) 2016 - 2021 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Taylor Couette flow
// See Sec. 4.2 of "An immersed interface method for discrete surfaces"
// by Ebrahim M. Kolahdouz et al., Journal of Computational Physics 400 (2020) 108854
// To get the right convergence results this code was run with tight Stokes solver tolerance
// with the command line flag: -stokes_ksp_rtol 1.0e-10 -ksp_rtol 1.0e-10

// This code was tested against the commit 57fb379454ea3f8f50c476f10226cb6b520a11a0
// which is currently on branch iim-1 at https://github.com/drwells/IBAMR

// Headers for basic PETSc functions
#include <petscsys.h>

// Headers for basic SAMRAI objects
#include <BergerRigoutsos.h>
#include <CartesianGridGeometry.h>
#include <LoadBalancer.h>
#include <StandardTagAndInitialize.h>

// Headers for basic libMesh objects
#include <libmesh/boundary_info.h>
#include <libmesh/boundary_mesh.h>
#include <libmesh/equation_systems.h>
#include <libmesh/exodusII_io.h>
#include <libmesh/face_quad.h>
#include <libmesh/face_quad4.h>
#include <libmesh/mesh.h>
#include <libmesh/mesh_generation.h>
#include <libmesh/mesh_triangle_interface.h>

// Headers for application-specific algorithm/data structure objects
#include <ibamr/IBExplicitHierarchyIntegrator.h>
#include <ibamr/IIMethod.h>
#include <ibamr/INSCollocatedHierarchyIntegrator.h>
#include <ibamr/INSStaggeredHierarchyIntegrator.h>

#include <ibtk/AppInitializer.h>
#include <ibtk/IndexUtilities.h>
#include <ibtk/LEInteractor.h>
#include <ibtk/ibtk_utilities.h>
#include <ibtk/libmesh_utilities.h>
#include <ibtk/muParserCartGridFunction.h>
#include <ibtk/muParserRobinBcCoefs.h>

#include <boost/multi_array.hpp>

// Set up application namespace declarations
#include <ibamr/app_namespaces.h>

// Elasticity model data.
namespace ModelData
{
// Tether (penalty) force functions.

static double kappa_s = 1.0e6;
static double eta_s = 0.0;
static double AA = 0.0;
static double BB = 0.0;
static double MU = 0.0;
static double R1 = 0.0;
static double R2 = 0.0;
static double OMEGA1 = 0.0;
static double OMEGA2 = 0.0;
static double shift = 0.0;
static double fac = 0.0;
static double L = 0.0;

void
tether_force_function_inner(VectorValue<double>& F,
                            const VectorValue<double>& n,
                            const VectorValue<double>& /*N*/,
                            const TensorValue<double>& /*FF*/,
                            const libMesh::Point& x,
                            const libMesh::Point& X,
                            Elem* const /*elem*/,
                            const unsigned short /*side*/,
                            const vector<const vector<double>*>& var_data,
                            const vector<const vector<VectorValue<double> >*>& /*grad_var_data*/,
                            double time,
                            void* /*ctx*/)
{
    const std::vector<double>& U = *var_data[0];

    double u_bndry_n = 0.0;
    for (unsigned int d = 0; d < NDIM; ++d)
    {
        u_bndry_n += n(d) * U[d];
    }
    F(0) = kappa_s * (X(0) * cos(OMEGA1 * time) - X(1) * sin(OMEGA1 * time) - x(0));
    F(1) = kappa_s * (X(0) * sin(OMEGA1 * time) + X(1) * cos(OMEGA1 * time) - x(1));
    F(2) = 0.0;

    return;
} // tether_force_function

inline unsigned int
idx(const unsigned int nr, const unsigned int i, const unsigned int j)
{
    return i + j * nr;

    return libMesh::invalid_uint;
}

} // namespace ModelData
using namespace ModelData;

void postprocess_data(Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                      Pointer<INSHierarchyIntegrator> navier_stokes_integrator,
                      Mesh& mesh,
                      EquationSystems* equation_systems,
                      const int iteration_num,
                      const double loop_time,
                      const string& data_dump_dirname);

// Function prototypes
void pressure_convergence(Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                          const int p_idx,
                          const double data_time,
                          const string& data_dump_dirname);

/*******************************************************************************
 * For each run, the input filename and restart information (if needed) must   *
 * be given on the command line.  For non-restarted case, command line is:     *
 *                                                                             *
 *    executable <input file name>                                             *
 *                                                                             *
 * For restarted run, command line is:                                         *
 *                                                                             *
 *    executable <input file name> <restart directory> <restart number>        *
 *                                                                             *
 *******************************************************************************/
int
main(int argc, char* argv[])
{
    // Initialize libMesh, PETSc, MPI, and SAMRAI.
    LibMeshInit init(argc, argv);
    SAMRAI_MPI::setCommunicator(PETSC_COMM_WORLD);
    SAMRAI_MPI::setCallAbortInSerialInsteadOfExit();
    SAMRAIManager::startup();

    PetscOptionsSetValue(nullptr, "-ksp_rtol", "1e-10");
    PetscOptionsSetValue(nullptr, "-stokes_ksp_atol", "1e-10");

    { // cleanup dynamically allocated objects prior to shutdown

        // Parse command line options, set some standard options from the input
        // file, initialize the restart database (if this is a restarted run),
        // and enable file logging.
        Pointer<AppInitializer> app_initializer = new AppInitializer(argc, argv, "IB.log");
        Pointer<Database> input_db = app_initializer->getInputDatabase();

        // Setup user-defined kernel function.

        // Get various standard options set in the input file.
        const bool dump_viz_data = app_initializer->dumpVizData();
        const int viz_dump_interval = app_initializer->getVizDumpInterval();
        const bool uses_visit = dump_viz_data && app_initializer->getVisItDataWriter();
        const bool uses_exodus = dump_viz_data && !app_initializer->getExodusIIFilename().empty();
        const string inner_exodus_filename = app_initializer->getExodusIIFilename("inner");

        const bool dump_restart_data = app_initializer->dumpRestartData();
        const int restart_dump_interval = app_initializer->getRestartDumpInterval();
        const string restart_dump_dirname = app_initializer->getRestartDumpDirectory();

        const bool dump_postproc_data = app_initializer->dumpPostProcessingData();
        const int postproc_data_dump_interval = app_initializer->getPostProcessingDataDumpInterval();
        const string postproc_data_dump_dirname = app_initializer->getPostProcessingDataDumpDirectory();
        if (dump_postproc_data && (postproc_data_dump_interval > 0) && !postproc_data_dump_dirname.empty())
        {
            Utilities::recursiveMkdir(postproc_data_dump_dirname);
        }

        const bool dump_timer_data = app_initializer->dumpTimerData();
        const int timer_dump_interval = app_initializer->getTimerDumpInterval();

        // Create a simple FE mesh.
        const double dx = input_db->getDouble("DX");
        OMEGA1 = input_db->getDouble("OMEGA1"); // radius of the inner circle
        OMEGA2 = input_db->getDouble("OMEGA2"); // radius of the outer circle

        AA = input_db->getDouble("AA"); // radius of the inner circle
        BB = input_db->getDouble("BB"); // radius of the outer circle
        shift = input_db->getDouble("SHIFT");
        fac = input_db->getDouble("FAC");
        const double ds = input_db->getDouble("MFAC") * dx;
        string elem_type = input_db->getString("ELEM_TYPE");
        R1 = input_db->getDouble("R1"); // radius of the inner circle
        R2 = input_db->getDouble("R2"); // radius of the outer circle
        L = input_db->getDouble("L");   // length in the z direction
        MU = input_db->getDouble("MU"); // length in the z direction

        Mesh inner_mesh(init.comm(), NDIM - 1);

        BoundaryInfo& boundary_info = inner_mesh.get_boundary_info();
        boundary_info.clear_boundary_node_ids();
        const unsigned int NXi_elem = ceil(L / ds);
        const unsigned int NRi_elem = ceil(2.0 * M_PI * R1 / ds);
        int node_id = 0;
        inner_mesh.reserve_nodes(NRi_elem * (NXi_elem + 1));
        inner_mesh.reserve_elem(NRi_elem * NXi_elem);

        for (unsigned int j = 0; j <= NXi_elem; j++)
        {
            for (unsigned int i = 0; i <= NRi_elem - 1; i++)
            {
                const double theta = 2.0 * M_PI * static_cast<double>(i) / static_cast<double>(NRi_elem);
                inner_mesh.add_point(libMesh::Point(R1 * cos(theta),
                                                    R1 * sin(theta),
                                                    -0.5 * L + L * static_cast<Real>(j) / static_cast<Real>(NXi_elem)),
                                     node_id++);
            }
        }

        for (unsigned int j = 0; j <= NXi_elem - 1; j++)
        {
            for (unsigned int i = 0; i <= NRi_elem - 2; i++)
            {
                Elem* elem = inner_mesh.add_elem(new Quad4);
                elem->set_node(0) = inner_mesh.node_ptr(idx(NRi_elem, i, j));
                elem->set_node(1) = inner_mesh.node_ptr(idx(NRi_elem, i + 1, j));
                elem->set_node(2) = inner_mesh.node_ptr(idx(NRi_elem, i + 1, j + 1));
                elem->set_node(3) = inner_mesh.node_ptr(idx(NRi_elem, i, j + 1));
            }
        }

        for (unsigned int j = 0; j <= NXi_elem - 1; j++)
        {
            Elem* elem = inner_mesh.add_elem(new Quad4);
            elem->set_node(0) = inner_mesh.node_ptr(idx(NRi_elem, NRi_elem - 1, j));
            elem->set_node(1) = inner_mesh.node_ptr(idx(NRi_elem, 0, j));
            elem->set_node(2) = inner_mesh.node_ptr(idx(NRi_elem, 0, j + 1));
            elem->set_node(3) = inner_mesh.node_ptr(idx(NRi_elem, NRi_elem - 1, j + 1));
        }

        MeshBase::const_element_iterator el_end = inner_mesh.elements_end();
        for (MeshBase::const_element_iterator el = inner_mesh.elements_begin(); el != el_end; ++el)
        {
            Elem* const elem = *el;
            for (unsigned int side = 0; side < elem->n_sides(); ++side)
            {
                const bool at_mesh_bdry = !elem->neighbor_ptr(side);
                if (at_mesh_bdry)
                {
                    if (boundary_info.has_boundary_id(elem, side, 1) || boundary_info.has_boundary_id(elem, side, 3))
                    {
                        boundary_info.add_side(elem, side, FEDataManager::ZERO_DISPLACEMENT_XY_BDRY_ID);
                    }
                }
            }
        }
        inner_mesh.prepare_for_use();

        kappa_s = input_db->getDouble("KAPPA_S");
        eta_s = input_db->getDouble("ETA_S");

        // Create major algorithm and data objects that comprise the
        // application.  These objects are configured from the input database
        // and, if this is a restarted run, from the restart database.
        Pointer<INSHierarchyIntegrator> navier_stokes_integrator = new INSStaggeredHierarchyIntegrator(
            "INSStaggeredHierarchyIntegrator",
            app_initializer->getComponentDatabase("INSStaggeredHierarchyIntegrator"));

        Pointer<IIMethod> ib_method_ops =
            new IIMethod("IIMethod",
                         app_initializer->getComponentDatabase("IIMethod"),
                         &inner_mesh,
                         app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"));
        Pointer<IBHierarchyIntegrator> time_integrator =
            new IBExplicitHierarchyIntegrator("IBHierarchyIntegrator",
                                              app_initializer->getComponentDatabase("IBHierarchyIntegrator"),
                                              ib_method_ops,
                                              navier_stokes_integrator);

        Pointer<CartesianGridGeometry<NDIM> > grid_geometry = new CartesianGridGeometry<NDIM>(
            "CartesianGeometry", app_initializer->getComponentDatabase("CartesianGeometry"));
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy = new PatchHierarchy<NDIM>("PatchHierarchy", grid_geometry);
        Pointer<StandardTagAndInitialize<NDIM> > error_detector =
            new StandardTagAndInitialize<NDIM>("StandardTagAndInitialize",
                                               time_integrator,
                                               app_initializer->getComponentDatabase("StandardTagAndInitialize"));
        Pointer<BergerRigoutsos<NDIM> > box_generator = new BergerRigoutsos<NDIM>();
        Pointer<LoadBalancer<NDIM> > load_balancer =
            new LoadBalancer<NDIM>("LoadBalancer", app_initializer->getComponentDatabase("LoadBalancer"));
        Pointer<GriddingAlgorithm<NDIM> > gridding_algorithm =
            new GriddingAlgorithm<NDIM>("GriddingAlgorithm",
                                        app_initializer->getComponentDatabase("GriddingAlgorithm"),
                                        error_detector,
                                        box_generator,
                                        load_balancer);

        // Configure the IBFE solver.
        ib_method_ops->initializeFEEquationSystems();
        std::vector<int> vars(NDIM);
        for (unsigned int d = 0; d < NDIM; ++d) vars[d] = d;
        vector<SystemData> sys_data(1, SystemData(IIMethod::VELOCITY_SYSTEM_NAME, vars));

        IIMethod::LagSurfaceForceFcnData tether_force_inner_data(tether_force_function_inner, sys_data);

        ib_method_ops->registerLagSurfaceForceFunction(tether_force_inner_data);

        EquationSystems* inner_equation_systems = ib_method_ops->getFEDataManager()->getEquationSystems();

        Pointer<CartGridFunction> u_init = new muParserCartGridFunction(
            "u_init", app_initializer->getComponentDatabase("VelocityInitialConditions"), grid_geometry);
        navier_stokes_integrator->registerVelocityInitialConditions(u_init);

        Pointer<CartGridFunction> p_init = new muParserCartGridFunction(
            "p_init", app_initializer->getComponentDatabase("PressureInitialConditions"), grid_geometry);

        if (input_db->keyExists("ForcingFunction"))
        {
            Pointer<CartGridFunction> f_fcn = new muParserCartGridFunction(
                "f_fcn", app_initializer->getComponentDatabase("ForcingFunction"), grid_geometry);
            time_integrator->registerBodyForceFunction(f_fcn);
        }

        // Create Eulerian boundary condition specification objects (when necessary).
        vector<RobinBcCoefStrategy<NDIM>*> u_bc_coefs(NDIM, static_cast<RobinBcCoefStrategy<NDIM>*>(NULL));
        const bool periodic_domain = grid_geometry->getPeriodicShift().min() > 0;
        if (!periodic_domain)
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                ostringstream bc_coefs_name_stream;
                bc_coefs_name_stream << "u_bc_coefs_" << d;
                const string bc_coefs_name = bc_coefs_name_stream.str();
                ostringstream bc_coefs_db_name_stream;
                bc_coefs_db_name_stream << "VelocityBcCoefs_" << d;
                const string bc_coefs_db_name = bc_coefs_db_name_stream.str();
                u_bc_coefs[d] = new muParserRobinBcCoefs(
                    bc_coefs_name, app_initializer->getComponentDatabase(bc_coefs_db_name), grid_geometry);
            }
            navier_stokes_integrator->registerPhysicalBoundaryConditions(u_bc_coefs);
        }
        // Create Eulerian body force function specification objects.
        // if (input_db->keyExists("ForcingFunction"))
        // {
        //    Pointer<CartGridFunction> f_fcn = new muParserCartGridFunction(
        //         "f_fcn", app_initializer->getComponentDatabase("ForcingFunction"), grid_geometry);
        //    time_integrator->registerBodyForceFunction(f_fcn);
        // }

        // Set up visualization plot file writers.

        Pointer<VisItDataWriter<NDIM> > visit_data_writer = app_initializer->getVisItDataWriter();
        if (uses_visit)
        {
            time_integrator->registerVisItDataWriter(visit_data_writer);
        }
        std::unique_ptr<ExodusII_IO> inner_exodus_io(uses_exodus ? new ExodusII_IO(inner_mesh) : NULL);

        // Initialize hierarchy configuration and data on all patches.
        ib_method_ops->initializeFEData();
        time_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);

        // Deallocate initialization objects.
        app_initializer.setNull();

        // Write out initial visualization data.
        int iteration_num = time_integrator->getIntegratorStep();
        double loop_time = time_integrator->getIntegratorTime();
        if (dump_viz_data)
        {
            pout << "\n\nWriting visualization files...\n\n";
            if (uses_visit)
            {
                time_integrator->setupPlotData();
                visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
            }
            if (uses_exodus)
            {
                inner_exodus_io->write_timestep(
                    inner_exodus_filename, *inner_equation_systems, iteration_num / viz_dump_interval + 1, loop_time);
            }
        }

        // Main time step loop.
        double loop_time_end = time_integrator->getEndTime();
        double dt = 0.0;
        while (!MathUtilities<double>::equalEps(loop_time, loop_time_end) && time_integrator->stepsRemaining())
        {
            iteration_num = time_integrator->getIntegratorStep();
            loop_time = time_integrator->getIntegratorTime();

            pout << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "At beginning of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";

            dt = time_integrator->getMaximumTimeStepSize();
            time_integrator->advanceHierarchy(dt);
            loop_time += dt;

            pout << "\n";
            pout << "At end       of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "\n";

            // At specified intervals, write visualization and restart files,
            // print out timer data, and store hierarchy data for post
            // processing.
            iteration_num += 1;
            const bool last_step = !time_integrator->stepsRemaining();
            if (dump_viz_data && (iteration_num % viz_dump_interval == 0 || last_step))
            {
                pout << "\nWriting visualization files...\n\n";
                if (uses_visit)
                {
                    time_integrator->setupPlotData();
                    visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
                }
                if (uses_exodus)
                {
                    inner_exodus_io->write_timestep(inner_exodus_filename,
                                                    *inner_equation_systems,
                                                    iteration_num / viz_dump_interval + 1,
                                                    loop_time);
                }
            }

            postprocess_data(patch_hierarchy,
                             navier_stokes_integrator,
                             inner_mesh,
                             inner_equation_systems,
                             iteration_num,
                             loop_time,
                             postproc_data_dump_dirname);
            if (dump_restart_data && (iteration_num % restart_dump_interval == 0 || last_step))
            {
                pout << "\nWriting restart files...\n\n";
                RestartManager::getManager()->writeRestartFile(restart_dump_dirname, iteration_num);
            }
            if (dump_timer_data && (iteration_num % timer_dump_interval == 0 || last_step))
            {
                pout << "\nWriting timer data...\n\n";
                TimerManager::getManager()->print(plog);
            }
        }

        pout << "\n"
             << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n"
             << "Computing error norms.\n\n";
        VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
        const int finest_ln = patch_hierarchy->getFinestLevelNumber();
        HierarchyMathOps hier_math_ops("hier_math_ops", patch_hierarchy);
        hier_math_ops.resetLevels(finest_ln, finest_ln);
        Pointer<hier::Variable<NDIM> > u_var = time_integrator->getVelocityVariable();
        const Pointer<VariableContext> u_ctx = time_integrator->getCurrentContext();
        const int u_idx = var_db->mapVariableAndContextToIndex(u_var, u_ctx);
        const int u_cloned_idx = var_db->registerClonedPatchDataIndex(u_var, u_idx);
        const Pointer<hier::Variable<NDIM> > p_var = time_integrator->getPressureVariable();
        const Pointer<VariableContext> p_ctx = time_integrator->getCurrentContext();

        const int p_idx = var_db->mapVariableAndContextToIndex(p_var, p_ctx);

        pressure_convergence(patch_hierarchy, p_idx, loop_time, postproc_data_dump_dirname);

        const int p_cloned_idx = var_db->registerClonedPatchDataIndex(p_var, p_idx);
        const int coarsest_ln = 0;
        for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
        {
            patch_hierarchy->getPatchLevel(ln)->allocatePatchData(u_cloned_idx, loop_time);
            patch_hierarchy->getPatchLevel(ln)->allocatePatchData(p_cloned_idx, loop_time);
        }
        u_init->setDataOnPatchHierarchy(u_cloned_idx, u_var, patch_hierarchy, loop_time);
        p_init->setDataOnPatchHierarchy(p_cloned_idx, p_var, patch_hierarchy, loop_time - 0.5 * dt);

        hier_math_ops.setPatchHierarchy(patch_hierarchy);
        hier_math_ops.resetLevels(coarsest_ln, finest_ln);
        const int wgt_sc_idx = hier_math_ops.getSideWeightPatchDescriptorIndex();
        HierarchySideDataOpsReal<NDIM, double> hier_sc_data_ops(patch_hierarchy, coarsest_ln, finest_ln);
        hier_sc_data_ops.subtract(u_idx, u_idx, u_cloned_idx);
        pout << std::setprecision(16) << "Error in u at time " << loop_time << ":\n"
             << "  L1-norm:  " << hier_sc_data_ops.L1Norm(u_idx, wgt_sc_idx) << "\n"
             << "  L2-norm:  " << hier_sc_data_ops.L2Norm(u_idx, wgt_sc_idx) << "\n"
             << "  max-norm: " << hier_sc_data_ops.maxNorm(u_idx, wgt_sc_idx) << "\n"
             << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";

        pout << " MU = " << MU << "\n"
             << "  dx:  " << dx << "\n"
             << "  dt: " << dt << "\n";

        if (input_db->getBool("USE_VELOCITY_JUMP_CONDITIONS"))
            pout << " Using the jump condition"
                 << "\n";
        else
            pout << " Using regular IB"
                 << "\n";
        if (dump_viz_data && uses_visit)
        {
            time_integrator->setupPlotData();
            visit_data_writer->writePlotData(patch_hierarchy, iteration_num + 1, loop_time);
        }

        // Cleanup Eulerian boundary condition specification objects (when
        // necessary).
        for (unsigned int d = 0; d < NDIM; ++d) delete u_bc_coefs[d];

    } // cleanup dynamically allocated objects prior to shutdown

    SAMRAIManager::shutdown();
    return 0;
} // main

void
velocity_convergence(Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                     const int u_idx,
                     const double /*data_time*/,
                     const string& /*data_dump_dirname*/)
{
    const int coarsest_ln = 0;
    const int finest_ln = patch_hierarchy->getFinestLevelNumber();

    HierarchyMathOps hier_math_ops("hier_math_ops", patch_hierarchy);
    hier_math_ops.resetLevels(finest_ln, finest_ln);
    const int wgt_cc_idx = hier_math_ops.getCellWeightPatchDescriptorIndex();
    const double X_min[3] = { -0.45 * L, -0.45 * L, -0.45 * L };
    const double X_max[3] = { 0.45 * L, 0.45 * L, 0.45 * L };

    double u_Eulerian_L2_norm = 0.0;
    double u_Eulerian_max_norm = 0.0;
    int N_max = 0;
    vector<double> pos_values;
    for (int ln = finest_ln; ln >= coarsest_ln; --ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        for (PatchLevel<NDIM>::Iterator p(level); p; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            const Box<NDIM>& patch_box = patch->getBox();
            const CellIndex<NDIM>& patch_lower = patch_box.lower();
            const CellIndex<NDIM>& patch_upper = patch_box.upper();

            const Pointer<CartesianPatchGeometry<NDIM> > patch_geom = patch->getPatchGeometry();
            const double* const patch_x_lower = patch_geom->getXLower();
            const double* const patch_x_upper = patch_geom->getXUpper();

            const double* const patch_dx = patch_geom->getDx();

            // Entire box containing the required data.
            Box<NDIM> box(IndexUtilities::getCellIndex(
                              &X_min[0], patch_x_lower, patch_x_upper, patch_dx, patch_lower, patch_upper),
                          IndexUtilities::getCellIndex(
                              &X_max[0], patch_x_lower, patch_x_upper, patch_dx, patch_lower, patch_upper));
            // Part of the box on this patch
            Box<NDIM> trim_box = patch_box * box;
            BoxList<NDIM> iterate_box_list = trim_box;

            // Trim the box covered by the finer region
            BoxList<NDIM> covered_boxes;
            if (ln < finest_ln)
            {
                BoxArray<NDIM> refined_region_boxes;
                Pointer<PatchLevel<NDIM> > next_finer_level = patch_hierarchy->getPatchLevel(ln + 1);
                refined_region_boxes = next_finer_level->getBoxes();
                refined_region_boxes.coarsen(next_finer_level->getRatioToCoarserLevel());
                for (int i = 0; i < refined_region_boxes.getNumberOfBoxes(); ++i)
                {
                    const Box<NDIM> refined_box = refined_region_boxes[i];
                    const Box<NDIM> covered_box = trim_box * refined_box;
                    covered_boxes.unionBoxes(covered_box);
                }
            }
            iterate_box_list.removeIntersections(covered_boxes);

            // Loop over the boxes and store the location and interpolated value.
            Pointer<SideData<NDIM, double> > u_data = patch->getPatchData(u_idx);
            const Pointer<CellData<NDIM, double> > wgt_cc_data = patch->getPatchData(wgt_cc_idx);

            for (BoxList<NDIM>::Iterator lit(iterate_box_list); lit; lit++)
            {
                const Box<NDIM>& iterate_box = *lit;
                for (Box<NDIM>::Iterator bit(iterate_box); bit; bit++)
                {
                    const CellIndex<NDIM>& lower_idx = *bit;

                    const double y = patch_x_lower[1] + patch_dx[1] * (lower_idx(1) - patch_lower(1) + 0.5);
                    const double x = patch_x_lower[0] + patch_dx[0] * (lower_idx(0) - patch_lower(0));

                    double u_ex, v_ex;
                    if (sqrt(x * x + y * y) < R1)
                    {
                        u_ex = -OMEGA1 * y;
                        v_ex = OMEGA1 * x;
                    }
                    else if (sqrt(x * x + y * y) > R2)
                    {
                        u_ex = 0.0;
                        v_ex = 0.0;
                    }
                    else
                    {
                        u_ex = -y * (AA + BB / (x * x + y * y));
                        v_ex = x * (AA + BB / (x * x + y * y));
                    }

                    const double u0 = (*u_data)(SideIndex<NDIM>(lower_idx, 0, SideIndex<NDIM>::Lower));
                    const double v0 = (*u_data)(SideIndex<NDIM>(lower_idx, 1, SideIndex<NDIM>::Lower));

                    N_max += 1;
                    u_Eulerian_L2_norm += std::abs(u0 - u_ex) * std::abs(u0 - u_ex) * (*wgt_cc_data)(lower_idx);
                    u_Eulerian_L2_norm += std::abs(v0 - v_ex) * std::abs(v0 - v_ex) * (*wgt_cc_data)(lower_idx);

                    u_Eulerian_max_norm = std::max(u_Eulerian_max_norm, std::abs(u0 - u_ex));
                    u_Eulerian_max_norm = std::max(u_Eulerian_max_norm, std::abs(v0 - v_ex));
                }
            }
        }
    }

    SAMRAI_MPI::sumReduction(&N_max, 1);
    SAMRAI_MPI::sumReduction(&u_Eulerian_L2_norm, 1);
    SAMRAI_MPI::maxReduction(&u_Eulerian_max_norm, 1);

    u_Eulerian_L2_norm = sqrt(u_Eulerian_L2_norm);

    pout << " u_Eulerian_L2_norm = " << u_Eulerian_L2_norm << "\n\n";
    pout << " u_Eulerian_max_norm = " << u_Eulerian_max_norm << "\n\n";

    return;
} // velocity_convergence

void
pressure_convergence(Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                     const int p_idx,
                     const double /*data_time*/,
                     const string& /*data_dump_dirname*/)
{
    const int coarsest_ln = 0;
    const int finest_ln = patch_hierarchy->getFinestLevelNumber();

    HierarchyMathOps hier_math_ops("hier_math_ops", patch_hierarchy);
    hier_math_ops.resetLevels(finest_ln, finest_ln);
    const int wgt_cc_idx = hier_math_ops.getCellWeightPatchDescriptorIndex();

    const double X_min[3] = { -0.45 * L, -0.45 * L, -0.45 * L };
    const double X_max[3] = { 0.45 * L, 0.45 * L, 0.45 * L };
    // vector<double> pos_values;
    double p_Eulerian_L2_norm = 0.0;
    double p_Eulerian_max_norm = 0.0;
    int N_max = 0;
    for (int ln = finest_ln; ln >= coarsest_ln; --ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        for (PatchLevel<NDIM>::Iterator p(level); p; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            const Box<NDIM>& patch_box = patch->getBox();
            const CellIndex<NDIM>& patch_lower = patch_box.lower();
            const CellIndex<NDIM>& patch_upper = patch_box.upper();

            const Pointer<CartesianPatchGeometry<NDIM> > patch_geom = patch->getPatchGeometry();
            const double* const patch_x_lower = patch_geom->getXLower();
            const double* const patch_x_upper = patch_geom->getXUpper();

            const double* const patch_dx = patch_geom->getDx();

            // Entire box containing the required data.
            Box<NDIM> box(IndexUtilities::getCellIndex(
                              &X_min[0], patch_x_lower, patch_x_upper, patch_dx, patch_lower, patch_upper),
                          IndexUtilities::getCellIndex(
                              &X_max[0], patch_x_lower, patch_x_upper, patch_dx, patch_lower, patch_upper));
            // Part of the box on this patch
            Box<NDIM> trim_box = patch_box * box;
            BoxList<NDIM> iterate_box_list = trim_box;

            // Trim the box covered by the finer region
            BoxList<NDIM> covered_boxes;
            if (ln < finest_ln)
            {
                BoxArray<NDIM> refined_region_boxes;
                Pointer<PatchLevel<NDIM> > next_finer_level = patch_hierarchy->getPatchLevel(ln + 1);
                refined_region_boxes = next_finer_level->getBoxes();
                refined_region_boxes.coarsen(next_finer_level->getRatioToCoarserLevel());
                for (int i = 0; i < refined_region_boxes.getNumberOfBoxes(); ++i)
                {
                    const Box<NDIM> refined_box = refined_region_boxes[i];
                    const Box<NDIM> covered_box = trim_box * refined_box;
                    covered_boxes.unionBoxes(covered_box);
                }
            }
            iterate_box_list.removeIntersections(covered_boxes);

            // Loop over the boxes and store the location and interpolated value.
            //~ Pointer<Data<NDIM, double> > p_data = patch->getPatchData(p_idx);
            const Pointer<CellData<NDIM, double> > p_data = patch->getPatchData(p_idx);
            const Pointer<CellData<NDIM, double> > wgt_cc_data = patch->getPatchData(wgt_cc_idx);
            for (BoxList<NDIM>::Iterator lit(iterate_box_list); lit; lit++)
            {
                const Box<NDIM>& iterate_box = *lit;
                for (Box<NDIM>::Iterator bit(iterate_box); bit; bit++)
                {
                    const CellIndex<NDIM>& cell_idx = *bit;

                    const double y = patch_x_lower[1] + patch_dx[1] * (cell_idx(1) - patch_lower(1) + 0.5);
                    const double x = patch_x_lower[0] + patch_dx[0] * (cell_idx(0) - patch_lower(0) + 0.5);
                    //~ double p_ex_qp = -2.*p_e*x/L + p_e;

                    const double p1 = (*p_data)(cell_idx);
                    double p_ex_qp;

                    N_max += 1;

                    if (sqrt(x * x + y * y) <= R1 - fac * patch_dx[0])
                    {
                        p_ex_qp = 0.5 * OMEGA1 * OMEGA1 * (x * x + y * y) + shift; // p1;
                    }
                    else if (sqrt(x * x + y * y) > (R1 - fac * patch_dx[0]) &&
                             sqrt(x * x + y * y) < (R1 + fac * patch_dx[0]))
                    {
                        p_ex_qp = p1;
                    }
                    else
                    {
                        p_ex_qp = 0.5 * AA * AA * (x * x + y * y) - 0.5 * BB * BB / (x * x + y * y) +
                                  AA * BB * log(x * x + y * y) + 0.5 * OMEGA1 * OMEGA1 * R1 * R1 -
                                  (0.5 * AA * AA * (R1 * R1) - 0.5 * BB * BB / (R1 * R1) + AA * BB * log(R1 * R1)) +
                                  shift;
                    }

                    p_Eulerian_L2_norm += std::abs(p1 - p_ex_qp) * std::abs(p1 - p_ex_qp) * (*wgt_cc_data)(cell_idx);
                    p_Eulerian_max_norm = std::max(p_Eulerian_max_norm, std::abs(p1 - p_ex_qp));
                }
            }
        }
    }
    SAMRAI_MPI::sumReduction(&N_max, 1);
    SAMRAI_MPI::sumReduction(&p_Eulerian_L2_norm, 1);
    SAMRAI_MPI::maxReduction(&p_Eulerian_max_norm, 1);

    p_Eulerian_L2_norm = sqrt(p_Eulerian_L2_norm);

    pout << " p_Eulerian_L2_norm = " << p_Eulerian_L2_norm << "\n\n";
    pout << " p_Eulerian_max_norm = " << p_Eulerian_max_norm << "\n\n";

    return;
} // pressure_convergence

void
postprocess_data(Pointer<PatchHierarchy<NDIM> > /*patch_hierarchy*/,
                 Pointer<INSHierarchyIntegrator> /*navier_stokes_integrator*/,
                 Mesh& mesh,
                 EquationSystems* equation_systems,
                 const int /*iteration_num*/,
                 const double loop_time,
                 const string& /*data_dump_dirname*/)
{
    const unsigned int dim = mesh.mesh_dimension();
    double F_integral[NDIM];
    for (unsigned int d = 0; d < NDIM; ++d) F_integral[d] = 0.0;

    System& x_system = equation_systems->get_system(IIMethod::COORDS_SYSTEM_NAME);
    System& U_system = equation_systems->get_system(IIMethod::VELOCITY_SYSTEM_NAME);
    NumericVector<double>* x_vec = x_system.solution.get();
    NumericVector<double>& X0_vec = x_system.get_vector("INITIAL_COORDINATES");
    NumericVector<double>* x_ghost_vec = x_system.current_local_solution.get();
    x_vec->localize(*x_ghost_vec);
    NumericVector<double>* U_vec = U_system.solution.get();
    NumericVector<double>* U_ghost_vec = U_system.current_local_solution.get();
    U_vec->localize(*U_ghost_vec);
    const DofMap& dof_map = x_system.get_dof_map();
    std::vector<std::vector<unsigned int> > dof_indices(NDIM);

    std::unique_ptr<FEBase> fe(FEBase::build(dim, dof_map.variable_type(0)));
    std::unique_ptr<QBase> qrule = QBase::build(QGAUSS, dim, SEVENTH);
    fe->attach_quadrature_rule(qrule.get());
    const vector<double>& JxW = fe->get_JxW();
    const vector<libMesh::Point>& q_point = fe->get_xyz();
    const vector<vector<double> >& phi = fe->get_phi();
    const vector<vector<VectorValue<double> > >& dphi = fe->get_dphi();
    // const std::vector<std::vector<double> >& dphi_dxi = fe->get_dphidxi();
    //  const std::vector<std::vector<double> >& dphi_deta = fe->get_dphideta();
    boost::array<VectorValue<double>, 2> dx_dxi;

    boost::array<const std::vector<std::vector<double> >*, NDIM - 1> dphi_dxi;
    dphi_dxi[0] = &fe->get_dphidxi();
    if (NDIM > 2) dphi_dxi[1] = &fe->get_dphideta();

    std::vector<double> U_qp_vec(NDIM);
    std::vector<const std::vector<double>*> var_data(1);
    var_data[0] = &U_qp_vec;
    std::vector<const std::vector<libMesh::VectorValue<double> >*> grad_var_data;
    void* force_fcn_ctx = NULL;

    TensorValue<double> FF_qp;
    boost::multi_array<double, 2> x_node, X_node, U_node, P_o_node, P_j_node;
    VectorValue<double> F_qp, U_qp, x_qp, X_qp, N, n;

    const MeshBase::const_element_iterator el_begin = mesh.active_local_elements_begin();
    const MeshBase::const_element_iterator el_end = mesh.active_local_elements_end();
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        Elem* const elem = *el_it;
        fe->reinit(elem);
        for (unsigned int d = 0; d < NDIM; ++d)
        {
            dof_map.dof_indices(elem, dof_indices[d], d);
        }
        get_values_for_interpolation(x_node, *x_ghost_vec, dof_indices);
        get_values_for_interpolation(X_node, X0_vec, dof_indices);
        get_values_for_interpolation(U_node, *U_ghost_vec, dof_indices);

        const unsigned int n_qp = qrule->n_points();
        for (unsigned int qp = 0; qp < n_qp; ++qp)
        {
            interpolate(x_qp, qp, x_node, phi);
            interpolate(X_qp, qp, X_node, phi);
            jacobian(FF_qp, qp, x_node, dphi);
            interpolate(U_qp, qp, U_node, phi);

            for (unsigned int d = 0; d < NDIM; ++d)
            {
                U_qp_vec[d] = U_qp(d);
            }
            tether_force_function_inner(
                F_qp, n, N, FF_qp, x_qp, q_point[qp], elem, 0, var_data, grad_var_data, loop_time, force_fcn_ctx);

            for (int d = 0; d < NDIM; ++d)
            {
                F_integral[d] += F_qp(d) * JxW[qp];
            }
        }
    }
    SAMRAI_MPI::sumReduction(F_integral, NDIM);

    {
        double WSS_L2_norm = 0.0, WSS_max_norm = 0.0;
        double U_L2_norm = 0.0, U_max_norm = 0.0;
        double P_L2_norm = 0.0, P_max_norm = 0.0;
        double disp_L2_norm = 0.0, disp_max_norm = 0.0;
        System& U_system = equation_systems->get_system<System>(IIMethod::VELOCITY_SYSTEM_NAME);
        System& WSS_system = equation_systems->get_system<System>(IIMethod::WSS_OUT_SYSTEM_NAME);
        System& P_o_system = equation_systems->get_system<System>(IIMethod::PRESSURE_OUT_SYSTEM_NAME);
        System& P_j_system = equation_systems->get_system<System>(IIMethod::PRESSURE_JUMP_SYSTEM_NAME);

        NumericVector<double>* U_vec = U_system.solution.get();
        NumericVector<double>* U_ghost_vec = U_system.current_local_solution.get();
        U_vec->localize(*U_ghost_vec);
        DofMap& U_dof_map = U_system.get_dof_map();
        std::vector<std::vector<unsigned int> > U_dof_indices(NDIM);

        NumericVector<double>* WSS_vec = WSS_system.solution.get();
        NumericVector<double>* WSS_ghost_vec = WSS_system.current_local_solution.get();
        WSS_vec->localize(*WSS_ghost_vec);
        DofMap& WSS_dof_map = WSS_system.get_dof_map();
        std::vector<std::vector<unsigned int> > WSS_dof_indices(NDIM);
        std::unique_ptr<FEBase> fe(FEBase::build(dim, WSS_dof_map.variable_type(0)));

        NumericVector<double>* P_o_vec = P_o_system.solution.get();
        NumericVector<double>* P_o_ghost_vec = P_o_system.current_local_solution.get();
        P_o_vec->localize(*P_o_ghost_vec);
        DofMap& P_o_dof_map = P_o_system.get_dof_map();
        std::vector<unsigned int> P_o_dof_indices;

        NumericVector<double>* P_j_vec = P_j_system.solution.get();
        NumericVector<double>* P_j_ghost_vec = P_j_system.current_local_solution.get();
        P_j_vec->localize(*P_j_ghost_vec);
        DofMap& P_j_dof_map = P_j_system.get_dof_map();
        std::vector<unsigned int> P_j_dof_indices;

        VectorValue<double> U_qp, WSS_qp;
        double P_o_qp, P_j_qp;
        VectorValue<double> tau1, tau2;
        int qp_tot = 0;
        boost::multi_array<double, 2> U_node, WSS_node;
        boost::multi_array<double, 1> P_o_node, P_j_node;
        const MeshBase::const_element_iterator el_begin = mesh.active_local_elements_begin();
        const MeshBase::const_element_iterator el_end = mesh.active_local_elements_end();
        for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
        {
            Elem* const elem = *el_it;
            // fe->reinit(elem);
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                dof_map.dof_indices(elem, dof_indices[d], d);
                U_dof_map.dof_indices(elem, U_dof_indices[d], d);
                WSS_dof_map.dof_indices(elem, WSS_dof_indices[d], d);
            }
            P_j_dof_map.dof_indices(elem, P_j_dof_indices);
            P_o_dof_map.dof_indices(elem, P_o_dof_indices);
            const int n_qp = qrule->n_points();
            get_values_for_interpolation(U_node, *U_ghost_vec, U_dof_indices);
            get_values_for_interpolation(WSS_node, *WSS_ghost_vec, WSS_dof_indices);
            get_values_for_interpolation(P_j_node, *P_j_ghost_vec, P_j_dof_indices);
            get_values_for_interpolation(P_o_node, *P_o_ghost_vec, P_o_dof_indices);
            get_values_for_interpolation(x_node, *x_ghost_vec, dof_indices);
            get_values_for_interpolation(X_node, X0_vec, dof_indices);

            for (int qp = 0; qp < n_qp; ++qp)
            {
                interpolate(x_qp, qp, x_node, phi);
                interpolate(X_qp, qp, X_node, phi);
                interpolate(U_qp, qp, U_node, phi);
                interpolate(WSS_qp, qp, WSS_node, phi);
                interpolate(P_o_qp, qp, P_o_node, phi);
                interpolate(P_j_qp, qp, P_j_node, phi);

                for (unsigned int k = 0; k < NDIM - 1; ++k)
                {
                    interpolate(dx_dxi[k], qp, x_node, *dphi_dxi[k]);
                }
                if (NDIM == 2)
                {
                    dx_dxi[1] = VectorValue<double>(0.0, 0.0, 1.0);
                }
                n = (dx_dxi[0].cross(dx_dxi[1])).unit();

                double ex_wss[NDIM];
                double ex_U[NDIM];

                ex_U[0] = (-x_qp(1) / sqrt(x_qp(0) * x_qp(0) + x_qp(1) * x_qp(1))) * R1 * OMEGA1;
                ex_U[1] = (x_qp(0) / sqrt(x_qp(0) * x_qp(0) + x_qp(1) * x_qp(1))) * R1 * OMEGA1;
                ex_U[2] = 0.0;
                ex_wss[0] = (-x_qp(1) / sqrt(x_qp(0) * x_qp(0) + x_qp(1) * x_qp(1))) * MU * (AA - BB / (R1 * R1));
                ex_wss[1] = (x_qp(0) / sqrt(x_qp(0) * x_qp(0) + x_qp(1) * x_qp(1))) * MU * (AA - BB / (R1 * R1));
                ex_wss[2] = 0.0;
                libMesh::Point X = q_point[qp];
                double p_ex_qp = 0.5 * OMEGA1 * OMEGA1 * R1 * R1 + shift;
                qp_tot += 1;
                for (unsigned int d = 0; d < NDIM; ++d)
                {
                    U_L2_norm += (U_qp(d) - ex_U[d]) * (U_qp(d) - ex_U[d]) * JxW[qp];
                    U_max_norm = std::max(U_max_norm, std::abs(U_qp(d) - ex_U[d]));

                    WSS_L2_norm += (WSS_qp(d) - ex_wss[d]) * (WSS_qp(d) - ex_wss[d]) * JxW[qp];
                    WSS_max_norm = std::max(WSS_max_norm, std::abs(WSS_qp(d) - ex_wss[d]));
                }
                P_L2_norm += std::abs(P_o_qp - p_ex_qp) * std::abs(P_o_qp - p_ex_qp) * JxW[qp];
                P_max_norm = std::max(P_max_norm, std::abs(P_o_qp - p_ex_qp));

                disp_L2_norm += (X_qp(0) * cos(OMEGA1 * loop_time) - X_qp(1) * sin(OMEGA1 * loop_time) - x_qp(0)) *
                                (X_qp(0) * cos(OMEGA1 * loop_time) - X_qp(1) * sin(OMEGA1 * loop_time) - x_qp(0)) *
                                JxW[qp];
                disp_L2_norm += (X_qp(0) * sin(OMEGA1 * loop_time) + X_qp(1) * cos(OMEGA1 * loop_time) - x_qp(1)) *
                                (X_qp(0) * sin(OMEGA1 * loop_time) + X_qp(1) * cos(OMEGA1 * loop_time) - x_qp(1)) *
                                JxW[qp];
                disp_L2_norm += (X_qp(2) - x_qp(2)) * (X_qp(2) - x_qp(2)) * JxW[qp];
                disp_max_norm =
                    std::max(disp_max_norm,
                             std::abs(X_qp(0) * cos(OMEGA1 * loop_time) - X_qp(1) * sin(OMEGA1 * loop_time) - x_qp(0)));
                disp_max_norm =
                    std::max(disp_max_norm,
                             std::abs(X_qp(0) * sin(OMEGA1 * loop_time) + X_qp(1) * cos(OMEGA1 * loop_time) - x_qp(1)));
                disp_max_norm = std::max(disp_max_norm, std::abs(X_qp(2) - x_qp(2)));
            }
        }

        SAMRAI_MPI::sumReduction(&qp_tot, 1);
        SAMRAI_MPI::sumReduction(&WSS_L2_norm, 1);
        SAMRAI_MPI::maxReduction(&WSS_max_norm, 1);
        SAMRAI_MPI::sumReduction(&U_L2_norm, 1);
        SAMRAI_MPI::maxReduction(&U_max_norm, 1);
        SAMRAI_MPI::sumReduction(&disp_L2_norm, 1);
        SAMRAI_MPI::maxReduction(&disp_max_norm, 1);
        SAMRAI_MPI::sumReduction(&P_L2_norm, 1);
        SAMRAI_MPI::maxReduction(&P_max_norm, 1);

        U_L2_norm = sqrt(U_L2_norm);
        WSS_L2_norm = sqrt(WSS_L2_norm);
        disp_L2_norm = sqrt(disp_L2_norm);
        P_L2_norm = sqrt(P_L2_norm);

        pout << " Lagrangian WSS_L2_norm = " << WSS_L2_norm << "\n\n";
        pout << " Lagrangian WSS_max_norm = " << WSS_max_norm << "\n\n";

        pout << " Lagrangian U_L2_norm = " << U_L2_norm << "\n\n";
        pout << " Lagrangian U_max_norm = " << U_max_norm << "\n\n";

        pout << " Lagrangian disp_L2_norm = " << disp_L2_norm << "\n\n";
        pout << " Lagrangian disp_max_norm = " << disp_max_norm << "\n\n";

        pout << "Lagrangian P_L2_norm = " << P_L2_norm << "\n\n";
        pout << "Lagrangian P_max_norm = " << P_max_norm << "\n\n";
    }

    return;
} // postprocess_data


  //==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_FAST_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_FAST_RSQRT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/arithmetic/functions/fast_rsqrt.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/sqr.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( fast_rsqrt_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_ < single_<A0>
                                              , boost::simd::tag::sse_
                                              >
                                      ))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      result_type nr  = _mm_rsqrt_ps( a0 );
                  nr *= 0.5f * (3.f - a0 * sqr(nr));

      return nr;
    }
  };
} } }

#endif

#endif


#include <boost/math/distributions/arcsine.hpp>


/* Copyright (c) 2020, Danish Technological Institute.
 * All rights reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 *
 * Original author: Lars Berscheid <lars.berscheid@kit.edu>
 */

#pragma once

#define BIN_PICKING_GEOMETRY

#include <geometry_msgs/PoseStamped.h>

#include <Eigen/Geometry>
#include <unsupported/Eigen/EulerAngles>

struct Affine {
private:
  using Euler = Eigen::EulerAngles<double, Eigen::EulerSystemZYX>;

  Eigen::Vector3d get_angles() const {
    Eigen::Vector3d angles = Euler::FromRotation<false, false, false>(data.rotation()).angles();
    Eigen::Vector3d angles_equal;
    angles_equal << angles[0] - M_PI, M_PI - angles[1], angles[2] - M_PI;

    if (angles_equal[1] > M_PI) {
      angles_equal[1] -= 2 * M_PI;
    }
    if (angles_equal[2] < -M_PI) {
      angles_equal[2] += 2 * M_PI;
    }

    if (angles.norm() < angles_equal.norm()) {
      return angles;
    }
    return angles_equal;
  }

public:
  using Vector6d = Eigen::Matrix<double, 6, 1>;

  Eigen::Affine3d data;

  Affine() {
    this->data = Eigen::Affine3d::Identity();
  }

  Affine(const Eigen::Affine3d& data) {
    this->data = data;
  }

  Affine(double x, double y, double z, double a, double b, double c) {
    data = Eigen::Translation<double, 3>(x, y, z) * Euler(a, b, c).toRotationMatrix();
  }

  Affine(const std::array<double, 6>& v): Affine(v[0], v[1], v[2], v[3], v[4], v[5]) { }

  Affine(const Eigen::Matrix<double, 6, 1>& v): Affine(v(0), v(1), v(2), v(3), v(4), v(5)) { }

  Affine(const geometry_msgs::Pose& pose) {
    auto position = pose.position;
    auto orientation = pose.orientation;

    Eigen::Translation<double, 3> t {position.x, position.y, position.z};
    Eigen::Quaternion<double> q {orientation.w, orientation.x, orientation.y, orientation.z};

    data = t * q;
  }

  Affine operator *(const Affine &a) const {
    Eigen::Affine3d result;
    result = data * a.data;
    return Affine(result);
  }

  Affine inverse() const {
    return Affine(data.inverse());
  }

  bool isApprox(const Affine &a) const {
    return data.isApprox(a.data);
  }

  Eigen::Ref<Eigen::Affine3d::MatrixType> matrix() {
    return data.matrix();
  }

  void translate(const Eigen::Vector3d &v) {
    data.translate(v);
  }

  void pretranslate(const Eigen::Vector3d &v) {
    data.pretranslate(v);
  }

  Eigen::Vector3d translation() const {
    Eigen::Vector3d v;
    v << data.translation();
    return v;
  }

  double x() const {
    return data.translation()(0);
  }

  void set_x(double x) {
    data.translation()(0) = x;
  }

  double y() const {
    return data.translation()(1);
  }

  void set_y(double y) {
    data.translation()(1) = y;
  }

  double z() const {
    return data.translation()(2);
  }

  void set_z(double z) {
    data.translation()(2) = z;
  }

  void rotate(const Eigen::Affine3d::LinearMatrixType &r) {
    data.rotate(r);
  }

  void prerotate(const Eigen::Affine3d::LinearMatrixType &r) {
    data.prerotate(r);
  }

  Eigen::Affine3d::LinearMatrixType rotation() const {
    Eigen::Affine3d::LinearMatrixType result;
    result << data.rotation();
    return result;
  }

  double a() const {
    return get_angles()(0);
  }

  double b() const {
    return get_angles()(1);
  }

  double c() const {
    return get_angles()(2);
  }

  void set_a(double a) {
    Eigen::Matrix<double, 3, 1> angles;
    angles << get_angles();
    data = Eigen::Translation<double, 3>(data.translation()) * Euler(a, angles(1), angles(2)).toRotationMatrix();
  }

  void set_b(double b) {
    Eigen::Matrix<double, 3, 1> angles;
    angles << get_angles();
    data = Eigen::Translation<double, 3>(data.translation()) * Euler(angles(0), b, angles(2)).toRotationMatrix();
  }

  void set_c(double c) {
    Eigen::Matrix<double, 3, 1> angles;
    angles << get_angles();
    data = Eigen::Translation<double, 3>(data.translation()) * Euler(angles(0), angles(1), c).toRotationMatrix();
  }

  geometry_msgs::Pose toPose() const {
    Eigen::Quaternion<double> q;
    q = data.rotation();
    auto t = data.translation();

    geometry_msgs::Pose result;
    result.position.x = t(0);
    result.position.y = t(1);
    result.position.z = t(2);
    result.orientation.x = q.x();
    result.orientation.y = q.y();
    result.orientation.z = q.z();
    result.orientation.w = q.w();
    return result;
  }

  std::array<double, 6> toArray() const {
    Eigen::Matrix<double, 6, 1> v;
    v << data.translation(), get_angles();
    return {v(0), v(1), v(2), v(3), v(4), v(5)};
  }

  Affine getInnerRandom() const {
    std::random_device r;
    std::default_random_engine engine(r());

    Eigen::Matrix<double, 6, 1> random;
    Eigen::Matrix<double, 6, 1> max;
    max << data.translation(), get_angles();

    for (int i = 0; i < 6; i++) {
      std::uniform_real_distribution<double> distribution(-max(i), max(i));
      random(i) = distribution(engine);
    }

    return Affine(random);
  }

  std::string toString() const {
    Eigen::Matrix<double, 6, 1> v;
    v << data.translation(), get_angles();

    return "[" + std::to_string(v(0)) + ", " + std::to_string(v(1)) + ", " + std::to_string(v(2))
      + ", " + std::to_string(v(3)) + ", " + std::to_string(v(4)) + ", " + std::to_string(v(5)) + "]";
  }
};


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE constexpr_vector_test

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/algebra/vector/vector.hpp>
#include <nil/crypto3/algebra/vector/math.hpp>
#include <nil/crypto3/algebra/vector/operators.hpp>
#include <nil/crypto3/algebra/vector/utility.hpp>

using namespace nil::crypto3::algebra;

static_assert(make_vector(1, 2, 3) == vector {1, 2, 3}, "make_vector and uniform initialization deduction guide");

static_assert(make_vector(1, 2, 3) == vector {{1, 2, 3}}, "make_vector and aggregate initialization deduction guide");

static_assert(elementwise([](double x) { return 1 / x; }, vector {1., 2., 4.}) == vector {1., 0.5, 0.25},
              "elementwise");

static_assert(vector {1, 2, 3} == vector {1, 2, 3}, "operator==");

static_assert(vector {1, 2, 3} != vector {3, 2, 1}, "operator!=");

static_assert(vector {1, 2, 3} + vector {1, 2, 3} == vector {2, 4, 6}, "operator+");

static_assert(sum(vector {1, 2, 3}) == 6, "sum");

static_assert(iota<5>(0) == vector {0, 1, 2, 3, 4}, "iota");

static_assert(iota<5, double>() == vector {0., 1., 2., 3., 4.}, "iota");

static_assert(fill<4>(2.) == vector {2., 2., 2., 2.}, "fill");

static_assert(generate<4>([](auto i) { return double(i * i); }) == vector {0., 1., 4., 9.}, "generate");

static_assert(vector {1, 2, 3} == slice<3>(vector {1, 2, 3, 4}), "slice-no offset");

static_assert(vector {2, 3, 4} == slice<3>(vector {1, 2, 3, 4}, 1), "slice with offset");


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2017, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_distance_geographic_pl_l
#endif

//#include <boost/geometry/core/srs.hpp>
#include <boost/test/included/unit_test.hpp>

#include "test_distance_geo_common.hpp"

typedef bg::cs::geographic<bg::degree> cs_type;
typedef bg::model::point<double, 2, cs_type> point_type;
typedef bg::model::multi_point<point_type> multi_point_type;

namespace services = bg::strategy::distance::services;
typedef bg::default_distance_result<point_type>::type return_type;

typedef bg::srs::spheroid<double> stype;

// Strategies for point-point distance

typedef bg::strategy::distance::andoyer<stype> andoyer;
typedef bg::strategy::distance::thomas<stype> thomas;
typedef bg::strategy::distance::vincenty<stype> vincenty;

//===========================================================================

template <typename Strategy>
inline bg::default_distance_result<point_type>::type
pp_distance(std::string const& wkt1,
            std::string const& wkt2,
            Strategy const& strategy)
{
    point_type p1, p2;
    bg::read_wkt(wkt1, p1);
    bg::read_wkt(wkt2, p2);
    return bg::distance(p1, p2, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_multipoint_point(Strategy const& strategy)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/point distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<multi_point_type, point_type> tester;

    tester::apply("mp-p-01",
                  "MULTIPOINT(1 1,1 2,2 3)",
                  "POINT(0 0)",
                  pp_distance("POINT(0 0)","POINT(1 1)",strategy),
                  strategy);

    tester::apply("mp-p-01",
                  "MULTIPOINT(0 0,0 2,2 0,2 2)",
                  "POINT(1.1 1.1)",
                  pp_distance("POINT(1.1 1.1)","POINT(2 2)",strategy),
                  strategy);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_empty_input_pointlike_linear(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "testing on empty inputs... " << std::flush;
#endif
    bg::model::multi_point<Point> multipoint_empty;
    Point point_empty;

    Point point = from_wkt<Point>("POINT(0 0)");

    // 1st geometry is empty
    test_empty_input(multipoint_empty, point, strategy);

    // 2nd geometry is empty
    test_empty_input(point, multipoint_empty, strategy);

    // both geometries are empty
    test_empty_input(multipoint_empty, point_empty, strategy);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << "done!" << std::endl;
#endif
}

//===========================================================================
//===========================================================================
//===========================================================================

BOOST_AUTO_TEST_CASE( test_all_point_segment )
{
    test_distance_multipoint_point(vincenty());
    test_distance_multipoint_point(thomas());
    test_distance_multipoint_point(andoyer());

    test_empty_input_pointlike_linear<point_type>(vincenty());
    test_empty_input_pointlike_linear<point_type>(thomas());
    test_empty_input_pointlike_linear<point_type>(andoyer());
}


#ifndef __UNIVARIATEQUADRATURE__CLASS__
#define __UNIVARIATEQUADRATURE__CLASS__

#include <vector>

#include <Eigen/Dense>

// struct that contains a particular quadrature rule
struct Quadrature {
  Eigen::VectorXd xi;
  Eigen::VectorXd w;
};

/**   \brief basis class for univariate quadrature rules
*
*/
class UnivariateQuadrature {
 public:
  UnivariateQuadrature(void) : _maxLvl(-1){};
  // setter...
  virtual void initQuadrature(int maxLvl) = 0;
  virtual void resizeQuadrature(int maxLvl) = 0;
  // tester...
  virtual void testQuadrature(int maxLvl) = 0;
  // getter...
  int get_maxLvl(void) const { return _maxLvl; };
  const std::vector<Quadrature> &get_Q(void) const {
    return (const std::vector<Quadrature> &)_Q;
  };

 protected:
  int _maxLvl;
  std::vector<Quadrature> _Q;
};

#endif


#define GLM_ENABLE_EXPERIMENTAL 1
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Halide.h>
#include <fstream>
#include <glm/ext.hpp>
#include <glm/glm.hpp>
#include <halide_image_io.h>
#include <stdio.h>
#include <iostream>
#include <ImfRgbaFile.h>
#include <ImfStringAttribute.h>
#include <ImfMatrixAttribute.h>
#include <ImfArray.h>
#include <algorithm>
#include <ImfNamespace.h>
#include <tuple>
#include <math.h>

namespace IMF = OPENEXR_IMF_NAMESPACE;

using namespace IMF;
using namespace IMATH_NAMESPACE;

using namespace Halide;
using namespace Halide::Tools;
using namespace Eigen;
using namespace std;

using Vector4h = Matrix<Halide::Expr, 4, 1>;
using Matrix4h = Matrix<Halide::Expr, 4, 4>;

Var x, y, c, i, ii, xo, yo, xi, yi, img;

Matrix4h read4x4MatFromCSV(string csvFile)
{
    Matrix4h transfMat;
    ifstream in(csvFile);
    vector<float> floatVec;
    if (in)
    {
        string line;
        while (getline(in, line))
        {
            stringstream sep(line);
            string field;
            while (getline(sep, field, ','))
            {
                float val = stod(field);
                floatVec.push_back(val);
            }
        }
    }
    transfMat << floatVec[0], floatVec[1], floatVec[2], floatVec[3],
        floatVec[4], floatVec[5], floatVec[6], floatVec[7],
        floatVec[8], floatVec[9], floatVec[10], floatVec[11],
        floatVec[12], floatVec[13], floatVec[14], floatVec[15];
    return transfMat;
}
ostream &operator<<(ostream &os, const Matrix4h &m)
{
    os << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << " " << m(0, 3) << "\n"
       << m(1, 0) << " " << m(1, 1) << " " << m(1, 2) << " " << m(1, 3) << "\n"
       << m(2, 0) << " " << m(2, 1) << " " << m(2, 2) << " " << m(2, 3) << "\n"
       << m(3, 0) << " " << m(3, 1) << " " << m(3, 2) << " " << m(3, 3) << endl;
    return os;
}

ostream &operator<<(ostream &os, const Vector4h &v)
{
    os << v(0) << "\n"
       << v(1) << "\n"
       << v(2) << "\n"
       << v(3) << endl;
    return os;
}

Matrix4h getCameraMatrix(float focalLen, float pxDim, int width, int height)
{

    focalLen = focalLen * 10e-3;
    float u0 = width / 2.0;
    float v0 = height / 2.0;
    Matrix4h camMat;
    float diagEntry = focalLen / pxDim;
    camMat << diagEntry, 0.0f, u0, 0.0f,
        0.0f, diagEntry, v0, 0.0f,
        0.0f, 0.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 1.0f;

    return camMat;
}

Matrix4h getInvCameraMat(float focalLen, float pxDim, int width, int height)
{
    focalLen = focalLen * 10e-3;
    float u0 = width / 2.0;
    float v0 = height / 2.0;
    Matrix4h camMat;
    camMat << pxDim / focalLen, 0.0f, -pxDim * u0 / focalLen, 0.0f,
        0.0f, pxDim / focalLen, -pxDim * v0 / focalLen, 0.0f,
        0.0f, 0.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 1.0f;

    return camMat;
}

Matrix4h getTransMatProjToCam()
{
    Matrix4h transMat;
    transMat << 0.9945219f, 0.0f, -0.10452846f, -0.2f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.10452846f, 0.0f, 0.9945219f, 0.0f,
        0.0f, 0.0f, 0.0f, 1.0f;
    return transMat;
}

tuple<int, int> readOpenEXR(const char filename[], Array2D<Rgba> &pixels)
{
    RgbaInputFile file(filename);
    Box2i dw = file.dataWindow();

    int width = dw.max.x - dw.min.x + 1;
    int height = dw.max.y - dw.min.y + 1;
    cout << "Width: " << width << "    Height: " << height << endl;
    tuple<int, int> dim(width, height);
    pixels.resizeErase(height, width);

    file.setFrameBuffer(&pixels[0][0] - dw.min.x - dw.min.y * width, 1, width);
    file.readPixels(dw.min.y, dw.max.y);
    return dim;
}

void exrArrayToHalideBuffer(Array2D<Rgba> &pixels, Buffer<float> &halideBuffer, int width, int height)
{
    for (int row{0}; row < height; row++)
    {
        for (int col{0}; col < width; col++)
        {
            isinf(pixels[row][col].r) ? pixels[row][col].r = 0.0f : pixels[row][col].r = pixels[row][col].r;
            halideBuffer(col, row) = pixels[row][col].r;
        }
    }
}

void saveImage(Expr result, size_t width, size_t height, const string &basename)
{
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y) = cast<uint8_t>(clamp(result, 0.0f, 1.0f) * 255.0f);
    byteResult.compile_jit(target);
    Buffer<uint8_t> output(width, height);
    byteResult.realize(output);
    stringstream filename;
    filename << basename << ".png";
    save_image(output, filename.str());
}

void saveImageRaw(Expr result, size_t width, size_t height, const string &basename)
{
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y) = cast<uint8_t>(result);
    byteResult.compile_jit(target);
    Buffer<uint8_t> output(width, height);
    byteResult.realize(output);
    stringstream filename;
    filename << basename;
    save_image(output, filename.str());
}

void saveImageEXR(Expr result, int width, int height, const char fileName[])
{
    result = cast<float>(result);
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y) = result;
    Buffer<float> output(width, height);
    byteResult.compile_jit(target);
    byteResult.realize(output);

    Array2D<Rgba> pixels;
    pixels.resizeErase(height, width);
    for (int row{0}; row < height; row++)
    {
        for (int col{0}; col < width; col++)
        {
            pixels[row][col].r = output(col, row);
            pixels[row][col].g = output(col, row);
            pixels[row][col].b = output(col, row);
        }
    }
    RgbaOutputFile file(fileName, width, height, WRITE_RGBA);
    file.setFrameBuffer(&pixels[0][0], 1, width);
    file.writePixels(height);
}

void debugImageEXR(Expr channel1Expr, Expr channel2Expr, Expr channel3Expr, int width, int height, const char fileName[])
{
    channel1Expr = cast<float>(channel1Expr);
    channel2Expr = cast<float>(channel2Expr);
    channel3Expr = cast<float>(channel3Expr);
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y, c) = 0.0f;
    byteResult(x, y, 0) = channel1Expr;
    byteResult(x, y, 1) = channel2Expr;
    byteResult(x, y, 2) = channel3Expr;
    Buffer<float> output(width, height, 3);
    byteResult.compile_jit(target);
    byteResult.realize(output);

    Array2D<Rgba> pixels;
    pixels.resizeErase(height, width);
    for (int row{0}; row < height; row++)
    {
        for (int col{0}; col < width; col++)
        {
            pixels[row][col].r = output(col, row, 0);
            pixels[row][col].g = output(col, row, 1);
            pixels[row][col].b = output(col, row, 2);
        }
    }
    RgbaOutputFile file(fileName, width, height, WRITE_RGBA);
    file.setFrameBuffer(&pixels[0][0], 1, width);
    file.writePixels(height);
}

int main(int argc, char **argv)
{
    const string INPUTFILE = "images/depth-img/Image0000.exr";
    const string TRANSF_MAT_WORLD_TO_PROJ_CSV = "matrices/transf-world-proj.csv";
    const string TRANSF_PROJ_CAM_CSV = "matrices/transf-proj-cam.csv";
    const string INV_CAM_MAT_CSV = "matrices/inv-cam-mat.csv";
    const string CAM_MAT_CSV = "matrices/cam-mat.csv";
    const string OUTPUT_X_PROJ_EXR = "images/ground-truth/x-val-proj-gt.exr";
    const string OUTPUT_X_PROJ_PNG = "images/ground-truth/x-val-proj-gt.png";
    const bool SAVE_DEBUG_IMAGES = false;

    Array2D<Rgba> pixels;
    int width, height;
    tie(width, height) = readOpenEXR(INPUTFILE.c_str(), pixels);
    Buffer<float> input(width, height);
    exrArrayToHalideBuffer(pixels, input, width, height);
    cout << "Width: " << width << "  Height: " << height << endl;
    Matrix4h camMat = read4x4MatFromCSV(CAM_MAT_CSV);
    Matrix4h camMatInv = read4x4MatFromCSV(INV_CAM_MAT_CSV);
    Matrix4h transfMatProjToCam = read4x4MatFromCSV(TRANSF_PROJ_CAM_CSV);
    cout << "Camera matrix" << endl;
    cout << camMat << endl;
    cout << "Inverse camera matrix" << endl;
    cout << camMatInv << endl;
    cout << "Transformation matrix projector to camera" << endl;
    cout << transfMatProjToCam << endl;

    Expr zDepthCam = input(x, y);
    zDepthCam = cast<double>(zDepthCam);
    Vector4h pxCam{x, y, 1.0f, 0.0f};
    pxCam(0) = cast<double>(pxCam(0));
    pxCam(1) = cast<double>(pxCam(1));
    pxCam(2) = cast<double>(pxCam(2));
    pxCam(3) = cast<double>(pxCam(3));
    Vector4h normCam = camMatInv * pxCam;
    Vector4h ptCam = normCam / normCam(2) * zDepthCam;
    ptCam(3) = 1.0f;
    Vector4h ptProj = transfMatProjToCam * ptCam;
    Vector4h normProj = ptProj / ptProj(2);
    ptProj(3) = 0.0f;
    Vector4h pxProj = camMat * normProj;
    //normProj = normProj / normProj(2);
    Expr xPxProj = pxProj(0);
    //saveImage(xValProj, width, height, "x-val-proj-gt");
    saveImageEXR(xPxProj, width, height, OUTPUT_X_PROJ_EXR.c_str());
    Expr toPng = xPxProj * 256 / 1920;
    saveImageRaw(toPng, width, height, OUTPUT_X_PROJ_PNG.c_str());

    if (SAVE_DEBUG_IMAGES)
    {
        debugImageEXR(pxCam(0), pxCam(1), pxCam(2), width, height, "pxCam.exr");
        debugImageEXR(normCam(0), normCam(1), normCam(2), width, height, "normCam.exr");
        debugImageEXR(ptCam(0), ptCam(1), ptCam(2), width, height, "ptCam.exr");
        debugImageEXR(ptProj(0), ptProj(1), ptProj(2), width, height, "ptProj.exr");
        debugImageEXR(normProj(0), normProj(1), normProj(3), width, height, "normProj.exr");
        debugImageEXR(pxProj(0), pxProj(1), pxProj(2), width, height, "pxProj.exr");
    }

    return 0;
}


// -------------------------------------------------------------------------------------------------
//                              Copyright 2016 - NumScale SAS
//
//                   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
// -------------------------------------------------------------------------------------------------

#include <simd_bench.hpp>
#include <boost/simd/function/simd/nearbyint.hpp>
#include <boost/simd/pack.hpp>
#include <cmath>

namespace nsb = ns::bench;

DEFINE_SIMD_BENCH(simd_nearbyint, boost::simd::nearbyint);

DEFINE_BENCH_MAIN()
{
  nsb::for_each<simd_nearbyint, NS_BENCH_IEEE_TYPES>(-10, 10);
}


#include <gtest/gtest.h>

#include <aslam/backend/ErrorTermMotionBST.hpp>
// This test harness makes it easy to test error terms.
#include <aslam/backend/test/ErrorTermTestHarness.hpp>
#include <sm/kinematics/Transformation.hpp>
#include <aslam/backend/TransformationBasic.hpp>

#include <aslam/backend/EuclideanPoint.hpp>
#include <sm/kinematics/RotationVector.hpp>
#include <aslam/splines/OPTBSpline.hpp>
#include <aslam/splines/implementation/OPTBSplineImpl.hpp>
#include <bsplines/EuclideanBSpline.hpp>
#include <aslam/backend/Scalar.hpp>

#include <boost/shared_ptr.hpp>

template <typename TConf, int ISplineOrder, int IDim, bool BDimRequired> struct ConfCreator {
  static inline TConf create(){
    return TConf(typename TConf::ManifoldConf(IDim), ISplineOrder);
  }
};

template <typename TConf, int ISplineOrder, int IDim> struct ConfCreator<TConf, ISplineOrder, IDim, false> {
  static inline TConf create(){
    BOOST_STATIC_ASSERT_MSG(IDim == TConf::Dimension::VALUE, "impossible dimension selected!");
    return TConf(typename TConf::ManifoldConf(), ISplineOrder);
  }
};

template <typename TConf, int ISplineOrder, int IDim> inline TConf createConf(){
  return ConfCreator<TConf, ISplineOrder, IDim, TConf::Dimension::IS_DYNAMIC>::create();
}

TEST(AslamVChargeBackendTestSuite, testEuclidean)
{
  try {
    using namespace aslam::backend;

    double sigma_n = 0.5;

    typedef aslam::splines::OPTBSpline<bsplines::EuclideanBSpline<4, 1>::CONF> PosSpline;
    PosSpline robotPosSpline;
    const int pointSize = robotPosSpline.getPointSize();

    PosSpline::point_t initPoint(pointSize);
    initPoint(0,0) = 10.0;

    robotPosSpline.initConstantUniformSpline(0, 10, 10, initPoint);

    // First, create a design variable for the wall position.
    boost::shared_ptr<aslam::backend::Scalar> dv_w(new aslam::backend::Scalar(5.0));

    // Create observation error
    auto vecVelExpr = robotPosSpline.getExpressionFactoryAt<1>(5).getValueExpression(1);

    aslam::backend::ErrorTermMotionBST eo(vecVelExpr, 1.0, sigma_n * sigma_n);

    EXPECT_NEAR(1.0/(sigma_n * sigma_n), eo.evaluateError(), 1e-14);

    // Create the test harness
    aslam::backend::ErrorTermTestHarness<1> harness(&eo);

    // Run the unit tests.
    harness.testAll(1e-5);
  }
  catch(const std::exception & e) {
    FAIL() << e.what();
  }
}


/* boost random/laplace_distribution.hpp header file
 *
 * Copyright Steven Watanabe 2014
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_LAPLACE_DISTRIBUTION_HPP
#define BOOST_RANDOM_LAPLACE_DISTRIBUTION_HPP

#include <cassert>
#include <istream>
#include <iosfwd>
#include <boost/random/detail/operators.hpp>
#include <boost/random/exponential_distribution.hpp>

namespace boost {
namespace random {

/**
 * The laplace distribution is a real-valued distribution with
 * two parameters, mean and beta.
 *
 * It has \f$\displaystyle p(x) = \frac{e^-{\frac{|x-\mu|}{\beta}}}{2\beta}\f$.
 */
template<class RealType = double>
class laplace_distribution {
public:
    typedef RealType result_type;
    typedef RealType input_type;

    class param_type {
    public:
        typedef laplace_distribution distribution_type;

        /**
         * Constructs a @c param_type from the "mean" and "beta" parameters
         * of the distribution.
         */
        explicit param_type(RealType mean_arg = RealType(0.0),
                            RealType beta_arg = RealType(1.0))
          : _mean(mean_arg), _beta(beta_arg)
        {}

        /** Returns the "mean" parameter of the distribtuion. */
        RealType mean() const { return _mean; }
        /** Returns the "beta" parameter of the distribution. */
        RealType beta() const { return _beta; }

        /** Writes a @c param_type to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        { os << parm._mean << ' ' << parm._beta; return os; }

        /** Reads a @c param_type from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        { is >> parm._mean >> std::ws >> parm._beta; return is; }

        /** Returns true if the two sets of parameters are the same. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._mean == rhs._mean && lhs._beta == rhs._beta; }

        /** Returns true if the two sets of parameters are the different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _mean;
        RealType _beta;
    };

    /**
     * Constructs an @c laplace_distribution from its "mean" and "beta" parameters.
     */
    explicit laplace_distribution(RealType mean_arg = RealType(0.0),
                               RealType beta_arg = RealType(1.0))
      : _mean(mean_arg), _beta(beta_arg)
    {}
    /** Constructs an @c laplace_distribution from its parameters. */
    explicit laplace_distribution(const param_type& parm)
      : _mean(parm.mean()), _beta(parm.beta())
    {}

    /**
     * Returns a random variate distributed according to the
     * laplace distribution.
     */
    template<class URNG>
    RealType operator()(URNG& urng) const
    {
        RealType exponential = exponential_distribution<RealType>()(urng);
        if(uniform_01<RealType>()(urng) < 0.5)
            exponential = -exponential;
        return _mean + _beta * exponential;
    }

    /**
     * Returns a random variate distributed accordint to the laplace
     * distribution with parameters specified by @c param.
     */
    template<class URNG>
    RealType operator()(URNG& urng, const param_type& parm) const
    {
        return laplace_distribution(parm)(urng);
    }

    /** Returns the "mean" parameter of the distribution. */
    RealType mean() const { return _mean; }
    /** Returns the "beta" parameter of the distribution. */
    RealType beta() const { return _beta; }

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return RealType(-std::numeric_limits<RealType>::infinity()); }
    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return RealType(std::numeric_limits<RealType>::infinity()); }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_mean, _beta); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        _mean = parm.mean();
        _beta = parm.beta();
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

    /** Writes an @c laplace_distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, laplace_distribution, wd)
    {
        os << wd.param();
        return os;
    }

    /** Reads an @c laplace_distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, laplace_distribution, wd)
    {
        param_type parm;
        if(is >> parm) {
            wd.param(parm);
        }
        return is;
    }

    /**
     * Returns true if the two instances of @c laplace_distribution will
     * return identical sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(laplace_distribution, lhs, rhs)
    { return lhs._mean == rhs._mean && lhs._beta == rhs._beta; }

    /**
     * Returns true if the two instances of @c laplace_distribution will
     * return different sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(laplace_distribution)

private:
    RealType _mean;
    RealType _beta;
};

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_LAPLACE_DISTRIBUTION_HPP


/***
 * @file arma_extend.hpp
 * @author Ryan Curtin
 *
 * Include Armadillo extensions which currently are not part of the main
 * Armadillo codebase.
 *
 * This will allow the use of the ccov() function (which performs the same
 * function as cov(trans(X)) but without the cost of computing trans(X)).  This
 * also gives sparse matrix support, if it is necessary.
 */
#ifndef __MY_MLPACK_CORE_ARMA_EXTEND_ARMA_EXTEND_HPP
#define __MY_MLPACK_CORE_ARMA_EXTEND_ARMA_EXTEND_HPP

// Add batch constructor for sparse matrix (if version <= 3.810.0).
#define ARMA_EXTRA_SPMAT_PROTO mips/my_mlpack/core/arma_extend/SpMat_extra_bones.hpp
#define ARMA_EXTRA_SPMAT_MEAT  mips/my_mlpack/core/arma_extend/SpMat_extra_meat.hpp

#include <armadillo>

namespace arma {
  // u64/s64
  #include "typedef.hpp"
  #include "traits.hpp"
  #include "promote_type.hpp"
  #include "restrictors.hpp"
  #include "hdf5_misc.hpp"

  // ccov()
  #include "op_ccov_proto.hpp"
  #include "op_ccov_meat.hpp"
  #include "glue_ccov_proto.hpp"
  #include "glue_ccov_meat.hpp"
  #include "fn_ccov.hpp"

  // inplace_reshape()
  #include "fn_inplace_reshape.hpp"
};

#endif


//
// Implements iLQR (on a traditional chain) for nonlinear dynamics and cost.
//
// Arun Venkatraman (arunvenk@cs.cmu.edu)
// December 2016
//

#pragma once

#include <templated/taylor_expansion.hh>
#include <utils/debug_utils.hh>

#include <Eigen/Dense>

#include <functional>
#include <vector>

namespace ilqr
{

// Defined in templated/taylor_expansion.hh
//template<int _rows, int _cols>
//using Matrix = Eigen::Matrix<double, _rows, _cols>;
//
//template<int _rows>
//using Vector = Eigen::Matrix<double, _rows, 1>;

template<int xdim, int udim>
class iLQRSolver
{
    static_assert(xdim > 0, "State dimension should be greater than 0");
    static_assert(udim > 0, "Control dimension should be greater than 0");
public:
    using Dynamics = std::function<Vector<xdim>(const Vector<xdim> &x, const Vector<udim> &u)>;
    using Cost = std::function<double(const Vector<xdim> &x, const Vector<udim> &u, const int t)>;
    using FinalCost = std::function<double(const Vector<xdim> &x)>;

    iLQRSolver(const Dynamics &dynamics,
         const FinalCost &final_cost,
         const Cost &cost
         )
    {
        this->dynamics_ = dynamics;
        this->cost_ = cost;
        this->final_cost_ = final_cost;
    }

    // Computes the control at timestep t at xt.
    // :param alpha - Backtracking line search parameter. Setting to 1 gives regular forward pass.
    inline Vector<udim> compute_control_stepsize(const Vector<xdim> &xt,
            const int t, const double alpha) const;

    // Computes the locally-optimal iLQR solution.
    // :param x_init - Initial state from which to start the system from.
    // :param u_nominal - Initial control used for the whole sequence during
    //      the first forward pass.
    // :param mu - Levenberg-Marquardt parameter for damping the least-squares.
    //      Setting it to 0 gets the default behavior. The damping makes the
    //      state-space steps smaller over iterations.
    // :param start_alpha - The initial step size to use when solving.
    // :param cost_conv_ratio - When the cost over iterations changes by less than
    //      this ratio, then the optimization is stopped.
    // :param warm_start - Warm start the solver using the previous solution stored
    //      in the object from the last call to solve.
    // :param t_offset - The warm start time offset. The input argument T to this call
    //      of solve should be T_{last_call} - t_offset.
    //      For example, if 1, then the first timestep of the previous solution
    //      is removed.
    inline void solve(const int T, const Vector<xdim> &x_init,
            const Vector<udim> u_nominal, const double mu,
            const int max_iters = 1000, bool verbose = false,
            const double cost_convg_ratio = 1e-4, const double start_alpha = 1.0,
            const bool warm_start = false, const int t_offset = 0
            );

    // :param alpha - Backtracking line search parameter. Setting to 1 gives regular forward pass.
    inline double forward_pass(const Vector<xdim> x_init,
            std::vector<Vector<xdim>> &states, std::vector<Vector<udim>> &controls,
            const double alpha ) const;

    // Returns how many timesteps we have computed control policies for.
    inline int timesteps() const;

private:
    Dynamics dynamics_;
    Cost cost_;
    FinalCost final_cost_;

    // Feedback control gains.
    std::vector<Matrix<udim, xdim>> Ks_;
    std::vector<Vector<udim>> ks_;

    // Linearization points.
    std::vector<Vector<xdim>> xhat_;
    std::vector<Vector<udim>> uhat_;

    // Performs one timestep of the bellman backup.
    // :param t - passed to the cost runction
    // :param mu - Levenberg-Marquardt parameter
    void bellman_backup(const int t, const double mu,
        const Matrix<xdim,xdim> &Vt1, const Matrix<1,xdim> &Gt1,
        Matrix<xdim,xdim> &Vt, Matrix<1,xdim> &Gt,
        Matrix<udim,xdim> &Kt, Vector<udim> &kt);

};

} // namespace lqr

#include <templated/iLQR_impl.hh>


#include <boost/test/unit_test.hpp>
#include "algorithms/math/roman_to_int.hpp"

BOOST_AUTO_TEST_SUITE(ConversionOfRomanToIntegerNumber)

BOOST_AUTO_TEST_CASE(main_numbers)
{
    BOOST_CHECK(1 == Algo::Math::RomanToInt::Convert("I"));
    BOOST_CHECK(5 == Algo::Math::RomanToInt::Convert("V"));
    BOOST_CHECK(10 == Algo::Math::RomanToInt::Convert("X"));
    BOOST_CHECK(50 == Algo::Math::RomanToInt::Convert("L"));
    BOOST_CHECK(100 == Algo::Math::RomanToInt::Convert("C"));
    BOOST_CHECK(500 == Algo::Math::RomanToInt::Convert("D"));
    BOOST_CHECK(1000 == Algo::Math::RomanToInt::Convert("M"));
}

BOOST_AUTO_TEST_CASE(main_susbstract_numbers)
{
    BOOST_CHECK(4 == Algo::Math::RomanToInt::Convert("IV"));
    BOOST_CHECK(9 == Algo::Math::RomanToInt::Convert("IX"));
    BOOST_CHECK(40 == Algo::Math::RomanToInt::Convert("XL"));
    BOOST_CHECK(90 == Algo::Math::RomanToInt::Convert("XC"));
    BOOST_CHECK(400 == Algo::Math::RomanToInt::Convert("CD"));
    BOOST_CHECK(900 == Algo::Math::RomanToInt::Convert("CM"));
}

BOOST_AUTO_TEST_CASE(main_valid_numbers)
{
    BOOST_CHECK(2 == Algo::Math::RomanToInt::Convert("II"));
    BOOST_CHECK(3 == Algo::Math::RomanToInt::Convert("III"));
    BOOST_CHECK(8 == Algo::Math::RomanToInt::Convert("VIII"));
    BOOST_CHECK(14 == Algo::Math::RomanToInt::Convert("XIV"));
    BOOST_CHECK(39 == Algo::Math::RomanToInt::Convert("XXXIX"));
    BOOST_CHECK(44 == Algo::Math::RomanToInt::Convert("XLIV"));
    BOOST_CHECK(58 == Algo::Math::RomanToInt::Convert("LVIII"));
    BOOST_CHECK(82 == Algo::Math::RomanToInt::Convert("LXXXII"));
    BOOST_CHECK(3994 == Algo::Math::RomanToInt::Convert("MMMCMXCIV"));
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * Common header used throughout project
 */

#pragma once

#include <Eigen/Eigen>

namespace solo {
/**
 * @brief Vector2d shortcut for the eigen vector of size 1.
 */
typedef Eigen::Matrix<double, 1, 1> Vector1d;

/**
 * @brief Vector2d shortcut for the eigen vector of size 2.
 */
typedef Eigen::Matrix<double, 2, 1> Vector2d;

/**
 * @brief Vector2d shortcut for the eigen vector of size 6.
 */
typedef Eigen::Matrix<double, 6, 1> Vector6d;

/**
 * @brief Vector8d shortcut for the eigen vector of size 8.
 */
typedef Eigen::Matrix<double, 8, 1> Vector8d;

/**
 * @brief Vector8d shortcut for the eigen vector of size 12.
 */
typedef Eigen::Matrix<double, 12, 1> Vector12d;

}  // namespace solo


#define _USE_MATH_DEFINES
#include <cmath>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <vector>
#include <cmath>
#include <stack>
#include <iomanip>
#include "prUtil.h"
#include "prCommon.h"

namespace pr {

cv::Mat getHomoMatFromInExNm(const cv::Mat &in_mat,
                             const cv::Mat &ex_mat,
                             const cv::Mat &nm_mat) {
    cv::Mat ex_mat_124 = (cv::Mat_<float>(3, 3) << ex_mat.at<float>(0, 0),
                                                   ex_mat.at<float>(0, 1),
                                                   ex_mat.at<float>(0, 3),
                                                   ex_mat.at<float>(1, 0),
                                                   ex_mat.at<float>(1, 1),
                                                   ex_mat.at<float>(1, 3),
                                                   ex_mat.at<float>(2, 0),
                                                   ex_mat.at<float>(2, 1),
                                                   ex_mat.at<float>(2, 3));
    cv::Mat H = in_mat * ex_mat_124 * nm_mat;
    H /= H.at<float>(2, 2);
    return H.clone();
}

cv::Mat getHomoMatFromInEx(const cv::Mat &in_mat,
                           const cv::Mat &ex_mat) {
    cv::Mat ex_mat_124 = (cv::Mat_<float>(3, 3) << ex_mat.at<float>(0, 0),
                                                   ex_mat.at<float>(0, 1),
                                                   ex_mat.at<float>(0, 3),
                                                   ex_mat.at<float>(1, 0),
                                                   ex_mat.at<float>(1, 1),
                                                   ex_mat.at<float>(1, 3),
                                                   ex_mat.at<float>(2, 0),
                                                   ex_mat.at<float>(2, 1),
                                                   ex_mat.at<float>(2, 3));
    cv::Mat H = in_mat * ex_mat_124;
    H /= H.at<float>(2, 2);
    return H.clone();
}

cv::Mat getAxisAngleFromRotationMatirx(const cv::Mat &R)
{
    float a = acosf((trace(R).val[0] - 1) / 2);
    cv::Mat r;
    if (a < EPS)
    {
        r = (cv::Mat_<float>(3, 1) << R.at<float>(2, 1) - R.at<float>(1, 2),
                                      R.at<float>(0, 2) - R.at<float>(2, 0),
                                      R.at<float>(1, 0) - R.at<float>(0, 1));
        r *= 0.5;
    }
    else if (a >(M_PI - EPS))
    {
        cv::Mat S = 0.5 * (R - (cv::Mat_<float>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1));
        float b = sqrt(S.at<float>(0, 0) + 1);
        float c = sqrt(S.at<float>(1, 1) + 1);
        float d = sqrt(S.at<float>(2, 2) + 1);
        if (b > EPS)
        {
            c = S.at<float>(1, 0) / b;
            d = S.at<float>(2, 0) / b;
        }
        else if (c > EPS)
        {
            b = S.at<float>(0, 1) / c;
            d = S.at<float>(2, 1) / c;
        }
        else
        {
            b = S.at<float>(0, 2) / d;
            c = S.at<float>(1, 2) / d;
        }
        r = (cv::Mat_<float>(3, 1) << b, c, d);
    }
    else
    {
        r = (cv::Mat_<float>(3, 1) << R.at<float>(2, 1) - R.at<float>(1, 2),
                                      R.at<float>(0, 2) - R.at<float>(2, 0),
                                      R.at<float>(1, 0) - R.at<float>(0, 1));
        r *= (a / 2 / sinf(a));
    }
    return r.clone();
}

cv::Mat getRotationMatrixFromAxisAngle(const cv::Mat &r)
{
    float rx, ry, rz;
    rx = r.at<float>(0);
    ry = r.at<float>(1);
    rz = r.at<float>(2);
    cv::Mat I = cv::Mat::eye(3, 3, CV_32F);
    cv::Mat W = getCrossProductMatrix(r);
    cv::Mat W2 = W * W;
    const float a = norm(r);
    if (a < EPS)
        return I + W + 0.5 * W2;
    else
        return I + W * sin(a) / a + W2 * (1 - cos(a)) / (a * a);
}

cv::Mat getAaParametersFromExtrinsicMatirx(const cv::Mat &ex_mat)
{
    cv::Mat p(6, 1, CV_32F);
    getAxisAngleFromRotationMatirx(ex_mat(cv::Rect(0, 0, 3, 3))).copyTo(p(cv::Rect(0, 0, 1, 3)));
    ex_mat(cv::Rect(3, 0, 1, 3)).copyTo(p(cv::Rect(0, 3, 1, 3)));
    return p.clone();
}

cv::Mat getExtrinsicMatrixFromAaParameters(const cv::Mat &p)
{
    cv::Mat ex_mat = (cv::Mat_<float>(4, 4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
    getRotationMatrixFromAxisAngle(p(cv::Rect(0, 0, 1, 3))).copyTo(ex_mat(cv::Rect(0, 0, 3, 3)));
    p(cv::Rect_<float>(0, 3, 1, 3)).copyTo(ex_mat(cv::Rect_<float>(3, 0, 1, 3)));
    return ex_mat.clone();
}

cv::Mat getCrossProductMatrix(const cv::Mat &r)
{
    float rx = r.at<float>(0);
    float ry = r.at<float>(1);
    float rz = r.at<float>(2);
    cv::Mat W = (cv::Mat_<float>(3, 3) << 0, -rz, ry, rz, 0, -rx, -ry, rx, 0);
    return W.clone();
}

R12t getR12t(const cv::Mat &ex_mat)
{
    R12t R12_t;
    R12_t.r11 = ex_mat.at<float>(0, 0);
    R12_t.r12 = ex_mat.at<float>(0, 1);
    R12_t.t1  = ex_mat.at<float>(0, 3);
    R12_t.r21 = ex_mat.at<float>(1, 0);
    R12_t.r22 = ex_mat.at<float>(1, 1);
    R12_t.t2  = ex_mat.at<float>(1, 3);
    R12_t.r31 = ex_mat.at<float>(2, 0);
    R12_t.r32 = ex_mat.at<float>(2, 1);
    R12_t.t3  = ex_mat.at<float>(2, 3);
    return R12_t;
}

JacobRr getJacobRr(const cv::Mat &r)
{
    JacobRr J_Rr;

    const float &rx = r.at<float>(0);
    const float &ry = r.at<float>(1);
    const float &rz = r.at<float>(2);

    float a = norm(r);
    if (a < EPS) {
        J_Rr.j11 = 0;
        J_Rr.j12 = -ry;
        J_Rr.j13 = -rz;
        J_Rr.j21 = ry / 2;
        J_Rr.j22 = rx / 2;
        J_Rr.j23 = -1;
        J_Rr.j31 = ry / 2;
        J_Rr.j32 = rx / 2;
        J_Rr.j33 = 1;
        J_Rr.j41 = -rx;
        J_Rr.j42 = 0;
        J_Rr.j43 = -rz;
        J_Rr.j51 = rz / 2;
        J_Rr.j52 = -1;
        J_Rr.j53 = rx / 2;
        J_Rr.j61 = 1;
        J_Rr.j62 = rz / 2;
        J_Rr.j63 = ry / 2;
    }
    else {
        cv::Mat W = getCrossProductMatrix(r);
        cv::Mat R = getRotationMatrixFromAxisAngle(r);
        cv::Mat Id_R = cv::Mat::eye(3, 3, CV_32F) - R;
        cv::Mat M = W * Id_R;
        cv::Mat R_a2 = R / (a*a);
        cv::Mat dR_dr0 = (r.at<float>(0)*W + getCrossProductMatrix(M.col(0)))*R_a2;
        cv::Mat dR_dr1 = (r.at<float>(1)*W + getCrossProductMatrix(M.col(1)))*R_a2;
        cv::Mat dR_dr2 = (r.at<float>(2)*W + getCrossProductMatrix(M.col(2)))*R_a2;
        J_Rr.j11 = dR_dr0.at<float>(0, 0);
        J_Rr.j12 = dR_dr1.at<float>(0, 0);
        J_Rr.j13 = dR_dr2.at<float>(0, 0);
        J_Rr.j21 = dR_dr0.at<float>(0, 1);
        J_Rr.j22 = dR_dr1.at<float>(0, 1);
        J_Rr.j23 = dR_dr2.at<float>(0, 1);
        J_Rr.j31 = dR_dr0.at<float>(1, 0);
        J_Rr.j32 = dR_dr1.at<float>(1, 0);
        J_Rr.j33 = dR_dr2.at<float>(1, 0);
        J_Rr.j41 = dR_dr0.at<float>(1, 1);
        J_Rr.j42 = dR_dr1.at<float>(1, 1);
        J_Rr.j43 = dR_dr2.at<float>(1, 1);
        J_Rr.j51 = dR_dr0.at<float>(2, 0);
        J_Rr.j52 = dR_dr1.at<float>(2, 0);
        J_Rr.j53 = dR_dr2.at<float>(2, 0);
        J_Rr.j61 = dR_dr0.at<float>(2, 1);
        J_Rr.j62 = dR_dr1.at<float>(2, 1);
        J_Rr.j63 = dR_dr2.at<float>(2, 1);
    }
    return J_Rr;
}

float polygonArea(const cv::Mat &corners) {
    float area = 0.f;
    int num = corners.cols;
    int j = num - 1;
    for (int i = 0; i < num; ++i) {
        area += (corners.at<float>(0, j) + corners.at<float>(0, i))
               *(corners.at<float>(1, j) - corners.at<float>(1, i));
        j = i;
    }
    return std::abs(area) * 0.5f;
}

void calRegion(const cv::Mat &boundary,
               const cv::Mat &H,
               int tw,
               int th,
               int iw,
               int ih,
               float scale,
               cv::Mat *corners,
               Region *region) {
    (*corners) = H * boundary;
    auto w = (cv::Mat(1.f / (*corners)(cv::Rect(0, 2, 4, 1)))).clone();
    cv::multiply((*corners)(cv::Rect(0, 0, 4, 1)), w, (*corners)(cv::Rect(0, 0, 4, 1)));
    cv::multiply((*corners)(cv::Rect(0, 1, 4, 1)), w, (*corners)(cv::Rect(0, 1, 4, 1)));

    double x_min, x_max, y_min, y_max, x_center, y_center, x_length, y_length;
    cv::minMaxIdx((*corners)(cv::Rect(0, 0, 4, 1)), &x_min, &x_max);
    cv::minMaxIdx((*corners)(cv::Rect(0, 1, 4, 1)), &y_min, &y_max);
    x_center = (x_max + x_min) / 2.0;
    y_center = (y_max + y_min) / 2.0;
    x_length = x_max - x_center;
    y_length = y_max - y_center;
    x_max = x_center + x_length * scale;
    y_max = y_center + y_length * scale;
    x_min = x_center - x_length * scale;
    y_min = y_center - y_length * scale;
    region->x_max = int(std::min(ceil(x_max), iw - 1.0));
    region->y_max = int(std::min(ceil(y_max), ih - 1.0));
    region->x_min = int(std::max(floor(x_min), 0.0));
    region->y_min = int(std::max(floor(y_min), 0.0));
}

void calPoseDiff(const cv::Mat &p1,
                 const cv::Mat &p2,
                 float *diff_r,
                 float *diff_t)
{
    auto R1 = getRotationMatrixFromAxisAngle(p1(cv::Rect(0, 0, 1, 3))).clone();
    auto t1 = p1(cv::Rect(0, 3, 1, 3)).clone();
    auto R2 = getRotationMatrixFromAxisAngle(p2(cv::Rect(0, 0, 1, 3))).clone();
    auto t2 = p2(cv::Rect(0, 3, 1, 3)).clone();
    cv::Mat R2_transpose;
    transpose(R2, R2_transpose);
    *diff_r = acosf((trace(R2_transpose*R1).val[0] - 1) / 2) * 180.f / M_PI;
    *diff_t = norm(t1 - t2) / norm(t1) * 100;
}

void calDeltaP(float *JtJ_ptr,
               float *JtE_ptr,
               cv::Mat *delta_p,
               cv::Mat *JtE) {
    Eigen::Matrix<float, 6, 6> H = Eigen::Map<Eigen::Matrix<float, 6, 6>>(JtJ_ptr);
    Eigen::Matrix<float, 6, 1> delta_p_eigen = H.inverse()*Eigen::Map<Eigen::Matrix<float, 6, 1>>(JtE_ptr);
    for (int i = 0; i < 6; ++i) {
        (*delta_p).at<float>(i) = delta_p_eigen[i];
        (*JtE).at<float>(i) = JtE_ptr[i];
    }
}

}  // namespace pr