// ---------------------------------------------------------------------
//
// Copyright (c) 2017 - 2019 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.
//
// ---------------------------------------------------------------------

// Config files
#include <IBAMR_config.h>
#include <IBTK_config.h>

#include <SAMRAI_config.h>

// 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/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/IBFEMethod.h>
#include <ibamr/IBFESurfaceMethod.h>
#include <ibamr/INSCollocatedHierarchyIntegrator.h>
#include <ibamr/INSStaggeredHierarchyIntegrator.h>

#include <ibtk/AppInitializer.h>
#include <ibtk/LEInteractor.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>

inline double
kernel(double x)
{
    x += 4.;
    const double x2 = x * x;
    const double x3 = x * x2;
    const double x4 = x * x3;
    const double x5 = x * x4;
    const double x6 = x * x5;
    const double x7 = x * x6;
    if (x <= 0.)
        return 0.;
    else if (x <= 1.)
        return .1984126984126984e-3 * x7;
    else if (x <= 2.)
        return .1111111111111111e-1 * x6 - .1388888888888889e-2 * x7 - .3333333333333333e-1 * x5 +
               .5555555555555556e-1 * x4 - .5555555555555556e-1 * x3 + .3333333333333333e-1 * x2 -
               .1111111111111111e-1 * x + .1587301587301587e-2;
    else if (x <= 3.)
        return .4333333333333333 * x5 - .6666666666666667e-1 * x6 + .4166666666666667e-2 * x7 - 1.500000000000000 * x4 +
               3.055555555555556 * x3 - 3.700000000000000 * x2 + 2.477777777777778 * x - .7095238095238095;
    else if (x <= 4.)
        return 9. * x4 - 1.666666666666667 * x5 + .1666666666666667 * x6 - .6944444444444444e-2 * x7 -
               28.44444444444444 * x3 + 53. * x2 - 54.22222222222222 * x + 23.59047619047619;
    else if (x <= 5.)
        return 96. * x3 - 22.11111111111111 * x4 + 3. * x5 - .2222222222222222 * x6 + .6944444444444444e-2 * x7 -
               245.6666666666667 * x2 + 344. * x - 203.9650793650794;
    else if (x <= 6.)
        return 483.5000000000000 * x2 - 147.0555555555556 * x3 + 26.50000000000000 * x4 - 2.833333333333333 * x5 +
               .1666666666666667 * x6 - .4166666666666667e-2 * x7 - 871.2777777777778 * x + 664.0904761904762;
    else if (x <= 7.)
        return 943.1222222222222 * x - 423.7000000000000 * x2 + 104.9444444444444 * x3 - 15.50000000000000 * x4 +
               1.366666666666667 * x5 - .6666666666666667e-1 * x6 + .1388888888888889e-2 * x7 - 891.1095238095238;
    else if (x <= 8.)
        return 416.1015873015873 - 364.0888888888889 * x + 136.5333333333333 * x2 - 28.44444444444444 * x3 +
               3.555555555555556 * x4 - .2666666666666667 * x5 + .1111111111111111e-1 * x6 - .1984126984126984e-3 * x7;
    else
        return 0.;
} // kernel

// Elasticity model data.
namespace ModelData
{
// The tether penalty functions each require some data that is set in the
// input file. This data is passed to each object through the void *ctx
// context data pointer. Here we collect all relevant tether data in a struct:
struct TetherData
{
    const double c1_s;
    const double kappa_s_body;
    const double eta_s_body;
    const double kappa_s_surface;
    const double eta_s_surface;

    TetherData(Pointer<Database> input_db)
        : c1_s(input_db->getDouble("C1_S")),
          kappa_s_body(input_db->getDouble("KAPPA_S_BODY")),
          eta_s_body(input_db->getDouble("ETA_S_BODY")),
          kappa_s_surface(input_db->getDouble("KAPPA_S_SURFACE")),
          eta_s_surface(input_db->getDouble("ETA_S_SURFACE"))
    {
    }
};

// Tether (penalty) stress function.
void
PK1_stress_function(TensorValue<double>& PP,
                    const TensorValue<double>& FF,
                    const libMesh::Point& /*x*/,
                    const libMesh::Point& /*X*/,
                    Elem* const /*elem*/,
                    const vector<const vector<double>*>& /*var_data*/,
                    const vector<const vector<VectorValue<double> >*>& /*grad_var_data*/,
                    double /*time*/,
                    void* ctx)
{
    const TetherData* const tether_data = reinterpret_cast<TetherData*>(ctx);

    PP = 2.0 * tether_data->c1_s * (FF - tensor_inverse_transpose(FF, NDIM));
    return;
} // PK1_stress_function

// Tether (penalty) force functions.
void
tether_force_function(VectorValue<double>& F,
                      const TensorValue<double>& /*FF*/,
                      const libMesh::Point& x,
                      const libMesh::Point& X,
                      Elem* const /*elem*/,
                      const vector<const vector<double>*>& var_data,
                      const vector<const vector<VectorValue<double> >*>& /*grad_var_data*/,
                      double /*time*/,
                      void* ctx)
{
    const TetherData* const tether_data = reinterpret_cast<TetherData*>(ctx);

    const std::vector<double>& U = *var_data[0];
    for (unsigned int d = 0; d < NDIM; ++d)
    {
        F(d) = tether_data->kappa_s_body * (X(d) - x(d)) - tether_data->eta_s_body * U[d];
    }
    return;
} // tether_force_function

void
tether_force_function(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 TetherData* const tether_data = reinterpret_cast<TetherData*>(ctx);

    VectorValue<double> D = X - x;
    VectorValue<double> D_n = (D * n) * n;
    VectorValue<double> U;
    for (unsigned int d = 0; d < NDIM; ++d) U(d) = (*var_data[0])[d];
    VectorValue<double> U_t = U - (U * n) * n;
    F = tether_data->kappa_s_surface * D - tether_data->eta_s_surface * U;
    return;
} // tether_force_function
} // namespace ModelData
using namespace ModelData;

