/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2000 - 2018 by the deal.II authors
 *
 * This file is modification of the version in the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, University of Heidelberg, 2000
 */
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/multithread_info.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/tensor_function.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_gmres.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>
#include <fstream>
#include <iostream>

#include <bench_base.hpp>

DEFINE_uint32(
    num_refine_cycles, 1,
    "Number of refinement cycles for the adaptive refinement within deal.ii");
DEFINE_uint32(init_refine_level, 4,
              "Initial level for the refinement of the mesh.");
DEFINE_bool(dealii_orig, false, "Solve with dealii iterative GMRES");
DEFINE_bool(vis_sol, false, "Print the solution for visualization");

#define CHECK_HERE std::cout << "Here " << __LINE__ << std::endl;

using namespace dealii;

template <int dim>
class AdvectionField : public TensorFunction<1, dim> {
public:
    virtual Tensor<1, dim> value(const Point<dim> &p) const override;
    DeclException2(ExcDimensionMismatch, unsigned int, unsigned int,
                   << "The vector has size " << arg1 << " but should have "
                   << arg2 << " elements.");
};

template <int dim>
Tensor<1, dim> AdvectionField<dim>::value(const Point<dim> &p) const
{
    Point<dim> value;
    value[0] = 2;
    for (unsigned int i = 1; i < dim; ++i)
        value[i] = 1 + 0.8 * std::sin(8. * numbers::PI * p[0]);
    return value;
}

template <int dim>
class RightHandSide : public Function<dim> {
public:
    virtual double value(const Point<dim> &p,
                         const unsigned int component = 0) const override;

private:
    static const Point<dim> center_point;
};

template <>
const Point<1> RightHandSide<1>::center_point = Point<1>(-0.75);
template <>
const Point<2> RightHandSide<2>::center_point = Point<2>(-0.75, -0.75);
template <>
const Point<3> RightHandSide<3>::center_point = Point<3>(-0.75, -0.75, -0.75);

template <int dim>
double RightHandSide<dim>::value(const Point<dim> &p,
                                 const unsigned int component) const
{
    (void)component;
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    const double diameter = 0.1;
    return ((p - center_point).norm_square() < diameter * diameter
                ? 0.1 / std::pow(diameter, dim)
                : 0.1);
}

template <int dim>
class BoundaryValues : public Function<dim> {
public:
    virtual double value(const Point<dim> &p,
                         const unsigned int component = 0) const override;
};

template <int dim>
double BoundaryValues<dim>::value(const Point<dim> &p,
                                  const unsigned int component) const
{
    (void)component;
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    const double sine_term = std::sin(16. * numbers::PI * p.norm_square());
    const double weight = std::exp(5. * (1. - p.norm_square()));
    return weight * sine_term;
}

template <int dim>
class AdvectionProblem : public BenchBase<double, int> {
public:
    AdvectionProblem();
    void run();
    void run(MPI_Comm mpi_communicator);

private:
    void setup_system();
    struct AssemblyScratchData {
        AssemblyScratchData(const FiniteElement<dim> &fe);
        AssemblyScratchData(const AssemblyScratchData &scratch_data);
        FEValues<dim> fe_values;
        FEFaceValues<dim> fe_face_values;
        std::vector<double> rhs_values;
        std::vector<Tensor<1, dim>> advection_directions;
        std::vector<double> face_boundary_values;
        std::vector<Tensor<1, dim>> face_advection_directions;
        AdvectionField<dim> advection_field;
        RightHandSide<dim> right_hand_side;
        BoundaryValues<dim> boundary_values;
    };
    struct AssemblyCopyData {
        FullMatrix<double> cell_matrix;
        Vector<double> cell_rhs;
        std::vector<types::global_dof_index> local_dof_indices;
    };
    void assemble_system();
    void local_assemble_system(
        const typename DoFHandler<dim>::active_cell_iterator &cell,
        AssemblyScratchData &scratch, AssemblyCopyData &copy_data);
    void copy_local_to_global(const AssemblyCopyData &copy_data);
    void solve();
    void solve(MPI_Comm mpi_communicator);
    void refine_grid();
    void output_results(const unsigned int cycle) const;
    Triangulation<dim> triangulation;
    DoFHandler<dim> dof_handler;
    FE_Q<dim> fe;
    AffineConstraints<double> hanging_node_constraints;
    SparsityPattern sparsity_pattern;
    SparseMatrix<double> system_matrix;
    Vector<double> solution;
    Vector<double> system_rhs;
};

class GradientEstimation {
public:
    template <int dim>
    static void estimate(const DoFHandler<dim> &dof,
                         const Vector<double> &solution,
                         Vector<float> &error_per_cell);
    DeclException2(ExcInvalidVectorLength, int, int,
                   << "Vector has length " << arg1 << ", but should have "
                   << arg2);
    DeclException0(ExcInsufficientDirections);

private:
    template <int dim>
    struct EstimateScratchData {
        EstimateScratchData(const FiniteElement<dim> &fe,
                            const Vector<double> &solution,
                            Vector<float> &error_per_cell);
        EstimateScratchData(const EstimateScratchData &data);
        FEValues<dim> fe_midpoint_value;
        std::vector<typename DoFHandler<dim>::active_cell_iterator>
            active_neighbors;
        const Vector<double> &solution;
        Vector<float> &error_per_cell;
        std::vector<double> cell_midpoint_value;
        std::vector<double> neighbor_midpoint_value;
    };
    struct EstimateCopyData {};
    template <int dim>
    static void estimate_cell(
        const typename DoFHandler<dim>::active_cell_iterator &cell,
        EstimateScratchData<dim> &scratch_data,
        const EstimateCopyData &copy_data);
};

template <int dim>
AdvectionProblem<dim>::AdvectionProblem() : dof_handler(triangulation), fe(5)
{}

template <int dim>
void AdvectionProblem<dim>::setup_system()
{
    dof_handler.distribute_dofs(fe);
    hanging_node_constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler,
                                            hanging_node_constraints);
    hanging_node_constraints.close();
    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp, hanging_node_constraints,
                                    /*keep_constrained_dofs =*/false);
    sparsity_pattern.copy_from(dsp);
    system_matrix.reinit(sparsity_pattern);
    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
}

template <int dim>
void AdvectionProblem<dim>::assemble_system()
{
    WorkStream::run(dof_handler.begin_active(), dof_handler.end(), *this,
                    &AdvectionProblem::local_assemble_system,
                    &AdvectionProblem::copy_local_to_global,
                    AssemblyScratchData(fe), AssemblyCopyData());
}

template <int dim>
AdvectionProblem<dim>::AssemblyScratchData::AssemblyScratchData(
    const FiniteElement<dim> &fe)
    : fe_values(fe, QGauss<dim>(fe.degree + 1),
                update_values | update_gradients | update_quadrature_points |
                    update_JxW_values),
      fe_face_values(fe, QGauss<dim - 1>(fe.degree + 1),
                     update_values | update_quadrature_points |
                         update_JxW_values | update_normal_vectors),
      rhs_values(fe_values.get_quadrature().size()),
      advection_directions(fe_values.get_quadrature().size()),
      face_boundary_values(fe_face_values.get_quadrature().size()),
      face_advection_directions(fe_face_values.get_quadrature().size())
{}

template <int dim>
AdvectionProblem<dim>::AssemblyScratchData::AssemblyScratchData(
    const AssemblyScratchData &scratch_data)
    : fe_values(scratch_data.fe_values.get_fe(),
                scratch_data.fe_values.get_quadrature(),
                update_values | update_gradients | update_quadrature_points |
                    update_JxW_values),
      fe_face_values(scratch_data.fe_face_values.get_fe(),
                     scratch_data.fe_face_values.get_quadrature(),
                     update_values | update_quadrature_points |
                         update_JxW_values | update_normal_vectors),
      rhs_values(scratch_data.rhs_values.size()),
      advection_directions(scratch_data.advection_directions.size()),
      face_boundary_values(scratch_data.face_boundary_values.size()),
      face_advection_directions(scratch_data.face_advection_directions.size())
{}

template <int dim>
void AdvectionProblem<dim>::local_assemble_system(
    const typename DoFHandler<dim>::active_cell_iterator &cell,
    AssemblyScratchData &scratch_data, AssemblyCopyData &copy_data)
{
    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    const unsigned int n_q_points =
        scratch_data.fe_values.get_quadrature().size();
    const unsigned int n_face_q_points =
        scratch_data.fe_face_values.get_quadrature().size();
    copy_data.cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
    copy_data.cell_rhs.reinit(dofs_per_cell);
    copy_data.local_dof_indices.resize(dofs_per_cell);
    scratch_data.fe_values.reinit(cell);
    scratch_data.advection_field.value_list(
        scratch_data.fe_values.get_quadrature_points(),
        scratch_data.advection_directions);
    scratch_data.right_hand_side.value_list(
        scratch_data.fe_values.get_quadrature_points(),
        scratch_data.rhs_values);
    const double delta = 0.1 * cell->diameter();
    for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
        for (unsigned int i = 0; i < dofs_per_cell; ++i) {
            const auto &sd = scratch_data;
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
                copy_data.cell_matrix(i, j) +=
                    ((sd.fe_values.shape_value(i, q_point) +       // (phi_i +
                      delta * (sd.advection_directions[q_point] *  // delta beta
                               sd.fe_values.shape_grad(
                                   i, q_point))) *           // grad phi_i)
                     sd.advection_directions[q_point] *      // beta
                     sd.fe_values.shape_grad(j, q_point)) *  // grad phi_j
                    sd.fe_values.JxW(q_point);               // dx
            copy_data.cell_rhs(i) +=
                (sd.fe_values.shape_value(i, q_point) +  // (phi_i +
                 delta *
                     (sd.advection_directions[q_point] *       // delta beta
                      sd.fe_values.shape_grad(i, q_point))) *  // grad phi_i)
                sd.rhs_values[q_point] *                       // f
                sd.fe_values.JxW(q_point);                     // dx
        }
    for (const auto &face : cell->face_iterators())
        if (face->at_boundary()) {
            scratch_data.fe_face_values.reinit(cell, face);
            scratch_data.boundary_values.value_list(
                scratch_data.fe_face_values.get_quadrature_points(),
                scratch_data.face_boundary_values);
            scratch_data.advection_field.value_list(
                scratch_data.fe_face_values.get_quadrature_points(),
                scratch_data.face_advection_directions);
            for (unsigned int q_point = 0; q_point < n_face_q_points; ++q_point)
                if (scratch_data.fe_face_values.normal_vector(q_point) *
                        scratch_data.face_advection_directions[q_point] <
                    0.)
                    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
                        for (unsigned int j = 0; j < dofs_per_cell; ++j)
                            copy_data.cell_matrix(i, j) -=
                                (scratch_data
                                     .face_advection_directions[q_point] *
                                 scratch_data.fe_face_values.normal_vector(
                                     q_point) *
                                 scratch_data.fe_face_values.shape_value(
                                     i, q_point) *
                                 scratch_data.fe_face_values.shape_value(
                                     j, q_point) *
                                 scratch_data.fe_face_values.JxW(q_point));
                        copy_data.cell_rhs(i) -=
                            (scratch_data.face_advection_directions[q_point] *
                             scratch_data.fe_face_values.normal_vector(
                                 q_point) *
                             scratch_data.face_boundary_values[q_point] *
                             scratch_data.fe_face_values.shape_value(i,
                                                                     q_point) *
                             scratch_data.fe_face_values.JxW(q_point));
                    }
        }
    cell->get_dof_indices(copy_data.local_dof_indices);
}

template <int dim>
void AdvectionProblem<dim>::copy_local_to_global(
    const AssemblyCopyData &copy_data)
{
    hanging_node_constraints.distribute_local_to_global(
        copy_data.cell_matrix, copy_data.cell_rhs, copy_data.local_dof_indices,
        system_matrix, system_rhs);
}

