#include "Scene.h"
// local
#include "Mesh.h"
// std
#include <fstream>
// boost
#include <boost/algorithm/string.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/assignment.hpp>

//******************************************************************************
// Scene
//******************************************************************************

Scene::Scene(std::shared_ptr<Scene_state> state)
    : state_(state)
{
}

//******************************************************************************
// load_ode
//******************************************************************************

void Scene::load_ode(
    const std::vector<std::string>& fnames,
    float cuve_min_rad,
    float tesseract_size/* = 200.f*/)
{
    assert(state_);
    if(state_ == nullptr)
        return;

    // Remove all previous curves
    state_->curves.clear();
    // Reset size of the tesseract
    for(auto& s : state_->tesseract_size)
        s = tesseract_size;

    // The aggregative origin and size for all curves
    Scene_vertex_t total_origin(5), total_size(5);
    for(char i = 0; i < 5; ++i)
    {
        total_origin(i) = std::numeric_limits<float>::max();
        total_size(i)   = std::numeric_limits<float>::min();
    }

    std::vector<std::shared_ptr<Curve>> curves;

    // Load curves from files and calculate statistics
    for(auto& fn: fnames)
    {
        // Load curves
        auto curve = load_curve(fn);
        if(!curve)
            continue;

        Scene_vertex_t origin, size;
        curve->get_boundaries(origin, size);

        for(char i = 0; i < 5; ++i)
        {
            if(total_origin(i) > origin(i)) total_origin(i) = origin(i);
            if(total_size(i)   < size(i)  ) total_size(i)   = size(i);
        }

        curves.push_back(std::move(curve));
    }

    if(!state_->scale_tesseract)
    {
        float max_size = std::numeric_limits<float>::min();
        for(char i = 0; i < 4; ++i)
        {
            if(max_size < total_size(i)) max_size = total_size(i);
        }

        for(char i = 0; i < 4; ++i)
        {
            state_->tesseract_size[i] =
                tesseract_size * total_size[i] / max_size;
        }
    }

    Scene_vertex_t scale(5);
    scale[0] = state_->tesseract_size[0] / total_size[0];
    scale[1] = state_->tesseract_size[1] / total_size[1];
    scale[2] = state_->tesseract_size[2] / total_size[2];
    scale[3] = state_->tesseract_size[3] / total_size[3];
    scale[4] = 1;

    for(auto& c : curves)
    {
        c->translate_vertices(-0.5f * total_size - total_origin);
        c->scale_vertices(scale);

        auto curve = std::make_shared<Curve>(
            c->get_simpified_curve(cuve_min_rad));
        curve->update_stats(
            state_->stat_kernel_size,
            state_->stat_max_movement,
            state_->stat_max_value);
        state_->curves.push_back(std::move(curve));
    }

    // Save curve origin and size to the class members
    create_tesseract();

    if(state_->curves.size() > 0)
    {
        // Set selection (currently we take the range of the first curve, but
        // this behaviour can be reconsidered in the future)
        auto& first_curve = state_->curves.front();
        state_->curve_selection = std::make_unique<Curve_selection>();
        state_->curve_selection->t_start = first_curve->t_min();
        state_->curve_selection->t_end = first_curve->t_max();
    }
}

//******************************************************************************
// load_curve
//******************************************************************************

std::shared_ptr<Curve> Scene::load_curve(std::string fname)
{
    std::ifstream stream(fname);
    if(!stream.is_open())
        return nullptr;

    auto curve = std::make_shared<Curve>();

    std::string line;
    while(getline(stream, line))
    {
        // Remove some characters to make it easier to load files we get from
        // Wolfram Mathematica
        boost::erase_all(line, "\"{");
        boost::erase_all(line, "}\"");
        boost::replace_all(line, ", ", " ");
        boost::replace_all(line, "*^", "e");

        std::vector<std::string> vals;
        boost::split(vals, line, boost::is_any_of("\t, "));
        if(vals.size() == 5)
        {
            float t = std::stof(vals[0]);
            Scene_vertex_t p(5);
            p <<= std::stof(vals[1]), std::stof(vals[2]), std::stof(vals[3]),
                std::stof(vals[4]), 1;

            curve->add_point(p, t);
        }
    }

    return curve;
}

//******************************************************************************
// normalize_curve
//******************************************************************************

void Scene::normalize_curve(Curve& curve)
{
    Scene_vertex_t origin, size;
    curve.get_boundaries(origin, size);

    Scene_vertex_t scale(5);
    scale[0] = state_->tesseract_size[0] / size[0];
    scale[1] = state_->tesseract_size[0] / size[1];
    scale[2] = state_->tesseract_size[0] / size[2];
    scale[3] = state_->tesseract_size[0] / size[3];
    scale[4] = 1;
    curve.scale_vertices(scale);
}

//******************************************************************************
// create_tesseract
//******************************************************************************

void Scene::create_tesseract()
{
    assert(state_);
    if(state_ == nullptr)
        return;

    Scene_vertex_t origin(4), size(4);
    for(char i = 0; i < 4; ++i)
    {
        origin(i) = -0.5f * state_->tesseract_size[i];
        size(i) = state_->tesseract_size[i];
    }

    state_->tesseract = std::make_unique<Tesseract>(
        origin,
        size,
        state_->get_color(X_axis),
        state_->get_color(Y_axis),
        state_->get_color(Z_axis),
        state_->get_color(W_axis));
}


/**
 *  @file   l2Example.cpp
 *  @author Ryan Watson & Jason Gross
 *  @brief  Factor graph to process GNSS data. This example only handles pseudorange

    Current states estimated ::

            {   delta Pos  }
        X = {   Clk Bias   }
            {  zenith Trop }

 *  Ex:

 * Running
        ./examples/l2Example -i data.txt --writeENU --dir example1
 **/

// GTSAM
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/inference/Key.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/gnssNavigation/GnssData.h>
#include <gtsam/gnssNavigation/GnssTools.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/gnssNavigation/FolderUtils.h>
#include <gtsam/gnssNavigation/GnssPostfit.h>
#include <gtsam/gnssNavigation/nonBiasStates.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/robustModels/PseudorangeMaxMix.h>
#include <gtsam/gnssNavigation/PseudorangeFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

// BOOST
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/serialization/export.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>

// STANDARD
#include <fstream>
#include <iostream>
#include <ios>

// Intel Threading Building Block
#ifdef GTSAM_USE_TBB
  #include <tbb/tbb.h>
  #undef max // TBB seems to include windows.h and we don't want these macros
  #undef min
#endif

using namespace std;
using namespace gtsam;
using namespace boost;
namespace po = boost::program_options;

typedef noiseModel::Diagonal diagNoise;
namespace NM = gtsam::noiseModel;

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

        bool noTrop, switchFactor, switchFactorWBetween, writeGraph, elWeight, using_fs=false;
        bool writeENU, writeECEF, writeBias, loose, tight, robust = false, first_ob = true;
        int currKey=-1, trop=1, startEpoch=0, satKeyPrev=-1, sc=1, nThreads, startKey;
        int num_gps_factors=0, factorCount=0, lastStep, firstStep, initIter;
        double measWeight, percentFaulty;
        string gnssFile, outputFile, residualTxtInit="initResidaul.txt";
        string residualTxtOut="finalResidual.txt",textExtension=".txt", strategy;
        string switchExtension = "Switch.txt", graphExtension=".dot", dir;
        vector<string> satIndexLiteral;
        vector<rnxData> data;
        vector<int> numFactors;

        cout.precision(10);
        // define std out print color
        const string red("\033[0;31m");
        const string green("\033[0;32m");
        const string lineBreak = "###########################\n";

        NonlinearFactorGraph graph;

        po::options_description desc("Available options");
        desc.add_options()
                ("help,h", "Print help message")
                ("gpsObs,i", po::value<string>(&gnssFile)->default_value(""),
                "Input GNSS data file")
                ("percentFaulty", po::value<double>(&percentFaulty)->default_value(0.0),
                "Percentage of observations to add faults. (scale [0,1]) )")
                ("outFile,o", po::value<string>(&outputFile)->default_value("initResults"),
                "Write graph and solution to the specified file.")
                ("firstStep,f", po::value<int>(&firstStep)->default_value(0),
                "First step to process from the dataset file")
                ("lastStep,l", po::value<int>(&lastStep)->default_value(-1),
                "Last step to process, or -1 to process until the end of the dataset")
                ("threads", po::value<int>(&nThreads)->default_value(4),
                "Number of threads, or -1 to use all processors")
                ("noTrop", "Will turn residual troposphere estimation off. Troposphere will still be modeled.")
                ("initIter",po::value<int>(&initIter)->default_value(10),
                "Number of iterations before initial postfit data edit")
                ("dir", po::value<string>(&dir)->default_value(""),
                "Total path to store generated data")
                ("elWeight,el", "Elevation angle dependant measuremnt weighting")
                ("measWeight", po::value<double>(&measWeight)->default_value(3.0),
                "Noise applied to each GNSS observable")
                ("writeGraph",
                "Write graph to text file. Do not write large graphs (i.e. Nodes>=100)")
                ("writeECEF", "write ecef solution to file")
                ("writeENU", "write enu solution to file")
        ;

        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
        po::notify(vm);

        writeGraph = (vm.count("writeGraph")>0);
        writeENU = (vm.count("writeENU")>0);
        writeECEF = (vm.count("writeECEF")>0);
        writeBias = (vm.count("writeBias")>0);
        noTrop = (vm.count("noTrop") > 0);

        if ( gnssFile.empty() ) {
                cout << red << "\n\n GNSS data must be specified\n"
                     << "\n\n" << green << desc << endl;
                exit(1);
        }

        if ( noTrop ) { trop = 0; }

    #ifdef GTSAM_USE_TBB
        std::auto_ptr<tbb::task_scheduler_init> init;
        if(nThreads > 0) {
                cout << "\n\n Using " << nThreads << " threads " << endl;
                init.reset(new tbb::task_scheduler_init(nThreads));
        }
        else
                cout << green << " \n\n Using threads for all processors" << endl;
    #else
        if(nThreads > 0) {
                cout << red <<" \n\n GTSAM is not compiled with TBB, so threading is"
                     << " disabled and the --threads option cannot be used."
                     << endl;
                exit(1);
        }
    #endif

        // set up directory to store all generated data
        if ( dir.empty() ) { dir = getTimestamp(); }
        makeDir( dir );
        chdir( dir.c_str() );

        // Specify the starting location of the platform in ECEF XYZ.
        Point3 nomNED(0.0, 0.0, 0.0);
        Point3 nomXYZ(856295.3346, -4843033.4111, 4048017.6649);

        double output_time = 0.0;
        double rangeWeight = pow(measWeight,2);

        nonBiasStates initEst((gtsam::Vector(5) << 0,0,0,0,0).finished());

        Values initial_values;
        using symbol_shorthand::X; // nonBiasStates ( dx, dy, dz, trop, cb )

        noiseModel::Diagonal::shared_ptr nonBias_InitNoise = noiseModel::Diagonal::Sigmas((gtsam::Vector(5) << 30.0, 30.0, 30.0, 1e3, 1e-1).finished());

        noiseModel::Diagonal::shared_ptr nonBias_ProcessNoise = noiseModel::Diagonal::Sigmas((gtsam::Vector(5) << 3.0, 3.0, 3.0, 10, 1e-3).finished());

        // Read GNSS data
        try {
                data = readGNSSFaulty(gnssFile, 0.0, 20.0, percentFaulty);
        }
        catch(std::exception& e)
        {
                cout << red << "\n\n Cannot read GNSS data file " << endl;
                exit(1);
        }

        strategy = "GNSS Only using L2 Optimization";
        cout << green << "\n\n" << lineBreak << " GNSS Data File :: "
             << gnssFile << endl;
        cout << "\n Processing Strategy  :: "  << strategy << endl;
        cout << lineBreak << endl;

        if ( firstStep != 0 ) {
                using_fs = true;
                for( unsigned int i = 0; i < data.size(); i++ ) {
                        if ( firstStep == get<1>(data[i]) ) { break; }
                        ++startEpoch;
                }
        }
        if ( lastStep < 0 ) { lastStep = get<0>(data.back()); }

        // Construct Graph --- Only Pseudorange factors
        for(unsigned int i = startEpoch; i < data.size()-1; i++ ) {

                // Get the current epoch's observables
                double gnssTime = get<0>(data[i]);
                int currKey = get<1>(data[i]);
                if (first_ob || using_fs) {
                        startKey = currKey; first_ob=false; using_fs=false;
                        graph.add(PriorFactor<nonBiasStates>(X(currKey), initEst,  nonBias_InitNoise));
                        ++factorCount;
                        initial_values.insert(X(currKey), initEst);
                }
                int nextKey = get<1>(data[i+1]);
                int svn = get<2>(data[i]);
                Point3 satXYZ = get<3>(data[i]);
                double dTrop = deltaTrop(satXYZ,nomXYZ);
                double rho = get<4>(data[i]);
                double range = get<5>(data[i]);

                // Generate pseudorange factor
                PseudorangeFactor gpsRangeFactor(X(currKey), (range-rho), satXYZ, nomXYZ,  diagNoise::Sigmas( (gtsam::Vector(1) << elDepWeight(satXYZ, nomXYZ, rangeWeight)).finished()));

                ++factorCount;
                graph.add(gpsRangeFactor);

                satIndexLiteral.push_back(to_string(currKey) + " " + to_string(svn));

                // Add prior and between factors
                if ( currKey != nextKey ) {
                        if ( lastStep == nextKey ) { break; }
                        initial_values.insert(X(nextKey),initEst);
                        graph.add(BetweenFactor<nonBiasStates>(X(currKey), X(nextKey), initEst, nonBias_ProcessNoise));
                        ++factorCount;
                }
        }

        // Optimize the graph
        LevenbergMarquardtParams params;
        params.maxIterations = initIter;
        Values result = LevenbergMarquardtOptimizer(graph, initial_values, params).optimize();

        try {
                // Render to PDF using "fdp pseudorange.dot -Tpdf > graph.phf"
                string enuSol = "enu.sol";
                string ecefSol = "ecef.sol";
                string optimizedGraph = "finalGraph.dot";
                string resultString = "finalResults.txt";
                writeStates( result, resultString );
                vector<double> residuals = getResiduals(nomXYZ, result, data);
                writeResiduals( residuals, residualTxtOut, satIndexLiteral );
                if (writeENU) { ofstream os(enuSol); writeNavFrame( result, nomXYZ, enuSol ); }
                if (writeGraph) { ofstream os(optimizedGraph); graph.saveGraph(os,result); }
                if (writeECEF) { ofstream os(ecefSol); writeEarthFrame( result, nomXYZ, ecefSol ); }
        }
        catch(std::exception& e) { cout << e.what() << endl; exit(1); }
        return 0;
}