// Function prototypes
static ofstream drag_stream, lift_stream, U_L1_norm_stream, U_L2_norm_stream, U_max_norm_stream;
void postprocess_data(Pointer<Database> input_db,
                      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();

    { // 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.
        LEInteractor::s_kernel_fcn = &kernel;
        LEInteractor::s_kernel_fcn_stencil_size = 8;

        // 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();
#ifdef LIBMESH_HAVE_EXODUS_API
        const bool uses_exodus = dump_viz_data && !app_initializer->getExodusIIFilename().empty();
#else
        const bool uses_exodus = false;
        if (!app_initializer->getExodusIIFilename().empty())
        {
            plog << "WARNING: libMesh was compiled without Exodus support, so no "
                 << "Exodus output will be written in this program.\n";
        }
#endif
        const string exodus_filename = app_initializer->getExodusIIFilename();

        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 string restart_read_dirname = app_initializer->getRestartReadDirectory();
        const int restart_restore_num = app_initializer->getRestartRestoreNumber();

        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 solid_mesh(init.comm(), NDIM);
        const double dx = input_db->getDouble("DX");
        const double ds = input_db->getDouble("MFAC") * dx;
        string elem_type = input_db->getString("ELEM_TYPE");
        const double R = 0.5;
        if (NDIM == 2 && (elem_type == "TRI3" || elem_type == "TRI6"))
        {
#ifdef LIBMESH_HAVE_TRIANGLE
            const int num_circum_nodes = ceil(2.0 * M_PI * R / ds);
            for (int k = 0; k < num_circum_nodes; ++k)
            {
                const double theta = 2.0 * M_PI * static_cast<double>(k) / static_cast<double>(num_circum_nodes);
                solid_mesh.add_point(libMesh::Point(R * cos(theta), R * sin(theta)));
            }
            TriangleInterface triangle(solid_mesh);
            triangle.triangulation_type() = TriangleInterface::GENERATE_CONVEX_HULL;
            triangle.elem_type() = Utility::string_to_enum<ElemType>(elem_type);
            triangle.desired_area() = 1.5 * sqrt(3.0) / 4.0 * ds * ds;
            triangle.insert_extra_points() = true;
            triangle.smooth_after_generating() = true;
            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
        }
        else
        {
            // NOTE: number of segments along boundary is 4*2^r.
            const double num_circum_segments = 2.0 * M_PI * R / ds;
            const int r = log2(0.25 * num_circum_segments);
            MeshTools::Generation::build_sphere(solid_mesh, R, r, Utility::string_to_enum<ElemType>(elem_type));
        }

        // Ensure nodes on the surface are on the analytic boundary.
        MeshBase::element_iterator el_end = solid_mesh.elements_end();
        for (MeshBase::element_iterator el = solid_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) 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 = R * n.unit();
                }
            }
        }
        solid_mesh.prepare_for_use();

        BoundaryMesh boundary_mesh(solid_mesh.comm(), solid_mesh.mesh_dimension() - 1);
        solid_mesh.boundary_info->sync(boundary_mesh);
        boundary_mesh.prepare_for_use();

        bool use_boundary_mesh = input_db->getBoolWithDefault("USE_BOUNDARY_MESH", false);
        Mesh& mesh = use_boundary_mesh ? boundary_mesh : solid_mesh;

        // 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;
        const string solver_type = app_initializer->getComponentDatabase("Main")->getString("solver_type");
        if (solver_type == "STAGGERED")
        {
            navier_stokes_integrator = new INSStaggeredHierarchyIntegrator(
                "INSStaggeredHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSStaggeredHierarchyIntegrator"));
        }
        else if (solver_type == "COLLOCATED")
        {
            navier_stokes_integrator = new INSCollocatedHierarchyIntegrator(
                "INSCollocatedHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSCollocatedHierarchyIntegrator"));
        }
        else
        {
            TBOX_ERROR("Unsupported solver type: " << solver_type << "\n"
                                                   << "Valid options are: COLLOCATED, STAGGERED");
        }
        Pointer<IBStrategy> ib_ops;
        if (use_boundary_mesh)
        {
            ib_ops = new IBFESurfaceMethod(
                "IBFEMethod",
                app_initializer->getComponentDatabase("IBFEMethod"),
                &mesh,
                app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                /*register_for_restart*/ true,
                restart_read_dirname,
                restart_restore_num);
        }
        else
        {
            ib_ops =
                new IBFEMethod("IBFEMethod",
                               app_initializer->getComponentDatabase("IBFEMethod"),
                               &mesh,
                               app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                               /*register_for_restart*/ true,
                               restart_read_dirname,
                               restart_restore_num);
        }
        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.
        TetherData tether_data(input_db);
        void* const tether_data_ptr = reinterpret_cast<void*>(&tether_data);
        EquationSystems* equation_systems;
        std::vector<int> vars(NDIM);
        for (unsigned int d = 0; d < NDIM; ++d) vars[d] = d;
        vector<SystemData> sys_data(1, SystemData(IBFEMethod::VELOCITY_SYSTEM_NAME, vars));
        if (use_boundary_mesh)
        {
            Pointer<IBFESurfaceMethod> ibfe_ops = ib_ops;
            ibfe_ops->initializeFEEquationSystems();
            equation_systems = ibfe_ops->getFEDataManager()->getEquationSystems();
            IBFESurfaceMethod::LagSurfaceForceFcnData surface_fcn_data(
                tether_force_function, sys_data, tether_data_ptr);
            ibfe_ops->registerLagSurfaceForceFunction(surface_fcn_data);
        }
        else
        {
            Pointer<IBFEMethod> ibfe_ops = ib_ops;
            ibfe_ops->initializeFEEquationSystems();
            equation_systems = ibfe_ops->getFEDataManager()->getEquationSystems();
            IBFEMethod::PK1StressFcnData PK1_stress_data(
                PK1_stress_function, std::vector<IBTK::SystemData>(), tether_data_ptr);
            PK1_stress_data.quad_order =
                Utility::string_to_enum<libMesh::Order>(input_db->getStringWithDefault("PK1_QUAD_ORDER", "THIRD"));
            ibfe_ops->registerPK1StressFunction(PK1_stress_data);

            IBFEMethod::LagBodyForceFcnData body_fcn_data(tether_force_function, sys_data, tether_data_ptr);
            ibfe_ops->registerLagBodyForceFunction(body_fcn_data);

            IBFEMethod::LagSurfaceForceFcnData surface_fcn_data(tether_force_function, sys_data, tether_data_ptr);
            ibfe_ops->registerLagSurfaceForceFunction(surface_fcn_data);

            if (input_db->getBoolWithDefault("ELIMINATE_PRESSURE_JUMPS", false))
            {
                ibfe_ops->registerStressNormalizationPart();
            }
        }

        // Create Eulerian initial condition specification objects.
        if (input_db->keyExists("VelocityInitialConditions"))
        {
            Pointer<CartGridFunction> u_init = new muParserCartGridFunction(
                "u_init", app_initializer->getComponentDatabase("VelocityInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerVelocityInitialConditions(u_init);
        }

        if (input_db->keyExists("PressureInitialConditions"))
        {
            Pointer<CartGridFunction> p_init = new muParserCartGridFunction(
                "p_init", app_initializer->getComponentDatabase("PressureInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerPressureInitialConditions(p_init);
        }

        // Create Eulerian boundary condition specification objects (when necessary).
        const IntVector<NDIM>& periodic_shift = grid_geometry->getPeriodicShift();
        vector<RobinBcCoefStrategy<NDIM>*> u_bc_coefs(NDIM);
        if (periodic_shift.min() > 0)
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                u_bc_coefs[d] = NULL;
            }
        }
        else
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                const std::string bc_coefs_name = "u_bc_coefs_" + std::to_string(d);

                const std::string bc_coefs_db_name = "VelocityBcCoefs_" + std::to_string(d);

                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> exodus_io(uses_exodus ? new ExodusII_IO(mesh) : NULL);

        // Check to see if this is a restarted run to append current exodus files
        if (uses_exodus)
        {
            const bool from_restart = RestartManager::getManager()->isFromRestart();
            exodus_io->append(from_restart);
        }

        // Initialize hierarchy configuration and data on all patches.
        if (use_boundary_mesh)
        {
            Pointer<IBFESurfaceMethod> ibfe_ops = ib_ops;
            ibfe_ops->initializeFEData();
        }
        else
        {
            Pointer<IBFEMethod> ibfe_ops = ib_ops;
            ibfe_ops->initializeFEData();
        }
        time_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);

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

        // Print the input database contents to the log file.
        plog << "Input database:\n";
        input_db->printClassData(plog);

        // 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)
            {
                exodus_io->write_timestep(
                    exodus_filename, *equation_systems, iteration_num / viz_dump_interval + 1, loop_time);
            }
        }

        // Open streams to save lift and drag coefficients and the norms of the
        // velocity.
        if (SAMRAI_MPI::getRank() == 0)
        {
            drag_stream.open("C_D.curve", ios_base::out | ios_base::trunc);
            lift_stream.open("C_L.curve", ios_base::out | ios_base::trunc);
            U_L1_norm_stream.open("U_L1.curve", ios_base::out | ios_base::trunc);
            U_L2_norm_stream.open("U_L2.curve", ios_base::out | ios_base::trunc);
            U_max_norm_stream.open("U_max.curve", ios_base::out | ios_base::trunc);

            drag_stream.precision(10);
            lift_stream.precision(10);
            U_L1_norm_stream.precision(10);
            U_L2_norm_stream.precision(10);
            U_max_norm_stream.precision(10);
        }

        // 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)
                {
                    exodus_io->write_timestep(
                        exodus_filename, *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 (use_boundary_mesh)
                {
                    dynamic_cast<IBFESurfaceMethod&>(*ib_ops).writeFEDataToRestartFile(restart_dump_dirname,
                                                                                iteration_num);
                }
                else
                {
                    dynamic_cast<IBFEMethod&>(*ib_ops).writeFEDataToRestartFile(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);
            }
            if (dump_postproc_data && (iteration_num % postproc_data_dump_interval == 0 || last_step))
            {
                postprocess_data(input_db,
                                 patch_hierarchy,
                                 navier_stokes_integrator,
                                 mesh,
                                 equation_systems,
                                 iteration_num,
                                 loop_time,
                                 postproc_data_dump_dirname);
            }
        }

        // Close the logging streams.
        if (SAMRAI_MPI::getRank() == 0)
        {
            drag_stream.close();
            lift_stream.close();
            U_L1_norm_stream.close();
            U_L2_norm_stream.close();
            U_max_norm_stream.close();
        }

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

void
postprocess_data(Pointer<Database> input_db,
                 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*/)
{
    TetherData tether_data(input_db);
    void* const tether_data_ptr = reinterpret_cast<void*>(&tether_data);

    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(IBFEMethod::COORDS_SYSTEM_NAME);
    System& U_system = equation_systems->get_system(IBFEMethod::VELOCITY_SYSTEM_NAME);
    NumericVector<double>* x_vec = x_system.solution.get();
    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();

    std::unique_ptr<FEBase> fe_face(FEBase::build(dim, dof_map.variable_type(0)));
    std::unique_ptr<QBase> qrule_face = QBase::build(QGAUSS, dim - 1, SEVENTH);
    fe_face->attach_quadrature_rule(qrule_face.get());
    const vector<double>& JxW_face = fe_face->get_JxW();
    const vector<libMesh::Point>& q_point_face = fe_face->get_xyz();
    const vector<libMesh::Point>& normal_face = fe_face->get_normals();
    const vector<vector<double> >& phi_face = fe_face->get_phi();
    const vector<vector<VectorValue<double> > >& dphi_face = fe_face->get_dphi();

    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;

    TensorValue<double> FF, FF_inv_trans;
    boost::multi_array<double, 2> x_node, U_node;
    VectorValue<double> F, N, U, n, x;

    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(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, x_node, phi);
            jacobian(FF, qp, x_node, dphi);
            interpolate(U, qp, U_node, phi);
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                U_qp_vec[d] = U(d);
            }
            tether_force_function(F, FF, x, q_point[qp], elem, var_data, grad_var_data, loop_time, tether_data_ptr);
            for (int d = 0; d < NDIM; ++d)
            {
                F_integral[d] += F(d) * JxW[qp];
            }
        }
        for (unsigned short int side = 0; side < elem->n_sides(); ++side)
        {
            if (elem->neighbor_ptr(side)) continue;
            fe_face->reinit(elem, side);
            const unsigned int n_qp_face = qrule_face->n_points();
            for (unsigned int qp = 0; qp < n_qp_face; ++qp)
            {
                interpolate(x, qp, x_node, phi_face);
                jacobian(FF, qp, x_node, dphi_face);
                interpolate(U, qp, U_node, phi_face);
                for (unsigned int d = 0; d < NDIM; ++d)
                {
                    U_qp_vec[d] = U(d);
                }
                N = normal_face[qp];
                tensor_inverse_transpose(FF_inv_trans, FF, NDIM);
                n = (FF_inv_trans * N).unit();

                tether_force_function(
                    F, n, N, FF, x, q_point_face[qp], elem, side, var_data, grad_var_data, loop_time, tether_data_ptr);
                for (int d = 0; d < NDIM; ++d)
                {
                    F_integral[d] += F(d) * JxW_face[qp];
                }
            }
        }
    }
    SAMRAI_MPI::sumReduction(F_integral, NDIM);
    static const double rho = 1.0;
    static const double U_max = 1.0;
    static const double D = 1.0;
    if (SAMRAI_MPI::getRank() == 0)
    {
        drag_stream << loop_time << " " << -F_integral[0] / (0.5 * rho * U_max * U_max * D) << endl;
        lift_stream << loop_time << " " << -F_integral[1] / (0.5 * rho * U_max * U_max * D) << endl;
    }
    return;
} // postprocess_data


// libgp - Gaussian process library for Machine Learning
// Copyright (c) 2013, Manuel Blum <mblum@informatik.uni-freiburg.de>
// All rights reserved.

#include "abstract_gp.h"
#include "gp.h"
#include "gp_utils.h"

#include <Eigen/Dense>

using namespace libgp;

int main (int argc, char const *argv[])
{
  int n=4000, m=1000;
  double tss = 0, error, f, y;
  // initialize Gaussian process for 2-D input using the squared exponential
  // covariance function with additive white noise.
  GaussianProcess gp(2, "CovSum ( CovSEiso, CovNoise)");
  // initialize hyper parameter vector
  Eigen::VectorXd params(gp.covf().get_param_dim());
  params << 0.0, 0.0, -2.0;
  // set parameters of covariance function
  gp.covf().set_loghyper(params);

  // add training patterns
  for(int i = 0; i < n; ++i) {
    double x[] = {drand48()*4-2, drand48()*4-2};
    y = Utils::hill(x[0], x[1]) + Utils::randn() * 0.1;
    gp.add_pattern(x, y);
  }

  // total squared error
  for(int i = 0; i < m; ++i) {
    double x[] = {drand48()*4-2, drand48()*4-2};
    f = gp.f(x);
    y = Utils::hill(x[0], x[1]);
    error = f - y;
    tss += error*error;
  }
  std::cout << "mse = " << tss/m << std::endl;
	return EXIT_SUCCESS;
}


//===- types.hh -----------------------------------------------------------===//
//
//                       The CIM Hardware Simulator Project
//
// See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
#pragma once
#include <Eigen/Dense>