template <int dim>
void AdvectionProblem<dim>::solve(MPI_Comm mpi_communicator)
{
    using ValueType = double;
    using IndexType = int;
    schwz::Metadata<ValueType, IndexType> metadata;
    schwz::Settings settings(FLAGS_executor);

    // Set solver metadata from command line args.
    metadata.mpi_communicator = mpi_communicator;
    MPI_Comm_rank(metadata.mpi_communicator, &metadata.my_rank);
    MPI_Comm_size(metadata.mpi_communicator, &metadata.comm_size);
    metadata.tolerance = FLAGS_set_tol;
    metadata.max_iters = FLAGS_num_iters;
    metadata.num_subdomains = metadata.comm_size;
    metadata.num_threads = FLAGS_num_threads;
    metadata.oned_laplacian_size = FLAGS_set_1d_laplacian_size;

    // Generic settings
    settings.write_debug_out = FLAGS_enable_debug_write;
    settings.write_perm_data = FLAGS_write_perm_data;
    settings.write_iters_and_residuals = FLAGS_write_iters_and_residuals;
    settings.print_matrices = FLAGS_print_matrices;
    settings.shifted_iter = FLAGS_shifted_iter;

    // Set solver settings from command line args.
    // Comm settings
    settings.comm_settings.enable_onesided = FLAGS_enable_onesided;
    if (FLAGS_remote_comm_type == "put") {
        settings.comm_settings.enable_put = true;
        settings.comm_settings.enable_get = false;
    } else if (FLAGS_remote_comm_type == "get") {
        settings.comm_settings.enable_put = false;
        settings.comm_settings.enable_get = true;
    }
    settings.comm_settings.enable_one_by_one = FLAGS_enable_one_by_one;
    settings.comm_settings.stage_through_host = FLAGS_stage_through_host;
    settings.comm_settings.enable_overlap = FLAGS_enable_comm_overlap;
    if (FLAGS_flush_type == "flush-all") {
        settings.comm_settings.enable_flush_all = true;
    } else if (FLAGS_flush_type == "flush-local") {
        settings.comm_settings.enable_flush_all = false;
        settings.comm_settings.enable_flush_local = true;
    }
    if (FLAGS_lock_type == "lock-all") {
        settings.comm_settings.enable_lock_all = true;
    } else if (FLAGS_lock_type == "lock-local") {
        settings.comm_settings.enable_lock_all = false;
        settings.comm_settings.enable_lock_local = true;
    }

    // Convergence settings
    settings.convergence_settings.put_all_local_residual_norms =
        FLAGS_enable_put_all_local_residual_norms;
    settings.convergence_settings.enable_global_check_iter_offset =
        FLAGS_enable_global_check_iter_offset;
    settings.convergence_settings.enable_global_check =
        FLAGS_enable_global_check;
    if (FLAGS_global_convergence_type == "centralized-tree") {
        settings.convergence_settings.enable_global_simple_tree = true;
    } else if (FLAGS_global_convergence_type == "decentralized") {
        settings.convergence_settings.enable_decentralized_leader_election =
            true;
        settings.convergence_settings.enable_accumulate =
            FLAGS_enable_decentralized_accumulate;
    }

    // General solver settings
    metadata.local_solver_tolerance = FLAGS_local_tol;
    metadata.local_precond = FLAGS_local_precond;
    metadata.local_max_iters = FLAGS_local_max_iters;
    metadata.updated_max_iters = FLAGS_updated_max_iters;
    settings.non_symmetric_matrix = FLAGS_non_symmetric_matrix;
    settings.restart_iter = FLAGS_restart_iter;
    settings.reset_local_crit_iter = FLAGS_reset_local_crit_iter;
    settings.enable_logging = FLAGS_enable_logging;
    metadata.precond_max_block_size = FLAGS_precond_max_block_size;
    settings.matrix_filename = FLAGS_matrix_filename;
    settings.explicit_laplacian = FLAGS_explicit_laplacian;
    settings.enable_random_rhs = FLAGS_enable_random_rhs;
    settings.use_mixed_precision = FLAGS_use_mixed_precision;
    settings.overlap = FLAGS_overlap;
    settings.naturally_ordered_factor = FLAGS_factor_ordering_natural;
    settings.reorder = FLAGS_local_reordering;
    settings.factorization = FLAGS_local_factorization;
    if (FLAGS_partition == "metis") {
        settings.partition =
            schwz::Settings::partition_settings::partition_metis;
        settings.metis_objtype = FLAGS_metis_objtype;
    } else if (FLAGS_partition == "regular") {
        settings.partition =
            schwz::Settings::partition_settings::partition_regular;
    } else if (FLAGS_partition == "regular2d") {
        settings.partition =
            schwz::Settings::partition_settings::partition_regular2d;
    }
    if (FLAGS_local_solver == "iterative-ginkgo") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::iterative_solver_ginkgo;
    } else if (FLAGS_local_solver == "direct-cholmod") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_cholmod;
    } else if (FLAGS_local_solver == "direct-umfpack") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_umfpack;
    } else if (FLAGS_local_solver == "direct-ginkgo") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_ginkgo;
    }
    settings.debug_print = FLAGS_debug;
    int gsize = 0;
    if (metadata.my_rank == 0) {
        metadata.global_size = system_matrix.m();
        std::cout << " Running on the " << FLAGS_executor << " executor on "
                  << metadata.num_subdomains << " ranks with "
                  << FLAGS_num_threads << " threads" << std::endl;
        std::cout << " Problem Size: " << metadata.global_size
                  << " Number of non-zeros: "
                  << system_matrix.n_nonzero_elements() << std::endl;
        gsize = metadata.global_size;
    }
    MPI_Bcast(&gsize, 1, MPI_INT, 0, MPI_COMM_WORLD);
    metadata.global_size = gsize;
    if (FLAGS_print_config) {
        if (metadata.my_rank == 0) {
            this->print_config();
        }
    }
    using vec_vtype = gko::matrix::Dense<ValueType>;
    std::shared_ptr<vec_vtype> solution_vector;
    schwz::SolverRAS<ValueType, IndexType> solver(settings, metadata);
    solver.initialize(system_matrix, system_rhs);
    auto start_time = std::chrono::steady_clock::now();
    solver.run(solution_vector);
    auto elapsed_time = std::chrono::duration<double>(
        std::chrono::steady_clock::now() - start_time);
    if (metadata.my_rank == 0) {
        std::cout << "Time for solve only: " << elapsed_time.count()
                  << std::endl;
    }
    if (FLAGS_timings_file != "null") {
        std::string rank_string = std::to_string(metadata.my_rank);
        if (metadata.my_rank < 10) {
            rank_string = "0" + std::to_string(metadata.my_rank);
        }
        std::string filename = FLAGS_timings_file + "_" + rank_string + ".csv";
        this->write_timings(metadata.time_struct, filename,
                            settings.comm_settings.enable_onesided);
    }
    if (FLAGS_write_comm_data) {
        std::string rank_string = std::to_string(metadata.my_rank);
        if (metadata.my_rank < 10) {
            rank_string = "0" + std::to_string(metadata.my_rank);
        }
        std::string filename_send = "num_send_" + rank_string + ".csv";
        std::string filename_recv = "num_recv_" + rank_string + ".csv";
        this->write_comm_data(metadata.num_subdomains, metadata.my_rank,
                              metadata.comm_data_struct, filename_send,
                              filename_recv);
    }

    if (metadata.my_rank == 0) {
        std::copy(solution_vector->get_values(),
                  solution_vector->get_values() + metadata.global_size,
                  solution.begin());
        hanging_node_constraints.distribute(solution);
    }
}

template <int dim>
void AdvectionProblem<dim>::solve()
{
    SolverControl solver_control(
        std::max<std::size_t>(1000, system_rhs.size() / 10),
        1e-10 * system_rhs.l2_norm());
    SolverGMRES<> solver(solver_control);
    PreconditionJacobi<> preconditioner;
    preconditioner.initialize(system_matrix, 1.0);
    auto start_time = std::chrono::steady_clock::now();
    solver.solve(system_matrix, solution, system_rhs, preconditioner);
    auto elapsed_time = std::chrono::duration<double>(
        std::chrono::steady_clock::now() - start_time);
    std::cout << "Time for solve only: " << elapsed_time.count() << std::endl;
    Vector<double> residual(dof_handler.n_dofs());
    system_matrix.vmult(residual, solution);
    residual -= system_rhs;
    std::cout << "   Iterations required for convergence: "
              << solver_control.last_step() << '\n'
              << "   Max norm of residual:                "
              << residual.linfty_norm() << '\n';
    hanging_node_constraints.distribute(solution);
}

template <int dim>
void AdvectionProblem<dim>::refine_grid()
{
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());
    GradientEstimation::estimate(dof_handler, solution,
                                 estimated_error_per_cell);
    GridRefinement::refine_and_coarsen_fixed_number(
        triangulation, estimated_error_per_cell, 0.3, 0.03);
    triangulation.execute_coarsening_and_refinement();
}

template <int dim>
void AdvectionProblem<dim>::output_results(const unsigned int cycle) const
{
    {
        GridOut grid_out;
        std::ofstream output("grid-" + std::to_string(cycle) + ".vtu");
        grid_out.write_vtu(triangulation, output);
    }
    {
        DataOut<dim> data_out;
        data_out.attach_dof_handler(dof_handler);
        data_out.add_data_vector(solution, "solution");
        data_out.build_patches(8);
        DataOutBase::VtkFlags vtk_flags;
        vtk_flags.compression_level =
            DataOutBase::VtkFlags::ZlibCompressionLevel::best_speed;
        data_out.set_flags(vtk_flags);
        std::ofstream output("solution-" + std::to_string(cycle) + ".vtu");
        data_out.write_vtu(output);
    }
}

template <int dim>
void AdvectionProblem<dim>::run(MPI_Comm mpi_communicator)
{
    int num_cycles = FLAGS_num_refine_cycles;
    int mpi_size, mpi_rank;
    MPI_Comm_size(MPI_COMM_WORLD, &mpi_size);
    MPI_Comm_rank(MPI_COMM_WORLD, &mpi_rank);
    for (unsigned int cycle = 0; cycle < num_cycles; ++cycle) {
        if (mpi_rank == 0) {
            std::cout << "Cycle " << cycle << ':' << std::endl;

            if (cycle == 0) {
                GridGenerator::hyper_cube(triangulation, -1, 1);
                triangulation.refine_global(FLAGS_init_refine_level);
            } else
                refine_grid();
            std::cout << "   Number of active cells:       "
                      << triangulation.n_active_cells() << std::endl;
            setup_system();
            std::cout << "   Number of degrees of freedom: "
                      << dof_handler.n_dofs() << std::endl;
            assemble_system();
        }
        this->solve(MPI_COMM_WORLD);
        if (mpi_rank == 0) {
            if (FLAGS_vis_sol) {
                output_results(cycle);
            }
        }
    }
}

template <int dim>
void AdvectionProblem<dim>::run()
{
    int num_cycles = FLAGS_num_refine_cycles;
    for (unsigned int cycle = 0; cycle < num_cycles; ++cycle) {
        std::cout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0) {
            GridGenerator::hyper_cube(triangulation, -1, 1);
            triangulation.refine_global(FLAGS_init_refine_level);
        } else
            refine_grid();
        std::cout << "   Number of active cells:       "
                  << triangulation.n_active_cells() << std::endl;
        setup_system();
        std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
                  << std::endl;
        assemble_system();
        this->solve();
        if (FLAGS_vis_sol) {
            output_results(cycle);
        }
    }
}

template <int dim>
GradientEstimation::EstimateScratchData<dim>::EstimateScratchData(
    const FiniteElement<dim> &fe, const Vector<double> &solution,
    Vector<float> &error_per_cell)
    : fe_midpoint_value(fe, QMidpoint<dim>(),
                        update_values | update_quadrature_points),
      solution(solution),
      error_per_cell(error_per_cell),
      cell_midpoint_value(1),
      neighbor_midpoint_value(1)
{
    active_neighbors.reserve(GeometryInfo<dim>::faces_per_cell *
                             GeometryInfo<dim>::max_children_per_face);
}

