// ---------------------------------------------------------------------
//
// 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 (2D)
// 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
// This code was run with a tight solver/preconditioner tolerance
// through 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 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/dof_map.h>
#include <libmesh/equation_systems.h>
#include <libmesh/exodusII_io.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 fac = 0.0;
static double eta_s = 0.0;
static double Re = 0.0;
static double MU = 0.0;
static double L = 0.0;
static double x_loc = 0.0;
static double y_loc_max = 2.0;
static double y_loc_min = -2.0;
static double R1 = 0.0;
static double R2 = 0.0;
static double OMEGA1 = 0.0;
static double OMEGA2 = 0.0;
static double AA = 0.0;
static double BB = 0.0;
static double shift = 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*/)
{
    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));

    return;
} // tether_force_function

} // namespace ModelData
using namespace ModelData;

void velocity_convergence(Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                          const int u_idx,
                          const double data_time,
                          const string& data_dump_dirname);

void compute_velocity_profile(Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                              const int u_idx,
                              const double data_time,
                              const string& data_dump_dirname);

void compute_pressure_profile(Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                              const int p_idx,
                              const double data_time,
                              const string& data_dump_dirname);

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

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);

/*******************************************************************************
 * 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.
        Mesh mesh_interior(init.comm(), NDIM);
        const double dx = input_db->getDouble("DX");
        MU = input_db->getDouble("MU");
        fac = input_db->getDouble("FAC");
        Re = input_db->getDouble("Re");
        shift = input_db->getDouble("SHIFT");
        L = input_db->getDouble("L");
        const double mfac = input_db->getDouble("MFAC");
        const double ds = 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
        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

#ifdef LIBMESH_HAVE_TRIANGLE
        const int num_circum_nodes1 = ceil(2.0 * M_PI * R1 / ds);
        for (int k = 0; k < num_circum_nodes1; ++k)
        {
            const double theta1 = 2.0 * M_PI * static_cast<double>(k) / static_cast<double>(num_circum_nodes1);
            mesh_interior.add_point(libMesh::Point(R1 * cos(theta1), R1 * sin(theta1)));
        }
        TriangleInterface inner_triangle(mesh_interior);
        inner_triangle.triangulation_type() = TriangleInterface::GENERATE_CONVEX_HULL;
        inner_triangle.elem_type() = Utility::string_to_enum<ElemType>(elem_type);
        inner_triangle.desired_area() = 1.5 * sqrt(3.0) / 4.0 * ds * ds;
        inner_triangle.insert_extra_points() = true;
        inner_triangle.smooth_after_generating() = true;
        inner_triangle.triangulate();
#else
        TBOX_ERROR("ERROR: libMesh appears to have been configured without support for Triangle,\n"
                   << "       but Triangle is required for TRI3 or TRI6 elements.\n");
#endif

        // Ensure nodes on the surface are on the analytic boundary.
        MeshBase::element_iterator el_end1 = mesh_interior.elements_end();
        for (MeshBase::element_iterator el = mesh_interior.elements_begin(); el != el_end1; ++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) continue;
                for (unsigned int k = 0; k < elem->n_nodes(); ++k)
                {
                    if (!elem->is_node_on_side(k, side)) continue;
                    Node& n = elem->node_ref(k);
                    n = R1 * n.unit();
                }
            }
        }
        mesh_interior.prepare_for_use();

        BoundaryMesh boundary_mesh_interior(mesh_interior.comm(), mesh_interior.mesh_dimension() - 1);
        BoundaryInfo& boundary_info = mesh_interior.get_boundary_info();
        boundary_info.sync(boundary_mesh_interior);
        boundary_mesh_interior.prepare_for_use();

        bool use_boundary_mesh = true;
        Mesh& inner_mesh = use_boundary_mesh ? boundary_mesh_interior : mesh_interior;

        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<IBStrategy> ib_ops;
        ib_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_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.

        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));
        Pointer<IIMethod> ibfe_ops = ib_ops;
        ibfe_ops->initializeFEEquationSystems();

        EquationSystems* inner_equation_systems = ibfe_ops->getFEDataManager()->getEquationSystems();
        IIMethod::LagSurfaceForceFcnData tether_force_inner_data(tether_force_function_inner, sys_data);

        ibfe_ops->registerLagSurfaceForceFunction(tether_force_inner_data);

        // Create Eulerian initial condition specification objects.

        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);
        }

        // 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.
        ibfe_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";

            VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
            Pointer<hier::Variable<NDIM> > u_var = time_integrator->getVelocityVariable();
            Pointer<VariableContext> current_ctx = time_integrator->getCurrentContext();
            const int u_idx = var_db->mapVariableAndContextToIndex(u_var, current_ctx);

            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);

            // 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);
                }
            }
            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);
            }

            postprocess_data(patch_hierarchy,
                             navier_stokes_integrator,
                             inner_mesh,
                             inner_equation_systems,
                             iteration_num,
                             loop_time,
                             postproc_data_dump_dirname);

            if (dump_postproc_data && (iteration_num % postproc_data_dump_interval == 0 || last_step))
            {
                pout << "\nWriting state data...\n\n";

                compute_velocity_profile(patch_hierarchy, u_idx, loop_time, postproc_data_dump_dirname);
                compute_pressure_profile(patch_hierarchy, p_idx, loop_time, postproc_data_dump_dirname);
            }
        }

        // Determine the accuracy of the computed solution.
        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);
        const int p_cloned_idx = var_db->registerClonedPatchDataIndex(p_var, p_idx);

        pressure_convergence(patch_hierarchy, p_idx, loop_time, postproc_data_dump_dirname);

        velocity_convergence(patch_hierarchy, u_idx, loop_time, postproc_data_dump_dirname);

        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 the Eulerian u at time " << loop_time << ":\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[2] = { -0.45 * L, -0.45 * L };
    const double X_max[2] = { 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 yu = patch_x_lower[1] + patch_dx[1] * (lower_idx(1) - patch_lower(1) + 0.5);
                    const double xu = patch_x_lower[0] + patch_dx[0] * (lower_idx(0) - patch_lower(0));

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

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

                    if (sqrt(xv * xv + yv * yv) < R1)
                    {
                        v_ex = OMEGA1 * xv;
                    }
                    else if (sqrt(xv * xv + yv * yv) > R2)
                    {
                        v_ex = 0.0;
                    }
                    else
                    {
                        v_ex = xv * (AA + BB / (xv * xv + yv * yv));
                    }

                    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[2] = { -0.45 * L, -0.45 * L };
    const double X_max[2] = { 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.
            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);

                    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";
    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();

    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);
                double P_i_qp = -(P_j_qp - P_o_qp);
                interpolate(&tau1(0), qp, x_node, dphi_dxi);

                tau2 = VectorValue<double>(0.0, 0.0, 1.0);

                n = tau1.cross(tau2);
                n = n.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_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));
                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_i_qp - p_ex_qp) * std::abs(P_i_qp - p_ex_qp) * JxW[qp];
                P_max_norm = std::max(P_max_norm, std::abs(P_i_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_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)));
            }
        }

        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

void
compute_velocity_profile(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();

    const double X_min[2] = { x_loc, -0.5 * L };
    const double X_max[2] = { x_loc, 0.5 * L };
    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();

            const bool inside_patch = x_loc >= patch_x_lower[0] && x_loc <= patch_x_upper[0] &&
                                      !(patch_x_upper[1] < y_loc_min || patch_x_lower[1] > y_loc_max);
            if (!inside_patch) continue;

            // 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);
            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;
                    CellIndex<NDIM> upper_idx = lower_idx;
                    upper_idx(0) += 1;
                    const double y = patch_x_lower[1] + patch_dx[1] * (lower_idx(1) - patch_lower(1) + 0.5);
                    const double x0 = patch_x_lower[0] + patch_dx[0] * (lower_idx(0) - patch_lower(0));
                    const double x1 = x0 + patch_dx[0];
                    const double u0 = (*u_data)(SideIndex<NDIM>(lower_idx, 0, SideIndex<NDIM>::Lower));
                    const double u1 = (*u_data)(SideIndex<NDIM>(upper_idx, 0, SideIndex<NDIM>::Lower));
                    pos_values.push_back(y);
                    pos_values.push_back(u0 + (u1 - u0) * (x_loc - x0) / (x1 - x0));
                }
            }
        }
    }

    const int nprocs = SAMRAI_MPI::getNodes();
    const int rank = SAMRAI_MPI::getRank();
    vector<int> data_size(nprocs, 0);
    data_size[rank] = static_cast<int>(pos_values.size());
    SAMRAI_MPI::sumReduction(&data_size[0], nprocs);
    int offset = 0;
    offset = std::accumulate(&data_size[0], &data_size[rank], offset);
    int size_array = 0;
    size_array = std::accumulate(&data_size[0], &data_size[0] + nprocs, size_array);

    // Write out the result in a file.
    string file_name = data_dump_dirname + "/" + "u_y_";
    char temp_buf[128];
    sprintf(temp_buf, "%.8f", data_time);
    file_name += temp_buf;

    MPI_Status status;
    MPI_Offset mpi_offset;
    MPI_File file;
    MPI_File_open(MPI_COMM_WORLD, file_name.c_str(), MPI_MODE_CREATE | MPI_MODE_WRONLY, MPI_INFO_NULL, &file);

    // First write the total size of the array.
    if (rank == 0)
    {
        mpi_offset = 0;
        MPI_File_seek(file, mpi_offset, MPI_SEEK_SET);
        MPI_File_write(file, &size_array, 1, MPI_INT, &status);
    }

    mpi_offset = sizeof(double) * offset + sizeof(int);
    MPI_File_seek(file, mpi_offset, MPI_SEEK_SET);
    MPI_File_write(file, &pos_values[0], data_size[rank], MPI_DOUBLE, &status);
    MPI_File_close(&file);

    return;
} // compute_velocity_profile

void
compute_pressure_profile(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();

    const double X_min[2] = { x_loc, -0.5 * L };
    const double X_max[2] = { x_loc, 0.5 * L };
    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();

            const bool inside_patch = x_loc >= patch_x_lower[0] && x_loc <= patch_x_upper[0] &&
                                      !(patch_x_upper[1] < y_loc_min || patch_x_lower[1] > y_loc_max);
            if (!inside_patch) continue;

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

            const Pointer<CellData<NDIM, double> > p_data = patch->getPatchData(p_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;
                    CellIndex<NDIM> upper_idx = lower_idx;
                    upper_idx(0) += 1;

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

                    const double x1 = x0 + patch_dx[0];
                    const double p0 = (*p_data)(lower_idx);
                    const double p1 = (*p_data)(upper_idx);
                    pos_values.push_back(y);
                    pos_values.push_back(p0 + (p1 - p0) * (x_loc - x0) / (x1 - x0));
                }
            }
        }
    }

    const int nprocs = SAMRAI_MPI::getNodes();
    const int rank = SAMRAI_MPI::getRank();
    vector<int> data_size(nprocs, 0);
    data_size[rank] = static_cast<int>(pos_values.size());
    SAMRAI_MPI::sumReduction(&data_size[0], nprocs);
    int offset = 0;
    offset = std::accumulate(&data_size[0], &data_size[rank], offset);
    int size_array = 0;
    size_array = std::accumulate(&data_size[0], &data_size[0] + nprocs, size_array);

    // Write out the result in a file.
    string file_name = data_dump_dirname + "/" + "p_";
    char temp_buf[128];
    sprintf(temp_buf, "%.8f", data_time);
    file_name += temp_buf;

    MPI_Status status;
    MPI_Offset mpi_offset;
    MPI_File file;
    MPI_File_open(MPI_COMM_WORLD, file_name.c_str(), MPI_MODE_CREATE | MPI_MODE_WRONLY, MPI_INFO_NULL, &file);

    // First write the total size of the array.
    if (rank == 0)
    {
        mpi_offset = 0;
        MPI_File_seek(file, mpi_offset, MPI_SEEK_SET);
        MPI_File_write(file, &size_array, 1, MPI_INT, &status);
    }

    mpi_offset = sizeof(double) * offset + sizeof(int);
    MPI_File_seek(file, mpi_offset, MPI_SEEK_SET);
    MPI_File_write(file, &pos_values[0], data_size[rank], MPI_DOUBLE, &status);
    MPI_File_close(&file);

    return;
} // compute_pressure_profile


//
// Created by Dominik Krupke, http://krupke.cc on 10/5/17.
//

#include <boost/program_options.hpp>
#include <iostream>
#include "problem_instance/grid_graph.h"
#include "problem_instance/save_and_load.h"
#include "apx/approximation.h"
#include "problem_instance/solution/visualization.h"
#include "arkin-apx/approximation.h"
#include "ip/general_cc_solver/ip_solver.h"
#include "ip/integer_programming.h"
#include "arkin-apx/approximation.h"
#include "solver.h"

namespace po = boost::program_options;

enum class Problem {
  CC, TOUR, SUBSET_CC, SUBSET_TOUR, PENALTY_CC, PENALTY_TOUR
};

struct LogEntry {
  std::string instance;
  std::string params;
  double lower_bound;
  double best_solution;
  double runtime;
  int nr_pixel_covered;
};

void run(std::string instance,
         turncostcover::Costs cost,
         Problem problem,
         turncostcover::Algorithm algorithm,
         bool use_callbacks,
         size_t timeout,
         LogEntry *log)
{
  using clock = std::chrono::high_resolution_clock;
  using std::chrono::duration_cast;
  using time_unit = std::chrono::milliseconds;

  namespace tc = turncostcover;
  namespace aapx = tc::arkinapx;

  auto start = clock::now();
  tc::IntegralSolution solution;
  tc::GridGraph gg;
  switch (problem) {
    case Problem::CC: {
      gg = turncostcover::LoadInstance(instance);
      solution = tc::ComputeFullCycleCover(gg, cost, algorithm, timeout);
      break;
    }
    case Problem::TOUR: {
      gg = tc::LoadInstance(instance);
      solution = tc::ComputeFullTour(gg, cost, algorithm, timeout, use_callbacks);
      break;
    }
    case Problem::SUBSET_CC:
    case Problem::SUBSET_TOUR:
    case Problem::PENALTY_CC:
    case Problem::PENALTY_TOUR:
    default: assert(false);
  }
  auto end = clock::now();
  log->lower_bound = solution.GetLowerBound();
  log->best_solution = solution.GetCoverageObjectiveValue(cost);
  if (problem == Problem::PENALTY_CC || problem == Problem::PENALTY_TOUR) {
    //TODO add penalties
    assert(false);
  }
  int nr_covered_fields = 0;
  for (auto v: gg.GetVertices()) {
    if (solution.IsCovered(v)) nr_covered_fields++;
  }
  log->nr_pixel_covered = nr_covered_fields;
  log->runtime = duration_cast<time_unit>(end - start).count();
}

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

  po::options_description desc("Allowed options");
  std::string filename;
  std::string problem;
  std::string algorithm;
  std::string output_file;
  size_t timeout;
  turncostcover::Costs costs;

  desc.add_options()
      ("help,h", "produce help message")
      ("input,i", po::value<std::string>(&filename), "Input file")
      ("problem,p",
       po::value<std::string>(&problem),
       "Problem name: fcc,ft,scc,st,pcc,pt")
      ("algorithm,a",
       po::value<std::string>(&algorithm),
       "Algorithm to use: ip,apx,aapx")
      ("turncost,t",
       po::value<turncostcover::CostUnit>(&(costs.turn_costs)),
       "turn costs")
      ("distcost,d",
       po::value<turncostcover::CostUnit>(&(costs.dist_costs))->default_value(1),
       "distance costs")
      ("timeout",
       po::value<size_t>(&timeout)->default_value(36000, "10 hours"),
       "timeout for IP in s.")
      ("penalty", po::value<double>(), "Uniform penalty for all fields")
      ("output,o", po::value<std::string>(&output_file), "Output file")
      ("nocallbacks", "Deactivates the usage of callbacks. Instead only to an optimally solved problem constraints are "
                      "added. This can sometimes be faster because the turn costs eliminate many subtours.");

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

  bool use_callbacks = vm.count("nocallbacks") == 0;

  //print help
  if (vm.count("help") > 0) {
    std::cout << desc << std::endl;
    return 0;
  }

  Problem p;
  if (problem == "fcc") p = Problem::CC;
  else if (problem == "ft") p = Problem::TOUR;
  else if (problem == "pcc") p = Problem::PENALTY_CC;
  else if (problem == "pt") p = Problem::PENALTY_TOUR;
  else {
    std::cerr << "Unknown Problem " << problem << std::endl;
    std::exit(1);
  }

  assert(vm.count("penalty") == 0 || algorithm != "aapx");
  using turncostcover::Algorithm;
  Algorithm a;
  if (algorithm == "ip") a = Algorithm::IP;
  else if (algorithm == "apx") a = Algorithm::KRUPKE_APX;
  else if (algorithm == "aapx") a = Algorithm::ARKINETAL_APX;
  else {
    std::cerr << "Unkown Algorithm " << algorithm << std::endl;
    std::exit(1);
  }
  LogEntry log;
  log.instance = filename;
  for (int i = 0; i < argc; ++i) {
    log.params += argv[i] + std::string("|");
  }
  try {
    if (vm.count("penalty")) {
      assert(a == Algorithm::KRUPKE_APX);
      auto penalty = vm.at("penalty").as<double>();
      std::map<turncostcover::Field, double> penalties;
      auto gg = turncostcover::LoadInstance(filename);
      for (auto v: gg.GetVertices()) {
        penalties[v] = penalty;
      }
      using clock = std::chrono::high_resolution_clock;
      using std::chrono::duration_cast;
      using time_unit = std::chrono::milliseconds;

      auto start = clock::now();
      turncostcover::IntegralSolution solution;
      if (p == Problem::PENALTY_CC) {
        solution = turncostcover::apx::ApproximatePenaltyCycleCover(gg,
                                                                    penalties,
                                                                    costs);
      } else if (p == Problem::PENALTY_TOUR) {
        solution =
            turncostcover::apx::ApproximatePenaltyTour(gg, penalties, costs);
      } else {
        assert(false);
      }
      auto end = clock::now();
      log.lower_bound = solution.GetLowerBound();
      log.best_solution = solution.GetCoverageObjectiveValue(costs);
      int nr_covered_fields = 0;
      for (auto v: gg.GetVertices()) {
        if (!solution.IsCovered(v)) log.best_solution += penalties.at(v);
        else nr_covered_fields++;
      }
      log.nr_pixel_covered = nr_covered_fields;
      log.runtime = duration_cast<time_unit>(end - start).count();
    } else {
      run(filename, costs, p, a, use_callbacks, timeout, &log);
    }
  } catch (turncostcover::CouldNotLoadInstanceException &e) {
    std::cerr << e.message << std::endl;
    std::exit(1);
  }

  std::ofstream log_file{output_file, std::ios_base::app};
  if (log_file.is_open()) {
    auto t = std::time(nullptr);
    auto tm = *std::localtime(&t);
    log_file << std::put_time(&tm, "%Y-%m-%d_%H-%M-%S") << "\t" << log.instance
             << "\t" << log.params << "\t" << log.lower_bound << "\t"
             << log.best_solution << "\t"
             << log.runtime << "\t"
             << log.nr_pixel_covered << std::endl;
  } else {
    std::cerr << "Could not log!" << std::endl;
  }
}


#include "camview.h"
#include "gldrow.h"
#include "workspace.h"
#include <QtMath>

#include <Eigen/Geometry>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "MultipleViewTriangulation.h"
#include <iostream>
#include "easytool.h"

using namespace Eigen;

CamView::CamView(QWidget *parent) : QOpenGLWidget(parent),
                                    m_model(0),
                                    m_camera(20.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f),
                                    m_program(0)
{
}

CamView::~CamView()
{
    makeCurrent();

    delete m_model;
    delete m_program;

    doneCurrent();
}

void CamView::initializeGL()
{
    initializeOpenGLFunctions();
    m_program = new QOpenGLShaderProgram(this);
    m_program->addShaderFromSourceFile(QOpenGLShader::Vertex, ":/shader/shader.vert");
    m_program->addShaderFromSourceFile(QOpenGLShader::Fragment, ":/shader/shader.frag");
    m_program->link();

    // glEnable(GL_TEXTURE_2D);
    // glShadeModel(GL_SMOOTH);
    glClearColor(0.0, 0.0, 0.0, 0.0);
    //glClearDepth(1.0);
    glEnable(GL_DEPTH_TEST);
    // glDepthFunc(GL_LEQUAL);
    // glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);

    cameraInit(80, 30, 6.0f);

    QString applicationDirPath = QCoreApplication::applicationDirPath();
    loadModel(applicationDirPath + "/../resource/3d/f15/drone.obj");
}

void CamView::loadModel(QString filename)
{
    makeCurrent();
    if (m_model != 0)
    {
        delete m_model;
    }
    m_model = new Model(filename, this);
    doneCurrent();
}

void CamView::updateGL()
{
}

#define TRAN_SIZE 1000
void CamView::paintGL()
{

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    gluLookAt(eye[0], eye[1], eye[2],
              center[0], center[1], center[2],
              up[0], up[1], up[2]);

    if (cs_show_model == Qt::CheckState::Checked)
    {
        glPushMatrix();
        QMatrix4x4 view; // = m_camera.view();
        view.lookAt(
            QVector3D(eye[0], eye[1], eye[2]),
            QVector3D(center[0], center[1], center[2]),
            QVector3D(up[0], up[1], up[2]));
        QMatrix4x4 model;
        if (size == 3)
        {
            view.translate(Xr[1](0, 0) / TRAN_SIZE, Xr[1](1, 0) / TRAN_SIZE, Xr[1](2, 0) / TRAN_SIZE);
            view.rotate(-pos[0](0, 0) * ARC_TO_DEG, 1, 0, 0);
            view.rotate(-pos[0](1, 0) * ARC_TO_DEG, 0, 1, 0);
            view.rotate(-pos[0](2, 0) * ARC_TO_DEG, 0, 0, 1);
        }

        m_program->bind();
        m_program->setUniformValue("view", view);
        m_program->setUniformValue("model", m_modelMat);
        m_program->setUniformValue("projection", m_projectionMat);

        if (m_model != 0)
        {
            m_model->draw(this);
        }
        m_program->release();
        glPopMatrix();
    }
    /*网格*/
    glPushMatrix();
    GLDrow::DrowGrid();
    glPopMatrix();

    //坐标轴显示
    if (cs_show_axis == Qt::CheckState::Checked)
    {
        glColor3f(1.0, 0.0, 0.0);
        GLDrow::DrowArrow(0, 0, 0, 0.8, 0, 0, 0.006);
        glColor3f(0.0, 1.0, 0.0);
        GLDrow::DrowArrow(0, 0, 0, 0.0, 0.8, 0, 0.006);
        glColor3f(0.0, 0.0, 1.0);
        GLDrow::DrowArrow(0, 0, 0, 0, 0, 0.8, 0.006);
    }

    if (isClearTrajectoryList)
    {
        isClearTrajectoryList = false;
        trajectoryList.clear();
    }

    for (int pm = 0; pm < size; pm++)
    {
        glPushMatrix();

        glTranslatef(Xr[pm](0, 0) / TRAN_SIZE, Xr[pm](1, 0) / TRAN_SIZE, Xr[pm](2, 0) / TRAN_SIZE);
        if (pm == 1 && size == 3)
            trajectoryList.append(Vector3d(Xr[pm](0, 0) / TRAN_SIZE, Xr[pm](1, 0) / TRAN_SIZE, Xr[pm](2, 0) / TRAN_SIZE));
        glColor3f(1.0, 0.0, 0.0);
        GLDrow::drawSphere();
        glPopMatrix();
    }

    if (cs_show_cam == Qt::CheckState::Checked)
        for (int i = 0; i < vision_param.CamNum; i++)
        {
            /*相机*/
            glPushMatrix();
            Matrix33d RR = vision_param.RGND;//.transpose();
            Vector3d  TT = -vision_param.TGND;
            vision_param.RTGND = EasyTool::getRT44d(RR,TT);
            Matrix44d R_T = EasyTool::getRT44d(vision_param.R[i], vision_param.T[i]);
            //R_T = R_T.inverse();
            //vision_param.RTGND = vision_param.RTGND.inverse();

            //R_T = R_T * vision_param.RTGND;
            R_T = vision_param.RTGND * R_T;
            //R_T = R_T.inverse();

            Matrix33d R_;
            Vector3d T_;
            EasyTool::RT44d(R_T, R_, T_);

            Vector3d v = R_.eulerAngles(0, 1, 2);

            glRotatef(v(0, 0) * ARC_TO_DEG, 1, 0, 0);
            glRotatef(v(1, 0) * ARC_TO_DEG, 0, 1, 0);
            glRotatef(v(2, 0) * ARC_TO_DEG, 0, 0, 1);

            glTranslatef(
                T_(0, 0) / TRAN_SIZE,
                T_(1, 0) / TRAN_SIZE,
                T_(2, 0) / TRAN_SIZE);

            // glBegin(GL_LINES);
            // glColor3f(0.0, 0.0, 1.0);
            // glVertex3f(0.0, 0.0, 0.0);
            // glVertex3f(0.0, 0.0, 10.0);
            // glEnd();

            GLDrow::DrowCam();
            glPopMatrix();
        }

    if (cs_show_trajectory == Qt::CheckState::Checked)
    {
        glColor3f(1.0, 0.0, 0.0);
        for (int i = 0; i < trajectoryList.size(); i++)
        {
            glBegin(GL_POINTS);
            glVertex3f(trajectoryList.at(i)[0], trajectoryList.at(i)[1], trajectoryList.at(i)[2]);
            glEnd();
        }
    }

    //Plan
    if (cs_show_plan == Qt::CheckState::Checked)
        if (ppList.size() >= 2)
        {
            for (int i = 0; i < ppList.size() - 1; i++)
            {
                if (ppList.at(i + 1)->state == PLAN_POINT_STATE_WAIT)
                    glColor3f(1.0, 1.0, 0.0);
                else if (ppList.at(i + 1)->state == PLAN_POINT_STATE_GOING)
                    glColor3f(0.0, 1.0, 1.0);
                else if (ppList.at(i + 1)->state == PLAN_POINT_STATE_ARRIVE)
                    glColor3f(1.0, 0.0, 1.0);

                GLDrow::DrowArrow(
                    ppList.at(i)->mpd->x, ppList.at(i)->mpd->y, ppList.at(i)->mpd->z,
                    ppList.at(i + 1)->mpd->x, ppList.at(i + 1)->mpd->y, ppList.at(i + 1)->mpd->z,
                    0.003);
            }
        }

    QMetaObject::invokeMethod(this, "update", Qt::QueuedConnection);
    view_fps_1s++;
}