namespace cimHW {

// basic element type
typedef unsigned int bitType;
typedef float numType;

// matrix with bitType
typedef Eigen::Matrix<bitType, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXuiRowMajor;
typedef Eigen::Matrix<bitType, 1, Eigen::Dynamic, Eigen::RowMajor> RowVectorXui;
typedef Eigen::Matrix<bitType, Eigen::Dynamic, 1> VectorXui;

// matrix with numType
typedef Eigen::Matrix<numType, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXfRowMajor;
typedef Eigen::Matrix<numType, 1, Eigen::Dynamic, Eigen::RowMajor> RowVectorXf;
typedef Eigen::Matrix<numType, Eigen::Dynamic, 1> VectorXnum;

} //namespace cimHW


// #define EIGEN_TAUCS_SUPPORT
// #define EIGEN_CHOLMOD_SUPPORT
#include <iostream>
#include <Eigen/Sparse>

// g++ -DSIZE=10000 -DDENSITY=0.001  sparse_cholesky.cpp -I.. -DDENSEMATRI -O3 -g0 -DNDEBUG   -DNBTRIES=1 -I /home/gael/Coding/LinearAlgebra/taucs_full/src/ -I/home/gael/Coding/LinearAlgebra/taucs_full/build/linux/  -L/home/gael/Coding/LinearAlgebra/taucs_full/lib/linux/ -ltaucs /home/gael/Coding/LinearAlgebra/GotoBLAS/libgoto.a -lpthread -I /home/gael/Coding/LinearAlgebra/SuiteSparse/CHOLMOD/Include/ $CHOLLIB -I /home/gael/Coding/LinearAlgebra/SuiteSparse/UFconfig/ /home/gael/Coding/LinearAlgebra/SuiteSparse/CCOLAMD/Lib/libccolamd.a   /home/gael/Coding/LinearAlgebra/SuiteSparse/CHOLMOD/Lib/libcholmod.a -lmetis /home/gael/Coding/LinearAlgebra/SuiteSparse/AMD/Lib/libamd.a  /home/gael/Coding/LinearAlgebra/SuiteSparse/CAMD/Lib/libcamd.a   /home/gael/Coding/LinearAlgebra/SuiteSparse/CCOLAMD/Lib/libccolamd.a  /home/gael/Coding/LinearAlgebra/SuiteSparse/COLAMD/Lib/libcolamd.a -llapack && ./a.out

#define NOGMM
#define NOMTL

#ifndef SIZE
#define SIZE 10
#endif

#ifndef DENSITY
#define DENSITY 0.01
#endif

#ifndef REPEAT
#define REPEAT 1
#endif

#include "BenchSparseUtil.h"

#ifndef MINDENSITY
#define MINDENSITY 0.0004
#endif

#ifndef NBTRIES
#define NBTRIES 10
#endif

#define BENCH(X) \
  timer.reset(); \
  for (int _j=0; _j<NBTRIES; ++_j) { \
    timer.start(); \
    for (int _k=0; _k<REPEAT; ++_k) { \
        X  \
  } timer.stop(); }

// typedef SparseMatrix<Scalar,UpperTriangular> EigenSparseTriMatrix;
typedef SparseMatrix<Scalar,SelfAdjoint|LowerTriangular> EigenSparseSelfAdjointMatrix;

void fillSpdMatrix(float density, int rows, int cols,  EigenSparseSelfAdjointMatrix& dst)
{
  dst.startFill(rows*cols*density);
  for(int j = 0; j < cols; j++)
  {
    dst.fill(j,j) = internal::random<Scalar>(10,20);
    for(int i = j+1; i < rows; i++)
    {
      Scalar v = (internal::random<float>(0,1) < density) ? internal::random<Scalar>() : 0;
      if (v!=0)
        dst.fill(i,j) = v;
    }

  }
  dst.endFill();
}

#include <Eigen/Cholesky>

template<int Backend>
void doEigen(const char* name, const EigenSparseSelfAdjointMatrix& sm1, int flags = 0)
{
  std::cout << name << "..." << std::flush;
  BenchTimer timer;
  timer.start();
  SparseLLT<EigenSparseSelfAdjointMatrix,Backend> chol(sm1, flags);
  timer.stop();
  std::cout << ":\t" << timer.value() << endl;

  std::cout << "  nnz: " << sm1.nonZeros() << " => " << chol.matrixL().nonZeros() << "\n";
//   std::cout << "sparse\n" << chol.matrixL() << "%\n";
}

int main(int argc, char *argv[])
{
  int rows = SIZE;
  int cols = SIZE;
  float density = DENSITY;
  BenchTimer timer;

  VectorXf b = VectorXf::Random(cols);
  VectorXf x = VectorXf::Random(cols);

  bool densedone = false;

  //for (float density = DENSITY; density>=MINDENSITY; density*=0.5)
//   float density = 0.5;
  {
    EigenSparseSelfAdjointMatrix sm1(rows, cols);
    std::cout << "Generate sparse matrix (might take a while)...\n";
    fillSpdMatrix(density, rows, cols, sm1);
    std::cout << "DONE\n\n";

    // dense matrices
    #ifdef DENSEMATRIX
    if (!densedone)
    {
      densedone = true;
      std::cout << "Eigen Dense\t" << density*100 << "%\n";
      DenseMatrix m1(rows,cols);
      eiToDense(sm1, m1);
      m1 = (m1 + m1.transpose()).eval();
      m1.diagonal() *= 0.5;

//       BENCH(LLT<DenseMatrix> chol(m1);)
//       std::cout << "dense:\t" << timer.value() << endl;

      BenchTimer timer;
      timer.start();
      LLT<DenseMatrix> chol(m1);
      timer.stop();
      std::cout << "dense:\t" << timer.value() << endl;
      int count = 0;
      for (int j=0; j<cols; ++j)
        for (int i=j; i<rows; ++i)
          if (!internal::isMuchSmallerThan(internal::abs(chol.matrixL()(i,j)), 0.1))
            count++;
      std::cout << "dense: " << "nnz = " << count << "\n";
//       std::cout << "dense:\n" << m1 << "\n\n" << chol.matrixL() << endl;
    }
    #endif

    // eigen sparse matrices
    doEigen<Eigen::DefaultBackend>("Eigen/Sparse", sm1, Eigen::IncompleteFactorization);

    #ifdef EIGEN_CHOLMOD_SUPPORT
    doEigen<Eigen::Cholmod>("Eigen/Cholmod", sm1, Eigen::IncompleteFactorization);
    #endif

    #ifdef EIGEN_TAUCS_SUPPORT
    doEigen<Eigen::Taucs>("Eigen/Taucs", sm1, Eigen::IncompleteFactorization);
    #endif

    #if 0
    // TAUCS
    {
      taucs_ccs_matrix A = sm1.asTaucsMatrix();

      //BENCH(taucs_ccs_matrix* chol = taucs_ccs_factor_llt(&A, 0, 0);)
//       BENCH(taucs_supernodal_factor_to_ccs(taucs_ccs_factor_llt_ll(&A));)
//       std::cout << "taucs:\t" << timer.value() << endl;

      taucs_ccs_matrix* chol = taucs_ccs_factor_llt(&A, 0, 0);

      for (int j=0; j<cols; ++j)
      {
        for (int i=chol->colptr[j]; i<chol->colptr[j+1]; ++i)
          std::cout << chol->values.d[i] << " ";
      }
    }

    // CHOLMOD
    #ifdef EIGEN_CHOLMOD_SUPPORT
    {
      cholmod_common c;
      cholmod_start (&c);
      cholmod_sparse A;
      cholmod_factor *L;

      A = sm1.asCholmodMatrix();
      BenchTimer timer;
//       timer.reset();
      timer.start();
      std::vector<int> perm(cols);
//       std::vector<int> set(ncols);
      for (int i=0; i<cols; ++i)
        perm[i] = i;
//       c.nmethods = 1;
//       c.method[0] = 1;

      c.nmethods = 1;
      c.method [0].ordering = CHOLMOD_NATURAL;
      c.postorder = 0;
      c.final_ll = 1;

      L = cholmod_analyze_p(&A, &perm[0], &perm[0], cols, &c);
      timer.stop();
      std::cout << "cholmod/analyze:\t" << timer.value() << endl;
      timer.reset();
      timer.start();
      cholmod_factorize(&A, L, &c);
      timer.stop();
      std::cout << "cholmod/factorize:\t" << timer.value() << endl;

      cholmod_sparse* cholmat = cholmod_factor_to_sparse(L, &c);

      cholmod_print_factor(L, "Factors", &c);

      cholmod_print_sparse(cholmat, "Chol", &c);
      cholmod_write_sparse(stdout, cholmat, 0, 0, &c);
//
//       cholmod_print_sparse(&A, "A", &c);
//       cholmod_write_sparse(stdout, &A, 0, 0, &c);

//       for (int j=0; j<cols; ++j)
//       {
//           for (int i=chol->colptr[j]; i<chol->colptr[j+1]; ++i)
//             std::cout << chol->values.s[i] << " ";
//       }
    }
    #endif

    #endif

  }

  return 0;
}


// The MIT License
// (c) 2019 Daniel Williams

#ifndef _INTERPOLATE_HPP_
#define _INTERPOLATE_HPP_

#include <Eigen/Geometry>

#include "geometry/cell_value.hpp"
#include "geometry/sample.hpp"

namespace terrain::geometry {

/**
 * Performs the low-level interpolation between this node and another node, at the requested location
 *
 * @param {Point} the x,y coordinates to interpolate at.
 * @param {quadtree::Node} n2 the other node to interpolate
 * @return {cell_value_t} The resultant value
 */
cell_value_t interpolate_linear( const Eigen::Vector2d& at, const Sample& s1, const Sample& s2);

/**
 * Performs bilinear-interpolation:
 * http://en.wikipedia.org/wiki/Bilinear_Interpolation
 *
 * Warning: Undefined behavior, if interpolation happens outside of the rectangle defined by the 4 samples
 *
 * @param the x,y coordinates to interpolate at.
 * @param ne quadrant sample point
 * @param nw quadrant sample point
 * @param sw quadrant sample point
 * @param se quadrant sample point
 * @return resultant value
 */
cell_value_t interpolate_bilinear( const Eigen::Vector2d& at, const Sample& ne, const Sample& nw, const Sample& sw, const Sample& se);

} // namespace terrain::geometry

#endif // #ifndef _INTERPOLATE_HPP_


//
// Copyright (c) 2015, J2 Innovations
// Copyright (c) 2012 Brian Frank
// Licensed under the Academic Free License version 3.0
// History:
//   19 Aug 2014  Radu Racariu<radur@2inn.com> Ported to C++
//   06 Jun 2011  Brian Frank  Creation
//
#include "coord.hpp"
#include <cstdio>
#include <sstream>
#include <stdexcept>

#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/predicate.hpp>

////////////////////////////////////////////////
// Coord
////////////////////////////////////////////////
using namespace haystack;

// private ctor
Coord::Coord(int32_t lat, int32_t lng) : ulat(lat), ulng(lng)
{
    if (ulat < -90000000 || ulat > 90000000) throw std::runtime_error("Invalid lat > +/- 90");
    if (ulng < -180000000 || ulng > 180000000) throw std::runtime_error("Invalid lng > +/- 180");
}