template <int dim>
GradientEstimation::EstimateScratchData<dim>::EstimateScratchData(
    const EstimateScratchData &scratch_data)
    : fe_midpoint_value(scratch_data.fe_midpoint_value.get_fe(),
                        scratch_data.fe_midpoint_value.get_quadrature(),
                        update_values | update_quadrature_points),
      solution(scratch_data.solution),
      error_per_cell(scratch_data.error_per_cell),
      cell_midpoint_value(1),
      neighbor_midpoint_value(1)
{}

template <int dim>
void GradientEstimation::estimate(const DoFHandler<dim> &dof_handler,
                                  const Vector<double> &solution,
                                  Vector<float> &error_per_cell)
{
    Assert(error_per_cell.size() ==
               dof_handler.get_triangulation().n_active_cells(),
           ExcInvalidVectorLength(
               error_per_cell.size(),
               dof_handler.get_triangulation().n_active_cells()));
    WorkStream::run(dof_handler.begin_active(), dof_handler.end(),
                    &GradientEstimation::template estimate_cell<dim>,
                    std::function<void(const EstimateCopyData &)>(),
                    EstimateScratchData<dim>(dof_handler.get_fe(), solution,
                                             error_per_cell),
                    EstimateCopyData());
}

template <int dim>
void GradientEstimation::estimate_cell(
    const typename DoFHandler<dim>::active_cell_iterator &cell,
    EstimateScratchData<dim> &scratch_data, const EstimateCopyData &)
{
    Tensor<2, dim> Y;
    scratch_data.fe_midpoint_value.reinit(cell);
    scratch_data.active_neighbors.clear();
    for (unsigned int face_n : GeometryInfo<dim>::face_indices())
        if (!cell->at_boundary(face_n)) {
            const auto face = cell->face(face_n);
            const auto neighbor = cell->neighbor(face_n);
            if (neighbor->is_active())
                scratch_data.active_neighbors.push_back(neighbor);
            else {
                if (dim == 1) {
                    auto neighbor_child = neighbor;
                    while (neighbor_child->has_children())
                        neighbor_child =
                            neighbor_child->child(face_n == 0 ? 1 : 0);
                    Assert(
                        neighbor_child->neighbor(face_n == 0 ? 1 : 0) == cell,
                        ExcInternalError());
                    scratch_data.active_neighbors.push_back(neighbor_child);
                } else
                    for (unsigned int subface_n = 0;
                         subface_n < face->n_children(); ++subface_n)
                        scratch_data.active_neighbors.push_back(
                            cell->neighbor_child_on_subface(face_n, subface_n));
            }
        }
    const Point<dim> this_center =
        scratch_data.fe_midpoint_value.quadrature_point(0);
    scratch_data.fe_midpoint_value.get_function_values(
        scratch_data.solution, scratch_data.cell_midpoint_value);
    Tensor<1, dim> projected_gradient;
    for (const auto &neighbor : scratch_data.active_neighbors) {
        scratch_data.fe_midpoint_value.reinit(neighbor);
        const Point<dim> neighbor_center =
            scratch_data.fe_midpoint_value.quadrature_point(0);
        scratch_data.fe_midpoint_value.get_function_values(
            scratch_data.solution, scratch_data.neighbor_midpoint_value);
        Tensor<1, dim> y = neighbor_center - this_center;
        const double distance = y.norm();
        y /= distance;
        for (unsigned int i = 0; i < dim; ++i)
            for (unsigned int j = 0; j < dim; ++j) Y[i][j] += y[i] * y[j];
        projected_gradient += (scratch_data.neighbor_midpoint_value[0] -
                               scratch_data.cell_midpoint_value[0]) /
                              distance * y;
    }
    AssertThrow(determinant(Y) != 0, ExcInsufficientDirections());
    const Tensor<2, dim> Y_inverse = invert(Y);
    const Tensor<1, dim> gradient = Y_inverse * projected_gradient;
    scratch_data.error_per_cell(cell->active_cell_index()) =
        (std::pow(cell->diameter(), 1 + 1.0 * dim / 2) * gradient.norm());
}

int main(int argc, char **argv)
{
    using namespace dealii;
    try {
        initialize_argument_parsing(&argc, &argv);
        MultithreadInfo::set_thread_limit();
        AdvectionProblem<2> advection_problem_2d;
        if (FLAGS_num_threads > 1) {
            int req_thread_support = MPI_THREAD_MULTIPLE;
            int prov_thread_support = MPI_THREAD_MULTIPLE;

            MPI_Init_thread(&argc, &argv, req_thread_support,
                            &prov_thread_support);
            if (prov_thread_support != req_thread_support) {
                std::cout << "Required thread support is " << req_thread_support
                          << " but provided thread support is only "
                          << prov_thread_support << std::endl;
            }
        } else {
            MPI_Init(&argc, &argv);
        }
        int rank = 0;
        MPI_Comm_rank(MPI_COMM_WORLD, &rank);

        if (FLAGS_dealii_orig) {
            if (rank == 0) {
                auto start_time = std::chrono::steady_clock::now();
                advection_problem_2d.run();
                auto elapsed_time = std::chrono::duration<double>(
                    std::chrono::steady_clock::now() - start_time);
                std::cout << "Total Time for setup+solve: "
                          << elapsed_time.count() << std::endl;
            }
        } else {
            auto start_time = std::chrono::steady_clock::now();
            advection_problem_2d.run(MPI_COMM_WORLD);
            auto elapsed_time = std::chrono::duration<double>(
                std::chrono::steady_clock::now() - start_time);
            if (rank == 0) {
                std::cout << "Total Time for setup+solve: "
                          << elapsed_time.count() << std::endl;
            }
        }
        MPI_Finalize();
    } catch (std::exception &exc) {
        std::cerr << std::endl
                  << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        std::cerr << "Exception on processing: " << std::endl
                  << exc.what() << std::endl
                  << "Aborting!" << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        return 1;
    } catch (...) {
        std::cerr << std::endl
                  << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        std::cerr << "Unknown exception!" << std::endl
                  << "Aborting!" << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        return 1;
    }
    return 0;
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int a, b, c; cin >> a >> b >> c;

    cout << (a * b * c * (a + 1) * (b + 1) * (c + 1) / 8) % 998244353 << endl;
}


#include <iostream>
#include <fstream>
#include <Eigen/Eigen>
//include the bie header files
#include "material.hh"
#include "precomputed_kernel.hh"
#include "bimat_interface.hh"
#include "infinite_boundary.hh"
//include the fem header files
#include "mesh_Abaqus_multi_faults.hpp"
#include "bcdof.hpp"
#include "bcdof_ptr.hpp"
#include "cal_ke.hpp"
#include "cal_fe_global_const_ke.hpp"
#include "mapglobal.hpp"
#include "maplocal.hpp"
#include "Slip_Weakening_lumpM.hpp"
#include "cal_slip_sliprate_angle.hpp"
#include "time_advance.hpp"
#include "BIE_correct.hpp"
#include "igl/list_to_matrix.h"
#include <math.h>
using namespace Eigen;
using namespace std;

typedef Eigen::Matrix<int, -1, -1,RowMajor> MatrixXi_rm;

void read_matrix(std::string fileName, Eigen::MatrixXd &outputMat);