void CamView::resizeGL(int w, int h)
{
    glViewport(0, 0, w, h);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(45.0f, w / float(h), 1.0f, 1000.0f);

    cameraTurn(yawCam, pitchCam, farCam);

    m_projectionMat.setToIdentity();
    m_projectionMat.perspective(45.0f, w / float(h), 1.0f, 1000.0f);

    //DBG("resizeGL");
}

void CamView::setAngle(float rol, float pit, float yaw)
{
    this->rol = rol;
    this->pit = pit;
    this->yaw = yaw;
}

void CamView::setPlan(QList<PlanPoint *> list)
{
    ppList = list;
}

void CamView::setPosition(Vector3d *Xr, int size, Vector3d *pos)
{
    // this->px = x;
    // this->py = y;
    // this->pz = z;
    this->Xr = Xr;
    this->size = size;
    this->pos = pos;
    point_fps_1s++;
}

void CamView::cameraInit(double yaw, double pitch, double R_long)
{

    yawCam = yaw;
    pitchCam = pitch;
    farCam = R_long;
}

void CamView::cameraTurn(double yaw, double pitch, double R_long)
{

    // if (pitch > 90)
    //     pitch = 90;
    // else if (pitch < -90)
    //     pitch = -90;

    double angle_yaw = yaw * M_PI / 180;
    double angle_pitch = pitch * M_PI / 180;

    Vector3d v(0, 1, 0);       //{ { 0 }, { 1 }, { 0 } };
    Vector3d v_top(0, 0, 1);   // { { 0 }, { 0 }, { 1 } };
    Vector3d yaw_k(0, 0, 1);   // { { 0 }, { 0 }, { 1 } };
    Vector3d pitch_k(1, 0, 0); // { { 1 }, { 0 }, { 0 } };

    Matrix3d E;
    E << 1, 0, 0,
        0, 1, 0,
        0, 0, 1;

    Matrix3d G_yaw;
    G_yaw << 0, -yaw_k[2], yaw_k[1],
        yaw_k[2], 0, -yaw_k[0],
        -yaw_k[1], yaw_k[0], 0;

    v = E * qCos(angle_yaw) * v + (1 - qCos(angle_yaw)) * yaw_k * yaw_k.transpose() * v + qSin(angle_yaw) * G_yaw * v;
    pitch_k = E * qCos(angle_yaw) * pitch_k + (1 - qCos(angle_yaw)) * yaw_k * yaw_k.transpose() * pitch_k + qSin(angle_yaw) * G_yaw * pitch_k;

    Matrix3d G_pitch;
    G_pitch << 0, -pitch_k[2], pitch_k[1],
        pitch_k[2], 0, -pitch_k[0],
        -pitch_k[1], pitch_k[0], 0;

    v = E * qCos(angle_pitch) * v + (1 - qCos(angle_pitch)) * pitch_k * pitch_k.transpose() * v + qSin(angle_pitch) * G_pitch * v;
    v = v * R_long;

    pitch_k << 1, 0, 0;

    v_top = E * qCos(angle_yaw) * v_top + (1 - qCos(angle_yaw)) * yaw_k * yaw_k.transpose() * v_top + qSin(angle_yaw) * G_yaw * v_top;
    pitch_k = E * qCos(angle_yaw) * pitch_k + (1 - qCos(angle_yaw)) * yaw_k * yaw_k.transpose() * pitch_k + qSin(angle_yaw) * G_yaw * pitch_k;

    G_pitch << 0, -pitch_k[2], pitch_k[1],
        pitch_k[2], 0, -pitch_k[0],
        -pitch_k[1], pitch_k[0], 0;

    v_top = E * qCos(angle_pitch) * v_top + (1 - qCos(angle_pitch)) * pitch_k * pitch_k.transpose() * v_top + qSin(angle_pitch) * G_pitch * v_top;

    eye[0] = v[0];
    eye[1] = v[1];
    eye[2] = v[2];
    center[0] = 0;
    center[1] = 0;
    center[2] = 0;
    up[0] = v_top[0];
    up[1] = v_top[1];
    up[2] = v_top[2];

    //DBG("yawCam:%0.2f\tpitchCam:%0.2f\tfarCam:%0.2f\t",yaw,pitch,R_long);
}