Coord::Coord(double lat, double lng) : ulat((int32_t)(lat * 1000000.0)), ulng((int32_t)(lng * 1000000.0))
{
    if (ulat < -90000000 || ulat > 90000000) throw std::runtime_error("Invalid lat > +/- 90");
    if (ulng < -180000000 || ulng > 180000000) throw std::runtime_error("Invalid lng > +/- 180");
}

////////////////////////////////////////////////
// statics
////////////////////////////////////////////////

// Parse from string fomat "C(lat,lng)" or raise runtime exception
Coord Coord::make(const std::string &s)
{
    if (!boost::starts_with(s, "C(")) throw std::runtime_error("Parse error");
    if (!boost::ends_with(s, ")")) throw std::runtime_error("Parse error");
    size_t comma = s.find(',');
    if (comma < 3) throw std::runtime_error("Parse error");

    std::string lat = s.substr(2, comma - 2);
    std::string lng = s.substr(comma + 1, s.size() - comma - 2);

    return Coord(boost::lexical_cast<double>(lat), boost::lexical_cast<double>(lng));
}

// Return if given latitude is legal value between -90.0 and +90.0 */
bool Coord::is_lat(double lat) { return -90.0 <= lat && lat <= 90.0; }

// Return if given is longtitude is legal value between -180.0 and +180.0
bool Coord::is_lng(double lng) { return -180.0 <= lng && lng <= 180.0; }

void u_to_str(std::stringstream &s, int ud)
{
    if (ud < 0) { s << '-'; ud = -ud; }
    if (ud < 1000000.0)
    {
        double d = (ud / 1000000.0);
        char buf[64];
        sprintf(buf, "%g", d);
        s << buf;
        if (d == 0)
            s << ".0";
        return;
    }
    std::string x = boost::lexical_cast<std::string>(ud);
    size_t dot = x.size() - 6;
    size_t end = x.size();

    while (end > dot + 1 && x[end - 1] == '0') --end;

    for (size_t i = 0; i < dot; ++i)
        s << x[i];

    s << '.';
    for (size_t i = dot; i < end; ++i) s << x[i];
}

//////////////////////////////////////////////////////////////////////////
// Access
//////////////////////////////////////////////////////////////////////////

// Latitude in decimal degrees
double Coord::lat() const { return ulat / 1000000.0; }

// Longtitude in decimal degrees
double Coord::lng() const { return ulng / 1000000.0; }

////////////////////////////////////////////////
// to zinc
////////////////////////////////////////////////

// Encode using double quotes and back slash escapes
const std::string Coord::to_zinc() const
{
	std::stringstream os;

    os << "C(";
    u_to_str(os, ulat);
    os << ',';
    u_to_str(os, ulng);
    os << ")";

	return os.str();
}

////////////////////////////////////////////////
// Equal
////////////////////////////////////////////////
bool Coord::operator ==(const Coord &other) const
{
	return ulat == other.ulat && ulng == other.ulng;
}

bool Coord::operator==(const Val &other) const
{
    if (type() != other.type())
        return false;
    return static_cast<const Coord&>(other).operator==(*this);
}

bool Coord::operator < (const Val &other) const
{
    return type() == other.type()
        && ulat < ((Coord&)other).ulat && ulng >((Coord&)other).ulng;
}

bool Coord::operator >(const Val &other) const
{
    return type() == other.type()
        && ulat > ((Coord&)other).ulat && ulng > ((Coord&)other).ulng;
}

Coord::auto_ptr_t Coord::clone() const
{
    return auto_ptr_t(new Coord(*this));
}


//
// Created by abakfja on 3/28/21.
//

#ifndef LA_VECTOR_HPP
#define LA_VECTOR_HPP

#include <boost/numeric/ublas/matrix/vector/vector_engine.hpp>
#include <boost/numeric/ublas/matrix/vector/traits/engine_traits.hpp>

namespace boost::numeric::ublas::experimental {

template<class engine>
class vector {
public:
    using scalar_type = typename engine::scalar_type;
    using scalar_reference = scalar_type &;
    using scalar_const_reference = const scalar_type &;

    using storage_traits_type = typename engine::storage_traits_type;

    using storage_type = typename storage_traits_type::storage_type;

    using size_type = typename storage_traits_type::size_type;
    using index_type = std::pair<size_type, size_type>;
    using difference_type = typename storage_traits_type::difference_type;

    using reference = typename storage_traits_type::reference;
    using const_reference = typename storage_traits_type::const_reference;

    using pointer = typename storage_traits_type::pointer;
    using const_pointer = typename storage_traits_type::const_pointer;

    using iterator = typename storage_traits_type::iterator;
    using const_iterator = typename storage_traits_type::const_iterator;

    using reverse_iterator = typename storage_traits_type::reverse_iterator;
    using const_reverse_iterator = typename storage_traits_type::const_reverse_iterator;

    using resizable_tag = typename storage_traits_type::resizable_tag;

//    vector() = default;

    template<typename E = engine,
            typename = detail::enable_if_static<E>>
    explicit vector(scalar_const_reference v): vector(resizable_tag{}) {
        fill(v);
    }

    template<typename E = engine,
            typename = detail::enable_if_dynamic<E>>
    vector(std::initializer_list<scalar_type> l)
            : vector(l.size(), resizable_tag{}) {
        std::copy(l.begin(), l.end(), m_data.begin());
    }

    template<typename E = engine,
            typename = detail::enable_if_dynamic<E>>
    constexpr explicit
    vector(size_type n, scalar_type v) :
            vector(n, resizable_tag{}) {
        fill(v);
    }

    [[nodiscard]] index_type size() const {
        return this->n_ele;
    }

    [[nodiscard]] bool empty() const {
        return m_data.empty();
    }

    constexpr auto begin() noexcept {
        return m_data.begin();
    }

    constexpr auto end() noexcept {
        return m_data.end();
    }

    constexpr auto begin() const noexcept {
        return m_data.begin();
    }

    constexpr auto end() const noexcept {
        return m_data.end();
    }

    constexpr auto at(size_type pos) const {
        return m_data.at(pos);
    }

    constexpr auto at(size_type pos) {
        return m_data.at(pos);
    }

    void fill(const_reference value) {
        std::fill(begin(), end(), value);
    }

    constexpr const_reference operator[](size_type i) const {
        return this->m_data[i];
    }

    constexpr reference operator[](size_type i) {
        return this->m_data[i];
    }

private:

    constexpr explicit vector(size_type n,
                              storage_resizable_container_tag t
    ) : n_ele{n}, m_data(n_ele) {
    }

    constexpr explicit vector(size_type n,
                              storage_static_container_tag t
    ) : n_ele{engine::n}, m_data(n_ele) {
    }

    constexpr explicit vector(storage_static_container_tag t) :
            n_ele{engine::n} {
    }

    storage_type m_data;
    size_type n_ele;
};

}

#endif //LA_VECTOR_HPP


#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <vector>

#include <utils/eigen_ext.hpp>

namespace ipc::rigid {

template <typename T> class Pose;
template <typename T> using Poses = std::vector<Pose<T>>;

/// @brief The position and rotation of an object.
template <typename T> class Pose {
public:
    Pose();
    Pose(const VectorMax3<T>& position, const VectorMax3<T>& rotation);
    Pose(const VectorMax6<T>& dof);
    Pose(const T& x, const T& y, const T& theta);
    Pose(
        const T& x,
        const T& y,
        const T& z,
        const T& theta_x,
        const T& theta_y,
        const T& theta_z);

    static Pose<T> Zero(int dim);

    static Poses<T> dofs_to_poses(const VectorX<T>& dofs, int dim);
    static VectorX<T> poses_to_dofs(const Poses<T>& poses);

    static int dim_to_ndof(const int dim) { return dim == 2 ? 3 : 6; }
    static int dim_to_pos_ndof(const int dim) { return dim; }
    static int dim_to_rot_ndof(const int dim)
    {
        return dim_to_ndof(dim) - dim_to_pos_ndof(dim);
    }
    int dim() const { return position.size(); }
    int pos_ndof() const { return position.size(); }
    int rot_ndof() const { return rotation.size(); }
    int ndof() const { return pos_ndof() + rot_ndof(); }

    VectorMax6<T> dof() const;

    /// @brief Replace a selected dof with the dof in other.
    /// @param R Rotation this rotations coordinates to the dof in
    ///          is_dof_selected.
    void select_dof(
        const VectorMax6b& is_dof_selected,
        const Pose<T>& other,
        const MatrixMax3d& R);
    /// @brief Replace a selected dof with the dof in other.
    void select_dof(const VectorMax6b& is_dof_selected, const Pose<T>& other)
    {
        select_dof(
            is_dof_selected, other,
            MatrixMax3d::Identity(rot_ndof(), rot_ndof()));
    }
    /// @brief Zero out the i-th dof if is_dof_zero(i) == true.
    void zero_dof(const VectorMax6b& is_dof_zero, const MatrixMax3d& R);
    /// @brief Zero out the i-th dof if is_dof_zero(i) == true.
    void zero_dof(const VectorMax6b& is_dof_zero)
    {
        zero_dof(is_dof_zero, MatrixMax3d::Identity(rot_ndof(), rot_ndof()));
    }

    MatrixMax3<T> construct_rotation_matrix() const;

    Eigen::Quaternion<T> construct_quaternion() const;

    static Pose<T> interpolate(const Pose<T>& pose0, const Pose<T>& pose1, T t);

    bool operator==(const Pose<T>& other) const;

    friend Pose<T> operator*(const Pose<T>& pose, const T& x)
    {
        return Pose<T>(pose.position * x, pose.rotation * x);
    }
    friend Pose<T> operator*(const T& x, const Pose<T>& pose)
    {
        return Pose<T>(x * pose.position, x * pose.rotation);
    }
    inline Pose<T>& operator*=(const T& x);
    Pose<T> operator/(const T& x) const;

    template <typename T1> Pose<T1> cast() const
    {
        return Pose<T1>(
            position.template cast<T1>(), rotation.template cast<T1>());
    }

    /// Position dof (either 2D or 3D)
    VectorMax3<T> position;
    /// Rotation dof (either 1D or 3D) expressed as a rotation vector
    VectorMax3<T> rotation;
};

typedef Pose<double> PoseD;
typedef Poses<double> PosesD;

template <typename T>
Poses<T> interpolate(const Poses<T>& pose0, const Poses<T>& pose1, T t);
template <typename T> Poses<T> operator*(const Poses<T>& poses, const T& x);
/// @brief Cast poses element-wise.
template <typename T, typename U> Poses<T> cast(const Poses<U>& poses);

template <typename T>
MatrixMax3<T> construct_rotation_matrix(const VectorMax3<T>& r);
template <typename Derived, typename T = typename Derived::Scalar>
Eigen::Quaternion<T> construct_quaternion(const Eigen::MatrixBase<Derived>& r);

template <typename T> Matrix3<T> rotate_to_z(Vector3<T> n);
template <typename T> Matrix3<T> rotate_around_z(const T& theta);
template <typename T>
void decompose_to_z_screwing(
    const Pose<T>& pose_t0,
    const Pose<T>& pose_t1,
    Matrix3<T>& R0,
    Matrix3<T>& P,
    T& omega);

} // namespace ipc::rigid