#include "fieldtrack/LikelihoodChain.h"
#include <boost/foreach.hpp>

namespace argus
{
LikelihoodChain::LikelihoodChain()
	: _initialized( false )
{}

LikelihoodChain::~LikelihoodChain()
{
	ClearChain();
}

OutputPort& LikelihoodChain::GetMeanLL()
{
	return _meanLL.GetOutput();
}

void LikelihoodChain::InitializeChain( const VectorType& x,
                                       const MatrixType& P )
{
	KalmanChain::Initialize( x, P );
}

void LikelihoodChain::ClearChain()
{
	_transCov->UnregisterAll();
	typedef SourceRegistry::value_type Item;
	BOOST_FOREACH( Item & item, _obsModels )
	{
		CovarianceModel::Ptr& model = item.second;
		model->UnregisterAll();
	}
	_glls.clear();
	_scalers.clear();
	_meanLL.UnregisterAllSources( false );
	KalmanChain::Clear();
	_initialized = false;
}

void LikelihoodChain::RegisterTransCov( const CovarianceModel::Ptr& model )
{
	_transCov = model;
}

void LikelihoodChain::RegisterObsSource( const std::string& name,
                                         const CovarianceModel::Ptr& model )
{
	if( _obsModels.count( name ) > 0 )
	{
		throw std::invalid_argument( "Source " + name + " already registered." );
	}
	_obsModels[name] = model;
}

void LikelihoodChain::ProcessInfo( const FilterInfo& info )
{
	boost::apply_visitor( *this, info );
}

void LikelihoodChain::operator()( const PredictInfo& info )
{
	// TODO Bounded?
	_accPreds.emplace_back( info );
}

void LikelihoodChain::operator()( const UpdateInfo& info )
{
	const std::string& source = info.frameId;
	if( _obsModels.count( source ) == 0 )
	{
		throw std::invalid_argument( "Source " + source + " unregistered!" );
	}

	ProcessPreds();

	if( !_initialized )
	{
		InitializeChain( info.prior_state, info.prior_state_cov );
		_initialized = true;
	}

	UpdateModulePtr upd = KalmanChain::AddLinearUpdate( info.obs_jacobian,
	                                                    info.obs );
	_obsModels[source]->BindUpdateModule( *upd, info );

	_glls.emplace_back();
	_scalers.emplace_back();
	GaussianLikelihoodModule& gll = _glls.back();

	_meanLL.RegisterSource( gll.GetLLOut() );
	link_ports( upd->GetVOut(), gll.GetXIn() );
	link_ports( upd->GetSOut(), gll.GetSIn() );
}

void LikelihoodChain::ProcessPreds()
{
	BOOST_FOREACH( const PredictInfo &info, _accPreds )
	{
		if( !_initialized )
		{
			InitializeChain( info.prior_state, info.prior_state_cov );
			_initialized = true;
		}
		PredictModulePtr pred = KalmanChain::AddLinearPredict( info.trans_jacobian );
		_transCov->BindPredictModule( *pred, info );
	}
	_accPreds.clear();
}
}


/// \file Unit tests for the line function in geometry

#include <nrlib/geometry/line.hpp>

#include <boost/test/unit_test.hpp>
//#include <math.h>
//#include <algorithm>

using namespace NRLib;

BOOST_AUTO_TEST_CASE ( OneLineNormalToXYPlane )
{
  Point pt1(2, 2, -1);
  Point pt2(2, 2, 1);
  Point pt3(-2, 1, 0);
  Point pt4(2, 1, 0);
  Line def_line(pt1, pt2);
  Line line_in(pt3, pt4);
  Point intersect_pt;
  bool is_intersect = def_line.IntersectXY(line_in, intersect_pt);
  BOOST_CHECK_EQUAL(is_intersect, false);
  BOOST_CHECK_EQUAL(intersect_pt, Point(0, 0, 0));
}

BOOST_AUTO_TEST_CASE( LinesAreCrossing )
{
  Point pt1(0, 0, 0);
  Point pt2(0, 2, 0);
  Point pt3(-2, 1, 0);
  Point pt4(2, 1, 0);
  Line def_line(pt1, pt2);
  Line line_in(pt3, pt4);
  Point intersect_pt;
  bool is_intersect = def_line.IntersectXY(line_in, intersect_pt);
  BOOST_CHECK_EQUAL(is_intersect, true);
  BOOST_CHECK_EQUAL(intersect_pt, Point(0, 1, 0));
}

BOOST_AUTO_TEST_CASE( LinesAreNotCrossing )
{
  Point pt1(0, 0, 0);
  Point pt2(0, 2, 0);
  Point pt3(-2, 3, 0);
  Point pt4(2, 3, 0);
  Line def_line(pt1, pt2);
  Line line_in(pt3, pt4);
  Point intersect_pt;
  bool is_intersect = def_line.IntersectXY(line_in, intersect_pt);
  BOOST_CHECK_EQUAL(is_intersect, false);
  BOOST_CHECK_EQUAL(intersect_pt, Point(0, 0, 0));
}

BOOST_AUTO_TEST_CASE(InfiniteLinesAreCrossing)
{
  Point pt1(0, 0, 0);
  Point pt2(0, 2, 0);
  Point pt3(-2, 3, 0);
  Point pt4(2, 3, 0);
  Line def_line(pt1, pt2);
  Line line_in(pt3, pt4);
  Point intersect_pt;
  bool is_intersect = def_line.IntersectXY(line_in, intersect_pt, true);
  BOOST_CHECK_EQUAL(is_intersect, true);
  BOOST_CHECK_EQUAL(intersect_pt, Point(0, 3, 0));
}


#include "sac_fit.h"

/// PROJECT
#include <csapex/model/node_modifier.h>
#include <csapex/msg/generic_value_message.hpp>
#include <csapex/msg/generic_vector_message.hpp>
#include <csapex/msg/io.h>
#include <csapex/param/parameter_factory.h>
#include <csapex/utility/register_apex_plugin.h>
#include <csapex_point_cloud/msg/indices_message.h>

/// #include "sac_segmentation.hpp"

/// SYSTEM
// clang-format off
#include <csapex/utility/suppress_warnings_start.h>
#include <boost/mpl/for_each.hpp>
#include <pcl/segmentation/sac_segmentation.h>
#include <tf/tf.h>
#include <csapex/utility/suppress_warnings_end.h>
// clang-format on

CSAPEX_REGISTER_CLASS(csapex::SacFit, csapex::Node)

using namespace csapex;
using namespace csapex::connection_types;
using namespace std;

SacFit::SacFit()
{
}