void CamView::mousePressEvent(QMouseEvent *event)
{
    if (event->buttons() == Qt::LeftButton)
    { //如果鼠标按下的是左键
        clickX = event->globalX();
        clickY = event->globalY();
        //DBG("LeftButton %d %d",event->globalX(),event->globalY());
    }
    else if (event->buttons() == Qt::RightButton)
    { //如果鼠标按下的是右键
        //DBG("RightButton %d %d",event->globalX(),event->globalY());
    }
}

void CamView::mouseMoveEvent(QMouseEvent *event)
{
    if (event->buttons() == Qt::LeftButton)
    { //如果鼠标按下的是左键
        //DBG("mouseMoveEvent %d %d",event->globalX(),event->globalY());

        if (clickX != event->globalX())
        {
            yawCam += (clickX - event->globalX()) * 1.0;
            clickX = event->globalX();
        }

        if (clickY != event->globalY())
        {
            pitchCam -= (clickY - event->globalY()) * 1.0;
            clickY = event->globalY();
        }

        cameraTurn(yawCam, pitchCam, farCam);
    }
}

void CamView::mouseReleaseEvent(QMouseEvent *event)
{
}

void CamView::mouseDoubleClickEvent(QMouseEvent *event)
{
}

void CamView::wheelEvent(QWheelEvent *event)
{

    //DBG("wheelEvent %d",event->delta());

    if (event->delta() > 0)
    {
        farCam -= 0.1 * 3;
    }
    else
    {
        farCam += 0.1 * 3;
    }

    cameraTurn(yawCam, pitchCam, farCam);
}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_HPP