#include "pose.tpp"


#include <algorithm>
#include <cfloat>
#include <cmath>
#include <cstddef>
#include <cstdlib>
#include <iterator>
#include <iomanip>
#include <iostream>
#include <limits>
#include <vector>

#include <Eigen/Geometry>

#include <nlohmann/json/json.hpp>

#include "geometry/polygon.hpp"

using std::cerr;
using std::max;
using std::min;
using std::endl;
using std::ostream;
using std::string;

using Eigen::Vector2d;

using terrain::geometry::Polygon;

//---------------------------------------------------------------
// Constructor
//
Polygon::Polygon(){
    set_default();
}

Polygon::Polygon(const size_t initial_capacity):
    points(initial_capacity)
{
    points.resize(initial_capacity);
}

Polygon::Polygon(nlohmann::json doc){
    load(doc);
}

Polygon::Polygon(std::vector<Vector2d>& init){
    load(init);
}

Polygon::Polygon(std::initializer_list<Vector2d> init_list)
{
    std::vector<Vector2d> pts = init_list;

    load(pts);
}

void Polygon::clear(){
    points.clear();
}

void Polygon::complete(){
    // cerr << "====== ====== ====== " << endl;
    // write_yaml(cerr, "    ");

    enclose_polygon();
    if(! is_right_handed()){
        std::reverse(std::begin(points), std::end(points));
    }
}

void Polygon::emplace(const double x, const double y){
    points.emplace_back(x,y);
}

void Polygon::enclose_polygon(){
    // ensure that polygon loops back
    const auto& first_Vector2d = points[0];
    const auto& last_Vector2d = points[points.size()-1];

    if( ! first_Vector2d.isApprox(last_Vector2d)){
        points.emplace_back(first_Vector2d);
    }
}

bool Polygon::is_right_handed() const {
    double sum = 0;

    // NOTE THE RANGE!:
    //   this needs to iterate over all doubles
    for( uint i = 0; i < (points.size()-1); ++i ){
        auto& p1 = points[i];
        auto& p2 = points[i+1];
        sum += (p1[0] * p2[1]) - (p1[1]*p2[0]);
    }

    // // The shoelace formula includes a divide-by-2 step; ... but we don't
    // // care about the magnitude, just the sign.  So skip this step.
    // sum /= 2;

    if( 0 > sum ){
        // left-handed / clockwise points
        return false;
    }else{
        // right-handed / counter-clockwise points
        return true;
    }
}

bool Polygon::load(std::vector<Vector2d> source){
    // if the new polygon contains insufficient points, abort and clear.
    if(4 > source.size()){
        return false;
    }

    points = std::move(source);

    complete();

    return true;
}

bool Polygon::load(nlohmann::json doc){
    if(doc.is_array() && doc[0].is_array() && (4 <= doc.size()) ){
        clear();
        points.resize(doc.size());

        size_t pair_index = 0;
        for( auto& pair : doc){
            Vector2d p(pair[0].get<double>(), pair[1].get<double>());
            points[pair_index] = p;
            ++pair_index;
        }

        complete();

        return true;
    }

    return false;
}

terrain::geometry::Layout Polygon::make_layout(const double precision) const {
    double min_x = DBL_MAX;
    double max_x = DBL_MIN;
    double min_y = DBL_MAX;
    double max_y = DBL_MIN;

    for( auto p : points ){
        min_x = min(min_x, p.x());
        max_x = max(max_x, p.x());
        min_y = min(min_y, p.y());
        max_y = max(max_y, p.y());
    }

    const double ctr_x = round( 0.5*( min_x + max_x ) );
    const double ctr_y = round( 0.5*( min_y + max_y ) );
    const double width = max(max_x - min_x, max_y - min_y);

    return Layout(precision, ctr_x, ctr_y, width);
}

Vector2d& Polygon::operator[](const size_t index){
    return points[index];
}

const Vector2d& Polygon::operator[](const size_t index) const {
    return points[index];
}

void Polygon::push_back(const Vector2d p){
    points.push_back(p);
}

size_t Polygon::size() const {
    return points.size();
}

void Polygon::set_default(){
    points.clear();
    points.emplace_back( 0, 0);
    points.emplace_back( 1, 0);
    points.emplace_back( 1, 1);
    points.emplace_back( 0, 1);
}

void Polygon::write_yaml(std::ostream& sink, string indent) const {
    sink << indent << "points: \n";
    for( uint i = 0; i < points.size(); ++i ){
        auto& p = points[i];
        sink << indent << "    - " << p[0] << ", " << p[1] << '\n';
    }
}


///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//                          *** Mesh.hpp ***                                 //
//                                                                           //
// Base class responsible for holding discretized mesh points and weights    //
//                                                                           //
// created November 26, 2017                                                 //
// copyright Christopher N. Singh Binghamton University Physics              //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

#ifndef Mesh_hpp
#define Mesh_hpp

#include <Eigen/Dense>
#include <numeric>
#include <fstream>
#include <string>
#include <sstream>
#include <vector>

class Mesh
{
public:
	Mesh(std::string klist_file);

	Eigen::Vector3d point(int index);

	int weight(int index);
	int size();
	int norm();

private:
	std::vector<Eigen::Vector3d> m_points;
	std::vector<int> m_weights;
};

#endif /* Mesh_hpp */


#ifndef LMHPP
#define LMHPP
/* -------------------------------------------------------

   Levenberg Marquardt algorithm
   Coded by J. de la Cruz Rodriguez (ISP-SU, 2020)

   ------------------------------------------------------- */

#include <vector>
#include <iostream>
#include <cmath>
#include <algorithm>

#include <Eigen/Dense>

#include "Milne.hpp"

namespace lm{

  // ***************************************** //

  template<typename T>
  struct Par{
    bool isCyclic;
    bool limited;
    T scale;
    T limits[2];

    Par(): isCyclic(false), limited(false), scale(1.0), limits{0,0}{};
    Par(bool const cyclic, bool const ilimited, T const scal, T const mi, T const ma):
      isCyclic(cyclic), limited(ilimited), scale(scal), limits{mi,ma}{};

    Par(Par<T> const& in): isCyclic(in.isCyclic) ,limited(in.limited), scale(in.scale), limits{in.limits[0], in.limits[1]}{};

    Par<T> &operator=(Par<T> const& in)
    {
      isCyclic = in.isCyclic, limited = in.limited, scale=in.scale, limits[0]=in.limits[0], limits[1]=in.limits[1];
      return *this;
    }

    inline void Normalize(T &val)const{val /= scale;};

    inline void Scale(T &val)const{val *= scale;};

    inline void Check(T &val)const{
      if(!limited) return;
      if(isCyclic){
	if(val > limits[1]) val -= 3.1415926f;
	if(val < limits[0]) val += 3.1416026f;
      }
      val = std::max<T>(std::min<T>(val, limits[1]),limits[0]);
    }

    inline void CheckNormalized(T &val)const{
      if(!limited) return;
      Scale(val);
      Check(val);
      Normalize(val);
    }

  };

  // ***************************************** //

  template<typename T>
  struct container{
    int const nDat;
    T const mu;
    int Nreal;
    const ml::Milne<T>& Me;
    const T* __restrict__ d;
    const T* __restrict__ sig;
    const std::vector<Par<T>> &Pinfo;

    container(int const nd, T const imu, ml::Milne<T> const& iMe, const T* __restrict__ din, const T* __restrict__ sigin, const std::vector<Par<T>> &Pi): nDat(nd), mu(imu), Nreal(1), Me(iMe), d(din), sig(sigin), Pinfo(Pi)
    {
      // --- only account for non-dummy points in the data array --- //
      Nreal = 0;
      for(int ii = 0; ii<nDat; ++ii)
	if(sig[ii] < 1.e20) Nreal += 1;

    }

  };

  // ***************************************** //

  template<typename T> constexpr inline T SQ(T const v){return v*v;}

  // ***************************************** //

  template<typename T>
  T getChi2(int const nDat, const T* __restrict__ r)
  {
    double sum = 0.0;
    int const nDat4 = nDat/4;

    if(nDat4*4 == nDat){
      double sumI = 0.0;
      double sumQ = 0.0;
      double sumU = 0.0;
      double sumV = 0.0;

      for(int ii=0; ii<nDat4; ++ii){
	sumI += SQ(r[ii*4+0]);
	sumQ += SQ(r[ii*4+1]);
	sumU += SQ(r[ii*4+2]);
	sumV += SQ(r[ii*4+3]);
      }
      sum = sumI + (sumQ + sumU + sumV);
    }else{

      for(int ii=0; ii<nDat;++ii)
	sum += SQ(r[ii]);
    }

    return static_cast<T>(sum);
  }

  // ***************************************** //

  template<typename T>
  T fx(container<T> const& myData, int const nPar, const T* __restrict__ m_in, T* __restrict__ syn, T* __restrict__ r)
  {

    // --- Copy model --- //

    T* __restrict__ m = new T [nPar]();
    std::memcpy(m,m_in,nPar*sizeof(T));

    // --- ///

    int const nDat = myData.nDat;
    const T* __restrict__ dat = myData.d;
    const T* __restrict__ sig = myData.sig;

    // --- Scale up model --- //

    for(int ii=0; ii<nPar; ++ii){
      myData.Pinfo[ii].Scale(m[ii]);
    }

    // --- calculate spectrum --- //

    myData.Me.synthesize(m, syn, myData.mu);

    // --- calculate residue --- //

    T const scl = sqrt(T(myData.Nreal));
    for(int ii=0; ii<nDat; ++ii) r[ii] = (dat[ii] - syn[ii]) / (sig[ii] * scl);

    delete [] m;

    // --- get Chi2 --- //

    return getChi2<T>(nDat, r);
  }
    // ***************************************** //

  template<typename T>
  T fx_dx(container<T> const& myData, int const nPar, const T* __restrict__ m_in, T* __restrict__ syn, T* __restrict__ r, T* __restrict__ J)
  {

    // --- Copy model --- //

    T* __restrict__ m = new T [nPar]();
    std::memcpy(m,m_in,nPar*sizeof(T));

    int const nDat = myData.nDat;
    const T* __restrict__ dat = myData.d;
    const T* __restrict__ sig = myData.sig;

    // --- Scale up model --- //

    for(int ii=0; ii<nPar; ++ii){
      myData.Pinfo[ii].Scale(m[ii]);
    }

    // --- calculate spectrum --- //

    myData.Me.synthesize_rf(m, syn, J, myData.mu);

    // --- calculate residue --- //

    T const scl = sqrt(T(myData.Nreal));
    for(int ii=0; ii<nDat; ++ii) r[ii] = (dat[ii] - syn[ii]) / (sig[ii] * scl);

    // --- scale J --- //

    for(int ii = 0; ii<nPar; ++ii){

      T const iScl = myData.Pinfo[ii].scale / scl;

      for(int ww = 0; ww<nDat; ++ww)
	J[ii*nDat + ww] *=  iScl / sig[ww];
    }

    // --- clean up model array --- //
    delete [] m;

    // --- get Chi2 --- //

    return getChi2<T>(nDat, r);
  }