void SacFit::setupParameters(Parameterizable& parameters)
{
    parameters.addParameter(param::factory::declareRange("iterations", 1, 20000, 5000, 200), max_iterations_);
    parameters.addParameter(param::factory::declareRange("min inliers", 5, 20000, 100, 100), min_inliers_);
    parameters.addParameter(param::factory::declareRange("normal distance weight", 0.0, 2.0, 0.085, 0.001), normal_distance_weight_);
    parameters.addParameter(param::factory::declareRange("distance threshold", 0.0, 2.0, 0.009, 0.001), distance_threshold_);
    parameters.addParameter(param::factory::declareValue<double>("model_main_axis_x", 0), model_main_axis_x_);
    parameters.addParameter(param::factory::declareValue<double>("model_main_axis_y", 0), model_main_axis_y_);
    parameters.addParameter(param::factory::declareValue<double>("model_main_axis_z", 0), model_main_axis_z_);
    parameters.addParameter(param::factory::declareValue<double>("model_angle_offset", 0), model_angle_offset_);
    parameters.addParameter(param::factory::declareRange("sphere min radius", 0.0, 2.0, 0.02, 0.005), sphere_r_max_);
    parameters.addParameter(param::factory::declareRange("sphere max radius", 0.0, 2.0, 0.8, 0.005), sphere_r_min_);

    parameters.addParameter(param::factory::declareBool("from normals", false), from_normals_);

    parameters.addParameter(param::factory::declareBool("optimize coefficients", true), optimize_coefficients_);

    std::map<std::string, int> model_types = { { "CIRCLE2D", pcl::SACMODEL_CIRCLE2D },
                                               { "CIRCLE3D", pcl::SACMODEL_CIRCLE3D },
                                               { "CONE", (int)pcl::SACMODEL_CONE },
                                               { "CYLINDER", (int)pcl::SACMODEL_CYLINDER },
                                               { "LINE", pcl::SACMODEL_LINE },
                                               { "NORMAL_PARALLEL_PLANE", (int)pcl::SACMODEL_NORMAL_PARALLEL_PLANE },
                                               { "NORMAL_PLANE", pcl::SACMODEL_NORMAL_PLANE },
                                               { "PARALLEL_LINE", pcl::SACMODEL_PARALLEL_LINE },
                                               { "PARALLEL_LINES", pcl::SACMODEL_PARALLEL_LINES },
                                               { "PARALLEL_PLANE", pcl::SACMODEL_PARALLEL_PLANE },
                                               { "PLANE", (int)pcl::SACMODEL_PLANE },
                                               { "PERPENDICULAR_PLANE", pcl::SACMODEL_PERPENDICULAR_PLANE },
                                               { "SPHERE", (int)pcl::SACMODEL_SPHERE },
                                               { "STICK", pcl::SACMODEL_STICK },
                                               { "TORUS", pcl::SACMODEL_TORUS } };

    parameters.addParameter(param::factory::declareParameterSet<int>("models", model_types, (int)pcl::SACMODEL_PLANE), model_type_);

    std::map<std::string, int> ransac_types = {
        { "LMEDS", pcl::SAC_LMEDS },   { "MLESAC", pcl::SAC_MLESAC }, { "MSAC", pcl::SAC_MSAC },       { "PROSAC", pcl::SAC_PROSAC },
        { "RANSAC", pcl::SAC_RANSAC }, { "RMSAC", pcl::SAC_RMSAC },   { "RRANSAC", pcl::SAC_RRANSAC },
    };

    parameters.addParameter(param::factory::declareParameterSet<int>("ransac type", ransac_types, (int)pcl::SAC_RANSAC), ransac_type_);
}

void SacFit::process()
{
    PointCloudMessage::ConstPtr msg(msg::getMessage<PointCloudMessage>(in_cloud_));
    boost::apply_visitor(PointCloudMessage::Dispatch<SacFit>(this, msg), msg->value);
}

void SacFit::setup(NodeModifier& node_modifier)
{
    in_cloud_ = node_modifier.addInput<PointCloudMessage>("PointCloud");
    in_indices_ = node_modifier.addOptionalInput<GenericVectorMessage, pcl::PointIndices>("Indices");  // optional input

    out_models_ = node_modifier.addOutput<GenericVectorMessage, ModelMessage>("Models");
    out_indices_ = node_modifier.addOutput<GenericVectorMessage, pcl::PointIndices>("Model Points");
}

template <class PointT>
void SacFit::inputCloud(typename pcl::PointCloud<PointT>::ConstPtr cloud)
{
    std::shared_ptr<std::vector<pcl::PointIndices>> out_indices(new std::vector<pcl::PointIndices>);
    std::shared_ptr<std::vector<ModelMessage>> out_models(new std::vector<ModelMessage>);

    /// configure the segmentation
    pcl::SACSegmentation<PointT>* segmenter;
    bool normals_needed = need_normals();  // some SAC models need normals / access conditional
    if (from_normals_ || normals_needed) {
        pcl::SACSegmentationFromNormals<PointT, pcl::Normal>* normal_segmenter;
        normal_segmenter = new pcl::SACSegmentationFromNormals<PointT, pcl::Normal>;
        normal_segmenter->setNormalDistanceWeight(normal_distance_weight_);

        pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
        estimateNormals<PointT>(cloud, normals);  /// input indices ??
        normal_segmenter->setInputNormals(normals);

        segmenter = normal_segmenter;
    } else {
        segmenter = new pcl::SACSegmentation<PointT>;
    }
    segmenter->setOptimizeCoefficients(optimize_coefficients_);
    segmenter->setModelType(model_type_);
    segmenter->setMethodType(ransac_type_);
    segmenter->setMaxIterations(max_iterations_);
    segmenter->setDistanceThreshold(distance_threshold_);

    //    segmenter->setRadiusLimits (sphere_r_min_, sphere_r_max_);      //
    //    circular objects / access conditional
    //    segmenter->setMinMaxOpeningAngle(sphere_r_min_, sphere_r_max_); // cones
    //    / access conditional

    if (model_main_axis_x_ != 0 || model_main_axis_y_ != 0 || model_main_axis_z_ != 0) {
        Eigen::Vector3f axis(model_main_axis_x_, model_main_axis_y_, model_main_axis_z_);
        segmenter->setAxis(axis);
        segmenter->setEpsAngle(model_angle_offset_);  /// angle to access conditional
    }

    /// let's fit
    segmenter->setInputCloud(cloud);
    if (msg::hasMessage(in_indices_)) {
        std::shared_ptr<std::vector<pcl::PointIndices> const> in_indices = msg::getMessage<GenericVectorMessage, pcl::PointIndices>(in_indices_);
        for (const pcl::PointIndices& indices : *in_indices) {
            pcl::ModelCoefficients::Ptr model_coefficients(new pcl::ModelCoefficients);
            pcl::PointIndices::Ptr indices_ptr(new pcl::PointIndices(indices));
            segmenter->setIndices(indices_ptr);

            pcl::PointIndices inliers;
            segmenter->segment(inliers, *model_coefficients);
            if ((int)inliers.indices.size() > min_inliers_) {
                out_indices->emplace_back(inliers);

                ModelMessage model;
                model.coefficients = model_coefficients;
                model.probability = segmenter->getProbability();
                model.frame_id = cloud->header.frame_id;
                model.model_type = (pcl::SacModel)model_type_;
                out_models->emplace_back(model);
            }
        }
    } else {
        /// modify for more than one model
        pcl::ModelCoefficients::Ptr model_coefficients(new pcl::ModelCoefficients);
        pcl::PointIndices inliers;
        segmenter->segment(inliers, *model_coefficients);
        if ((int)inliers.indices.size() > min_inliers_) {
            out_indices->emplace_back(inliers);

            ModelMessage model;
            model.coefficients = model_coefficients;
            model.probability = segmenter->getProbability();
            model.frame_id = cloud->header.frame_id;
            model.model_type = (pcl::SacModel)model_type_;
            out_models->emplace_back(model);
        }
    }

    msg::publish<GenericVectorMessage, pcl::PointIndices>(out_indices_, out_indices);
    msg::publish<GenericVectorMessage, ModelMessage>(out_models_, out_models);

    delete segmenter;
}

template <class PointT>
void SacFit::estimateNormals(typename pcl::PointCloud<PointT>::ConstPtr cloud, pcl::PointCloud<pcl::Normal>::Ptr normals)
{
    typename pcl::NormalEstimation<PointT, pcl::Normal> normal_estimation;
    typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);

    normal_estimation.setSearchMethod(tree);
    normal_estimation.setInputCloud(cloud);
    normal_estimation.setKSearch(50);
    normal_estimation.compute(*normals);
}

bool SacFit::need_normals()
{
    switch (model_type_) {
        case pcl::SACMODEL_NORMAL_PARALLEL_PLANE:
            return true;
        case pcl::SACMODEL_NORMAL_PLANE:
            return true;
        case pcl::SACMODEL_CONE:
            return true;
        default:
            return false;
    }
}


// Copyright 2018-2021 Jan Niklas Hasse <jhasse@bixense.com>
// For conditions of distribution and use, see copyright notice in LICENSE.txt

#include "opengl.hpp"

#include "jngl/App.hpp"

#include <boost/qvm/map_vec_mat.hpp>
#include <boost/qvm/mat_operations3.hpp>
#include <boost/qvm/vec.hpp>
#include <stdexcept>

namespace opengl {

jngl::Mat3 modelview;
jngl::Mat4 projection;
GLuint vaoStream;
GLuint vboStream;

void translate(float x, float y) {
	modelview *= boost::qvm::translation_mat(boost::qvm::vec<float, 2>{{ x, y }});
}

void scale(const float x, const float y) {
	modelview *= boost::qvm::diag_mat(boost::qvm::vec<float, 3>{{ x, y, 1 }});
}

GLuint genAndBindTexture() {
	GLuint texture;
	glGenTextures(1, &texture);
	glBindTexture(GL_TEXTURE_2D, texture);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER,
	                jngl::App::isPixelArt() ? GL_NEAREST : GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
	return texture;
}

} // namespace opengl


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_ElasticElectronMomentsEvaluator.hpp
//! \author Luke Kersting
//! \brief  Elastic electron cross section moments evaluator declaration
//!
//---------------------------------------------------------------------------//

#ifndef DATA_GEN_ELASTIC_ELECTRON_MOMENTS_EVALUATOR_HPP
#define DATA_GEN_ELASTIC_ELECTRON_MOMENTS_EVALUATOR_HPP

// Boost Includes
#include <boost/scoped_ptr.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "Data_EvaluatedElectronDataContainer.hpp"
#include "MonteCarlo_ElectroatomicReaction.hpp"
#include "Utility_OneDDistribution.hpp"
#include "Utility_TabularOneDDistribution.hpp"
#include "MonteCarlo_ScreenedRutherfordElasticElectronScatteringDistribution.hpp"
#include "Utility_SloanRadauQuadrature.hpp"

namespace DataGen{

//! The elastic electron cross section moments class
class ElasticElectronMomentsEvaluator
{

public:

  //! Typedef for the elastic distribution
  typedef Teuchos::Array<Utility::Pair<double,
		       Teuchos::RCP<const Utility::TabularOneDDistribution> > >
  ElasticDistribution;

  //! Constructor
  ElasticElectronMomentsEvaluator(
    const Data::EvaluatedElectronDataContainer& native_eedl_data,
    const double& cutoff_angle = 1.0e-6 );

  //! Destructor
  ~ElasticElectronMomentsEvaluator()
  { /* ... */ }

  //! Evaluate the Legnendre Polynomial expansion of the screened rutherford pdf
  double evaluateLegendreExpandedRutherford( const double scattering_angle,
                                        const double incoming_energy,
                                        const int polynomial_order = 0) const;

  //! Evaluate the Legnendre Polynomial expansion of the differential hard elastic pdf
  double evaluateLegendreExpandedPDF( const double scattering_angle,
                                      const double incoming_energy,
                                      const int polynomial_order = 0) const;

  //! Evaluate the Legnendre Polynomial expansion of the differential hard elastic pdf
  double evaluateLegendreExpandedPDFAtEnergyBin(
                                const double scattering_angle,
                                const unsigned incoming_energy_bin,
                                const int polynomial_order = 0) const;

  //! Return the moment of the elastic scattering distribution at a given energy and polynomial order
  void evaluateElasticMoment(
            Teuchos::Array<Utility::long_float>& legendre_moments,
            const double energy,
            const int n,
            const double precision ) const;

  //! Return the moment of the elastic scattering distribution at a given energy and polynomial order
  void evaluateElasticMoment(
            Teuchos::Array<Utility::long_float>& legendre_moments,
            const unsigned energy_bin,
            const int n,
            const double precision ) const;

  //! Evaluate the nth cross section moment of the screened Rutherford peak distribution
  void evaluateScreenedRutherfordMoment(
            Utility::long_float& rutherford_moments,
            const double energy,
            const int n ) const;

private:

  // The analog reaction
  Teuchos::RCP<MonteCarlo::ElectroatomicReaction> d_analog_reaction;

  // The screened Rutherford reaction
  Teuchos::RCP<MonteCarlo::ElectroatomicReaction> d_rutherford_reaction;