#include <boost/numeric/bindings/lapack/computational/bdsdc.hpp>
#include <boost/numeric/bindings/lapack/computational/bdsqr.hpp>
#include <boost/numeric/bindings/lapack/computational/gbbrd.hpp>
#include <boost/numeric/bindings/lapack/computational/gbcon.hpp>
#include <boost/numeric/bindings/lapack/computational/gbequ.hpp>
#include <boost/numeric/bindings/lapack/computational/gbrfs.hpp>
#include <boost/numeric/bindings/lapack/computational/gbtrf.hpp>
#include <boost/numeric/bindings/lapack/computational/gbtrs.hpp>
#include <boost/numeric/bindings/lapack/computational/gebak.hpp>
#include <boost/numeric/bindings/lapack/computational/gebal.hpp>
#include <boost/numeric/bindings/lapack/computational/gebrd.hpp>
#include <boost/numeric/bindings/lapack/computational/gecon.hpp>
#include <boost/numeric/bindings/lapack/computational/geequ.hpp>
#include <boost/numeric/bindings/lapack/computational/gehrd.hpp>
#include <boost/numeric/bindings/lapack/computational/gelqf.hpp>
#include <boost/numeric/bindings/lapack/computational/geqlf.hpp>
#include <boost/numeric/bindings/lapack/computational/geqp3.hpp>
#include <boost/numeric/bindings/lapack/computational/geqrf.hpp>
#include <boost/numeric/bindings/lapack/computational/gerfs.hpp>
#include <boost/numeric/bindings/lapack/computational/gerqf.hpp>
#include <boost/numeric/bindings/lapack/computational/getrf.hpp>
#include <boost/numeric/bindings/lapack/computational/getri.hpp>
#include <boost/numeric/bindings/lapack/computational/getrs.hpp>
#include <boost/numeric/bindings/lapack/computational/ggbak.hpp>
#include <boost/numeric/bindings/lapack/computational/ggbal.hpp>
#include <boost/numeric/bindings/lapack/computational/gghrd.hpp>
#include <boost/numeric/bindings/lapack/computational/ggqrf.hpp>
#include <boost/numeric/bindings/lapack/computational/ggrqf.hpp>
#include <boost/numeric/bindings/lapack/computational/ggsvp.hpp>
#include <boost/numeric/bindings/lapack/computational/gtrfs.hpp>
#include <boost/numeric/bindings/lapack/computational/gttrs.hpp>
#include <boost/numeric/bindings/lapack/computational/hbgst.hpp>
#include <boost/numeric/bindings/lapack/computational/hbtrd.hpp>
#include <boost/numeric/bindings/lapack/computational/hecon.hpp>
#include <boost/numeric/bindings/lapack/computational/hegst.hpp>
#include <boost/numeric/bindings/lapack/computational/herfs.hpp>
#include <boost/numeric/bindings/lapack/computational/hetrd.hpp>
#include <boost/numeric/bindings/lapack/computational/hetrf.hpp>
#include <boost/numeric/bindings/lapack/computational/hetri.hpp>
#include <boost/numeric/bindings/lapack/computational/hetrs.hpp>
#include <boost/numeric/bindings/lapack/computational/hgeqz.hpp>
#include <boost/numeric/bindings/lapack/computational/hpcon.hpp>
#include <boost/numeric/bindings/lapack/computational/hprfs.hpp>
#include <boost/numeric/bindings/lapack/computational/hptrd.hpp>
#include <boost/numeric/bindings/lapack/computational/hptrf.hpp>
#include <boost/numeric/bindings/lapack/computational/hptri.hpp>
#include <boost/numeric/bindings/lapack/computational/hptrs.hpp>
#include <boost/numeric/bindings/lapack/computational/hsein.hpp>
#include <boost/numeric/bindings/lapack/computational/hseqr.hpp>
#include <boost/numeric/bindings/lapack/computational/opgtr.hpp>
#include <boost/numeric/bindings/lapack/computational/opmtr.hpp>
#include <boost/numeric/bindings/lapack/computational/orgbr.hpp>
#include <boost/numeric/bindings/lapack/computational/orghr.hpp>
#include <boost/numeric/bindings/lapack/computational/orglq.hpp>
#include <boost/numeric/bindings/lapack/computational/orgql.hpp>
#include <boost/numeric/bindings/lapack/computational/orgqr.hpp>
#include <boost/numeric/bindings/lapack/computational/orgrq.hpp>
#include <boost/numeric/bindings/lapack/computational/orgtr.hpp>
#include <boost/numeric/bindings/lapack/computational/ormbr.hpp>
#include <boost/numeric/bindings/lapack/computational/ormhr.hpp>
#include <boost/numeric/bindings/lapack/computational/ormlq.hpp>
#include <boost/numeric/bindings/lapack/computational/ormql.hpp>
#include <boost/numeric/bindings/lapack/computational/ormqr.hpp>
#include <boost/numeric/bindings/lapack/computational/ormrq.hpp>
#include <boost/numeric/bindings/lapack/computational/ormrz.hpp>
#include <boost/numeric/bindings/lapack/computational/ormtr.hpp>
#include <boost/numeric/bindings/lapack/computational/pbcon.hpp>
#include <boost/numeric/bindings/lapack/computational/pbequ.hpp>
#include <boost/numeric/bindings/lapack/computational/pbrfs.hpp>
#include <boost/numeric/bindings/lapack/computational/pbstf.hpp>
#include <boost/numeric/bindings/lapack/computational/pbtrf.hpp>
#include <boost/numeric/bindings/lapack/computational/pbtrs.hpp>
#include <boost/numeric/bindings/lapack/computational/pftrf.hpp>
#include <boost/numeric/bindings/lapack/computational/pftri.hpp>
#include <boost/numeric/bindings/lapack/computational/pftrs.hpp>
#include <boost/numeric/bindings/lapack/computational/pocon.hpp>
#include <boost/numeric/bindings/lapack/computational/poequ.hpp>
#include <boost/numeric/bindings/lapack/computational/porfs.hpp>
#include <boost/numeric/bindings/lapack/computational/potrf.hpp>
#include <boost/numeric/bindings/lapack/computational/potri.hpp>
#include <boost/numeric/bindings/lapack/computational/potrs.hpp>
#include <boost/numeric/bindings/lapack/computational/ppcon.hpp>
#include <boost/numeric/bindings/lapack/computational/ppequ.hpp>
#include <boost/numeric/bindings/lapack/computational/pprfs.hpp>
#include <boost/numeric/bindings/lapack/computational/pptrf.hpp>
#include <boost/numeric/bindings/lapack/computational/pptri.hpp>
#include <boost/numeric/bindings/lapack/computational/pptrs.hpp>
#include <boost/numeric/bindings/lapack/computational/pstrf.hpp>
#include <boost/numeric/bindings/lapack/computational/ptcon.hpp>
#include <boost/numeric/bindings/lapack/computational/pteqr.hpp>
#include <boost/numeric/bindings/lapack/computational/ptrfs.hpp>
#include <boost/numeric/bindings/lapack/computational/pttrf.hpp>
#include <boost/numeric/bindings/lapack/computational/pttrs.hpp>
#include <boost/numeric/bindings/lapack/computational/sbgst.hpp>
#include <boost/numeric/bindings/lapack/computational/sbtrd.hpp>
#include <boost/numeric/bindings/lapack/computational/spcon.hpp>
#include <boost/numeric/bindings/lapack/computational/sprfs.hpp>
#include <boost/numeric/bindings/lapack/computational/sptrd.hpp>
#include <boost/numeric/bindings/lapack/computational/sptrf.hpp>
#include <boost/numeric/bindings/lapack/computational/sptri.hpp>
#include <boost/numeric/bindings/lapack/computational/sptrs.hpp>
#include <boost/numeric/bindings/lapack/computational/stebz.hpp>
#include <boost/numeric/bindings/lapack/computational/stedc.hpp>
#include <boost/numeric/bindings/lapack/computational/stegr.hpp>
#include <boost/numeric/bindings/lapack/computational/stein.hpp>
#include <boost/numeric/bindings/lapack/computational/stemr.hpp>
#include <boost/numeric/bindings/lapack/computational/steqr.hpp>
#include <boost/numeric/bindings/lapack/computational/sterf.hpp>
#include <boost/numeric/bindings/lapack/computational/sycon.hpp>
#include <boost/numeric/bindings/lapack/computational/sygst.hpp>
#include <boost/numeric/bindings/lapack/computational/syrfs.hpp>
#include <boost/numeric/bindings/lapack/computational/sytrd.hpp>
#include <boost/numeric/bindings/lapack/computational/sytrf.hpp>
#include <boost/numeric/bindings/lapack/computational/sytri.hpp>
#include <boost/numeric/bindings/lapack/computational/sytrs.hpp>
#include <boost/numeric/bindings/lapack/computational/tbcon.hpp>
#include <boost/numeric/bindings/lapack/computational/tbrfs.hpp>
#include <boost/numeric/bindings/lapack/computational/tbtrs.hpp>
#include <boost/numeric/bindings/lapack/computational/tftri.hpp>
#include <boost/numeric/bindings/lapack/computational/tgevc.hpp>
#include <boost/numeric/bindings/lapack/computational/tgexc.hpp>
#include <boost/numeric/bindings/lapack/computational/tgsen.hpp>
#include <boost/numeric/bindings/lapack/computational/tgsja.hpp>
#include <boost/numeric/bindings/lapack/computational/tgsna.hpp>
#include <boost/numeric/bindings/lapack/computational/tgsyl.hpp>
#include <boost/numeric/bindings/lapack/computational/tpcon.hpp>
#include <boost/numeric/bindings/lapack/computational/tprfs.hpp>
#include <boost/numeric/bindings/lapack/computational/tptri.hpp>
#include <boost/numeric/bindings/lapack/computational/tptrs.hpp>
#include <boost/numeric/bindings/lapack/computational/trcon.hpp>
#include <boost/numeric/bindings/lapack/computational/trevc.hpp>
#include <boost/numeric/bindings/lapack/computational/trexc.hpp>
#include <boost/numeric/bindings/lapack/computational/trrfs.hpp>
#include <boost/numeric/bindings/lapack/computational/trsen.hpp>
#include <boost/numeric/bindings/lapack/computational/trsna.hpp>
#include <boost/numeric/bindings/lapack/computational/trsyl.hpp>
#include <boost/numeric/bindings/lapack/computational/trtri.hpp>
#include <boost/numeric/bindings/lapack/computational/trtrs.hpp>
#include <boost/numeric/bindings/lapack/computational/tzrzf.hpp>
#include <boost/numeric/bindings/lapack/computational/ungbr.hpp>
#include <boost/numeric/bindings/lapack/computational/unghr.hpp>
#include <boost/numeric/bindings/lapack/computational/unglq.hpp>
#include <boost/numeric/bindings/lapack/computational/ungql.hpp>
#include <boost/numeric/bindings/lapack/computational/ungqr.hpp>
#include <boost/numeric/bindings/lapack/computational/ungrq.hpp>
#include <boost/numeric/bindings/lapack/computational/ungtr.hpp>
#include <boost/numeric/bindings/lapack/computational/unmbr.hpp>
#include <boost/numeric/bindings/lapack/computational/unmhr.hpp>
#include <boost/numeric/bindings/lapack/computational/unmlq.hpp>
#include <boost/numeric/bindings/lapack/computational/unmql.hpp>
#include <boost/numeric/bindings/lapack/computational/unmqr.hpp>
#include <boost/numeric/bindings/lapack/computational/unmrq.hpp>
#include <boost/numeric/bindings/lapack/computational/unmrz.hpp>
#include <boost/numeric/bindings/lapack/computational/unmtr.hpp>
#include <boost/numeric/bindings/lapack/computational/upgtr.hpp>
#include <boost/numeric/bindings/lapack/computational/upmtr.hpp>