  // **************************************** //

  template<typename T>
  struct LevMar{
    int nPar;
    std::vector<T> diag;
    std::vector<Par<T>> Pinfo;

    void set(int const& iPar){
      nPar = iPar;
      diag = std::vector<T>(iPar,0.0);
      Pinfo = std::vector<Par<T>>(iPar, Par<T>());
    }

    LevMar(): nPar(0), diag(), Pinfo(){};
    LevMar(int const &nPar_i):LevMar(){set(nPar_i);}

    LevMar(LevMar<T> const& in): LevMar(){nPar = in.nPar, diag=in.diag, Pinfo = in.Pinfo;}

    LevMar<T> &operator=(LevMar<T> const& in){nPar = in.nPar, diag=in.diag(), Pinfo = in.Pinfo; return *this;}

    static inline T checkLambda(T val, T const &mi, T const& ma){return std::max<T>(std::min<T>(ma, val), mi);}

    // ------------------------------------------------------------------------------ //

    T getCorrection(container<T> const& myData, T* __restrict__ m, const T* __restrict__ J, T* __restrict__ syn, T* __restrict__ r, T const iLam)const
    {

      // --- Simplify the notation --- //

      using Mat = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
      using Vec = Eigen::Matrix<T, Eigen::Dynamic, 1>;

      using cVecMap = Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, 1>>;
      using cMatMap = Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>;

      // --- Define some quantities --- //

      int const nDat = myData.nDat;
      int const cPar = nPar;

      Mat A(cPar, cPar); A.setZero();
      Vec B(cPar); B.setZero();
      Vec dm(cPar); dm.setZero();

      // --- get Hessian matrix --- //

      for(int jj = 0; jj<cPar; ++jj){

	// --- Compute left-hand side of the system --- //

	for(int ii=0; ii<=jj; ++ii){
	  double sum = 0.0;

	  for(int ww=0; ww<nDat; ++ww) sum += J[jj*nDat + ww]*J[ii*nDat + ww];
	  A(jj,ii) = A(ii,jj) = static_cast<T>(sum);
	}//ii

	// --- Compute right-hand side of the system --- //

	double sum = 0.0;
	for(int ww = 0; ww<nDat; ww++) sum += J[jj*nDat+ww] * r[ww];
	B[jj] = static_cast<T>(sum);

	A(jj,jj) *= 1+iLam;
      } // jj

      // --- Solve linear system to get solution --- //

      //Eigen::BDCSVD<Mat> sy(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
      Eigen::ColPivHouseholderQR<Mat> sy(A); // Also rank revealing but much faster than SVD
      sy.setThreshold(1.e-14);

      dm = sy.solve(B);

      // --- add to model and check parameters --- //

      for(int ii =0; ii<cPar; ++ii){
	m[ii] += dm[ii];
	Pinfo[ii].CheckNormalized(m[ii]);
      }

      return fx<T>(myData, nPar, m, syn, r);
    }

    // ------------------------------------------------------------------------------ //

    T getStep(container<T> const& myData, T* __restrict__ m, const T* __restrict__ J, T* __restrict__ syn, T* __restrict__ r, T &iLam, bool braket, T const maxLam, T const minLam)const{

      // if(!braket){
	return getCorrection(myData, m, J, syn, r, iLam);
	// }

    }

    // ------------------------------------------------------------------------------ //

    T fitData(ml::Milne<T> const& Me,  int const nDat, const T* __restrict__ dat, T* __restrict__ syn, const T* __restrict__ sig, T* __restrict__ m, T const mu, int const max_iter = 20, T iLam = sqrt(10.0f), T const Chi2_thres = 1.0, T const fx_thres = 2.e-3, int const delay_braket = 2, bool verbose = true)const
    {
      static constexpr T const facLam = 3.1622776601683795;
      static constexpr T const maxLam = 100*facLam;
      static constexpr T const minLam = 1.e-4;
      static constexpr int const max_n_reject = 6;

      // --- Init container --- //

      container<T> myData(nDat, mu, Me, dat, sig, Pinfo);

      // --- Check initial Lambda value --- //

      iLam = checkLambda(iLam, minLam, maxLam);

      // --- Init temp arrays and values--- //

      int const cPar = nPar;
      T bestChi2     = 1.e32;
      T     Chi2     = 1.e32;

      T* __restrict__ bestModel  = new T [cPar]();
      T* __restrict__ bestSyn    = new T [nDat]();

      T* __restrict__     J      = new T [cPar*nDat]();
      T* __restrict__     r      = new T [nDat]();

      // --- Work with normalized quantities --- //

      for(int ii =0; ii<cPar; ++ii){
	Pinfo[ii].Check(m[ii]);
	Pinfo[ii].Normalize(m[ii]);
      }

      std::memcpy(bestModel,  m, cPar*sizeof(T));

      // --- get derivatives and init Chi2 --- //

      bestChi2 = fx_dx<T>(myData, nPar, bestModel, bestSyn, r, J);

      // --- Init iteration --- //

      if(verbose){
	fprintf(stderr, "\nLevDer::fitData: [Init] Chi2=%13.5f\n", bestChi2);
      }

      // --- Iterate --- //

      int iter = 0, n_rejected = 0;
      bool quit = false, tooSmall = false;
      T oLam = 0, dfx = 0;

      while(iter < max_iter){

	oLam = iLam;
	std::memcpy(m, bestModel, nPar*sizeof(T));

	// --- Get model correction --- //

	Chi2 = getStep(myData, m, J, syn, r, iLam, false, minLam, maxLam);

	// --- Did Chi2 improve? --- //

	if(Chi2 < bestChi2){

	  oLam = iLam;
	  dfx = (bestChi2 - Chi2) / bestChi2;

	  bestChi2 = Chi2;
	  std::memcpy(bestModel,   m, cPar*sizeof(T));
	  std::memcpy(bestSyn  , syn, nDat*sizeof(T));

	  if(iLam > 1.0001*minLam)
	    iLam = checkLambda(iLam/facLam, minLam, maxLam);
	  else
	    iLam = 10*minLam;

	  if(dfx < fx_thres){
	    if(tooSmall) quit = true;
	    else tooSmall = true;
	  }

	  n_rejected = 0;
	}else{

	  // --- Increase lambda and re-try --- //

	  iLam = checkLambda(iLam*SQ<T>(facLam), minLam, maxLam);
	  n_rejected += 1;
	  if(verbose)
	    fprintf(stderr,"LevMar::fitData: Chi2=%13.5f > %13.5f -> Increasing lambda %f -> %f\n", Chi2, bestChi2, oLam, iLam);

	  if(n_rejected<max_n_reject) continue;

	}

	// --- Check what has happened with Chi2 --- //

	if(n_rejected >= max_n_reject){
	  if(verbose)
	    fprintf(stderr, "LevMar::fitData: maximum number of rejected iterations reached, finishing inversion");
	  break;
	}

	if(verbose)
	  fprintf(stderr, "LevMar::fitData [%3d] Chi2=%13.5f, lambda=%e\n", iter, Chi2, oLam);

	if(bestChi2 < Chi2_thres){
	  if(verbose)
	    fprintf(stderr, "LevMar::fitData: Chi2 (%f) < Chi2_threshold (%f), finishing inversion", bestChi2, Chi2_thres);
	  break;
	}

	if(quit){
	  if(verbose)
	    fprintf(stderr, "LevMar::fitData: Chi2 improvement too small for 2-iterations, finishing inversion\n");
	  break;
	}

	iter++;
	if(iter >= max_iter){
	  break;
	}

	// --- compute gradient of the new model for next iteration --- //

	std::memcpy(m, bestModel, cPar*sizeof(T));
	fx_dx<T>(myData, nPar, m, syn, r, J);
      }

      std::memcpy(m, bestModel, cPar*sizeof(T));
      std::memcpy(syn, bestSyn, nDat*sizeof(T));

      for(int ii=0; ii<cPar; ++ii)
	Pinfo[ii].Scale(m[ii]);

      // --- Clean-up --- //

      delete [] bestModel;
      delete [] bestSyn;
      delete [] r;
      delete [] J;

      return bestChi2;
    }

  };

}

#endif


#ifndef ALEPH_GEOMETRY_DOWKER_COMPLEX_HH__
#define ALEPH_GEOMETRY_DOWKER_COMPLEX_HH__

#include <aleph/math/Combinations.hh>

#include <aleph/topology/filtrations/Data.hh>

#include <aleph/topology/Simplex.hh>
#include <aleph/topology/SimplicialComplex.hh>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>

#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>

#include <algorithm>
#include <iterator>
#include <limits>
#include <unordered_map>
#include <vector>