double time_fem=0;
double time_bie;
int main() {
    std::string filename = "abaqus_cpp_5degree_100m_10km_4km.inp";
    std::cout<<filename<<std::endl;
    double fault_angle  = 5/180.0*M_PI;
    int dim = 2.0;
    double x_max = 5.0e3;
    double x_min = -5.0e3;
    double dx = 100.0;
    double dy = 100.0;
    std::vector<int> BIE_top, BIE_bot;
    MatrixXd Node;
    MatrixXi_rm Element;
    int num_faults = 1;
    std::vector<std::vector<int>> fault_nodes(2*num_faults);
    mesh_Abaqus_multi_faults(filename, Node, Element, BIE_top, BIE_bot,fault_nodes);
    std::cout<<Node<<std::endl;
    std::cout<<"Node size=" << Node.rows()<<std::endl;
    std::cout<<Element<<std::endl;
    std::cout<<"Element size="<<Element.rows()<<std::endl;
    int nx_BIE = BIE_top.size();
    int n_nodes = Node.rows();
    int n_el = Element.rows();
    int Ndofn = 2;
    int Nnel = Element.cols();
    // Material
    double density = 2670.0;
    double v_s =3.464e3;
    double v_p = 6.0e3;
    double G= pow(v_s,2)*density;
    double Lambda = pow(v_p,2)*density-2.0*G;
    double E  = G*(3.0*Lambda+2.0*G)/(Lambda+G);
    double nu = Lambda/(2.0*(Lambda+G));
    // Time
    double alpha = 0.4;
    double dt = alpha*dx/v_p;
    // Reyleigh Damping
    double beta =0.1;
    double q = beta*dt;
    double time_run = 6.0;
    int numt = time_run/dt;
    //numt = 1;
    VectorXd time = dt*VectorXd::LinSpaced(numt,1,numt);
    // Slip weakening friction parameters
    double Dc = 0.2;
    double mu_d= 0.5;
    double mu_s = 0.6;
    // Intialization
    // disp velocity current and next time step (new)
    VectorXd u_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd u_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd a_n = VectorXd::Zero(n_nodes*Ndofn,1);
    // slip and slip-rate
    std::vector<Eigen::Array<double, -1, 1> > delt_u_n(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > delt_v_n(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > T_c(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > T_0(num_faults);
    std::vector<Eigen::Array<double, -1, 1> > tau_s(num_faults);

    // Vector containing number of elements on each faults (nx)
    std::vector<int> nx_faults(num_faults);
    for (int i=0;i<num_faults;i++)
    {
        nx_faults[i] =fault_nodes[2*i].size();
    }

    for (int i=0;i<num_faults;i++)
    {
        delt_u_n[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        delt_v_n[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        T_c[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        T_0[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
       // tau_s[i] = ArrayXd::Zero(Ndofn*(nx_faults[i]),1);
        tau_s[i] = 0.6*ArrayXd::Ones((nx_faults[i]),1)*50.0e6;
    }
    VectorXd F_ext_global = VectorXd::Zero(n_nodes*Ndofn,1);

    VectorXd x = VectorXd::Zero(nx_faults[0], 1);
    for (int i=0; i<x.size(); i++)
    {
        x(i) = Node(fault_nodes[0][i],0);
    }
    std::ofstream x_output("results/fault_x_coord.txt");
    x_output << x;

    std::cout<<x<<std::endl;
    // Setting intial stress on the fault
    for (int j=0; j<num_faults;j++)
    {
        for (int i=0; i<nx_faults[j]; i++)
        {
            T_0[j](2*i+1) = -50.0e6;
            T_c[j](2*i+1) = T_0[j](2*i+1);
        }
        if (j==0)
        {
            VectorXd x = VectorXd::LinSpaced(nx_faults[j], x_min, x_max);
            for (int i=0 ; i<nx_faults[j]; i++)
            {
                //if ((x(i)<=(fault_pos(j,1)+fault_pos(j,2))/2+0.2e3)&&(x(i)>=(fault_pos(j,1)+fault_pos(j,2))/2-0.2e3))
                if ((x(i)<=(x_min+x_max)/2+0.6e3)&&(x(i)>=(x_min+x_max)/2-0.6e3))
                {
                    T_0[j](2*i) = 31.0e6;
                    T_c[j](2*i) = T_0[j](2*i);
                }
                else
                {
                    T_0[j](2*i) = 27.5e6;
                    T_c[j](2*i) = T_0[j](2*i);
                }
            }
        }
        else
        {
            for (int i=0 ; i<nx_faults[j]; i++)
            {
                T_0[j](2*i) = 27.5e6;
            }
        }
    }
    // Get the index degree of freedome for each element
    VectorXi index_el = VectorXi::Zero(Ndofn*Nnel,1);
    MatrixXi index_store = MatrixXi::Zero(Nnel*Ndofn,n_el);
    for (int i=0;i<n_el;i++)
    {
        bcdof(Element.row(i),dim,index_el);
        index_store.col(i) = index_el;
    }
  //  VectorXi BIE_top_surf_index = VectorXi::Zero(Ndofn*(BIE_top_surf_nodes.size()),1);
   // VectorXi BIE_bot_surf_index = VectorXi::Zero(Ndofn*(BIE_bot_surf_nodes.size()),1);
    VectorXi BIE_top_surf_index = VectorXi::Zero(Ndofn*(BIE_top.size()),1);
    VectorXi BIE_bot_surf_index = VectorXi::Zero(Ndofn*(BIE_bot.size()),1);
    std::vector<Eigen::ArrayXi> Fault_surf_index(num_faults*2);
    for (int i=0; i <num_faults*2; i++)
    {
        Fault_surf_index[i] = ArrayXi::Zero(Ndofn*(fault_nodes[i].size()),1);
    }

    // Getting the faults DOF index
    for (int i=0; i<num_faults*2;i++)
    {
        bcdof_ptr(fault_nodes[i], dim, Fault_surf_index[i].data());
    }

    bcdof_ptr(BIE_top,dim,BIE_top_surf_index.data());
    bcdof_ptr(BIE_bot,dim,BIE_bot_surf_index.data());
    //bcdof(BIE_top_surf_nodes,dim,BIE_top_surf_index);
    //bcdof(BIE_bot_surf_nodes,dim,BIE_bot_surf_index);
    // Calculating the Global Mass Vector (lumped mass)
    // Element mass
    double M=density*dx*dy*1.0;
    VectorXd M_el_vec = M/4*VectorXd::Ones(Nnel*Ndofn,1);
    VectorXd M_global_vec=VectorXd::Zero(n_nodes*Ndofn,1);
    for (int i=0 ; i<n_el;i++)
    {
        index_el = index_store.col(i);
        mapglobal(index_el,M_global_vec,M_el_vec);
    }
    // Element matrix
    MatrixXd ke = MatrixXd::Zero(8,8);
    MatrixXd coord = MatrixXd::Zero(4,2);
    VectorXi Element_0= Element.row(0);
    coord.row(0) = Node.row(Element_0(0));
    coord.row(1) = Node.row(Element_0(1));
    coord.row(2) = Node.row(Element_0(2));
    coord.row(3) = Node.row(Element_0(3));
    cal_ke (coord,E,nu,ke);
    // BIE part initiation
    // Setting up the material property for the BIE code
    Material BIE_top_mat = Material(E,nu,density);
    Material BIE_bot_mat = Material(E,nu,density);
    double length = x_max-x_min;
    // infinte bc BIE call infinite_boundary.cc
    PrecomputedKernel h11("kernels/nu_.25_h11.dat");
    PrecomputedKernel h12("kernels/nu_.25_k12.dat");
    PrecomputedKernel h22("kernels/nu_.25_h22.dat");
    InfiniteBoundary BIE_inf_top(length,nx_BIE,1.0,&BIE_top_mat,&h11,&h12,&h22);
    InfiniteBoundary BIE_inf_bot(length,nx_BIE,-1.0,&BIE_bot_mat,&h11,&h12,&h22);
    // BIE setting time step
    BIE_inf_top.setTimeStep(dt);
    BIE_inf_bot.setTimeStep(dt);
    // BIE initialization
    BIE_inf_top.init();
    BIE_inf_bot.init();
    printf("ready to start\n");
    // Output
    ofstream file;
    file.open("results/num_nodes_fault.bin",ios::binary);
    file.write((char*)(nx_faults.data()),nx_faults.size()*sizeof(int));
    file.close();

    file.open("results/u_n.bin",ios::binary);
   // file.write((char*)(u_n.data()),u_n.size()*sizeof(double));
    file.close();

    for (int i=0;i<num_faults;i++)
    {
        std::string slip = "results/slip_"+std::to_string(i)+".bin";
        file.open(slip);
        file.close();
        std::string slip_rate = "results/slip_rate_"+std::to_string(i)+".bin";
        file.open(slip_rate);
        file.close();
        std::string shear = "results/shear_"+std::to_string(i)+".bin";
        file.open(shear);
        file.close();
    }
    std::ofstream Element_output("results/Element.txt");
    std::ofstream Node_output("results/Node.txt");
    Node_output<<Node;
    Element_output<<Element;
    //    std::ofstream shear_x("results/shear.txt");
    //double start = omp_get_wtime();

    // Main time loop
    for (int j=0;j<numt;j++)
    {
        // Compute the global internal force
        VectorXd fe_global= VectorXd::Zero(n_nodes*Ndofn,1);
        cal_fe_global_const_ke(n_nodes, n_el, index_store, q, u_n, v_n, Ndofn, ke, fe_global);
        // Friction subroutine
        VectorXd F_total = F_ext_global-fe_global;
        for (int i=0 ; i<num_faults; i++)
        {
            VectorXd F_fault = VectorXd::Zero(Ndofn*(nx_faults[i]),1);
            VectorXd M_pos = VectorXd::Zero(nx_faults[i], 1);
            VectorXd M_neg = VectorXd::Zero(nx_faults[i],1);
            maplocal(Fault_surf_index[2*i], M_global_vec, M_pos);
            maplocal(Fault_surf_index[2*i+1], M_global_vec, M_neg);
            //std::cout<<M_pos<<std::endl;
            Slip_Weakening_lumpM(M_global_vec, Fault_surf_index[2*i], Fault_surf_index[2*i+1], fe_global, dt, dx, dy, nx_faults[i]-1, delt_v_n[i], delt_u_n[i], T_0[i], tau_s[i], mu_s, mu_d, Dc, Ndofn, M_pos, M_neg , F_fault, T_c[i], fault_angle);
            mapglobal(Fault_surf_index[2*i],F_total,-F_fault);
            mapglobal(Fault_surf_index[2*i+1],F_total,F_fault);
        }

        // Central Difference Time integration
        time_advance(u_n, v_n, F_total, M_global_vec, dt);
        // Get the slip and slip rate

        for (int i=0; i<num_faults; i++)
        {
            cal_slip_slip_rate_angle(u_n, v_n,  Fault_surf_index[2*i], Fault_surf_index[2*i+1] , Ndofn, nx_faults[i]-1, delt_u_n[i], delt_v_n[i],fault_angle);
        }
        // Correct the BIE surf nodes solutions from FEM with the BIE solution
        BIE_correct(BIE_top_surf_index, BIE_bot_surf_index, fe_global, Ndofn, nx_BIE-1, dx, BIE_inf_top, BIE_inf_bot, u_n, v_n);

      //  if (j%4==3)
       // {
        for (int i=0;i<num_faults;i++)
        {
            std::string slip = "results/slip_"+std::to_string(i)+".bin";
            file.open(slip,ios::binary | ios::app);
            file.write((char*)(delt_u_n[i].data()),delt_u_n[i].size()*sizeof(double));
            file.close();

            std::string slip_rate = "results/slip_rate_"+std::to_string(i)+".bin";
            file.open(slip_rate,ios::binary | ios::app);
            file.write((char*)(delt_v_n[i].data()),delt_v_n[i].size()*sizeof(double));
            file.close();

            std::string shear = "results/shear_"+std::to_string(i)+".bin";
            file.open(shear,ios::binary | ios::app);
            file.write((char*)(T_c[i].data()),T_c[i].size()*sizeof(double));
            file.close();
        }
        file.open("results/u_n.bin",ios::binary | ios::app);
        file.write((char*)(u_n.data()),u_n.size()*sizeof(double));
        file.close();
       // }
        printf("Simulation time = %f\n",time(j));
      //  double end_t = omp_get_wtime();
       // std::cout<<"time_cpu_t="<<end_t-start<<std::endl;
    //   std::cout<<"time_fem="<< time_fem<<std::endl;
    //   std::cout<<"time_bie="<< time_bie<<std::endl;
    }
    //double end = omp_get_wtime();
    //std::cout<<"time_cpu="<<end-start<<std::endl;
    return 0;
}

void read_matrix(std::string fileName, Eigen::MatrixXd &outputMat) {
    fstream cin;
    cin.open(fileName.c_str());
    if (cin.fail())
    {
        std::cerr << "Failed to open file: " << fileName << std::endl;
        std::cin.get(); }
    string s;
    vector <vector <double> > matrix;
    while (getline(cin, s)) {
        stringstream input(s);
        double temp;
        vector <double> currentLine;
        while (input >> temp)
            currentLine.push_back(temp);
        matrix.push_back(currentLine);
    }
    if (!igl::list_to_matrix(matrix, outputMat))
    { std::cerr << "list tom matrix error" << std::endl; std::cin.get();
        //return false;
    }
}


#include "tbb/parallel_for.h"
#include <iostream>
#include <mex.h>
#include <omp.h>
#include <vector>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>
// #include <Eigen/StdVector>

// #include <range/v3/all.hpp>
// #include <range/v3/core.hpp>

// #include "storage_aliases.h"

// #define ngp int(*mxGetPr(prhs[0]))

#define int_space_ptr mxGetPr(prhs[0])
#define delta_ptr mxGetPr(prhs[1])
#define Hdo_raw prhs[2]
#define Hdo_ptr mxGetPr(prhs[2])
#define int_time_ptr mxGetPr(prhs[3])
#define d_alpha_ptr mxGetPr(prhs[4])
#define alpha_ptr mxGetPr(prhs[5])
#define inv_hooke_ptr mxGetPr(prhs[6])
#define gradient_ptr mxGetPr(prhs[7])
#define dof_imposed_raw prhs[8]
#define dof_imposed_ptr mxGetPr(prhs[8])
#define inv_hooke_full_ptr mxGetPr(prhs[9])
#define gradient_width int(*mxGetPr(prhs[10]))

#define output plhs[0]

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  // auto t1 = omp_get_wtime();
  using sparse_matrix = Eigen::SparseMatrix<double, Eigen::RowMajor>;
  using Matrix = Eigen::MatrixXd;
  using Vector = Eigen::VectorXd;
  using matrix6 = Eigen::Matrix<double, 6, 6>;
  using vector6 = Eigen::Matrix<double, 6, 1>;

  auto Hdo_Dimensions = mxGetDimensions(Hdo_raw);
  auto dof_imposed_Dimensions = mxGetDimensions(dof_imposed_raw);
  size_t const ncomp = 6; // TODO 3D only
  size_t const ngp = Hdo_Dimensions[2];
  size_t const ntp = Hdo_Dimensions[3];
  size_t const ndof_imp = dof_imposed_Dimensions[0];
  size_t const tensor_shift = ncomp * ncomp;
  size_t const delta_shift = ncomp;

  Vector alpha = Eigen::Map<Vector>(alpha_ptr, ntp);
  Vector d_alpha = Eigen::Map<Vector>(d_alpha_ptr, ntp);
  Eigen::Map<Matrix> int_time(int_time_ptr, ntp, ntp);
  Eigen::Map<Matrix> gradient(gradient_ptr, ngp * ncomp, gradient_width);
  Eigen::Map<matrix6> inv_hooke(inv_hooke_ptr);
  Eigen::Map<Matrix> inv_hooke_full(inv_hooke_full_ptr, ngp * ncomp,
                                    ngp * ncomp);
  std::vector<std::int32_t> fixed_dofs;
  for (size_t j = 0; j < ndof_imp; j++)
    fixed_dofs.push_back(*(dof_imposed_ptr + j));

  sparse_matrix A(ncomp * ngp, ncomp * ngp);
  sparse_matrix A_noint(ncomp * ngp, ncomp * ngp);
  Vector b(ngp * ncomp);
  Vector Q3_vec(ngp * ncomp);
  Vector space_mode(ngp * ncomp);

  double int_alpha_d_alpha = alpha.transpose() * int_time * d_alpha;
  Vector int_alpha_alpha = int_time * alpha.cwiseProduct(alpha);
  Vector int_alpha = int_time * alpha;
  Vector int_d_alpha = int_time * d_alpha;
  // auto t2 = omp_get_wtime();
  // std::cout << " /* block1 */ " << t2 - t1 << '\n';

  // t1 = omp_get_wtime();
  // #pragma omp parallel for
  for (size_t i = 0; i < ngp; i++) {
    size_t const position = i * ncomp;
    size_t const int_idx = i * tensor_shift;
    matrix6 B = matrix6::Zeros();
    vector6 Q3 = vector6::Zeros();

    // #pragma omp parallel for
    for (size_t j = 0; j < ntp; j++) {
      int const delta_idx = (j * ngp + i) * delta_shift;
      int const tensor_idx = (j * ngp + i) * tensor_shift;
      matrix6 H = Eigen::Map<matrix6>(Hdo_ptr + tensor_idx);
      vector6 delta = Eigen::Map<vector6>(delta_ptr + delta_idx);
      B.noalias() += H * int_alpha_alpha(j);
      Q3 += delta * int_alpha(j);
    }

    matrix6 Int_space = Eigen::Map<matrix6>(int_space_ptr + int_idx, ncomp,
                                            ncomp); // TODO: alligned
    matrix6 Z = (B + int_alpha_d_alpha * inv_hooke).inverse();
    matrix6 int_Z = Int_space * Z;

    // #pragma omp critical
    for (size_t k = 0; k < ncomp; k++) {
      for (size_t l = 0; l < ncomp; l++) {
        A.insert(position + k, position + l) = int_Z(k, l);
        A_noint.insert(position + k, position + l) = Z(k, l);
      }
    }
    b.segment<6>(position) = -int_Z * Q3;
    Q3_vec.segment<6>(position) = Q3;
  }
  // t2 = omp_get_wtime();
  // std::cout << " /* block2 */ " << t2 - t1 << '\n';

  // t1 = omp_get_wtime();
  Matrix K = gradient.transpose() * A * gradient;
  Vector F = gradient.transpose() * b;
  for (auto const fixed_dof : fixed_dofs) {
    auto const diagonal_entry = K.coeff(fixed_dof, fixed_dof); // TODO //
    F(fixed_dof) = 0.0;
    for (int i = 0; i < K.rows(); i++) {
      for (int j = 0; j < K.cols(); j++) {
        K.coeffRef(fixed_dof, j) = 0.0;
        K.coeffRef(i, fixed_dof) = 0.0; // TODO K.row(i)=0;
      }
    }
    K.coeffRef(fixed_dof, fixed_dof) = diagonal_entry;
  }
  // if (b.norm() > 0)
  auto U_tilde = K.llt().solve(F); // TODO: is this ok?
  // else {
  //   U_tilde = F;
  // }
  //   t2 = omp_get_wtime();
  //   std::cout << " /* block3 */ " << t2 - t1 << '\n';

  // t1 = omp_get_wtime();
  auto E_tilde = gradient * U_tilde;
  auto sigma = A_noint * (E_tilde + Q3_vec);
  space_mode = E_tilde / int_alpha_d_alpha - inv_hooke_full * sigma;
  // t2 = omp_get_wtime();
  // std::cout << " /* block4 */ " << t2 - t1 << '\n';

  // t1 = omp_get_wtime();
  output = mxCreateDoubleMatrix(ngp * ncomp, 1, mxREAL);
  Eigen::Map<Matrix>(mxGetPr(output), ngp * ncomp, 1) = space_mode;
  // t2 = omp_get_wtime();
  // std::cout << " /* block5 */ " << t2 - t1 << '\n';

  return;
}

// mxArray *cpp_to_MexArray(const std::vector<double> &v) {
//   mxArray *mx = mxCreateDoubleMatrix(1, v.size(), mxREAL);
//   std::copy(v.begin(), v.end(), mxGetPr(mx));
//
//   return mx;
// }


#include <iostream>
#include <string>
#include <vector>
#include <valarray>

#include <thread>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Dense>

#include <stdio.h>

#include "refine_variational.h"

using std::cout;
using std::endl;
using std::vector;

namespace OFC
{

  VarRefClass::VarRefClass(const float * im_ao_in, const float * im_ao_dx_in, const float * im_ao_dy_in,
                            const float * im_bo_in, const float * im_bo_dx_in, const float * im_bo_dy_in,
                           const camparam* cpt_in,const camparam* cpo_in,const optparam* op_in, float *flowout)
  : cpt(cpt_in), cpo(cpo_in), op(op_in)
{

  // initialize parameters
  tvparams.alpha = op->tv_alpha;
  tvparams.beta = 0.0f;  // for matching term, not needed for us
  tvparams.gamma = op->tv_gamma;
  tvparams.delta = op->tv_delta;
  tvparams.n_inner_iteration = op->tv_innerit * (cpt->curr_lv+1);
  tvparams.n_solver_iteration = op->tv_solverit;//5;
  tvparams.sor_omega = op->tv_sor;

  tvparams.tmp_quarter_alpha = 0.25f*tvparams.alpha;
  tvparams.tmp_half_gamma_over3 = tvparams.gamma*0.5f/3.0f;
  tvparams.tmp_half_delta_over3 = tvparams.delta*0.5f/3.0f;
  tvparams.tmp_half_beta = tvparams.beta*0.5f;

  float deriv_filter[3] = {0.0f, -8.0f/12.0f, 1.0f/12.0f};
  deriv = convolution_new(2, deriv_filter, 0);
  float deriv_filter_flow[2] = {0.0f, -0.5f};
  deriv_flow = convolution_new(1, deriv_filter_flow, 0);

  // copy flow initialization into FV structs
  #if (SELECTMODE==1)
  static int noparam = 2; // Optical flow
  #else
  static int noparam = 1; // Only horizontal displacements for stereo depth
  #endif
  std::vector<image_t*> flow_sep(noparam);

  for (int i = 0; i < noparam; ++i )
    flow_sep[i] = image_new(cpt->width,cpt->height);

  for (int iy = 0; iy < cpt->height; ++iy)
    for (int ix = 0; ix < cpt->width; ++ix)
    {
      int i  = iy * cpt->width          + ix;
      int is = iy * flow_sep[0]->stride + ix;
      for (int j = 0; j < noparam; ++j)
        flow_sep[j]->c1[is] = flowout[i*noparam + j];
    }

  // copy image data into FV structs
  #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
  image_t * im_ao, *im_bo;
  im_ao = image_new(cpt->width,cpt->height);
  im_bo = image_new(cpt->width,cpt->height);
  #else
  color_image_t * im_ao, *im_bo;
  im_ao = color_image_new(cpt->width,cpt->height);
  im_bo = color_image_new(cpt->width,cpt->height);
  #endif

  copyimage(im_ao_in, im_ao);
  copyimage(im_bo_in, im_bo);

  // Call solver
  #if (SELECTMODE==1)
  RefLevelOF(flow_sep[0], flow_sep[1], im_ao, im_bo);
  #else
  RefLevelDE(flow_sep[0], im_ao, im_bo);
  #endif

  // Copy flow result back
  for (int iy = 0; iy < cpt->height; ++iy)
    for (int ix = 0; ix < cpt->width; ++ix)
    {
      int i  = iy * cpt->width          + ix;
      int is = iy * flow_sep[0]->stride + ix;
      for (int j = 0; j < noparam; ++j)
        flowout[i*noparam + j] = flow_sep[j]->c1[is];
    }

  // free FV structs
  for (int i = 0; i < noparam; ++i )
    image_delete(flow_sep[i]);

  convolution_delete(deriv);
  convolution_delete(deriv_flow);

  #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
  image_delete(im_ao);
  image_delete(im_bo);
  #else
  color_image_delete(im_ao);
  color_image_delete(im_bo);
  #endif
}

#if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
void VarRefClass::copyimage(const float* img, image_t * img_t)
#else
void VarRefClass::copyimage(const float* img, color_image_t * img_t)
#endif
{
  #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
  const float * img_st = img +     (cpt->tmp_w + 1 ) * (cpt->imgpadding); // remove image padding, start at first valid pixel
  #else
  const float * img_st = img + 3 * (cpt->tmp_w + 1 ) * (cpt->imgpadding);
  #endif

  for (int yi = 0; yi < cpt->height; ++yi)
  {
    for (int xi = 0; xi < cpt->width; ++xi, ++img_st)
    {
      int i    = yi*img_t->stride+ xi;

      img_t->c1[i] =  (*img_st);
      #if (SELECTCHANNEL==3)
      ++img_st; img_t->c2[i] =  (*img_st);
      ++img_st; img_t->c3[i] =  (*img_st);
      #endif
    }
    #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
    img_st +=     2 * cpt->imgpadding;
    #else
    img_st += 3 * 2 * cpt->imgpadding;
    #endif
  }
}

#if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
void VarRefClass::RefLevelOF(image_t *wx, image_t *wy, const image_t *im1, const image_t *im2)
#else
void VarRefClass::RefLevelOF(image_t *wx, image_t *wy, const color_image_t *im1, const color_image_t *im2)
#endif
{
    int i_inner_iteration;
    int width  = wx->width;
    int height = wx->height;
    int stride = wx->stride;

    image_t *du = image_new(width,height), *dv = image_new(width,height), // the flow increment
      *mask = image_new(width,height), // mask containing 0 if a point goes outside image boundary, 1 otherwise
      *smooth_horiz = image_new(width,height), *smooth_vert = image_new(width,height), // horiz: (i,j) contains the diffusivity coeff. from (i,j) to (i+1,j)
      *uu = image_new(width,height), *vv = image_new(width,height), // flow plus flow increment
      *a11 = image_new(width,height), *a12 = image_new(width,height), *a22 = image_new(width,height), // system matrix A of Ax=b for each pixel
      *b1 = image_new(width,height), *b2 = image_new(width,height); // system matrix b of Ax=b for each pixel

    #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // use single band image
    image_t *w_im2 = image_new(width,height), // warped second image
        *Ix = image_new(width,height), *Iy = image_new(width,height), *Iz = image_new(width,height), // first order derivatives
        *Ixx = image_new(width,height), *Ixy = image_new(width,height), *Iyy = image_new(width,height), *Ixz = image_new(width,height), *Iyz = image_new(width,height); // second order derivatives
    #else                                     // use RGB image
    color_image_t *w_im2 = color_image_new(width,height), // warped second image
        *Ix = color_image_new(width,height), *Iy = color_image_new(width,height), *Iz = color_image_new(width,height), // first order derivatives
        *Ixx = color_image_new(width,height), *Ixy = color_image_new(width,height), *Iyy = color_image_new(width,height), *Ixz = color_image_new(width,height), *Iyz = color_image_new(width,height); // second order derivatives
    #endif

    // warp second image
    image_warp(w_im2, mask, im2, wx, wy);
    // compute derivatives
    get_derivatives(im1, w_im2, deriv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz);
    // erase du and dv
    image_erase(du);
    image_erase(dv);
    // initialize uu and vv
    memcpy(uu->c1,wx->c1,wx->stride*wx->height*sizeof(float));
    memcpy(vv->c1,wy->c1,wy->stride*wy->height*sizeof(float));
    // inner fixed point iterations
    for(i_inner_iteration = 0 ; i_inner_iteration < tvparams.n_inner_iteration ; i_inner_iteration++)
    {
        //  compute robust function and system
        compute_smoothness(smooth_horiz, smooth_vert, uu, vv, deriv_flow, tvparams.tmp_quarter_alpha );
        //compute_data_and_match(a11, a12, a22, b1, b2, mask, wx, wy, du, dv, uu, vv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, desc_weight, desc_flow_x, desc_flow_y, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
        compute_data(a11, a12, a22, b1, b2, mask, wx, wy, du, dv, uu, vv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
        sub_laplacian(b1, wx, smooth_horiz, smooth_vert);
        sub_laplacian(b2, wy, smooth_horiz, smooth_vert);

        // solve system
        #ifdef WITH_OPENMP
        sor_coupled_slow_but_readable(du, dv, a11, a12, a22, b1, b2, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega); // slower but parallelized
        #else
        sor_coupled(du, dv, a11, a12, a22, b1, b2, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega);
        #endif

        // update flow plus flow increment
        int i;
        v4sf *uup = (v4sf*) uu->c1, *vvp = (v4sf*) vv->c1, *wxp = (v4sf*) wx->c1, *wyp = (v4sf*) wy->c1, *dup = (v4sf*) du->c1, *dvp = (v4sf*) dv->c1;
        for( i=0 ; i<height*stride/4 ; i++)
        {
          (*uup) = (*wxp) + (*dup);
          (*vvp) = (*wyp) + (*dvp);
          uup+=1; vvp+=1; wxp+=1; wyp+=1;dup+=1;dvp+=1;
        }

    }
    // add flow increment to current flow
    memcpy(wx->c1,uu->c1,uu->stride*uu->height*sizeof(float));
    memcpy(wy->c1,vv->c1,vv->stride*vv->height*sizeof(float));

    // free memory
    image_delete(du); image_delete(dv);
    image_delete(mask);
    image_delete(smooth_horiz); image_delete(smooth_vert);
    image_delete(uu); image_delete(vv);
    image_delete(a11); image_delete(a12); image_delete(a22);
    image_delete(b1); image_delete(b2);

    #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // use single band image
    image_delete(w_im2);
    image_delete(Ix); image_delete(Iy); image_delete(Iz);
    image_delete(Ixx); image_delete(Ixy); image_delete(Iyy); image_delete(Ixz); image_delete(Iyz);
    #else
    color_image_delete(w_im2);
    color_image_delete(Ix); color_image_delete(Iy); color_image_delete(Iz);
    color_image_delete(Ixx); color_image_delete(Ixy); color_image_delete(Iyy); color_image_delete(Ixz); color_image_delete(Iyz);
    #endif

}

#if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
void VarRefClass::RefLevelDE(image_t *wx, const image_t *im1, const image_t *im2)
#else
void VarRefClass::RefLevelDE(image_t *wx, const color_image_t *im1, const color_image_t *im2)
#endif
{
    int i_inner_iteration;
    int width  = wx->width;
    int height = wx->height;
    int stride = wx->stride;

      image_t *du = image_new(width,height), *wy_dummy = image_new(width,height), // the flow increment
        *mask = image_new(width,height), // mask containing 0 if a point goes outside image boundary, 1 otherwise
        *smooth_horiz = image_new(width,height), *smooth_vert = image_new(width,height), // horiz: (i,j) contains the diffusivity coeff. from (i,j) to (i+1,j)
        *uu = image_new(width,height), // flow plus flow increment
        *a11 = image_new(width,height), // system matrix A of Ax=b for each pixel
        *b1 = image_new(width,height); // system matrix b of Ax=b for each pixel

      image_erase(wy_dummy);

      #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // use single band image
      image_t *w_im2 = image_new(width,height), // warped second image
          *Ix = image_new(width,height), *Iy = image_new(width,height), *Iz = image_new(width,height), // first order derivatives
          *Ixx = image_new(width,height), *Ixy = image_new(width,height), *Iyy = image_new(width,height), *Ixz = image_new(width,height), *Iyz = image_new(width,height); // second order derivatives
      #else                                     // use RGB image
      color_image_t *w_im2 = color_image_new(width,height), // warped second image
          *Ix = color_image_new(width,height), *Iy = color_image_new(width,height), *Iz = color_image_new(width,height), // first order derivatives
          *Ixx = color_image_new(width,height), *Ixy = color_image_new(width,height), *Iyy = color_image_new(width,height), *Ixz = color_image_new(width,height), *Iyz = color_image_new(width,height); // second order derivatives
      #endif

      // warp second image
      image_warp(w_im2, mask, im2, wx, wy_dummy);
      // compute derivatives
      get_derivatives(im1, w_im2, deriv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz);
      // erase du and dv
      image_erase(du);

      // initialize uu and vv
      memcpy(uu->c1,wx->c1,wx->stride*wx->height*sizeof(float));

      // inner fixed point iterations
      for(i_inner_iteration = 0 ; i_inner_iteration < tvparams.n_inner_iteration ; i_inner_iteration++)
      {
          //  compute robust function and system
          compute_smoothness(smooth_horiz, smooth_vert, uu, wy_dummy, deriv_flow, tvparams.tmp_quarter_alpha );
          compute_data_DE(a11, b1, mask, wx, du, uu, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
          sub_laplacian(b1, wx, smooth_horiz, smooth_vert);

          // solve system
          sor_coupled_slow_but_readable_DE(du, a11, b1, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega);

          // update flow plus flow increment
          int i;
          v4sf *uup = (v4sf*) uu->c1, *wxp = (v4sf*) wx->c1, *dup = (v4sf*) du->c1;

          if(cpt->camlr==0)  // check if right or left camera, needed to truncate values above/below zero
          {
            for( i=0 ; i<height*stride/4 ; i++)
            {
                (*uup) = __builtin_ia32_minps(   (*wxp) + (*dup)   ,  op->zero);
                uup+=1; wxp+=1; dup+=1;
            }
          }
          else
          {
            for( i=0 ; i<height*stride/4 ; i++)
            {
                (*uup) = __builtin_ia32_maxps(   (*wxp) + (*dup)   ,  op->zero);
                uup+=1; wxp+=1; dup+=1;
            }
          }
      }
      // add flow increment to current flow
      memcpy(wx->c1,uu->c1,uu->stride*uu->height*sizeof(float));

      // free memory
      image_delete(du); image_delete(wy_dummy);
      image_delete(mask);
      image_delete(smooth_horiz); image_delete(smooth_vert);
      image_delete(uu);
      image_delete(a11);
      image_delete(b1);

      #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)
      image_delete(w_im2);
      image_delete(Ix); image_delete(Iy); image_delete(Iz);
      image_delete(Ixx); image_delete(Ixy); image_delete(Iyy); image_delete(Ixz); image_delete(Iyz);
      #else
      color_image_delete(w_im2);
      color_image_delete(Ix); color_image_delete(Iy); color_image_delete(Iz);
      color_image_delete(Ixx); color_image_delete(Ixy); color_image_delete(Iyy); color_image_delete(Ixz); color_image_delete(Iyz);
      #endif
}

VarRefClass::~VarRefClass()
{

}

}


#pragma once
#include "bank/interfaces/DataSource/IDataObjectMetaInfo.h"
#include "bank/DataSources//VirtualObjectMetaInfo.h"
#include "constants.h"
#include <boost/date_time/gregorian/gregorian.hpp>
#include <string>
#include <vector>
#include <ctime>

#define AllowCreating virtual void makeVirtual() override {}

namespace DatabaseObjects {
    using namespace boost::gregorian;
    using namespace constants;

    using id = std::size_t;

    struct DatabaseObject {
        id object_id;
        interfaces::DataSource::IDataObjectMetaInfo* meta_info;

        virtual void makeVirtual() = 0;
    };

    struct Client: DatabaseObject {
        std::string name;
        std::string surname;
        std::string address;
        std::string passport;

        AllowCreating;
    };

    struct BankAccount: DatabaseObject {
        id client_id;
        date opened_date;
        double money;

        virtual BankAccount* lookFuture(date futureMoment, date_duration periodInterval) = 0;

    protected:
        struct PaymentsResult {
            date closest_payment;
            date latest_payment;
            size_t num_payments;
        };

        PaymentsResult getPaymentsBound(date today, date future_moment, date_duration period_interval) {
            size_t num_payments = 0;

            auto it = day_iterator(opened_date, period_interval.days());
            while (today > *it) {
                ++it;
            }
            date closest_payment = *it;
            while (future_moment >= *it) {
                ++it;
                num_payments += 1;
            }
            date last_payment = *(--it);
            return {closest_payment, last_payment, num_payments};
        }
    };

    struct DebitAccount: BankAccount {
        double interest_rate;
        double accumulated;

        virtual BankAccount* lookFuture(date future_moment, date_duration period_interval) override {
            DebitAccount* object = new DebitAccount();
            object->meta_info = new VirtualDataObjectMetaInfo();
            object->opened_date = opened_date;
            object->client_id = client_id;
            object->interest_rate = interest_rate;

            date today = day_clock::local_day();
            auto res = getPaymentsBound(today, future_moment, period_interval);

            double future_money = accumulated + money * (1 + interest_rate / daysPerYear * (res.closest_payment - today).days());
            future_money *= pow(1 + interest_rate / daysPerYear * period_interval.days(), res.num_payments - 1);
            object->money = future_money;

            return object;
        }

        AllowCreating;
    };

    struct DepositAccount: BankAccount {
        double interest_rate;
        double accumulated;
        date end_date;

        virtual BankAccount* lookFuture(date future_moment, date_duration period_interval) override {
            DepositAccount* object = new DepositAccount();
            object->meta_info = new VirtualDataObjectMetaInfo();
            object->opened_date = opened_date;
            object->client_id = client_id;
            object->interest_rate = interest_rate;

            date today = day_clock::local_day();
            future_moment = std::min(future_moment, end_date);
            auto res = getPaymentsBound(today, future_moment, period_interval);

            double future_money = accumulated + money * (1 + interest_rate / daysPerYear * (res.closest_payment - today).days());
            future_money *= pow(1 + interest_rate / daysPerYear * period_interval.days(), res.num_payments - 1);
            object->money = future_money;

            return object;
        }

        AllowCreating;
    };

    struct CreditAccount: BankAccount {
        double credit_limit;
        double commission;

        virtual BankAccount* lookFuture(date future_moment, date_duration period_interval) override {
            CreditAccount* object = new CreditAccount();
            object->meta_info = new VirtualDataObjectMetaInfo();
            object->opened_date = opened_date;
            object->client_id = client_id;
            object->commission = commission;
            object->credit_limit = credit_limit;
            object->money = money;
            return object;
        }

        AllowCreating;
    };

    struct Transaction: DatabaseObject {
        time_t time;
        double money;

        virtual std::vector<id> getAssociatedAccounts() = 0;
    };

    struct PutTransaction: Transaction {
        id bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };

    struct SendTransaction: Transaction {
        id from_bank_account_id;
        id to_bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {from_bank_account_id, to_bank_account_id};
        }

        AllowCreating;
    };

    struct GetTransaction: Transaction {
        id bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };

    struct DebitPercentsTransaction: Transaction {
        date begin_period;
        date end_period;
        id bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };

    struct DepositEndTransaction: Transaction {
        id bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };

    struct CreditPercentsTransaction: Transaction {
        id bank_account_id;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };

    struct CancelTransaction: Transaction {
        id transaction_id;
        id bank_account_id;
        std::string reason;

        virtual std::vector<id> getAssociatedAccounts() override {
            return {bank_account_id};
        }

        AllowCreating;
    };
}


//#include "pipeline.hpp"
#include <boost/filesystem.hpp>
#include <cstdlib>
#include <iostream>
#include <string>
#include <iterator>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/corefinement.h>
#include <fstream>

#include <CGAL/Mesh_triangulation_3.h>
#include <CGAL/Mesh_complex_3_in_triangulation_3.h>
#include <CGAL/Mesh_criteria_3.h>
#include <CGAL/Polyhedral_complex_mesh_domain_3.h>
#include <CGAL/make_mesh_3.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K; /*inexact*/
typedef CGAL::Exact_predicates_exact_constructions_kernel EK;
typedef CGAL::Surface_mesh<K::Point_3> Mesh;
typedef CGAL::Surface_mesh<EK::Point_3> Mesh_exact;

using namespace std;

//#include <CGAL/MP_Float.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/Polygon_mesh_processing/remesh.h>
#include <CGAL/Polygon_mesh_processing/border.h>

typedef K::Compare_dihedral_angle_3                    Compare_dihedral_angle_3;

typedef boost::graph_traits<Mesh>::halfedge_descriptor halfedge_descriptor;
typedef boost::graph_traits<Mesh>::edge_descriptor     edge_descriptor;
typedef boost::graph_traits<Mesh>::face_descriptor     face_descriptor;

//typedef CGAL::Quotient<CGAL::MP_Float> mp_number;

template <typename G>
struct Constraint : public boost::put_get_helper<bool,Constraint<G> >
{
  typedef typename boost::graph_traits<G>::edge_descriptor edge_descriptor;
  typedef boost::readable_property_map_tag      category;
  typedef bool                                  value_type;
  typedef bool                                  reference;
  typedef edge_descriptor                       key_type;

  Constraint()
    :g_(NULL)
  {}

  Constraint(G& g, double bound)
    : g_(&g), bound_(bound)
  {}

  bool operator[](edge_descriptor e) const
  {
    const G& g = *g_;
    return compare_(g.point(source(e, g)),
                    g.point(target(e, g)),
                    g.point(target(next(halfedge(e, g), g), g)),
                    g.point(target(next(opposite(halfedge(e, g), g), g), g)),
                   bound_) == CGAL::SMALLER;
  }

  const G* g_;
  Compare_dihedral_angle_3 compare_;
  double bound_;
};

struct halfedge2edge
{
  halfedge2edge(const Mesh& m, vector<edge_descriptor>& edges)
    : m_mesh(m), m_edges(edges)
  {}
  void operator()(const halfedge_descriptor& h) const
  {
    m_edges.push_back(edge(h, m_mesh));
  }
  const Mesh& m_mesh;
  vector<edge_descriptor>& m_edges;
};

using namespace CGAL ;

using namespace Polygon_mesh_processing;

template <typename PolygonMesh
        , typename NamedParameters>
size_t make_vector_of_connected_components(
    PolygonMesh& pmesh,
    pair<int, int> pair_,
    vector<PolygonMesh> &mesh_vec,
    vector<pair<int, int> >  &pair_vec,
    const NamedParameters& np
)
{
  typedef PolygonMesh PM;
  typedef typename boost::graph_traits<PM>::face_descriptor face_descriptor;
  using boost::choose_param;
  using boost::get_param;
                                                                            //FaceIndexMap
  typedef typename GetFaceIndexMap<PM,  NamedParameters>::type FaceIndexMap;
  FaceIndexMap fimap = choose_param(get_param(np, internal_np::face_index),
                                    get_property_map(boost::face_index, pmesh));
                                                                            //vector_property_map
  boost::vector_property_map<size_t, FaceIndexMap> face_cc(fimap);
  size_t num = connected_components(pmesh, face_cc, np);
  vector< pair<size_t, size_t> > component_size(num);

  for(size_t i=0; i < num; i++)
    component_size[i] = make_pair(i,0);

  BOOST_FOREACH(face_descriptor f, faces(pmesh))
    ++component_size[face_cc[f]].second;
                                                // we sort the range [0, num) by component size
  sort(component_size.begin(), component_size.end(), PMP::internal::MoreSecond());
  vector<size_t> cc_to_keep;

  for(size_t i=0; i<num; ++i) {
      PM tmp_mesh = pmesh;
      cc_to_keep.clear();
      cc_to_keep.push_back( component_size[i].first );
      keep_connected_components(tmp_mesh, cc_to_keep, face_cc, np);
      mesh_vec.push_back(tmp_mesh);
      pair_vec.push_back(pair_);
  }
  int indx=0;                                   // dbg verify written to mesh_vec
  BOOST_FOREACH(PM cc_mesh , mesh_vec){
      string filename =  "data/blobby_vec";
      filename +=  to_string(indx) + ".off";
      ofstream outfile(filename);
      outfile << cc_mesh;
      outfile.close();
      indx++;
  }
  return num;
}

int mkvec_cc(
        vector<Mesh_exact> &patch_vec,
        vector<pair<int, int> >  &pair_vec,
        std::string out_dir
            )
{
    boost::filesystem::path p (out_dir.c_str());
    vector<Mesh_exact> new_patch_vec;
    vector<Mesh_exact> new_patch_vec2;
    vector<pair<int, int> >  new_pair_vec;
    const double bound = cos(0.75 * CGAL_PI);

    int idx =0;
    BOOST_FOREACH(Mesh_exact pmesh, patch_vec){
        make_vector_of_connected_components(pmesh, pair_vec.at(idx), new_patch_vec, new_pair_vec,
                PMP::parameters::edge_is_constrained_map(Constraint<Mesh_exact>(pmesh, bound)) );
        idx++;    // no match for epick  epeck
    }

    for(int i=0; i< patch_vec.size() ;i++){
        std::string filename = p.c_str();
        filename += "/patch_num_" + to_string(i) + ".off";
        std::ofstream output(filename);
        output << patch_vec.at(i);
        output.close();
    }

    patch_vec.swap(new_patch_vec); /*new_patch_vec2*/
    pair_vec.swap(new_pair_vec);
    return patch_vec.size();
}

// int iso_remesh(Mesh &nmesh, unsigned int nb_iter, double target_edge_length){
//     vector<edge_descriptor> border;
//     PMP::border_halfedges(
//                 faces(nmesh),
//                 nmesh,
//                 boost::make_function_output_iterator(halfedge2edge(nmesh, border))
//                 );
//     PMP::split_long_edges(border, target_edge_length, nmesh);
//
//     PMP::isotropic_remeshing(
//                 faces(nmesh),
//                 target_edge_length,
//                 nmesh,                                    //protect border, here
//                 PMP::parameters::number_of_iterations(nb_iter).protect_constraints(true)
//                 );
// }


/******************************************************************************
 * Copyright 2017 Baidu Robotic Vision Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "feature_utils.h"
#include <Eigen/Dense>

namespace XP {
namespace warp {

using Eigen::Vector2f;
using Eigen::Vector3f;
using Eigen::Matrix2f;
using Eigen::Matrix3f;

// Compute affine warp matrix A_ref_cur
// The warping matrix is warping the ref patch (at level_ref) to the current frame (at pyr 0)
//输入左右相机类,左相机像素坐标,归一化坐标,估计的深度,特征点所在的金字塔层,外参
bool getWarpMatrixAffine(const vio::cameras::CameraBase& cam_ref,
                         const vio::cameras::CameraBase& cam_cur,
                         const Vector2f& px_ref,  // distorted pixel at pyr0
                         const Vector3f& f_ref,   // undist ray in unit plane
                         const float depth_ref,
                         const Matrix3f& R_cur_ref,
                         const Vector3f& t_cur_ref,
                         const int level_ref,
                         Eigen::Matrix2f* A_cur_ref) {
  CHECK_NOTNULL(A_cur_ref);
  // TODO(mingyu): tune the *d_unit* size in pixel for different 1st order approximation
  const int halfpatch_size = 5;
  const Vector3f xyz_ref(f_ref * depth_ref);// 特征点在左相机坐标中的位置
  //这个是一半块的大小,在不同的金子塔层patch的大小也是需要缩放的
  float d_unit = halfpatch_size * (1 << level_ref);
  //这里在算以px_ref为原点,uv的方向
  Vector2f du_ref(px_ref + Vector2f(d_unit, 0));
  Vector2f dv_ref(px_ref + Vector2f(0, d_unit));
  Vector3f xyz_du_ref, xyz_dv_ref;
  //反投影
  if (cam_ref.backProject(du_ref, &xyz_du_ref) && cam_ref.backProject(dv_ref, &xyz_dv_ref))
  {
    // Make sure the back project succeed for both du_ref & dv_ref
    //初始深度
    xyz_du_ref *= xyz_ref[2] / xyz_du_ref[2];
    xyz_dv_ref *= xyz_ref[2] / xyz_dv_ref[2];
    Vector2f px_cur, du_cur, dv_cur;
      // 利用外参把这三点变换到右相机坐标系下
    if (vio::cameras::CameraBase::ProjectionStatus::Successful ==
        cam_cur.project(R_cur_ref * xyz_ref + t_cur_ref, &px_cur) &&
        vio::cameras::CameraBase::ProjectionStatus::Successful ==
            cam_cur.project(R_cur_ref * xyz_du_ref + t_cur_ref, &du_cur) &&
        vio::cameras::CameraBase::ProjectionStatus::Successful ==
            cam_cur.project(R_cur_ref * xyz_dv_ref + t_cur_ref, &dv_cur)) {
        //如果都投影成功的话,计算仿射变换(每列就是某轴变换以后的方向)
      A_cur_ref->col(0) = (du_cur - px_cur) / halfpatch_size;
      A_cur_ref->col(1) = (dv_cur - px_cur) / halfpatch_size;
      return true;
    }
  }
  A_cur_ref->setIdentity();  // No warping
  return false;
}
// 找到合适金字塔层
// Compute patch level in other image (based on pyramid level 0)
int getBestSearchLevel(const Eigen::Matrix2f& A_cur_ref,
                       const int max_level) {
  int search_level = 0;
  float D = A_cur_ref.determinant();
  //行列式小于3为止
  while (D > 3.f && search_level < max_level) {
    ++search_level;
    D *= 0.25;
  }
  return search_level;
}

namespace {
// Return value between 0 and 255
// [NOTE] Does not check whether the x/y is within the border
inline float interpolateMat_8u(const cv::Mat& mat, float u, float v) {
  CHECK_EQ(mat.type(), CV_8U);
  int x = floor(u);
  int y = floor(v);
  float subpix_x = u - x;
  float subpix_y = v - y;

  float w00 = (1.0f - subpix_x) * (1.0f - subpix_y);
  float w01 = (1.0f - subpix_x) * subpix_y;
  float w10 = subpix_x * (1.0f - subpix_y);
  float w11 = 1.0f - w00 - w01 - w10;

  const int stride = mat.step.p[0];
  uint8_t* ptr = mat.data + y * stride + x;
  return w00 * ptr[0] + w01 * ptr[stride] + w10 * ptr[1] + w11 * ptr[stride+1];
}
}  // namespace

//将左相机图像特征点中心的图像块warp到右相机图像坐标系中
// Compute acc squared patch that is *warperd* from img_ref with A_cur_ref.
//输入之前得到的粗略的仿射矩阵,左相机特征点所在所在金字塔图像,左特征点像素坐标,左特征的金字塔层，右相机需要搜索的金字塔层,
bool warpAffine(const Eigen::Matrix2f& A_cur_ref,
                const cv::Mat& img_ref,         // at pyramid level_ref
                const Eigen::Vector2f& px_ref,  // at pyramid 0
                const int level_ref,
                const int level_cur,
                const int halfpatch_size,
                uint8_t* patch) {
  const int patch_size = halfpatch_size * 2;
  const Matrix2f A_ref_cur = A_cur_ref.inverse();
  if (std::isnan(A_ref_cur(0, 0))) {
    // TODO(mingyu): Use looser criteria for invalid affine warp?
    //               I suspect A_ref_cur can barely hit NaN.
    LOG(ERROR) << "Invalid affine warp matrix (NaN)";
    return false;
  }

  // px_ref is at pyr0, img_ref is at level_ref pyr already
  CHECK_NOTNULL(patch);
  uint8_t* patch_ptr = patch;
  const Vector2f px_ref_pyr = px_ref / (1<< level_ref);  // pixel at pyramid level_ref//变换到对应的金字塔层坐标上
  for (int y = 0; y < patch_size; ++y)
  {
    for (int x = 0; x < patch_size; ++x, ++patch_ptr)// // 以建立patch坐标系
    {
      Vector2f px_patch(x - halfpatch_size, y - halfpatch_size);
      px_patch *= (1 << level_cur);//缩放
      const Vector2f px(A_ref_cur * px_patch + px_ref_pyr);  // pixel at pyramid level_ref
      if (px[0] < 0 || px[1] < 0 || px[0] >= img_ref.cols - 1 || px[1] >= img_ref.rows - 1) {
        *patch_ptr = 0;
      } else {
        *patch_ptr = interpolateMat_8u(img_ref, px[0], px[1]);//将左相机图像warp到右相机图像坐标系中
      }
    }
  }
  return true;
}

}  // namespace warp
}  // namespace XP


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

#include <Eigen/Dense>
#include <vector>
#include <algorithm>
#include "get_valtonenornhag_arxiv_2021.hpp"
#include "normalize2dpts.hpp"
#include "radial.hpp"
#include "relpose.hpp"
#include "solver_frEfr.hpp"

namespace DronePoseLib {
namespace ValtonenOrnhagArxiv2021 {
    inline Eigen::Vector3d extract_translation(
        const double f,
        const double r,
        const Eigen::Matrix3d &R1,
        const Eigen::Matrix3d &R2,
        const Eigen::Matrix2d &x1,
        const Eigen::Matrix2d &x2);

    std::vector<RelPose> get_frEfr(
        const Eigen::MatrixXd &p1,
        const Eigen::MatrixXd &p2,
        const Eigen::Matrix3d &R1,
        const Eigen::Matrix3d &R2,
        const bool use_fast_solver
    ) {
        // This is a 4-point method
        const int nbr_pts = 4;

        // We expect inhomogenous input data, i.e. p1 and p2 are 2x3 matrices
        assert(p1.rows() == 2);
        assert(p2.rows() == 2);
        assert(p1.cols() == nbr_pts);
        assert(p2.cols() == nbr_pts);

        // Compute normalization matrix
        double scale1 = normalize2dpts(p1);
        double scale2 = normalize2dpts(p2);
        double scale = std::max(scale1, scale2);
        Eigen::Vector3d s;
        s << scale, scale, 1.0;
        Eigen::DiagonalMatrix<double, 3> S = s.asDiagonal();

        // Normalize data
        Eigen::Matrix<double, 3, nbr_pts> x1;
        Eigen::Matrix<double, 3, nbr_pts> x2;
        x1 = p1.colwise().homogeneous();
        x2 = p2.colwise().homogeneous();
        x1 = S * x1;
        x2 = S * x2;

        Eigen::Matrix<double, 2, nbr_pts> x1t;
        Eigen::Matrix<double, 2, nbr_pts> x2t;
        x1t << x1.colwise().hnormalized();
        x2t << x2.colwise().hnormalized();

        // Compute relative rotation
        Eigen::Matrix3d R = R2 * R1.transpose();

        // Wrap input data to expected format
        Eigen::VectorXd input(25);
        input << Eigen::Map<Eigen::VectorXd>(x1t.data(), 8),
                 Eigen::Map<Eigen::VectorXd>(x2t.data(), 8),
                 Eigen::Map<Eigen::VectorXd>(R.data(), 9);

        // Extract solution
        Eigen::MatrixXcd sols = DronePoseLib::ValtonenOrnhagArxiv2021::solver_frEfr(input, use_fast_solver);

        // Pre-processing: Remove complex-valued solutions
        double thresh = 1e-12;
        Eigen::ArrayXd real_sols(11);
        real_sols = sols.imag().cwiseAbs().colwise().sum();
        int nbr_real_sols = (real_sols <= thresh).count();

        // Construct putative output
        // Eigen::Vector3d t;
        std::vector<RelPose> output;
        RelPose relpose;
        double f, r;
        Eigen::Vector3d kinv;
        Eigen::DiagonalMatrix<double, 3> Kinv;
        Eigen::Matrix3d skew_t;

        // Loop over real solutions
        for (int i=0; i < real_sols.size(); i++) {
            if (real_sols(i) < thresh) {
                f = sols(0, i).real();
                r = sols(1, i).real();

                // Extract translation
                relpose.t = extract_translation(f, r, R1, R2, x1t.leftCols<2>(), x2t.leftCols<2>());
                relpose.f = f / scale;
                relpose.r = r * std::pow(scale, 2);

                // Compute fundamental matrix
                kinv << 1.0 / relpose.f, 1.0 / relpose.f, 1.0;
                Kinv = kinv.asDiagonal();
                skew_t << 0, -relpose.t(2), relpose.t(1),
                          relpose.t(2), 0, -relpose.t(0),
                         -relpose.t(1), relpose.t(0), 0;
                relpose.F = Kinv * skew_t * R * Kinv;

                // Add
                output.push_back(relpose);
            }
        }

        return output;
    }

    inline Eigen::Vector3d extract_translation(
        const double f,
        const double r,
        const Eigen::Matrix3d &R1,
        const Eigen::Matrix3d &R2,
        const Eigen::Matrix2d &x1,
        const Eigen::Matrix2d &x2
    ) {
        Eigen::Vector3d fmat;
        fmat << 1.0 / f, 1.0 / f, 1.0;
        Eigen::DiagonalMatrix<double, 3> Kinv = fmat.asDiagonal();

        // Transform points
        Eigen::Matrix<double, 3, 2> y1, y2;
        y1 = R1.transpose() * Kinv * DronePoseLib::radialundistort(x1, r).colwise().homogeneous();
        y2 = R2.transpose() * Kinv * DronePoseLib::radialundistort(x2, r).colwise().homogeneous();

        // Extract translation
        Eigen::Vector3d t;
        t = R2 * y1.col(0).cross(y2.col(0)).cross(y1.col(1).cross(y2.col(1)));
        return t;
    }
}  // namespace ValtonenOrnhagArxiv2021
}  // namespace DronePoseLib


#include "Surfelizer.hpp"

#include "Utils.hpp"

#include <boost/circular_buffer.hpp>

using namespace maps;

struct Surfelizer::Helper {
  struct ScanData {
    std::vector<Eigen::Vector3f> mPoints;
    std::vector<float> mRanges;
    std::vector<bool> mValid;
    Eigen::Isometry3f mPose;
    float mIntraScanAngle;
  };

  int mScanRadius;
  int mPointRadius;
  SizeMethod mSizeMethod;
  float mNominalSize;
  int mDecimation;
  boost::circular_buffer<ScanData> mDataBuffer;

  std::vector<Surfel> addScan(const maps::PointSet& iScan) {
    // cache some basic information
    ScanData data;
    data.mPose = Utils::getPose(*iScan.mCloud);
    data.mRanges.resize(iScan.mCloud->size());
    data.mValid.resize(iScan.mCloud->size());
    data.mPoints.resize(iScan.mCloud->size());
    data.mIntraScanAngle = 0;
    int counter = 0;
    for (int i = 0; i < iScan.mCloud->size(); ++i) {
      data.mPoints[i] = data.mPose*(*(iScan.mCloud))[i].getVector3fMap();
      Eigen::Vector3f ray = data.mPoints[i] - data.mPose.translation();
      data.mRanges[i] = ray.norm();
      data.mValid[i] = (data.mRanges[i] >= iScan.mMinRange) &&
        (data.mRanges[i] <= iScan.mMaxRange);
      if ((i>0) && data.mValid[i] && data.mValid[i-1]) {
        Eigen::Vector3f ray2 = data.mPoints[i-1] - data.mPose.translation();
        data.mIntraScanAngle += acos(ray.normalized().dot(ray2.normalized()));
        ++counter;
      }
    }
    data.mIntraScanAngle /= counter;
    mDataBuffer.push_back(data);

    // check to see whether we have enough scans in the buffer to proceed
    std::vector<Surfel> surfels;
    int r1 = mScanRadius;
    int r2 = mPointRadius;
    int w1 = 2*r1+1;
    int w2 = 2*r2+1;
    if (mDataBuffer.size() < w1) return surfels;

    const ScanData& curScan = mDataBuffer[r1];
    surfels.reserve(curScan.mPoints.size());
    std::vector<Eigen::Vector3f> points;
    points.reserve(w1*w2);

    float interScanAngle =
      acos(mDataBuffer[r1-1].mPose.linear().col(2).dot
           (mDataBuffer[r1+1].mPose.linear().col(2))) / 2;

    // loop over scan points
    int numPoints = curScan.mPoints.size();
    for (int i = r2; i < numPoints - r2; i += mDecimation) {
      if (!curScan.mValid[i]) continue;

      // initiate surfel
      Surfel surfel;
      surfel.mCenter = curScan.mPoints[i];

      // approximate surfel orientation
      points.clear();
      for (int j = 0; j < w1; ++j) {
        int kMax = std::min(i+r2, (int)mDataBuffer[j].mValid.size()-1);
        for (int k = i-r2; k <= kMax; ++k) {
          if (mDataBuffer[j].mValid[k]) {
            points.push_back(mDataBuffer[j].mPoints[k]);
          }
        }
      }
      if (points.size() < 3) continue;
      surfel.mOrientation = estimateOrientation(points,curScan.mPose.linear());

      // approximate surfel size
      surfel.mSize = Eigen::Vector2f(mNominalSize, mNominalSize);
      if (mSizeMethod == SizeMethodRange) {
        surfel.mSize *= curScan.mRanges[i];
      }
      else if (mSizeMethod == SizeMethodAngles) {
        float range = curScan.mRanges[i];
        Eigen::Vector3f xPt = curScan.mPose.translation() +
          curScan.mPose.linear().col(0)*range;
        float dist = (xPt-surfel.mCenter).norm();
        surfel.mSize = Eigen::Vector2f(interScanAngle*dist,
                                       curScan.mIntraScanAngle*range);
      }
      else if (mSizeMethod == SizeMethodNeighbors) {
        if (!mDataBuffer[r1+1].mValid[i] || !mDataBuffer[r1-1].mValid[i] ||
            !mDataBuffer[r1].mValid[i-1] || !mDataBuffer[r1].mValid[i+1]) {
          continue;
        }
        float d1 = (mDataBuffer[r1+1].mPoints[i] -
                    mDataBuffer[r1-1].mPoints[i]).norm();
        float d2 = (mDataBuffer[r1].mPoints[i-1] -
                    mDataBuffer[r1].mPoints[i+1]).norm();
        surfel.mSize = Eigen::Vector2f(d1/2,d2/2);
      }

      surfels.push_back(surfel);
    }
    return surfels;
  }

  Eigen::Matrix3f
  estimateOrientation(const std::vector<Eigen::Vector3f>& iPoints,
                      const Eigen::Matrix3f& iScanOrientation) {
    Eigen::Vector3f mean = Eigen::Vector3f::Zero();
    Eigen::Matrix3f meanSq = Eigen::Matrix3f::Zero();
    int n = iPoints.size();
    for (int i = 0; i < n; ++i) {
      mean += iPoints[i];
      meanSq += iPoints[i]*iPoints[i].transpose();
    }
    mean /= n;
    meanSq /= n;
    Eigen::Matrix3f cov = meanSq - mean*mean.transpose();
    Eigen::JacobiSVD<Eigen::Matrix3f> svd;
    svd.compute(cov, Eigen::ComputeFullV);
    Eigen::Matrix3f v = svd.matrixV();

    Eigen::Matrix3f orientation;
    orientation.col(2) = v.col(2);
    orientation.col(1) = v.col(2).cross(iScanOrientation.col(2));
    orientation.col(0) = orientation.col(1).cross(v.col(2));
    for (int k = 0; k < 3; ++k) orientation.col(k).normalize();
    return orientation;
  }

};

Surfelizer::
Surfelizer() {
  mHelper.reset(new Helper());
  setScanRadius(1);
  setPointRadius(1);
  setSizeMethod(SizeMethodAngles);
  setNominalSize(1);
  setDecimation(1);
}

Surfelizer::
~Surfelizer() {
}

void Surfelizer::
setScanRadius(const int iRadius) {
  mHelper->mScanRadius = iRadius;
  mHelper->mDataBuffer.set_capacity(2*iRadius+1);
}

void Surfelizer::
setPointRadius(const int iRadius) {
  mHelper->mPointRadius = iRadius;
}

void Surfelizer::
setSizeMethod(const SizeMethod iMethod) {
  mHelper->mSizeMethod = iMethod;
}

void Surfelizer::
setNominalSize(const float iSize) {
  mHelper->mNominalSize = iSize;
}

void Surfelizer::
setDecimation(const int iDecimation) {
  mHelper->mDecimation = iDecimation;
}

std::vector<Surfelizer::Surfel> Surfelizer::
addScan(const maps::PointSet& iScan) {
  return mHelper->addScan(iScan);
}