#endif


#include <boost/graph/floyd_warshall_shortest.hpp>


/**
 * NOTE: This emission process is not being used for now. The NN basis function
 * is used instead.
 */

#ifndef NN_EMISSION_H
#define NN_EMISSION_H

#include <mlpack/core.hpp>

// The following include will be redundant in the newer versions of mlpack.
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>
#include <mlpack/methods/ann/loss_functions/mean_squared_error.hpp>
#include <armadillo>
#include <emissions.hpp>

namespace hsmm {

    typedef mlpack::ann::FFN<mlpack::ann::MeanSquaredError<>,
            mlpack::ann::RandomInitialization> NNmodel;

    class NNEmission : public AbstractEmissionOnlineSetting {
        public:
            NNEmission(int nstates, int njoints);

            NNEmission* clone() const;

            double loglikelihood(int state,
                    const arma::field<arma::mat>& obs) const;

            void reestimate(int min_duration,
                    const arma::field<arma::cube>& meta,
                    const arma::field<arma::field<arma::mat>>& mobs);

            arma::field<arma::mat> sampleFromState(int state, int size,
                    std::mt19937 &rng) const;

            arma::field<arma::mat> sampleNextObsGivenPastObs(int state,
                    int seg_dur, const arma::field<arma::mat>& past_obs,
                    std::mt19937 &rng) const;

        protected:

            arma::vec getSampleLocations(int length) const;

            std::vector<NNmodel> ffns_;
            arma::mat noise_var_;

            // Delta for emissions which are not dependent on the total
            // duration.
            double sample_locations_delta_ = -1;
    };

};
#endif


#ifndef RAY_HH
#define RAY_HH

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <optional>

#include "Hittable.hh"
#include "color.hh"

namespace ray
{

using Ray = Eigen::ParametrizedLine<double, 3>;

/**
 * @brief Calculate the color of a given ray
 *
 * @param ray Ray to calculate color of
 * @returns Color of the ray
 */
auto ray_color(const Ray& ray, const hittable::Hittable& world) -> color::Color;

/**
 * @brief Determine if given ray intersects with given sphere
 *
 * @param center Center of sphere
 * @param radius Radius of sphere
 * @param ray Ray to use to check intersection
 * @returns value of t when ray intersects with sphere or nullopt
 */
auto hit_sphere(const Eigen::Vector3d& center, double radius, const Ray& ray)
    -> std::optional<double>;

} // namespace ray

#endif // RAY_HH