namespace aleph
{

namespace geometry
{

namespace detail
{

// FIXME: the weight type of an edge should be configurable as an
// additional template parameter
using EdgeWeightProperty = boost::property<boost::edge_weight_t, double>;

// FIXME: the data type of the graph should be configurable as an
// additional template parameter.
using Graph = boost::adjacency_list<
  boost::vecS,
  boost::vecS,
  boost::directedS,
  boost::no_property,
  EdgeWeightProperty
>;

using VertexDescriptor = boost::graph_traits<Graph>::vertex_descriptor;

template <class T, class I = std::size_t> struct Pair
{
  I p; // first index
  I q; // second index
  T w; // weight
};

template <class D, class V> struct Vertex
{
  V p; // vertex index
  D w; // weight
};

template <class D, class V> void swap( Vertex<D, V>& first, Vertex<D, V>& second )
{
  std::swap( first.p, second.p );
  std::swap( first.w, second.w );
}

} // namespace detail

/**
  Calculates a set of admissible pairs from a matrix of weights and
  a given distance threshold. The matrix of weights does *not* have
  to satisfy symmetry constraints.

  @param W Weighted adjacency matrix
  @param R Maximum weight
*/

template <class Matrix, class T> std::vector< detail::Pair<T> > admissiblePairs( const Matrix& W, T R )
{
  using namespace detail;

  // Convert matrix into a graph ---------------------------------------

  auto n          = W.size();
  using IndexType = decltype(n);

  detail::Graph G( n );

  for( IndexType i = 0; i < n; i++ )
  {
    for( IndexType j = 0; j < n; j++ )
    {
      if( W[i][j] > 0 )
      {
        EdgeWeightProperty weight = W[i][j];

        boost::add_edge( VertexDescriptor(i), VertexDescriptor(j),
                         weight,
                         G );
      }
    }
  }

  double density
    = static_cast<double>( boost::num_edges(G) ) / static_cast<double>( boost::num_vertices(G) * ( boost::num_vertices(G) - 1 ) );

  // This 'pseudo-matrix' contains the completion of the weight function
  // specified by the input matrix.
  std::vector< std::vector<double> > D( boost::num_vertices(G),
                                        std::vector<double>( boost::num_vertices(G) ) );

  if( density >= 0.5 )
    boost::floyd_warshall_all_pairs_shortest_paths( G, D );
  else
    boost::johnson_all_pairs_shortest_paths( G, D );

  std::vector< Pair<T> > pairs;

  // Create admissible pairs -------------------------------------------
  //
  // A pair is admissible if it satisfies a reachability property,
  // meaning that the induced graph distance permits to reach both
  // vertices under the specified distance threshold.

  for( IndexType i = 0; i < n; i++ )
  {
    for( IndexType j = 0; j < n; j++ )
    {
      if( D[i][j] <= R )
        pairs.push_back( {i, j, static_cast<T>( D[i][j] ) } );
    }
  }

  return pairs;
}

/**
  Creates a Dowker sink complex and a Dowker source complex from a given
  set of admissible pairs. A *general* Dowker complex contains a simplex
  if all of its vertices satisfy the admissibility condition.

  @param pairs     Set of admissible pairs
  @param dimension Maximum dimension for expansion. If set to zero, will
                   expand the complex to its maximum dimension.
*/

template <class V, class D, class T>
std::pair<
  topology::SimplicialComplex< topology::Simplex<D, V> >,
  topology::SimplicialComplex< topology::Simplex<D, V> >
> buildDowkerSinkSourceComplexes( const std::vector<detail::Pair<T> >& pairs,
                                  unsigned dimension = 0 )
{
  using namespace detail;

  using Simplex           = topology::Simplex<D, V>;
  using SimplicialComplex = topology::SimplicialComplex<Simplex>;

  using VertexType     = V;
  VertexType maxVertex = VertexType();

  for( auto&& pair : pairs )
  {
    maxVertex = std::max(maxVertex, VertexType(pair.p) );
    maxVertex = std::max(maxVertex, VertexType(pair.q) );
  }

  using Vertex = Vertex<D, V>;

  // Keep track of the mapping induced by fixing either the source
  // points or the sink points.
  std::unordered_map< VertexType, std::vector<Vertex> > sourceBasePointMap;
  std::unordered_map< VertexType, std::vector<Vertex> > sinkBasePointMap;

  for( auto&& pair : pairs )
  {
    auto&& p = pair.p;
    auto&& q = pair.q;

    sourceBasePointMap[ VertexType(p) ].push_back( { VertexType(q), pair.w } );
    sinkBasePointMap[ VertexType(q) ].push_back( { VertexType(p), pair.w } );
  }

  // Auxiliary weight calculation lambda function ----------------------
  //
  // This function calculates the *maximum* weight of a range of
  // vertices. It is used to determine the weight of a simplex.

  using Iterator = typename std::vector<Vertex>::const_iterator;
  auto getWeight = [] ( Iterator begin, Iterator end )
  {
    using DataType  = D;
    DataType weight = std::numeric_limits<DataType>::lowest();

    for( auto it = begin; it != end; ++it )
      weight = std::max( weight, it->w );

    return weight;
  };

  auto makeSimplices = [&dimension, &getWeight] ( const std::unordered_map< VertexType, std::vector<Vertex> >& map )
  {
    std::vector<Simplex> simplices;
    std::unordered_map<Simplex, D> simplex_to_weight;

    for( auto&& pair : map )
    {
      auto vertices            = pair.second;
      std::size_t maxDimension = 0;

      if( dimension == 0 )
        maxDimension = vertices.size();
      else
        maxDimension = dimension + 1;

      using DifferenceType = typename decltype(vertices)::difference_type;

      for( std::size_t d = std::min( vertices.size(), maxDimension ); d >= 1; d-- )
      {
        math::for_each_combination( vertices.begin(), vertices.begin() + DifferenceType(d), vertices.end(),
          [&simplex_to_weight, &getWeight] ( Iterator first, Iterator last )
          {
            std::vector<V> vertices_;
            vertices_.reserve( typename std::vector<V>::size_type( std::distance( first, last ) ) );

            for( auto it = first; it != last; ++it )
              vertices_.push_back( it->p );

            Simplex s( vertices_.begin(), vertices_.end() );

            if( simplex_to_weight.find(s) == simplex_to_weight.end() )
              simplex_to_weight[s] = getWeight(first, last);
            else
              simplex_to_weight[s] = std::min( simplex_to_weight[s], getWeight(first, last) );

            return false;
          }
        );
      }
    }

    for( auto&& pair : simplex_to_weight )
    {
      auto s = pair.first;
      s.setData( pair.second );

      simplices.push_back( s );
    }

    return simplices;
  };

  auto sourceEdges = makeSimplices( sourceBasePointMap );
  auto sinkEdges   = makeSimplices( sinkBasePointMap );

  SimplicialComplex dowkerSourceComplex( sourceEdges.begin(), sourceEdges.end() );
  SimplicialComplex dowkerSinkComplex  ( sinkEdges.begin()  , sinkEdges.end()   );

  dowkerSourceComplex.sort( topology::filtrations::Data<Simplex>() );
  dowkerSinkComplex.sort( topology::filtrations::Data<Simplex>() );

  return std::make_pair( dowkerSourceComplex, dowkerSinkComplex );
}

/**
  Given a matrix and a maximum radius, creates a Dowker source complex that
  contains a simplex if all of its vertices are admissible.

  @param matrix    Matrix of weighted adjacencies. The matrix is *not*
                   assumed to be symmetric.

  @param epsilon   Maximum radius for expansion. I refer to this as epsilon
                   in order to show the connection to other simplicial complex
                   creation algorithms.

  @param dimension Maximum dimension for expansion. If set to zero, will
                   expand the complex to its maximum dimension.
*/

template
<
  class Matrix,
  class VertexType,
  class DataType
> topology::SimplicialComplex< topology::Simplex<DataType, VertexType> >
    buildDowkerSourceCompplex( const Matrix& matrix,
                               DataType epsilon,
                               unsigned dimension = 0 )
{
  using namespace detail;

  auto pairs = admissiblePairs( matrix, epsilon );

  using Simplex           = topology::Simplex<DataType, VertexType>;
  using SimplicialComplex = topology::SimplicialComplex<Simplex>;

  VertexType maxVertex = VertexType();

  for( auto&& pair : pairs )
  {
    maxVertex = std::max(maxVertex, VertexType(pair.p) );
    maxVertex = std::max(maxVertex, VertexType(pair.q) );
  }

  using Vertex = Vertex<DataType, VertexType>;

  // Keep track of the mapping induced by fixing the source points. In
  // essence, this is a adjacency list representation of a matrix that
  // tracks the admissibility of vertices.
  std::unordered_map< VertexType, std::vector<Vertex> > sourceBasePointMap;

  for( auto&& pair : pairs )
  {
    auto&& p = pair.p;
    auto&& q = pair.q;

    sourceBasePointMap[ VertexType(p) ].push_back( { VertexType(q), pair.w } );
  }

  // Auxiliary weight calculation lambda function ----------------------
  //
  // This function calculates the *maximum* weight of a range of
  // vertices. It is used to determine the weight of a simplex.

  using Iterator = typename std::vector<Vertex>::const_iterator;
  auto getWeight = [] ( Iterator begin, Iterator end )
  {
    DataType weight = std::numeric_limits<DataType>::lowest();

    for( auto it = begin; it != end; ++it )
      weight = std::max( weight, it->w );

    return weight;
  };

  // Create all valid simplices ----------------------------------------
  //
  // All valid simplices with respect to the source point are created
  // by generating all combinations of admissible pairs, with respect
  // to the given source point.
  //
  std::vector<Simplex> simplices;

  {
    // The same simplex may occur multiple times because it is
    // 'observed' by multiple source points. We need to obtain
    // the *earliest* weight at which the simplex occurs!
    std::unordered_map<Simplex, DataType> simplex_to_weight;

    for( auto&& pair : sourceBasePointMap )
    {
      auto vertices            = pair.second;
      std::size_t maxDimension = 0;

      if( dimension == 0 )
        maxDimension = vertices.size();
      else
        maxDimension = dimension + 1;

      using DifferenceType = typename decltype(vertices)::difference_type;

      for( std::size_t d = std::min( vertices.size(), maxDimension ); d >= 1; d-- )
      {
        math::for_each_combination( vertices.begin(), vertices.begin() + DifferenceType(d), vertices.end(),
          [&simplex_to_weight, &getWeight] ( Iterator first, Iterator last )
          {
            std::vector<VertexType> vertices_;
            vertices_.reserve( typename std::vector<VertexType>::size_type( std::distance( first, last ) ) );

            for( auto it = first; it != last; ++it )
              vertices_.push_back( it->p );

            Simplex s( vertices_.begin(), vertices_.end() );

            // Ensures that we do not take the default weight, which is
            // zero, as the weight of the simplex---there does not seem
            // to be a way to solve this more efficiently...
            if( simplex_to_weight.find(s) == simplex_to_weight.end() )
              simplex_to_weight[s] = getWeight(first, last);
            else
              simplex_to_weight[s] = std::min( simplex_to_weight[s], getWeight(first, last) );

            return false;
          }
        );
      }
    }

    // Set the weights of all simplices prior to inserting them into the
    // simplicial complex.
    for( auto&& pair : simplex_to_weight )
    {
      auto s = pair.first;
      s.setData( pair.second );

      simplices.push_back( s );
    }
  }

  SimplicialComplex K( simplices.begin(), simplices.end() );
  K.sort( topology::filtrations::Data<Simplex>() );

  return K;
}

} // namespace geometry

} // namespace aleph

#endif


// Boost.Geometry

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to 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)

#ifndef BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_SIDE_BY_AZIMUTH_HPP
#define BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_SIDE_BY_AZIMUTH_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>
#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/algorithms/detail/azimuth.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategies/side.hpp>
//#include <boost/geometry/strategies/concepts/side_concept.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Check at which side of a segment a point lies
         left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam Model Reference model of coordinate system.
\tparam CalculationType \tparam_calculation
 */
template <typename Model, typename CalculationType = void>
class side_by_azimuth
{
public:
    side_by_azimuth(Model const& model = Model())
        : m_model(model)
    {}

    template <typename P1, typename P2, typename P>
    inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename promote_floating_point
            <
                typename select_calculation_type_alt
                    <
                        CalculationType,
                        P1, P2, P
                    >::type
            >::type calc_t;