  // The analog distribution
  Teuchos::RCP<const MonteCarlo::ScreenedRutherfordElasticElectronScatteringDistribution>
    d_rutherford_distribution;

  // The screened Rutherford distribution
  Teuchos::RCP<const MonteCarlo::AnalogElasticElectronScatteringDistribution>
    d_analog_distribution;

  // The raw ace electron data extractor
  Data::EvaluatedElectronDataContainer d_native_eedl_data;

  // The angle cutoff between hard and soft scattering
  double d_cutoff_angle;

  // The angle cosine cutoff between hard and soft scattering
  double d_cutoff_angle_cosine;

  // The angle cutoff between the distrubution and screened Rutherford scattering
  static double s_rutherford_cutoff_angle;

  // The angle cosine cutoff between the distrubution and screened Rutherford scattering
  static double s_rutherford_cutoff_angle_cosine;
};

} // end DataGen namespace

#endif // end DATA_GEN_ELASTIC_ELECTRON_MOMENTS_EVALUATOR_HPP

//---------------------------------------------------------------------------//
// end DataGen_ElasticElectronMomentsEvaluator.hpp
//---------------------------------------------------------------------------//


//#include <boost/test/unit_test.hpp>
//#include <iostream>
//
//#include "ssmkit/model/linear_gaussian.hpp"
//#include "ssmkit/distribution/gaussian.hpp"
//#include "ssmkit/distribution/conditional_distribution.hpp"
//
//using namespace PROJECT_NAME;
//
//BOOST_AUTO_TEST_SUITE(conditional_distribution_test);
//
//BOOST_AUTO_TEST_CASE(one_step_test)
//{
//    model::LinearGaussian<2,2> f { {{1, 0}, {0, 1}}, {{1, 0},{0, 1}} };
//    distribution::Gaussian<2> g;
//    auto cpdf = distribution::makeParametericConditional(g,f);
//    std::cout << cpdf.random({100, 0}) << std::endl;
//}
//
//BOOST_AUTO_TEST_SUITE_END();


#include <scitbx/array_family/boost_python/flex_fwd.h>
#include <boost/python.hpp>

#include <tntbx/svd.h>
#include <tntbx/generalized_inverse.h>

namespace tntbx { namespace {

  template <typename FloatType>
  struct svd_m_ge_n_wrapper
  {
    typedef svd_m_ge_n<FloatType> w_t;

    static void
    wrap(const char* python_name)
    {
      using namespace boost::python;
      class_<w_t>(python_name, no_init)
        .def(init<af::const_ref<FloatType, af::c_grid<2> > const&>((
          arg_("m"))))
        .def("singular_values", &w_t::singular_values)
        .def("s", &w_t::s)
        .def("u", &w_t::u)
        .def("v", &w_t::v)
        .def("norm2", &w_t::norm2)
        .def("cond", &w_t::cond)
        .def("rank", &w_t::rank)
      ;
    };
  };

}} // namespace tntbx::<anonymous>

BOOST_PYTHON_MODULE(tntbx_ext)
{
  using namespace boost::python;
  tntbx::svd_m_ge_n_wrapper<double>::wrap("svd_m_ge_n_double");
  def("generalized_inverse", tntbx::generalized_inverse, (
    arg_("square_matrix")));
}


#ifndef MEMORY_NETWORK
#define MEMORY_NETWORK

#include <Eigen/Dense>
#include <map>
#include <set>
#include <string>
#include <random>
#include <chrono>
#include <thread>
#include <functional>

typedef Eigen::MatrixXd MemoryMatrix;
typedef Eigen::VectorXd MemoryVector;

typedef std::function<void(std::chrono::duration<long int, std::micro>,
                           const MemoryVector&)> LoggingFunction;

class MemoryNetwork
{

public:

    /** Creates a new associative memory network, initially empty.
     *
     * Call `add_unit` to add new units to the network. `size` returns the
     * current size of the network.
     *
     * Call `start` to start the network. Note that new units can be added at
     * any time, including when the network is running.
     *
     */
    MemoryNetwork(LoggingFunction activations_log_fn = nullptr, // user-defined callback used to store activation history
                  LoggingFunction external_activations_log_fn = nullptr, // user-defined callback used to store external activation history
                  double Dg = 0.2,     // activation decay (per ms)
                  double Lg = 0.01,    // learning rate (per ms)
                  double Eg = 0.6,     // external influence
                  double Ig = 0.3,     // internal influence
                  double Amax = 1.0,   // maximum activation
                  double Amin = -0.2,  // minimum activation
                  double Arest = -0.1, // rest activation
                  double Winit = 0.0); // initial weights

    void reset();

    /** Activate one unit at a specific level, for a specific duration.
     */
    void activate_unit(size_t id,
                    double level = 1.0,
                    std::chrono::microseconds duration = std::chrono::milliseconds(200));

    /** Activate one unit at a specific level, for a specific duration.
     *
     * Raises a `range_error` exception is the unit does not exist.
     */
    void activate_unit(const std::string& name,
                    double level = 1.0,
                    std::chrono::microseconds duration = std::chrono::milliseconds(200));

    /** Returns the list of all unit names, ordered by their internal IDs.
     *
     * The order is guaranteed to remain the same from one call to the other,
     * even after calling `reset` or `stop`.
     */
    std::vector<std::string> units_names() const {return _units_names;}

    /**
     * Adds a new unit to the network.
     *
     * Returns the internal ID of the newly created unit.
     *
     * Raises a `runtime_error` if the name is already in used.
     */
    size_t add_unit(const std::string& name);

    /** Returns true if the network already has a unit named `name`, false
     * otherwise.
     */
    bool has_unit(const std::string& name) const;

    /** Returns the internal ID of a unit.
     *
     * Raises a `range_error` exception is the unit does not exist.
     */
    size_t unit_id(const std::string& name) const;

    MemoryVector activations() const {return _activations;}
    MemoryMatrix weights() const {return _weights;}

    size_t size() const {return _size;}
    int frequency() const {return _frequency;}

    /** Slow down the memory update mechanism (typically, for debugging) up to
     * 'freq' updated per seconds
     *
     * The update frequency is set to max_freq.  Calling slowdown with
     * max_freq=0 removes any previously set limit.
     */
    void max_frequency(double freq);
    std::chrono::microseconds internal_period() const {return _min_period;}

    /** Changes between physical time and simulated time.
     *
     * By default, the network uses real, physical time.
     */
    void use_physical_time(bool use) {_use_physical_time = use;}

    /** Returns true if using real physical time, false if using simulated time.
     */
    bool is_using_physical_time() const {return _use_physical_time;}

    /** Returns the elapsed time since the network started.
     *
     * If the network has not started yet, returns 0.
     */
    std::chrono::microseconds elapsed_time() const;

    /** Configure the network parameters.
     * Possible parameter names are:
     *  - Dg: decay rate
     *  - Lg: learning rate
     *  - Eg: external influence
     *  - Ig: internal influence
     *  - Amax: maximum activation
     *  - Amin: minimum activation
     *  - Arest: rest activation
     *  - Winit: initial weights
     */
    void set_parameter(const std::string& name, double value);

    /* Returns the current value of a network parameter.
     * See `set_parameter` documentation for the list of parameters.
     *
     * Throws a `range_error` exception if the parameter does not exist.
     */
    double get_parameter(const std::string& name) const;

    /** Starts the memory system
     *
     * Note that the current weights and activations are *not* reset: as such,
     * one may call stop() then start() to pause/unpause the network.
     * Call reset() to actually reset the network to its initial empty
     * state.
     */
    void start();
    void stop();
    bool isrunning() const {return _is_running;}

    void record(bool enabled) {_is_recording=enabled;}
    bool isrecording() {return _is_recording;}
    void save_record();

    double Dg;
    double Lg;
    double Eg;
    double Ig;
    double Amax;
    double Amin;
    double Arest;
    double Winit;

private:
    MemoryVector rest_activations; // constant

    std::vector<std::string> _units_names;

    MemoryVector external_activations;
    MemoryVector external_activations_decay;
    MemoryVector internal_activations;
    MemoryVector net_activations;
    MemoryVector _activations;
    MemoryMatrix _weights;

    LoggingFunction _log_activation;
    LoggingFunction _log_external_activation;

    std::random_device rd;
    std::default_random_engine gen;

    void compute_internal_activations();

    void run();
    void step();

    size_t _size = 0;

    /** Conservatively increment the size the network. Conserves the current
     * weights, activations.
     *
     * *Needs to be called from the network update thread!*
     */
    void incrementsize();

    std::thread _network_thread;

    bool _is_running = false;

    bool _is_recording = false;
    std::map<size_t, std::vector<std::tuple<float, std::chrono::microseconds, std::chrono::microseconds>>> _activations_history;

    void printout();

    std::chrono::microseconds _min_period = std::chrono::microseconds::zero();
    bool _use_physical_time = true;

    int _frequency = 0;
    int _steps_since_last_frequency_update = 0;

    std::chrono::high_resolution_clock::time_point _start_time;
    std::chrono::high_resolution_clock::time_point _last_timestamp;
    std::chrono::high_resolution_clock::time_point _last_freq_computation;

    // only used when _use_physical_time = false
    std::chrono::microseconds _elapsed_time;
};

#endif


#include <utility/io/read_many.hpp>

#include <burst/range/intersect.hpp>

#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/iterator_range.hpp>

#include <iostream>
#include <numeric>
#include <vector>

template <typename Container>
void test_on_the_fly_intersect (const Container & values)
{
    using nested_container_type = typename Container::value_type;

    std::vector<boost::iterator_range<typename nested_container_type::const_iterator>> ranges;
    boost::for_each(values,
        [& ranges] (const nested_container_type & values)
        {
            ranges.push_back(boost::make_iterator_range(values));
        });

    clock_t intersect_time = clock();
    auto intersected_range = burst::intersect(ranges);
    auto distance = static_cast<std::size_t>(std::distance(intersected_range.begin(), intersected_range.end()));
    intersect_time = clock() - intersect_time;

    std::cout << "Пересечение на лету: " << distance << std::endl;
    std::cout << "\t" << static_cast<double>(intersect_time) / CLOCKS_PER_SEC << std::endl;
    std::cout << std::endl;
}

template <typename Container>
void test_std_intersect (const Container & values)
{
    typename Container::value_type intersected(values.front());

    clock_t intersect_time = clock();
    for (const auto & row: boost::make_iterator_range(std::next(values.begin()), values.end()))
    {
        typename Container::value_type result;
        result.reserve(intersected.size() + row.size());
        std::set_intersection(intersected.begin(), intersected.end(), row.begin(), row.end(), std::back_inserter(result));
        std::swap(result, intersected);
    }
    intersect_time = clock() - intersect_time;

    std::cout << "std::set_intersection: " << intersected.size() << std::endl;
    std::cout << "\t" << static_cast<double>(intersect_time) / CLOCKS_PER_SEC << std::endl;
    std::cout << std::endl;
}

int main ()
{
    std::vector<std::vector<std::int64_t>> values;
    utility::read_many(std::cin, values);

    test_std_intersect(values);
    test_on_the_fly_intersect(values);
}


//
//  rule_matrix.cpp
//  Prisoners Dilemma II
//
//  Created by Sergey Bogolepov on 11/24/14.
//  Copyright (c) 2014 Sergey Bogolepov. All rights reserved.
//

#include "rule_matrix.h"

#include <fstream>
#include <boost/algorithm/string.hpp>

void RuleMatrix::use_default_matrix(size_t size) {
    _matrix[{DEFECT, DEFECT, DEFECT}] = {1, 1, 1};
    _matrix[{DEFECT, DEFECT, COOPERATE}] = {3, 3, 0};
    _matrix[{DEFECT, COOPERATE, DEFECT}] = {3, 0, 3};
    _matrix[{DEFECT, COOPERATE, COOPERATE}] = {5, 2, 2};
    _matrix[{COOPERATE, DEFECT, DEFECT}] = {0, 3, 3};
    _matrix[{COOPERATE, DEFECT, COOPERATE}] = {2, 5, 2};
    _matrix[{COOPERATE, COOPERATE, DEFECT}] = {2, 2, 5};
    _matrix[{COOPERATE, COOPERATE, COOPERATE}] = {4, 4, 4};
}

void RuleMatrix::use_external_matrix(const std::string& path) {
    std::ifstream file(path);

    //file format
    // DECISIONS SCORES
    // D D C 3 3 0
    if (file.good()) {
        while (std::getline(file, _line)) {
            _parts.clear();
            _decision.clear();
            _score.clear();

            _split_string(_line);
            for (auto part: _parts) {
                if (part == "D") {
                    _decision.push_back(DEFECT);
                } else if (part == "C") {
                    _decision.push_back(COOPERATE);
                } else {
                    _score.push_back(std::stoi(part));
                }
            }
            _matrix[_decision] = _score;
        }
        file.close();
    }
}

void RuleMatrix::_split_string(const std::string& input) {
    boost::split(_parts, input, boost::is_any_of("\t "));
}


#pragma once

#include <boost/align/aligned_allocator.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <time.h>
#include <tuple>
#include <vector>

using namespace std;
namespace ub = boost::numeric::ublas;

#define ALIGNMENT 32

// note: a matrix of doubles of size 16e9 will require about 128GB to represent
// in memory

using Matrix = std::vector<std::vector<
                 double,
                 boost::alignment::aligned_allocator<double, ALIGNMENT> >>;
bool compare_matrices(Matrix a, Matrix b);
bool compare_matrix_with_boost_matrix(Matrix a, ub::matrix<double> b);
ub::matrix<double> copy_matrix_to_boost_matrix(Matrix input);
Matrix generate_matrix(size_t n);
void print_matrix(Matrix m);
void print_matrix(ub::matrix<double> m);


// COLMAP - Structure-from-Motion and Multi-View Stereo.
// Copyright (C) 2017  Johannes L. Schoenberger <jsch at inf.ethz.ch>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include "controllers/incremental_mapper.h"
#include "controllers/bundle_adjustment.h"
#include "util/logging.h"
#include "util/misc.h"
#include "util/option_manager.h"
#include <Eigen/Geometry>

using namespace colmap;

namespace {
    typedef Eigen::Matrix3d R_t;
    typedef Eigen::Vector3d tvec_t;
    typedef Eigen::Vector4d qvec_t;
    typedef Eigen::Matrix<double, 9, 9> cov_t;
}

void ReadCameraMeasurements(const std::string& path,
                         std::vector<std::string>* image_names,
                         std::vector<tvec_t>* ricI_vec,
                         std::vector<tvec_t>* rciC_vec,
                         std::vector<R_t>* RIC_vec,
                         std::vector<cov_t>* cov_vec) {
  std::vector<std::string> lines = ReadTextFileLines(path);
  for (const auto line : lines) {
    std::stringstream line_parser(line);

    // Buffers to store data in
    std::string image_name = "";
    tvec_t ricI;
    tvec_t rciC;
    R_t RIC;
    cov_t cov;

    // Read data into buffers
    // Translation
    line_parser >>
        image_name >>
        ricI(0) >>
        ricI(1) >>
        ricI(2) >>
        rciC(0) >>
        rciC(1) >>
        rciC(2);

    // Rotation
    for(int i = 0; i < 3; i ++) {
        for(int j = 0; j < 3; j++) {
            line_parser >> RIC(j, i);
        }
    }

    // Covariance
    for(int i = 0; i < 9; i++) {
        for(int j = 0; j < 9; j++) {
            line_parser >> cov(j, i);
        }
    }

    // Push data into vectors
    image_names->push_back(image_name);
    ricI_vec->push_back(ricI);
    rciC_vec->push_back(rciC);
    RIC_vec->push_back(RIC);
    cov_vec->push_back(cov);
  }
}

int main(int argc, char** argv) {
  InitializeGlog(argv);

  std::string import_path;
  std::string priors_path;
  std::string aligned_path;
  std::string metadata_path;

  OptionManager options;
  options.AddRequiredOption("import_path", &import_path);
  options.AddRequiredOption("priors_path", &priors_path);
  options.AddRequiredOption("aligned_path", &aligned_path);
  options.AddRequiredOption("metadata_path", &metadata_path);
  options.AddBundleAdjustmentOptions();
  options.Parse(argc, argv);

  Reconstruction reconstruction;
  reconstruction.Read(import_path);

  /* GAME PLAN
  * 1) Read in the measurements
  * 2) Apply similarity transform to data. Bring reconstruction into global frame.
  * 3) Insert the measurements into Colmap
  * 4) Run global BA
  */

  /* 1) Read in the camera measurements */
  std::vector<std::string> image_names;
  std::vector<tvec_t> ricI_vec;
  std::vector<tvec_t> rciC_vec;
  std::vector<R_t> RIC_vec;
  std::vector<cov_t> cov_vec;
  ReadCameraMeasurements(
          metadata_path,
          &image_names,
          &ricI_vec,
          &rciC_vec,
          &RIC_vec,
          &cov_vec);

  /* 2) Align reconstruction with ECEF measurements to bring it into global frame. */
  RANSACOptions ransac_options;
  ransac_options.max_error = 0.01;
  bool alignment_success = reconstruction.AlignRobust(
          image_names,
          ricI_vec,
          3,
          ransac_options);

  if (alignment_success) {
    std::cout << " => Alignment succeeded" << std::endl;
    reconstruction.Write(aligned_path);
    reconstruction.WriteText(aligned_path);
  } else {
    std::cout << " => Alignment failed" << std::endl;
    return EXIT_FAILURE;
  }

  /* 3) Insert the measurements into reconstruction */
  std::unordered_map< std::string, std::tuple<tvec_t, qvec_t, Eigen::Matrix<double, 6, 6> > > image_priors;
  for(size_t i = 0; i < rciC_vec.size(); i++) {
    image_priors.insert({
                image_names[i],
                std::make_tuple(ricI_vec[i],
                                RotationMatrixToQuaternion(RIC_vec[i]),
                                cov_vec[i].bottomRightCorner<6,6>())      // Bottom right corner is [eIC, ricI]
    });
  }

  reconstruction.AddPriors(image_priors);

  /* 4) Run global BA */
  // Configure BA
  options.bundle_adjustment->cov.compute = false;
  options.bundle_adjustment->priors = true;
  // options.bundle_adjustment->refine_focal_length = true;
  // options.bundle_adjustment->refine_extra_params = true;
  // options.bundle_adjustment->refine_principal_point = true;
  // options.bundle_adjustment->loss_function_type =
  //     BundleAdjustmentOptions::LossFunctionType::CAUCHY;
  // options.bundle_adjustment->loss_function_scale = 5;
  options.bundle_adjustment->solver_options.max_num_iterations = 100;

  // Run BA
  BundleAdjustmentController ba_controller(options, &reconstruction);
  ba_controller.Start();
  ba_controller.Wait();

  // Save output
  reconstruction.Write(priors_path);
  reconstruction.WriteText(priors_path);

  std::cout << "Success!" << std::endl;

  return EXIT_SUCCESS;
}


/* Author: Guido Kanschat, Texas A&M University, 2009 */