        calc_t d1 = 0.001;
        calc_t crs_AD = geometry::detail::azimuth<calc_t>(p1, p, m_model);
        calc_t crs_AB = geometry::detail::azimuth<calc_t>(p1, p2, m_model);
        calc_t XTD = asin(sin(d1) * sin(crs_AD - crs_AB));

        return math::equals(XTD, 0) ? 0 : XTD < 0 ? 1 : -1;
    }

private:
    Model m_model;
};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_SIDE_BY_AZIMUTH_HPP


#include "catch.hpp"

#include "libirc/io.h"

#include "config.h"

#ifdef HAVE_ARMA
#include <armadillo>
using arma::mat;
using arma::vec;
using arma::vec3;
#elif HAVE_EIGEN3
#include <Eigen/Dense>
using vec3 = Eigen::Vector3d;
using vec = Eigen::VectorXd;
using mat = Eigen::MatrixXd;

#else
#error
#endif

TEST_CASE("File not found") {
  using irc::io::load_xyz;

  REQUIRE_THROWS_AS(load_xyz<vec3>(irc::config::molecules_dir + "ABC.xyz"),
                    std::runtime_error);

  REQUIRE_NOTHROW(load_xyz<vec3>(irc::config::molecules_dir + "caffeine.xyz"));
}

TEST_CASE("Print molecule") {
  using namespace irc;
  using namespace io;
  using namespace connectivity;
  using namespace molecule;
  using namespace tools;

  // Load toluene molecule
  const auto mol = load_xyz<vec3>(config::molecules_dir + "benzene_dimer.xyz");

  // Compute interatomic distance for formaldehyde molecule
  const mat dd{distances<vec3, mat>(mol)};

  // Build graph based on the adjacency matrix
  const UGraph adj{adjacency_matrix(dd, mol)};

  // Compute distance matrix and predecessor matrix
  mat dist{distance_matrix<mat>(adj)};

  // Compute bonds
  const std::vector<Bond> B{bonds(dist, mol)};

  // Print bonds to std::cout
  print_bonds<vec3, vec>(to_cartesian<vec3, vec>(mol), B);

  // Compute angles
  const std::vector<Angle> A{angles(dist, mol)};

  // Print angles to std::cout
  print_angles<vec3, vec>(to_cartesian<vec3, vec>(mol), A);

  // Compute dihedral angles
  const std::vector<Dihedral> D{dihedrals(dist, mol)};

  // Print dihedrals to std::cout
  print_dihedrals<vec3, vec>(to_cartesian<vec3, vec>(mol), D);

  // Compute out of plane bends
  const std::vector<OutOfPlaneBend> OOPB{out_of_plane_bends(dist, mol)};

  // Print dihedrals to std::cout
  print_out_of_plane_bends<vec3, vec>(to_cartesian<vec3, vec>(mol), OOPB);
}


#ifndef __BETA_H__
#define __BETA_H__

#include <armadillo>

#include <vector>

/**
 * Estimates parameters for the beta distribution using the
 * method of moments. The method of moments estimates will have
 * a higher variance than maximum likelihood, but is faster and
 * simpler.
 *
 * @param samples A list of samples to fit a beta distribution to.
 *
 * @return The estimated parameters, if they could not be estimated,
 *         the parameters 1,1 will be returned.
 */
arma::vec mom_beta(const arma::vec &samples);

#endif /* End of __BETA_H__ */


/*
 [auto_generated]
 libs/numeric/odeint/test/integrate_times.cpp

 [begin_description]
 This file tests the integrate_times function and its variants.
 [end_description]

 Copyright 2011-2012 Karsten Ahnert
 Copyright 2011-2012 Mario Mulansky

 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)
 */

#define BOOST_TEST_MODULE odeint_integrate_times

#include <boost/test/unit_test.hpp>

#include <utility>
#include <iostream>
#include <vector>

#include <boost/ref.hpp>
#include <boost/iterator/counting_iterator.hpp>

#ifndef ODEINT_INTEGRATE_ITERATOR
#include <boost/numeric/odeint/integrate/integrate_times.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#else
#include <boost/numeric/odeint/iterator/integrate/integrate_times.hpp>
#include <boost/numeric/odeint/iterator/integrate/integrate_adaptive.hpp>
#endif
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp>
#include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

typedef double value_type;
typedef std::vector< value_type > state_type;

void lorenz( const state_type &x , state_type &dxdt , const value_type t )
{
    BOOST_CHECK( t >= 0.0 );

    const value_type sigma( 10.0 );
    const value_type R( 28.0 );
    const value_type b( value_type( 8.0 ) / value_type( 3.0 ) );

    dxdt[0] = sigma * ( x[1] - x[0] );
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

struct push_back_time
{
    std::vector< double >& m_times;

    push_back_time( std::vector< double > &times )
    :  m_times( times ) { }

    void operator()( const state_type &x , double t )
    {
        m_times.push_back( t );
    }
};

BOOST_AUTO_TEST_SUITE( integrate_times_test )

BOOST_AUTO_TEST_CASE( test_integrate_times )
{

    state_type x( 3 );
    x[0] = x[1] = x[2] = 10.0;

    const value_type dt = 0.03;

    std::vector< double > times;

    std::cout << "test rk4 stepper" << std::endl;

    // simple stepper
    integrate_times( runge_kutta4< state_type >() , lorenz , x , boost::counting_iterator<int>(0) , boost::counting_iterator<int>(10) ,
                dt , push_back_time( times ) );

    for( int i=0 ; i<10 ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_EQUAL( times[i] , static_cast<double>(i) );
    times.clear();

    std::cout << "test dopri5 stepper" << std::endl;

    // controlled stepper
    integrate_times( controlled_runge_kutta< runge_kutta_dopri5< state_type > >() , lorenz , x ,
                boost::counting_iterator<int>(0) , boost::counting_iterator<int>(10) ,
                dt , push_back_time( times ) );

    for( int i=0 ; i<10 ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_EQUAL( times[i] , static_cast<double>(i) );
    times.clear();

    std::cout << "test BS stepper" << std::endl;

    //another controlled stepper
    integrate_times( bulirsch_stoer< state_type >() , lorenz , x ,
                boost::counting_iterator<int>(0) , boost::counting_iterator<int>(10) ,
                dt , push_back_time( times ) );

    for( int i=0 ; i<10 ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_EQUAL( times[i] , static_cast<double>(i) );
    times.clear();

    std::cout << "test dense_out stepper" << std::endl;

    // dense output stepper
    integrate_times( dense_output_runge_kutta< controlled_runge_kutta< runge_kutta_dopri5< state_type > > >() ,
                     lorenz , x ,
                     boost::counting_iterator<int>(0) , boost::counting_iterator<int>(10) ,
                     dt , push_back_time( times ) );

    for( int i=0 ; i<10 ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_EQUAL( times[i] , static_cast<double>(i) );

    std::cout << "test BS_do stepper" << std::endl;

    integrate_times( bulirsch_stoer_dense_out< state_type >() , lorenz , x ,
                boost::counting_iterator<int>(0) , boost::counting_iterator<int>(10) ,
                dt , push_back_time( times ) );

    for( int i=0 ; i<10 ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_EQUAL( times[i] , static_cast<double>(i) );

}

BOOST_AUTO_TEST_CASE( test_integrate_times_ranges )
{

    state_type x( 3 );
    x[0] = x[1] = x[2] = 10.0;

    const value_type dt = 0.03;

    std::vector< double > times;

    // simple stepper
    integrate_times( runge_kutta4< state_type >() , lorenz , x ,
                std::make_pair( boost::counting_iterator<int>(0) , boost::counting_iterator<int>(10) ) ,
                dt , push_back_time( times ) );

    for( int i=0 ; i<10 ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_EQUAL( times[i] , static_cast<double>(i) );
    times.clear();

    // controlled stepper
    integrate_times( controlled_runge_kutta< runge_kutta_dopri5< state_type > >() , lorenz , x ,
                std::make_pair( boost::counting_iterator<int>(0) , boost::counting_iterator<int>(10) ) ,
                dt , push_back_time( times ) );

    for( int i=0 ; i<10 ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_EQUAL( times[i] , static_cast<double>(i) );
    times.clear();

    //another controlled stepper
    integrate_times( bulirsch_stoer< state_type >() , lorenz , x ,
                std::make_pair( boost::counting_iterator<int>(0) , boost::counting_iterator<int>(10) ) ,
                dt , push_back_time( times ) );

    for( int i=0 ; i<10 ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_EQUAL( times[i] , static_cast<double>(i) );
    times.clear();

    // dense output stepper
    integrate_times( bulirsch_stoer_dense_out< state_type >() , lorenz , x ,
                std::make_pair( boost::counting_iterator<int>(0) , boost::counting_iterator<int>(10) ) ,
                dt , push_back_time( times ) );

    for( int i=0 ; i<10 ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_EQUAL( times[i] , static_cast<double>(i) );

}

BOOST_AUTO_TEST_CASE( test_integrate_times_overshoot )
{
    state_type x( 3 );
    x[0] = x[1] = x[2] = 10.0;
    double dt = -0.1;

    std::vector<double> times( 10 );
    for( int i=0 ; i<10 ; ++i )
            times[i] = 1.0-i*1.0/9.0;

    std::cout << "test rk4 stepper" << std::endl;
    // simple stepper
    std::vector<double> obs_times;
    int steps = integrate_times( runge_kutta4< state_type >() , lorenz , x ,
                                 times.begin() , times.end() ,
                                 dt , push_back_time( obs_times ) );
// different behavior for the iterator based integrate implementaton
#ifndef ODEINT_INTEGRATE_ITERATOR
    BOOST_CHECK_EQUAL( steps , 18 ); // we really need 18 steps because dt and
                                     // the difference of the observation times
                                     // are so out of sync
#else
    // iterator based implementation can only return the number of iteration steps
    BOOST_CHECK_EQUAL( steps , 9 );
#endif
    for( int i=0 ; i<10 ; ++i )
        BOOST_CHECK_EQUAL( times[i] , obs_times[i] );

    std::cout << "test rk_ck stepper" << std::endl;
    // controlled stepper
    obs_times.clear();
    integrate_times( controlled_runge_kutta< runge_kutta_cash_karp54< state_type > >() , lorenz , x ,
                     times.begin() , times.end() ,
                     dt , push_back_time( obs_times ) );
    for( int i=0 ; i<10 ; ++i )
        BOOST_CHECK_EQUAL( times[i] , obs_times[i] );

    std::cout << "test dopri5 stepper" << std::endl;
    // controlled stepper
    obs_times.clear();
    integrate_times( dense_output_runge_kutta< controlled_runge_kutta< runge_kutta_dopri5< state_type > > >() , lorenz , x ,
                     times.begin() , times.end() ,
                     dt , push_back_time( obs_times ) );
    for( int i=0 ; i<10 ; ++i )
        BOOST_CHECK_EQUAL( times[i] , obs_times[i] );
}

BOOST_AUTO_TEST_SUITE_END()