/*    $Id: step-39.cc 28552 2013-02-25 15:18:15Z kanschat $       */
/*                                                                */
/*    Copyright (C) 2010-2013 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// The include files for the linear algebra: A regular SparseMatrix, which in
// turn will include the necessary files for SparsityPattern and Vector
// classes.
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/compressed_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/precondition_block.h>
#include <deal.II/lac/block_vector.h>

// Include files for setting up the mesh
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>

// Include files for FiniteElement classes and DoFHandler.
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_dgp.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/multigrid/mg_dof_handler.h>

// The include files for using the MeshWorker framework
#include <deal.II/meshworker/dof_info.h>
#include <deal.II/meshworker/integration_info.h>
#include <deal.II/meshworker/assembler.h>
#include <deal.II/meshworker/loop.h>

// The include file for local integrators associated with the Laplacian
#include <deal.II/integrators/laplace.h>

// Support for multigrid methods
#include <deal.II/multigrid/mg_tools.h>
#include <deal.II/multigrid/multigrid.h>
#include <deal.II/multigrid/mg_matrix.h>
#include <deal.II/multigrid/mg_transfer.h>
#include <deal.II/multigrid/mg_coarse.h>
#include <deal.II/multigrid/mg_smoother.h>

// Finally, we take our exact solution from the library as well as quadrature
// and additional tools.
#include <deal.II/base/function_lib.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/data_out.h>

#include <iostream>
#include <fstream>

// All classes of the deal.II library are in the namespace dealii. In order to
// save typing, we tell the compiler to search names in there as well.
namespace Step39
{
  using namespace dealii;

  // This is the function we use to set the boundary values and also the exact
  // solution we compare to.
  Functions::SlitSingularityFunction<2> exact_solution;

  // @sect3{The local integrators}

  // MeshWorker separates local integration from the loops over cells and
  // faces. Thus, we have to write local integration classes for generating
  // matrices, the right hand side and the error estimator.

  // All these classes have the same three functions for integrating over
  // cells, boundary faces and interior faces, respectively. All the
  // information needed for the local integration is provided by
  // MeshWorker::IntegrationInfo<dim>. Note that the signature of the
  // functions cannot be changed, because it is expected by
  // MeshWorker::integration_loop().

  // The first class defining local integrators is responsible for computing
  // cell and face matrices. It is used to assemble the global matrix as well
  // as the level matrices.
  template <int dim>
  class MatrixIntegrator : public MeshWorker::LocalIntegrator<dim>
  {
  public:
    void cell(MeshWorker::DoFInfo<dim> &dinfo,
	      typename MeshWorker::IntegrationInfo<dim> &info) const;
    void boundary(MeshWorker::DoFInfo<dim> &dinfo,
		  typename MeshWorker::IntegrationInfo<dim> &info) const;
    void face(MeshWorker::DoFInfo<dim> &dinfo1,
	      MeshWorker::DoFInfo<dim> &dinfo2,
	      typename MeshWorker::IntegrationInfo<dim> &info1,
	      typename MeshWorker::IntegrationInfo<dim> &info2) const;
  };

  // On each cell, we integrate the Dirichlet form. We use the library of
  // ready made integrals in LocalIntegrators to avoid writing these loops
  // ourselves. Similarly, we implement Nitsche boundary conditions and the
  // interior penalty fluxes between cells.
  //
  // The boundary und flux terms need a penalty parameter, which should be
  // adjusted to the cell size and the polynomial degree. A safe choice of
  // this parameter for constant coefficients can be found in
  // LocalIntegrators::Laplace::compute_penalty() and we use this below.
  template <int dim>
  void MatrixIntegrator<dim>::cell(
    MeshWorker::DoFInfo<dim> &dinfo,
    typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    LocalIntegrators::Laplace::cell_matrix(dinfo.matrix(0,false).matrix, info.fe_values());
  }

  template <int dim>
  void MatrixIntegrator<dim>::boundary(
    MeshWorker::DoFInfo<dim> &dinfo,
    typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const unsigned int deg = info.fe_values(0).get_fe().tensor_degree();
    LocalIntegrators::Laplace::nitsche_matrix(
      dinfo.matrix(0,false).matrix, info.fe_values(0),
      LocalIntegrators::Laplace::compute_penalty(dinfo, dinfo, deg, deg));
  }

  // Interior faces use the interior penalty method
  template <int dim>
  void MatrixIntegrator<dim>::face(
    MeshWorker::DoFInfo<dim> &dinfo1,
    MeshWorker::DoFInfo<dim> &dinfo2,
    typename MeshWorker::IntegrationInfo<dim> &info1,
    typename MeshWorker::IntegrationInfo<dim> &info2) const
  {
    const unsigned int deg = info1.fe_values(0).get_fe().tensor_degree();
    LocalIntegrators::Laplace::ip_matrix(
      dinfo1.matrix(0,false).matrix, dinfo1.matrix(0,true).matrix,
      dinfo2.matrix(0,true).matrix, dinfo2.matrix(0,false).matrix,
      info1.fe_values(0), info2.fe_values(0),
      LocalIntegrators::Laplace::compute_penalty(dinfo1, dinfo2, deg, deg));
  }

  // The second local integrator builds the right hand side. In our example,
  // the right hand side function is zero, such that only the boundary
  // condition is set here in weak form.
  template <int dim>
  class RHSIntegrator : public MeshWorker::LocalIntegrator<dim>
  {
  public:
    void cell(MeshWorker::DoFInfo<dim> &dinfo, typename MeshWorker::IntegrationInfo<dim> &info) const;
    void boundary(MeshWorker::DoFInfo<dim> &dinfo, typename MeshWorker::IntegrationInfo<dim> &info) const;
    void face(MeshWorker::DoFInfo<dim> &dinfo1,
	      MeshWorker::DoFInfo<dim> &dinfo2,
	      typename MeshWorker::IntegrationInfo<dim> &info1,
	      typename MeshWorker::IntegrationInfo<dim> &info2) const;
  };

  template <int dim>
  void RHSIntegrator<dim>::cell(MeshWorker::DoFInfo<dim> &, typename MeshWorker::IntegrationInfo<dim> &) const
  {}

  template <int dim>
  void RHSIntegrator<dim>::boundary(MeshWorker::DoFInfo<dim> &dinfo, typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const FEValuesBase<dim> &fe = info.fe_values();
    Vector<double> &local_vector = dinfo.vector(0).block(0);

    std::vector<double> boundary_values(fe.n_quadrature_points);
    exact_solution.value_list(fe.get_quadrature_points(), boundary_values);

    const unsigned int deg = fe.get_fe().tensor_degree();
    const double penalty = 2. * deg * (deg+1) * dinfo.face->measure() / dinfo.cell->measure();

    for (unsigned k=0; k<fe.n_quadrature_points; ++k)
      for (unsigned int i=0; i<fe.dofs_per_cell; ++i)
        local_vector(i) += (- fe.shape_value(i,k) * penalty * boundary_values[k]
                            + (fe.normal_vector(k) * fe.shape_grad(i,k)) * boundary_values[k])
                           * fe.JxW(k);
  }

  template <int dim>
  void RHSIntegrator<dim>::face(MeshWorker::DoFInfo<dim> &,
                                MeshWorker::DoFInfo<dim> &,
                                typename MeshWorker::IntegrationInfo<dim> &,
                                typename MeshWorker::IntegrationInfo<dim> &) const
  {}

  // The third local integrator is responsible for the contributions to the
  // error estimate. This is the standard energy estimator due to Karakashian
  // and Pascal (2003).
  template <int dim>
  class Estimator : public MeshWorker::LocalIntegrator<dim>
  {
  public:
    void cell(MeshWorker::DoFInfo<dim> &dinfo, typename MeshWorker::IntegrationInfo<dim> &info) const;
    void boundary(MeshWorker::DoFInfo<dim> &dinfo, typename MeshWorker::IntegrationInfo<dim> &info) const;
    void face(MeshWorker::DoFInfo<dim> &dinfo1,
	      MeshWorker::DoFInfo<dim> &dinfo2,
	      typename MeshWorker::IntegrationInfo<dim> &info1,
	      typename MeshWorker::IntegrationInfo<dim> &info2) const;
  };

  // The cell contribution is the Laplacian of the discrete solution, since
  // the right hand side is zero.
  template <int dim>
  void Estimator<dim>::cell(MeshWorker::DoFInfo<dim> &dinfo, typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const FEValuesBase<dim> &fe = info.fe_values();

    const std::vector<Tensor<2,dim> > &DDuh = info.hessians[0][0];
    for (unsigned k=0; k<fe.n_quadrature_points; ++k)
      {
        const double t = dinfo.cell->diameter() * trace(DDuh[k]);
        dinfo.value(0) +=  t*t * fe.JxW(k);
      }
    dinfo.value(0) = std::sqrt(dinfo.value(0));
  }

  // At the boundary, we use simply a weighted form of the boundary residual,
  // namely the norm of the difference between the finite element solution and
  // the correct boundary condition.
  template <int dim>
  void Estimator<dim>::boundary(MeshWorker::DoFInfo<dim> &dinfo, typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const FEValuesBase<dim> &fe = info.fe_values();

    std::vector<double> boundary_values(fe.n_quadrature_points);
    exact_solution.value_list(fe.get_quadrature_points(), boundary_values);

    const std::vector<double> &uh = info.values[0][0];

    const unsigned int deg = fe.get_fe().tensor_degree();
    const double penalty = 2. * deg * (deg+1) * dinfo.face->measure() / dinfo.cell->measure();

    for (unsigned k=0; k<fe.n_quadrature_points; ++k)
      dinfo.value(0) += penalty * (boundary_values[k] - uh[k]) * (boundary_values[k] - uh[k])
                        * fe.JxW(k);
    dinfo.value(0) = std::sqrt(dinfo.value(0));
  }

  // Finally, on interior faces, the estimator consists of the jumps of the
  // solution and its normal derivative, weighted appropriately.
  template <int dim>
  void Estimator<dim>::face(MeshWorker::DoFInfo<dim> &dinfo1,
                            MeshWorker::DoFInfo<dim> &dinfo2,
                            typename MeshWorker::IntegrationInfo<dim> &info1,
                            typename MeshWorker::IntegrationInfo<dim> &info2) const
  {
    const FEValuesBase<dim> &fe = info1.fe_values();
    const std::vector<double> &uh1 = info1.values[0][0];
    const std::vector<double> &uh2 = info2.values[0][0];
    const std::vector<Tensor<1,dim> > &Duh1 = info1.gradients[0][0];
    const std::vector<Tensor<1,dim> > &Duh2 = info2.gradients[0][0];

    const unsigned int deg = fe.get_fe().tensor_degree();
    const double penalty1 = deg * (deg+1) * dinfo1.face->measure() / dinfo1.cell->measure();
    const double penalty2 = deg * (deg+1) * dinfo2.face->measure() / dinfo2.cell->measure();
    const double penalty = penalty1 + penalty2;
    const double h = dinfo1.face->measure();

    for (unsigned k=0; k<fe.n_quadrature_points; ++k)
      {
        double diff1 = uh1[k] - uh2[k];
        double diff2 = fe.normal_vector(k) * Duh1[k] - fe.normal_vector(k) * Duh2[k];
        dinfo1.value(0) += (penalty * diff1*diff1 + h * diff2*diff2)
                           * fe.JxW(k);
      }
    dinfo1.value(0) = std::sqrt(dinfo1.value(0));
    dinfo2.value(0) = dinfo1.value(0);
  }

  // Finally we have an integrator for the error. Since the energy norm for
  // discontinuous Galerkin problems not only involves the difference of the
  // gradient inside the cells, but also the jump terms across faces and at
  // the boundary, we cannot just use VectorTools::integrate_difference().
  // Instead, we use the MeshWorker interface to compute the error ourselves.

  // There are several different ways to define this energy norm, but all of
  // them are equivalent to each other uniformly with mesh size (some not
  // uniformly with polynomial degree). Here, we choose @f[ \|u\|_{1,h} =
  // \sum_{K\in \mathbb T_h} \|\nabla u\|_K^2 + \sum_{F \in F_h^i}
  // 4\sigma_F\|\{\!\{ u \mathbf n\}\!\}\|^2_F + \sum_{F \in F_h^b}
  // 2\sigma_F\|u\|^2_F @f]

  template <int dim>
  class ErrorIntegrator : public MeshWorker::LocalIntegrator<dim>
  {
  public:
    void cell(MeshWorker::DoFInfo<dim> &dinfo, typename MeshWorker::IntegrationInfo<dim> &info) const;
    void boundary(MeshWorker::DoFInfo<dim> &dinfo, typename MeshWorker::IntegrationInfo<dim> &info) const;
    void face(MeshWorker::DoFInfo<dim> &dinfo1,
	      MeshWorker::DoFInfo<dim> &dinfo2,
	      typename MeshWorker::IntegrationInfo<dim> &info1,
	      typename MeshWorker::IntegrationInfo<dim> &info2) const;
  };

  // Here we have the integration on cells. There is currently no good
  // interfce in MeshWorker that would allow us to access values of regular
  // functions in the quadrature points. Thus, we have to create the vectors
  // for the exact function's values and gradients inside the cell
  // integrator. After that, everything is as before and we just add up the
  // squares of the differences.

  // Additionally to computing the error in the energy norm, we use the
  // capability of the mesh worker to compute two functionals at the same time
  // and compute the L2-error in the same loop. Obviously,
  // this one does not have any jump terms and only appears in the integration
  // on cells.
  template <int dim>
  void ErrorIntegrator<dim>::cell(
    MeshWorker::DoFInfo<dim> &dinfo,
    typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const FEValuesBase<dim> &fe = info.fe_values();
    std::vector<Tensor<1,dim> > exact_gradients(fe.n_quadrature_points);
    std::vector<double> exact_values(fe.n_quadrature_points);

    exact_solution.gradient_list(fe.get_quadrature_points(), exact_gradients);
    exact_solution.value_list(fe.get_quadrature_points(), exact_values);

    const std::vector<Tensor<1,dim> > &Duh = info.gradients[0][0];
    const std::vector<double> &uh = info.values[0][0];

    for (unsigned k=0; k<fe.n_quadrature_points; ++k)
      {
        double sum = 0;
        for (unsigned int d=0; d<dim; ++d)
          {
            const double diff = exact_gradients[k][d] - Duh[k][d];
            sum += diff*diff;
          }
        const double diff = exact_values[k] - uh[k];
        dinfo.value(0) +=  sum * fe.JxW(k);
        dinfo.value(1) +=  diff*diff * fe.JxW(k);
      }
    dinfo.value(0) = std::sqrt(dinfo.value(0));
    dinfo.value(1) = std::sqrt(dinfo.value(1));
  }

  template <int dim>
  void ErrorIntegrator<dim>::boundary(
    MeshWorker::DoFInfo<dim> &dinfo,
    typename MeshWorker::IntegrationInfo<dim> &info) const
  {
    const FEValuesBase<dim> &fe = info.fe_values();

    std::vector<double> exact_values(fe.n_quadrature_points);
    exact_solution.value_list(fe.get_quadrature_points(), exact_values);

    const std::vector<double> &uh = info.values[0][0];

    const unsigned int deg = fe.get_fe().tensor_degree();
    const double penalty = 2. * deg * (deg+1) * dinfo.face->measure() / dinfo.cell->measure();

    for (unsigned k=0; k<fe.n_quadrature_points; ++k)
      {
        const double diff = exact_values[k] - uh[k];
        dinfo.value(0) += penalty * diff * diff * fe.JxW(k);
      }
    dinfo.value(0) = std::sqrt(dinfo.value(0));
  }

  template <int dim>
  void ErrorIntegrator<dim>::face(
    MeshWorker::DoFInfo<dim> &dinfo1,
    MeshWorker::DoFInfo<dim> &dinfo2,
    typename MeshWorker::IntegrationInfo<dim> &info1,
    typename MeshWorker::IntegrationInfo<dim> &info2) const
  {
    const FEValuesBase<dim> &fe = info1.fe_values();
    const std::vector<double> &uh1 = info1.values[0][0];
    const std::vector<double> &uh2 = info2.values[0][0];

    const unsigned int deg = fe.get_fe().tensor_degree();
    const double penalty1 = deg * (deg+1) * dinfo1.face->measure() / dinfo1.cell->measure();
    const double penalty2 = deg * (deg+1) * dinfo2.face->measure() / dinfo2.cell->measure();
    const double penalty = penalty1 + penalty2;

    for (unsigned k=0; k<fe.n_quadrature_points; ++k)
      {
        double diff = uh1[k] - uh2[k];
        dinfo1.value(0) += (penalty * diff*diff)
                           * fe.JxW(k);
      }
    dinfo1.value(0) = std::sqrt(dinfo1.value(0));
    dinfo2.value(0) = dinfo1.value(0);
  }

  // @sect3{The main class}

  // This class does the main job, like in previous examples. For a
  // description of the functions declared here, please refer to the
  // implementation below.
  template <int dim>
  class InteriorPenaltyProblem
  {
  public:
    typedef MeshWorker::IntegrationInfo<dim> CellInfo;

    InteriorPenaltyProblem(const FiniteElement<dim> &fe);

    void run(unsigned int n_steps);

  private:
    void setup_system ();
    void assemble_matrix ();
    void assemble_mg_matrix ();
    void assemble_right_hand_side ();
    void error ();
    double estimate ();
    void solve ();
    void output_results (const unsigned int cycle) const;

    // The member objects related to the discretization are here.
    Triangulation<dim>        triangulation;
    const MappingQ1<dim>      mapping;
    const FiniteElement<dim> &fe;
    MGDoFHandler<dim>         mg_dof_handler;
    DoFHandler<dim>          &dof_handler;

    // Then, we have the matrices and vectors related to the global discrete
    // system.
    SparsityPattern      sparsity;
    SparseMatrix<double> matrix;
    Vector<double>       solution;
    Vector<double>       right_hand_side;
    BlockVector<double>  estimates;

    // Finally, we have a group of sparsity patterns and sparse matrices
    // related to the multilevel preconditioner.  First, we have a level
    // matrix and its sparsity pattern.
    MGLevelObject<SparsityPattern> mg_sparsity;
    MGLevelObject<SparseMatrix<double> > mg_matrix;

    // When we perform multigrid with local smoothing on locally refined
    // meshes, additional matrices are required; see Kanschat (2004). Here is
    // the sparsity pattern for these edge matrices. We only need one, because
    // the pattern of the up matrix is the transpose of that of the down
    // matrix. Actually, we do not care too much about these details, since
    // the MeshWorker is filling these matrices.
    MGLevelObject<SparsityPattern> mg_sparsity_dg_interface;
    // The flux matrix at the refinement edge, coupling fine level degrees of
    // freedom to coarse level.
    MGLevelObject<SparseMatrix<double> > mg_matrix_dg_down;
    // The transpose of the flux matrix at the refinement edge, coupling
    // coarse level degrees of freedom to fine level.
    MGLevelObject<SparseMatrix<double> > mg_matrix_dg_up;
  };

  // The constructor simply sets up the coarse grid and the DoFHandler. The
  // FiniteElement is provided as a parameter to allow flexibility.
  template <int dim>
  InteriorPenaltyProblem<dim>::InteriorPenaltyProblem(const FiniteElement<dim> &fe)
    :
    mapping(),
    fe(fe),
    mg_dof_handler(triangulation),
    dof_handler(mg_dof_handler),
    estimates(1)
  {
    GridGenerator::hyper_cube_slit(triangulation, -1, 1);
  }

  // In this function, we set up the dimension of the linear system and the
  // sparsity patterns for the global matrix as well as the level matrices.
  template <int dim>
  void
  InteriorPenaltyProblem<dim>::setup_system()
  {
    // First, we use the finite element to distribute degrees of freedom over
    // the mesh and number them.
    dof_handler.distribute_dofs(fe);
    unsigned int n_dofs = dof_handler.n_dofs();
    // Then, we already know the size of the vectors representing finite
    // element functions.
    solution.reinit(n_dofs);
    right_hand_side.reinit(n_dofs);

    // Next, we set up the sparsity pattern for the global matrix. Since we do
    // not know the row sizes in advance, we first fill a temporary
    // CompressedSparsityPattern object and copy it to the regular
    // SparsityPattern once it is complete.
    CompressedSparsityPattern c_sparsity(n_dofs);
    DoFTools::make_flux_sparsity_pattern(dof_handler, c_sparsity);
    sparsity.copy_from(c_sparsity);
    matrix.reinit(sparsity);

    const unsigned int n_levels = triangulation.n_levels();
    // The global system is set up, now we attend to the level matrices. We
    // resize all matrix objects to hold one matrix per level.
    mg_matrix.resize(0, n_levels-1);
    mg_matrix.clear();
    mg_matrix_dg_up.resize(0, n_levels-1);
    mg_matrix_dg_up.clear();
    mg_matrix_dg_down.resize(0, n_levels-1);
    mg_matrix_dg_down.clear();
    // It is important to update the sparsity patterns after <tt>clear()</tt>
    // was called for the level matrices, since the matrices lock the sparsity
    // pattern through the Smartpointer ans Subscriptor mechanism.
    mg_sparsity.resize(0, n_levels-1);
    mg_sparsity_dg_interface.resize(0, n_levels-1);

    // Now all objects are prepared to hold one sparsity pattern or matrix per
    // level. What's left is setting up the sparsity patterns on each level.
    for (unsigned int level=mg_sparsity.min_level();
         level<=mg_sparsity.max_level(); ++level)
      {
        // These are roughly the same lines as above for the global matrix,
        // now for each level.
        CompressedSparsityPattern c_sparsity(mg_dof_handler.n_dofs(level));
        MGTools::make_flux_sparsity_pattern(mg_dof_handler, c_sparsity, level);
        mg_sparsity[level].copy_from(c_sparsity);
        mg_matrix[level].reinit(mg_sparsity[level]);

        // Additionally, we need to initialize the transfer matrices at the
        // refinement edge between levels. They are stored at the index
        // referring to the finer of the two indices, thus there is no such
        // object on level 0.
        if (level>0)
          {
            CompressedSparsityPattern ci_sparsity;
            ci_sparsity.reinit(mg_dof_handler.n_dofs(level-1), mg_dof_handler.n_dofs(level));
            MGTools::make_flux_sparsity_pattern_edge(mg_dof_handler, ci_sparsity, level);
            mg_sparsity_dg_interface[level].copy_from(ci_sparsity);
            mg_matrix_dg_up[level].reinit(mg_sparsity_dg_interface[level]);
            mg_matrix_dg_down[level].reinit(mg_sparsity_dg_interface[level]);
          }
      }
  }

  // In this function, we assemble the global system matrix, where by global
  // we indicate that this is the matrix of the discrete system we solve and
  // it is covering the whole mesh.
  template <int dim>
  void
  InteriorPenaltyProblem<dim>::assemble_matrix()
  {
    // First, we need t set up the object providing the values we
    // integrate. This object contains all FEValues and FEFaceValues objects
    // needed and also maintains them automatically such that they always
    // point to the current cell. To this end, we need to tell it first, where
    // and what to compute. Since we are not doing anything fancy, we can rely
    // on their standard choice for quadrature rules.
    //
    // Since their default update flags are minimal, we add what we need
    // additionally, namely the values and gradients of shape functions on all
    // objects (cells, boundary and interior faces). Afterwards, we are ready
    // to initialize the container, which will create all necessary
    // FEValuesBase objects for integration.
    MeshWorker::IntegrationInfoBox<dim> info_box;
    UpdateFlags update_flags = update_values | update_gradients;
    info_box.add_update_flags_all(update_flags);
    info_box.initialize(fe, mapping);

    // This is the object into which we integrate local data. It is filled by
    // the local integration routines in MatrixIntegrator and then used by the
    // assembler to distribute the information into the global matrix.
    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

    // Furthermore, we need an object that assembles the local matrix into the
    // global matrix. These assembler objects have all the knowledge
    // of the structures of the target object, in this case a
    // SparseMatrix, possible constraints and the mesh structure.
    MeshWorker::Assembler::MatrixSimple<SparseMatrix<double> > assembler;
    assembler.initialize(matrix);

    // Now comes the part we coded ourselves, the local
    // integrator. This is the only part which is problem dependent.
    MatrixIntegrator<dim> integrator;
    // Now, we throw everything into a MeshWorker::loop(), which here
    // traverses all active cells of the mesh, computes cell and face matrices
    // and assembles them into the global matrix. We use the variable
    // <tt>dof_handler</tt> here in order to use the global numbering of
    // degrees of freedom.
    MeshWorker::integration_loop<dim, dim>(
      dof_handler.begin_active(), dof_handler.end(),
      dof_info, info_box,
      integrator, assembler);
  }

  // Now, we do the same for the level matrices. Not too surprisingly, this
  // function looks like a twin of the previous one. Indeed, there are only
  // two minor differences.
  template <int dim>
  void
  InteriorPenaltyProblem<dim>::assemble_mg_matrix()
  {
    MeshWorker::IntegrationInfoBox<dim> info_box;
    UpdateFlags update_flags = update_values | update_gradients;
    info_box.add_update_flags_all(update_flags);
    info_box.initialize(fe, mapping);

    MeshWorker::DoFInfo<dim> dof_info(mg_dof_handler);

    // Obviously, the assembler needs to be replaced by one filling level
    // matrices. Note that it automatically fills the edge matrices as well.
    MeshWorker::Assembler::MGMatrixSimple<SparseMatrix<double> > assembler;
    assembler.initialize(mg_matrix);
    assembler.initialize_fluxes(mg_matrix_dg_up, mg_matrix_dg_down);

    MatrixIntegrator<dim> integrator;
    // Here is the other difference to the previous function: we run over all
    // cells, not only the active ones. And we use <tt>mg_dof_handler</tt>,
    // since we need the degrees of freedom on each level, not the global
    // numbering.
    MeshWorker::integration_loop<dim, dim> (
      mg_dof_handler.begin(), mg_dof_handler.end(),
      dof_info, info_box,
      integrator, assembler);
  }

  // Here we have another clone of the assemble function. The difference to
  // assembling the system matrix consists in that we assemble a vector here.
  template <int dim>
  void
  InteriorPenaltyProblem<dim>::assemble_right_hand_side()
  {
    MeshWorker::IntegrationInfoBox<dim> info_box;
    UpdateFlags update_flags = update_quadrature_points | update_values | update_gradients;
    info_box.add_update_flags_all(update_flags);
    info_box.initialize(fe, mapping);

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

    // Since this assembler alows us to fill several vectors, the interface is
    // a little more complicated as above. The pointers to the vectors have to
    // be stored in a NamedData object. While this seems to cause two extra
    // lines of code here, it actually comes handy in more complex
    // applications.
    MeshWorker::Assembler::ResidualSimple<Vector<double> > assembler;
    NamedData<Vector<double>* > data;
    Vector<double> *rhs = &right_hand_side;
    data.add(rhs, "RHS");
    assembler.initialize(data);

    RHSIntegrator<dim> integrator;
    MeshWorker::integration_loop<dim, dim>(
      dof_handler.begin_active(), dof_handler.end(),
      dof_info, info_box,
      integrator, assembler);

    right_hand_side *= -1.;
  }

  // Now that we have coded all functions building the discrete linear system,
  // it is about time that we actually solve it.
  template <int dim>
  void
  InteriorPenaltyProblem<dim>::solve()
  {
    // The solver of choice is conjugate gradient.
    SolverControl control(1000, 1.e-12);
    SolverCG<Vector<double> > solver(control);

    // Now we are setting up the components of the multilevel
    // preconditioner. First, we need transfer between grid levels. The object
    // we are using here generates sparse matrices for these transfers.
    MGTransferPrebuilt<Vector<double> > mg_transfer;
    mg_transfer.build_matrices(mg_dof_handler);

    // Then, we need an exact solver for the matrix on the coarsest level.
    FullMatrix<double> coarse_matrix;
    coarse_matrix.copy_from (mg_matrix[0]);
    MGCoarseGridHouseholder<double, Vector<double> > mg_coarse;
    mg_coarse.initialize(coarse_matrix);

    // While transfer and coarse grid solver are pretty much generic, more
    // flexibility is offered for the smoother. First, we choose Gauss-Seidel
    // as our smoothing method.
    GrowingVectorMemory<Vector<double> > mem;
    typedef PreconditionSOR<SparseMatrix<double> > RELAXATION;
    mg::SmootherRelaxation<RELAXATION, Vector<double> >
    mg_smoother;
    RELAXATION::AdditionalData smoother_data(1.);
    mg_smoother.initialize(mg_matrix, smoother_data);

    // Do two smoothing steps on each level.
    mg_smoother.set_steps(2);
    // Since the SOR method is not symmetric, but we use conjugate gradient
    // iteration below, here is a trick to make the multilevel preconditioner
    // a symmetric operator even for nonsymmetric smoothers.
    mg_smoother.set_symmetric(true);
    // The smoother class optionally implements the variable V-cycle, which we
    // do not want here.
    mg_smoother.set_variable(false);

    // Finally, we must wrap our matrices in an object having the required
    // multiplication functions.
    MGMatrix<SparseMatrix<double>, Vector<double> > mgmatrix(&mg_matrix);
    MGMatrix<SparseMatrix<double>, Vector<double> > mgdown(&mg_matrix_dg_down);
    MGMatrix<SparseMatrix<double>, Vector<double> > mgup(&mg_matrix_dg_up);

    // Now, we are ready to set up the V-cycle operator and the multilevel
    // preconditioner.
    Multigrid<Vector<double> > mg(mg_dof_handler, mgmatrix,
                                  mg_coarse, mg_transfer,
                                  mg_smoother, mg_smoother);
    // Let us not forget the edge matrices needed because of the adaptive
    // refinement.
    mg.set_edge_flux_matrices(mgdown, mgup);

    // After all preparations, wrap the Multigrid object into another object,
    // which can be used as a regular preconditioner,
    PreconditionMG<dim, Vector<double>,
                   MGTransferPrebuilt<Vector<double> > >
                   preconditioner(mg_dof_handler, mg, mg_transfer);
    // and use it to solve the system.
    solver.solve(matrix, solution, right_hand_side, preconditioner);
  }

  // Another clone of the assemble function. The big difference to the
  // previous ones is here that we also have an input vector.
  template <int dim>
  double
  InteriorPenaltyProblem<dim>::estimate()
  {
    // The results of the estimator are stored in a vector with one entry per
    // cell. Since cells in deal.II are not numbered, we have to create our
    // own numbering in order to use this vector.
    //
    // On the other hand, somebody might have used the user indices
    // already. So, let's be good citizens and save them before tampering with
    // them.
    std::vector<unsigned int> old_user_indices;
    triangulation.save_user_indices(old_user_indices);

    estimates.block(0).reinit(triangulation.n_active_cells());
    unsigned int i=0;
    for (typename Triangulation<dim>::active_cell_iterator cell = triangulation.begin_active();
         cell != triangulation.end(); ++cell,++i)
      cell->set_user_index(i);

    // This starts like before,
    MeshWorker::IntegrationInfoBox<dim> info_box;
    const unsigned int n_gauss_points = dof_handler.get_fe().tensor_degree()+1;
    info_box.initialize_gauss_quadrature(n_gauss_points, n_gauss_points+1, n_gauss_points);

    // but now we need to notify the info box of the finite element functio we
    // want to evaluate in the quadrature points. First, we create a NamedData
    // object with this vector, which is the solution we just computed.
    NamedData<Vector<double>* > solution_data;
    solution_data.add(&solution, "solution");

    // Then, we tell the Meshworker::VectorSelector for cells, that we need
    // the second derivatives of this solution (to compute the
    // Laplacian). Therefore, the boolean arguments selecting function values
    // and first derivatives a false, only the last one selecting second
    // derivatives is true.
    info_box.cell_selector.add("solution", false, false, true);
    // On interior and boundary faces, we need the function values and the
    // first derivatives, but not second derivatives.
    info_box.boundary_selector.add("solution", true, true, false);
    info_box.face_selector.add("solution", true, true, false);

    // And we continue as before, with the exception that the default update
    // flags are already adjusted to the values and derivatives we requested
    // above.
    info_box.add_update_flags_boundary(update_quadrature_points);
    info_box.initialize(fe, mapping, solution_data);

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

    // The assembler stores one number per cell, but else this is the same as
    // in the computation of the right hand side.
    MeshWorker::Assembler::CellsAndFaces<double> assembler;
    NamedData<BlockVector<double>* > out_data;
    BlockVector<double> *est = &estimates;
    out_data.add(est, "cells");
    assembler.initialize(out_data, false);

    Estimator<dim> integrator;
    MeshWorker::integration_loop<dim, dim> (
      dof_handler.begin_active(), dof_handler.end(),
      dof_info, info_box,
      integrator, assembler);

    // Right before we return the result of the error estimate, we restore the
    // old user indices.
    triangulation.load_user_indices(old_user_indices);
    return estimates.block(0).l2_norm();
  }

  // Here we compare our finite element solution with the (known) exact
  // solution and compute the mean quadratic error of the gradient and the
  // function itself. This function is a clone of the estimation function
  // right above.

  // Since we compute the error in the energy and the
  // L2-norm, respectively, our block vector needs two
  // blocks here.
  template <int dim>
  void
  InteriorPenaltyProblem<dim>::error()
  {
    BlockVector<double> errors(2);
    errors.block(0).reinit(triangulation.n_active_cells());
    errors.block(1).reinit(triangulation.n_active_cells());
    unsigned int i=0;
    for (typename Triangulation<dim>::active_cell_iterator cell = triangulation.begin_active();
         cell != triangulation.end(); ++cell,++i)
      cell->set_user_index(i);

    MeshWorker::IntegrationInfoBox<dim> info_box;
    const unsigned int n_gauss_points = dof_handler.get_fe().tensor_degree()+1;
    info_box.initialize_gauss_quadrature(n_gauss_points, n_gauss_points+1, n_gauss_points);

    NamedData<Vector<double>* > solution_data;
    solution_data.add(&solution, "solution");

    info_box.cell_selector.add("solution", true, true, false);
    info_box.boundary_selector.add("solution", true, false, false);
    info_box.face_selector.add("solution", true, false, false);

    info_box.add_update_flags_cell(update_quadrature_points);
    info_box.add_update_flags_boundary(update_quadrature_points);
    info_box.initialize(fe, mapping, solution_data);

    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

    MeshWorker::Assembler::CellsAndFaces<double> assembler;
    NamedData<BlockVector<double>* > out_data;
    BlockVector<double> *est = &errors;
    out_data.add(est, "cells");
    assembler.initialize(out_data, false);

    ErrorIntegrator<dim> integrator;
    MeshWorker::integration_loop<dim, dim> (
      dof_handler.begin_active(), dof_handler.end(),
      dof_info, info_box,
      integrator, assembler);

    deallog << "energy-error: " << errors.block(0).l2_norm() << std::endl;
    deallog << "L2-error:     " << errors.block(1).l2_norm() << std::endl;
  }

  // Some graphical output
  template <int dim>
  void InteriorPenaltyProblem<dim>::output_results (const unsigned int cycle) const
  {
    // Output of the solution in gnuplot format.
    char *fn = new char[100];
    sprintf(fn, "sol-%02d", cycle);

    std::string filename(fn);
    filename += ".gnuplot";
    deallog << "Writing solution to <" << filename << ">..."
            << std::endl << std::endl;
    std::ofstream gnuplot_output (filename.c_str());

    DataOut<dim> data_out;
    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (solution, "u");
    data_out.add_data_vector (estimates.block(0), "est");

    data_out.build_patches ();

    data_out.write_gnuplot(gnuplot_output);
  }

  // And finally the adaptive loop, more or less like in previous examples.
  template <int dim>
  void
  InteriorPenaltyProblem<dim>::run(unsigned int n_steps)
  {
    deallog << "Element: " << fe.get_name() << std::endl;
    for (unsigned int s=0; s<n_steps; ++s)
      {
        deallog << "Step " << s << std::endl;
        if (estimates.block(0).size() == 0)
          triangulation.refine_global(1);
        else
          {
            GridRefinement::refine_and_coarsen_fixed_fraction (triangulation,
                                                               estimates.block(0),
                                                               0.5, 0.0);
            triangulation.execute_coarsening_and_refinement ();
          }

        deallog << "Triangulation "
                << triangulation.n_active_cells() << " cells, "
                << triangulation.n_levels() << " levels" << std::endl;

        setup_system();
        deallog << "DoFHandler " << dof_handler.n_dofs() << " dofs, level dofs";
        for (unsigned int l=0; l<triangulation.n_levels(); ++l)
          deallog << ' ' << mg_dof_handler.n_dofs(l);
        deallog << std::endl;

        deallog << "Assemble matrix" << std::endl;
        assemble_matrix();
        deallog << "Assemble multilevel matrix" << std::endl;
        assemble_mg_matrix();
        deallog << "Assemble right hand side" << std::endl;
        assemble_right_hand_side();
        deallog << "Solve" << std::endl;
        solve();
        error();
        deallog << "Estimate " << estimate() << std::endl;
        output_results(s);
      }
  }
}

int main()
{
  try
    {
      using namespace dealii;
      using namespace Step39;

      std::ofstream logfile("deallog");
      deallog.attach(logfile);
      FE_DGQ<2> fe1(3);
      InteriorPenaltyProblem<2> test1(fe1);
      test1.run(12);
    }
  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;
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/ext/std/integer_sequence.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/front.hpp>

#include <type_traits>
#include <utility>
namespace hana = boost::hana;

int main() {
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::front(std::index_sequence<0>{}),
        std::integral_constant<std::size_t, 0>{}
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::front(std::index_sequence<0, 1>{}),
        std::integral_constant<std::size_t, 0>{}
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::front(std::integer_sequence<int, -3, 1, 2, 3, 4>{}),
        std::integral_constant<int, -3>{}
    ));
}